From ddee2ff54dc5c4ce723cef5e3e91b2b3dee28ebf Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 24 Sep 2024 11:29:02 -0700 Subject: [PATCH 01/21] added tracer, still nt rn --- src/main/java/frc/robot/Robot.java | 47 ++++- src/main/java/frc/robot/utils/Tracer.java | 226 ++++++++++++++++++++++ 2 files changed, 272 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/utils/Tracer.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6abd2d92..eb102d3b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -17,6 +17,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.PowerDistribution; @@ -50,7 +51,11 @@ import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem.AutoAimStates; import frc.robot.utils.CommandXboxControllerSubsystem; +import frc.robot.utils.Tracer; import frc.robot.utils.autoaim.AutoAim; + +import java.util.HashMap; +import java.util.function.BiConsumer; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; @@ -157,6 +162,7 @@ public void robotInit() { Logger.recordMetadata("GitDirty", "Unknown"); break; } + setUpLogging(); switch (mode) { case REAL: @@ -467,7 +473,8 @@ public void robotInit() { @Override public void robotPeriodic() { - CommandScheduler.getInstance().run(); + Tracer.startTrace("RobotPeriodic"); + Tracer.traceFunc("CommandScheduler", CommandScheduler.getInstance()::run); // Update ascope mechanism visualization Logger.recordOutput( "Mechanism Poses", @@ -485,6 +492,44 @@ public void robotPeriodic() { Logger.recordOutput( "AutoAim/Actual Distance", swerve.getPose().minus(FieldConstants.getSpeaker()).getTranslation().getNorm()); + + Tracer.endTrace(); + } + + @Override + public void loopFunc() { + Tracer.startTrace("Robot"); + Tracer.traceFunc("LoopFunc", super::loopFunc); + Tracer.endTrace(); + } + + private void setUpLogging() { + HashMap commandCounts = new HashMap<>(); + BiConsumer logCommandFunction = + (Command command, Boolean active) -> { + String name = command.getName(); + int count = commandCounts.getOrDefault(name, 0) + (active ? 1 : -1); + commandCounts.put(name, count); + Logger.recordOutput( + "Commands/CommandsUnique/" + name + "_" + Integer.toHexString(command.hashCode()), + active.booleanValue()); + Logger.recordOutput("Commands/CommandsAll/" + name, count > 0); + }; + CommandScheduler.getInstance() + .onCommandInitialize( + (Command command) -> { + logCommandFunction.accept(command, true); + }); + CommandScheduler.getInstance() + .onCommandFinish( + (Command command) -> { + logCommandFunction.accept(command, false); + }); + CommandScheduler.getInstance() + .onCommandInterrupt( + (Command command) -> { + logCommandFunction.accept(command, false); + }); } private Command shootWithDashboard() { diff --git a/src/main/java/frc/robot/utils/Tracer.java b/src/main/java/frc/robot/utils/Tracer.java new file mode 100644 index 00000000..1936363a --- /dev/null +++ b/src/main/java/frc/robot/utils/Tracer.java @@ -0,0 +1,226 @@ +package frc.robot.utils; + +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DriverStation; +import java.lang.management.GarbageCollectorMXBean; +import java.lang.management.ManagementFactory; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; + +/** + * A Utility class for tracing code execution time. Will put info to NetworkTables under the + * "Tracer" table. + * + *

+ *
+ * @Override
+ * public void loopFunc() {
+ *    Tracer.traceFunc("LoopFunc", super::loopFunc);
+ * }
+ *
+ * 

+ * + * @Override + * public void robotPeriodic() { + * Tracer.startTrace("RobotPeriodic"); + * Tracer.traceFunc("CommandScheduler", scheduler::run); + * Tracer.traceFunc("Monologue", Monologue::updateAll); + * Tracer.endTrace(); + * } + * + *

+ */ +public class Tracer { + private static final class TraceStartData { + private double startTime = 0.0; + private double startGCTotalTime = 0.0; + + private void set(double startTime, double startGCTotalTime) { + this.startTime = startTime; + this.startGCTotalTime = startGCTotalTime; + } + } + + /** + * All of the tracers persistent state in a single object to be stored in a {@link ThreadLocal}. + */ + private static final class TracerState { + private final NetworkTable rootTable; + + // the stack of traces, every startTrace will add to this stack + // and every endTrace will remove from this stack + private final ArrayList traceStack = new ArrayList<>(); + // ideally we only need `traceStack` but in the interest of memory optimization + // and string concatenation speed we store the history of the stack to reuse the stack names + private final ArrayList traceStackHistory = new ArrayList<>(); + // the time of each trace, the key is the trace name, modified every endTrace + private final HashMap traceTimes = new HashMap<>(); + // the start time of each trace and the gc time at the start of the trace, + // the key is the trace name, modified every startTrace and endTrace. + private final HashMap traceStartTimes = new HashMap<>(); + // the publishers for each trace, the key is the trace name, modified every endCycle + private final HashMap publisherHeap = new HashMap<>(); + + // the garbage collector beans + private final ArrayList gcs = + new ArrayList<>(ManagementFactory.getGarbageCollectorMXBeans()); + private final DoublePublisher gcTimeEntry; + private double gcTimeThisCycle = 0.0; + + private TracerState(String threadName) { + this.rootTable = NetworkTableInstance.getDefault().getTable("Tracer").getSubTable(threadName); + this.gcTimeEntry = + rootTable.getDoubleTopic("GCTime").publishEx("double", "{ \"cached\": false}"); + } + + private String appendTraceStack(String trace) { + traceStack.add(trace); + StringBuilder sb = new StringBuilder(); + for (int i = 0; i < traceStack.size(); i++) { + sb.append(traceStack.get(i)); + if (i < traceStack.size() - 1) { + sb.append("/"); + } + } + String str = sb.toString(); + traceStackHistory.add(str); + return str; + } + + private String popTraceStack() { + traceStack.remove(traceStack.size() - 1); + return traceStackHistory.remove(traceStackHistory.size() - 1); + } + + private double totalGCTime() { + double gcTime = 0; + for (GarbageCollectorMXBean gc : gcs) { + gcTime += gc.getCollectionTime(); + } + return gcTime; + } + + private void endCycle() { + // update times for all already existing publishers + for (var publisher : publisherHeap.entrySet()) { + // if the entry isn't found, time will null-cast to 0.0 + Double time = traceTimes.remove(publisher.getKey()); + if (time == null) time = 0.0; + publisher.getValue().set(time); + } + // create publishers for all new entries + for (var traceTime : traceTimes.entrySet()) { + DoublePublisher publisher = + rootTable + .getDoubleTopic(traceTime.getKey()) + .publishEx("double", "{ \"cached\": false}"); + publisher.set(traceTime.getValue()); + publisherHeap.put(traceTime.getKey(), publisher); + } + + // log gc time + if (gcs.size() > 0) gcTimeEntry.set(gcTimeThisCycle); + gcTimeThisCycle = 0.0; + + // clean up state + traceTimes.clear(); + traceStackHistory.clear(); + } + } + + private static final ThreadLocal threadLocalState = + ThreadLocal.withInitial( + () -> { + return new TracerState(Thread.currentThread().getName()); + }); + + public static void disableGcLoggingForCurrentThread() { + TracerState state = threadLocalState.get(); + state.gcTimeEntry.close(); + state.gcs.clear(); + } + + private static void startTrace(final String name, final TracerState state) { + String stack = state.appendTraceStack(name); + TraceStartData data = state.traceStartTimes.get(stack); + if (data == null) { + data = new TraceStartData(); + state.traceStartTimes.put(stack, data); + } + data.set(Logger.getRealTimestamp() / 1000.0, state.totalGCTime()); + } + + private static void endTrace(final TracerState state) { + try { + String stack = state.popTraceStack(); + var startData = state.traceStartTimes.get(stack); + double gcTimeSinceStart = state.totalGCTime() - startData.startGCTotalTime; + state.gcTimeThisCycle += gcTimeSinceStart; + state.traceTimes.put( + stack, Logger.getRealTimestamp() / 1000.0 - startData.startTime - gcTimeSinceStart); + if (state.traceStack.size() == 0) { + state.endCycle(); + } + } catch (Exception e) { + DriverStation.reportError( + "[Tracer] An end trace was called with no opening trace " + e, true); + } + } + + /** + * Starts a trace, should be called at the beginning of a function thats not being called by user + * code. Should be paired with {@link Tracer#endTrace()} at the end of the function. + * + *

Best used in periodic functions in Subsystems and Robot.java. + * + * @param name the name of the trace, should be unique to the function. + */ + public static void startTrace(String name) { + startTrace(name, threadLocalState.get()); + } + + /** + * Ends a trace, should only be called at the end of a function thats not being called by user + * code. If a {@link Tracer#startTrace(String)} is not paired with a {@link Tracer#endTrace()} + * there could be a crash. + */ + public static void endTrace() { + endTrace(threadLocalState.get()); + } + + /** + * Traces a function, should be used in place of {@link Tracer#startTrace(String)} and {@link + * Tracer#endTrace()} for functions called by user code like {@code CommandScheduler.run()} and + * other expensive functions. + * + * @param name the name of the trace, should be unique to the function. + * @param runnable the function to trace. + * @apiNote If you want to return a value then use {@link Tracer#traceFunc(String, Supplier)}. + */ + public static void traceFunc(String name, Runnable runnable) { + final TracerState state = threadLocalState.get(); + startTrace(name, state); + runnable.run(); + endTrace(state); + } + + /** + * Traces a function, should be used in place of {@link Tracer#startTrace(String)} and {@link + * Tracer#endTrace()} for functions called by user code like {@code CommandScheduler.run()} and + * other expensive functions. + * + * @param name the name of the trace, should be unique to the function. + * @param supplier the function to trace. + */ + public static T traceFunc(String name, Supplier supplier) { + final TracerState state = threadLocalState.get(); + startTrace(name, state); + T ret = supplier.get(); + endTrace(state); + return ret; + } +} From 7c499e87730dd7a221ee3418e4618f306fbc77df Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 24 Sep 2024 19:29:33 -0700 Subject: [PATCH 02/21] replaced networktables publishing with logging --- src/main/java/frc/robot/Robot.java | 78 +++++++++++------------ src/main/java/frc/robot/utils/Tracer.java | 33 ++-------- 2 files changed, 43 insertions(+), 68 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index eb102d3b..4fc8c861 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -17,7 +17,6 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.PowerDistribution; @@ -53,7 +52,6 @@ import frc.robot.utils.CommandXboxControllerSubsystem; import frc.robot.utils.Tracer; import frc.robot.utils.autoaim.AutoAim; - import java.util.HashMap; import java.util.function.BiConsumer; import java.util.function.DoubleSupplier; @@ -492,44 +490,44 @@ public void robotPeriodic() { Logger.recordOutput( "AutoAim/Actual Distance", swerve.getPose().minus(FieldConstants.getSpeaker()).getTranslation().getNorm()); - - Tracer.endTrace(); - } - - @Override - public void loopFunc() { - Tracer.startTrace("Robot"); - Tracer.traceFunc("LoopFunc", super::loopFunc); - Tracer.endTrace(); - } - - private void setUpLogging() { - HashMap commandCounts = new HashMap<>(); - BiConsumer logCommandFunction = - (Command command, Boolean active) -> { - String name = command.getName(); - int count = commandCounts.getOrDefault(name, 0) + (active ? 1 : -1); - commandCounts.put(name, count); - Logger.recordOutput( - "Commands/CommandsUnique/" + name + "_" + Integer.toHexString(command.hashCode()), - active.booleanValue()); - Logger.recordOutput("Commands/CommandsAll/" + name, count > 0); - }; - CommandScheduler.getInstance() - .onCommandInitialize( - (Command command) -> { - logCommandFunction.accept(command, true); - }); - CommandScheduler.getInstance() - .onCommandFinish( - (Command command) -> { - logCommandFunction.accept(command, false); - }); - CommandScheduler.getInstance() - .onCommandInterrupt( - (Command command) -> { - logCommandFunction.accept(command, false); - }); + + Tracer.endTrace(); + } + + @Override + public void loopFunc() { + Tracer.startTrace("Robot"); + Tracer.traceFunc("LoopFunc", super::loopFunc); + Tracer.endTrace(); + } + + private void setUpLogging() { + HashMap commandCounts = new HashMap<>(); + BiConsumer logCommandFunction = + (Command command, Boolean active) -> { + String name = command.getName(); + int count = commandCounts.getOrDefault(name, 0) + (active ? 1 : -1); + commandCounts.put(name, count); + Logger.recordOutput( + "Commands/CommandsUnique/" + name + "_" + Integer.toHexString(command.hashCode()), + active.booleanValue()); + Logger.recordOutput("Commands/CommandsAll/" + name, count > 0); + }; + CommandScheduler.getInstance() + .onCommandInitialize( + (Command command) -> { + logCommandFunction.accept(command, true); + }); + CommandScheduler.getInstance() + .onCommandFinish( + (Command command) -> { + logCommandFunction.accept(command, false); + }); + CommandScheduler.getInstance() + .onCommandInterrupt( + (Command command) -> { + logCommandFunction.accept(command, false); + }); } private Command shootWithDashboard() { diff --git a/src/main/java/frc/robot/utils/Tracer.java b/src/main/java/frc/robot/utils/Tracer.java index 1936363a..db0d982c 100644 --- a/src/main/java/frc/robot/utils/Tracer.java +++ b/src/main/java/frc/robot/utils/Tracer.java @@ -1,8 +1,5 @@ package frc.robot.utils; -import edu.wpi.first.networktables.DoublePublisher; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import java.lang.management.GarbageCollectorMXBean; import java.lang.management.ManagementFactory; @@ -49,7 +46,6 @@ private void set(double startTime, double startGCTotalTime) { * All of the tracers persistent state in a single object to be stored in a {@link ThreadLocal}. */ private static final class TracerState { - private final NetworkTable rootTable; // the stack of traces, every startTrace will add to this stack // and every endTrace will remove from this stack @@ -62,20 +58,13 @@ private static final class TracerState { // the start time of each trace and the gc time at the start of the trace, // the key is the trace name, modified every startTrace and endTrace. private final HashMap traceStartTimes = new HashMap<>(); - // the publishers for each trace, the key is the trace name, modified every endCycle - private final HashMap publisherHeap = new HashMap<>(); // the garbage collector beans private final ArrayList gcs = new ArrayList<>(ManagementFactory.getGarbageCollectorMXBeans()); - private final DoublePublisher gcTimeEntry; private double gcTimeThisCycle = 0.0; - private TracerState(String threadName) { - this.rootTable = NetworkTableInstance.getDefault().getTable("Tracer").getSubTable(threadName); - this.gcTimeEntry = - rootTable.getDoubleTopic("GCTime").publishEx("double", "{ \"cached\": false}"); - } + private TracerState(String threadName) {} private String appendTraceStack(String trace) { traceStack.add(trace); @@ -105,25 +94,14 @@ private double totalGCTime() { } private void endCycle() { - // update times for all already existing publishers - for (var publisher : publisherHeap.entrySet()) { - // if the entry isn't found, time will null-cast to 0.0 - Double time = traceTimes.remove(publisher.getKey()); - if (time == null) time = 0.0; - publisher.getValue().set(time); - } - // create publishers for all new entries for (var traceTime : traceTimes.entrySet()) { - DoublePublisher publisher = - rootTable - .getDoubleTopic(traceTime.getKey()) - .publishEx("double", "{ \"cached\": false}"); - publisher.set(traceTime.getValue()); - publisherHeap.put(traceTime.getKey(), publisher); + Double time = traceTimes.remove(traceTime.getKey()); + if (time == null) time = 0.0; + Logger.recordOutput("Tracer/" + traceTime.getKey(), traceTime.getValue()); } // log gc time - if (gcs.size() > 0) gcTimeEntry.set(gcTimeThisCycle); + if (gcs.size() > 0) Logger.recordOutput("Tracer/GCTime", gcTimeThisCycle); gcTimeThisCycle = 0.0; // clean up state @@ -140,7 +118,6 @@ private void endCycle() { public static void disableGcLoggingForCurrentThread() { TracerState state = threadLocalState.get(); - state.gcTimeEntry.close(); state.gcs.clear(); } From b659f7d4987329cf8b847419ebc6365ec558e2ca Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 24 Sep 2024 21:01:10 -0700 Subject: [PATCH 03/21] should actually log + traces some methods in swerve --- .../swerve/PhoenixOdometryThread.java | 19 ++++++++--- .../subsystems/swerve/SwerveSubsystem.java | 26 +++++++++------ src/main/java/frc/robot/utils/Tracer.java | 32 ++++++++++++++++--- 3 files changed, 58 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/swerve/PhoenixOdometryThread.java index faa3aca0..f37dd2fe 100644 --- a/src/main/java/frc/robot/subsystems/swerve/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/swerve/PhoenixOdometryThread.java @@ -21,6 +21,7 @@ import com.google.common.collect.EvictingQueue; import com.google.common.collect.Sets; import frc.robot.subsystems.swerve.Module.ModuleConstants; +import frc.robot.utils.Tracer; import java.util.Arrays; import java.util.Collection; import java.util.List; @@ -133,25 +134,32 @@ public void registerSignals(Registration... registrations) { } public List samplesSince(double timestamp) { + Tracer.startTrace("samples since"); var readLock = journalLock.readLock(); try { readLock.lock(); - return journal.stream() - .filter(s -> s.timestamp > timestamp) - .collect(Collectors.toUnmodifiableList()); + return Tracer.traceFunc( + "stream timestamps", + () -> + journal.stream() + .filter(s -> s.timestamp > timestamp) + .collect(Collectors.toUnmodifiableList())); } finally { readLock.unlock(); + Tracer.endTrace(); } } @Override public void run() { - System.out.println("Starting Odo Thread"); while (true) { // Wait for updates from all signals var writeLock = journalLock.writeLock(); - BaseStatusSignal.waitForAll(2.0 / Module.ODOMETRY_FREQUENCY_HZ, signalArr); + Tracer.startTrace("Odometry Thread"); + Tracer.traceFunc( + "wait for all", + () -> BaseStatusSignal.waitForAll(2.0 / Module.ODOMETRY_FREQUENCY_HZ, signalArr)); try { writeLock.lock(); var filteredSignals = @@ -168,6 +176,7 @@ public void run() { s -> s.signal().getValueAsDouble())))); } finally { writeLock.unlock(); + Tracer.endTrace(); } } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 525018e6..cb9e23f5 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -83,6 +83,7 @@ import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOReal; import frc.robot.subsystems.vision.VisionIOSim; +import frc.robot.utils.Tracer; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.ShotData; import java.io.File; @@ -372,23 +373,26 @@ public static VisionIO[] createSimCameras() { } public void periodic() { + Tracer.startTrace("SwervePeriodic"); for (var camera : cameras) { - camera.updateInputs(); - camera.processInputs(); + Tracer.traceFunc("Update cam inputs", camera::updateInputs); + Tracer.traceFunc("Process cam inputs", camera::processInputs); } - odoThread.updateInputs(odoThreadInputs, lastOdometryUpdateTimestamp); + Tracer.traceFunc( + "Update odo inputs", + () -> odoThread.updateInputs(odoThreadInputs, lastOdometryUpdateTimestamp)); Logger.processInputs("Async Odo", odoThreadInputs); if (!odoThreadInputs.sampledStates.isEmpty()) { lastOdometryUpdateTimestamp = odoThreadInputs.sampledStates.get(odoThreadInputs.sampledStates.size() - 1).timestamp(); } - gyroIO.updateInputs(gyroInputs); - for (var module : modules) { - module.updateInputs(); + Tracer.traceFunc("update gyro inputs", () -> gyroIO.updateInputs(gyroInputs)); + for (int i = 0; i < modules.length; i++) { + Tracer.traceFunc("SwerveModule update inputs[" + i + "]", modules[i]::updateInputs); } Logger.processInputs("Swerve/Gyro", gyroInputs); - for (var module : modules) { - module.periodic(); + for (int i = 0; i < modules.length; i++) { + Tracer.traceFunc("SwerveModule periodic[" + i + "]", modules[i]::periodic); } // Stop moving when disabled @@ -409,8 +413,10 @@ public void periodic() { Logger.recordOutput("ShotData/Flight Time", AutoAimStates.curShotData.getFlightTimeSeconds()); Logger.recordOutput("ShotData/Lookahead", AutoAimStates.lookaheadTime); - updateOdometry(); - updateVision(); + Tracer.traceFunc("Update odometry", () -> updateOdometry()); + Tracer.traceFunc("update vision", () -> updateVision()); + + Tracer.endTrace(); } private void updateOdometry() { diff --git a/src/main/java/frc/robot/utils/Tracer.java b/src/main/java/frc/robot/utils/Tracer.java index db0d982c..c61b60a4 100644 --- a/src/main/java/frc/robot/utils/Tracer.java +++ b/src/main/java/frc/robot/utils/Tracer.java @@ -1,5 +1,8 @@ package frc.robot.utils; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import java.lang.management.GarbageCollectorMXBean; import java.lang.management.ManagementFactory; @@ -46,6 +49,7 @@ private void set(double startTime, double startGCTotalTime) { * All of the tracers persistent state in a single object to be stored in a {@link ThreadLocal}. */ private static final class TracerState { + private final NetworkTable rootTable; // the stack of traces, every startTrace will add to this stack // and every endTrace will remove from this stack @@ -58,13 +62,20 @@ private static final class TracerState { // the start time of each trace and the gc time at the start of the trace, // the key is the trace name, modified every startTrace and endTrace. private final HashMap traceStartTimes = new HashMap<>(); + // the publishers for each trace, the key is the trace name, modified every endCycle + private final HashMap publisherHeap = new HashMap<>(); // the garbage collector beans private final ArrayList gcs = new ArrayList<>(ManagementFactory.getGarbageCollectorMXBeans()); + private final DoublePublisher gcTimeEntry; private double gcTimeThisCycle = 0.0; - private TracerState(String threadName) {} + private TracerState(String threadName) { + this.rootTable = NetworkTableInstance.getDefault().getTable("Tracer").getSubTable(threadName); + this.gcTimeEntry = + rootTable.getDoubleTopic("GCTime").publishEx("double", "{ \"cached\": false}"); + } private String appendTraceStack(String trace) { traceStack.add(trace); @@ -94,14 +105,26 @@ private double totalGCTime() { } private void endCycle() { - for (var traceTime : traceTimes.entrySet()) { - Double time = traceTimes.remove(traceTime.getKey()); + // update times for all already existing publishers + for (var publisher : publisherHeap.entrySet()) { + // if the entry isn't found, time will null-cast to 0.0 + Double time = traceTimes.remove(publisher.getKey()); if (time == null) time = 0.0; + Logger.recordOutput("Tracer/" + publisher.getKey(), time); + } + // create publishers for all new entries + for (var traceTime : traceTimes.entrySet()) { + DoublePublisher publisher = + rootTable + .getDoubleTopic(traceTime.getKey()) + .publishEx("double", "{ \"cached\": false}"); + publisher.set(traceTime.getValue()); Logger.recordOutput("Tracer/" + traceTime.getKey(), traceTime.getValue()); + publisherHeap.put(traceTime.getKey(), publisher); } // log gc time - if (gcs.size() > 0) Logger.recordOutput("Tracer/GCTime", gcTimeThisCycle); + if (gcs.size() > 0) gcTimeEntry.set(gcTimeThisCycle); gcTimeThisCycle = 0.0; // clean up state @@ -118,6 +141,7 @@ private void endCycle() { public static void disableGcLoggingForCurrentThread() { TracerState state = threadLocalState.get(); + state.gcTimeEntry.close(); state.gcs.clear(); } From cb252c8b8956248c7d602bdd99a3e8e266f4ed5a Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 24 Sep 2024 21:13:49 -0700 Subject: [PATCH 04/21] removed publishers fr wooo --- src/main/java/frc/robot/utils/Tracer.java | 34 ++++++----------------- 1 file changed, 9 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/utils/Tracer.java b/src/main/java/frc/robot/utils/Tracer.java index c61b60a4..784f4689 100644 --- a/src/main/java/frc/robot/utils/Tracer.java +++ b/src/main/java/frc/robot/utils/Tracer.java @@ -1,8 +1,5 @@ package frc.robot.utils; -import edu.wpi.first.networktables.DoublePublisher; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import java.lang.management.GarbageCollectorMXBean; import java.lang.management.ManagementFactory; @@ -49,7 +46,6 @@ private void set(double startTime, double startGCTotalTime) { * All of the tracers persistent state in a single object to be stored in a {@link ThreadLocal}. */ private static final class TracerState { - private final NetworkTable rootTable; // the stack of traces, every startTrace will add to this stack // and every endTrace will remove from this stack @@ -62,20 +58,14 @@ private static final class TracerState { // the start time of each trace and the gc time at the start of the trace, // the key is the trace name, modified every startTrace and endTrace. private final HashMap traceStartTimes = new HashMap<>(); - // the publishers for each trace, the key is the trace name, modified every endCycle - private final HashMap publisherHeap = new HashMap<>(); + private final ArrayList entryArray = new ArrayList<>(); // the garbage collector beans private final ArrayList gcs = new ArrayList<>(ManagementFactory.getGarbageCollectorMXBeans()); - private final DoublePublisher gcTimeEntry; private double gcTimeThisCycle = 0.0; - private TracerState(String threadName) { - this.rootTable = NetworkTableInstance.getDefault().getTable("Tracer").getSubTable(threadName); - this.gcTimeEntry = - rootTable.getDoubleTopic("GCTime").publishEx("double", "{ \"cached\": false}"); - } + private TracerState(String threadName) {} private String appendTraceStack(String trace) { traceStack.add(trace); @@ -105,26 +95,21 @@ private double totalGCTime() { } private void endCycle() { - // update times for all already existing publishers - for (var publisher : publisherHeap.entrySet()) { + // update times for all already existing entries + for (var entry : entryArray) { // if the entry isn't found, time will null-cast to 0.0 - Double time = traceTimes.remove(publisher.getKey()); + Double time = traceTimes.remove(entry); if (time == null) time = 0.0; - Logger.recordOutput("Tracer/" + publisher.getKey(), time); + Logger.recordOutput("Tracer/" + entry, time); } - // create publishers for all new entries + // log all new entries for (var traceTime : traceTimes.entrySet()) { - DoublePublisher publisher = - rootTable - .getDoubleTopic(traceTime.getKey()) - .publishEx("double", "{ \"cached\": false}"); - publisher.set(traceTime.getValue()); Logger.recordOutput("Tracer/" + traceTime.getKey(), traceTime.getValue()); - publisherHeap.put(traceTime.getKey(), publisher); + entryArray.add(traceTime.getKey()); } // log gc time - if (gcs.size() > 0) gcTimeEntry.set(gcTimeThisCycle); + if (gcs.size() > 0) Logger.recordOutput("Tracer/GCTime", gcTimeThisCycle); gcTimeThisCycle = 0.0; // clean up state @@ -141,7 +126,6 @@ private void endCycle() { public static void disableGcLoggingForCurrentThread() { TracerState state = threadLocalState.get(); - state.gcTimeEntry.close(); state.gcs.clear(); } From b21c643a0e3afdfc50a9f64e13cc9886f3cabd33 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 25 Sep 2024 15:51:30 -0700 Subject: [PATCH 05/21] add velocity check to choreo traj following command --- .../java/frc/robot/subsystems/swerve/SwerveSubsystem.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 525018e6..a0cb6edc 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -685,7 +685,7 @@ public Command runChoreoTraj(ChoreoTrajectory traj, boolean resetPose) { * @param requirements The subsystem(s) to require, typically your drive subsystem only. * @return A command that follows a Choreo path. */ - public static Command choreoFullFollowSwerveCommand( + public Command choreoFullFollowSwerveCommand( ChoreoTrajectory trajectory, Supplier poseSupplier, ChoreoControlFunction controller, @@ -727,6 +727,7 @@ public static Command choreoFullFollowSwerveCommand( mirrorTrajectory.getAsBoolean() ? trajectory.getFinalState().flipped().getPose() : trajectory.getFinalState().getPose(); + var vel = getVelocity(); Logger.recordOutput("Swerve/Current Traj End Pose", finalPose); return timer.hasElapsed(trajectory.getTotalTime()) && (MathUtil.isNear(finalPose.getX(), poseSupplier.get().getX(), 0.4) @@ -735,7 +736,8 @@ public static Command choreoFullFollowSwerveCommand( (poseSupplier.get().getRotation().getDegrees() - finalPose.getRotation().getDegrees()) % 360) - < 20.0); + < 20.0) + && MathUtil.isNear( 0.0, vel.vxMetersPerSecond * vel.vxMetersPerSecond + vel.vyMetersPerSecond * vel.vyMetersPerSecond, 0.25); }, requirements); } From 547a9da83e887c0a31d1ea15626a634b1414a17c Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 25 Sep 2024 16:58:10 -0700 Subject: [PATCH 06/21] rename module logs --- src/main/java/frc/robot/subsystems/swerve/Module.java | 5 +++++ .../subsystems/swerve/PhoenixOdometryThread.java | 2 +- .../frc/robot/subsystems/swerve/SwerveSubsystem.java | 11 +++++++++-- 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/Module.java b/src/main/java/frc/robot/subsystems/swerve/Module.java index c67e3dd8..44bd5f3c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Module.java +++ b/src/main/java/frc/robot/subsystems/swerve/Module.java @@ -143,6 +143,11 @@ public double getPositionMeters() { return inputs.drivePositionMeters; } + /** Returns this modules prefix ie "Back Left" */ + public String getPrefix() { + return inputs.prefix; + } + /** * Returns the current drive velocity of the module in meters per second withat normal sampling * frequency. diff --git a/src/main/java/frc/robot/subsystems/swerve/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/swerve/PhoenixOdometryThread.java index f37dd2fe..880becce 100644 --- a/src/main/java/frc/robot/subsystems/swerve/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/swerve/PhoenixOdometryThread.java @@ -176,8 +176,8 @@ public void run() { s -> s.signal().getValueAsDouble())))); } finally { writeLock.unlock(); - Tracer.endTrace(); } + Tracer.endTrace(); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index cb9e23f5..665d028c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -388,11 +388,14 @@ public void periodic() { } Tracer.traceFunc("update gyro inputs", () -> gyroIO.updateInputs(gyroInputs)); for (int i = 0; i < modules.length; i++) { - Tracer.traceFunc("SwerveModule update inputs[" + i + "]", modules[i]::updateInputs); + Tracer.traceFunc( + "SwerveModule update inputs from " + modules[i].getPrefix() + " Module", + modules[i]::updateInputs); } Logger.processInputs("Swerve/Gyro", gyroInputs); for (int i = 0; i < modules.length; i++) { - Tracer.traceFunc("SwerveModule periodic[" + i + "]", modules[i]::periodic); + Tracer.traceFunc( + "SwerveModule periodic from " + modules[i].getPrefix() + " Module", modules[i]::periodic); } // Stop moving when disabled @@ -422,7 +425,10 @@ public void periodic() { private void updateOdometry() { Logger.recordOutput("Swerve/Updates Since Last", odoThreadInputs.sampledStates.size()); var sampleStates = odoThreadInputs.sampledStates; + var i = 0; for (var sample : sampleStates) { + Tracer.startTrace("Sample " + i); + i++; // Read wheel deltas from each module SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; SwerveModulePosition[] moduleDeltas = @@ -500,6 +506,7 @@ private void updateOdometry() { Logger.recordOutput("Odometry/Gyro Rotation", lastGyroRotation); // Apply update estimator.updateWithTime(sample.timestamp(), rawGyroRotation, modulePositions); + Tracer.endTrace(); } } From 5d4a37c168b43deaa9c0d63dd7986fd71a1d53b6 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 26 Sep 2024 16:41:32 -0700 Subject: [PATCH 07/21] [amp 4 local] update choreo voltage limit, reduce max angle for spin on auto --- choreo.chor | 2658 +++++++++-------- src/main/deploy/choreo/amp 4 local.1.traj | 938 ++++-- src/main/deploy/choreo/amp 4 local.2.traj | 920 ++++-- src/main/deploy/choreo/amp 4 local.3.traj | 737 +++-- src/main/deploy/choreo/amp 4 local.traj | 2525 +++++++++++----- src/main/java/frc/robot/Robot.java | 2 +- .../subsystems/swerve/SwerveSubsystem.java | 6 +- 7 files changed, 5187 insertions(+), 2599 deletions(-) diff --git a/choreo.chor b/choreo.chor index 65ed1ed4..445cd96f 100644 --- a/choreo.chor +++ b/choreo.chor @@ -3,7 +3,7 @@ "robotConfiguration": { "mass": 74.08797700309194, "rotationalInertia": 5.724, - "motorMaxTorque": 0.5811475409836065, + "motorMaxTorque": 0.484, "motorMaxVelocity": 4500, "gearing": 6.12, "wheelbase": 0.552, @@ -22,7 +22,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 17 }, { "x": 1.7956033945083618, @@ -31,7 +31,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 12 + "controlIntervalCount": 14 }, { "x": 2.8381898403167725, @@ -40,7 +40,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 18 }, { "x": 1.6989272832870483, @@ -49,7 +49,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 11 + "controlIntervalCount": 13 }, { "x": 2.8705179691314697, @@ -58,7 +58,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 + "controlIntervalCount": 13 }, { "x": 2.0850512981414795, @@ -67,7 +67,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 10 + "controlIntervalCount": 11 }, { "x": 2.531447649002075, @@ -84,10 +84,9 @@ "x": 0.7470832467079163, "y": 6.625458717346191, "heading": 1.056495497952334, - "angularVelocity": -1.4679265201702596e-22, - "velocityX": 9.483029461049396e-23, - "velocityY": 5.2317038665130075e-22, - "timestamp": 0, + "angularVelocity": -2.814922481994462e-23, + "velocityX": 3.0229602263614203e-24, + "velocityY": -3.0164882691321217e-22, "moduleForcesX": [ 0, 0, @@ -99,1606 +98,1895 @@ 0, 0, 0 - ] + ], + "timestamp": 0 }, { - "x": 0.7547543197229795, - "y": 6.625912789866557, - "heading": 1.0484306824677847, - "angularVelocity": -0.1891115926088127, - "velocityX": 0.17987873841334215, - "velocityY": 0.010647531571001236, - "timestamp": 0.04264580173692322, + "x": 0.7539026198838662, + "y": 6.625839605524516, + "heading": 1.0492729313853104, + "angularVelocity": -0.15166849427224313, + "velocityX": 0.14320173471285522, + "velocityY": 0.00799836677952159, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 57.85992865453149, + 55.57487201027826, + 51.318064652077474, + 58.039260452127415 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 6.907999308730754, + -17.49184693835158, + 27.569744910492076, + -4.542101533123827 + ], + "timestamp": 0.047620744187380236 }, { - "x": 0.7701156825333854, - "y": 6.626850739979297, - "heading": 1.0325597341852575, - "angularVelocity": -0.37215734342229334, - "velocityX": 0.3602080904743721, - "velocityY": 0.021993961293704876, - "timestamp": 0.08529160347384644, + "x": 0.7675577009381795, + "y": 6.626628550621342, + "heading": 1.0350515766053865, + "angularVelocity": -0.29863781052990607, + "velocityX": 0.2867464859554202, + "velocityY": 0.01656725677619831, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 57.88100930307201, + 55.714508532181725, + 51.61400892290664, + 58.11626141362601 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 6.706391089887608, + -17.030469565022152, + 27.005128287026007, + -3.349638557212904 + ], + "timestamp": 0.09524148837476047 }, { - "x": 0.7931884474144321, - "y": 6.628306307811922, - "heading": 1.0091781276812968, - "angularVelocity": -0.5482745206245366, - "velocityX": 0.5410325035833482, - "velocityY": 0.03413156215481, - "timestamp": 0.12793740521076966, + "x": 0.7880663188184851, + "y": 6.627856082150549, + "heading": 1.014085545158225, + "angularVelocity": -0.44027097444473756, + "velocityX": 0.4306656317592878, + "velocityY": 0.0257772437233815, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 57.91162551157599, + 55.863804025506134, + 51.9553282692449, + 58.17751046150169 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 6.409362353495803, + -16.520382189151235, + 26.3352269347313, + -1.8953822815394896 + ], + "timestamp": 0.1428622325621407 }, { - "x": 0.8239951854040302, - "y": 6.630316938342184, - "heading": 0.9786149775936921, - "angularVelocity": -0.7166742995276547, - "velocityX": 0.7223861842167063, - "velocityY": 0.0471472090656353, - "timestamp": 0.17058320694769288, + "x": 0.8154474139272968, + "y": 6.629555951163622, + "heading": 0.9866565513822839, + "angularVelocity": -0.5759883480193475, + "velocityX": 0.5749825118454964, + "velocityY": 0.03569597750058087, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 57.94827042148509, + 56.03347528236485, + 52.34203591069722, + 58.203278502637744 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 6.035406626296778, + -15.918521504125732, + 25.54970777274484, + -0.23510536726652978 + ], + "timestamp": 0.19048297674952094 }, { - "x": 0.8625597183369016, - "y": 6.63292352204424, - "heading": 0.9412340847359876, - "angularVelocity": -0.8765433251390755, - "velocityX": 0.9042984622676677, - "velocityY": 0.06112169535784003, - "timestamp": 0.2132290086846161, + "x": 0.8497208018135379, + "y": 6.6317649710027196, + "heading": 0.9530739033866412, + "angularVelocity": -0.7052104827152668, + "velocityX": 0.7197155036338936, + "velocityY": 0.04638776392079282, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 57.987036343437325, + 56.235225818874625, + 52.774229558093566, + 58.17795169052109 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 5.608046706017465, + -15.169403162610594, + 24.63487493770155, + 1.560678231687874 + ], + "timestamp": 0.2381037209369012 }, { - "x": 0.9089072743014975, - "y": 6.636170316792377, - "heading": 0.8974431328952391, - "angularVelocity": -1.0268525870586154, - "velocityX": 1.086802312933599, - "velocityY": 0.07613398308622592, - "timestamp": 0.2558748104215393, + "x": 0.8909071759065187, + "y": 6.634522965587952, + "heading": 0.9136797710629836, + "angularVelocity": -0.8272473056835844, + "velocityX": 0.8648830419558078, + "velocityY": 0.057915822868710255, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 58.02382176629463, + 56.48076577385563, + 53.2518632286912, + 58.094056700540655 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 5.158035303537173, + -14.201163704424598, + 23.572862374375507, + 3.4055289208270785 + ], + "timestamp": 0.2857244651242814 }, { - "x": 0.9630650383734942, - "y": 6.64010548361246, - "heading": 0.8477159062035999, - "angularVelocity": -1.1660521004717048, - "velocityX": 1.269943625543481, - "velocityY": 0.0922755971234286, - "timestamp": 0.29852061215846254, + "x": 0.9390283845014781, + "y": 6.637873093075656, + "heading": 0.8688628769210628, + "angularVelocity": -0.9411212467737456, + "velocityX": 1.0105093781317218, + "velocityY": 0.07035017081046684, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 58.05445637477262, + 56.77945893480036, + 53.77458368672696, + 57.955802544053434 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 4.726988301952687, + -12.920128155574607, + 22.340010342191754, + 5.198389651263037 + ], + "timestamp": 0.3333452093116616 }, { - "x": 1.0250628809506594, - "y": 6.644782896884854, - "heading": 0.7926327969571765, - "angularVelocity": -1.2916420140539158, - "velocityX": 1.4537853681265616, - "velocityY": 0.10968050973102496, - "timestamp": 0.34116641389538577, + "x": 0.99410787725357, + "y": 6.641862960540765, + "heading": 0.8190840274654485, + "angularVelocity": -1.0453185960249207, + "velocityX": 1.156628139521772, + "velocityY": 0.08378423170811934, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 58.0745823349482, + 57.13326696521539, + 54.34162138758188, + 57.78094210439999 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 4.373537369916966, + -11.203858122136547, + 20.903786932368053, + 6.827139466144138 + ], + "timestamp": 0.38096595349904183 }, { - "x": 1.094933572481574, - "y": 6.650266047499182, - "heading": 0.7329446248446481, - "angularVelocity": -1.3996259814913892, - "velocityX": 1.6383955438788211, - "velocityY": 0.1285742181177263, - "timestamp": 0.383812215632309, + "x": 1.0561709558415282, + "y": 6.646547058923123, + "heading": 0.764916720396532, + "angularVelocity": -1.1374729226359126, + "velocityX": 1.3032782172355293, + "velocityY": 0.09836256157458603, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 58.07884230896552, + 57.52664576524135, + 54.95165712906377, + 57.59988532120776 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 4.18414702790263, + -8.893502901162272, + 19.216964845382883, + 8.173241281714256 + ], + "timestamp": 0.42858669768642205 }, { - "x": 1.1727109614146445, - "y": 6.6566348315708534, - "heading": 0.6696642878742429, - "angularVelocity": -1.4838585369029722, - "velocityX": 1.8237994307826475, - "velocityY": 0.149341407882537, - "timestamp": 0.4264580173692322, + "x": 1.1252440265364065, + "y": 6.651991066133351, + "heading": 0.707105481038992, + "angularVelocity": -1.2139927744527004, + "velocityX": 1.4504828068853088, + "velocityY": 0.11432007842645366, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 58.058063356768365, + 57.90773275166756, + 55.60238220835145, + 57.45155749012649 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 4.293210892903159, + -5.788564619162194, + 17.206213788359637, + 9.115718390676392 + ], + "timestamp": 0.47620744187380226 }, { - "x": 1.2584230921899875, - "y": 6.663995676876955, - "heading": 0.6041925063971192, - "angularVelocity": -1.5352456469457696, - "velocityX": 2.0098609308388866, - "velocityY": 0.17260421908608822, - "timestamp": 0.46910381910615545, + "x": 1.2013513334108457, + "y": 6.658278411112851, + "heading": 0.6466442567332923, + "angularVelocity": -1.2696404757513684, + "velocityX": 1.5981965039220822, + "velocityY": 0.1320295406295972, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 57.99034977388328, + 58.15656069329295, + 56.288724673225055, + 57.376165910837095 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 4.923367257090649, + -1.6522597130636139, + 14.748411409233862, + 9.532722214598236 + ], + "timestamp": 0.5238281860611825 }, { - "x": 1.3520739703395739, - "y": 6.67249503645535, - "heading": 0.5385009897728867, - "angularVelocity": -1.5403982091713377, - "velocityX": 2.19601635648234, - "velocityY": 0.19930120274971816, - "timestamp": 0.5117496208430786, + "x": 1.2845063733027953, + "y": 6.665519148313447, + "heading": 0.5848804980188779, + "angularVelocity": -1.2969927238302643, + "velocityX": 1.7461936244580183, + "velocityY": 0.15205006398272194, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 57.811915108319404, + 58.03976645514236, + 56.99574350032489, + 57.40532402483411 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 6.474842781770424, + 3.7555604314694015, + 11.617621448775878, + 9.299747156494563 + ], + "timestamp": 0.5714489302485628 }, { - "x": 1.453595786870469, - "y": 6.682338668837949, - "heading": 0.4754960223298961, - "angularVelocity": -1.4774014059264435, - "velocityX": 2.3805817312843804, - "velocityY": 0.23082301144958858, - "timestamp": 0.5543954225800019, + "x": 1.3746921013206055, + "y": 6.673861244609411, + "heading": 0.5236730878733165, + "angularVelocity": -1.285309820122082, + "velocityX": 1.8938328150216104, + "velocityY": 0.17517778099265666, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 57.310185080815664, + 57.16712059629645, + 57.669087251929504, + 57.549491540761366 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 9.74828832700739, + 10.593301460429908, + 7.3549113244132425, + 8.285418011961559 + ], + "timestamp": 0.6190696744359431 }, { - "x": 1.5626754965988896, - "y": 6.6938349914873125, - "heading": 0.42023075468632287, - "angularVelocity": -1.2959134403076285, - "velocityX": 2.5578065198848923, - "velocityY": 0.2695768910684973, - "timestamp": 0.5970412243169251, + "x": 1.4718109314175543, + "y": 6.683507923259447, + "heading": 0.46574387997473543, + "angularVelocity": -1.2164700255552203, + "velocityX": 2.0394227716140083, + "velocityY": 0.20257303439185328, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 55.65121903382453, + 54.99704715282837, + 58.07803857155044, + 57.78139771397302 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 16.565429620324096, + 18.792767490211176, + 0.9116734716158617, + 6.3514478887901085 + ], + "timestamp": 0.6666904186233233 + }, + { + "x": 1.57550342839463, + "y": 6.694752929350011, + "heading": 0.4158791410788935, + "angularVelocity": -1.0471222100106607, + "velocityX": 2.1774648579422013, + "velocityY": 0.2361367148383242, + "moduleForcesX": [ + 48.75296553631865, + 50.943800913157915, + 57.05405288058638, + 58.01396630829892 + ], + "moduleForcesY": [ + 31.297064627265303, + 27.93547289382856, + -10.403475397779616, + 3.3890463395149384 + ], + "timestamp": 0.7143111628107036 }, { - "x": 1.6778273819318938, - "y": 6.707315267421264, + "x": 1.6843562594767798, + "y": 6.707809824340588, "heading": 0.3846859068543657, - "angularVelocity": -0.8334899658172448, - "velocityX": 2.7001927655941973, - "velocityY": 0.3160985462791911, - "timestamp": 0.6396870260538483, + "angularVelocity": -0.6550345811856123, + "velocityX": 2.2858280133932967, + "velocityY": 0.27418502615581647, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 18.204183715975507, + 44.46669574639313, + 47.828415859508524, + 58.091244495239216 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 54.82018208894905, + 37.37367730553576, + -32.52805362351062, + -0.47054408407509796 + ], + "timestamp": 0.7619319069980839 }, { "x": 1.7956033945083618, "y": 6.721975326538086, "heading": 0.3846859068543657, - "angularVelocity": -2.7339775014955035e-22, - "velocityX": 2.7617258388765618, - "velocityY": 0.34376324326736624, - "timestamp": 0.6823328277907715, + "angularVelocity": -3.554059362055802e-23, + "velocityX": 2.336106605008982, + "velocityY": 0.2974649480856219, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -28.169968798994756, + 34.96729290232753, + 13.531484107057947, + 57.89422679118955 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 50.55108957029159, + 46.375637620891645, + -56.177268901226164, + -4.530739956145902 + ], + "timestamp": 0.8095526511854642 }, { - "x": 1.9702980732827642, - "y": 6.757682275421422, + "x": 1.9448813303029442, + "y": 6.750262783810755, "heading": 0.3846859068543657, - "angularVelocity": -3.55936785030336e-23, - "velocityX": 2.8736582107292628, - "velocityY": 0.587365153641545, - "timestamp": 0.7431245663914694, + "angularVelocity": -7.071920034257376e-24, + "velocityX": 2.4334235311170778, + "velocityY": 0.4611221597912817, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 29.383140382480864, + 29.383140382480864, + 29.383140382480864, + 29.383140382480864 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 49.41342702102415, + 49.41342702102415, + 49.41342702102415, + 49.41342702102415 + ], + "timestamp": 0.8708974741830685 }, { - "x": 2.128777504610528, - "y": 6.795024747259092, + "x": 2.0829613480368456, + "y": 6.781960040020516, "heading": 0.3846859068543657, - "angularVelocity": -3.5593719407474505e-23, - "velocityX": 2.606923818525971, - "velocityY": 0.6142688578615821, - "timestamp": 0.8039163049921673, + "angularVelocity": -7.072310064547998e-24, + "velocityX": 2.250882975720603, + "velocityY": 0.5167062950201712, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -55.1149217223832, + -55.11492172238321, + -55.1149217223832, + -55.1149217223832 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 16.78265553368512, + 16.782655533685116, + 16.78265553368512, + 16.78265553368512 + ], + "timestamp": 0.9322422971806729 }, { - "x": 2.2710818815122873, - "y": 6.830372862436407, + "x": 2.209277628517863, + "y": 6.812924670308866, "heading": 0.3846859068543657, - "angularVelocity": -3.5593679346398325e-23, - "velocityX": 2.340850585578186, - "velocityY": 0.5814624814318021, - "timestamp": 0.8647080435928651, + "angularVelocity": -7.072257174941514e-24, + "velocityX": 2.0591188352756387, + "velocityY": 0.5047635444242788, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -57.89982158665208, + -57.89982158665208, + -57.89982158665208, + -57.89982158665208 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -3.6059042485813855, + -3.605904248581385, + -3.6059042485813873, + -3.605904248581387 + ], + "timestamp": 0.9935871201782772 }, { - "x": 2.397370829093984, - "y": 6.862701695876428, + "x": 2.323926038493632, + "y": 6.842067117238504, "heading": 0.3846859068543657, - "angularVelocity": -3.5593640063590435e-23, - "velocityX": 2.0774031223421394, - "velocityY": 0.5317964938026807, - "timestamp": 0.925499782193563, + "angularVelocity": -7.072361618794593e-24, + "velocityX": 1.8689174468764331, + "velocityY": 0.4750595976253187, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -57.42797599329124, + -57.42797599329124, + -57.42797599329124, + -57.42797599329124 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -8.968586181381475, + -8.968586181381475, + -8.968586181381475, + -8.968586181381475 + ], + "timestamp": 1.0549319431758817 }, { - "x": 2.507741549731751, - "y": 6.891534791754925, + "x": 2.4269824153429544, + "y": 6.868901069621197, "heading": 0.3846859068543657, - "angularVelocity": -3.5593769660186765e-23, - "velocityX": 1.8155545996590319, - "velocityY": 0.47429299674884, - "timestamp": 0.9862915207942609, + "angularVelocity": -7.072821947364684e-24, + "velocityX": 1.679952305891357, + "velocityY": 0.4374281491323442, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -57.0547127515352, + -57.0547127515352, + -57.0547127515352, + -57.0547127515352 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -11.362156390317617, + -11.362156390317615, + -11.362156390317617, + -11.362156390317617 + ], + "timestamp": 1.116276766173486 }, { - "x": 2.6022568329985316, - "y": 6.916597570791243, + "x": 2.5184986692010405, + "y": 6.893152527814633, "heading": 0.3846859068543657, - "angularVelocity": -3.559368485205426e-23, - "velocityX": 1.5547389405588703, - "velocityY": 0.41227277938109086, - "timestamp": 1.0470832593949586, + "angularVelocity": -7.07105136075711e-24, + "velocityX": 1.491833367286101, + "velocityY": 0.3953301518920652, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -56.79921677244283, + -56.79921677244283, + -56.79921677244284, + -56.79921677244284 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -12.71075250936749, + -12.71075250936749, + -12.710752509367488, + -12.71075250936749 + ], + "timestamp": 1.1776215891710904 }, { - "x": 2.6809601695719074, - "y": 6.937711608282982, + "x": 2.5985115833830923, + "y": 6.9146459631069135, "heading": 0.3846859068543657, - "angularVelocity": -3.559368363101467e-23, - "velocityX": 1.2946386858636783, - "velocityY": 0.3473175463926733, - "timestamp": 1.1078749979956564, + "angularVelocity": -7.071515558519895e-24, + "velocityX": 1.3043140443192134, + "velocityY": 0.35037080949961336, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -56.61817333855819, + -56.61817333855819, + -56.61817333855819, + -56.61817333855819 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -13.574685533676519, + -13.574685533676519, + -13.574685533676519, + -13.574685533676519 + ], + "timestamp": 1.2389664121686947 }, { - "x": 2.7438833546228816, - "y": 6.954751696784365, + "x": 2.6670483577421855, + "y": 6.9332594017529505, "heading": 0.3846859068543657, - "angularVelocity": -3.559362874104829e-23, - "velocityX": 1.0350614491267762, - "velocityY": 0.28030270055785556, - "timestamp": 1.1686667365963541, + "angularVelocity": -7.07550113164988e-24, + "velocityX": 1.117238114156259, + "velocityY": 0.30342313721833786, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -56.48429865176341, + -56.4842986517634, + -56.48429865176341, + -56.48429865176341 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -14.175026898601002, + -14.175026898601002, + -14.175026898601004, + -14.175026898601008 + ], + "timestamp": 1.300311235166299 }, { - "x": 2.791050608826443, - "y": 6.967625143508777, + "x": 2.72412984860515, + "y": 6.948903188901549, "heading": 0.3846859068543657, - "angularVelocity": -3.5593782108627554e-23, - "velocityX": 0.775882632891501, - "velocityY": 0.21176309512990737, - "timestamp": 1.2294584751970519, + "angularVelocity": -7.06791759690408e-24, + "velocityX": 0.9305021691104057, + "velocityY": 0.2550139748420099, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -56.38164610381262, + -56.38164610381262, + -56.38164610381262, + -56.38164610381262 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -14.616298220537676, + -14.616298220537676, + -14.616298220537674, + -14.616298220537672 + ], + "timestamp": 1.3616560581639034 }, { - "x": 2.8224809815825402, - "y": 6.976260567480704, + "x": 2.769772527142252, + "y": 6.961508658865873, "heading": 0.3846859068543657, - "angularVelocity": -3.5593532451099235e-23, - "velocityX": 0.5170171717335424, - "velocityY": 0.14204930095266424, - "timestamp": 1.2902502137977496, + "angularVelocity": -7.080226941592603e-24, + "velocityX": 0.744034725454898, + "velocityY": 0.2054854729113339, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -56.30057681442044, + -56.30057681442044, + -56.30057681442043, + -56.30057681442044 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -14.954263182815012, + -14.954263182815014, + -14.954263182815021, + -14.954263182815023 + ], + "timestamp": 1.4230008811615078 + }, + { + "x": 2.8039897177250084, + "y": 6.971021542915001, + "heading": 0.3846859068543657, + "angularVelocity": -7.06236523633029e-24, + "velocityX": 0.5577844862979259, + "velocityY": 0.15507232043850186, + "moduleForcesX": [ + -56.2349957225445, + -56.2349957225445, + -56.2349957225445, + -56.2349957225445 + ], + "moduleForcesY": [ + -15.221367910729839, + -15.221367910729839, + -15.221367910729835, + -15.221367910729835 + ], + "timestamp": 1.4843457041591122 + }, + { + "x": 2.8267924145674423, + "y": 6.97739787426348, + "heading": 0.3846859068543657, + "angularVelocity": -7.077974709960036e-24, + "velocityX": 0.371713467056942, + "velocityY": 0.10394245246626038, + "moduleForcesX": [ + -56.18088340969801, + -56.18088340969801, + -56.18088340969801, + -56.18088340969801 + ], + "moduleForcesY": [ + -15.437767595508772, + -15.437767595508774, + -15.437767595508781, + -15.437767595508783 + ], + "timestamp": 1.5456905271567165 }, { "x": 2.8381898403167725, "y": 6.9806013107299805, "heading": 0.3846859068543657, - "angularVelocity": -3.55938793253341e-23, - "velocityX": 0.2584044986344835, - "velocityY": 0.07140350562744292, - "timestamp": 1.3510419523984474, + "angularVelocity": -7.070376775728643e-24, + "velocityX": 0.18579278890046133, + "velocityY": 0.05222015990861092, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -56.13549055392384, + -56.13549055392384, + -56.13549055392384, + -56.13549055392384 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -15.616639816973526, + -15.616639816973523, + -15.616639816973528, + -15.616639816973525 + ], + "timestamp": 1.6070353501543209 }, { "x": 2.8381898403167725, "y": 6.9806013107299805, "heading": 0.3846859068543657, - "angularVelocity": -5.573807154532037e-19, - "velocityX": -9.35615466765533e-16, - "velocityY": -9.896680587980253e-18, - "timestamp": 1.4118336909991451, + "angularVelocity": 1.6334100500794593e-18, + "velocityX": -2.855209079304936e-18, + "velocityY": 1.6102083009631126e-18, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -56.096876634297416, + -56.096876634297416, + -56.09687663429742, + -56.096876634297416 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -15.766962138590067, + -15.766962138590072, + -15.766962138590065, + -15.766962138590062 + ], + "timestamp": 1.6683801731519252 }, { - "x": 2.8128886138231195, - "y": 6.955100739151656, - "heading": 0.3773890772908297, - "angularVelocity": -0.08078200568400537, - "velocityX": -0.2801057369692363, - "velocityY": -0.2823126537693574, - "timestamp": 1.5021611036665428, + "x": 2.821131681761048, + "y": 6.962941949661055, + "heading": 0.37963298615970775, + "angularVelocity": -0.05715420706543545, + "velocityX": -0.1929469281557583, + "velocityY": -0.19974720368037308, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -37.095177903701845, + -39.323311531074836, + -41.803716726316495, + -43.47083407085052 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -44.94946586017272, + -43.0174583751275, + -40.605077451410324, + -38.81979329867605 + ], + "timestamp": 1.756788725279557 }, { - "x": 2.7622861408558537, - "y": 6.904099615654192, - "heading": 0.3627975933198322, - "angularVelocity": -0.16153993057112973, - "velocityX": -0.5602116951334734, - "velocityY": -0.564625089894962, - "timestamp": 1.5924885163339404, + "x": 2.7870198730695646, + "y": 6.927621262535135, + "heading": 0.36952776277295296, + "angularVelocity": -0.11430142382793766, + "velocityX": -0.38584286101911414, + "velocityY": -0.3995166335823328, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -37.072533334197914, + -39.34365755876542, + -41.75968748882494, + -43.474426866266874 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -44.962788854185376, + -42.99375240452121, + -40.64405289944275, + -38.80982680576702 + ], + "timestamp": 1.845197277407189 }, { - "x": 2.6863823744143596, - "y": 6.827597945234058, - "heading": 0.3409166257799037, - "angularVelocity": -0.24224060995125085, - "velocityX": -0.8403181736340153, - "velocityY": -0.8469374707052851, - "timestamp": 1.682815929001338, + "x": 2.7358604135298354, + "y": 6.874636621269694, + "heading": 0.35437217454755016, + "angularVelocity": -0.17142672129188716, + "velocityX": -0.5786709352040128, + "velocityY": -0.5993157900487962, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -37.04188686591134, + -39.376245866938596, + -41.69487105411659, + -43.48043465252771 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -44.98092590316717, + -42.957112494872305, + -40.70214264581303, + -38.7951513225733 + ], + "timestamp": 1.9336058295348209 }, { - "x": 2.585177249564827, - "y": 6.725595708318326, - "heading": 0.311754466827316, - "angularVelocity": -0.32284948822656157, - "velocityX": -1.1204253710064276, - "velocityY": -1.1292500682040065, - "timestamp": 1.7731433416687357, + "x": 2.6676616985406656, + "y": 6.803984339073742, + "heading": 0.33416944271042276, + "angularVelocity": -0.22851558306216296, + "velocityX": -0.7714040480010793, + "velocityY": -0.7991566482612914, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -37.00181302221145, + -39.4192239370053, + -41.60655692235178, + -43.486265185245315 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -45.00397284969973, + -42.90816343331507, + -40.78067451259032, + -38.777468393572995 + ], + "timestamp": 2.0220143816624527 }, { - "x": 2.4586707006639994, - "y": 6.598092843845294, - "heading": 0.27532262336823443, - "angularVelocity": -0.40333097542716767, - "velocityX": -1.4005332951050296, - "velocityY": -1.4115633417063895, - "timestamp": 1.8634707543361333, + "x": 2.582436328275892, + "y": 6.71565888253476, + "heading": 0.3089239458735266, + "angularVelocity": -0.2855549178144013, + "velocityX": -0.9639946386831083, + "velocityY": -0.9990600955829445, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -36.94888772357164, + -39.46875969787357, + -41.48963891451579, + -43.48713660247826 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -45.03260236713544, + -42.8483286467539, + -40.882056657335006, + -38.75974233145917 + ], + "timestamp": 2.1104229337900846 }, { - "x": 2.306862702051818, - "y": 6.445089212010339, - "heading": 0.2316358112414208, - "angularVelocity": -0.4836495459854555, - "velocityX": -1.6806415032828872, - "velocityY": -1.6938781629619528, - "timestamp": 1.953798167003531, + "x": 2.4802053222003866, + "y": 6.609651042778182, + "heading": 0.2786408401009175, + "angularVelocity": -0.34253593169233965, + "velocityX": -1.156347475614342, + "velocityY": -1.1990677056167498, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -36.873873585620636, + -39.51569610358064, + -41.33281589762392, + -43.4727953778751 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -45.06942043742109, + -42.781255183974444, + -41.011452230389395, + -38.74789240095698 + ], + "timestamp": 2.1988314859177165 }, { - "x": 2.1297534370368134, - "y": 6.266584440083468, - "heading": 0.18071175454391936, - "angularVelocity": -0.5637718959659755, - "velocityX": -1.960747682084755, - "velocityY": -1.9761971106474765, - "timestamp": 2.0441255796709283, + "x": 2.361010738415153, + "y": 6.485942489444843, + "heading": 0.2433248076280929, + "angularVelocity": -0.39946398422904045, + "velocityX": -1.3482245881954655, + "velocityY": -1.3992826525961657, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -36.74649413708812, + -39.53225346091061, + -41.10333275151675, + -43.414434944697646 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -45.1243385113088, + -42.718426403813595, + -41.18352942870808, + -38.75747804587791 + ], + "timestamp": 2.2872400380453484 }, { - "x": 1.9873417449288229, - "y": 6.088213430389112, - "heading": 0.1304328979304444, - "angularVelocity": -0.5566289914548207, - "velocityX": -1.5766165320272711, - "velocityY": -1.974716250895523, - "timestamp": 2.1344529923383257, + "x": 2.22497852013832, + "y": 6.344478967922724, + "heading": 0.20297375459292902, + "angularVelocity": -0.4564157206975939, + "velocityX": -1.5386771415558158, + "velocityY": -1.6001112801609332, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -36.40940346518017, + -39.379512733261826, + -40.64030165250597, + -43.1734908685818 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -45.25140648250694, + -42.717694890088865, + -41.4691945783061, + -38.85975194723375 + ], + "timestamp": 2.3756485901729802 }, { - "x": 1.870232723728645, - "y": 5.935342556724319, - "heading": 0.08735848478659979, - "angularVelocity": -0.47686977709234435, - "velocityX": -1.2964948041918032, - "velocityY": -1.692408419008837, - "timestamp": 2.224780405005723, + "x": 2.0952329573157353, + "y": 6.186474139637454, + "heading": 0.15831835366452565, + "angularVelocity": -0.5051027287941129, + "velocityX": -1.467568008978098, + "velocityY": -1.7872120341611801, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 16.709059309020606, + 12.736243666460043, + 17.192391398004176, + 12.953053946000171 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -39.41171649809029, + -40.61065023266987, + -37.72694053413655, + -39.04453253941978 + ], + "timestamp": 2.464057142300612 }, { - "x": 1.7784261170686546, - "y": 5.807972234685134, - "heading": 0.05147629980122907, - "angularVelocity": -0.3972457964408093, - "velocityX": -1.0163759145574802, - "velocityY": -1.410095986129178, - "timestamp": 2.3151078176731206, + "x": 1.9826977478470895, + "y": 6.045863415934052, + "heading": 0.11862394841971062, + "angularVelocity": -0.44898829682800195, + "velocityX": -1.2728995867524602, + "velocityY": -1.5904651792103528, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 37.390210370998965, + 40.475901652172276, + 41.278591550019264, + 43.990980045515705 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 44.44300054730394, + 41.677348946779276, + 40.830424502770605, + 37.92667410323451 + ], + "timestamp": 2.552465694428244 }, { - "x": 1.7119218452051854, - "y": 5.706102649506714, - "heading": 0.022777291492800653, - "angularVelocity": -0.31772202325989357, - "velocityX": -0.736257907751286, - "velocityY": -1.1277815025423439, - "timestamp": 2.405435230340518, + "x": 1.8873135024006749, + "y": 5.92277267476745, + "heading": 0.08389641882694215, + "angularVelocity": -0.3928073558159144, + "velocityX": -1.0789029245577086, + "velocityY": -1.3922945032387968, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 37.21476272459083, + 40.450247243359854, + 41.00550775325207, + 43.902218755828535 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 44.73999822966322, + 41.84966110698525, + 41.279760388849354, + 38.20121632419051 + ], + "timestamp": 2.640874246555876 }, { - "x": 1.6707198619792896, - "y": 5.629733910265785, - "heading": 0.0012553801790747192, - "angularVelocity": -0.23826555724598256, - "velocityX": -0.4561404119627787, - "velocityY": -0.8454658113853617, - "timestamp": 2.4957626430079154, + "x": 1.8090603018164058, + "y": 5.817243772643182, + "heading": 0.05413403725183304, + "angularVelocity": -0.3366459562887355, + "velocityX": -0.8851315704310847, + "velocityY": -1.1936503831882683, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 37.15959386222587, + 40.498806672558835, + 40.847090706611624, + 43.87843297066345 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 44.83556164168193, + 41.85167911227039, + 41.49451958786403, + 38.28563046961167 + ], + "timestamp": 2.7292827986835078 }, { - "x": 1.6548201302688181, - "y": 5.578866079528388, - "heading": -0.01309273794263427, - "angularVelocity": -0.15884566709086995, - "velocityX": -0.17602332714722002, - "velocityY": -0.5631494275703062, - "timestamp": 2.586090055675313, + "x": 1.747928153071767, + "y": 5.729297635459773, + "heading": 0.02933424645224599, + "angularVelocity": -0.2805134820416991, + "velocityX": -0.6914732486104376, + "velocityY": -0.9947695677273937, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 37.13549798902941, + 40.55427442990822, + 40.73097140342639, + 43.868457258457035 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 44.88031315505466, + 41.82243046412967, + 41.63740433004384, + 38.32559792736543 + ], + "timestamp": 2.8176913508111396 }, { - "x": 1.664222615534791, - "y": 5.55349918288868, - "heading": -0.020267736871838676, - "angularVelocity": -0.079433238674075, - "velocityX": 0.10409337529313177, - "velocityY": -0.2808327604066967, - "timestamp": 2.67641746834271, + "x": 1.7039110423935377, + "y": 5.658946821606213, + "heading": 0.009494945599922864, + "angularVelocity": -0.22440477052131733, + "velocityX": -0.49788294931788657, + "velocityY": -0.7957467027850181, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 37.12351320058831, + 40.60313571714578, + 40.64272563549292, + 43.86282241005115 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 44.90506437230603, + 41.789697152404564, + 41.740842496734864, + 38.349181908010294 + ], + "timestamp": 2.9060999029387715 + }, + { + "x": 1.6770049484081522, + "y": 5.606199701903595, + "heading": -0.0053851285855366264, + "angularVelocity": -0.16831034812082146, + "velocityX": -0.30433813627602885, + "velocityY": -0.5966291544563317, + "moduleForcesX": [ + 37.116682462766875, + 40.64090572081128, + 40.57759789476367, + 43.858892578344765 + ], + "moduleForcesY": [ + 44.920576626812874, + 41.76277059020072, + 41.815668830634316, + 38.365116284149615 + ], + "timestamp": 2.9945084550664034 + }, + { + "x": 1.6672069897589075, + "y": 5.571062249143735, + "heading": -0.015306267888284994, + "angularVelocity": -0.11221922612673975, + "velocityX": -0.1108259146140006, + "velocityY": -0.3974440471452618, + "moduleForcesX": [ + 37.111681394515514, + 40.665682324503166, + 40.533570764783825, + 43.855832002950336 + ], + "moduleForcesY": [ + 44.93173600136994, + 41.745649789594474, + 41.866559403537146, + 38.37680279508834 + ], + "timestamp": 3.0829170071940353 + }, + { + "x": 1.6745149985934764, + "y": 5.553538932287859, + "heading": -0.0202677368718387, + "angularVelocity": -0.05611978552019538, + "velocityX": 0.08266178620387737, + "velocityY": -0.1982083908644825, + "moduleForcesX": [ + 37.106714226875006, + 40.67652070236513, + 40.509643285006895, + 43.85333935847226 + ], + "moduleForcesY": [ + 44.94109264735369, + 41.7403427171622, + 41.89586596082861, + 38.38580762158759 + ], + "timestamp": 3.171325559321667 }, { "x": 1.6989272832870483, "y": 5.553633213043213, - "heading": -0.020267736871838676, - "angularVelocity": -3.9423001343697265e-19, - "velocityX": 0.38420969589870047, - "velocityY": 0.0014838259015203296, - "timestamp": 2.7667448810101076, + "heading": -0.020267736871838703, + "angularVelocity": -3.99357376917391e-18, + "velocityX": 0.2761303528456057, + "velocityY": 0.00106642121248368, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 37.10080834917557, + 40.672885944439045, + 40.50526475056877, + 43.851223740402496 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 44.95004105053435, + 41.747969933406075, + 41.90488405097919, + 38.39302718242146 + ], + "timestamp": 3.259734111449299 }, { - "x": 1.765875287477924, - "y": 5.553891767639138, - "heading": -0.020267736871838676, - "angularVelocity": -1.268800151544e-19, - "velocityX": 0.7684200358337809, - "velocityY": 0.0029676542903228132, - "timestamp": 2.8538691113384247, + "x": 1.7476866611230668, + "y": 5.553821522782783, + "heading": -0.0202677368718387, + "angularVelocity": 5.910944865182772e-20, + "velocityX": 0.5533641655006356, + "velocityY": 0.0021371040098882763, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 58.27560878459361, + 58.27560878459361, + 58.27560878459361, + 58.27560878459361 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 0.22506162302642355, + 0.22506162302642355, + 0.22506162302642416, + 0.22506162302642413 + ], + "timestamp": 3.347848559178138 }, { - "x": 1.8662973205168285, - "y": 5.554279599636344, - "heading": -0.02026773687183868, - "angularVelocity": -1.2688001508205996e-19, - "velocityX": 1.1526303608132487, - "velocityY": 0.004451482621366332, - "timestamp": 2.9409933416667418, + "x": 1.8208713864961603, + "y": 5.554104163726683, + "heading": -0.0202677368718387, + "angularVelocity": 5.910945255784143e-20, + "velocityX": 0.8305644222875951, + "velocityY": 0.003207657213826796, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 58.268555212659614, + 58.268555212659614, + 58.268555212659614, + 58.268555212659614 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 0.2250343819837174, + 0.2250343819837174, + 0.2250343819837181, + 0.2250343819837181 + ], + "timestamp": 3.435963006906977 }, { - "x": 2.000193380141947, - "y": 5.554796709026097, - "heading": -0.02026773687183868, - "angularVelocity": -1.2688001524330257e-19, - "velocityX": 1.53684065983192, - "velocityY": 0.005935310852148713, - "timestamp": 3.028117571995059, + "x": 1.9184768732294062, + "y": 5.554481118163, + "heading": -0.0202677368718387, + "angularVelocity": 5.910945067058074e-20, + "velocityX": 1.1077126311182772, + "velocityY": 0.004278009407466852, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 58.25761453298922, + 58.25761453298922, + 58.25761453298922, + 58.25761453298922 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 0.22499212884943065, + 0.22499212884943065, + 0.22499212884943104, + 0.22499212884943104 + ], + "timestamp": 3.524077454635816 }, { - "x": 2.167563461458264, - "y": 5.555443095789492, - "heading": -0.020267736871838683, - "angularVelocity": -1.2688001514027187e-19, - "velocityX": 1.9210509026662645, - "velocityY": 0.0074191388659460445, - "timestamp": 3.115241802323376, + "x": 2.040495046553733, + "y": 5.554952354906809, + "heading": -0.0202677368718387, + "angularVelocity": 5.910945105533479e-20, + "velocityX": 1.3847692003906387, + "velocityY": 0.005348007687211211, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 58.238351546996455, + 58.238351546996455, + 58.238351546996455, + 58.238351546996455 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 0.22491773479362803, + 0.22491773479362803, + 0.2249177347936283, + 0.2249177347936283 + ], + "timestamp": 3.6121919023646547 }, { - "x": 2.3684075459339367, - "y": 5.556218759854957, - "heading": -0.020267736871838683, - "angularVelocity": -1.2688001518380097e-19, - "velocityX": 2.3052609327946625, - "velocityY": 0.00890296605826855, - "timestamp": 3.202366032651693, + "x": 2.186907919553257, + "y": 5.555517804492261, + "heading": -0.0202677368718387, + "angularVelocity": 5.910945033275698e-20, + "velocityX": 1.6616216383729834, + "velocityY": 0.006417217607633379, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 58.195442368336856, + 58.195442368336856, + 58.195442368336856, + 58.195442368336856 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 0.22475201864594074, + 0.22475201864594074, + 0.22475201864594233, + 0.22475201864594233 + ], + "timestamp": 3.7003063500934936 }, { - "x": 2.5357776764404463, - "y": 5.556865146808326, - "heading": -0.020267736871838683, - "angularVelocity": -1.2688001516210267e-19, - "velocityX": 1.921051467264574, - "velocityY": 0.007419141046436553, - "timestamp": 3.28949026298001, + "x": 2.3576403392711733, + "y": 5.556177176677063, + "heading": -0.0202677368718387, + "angularVelocity": 5.910945066362e-20, + "velocityX": 1.9376211747172734, + "velocityY": 0.007483133604046069, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 58.016159178761185, + 58.016159178761185, + 58.016159178761185, + 58.016159178761185 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 0.22405962320866032, + 0.2240596232086603, + 0.22405962320865966, + 0.22405962320865963 + ], + "timestamp": 3.7884207978223325 }, { - "x": 2.669673789778629, - "y": 5.5573822564055195, - "heading": -0.020267736871838686, - "angularVelocity": -1.268800151621384e-19, - "velocityX": 1.5368412763431272, - "velocityY": 0.005935313233127921, - "timestamp": 3.376614493308327, + "x": 2.504136867684685, + "y": 5.556742949341478, + "heading": -0.020267736871838696, + "angularVelocity": 5.910945066536768e-20, + "velocityX": 1.6625710333489974, + "velocityY": 0.006420884190937289, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -57.81659272013044, + -57.81659272013044, + -57.81659272013044, + -57.81659272013044 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -0.22328889336099703, + -0.22328889336099703, + -0.22328889336099597, + -0.22328889336099597 + ], + "timestamp": 3.8765352455511715 }, { - "x": 2.7700958781366665, - "y": 5.557770088616369, - "heading": -0.020267736871838686, - "angularVelocity": -1.2688001516248343e-19, - "velocityX": 1.1526309957586889, - "velocityY": 0.004451485073538937, - "timestamp": 3.463738723636644, + "x": 2.626247488507551, + "y": 5.557214543119464, + "heading": -0.020267736871838696, + "angularVelocity": 5.910945018736598e-20, + "velocityX": 1.3858183756498825, + "velocityY": 0.005352059624061109, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -58.17446817080217, + -58.17446817080217, + -58.17446817080217, + -58.17446817080217 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -0.22467101585528326, + -0.2246710158552833, + -0.22467101585528407, + -0.22467101585528407 + ], + "timestamp": 3.9646496932800104 }, { - "x": 2.8370439384630357, - "y": 5.558028643429091, - "heading": -0.02026773687183869, - "angularVelocity": -1.268800151622008e-19, - "velocityX": 0.7684206801492988, - "velocityY": 0.0029676567786828525, - "timestamp": 3.550862953964961, + "x": 2.7239485756809234, + "y": 5.557591866766686, + "heading": -0.020267736871838696, + "angularVelocity": 5.910945114498727e-20, + "velocityX": 1.108797588722742, + "velocityY": 0.0042821995364842646, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -58.2308299610416, + -58.2308299610416, + -58.2308299610416, + -58.2308299610416 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -0.2248886862709617, + -0.22488868627096165, + -0.22488868627095934, + -0.2248886862709593 + ], + "timestamp": 4.052764141008849 + }, + { + "x": 2.7972305085837634, + "y": 5.557874883128104, + "heading": -0.020267736871838696, + "angularVelocity": 5.910945064257137e-20, + "velocityX": 0.8316676185539519, + "velocityY": 0.003211917780938857, + "moduleForcesX": [ + -58.253780696434475, + -58.253780696434475, + -58.253780696434475, + -58.253780696434475 + ], + "moduleForcesY": [ + -0.2249773224922699, + -0.2249773224922699, + -0.22497732249226998, + -0.22497732249226998 + ], + "timestamp": 4.140878588737688 + }, + { + "x": 2.846088065780519, + "y": 5.558063572038424, + "heading": -0.020267736871838696, + "angularVelocity": 5.910945075970797e-20, + "velocityX": 0.5544783909570489, + "velocityY": 0.0021414071719635906, + "moduleForcesX": [ + -58.266236834685465, + -58.266236834685465, + -58.266236834685465, + -58.266236834685465 + ], + "moduleForcesY": [ + -0.22502542835937098, + -0.22502542835937098, + -0.22502542835937084, + -0.22502542835937084 + ], + "timestamp": 4.228993036466527 }, { "x": 2.8705179691314697, "y": 5.558157920837402, - "heading": -0.02026773687183869, - "angularVelocity": -1.2688001484574509e-19, - "velocityX": 0.3842103458738335, - "velocityY": 0.0014838284117379952, - "timestamp": 3.6379871842932783, + "heading": -0.020267736871838693, + "angularVelocity": 5.910945064090654e-20, + "velocityX": 0.2772519601567551, + "velocityY": 0.0010707528834367668, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -58.27405709046732, + -58.27405709046732, + -58.27405709046732, + -58.27405709046732 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -0.2250556303511726, + -0.22505563035117257, + -0.2250556303511722, + -0.2250556303511722 + ], + "timestamp": 4.317107484195366 }, { "x": 2.8705179691314697, "y": 5.558157920837402, "heading": -0.020267736871838693, - "angularVelocity": -3.4830373991096207e-20, - "velocityX": 7.082195607551611e-16, - "velocityY": 1.9869518370590596e-18, - "timestamp": 3.7251114146215953, + "angularVelocity": 1.9702981537661184e-20, + "velocityX": 1.653032544870731e-18, + "velocityY": -5.96028342388569e-21, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -58.279423458932314, + -58.279423458932314, + -58.279423458932314, + -58.279423458932314 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -0.2250763553787051, + -0.22507635537870505, + -0.2250763553787032, + -0.22507635537870316 + ], + "timestamp": 4.4052219319242045 }, { - "x": 2.8404610494153317, - "y": 5.534143912451309, - "heading": -0.03485317920513617, - "angularVelocity": -0.15571220333723793, - "velocityX": -0.32088359664250343, - "velocityY": -0.25637029521011345, - "timestamp": 3.8187806455534106, + "x": 2.8439249599428864, + "y": 5.537645538216833, + "heading": -0.032906727963708735, + "angularVelocity": -0.12199820814045388, + "velocityX": -0.25668975050996623, + "velocityY": -0.1979963358001305, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -40.264650278600634, + -45.64387186701001, + -46.79690535956238, + -50.862712927454936 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -42.14146610565898, + -36.25113454290553, + -34.737790742143396, + -28.463961726608957 + ], + "timestamp": 4.508821741171559 }, { - "x": 2.7804008780683063, - "y": 5.486049425013135, - "heading": -0.06402371208298976, - "angularVelocity": -0.3114206510256051, - "velocityX": -0.6411942400887777, - "velocityY": -0.5134502222312821, - "timestamp": 3.912449876485226, + "x": 2.790863562177045, + "y": 5.496466451968379, + "heading": -0.058207005960777755, + "angularVelocity": -0.2442116272305314, + "velocityX": -0.512176597151375, + "velocityY": -0.39748225935566833, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -40.004992650057915, + -45.57335006506032, + -46.39579797648287, + -50.73375960353931 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -42.381446517676004, + -36.332917243845905, + -35.26208895782567, + -28.68315856947041 + ], + "timestamp": 4.612421550418913 }, { - "x": 2.690426928059957, - "y": 5.4137641868079385, - "heading": -0.10777934842034757, - "angularVelocity": -0.46712923659220595, - "velocityX": -0.9605496822520554, - "velocityY": -0.7717073951190555, - "timestamp": 4.006119107417041, + "x": 2.7115257184978163, + "y": 5.434386408788842, + "heading": -0.09620042670910543, + "angularVelocity": -0.36673253574835096, + "velocityX": -0.7658107119657117, + "velocityY": -0.5992293193447437, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -39.621387787708365, + -45.46476663407919, + -45.77146561371144, + -50.52532460582109 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -42.73042746349606, + -36.458316770771056, + -36.05441082201013, + -29.03347681690111 + ], + "timestamp": 4.716021359666268 }, { - "x": 2.5707182617497417, - "y": 5.317070431884298, - "heading": -0.1661422083280734, - "angularVelocity": -0.6230739734610408, - "velocityX": -1.2779934789616767, - "velocityY": -1.0322894077568259, - "timestamp": 4.099788338348856, + "x": 2.606245266180635, + "y": 5.351008207802818, + "heading": -0.14695247874407716, + "angularVelocity": -0.4898855741500109, + "velocityX": -1.0162224533233901, + "velocityY": -0.804810371676945, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -38.975029944558344, + -45.212668298575124, + -44.75754242271995, + -50.13326797035515 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -43.30431649298782, + -36.752852470976016, + -37.281821264300945, + -29.679468474626695 + ], + "timestamp": 4.819621168913622 }, { - "x": 2.4218123673171084, - "y": 5.195334657114955, - "heading": -0.239237988952977, - "angularVelocity": -0.7803606360141083, - "velocityX": -1.5896991247961374, - "velocityY": -1.299634613824878, - "timestamp": 4.193457569280671, + "x": 2.475745367457033, + "y": 5.245512799725402, + "heading": -0.2106299193340097, + "angularVelocity": -0.6146482416574408, + "velocityX": -1.2596538514083622, + "velocityY": -1.0182973196942513, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -37.59665158673427, + -44.46910269089497, + -42.8065712580459, + -49.21428672911071 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -44.47297089839765, + -37.612897024695, + -39.459327933877574, + -31.127055280117744 + ], + "timestamp": 4.923220978160977 + }, + { + "x": 2.3227167278281233, + "y": 5.1152633537373084, + "heading": -0.28781914697582284, + "angularVelocity": -0.745071136738453, + "velocityX": -1.4771131408508664, + "velocityY": -1.2572363495101748, + "moduleForcesX": [ + -32.57681783967148, + -41.03171735775828, + -36.4212961306049, + -45.48318507972898 + ], + "moduleForcesY": [ + -48.168748844471274, + -41.22246689674932, + -45.27956258136952, + -36.203178634964225 + ], + "timestamp": 5.026820787408331 }, { - "x": 2.2812263727256172, - "y": 5.035989894533815, - "heading": -0.32521378731763706, - "angularVelocity": -0.9178659577897541, - "velocityX": -1.500877002970466, - "velocityY": -1.7011430647608603, - "timestamp": 4.287126800212486, + "x": 2.203106950739979, + "y": 4.983538065028958, + "heading": -0.35945125169862574, + "angularVelocity": -0.691430855357799, + "velocityX": -1.1545366536589345, + "velocityY": -1.2714819618426536, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 57.768566863317616, + 57.810702012324775, + 57.59277430823837, + 57.5140897196349 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 0.8932781027881612, + -0.9152785781841101, + -4.374216376079367, + -5.791336644532758 + ], + "timestamp": 5.130420596655686 }, { - "x": 2.1719619380449973, - "y": 4.899006132834784, - "heading": -0.39709906634612513, - "angularVelocity": -0.7674374852166317, - "velocityX": -1.1664922792005954, - "velocityY": -1.4624200533763867, - "timestamp": 4.3807960311443015, + "x": 2.1130561975087887, + "y": 4.867723147947118, + "heading": -0.4197404118462817, + "angularVelocity": -0.5819427717642794, + "velocityX": -0.8692173652191356, + "velocityY": -1.1179066633734913, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 47.18108856870023, + 51.63439597749474, + 50.708340450422064, + 54.51833525481436 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 34.07484296534565, + 26.874618698432997, + 28.520495989083397, + 20.35730229951914 + ], + "timestamp": 5.23402040590304 }, { - "x": 2.0934918282245056, - "y": 4.785094183316171, - "heading": -0.4547023997403661, - "angularVelocity": -0.6149653714587694, - "velocityX": -0.8377362452950287, - "velocityY": -1.2161085170170074, - "timestamp": 4.474465262076117, + "x": 2.051552972727542, + "y": 4.7695988413626225, + "heading": -0.46820764413999405, + "angularVelocity": -0.46783128893598946, + "velocityX": -0.5936615639359138, + "velocityY": -0.9471475603803009, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 45.02951499832376, + 50.355649146304515, + 48.32912609649593, + 53.34564661451518 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 36.95043417621518, + 29.289508389016593, + 32.4935418039393, + 23.38253676579243 + ], + "timestamp": 5.337620215150395 }, { - "x": 2.045638672399353, - "y": 4.694485163192149, - "heading": -0.4979506864820333, - "angularVelocity": -0.46171284114790145, - "velocityX": -0.5108737986755416, - "velocityY": -0.9673296046380411, - "timestamp": 4.568134493007932, + "x": 2.018185626236221, + "y": 4.689823401404505, + "heading": -0.5046715625693844, + "angularVelocity": -0.35196897266798394, + "velocityX": -0.32207922711183234, + "velocityY": -0.7700346220488296, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 44.230946349601425, + 49.9139754803189, + 47.23993285880785, + 52.83351377397607 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 37.9315483504293, + 30.06878585422672, + 34.0958495054322, + 24.563695274073133 + ], + "timestamp": 5.441220024397749 }, { - "x": 2.028313474321192, - "y": 4.627293480949165, - "heading": -0.5268031269208548, - "angularVelocity": -0.3080247393065947, - "velocityX": -0.18496146392802787, - "velocityY": -0.7173292827811862, - "timestamp": 4.661803723939747, + "x": 2.0127316672603737, + "y": 4.628738886692783, + "heading": -0.5290344166885111, + "angularVelocity": -0.23516311753970603, + "velocityX": -0.05264448858998858, + "velocityY": -0.589619953506647, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 43.82380015207529, + 49.697676016896175, + 46.61701661238326, + 52.544050328156345 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 38.41604200389433, + 30.441960825176444, + 34.962319678036984, + 25.200743372075255 + ], + "timestamp": 5.544819833645104 }, { - "x": 2.041462835791116, - "y": 4.583587383113917, + "x": 2.0350519350972927, + "y": 4.586554708774609, "heading": -0.5412356183232561, - "angularVelocity": -0.15407931995200425, - "velocityX": 0.14038079889324448, - "velocityY": -0.46660037026527035, - "timestamp": 4.755472954871562, + "angularVelocity": -0.117772433399114, + "velocityX": 0.2154469974324671, + "velocityY": -0.40718393426242, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 43.56673373099433, + 49.55277078584493, + 46.24089930341583, + 52.361530761045266 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 38.71631589559412, + 30.687678310009126, + 35.47018296719196, + 25.592429826745295 + ], + "timestamp": 5.648419642892458 }, { "x": 2.0850512981414795, "y": 4.563412189483643, "heading": -0.5412356183232561, - "angularVelocity": -1.875746881389731e-20, - "velocityX": 0.465344509789913, - "velocityY": -0.2153876297432254, - "timestamp": 4.849142185803378, + "angularVelocity": 2.0482990469860628e-24, + "velocityX": 0.48262022302385593, + "velocityY": -0.22338380214302983, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 43.37104440974198, + 49.42269040291651, + 46.02875544788031, + 52.24276301802586 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 38.941407034185524, + 30.903759210330502, + 35.75302989110585, + 25.843937402615378 + ], + "timestamp": 5.7520194521398125 }, { - "x": 2.1256328053913753, - "y": 4.544628783911912, + "x": 2.1250808762861264, + "y": 4.544884247765605, "heading": -0.5412356183232561, - "angularVelocity": 1.7413793657350135e-22, - "velocityX": 0.6980165405938983, - "velocityY": -0.3230813408929243, - "timestamp": 4.90728050329277, + "angularVelocity": 4.429972765944174e-25, + "velocityX": 0.6566001115017817, + "velocityY": -0.30391148650137534, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 52.857513824275856, + 52.857513824275856, + 52.857513824275856, + 52.857513824275856 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -24.465432335003626, + -24.465432335003626, + -24.465432335003626, + -24.465432335003626 + ], + "timestamp": 5.812984379564346 }, { - "x": 2.1797414712659067, - "y": 4.519584247990461, + "x": 2.1757132373979986, + "y": 4.521448741342388, "heading": -0.5412356183232561, - "angularVelocity": 1.7413789738422444e-22, - "velocityX": 0.930688540899089, - "velocityY": -0.4307750379260645, - "timestamp": 4.965418820782163, + "angularVelocity": 4.426850844156769e-25, + "velocityX": 0.8305162205687572, + "velocityY": -0.38440965015868744, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 52.83813675072153, + 52.83813675072153, + 52.83813675072153, + 52.83813675072153 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -24.45646353477834, + -24.45646353477834, + -24.45646353477834, + -24.45646353477834 + ], + "timestamp": 5.87394930698888 }, { - "x": 2.2473772913410164, - "y": 4.488278583766992, + "x": 2.2369405764562798, + "y": 4.493109282816916, "heading": -0.5412356183232561, - "angularVelocity": 1.7413788470930633e-22, - "velocityX": 1.1633604651089158, - "velocityY": -0.5384686997379875, - "timestamp": 5.023557138271555, + "angularVelocity": 4.433094585405618e-25, + "velocityX": 1.0043043048656628, + "velocityY": -0.4648485567468957, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 52.79924104206797, + 52.799241042067976, + 52.79924104206797, + 52.799241042067976 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -24.438460411677465, + -24.438460411677465, + -24.43846041167746, + -24.438460411677465 + ], + "timestamp": 5.934914234413414 }, { - "x": 2.328540234996748, - "y": 4.450711805414148, + "x": 2.308739248778686, + "y": 4.459876816278918, "heading": -0.5412356183232561, - "angularVelocity": 1.7413787765697518e-22, - "velocityX": 1.3960318626444466, - "velocityY": -0.646162117775404, - "timestamp": 5.081695455760948, + "angularVelocity": 4.429972551089785e-25, + "velocityX": 1.1777045484271702, + "velocityY": -0.5451079488142321, "moduleForcesX": [ - 0, - 0, - 0, - 0 + 52.681409623665104, + 52.681409623665104, + 52.681409623665104, + 52.681409623665104 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + -24.38392139942926, + -24.38392139942926, + -24.38392139942926, + -24.38392139942926 + ], + "timestamp": 5.995879161837948 }, { - "x": 2.396176035002005, - "y": 4.419406150480137, + "x": 2.372336860863181, + "y": 4.430440261992736, "heading": -0.5412356183232561, - "angularVelocity": 1.7413789176163748e-22, - "velocityX": 1.1633601199002148, - "velocityY": -0.5384685399559715, - "timestamp": 5.1398337732503405, + "angularVelocity": 4.429972551089667e-25, + "velocityX": 1.0431835937674017, + "velocityY": -0.482844079862508, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -40.86934002998661, + -40.86934002998661, + -40.86934002998661, + -40.86934002998661 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 18.91663078221956, + 18.91663078221956, + 18.91663078221956, + 18.91663078221956 + ], + "timestamp": 6.056844089262482 }, { - "x": 2.45028467878785, - "y": 4.394361624782573, + "x": 2.4253616014568182, + "y": 4.405897427696982, "heading": -0.5412356183232561, - "angularVelocity": 1.7413789463080967e-22, - "velocityX": 0.930688160965733, - "velocityY": -0.4307748620715239, - "timestamp": 5.197972090739733, + "angularVelocity": 4.43092756165992e-25, + "velocityX": 0.8697581188671928, + "velocityY": -0.4025730093115453, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -52.68907525585909, + -52.68907525585909, + -52.68907525585909, + -52.68907525585909 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 24.38746948544725, + 24.38746948544725, + 24.38746948544725, + 24.38746948544725 + ], + "timestamp": 6.117809016687016 }, { - "x": 2.490866163227744, - "y": 4.375578229768594, + "x": 2.4677909785651333, + "y": 4.386258723952594, "heading": -0.5412356183232561, - "angularVelocity": 1.741380781979936e-22, - "velocityX": 0.6980161482536459, - "velocityY": -0.3230811592957737, - "timestamp": 5.256110408229126, + "angularVelocity": 4.429017210736591e-25, + "velocityX": 0.6959637106242286, + "velocityY": -0.3221311756451756, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -52.801162344979105, + -52.801162344979105, + -52.801162344979105, + -52.801162344979105 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 24.439349698799905, + 24.439349698799905, + 24.439349698799905, + 24.439349698799905 + ], + "timestamp": 6.1787739441115495 }, { - "x": 2.5179204869025957, - "y": 4.363055966095035, + "x": 2.4996174011461876, + "y": 4.371527664321022, "heading": -0.5412356183232561, - "angularVelocity": 1.7413772411298374e-22, - "velocityX": 0.465344111132695, - "velocityY": -0.2153874452222276, - "timestamp": 5.314248725718518, + "angularVelocity": 4.392830640142467e-25, + "velocityX": 0.5220447874796567, + "velocityY": -0.24163170947436932, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -52.838991706698124, + -52.838991706698124, + -52.838991706698124, + -52.838991706698124 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 24.45685925652313, + 24.45685925652313, + 24.45685925652313, + 24.45685925652313 + ], + "timestamp": 6.239738871536083 + }, + { + "x": 2.520837055868033, + "y": 4.361706013826914, + "heading": -0.5412356183232561, + "angularVelocity": 4.46711444507948e-25, + "velocityX": 0.34806331473308955, + "velocityY": -0.16110329182733052, + "moduleForcesX": [ + -52.85799514715946, + -52.85799514715946, + -52.85799514715946, + -52.85799514715946 + ], + "moduleForcesY": [ + 24.465655118324054, + 24.465655118324058, + 24.46565511832406, + 24.465655118324058 + ], + "timestamp": 6.300703798960617 }, { "x": 2.531447649002075, "y": 4.356794834136963, "heading": -0.5412356183232561, - "angularVelocity": 1.7413769362387268e-22, - "velocityX": 0.23267206007376748, - "velocityY": -0.10769372469740146, - "timestamp": 5.372387043207911, + "angularVelocity": 4.42997254240294e-25, + "velocityX": 0.17404421824625813, + "velocityY": -0.08055745979575053, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -52.86942576358611, + -52.86942576358611, + -52.86942576358611, + -52.86942576358611 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 24.470945850946546, + 24.470945850946546, + 24.470945850946546, + 24.470945850946546 + ], + "timestamp": 6.361668726385151 }, { "x": 2.531447649002075, "y": 4.356794834136963, "heading": -0.5412356183232561, - "angularVelocity": 5.804601738611348e-23, - "velocityX": 1.3725068175106304e-19, - "velocityY": -2.6971290693406307e-19, - "timestamp": 5.430525360697303, + "angularVelocity": 1.4766575121958547e-25, + "velocityX": 1.1572658590616387e-19, + "velocityY": 2.5061165774890407e-19, "moduleForcesX": [ - 0, - 0, - 0, - 0 + -52.87705810406985, + -52.87705810406985, + -52.87705810406985, + -52.87705810406985 ], "moduleForcesY": [ - 0, - 0, - 0, - 0 - ] + 24.474478527687335, + 24.474478527687335, + 24.474478527687335, + 24.474478527687335 + ], + "timestamp": 6.422633653809685 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 0.7470832467079163, + "y": 6.625458717346191, + "heading": 1.056495497952334, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 0.8095526511854642, + "isStopPoint": false, + "x": 1.7956033945083618, + "y": 6.721975326538086, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 14 + }, + { + "timestamp": 1.6683801731519252, + "isStopPoint": true, + "x": 2.8381898403167725, + "y": 6.9806013107299805, + "heading": 0.3846859068543657, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "timestamp": 3.259734111449299, + "isStopPoint": false, + "x": 1.6989272832870483, + "y": 5.553633213043213, + "heading": -0.016665299843821514, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "timestamp": 4.4052219319242045, + "isStopPoint": true, + "x": 2.8705179691314697, + "y": 5.558157920837402, + "heading": -0.020267736871838693, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 5.7520194521398125, + "isStopPoint": false, + "x": 2.0850512981414795, + "y": 4.563412189483643, + "heading": 0.03752685285960867, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 11 + }, + { + "timestamp": 6.422633653809685, + "isStopPoint": true, + "x": 2.531447649002075, + "y": 4.356794834136963, + "heading": -0.5412356183232561, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 } ], - "trajectoryWaypoints": [], "constraints": [ { "scope": [ @@ -1768,7 +2056,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": true + "isTrajectoryStale": false }, "test": { "waypoints": [ diff --git a/src/main/deploy/choreo/amp 4 local.1.traj b/src/main/deploy/choreo/amp 4 local.1.traj index 345ccc69..15fbc262 100644 --- a/src/main/deploy/choreo/amp 4 local.1.traj +++ b/src/main/deploy/choreo/amp 4 local.1.traj @@ -1,265 +1,677 @@ { - "samples": [ - { - "x": 0.7470832467079163, - "y": 6.625458717346191, - "heading": 1.056495497952334, - "angularVelocity": -1.4679265201702596e-22, - "velocityX": 9.483029461049396e-23, - "velocityY": 5.2317038665130075e-22, - "timestamp": 0 - }, - { - "x": 0.7547543197229795, - "y": 6.625912789866557, - "heading": 1.0484306824677847, - "angularVelocity": -0.1891115926088127, - "velocityX": 0.17987873841334215, - "velocityY": 0.010647531571001236, - "timestamp": 0.04264580173692322 - }, - { - "x": 0.7701156825333854, - "y": 6.626850739979297, - "heading": 1.0325597341852575, - "angularVelocity": -0.37215734342229334, - "velocityX": 0.3602080904743721, - "velocityY": 0.021993961293704876, - "timestamp": 0.08529160347384644 - }, - { - "x": 0.7931884474144321, - "y": 6.628306307811922, - "heading": 1.0091781276812968, - "angularVelocity": -0.5482745206245366, - "velocityX": 0.5410325035833482, - "velocityY": 0.03413156215481, - "timestamp": 0.12793740521076966 - }, - { - "x": 0.8239951854040302, - "y": 6.630316938342184, - "heading": 0.9786149775936921, - "angularVelocity": -0.7166742995276547, - "velocityX": 0.7223861842167063, - "velocityY": 0.0471472090656353, - "timestamp": 0.17058320694769288 - }, - { - "x": 0.8625597183369016, - "y": 6.63292352204424, - "heading": 0.9412340847359876, - "angularVelocity": -0.8765433251390755, - "velocityX": 0.9042984622676677, - "velocityY": 0.06112169535784003, - "timestamp": 0.2132290086846161 - }, - { - "x": 0.9089072743014975, - "y": 6.636170316792377, - "heading": 0.8974431328952391, - "angularVelocity": -1.0268525870586154, - "velocityX": 1.086802312933599, - "velocityY": 0.07613398308622592, - "timestamp": 0.2558748104215393 - }, - { - "x": 0.9630650383734942, - "y": 6.64010548361246, - "heading": 0.8477159062035999, - "angularVelocity": -1.1660521004717048, - "velocityX": 1.269943625543481, - "velocityY": 0.0922755971234286, - "timestamp": 0.29852061215846254 - }, - { - "x": 1.0250628809506594, - "y": 6.644782896884854, - "heading": 0.7926327969571765, - "angularVelocity": -1.2916420140539158, - "velocityX": 1.4537853681265616, - "velocityY": 0.10968050973102496, - "timestamp": 0.34116641389538577 - }, - { - "x": 1.094933572481574, - "y": 6.650266047499182, - "heading": 0.7329446248446481, - "angularVelocity": -1.3996259814913892, - "velocityX": 1.6383955438788211, - "velocityY": 0.1285742181177263, - "timestamp": 0.383812215632309 - }, - { - "x": 1.1727109614146445, - "y": 6.6566348315708534, - "heading": 0.6696642878742429, - "angularVelocity": -1.4838585369029722, - "velocityX": 1.8237994307826475, - "velocityY": 0.149341407882537, - "timestamp": 0.4264580173692322 - }, - { - "x": 1.2584230921899875, - "y": 6.663995676876955, - "heading": 0.6041925063971192, - "angularVelocity": -1.5352456469457696, - "velocityX": 2.0098609308388866, - "velocityY": 0.17260421908608822, - "timestamp": 0.46910381910615545 - }, - { - "x": 1.3520739703395739, - "y": 6.67249503645535, - "heading": 0.5385009897728867, - "angularVelocity": -1.5403982091713377, - "velocityX": 2.19601635648234, - "velocityY": 0.19930120274971816, - "timestamp": 0.5117496208430786 - }, - { - "x": 1.453595786870469, - "y": 6.682338668837949, - "heading": 0.4754960223298961, - "angularVelocity": -1.4774014059264435, - "velocityX": 2.3805817312843804, - "velocityY": 0.23082301144958858, - "timestamp": 0.5543954225800019 - }, - { - "x": 1.5626754965988896, - "y": 6.6938349914873125, - "heading": 0.42023075468632287, - "angularVelocity": -1.2959134403076285, - "velocityX": 2.5578065198848923, - "velocityY": 0.2695768910684973, - "timestamp": 0.5970412243169251 - }, - { - "x": 1.6778273819318938, - "y": 6.707315267421264, - "heading": 0.3846859068543657, - "angularVelocity": -0.8334899658172448, - "velocityX": 2.7001927655941973, - "velocityY": 0.3160985462791911, - "timestamp": 0.6396870260538483 - }, - { - "x": 1.7956033945083618, - "y": 6.721975326538086, - "heading": 0.3846859068543657, - "angularVelocity": -2.7339775014955035e-22, - "velocityX": 2.7617258388765618, - "velocityY": 0.34376324326736624, - "timestamp": 0.6823328277907715 - }, - { - "x": 1.9702980732827642, - "y": 6.757682275421422, - "heading": 0.3846859068543657, - "angularVelocity": -3.55936785030336e-23, - "velocityX": 2.8736582107292628, - "velocityY": 0.587365153641545, - "timestamp": 0.7431245663914694 - }, - { - "x": 2.128777504610528, - "y": 6.795024747259092, - "heading": 0.3846859068543657, - "angularVelocity": -3.5593719407474505e-23, - "velocityX": 2.606923818525971, - "velocityY": 0.6142688578615821, - "timestamp": 0.8039163049921673 - }, - { - "x": 2.2710818815122873, - "y": 6.830372862436407, - "heading": 0.3846859068543657, - "angularVelocity": -3.5593679346398325e-23, - "velocityX": 2.340850585578186, - "velocityY": 0.5814624814318021, - "timestamp": 0.8647080435928651 - }, - { - "x": 2.397370829093984, - "y": 6.862701695876428, - "heading": 0.3846859068543657, - "angularVelocity": -3.5593640063590435e-23, - "velocityX": 2.0774031223421394, - "velocityY": 0.5317964938026807, - "timestamp": 0.925499782193563 - }, - { - "x": 2.507741549731751, - "y": 6.891534791754925, - "heading": 0.3846859068543657, - "angularVelocity": -3.5593769660186765e-23, - "velocityX": 1.8155545996590319, - "velocityY": 0.47429299674884, - "timestamp": 0.9862915207942609 - }, - { - "x": 2.6022568329985316, - "y": 6.916597570791243, - "heading": 0.3846859068543657, - "angularVelocity": -3.559368485205426e-23, - "velocityX": 1.5547389405588703, - "velocityY": 0.41227277938109086, - "timestamp": 1.0470832593949586 - }, - { - "x": 2.6809601695719074, - "y": 6.937711608282982, - "heading": 0.3846859068543657, - "angularVelocity": -3.559368363101467e-23, - "velocityX": 1.2946386858636783, - "velocityY": 0.3473175463926733, - "timestamp": 1.1078749979956564 - }, - { - "x": 2.7438833546228816, - "y": 6.954751696784365, - "heading": 0.3846859068543657, - "angularVelocity": -3.559362874104829e-23, - "velocityX": 1.0350614491267762, - "velocityY": 0.28030270055785556, - "timestamp": 1.1686667365963541 - }, - { - "x": 2.791050608826443, - "y": 6.967625143508777, - "heading": 0.3846859068543657, - "angularVelocity": -3.5593782108627554e-23, - "velocityX": 0.775882632891501, - "velocityY": 0.21176309512990737, - "timestamp": 1.2294584751970519 - }, - { - "x": 2.8224809815825402, - "y": 6.976260567480704, - "heading": 0.3846859068543657, - "angularVelocity": -3.5593532451099235e-23, - "velocityX": 0.5170171717335424, - "velocityY": 0.14204930095266424, - "timestamp": 1.2902502137977496 - }, - { - "x": 2.8381898403167725, - "y": 6.9806013107299805, - "heading": 0.3846859068543657, - "angularVelocity": -3.55938793253341e-23, - "velocityX": 0.2584044986344835, - "velocityY": 0.07140350562744292, - "timestamp": 1.3510419523984474 - }, - { - "x": 2.8381898403167725, - "y": 6.9806013107299805, - "heading": 0.3846859068543657, - "angularVelocity": -5.573807154532037e-19, - "velocityX": -9.35615466765533e-16, - "velocityY": -9.896680587980253e-18, - "timestamp": 1.4118336909991451 - } - ] + "samples": [ + { + "x": 0.7470832467079163, + "y": 6.625458717346191, + "heading": 1.056495497952334, + "angularVelocity": -2.814922481994462e-23, + "velocityX": 3.0229602263614203e-24, + "velocityY": -3.0164882691321217e-22, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ], + "timestamp": 0 + }, + { + "x": 0.7539026198838662, + "y": 6.625839605524516, + "heading": 1.0492729313853104, + "angularVelocity": -0.15166849427224313, + "velocityX": 0.14320173471285522, + "velocityY": 0.00799836677952159, + "moduleForcesX": [ + 57.85992865453149, + 55.57487201027826, + 51.318064652077474, + 58.039260452127415 + ], + "moduleForcesY": [ + 6.907999308730754, + -17.49184693835158, + 27.569744910492076, + -4.542101533123827 + ], + "timestamp": 0.047620744187380236 + }, + { + "x": 0.7675577009381795, + "y": 6.626628550621342, + "heading": 1.0350515766053865, + "angularVelocity": -0.29863781052990607, + "velocityX": 0.2867464859554202, + "velocityY": 0.01656725677619831, + "moduleForcesX": [ + 57.88100930307201, + 55.714508532181725, + 51.61400892290664, + 58.11626141362601 + ], + "moduleForcesY": [ + 6.706391089887608, + -17.030469565022152, + 27.005128287026007, + -3.349638557212904 + ], + "timestamp": 0.09524148837476047 + }, + { + "x": 0.7880663188184851, + "y": 6.627856082150549, + "heading": 1.014085545158225, + "angularVelocity": -0.44027097444473756, + "velocityX": 0.4306656317592878, + "velocityY": 0.0257772437233815, + "moduleForcesX": [ + 57.91162551157599, + 55.863804025506134, + 51.9553282692449, + 58.17751046150169 + ], + "moduleForcesY": [ + 6.409362353495803, + -16.520382189151235, + 26.3352269347313, + -1.8953822815394896 + ], + "timestamp": 0.1428622325621407 + }, + { + "x": 0.8154474139272968, + "y": 6.629555951163622, + "heading": 0.9866565513822839, + "angularVelocity": -0.5759883480193475, + "velocityX": 0.5749825118454964, + "velocityY": 0.03569597750058087, + "moduleForcesX": [ + 57.94827042148509, + 56.03347528236485, + 52.34203591069722, + 58.203278502637744 + ], + "moduleForcesY": [ + 6.035406626296778, + -15.918521504125732, + 25.54970777274484, + -0.23510536726652978 + ], + "timestamp": 0.19048297674952094 + }, + { + "x": 0.8497208018135379, + "y": 6.6317649710027196, + "heading": 0.9530739033866412, + "angularVelocity": -0.7052104827152668, + "velocityX": 0.7197155036338936, + "velocityY": 0.04638776392079282, + "moduleForcesX": [ + 57.987036343437325, + 56.235225818874625, + 52.774229558093566, + 58.17795169052109 + ], + "moduleForcesY": [ + 5.608046706017465, + -15.169403162610594, + 24.63487493770155, + 1.560678231687874 + ], + "timestamp": 0.2381037209369012 + }, + { + "x": 0.8909071759065187, + "y": 6.634522965587952, + "heading": 0.9136797710629836, + "angularVelocity": -0.8272473056835844, + "velocityX": 0.8648830419558078, + "velocityY": 0.057915822868710255, + "moduleForcesX": [ + 58.02382176629463, + 56.48076577385563, + 53.2518632286912, + 58.094056700540655 + ], + "moduleForcesY": [ + 5.158035303537173, + -14.201163704424598, + 23.572862374375507, + 3.4055289208270785 + ], + "timestamp": 0.2857244651242814 + }, + { + "x": 0.9390283845014781, + "y": 6.637873093075656, + "heading": 0.8688628769210628, + "angularVelocity": -0.9411212467737456, + "velocityX": 1.0105093781317218, + "velocityY": 0.07035017081046684, + "moduleForcesX": [ + 58.05445637477262, + 56.77945893480036, + 53.77458368672696, + 57.955802544053434 + ], + "moduleForcesY": [ + 4.726988301952687, + -12.920128155574607, + 22.340010342191754, + 5.198389651263037 + ], + "timestamp": 0.3333452093116616 + }, + { + "x": 0.99410787725357, + "y": 6.641862960540765, + "heading": 0.8190840274654485, + "angularVelocity": -1.0453185960249207, + "velocityX": 1.156628139521772, + "velocityY": 0.08378423170811934, + "moduleForcesX": [ + 58.0745823349482, + 57.13326696521539, + 54.34162138758188, + 57.78094210439999 + ], + "moduleForcesY": [ + 4.373537369916966, + -11.203858122136547, + 20.903786932368053, + 6.827139466144138 + ], + "timestamp": 0.38096595349904183 + }, + { + "x": 1.0561709558415282, + "y": 6.646547058923123, + "heading": 0.764916720396532, + "angularVelocity": -1.1374729226359126, + "velocityX": 1.3032782172355293, + "velocityY": 0.09836256157458603, + "moduleForcesX": [ + 58.07884230896552, + 57.52664576524135, + 54.95165712906377, + 57.59988532120776 + ], + "moduleForcesY": [ + 4.18414702790263, + -8.893502901162272, + 19.216964845382883, + 8.173241281714256 + ], + "timestamp": 0.42858669768642205 + }, + { + "x": 1.1252440265364065, + "y": 6.651991066133351, + "heading": 0.707105481038992, + "angularVelocity": -1.2139927744527004, + "velocityX": 1.4504828068853088, + "velocityY": 0.11432007842645366, + "moduleForcesX": [ + 58.058063356768365, + 57.90773275166756, + 55.60238220835145, + 57.45155749012649 + ], + "moduleForcesY": [ + 4.293210892903159, + -5.788564619162194, + 17.206213788359637, + 9.115718390676392 + ], + "timestamp": 0.47620744187380226 + }, + { + "x": 1.2013513334108457, + "y": 6.658278411112851, + "heading": 0.6466442567332923, + "angularVelocity": -1.2696404757513684, + "velocityX": 1.5981965039220822, + "velocityY": 0.1320295406295972, + "moduleForcesX": [ + 57.99034977388328, + 58.15656069329295, + 56.288724673225055, + 57.376165910837095 + ], + "moduleForcesY": [ + 4.923367257090649, + -1.6522597130636139, + 14.748411409233862, + 9.532722214598236 + ], + "timestamp": 0.5238281860611825 + }, + { + "x": 1.2845063733027953, + "y": 6.665519148313447, + "heading": 0.5848804980188779, + "angularVelocity": -1.2969927238302643, + "velocityX": 1.7461936244580183, + "velocityY": 0.15205006398272194, + "moduleForcesX": [ + 57.811915108319404, + 58.03976645514236, + 56.99574350032489, + 57.40532402483411 + ], + "moduleForcesY": [ + 6.474842781770424, + 3.7555604314694015, + 11.617621448775878, + 9.299747156494563 + ], + "timestamp": 0.5714489302485628 + }, + { + "x": 1.3746921013206055, + "y": 6.673861244609411, + "heading": 0.5236730878733165, + "angularVelocity": -1.285309820122082, + "velocityX": 1.8938328150216104, + "velocityY": 0.17517778099265666, + "moduleForcesX": [ + 57.310185080815664, + 57.16712059629645, + 57.669087251929504, + 57.549491540761366 + ], + "moduleForcesY": [ + 9.74828832700739, + 10.593301460429908, + 7.3549113244132425, + 8.285418011961559 + ], + "timestamp": 0.6190696744359431 + }, + { + "x": 1.4718109314175543, + "y": 6.683507923259447, + "heading": 0.46574387997473543, + "angularVelocity": -1.2164700255552203, + "velocityX": 2.0394227716140083, + "velocityY": 0.20257303439185328, + "moduleForcesX": [ + 55.65121903382453, + 54.99704715282837, + 58.07803857155044, + 57.78139771397302 + ], + "moduleForcesY": [ + 16.565429620324096, + 18.792767490211176, + 0.9116734716158617, + 6.3514478887901085 + ], + "timestamp": 0.6666904186233233 + }, + { + "x": 1.57550342839463, + "y": 6.694752929350011, + "heading": 0.4158791410788935, + "angularVelocity": -1.0471222100106607, + "velocityX": 2.1774648579422013, + "velocityY": 0.2361367148383242, + "moduleForcesX": [ + 48.75296553631865, + 50.943800913157915, + 57.05405288058638, + 58.01396630829892 + ], + "moduleForcesY": [ + 31.297064627265303, + 27.93547289382856, + -10.403475397779616, + 3.3890463395149384 + ], + "timestamp": 0.7143111628107036 + }, + { + "x": 1.6843562594767798, + "y": 6.707809824340588, + "heading": 0.3846859068543657, + "angularVelocity": -0.6550345811856123, + "velocityX": 2.2858280133932967, + "velocityY": 0.27418502615581647, + "moduleForcesX": [ + 18.204183715975507, + 44.46669574639313, + 47.828415859508524, + 58.091244495239216 + ], + "moduleForcesY": [ + 54.82018208894905, + 37.37367730553576, + -32.52805362351062, + -0.47054408407509796 + ], + "timestamp": 0.7619319069980839 + }, + { + "x": 1.7956033945083618, + "y": 6.721975326538086, + "heading": 0.3846859068543657, + "angularVelocity": -3.554059362055802e-23, + "velocityX": 2.336106605008982, + "velocityY": 0.2974649480856219, + "moduleForcesX": [ + -28.169968798994756, + 34.96729290232753, + 13.531484107057947, + 57.89422679118955 + ], + "moduleForcesY": [ + 50.55108957029159, + 46.375637620891645, + -56.177268901226164, + -4.530739956145902 + ], + "timestamp": 0.8095526511854642 + }, + { + "x": 1.9448813303029442, + "y": 6.750262783810755, + "heading": 0.3846859068543657, + "angularVelocity": -7.071920034257376e-24, + "velocityX": 2.4334235311170778, + "velocityY": 0.4611221597912817, + "moduleForcesX": [ + 29.383140382480864, + 29.383140382480864, + 29.383140382480864, + 29.383140382480864 + ], + "moduleForcesY": [ + 49.41342702102415, + 49.41342702102415, + 49.41342702102415, + 49.41342702102415 + ], + "timestamp": 0.8708974741830685 + }, + { + "x": 2.0829613480368456, + "y": 6.781960040020516, + "heading": 0.3846859068543657, + "angularVelocity": -7.072310064547998e-24, + "velocityX": 2.250882975720603, + "velocityY": 0.5167062950201712, + "moduleForcesX": [ + -55.1149217223832, + -55.11492172238321, + -55.1149217223832, + -55.1149217223832 + ], + "moduleForcesY": [ + 16.78265553368512, + 16.782655533685116, + 16.78265553368512, + 16.78265553368512 + ], + "timestamp": 0.9322422971806729 + }, + { + "x": 2.209277628517863, + "y": 6.812924670308866, + "heading": 0.3846859068543657, + "angularVelocity": -7.072257174941514e-24, + "velocityX": 2.0591188352756387, + "velocityY": 0.5047635444242788, + "moduleForcesX": [ + -57.89982158665208, + -57.89982158665208, + -57.89982158665208, + -57.89982158665208 + ], + "moduleForcesY": [ + -3.6059042485813855, + -3.605904248581385, + -3.6059042485813873, + -3.605904248581387 + ], + "timestamp": 0.9935871201782772 + }, + { + "x": 2.323926038493632, + "y": 6.842067117238504, + "heading": 0.3846859068543657, + "angularVelocity": -7.072361618794593e-24, + "velocityX": 1.8689174468764331, + "velocityY": 0.4750595976253187, + "moduleForcesX": [ + -57.42797599329124, + -57.42797599329124, + -57.42797599329124, + -57.42797599329124 + ], + "moduleForcesY": [ + -8.968586181381475, + -8.968586181381475, + -8.968586181381475, + -8.968586181381475 + ], + "timestamp": 1.0549319431758817 + }, + { + "x": 2.4269824153429544, + "y": 6.868901069621197, + "heading": 0.3846859068543657, + "angularVelocity": -7.072821947364684e-24, + "velocityX": 1.679952305891357, + "velocityY": 0.4374281491323442, + "moduleForcesX": [ + -57.0547127515352, + -57.0547127515352, + -57.0547127515352, + -57.0547127515352 + ], + "moduleForcesY": [ + -11.362156390317617, + -11.362156390317615, + -11.362156390317617, + -11.362156390317617 + ], + "timestamp": 1.116276766173486 + }, + { + "x": 2.5184986692010405, + "y": 6.893152527814633, + "heading": 0.3846859068543657, + "angularVelocity": -7.07105136075711e-24, + "velocityX": 1.491833367286101, + "velocityY": 0.3953301518920652, + "moduleForcesX": [ + -56.79921677244283, + -56.79921677244283, + -56.79921677244284, + -56.79921677244284 + ], + "moduleForcesY": [ + -12.71075250936749, + -12.71075250936749, + -12.710752509367488, + -12.71075250936749 + ], + "timestamp": 1.1776215891710904 + }, + { + "x": 2.5985115833830923, + "y": 6.9146459631069135, + "heading": 0.3846859068543657, + "angularVelocity": -7.071515558519895e-24, + "velocityX": 1.3043140443192134, + "velocityY": 0.35037080949961336, + "moduleForcesX": [ + -56.61817333855819, + -56.61817333855819, + -56.61817333855819, + -56.61817333855819 + ], + "moduleForcesY": [ + -13.574685533676519, + -13.574685533676519, + -13.574685533676519, + -13.574685533676519 + ], + "timestamp": 1.2389664121686947 + }, + { + "x": 2.6670483577421855, + "y": 6.9332594017529505, + "heading": 0.3846859068543657, + "angularVelocity": -7.07550113164988e-24, + "velocityX": 1.117238114156259, + "velocityY": 0.30342313721833786, + "moduleForcesX": [ + -56.48429865176341, + -56.4842986517634, + -56.48429865176341, + -56.48429865176341 + ], + "moduleForcesY": [ + -14.175026898601002, + -14.175026898601002, + -14.175026898601004, + -14.175026898601008 + ], + "timestamp": 1.300311235166299 + }, + { + "x": 2.72412984860515, + "y": 6.948903188901549, + "heading": 0.3846859068543657, + "angularVelocity": -7.06791759690408e-24, + "velocityX": 0.9305021691104057, + "velocityY": 0.2550139748420099, + "moduleForcesX": [ + -56.38164610381262, + -56.38164610381262, + -56.38164610381262, + -56.38164610381262 + ], + "moduleForcesY": [ + -14.616298220537676, + -14.616298220537676, + -14.616298220537674, + -14.616298220537672 + ], + "timestamp": 1.3616560581639034 + }, + { + "x": 2.769772527142252, + "y": 6.961508658865873, + "heading": 0.3846859068543657, + "angularVelocity": -7.080226941592603e-24, + "velocityX": 0.744034725454898, + "velocityY": 0.2054854729113339, + "moduleForcesX": [ + -56.30057681442044, + -56.30057681442044, + -56.30057681442043, + -56.30057681442044 + ], + "moduleForcesY": [ + -14.954263182815012, + -14.954263182815014, + -14.954263182815021, + -14.954263182815023 + ], + "timestamp": 1.4230008811615078 + }, + { + "x": 2.8039897177250084, + "y": 6.971021542915001, + "heading": 0.3846859068543657, + "angularVelocity": -7.06236523633029e-24, + "velocityX": 0.5577844862979259, + "velocityY": 0.15507232043850186, + "moduleForcesX": [ + -56.2349957225445, + -56.2349957225445, + -56.2349957225445, + -56.2349957225445 + ], + "moduleForcesY": [ + -15.221367910729839, + -15.221367910729839, + -15.221367910729835, + -15.221367910729835 + ], + "timestamp": 1.4843457041591122 + }, + { + "x": 2.8267924145674423, + "y": 6.97739787426348, + "heading": 0.3846859068543657, + "angularVelocity": -7.077974709960036e-24, + "velocityX": 0.371713467056942, + "velocityY": 0.10394245246626038, + "moduleForcesX": [ + -56.18088340969801, + -56.18088340969801, + -56.18088340969801, + -56.18088340969801 + ], + "moduleForcesY": [ + -15.437767595508772, + -15.437767595508774, + -15.437767595508781, + -15.437767595508783 + ], + "timestamp": 1.5456905271567165 + }, + { + "x": 2.8381898403167725, + "y": 6.9806013107299805, + "heading": 0.3846859068543657, + "angularVelocity": -7.070376775728643e-24, + "velocityX": 0.18579278890046133, + "velocityY": 0.05222015990861092, + "moduleForcesX": [ + -56.13549055392384, + -56.13549055392384, + -56.13549055392384, + -56.13549055392384 + ], + "moduleForcesY": [ + -15.616639816973526, + -15.616639816973523, + -15.616639816973528, + -15.616639816973525 + ], + "timestamp": 1.6070353501543209 + }, + { + "x": 2.8381898403167725, + "y": 6.9806013107299805, + "heading": 0.3846859068543657, + "angularVelocity": 1.6334100500794593e-18, + "velocityX": -2.855209079304936e-18, + "velocityY": 1.6102083009631126e-18, + "moduleForcesX": [ + -56.096876634297416, + -56.096876634297416, + -56.09687663429742, + -56.096876634297416 + ], + "moduleForcesY": [ + -15.766962138590067, + -15.766962138590072, + -15.766962138590065, + -15.766962138590062 + ], + "timestamp": 1.6683801731519252 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/amp 4 local.2.traj b/src/main/deploy/choreo/amp 4 local.2.traj index 97833e87..50a22478 100644 --- a/src/main/deploy/choreo/amp 4 local.2.traj +++ b/src/main/deploy/choreo/amp 4 local.2.traj @@ -1,247 +1,677 @@ { - "samples": [ - { - "x": 2.8381898403167725, - "y": 6.9806013107299805, - "heading": 0.3846859068543657, - "angularVelocity": -5.573807154532037e-19, - "velocityX": -9.35615466765533e-16, - "velocityY": -9.896680587980253e-18, - "timestamp": 0 - }, - { - "x": 2.8128886138231195, - "y": 6.955100739151656, - "heading": 0.3773890772908297, - "angularVelocity": -0.08078200568400537, - "velocityX": -0.2801057369692363, - "velocityY": -0.2823126537693574, - "timestamp": 0.09032741266739763 - }, - { - "x": 2.7622861408558537, - "y": 6.904099615654192, - "heading": 0.3627975933198322, - "angularVelocity": -0.16153993057112973, - "velocityX": -0.5602116951334734, - "velocityY": -0.564625089894962, - "timestamp": 0.18065482533479527 - }, - { - "x": 2.6863823744143596, - "y": 6.827597945234058, - "heading": 0.3409166257799037, - "angularVelocity": -0.24224060995125085, - "velocityX": -0.8403181736340153, - "velocityY": -0.8469374707052851, - "timestamp": 0.2709822380021929 - }, - { - "x": 2.585177249564827, - "y": 6.725595708318326, - "heading": 0.311754466827316, - "angularVelocity": -0.32284948822656157, - "velocityX": -1.1204253710064276, - "velocityY": -1.1292500682040065, - "timestamp": 0.36130965066959053 - }, - { - "x": 2.4586707006639994, - "y": 6.598092843845294, - "heading": 0.27532262336823443, - "angularVelocity": -0.40333097542716767, - "velocityX": -1.4005332951050296, - "velocityY": -1.4115633417063895, - "timestamp": 0.45163706333698816 - }, - { - "x": 2.306862702051818, - "y": 6.445089212010339, - "heading": 0.2316358112414208, - "angularVelocity": -0.4836495459854555, - "velocityX": -1.6806415032828872, - "velocityY": -1.6938781629619528, - "timestamp": 0.5419644760043858 - }, - { - "x": 2.1297534370368134, - "y": 6.266584440083468, - "heading": 0.18071175454391936, - "angularVelocity": -0.5637718959659755, - "velocityX": -1.960747682084755, - "velocityY": -1.9761971106474765, - "timestamp": 0.6322918886717832 - }, - { - "x": 1.9873417449288229, - "y": 6.088213430389112, - "heading": 0.1304328979304444, - "angularVelocity": -0.5566289914548207, - "velocityX": -1.5766165320272711, - "velocityY": -1.974716250895523, - "timestamp": 0.7226193013391806 - }, - { - "x": 1.870232723728645, - "y": 5.935342556724319, - "heading": 0.08735848478659979, - "angularVelocity": -0.47686977709234435, - "velocityX": -1.2964948041918032, - "velocityY": -1.692408419008837, - "timestamp": 0.812946714006578 - }, - { - "x": 1.7784261170686546, - "y": 5.807972234685134, - "heading": 0.05147629980122907, - "angularVelocity": -0.3972457964408093, - "velocityX": -1.0163759145574802, - "velocityY": -1.410095986129178, - "timestamp": 0.9032741266739754 - }, - { - "x": 1.7119218452051854, - "y": 5.706102649506714, - "heading": 0.022777291492800653, - "angularVelocity": -0.31772202325989357, - "velocityX": -0.736257907751286, - "velocityY": -1.1277815025423439, - "timestamp": 0.9936015393413729 - }, - { - "x": 1.6707198619792896, - "y": 5.629733910265785, - "heading": 0.0012553801790747192, - "angularVelocity": -0.23826555724598256, - "velocityX": -0.4561404119627787, - "velocityY": -0.8454658113853617, - "timestamp": 1.0839289520087703 - }, - { - "x": 1.6548201302688181, - "y": 5.578866079528388, - "heading": -0.01309273794263427, - "angularVelocity": -0.15884566709086995, - "velocityX": -0.17602332714722002, - "velocityY": -0.5631494275703062, - "timestamp": 1.1742563646761677 - }, - { - "x": 1.664222615534791, - "y": 5.55349918288868, - "heading": -0.020267736871838676, - "angularVelocity": -0.079433238674075, - "velocityX": 0.10409337529313177, - "velocityY": -0.2808327604066967, - "timestamp": 1.264583777343565 - }, - { - "x": 1.6989272832870483, - "y": 5.553633213043213, - "heading": -0.020267736871838676, - "angularVelocity": -3.9423001343697265e-19, - "velocityX": 0.38420969589870047, - "velocityY": 0.0014838259015203296, - "timestamp": 1.3549111900109625 - }, - { - "x": 1.765875287477924, - "y": 5.553891767639138, - "heading": -0.020267736871838676, - "angularVelocity": -1.268800151544e-19, - "velocityX": 0.7684200358337809, - "velocityY": 0.0029676542903228132, - "timestamp": 1.4420354203392796 - }, - { - "x": 1.8662973205168285, - "y": 5.554279599636344, - "heading": -0.02026773687183868, - "angularVelocity": -1.2688001508205996e-19, - "velocityX": 1.1526303608132487, - "velocityY": 0.004451482621366332, - "timestamp": 1.5291596506675966 - }, - { - "x": 2.000193380141947, - "y": 5.554796709026097, - "heading": -0.02026773687183868, - "angularVelocity": -1.2688001524330257e-19, - "velocityX": 1.53684065983192, - "velocityY": 0.005935310852148713, - "timestamp": 1.6162838809959137 - }, - { - "x": 2.167563461458264, - "y": 5.555443095789492, - "heading": -0.020267736871838683, - "angularVelocity": -1.2688001514027187e-19, - "velocityX": 1.9210509026662645, - "velocityY": 0.0074191388659460445, - "timestamp": 1.7034081113242308 - }, - { - "x": 2.3684075459339367, - "y": 5.556218759854957, - "heading": -0.020267736871838683, - "angularVelocity": -1.2688001518380097e-19, - "velocityX": 2.3052609327946625, - "velocityY": 0.00890296605826855, - "timestamp": 1.7905323416525478 - }, - { - "x": 2.5357776764404463, - "y": 5.556865146808326, - "heading": -0.020267736871838683, - "angularVelocity": -1.2688001516210267e-19, - "velocityX": 1.921051467264574, - "velocityY": 0.007419141046436553, - "timestamp": 1.8776565719808649 - }, - { - "x": 2.669673789778629, - "y": 5.5573822564055195, - "heading": -0.020267736871838686, - "angularVelocity": -1.268800151621384e-19, - "velocityX": 1.5368412763431272, - "velocityY": 0.005935313233127921, - "timestamp": 1.964780802309182 - }, - { - "x": 2.7700958781366665, - "y": 5.557770088616369, - "heading": -0.020267736871838686, - "angularVelocity": -1.2688001516248343e-19, - "velocityX": 1.1526309957586889, - "velocityY": 0.004451485073538937, - "timestamp": 2.051905032637499 - }, - { - "x": 2.8370439384630357, - "y": 5.558028643429091, - "heading": -0.02026773687183869, - "angularVelocity": -1.268800151622008e-19, - "velocityX": 0.7684206801492988, - "velocityY": 0.0029676567786828525, - "timestamp": 2.139029262965816 - }, - { - "x": 2.8705179691314697, - "y": 5.558157920837402, - "heading": -0.02026773687183869, - "angularVelocity": -1.2688001484574509e-19, - "velocityX": 0.3842103458738335, - "velocityY": 0.0014838284117379952, - "timestamp": 2.226153493294133 - }, - { - "x": 2.8705179691314697, - "y": 5.558157920837402, - "heading": -0.020267736871838693, - "angularVelocity": -3.4830373991096207e-20, - "velocityX": 7.082195607551611e-16, - "velocityY": 1.9869518370590596e-18, - "timestamp": 2.31327772362245 - } - ] + "samples": [ + { + "x": 2.8381898403167725, + "y": 6.9806013107299805, + "heading": 0.3846859068543657, + "angularVelocity": 1.6334100500794593e-18, + "velocityX": -2.855209079304936e-18, + "velocityY": 1.6102083009631126e-18, + "moduleForcesX": [ + -56.096876634297416, + -56.096876634297416, + -56.09687663429742, + -56.096876634297416 + ], + "moduleForcesY": [ + -15.766962138590067, + -15.766962138590072, + -15.766962138590065, + -15.766962138590062 + ], + "timestamp": 0 + }, + { + "x": 2.821131681761048, + "y": 6.962941949661055, + "heading": 0.37963298615970775, + "angularVelocity": -0.05715420706543545, + "velocityX": -0.1929469281557583, + "velocityY": -0.19974720368037308, + "moduleForcesX": [ + -37.095177903701845, + -39.323311531074836, + -41.803716726316495, + -43.47083407085052 + ], + "moduleForcesY": [ + -44.94946586017272, + -43.0174583751275, + -40.605077451410324, + -38.81979329867605 + ], + "timestamp": 0.08840855212763188 + }, + { + "x": 2.7870198730695646, + "y": 6.927621262535135, + "heading": 0.36952776277295296, + "angularVelocity": -0.11430142382793766, + "velocityX": -0.38584286101911414, + "velocityY": -0.3995166335823328, + "moduleForcesX": [ + -37.072533334197914, + -39.34365755876542, + -41.75968748882494, + -43.474426866266874 + ], + "moduleForcesY": [ + -44.962788854185376, + -42.99375240452121, + -40.64405289944275, + -38.80982680576702 + ], + "timestamp": 0.17681710425526376 + }, + { + "x": 2.7358604135298354, + "y": 6.874636621269694, + "heading": 0.35437217454755016, + "angularVelocity": -0.17142672129188716, + "velocityX": -0.5786709352040128, + "velocityY": -0.5993157900487962, + "moduleForcesX": [ + -37.04188686591134, + -39.376245866938596, + -41.69487105411659, + -43.48043465252771 + ], + "moduleForcesY": [ + -44.98092590316717, + -42.957112494872305, + -40.70214264581303, + -38.7951513225733 + ], + "timestamp": 0.26522565638289564 + }, + { + "x": 2.6676616985406656, + "y": 6.803984339073742, + "heading": 0.33416944271042276, + "angularVelocity": -0.22851558306216296, + "velocityX": -0.7714040480010793, + "velocityY": -0.7991566482612914, + "moduleForcesX": [ + -37.00181302221145, + -39.4192239370053, + -41.60655692235178, + -43.486265185245315 + ], + "moduleForcesY": [ + -45.00397284969973, + -42.90816343331507, + -40.78067451259032, + -38.777468393572995 + ], + "timestamp": 0.3536342085105275 + }, + { + "x": 2.582436328275892, + "y": 6.71565888253476, + "heading": 0.3089239458735266, + "angularVelocity": -0.2855549178144013, + "velocityX": -0.9639946386831083, + "velocityY": -0.9990600955829445, + "moduleForcesX": [ + -36.94888772357164, + -39.46875969787357, + -41.48963891451579, + -43.48713660247826 + ], + "moduleForcesY": [ + -45.03260236713544, + -42.8483286467539, + -40.882056657335006, + -38.75974233145917 + ], + "timestamp": 0.4420427606381594 + }, + { + "x": 2.4802053222003866, + "y": 6.609651042778182, + "heading": 0.2786408401009175, + "angularVelocity": -0.34253593169233965, + "velocityX": -1.156347475614342, + "velocityY": -1.1990677056167498, + "moduleForcesX": [ + -36.873873585620636, + -39.51569610358064, + -41.33281589762392, + -43.4727953778751 + ], + "moduleForcesY": [ + -45.06942043742109, + -42.781255183974444, + -41.011452230389395, + -38.74789240095698 + ], + "timestamp": 0.5304513127657913 + }, + { + "x": 2.361010738415153, + "y": 6.485942489444843, + "heading": 0.2433248076280929, + "angularVelocity": -0.39946398422904045, + "velocityX": -1.3482245881954655, + "velocityY": -1.3992826525961657, + "moduleForcesX": [ + -36.74649413708812, + -39.53225346091061, + -41.10333275151675, + -43.414434944697646 + ], + "moduleForcesY": [ + -45.1243385113088, + -42.718426403813595, + -41.18352942870808, + -38.75747804587791 + ], + "timestamp": 0.6188598648934232 + }, + { + "x": 2.22497852013832, + "y": 6.344478967922724, + "heading": 0.20297375459292902, + "angularVelocity": -0.4564157206975939, + "velocityX": -1.5386771415558158, + "velocityY": -1.6001112801609332, + "moduleForcesX": [ + -36.40940346518017, + -39.379512733261826, + -40.64030165250597, + -43.1734908685818 + ], + "moduleForcesY": [ + -45.25140648250694, + -42.717694890088865, + -41.4691945783061, + -38.85975194723375 + ], + "timestamp": 0.707268417021055 + }, + { + "x": 2.0952329573157353, + "y": 6.186474139637454, + "heading": 0.15831835366452565, + "angularVelocity": -0.5051027287941129, + "velocityX": -1.467568008978098, + "velocityY": -1.7872120341611801, + "moduleForcesX": [ + 16.709059309020606, + 12.736243666460043, + 17.192391398004176, + 12.953053946000171 + ], + "moduleForcesY": [ + -39.41171649809029, + -40.61065023266987, + -37.72694053413655, + -39.04453253941978 + ], + "timestamp": 0.7956769691486869 + }, + { + "x": 1.9826977478470895, + "y": 6.045863415934052, + "heading": 0.11862394841971062, + "angularVelocity": -0.44898829682800195, + "velocityX": -1.2728995867524602, + "velocityY": -1.5904651792103528, + "moduleForcesX": [ + 37.390210370998965, + 40.475901652172276, + 41.278591550019264, + 43.990980045515705 + ], + "moduleForcesY": [ + 44.44300054730394, + 41.677348946779276, + 40.830424502770605, + 37.92667410323451 + ], + "timestamp": 0.8840855212763188 + }, + { + "x": 1.8873135024006749, + "y": 5.92277267476745, + "heading": 0.08389641882694215, + "angularVelocity": -0.3928073558159144, + "velocityX": -1.0789029245577086, + "velocityY": -1.3922945032387968, + "moduleForcesX": [ + 37.21476272459083, + 40.450247243359854, + 41.00550775325207, + 43.902218755828535 + ], + "moduleForcesY": [ + 44.73999822966322, + 41.84966110698525, + 41.279760388849354, + 38.20121632419051 + ], + "timestamp": 0.9724940734039507 + }, + { + "x": 1.8090603018164058, + "y": 5.817243772643182, + "heading": 0.05413403725183304, + "angularVelocity": -0.3366459562887355, + "velocityX": -0.8851315704310847, + "velocityY": -1.1936503831882683, + "moduleForcesX": [ + 37.15959386222587, + 40.498806672558835, + 40.847090706611624, + 43.87843297066345 + ], + "moduleForcesY": [ + 44.83556164168193, + 41.85167911227039, + 41.49451958786403, + 38.28563046961167 + ], + "timestamp": 1.0609026255315825 + }, + { + "x": 1.747928153071767, + "y": 5.729297635459773, + "heading": 0.02933424645224599, + "angularVelocity": -0.2805134820416991, + "velocityX": -0.6914732486104376, + "velocityY": -0.9947695677273937, + "moduleForcesX": [ + 37.13549798902941, + 40.55427442990822, + 40.73097140342639, + 43.868457258457035 + ], + "moduleForcesY": [ + 44.88031315505466, + 41.82243046412967, + 41.63740433004384, + 38.32559792736543 + ], + "timestamp": 1.1493111776592144 + }, + { + "x": 1.7039110423935377, + "y": 5.658946821606213, + "heading": 0.009494945599922864, + "angularVelocity": -0.22440477052131733, + "velocityX": -0.49788294931788657, + "velocityY": -0.7957467027850181, + "moduleForcesX": [ + 37.12351320058831, + 40.60313571714578, + 40.64272563549292, + 43.86282241005115 + ], + "moduleForcesY": [ + 44.90506437230603, + 41.789697152404564, + 41.740842496734864, + 38.349181908010294 + ], + "timestamp": 1.2377197297868463 + }, + { + "x": 1.6770049484081522, + "y": 5.606199701903595, + "heading": -0.0053851285855366264, + "angularVelocity": -0.16831034812082146, + "velocityX": -0.30433813627602885, + "velocityY": -0.5966291544563317, + "moduleForcesX": [ + 37.116682462766875, + 40.64090572081128, + 40.57759789476367, + 43.858892578344765 + ], + "moduleForcesY": [ + 44.920576626812874, + 41.76277059020072, + 41.815668830634316, + 38.365116284149615 + ], + "timestamp": 1.3261282819144782 + }, + { + "x": 1.6672069897589075, + "y": 5.571062249143735, + "heading": -0.015306267888284994, + "angularVelocity": -0.11221922612673975, + "velocityX": -0.1108259146140006, + "velocityY": -0.3974440471452618, + "moduleForcesX": [ + 37.111681394515514, + 40.665682324503166, + 40.533570764783825, + 43.855832002950336 + ], + "moduleForcesY": [ + 44.93173600136994, + 41.745649789594474, + 41.866559403537146, + 38.37680279508834 + ], + "timestamp": 1.41453683404211 + }, + { + "x": 1.6745149985934764, + "y": 5.553538932287859, + "heading": -0.0202677368718387, + "angularVelocity": -0.05611978552019538, + "velocityX": 0.08266178620387737, + "velocityY": -0.1982083908644825, + "moduleForcesX": [ + 37.106714226875006, + 40.67652070236513, + 40.509643285006895, + 43.85333935847226 + ], + "moduleForcesY": [ + 44.94109264735369, + 41.7403427171622, + 41.89586596082861, + 38.38580762158759 + ], + "timestamp": 1.502945386169742 + }, + { + "x": 1.6989272832870483, + "y": 5.553633213043213, + "heading": -0.020267736871838703, + "angularVelocity": -3.99357376917391e-18, + "velocityX": 0.2761303528456057, + "velocityY": 0.00106642121248368, + "moduleForcesX": [ + 37.10080834917557, + 40.672885944439045, + 40.50526475056877, + 43.851223740402496 + ], + "moduleForcesY": [ + 44.95004105053435, + 41.747969933406075, + 41.90488405097919, + 38.39302718242146 + ], + "timestamp": 1.5913539382973738 + }, + { + "x": 1.7476866611230668, + "y": 5.553821522782783, + "heading": -0.0202677368718387, + "angularVelocity": 5.910944865182772e-20, + "velocityX": 0.5533641655006356, + "velocityY": 0.0021371040098882763, + "moduleForcesX": [ + 58.27560878459361, + 58.27560878459361, + 58.27560878459361, + 58.27560878459361 + ], + "moduleForcesY": [ + 0.22506162302642355, + 0.22506162302642355, + 0.22506162302642416, + 0.22506162302642413 + ], + "timestamp": 1.6794683860262127 + }, + { + "x": 1.8208713864961603, + "y": 5.554104163726683, + "heading": -0.0202677368718387, + "angularVelocity": 5.910945255784143e-20, + "velocityX": 0.8305644222875951, + "velocityY": 0.003207657213826796, + "moduleForcesX": [ + 58.268555212659614, + 58.268555212659614, + 58.268555212659614, + 58.268555212659614 + ], + "moduleForcesY": [ + 0.2250343819837174, + 0.2250343819837174, + 0.2250343819837181, + 0.2250343819837181 + ], + "timestamp": 1.7675828337550517 + }, + { + "x": 1.9184768732294062, + "y": 5.554481118163, + "heading": -0.0202677368718387, + "angularVelocity": 5.910945067058074e-20, + "velocityX": 1.1077126311182772, + "velocityY": 0.004278009407466852, + "moduleForcesX": [ + 58.25761453298922, + 58.25761453298922, + 58.25761453298922, + 58.25761453298922 + ], + "moduleForcesY": [ + 0.22499212884943065, + 0.22499212884943065, + 0.22499212884943104, + 0.22499212884943104 + ], + "timestamp": 1.8556972814838906 + }, + { + "x": 2.040495046553733, + "y": 5.554952354906809, + "heading": -0.0202677368718387, + "angularVelocity": 5.910945105533479e-20, + "velocityX": 1.3847692003906387, + "velocityY": 0.005348007687211211, + "moduleForcesX": [ + 58.238351546996455, + 58.238351546996455, + 58.238351546996455, + 58.238351546996455 + ], + "moduleForcesY": [ + 0.22491773479362803, + 0.22491773479362803, + 0.2249177347936283, + 0.2249177347936283 + ], + "timestamp": 1.9438117292127295 + }, + { + "x": 2.186907919553257, + "y": 5.555517804492261, + "heading": -0.0202677368718387, + "angularVelocity": 5.910945033275698e-20, + "velocityX": 1.6616216383729834, + "velocityY": 0.006417217607633379, + "moduleForcesX": [ + 58.195442368336856, + 58.195442368336856, + 58.195442368336856, + 58.195442368336856 + ], + "moduleForcesY": [ + 0.22475201864594074, + 0.22475201864594074, + 0.22475201864594233, + 0.22475201864594233 + ], + "timestamp": 2.0319261769415684 + }, + { + "x": 2.3576403392711733, + "y": 5.556177176677063, + "heading": -0.0202677368718387, + "angularVelocity": 5.910945066362e-20, + "velocityX": 1.9376211747172734, + "velocityY": 0.007483133604046069, + "moduleForcesX": [ + 58.016159178761185, + 58.016159178761185, + 58.016159178761185, + 58.016159178761185 + ], + "moduleForcesY": [ + 0.22405962320866032, + 0.2240596232086603, + 0.22405962320865966, + 0.22405962320865963 + ], + "timestamp": 2.1200406246704073 + }, + { + "x": 2.504136867684685, + "y": 5.556742949341478, + "heading": -0.020267736871838696, + "angularVelocity": 5.910945066536768e-20, + "velocityX": 1.6625710333489974, + "velocityY": 0.006420884190937289, + "moduleForcesX": [ + -57.81659272013044, + -57.81659272013044, + -57.81659272013044, + -57.81659272013044 + ], + "moduleForcesY": [ + -0.22328889336099703, + -0.22328889336099703, + -0.22328889336099597, + -0.22328889336099597 + ], + "timestamp": 2.2081550723992462 + }, + { + "x": 2.626247488507551, + "y": 5.557214543119464, + "heading": -0.020267736871838696, + "angularVelocity": 5.910945018736598e-20, + "velocityX": 1.3858183756498825, + "velocityY": 0.005352059624061109, + "moduleForcesX": [ + -58.17446817080217, + -58.17446817080217, + -58.17446817080217, + -58.17446817080217 + ], + "moduleForcesY": [ + -0.22467101585528326, + -0.2246710158552833, + -0.22467101585528407, + -0.22467101585528407 + ], + "timestamp": 2.296269520128085 + }, + { + "x": 2.7239485756809234, + "y": 5.557591866766686, + "heading": -0.020267736871838696, + "angularVelocity": 5.910945114498727e-20, + "velocityX": 1.108797588722742, + "velocityY": 0.0042821995364842646, + "moduleForcesX": [ + -58.2308299610416, + -58.2308299610416, + -58.2308299610416, + -58.2308299610416 + ], + "moduleForcesY": [ + -0.2248886862709617, + -0.22488868627096165, + -0.22488868627095934, + -0.2248886862709593 + ], + "timestamp": 2.3843839678569236 + }, + { + "x": 2.7972305085837634, + "y": 5.557874883128104, + "heading": -0.020267736871838696, + "angularVelocity": 5.910945064257137e-20, + "velocityX": 0.8316676185539519, + "velocityY": 0.003211917780938857, + "moduleForcesX": [ + -58.253780696434475, + -58.253780696434475, + -58.253780696434475, + -58.253780696434475 + ], + "moduleForcesY": [ + -0.2249773224922699, + -0.2249773224922699, + -0.22497732249226998, + -0.22497732249226998 + ], + "timestamp": 2.4724984155857626 + }, + { + "x": 2.846088065780519, + "y": 5.558063572038424, + "heading": -0.020267736871838696, + "angularVelocity": 5.910945075970797e-20, + "velocityX": 0.5544783909570489, + "velocityY": 0.0021414071719635906, + "moduleForcesX": [ + -58.266236834685465, + -58.266236834685465, + -58.266236834685465, + -58.266236834685465 + ], + "moduleForcesY": [ + -0.22502542835937098, + -0.22502542835937098, + -0.22502542835937084, + -0.22502542835937084 + ], + "timestamp": 2.5606128633146015 + }, + { + "x": 2.8705179691314697, + "y": 5.558157920837402, + "heading": -0.020267736871838693, + "angularVelocity": 5.910945064090654e-20, + "velocityX": 0.2772519601567551, + "velocityY": 0.0010707528834367668, + "moduleForcesX": [ + -58.27405709046732, + -58.27405709046732, + -58.27405709046732, + -58.27405709046732 + ], + "moduleForcesY": [ + -0.2250556303511726, + -0.22505563035117257, + -0.2250556303511722, + -0.2250556303511722 + ], + "timestamp": 2.6487273110434404 + }, + { + "x": 2.8705179691314697, + "y": 5.558157920837402, + "heading": -0.020267736871838693, + "angularVelocity": 1.9702981537661184e-20, + "velocityX": 1.653032544870731e-18, + "velocityY": -5.96028342388569e-21, + "moduleForcesX": [ + -58.279423458932314, + -58.279423458932314, + -58.279423458932314, + -58.279423458932314 + ], + "moduleForcesY": [ + -0.2250763553787051, + -0.22507635537870505, + -0.2250763553787032, + -0.22507635537870316 + ], + "timestamp": 2.7368417587722793 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/amp 4 local.3.traj b/src/main/deploy/choreo/amp 4 local.3.traj index 9414a5a6..23fab288 100644 --- a/src/main/deploy/choreo/amp 4 local.3.traj +++ b/src/main/deploy/choreo/amp 4 local.3.traj @@ -1,211 +1,530 @@ { - "samples": [ - { - "x": 2.8705179691314697, - "y": 5.558157920837402, - "heading": -0.020267736871838693, - "angularVelocity": -3.4830373991096207e-20, - "velocityX": 7.082195607551611e-16, - "velocityY": 1.9869518370590596e-18, - "timestamp": 0 - }, - { - "x": 2.8404610494153317, - "y": 5.534143912451309, - "heading": -0.03485317920513617, - "angularVelocity": -0.15571220333723793, - "velocityX": -0.32088359664250343, - "velocityY": -0.25637029521011345, - "timestamp": 0.09366923093181523 - }, - { - "x": 2.7804008780683063, - "y": 5.486049425013135, - "heading": -0.06402371208298976, - "angularVelocity": -0.3114206510256051, - "velocityX": -0.6411942400887777, - "velocityY": -0.5134502222312821, - "timestamp": 0.18733846186363046 - }, - { - "x": 2.690426928059957, - "y": 5.4137641868079385, - "heading": -0.10777934842034757, - "angularVelocity": -0.46712923659220595, - "velocityX": -0.9605496822520554, - "velocityY": -0.7717073951190555, - "timestamp": 0.28100769279544524 - }, - { - "x": 2.5707182617497417, - "y": 5.317070431884298, - "heading": -0.1661422083280734, - "angularVelocity": -0.6230739734610408, - "velocityX": -1.2779934789616767, - "velocityY": -1.0322894077568259, - "timestamp": 0.37467692372726047 - }, - { - "x": 2.4218123673171084, - "y": 5.195334657114955, - "heading": -0.239237988952977, - "angularVelocity": -0.7803606360141083, - "velocityX": -1.5896991247961374, - "velocityY": -1.299634613824878, - "timestamp": 0.4683461546590757 - }, - { - "x": 2.2812263727256172, - "y": 5.035989894533815, - "heading": -0.32521378731763706, - "angularVelocity": -0.9178659577897541, - "velocityX": -1.500877002970466, - "velocityY": -1.7011430647608603, - "timestamp": 0.5620153855908909 - }, - { - "x": 2.1719619380449973, - "y": 4.899006132834784, - "heading": -0.39709906634612513, - "angularVelocity": -0.7674374852166317, - "velocityX": -1.1664922792005954, - "velocityY": -1.4624200533763867, - "timestamp": 0.6556846165227062 - }, - { - "x": 2.0934918282245056, - "y": 4.785094183316171, - "heading": -0.4547023997403661, - "angularVelocity": -0.6149653714587694, - "velocityX": -0.8377362452950287, - "velocityY": -1.2161085170170074, - "timestamp": 0.7493538474545214 - }, - { - "x": 2.045638672399353, - "y": 4.694485163192149, - "heading": -0.4979506864820333, - "angularVelocity": -0.46171284114790145, - "velocityX": -0.5108737986755416, - "velocityY": -0.9673296046380411, - "timestamp": 0.8430230783863366 - }, - { - "x": 2.028313474321192, - "y": 4.627293480949165, - "heading": -0.5268031269208548, - "angularVelocity": -0.3080247393065947, - "velocityX": -0.18496146392802787, - "velocityY": -0.7173292827811862, - "timestamp": 0.9366923093181518 - }, - { - "x": 2.041462835791116, - "y": 4.583587383113917, - "heading": -0.5412356183232561, - "angularVelocity": -0.15407931995200425, - "velocityX": 0.14038079889324448, - "velocityY": -0.46660037026527035, - "timestamp": 1.030361540249967 - }, - { - "x": 2.0850512981414795, - "y": 4.563412189483643, - "heading": -0.5412356183232561, - "angularVelocity": -1.875746881389731e-20, - "velocityX": 0.465344509789913, - "velocityY": -0.2153876297432254, - "timestamp": 1.1240307711817823 - }, - { - "x": 2.1256328053913753, - "y": 4.544628783911912, - "heading": -0.5412356183232561, - "angularVelocity": 1.7413793657350135e-22, - "velocityX": 0.6980165405938983, - "velocityY": -0.3230813408929243, - "timestamp": 1.1821690886711749 - }, - { - "x": 2.1797414712659067, - "y": 4.519584247990461, - "heading": -0.5412356183232561, - "angularVelocity": 1.7413789738422444e-22, - "velocityX": 0.930688540899089, - "velocityY": -0.4307750379260645, - "timestamp": 1.2403074061605674 - }, - { - "x": 2.2473772913410164, - "y": 4.488278583766992, - "heading": -0.5412356183232561, - "angularVelocity": 1.7413788470930633e-22, - "velocityX": 1.1633604651089158, - "velocityY": -0.5384686997379875, - "timestamp": 1.29844572364996 - }, - { - "x": 2.328540234996748, - "y": 4.450711805414148, - "heading": -0.5412356183232561, - "angularVelocity": 1.7413787765697518e-22, - "velocityX": 1.3960318626444466, - "velocityY": -0.646162117775404, - "timestamp": 1.3565840411393526 - }, - { - "x": 2.396176035002005, - "y": 4.419406150480137, - "heading": -0.5412356183232561, - "angularVelocity": 1.7413789176163748e-22, - "velocityX": 1.1633601199002148, - "velocityY": -0.5384685399559715, - "timestamp": 1.4147223586287452 - }, - { - "x": 2.45028467878785, - "y": 4.394361624782573, - "heading": -0.5412356183232561, - "angularVelocity": 1.7413789463080967e-22, - "velocityX": 0.930688160965733, - "velocityY": -0.4307748620715239, - "timestamp": 1.4728606761181378 - }, - { - "x": 2.490866163227744, - "y": 4.375578229768594, - "heading": -0.5412356183232561, - "angularVelocity": 1.741380781979936e-22, - "velocityX": 0.6980161482536459, - "velocityY": -0.3230811592957737, - "timestamp": 1.5309989936075303 - }, - { - "x": 2.5179204869025957, - "y": 4.363055966095035, - "heading": -0.5412356183232561, - "angularVelocity": 1.7413772411298374e-22, - "velocityX": 0.465344111132695, - "velocityY": -0.2153874452222276, - "timestamp": 1.589137311096923 - }, - { - "x": 2.531447649002075, - "y": 4.356794834136963, - "heading": -0.5412356183232561, - "angularVelocity": 1.7413769362387268e-22, - "velocityX": 0.23267206007376748, - "velocityY": -0.10769372469740146, - "timestamp": 1.6472756285863155 - }, - { - "x": 2.531447649002075, - "y": 4.356794834136963, - "heading": -0.5412356183232561, - "angularVelocity": 5.804601738611348e-23, - "velocityX": 1.3725068175106304e-19, - "velocityY": -2.6971290693406307e-19, - "timestamp": 1.705413946075708 - } - ] + "samples": [ + { + "x": 2.8705179691314697, + "y": 5.558157920837402, + "heading": -0.020267736871838693, + "angularVelocity": 1.9702981537661184e-20, + "velocityX": 1.653032544870731e-18, + "velocityY": -5.96028342388569e-21, + "moduleForcesX": [ + -58.279423458932314, + -58.279423458932314, + -58.279423458932314, + -58.279423458932314 + ], + "moduleForcesY": [ + -0.2250763553787051, + -0.22507635537870505, + -0.2250763553787032, + -0.22507635537870316 + ], + "timestamp": 0 + }, + { + "x": 2.8439249599428864, + "y": 5.537645538216833, + "heading": -0.032906727963708735, + "angularVelocity": -0.12199820814045388, + "velocityX": -0.25668975050996623, + "velocityY": -0.1979963358001305, + "moduleForcesX": [ + -40.264650278600634, + -45.64387186701001, + -46.79690535956238, + -50.862712927454936 + ], + "moduleForcesY": [ + -42.14146610565898, + -36.25113454290553, + -34.737790742143396, + -28.463961726608957 + ], + "timestamp": 0.10359980924735446 + }, + { + "x": 2.790863562177045, + "y": 5.496466451968379, + "heading": -0.058207005960777755, + "angularVelocity": -0.2442116272305314, + "velocityX": -0.512176597151375, + "velocityY": -0.39748225935566833, + "moduleForcesX": [ + -40.004992650057915, + -45.57335006506032, + -46.39579797648287, + -50.73375960353931 + ], + "moduleForcesY": [ + -42.381446517676004, + -36.332917243845905, + -35.26208895782567, + -28.68315856947041 + ], + "timestamp": 0.20719961849470891 + }, + { + "x": 2.7115257184978163, + "y": 5.434386408788842, + "heading": -0.09620042670910543, + "angularVelocity": -0.36673253574835096, + "velocityX": -0.7658107119657117, + "velocityY": -0.5992293193447437, + "moduleForcesX": [ + -39.621387787708365, + -45.46476663407919, + -45.77146561371144, + -50.52532460582109 + ], + "moduleForcesY": [ + -42.73042746349606, + -36.458316770771056, + -36.05441082201013, + -29.03347681690111 + ], + "timestamp": 0.31079942774206337 + }, + { + "x": 2.606245266180635, + "y": 5.351008207802818, + "heading": -0.14695247874407716, + "angularVelocity": -0.4898855741500109, + "velocityX": -1.0162224533233901, + "velocityY": -0.804810371676945, + "moduleForcesX": [ + -38.975029944558344, + -45.212668298575124, + -44.75754242271995, + -50.13326797035515 + ], + "moduleForcesY": [ + -43.30431649298782, + -36.752852470976016, + -37.281821264300945, + -29.679468474626695 + ], + "timestamp": 0.41439923698941783 + }, + { + "x": 2.475745367457033, + "y": 5.245512799725402, + "heading": -0.2106299193340097, + "angularVelocity": -0.6146482416574408, + "velocityX": -1.2596538514083622, + "velocityY": -1.0182973196942513, + "moduleForcesX": [ + -37.59665158673427, + -44.46910269089497, + -42.8065712580459, + -49.21428672911071 + ], + "moduleForcesY": [ + -44.47297089839765, + -37.612897024695, + -39.459327933877574, + -31.127055280117744 + ], + "timestamp": 0.5179990462367723 + }, + { + "x": 2.3227167278281233, + "y": 5.1152633537373084, + "heading": -0.28781914697582284, + "angularVelocity": -0.745071136738453, + "velocityX": -1.4771131408508664, + "velocityY": -1.2572363495101748, + "moduleForcesX": [ + -32.57681783967148, + -41.03171735775828, + -36.4212961306049, + -45.48318507972898 + ], + "moduleForcesY": [ + -48.168748844471274, + -41.22246689674932, + -45.27956258136952, + -36.203178634964225 + ], + "timestamp": 0.6215988554841267 + }, + { + "x": 2.203106950739979, + "y": 4.983538065028958, + "heading": -0.35945125169862574, + "angularVelocity": -0.691430855357799, + "velocityX": -1.1545366536589345, + "velocityY": -1.2714819618426536, + "moduleForcesX": [ + 57.768566863317616, + 57.810702012324775, + 57.59277430823837, + 57.5140897196349 + ], + "moduleForcesY": [ + 0.8932781027881612, + -0.9152785781841101, + -4.374216376079367, + -5.791336644532758 + ], + "timestamp": 0.7251986647314812 + }, + { + "x": 2.1130561975087887, + "y": 4.867723147947118, + "heading": -0.4197404118462817, + "angularVelocity": -0.5819427717642794, + "velocityX": -0.8692173652191356, + "velocityY": -1.1179066633734913, + "moduleForcesX": [ + 47.18108856870023, + 51.63439597749474, + 50.708340450422064, + 54.51833525481436 + ], + "moduleForcesY": [ + 34.07484296534565, + 26.874618698432997, + 28.520495989083397, + 20.35730229951914 + ], + "timestamp": 0.8287984739788357 + }, + { + "x": 2.051552972727542, + "y": 4.7695988413626225, + "heading": -0.46820764413999405, + "angularVelocity": -0.46783128893598946, + "velocityX": -0.5936615639359138, + "velocityY": -0.9471475603803009, + "moduleForcesX": [ + 45.02951499832376, + 50.355649146304515, + 48.32912609649593, + 53.34564661451518 + ], + "moduleForcesY": [ + 36.95043417621518, + 29.289508389016593, + 32.4935418039393, + 23.38253676579243 + ], + "timestamp": 0.9323982832261901 + }, + { + "x": 2.018185626236221, + "y": 4.689823401404505, + "heading": -0.5046715625693844, + "angularVelocity": -0.35196897266798394, + "velocityX": -0.32207922711183234, + "velocityY": -0.7700346220488296, + "moduleForcesX": [ + 44.230946349601425, + 49.9139754803189, + 47.23993285880785, + 52.83351377397607 + ], + "moduleForcesY": [ + 37.9315483504293, + 30.06878585422672, + 34.0958495054322, + 24.563695274073133 + ], + "timestamp": 1.0359980924735446 + }, + { + "x": 2.0127316672603737, + "y": 4.628738886692783, + "heading": -0.5290344166885111, + "angularVelocity": -0.23516311753970603, + "velocityX": -0.05264448858998858, + "velocityY": -0.589619953506647, + "moduleForcesX": [ + 43.82380015207529, + 49.697676016896175, + 46.61701661238326, + 52.544050328156345 + ], + "moduleForcesY": [ + 38.41604200389433, + 30.441960825176444, + 34.962319678036984, + 25.200743372075255 + ], + "timestamp": 1.139597901720899 + }, + { + "x": 2.0350519350972927, + "y": 4.586554708774609, + "heading": -0.5412356183232561, + "angularVelocity": -0.117772433399114, + "velocityX": 0.2154469974324671, + "velocityY": -0.40718393426242, + "moduleForcesX": [ + 43.56673373099433, + 49.55277078584493, + 46.24089930341583, + 52.361530761045266 + ], + "moduleForcesY": [ + 38.71631589559412, + 30.687678310009126, + 35.47018296719196, + 25.592429826745295 + ], + "timestamp": 1.2431977109682535 + }, + { + "x": 2.0850512981414795, + "y": 4.563412189483643, + "heading": -0.5412356183232561, + "angularVelocity": 2.0482990469860628e-24, + "velocityX": 0.48262022302385593, + "velocityY": -0.22338380214302983, + "moduleForcesX": [ + 43.37104440974198, + 49.42269040291651, + 46.02875544788031, + 52.24276301802586 + ], + "moduleForcesY": [ + 38.941407034185524, + 30.903759210330502, + 35.75302989110585, + 25.843937402615378 + ], + "timestamp": 1.346797520215608 + }, + { + "x": 2.1250808762861264, + "y": 4.544884247765605, + "heading": -0.5412356183232561, + "angularVelocity": 4.429972765944174e-25, + "velocityX": 0.6566001115017817, + "velocityY": -0.30391148650137534, + "moduleForcesX": [ + 52.857513824275856, + 52.857513824275856, + 52.857513824275856, + 52.857513824275856 + ], + "moduleForcesY": [ + -24.465432335003626, + -24.465432335003626, + -24.465432335003626, + -24.465432335003626 + ], + "timestamp": 1.4077624476401418 + }, + { + "x": 2.1757132373979986, + "y": 4.521448741342388, + "heading": -0.5412356183232561, + "angularVelocity": 4.426850844156769e-25, + "velocityX": 0.8305162205687572, + "velocityY": -0.38440965015868744, + "moduleForcesX": [ + 52.83813675072153, + 52.83813675072153, + 52.83813675072153, + 52.83813675072153 + ], + "moduleForcesY": [ + -24.45646353477834, + -24.45646353477834, + -24.45646353477834, + -24.45646353477834 + ], + "timestamp": 1.4687273750646757 + }, + { + "x": 2.2369405764562798, + "y": 4.493109282816916, + "heading": -0.5412356183232561, + "angularVelocity": 4.433094585405618e-25, + "velocityX": 1.0043043048656628, + "velocityY": -0.4648485567468957, + "moduleForcesX": [ + 52.79924104206797, + 52.799241042067976, + 52.79924104206797, + 52.799241042067976 + ], + "moduleForcesY": [ + -24.438460411677465, + -24.438460411677465, + -24.43846041167746, + -24.438460411677465 + ], + "timestamp": 1.5296923024892095 + }, + { + "x": 2.308739248778686, + "y": 4.459876816278918, + "heading": -0.5412356183232561, + "angularVelocity": 4.429972551089785e-25, + "velocityX": 1.1777045484271702, + "velocityY": -0.5451079488142321, + "moduleForcesX": [ + 52.681409623665104, + 52.681409623665104, + 52.681409623665104, + 52.681409623665104 + ], + "moduleForcesY": [ + -24.38392139942926, + -24.38392139942926, + -24.38392139942926, + -24.38392139942926 + ], + "timestamp": 1.5906572299137434 + }, + { + "x": 2.372336860863181, + "y": 4.430440261992736, + "heading": -0.5412356183232561, + "angularVelocity": 4.429972551089667e-25, + "velocityX": 1.0431835937674017, + "velocityY": -0.482844079862508, + "moduleForcesX": [ + -40.86934002998661, + -40.86934002998661, + -40.86934002998661, + -40.86934002998661 + ], + "moduleForcesY": [ + 18.91663078221956, + 18.91663078221956, + 18.91663078221956, + 18.91663078221956 + ], + "timestamp": 1.6516221573382772 + }, + { + "x": 2.4253616014568182, + "y": 4.405897427696982, + "heading": -0.5412356183232561, + "angularVelocity": 4.43092756165992e-25, + "velocityX": 0.8697581188671928, + "velocityY": -0.4025730093115453, + "moduleForcesX": [ + -52.68907525585909, + -52.68907525585909, + -52.68907525585909, + -52.68907525585909 + ], + "moduleForcesY": [ + 24.38746948544725, + 24.38746948544725, + 24.38746948544725, + 24.38746948544725 + ], + "timestamp": 1.712587084762811 + }, + { + "x": 2.4677909785651333, + "y": 4.386258723952594, + "heading": -0.5412356183232561, + "angularVelocity": 4.429017210736591e-25, + "velocityX": 0.6959637106242286, + "velocityY": -0.3221311756451756, + "moduleForcesX": [ + -52.801162344979105, + -52.801162344979105, + -52.801162344979105, + -52.801162344979105 + ], + "moduleForcesY": [ + 24.439349698799905, + 24.439349698799905, + 24.439349698799905, + 24.439349698799905 + ], + "timestamp": 1.773552012187345 + }, + { + "x": 2.4996174011461876, + "y": 4.371527664321022, + "heading": -0.5412356183232561, + "angularVelocity": 4.392830640142467e-25, + "velocityX": 0.5220447874796567, + "velocityY": -0.24163170947436932, + "moduleForcesX": [ + -52.838991706698124, + -52.838991706698124, + -52.838991706698124, + -52.838991706698124 + ], + "moduleForcesY": [ + 24.45685925652313, + 24.45685925652313, + 24.45685925652313, + 24.45685925652313 + ], + "timestamp": 1.8345169396118788 + }, + { + "x": 2.520837055868033, + "y": 4.361706013826914, + "heading": -0.5412356183232561, + "angularVelocity": 4.46711444507948e-25, + "velocityX": 0.34806331473308955, + "velocityY": -0.16110329182733052, + "moduleForcesX": [ + -52.85799514715946, + -52.85799514715946, + -52.85799514715946, + -52.85799514715946 + ], + "moduleForcesY": [ + 24.465655118324054, + 24.465655118324058, + 24.46565511832406, + 24.465655118324058 + ], + "timestamp": 1.8954818670364126 + }, + { + "x": 2.531447649002075, + "y": 4.356794834136963, + "heading": -0.5412356183232561, + "angularVelocity": 4.42997254240294e-25, + "velocityX": 0.17404421824625813, + "velocityY": -0.08055745979575053, + "moduleForcesX": [ + -52.86942576358611, + -52.86942576358611, + -52.86942576358611, + -52.86942576358611 + ], + "moduleForcesY": [ + 24.470945850946546, + 24.470945850946546, + 24.470945850946546, + 24.470945850946546 + ], + "timestamp": 1.9564467944609465 + }, + { + "x": 2.531447649002075, + "y": 4.356794834136963, + "heading": -0.5412356183232561, + "angularVelocity": 1.4766575121958547e-25, + "velocityX": 1.1572658590616387e-19, + "velocityY": 2.5061165774890407e-19, + "moduleForcesX": [ + -52.87705810406985, + -52.87705810406985, + -52.87705810406985, + -52.87705810406985 + ], + "moduleForcesY": [ + 24.474478527687335, + 24.474478527687335, + 24.474478527687335, + 24.474478527687335 + ], + "timestamp": 2.0174117218854803 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/amp 4 local.traj b/src/main/deploy/choreo/amp 4 local.traj index af8e65c5..c05b6795 100644 --- a/src/main/deploy/choreo/amp 4 local.traj +++ b/src/main/deploy/choreo/amp 4 local.traj @@ -1,697 +1,1832 @@ { - "samples": [ - { - "x": 0.7470832467079163, - "y": 6.625458717346191, - "heading": 1.056495497952334, - "angularVelocity": -1.4679265201702596e-22, - "velocityX": 9.483029461049396e-23, - "velocityY": 5.2317038665130075e-22, - "timestamp": 0 - }, - { - "x": 0.7547543197229795, - "y": 6.625912789866557, - "heading": 1.0484306824677847, - "angularVelocity": -0.1891115926088127, - "velocityX": 0.17987873841334215, - "velocityY": 0.010647531571001236, - "timestamp": 0.04264580173692322 - }, - { - "x": 0.7701156825333854, - "y": 6.626850739979297, - "heading": 1.0325597341852575, - "angularVelocity": -0.37215734342229334, - "velocityX": 0.3602080904743721, - "velocityY": 0.021993961293704876, - "timestamp": 0.08529160347384644 - }, - { - "x": 0.7931884474144321, - "y": 6.628306307811922, - "heading": 1.0091781276812968, - "angularVelocity": -0.5482745206245366, - "velocityX": 0.5410325035833482, - "velocityY": 0.03413156215481, - "timestamp": 0.12793740521076966 - }, - { - "x": 0.8239951854040302, - "y": 6.630316938342184, - "heading": 0.9786149775936921, - "angularVelocity": -0.7166742995276547, - "velocityX": 0.7223861842167063, - "velocityY": 0.0471472090656353, - "timestamp": 0.17058320694769288 - }, - { - "x": 0.8625597183369016, - "y": 6.63292352204424, - "heading": 0.9412340847359876, - "angularVelocity": -0.8765433251390755, - "velocityX": 0.9042984622676677, - "velocityY": 0.06112169535784003, - "timestamp": 0.2132290086846161 - }, - { - "x": 0.9089072743014975, - "y": 6.636170316792377, - "heading": 0.8974431328952391, - "angularVelocity": -1.0268525870586154, - "velocityX": 1.086802312933599, - "velocityY": 0.07613398308622592, - "timestamp": 0.2558748104215393 - }, - { - "x": 0.9630650383734942, - "y": 6.64010548361246, - "heading": 0.8477159062035999, - "angularVelocity": -1.1660521004717048, - "velocityX": 1.269943625543481, - "velocityY": 0.0922755971234286, - "timestamp": 0.29852061215846254 - }, - { - "x": 1.0250628809506594, - "y": 6.644782896884854, - "heading": 0.7926327969571765, - "angularVelocity": -1.2916420140539158, - "velocityX": 1.4537853681265616, - "velocityY": 0.10968050973102496, - "timestamp": 0.34116641389538577 - }, - { - "x": 1.094933572481574, - "y": 6.650266047499182, - "heading": 0.7329446248446481, - "angularVelocity": -1.3996259814913892, - "velocityX": 1.6383955438788211, - "velocityY": 0.1285742181177263, - "timestamp": 0.383812215632309 - }, - { - "x": 1.1727109614146445, - "y": 6.6566348315708534, - "heading": 0.6696642878742429, - "angularVelocity": -1.4838585369029722, - "velocityX": 1.8237994307826475, - "velocityY": 0.149341407882537, - "timestamp": 0.4264580173692322 - }, - { - "x": 1.2584230921899875, - "y": 6.663995676876955, - "heading": 0.6041925063971192, - "angularVelocity": -1.5352456469457696, - "velocityX": 2.0098609308388866, - "velocityY": 0.17260421908608822, - "timestamp": 0.46910381910615545 - }, - { - "x": 1.3520739703395739, - "y": 6.67249503645535, - "heading": 0.5385009897728867, - "angularVelocity": -1.5403982091713377, - "velocityX": 2.19601635648234, - "velocityY": 0.19930120274971816, - "timestamp": 0.5117496208430786 - }, - { - "x": 1.453595786870469, - "y": 6.682338668837949, - "heading": 0.4754960223298961, - "angularVelocity": -1.4774014059264435, - "velocityX": 2.3805817312843804, - "velocityY": 0.23082301144958858, - "timestamp": 0.5543954225800019 - }, - { - "x": 1.5626754965988896, - "y": 6.6938349914873125, - "heading": 0.42023075468632287, - "angularVelocity": -1.2959134403076285, - "velocityX": 2.5578065198848923, - "velocityY": 0.2695768910684973, - "timestamp": 0.5970412243169251 - }, - { - "x": 1.6778273819318938, - "y": 6.707315267421264, - "heading": 0.3846859068543657, - "angularVelocity": -0.8334899658172448, - "velocityX": 2.7001927655941973, - "velocityY": 0.3160985462791911, - "timestamp": 0.6396870260538483 - }, - { - "x": 1.7956033945083618, - "y": 6.721975326538086, - "heading": 0.3846859068543657, - "angularVelocity": -2.7339775014955035e-22, - "velocityX": 2.7617258388765618, - "velocityY": 0.34376324326736624, - "timestamp": 0.6823328277907715 - }, - { - "x": 1.9702980732827642, - "y": 6.757682275421422, - "heading": 0.3846859068543657, - "angularVelocity": -3.55936785030336e-23, - "velocityX": 2.8736582107292628, - "velocityY": 0.587365153641545, - "timestamp": 0.7431245663914694 - }, - { - "x": 2.128777504610528, - "y": 6.795024747259092, - "heading": 0.3846859068543657, - "angularVelocity": -3.5593719407474505e-23, - "velocityX": 2.606923818525971, - "velocityY": 0.6142688578615821, - "timestamp": 0.8039163049921673 - }, - { - "x": 2.2710818815122873, - "y": 6.830372862436407, - "heading": 0.3846859068543657, - "angularVelocity": -3.5593679346398325e-23, - "velocityX": 2.340850585578186, - "velocityY": 0.5814624814318021, - "timestamp": 0.8647080435928651 - }, - { - "x": 2.397370829093984, - "y": 6.862701695876428, - "heading": 0.3846859068543657, - "angularVelocity": -3.5593640063590435e-23, - "velocityX": 2.0774031223421394, - "velocityY": 0.5317964938026807, - "timestamp": 0.925499782193563 - }, - { - "x": 2.507741549731751, - "y": 6.891534791754925, - "heading": 0.3846859068543657, - "angularVelocity": -3.5593769660186765e-23, - "velocityX": 1.8155545996590319, - "velocityY": 0.47429299674884, - "timestamp": 0.9862915207942609 - }, - { - "x": 2.6022568329985316, - "y": 6.916597570791243, - "heading": 0.3846859068543657, - "angularVelocity": -3.559368485205426e-23, - "velocityX": 1.5547389405588703, - "velocityY": 0.41227277938109086, - "timestamp": 1.0470832593949586 - }, - { - "x": 2.6809601695719074, - "y": 6.937711608282982, - "heading": 0.3846859068543657, - "angularVelocity": -3.559368363101467e-23, - "velocityX": 1.2946386858636783, - "velocityY": 0.3473175463926733, - "timestamp": 1.1078749979956564 - }, - { - "x": 2.7438833546228816, - "y": 6.954751696784365, - "heading": 0.3846859068543657, - "angularVelocity": -3.559362874104829e-23, - "velocityX": 1.0350614491267762, - "velocityY": 0.28030270055785556, - "timestamp": 1.1686667365963541 - }, - { - "x": 2.791050608826443, - "y": 6.967625143508777, - "heading": 0.3846859068543657, - "angularVelocity": -3.5593782108627554e-23, - "velocityX": 0.775882632891501, - "velocityY": 0.21176309512990737, - "timestamp": 1.2294584751970519 - }, - { - "x": 2.8224809815825402, - "y": 6.976260567480704, - "heading": 0.3846859068543657, - "angularVelocity": -3.5593532451099235e-23, - "velocityX": 0.5170171717335424, - "velocityY": 0.14204930095266424, - "timestamp": 1.2902502137977496 - }, - { - "x": 2.8381898403167725, - "y": 6.9806013107299805, - "heading": 0.3846859068543657, - "angularVelocity": -3.55938793253341e-23, - "velocityX": 0.2584044986344835, - "velocityY": 0.07140350562744292, - "timestamp": 1.3510419523984474 - }, - { - "x": 2.8381898403167725, - "y": 6.9806013107299805, - "heading": 0.3846859068543657, - "angularVelocity": -5.573807154532037e-19, - "velocityX": -9.35615466765533e-16, - "velocityY": -9.896680587980253e-18, - "timestamp": 1.4118336909991451 - }, - { - "x": 2.8128886138231195, - "y": 6.955100739151656, - "heading": 0.3773890772908297, - "angularVelocity": -0.08078200568400537, - "velocityX": -0.2801057369692363, - "velocityY": -0.2823126537693574, - "timestamp": 1.5021611036665428 - }, - { - "x": 2.7622861408558537, - "y": 6.904099615654192, - "heading": 0.3627975933198322, - "angularVelocity": -0.16153993057112973, - "velocityX": -0.5602116951334734, - "velocityY": -0.564625089894962, - "timestamp": 1.5924885163339404 - }, - { - "x": 2.6863823744143596, - "y": 6.827597945234058, - "heading": 0.3409166257799037, - "angularVelocity": -0.24224060995125085, - "velocityX": -0.8403181736340153, - "velocityY": -0.8469374707052851, - "timestamp": 1.682815929001338 - }, - { - "x": 2.585177249564827, - "y": 6.725595708318326, - "heading": 0.311754466827316, - "angularVelocity": -0.32284948822656157, - "velocityX": -1.1204253710064276, - "velocityY": -1.1292500682040065, - "timestamp": 1.7731433416687357 - }, - { - "x": 2.4586707006639994, - "y": 6.598092843845294, - "heading": 0.27532262336823443, - "angularVelocity": -0.40333097542716767, - "velocityX": -1.4005332951050296, - "velocityY": -1.4115633417063895, - "timestamp": 1.8634707543361333 - }, - { - "x": 2.306862702051818, - "y": 6.445089212010339, - "heading": 0.2316358112414208, - "angularVelocity": -0.4836495459854555, - "velocityX": -1.6806415032828872, - "velocityY": -1.6938781629619528, - "timestamp": 1.953798167003531 - }, - { - "x": 2.1297534370368134, - "y": 6.266584440083468, - "heading": 0.18071175454391936, - "angularVelocity": -0.5637718959659755, - "velocityX": -1.960747682084755, - "velocityY": -1.9761971106474765, - "timestamp": 2.0441255796709283 - }, - { - "x": 1.9873417449288229, - "y": 6.088213430389112, - "heading": 0.1304328979304444, - "angularVelocity": -0.5566289914548207, - "velocityX": -1.5766165320272711, - "velocityY": -1.974716250895523, - "timestamp": 2.1344529923383257 - }, - { - "x": 1.870232723728645, - "y": 5.935342556724319, - "heading": 0.08735848478659979, - "angularVelocity": -0.47686977709234435, - "velocityX": -1.2964948041918032, - "velocityY": -1.692408419008837, - "timestamp": 2.224780405005723 - }, - { - "x": 1.7784261170686546, - "y": 5.807972234685134, - "heading": 0.05147629980122907, - "angularVelocity": -0.3972457964408093, - "velocityX": -1.0163759145574802, - "velocityY": -1.410095986129178, - "timestamp": 2.3151078176731206 - }, - { - "x": 1.7119218452051854, - "y": 5.706102649506714, - "heading": 0.022777291492800653, - "angularVelocity": -0.31772202325989357, - "velocityX": -0.736257907751286, - "velocityY": -1.1277815025423439, - "timestamp": 2.405435230340518 - }, - { - "x": 1.6707198619792896, - "y": 5.629733910265785, - "heading": 0.0012553801790747192, - "angularVelocity": -0.23826555724598256, - "velocityX": -0.4561404119627787, - "velocityY": -0.8454658113853617, - "timestamp": 2.4957626430079154 - }, - { - "x": 1.6548201302688181, - "y": 5.578866079528388, - "heading": -0.01309273794263427, - "angularVelocity": -0.15884566709086995, - "velocityX": -0.17602332714722002, - "velocityY": -0.5631494275703062, - "timestamp": 2.586090055675313 - }, - { - "x": 1.664222615534791, - "y": 5.55349918288868, - "heading": -0.020267736871838676, - "angularVelocity": -0.079433238674075, - "velocityX": 0.10409337529313177, - "velocityY": -0.2808327604066967, - "timestamp": 2.67641746834271 - }, - { - "x": 1.6989272832870483, - "y": 5.553633213043213, - "heading": -0.020267736871838676, - "angularVelocity": -3.9423001343697265e-19, - "velocityX": 0.38420969589870047, - "velocityY": 0.0014838259015203296, - "timestamp": 2.7667448810101076 - }, - { - "x": 1.765875287477924, - "y": 5.553891767639138, - "heading": -0.020267736871838676, - "angularVelocity": -1.268800151544e-19, - "velocityX": 0.7684200358337809, - "velocityY": 0.0029676542903228132, - "timestamp": 2.8538691113384247 - }, - { - "x": 1.8662973205168285, - "y": 5.554279599636344, - "heading": -0.02026773687183868, - "angularVelocity": -1.2688001508205996e-19, - "velocityX": 1.1526303608132487, - "velocityY": 0.004451482621366332, - "timestamp": 2.9409933416667418 - }, - { - "x": 2.000193380141947, - "y": 5.554796709026097, - "heading": -0.02026773687183868, - "angularVelocity": -1.2688001524330257e-19, - "velocityX": 1.53684065983192, - "velocityY": 0.005935310852148713, - "timestamp": 3.028117571995059 - }, - { - "x": 2.167563461458264, - "y": 5.555443095789492, - "heading": -0.020267736871838683, - "angularVelocity": -1.2688001514027187e-19, - "velocityX": 1.9210509026662645, - "velocityY": 0.0074191388659460445, - "timestamp": 3.115241802323376 - }, - { - "x": 2.3684075459339367, - "y": 5.556218759854957, - "heading": -0.020267736871838683, - "angularVelocity": -1.2688001518380097e-19, - "velocityX": 2.3052609327946625, - "velocityY": 0.00890296605826855, - "timestamp": 3.202366032651693 - }, - { - "x": 2.5357776764404463, - "y": 5.556865146808326, - "heading": -0.020267736871838683, - "angularVelocity": -1.2688001516210267e-19, - "velocityX": 1.921051467264574, - "velocityY": 0.007419141046436553, - "timestamp": 3.28949026298001 - }, - { - "x": 2.669673789778629, - "y": 5.5573822564055195, - "heading": -0.020267736871838686, - "angularVelocity": -1.268800151621384e-19, - "velocityX": 1.5368412763431272, - "velocityY": 0.005935313233127921, - "timestamp": 3.376614493308327 - }, - { - "x": 2.7700958781366665, - "y": 5.557770088616369, - "heading": -0.020267736871838686, - "angularVelocity": -1.2688001516248343e-19, - "velocityX": 1.1526309957586889, - "velocityY": 0.004451485073538937, - "timestamp": 3.463738723636644 - }, - { - "x": 2.8370439384630357, - "y": 5.558028643429091, - "heading": -0.02026773687183869, - "angularVelocity": -1.268800151622008e-19, - "velocityX": 0.7684206801492988, - "velocityY": 0.0029676567786828525, - "timestamp": 3.550862953964961 - }, - { - "x": 2.8705179691314697, - "y": 5.558157920837402, - "heading": -0.02026773687183869, - "angularVelocity": -1.2688001484574509e-19, - "velocityX": 0.3842103458738335, - "velocityY": 0.0014838284117379952, - "timestamp": 3.6379871842932783 - }, - { - "x": 2.8705179691314697, - "y": 5.558157920837402, - "heading": -0.020267736871838693, - "angularVelocity": -3.4830373991096207e-20, - "velocityX": 7.082195607551611e-16, - "velocityY": 1.9869518370590596e-18, - "timestamp": 3.7251114146215953 - }, - { - "x": 2.8404610494153317, - "y": 5.534143912451309, - "heading": -0.03485317920513617, - "angularVelocity": -0.15571220333723793, - "velocityX": -0.32088359664250343, - "velocityY": -0.25637029521011345, - "timestamp": 3.8187806455534106 - }, - { - "x": 2.7804008780683063, - "y": 5.486049425013135, - "heading": -0.06402371208298976, - "angularVelocity": -0.3114206510256051, - "velocityX": -0.6411942400887777, - "velocityY": -0.5134502222312821, - "timestamp": 3.912449876485226 - }, - { - "x": 2.690426928059957, - "y": 5.4137641868079385, - "heading": -0.10777934842034757, - "angularVelocity": -0.46712923659220595, - "velocityX": -0.9605496822520554, - "velocityY": -0.7717073951190555, - "timestamp": 4.006119107417041 - }, - { - "x": 2.5707182617497417, - "y": 5.317070431884298, - "heading": -0.1661422083280734, - "angularVelocity": -0.6230739734610408, - "velocityX": -1.2779934789616767, - "velocityY": -1.0322894077568259, - "timestamp": 4.099788338348856 - }, - { - "x": 2.4218123673171084, - "y": 5.195334657114955, - "heading": -0.239237988952977, - "angularVelocity": -0.7803606360141083, - "velocityX": -1.5896991247961374, - "velocityY": -1.299634613824878, - "timestamp": 4.193457569280671 - }, - { - "x": 2.2812263727256172, - "y": 5.035989894533815, - "heading": -0.32521378731763706, - "angularVelocity": -0.9178659577897541, - "velocityX": -1.500877002970466, - "velocityY": -1.7011430647608603, - "timestamp": 4.287126800212486 - }, - { - "x": 2.1719619380449973, - "y": 4.899006132834784, - "heading": -0.39709906634612513, - "angularVelocity": -0.7674374852166317, - "velocityX": -1.1664922792005954, - "velocityY": -1.4624200533763867, - "timestamp": 4.3807960311443015 - }, - { - "x": 2.0934918282245056, - "y": 4.785094183316171, - "heading": -0.4547023997403661, - "angularVelocity": -0.6149653714587694, - "velocityX": -0.8377362452950287, - "velocityY": -1.2161085170170074, - "timestamp": 4.474465262076117 - }, - { - "x": 2.045638672399353, - "y": 4.694485163192149, - "heading": -0.4979506864820333, - "angularVelocity": -0.46171284114790145, - "velocityX": -0.5108737986755416, - "velocityY": -0.9673296046380411, - "timestamp": 4.568134493007932 - }, - { - "x": 2.028313474321192, - "y": 4.627293480949165, - "heading": -0.5268031269208548, - "angularVelocity": -0.3080247393065947, - "velocityX": -0.18496146392802787, - "velocityY": -0.7173292827811862, - "timestamp": 4.661803723939747 - }, - { - "x": 2.041462835791116, - "y": 4.583587383113917, - "heading": -0.5412356183232561, - "angularVelocity": -0.15407931995200425, - "velocityX": 0.14038079889324448, - "velocityY": -0.46660037026527035, - "timestamp": 4.755472954871562 - }, - { - "x": 2.0850512981414795, - "y": 4.563412189483643, - "heading": -0.5412356183232561, - "angularVelocity": -1.875746881389731e-20, - "velocityX": 0.465344509789913, - "velocityY": -0.2153876297432254, - "timestamp": 4.849142185803378 - }, - { - "x": 2.1256328053913753, - "y": 4.544628783911912, - "heading": -0.5412356183232561, - "angularVelocity": 1.7413793657350135e-22, - "velocityX": 0.6980165405938983, - "velocityY": -0.3230813408929243, - "timestamp": 4.90728050329277 - }, - { - "x": 2.1797414712659067, - "y": 4.519584247990461, - "heading": -0.5412356183232561, - "angularVelocity": 1.7413789738422444e-22, - "velocityX": 0.930688540899089, - "velocityY": -0.4307750379260645, - "timestamp": 4.965418820782163 - }, - { - "x": 2.2473772913410164, - "y": 4.488278583766992, - "heading": -0.5412356183232561, - "angularVelocity": 1.7413788470930633e-22, - "velocityX": 1.1633604651089158, - "velocityY": -0.5384686997379875, - "timestamp": 5.023557138271555 - }, - { - "x": 2.328540234996748, - "y": 4.450711805414148, - "heading": -0.5412356183232561, - "angularVelocity": 1.7413787765697518e-22, - "velocityX": 1.3960318626444466, - "velocityY": -0.646162117775404, - "timestamp": 5.081695455760948 - }, - { - "x": 2.396176035002005, - "y": 4.419406150480137, - "heading": -0.5412356183232561, - "angularVelocity": 1.7413789176163748e-22, - "velocityX": 1.1633601199002148, - "velocityY": -0.5384685399559715, - "timestamp": 5.1398337732503405 - }, - { - "x": 2.45028467878785, - "y": 4.394361624782573, - "heading": -0.5412356183232561, - "angularVelocity": 1.7413789463080967e-22, - "velocityX": 0.930688160965733, - "velocityY": -0.4307748620715239, - "timestamp": 5.197972090739733 - }, - { - "x": 2.490866163227744, - "y": 4.375578229768594, - "heading": -0.5412356183232561, - "angularVelocity": 1.741380781979936e-22, - "velocityX": 0.6980161482536459, - "velocityY": -0.3230811592957737, - "timestamp": 5.256110408229126 - }, - { - "x": 2.5179204869025957, - "y": 4.363055966095035, - "heading": -0.5412356183232561, - "angularVelocity": 1.7413772411298374e-22, - "velocityX": 0.465344111132695, - "velocityY": -0.2153874452222276, - "timestamp": 5.314248725718518 - }, - { - "x": 2.531447649002075, - "y": 4.356794834136963, - "heading": -0.5412356183232561, - "angularVelocity": 1.7413769362387268e-22, - "velocityX": 0.23267206007376748, - "velocityY": -0.10769372469740146, - "timestamp": 5.372387043207911 - }, - { - "x": 2.531447649002075, - "y": 4.356794834136963, - "heading": -0.5412356183232561, - "angularVelocity": 5.804601738611348e-23, - "velocityX": 1.3725068175106304e-19, - "velocityY": -2.6971290693406307e-19, - "timestamp": 5.430525360697303 - } - ] + "samples": [ + { + "x": 0.7470832467079163, + "y": 6.625458717346191, + "heading": 1.056495497952334, + "angularVelocity": -2.814922481994462e-23, + "velocityX": 3.0229602263614203e-24, + "velocityY": -3.0164882691321217e-22, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ], + "timestamp": 0 + }, + { + "x": 0.7539026198838662, + "y": 6.625839605524516, + "heading": 1.0492729313853104, + "angularVelocity": -0.15166849427224313, + "velocityX": 0.14320173471285522, + "velocityY": 0.00799836677952159, + "moduleForcesX": [ + 57.85992865453149, + 55.57487201027826, + 51.318064652077474, + 58.039260452127415 + ], + "moduleForcesY": [ + 6.907999308730754, + -17.49184693835158, + 27.569744910492076, + -4.542101533123827 + ], + "timestamp": 0.047620744187380236 + }, + { + "x": 0.7675577009381795, + "y": 6.626628550621342, + "heading": 1.0350515766053865, + "angularVelocity": -0.29863781052990607, + "velocityX": 0.2867464859554202, + "velocityY": 0.01656725677619831, + "moduleForcesX": [ + 57.88100930307201, + 55.714508532181725, + 51.61400892290664, + 58.11626141362601 + ], + "moduleForcesY": [ + 6.706391089887608, + -17.030469565022152, + 27.005128287026007, + -3.349638557212904 + ], + "timestamp": 0.09524148837476047 + }, + { + "x": 0.7880663188184851, + "y": 6.627856082150549, + "heading": 1.014085545158225, + "angularVelocity": -0.44027097444473756, + "velocityX": 0.4306656317592878, + "velocityY": 0.0257772437233815, + "moduleForcesX": [ + 57.91162551157599, + 55.863804025506134, + 51.9553282692449, + 58.17751046150169 + ], + "moduleForcesY": [ + 6.409362353495803, + -16.520382189151235, + 26.3352269347313, + -1.8953822815394896 + ], + "timestamp": 0.1428622325621407 + }, + { + "x": 0.8154474139272968, + "y": 6.629555951163622, + "heading": 0.9866565513822839, + "angularVelocity": -0.5759883480193475, + "velocityX": 0.5749825118454964, + "velocityY": 0.03569597750058087, + "moduleForcesX": [ + 57.94827042148509, + 56.03347528236485, + 52.34203591069722, + 58.203278502637744 + ], + "moduleForcesY": [ + 6.035406626296778, + -15.918521504125732, + 25.54970777274484, + -0.23510536726652978 + ], + "timestamp": 0.19048297674952094 + }, + { + "x": 0.8497208018135379, + "y": 6.6317649710027196, + "heading": 0.9530739033866412, + "angularVelocity": -0.7052104827152668, + "velocityX": 0.7197155036338936, + "velocityY": 0.04638776392079282, + "moduleForcesX": [ + 57.987036343437325, + 56.235225818874625, + 52.774229558093566, + 58.17795169052109 + ], + "moduleForcesY": [ + 5.608046706017465, + -15.169403162610594, + 24.63487493770155, + 1.560678231687874 + ], + "timestamp": 0.2381037209369012 + }, + { + "x": 0.8909071759065187, + "y": 6.634522965587952, + "heading": 0.9136797710629836, + "angularVelocity": -0.8272473056835844, + "velocityX": 0.8648830419558078, + "velocityY": 0.057915822868710255, + "moduleForcesX": [ + 58.02382176629463, + 56.48076577385563, + 53.2518632286912, + 58.094056700540655 + ], + "moduleForcesY": [ + 5.158035303537173, + -14.201163704424598, + 23.572862374375507, + 3.4055289208270785 + ], + "timestamp": 0.2857244651242814 + }, + { + "x": 0.9390283845014781, + "y": 6.637873093075656, + "heading": 0.8688628769210628, + "angularVelocity": -0.9411212467737456, + "velocityX": 1.0105093781317218, + "velocityY": 0.07035017081046684, + "moduleForcesX": [ + 58.05445637477262, + 56.77945893480036, + 53.77458368672696, + 57.955802544053434 + ], + "moduleForcesY": [ + 4.726988301952687, + -12.920128155574607, + 22.340010342191754, + 5.198389651263037 + ], + "timestamp": 0.3333452093116616 + }, + { + "x": 0.99410787725357, + "y": 6.641862960540765, + "heading": 0.8190840274654485, + "angularVelocity": -1.0453185960249207, + "velocityX": 1.156628139521772, + "velocityY": 0.08378423170811934, + "moduleForcesX": [ + 58.0745823349482, + 57.13326696521539, + 54.34162138758188, + 57.78094210439999 + ], + "moduleForcesY": [ + 4.373537369916966, + -11.203858122136547, + 20.903786932368053, + 6.827139466144138 + ], + "timestamp": 0.38096595349904183 + }, + { + "x": 1.0561709558415282, + "y": 6.646547058923123, + "heading": 0.764916720396532, + "angularVelocity": -1.1374729226359126, + "velocityX": 1.3032782172355293, + "velocityY": 0.09836256157458603, + "moduleForcesX": [ + 58.07884230896552, + 57.52664576524135, + 54.95165712906377, + 57.59988532120776 + ], + "moduleForcesY": [ + 4.18414702790263, + -8.893502901162272, + 19.216964845382883, + 8.173241281714256 + ], + "timestamp": 0.42858669768642205 + }, + { + "x": 1.1252440265364065, + "y": 6.651991066133351, + "heading": 0.707105481038992, + "angularVelocity": -1.2139927744527004, + "velocityX": 1.4504828068853088, + "velocityY": 0.11432007842645366, + "moduleForcesX": [ + 58.058063356768365, + 57.90773275166756, + 55.60238220835145, + 57.45155749012649 + ], + "moduleForcesY": [ + 4.293210892903159, + -5.788564619162194, + 17.206213788359637, + 9.115718390676392 + ], + "timestamp": 0.47620744187380226 + }, + { + "x": 1.2013513334108457, + "y": 6.658278411112851, + "heading": 0.6466442567332923, + "angularVelocity": -1.2696404757513684, + "velocityX": 1.5981965039220822, + "velocityY": 0.1320295406295972, + "moduleForcesX": [ + 57.99034977388328, + 58.15656069329295, + 56.288724673225055, + 57.376165910837095 + ], + "moduleForcesY": [ + 4.923367257090649, + -1.6522597130636139, + 14.748411409233862, + 9.532722214598236 + ], + "timestamp": 0.5238281860611825 + }, + { + "x": 1.2845063733027953, + "y": 6.665519148313447, + "heading": 0.5848804980188779, + "angularVelocity": -1.2969927238302643, + "velocityX": 1.7461936244580183, + "velocityY": 0.15205006398272194, + "moduleForcesX": [ + 57.811915108319404, + 58.03976645514236, + 56.99574350032489, + 57.40532402483411 + ], + "moduleForcesY": [ + 6.474842781770424, + 3.7555604314694015, + 11.617621448775878, + 9.299747156494563 + ], + "timestamp": 0.5714489302485628 + }, + { + "x": 1.3746921013206055, + "y": 6.673861244609411, + "heading": 0.5236730878733165, + "angularVelocity": -1.285309820122082, + "velocityX": 1.8938328150216104, + "velocityY": 0.17517778099265666, + "moduleForcesX": [ + 57.310185080815664, + 57.16712059629645, + 57.669087251929504, + 57.549491540761366 + ], + "moduleForcesY": [ + 9.74828832700739, + 10.593301460429908, + 7.3549113244132425, + 8.285418011961559 + ], + "timestamp": 0.6190696744359431 + }, + { + "x": 1.4718109314175543, + "y": 6.683507923259447, + "heading": 0.46574387997473543, + "angularVelocity": -1.2164700255552203, + "velocityX": 2.0394227716140083, + "velocityY": 0.20257303439185328, + "moduleForcesX": [ + 55.65121903382453, + 54.99704715282837, + 58.07803857155044, + 57.78139771397302 + ], + "moduleForcesY": [ + 16.565429620324096, + 18.792767490211176, + 0.9116734716158617, + 6.3514478887901085 + ], + "timestamp": 0.6666904186233233 + }, + { + "x": 1.57550342839463, + "y": 6.694752929350011, + "heading": 0.4158791410788935, + "angularVelocity": -1.0471222100106607, + "velocityX": 2.1774648579422013, + "velocityY": 0.2361367148383242, + "moduleForcesX": [ + 48.75296553631865, + 50.943800913157915, + 57.05405288058638, + 58.01396630829892 + ], + "moduleForcesY": [ + 31.297064627265303, + 27.93547289382856, + -10.403475397779616, + 3.3890463395149384 + ], + "timestamp": 0.7143111628107036 + }, + { + "x": 1.6843562594767798, + "y": 6.707809824340588, + "heading": 0.3846859068543657, + "angularVelocity": -0.6550345811856123, + "velocityX": 2.2858280133932967, + "velocityY": 0.27418502615581647, + "moduleForcesX": [ + 18.204183715975507, + 44.46669574639313, + 47.828415859508524, + 58.091244495239216 + ], + "moduleForcesY": [ + 54.82018208894905, + 37.37367730553576, + -32.52805362351062, + -0.47054408407509796 + ], + "timestamp": 0.7619319069980839 + }, + { + "x": 1.7956033945083618, + "y": 6.721975326538086, + "heading": 0.3846859068543657, + "angularVelocity": -3.554059362055802e-23, + "velocityX": 2.336106605008982, + "velocityY": 0.2974649480856219, + "moduleForcesX": [ + -28.169968798994756, + 34.96729290232753, + 13.531484107057947, + 57.89422679118955 + ], + "moduleForcesY": [ + 50.55108957029159, + 46.375637620891645, + -56.177268901226164, + -4.530739956145902 + ], + "timestamp": 0.8095526511854642 + }, + { + "x": 1.9448813303029442, + "y": 6.750262783810755, + "heading": 0.3846859068543657, + "angularVelocity": -7.071920034257376e-24, + "velocityX": 2.4334235311170778, + "velocityY": 0.4611221597912817, + "moduleForcesX": [ + 29.383140382480864, + 29.383140382480864, + 29.383140382480864, + 29.383140382480864 + ], + "moduleForcesY": [ + 49.41342702102415, + 49.41342702102415, + 49.41342702102415, + 49.41342702102415 + ], + "timestamp": 0.8708974741830685 + }, + { + "x": 2.0829613480368456, + "y": 6.781960040020516, + "heading": 0.3846859068543657, + "angularVelocity": -7.072310064547998e-24, + "velocityX": 2.250882975720603, + "velocityY": 0.5167062950201712, + "moduleForcesX": [ + -55.1149217223832, + -55.11492172238321, + -55.1149217223832, + -55.1149217223832 + ], + "moduleForcesY": [ + 16.78265553368512, + 16.782655533685116, + 16.78265553368512, + 16.78265553368512 + ], + "timestamp": 0.9322422971806729 + }, + { + "x": 2.209277628517863, + "y": 6.812924670308866, + "heading": 0.3846859068543657, + "angularVelocity": -7.072257174941514e-24, + "velocityX": 2.0591188352756387, + "velocityY": 0.5047635444242788, + "moduleForcesX": [ + -57.89982158665208, + -57.89982158665208, + -57.89982158665208, + -57.89982158665208 + ], + "moduleForcesY": [ + -3.6059042485813855, + -3.605904248581385, + -3.6059042485813873, + -3.605904248581387 + ], + "timestamp": 0.9935871201782772 + }, + { + "x": 2.323926038493632, + "y": 6.842067117238504, + "heading": 0.3846859068543657, + "angularVelocity": -7.072361618794593e-24, + "velocityX": 1.8689174468764331, + "velocityY": 0.4750595976253187, + "moduleForcesX": [ + -57.42797599329124, + -57.42797599329124, + -57.42797599329124, + -57.42797599329124 + ], + "moduleForcesY": [ + -8.968586181381475, + -8.968586181381475, + -8.968586181381475, + -8.968586181381475 + ], + "timestamp": 1.0549319431758817 + }, + { + "x": 2.4269824153429544, + "y": 6.868901069621197, + "heading": 0.3846859068543657, + "angularVelocity": -7.072821947364684e-24, + "velocityX": 1.679952305891357, + "velocityY": 0.4374281491323442, + "moduleForcesX": [ + -57.0547127515352, + -57.0547127515352, + -57.0547127515352, + -57.0547127515352 + ], + "moduleForcesY": [ + -11.362156390317617, + -11.362156390317615, + -11.362156390317617, + -11.362156390317617 + ], + "timestamp": 1.116276766173486 + }, + { + "x": 2.5184986692010405, + "y": 6.893152527814633, + "heading": 0.3846859068543657, + "angularVelocity": -7.07105136075711e-24, + "velocityX": 1.491833367286101, + "velocityY": 0.3953301518920652, + "moduleForcesX": [ + -56.79921677244283, + -56.79921677244283, + -56.79921677244284, + -56.79921677244284 + ], + "moduleForcesY": [ + -12.71075250936749, + -12.71075250936749, + -12.710752509367488, + -12.71075250936749 + ], + "timestamp": 1.1776215891710904 + }, + { + "x": 2.5985115833830923, + "y": 6.9146459631069135, + "heading": 0.3846859068543657, + "angularVelocity": -7.071515558519895e-24, + "velocityX": 1.3043140443192134, + "velocityY": 0.35037080949961336, + "moduleForcesX": [ + -56.61817333855819, + -56.61817333855819, + -56.61817333855819, + -56.61817333855819 + ], + "moduleForcesY": [ + -13.574685533676519, + -13.574685533676519, + -13.574685533676519, + -13.574685533676519 + ], + "timestamp": 1.2389664121686947 + }, + { + "x": 2.6670483577421855, + "y": 6.9332594017529505, + "heading": 0.3846859068543657, + "angularVelocity": -7.07550113164988e-24, + "velocityX": 1.117238114156259, + "velocityY": 0.30342313721833786, + "moduleForcesX": [ + -56.48429865176341, + -56.4842986517634, + -56.48429865176341, + -56.48429865176341 + ], + "moduleForcesY": [ + -14.175026898601002, + -14.175026898601002, + -14.175026898601004, + -14.175026898601008 + ], + "timestamp": 1.300311235166299 + }, + { + "x": 2.72412984860515, + "y": 6.948903188901549, + "heading": 0.3846859068543657, + "angularVelocity": -7.06791759690408e-24, + "velocityX": 0.9305021691104057, + "velocityY": 0.2550139748420099, + "moduleForcesX": [ + -56.38164610381262, + -56.38164610381262, + -56.38164610381262, + -56.38164610381262 + ], + "moduleForcesY": [ + -14.616298220537676, + -14.616298220537676, + -14.616298220537674, + -14.616298220537672 + ], + "timestamp": 1.3616560581639034 + }, + { + "x": 2.769772527142252, + "y": 6.961508658865873, + "heading": 0.3846859068543657, + "angularVelocity": -7.080226941592603e-24, + "velocityX": 0.744034725454898, + "velocityY": 0.2054854729113339, + "moduleForcesX": [ + -56.30057681442044, + -56.30057681442044, + -56.30057681442043, + -56.30057681442044 + ], + "moduleForcesY": [ + -14.954263182815012, + -14.954263182815014, + -14.954263182815021, + -14.954263182815023 + ], + "timestamp": 1.4230008811615078 + }, + { + "x": 2.8039897177250084, + "y": 6.971021542915001, + "heading": 0.3846859068543657, + "angularVelocity": -7.06236523633029e-24, + "velocityX": 0.5577844862979259, + "velocityY": 0.15507232043850186, + "moduleForcesX": [ + -56.2349957225445, + -56.2349957225445, + -56.2349957225445, + -56.2349957225445 + ], + "moduleForcesY": [ + -15.221367910729839, + -15.221367910729839, + -15.221367910729835, + -15.221367910729835 + ], + "timestamp": 1.4843457041591122 + }, + { + "x": 2.8267924145674423, + "y": 6.97739787426348, + "heading": 0.3846859068543657, + "angularVelocity": -7.077974709960036e-24, + "velocityX": 0.371713467056942, + "velocityY": 0.10394245246626038, + "moduleForcesX": [ + -56.18088340969801, + -56.18088340969801, + -56.18088340969801, + -56.18088340969801 + ], + "moduleForcesY": [ + -15.437767595508772, + -15.437767595508774, + -15.437767595508781, + -15.437767595508783 + ], + "timestamp": 1.5456905271567165 + }, + { + "x": 2.8381898403167725, + "y": 6.9806013107299805, + "heading": 0.3846859068543657, + "angularVelocity": -7.070376775728643e-24, + "velocityX": 0.18579278890046133, + "velocityY": 0.05222015990861092, + "moduleForcesX": [ + -56.13549055392384, + -56.13549055392384, + -56.13549055392384, + -56.13549055392384 + ], + "moduleForcesY": [ + -15.616639816973526, + -15.616639816973523, + -15.616639816973528, + -15.616639816973525 + ], + "timestamp": 1.6070353501543209 + }, + { + "x": 2.8381898403167725, + "y": 6.9806013107299805, + "heading": 0.3846859068543657, + "angularVelocity": 1.6334100500794593e-18, + "velocityX": -2.855209079304936e-18, + "velocityY": 1.6102083009631126e-18, + "moduleForcesX": [ + -56.096876634297416, + -56.096876634297416, + -56.09687663429742, + -56.096876634297416 + ], + "moduleForcesY": [ + -15.766962138590067, + -15.766962138590072, + -15.766962138590065, + -15.766962138590062 + ], + "timestamp": 1.6683801731519252 + }, + { + "x": 2.821131681761048, + "y": 6.962941949661055, + "heading": 0.37963298615970775, + "angularVelocity": -0.05715420706543545, + "velocityX": -0.1929469281557583, + "velocityY": -0.19974720368037308, + "moduleForcesX": [ + -37.095177903701845, + -39.323311531074836, + -41.803716726316495, + -43.47083407085052 + ], + "moduleForcesY": [ + -44.94946586017272, + -43.0174583751275, + -40.605077451410324, + -38.81979329867605 + ], + "timestamp": 1.756788725279557 + }, + { + "x": 2.7870198730695646, + "y": 6.927621262535135, + "heading": 0.36952776277295296, + "angularVelocity": -0.11430142382793766, + "velocityX": -0.38584286101911414, + "velocityY": -0.3995166335823328, + "moduleForcesX": [ + -37.072533334197914, + -39.34365755876542, + -41.75968748882494, + -43.474426866266874 + ], + "moduleForcesY": [ + -44.962788854185376, + -42.99375240452121, + -40.64405289944275, + -38.80982680576702 + ], + "timestamp": 1.845197277407189 + }, + { + "x": 2.7358604135298354, + "y": 6.874636621269694, + "heading": 0.35437217454755016, + "angularVelocity": -0.17142672129188716, + "velocityX": -0.5786709352040128, + "velocityY": -0.5993157900487962, + "moduleForcesX": [ + -37.04188686591134, + -39.376245866938596, + -41.69487105411659, + -43.48043465252771 + ], + "moduleForcesY": [ + -44.98092590316717, + -42.957112494872305, + -40.70214264581303, + -38.7951513225733 + ], + "timestamp": 1.9336058295348209 + }, + { + "x": 2.6676616985406656, + "y": 6.803984339073742, + "heading": 0.33416944271042276, + "angularVelocity": -0.22851558306216296, + "velocityX": -0.7714040480010793, + "velocityY": -0.7991566482612914, + "moduleForcesX": [ + -37.00181302221145, + -39.4192239370053, + -41.60655692235178, + -43.486265185245315 + ], + "moduleForcesY": [ + -45.00397284969973, + -42.90816343331507, + -40.78067451259032, + -38.777468393572995 + ], + "timestamp": 2.0220143816624527 + }, + { + "x": 2.582436328275892, + "y": 6.71565888253476, + "heading": 0.3089239458735266, + "angularVelocity": -0.2855549178144013, + "velocityX": -0.9639946386831083, + "velocityY": -0.9990600955829445, + "moduleForcesX": [ + -36.94888772357164, + -39.46875969787357, + -41.48963891451579, + -43.48713660247826 + ], + "moduleForcesY": [ + -45.03260236713544, + -42.8483286467539, + -40.882056657335006, + -38.75974233145917 + ], + "timestamp": 2.1104229337900846 + }, + { + "x": 2.4802053222003866, + "y": 6.609651042778182, + "heading": 0.2786408401009175, + "angularVelocity": -0.34253593169233965, + "velocityX": -1.156347475614342, + "velocityY": -1.1990677056167498, + "moduleForcesX": [ + -36.873873585620636, + -39.51569610358064, + -41.33281589762392, + -43.4727953778751 + ], + "moduleForcesY": [ + -45.06942043742109, + -42.781255183974444, + -41.011452230389395, + -38.74789240095698 + ], + "timestamp": 2.1988314859177165 + }, + { + "x": 2.361010738415153, + "y": 6.485942489444843, + "heading": 0.2433248076280929, + "angularVelocity": -0.39946398422904045, + "velocityX": -1.3482245881954655, + "velocityY": -1.3992826525961657, + "moduleForcesX": [ + -36.74649413708812, + -39.53225346091061, + -41.10333275151675, + -43.414434944697646 + ], + "moduleForcesY": [ + -45.1243385113088, + -42.718426403813595, + -41.18352942870808, + -38.75747804587791 + ], + "timestamp": 2.2872400380453484 + }, + { + "x": 2.22497852013832, + "y": 6.344478967922724, + "heading": 0.20297375459292902, + "angularVelocity": -0.4564157206975939, + "velocityX": -1.5386771415558158, + "velocityY": -1.6001112801609332, + "moduleForcesX": [ + -36.40940346518017, + -39.379512733261826, + -40.64030165250597, + -43.1734908685818 + ], + "moduleForcesY": [ + -45.25140648250694, + -42.717694890088865, + -41.4691945783061, + -38.85975194723375 + ], + "timestamp": 2.3756485901729802 + }, + { + "x": 2.0952329573157353, + "y": 6.186474139637454, + "heading": 0.15831835366452565, + "angularVelocity": -0.5051027287941129, + "velocityX": -1.467568008978098, + "velocityY": -1.7872120341611801, + "moduleForcesX": [ + 16.709059309020606, + 12.736243666460043, + 17.192391398004176, + 12.953053946000171 + ], + "moduleForcesY": [ + -39.41171649809029, + -40.61065023266987, + -37.72694053413655, + -39.04453253941978 + ], + "timestamp": 2.464057142300612 + }, + { + "x": 1.9826977478470895, + "y": 6.045863415934052, + "heading": 0.11862394841971062, + "angularVelocity": -0.44898829682800195, + "velocityX": -1.2728995867524602, + "velocityY": -1.5904651792103528, + "moduleForcesX": [ + 37.390210370998965, + 40.475901652172276, + 41.278591550019264, + 43.990980045515705 + ], + "moduleForcesY": [ + 44.44300054730394, + 41.677348946779276, + 40.830424502770605, + 37.92667410323451 + ], + "timestamp": 2.552465694428244 + }, + { + "x": 1.8873135024006749, + "y": 5.92277267476745, + "heading": 0.08389641882694215, + "angularVelocity": -0.3928073558159144, + "velocityX": -1.0789029245577086, + "velocityY": -1.3922945032387968, + "moduleForcesX": [ + 37.21476272459083, + 40.450247243359854, + 41.00550775325207, + 43.902218755828535 + ], + "moduleForcesY": [ + 44.73999822966322, + 41.84966110698525, + 41.279760388849354, + 38.20121632419051 + ], + "timestamp": 2.640874246555876 + }, + { + "x": 1.8090603018164058, + "y": 5.817243772643182, + "heading": 0.05413403725183304, + "angularVelocity": -0.3366459562887355, + "velocityX": -0.8851315704310847, + "velocityY": -1.1936503831882683, + "moduleForcesX": [ + 37.15959386222587, + 40.498806672558835, + 40.847090706611624, + 43.87843297066345 + ], + "moduleForcesY": [ + 44.83556164168193, + 41.85167911227039, + 41.49451958786403, + 38.28563046961167 + ], + "timestamp": 2.7292827986835078 + }, + { + "x": 1.747928153071767, + "y": 5.729297635459773, + "heading": 0.02933424645224599, + "angularVelocity": -0.2805134820416991, + "velocityX": -0.6914732486104376, + "velocityY": -0.9947695677273937, + "moduleForcesX": [ + 37.13549798902941, + 40.55427442990822, + 40.73097140342639, + 43.868457258457035 + ], + "moduleForcesY": [ + 44.88031315505466, + 41.82243046412967, + 41.63740433004384, + 38.32559792736543 + ], + "timestamp": 2.8176913508111396 + }, + { + "x": 1.7039110423935377, + "y": 5.658946821606213, + "heading": 0.009494945599922864, + "angularVelocity": -0.22440477052131733, + "velocityX": -0.49788294931788657, + "velocityY": -0.7957467027850181, + "moduleForcesX": [ + 37.12351320058831, + 40.60313571714578, + 40.64272563549292, + 43.86282241005115 + ], + "moduleForcesY": [ + 44.90506437230603, + 41.789697152404564, + 41.740842496734864, + 38.349181908010294 + ], + "timestamp": 2.9060999029387715 + }, + { + "x": 1.6770049484081522, + "y": 5.606199701903595, + "heading": -0.0053851285855366264, + "angularVelocity": -0.16831034812082146, + "velocityX": -0.30433813627602885, + "velocityY": -0.5966291544563317, + "moduleForcesX": [ + 37.116682462766875, + 40.64090572081128, + 40.57759789476367, + 43.858892578344765 + ], + "moduleForcesY": [ + 44.920576626812874, + 41.76277059020072, + 41.815668830634316, + 38.365116284149615 + ], + "timestamp": 2.9945084550664034 + }, + { + "x": 1.6672069897589075, + "y": 5.571062249143735, + "heading": -0.015306267888284994, + "angularVelocity": -0.11221922612673975, + "velocityX": -0.1108259146140006, + "velocityY": -0.3974440471452618, + "moduleForcesX": [ + 37.111681394515514, + 40.665682324503166, + 40.533570764783825, + 43.855832002950336 + ], + "moduleForcesY": [ + 44.93173600136994, + 41.745649789594474, + 41.866559403537146, + 38.37680279508834 + ], + "timestamp": 3.0829170071940353 + }, + { + "x": 1.6745149985934764, + "y": 5.553538932287859, + "heading": -0.0202677368718387, + "angularVelocity": -0.05611978552019538, + "velocityX": 0.08266178620387737, + "velocityY": -0.1982083908644825, + "moduleForcesX": [ + 37.106714226875006, + 40.67652070236513, + 40.509643285006895, + 43.85333935847226 + ], + "moduleForcesY": [ + 44.94109264735369, + 41.7403427171622, + 41.89586596082861, + 38.38580762158759 + ], + "timestamp": 3.171325559321667 + }, + { + "x": 1.6989272832870483, + "y": 5.553633213043213, + "heading": -0.020267736871838703, + "angularVelocity": -3.99357376917391e-18, + "velocityX": 0.2761303528456057, + "velocityY": 0.00106642121248368, + "moduleForcesX": [ + 37.10080834917557, + 40.672885944439045, + 40.50526475056877, + 43.851223740402496 + ], + "moduleForcesY": [ + 44.95004105053435, + 41.747969933406075, + 41.90488405097919, + 38.39302718242146 + ], + "timestamp": 3.259734111449299 + }, + { + "x": 1.7476866611230668, + "y": 5.553821522782783, + "heading": -0.0202677368718387, + "angularVelocity": 5.910944865182772e-20, + "velocityX": 0.5533641655006356, + "velocityY": 0.0021371040098882763, + "moduleForcesX": [ + 58.27560878459361, + 58.27560878459361, + 58.27560878459361, + 58.27560878459361 + ], + "moduleForcesY": [ + 0.22506162302642355, + 0.22506162302642355, + 0.22506162302642416, + 0.22506162302642413 + ], + "timestamp": 3.347848559178138 + }, + { + "x": 1.8208713864961603, + "y": 5.554104163726683, + "heading": -0.0202677368718387, + "angularVelocity": 5.910945255784143e-20, + "velocityX": 0.8305644222875951, + "velocityY": 0.003207657213826796, + "moduleForcesX": [ + 58.268555212659614, + 58.268555212659614, + 58.268555212659614, + 58.268555212659614 + ], + "moduleForcesY": [ + 0.2250343819837174, + 0.2250343819837174, + 0.2250343819837181, + 0.2250343819837181 + ], + "timestamp": 3.435963006906977 + }, + { + "x": 1.9184768732294062, + "y": 5.554481118163, + "heading": -0.0202677368718387, + "angularVelocity": 5.910945067058074e-20, + "velocityX": 1.1077126311182772, + "velocityY": 0.004278009407466852, + "moduleForcesX": [ + 58.25761453298922, + 58.25761453298922, + 58.25761453298922, + 58.25761453298922 + ], + "moduleForcesY": [ + 0.22499212884943065, + 0.22499212884943065, + 0.22499212884943104, + 0.22499212884943104 + ], + "timestamp": 3.524077454635816 + }, + { + "x": 2.040495046553733, + "y": 5.554952354906809, + "heading": -0.0202677368718387, + "angularVelocity": 5.910945105533479e-20, + "velocityX": 1.3847692003906387, + "velocityY": 0.005348007687211211, + "moduleForcesX": [ + 58.238351546996455, + 58.238351546996455, + 58.238351546996455, + 58.238351546996455 + ], + "moduleForcesY": [ + 0.22491773479362803, + 0.22491773479362803, + 0.2249177347936283, + 0.2249177347936283 + ], + "timestamp": 3.6121919023646547 + }, + { + "x": 2.186907919553257, + "y": 5.555517804492261, + "heading": -0.0202677368718387, + "angularVelocity": 5.910945033275698e-20, + "velocityX": 1.6616216383729834, + "velocityY": 0.006417217607633379, + "moduleForcesX": [ + 58.195442368336856, + 58.195442368336856, + 58.195442368336856, + 58.195442368336856 + ], + "moduleForcesY": [ + 0.22475201864594074, + 0.22475201864594074, + 0.22475201864594233, + 0.22475201864594233 + ], + "timestamp": 3.7003063500934936 + }, + { + "x": 2.3576403392711733, + "y": 5.556177176677063, + "heading": -0.0202677368718387, + "angularVelocity": 5.910945066362e-20, + "velocityX": 1.9376211747172734, + "velocityY": 0.007483133604046069, + "moduleForcesX": [ + 58.016159178761185, + 58.016159178761185, + 58.016159178761185, + 58.016159178761185 + ], + "moduleForcesY": [ + 0.22405962320866032, + 0.2240596232086603, + 0.22405962320865966, + 0.22405962320865963 + ], + "timestamp": 3.7884207978223325 + }, + { + "x": 2.504136867684685, + "y": 5.556742949341478, + "heading": -0.020267736871838696, + "angularVelocity": 5.910945066536768e-20, + "velocityX": 1.6625710333489974, + "velocityY": 0.006420884190937289, + "moduleForcesX": [ + -57.81659272013044, + -57.81659272013044, + -57.81659272013044, + -57.81659272013044 + ], + "moduleForcesY": [ + -0.22328889336099703, + -0.22328889336099703, + -0.22328889336099597, + -0.22328889336099597 + ], + "timestamp": 3.8765352455511715 + }, + { + "x": 2.626247488507551, + "y": 5.557214543119464, + "heading": -0.020267736871838696, + "angularVelocity": 5.910945018736598e-20, + "velocityX": 1.3858183756498825, + "velocityY": 0.005352059624061109, + "moduleForcesX": [ + -58.17446817080217, + -58.17446817080217, + -58.17446817080217, + -58.17446817080217 + ], + "moduleForcesY": [ + -0.22467101585528326, + -0.2246710158552833, + -0.22467101585528407, + -0.22467101585528407 + ], + "timestamp": 3.9646496932800104 + }, + { + "x": 2.7239485756809234, + "y": 5.557591866766686, + "heading": -0.020267736871838696, + "angularVelocity": 5.910945114498727e-20, + "velocityX": 1.108797588722742, + "velocityY": 0.0042821995364842646, + "moduleForcesX": [ + -58.2308299610416, + -58.2308299610416, + -58.2308299610416, + -58.2308299610416 + ], + "moduleForcesY": [ + -0.2248886862709617, + -0.22488868627096165, + -0.22488868627095934, + -0.2248886862709593 + ], + "timestamp": 4.052764141008849 + }, + { + "x": 2.7972305085837634, + "y": 5.557874883128104, + "heading": -0.020267736871838696, + "angularVelocity": 5.910945064257137e-20, + "velocityX": 0.8316676185539519, + "velocityY": 0.003211917780938857, + "moduleForcesX": [ + -58.253780696434475, + -58.253780696434475, + -58.253780696434475, + -58.253780696434475 + ], + "moduleForcesY": [ + -0.2249773224922699, + -0.2249773224922699, + -0.22497732249226998, + -0.22497732249226998 + ], + "timestamp": 4.140878588737688 + }, + { + "x": 2.846088065780519, + "y": 5.558063572038424, + "heading": -0.020267736871838696, + "angularVelocity": 5.910945075970797e-20, + "velocityX": 0.5544783909570489, + "velocityY": 0.0021414071719635906, + "moduleForcesX": [ + -58.266236834685465, + -58.266236834685465, + -58.266236834685465, + -58.266236834685465 + ], + "moduleForcesY": [ + -0.22502542835937098, + -0.22502542835937098, + -0.22502542835937084, + -0.22502542835937084 + ], + "timestamp": 4.228993036466527 + }, + { + "x": 2.8705179691314697, + "y": 5.558157920837402, + "heading": -0.020267736871838693, + "angularVelocity": 5.910945064090654e-20, + "velocityX": 0.2772519601567551, + "velocityY": 0.0010707528834367668, + "moduleForcesX": [ + -58.27405709046732, + -58.27405709046732, + -58.27405709046732, + -58.27405709046732 + ], + "moduleForcesY": [ + -0.2250556303511726, + -0.22505563035117257, + -0.2250556303511722, + -0.2250556303511722 + ], + "timestamp": 4.317107484195366 + }, + { + "x": 2.8705179691314697, + "y": 5.558157920837402, + "heading": -0.020267736871838693, + "angularVelocity": 1.9702981537661184e-20, + "velocityX": 1.653032544870731e-18, + "velocityY": -5.96028342388569e-21, + "moduleForcesX": [ + -58.279423458932314, + -58.279423458932314, + -58.279423458932314, + -58.279423458932314 + ], + "moduleForcesY": [ + -0.2250763553787051, + -0.22507635537870505, + -0.2250763553787032, + -0.22507635537870316 + ], + "timestamp": 4.4052219319242045 + }, + { + "x": 2.8439249599428864, + "y": 5.537645538216833, + "heading": -0.032906727963708735, + "angularVelocity": -0.12199820814045388, + "velocityX": -0.25668975050996623, + "velocityY": -0.1979963358001305, + "moduleForcesX": [ + -40.264650278600634, + -45.64387186701001, + -46.79690535956238, + -50.862712927454936 + ], + "moduleForcesY": [ + -42.14146610565898, + -36.25113454290553, + -34.737790742143396, + -28.463961726608957 + ], + "timestamp": 4.508821741171559 + }, + { + "x": 2.790863562177045, + "y": 5.496466451968379, + "heading": -0.058207005960777755, + "angularVelocity": -0.2442116272305314, + "velocityX": -0.512176597151375, + "velocityY": -0.39748225935566833, + "moduleForcesX": [ + -40.004992650057915, + -45.57335006506032, + -46.39579797648287, + -50.73375960353931 + ], + "moduleForcesY": [ + -42.381446517676004, + -36.332917243845905, + -35.26208895782567, + -28.68315856947041 + ], + "timestamp": 4.612421550418913 + }, + { + "x": 2.7115257184978163, + "y": 5.434386408788842, + "heading": -0.09620042670910543, + "angularVelocity": -0.36673253574835096, + "velocityX": -0.7658107119657117, + "velocityY": -0.5992293193447437, + "moduleForcesX": [ + -39.621387787708365, + -45.46476663407919, + -45.77146561371144, + -50.52532460582109 + ], + "moduleForcesY": [ + -42.73042746349606, + -36.458316770771056, + -36.05441082201013, + -29.03347681690111 + ], + "timestamp": 4.716021359666268 + }, + { + "x": 2.606245266180635, + "y": 5.351008207802818, + "heading": -0.14695247874407716, + "angularVelocity": -0.4898855741500109, + "velocityX": -1.0162224533233901, + "velocityY": -0.804810371676945, + "moduleForcesX": [ + -38.975029944558344, + -45.212668298575124, + -44.75754242271995, + -50.13326797035515 + ], + "moduleForcesY": [ + -43.30431649298782, + -36.752852470976016, + -37.281821264300945, + -29.679468474626695 + ], + "timestamp": 4.819621168913622 + }, + { + "x": 2.475745367457033, + "y": 5.245512799725402, + "heading": -0.2106299193340097, + "angularVelocity": -0.6146482416574408, + "velocityX": -1.2596538514083622, + "velocityY": -1.0182973196942513, + "moduleForcesX": [ + -37.59665158673427, + -44.46910269089497, + -42.8065712580459, + -49.21428672911071 + ], + "moduleForcesY": [ + -44.47297089839765, + -37.612897024695, + -39.459327933877574, + -31.127055280117744 + ], + "timestamp": 4.923220978160977 + }, + { + "x": 2.3227167278281233, + "y": 5.1152633537373084, + "heading": -0.28781914697582284, + "angularVelocity": -0.745071136738453, + "velocityX": -1.4771131408508664, + "velocityY": -1.2572363495101748, + "moduleForcesX": [ + -32.57681783967148, + -41.03171735775828, + -36.4212961306049, + -45.48318507972898 + ], + "moduleForcesY": [ + -48.168748844471274, + -41.22246689674932, + -45.27956258136952, + -36.203178634964225 + ], + "timestamp": 5.026820787408331 + }, + { + "x": 2.203106950739979, + "y": 4.983538065028958, + "heading": -0.35945125169862574, + "angularVelocity": -0.691430855357799, + "velocityX": -1.1545366536589345, + "velocityY": -1.2714819618426536, + "moduleForcesX": [ + 57.768566863317616, + 57.810702012324775, + 57.59277430823837, + 57.5140897196349 + ], + "moduleForcesY": [ + 0.8932781027881612, + -0.9152785781841101, + -4.374216376079367, + -5.791336644532758 + ], + "timestamp": 5.130420596655686 + }, + { + "x": 2.1130561975087887, + "y": 4.867723147947118, + "heading": -0.4197404118462817, + "angularVelocity": -0.5819427717642794, + "velocityX": -0.8692173652191356, + "velocityY": -1.1179066633734913, + "moduleForcesX": [ + 47.18108856870023, + 51.63439597749474, + 50.708340450422064, + 54.51833525481436 + ], + "moduleForcesY": [ + 34.07484296534565, + 26.874618698432997, + 28.520495989083397, + 20.35730229951914 + ], + "timestamp": 5.23402040590304 + }, + { + "x": 2.051552972727542, + "y": 4.7695988413626225, + "heading": -0.46820764413999405, + "angularVelocity": -0.46783128893598946, + "velocityX": -0.5936615639359138, + "velocityY": -0.9471475603803009, + "moduleForcesX": [ + 45.02951499832376, + 50.355649146304515, + 48.32912609649593, + 53.34564661451518 + ], + "moduleForcesY": [ + 36.95043417621518, + 29.289508389016593, + 32.4935418039393, + 23.38253676579243 + ], + "timestamp": 5.337620215150395 + }, + { + "x": 2.018185626236221, + "y": 4.689823401404505, + "heading": -0.5046715625693844, + "angularVelocity": -0.35196897266798394, + "velocityX": -0.32207922711183234, + "velocityY": -0.7700346220488296, + "moduleForcesX": [ + 44.230946349601425, + 49.9139754803189, + 47.23993285880785, + 52.83351377397607 + ], + "moduleForcesY": [ + 37.9315483504293, + 30.06878585422672, + 34.0958495054322, + 24.563695274073133 + ], + "timestamp": 5.441220024397749 + }, + { + "x": 2.0127316672603737, + "y": 4.628738886692783, + "heading": -0.5290344166885111, + "angularVelocity": -0.23516311753970603, + "velocityX": -0.05264448858998858, + "velocityY": -0.589619953506647, + "moduleForcesX": [ + 43.82380015207529, + 49.697676016896175, + 46.61701661238326, + 52.544050328156345 + ], + "moduleForcesY": [ + 38.41604200389433, + 30.441960825176444, + 34.962319678036984, + 25.200743372075255 + ], + "timestamp": 5.544819833645104 + }, + { + "x": 2.0350519350972927, + "y": 4.586554708774609, + "heading": -0.5412356183232561, + "angularVelocity": -0.117772433399114, + "velocityX": 0.2154469974324671, + "velocityY": -0.40718393426242, + "moduleForcesX": [ + 43.56673373099433, + 49.55277078584493, + 46.24089930341583, + 52.361530761045266 + ], + "moduleForcesY": [ + 38.71631589559412, + 30.687678310009126, + 35.47018296719196, + 25.592429826745295 + ], + "timestamp": 5.648419642892458 + }, + { + "x": 2.0850512981414795, + "y": 4.563412189483643, + "heading": -0.5412356183232561, + "angularVelocity": 2.0482990469860628e-24, + "velocityX": 0.48262022302385593, + "velocityY": -0.22338380214302983, + "moduleForcesX": [ + 43.37104440974198, + 49.42269040291651, + 46.02875544788031, + 52.24276301802586 + ], + "moduleForcesY": [ + 38.941407034185524, + 30.903759210330502, + 35.75302989110585, + 25.843937402615378 + ], + "timestamp": 5.7520194521398125 + }, + { + "x": 2.1250808762861264, + "y": 4.544884247765605, + "heading": -0.5412356183232561, + "angularVelocity": 4.429972765944174e-25, + "velocityX": 0.6566001115017817, + "velocityY": -0.30391148650137534, + "moduleForcesX": [ + 52.857513824275856, + 52.857513824275856, + 52.857513824275856, + 52.857513824275856 + ], + "moduleForcesY": [ + -24.465432335003626, + -24.465432335003626, + -24.465432335003626, + -24.465432335003626 + ], + "timestamp": 5.812984379564346 + }, + { + "x": 2.1757132373979986, + "y": 4.521448741342388, + "heading": -0.5412356183232561, + "angularVelocity": 4.426850844156769e-25, + "velocityX": 0.8305162205687572, + "velocityY": -0.38440965015868744, + "moduleForcesX": [ + 52.83813675072153, + 52.83813675072153, + 52.83813675072153, + 52.83813675072153 + ], + "moduleForcesY": [ + -24.45646353477834, + -24.45646353477834, + -24.45646353477834, + -24.45646353477834 + ], + "timestamp": 5.87394930698888 + }, + { + "x": 2.2369405764562798, + "y": 4.493109282816916, + "heading": -0.5412356183232561, + "angularVelocity": 4.433094585405618e-25, + "velocityX": 1.0043043048656628, + "velocityY": -0.4648485567468957, + "moduleForcesX": [ + 52.79924104206797, + 52.799241042067976, + 52.79924104206797, + 52.799241042067976 + ], + "moduleForcesY": [ + -24.438460411677465, + -24.438460411677465, + -24.43846041167746, + -24.438460411677465 + ], + "timestamp": 5.934914234413414 + }, + { + "x": 2.308739248778686, + "y": 4.459876816278918, + "heading": -0.5412356183232561, + "angularVelocity": 4.429972551089785e-25, + "velocityX": 1.1777045484271702, + "velocityY": -0.5451079488142321, + "moduleForcesX": [ + 52.681409623665104, + 52.681409623665104, + 52.681409623665104, + 52.681409623665104 + ], + "moduleForcesY": [ + -24.38392139942926, + -24.38392139942926, + -24.38392139942926, + -24.38392139942926 + ], + "timestamp": 5.995879161837948 + }, + { + "x": 2.372336860863181, + "y": 4.430440261992736, + "heading": -0.5412356183232561, + "angularVelocity": 4.429972551089667e-25, + "velocityX": 1.0431835937674017, + "velocityY": -0.482844079862508, + "moduleForcesX": [ + -40.86934002998661, + -40.86934002998661, + -40.86934002998661, + -40.86934002998661 + ], + "moduleForcesY": [ + 18.91663078221956, + 18.91663078221956, + 18.91663078221956, + 18.91663078221956 + ], + "timestamp": 6.056844089262482 + }, + { + "x": 2.4253616014568182, + "y": 4.405897427696982, + "heading": -0.5412356183232561, + "angularVelocity": 4.43092756165992e-25, + "velocityX": 0.8697581188671928, + "velocityY": -0.4025730093115453, + "moduleForcesX": [ + -52.68907525585909, + -52.68907525585909, + -52.68907525585909, + -52.68907525585909 + ], + "moduleForcesY": [ + 24.38746948544725, + 24.38746948544725, + 24.38746948544725, + 24.38746948544725 + ], + "timestamp": 6.117809016687016 + }, + { + "x": 2.4677909785651333, + "y": 4.386258723952594, + "heading": -0.5412356183232561, + "angularVelocity": 4.429017210736591e-25, + "velocityX": 0.6959637106242286, + "velocityY": -0.3221311756451756, + "moduleForcesX": [ + -52.801162344979105, + -52.801162344979105, + -52.801162344979105, + -52.801162344979105 + ], + "moduleForcesY": [ + 24.439349698799905, + 24.439349698799905, + 24.439349698799905, + 24.439349698799905 + ], + "timestamp": 6.1787739441115495 + }, + { + "x": 2.4996174011461876, + "y": 4.371527664321022, + "heading": -0.5412356183232561, + "angularVelocity": 4.392830640142467e-25, + "velocityX": 0.5220447874796567, + "velocityY": -0.24163170947436932, + "moduleForcesX": [ + -52.838991706698124, + -52.838991706698124, + -52.838991706698124, + -52.838991706698124 + ], + "moduleForcesY": [ + 24.45685925652313, + 24.45685925652313, + 24.45685925652313, + 24.45685925652313 + ], + "timestamp": 6.239738871536083 + }, + { + "x": 2.520837055868033, + "y": 4.361706013826914, + "heading": -0.5412356183232561, + "angularVelocity": 4.46711444507948e-25, + "velocityX": 0.34806331473308955, + "velocityY": -0.16110329182733052, + "moduleForcesX": [ + -52.85799514715946, + -52.85799514715946, + -52.85799514715946, + -52.85799514715946 + ], + "moduleForcesY": [ + 24.465655118324054, + 24.465655118324058, + 24.46565511832406, + 24.465655118324058 + ], + "timestamp": 6.300703798960617 + }, + { + "x": 2.531447649002075, + "y": 4.356794834136963, + "heading": -0.5412356183232561, + "angularVelocity": 4.42997254240294e-25, + "velocityX": 0.17404421824625813, + "velocityY": -0.08055745979575053, + "moduleForcesX": [ + -52.86942576358611, + -52.86942576358611, + -52.86942576358611, + -52.86942576358611 + ], + "moduleForcesY": [ + 24.470945850946546, + 24.470945850946546, + 24.470945850946546, + 24.470945850946546 + ], + "timestamp": 6.361668726385151 + }, + { + "x": 2.531447649002075, + "y": 4.356794834136963, + "heading": -0.5412356183232561, + "angularVelocity": 1.4766575121958547e-25, + "velocityX": 1.1572658590616387e-19, + "velocityY": 2.5061165774890407e-19, + "moduleForcesX": [ + -52.87705810406985, + -52.87705810406985, + -52.87705810406985, + -52.87705810406985 + ], + "moduleForcesY": [ + 24.474478527687335, + 24.474478527687335, + 24.474478527687335, + 24.474478527687335 + ], + "timestamp": 6.422633653809685 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6abd2d92..e75fe8b5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -729,7 +729,7 @@ private Command autoIntake() { .getRotation() .getDegrees(), 0, - 35)), + 20)), () -> 0.0, () -> 0.0) .until(() -> feeder.getFirstBeambreak()) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index a0cb6edc..d977da35 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -737,7 +737,11 @@ public Command choreoFullFollowSwerveCommand( - finalPose.getRotation().getDegrees()) % 360) < 20.0) - && MathUtil.isNear( 0.0, vel.vxMetersPerSecond * vel.vxMetersPerSecond + vel.vyMetersPerSecond * vel.vyMetersPerSecond, 0.25); + && MathUtil.isNear( + 0.0, + vel.vxMetersPerSecond * vel.vxMetersPerSecond + + vel.vyMetersPerSecond * vel.vyMetersPerSecond, + 0.25); }, requirements); } From 69de25daf431c17f061f34e8ffeb31de5cd83b4d Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 26 Sep 2024 17:29:14 -0700 Subject: [PATCH 08/21] increase traj follow vel tolerance --- src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 2cb62681..936abef7 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -754,7 +754,7 @@ public Command choreoFullFollowSwerveCommand( 0.0, vel.vxMetersPerSecond * vel.vxMetersPerSecond + vel.vyMetersPerSecond * vel.vyMetersPerSecond, - 0.25); + 0.75); }, requirements); } From c7052d85fcdf01c0f8fd931eed0bce3356d239e2 Mon Sep 17 00:00:00 2001 From: team computer Date: Sat, 28 Sep 2024 10:23:14 -0700 Subject: [PATCH 09/21] after q3, turned down intake supply --- src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java index 34a18807..4ac0f530 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java @@ -36,7 +36,7 @@ public class IntakeIOReal implements IntakeIO { public IntakeIOReal() { var intakeConfig = new TalonFXConfiguration(); - intakeConfig.CurrentLimits.SupplyCurrentLimit = 20.0; + intakeConfig.CurrentLimits.SupplyCurrentLimit = 15.0; intakeConfig.CurrentLimits.SupplyCurrentLimitEnable = true; intakeConfig.CurrentLimits.StatorCurrentLimit = 120.0; intakeConfig.CurrentLimits.StatorCurrentLimitEnable = true; From 0713411d7df6abbf6016944a3ef4d9ce68ad5d1d Mon Sep 17 00:00:00 2001 From: team computer Date: Sat, 28 Sep 2024 12:27:21 -0700 Subject: [PATCH 10/21] removed tele current threshold and turned up supply limit --- src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java index 381f3d35..7d8343a1 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java @@ -91,10 +91,10 @@ public ModuleIOReal(ModuleConstants constants) { var driveConfig = new TalonFXConfiguration(); // Current limits // TODO: Do we want to limit supply current? - driveConfig.CurrentLimits.SupplyCurrentLimit = 30.0; + driveConfig.CurrentLimits.SupplyCurrentLimit = 60.0; driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - driveConfig.CurrentLimits.SupplyCurrentThreshold = 60.0; - driveConfig.CurrentLimits.SupplyTimeThreshold = 0.5; + // driveConfig.CurrentLimits.SupplyCurrentThreshold = 30.0; + // driveConfig.CurrentLimits.SupplyTimeThreshold = 0.5; driveConfig.CurrentLimits.StatorCurrentLimit = 120.0; driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; // Inverts From 9b902359418e3cff3615d8c760d3f2497f3f3cbf Mon Sep 17 00:00:00 2001 From: team computer Date: Sat, 28 Sep 2024 13:17:58 -0700 Subject: [PATCH 11/21] try new pid values --- .../java/frc/robot/subsystems/swerve/SwerveSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 936abef7..2ecc5de7 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -656,8 +656,8 @@ public Command runChoreoTraj(ChoreoTrajectory traj, boolean resetPose) { traj, this::getPose, Choreo.choreoSwerveController( - new PIDController(1.5, 0.0, 0.0), - new PIDController(1.5, 0.0, 0.0), + new PIDController(10, 0.0, 0.0), + new PIDController(10, 0.0, 0.0), new PIDController(3.0, 0.0, 0.0)), (ChassisSpeeds speeds) -> this.runVelocity(speeds), () -> { From 294d49e1668b113a22f5124aa09be92aef5a768d Mon Sep 17 00:00:00 2001 From: Awesomeplayer165 Date: Sat, 28 Sep 2024 13:21:24 -0700 Subject: [PATCH 12/21] update choreo amp and pickup --- choreo.chor | 5862 +++++++++++------------ src/main/deploy/choreo/line test.1.traj | 373 -- src/main/deploy/choreo/source 3.1.traj | 2766 +++++------ src/main/deploy/choreo/source 3.2.traj | 3086 ++++++------ src/main/deploy/choreo/source 3.traj | 5824 +++++++++++----------- src/main/deploy/choreo/test.1.traj | 3635 +++++++++----- src/main/deploy/choreo/test.traj | 3635 +++++++++----- 7 files changed, 13857 insertions(+), 11324 deletions(-) delete mode 100644 src/main/deploy/choreo/line test.1.traj diff --git a/choreo.chor b/choreo.chor index 445cd96f..0f807e40 100644 --- a/choreo.chor +++ b/choreo.chor @@ -3,8 +3,8 @@ "robotConfiguration": { "mass": 74.08797700309194, "rotationalInertia": 5.724, - "motorMaxTorque": 0.484, - "motorMaxVelocity": 4500, + "motorMaxTorque": 0.5811475409836065, + "motorMaxVelocity": 4800, "gearing": 6.12, "wheelbase": 0.552, "trackWidth": 0.54, @@ -15929,8 +15929,8 @@ "controlIntervalCount": 34 }, { - "x": 7.134490489959717, - "y": 0.7612675428390503, + "x": 7.107321739196777, + "y": 1.0795719623565674, "heading": -0.061776917324722916, "isInitialGuess": false, "translationConstrained": true, @@ -15938,8 +15938,8 @@ "controlIntervalCount": 14 }, { - "x": 8.828963279724121, - "y": 0.7209256887435913, + "x": 8.881949424743652, + "y": 1.0173043012619019, "heading": -0.030602705565700654, "isInitialGuess": false, "translationConstrained": true, @@ -16015,9 +16015,9 @@ "x": 0.7778744697570801, "y": 4.333664894104004, "heading": -1.051650156247713, - "angularVelocity": -3.718899994914549e-23, - "velocityX": 1.067537172973605e-21, - "velocityY": 5.775370181920206e-22, + "angularVelocity": -1.3297634990832717e-18, + "velocityX": 8.096904896283762e-18, + "velocityY": -2.035450603469087e-18, "moduleForcesX": [ 0, 0, @@ -16033,4099 +16033,4099 @@ "timestamp": 0 }, { - "x": 0.7922205153045683, - "y": 4.322569747233587, - "heading": -1.038589316498916, - "angularVelocity": 0.18665078221022835, - "velocityX": 0.2050174930986984, - "velocityY": -0.15855931792528452, + "x": 0.7908719793717586, + "y": 4.323476837419605, + "heading": -1.0413708097378205, + "angularVelocity": 0.15431550267407224, + "velocityX": 0.19512108359903374, + "velocityY": -0.15294504247054264, "moduleForcesX": [ - 64.99353342135232, - 58.994889041922896, - 48.6051352797362, - 44.47522386087147 + 63.74590962361433, + 58.38725396448112, + 49.37860903101583, + 45.506342584710694 ], "moduleForcesY": [ - -25.906794419809312, - -37.63755378362518, - -50.312927149408814, - -54.02243790369699 + -28.834822023862102, + -38.568182065872634, + -49.55287419418438, + -53.15308295520182 ], - "timestamp": 0.06997473889011878 + "timestamp": 0.06661253297151565 }, { - "x": 0.8209322601683935, - "y": 4.300376239957637, - "heading": -1.0127877530474698, - "angularVelocity": 0.3687268271477519, - "velocityX": 0.4103158556820229, - "velocityY": -0.3171645600678816, + "x": 0.8168891188149916, + "y": 4.303112092011227, + "heading": -1.0210382023309228, + "angularVelocity": 0.3052369651757899, + "velocityX": 0.3905742400511663, + "velocityY": -0.30571942695958465, "moduleForcesX": [ - 64.75012154981113, - 59.119805707703335, - 48.51394542600652, - 44.98228840684379 + 63.57502695706841, + 58.50337248946953, + 49.388651702378716, + 45.92040304941029 ], "moduleForcesY": [ - -26.49930496933997, - -37.43568863955079, - -50.39606166150177, - -53.59728170965293 + -29.201580891943905, + -38.38697260898019, + -49.53833328341483, + -52.7922647589104 ], - "timestamp": 0.13994947778023756 + "timestamp": 0.1332250659430313 }, { - "x": 0.8640312929701923, - "y": 4.267081252649147, - "heading": -0.9746011298056743, - "angularVelocity": 0.5457201248261894, - "velocityX": 0.6159227384824854, - "velocityY": -0.47581438439909957, + "x": 0.8559503614500349, + "y": 4.272583897748778, + "heading": -0.9908994985401418, + "angularVelocity": 0.4524479470502735, + "velocityX": 0.5863947952819387, + "velocityY": -0.45829505200625403, "moduleForcesX": [ - 64.44611808173948, - 59.310850143621366, - 48.32509669282183, - 45.610751749821475 + 63.36650355926644, + 58.673994480374716, + 49.332465125535784, + 46.42312032557307 ], "moduleForcesY": [ - -27.21858213973262, - -37.12584717532736, - -50.57187006691617, - -53.05924040607812 + -29.64202317080124, + -38.12000070194584, + -49.58929691252429, + -52.34676597725229 ], - "timestamp": 0.20992421667035632 + "timestamp": 0.19983759891454694 }, { - "x": 0.9215421276125146, - "y": 4.222681267524096, - "heading": -0.9244509530719824, - "angularVelocity": 0.7166897301673768, - "velocityX": 0.8218799463136471, - "velocityY": -0.6345144809296546, + "x": 0.9080832434863456, + "y": 4.231907621482131, + "heading": -0.9512364839325822, + "angularVelocity": 0.5954287104578356, + "velocityX": 0.7826287293204035, + "velocityY": -0.6106399869832296, "moduleForcesX": [ - 64.05688808437033, - 59.54566542911579, - 48.099567813510134, - 46.36161309260976 + 63.1087538854026, + 58.88683733974907, + 49.24358469003159, + 47.01667690510743 ], "moduleForcesY": [ - -28.108700702243887, - -36.74020593171785, - -50.78046522808163, - -52.39939521356428 + -30.17612796676512, + -37.78365700090767, + -49.672009408886844, + -51.809713634059214 ], - "timestamp": 0.2798989555604751 + "timestamp": 0.2664501318860626 }, { - "x": 0.9934931064812429, - "y": 4.167171543523546, - "heading": -0.8628639745372912, - "angularVelocity": 0.880131594794532, - "velocityX": 1.0282421915387603, - "velocityY": -0.7932823313241175, + "x": 0.9733191777952942, + "y": 4.181101065602064, + "heading": -0.9023828510876672, + "angularVelocity": 0.7334000174682638, + "velocityX": 0.9793342393516886, + "velocityY": -0.7627176690877809, "moduleForcesX": [ - 63.55085157875546, - 59.79482586920611, - 47.91149012286905, - 47.23541305248315 + 62.78758091347448, + 59.12644232420397, + 49.162542801992586, + 47.70378434486226 ], "moduleForcesY": [ - -29.21888766916689, - -36.32352543051382, - -50.95113504071442, - -51.60695549029941 + -30.82641384591526, + -37.399521862180414, + -49.74596927329427, + -51.172358294771385 ], - "timestamp": 0.3498736944505939 + "timestamp": 0.33306266485757824 }, { - "x": 1.0799170133820089, - "y": 4.100544943426046, - "heading": -0.7905211436285549, - "angularVelocity": 1.0338420986798709, - "velocityX": 1.2350729459166292, - "velocityY": -0.9521521788330469, + "x": 1.051694367518301, + "y": 4.120184874713526, + "heading": -0.8447458342032448, + "angularVelocity": 0.8652578473343552, + "velocityX": 1.1765832378198438, + "velocityY": -0.9144854304607654, "moduleForcesX": [ - 62.890212743310435, - 60.01889414692214, - 47.84890703697811, - 48.23061564013138 + 62.38681753882747, + 59.37326562455472, + 49.13769712317514, + 48.48705032206493 ], "moduleForcesY": [ - -30.59654996800034, - -35.939691766596795, - -51.0018835950638, - -50.67037098511977 + -31.615322936234296, + -36.99658263811015, + -49.763347170131574, + -50.42430963220079 ], - "timestamp": 0.4198484333407127 + "timestamp": 0.3996751978290939 }, { - "x": 1.1808511971073097, - "y": 4.022790644493082, - "heading": -0.708315499162293, - "angularVelocity": 1.174790299615828, - "velocityX": 1.4424374470878363, - "velocityY": -1.1111766927070885, + "x": 1.1432508333296558, + "y": 4.049183171168427, + "heading": -0.778832566995066, + "angularVelocity": 0.9895024895144598, + "velocityX": 1.3744630586335072, + "velocityY": -1.065891062504106, "moduleForcesX": [ - 62.034245895880275, - 60.16310541057914, - 48.01415268705809, - 49.342246941586 + 61.889733856839435, + 59.602244362756366, + 49.22597947357009, + 49.36848790198351 ], "moduleForcesY": [ - -32.275569212980486, - -35.68171414287474, - -50.836548780375075, - -49.57842210438686 + -32.56119938527781, + -36.61435191109544, + -49.66762908534543, + -49.55361350286737 ], - "timestamp": 0.4898231722308315 + "timestamp": 0.46628773080060953 }, { - "x": 1.2963371243384598, - "y": 3.933893322355774, - "heading": -0.6174207883006573, - "angularVelocity": 1.298964630712914, - "velocityX": 1.650394543272217, - "velocityY": -1.2704202051672215, + "x": 1.2480376237140218, + "y": 3.968124696368845, + "heading": -0.7052817755202949, + "angularVelocity": 1.1041584547794074, + "velocityX": 1.57307920461716, + "velocityY": -1.2168652231591865, "moduleForcesX": [ - 60.94746004979524, - 60.146829265545456, - 48.525569264290525, - 50.56132114483972 + 61.28176481668813, + 59.78038532288985, + 49.4938549994705, + 50.34939808767109 ], "moduleForcesY": [ - -34.25932977118567, - -35.687927503268604, - -50.336093067995485, - -48.32077560597764 + -33.67250970561668, + -36.30739888304486, + -49.39059309707039, + -48.546399889287585 ], - "timestamp": 0.5597979111209502 + "timestamp": 0.5329002637721252 }, { - "x": 1.4264194135128296, - "y": 3.833833792551993, - "heading": -0.5193851332120467, - "angularVelocity": 1.4010149468732676, - "velocityX": 1.8589892758104873, - "velocityY": -1.429937880309981, + "x": 1.3661123895110021, + "y": 3.877044967671322, + "heading": -0.6249031875065618, + "angularVelocity": 1.206658633565307, + "velocityX": 1.7725608159574173, + "velocityY": -1.3673061905102768, "moduleForcesX": [ - 59.614996756079215, - 59.84049412454702, - 49.524716335836736, - 51.876090247246715 + 60.55523039747262, + 59.86237923234887, + 50.01933061446567, + 51.43105327265715 ], "moduleForcesY": [ - -36.50078923453568, - -36.17056672706365, - -49.33646413057858, - -46.88658430312503 + -34.9402966200205, + -36.15231999767806, + -48.845768467491496, + -47.385486747890475 ], - "timestamp": 0.629772650011069 + "timestamp": 0.5995127967436409 }, { - "x": 1.5711438836172125, - "y": 3.722592592486754, - "heading": -0.416299104221778, - "angularVelocity": 1.4731891912043358, - "velocityX": 2.0682388016001525, - "velocityY": -1.589733692896242, + "x": 1.4975436678185934, + "y": 3.7759903450547028, + "heading": -0.53873230694548, + "angularVelocity": 1.293613629704331, + "velocityX": 1.9730713192334703, + "velocityY": -1.5170511930514805, "moduleForcesX": [ - 58.065083972008736, - 59.00957277137866, - 51.197174245280834, - 53.27774962436911 + 59.71660813639446, + 59.781944702186415, + 50.89674120838264, + 52.617056525494455 ], "moduleForcesY": [ - -38.886690128517934, - -37.47125653206859, - -47.574170293153024, - -45.2567742927428 + -36.32951289772708, + -36.25934407553948, + -47.91447235046562, + -46.0464745895729 ], - "timestamp": 0.6997473889011877 + "timestamp": 0.6661253297151566 }, { - "x": 1.7305504220603747, - "y": 3.6001600451974833, - "heading": -0.3111713227790686, - "angularVelocity": 1.502367613086651, - "velocityX": 2.2780583532219847, - "velocityY": -1.749667797710917, + "x": 1.6424143533198352, + "y": 3.665025662326569, + "heading": -0.4481205140243463, + "angularVelocity": 1.3602814497368003, + "velocityX": 2.174826253239616, + "velocityY": -1.665822898156168, "moduleForcesX": [ - 56.39097412755216, - 57.161179416740495, - 53.820924588217835, - 54.780035432769175 + 58.79631756896231, + 59.433248892591386, + 52.247409534940125, + 53.9194587893842 ], "moduleForcesY": [ - -41.23803886906891, - -40.17856661949012, - -44.545308010292885, - -43.37339903624029 + -37.77070605252747, + -36.79353882986422, + -46.41555660113537, + -44.48747884319979 ], - "timestamp": 0.7697221277913064 + "timestamp": 0.7327378626866723 }, { - "x": 1.904638990279975, - "y": 3.4665697709891212, - "heading": -0.20885800973766258, - "angularVelocity": 1.462146406892186, - "velocityX": 2.4878773537543495, - "velocityY": -1.909121439068728, + "x": 1.800826604673265, + "y": 3.5442490372900517, + "heading": -0.3549074004558732, + "angularVelocity": 1.3993329694928751, + "velocityX": 2.3781148124358094, + "velocityY": -1.8131216401598682, "moduleForcesX": [ - 54.74964870144604, - 53.05199961529088, - 57.85147404335248, - 56.499407721715734 + 57.85964296595883, + 58.62699304555619, + 54.24098287937368, + 55.37454877680063 ], "moduleForcesY": [ - -43.34419920923391, - -45.385040535603636, - -39.0969722773908, - -41.00039456972551 + -39.15408809177324, + -38.01758274488999, + -44.03657792902701, + -42.620768436938704 ], - "timestamp": 0.8396968666814252 + "timestamp": 0.799350395658188 }, { - "x": 2.09320393495465, - "y": 3.3220757989813476, - "heading": -0.11854546488401964, - "angularVelocity": 1.2906449711153707, - "velocityX": 2.694757389102635, - "velocityY": -2.064944782926198, + "x": 1.9729064746231015, + "y": 3.4138237900863677, + "heading": -0.261785676023239, + "angularVelocity": 1.3979610184235784, + "velocityX": 2.58329569937567, + "velocityY": -1.9579685891760825, "moduleForcesX": [ - 53.2728143917394, - 42.775288784328325, - 63.8442089345851, - 59.14849524914464 + 57.01503970270268, + 56.97264868755776, + 57.13055506988329, + 57.088614242525495 ], "moduleForcesY": [ - -45.07889412240785, - -55.05640389507177, - -28.09637912144248, - -36.751236686389184 + -40.32815265808299, + -40.3871603414814, + -40.16401782504414, + -40.22274635092099 ], - "timestamp": 0.9096716055715439 + "timestamp": 0.8659629286297037 }, { - "x": 2.295164186316796, - "y": 3.1696331321710134, - "heading": -0.06408923152074174, - "angularVelocity": 0.778227032026378, - "velocityX": 2.886187995346207, - "velocityY": -2.178538558746386, + "x": 2.1587959216901558, + "y": 3.2740626515960307, + "heading": -0.1731498612166921, + "angularVelocity": 1.3306176908848175, + "velocityX": 2.7906076945992013, + "velocityY": -2.0981207628015075, "moduleForcesX": [ - 51.59206019363968, - 13.364464020811422, - 69.52592713911473, - 68.20078231069633 + 56.41042369707265, + 53.501719202644416, + 61.26419264721754, + 59.40078934825349 ], "moduleForcesY": [ - -46.885084851896735, - -68.25325997043356, - -3.8069250749386283, - -1.3257476748867494 + -41.1079010332853, + -44.78370054636255, + -33.42235417217702, + -36.56648525897984 ], - "timestamp": 0.9796463444616627 + "timestamp": 0.9325754616012194 }, { - "x": 2.5102036576315885, - "y": 3.0099910805069405, - "heading": -0.048739655045187506, - "angularVelocity": 0.21935882461323145, - "velocityX": 3.0731014466873368, - "velocityY": -2.281424042392765, + "x": 2.3585571947909116, + "y": 3.1257497783866914, + "heading": -0.09743941845228067, + "angularVelocity": 1.1365795502294775, + "velocityX": 2.9988541823830035, + "velocityY": -2.2265010290594955, "moduleForcesX": [ - 51.68312483431202, - 11.490644239292044, - 69.23100690631284, - 65.49577647910316 + 56.18321511797377, + 45.13645782831661, + 66.6575745567656, + 63.63924131102192 ], "moduleForcesY": [ - -46.34143932256032, - -68.12951985238924, - -1.4483890023414814, - 6.986075009746601 + -41.32675572742186, + -53.045678977052305, + -20.39623436243562, + -28.01878833525786 ], - "timestamp": 1.0496210833517814 + "timestamp": 0.9991879945727351 }, { - "x": 2.7349332678515874, - "y": 2.854041862643359, - "heading": -0.04856305611607661, - "angularVelocity": 0.002523752598608622, - "velocityX": 3.2115819763599447, - "velocityY": -2.228650229170103, + "x": 2.571323749679796, + "y": 2.9722890611298696, + "heading": -0.05631702040111217, + "angularVelocity": 0.6173372519665775, + "velocityX": 3.1940919433264323, + "velocityY": -2.303781441886374, "moduleForcesX": [ - 44.06076488703463, - 32.04578879681631, - 41.59848030725396, - 28.915624556417924 + 56.49315652197762, + 24.727674210970267, + 69.53856728119115, + 66.38848839184168 ], "moduleForcesY": [ - 2.892829937081654, - 5.007695054030006, - 21.07556807654082, - 26.89985763143924 + -40.73364522523241, + -64.83876276810156, + 2.4259673258224868, + 17.19340461150298 ], - "timestamp": 1.1195958222419002 + "timestamp": 1.0658005275442508 }, { - "x": 2.9682298777966594, - "y": 2.7112552443984246, - "heading": -0.048304755129702115, - "angularVelocity": 0.0036913461982344513, - "velocityX": 3.3340118683603395, - "velocityY": -2.040545209738506, + "x": 2.7978969801578932, + "y": 2.816123256954651, + "heading": -0.04923559160029977, + "angularVelocity": 0.1063077544857887, + "velocityX": 3.4013603802603107, + "velocityY": -2.3443907206170564, "moduleForcesX": [ - 32.3225255016905, - 32.43770393632479, - 32.3755224257375, - 32.49078432468219 + 61.70982060481134, + 40.01285570538606, + 68.37167087206099, + 60.43433119111175 ], "moduleForcesY": [ - 49.850764221083146, - 49.79054196126857, - 49.79062122677659, - 49.730235606563355 + -31.4520746032983, + -55.70693219082933, + 10.727828044445207, + 31.264613623300775 ], - "timestamp": 1.189570561132019 + "timestamp": 1.1324130605157665 }, { - "x": 3.206226948407469, - "y": 2.57644940649246, - "heading": -0.04804302634103678, - "angularVelocity": 0.0037403324802159203, - "velocityX": 3.4011855476093436, - "velocityY": -1.9264929036412723, + "x": 3.0347601843563274, + "y": 2.6710905971518515, + "heading": -0.04900748324484257, + "angularVelocity": 0.003424405968089362, + "velocityX": 3.55583542063653, + "velocityY": -2.1772578422264344, "moduleForcesX": [ - 17.778201901232073, - 17.782317553134607, - 17.77881545073154, - 17.782931249096187 + 50.41350702863919, + 41.75482316229609, + 44.362342200824024, + 35.27998738074212 ], "moduleForcesY": [ - 30.19121333723645, - 30.19026343520494, - 30.187987107924002, - 30.187037184259207 + 38.99730957905846, + 46.617569740155716, + 46.965805915927774, + 53.3083103983914 ], - "timestamp": 1.259545300022138 + "timestamp": 1.1990255934872822 }, { - "x": 3.4454066255541647, - "y": 2.443753454780269, - "heading": -0.04778008796546239, - "angularVelocity": 0.003757618531271475, - "velocityX": 3.418086025619602, - "velocityY": -1.896340791218425, + "x": 3.2779712543514585, + "y": 2.5369613930825095, + "heading": -0.04879559855232118, + "angularVelocity": 0.0031808532579297136, + "velocityX": 3.6511307879424364, + "velocityY": -2.0135730933201033, "moduleForcesX": [ - 4.472855661174314, - 4.474147280296156, - 4.472811619653854, - 4.474103244435602 + 26.514032908634963, + 26.48917929707793, + 26.505657516129617, + 26.480809194623138 ], "moduleForcesY": [ - 7.9818138726911165, - 7.981734606875763, - 7.980518679466535, - 7.980439415924407 + 45.5016606384836, + 45.51171619004381, + 45.5152401681101, + 45.525290455269825 ], - "timestamp": 1.3295200389122568 + "timestamp": 1.265638126458798 }, { - "x": 3.68491109766373, - "y": 2.3116446526478165, - "heading": -0.04751696829794281, - "angularVelocity": 0.003760209351159636, - "velocityX": 3.4227276286898194, - "velocityY": -1.8879499120375856, + "x": 3.5240170136884648, + "y": 2.4081056080213394, + "heading": -0.04858282682880418, + "angularVelocity": 0.003194169535753657, + "velocityX": 3.6936856828761946, + "velocityY": -1.93440752532666, "moduleForcesX": [ - 1.2285197916199417, - 1.228711642997039, - 1.2285106844862757, - 1.228702535899101 + 11.832044679638638, + 11.833159737293062, + 11.83210215812816, + 11.833217225479276 ], "moduleForcesY": [ - 2.2211300227139463, - 2.2211206994127397, - 2.22093420243456, - 2.220924879148784 + 22.01300562318127, + 22.0128485655156, + 22.01202572778171, + 22.011868671306686 ], - "timestamp": 1.3994947778023756 + "timestamp": 1.3322506594303136 }, { - "x": 3.924585147927277, - "y": 2.1798436167917243, - "heading": -0.047254057832202365, - "angularVelocity": 0.0037572196754216245, - "velocityX": 3.425151048293395, - "velocityY": -1.883551663737667, + "x": 3.7708350348315482, + "y": 2.2807352279621504, + "heading": -0.0483699449005901, + "angularVelocity": 0.0031958239496029865, + "velocityX": 3.7052790238231275, + "velocityY": -1.9121083432400703, "moduleForcesX": [ - 0.6415734362803132, - 0.6413521699115629, - 0.6415840632535661, - 0.6413627969097583 + 3.2235324585038754, + 3.2236618531632817, + 3.2235271302506407, + 3.2236565249546465 ], "moduleForcesY": [ - 1.164077957656933, - 1.1640884844019281, - 1.1643040474316328, - 1.1643145741874217 + 6.200483556552904, + 6.200476317722027, + 6.200352895038578, + 6.200345656224632 ], - "timestamp": 1.4694695166924945 + "timestamp": 1.3988631924018293 }, { - "x": 4.164406438877476, - "y": 2.0483104663276324, - "heading": -0.04699153890493271, - "angularVelocity": 0.0037516242494578765, - "velocityX": 3.427255246022282, - "velocityY": -1.8797233480304725, + "x": 4.017945860044819, + "y": 2.153933675847828, + "heading": -0.04815735530566025, + "angularVelocity": 0.0031914353868025967, + "velocityX": 3.709674654151622, + "velocityY": -1.90356899006594, "moduleForcesX": [ - 0.5571686890446169, - 0.556754586714119, - 0.5571884977126166, - 0.556774395458144 + 1.2223925594156837, + 1.2220511392203353, + 1.2224089657777908, + 1.222067545703039 ], "moduleForcesY": [ - 1.0131163405590924, - 1.013135903625798, - 1.013539512185854, - 1.0135590752854269 + 2.374233190641205, + 2.3742500204088812, + 2.3745815684863776, + 2.374598398301541 ], - "timestamp": 1.5394442555826133 + "timestamp": 1.465475725373345 }, { - "x": 4.404389812405626, - "y": 1.917073025680941, - "heading": -0.046729445162668845, - "angularVelocity": 0.0037455479851869126, - "velocityX": 3.4295715473121766, - "velocityY": -1.8754973970359978, + "x": 4.265335286953013, + "y": 2.027676292006606, + "heading": -0.04794522174606807, + "angularVelocity": 0.0031845892976762468, + "velocityX": 3.7138570757244884, + "velocityY": -1.895399832569214, "moduleForcesX": [ - 0.6133285579559523, - 0.6128788486139368, - 0.6133499263194478, - 0.6129002170763577 + 1.163199663544347, + 1.1626670888284039, + 1.1632251975919956, + 1.1626926239772695 ], "moduleForcesY": [ - 1.1183496176451213, - 1.1183707656322825, - 1.1188091442741797, - 1.1188302923040183 + 2.271195979291761, + 2.271222065843487, + 2.271739492202394, + 2.2717655728058856 ], - "timestamp": 1.6094189944727322 + "timestamp": 1.5320882583448607 }, { - "x": 4.6445839308598735, - "y": 1.7862215631603924, - "heading": -0.04646750224020687, - "angularVelocity": 0.0037433926387822392, - "velocityX": 3.432583275965109, - "velocityY": -1.8699814332429918, + "x": 4.513387263491877, + "y": 1.9027254137370513, + "heading": -0.04773334654928872, + "angularVelocity": 0.003180710706046184, + "velocityX": 3.7238033966511064, + "velocityY": -1.8757863227177534, "moduleForcesX": [ - 0.7972668572924495, - 0.7971073142412101, - 0.7972743634616845, - 0.7971148204266829 + 2.765776223844279, + 2.765473269227589, + 2.7657891163065877, + 2.7654861619049216 ], "moduleForcesY": [ - 1.4599652235465161, - 1.4599727135025415, - 1.4601282052544333, - 1.4601356952174342 + 5.453486682132146, + 5.453502875542226, + 5.453793409345764, + 5.453809602835306 ], - "timestamp": 1.679393733362851 + "timestamp": 1.5987007913163764 }, { - "x": 4.885064829086469, - "y": 1.6558983523007769, - "heading": -0.04620453225018068, - "angularVelocity": 0.003758070329338863, - "velocityX": 3.4366816088334584, - "velocityY": -1.8624322566499572, + "x": 4.7636905451889415, + "y": 1.7823482264471835, + "heading": -0.04752024111954168, + "angularVelocity": 0.003199179197076531, + "velocityX": 3.7576004173883883, + "velocityY": -1.8071252048220714, "moduleForcesX": [ - 1.0842918084117592, - 1.0853786132925534, - 1.084241424204242, - 1.085328230114811 + 9.396675512144679, + 9.39819901784068, + 9.396709254967268, + 9.398232777338956 ], "moduleForcesY": [ - 1.9988129858279566, - 1.9987618336162647, - 1.997703395532972, - 1.9976522437601878 + 19.092390606150673, + 19.092220862164694, + 19.091009053610517, + 19.090839312476135 ], - "timestamp": 1.7493684722529699 + "timestamp": 1.6653133242878921 }, { - "x": 5.125797171042743, - "y": 1.5260410914198634, - "heading": -0.04593987286096788, - "angularVelocity": 0.0037822133159851846, - "velocityX": 3.4402749588575916, - "velocityY": -1.8557734253903861, + "x": 5.0188400094891765, + "y": 1.6726490442267128, + "heading": -0.04723633820300304, + "angularVelocity": 0.004262004518879671, + "velocityX": 3.8303522313036735, + "velocityY": -1.646824964115687, "moduleForcesX": [ - 0.9502910191372389, - 0.9520784538822461, - 0.95020827131857, - 0.9519957085202748 + 20.160339598017387, + 20.271778827791216, + 20.186266828439518, + 20.297839903273452 ], "moduleForcesY": [ - 1.7635166253693095, - 1.7634333067623955, - 1.7616912237363618, - 1.7616079061721086 + 44.61714719861703, + 44.58388431235552, + 44.56094399930638, + 44.52760480062931 ], - "timestamp": 1.8193432111430887 + "timestamp": 1.7319258572594078 }, { - "x": 5.366788045768459, - "y": 1.396664893828562, - "heading": -0.04567411476495212, - "angularVelocity": 0.0037979147936955102, - "velocityX": 3.443969617437849, - "velocityY": -1.8488986117470256, + "x": 5.276040962243286, + "y": 1.5678524061254324, + "heading": -0.046947349952411, + "angularVelocity": 0.0043383465197998905, + "velocityX": 3.8611495657144865, + "velocityY": -1.5732270404143434, "moduleForcesX": [ - 0.977404742123205, - 0.9785672603177287, - 0.9773512911124315, - 0.978513810379268 + 8.560118396401565, + 8.566501267033127, + 8.560247116014803, + 8.56663029963911 ], "moduleForcesY": [ - 1.8203523461919868, - 1.820298421093803, - 1.8191652136181402, - 1.8191112889734293 + 20.46747803904423, + 20.466793414894244, + 20.461850439324866, + 20.461165848040086 ], - "timestamp": 1.8893179500332076 + "timestamp": 1.7985383902309235 }, { - "x": 5.608450195426062, - "y": 1.2685474906510974, - "heading": -0.045407399320727376, - "angularVelocity": 0.003811596133907239, - "velocityX": 3.453562721214088, - "velocityY": -1.830909342564999, + "x": 5.534216899574317, + "y": 1.4654824117182799, + "heading": -0.04665631309500156, + "angularVelocity": 0.004369100594536302, + "velocityX": 3.8757862194105366, + "velocityY": -1.536797804264961, "moduleForcesX": [ - 2.5387638123361675, - 2.539779903515735, - 2.538721732122775, - 2.5397378254255 + 4.068624748426005, + 4.071060153628479, + 4.068549709495257, + 4.070985139742783 ], "moduleForcesY": [ - 4.762218612064458, - 4.762167761549727, - 4.761187255842194, - 4.761136406193755 + 10.13063153345665, + 10.130481600946313, + 10.128231656616295, + 10.128081730271346 ], - "timestamp": 1.9592926889233264 + "timestamp": 1.8651509232024392 }, { - "x": 5.852933619648411, - "y": 1.1459010453590361, - "heading": -0.04513772147587357, - "angularVelocity": 0.0038539314205556937, - "velocityX": 3.493881193415528, - "velocityY": -1.7527245865776317, + "x": 5.794372355867246, + "y": 1.3682547977858872, + "heading": -0.04636219278537718, + "angularVelocity": 0.004415389964980432, + "velocityX": 3.905503134135057, + "velocityY": -1.459599411629766, "moduleForcesX": [ - 10.670372351980113, - 10.673724191649343, - 10.670507188995435, - 10.673859113242672 + 8.260963536224425, + 8.264869277780319, + 8.261045504013175, + 8.264951367768367 ], "moduleForcesY": [ - 20.696850436257613, - 20.696435019435945, - 20.693861640190523, - 20.69344623623891 + 21.467349064567532, + 21.466934299940117, + 21.46397136076802, + 21.463556605012805 ], - "timestamp": 2.029267427813445 + "timestamp": 1.931763456173955 }, { - "x": 6.1035014115315445, - "y": 1.0362251026771625, - "heading": -0.04485903801696482, - "angularVelocity": 0.003982629493571362, - "velocityX": 3.580832109664594, - "velocityY": -1.5673648008047245, + "x": 6.058520814972377, + "y": 1.282472529523383, + "heading": -0.04606005529713513, + "angularVelocity": 0.004535745373490557, + "velocityX": 3.965446850941467, + "velocityY": -1.2877797080496214, "moduleForcesX": [ - 23.006805416844802, - 23.020298874307237, - 23.01072211535244, - 23.02421749157213 + 16.659711714491884, + 16.673161829473102, + 16.66222092531276, + 16.675673239510985 ], "moduleForcesY": [ - 49.06916713204879, - 49.064472309009574, - 49.06330827580301, - 49.05861198170382 + 47.77989163980646, + 47.77656557923747, + 47.77432962004692, + 47.771002564051436 ], - "timestamp": 2.0992421667035637 + "timestamp": 1.9983759891454707 }, { - "x": 6.360440389666232, - "y": 0.9424571330795427, - "heading": -0.04456198298429893, - "angularVelocity": 0.004245175292934805, - "velocityX": 3.6718819135310827, - "velocityY": -1.3400260020242942, + "x": 6.326935018479887, + "y": 1.2111884138637838, + "heading": -0.04572387766988923, + "angularVelocity": 0.0050467624071529864, + "velocityX": 4.0294850163149665, + "velocityY": -1.0701306868195675, "moduleForcesX": [ - 24.07899931263681, - 24.111464518434406, - 24.089464997361308, - 24.121942503866567 + 17.76434909368371, + 17.833039573651952, + 17.77928266449683, + 17.84803536932326 ], "moduleForcesY": [ - 60.18510366880944, - 60.17310602326288, - 60.17798562768333, - 60.165977537276476 + 60.533843276425394, + 60.51552676552759, + 60.52159010023538, + 60.50324061772363 ], - "timestamp": 2.1692169055936823 + "timestamp": 2.064988522116986 }, { - "x": 6.622860129273645, - "y": 0.8657147607838669, - "heading": -0.04392950732433488, - "angularVelocity": 0.009038628367834598, - "velocityX": 3.7502067713248666, - "velocityY": -1.096715379191114, + "x": 6.598149001232439, + "y": 1.1556493567666084, + "heading": -0.04255962682130831, + "angularVelocity": 0.04750233488244815, + "velocityX": 4.071515834242889, + "velocityY": -0.8337628764383538, "moduleForcesX": [ - 20.32544719556, - 20.966458363180468, - 20.495346686232793, - 21.14167832393734 + 8.287572421549362, + 14.286479207247982, + 8.808436728764809, + 15.365145055742365 ], "moduleForcesY": [ - 64.54278261295427, - 64.34395175999101, - 64.46421735586846, - 64.26189706479809 + 66.4507233464468, + 65.45898479609045, + 66.07372493186959, + 64.91023150582662 ], - "timestamp": 2.239191644483801 + "timestamp": 2.1316010550885016 }, { - "x": 6.884679748309282, - "y": 0.806794359236595, - "heading": -0.030602705565700657, - "angularVelocity": 0.1904516111101361, - "velocityX": 3.7416305253638975, - "velocityY": -0.8420238857881919, + "x": 6.859559783326081, + "y": 1.113135958184906, + "heading": -0.030602705565701032, + "angularVelocity": 0.17949957346195378, + "velocityX": 3.9243483235418233, + "velocityY": -0.6382192161928673, "moduleForcesX": [ - -12.193365917081998, - 9.151089894849695, - -18.363615689541977, - 12.325518942868378 + -42.45988336627168, + -29.346972124084434, + -52.177609628795565, + -39.698593640879075 ], "moduleForcesY": [ - 67.8990355400377, - 68.35833703061081, - 66.03471322560178, - 67.3706211378861 + 54.43758266703957, + 62.32119709611513, + 44.88720682067673, + 55.84212799694645 ], - "timestamp": 2.3091663833739196 + "timestamp": 2.198213588060017 }, { - "x": 7.134490489959717, - "y": 0.7612675428390503, - "heading": -0.03060270556570066, - "angularVelocity": 1.4169381433025497e-21, - "velocityX": 3.570013202088712, - "velocityY": -0.6506178818192586, + "x": 7.107321739196777, + "y": 1.0795719623565674, + "heading": -0.030602705565701018, + "angularVelocity": 3.347783365519795e-18, + "velocityX": 3.7194495512825236, + "velocityY": -0.5038690818541787, "moduleForcesX": [ - -45.9398192805324, - -59.450192458631626, - -29.698787647568384, - -46.616491719965026 + -61.323422950505716, + -65.95546140653828, + -44.62846950333632, + -55.985729468867824 ], "moduleForcesY": [ - 51.92283929106931, - 36.09371888383255, - 62.89922393782059, - 51.7414031119671 + 32.44664959475204, + 22.20881577411158, + 53.33306981117861, + 41.43874553341409 ], - "timestamp": 2.379141122264038 + "timestamp": 2.2648261210315326 }, { - "x": 7.356199543080895, - "y": 0.7288568209198342, - "heading": -0.03060270556570066, - "angularVelocity": -2.149395300731945e-22, - "velocityX": 3.3790601126213313, - "velocityY": -0.49397070672899657, + "x": 7.342124626950215, + "y": 1.053916100516507, + "heading": -0.030602705565701004, + "angularVelocity": 7.672386391662901e-19, + "velocityX": 3.4979351696684997, + "velocityY": -0.3822037381956934, "moduleForcesX": [ - -53.90473620010748, - -53.90473620010748, - -53.90473620010747, - -53.90473620010747 + -61.122061696457, + -61.122061696457, + -61.122061696457, + -61.122061696457 ], "moduleForcesY": [ - 44.22041388953324, - 44.22041388953323, - 44.22041388953325, - 44.22041388953323 + 33.57089768760224, + 33.57089768760224, + 33.57089768760223, + 33.57089768760224 ], - "timestamp": 2.444753761794498 + "timestamp": 2.3319522615585138 }, { - "x": 7.563645437858805, - "y": 0.7041763764831177, - "heading": -0.03060270556570066, - "angularVelocity": -2.1492569533532087e-22, - "velocityX": 3.161675803053267, - "velocityY": -0.376153811420113, + "x": 7.56101179722364, + "y": 1.0341858544549376, + "heading": -0.03060270556570099, + "angularVelocity": 7.672403149471102e-19, + "velocityX": 3.260833537501588, + "velocityY": -0.2939279080649534, "moduleForcesX": [ - -61.36608679123134, - -61.36608679123133, - -61.36608679123134, - -61.36608679123134 + -65.42302348088484, + -65.42302348088484, + -65.42302348088484, + -65.42302348088484 ], "moduleForcesY": [ - 33.25889452354701, - 33.258894523547006, - 33.258894523547006, - 33.258894523547006 + 24.35778975736353, + 24.357789757363527, + 24.357789757363534, + 24.357789757363534 ], - "timestamp": 2.510366401324958 + "timestamp": 2.399078402085495 }, { - "x": 7.756067149920293, - "y": 0.6856463295872331, - "heading": -0.03060270556570066, - "angularVelocity": -2.148886089665622e-22, - "velocityX": 2.932692746984486, - "velocityY": -0.28241581238752267, + "x": 7.76353893371165, + "y": 1.0190536925576863, + "heading": -0.030602705565700973, + "angularVelocity": 7.672257598260093e-19, + "velocityX": 3.0171127804763933, + "velocityY": -0.22542874919449496, "moduleForcesX": [ - -64.6403326917135, - -64.6403326917135, - -64.6403326917135, - -64.6403326917135 + -67.24942660173558, + -67.24942660173558, + -67.24942660173558, + -67.24942660173558 ], "moduleForcesY": [ - 26.461588675373616, - 26.461588675373616, - 26.461588675373616, - 26.461588675373616 + 18.900848713116805, + 18.900848713116805, + 18.9008487131168, + 18.900848713116808 ], - "timestamp": 2.575979040855418 + "timestamp": 2.466204542612476 }, { - "x": 7.9330736647908635, - "y": 0.6722308870628335, - "heading": -0.03060270556570066, - "angularVelocity": -2.14835209295751e-22, - "velocityX": 2.697750252653663, - "velocityY": -0.2044643017016828, + "x": 7.949481024014891, + "y": 1.0076590456800256, + "heading": -0.03060270556570096, + "angularVelocity": 7.672013523245006e-19, + "velocityX": 2.7700399403791343, + "velocityY": -0.16974976943713838, "moduleForcesX": [ - -66.32264088747145, - -66.32264088747145, - -66.32264088747145, - -66.32264088747145 + -68.17436080640248, + -68.17436080640248, + -68.17436080640248, + -68.17436080640248 ], "moduleForcesY": [ - 22.005172221305003, - 22.005172221305003, - 22.005172221304996, - 22.005172221304996 + 15.36339993436348, + 15.363399934363485, + 15.363399934363473, + 15.363399934363477 ], - "timestamp": 2.641591680385878 + "timestamp": 2.5333306831394573 }, { - "x": 8.094438850207247, - "y": 0.6632084825286524, - "heading": -0.03060270556570066, - "angularVelocity": -2.1476865288778444e-22, - "velocityX": 2.4593612842152437, - "velocityY": -0.13751015960869134, + "x": 8.118709429041983, + "y": 0.9994031964377751, + "heading": -0.030602705565700945, + "angularVelocity": 7.671706888831663e-19, + "velocityX": 2.5210507217984657, + "velocityY": -0.12299007774670825, "moduleForcesX": [ - -67.29555668636523, - -67.29555668636523, - -67.29555668636523, - -67.29555668636523 + -68.70314364670283, + -68.70314364670283, + -68.70314364670283, + -68.70314364670283 ], "moduleForcesY": [ - 18.90069114406037, - 18.90069114406037, - 18.900691144060374, - 18.900691144060374 + 12.90231694942824, + 12.902316949428238, + 12.902316949428242, + 12.902316949428242 ], - "timestamp": 2.707204319916338 + "timestamp": 2.6004568236664385 }, { - "x": 8.240020092552498, - "y": 0.6580507593309295, - "heading": -0.03060270556570066, - "angularVelocity": -2.146966814878084e-22, - "velocityX": 2.2187987465078, - "velocityY": -0.07860868324507067, + "x": 8.271143977771962, + "y": 0.9938469245659062, + "heading": -0.030602705565700928, + "angularVelocity": 7.671359538796945e-19, + "velocityX": 2.270867169380966, + "velocityY": -0.08277359353968143, "moduleForcesX": [ - -67.9091402549072, - -67.9091402549072, - -67.9091402549072, - -67.9091402549072 + -69.0326940172405, + -69.0326940172405, + -69.0326940172405, + -69.0326940172405 ], "moduleForcesY": [ - 16.627479314598546, - 16.627479314598546, - 16.627479314598553, - 16.62747931459855 + 11.096861571799984, + 11.096861571799986, + 11.096861571799984, + 11.096861571799984 ], - "timestamp": 2.7728169594467977 + "timestamp": 2.6675829641934197 }, { - "x": 8.369721422210265, - "y": 0.6563552816375592, - "heading": -0.03060270556570066, - "angularVelocity": -2.146156534170985e-22, - "velocityX": 1.9767735391525214, - "velocityY": -0.02584071766512831, + "x": 8.406731406019375, + "y": 0.9906547367603562, + "heading": -0.030602705565700914, + "angularVelocity": 7.670985863150012e-19, + "velocityX": 2.019890123027612, + "velocityY": -0.047555062461336506, "moduleForcesX": [ - -68.32204177817843, - -68.32204177817843, - -68.32204177817843, - -68.32204177817843 + -69.25164136036881, + -69.25164136036881, + -69.25164136036881, + -69.25164136036881 ], "moduleForcesY": [ - 14.896031650164923, - 14.896031650164922, - 14.896031650164927, - 14.896031650164925 + 9.717785426549383, + 9.717785426549383, + 9.717785426549389, + 9.717785426549389 ], - "timestamp": 2.8384295989772577 + "timestamp": 2.734709104720401 }, { - "x": 8.483474950917365, - "y": 0.6578057698994572, - "heading": -0.03060270556570066, - "angularVelocity": -2.1453703946144053e-22, - "velocityX": 1.7337136490948615, - "velocityY": 0.022106842100517037, + "x": 8.5254345460703, + "y": 0.9895621634069518, + "heading": -0.0306027055657009, + "angularVelocity": 7.670595843658019e-19, + "velocityX": 1.7683593771223813, + "velocityY": -0.016276421448131698, "moduleForcesX": [ - -68.61412554743417, - -68.61412554743417, - -68.61412554743417, - -68.61412554743417 + -69.40442267327488, + -69.40442267327488, + -69.40442267327488, + -69.40442267327488 ], "moduleForcesY": [ - 13.535264434918018, - 13.535264434918016, - 13.53526443491802, - 13.53526443491802 + 8.630658704124505, + 8.630658704124507, + 8.630658704124507, + 8.630658704124508 ], - "timestamp": 2.9040422385077176 + "timestamp": 2.801835245247382 }, { - "x": 8.581230723210314, - "y": 0.6621472949390711, - "heading": -0.030602705565700657, - "angularVelocity": -2.1446182627185602e-22, - "velocityX": 1.4898923895230367, - "velocityY": 0.06616903497074665, + "x": 8.627226437405287, + "y": 0.9903554058889321, + "heading": -0.030602705565700883, + "angularVelocity": 7.670197234878886e-19, + "velocityX": 1.516426991569238, + "velocityY": 0.011817191868226978, "moduleForcesX": [ - -68.82905489435568, - -68.82905489435568, - -68.82905489435568, - -68.82905489435568 + -69.51524637407707, + -69.51524637407705, + -69.51524637407707, + -69.51524637407705 ], "moduleForcesY": [ - 12.438452238151038, - 12.438452238151038, - 12.438452238151038, - 12.438452238151038 + 7.751819786448097, + 7.751819786448097, + 7.751819786448097, + 7.751819786448097 ], - "timestamp": 2.9696548780381775 + "timestamp": 2.868961385774363 }, { - "x": 8.66295079020209, - "y": 0.6691700762270508, - "heading": -0.030602705565700657, - "angularVelocity": -2.1439007784276e-22, - "velocityX": 1.2454927522590922, - "velocityY": 0.10703396995207695, + "x": 8.712086900940639, + "y": 0.9928580419153559, + "heading": -0.03060270556570087, + "angularVelocity": 7.6697962024819e-19, + "velocityX": 1.2641939916274305, + "velocityY": 0.03728258479896153, "moduleForcesX": [ - -68.99232691579692, - -68.99232691579692, - -68.99232691579692, - -68.99232691579692 + -69.59819435728923, + -69.59819435728923, + -69.59819435728923, + -69.59819435728923 ], "moduleForcesY": [ - 11.535888453791287, - 11.535888453791287, - 11.535888453791285, - 11.535888453791285 + 7.026619700405902, + 7.026619700405903, + 7.0266197004059014, + 7.0266197004059014 ], - "timestamp": 3.0352675175686374 + "timestamp": 2.9360875263013444 }, { - "x": 8.728605558744404, - "y": 0.678698482418167, - "heading": -0.030602705565700657, - "angularVelocity": -2.1432640541955167e-22, - "velocityX": 1.0006420868319634, - "velocityY": 0.14522211359432977, + "x": 8.780000437865695, + "y": 0.996921987964414, + "heading": -0.03060270556570085, + "angularVelocity": 7.669398274852111e-19, + "velocityX": 1.011730100850266, + "velocityY": 0.06054192922688254, "moduleForcesX": [ - -69.11964904618539, - -69.11964904618539, - -69.11964904618539, - -69.11964904618539 + -69.66190364637255, + -69.66190364637255, + -69.66190364637255, + -69.66190364637255 ], "moduleForcesY": [ - 10.780248776015798, - 10.780248776015798, - 10.780248776015798, - 10.780248776015798 + 6.417908736725351, + 6.417908736725351, + 6.417908736725356, + 6.417908736725355 ], - "timestamp": 3.1008801570990974 + "timestamp": 3.0032136668283256 }, { - "x": 8.778171440675647, - "y": 0.6905833152058894, - "heading": -0.030602705565700657, - "angularVelocity": -2.1427532944253698e-22, - "velocityX": 0.7554319150387594, - "velocityY": 0.18113633093826434, + "x": 8.830954883407468, + "y": 1.0024211476734168, + "heading": -0.030602705565700838, + "angularVelocity": 7.669008842488089e-19, + "velocityX": 0.75908498748391, + "velocityY": 0.08192277502968102, "moduleForcesX": [ - -69.22113520637045, - -69.22113520637045, - -69.22113520637045, - -69.22113520637045 + -69.71190806731377, + -69.71190806731377, + -69.71190806731377, + -69.71190806731377 ], "moduleForcesY": [ - 10.138335112343585, - 10.138335112343585, - 10.138335112343585, - 10.138335112343585 + 5.89957801675687, + 5.899578016756869, + 5.899578016756865, + 5.899578016756865 ], - "timestamp": 3.1664927966295573 + "timestamp": 3.0703398073553068 }, { - "x": 8.811629282013929, - "y": 0.7046962431476756, - "heading": -0.030602705565700657, - "angularVelocity": -2.14229895305694e-22, - "velocityX": 0.5099298180611922, - "velocityY": 0.2150946531458243, + "x": 8.86494051215884, + "y": 1.0092468204121565, + "heading": -0.030602705565700824, + "angularVelocity": 7.668631929139065e-19, + "velocityX": 0.5062949915570888, + "velocityY": 0.10168427210552212, "moduleForcesX": [ - -69.303543666465, - -69.303543666465, - -69.303543666465, - -69.303543666465 + -69.7518852496981, + -69.7518852496981, + -69.7518852496981, + -69.7518852496981 ], "moduleForcesY": [ - 9.586199445644239, - 9.586199445644237, - 9.586199445644244, - 9.586199445644244 + 5.452754058783733, + 5.452754058783732, + 5.452754058783737, + 5.452754058783736 ], - "timestamp": 3.232105436160017 + "timestamp": 3.137465947882288 }, { - "x": 8.828963279724121, - "y": 0.7209256887435913, - "heading": -0.030602705565700657, - "angularVelocity": 3.635197207581808e-20, - "velocityX": 0.26418686756452614, - "velocityY": 0.24735242648455466, + "x": 8.881949424743652, + "y": 1.017304301261902, + "heading": -0.030602705565700813, + "angularVelocity": -6.192148052914439e-17, + "velocityX": 0.25338731604818493, + "velocityY": 0.12003491913118446, "moduleForcesX": [ - -69.3715349487538, - -69.3715349487538, - -69.3715349487538, - -69.3715349487538 + -69.78435636275017, + -69.78435636275017, + -69.78435636275017, + -69.78435636275017 ], "moduleForcesY": [ - 9.10614626386413, - 9.10614626386413, - 9.10614626386413, - 9.10614626386413 + 5.063460762339646, + 5.063460762339647, + 5.063460762339649, + 5.06346076233965 ], - "timestamp": 3.297718075690477 + "timestamp": 3.204592088409269 }, { - "x": 8.828203904774893, - "y": 0.741593975643098, - "heading": -0.03505435831192594, - "angularVelocity": -0.06067171185490822, - "velocityX": -0.010349544480640927, - "velocityY": 0.28168871625594366, + "x": 8.880530486218973, + "y": 1.027348591072611, + "heading": -0.034530809902176336, + "angularVelocity": -0.054161349122254336, + "velocityX": -0.019564557922623505, + "velocityY": 0.13849232110933965, "moduleForcesX": [ - -69.82238913204013, - -69.84216616588223, - -68.65753096355249, - -68.89031773540574 + -69.96707712828831, + -69.96918542605992, + -69.4046006596549, + -69.48963562370783 ], "moduleForcesY": [ - 4.5647052079056, - 4.326632215396829, - 13.498725767769074, - 12.28091309563698 + 0.7821062724142991, + 0.9492571184496107, + 8.889772333230777, + 8.233787400699313 ], - "timestamp": 3.3710908652513036 + "timestamp": 3.277118057576761 }, { - "x": 8.807299851592818, - "y": 0.7647796846719664, - "heading": -0.04385437316260252, - "angularVelocity": -0.11993567238412434, - "velocityX": -0.2849019821543826, - "velocityY": 0.31599873969146675, + "x": 8.859315047264229, + "y": 1.03873366112627, + "heading": -0.04230269871701783, + "angularVelocity": -0.10716008216259976, + "velocityX": -0.29252196417731824, + "velocityY": 0.15697921977944768, "moduleForcesX": [ - -69.81588658834606, - -69.83182481459568, - -68.68488260159317, - -68.89599182771352 + -69.96360527609723, + -69.96507878923691, + -69.41623345810719, + -69.49123274928873 ], "moduleForcesY": [ - 4.61919336637954, - 4.448281304435051, - 13.343438108461418, - 12.233541145410339 + 0.849054357453386, + 1.0605014457335615, + 8.776783714333206, + 8.198715576096298 ], - "timestamp": 3.44446365481213 + "timestamp": 3.3496440267442527 }, { - "x": 8.766249783637008, - "y": 0.7904802826298837, - "heading": -0.05688378166614004, - "angularVelocity": -0.17757820823666606, - "velocityX": -0.559472635584879, - "velocityY": 0.3502742380622083, + "x": 8.81830267354523, + "y": 1.0514616761704456, + "heading": -0.053822469418188024, + "angularVelocity": -0.15883649447922438, + "velocityX": -0.5654853591032976, + "velocityY": 0.17549596634529624, "moduleForcesX": [ - -69.80907759591098, - -69.8190745845742, - -68.71780064776569, - -68.90102631540906 + -69.95969350130241, + -69.9600439550638, + -69.43034005880952, + -69.49219040870635 ], "moduleForcesY": [ - 4.671002982456192, - 4.596843952033549, - 13.154721929440731, - 12.187023469399424 + 0.916057569851737, + 1.1945950847132014, + 8.639230967068034, + 8.165662210562154 ], - "timestamp": 3.5178364443729566 + "timestamp": 3.4221699959117444 }, { - "x": 8.705052173115664, - "y": 0.8186924642311597, - "heading": -0.07400373694683447, - "angularVelocity": -0.2333283957604187, - "velocityX": -0.8340641113366615, - "velocityY": 0.3845046885928745, + "x": 8.75749289682575, + "y": 1.0655348258499984, + "heading": -0.06897986858552595, + "angularVelocity": -0.2089927144919403, + "velocityX": -0.838455210147377, + "velocityY": 0.1940429040948802, "moduleForcesX": [ - -69.80139202754168, - -69.80353156229272, - -68.75655115056365, - -68.90652968620067 + -69.9551810729799, + -69.95383968280234, + -69.44688305083558, + -69.49295928173186 ], "moduleForcesY": [ - 4.726967109732889, - 4.773715711987706, - 12.929025319368964, - 12.134397256845864 + 0.9882804241447996, + 1.352618156589803, + 8.475469948022155, + 8.130018720639756 ], - "timestamp": 3.591209233933783 + "timestamp": 3.494695965079236 }, { - "x": 8.623705265045926, - "y": 0.8494119202761454, - "heading": -0.09504978733566269, - "angularVelocity": -0.28683726644168145, - "velocityX": -1.108679505803711, - "velocityY": 0.41867640890931357, + "x": 8.67688521696236, + "y": 1.080955321413936, + "heading": -0.08764661363140669, + "angularVelocity": -0.2573801530723142, + "velocityX": -1.1114319572514513, + "velocityY": 0.2126203309097312, "moduleForcesX": [ - -69.79205213254602, - -69.78467756927584, - -68.80147068719397, - -68.91395589602016 + -69.94983324196707, + -69.94615057660823, + -69.46581320027535, + -69.49411065050936 ], "moduleForcesY": [ - 4.795991300911855, - 4.980866247525471, - 12.661611125036274, - 12.06633404852973 + 1.0723396108261634, + 1.5360717401888286, + 8.283336243627176, + 8.085785369496797 ], - "timestamp": 3.6645820234946096 + "timestamp": 3.567221934246728 }, { - "x": 8.522207036579841, - "y": 0.8826330056369158, - "heading": -0.11982385908892228, - "angularVelocity": -0.3376465839931265, - "velocityX": -1.3833224697275863, - "velocityY": 0.45277119160407436, + "x": 8.576479109537585, + "y": 1.0977253883199334, + "heading": -0.1096713977003478, + "angularVelocity": -0.3036813478255371, + "velocityX": -1.3844159351099252, + "velocityY": 0.23122844270129664, "moduleForcesX": [ - -69.77997172662764, - -69.76180279178641, - -68.85297448408464, - -68.92524546744818 + -69.94330252963142, + -69.9365583049015, + -69.4870568794036, + -69.49637643933679 ], "moduleForcesY": [ - 4.889881316073384, - 5.2210669442672835, - 12.346127343474148, - 11.970039561538547 + 1.176823061341541, + 1.7470252667944042, + 8.060012105446882, + 8.025018376878974 ], - "timestamp": 3.737954813055436 + "timestamp": 3.6397479034142197 }, { - "x": 8.400555154994208, - "y": 0.9183482483752978, - "heading": -0.14808264155198017, - "angularVelocity": -0.3851398131678119, - "velocityX": -1.6579972264075493, - "velocityY": 0.48676413902423377, + "x": 8.456274044587177, + "y": 1.1158472524014584, + "heading": -0.13487291738284943, + "angularVelocity": -0.347482701327652, + "velocityX": -1.6574072202027315, + "velocityY": 0.24986724465107038, "moduleForcesX": [ - -69.76356912538407, - -69.73390347020135, - -68.91158189541561, - -68.94304262893307 + -69.93505805724766, + -69.92449408332656, + -69.51050132115871, + -69.50070530398553 ], "moduleForcesY": [ - 5.024739981390052, - 5.498335546770225, - 11.973827493944482, - 11.82738424849502 + 1.3131272162432313, + 1.98838442321693, + 7.801780109725148, + 7.936938199533164 ], - "timestamp": 3.8113276026162626 + "timestamp": 3.7122738725817115 }, { - "x": 8.258746945464983, - "y": 0.9565475945096078, - "heading": -0.1795200221320126, - "angularVelocity": -0.42846102442337525, - "velocityX": -1.932708438346409, - "velocityY": 0.5206200604195711, + "x": 8.316269527386241, + "y": 1.1353231154725127, + "heading": -0.16302980404777723, + "angularVelocity": -0.38823178770838385, + "velocityX": -1.930405326643809, + "velocityY": 0.26853640557469155, "moduleForcesX": [ - -69.74040444259873, - -69.6994878605935, - -68.97797470393978, - -68.97104071246528 + -69.92425253763372, + -69.90915580434691, + -69.53597671993317, + -69.50834196529713 ], "moduleForcesY": [ - 5.223448587988533, - 5.818826004851896, - 11.532106083145074, - 11.611544934432994 + 1.4968639656801608, + 2.264388307053663, + 7.503564176470051, + 7.80642632120717 ], - "timestamp": 3.884700392177089 + "timestamp": 3.7847998417492033 }, { - "x": 8.096779401992974, - "y": 0.9972171857084491, - "heading": -0.21373905317621672, - "angularVelocity": -0.46637222394054323, - "velocityX": -2.2074606191405195, - "velocityY": 0.5542871061911322, + "x": 8.156465184614108, + "y": 1.1561551126608645, + "heading": -0.1938654641731384, + "angularVelocity": -0.42516715708305197, + "velocityX": -2.2034085804916796, + "velocityY": 0.28723500599140817, "moduleForcesX": [ - -69.70643217846147, - -69.65617966490848, - -69.0531155698116, - -69.01454849786487 + -69.9094604865732, + -69.88935388472542, + -69.5632321514312, + -69.52093877070061 ], "moduleForcesY": [ - 5.520358534554248, - 6.192692191683197, - 11.001614243637365, - 11.280543987218508 + 1.7503444073984757, + 2.581564547481693, + 7.158055486956702, + 7.611351867199961 ], - "timestamp": 3.9580731817379156 + "timestamp": 3.857325810916695 }, { - "x": 7.914649343266423, - "y": 1.0403372462568932, - "heading": -0.2502040431261532, - "angularVelocity": -0.49698246677273733, - "velocityX": -2.482256158130169, - "velocityY": 0.5876846281372092, + "x": 7.976860949117206, + "y": 1.1783452360639273, + "heading": -0.2270240484884978, + "angularVelocity": -0.45719601814734345, + "velocityX": -2.4764127602640795, + "velocityY": 0.3059610737751436, "moduleForcesX": [ - -69.6543272195074, - -69.59982811388493, - -69.13847224033395, - -69.08142915869702 + -69.88812974316788, + -69.86320182607882, + -69.59189618603347, + -69.5407034064261 ], "moduleForcesY": [ - 5.970871351408315, - 6.638279683857717, - 10.350201343001592, - 10.763705620631843 + 2.107233171390243, + 2.950662034921671, + 6.753983199950573, + 7.317496840416227 ], - "timestamp": 4.031445971298742 + "timestamp": 3.929851780084187 }, { - "x": 7.712354049624925, - "y": 1.0858780921373514, - "heading": -0.28815099090474133, - "angularVelocity": -0.517180115485863, - "velocityX": -2.7570887634549326, - "velocityY": 0.6206775856968689, + "x": 7.777457476125625, + "y": 1.201895191338467, + "heading": -0.26202980984331203, + "angularVelocity": -0.4826651991962036, + "velocityX": -2.74940790561615, + "velocityY": 0.3247106594351006, "moduleForcesX": [ - -69.56928399894086, - -69.52224882009111, - -69.23640181351112, - -69.18354973211308 + -69.85533117072832, + -69.82743416827404, + -69.62139360036666, + -69.57054322743649 ], "moduleForcesY": [ - 6.673022110744875, - 7.1925990350588, - 9.51901410508811, - 9.929914946454016 + 2.6219385333270773, + 3.3908613060937713, + 6.272491751359185, + 6.868108041742767 ], - "timestamp": 4.104818760859568 + "timestamp": 4.0023777492516786 }, { - "x": 7.489893664384698, - "y": 1.133791594453917, - "heading": -0.3263979812876508, - "angularVelocity": -0.5212694053454582, - "velocityX": -3.03191941551856, - "velocityY": 0.6530145930576222, + "x": 7.558257164116274, + "y": 1.2268061069502, + "heading": -0.2982123205048736, + "angularVelocity": -0.49889041232006653, + "velocityX": -3.022369980373853, + "velocityY": 0.3434758045660868, "moduleForcesX": [ - -69.41666539245078, - -69.40439565809636, - -69.35057994409232, - -69.33787106983414 + -69.80056727463466, + -69.77570886950674, + -69.6507199544232, + -69.61392322278483 ], "moduleForcesY": [ - 7.822962646502443, - 7.941889554066643, - 8.38638483270236, - 8.500969214195738 + 3.39043477997722, + 3.9399463888437256, + 5.678903958822243, + 6.160009083534089 ], - "timestamp": 4.178191550420395 + "timestamp": 4.07490371841917 }, { - "x": 7.247281611648338, - "y": 1.183989156390833, - "heading": -0.3628608681557275, - "angularVelocity": -0.4969538038055478, - "velocityX": -3.3065671100760374, - "velocityY": 0.6841441116982726, + "x": 7.319267013948266, + "y": 1.2530778743434587, + "heading": -0.3345519884568234, + "angularVelocity": -0.5010573229066131, + "velocityX": -3.2952355261337996, + "velocityY": 0.3622394529136627, "moduleForcesX": [ - -69.09618755623467, - -69.18927818386388, - -69.48444757611301, - -69.55485789829521 + -69.69826365815047, + -69.69360285262502, + -69.67768220142874, + -69.67276264637083 ], "moduleForcesY": [ - 9.887138047237972, - 9.135257071526894, - 6.655220631175712, - 5.755332008623882 + 4.602649202702627, + 4.682797548651256, + 4.9016741603909, + 4.9806442793840855 ], - "timestamp": 4.251564339981221 + "timestamp": 4.147429687586662 }, { - "x": 6.9846108615466544, - "y": 1.2362643691091493, - "heading": -0.3928314758501629, - "angularVelocity": -0.40847033176501496, - "velocityX": -3.5799477118684186, - "velocityY": 0.7124604779402591, + "x": 7.060508520408783, + "y": 1.2807073809011449, + "heading": -0.3693010360245331, + "angularVelocity": -0.479125587246758, + "velocityX": -3.5678046982302343, + "velocityY": 0.38096018398438164, "moduleForcesX": [ - -68.19397798012086, - -68.604096270111, - -69.61287896515513, - -69.63437448602505 + -69.47236753682394, + -69.53951749408205, + -69.69564436263259, + -69.73202523829484 ], "moduleForcesY": [ - 14.31877505066931, - 11.837279257782388, - 3.3817824033703014, - -0.9454620249074384 + 6.700012628800592, + 5.851281299757406, + 3.768542257976925, + 2.8040874089418844 ], - "timestamp": 4.324937129542048 + "timestamp": 4.219955656754154 }, { - "x": 6.703415297553567, - "y": 1.290040410483288, - "heading": -0.3945237678570967, - "angularVelocity": -0.02306429968198066, - "velocityX": -3.8324229687352247, - "velocityY": 0.7329153177358495, + "x": 6.7820675423003, + "y": 1.3096827333084073, + "heading": -0.398737971474847, + "angularVelocity": -0.40588131104575736, + "velocityX": -3.839190035026568, + "velocityY": 0.39951692807238764, "moduleForcesX": [ - -63.16947604320132, - -61.34983762111749, - -69.05769594451374, - -61.35920225606092 + -68.80858735271256, + -69.12938539141281, + -69.67366590353845, + -69.61858416268272 ], "moduleForcesY": [ - 28.4635156424948, - 30.27498948987804, - -6.582220178863257, - -31.50206546085697 + 10.998366071640108, + 8.339611523596144, + 1.7520720276263266, + -2.1336448340952816 ], - "timestamp": 4.398309919102874 + "timestamp": 4.292481625921646 }, { - "x": 6.41959619572841, - "y": 1.3315714425952379, - "heading": -0.3945728781781997, - "angularVelocity": -0.0006693260730159474, - "velocityX": -3.868179246338573, - "velocityY": 0.566027710824874, + "x": 6.484732150451617, + "y": 1.3400281940076402, + "heading": -0.4090628910860552, + "angularVelocity": -0.142361690988409, + "velocityX": -4.099709321525832, + "velocityY": 0.4184082067095552, "moduleForcesX": [ - -9.773676203527769, - -7.669404194901154, - -10.354384240238268, - -8.307340543871192 + -65.20316881632876, + -65.7489206217446, + -69.31239778456624, + -65.86566076035609 ], "moduleForcesY": [ - -41.36655808090332, - -42.00286570902313, - -42.261025709276105, - -42.88386291727224 + 23.606521909357, + 20.444429351523326, + -3.7507078277888724, + -21.00209915007702 ], - "timestamp": 4.4716827086637005 + "timestamp": 4.365007595089137 }, { - "x": 6.135687490084942, - "y": 1.372487307589655, - "heading": -0.3946215087945329, - "angularVelocity": -0.0006627881619921617, - "velocityX": -3.8694004595273754, - "velocityY": 0.5576435793067078, + "x": 6.1842404767603805, + "y": 1.3742213267083638, + "heading": -0.40911116386317103, + "angularVelocity": -0.0006655929961390624, + "velocityX": -4.1432286550667365, + "velocityY": 0.4714605415577361, "moduleForcesX": [ - -0.30840180759639224, - -0.3079749898198458, - -0.30858337724113316, - -0.30815655961690325 + -13.325449562031311, + -4.164768682569861, + -18.043544953356022, + -8.922855853125498 ], "moduleForcesY": [ - -2.116157086062331, - -2.116334618026183, - -2.1165925122839986, - -2.116770044133725 + 19.90592667522208, + 16.82639877386449, + 10.611237303210721, + 6.85137196260812 ], - "timestamp": 4.545055498224527 + "timestamp": 4.437533564256629 }, { - "x": 5.85352801680754, - "y": 1.4241018905198637, - "heading": -0.39467053194899787, - "angularVelocity": -0.0006681380762323543, - "velocityX": -3.845560117943876, - "velocityY": 0.7034567342900305, + "x": 5.886052139153465, + "y": 1.4246963553310026, + "heading": -0.4091589633873657, + "angularVelocity": -0.0006590677069691712, + "velocityX": -4.111469878027867, + "velocityY": 0.6959580023818284, "moduleForcesX": [ - 6.0183308507069375, - 6.017872402285157, - 6.018488007158529, - 6.018029561236691 + 8.110433382725144, + 8.111187384355791, + 8.110199033033277, + 8.110953042580169 ], "moduleForcesY": [ - 36.80841670957, - 36.80855524647377, - 36.808665373492616, - 36.808803909707684 + 57.33325913716358, + 57.33311245310866, + 57.33314368701091, + 57.33299700322065 ], - "timestamp": 4.6184282877853535 + "timestamp": 4.510059533424121 }, { - "x": 5.5751319379159305, - "y": 1.493186559151982, - "heading": -0.39472121211754135, - "angularVelocity": -0.0006907215719449539, - "velocityX": -3.7942687003990416, - "velocityY": 0.941557068303184, + "x": 5.591555275132608, + "y": 1.4934998764417577, + "heading": -0.4092089626864391, + "angularVelocity": -0.0006893985650598928, + "velocityX": -4.060571232640143, + "velocityY": 0.94867427351207, "moduleForcesX": [ - 12.948916142731543, - 12.946169957519793, - 12.949515473189427, - 12.946769400230588 + 13.000242789451551, + 12.99626661282106, + 13.001163654267753, + 12.99718771199267 ], "moduleForcesY": [ - 60.104836688704204, - 60.10549713360235, - 60.105088950128234, - 60.105749386647936 + 64.53930421343945, + 64.54015945588058, + 64.53937921708196, + 64.54023445819007 ], - "timestamp": 4.69180107734618 + "timestamp": 4.582585502591613 }, { - "x": 5.302053451538086, - "y": 1.5809556245803833, - "heading": -0.39477498805993555, - "angularVelocity": -0.0007329139687353689, - "velocityX": -3.721795068885318, - "velocityY": 1.196207285476589, + "x": 5.302053451538088, + "y": 1.5809556245803824, + "heading": -0.4092627987960467, + "angularVelocity": -0.000742301140211202, + "velocityX": -3.991698793103366, + "velocityY": 1.2058542497602232, "moduleForcesX": [ - 18.297412052733495, - 18.291870609159098, - 18.298156444631005, - 18.292615475816596 + 17.591944196413664, + 17.584790178826015, + 17.593089676218266, + 17.5859364455025 ], "moduleForcesY": [ - 64.28225176659839, - 64.2838616989904, - 64.2823244352569, - 64.2839342864753 + 65.6787937062123, + 65.68074285605304, + 65.67871632504414, + 65.68066538032537 ], - "timestamp": 4.7651738669070065 + "timestamp": 4.6551114717591044 }, { - "x": 5.07390423214392, - "y": 1.6694230318569008, - "heading": -0.3948239257142182, - "angularVelocity": -0.0007817718663467028, - "velocityX": -3.644650395807531, - "velocityY": 1.4132538862180757, + "x": 5.060781264234282, + "y": 1.668503203961895, + "heading": -0.4093136867686553, + "angularVelocity": -0.0008266775577253413, + "velocityX": -3.9194782641972647, + "velocityY": 1.4222146294751892, "moduleForcesX": [ - 22.829580522567618, - 22.8220128584525, - 22.830066989794148, - 22.82250022697142 + 21.736557880357715, + 21.723118914680356, + 21.73784859212575, + 21.72441246515569 ], "moduleForcesY": [ - 64.21970113385147, - 64.22240309693508, - 64.21976226256318, - 64.22246395577116 + 65.09863070377499, + 65.10314246149298, + 65.09850389768496, + 65.10301496712766 ], - "timestamp": 4.827772249362731 + "timestamp": 4.716668692144205 }, { - "x": 4.851229056834639, - "y": 1.7708905614247654, - "heading": -0.3948711845379385, - "angularVelocity": -0.0007549527937680354, - "velocityX": -3.5572033425428917, - "velocityY": 1.6209289375749065, + "x": 4.824591541574993, + "y": 1.768967581170856, + "heading": -0.4093625786347868, + "angularVelocity": -0.0007942507121868569, + "velocityX": -3.836913382080745, + "velocityY": 1.632048630208832, "moduleForcesX": [ - 25.872414895839015, - 25.87639732222832, - 25.872346027139017, - 25.876328704106957 + 24.840611640815233, + 24.84563714723139, + 24.840370639682614, + 24.84539654710804 ], "moduleForcesY": [ - 61.44911151867199, - 61.44743545397816, - 61.44890652890951, - 61.447230357726816 + 63.138133963812095, + 63.136150247952436, + 63.13804254630454, + 63.13605868744737 ], - "timestamp": 4.890370631818455 + "timestamp": 4.778225912529306 }, { - "x": 4.634382563500368, - "y": 1.8842851362906166, - "heading": -0.3949168954426069, - "angularVelocity": -0.000730225013412799, - "velocityX": -3.4640910008760675, - "velocityY": 1.8114617409811675, + "x": 4.5940300417851905, + "y": 1.8817568503359232, + "heading": -0.40940943255755435, + "angularVelocity": -0.00076114422441599, + "velocityX": -3.7454826314023215, + "velocityY": 1.8322670916501005, "moduleForcesX": [ - 27.548950472331388, - 27.55233169388033, - 27.548972415747215, - 27.552353816038643 + 27.508225922613867, + 27.513128573012004, + 27.508193765462636, + 27.51309679763414 ], "moduleForcesY": [ - 56.377068082107755, - 56.375442909315176, - 56.3765916436959, - 56.3749663731007 + 60.24514716829222, + 60.242914588066654, + 60.24482580432994, + 60.242593041950194 ], - "timestamp": 4.95296901427418 + "timestamp": 4.839783132914407 }, { - "x": 4.422622805341516, - "y": 2.0069183796517893, - "heading": -0.39496118737413566, - "angularVelocity": -0.0007075571251397378, - "velocityX": -3.382831150767024, - "velocityY": 1.9590481183425312, + "x": 4.369349606253567, + "y": 2.0058517685581454, + "heading": -0.4094544926877705, + "angularVelocity": -0.0007320039783130824, + "velocityX": -3.6499444602278026, + "velocityY": 2.015927903272623, "moduleForcesX": [ - 24.042485271707772, - 24.045024480603296, - 24.042303530281014, - 24.044842834314085 + 28.744538248232754, + 28.74851367433131, + 28.744577870233446, + 28.74855354416703 ], "moduleForcesY": [ - 43.66989864479118, - 43.66861606069169, - 43.66891333264767, - 43.66763069286199 + 55.26315032046569, + 55.261123506293146, + 55.262531653571756, + 55.260504693222536 ], - "timestamp": 5.015567396729904 + "timestamp": 4.901340353299508 }, { - "x": 4.213243294089462, - "y": 2.1335730846638814, - "heading": -0.3950047652007233, - "angularVelocity": -0.0006961494031973518, - "velocityX": -3.344807054721382, - "velocityY": 2.023290379774191, + "x": 4.149973859162333, + "y": 2.13910073076137, + "heading": -0.4094996155694483, + "angularVelocity": -0.0007330233788204265, + "velocityX": -3.563769541880253, + "velocityY": 2.1646357871525597, "moduleForcesX": [ - 11.25048576449635, - 11.251433113415713, - 11.250173496493936, - 11.251120853755697 + 25.92928590684574, + 25.929168249602895, + 25.929292292013468, + 25.929174634983514 ], "moduleForcesY": [ - 19.00901574015739, - 19.008592674544687, - 19.00819786419031, - 19.007774795539515 + 44.74476438288918, + 44.74482729836071, + 44.74480699802511, + 44.744869913360205 ], - "timestamp": 5.078165779185628 + "timestamp": 4.962897573684609 }, { - "x": 4.004677598787759, - "y": 2.2615635179547406, - "heading": -0.39504821197194406, - "angularVelocity": -0.0006940558128879706, - "velocityX": -3.3318064639964438, - "velocityY": 2.0446284435765874, + "x": 3.9353204017277856, + "y": 2.278088242273816, + "heading": -0.41252140844778834, + "angularVelocity": -0.04908917036026353, + "velocityX": -3.4870557197299563, + "velocityY": 2.2578587961400522, "moduleForcesX": [ - 3.8466474195591016, - 3.8468089460712034, - 3.846580474426278, - 3.8467420010287405 + 24.953488052198967, + 20.4567569278476, + 25.589392523319265, + 21.330260691348453 ], "moduleForcesY": [ - 6.31375253757312, - 6.313684558372743, - 6.313590446713757, - 6.313522467489779 + 25.14686564166064, + 27.7645218324562, + 28.406596524112476, + 30.881756387142097 ], - "timestamp": 5.140764161641353 + "timestamp": 5.02445479406971 }, { - "x": 3.796955991966042, - "y": 2.3909094414143865, - "heading": -0.39510465976032694, - "angularVelocity": -0.0009017451596737262, - "velocityX": -3.318322274040202, - "velocityY": 2.066282200680384, + "x": 3.7318642433739195, + "y": 2.4104934513603675, + "heading": -0.43615839467618756, + "angularVelocity": -0.38398397589611083, + "velocityX": -3.305155058676315, + "velocityY": 2.150928977270674, "moduleForcesX": [ - 3.9944820139638706, - 3.9784542931865783, - 4.001115299988056, - 3.9850884886982674 + 49.89592032127635, + 34.025418039882794, + 66.93891398556973, + 68.06860648029641 ], "moduleForcesY": [ - 6.39563239881223, - 6.402385340358186, - 6.411708329457698, - 6.418461041024309 + -47.83606477558671, + -59.36994474295037, + -17.100092585775965, + -4.390644167479697 ], - "timestamp": 5.203362544097077 + "timestamp": 5.086012014454811 }, { - "x": 3.597726364612312, - "y": 2.517446428244699, - "heading": -0.42282084717405416, - "angularVelocity": -0.4427620383534821, - "velocityX": -3.1826641446309787, - "velocityY": 2.021409849045414, + "x": 3.540415993553555, + "y": 2.5354156131468453, + "heading": -0.46498951807167194, + "angularVelocity": -0.4683629835017646, + "velocityX": -3.110086008150945, + "velocityY": 2.0293665146828603, "moduleForcesX": [ - 43.79907538164046, - 18.529360915334063, - 57.13510334534536, - 41.093905736742414 + 55.83155696216655, + 54.18327481859195, + 62.40116195557468, + 62.36184788712686 ], "moduleForcesY": [ - -35.85922709226587, - -37.9829446448005, - -1.354929099210182, - 22.08867174295008 + -41.50061361131458, + -43.48191734914588, + -30.734501757153353, + -30.59101829583353 ], - "timestamp": 5.265960926552801 + "timestamp": 5.147569234839912 }, { - "x": 3.4109531206363775, - "y": 2.6365558877135373, - "heading": -0.45770016209152625, - "angularVelocity": -0.5571919520786697, - "velocityX": -2.9836752428553495, - "velocityY": 1.9027561862814057, + "x": 3.361022505593955, + "y": 2.652736487945131, + "heading": -0.4956638148847116, + "angularVelocity": -0.49830542414467005, + "velocityX": -2.9142558230751625, + "velocityY": 1.9058832426858676, "moduleForcesX": [ - 55.40246671070086, - 52.925950183673045, - 63.50311672291328, - 63.68070488263328 + 57.73901804404707, + 57.438460533401326, + 60.30587505828676, + 60.21056114711604 ], "moduleForcesY": [ - -41.613108609361404, - -44.404319792002745, - -27.690981599642193, - -26.723490845822106 + -39.07130416952509, + -39.474125567373356, + -34.97681089661358, + -35.097625013264334 ], - "timestamp": 5.328559309008526 + "timestamp": 5.209126455225013 }, { - "x": 3.236759101607606, - "y": 2.7480086789134943, - "heading": -0.49382230864067395, - "angularVelocity": -0.5770460055369145, - "velocityX": -2.7827239649839024, - "velocityY": 1.7804420310506774, + "x": 3.1936838062667205, + "y": 2.7624068712077934, + "heading": -0.5266594310996341, + "angularVelocity": -0.5035252732516518, + "velocityX": -2.7184252030934304, + "velocityY": 1.7816006404539642, "moduleForcesX": [ - 58.68329565175249, - 58.536897284256675, - 60.338571121696745, - 60.27603405724038 + 58.700566811459886, + 58.67734780171254, + 59.16682840463337, + 59.14969530624501 ], "moduleForcesY": [ - -37.48822564336687, - -37.68393200712494, - -34.759584038339426, - -34.83251521985918 + -37.749793809130026, + -37.78083859729802, + -37.014524694721786, + -37.03675261323169 ], - "timestamp": 5.39115769146425 + "timestamp": 5.270683675610114 }, { - "x": 3.0751440767985594, - "y": 2.851743750610944, - "heading": -0.529353385941278, - "angularVelocity": -0.5676037607798451, - "velocityX": -2.5817763729495224, - "velocityY": 1.6571525912961371, + "x": 3.038390509303798, + "y": 2.8643948429617936, + "heading": -0.5570773161987834, + "angularVelocity": -0.4941400035435474, + "velocityX": -2.5227470634868117, + "velocityY": 1.6567994967278303, "moduleForcesX": [ - 59.85882515009252, - 59.8712013872914, - 59.03484963172741, - 59.065559593293514 + 59.30031933124251, + 59.30307854027269, + 58.44288594465124, + 58.46463470158717 ], "moduleForcesY": [ - -35.82050595797904, - -35.81050714543758, - -37.16303360424269, - -37.12450292455551 + -36.885288676049825, + -36.88814111006628, + -38.229406656940725, + -38.203170518961 ], - "timestamp": 5.453756073919974 + "timestamp": 5.332240895995215 }, { - "x": 2.9260957467057915, - "y": 2.9477247616331526, - "heading": -0.563363095015486, - "angularVelocity": -0.5433001259779049, - "velocityX": -2.381025263682967, - "velocityY": 1.5332826066247958, + "x": 2.895129799571264, + "y": 2.9586740032068897, + "heading": -0.5863104114525834, + "angularVelocity": -0.4748930356337662, + "velocityX": -2.3272771063459046, + "velocityY": 1.5315694837305227, "moduleForcesX": [ - 60.486958658404426, - 60.41900268077107, - 58.32195697914738, - 58.369971326869816 + 59.717187192796516, + 59.657546410038414, + 57.934272823511364, + 57.95135152475748 ], "moduleForcesY": [ - -34.87489354988502, - -35.01332620759317, - -38.3857086444205, - -38.33172202686071 + -36.2638573648305, + -36.37433120370207, + -39.04885966973737, + -39.03512982178747 ], - "timestamp": 5.516354456375699 + "timestamp": 5.3937981163803155 }, { - "x": 2.789594973687008, - "y": 3.0359157910118872, - "heading": -0.595278042536244, - "angularVelocity": -0.5098366166782579, - "velocityX": -2.1805798754517425, - "velocityY": 1.4088387897420815, + "x": 2.763886371067874, + "y": 3.045218511804947, + "heading": -0.6139222009668763, + "angularVelocity": -0.4485548460699908, + "velocityX": -2.132055795929944, + "velocityY": 1.4059196964488687, "moduleForcesX": [ - 60.865075509815334, - 60.664615260538106, - 57.84868096481913, - 57.85768343206302 + 60.015428697215164, + 59.85831037769707, + 57.55335306922513, + 57.534003758004665 ], "moduleForcesY": [ - -34.288218926493336, - -34.66454967020803, - -39.16244097534043, - -39.169596867030975 + -35.80957668058033, + -36.086111428235434, + -39.64516852205637, + -39.68654602916551 ], - "timestamp": 5.578952838831423 + "timestamp": 5.455355336765416 }, { - "x": 2.6656099583026958, - "y": 3.1162638597823777, - "heading": -0.6247411596443208, - "angularVelocity": -0.4706689846006165, - "velocityX": -1.9806424786136814, - "velocityY": 1.2835486416493385, + "x": 2.644643305476117, + "y": 3.1240017980714323, + "heading": -0.6395960472514493, + "angularVelocity": -0.41707286527865206, + "velocityX": -1.937109324394044, + "velocityY": 1.2798382671215223, "moduleForcesX": [ - 61.0383972954406, - 60.7049969301487, - 57.47499394607686, - 57.4164365296211 + 60.22326407527744, + 59.95837931731987, + 57.26239607352692, + 57.186270668215975 ], "moduleForcesY": [ - -34.03127548799677, - -34.6446033347473, - -39.75325119619021, - -39.857347146466026 + -35.49033319006886, + -35.95108026507241, + -40.09166456022141, + -40.21383292620102 ], - "timestamp": 5.6415512212871475 + "timestamp": 5.516912557150517 }, { - "x": 2.554110374142435, - "y": 3.1887192741774415, - "heading": -0.6515321237552656, - "angularVelocity": -0.42798173147515556, - "velocityX": -1.7811895417445422, - "velocityY": 1.1574646428973032, + "x": 2.537383808831169, + "y": 3.1949980499443154, + "heading": -0.6631036099321576, + "angularVelocity": -0.38188148414867673, + "velocityX": -1.7424356716228648, + "velocityY": 1.1533375196074727, "moduleForcesX": [ - 61.11771507187923, - 60.66368506097603, - 57.21052652343248, - 57.0695180689212 + 60.36537448302678, + 59.99512146624315, + 57.04267536961116, + 56.89878422586106 ], "moduleForcesY": [ - -33.92654139856753, - -34.75226319319532, - -40.16455016546468, - -40.382680062539976 + -35.27267783600763, + -35.913635660706774, + -40.42480723386195, + -40.64046601274349 ], - "timestamp": 5.704149603742872 + "timestamp": 5.578469777535618 }, { - "x": 2.455069827309922, - "y": 3.2532389005840123, - "heading": -0.6755130530260514, - "angularVelocity": -0.3830918360829452, - "velocityX": -1.5821582435067625, - "velocityY": 1.0306915909880887, + "x": 2.4420919536077847, + "y": 3.2581828688425367, + "heading": -0.6842805929390149, + "angularVelocity": -0.344021105475332, + "velocityX": -1.5480207622638111, + "velocityY": 1.0264404159729446, "moduleForcesX": [ - 61.14927721542877, - 60.589796141080875, - 57.02529080592471, - 56.798052441013304 + 60.4598584765929, + 59.99281023891339, + 56.876528556020105, + 56.661344392247265 ], "moduleForcesY": [ - -33.89819653017852, - -34.90689829521632, - -40.450798746634256, - -40.78566638525383 + -35.130249715774205, + -35.93621486669289, + -40.675063944105, + -40.98709766240873 ], - "timestamp": 5.766747986198596 + "timestamp": 5.640026997920719 }, { - "x": 2.368465422506688, - "y": 3.3097852773291314, - "heading": -0.696597572734595, - "angularVelocity": -0.33682211714426863, - "velocityX": -1.3834926943118622, - "velocityY": 0.9033200943349304, + "x": 2.3587528835015927, + "y": 3.3135334333216804, + "heading": -0.7030081263323279, + "angularVelocity": -0.3042296789257516, + "velocityX": -1.3538471942824237, + "velocityY": 0.8991725768784151, "moduleForcesX": [ - 61.152198837351406, - 60.50298885718338, - 56.892015366210785, - 56.582333241123905 + 60.5201219775476, + 59.967443909712394, + 56.748515348358374, + 56.463990903711384 ], "moduleForcesY": [ - -33.915173903721346, - -35.07698541197515, - -40.656437065616224, - -41.10124961605489 + -35.04253435660417, + -35.99355809852384, + -40.867195346689, + -41.27154173850131 ], - "timestamp": 5.8293463686543205 + "timestamp": 5.70158421830582 }, { - "x": 2.2942772044503497, - "y": 3.3583257141932483, - "heading": -0.7147311185238733, - "angularVelocity": -0.28968074058629395, - "velocityX": -1.1851459278330674, - "velocityY": 0.7754263762078791, + "x": 2.2873528689676483, + "y": 3.3610285743692394, + "heading": -0.7191989284072005, + "angularVelocity": -0.26302035689049935, + "velocityX": -1.1598966634176864, + "velocityY": 0.7715608461595652, "moduleForcesX": [ - 61.13821655127134, - 60.41423668737451, - 56.79179682900103, - 56.40799267466763 + 60.55636984330847, + 59.92996721273085, + 56.64674423048133, + 56.29855167271921 ], "moduleForcesY": [ - -33.958143361421655, - -35.245265434945416, - -40.81102911719036, - -41.35348048483787 + -34.99335531824572, + -36.06824538837232, + -41.01955138323022, + -41.50757259528159 ], - "timestamp": 5.891944751110045 + "timestamp": 5.763141438690921 }, { - "x": 2.2324876517353642, - "y": 3.398831509029573, - "heading": -0.7298786663246292, - "angularVelocity": -0.2419798596468502, - "velocityX": -0.9870790632439912, - "velocityY": 0.6470741454857069, + "x": 2.2278792676988664, + "y": 3.4006487477183196, + "heading": -0.7327874449132579, + "angularVelocity": -0.2207461029112197, + "velocityX": -0.966151507438403, + "velocityY": 0.6436316178868581, "moduleForcesX": [ - 61.11437176518798, - 60.32958782211226, - 56.7120114962976, - 56.26499529096948 + 60.57586928152923, + 59.88760410989669, + 56.562363081650766, + 56.15861493971373 ], "moduleForcesY": [ - -34.01552925312056, - -35.40260151463775, - -40.933861723710784, - -41.55859600904434 + -34.97098554580845, + -36.148822416267066, + -41.14545732873801, + -41.70558761712785 ], - "timestamp": 5.954543133565769 + "timestamp": 5.824698659076022 }, { - "x": 2.1830812470403056, - "y": 3.4312772741579374, - "heading": -0.7420173390092132, - "angularVelocity": -0.19391351994070552, - "velocityX": -0.7892600855941332, - "velocityY": 0.5183163502876942, + "x": 2.180320435943854, + "y": 3.432375929705667, + "heading": -0.7437231910234625, + "angularVelocity": -0.17765172049338343, + "velocityX": -0.7725955047594213, + "velocityY": 0.5154095943394352, "moduleForcesX": [ - 61.08457488728973, - 60.252153365783734, - 56.64434890830058, - 56.14650396699435 + 60.58340632519516, + 59.84490600733586, + 56.48902920756478, + 56.03945201723916 ], "moduleForcesY": [ - -34.08103172001482, - -35.54464933149016, - -41.037463508837504, - -41.72744758205663 + -34.96766125833015, + -36.22817172523544, + -41.25431832732434, + -41.87309909683168 ], - "timestamp": 6.017141516021494 + "timestamp": 5.886255879461123 }, { - "x": 2.146044122527282, - "y": 3.455640363277112, - "heading": -0.7511321714311937, - "angularVelocity": -0.14560811421009826, - "velocityX": -0.5916626446253628, - "velocityY": 0.38919678374128486, + "x": 2.1446656199937997, + "y": 3.4561934671002783, + "heading": -0.7519665199163653, + "angularVelocity": -0.13391327355800717, + "velocityX": -0.5792141965962733, + "velocityY": 0.3869170382549771, "moduleForcesX": [ - 61.050689733359725, - 60.18330523590073, - 56.58339768822803, - 56.04798993012374 + 60.58183660174803, + 59.80457338750152, + 56.42234143923046, + 55.9377862653548 ], "moduleForcesY": [ - -34.15181538875617, - -35.66983583289213, - -41.129938480696744, - -41.867174962927784 + -34.97878115536029, + -36.30217808780585, + -41.352605880404866, + -42.01528814031888 ], - "timestamp": 6.079739898477218 + "timestamp": 5.947813099846224 }, { - "x": 2.1213637716998797, - "y": 3.4719003980089873, - "heading": -0.757213576537006, - "angularVelocity": -0.0971495567016255, - "velocityX": -0.3942649931067891, - "velocityY": 0.2597516755864367, + "x": 2.120904845915518, + "y": 3.472085915141707, + "heading": -0.7574858469261834, + "angularVelocity": -0.08966173221089468, + "velocityX": -0.3859949154564824, + "velocityY": 0.258173581295686, "moduleForcesX": [ - 61.01354989752815, - 60.123568909957186, - 56.52548269604857, - 55.96632136080857 + 60.57287518432625, + 59.76823373322807, + 56.35914684811399, + 55.851272292620784 ], "moduleForcesY": [ - -34.22674949510225, - -35.77786460913943, - -41.2167566576027, - -41.98268691331134 + -35.001611618471976, + -36.36844720693236, + -41.44491977809569, + -42.13584960752861 ], - "timestamp": 6.142338280932942 + "timestamp": 6.009370320231325 }, { - "x": 2.1090288162231445, - "y": 3.48003888130188, - "heading": -0.7602557704725093, - "angularVelocity": -0.04859860296957364, - "velocityX": -0.19704910882417112, - "velocityY": 0.13001107973754503, + "x": 2.109028816223142, + "y": 3.4800388813018817, + "heading": -0.7602557704725089, + "angularVelocity": -0.044997540970748186, + "velocityX": -0.19292667242090425, + "velocityY": 0.12919631702701284, "moduleForcesX": [ - 60.97345110924072, - 60.073045581715306, - 56.46803335710757, - 55.89926318732195 + 60.5575482023407, + 59.73687419838248, + 56.29713329942968, + 55.778188462220434 ], "moduleForcesY": [ - -34.30556428252632, - -35.869027517591256, - -41.3017196129008, - -42.07746912434651 + -35.034541448499795, + -36.42559455322768, + -41.5346063390199, + -42.237487649673916 ], - "timestamp": 6.204936663388667 + "timestamp": 6.070927540616426 }, { - "x": 2.1090288162231445, - "y": 3.48003888130188, - "heading": -0.7602557704725093, - "angularVelocity": -9.209824503951875e-20, - "velocityX": -1.2993679631111057e-18, - "velocityY": -2.0831148734143083e-18, + "x": 2.109028816223143, + "y": 3.4800388813018808, + "heading": -0.760255770472509, + "angularVelocity": -4.7854036756238116e-17, + "velocityX": -4.1684189280839564e-16, + "velocityY": 3.182458036241524e-16, "moduleForcesX": [ - 60.93038441390968, - 60.03160907225733, - 56.409223413469284, - 55.845190143852285 + 60.53644088216914, + 59.71107786211956, + 56.23458372965665, + 55.717252600818924 ], "moduleForcesY": [ - -34.38845870016777, - -35.943896076352594, - -41.38750873505681, - -42.15404676926127 + -35.07667248160167, + -36.472859348504336, + -41.62412921793918, + -42.32221267335896 ], - "timestamp": 6.267535045844391 + "timestamp": 6.1324847610015265 }, { - "x": 2.117794568665661, - "y": 3.4681155240753, - "heading": -0.7530398045067034, - "angularVelocity": 0.11476292191713451, - "velocityX": 0.13941076882464276, - "velocityY": -0.18962940247615553, + "x": 2.1173647314297415, + "y": 3.4676489109609023, + "heading": -0.7574096677464409, + "angularVelocity": 0.045234117251022915, + "velocityX": 0.13248564867185347, + "velocityY": -0.19691818078357895, "moduleForcesX": [ - 48.13501730949312, - 48.836835407072094, - 31.251459327861657, - 36.04401089708163 + 41.520736384271714, + 42.51429487640815, + 35.1511276560287, + 36.81654326527437 ], "moduleForcesY": [ - -50.76492473541808, - -50.1079347694781, - -62.59287500981053, - -59.97406542598307 + -56.31022663437434, + -55.57003233589047, + -60.49287111022869, + -59.49929247213998 ], - "timestamp": 6.330412200288196 + "timestamp": 6.1954041435170275 }, { - "x": 2.1355122480176822, - "y": 3.4444032062204983, - "heading": -0.7387280504849648, - "angularVelocity": 0.22761453103812976, - "velocityX": 0.28178246151161096, - "velocityY": -0.37712135774201583, + "x": 2.1342750283552188, + "y": 3.4430345558261144, + "heading": -0.751664349766151, + "angularVelocity": 0.09131237069712948, + "velocityX": 0.2687613299655501, + "velocityY": -0.39120465190077036, "moduleForcesX": [ - 48.80717190467651, - 49.50131566328889, - 32.41122454849307, - 37.036459214586024 + 42.70173098288214, + 43.611128958075156, + 36.275170342746776, + 37.87745965146702 ], "moduleForcesY": [ - -50.11417943760879, - -49.44755209409215, - -61.99648850314001, - -59.36303177557035 + -55.41582346031395, + -54.70978545558125, + -59.821763435546124, + -58.826195167217094 ], - "timestamp": 6.3932893547319996 + "timestamp": 6.2583235260325285 }, { - "x": 2.162391169686947, - "y": 3.4090581131486557, - "heading": -0.717466130764082, - "angularVelocity": 0.3381501581768513, - "velocityX": 0.42748311222143226, - "velocityY": -0.5621293359169338, + "x": 2.1600269154540004, + "y": 3.4063896499792783, + "heading": -0.7429636060218578, + "angularVelocity": 0.1382839976560426, + "velocityX": 0.4092838497332282, + "velocityY": -0.5824104494002036, "moduleForcesX": [ - 49.51581154621932, - 50.265844017076226, - 33.70048897709995, - 38.19652881261669 + 44.01151222332591, + 44.83593128986605, + 37.54047868266639, + 39.07824731715444 ], "moduleForcesY": [ - -49.40850610951917, - -48.66541972742287, - -61.300931724870296, - -58.619530919336384 + -54.37659249907092, + -53.70636995290632, + -59.03155043231922, + -58.03154166424591 ], - "timestamp": 6.456166509175803 + "timestamp": 6.3212429085480295 }, { - "x": 2.1986681969801216, - "y": 3.3622638578182182, - "heading": -0.6894300597240235, - "angularVelocity": 0.4458864477576762, - "velocityX": 0.576950843499077, - "velocityY": -0.7442171285321014, + "x": 2.1949215222317773, + "y": 3.357943659865962, + "heading": -0.7312478740657462, + "angularVelocity": 0.18620227166446401, + "velocityX": 0.5545923272400236, + "velocityY": -0.7699692555212343, "moduleForcesX": [ - 50.28381428848592, - 51.13744068398159, - 35.15534244893406, - 39.540816535356164 + 45.47389898312857, + 46.209380246970596, + 38.97509187538921, + 40.44329438249183 ], "moduleForcesY": [ - -48.620147379296554, - -47.74309558757011, - -60.4734352958337, - -57.716863658050855 + -53.154044492191325, + -52.52435244840178, + -58.08923297674164, + -57.08406886084209 ], - "timestamp": 6.519043663619607 + "timestamp": 6.3841622910635305 }, { - "x": 2.2446132442928994, - "y": 3.304239125606535, - "heading": -0.6548330373393313, - "angularVelocity": 0.550231999057986, - "velocityX": 0.7307113007768327, - "velocityY": -0.922826942869088, + "x": 2.2393001625829205, + "y": 3.297971653928642, + "heading": -0.7164541055664759, + "angularVelocity": 0.23512259510216332, + "velocityX": 0.7053254271878769, + "velocityY": -0.9531563015982434, "moduleForcesX": [ - 51.13905740321851, - 52.12476715621361, - 36.82116804073696, - 41.09052851792727 + 47.11716302884608, + 47.75573925943408, + 40.613851617327796, + 42.00243934560986 ], "moduleForcesY": [ - -47.71212916324656, - -46.6564142258722, - -59.46813562411414, - -56.61877073126656 + -51.69634105716302, + -51.116592706220615, + -56.94950428119139, + -55.94148520473914 ], - "timestamp": 6.581920818063411 + "timestamp": 6.4470816735790315 }, { - "x": 2.3005361732090566, - "y": 3.235248258796662, - "heading": -0.6139347954947619, - "angularVelocity": 0.650446767293226, - "velocityX": 0.8893998052366954, - "velocityY": -1.0972326502391698, + "x": 2.293551835832764, + "y": 3.226807987294763, + "heading": -0.6985158525170084, + "angularVelocity": 0.28509900021743295, + "velocityX": 0.862241030996735, + "velocityY": -1.1310293233781603, "moduleForcesX": [ - 52.11606824369092, - 53.23896165126432, - 38.75432059140504, - 42.87287498290715 + 48.974244926718995, + 49.50301706537962, + 42.49983724647496, + 43.79203201526126 ], "moduleForcesY": [ - -46.633926917902656, - -45.372711713166105, - -58.220079562306445, - -55.27504395126715 + -49.932591819303816, + -49.41939613494972, + -55.54905272250912, + -54.54558483413394 ], - "timestamp": 6.644797972507215 + "timestamp": 6.5100010560945325 }, { - "x": 2.3667954166739076, - "y": 3.155616243498738, - "heading": -0.5670546584241174, - "angularVelocity": 0.7455829940991222, - "velocityX": 1.0537888371533939, - "velocityY": -1.2664697695423583, + "x": 2.3581219992537226, + "y": 3.144865400496988, + "heading": -0.6773637507894199, + "angularVelocity": 0.3361778339508891, + "velocityX": 1.0262364447879162, + "velocityY": -1.302342513892083, "moduleForcesX": [ - 53.25761026058277, - 54.49467377205829, - 41.023942389693836, - 44.92291476717264 + 51.081995362995904, + 51.48237998253363, + 44.68553797550306, + 45.85573965609478 ], "moduleForcesY": [ - -45.315096693303126, - -43.846496735504985, - -56.635631853416726, - -53.61440385265008 + -47.76441461323329, + -47.34537980089634, + -53.79777393977218, + -52.8148137116725 ], - "timestamp": 6.707675126951019 + "timestamp": 6.5729204386100335 }, { - "x": 2.4438086218512485, - "y": 3.065750442430627, - "heading": -0.5145902034841154, - "angularVelocity": 0.8343961396486453, - "velocityX": 1.2248201410922746, - "velocityY": -1.4292281809353713, + "x": 2.433522291274826, + "y": 3.0526620913362525, + "heading": -0.652926734850225, + "angularVelocity": 0.3883861373428849, + "velocityX": 1.1983635090908211, + "velocityY": -1.46541980347662, "moduleForcesX": [ - 54.61557385722636, - 55.91123819153664, - 43.7131279622274, - 47.285771912845235 + 53.47798671720776, + 53.72572515007777, + 47.232726000587874, + 48.24428581121922 ], "moduleForcesY": [ - -43.65585603926645, - -42.012507491987115, - -54.57686147012399, - -51.53256053951273 + -45.05355867588098, + -44.773023132097414, + -51.56510924673164, + -50.632864772568915 ], - "timestamp": 6.7705522813948225 + "timestamp": 6.6358398211255345 }, { - "x": 2.532065289837757, - "y": 2.9661728933690776, - "heading": -0.45704452569034076, - "angularVelocity": 0.9152080481823572, - "velocityX": 1.4036364839854023, - "velocityY": -1.583684089116122, + "x": 2.520339981100692, + "y": 2.9508605578412346, + "heading": -0.6251343348430549, + "angularVelocity": 0.4417144430863272, + "velocityX": 1.3798242505713987, + "velocityY": -1.6179677775753416, "moduleForcesX": [ - 56.24963874793976, - 57.513648109663556, - 46.9169797801361, - 50.01853513267831 + 56.19197506453079, + 56.259276660576184, + 50.20804830120448, + 51.011888473748435 ], "moduleForcesY": [ - -41.51321004507581, - -39.77429977030871, - -51.835917665532385, - -48.87154151079314 + -41.604044996750225, + -41.53144010163077, + -48.65926448291237, + -47.83146963835682 ], - "timestamp": 6.833429435838626 + "timestamp": 6.6987592036410355 }, { - "x": 2.6321403093335114, - "y": 2.8575694307170187, - "heading": -0.3950667539653817, - "angularVelocity": 0.985696192411999, - "velocityX": 1.591595872634394, - "velocityY": -1.727232468020187, + "x": 2.6192438141173406, + "y": 2.8403235512778635, + "heading": -0.5939211843905473, + "angularVelocity": 0.4960816397843361, + "velocityX": 1.5719135989976136, + "velocityY": -1.7568037406619357, "moduleForcesX": [ - 58.220168914381084, - 59.332124325032076, - 50.73064435062025, - 53.18908968161254 + 59.2250417610776, + 59.087826967496994, + 53.66857872752665, + 54.204993983215175 ], "moduleForcesY": [ - -38.68115242041676, - -36.98517279536072, - -48.09349009166419, - -45.382842135502834 + -37.1392249820516, + -37.38030580647965, + -44.79597130253933, + -44.16473609829124 ], - "timestamp": 6.89630659028243 + "timestamp": 6.7616785861565365 }, { - "x": 2.7447041232794764, - "y": 2.740865636486064, - "heading": -0.32951256744957347, - "angularVelocity": 1.0425755919727022, - "velocityX": 1.7902180043240945, - "velocityY": -1.8560603650608418, + "x": 2.7309783267356824, + "y": 2.7221935960005026, + "heading": -0.5592365908276438, + "angularVelocity": 0.5512545129373857, + "velocityX": 1.7758361279342616, + "velocityY": -1.8774811600901864, "moduleForcesX": [ - 60.5677211979897, - 61.39663828373237, - 55.20909444556158, - 56.86245394897771 + 62.5038695327194, + 62.160031630846326, + 57.62319015145175, + 57.832996172310274 ], "moduleForcesY": [ - -34.86502705000924, - -33.416146228705585, - -42.85575978754388, - -40.660621351301806 + -31.280112248684674, + -31.988407393096097, + -39.55612203914035, + -39.27379472999048 ], - "timestamp": 6.959183744726234 + "timestamp": 6.8245979686720375 }, { - "x": 2.870516828572632, - "y": 2.6173440746550423, - "heading": -0.26153269734842044, - "angularVelocity": 1.081153730675094, - "velocityX": 2.0009287380458565, - "velocityY": -1.9644903291770046, + "x": 2.856329605423656, + "y": 2.598000279152458, + "heading": -0.5210635851729407, + "angularVelocity": 0.606697080113568, + "velocityX": 1.9922522071333428, + "velocityY": -1.9738483100568995, "moduleForcesX": [ - 63.264661428716025, - 63.71631261983922, - 60.25491380232913, - 61.043986331952446 + 65.79278778008134, + 65.30020556417972, + 61.939549113121835, + 61.79878070206867 ], "moduleForcesY": [ - -29.65704803224665, - -28.70294866760673, - -35.37956356936107, - -34.023166255951196 + -23.548304579936264, + -24.925035881299397, + -32.34851276785782, + -32.65108476149218 ], - "timestamp": 7.022060899170038 + "timestamp": 6.8875173511875385 }, { - "x": 3.0103732419935905, - "y": 2.488816613373573, - "heading": -0.1926918880041953, - "angularVelocity": 1.094846132163167, - "velocityX": 2.224280259787407, - "velocityY": -2.0441042922249113, + "x": 2.996031354964205, + "y": 2.46978535540666, + "heading": -0.47945305051497955, + "angularVelocity": 0.6613309443669427, + "velocityX": 2.220329315948571, + "velocityY": -2.0377651308674327, "moduleForcesX": [ - 66.11806816346805, - 66.21721889876862, - 65.35371655844693, - 65.48547608931766 + 68.5564837925553, + 68.09308895613952, + 66.1567792079864, + 65.75590846714256 ], "moduleForcesY": [ - -22.54079142004972, - -22.263089069440703, - -24.6715805356649, - -24.33345121959331 + -13.459642197003902, + -15.709687602642282, + -22.445832125017436, + -23.647305076742175 ], - "timestamp": 7.0849380536138415 + "timestamp": 6.9504367337030395 }, { - "x": 3.1649283508936707, - "y": 2.3578354928773337, - "heading": -0.12509625596484528, - "angularVelocity": 1.075042797933275, - "velocityX": 2.458048718445309, - "velocityY": -2.0831273561099635, + "x": 3.1505724071472994, + "y": 2.340193763604278, + "heading": -0.43457360660126365, + "angularVelocity": 0.7132848753348665, + "velocityX": 2.456175601294578, + "velocityY": -2.059645003198423, "moduleForcesX": [ - 68.61737088139724, - 68.5714962678954, - 69.13779393676624, - 69.12206655275224 + 69.84706521344005, + 69.75290941394955, + 69.22551312159781, + 68.88502793630397 ], "moduleForcesY": [ - -13.001403954681635, - -13.198507429125556, - -9.864173290779956, - -9.916683464178158 + -0.8233024041036456, + -4.014815042921486, + -9.277060723605338, + -11.648512019585809 ], - "timestamp": 7.147815208057645 + "timestamp": 7.0133561162185405 }, { - "x": 3.3343139997886584, - "y": 2.227807798534294, - "heading": -0.061425277024888265, - "angularVelocity": 1.0126250067003624, - "velocityX": 2.6939140359218023, - "velocityY": -2.067964040250105, + "x": 3.3199095005462036, + "y": 2.212409184758706, + "heading": -0.3867586308994902, + "angularVelocity": 0.7599403202978621, + "velocityX": 2.691334317484535, + "velocityY": -2.03092550716134, "moduleForcesX": [ - 69.81884664866837, - 69.79428970681542, - 69.29948552018702, - 69.00682870384438 + 68.47925044891942, + 69.15340266140203, + 69.49958829473019, + 69.7686571831701 ], "moduleForcesY": [ - -0.8612358805856215, - -0.3297910827818419, - 8.567864185381797, - 10.490055875380158 + 13.736045870694008, + 9.89537651518505, + 6.815502213890215, + 3.370466430920167 ], - "timestamp": 7.210692362501449 + "timestamp": 7.0762754987340415 }, { - "x": 3.5176404379599324, - "y": 2.1026847463287157, - "heading": -0.004525789100840003, - "angularVelocity": 0.9049310266561369, - "velocityX": 2.9156287334077797, - "velocityY": -1.9899604763031347, + "x": 3.5032276259952, + "y": 2.089811754517775, + "heading": -0.3365088719529981, + "angularVelocity": 0.7986371915540097, + "velocityX": 2.9135398047467214, + "velocityY": -1.9484843197678885, "moduleForcesX": [ - 68.55777812799681, - 67.6445550813489, - 64.16468459069685, - 60.8787964212339 + 63.762980248590615, + 65.33381311946748, + 65.64375774359121, + 66.90783722378835 ], "moduleForcesY": [ - 13.196294200522606, - 17.05028282885496, - 27.549743552264584, - 34.11506286093019 + 28.499981635594498, + 24.717827613930762, + 23.81826750682116, + 20.038950866645536 ], - "timestamp": 7.273569516945253 + "timestamp": 7.1391948812495425 }, { - "x": 3.712921078466066, - "y": 1.986111498522952, - "heading": 0.04367317548530425, - "angularVelocity": 0.7665576633119034, - "velocityX": 3.105748697337491, - "velocityY": -1.8539841511108714, + "x": 3.698983205941932, + "y": 1.9754559392748747, + "heading": -0.2844250046592506, + "angularVelocity": 0.8277873242146967, + "velocityX": 3.111212668028096, + "velocityY": -1.8174974176634262, "moduleForcesX": [ - 64.1546735583916, - 59.222021183604404, - 55.03127871584201, - 45.60985368543614 + 56.27400205759313, + 58.363284645856304, + 58.070669393657745, + 60.053109943523204 ], "moduleForcesY": [ - 27.543396744432503, - 36.83392520078611, - 43.0063108958907, - 52.83689467676779 + 41.37894474295054, + 38.388062514062604, + 38.80031879570978, + 35.67059242066261 ], - "timestamp": 7.336446671389057 + "timestamp": 7.2021142637650435 }, { - "x": 3.917974347119227, - "y": 1.880569748380944, - "heading": 0.0840407650336414, - "angularVelocity": 0.6420072585252761, - "velocityX": 3.261172845161521, - "velocityY": -1.678538907741677, + "x": 3.9052468507649114, + "y": 1.8717029681737345, + "heading": -0.23110382341189709, + "angularVelocity": 0.8474523924995162, + "velocityX": 3.2782210596577848, + "velocityY": -1.6489826656448716, "moduleForcesX": [ - 56.176200766123785, - 46.718354113326974, - 45.97750919549416, - 34.263779007829406 + 47.55707312914185, + 49.53638736807368, + 48.78953373366416, + 50.77045950153568 ], "moduleForcesY": [ - 41.47332187047582, - 51.820224724822296, - 52.59176748493925, - 60.84133045416032 + 51.17696939290848, + 49.268536466648314, + 49.99446973782349, + 47.98722716731246 ], - "timestamp": 7.399323825832861 + "timestamp": 7.2650336462805445 }, { - "x": 4.131094513594558, - "y": 1.787487140153957, - "heading": 0.11947354396561721, - "angularVelocity": 0.5635238942570701, - "velocityX": 3.3894690108122587, - "velocityY": -1.4803883707901997, + "x": 4.120080601641824, + "y": 1.7801799153533593, + "heading": -0.17706327947410902, + "angularVelocity": 0.8588854781668361, + "velocityX": 3.4144287862962694, + "velocityY": -1.454608248862383, "moduleForcesX": [ - 45.44840572457847, - 37.20679327325788, - 38.51657585799193, - 29.99924193711389 + 39.05701336414032, + 40.46649013860068, + 39.71798936608046, + 41.14398002340263 ], "moduleForcesY": [ - 53.034304370620895, - 59.079362573109876, - 58.2883307755835, - 63.07823138252855 + 57.94278647291951, + 56.96920228244361, + 57.48746696114792, + 56.477668387844396 ], - "timestamp": 7.4622009802766645 + "timestamp": 7.3279530287960455 }, { - "x": 4.350770411068689, - "y": 1.7077754935808809, - "heading": 0.1521039131255066, - "angularVelocity": 0.5189542918812017, - "velocityX": 3.493731537588328, - "velocityY": -1.2677362275406037, + "x": 4.341754671827057, + "y": 1.7019368196307447, + "heading": -0.12271990769558694, + "angularVelocity": 0.8636984281454757, + "velocityX": 3.523144400386022, + "velocityY": -1.243545193777744, "moduleForcesX": [ - 35.38842380663489, - 29.894307911171417, - 31.489034487421396, - 26.080480439830673 + 31.542569411645978, + 32.211867496766956, + 31.792298515347188, + 32.46659837801001 ], "moduleForcesY": [ - 60.23788124052505, - 63.133637641285205, - 62.37825455380923, - 64.81765821899063 + 62.36899517370198, + 62.02650340460398, + 62.24055631529311, + 61.8920479920422 ], - "timestamp": 7.525078134720468 + "timestamp": 7.3908724113115465 }, { - "x": 4.575688712037729, - "y": 1.6420223189913672, - "heading": 0.18322659040704492, - "angularVelocity": 0.49497591862802676, - "velocityX": 3.5771068674880655, - "velocityY": -1.0457403037899842, + "x": 4.568810602625837, + "y": 1.6376320022464737, + "heading": -0.06839919997587875, + "angularVelocity": 0.8633382202427946, + "velocityX": 3.6086802146038512, + "velocityY": -1.0220192063777647, "moduleForcesX": [ - 27.158137877972838, - 23.927370915342486, - 25.160312496417824, - 21.995099103025833 + 25.215137722620277, + 25.161326804862117, + 25.198140713892162, + 25.144365342135142 ], "moduleForcesY": [ - 64.38875092826233, - 65.65351312146176, - 65.20196768922268, - 66.33295061137039 + 65.19836347228741, + 65.21911954217529, + 65.20503178610034, + 65.2257580831436 ], - "timestamp": 7.587955289164272 + "timestamp": 7.4537917938270475 }, { - "x": 4.804716248215296, - "y": 1.5906097298688888, - "heading": 0.21347571274604327, - "angularVelocity": 0.48108287670736033, - "velocityX": 3.6424602576800607, - "velocityY": -0.8176672366499624, + "x": 4.800045847269579, + "y": 1.5876743304432248, + "heading": -0.014355888196568429, + "angularVelocity": 0.8589294684510949, + "velocityX": 3.675103527705061, + "velocityY": -0.793994947279417, "moduleForcesX": [ - 20.777362325555703, - 18.79970144179875, - 19.686842590719646, - 17.74181237154826 + 19.993448516674185, + 19.3060989973268, + 19.797835730589334, + 19.116494526678093 ], "moduleForcesY": [ - 66.73767220647805, - 67.31957624630218, - 67.070927016661, - 67.60968811178219 + 66.99591754938321, + 67.196927406319, + 67.05502467711538, + 67.25215990996023 ], - "timestamp": 7.650832443608076 + "timestamp": 7.5167111763425485 }, { - "x": 5.036866817289369, - "y": 1.55378761076971, - "heading": 0.24298831748702848, - "angularVelocity": 0.469369280496969, - "velocityX": 3.692129059077492, - "velocityY": -0.5856199986290461, + "x": 5.034473115872903, + "y": 1.5523169276578892, + "heading": 0.039207016322208126, + "angularVelocity": 0.8512941859462895, + "velocityX": 3.725835493467699, + "velocityY": -0.5619477078724477, "moduleForcesX": [ - 15.925448739090935, - 14.192665989132097, - 15.055141930627649, - 13.351354517579729 + 15.705471617855421, + 14.48469807799971, + 15.374313004019221, + 14.172732804713208 ], "moduleForcesY": [ - 68.07164789067083, - 68.45258203067056, - 68.27203655365994, - 68.62435165252099 + 68.13853193873344, + 68.40802253366346, + 68.21561194224486, + 68.47494827858137 ], - "timestamp": 7.71370959805188 + "timestamp": 7.5796305588580495 }, { "x": 5.271278381347656, "y": 1.5317155122756958, - "heading": 0.2715710097371584, - "angularVelocity": 0.45457992657214513, - "velocityX": 3.7280879857212588, - "velocityY": -0.3510352637497425, + "heading": 0.09212323355733296, + "angularVelocity": 0.8410161562232161, + "velocityX": 3.7636298388086282, + "velocityY": -0.3274255810936793, "moduleForcesX": [ - 12.231894678034537, - 9.978543755139432, - 11.179968921219166, - 8.979896356158925 + 12.17583308633574, + 10.508988015592593, + 11.72461754628835, + 10.093645719719404 ], "moduleForcesY": [ - 68.84172245876557, - 69.20285737211027, - 69.02334612966767, - 69.34261769004998 + 68.86508793588061, + 69.1385750114367, + 68.94526551565065, + 69.20238200518273 ], - "timestamp": 7.7765867524956835 + "timestamp": 7.6425499413735505 }, { - "x": 5.388703997390188, - "y": 1.5243911275820383, - "heading": 0.2855249380175652, - "angularVelocity": 0.4444472090652179, - "velocityX": 3.7401286773236566, - "velocityY": -0.2332893125004083, + "x": 5.388681610311286, + "y": 1.5251584282304298, + "heading": 0.11807334458511697, + "angularVelocity": 0.8348404097837044, + "velocityX": 3.7769765097724286, + "velocityY": -0.21094779615682893, "moduleForcesX": [ - 9.343299160452919, - 6.163463777084662, - 7.986565873923746, - 4.920049004946797 + 9.224601000293218, + 7.170145322617407, + 8.707960432775863, + 6.708850729084163 ], "moduleForcesY": [ - 69.21664100426783, - 69.5686586634018, - 69.39371949997597, - 69.67546873681157 + 69.25444608627201, + 69.49615053644614, + 69.32560078879362, + 69.54657168705602 ], - "timestamp": 7.807982895830131 + "timestamp": 7.673633859597523 }, { - "x": 5.50644545946061, - "y": 1.5207685343702697, - "heading": 0.2991263551425099, - "angularVelocity": 0.43321936009960066, - "velocityX": 3.7501887036309256, - "velocityY": -0.11538338238486952, + "x": 5.506442673711366, + "y": 1.5222273963184734, + "heading": 0.14379764901773748, + "angularVelocity": 0.8275759911368509, + "velocityX": 3.7884883929871296, + "velocityY": -0.0942941585046398, "moduleForcesX": [ - 8.420939086179335, - 4.879883420406134, - 6.919098093280834, - 3.519523756844695 + 8.369389483724206, + 5.949973696893992, + 7.73117703857559, + 5.38783266312984 ], "moduleForcesY": [ - 69.32400261131451, - 69.65903960081413, - 69.4987054257006, - 69.75025512359892 + 69.35385904945188, + 69.60180730909208, + 69.43322409042021, + 69.65302052985173 ], - "timestamp": 7.839379039164578 + "timestamp": 7.704717777821495 }, { - "x": 5.624430269287079, - "y": 1.5208521505351245, - "heading": 0.31232341935960334, - "angularVelocity": 0.4203402971031069, - "velocityX": 3.757939584159604, - "velocityY": 0.002663262298303611, + "x": 5.624497338635267, + "y": 1.522927459386341, + "heading": 0.16925769890783504, + "angularVelocity": 0.8190746644823769, + "velocityX": 3.7979338406848053, + "velocityY": 0.02252171244382533, "moduleForcesX": [ - 7.427158187557407, - 3.3353613300752003, - 5.7144874128058545, - 1.8133624389873595 + 7.409211713616261, + 4.57253345050813, + 6.632095741425086, + 3.899218959272906 ], "moduleForcesY": [ - 69.4251012893418, - 69.73687602026827, - 69.59766681374457, - 69.80441399722511 + 69.45271134204867, + 69.69519778275762, + 69.53780580291625, + 69.74287628352383 ], - "timestamp": 7.870775182499026 + "timestamp": 7.735801696045467 }, { - "x": 5.742576714793087, - "y": 1.5246451742965244, - "heading": 0.3250572744698019, - "angularVelocity": 0.4055866026139322, - "velocityX": 3.7630878495951756, - "velocityY": 0.12081177363075021, + "x": 5.742772793309857, + "y": 1.5272629148189605, + "heading": 0.19440986357494344, + "angularVelocity": 0.8091696962357507, + "velocityX": 3.8050368625463413, + "velocityY": 0.13947583446143447, "moduleForcesX": [ - 6.306930660430928, - 1.5778924322604664, - 4.3676425070670675, - -0.10369410469570728 + 6.320859466741637, + 3.0067323445032117, + 5.3877760442274845, + 2.2145608793254503 ], "moduleForcesY": [ - 69.52171601095, - 69.78371617293728, - 69.68348163684516, - 69.815527135674 + 69.54854425346966, + 69.76816296129176, + 69.63518331243104, + 69.80621969634359 ], - "timestamp": 7.902171325833473 + "timestamp": 7.766885614269439 }, { - "x": 5.860794105940623, - "y": 1.5321490223767453, - "heading": 0.33726992474980966, - "angularVelocity": 0.3889856836845344, - "velocityX": 3.7653475424743803, - "velocityY": 0.2390054090493234, + "x": 5.861185901308817, + "y": 1.5352368348891807, + "heading": 0.21920461001465913, + "angularVelocity": 0.7976712028728045, + "velocityX": 3.8094653043977926, + "velocityY": 0.2565287944964038, "moduleForcesX": [ - 5.00586577439091, - -0.3668746684666077, - 2.864339879121958, - -2.17095393844088 + 5.0743871573767105, + 1.212540305184361, + 3.969650176486514, + 0.29853628468642324 ], "moduleForcesY": [ - 69.611426474232, - 69.78350871775525, - 69.74815431030521, - 69.76783434978745 + 69.63726174457908, + 69.80849842797434, + 69.71904021769083, + 69.82888852399873 ], - "timestamp": 7.9335674691679205 + "timestamp": 7.797969532493411 }, { - "x": 5.9789826775060435, - "y": 1.5433627560426824, - "heading": 0.3489121084818783, - "angularVelocity": 0.37081572752583847, - "velocityX": 3.764429608643845, - "velocityY": 0.3571691448367622, + "x": 5.979640967451708, + "y": 1.5468503445815898, + "heading": 0.24358557158644084, + "angularVelocity": 0.7843593396465376, + "velocityX": 3.810815138856516, + "velocityY": 0.3736179463840169, "moduleForcesX": [ - 3.4623816637274447, - -2.475568668701702, - 1.1828304235517055, - -4.335764900525163 + 3.6307956438747206, + -0.8621551207472431, + 2.3418002525812245, + -1.893133883598176 ], "moduleForcesY": [ - 69.68689216287859, - 69.72122437760291, - 69.7815156598602, - 69.65073511362182 + 69.7120826368421, + 69.79764527340737, + 69.77979201753804, + 69.79043163606336 ], - "timestamp": 7.964963612502368 + "timestamp": 7.8290534507173835 }, { - "x": 6.097033194516696, - "y": 1.5582824506328423, - "heading": 0.3599519448622258, - "angularVelocity": 0.35163033442501535, - "velocityX": 3.7600324266938894, - "velocityY": 0.47520787605114534, + "x": 6.098026846695146, + "y": 1.5621015192437462, + "heading": 0.2674883023191033, + "angularVelocity": 0.7689741866013711, + "velocityX": 3.808589328746145, + "velocityY": 0.4906451803233273, "moduleForcesX": [ - 1.6038425315720386, - -4.721980038884405, - -0.7091635039964642, - -6.549079033729455 + 1.9386808198061933, + -3.28598819102792, + 0.4585755906663063, + -4.416447800806846 ], "moduleForcesY": [ - 69.73338633060467, - 69.58337205904242, - 69.76976708048174, - 69.45885860862715 + 69.76178277329687, + 69.70685348700545, + 69.80282869693546, + 69.6609062566512 ], - "timestamp": 7.996359755836815 + "timestamp": 7.860137368941356 }, { - "x": 6.214826085217277, - "y": 1.5769004197748018, - "heading": 0.37038445324917163, - "angularVelocity": 0.3322863026777996, - "velocityX": 3.7518267592867267, - "velocityY": 0.5930017882652427, + "x": 6.216213161429717, + "y": 1.5809836737297318, + "heading": 0.2908385525285553, + "angularVelocity": 0.7512003487206494, + "velocityX": 3.8021691436385594, + "velocityY": 0.6074573465909969, "moduleForcesX": [ - -0.6554876781322178, - -7.078557663607612, - -2.860110163308246, - -8.769410640028871 + -0.07065192727705566, + -6.150375646776657, + -1.738667342074545, + -7.342704323857981 ], "moduleForcesY": [ - 69.7245742748119, - 69.35870703983915, - 69.69292267797944, - 69.1914612156941 + 69.76766630955797, + 69.49064187075653, + 69.76568210247977, + 69.39577088637856 ], - "timestamp": 8.027755899171263 + "timestamp": 7.891221287165328 }, { - "x": 6.332229852804532, - "y": 1.5992041212873251, - "heading": 0.3802420776714249, - "angularVelocity": 0.31397564717566995, - "velocityX": 3.7394327811735093, - "velocityY": 0.7103962188901063, + "x": 6.334045309853754, + "y": 1.6034826557006305, + "heading": 0.313549807262191, + "angularVelocity": 0.7306432403402956, + "velocityX": 3.790775267616168, + "velocityY": 0.7238142182972129, "moduleForcesX": [ - -3.4164906317976134, - -9.51983860988743, - -5.342993033888524, - -10.967733824511571 + -2.490383343681562, + -9.578064211771972, + -4.3259113321238125, + -10.762745909847968 ], "moduleForcesY": [ - 69.61603638629833, - 69.03834092530526, - 69.5200097527037, - 68.85058965896839 + 69.69820129934999, + 69.07536900988754, + 69.63337299442767, + 68.9276312183353 ], - "timestamp": 8.05915204250571 + "timestamp": 7.9223052053893 }, { - "x": 6.449098535030912, - "y": 1.625174456079489, - "heading": 0.3896069207325694, - "angularVelocity": 0.29828004546244535, - "velocityX": 3.722389752825253, - "velocityY": 0.827182323494794, + "x": 6.451337870714877, + "y": 1.6295724757424068, + "heading": 0.33551971883232135, + "angularVelocity": 0.706793506913393, + "velocityX": 3.7734162088570447, + "velocityY": 0.8393349851774983, "moduleForcesX": [ - -6.797799685550791, - -12.023870431612343, - -8.267497900609209, - -13.12862175589208 + -5.448866729736701, + -13.733572282788284, + -7.401927508185858, + -14.790647300726173 ], "moduleForcesY": [ - 69.33533463241886, - 68.61540564974591, - 69.19995880316955, - 68.43876233553452 + 69.49922387521676, + 68.33891344443161, + 69.35066212276465, + 68.15293380811582 ], - "timestamp": 8.090548185840158 + "timestamp": 7.953389123613272 }, { - "x": 6.5652678833030125, - "y": 1.6547829667795249, - "heading": 0.39862574645972415, - "angularVelocity": 0.287259031502107, - "velocityX": 3.7001152350021482, - "velocityY": 0.9430620310472863, + "x": 6.567866022044423, + "y": 1.659208124359565, + "heading": 0.35662502542728386, + "angularVelocity": 0.6789783206509007, + "velocityX": 3.7488244078469917, + "velocityY": 0.9534077526398532, "moduleForcesX": [ - -10.936219914888005, - -14.574762118658219, - -11.799686531958358, - -15.252283089212833 + -9.125353794114547, + -18.83172539656305, + -11.094164589642684, + -19.562885061791146 ], "moduleForcesY": [ - 68.76683316861762, - 68.08405084461447, - 68.64363965268268, - 67.95604052661037 + 69.0754447164273, + 67.07463735744341, + 68.82909121269832, + 66.91127860350836 ], - "timestamp": 8.121944329174605 + "timestamp": 7.984473041837244 }, { - "x": 6.6805489880212905, - "y": 1.6879868491141017, - "heading": 0.40752627429499416, - "angularVelocity": 0.28349111992696274, - "velocityX": 3.671823748867664, - "velocityY": 1.057578377728512, + "x": 6.683354928219009, + "y": 1.6923136502791418, + "heading": 0.37671482456009425, + "angularVelocity": 0.6463084540390093, + "velocityX": 3.7153908764796526, + "velocityY": 1.0650370934911797, "moduleForcesX": [ - -15.976487504436525, - -17.19633335319417, - -16.19563370942626, - -17.393215864457243 + -13.771593248917489, + -25.130414558467773, + -15.562652582080137, + -25.223578565092154 ], "moduleForcesY": [ - 67.73039056976316, - 67.42967661497167, - 67.68616175746094, - 67.38710414645385 + 68.25443383699609, + 64.93026638806907, + 67.92556316929904, + 64.95632378013195 ], - "timestamp": 8.153340472509052 + "timestamp": 8.015556960061216 }, { - "x": 6.794712373597259, - "y": 1.7247180908804576, - "heading": 0.4166128430796408, - "angularVelocity": 0.28941671873044894, - "velocityX": 3.6362232252491316, - "velocityY": 1.1699284646230703, + "x": 6.7974682626271505, + "y": 1.7287625418402501, + "heading": 0.3956032132276822, + "angularVelocity": 0.6076579063002795, + "velocityX": 3.671137389627324, + "velocityY": 1.1725964306841732, "moduleForcesX": [ - -22.036733716797762, - -20.18398244295977, - -21.84839807752641, - -19.94027338243488 + -19.7338671115633, + -32.869545661536705, + -20.995212284811174, + -31.878789101972526 ], "moduleForcesY": [ - 65.95765425448234, - 66.54948915551118, - 66.00569479257204, - 66.6085993405814 + 66.71548826157209, + 61.31804473588162, + 66.4083595377785, + 61.9239118979002 ], - "timestamp": 8.1847366158435 + "timestamp": 8.046640878285189 }, { - "x": 6.907432636307125, - "y": 1.7648507330806484, - "heading": 0.4261265032765866, - "angularVelocity": 0.30302002687404705, - "velocityX": 3.590258252713168, - "velocityY": 1.278266625702327, + "x": 6.909800194013484, + "y": 1.7683468561362488, + "heading": 0.41306566672219286, + "angularVelocity": 0.5617841794810646, + "velocityX": 3.6138279150310604, + "velocityY": 1.2734660415324053, "moduleForcesX": [ - -29.201525856972616, - -25.15437815757134, - -29.219412722371683, - -24.891885489663608 + -27.444972321937637, + -42.075345805688336, + -27.57805811433052, + -39.49775701748281 ], "moduleForcesY": [ - 63.049097714214916, - 64.77103172702344, - 63.0005849187992, - 64.8334626863064 + 63.85363325543449, + 55.33672689274484, + 63.908626512967174, + 57.321971693817275 ], - "timestamp": 8.216132759177947 + "timestamp": 8.07772479650916 }, { - "x": 7.018202438708799, - "y": 1.808117637629141, - "heading": 0.4359499136905104, - "angularVelocity": 0.31288589522859334, - "velocityX": 3.5281340520616546, - "velocityY": 1.3780961593782959, + "x": 7.019881390468804, + "y": 1.810734119873736, + "heading": 0.42885115065317647, + "angularVelocity": 0.5078344312078966, + "velocityX": 3.5414195746540194, + "velocityY": 1.363639661900725, "moduleForcesX": [ - -37.84606166215224, - -35.12657257182429, - -38.23355604485398, - -35.39322800560654 + -37.27804152080681, + -52.1405235304012, + -35.411432044620646, + -47.75401943748761 ], "moduleForcesY": [ - 58.175724092585945, - 59.8531774353204, - 57.88611968366453, - 59.66067088624069 + 58.55809505279019, + 45.896730636123046, + 59.86837524976089, + 50.604050638682544 ], - "timestamp": 8.247528902512395 + "timestamp": 8.108808714733133 }, { - "x": 7.126380703050267, - "y": 1.854009244621213, - "heading": 0.4456272830552769, - "angularVelocity": 0.3082343350798942, - "velocityX": 3.4455908545549447, - "velocityY": 1.461695677179563, + "x": 7.127216489228573, + "y": 1.8554226010135964, + "heading": 0.44272434085459006, + "angularVelocity": 0.4463140747396045, + "velocityX": 3.4530749304632886, + "velocityY": 1.4376720726731453, "moduleForcesX": [ - -48.349849951021866, - -49.399500416665845, - -47.99842192068982, - -49.03597721530752 + -48.97728499858189, + -61.38705872437014, + -44.32215534808062, + -55.881419683225104 ], "moduleForcesY": [ - 49.65036150580679, - 48.61361186301433, - 50.011197849600876, - 49.001261315065825 + 49.06528866999657, + 32.434718317404084, + 53.54352333978887, + 41.41142967075846 ], - "timestamp": 8.278925045846842 + "timestamp": 8.139892632957105 }, { "x": 7.231364727020264, "y": 1.90172815322876, "heading": 0.45451541638849, - "angularVelocity": 0.28309634207400236, - "velocityX": 3.343850958114625, - "velocityY": 1.5198971446658642, + "angularVelocity": 0.3793304128823317, + "velocityX": 3.3505505014284465, + "velocityY": 1.489694828094504, "moduleForcesX": [ - -59.49373787912882, - -62.82905712054162, - -57.10726545680866, - -60.65366662655575 + -60.49418247701689, + -67.53589230331421, + -53.57894617228146, + -62.756167691165814 ], "moduleForcesY": [ - 35.35799304336133, - 29.157332713084195, - 39.2405425695758, - 33.58676445334213 + 33.72227739407681, + 16.098766492768572, + 44.212603887497394, + 29.961682528729266 ], - "timestamp": 8.31032118918129 + "timestamp": 8.170976551181077 }, { - "x": 7.423388079224838, - "y": 1.9985849999652825, - "heading": 0.46645537399184306, - "angularVelocity": 0.19401513991291586, - "velocityX": 3.1202319792203808, - "velocityY": 1.5738493632366841, + "x": 7.4238027530368695, + "y": 1.9965622542854489, + "heading": 0.4692934758689833, + "angularVelocity": 0.24023905191549066, + "velocityX": 3.1283626232349375, + "velocityY": 1.5416675347118225, "moduleForcesX": [ - -67.89958925754117, - -69.39640044262892, - -64.37618878024341, - -67.53660965026158 + -68.24273916485633, + -69.7323377330834, + -62.01731512678528, + -67.61265076621554 ], "moduleForcesY": [ - 15.193911023893614, - 5.976626851451594, - 26.574607140432867, - 17.20645904856515 + 13.81064712566369, + -0.3213659067003291, + 31.862172293048193, + 17.244933344769986 ], - "timestamp": 8.371862559657389 + "timestamp": 8.232490528041474 }, { - "x": 7.601264963854788, - "y": 2.094205845435507, - "heading": 0.47375371275159783, - "angularVelocity": 0.1185923989552555, - "velocityX": 2.8903627471056996, - "velocityY": 1.5537652920381468, + "x": 7.602133310590925, + "y": 2.090716538878092, + "heading": 0.47717310494630966, + "angularVelocity": 0.12809493841064415, + "velocityX": 2.899025012783137, + "velocityY": 1.5306161200782256, "moduleForcesX": [ - -68.97563128408632, - -68.37332903743547, - -69.6787002752127, - -69.70563687256657 + -69.13754121748958, + -68.20178675329157, + -69.06817284359947, + -69.80874265266795 ], "moduleForcesY": [ - -10.02818095513775, - -13.79457079772149, - 2.5400050436943973, - -2.895918692242975 + -9.046985055914966, + -14.8227009169618, + 9.835976008332125, + 0.7232885771622478 ], - "timestamp": 8.433403930133489 + "timestamp": 8.29400450490187 }, { - "x": 7.765427995394615, - "y": 2.1860237902182353, - "heading": 0.4771503707617556, - "angularVelocity": 0.055193083674938345, - "velocityX": 2.667523168070835, - "velocityY": 1.491971076893497, + "x": 7.76664139791012, + "y": 2.1816852539691975, + "heading": 0.47965086497918497, + "angularVelocity": 0.04027962683177553, + "velocityX": 2.6743204669166074, + "velocityY": 1.4788300112274317, "moduleForcesX": [ - -65.89884406545639, - -65.46067825744251, - -68.71649541984166, - -68.19447216007241 + -66.40756278498199, + -65.73447233946021, + -69.52981390692969, + -68.96429053135994 ], "moduleForcesY": [ - -22.954179123046586, - -24.267429442756974, - -12.195515440032587, - -14.975245795855631 + -21.48510284383042, + -23.5840541324825, + -6.248641320167586, + -11.053848562321154 ], - "timestamp": 8.494945300609588 + "timestamp": 8.355518481762267 }, { - "x": 7.916364373904703, - "y": 2.2725370209726456, - "heading": 0.4771759891464777, - "angularVelocity": 0.00041627907412384533, - "velocityX": 2.4526002157964135, - "velocityY": 1.4057735485108946, + "x": 7.917760773522142, + "y": 2.267858010889966, + "heading": 0.4778609500693658, + "angularVelocity": -0.029097694559421974, + "velocityX": 2.456667302700648, + "velocityY": 1.4008646704200718, "moduleForcesX": [ - -63.034541640882665, - -62.936246190035384, - -66.59623107773926, - -66.17286001113268 + -63.7079413269904, + -63.48861495951544, + -67.77169795111662, + -67.17513898955453 ], "moduleForcesY": [ - -30.054599009672646, - -30.30989854312364, - -21.022090416114956, - -22.384264891991062 + -28.621263222946787, + -29.166159121005602, + -16.89631000921434, + -19.21841268100821 ], - "timestamp": 8.556486671085688 + "timestamp": 8.417032458622664 }, { - "x": 8.054483029258073, - "y": 2.3527934772526407, - "heading": 0.4742054330913605, - "angularVelocity": -0.04826925419658775, - "velocityX": 2.2443220598574287, - "velocityY": 1.3041057691616338, + "x": 8.055903927968282, + "y": 2.3481558395536353, + "heading": 0.47263055774144574, + "angularVelocity": -0.08502770581375621, + "velocityX": 2.245719777793735, + "velocityY": 1.3053590868608649, "moduleForcesX": [ - -60.81370264139378, - -60.97714530202473, - -64.58773267429666, - -64.36180927085024 + -61.53663246666227, + -61.636381591712215, + -65.62140779995624, + -65.27264016289558 ], "moduleForcesY": [ - -34.39454115337558, - -34.13615632978172, - -26.64149301034084, - -27.222875461504188 + -33.09645471160549, + -32.94683673534328, + -24.002173782199, + -24.982303085352363 ], - "timestamp": 8.618028041561788 + "timestamp": 8.47854643548306 }, { - "x": 8.180111081085983, - "y": 2.4261456194668596, - "heading": 0.4685113229121613, - "angularVelocity": -0.09252491673727926, - "velocityX": 2.0413593466641706, - "velocityY": 1.1919159688311711, + "x": 8.181425593296224, + "y": 2.421820395971198, + "heading": 0.4645721903392223, + "angularVelocity": -0.13100059228021374, + "velocityX": 2.0405389430894125, + "velocityY": 1.197525508466805, "moduleForcesX": [ - -59.113865720735134, - -59.466549474512234, - -62.90136023448186, - -62.859497948105854 + -59.837069189363575, + -60.1347503268217, + -63.62558289863267, + -63.52420699677533 ], "moduleForcesY": [ - -37.28394866924835, - -36.74169723968548, - -30.46123969045995, - -30.575354363117626 + -36.12028649557102, + -35.64612522470202, + -28.92915290640471, + -29.180149543959885 ], - "timestamp": 8.679569412037887 + "timestamp": 8.540060412343458 }, { - "x": 8.293510268919242, - "y": 2.4921279456843495, - "heading": 0.4602987892888278, - "angularVelocity": -0.1334473632907311, - "velocityX": 1.842649699803164, - "velocityY": 1.0721621196771174, + "x": 8.294623187946847, + "y": 2.488295400127025, + "heading": 0.4541517988040101, + "angularVelocity": -0.16939876215222985, + "velocityX": 1.8401930817693604, + "velocityY": 1.0806487817669281, "moduleForcesX": [ - -57.78992161833766, - -58.284723665890425, - -61.51620046633014, - -61.63027709798539 + -58.495272320583396, + -58.9108341498489, + -61.90125817659251, + -61.99096011956313 ], "moduleForcesY": [ - -39.335088546545315, - -38.615438325466826, - -33.20466598872263, - -33.01319912531834 + -38.28430915682556, + -37.65794710311188, + -32.488649405293295, + -32.336456191223455 ], - "timestamp": 8.741110782513987 + "timestamp": 8.601574389203854 }, { - "x": 8.39489265808716, - "y": 2.5503910578782976, - "heading": 0.4497269700993511, - "angularVelocity": -0.17178394156143129, - "velocityX": 1.6473859516549267, - "velocityY": 0.946730820961736, + "x": 8.395745589032531, + "y": 2.5471579028047797, + "heading": 0.4417334392214656, + "angularVelocity": -0.20187866589616352, + "velocityX": 1.6438930832772425, + "velocityY": 0.9568963946411755, "moduleForcesX": [ - -56.73532643582359, - -57.3433644167481, - -60.37384118279297, - -60.62016736060447 + -57.41823039591145, + -57.901215370721474, + -60.440306372625834, + -60.66569970555321 ], "moduleForcesY": [ - -40.86369626897553, - -40.01966277609491, - -35.26442066300551, - -34.85553891808558 + -39.90297584100897, + -39.210477609648265, + -35.155958840161816, + -34.77905570626861 ], - "timestamp": 8.802652152990087 + "timestamp": 8.663088366064251 }, { - "x": 8.484432685070649, - "y": 2.600663620222655, - "heading": 0.43692255880634423, - "angularVelocity": -0.20806184837855615, - "velocityX": 1.4549566623359964, - "velocityY": 0.8168905234874689, + "x": 8.48500235236776, + "y": 2.598076660395606, + "heading": 0.4276083189600344, + "angularVelocity": -0.2296245663564786, + "velocityX": 1.4509997221898334, + "velocityY": 0.8277591563683715, "moduleForcesX": [ - -55.87704936399228, - -56.580812311815635, - -59.42051911915581, - -59.78199094341051 + -56.53862920087126, + -57.05702405798682, + -59.20466714974995, + -59.52214725960545 ], "moduleForcesY": [ - -42.04664160695578, - -41.10591420167531, - -36.867255229715404, - -36.29138091153048 + -41.15627446217204, + -40.443127482255576, + -37.21736934315343, + -36.717258988355645 ], - "timestamp": 8.864193523466186 + "timestamp": 8.724602342924648 }, { - "x": 8.56227596922161, - "y": 2.6427290793082396, - "heading": 0.42198862978658847, - "angularVelocity": -0.2426648757449358, - "velocityX": 1.2648935756345077, - "velocityY": 0.6835313994497664, + "x": 8.562571509547876, + "y": 2.640785806675407, + "heading": 0.41201408826118213, + "angularVelocity": -0.2535071132572452, + "velocityX": 1.2610005260455623, + "velocityY": 0.6942998723156388, "moduleForcesX": [ - -55.16507312556064, - -55.95401657048999, - -58.61387114486394, - -59.078804118808506 + -55.80869308264709, + -56.34173719772094, + -58.15451187186907, + -58.53176769808956 ], "moduleForcesY": [ - -42.989859151973775, - -41.96722249696166, - -38.151657125904684, - -37.43867247748044 + -42.15377542537939, + -41.44512997463475, + -38.852027537364954, + -38.28860744687602 ], - "timestamp": 8.925734893942286 + "timestamp": 8.786116319785044 }, { - "x": 8.628545730522124, - "y": 2.6764107009683005, - "heading": 0.405010585221208, - "angularVelocity": -0.27588018326589114, - "velocityX": 1.0768327189959002, - "velocityY": 0.5473004809527537, + "x": 8.62860576010961, + "y": 2.6750675306808085, + "heading": 0.3951480099492571, + "angularVelocity": -0.2741828633548061, + "velocityX": 1.0734836850427496, + "velocityY": 0.557299751293951, "moduleForcesX": [ - -54.56456662161311, - -55.43245939238377, - -57.921816727762916, - -58.48249206819515 + -55.19425623129184, + -55.728131941039244, + -57.25555323504394, + -57.66899798719165 ], "moduleForcesY": [ - -43.76032221451713, - -42.663599201968246, - -39.206371128583605, - -38.37439371125214 + -42.96566364835716, + -42.27592065616304, + -40.176385790672825, + -39.58618560604843 ], - "timestamp": 8.987276264418385 + "timestamp": 8.847630296645441 }, { - "x": 8.683347508309266, - "y": 2.7015615497885435, - "heading": 0.3860602892164142, - "angularVelocity": -0.30792775426009245, - "velocityX": 0.8904867955195352, - "velocityY": 0.4086819748352914, + "x": 8.683237297728128, + "y": 2.7007402779978236, + "heading": 0.3771761995999053, + "angularVelocity": -0.2921581609028107, + "velocityX": 0.8881158462978955, + "velocityY": 0.41734819674036067, "moduleForcesX": [ - -54.05076426253945, - -54.994086710555564, - -57.320209102597595, - -57.971713485412614 + -54.67049432361609, + -55.195762968597755, + -56.480010214696975, + -56.91239458884492 ], "moduleForcesY": [ - -44.402281739369485, - -43.23537505493689, - -40.090633053222774, - -39.150748009158825 + -43.638836888514874, + -42.97645664054756, + -41.268832050266276, + -40.67476273388533 ], - "timestamp": 9.048817634894485 + "timestamp": 8.909144273505838 }, { - "x": 8.72677268008055, - "y": 2.7180575426494102, - "heading": 0.36519902233774076, - "angularVelocity": -0.3389795631342851, - "velocityX": 0.7056256861902239, - "velocityY": 0.26804721333388104, + "x": 8.726581564114948, + "y": 2.717650467677025, + "heading": 0.35824027480182863, + "angularVelocity": -0.30783125664352257, + "velocityX": 0.7046246820488491, + "velocityY": 0.2748999583879684, "moduleForcesX": [ - -53.60568984052227, - -54.62264590972197, - -56.790761924991, - -57.530148641518124 + -54.21904272949312, + -54.729099389545915, + -55.805815985531794, + -56.244420654225245 ], "moduleForcesY": [ - -44.94605887665676, - -43.71059919852908, - -40.84537555251459, - -39.80431908016251 + -44.20576368971399, + -43.57579607050465, + -42.183797695943305, + -41.60055887427103 ], - "timestamp": 9.110359005370585 + "timestamp": 8.970658250366235 }, { - "x": 8.758901125853505, - "y": 2.725792489640272, - "heading": 0.34247964491820077, - "angularVelocity": -0.36917243219930057, - "velocityX": 0.522062565789479, - "velocityY": 0.1256869473497624, + "x": 8.758740183629461, + "y": 2.7256665220403926, + "heading": 0.338462250167195, + "angularVelocity": -0.32152082573882035, + "velocityX": 0.5227855709523735, + "velocityY": 0.13031273171557645, "moduleForcesX": [ - -53.2160444557517, - -54.305935154073225, - -56.31947803489789, - -57.14517521142788 + -53.82607120430835, + -54.316194914018055, + -55.21550784813393, + -55.65085738495609 ], "moduleForcesY": [ - -45.4130693587794, - -44.10935378815243, - -41.49973296414931, - -40.36148436994362 + -44.689609283680795, + -44.09507977862367, + -42.960129353038816, + -42.39731433858149 ], - "timestamp": 9.171900375846684 + "timestamp": 9.032172227226631 }, { "x": 8.779803276062012, "y": 2.7246744632720947, "heading": 0.31794821481648144, - "angularVelocity": -0.398616896437924, - "velocityX": 0.33964388584139615, - "velocityY": -0.018167069721194994, + "angularVelocity": -0.33348576043569805, + "velocityX": 0.34241148934902593, + "velocityY": -0.016127371679270322, "moduleForcesX": [ - -52.8718209998866, - -54.034634846892985, - -55.89551210582599, - -56.80690426412013 + -53.48098782939392, + -53.947745103828254, + -54.695219377692204, + -55.12018458029143 ], "moduleForcesY": [ - -45.818828916724534, - -44.44637942204098, - -42.074976138022336, - -40.84174218658695 + -45.107332847724315, + -44.55002787941316, + -43.626228500899906, + -43.09016461444635 ], - "timestamp": 9.233441746322784 + "timestamp": 9.093686204087028 }, { - "x": 8.789008750560857, - "y": 2.7112787087692207, - "heading": 0.2870074693343456, - "angularVelocity": -0.4321051934641618, - "velocityX": 0.12855971235566702, - "velocityY": -0.18707936737997807, + "x": 8.789390520920405, + "y": 2.711156481525927, + "heading": 0.29317696313872205, + "angularVelocity": -0.3456625720749201, + "velocityX": 0.13378216652004424, + "velocityY": -0.1886323872700164, "moduleForcesX": [ - -52.566704392180235, - -53.81516704620926, - -55.506512855841905, - -56.51638685989295 + -53.179448380447525, + -53.62389224585945, + -54.2363017447046, + -54.649105974722524 ], "moduleForcesY": [ - -46.18319326982662, - -44.72589340151746, - -42.6029828956687, - -41.258265442390176 + -45.47706110036372, + -44.95351001122035, + -44.210687438028955, + -43.700838292783004 ], - "timestamp": 9.305046406368838 + "timestamp": 9.165349304691366 }, { - "x": 8.782890992002454, - "y": 2.6860530746256153, - "heading": 0.25377121085635285, - "angularVelocity": -0.46416334434959966, - "velocityX": -0.08543799460076026, - "velocityY": -0.3522903974040376, + "x": 8.783794368286188, + "y": 2.6855639318912203, + "heading": 0.267539420337893, + "angularVelocity": -0.35775095669356133, + "velocityX": -0.07808973637788985, + "velocityY": -0.35712311383241235, "moduleForcesX": [ - -53.439272111965224, - -54.64572763634496, - -56.1717249078838, - -57.1626240496705 + -54.03803305894964, + -54.47570349158013, + -55.06023489691758, + -55.46707669488016 ], "moduleForcesY": [ - -45.16660023903551, - -43.7030817122355, - -41.717468923960176, - -40.353553056706815 + -44.44927461362763, + -43.91326452339743, + -43.17589421817186, + -42.65354228524133 ], - "timestamp": 9.376651066414892 + "timestamp": 9.237012405295705 }, { - "x": 8.761217284811817, - "y": 2.6493049073601336, - "heading": 0.2183564997566526, - "angularVelocity": -0.49458668020939406, - "velocityX": -0.302685707560046, - "velocityY": -0.5132091576420572, + "x": 8.76275609467282, + "y": 2.6482308380775303, + "heading": 0.2410424727336723, + "angularVelocity": -0.3697432483491591, + "velocityX": -0.2935719140806326, + "velocityY": -0.5209528125193889, "moduleForcesX": [ - -54.41587486021302, - -55.56438232577154, - -56.92221968880065, - -57.87959109885894 + -54.99526164627351, + -55.42219111728091, + -55.979540705697985, + -56.37649095420048 ], "moduleForcesY": [ - -43.98034800365324, - -42.52410083429765, - -40.68222445745944, - -39.312900897281004 + -43.254451016653434, + -42.70766979489076, + -41.972052319967815, + -41.439058648655944 ], - "timestamp": 9.448255726460946 + "timestamp": 9.308675505900043 }, { - "x": 8.723726927916339, - "y": 2.601394409233509, - "heading": 0.1808983892167063, - "angularVelocity": -0.5231239212064431, - "velocityX": -0.5235742599904201, - "velocityY": -0.6690974874513724, + "x": 8.725986587983398, + "y": 2.5995490914917334, + "heading": 0.21369371175484705, + "angularVelocity": -0.38162960782036737, + "velocityX": -0.5130884148096344, + "velocityY": -0.679313986909043, "moduleForcesX": [ - -55.513204951308495, - -56.58432740025471, - -57.772952193024096, - -58.67869063733703 + -56.06650028997491, + -56.47788343064675, + -57.008812567039705, + -57.391121229420555 ], "moduleForcesY": [ - -42.58123461820781, - -41.151361111760814, - -39.45859689985431, - -38.10349107560392 + -41.850827206177, + -41.29576254132913, + -40.5570050084547, + -40.01607334967796 ], - "timestamp": 9.519860386507 + "timestamp": 9.380338606504381 }, { - "x": 8.670126749763783, - "y": 2.5427487589796156, - "heading": 0.14155435817055417, - "angularVelocity": -0.5494618788895446, - "velocityX": -0.748557120696901, - "velocityY": -0.8190200221071341, + "x": 8.673161820665543, + "y": 2.5399838352697053, + "heading": 0.1855016713653994, + "angularVelocity": -0.3933968827988571, + "velocityX": -0.7371264552102906, + "velocityY": -0.8311844689904736, "moduleForcesX": [ - -56.750175428910545, - -57.721068226165364, - -58.740871201237205, - -59.57336306357096 + -57.26911890755975, + -57.659157229516524, + -58.16431071641967, + -58.52626643425478 ], "moduleForcesY": [ - -40.9113473334207, - -39.5340460978986, - -37.9953221829711, - -36.68127355062254 + -40.18252449430719, + -39.62280351144781, + -38.874499091161304, + -38.32952205223615 ], - "timestamp": 9.591465046553054 + "timestamp": 9.45200170710872 }, { - "x": 8.600086131198294, - "y": 2.4738813585896815, - "heading": 0.10051029066858429, - "angularVelocity": -0.5732038595752198, - "velocityX": -0.9781572668656089, - "velocityY": -0.9617726045433423, + "x": 8.603918051131696, + "y": 2.4700943215420597, + "heading": 0.156476193662526, + "angularVelocity": -0.4050268193547327, + "velocityX": -0.9662402121860534, + "velocityY": -0.9752510446556707, "moduleForcesX": [ - -58.14728541567019, - -58.992253649512634, - -59.84438084571835, - -60.57897527375518 + -58.62178890554708, + -58.98353550726006, + -59.46309693106811, + -59.79789639516721 ], "moduleForcesY": [ - -38.8916920572076, - -37.60212331585296, - -36.2234088942802, - -34.98615199145863 + -38.17360914029891, + -37.61450756539028, + -36.848345847349954, + -36.304918084781264 ], - "timestamp": 9.663069706599108 + "timestamp": 9.523664807713057 }, { - "x": 8.513231926199088, - "y": 2.395418529280188, - "heading": 0.0579886645280164, - "angularVelocity": -0.5938388103961255, - "velocityX": -1.2129686104695387, - "velocityY": -1.09577825324536, + "x": 8.517847357752716, + "y": 2.3905626117140066, + "heading": 0.12662900025486085, + "angularVelocity": -0.41649319044197497, + "velocityX": -1.2010461821096667, + "velocityY": -1.1098000108474018, "moduleForcesX": [ - -59.72456510756159, - -60.41656271939983, - -61.10169314907387, - -61.71200087220666 + -60.14239359593657, + -60.46764931194161, + -60.920705975153766, + -61.22039017159285 ], "moduleForcesY": [ - -36.41263890584794, - -35.257097835848604, - -34.04864509744792, - -32.934708796771055 + -35.71931021724835, + -35.16851680517709, + -34.373858499651185, + -33.84003668835228 ], - "timestamp": 9.734674366645162 + "timestamp": 9.595327908317396 }, { - "x": 8.409144338572357, - "y": 2.3081371726052624, - "heading": 0.014259918978119247, - "angularVelocity": -0.6106969228227869, - "velocityX": -1.4536426478358366, - "velocityY": -1.2189340277404956, + "x": 8.414494950035653, + "y": 2.302233586652294, + "heading": 0.09597460073164957, + "angularVelocity": -0.4277570920697149, + "velocityX": -1.4421983816704484, + "velocityY": -1.2325593550492429, "moduleForcesX": [ - -61.49629524842574, - -62.01024906206618, - -62.5267777803112, - -62.98751725355034 + -61.84288712238317, + -62.122276292758905, + -62.54567545312701, + -62.801268264067 ], "moduleForcesY": [ - -33.31963140107074, - -32.35756349250855, - -31.340658297933924, - -30.409079395346843 + -32.67310530550032, + -32.14185463603263, + -31.305402017642134, + -30.792810837453125 ], - "timestamp": 9.806279026691216 + "timestamp": 9.666991008921734 }, { - "x": 8.287356319734036, - "y": 2.21301866214228, - "heading": -0.030341619606405787, - "angularVelocity": -0.6228859763573827, - "velocityX": -1.700839285599414, - "velocityY": -1.3283843593671696, + "x": 8.2933614907194, + "y": 2.2061711843005387, + "heading": 0.06453176165426741, + "angularVelocity": -0.4387591216710293, + "velocityX": -1.690318424610812, + "velocityY": -1.340472314784823, "moduleForcesX": [ - -63.458562590492434, - -63.778062663565834, - -64.12020804800913, - -64.4128163560726 + -63.717577250904, + -63.94109844594106, + -64.32744860294157, + -64.52959830151751 ], "moduleForcesY": [ - -29.392355415191965, - -28.696611629664407, - -27.917314517688634, - -27.239890506735552 + -28.828283743204985, + -28.332961805960878, + -27.43891241104092, + -26.96427075977155 ], - "timestamp": 9.87788368673727 + "timestamp": 9.738654109526072 }, { - "x": 8.147362398954167, - "y": 2.111326488188939, - "heading": -0.07539545756391505, - "angularVelocity": -0.6292025955926854, - "velocityX": -1.955095110986199, - "velocityY": -1.4201893268948602, + "x": 8.153917386350976, + "y": 2.103737230084854, + "heading": 0.032325911066374, + "angularVelocity": -0.4494063237049454, + "velocityX": -1.9458285113605962, + "velocityY": -1.4293821136938516, "moduleForcesX": [ - -65.56127142602155, - -65.69084909052835, - -65.8493885566579, - -65.97214747432078 + -65.71756976574333, + -65.87625105005283, + -66.21063177131644, + -66.35137198001577 ], "moduleForcesY": [ - -24.316794034856503, - -23.9674759446695, - -23.52399917574761, - -23.180580992000287 + -23.893867912535846, + -23.458019215236998, + -22.489715553229402, + -22.076652060585157 ], - "timestamp": 9.949488346783324 + "timestamp": 9.81031721013041 }, { - "x": 7.988650071662026, - "y": 2.0047168985365014, - "heading": -0.12036455636704498, - "angularVelocity": -0.6280191648728856, - "velocityX": -2.216508355602279, - "velocityY": -1.4888638474628435, + "x": 7.995643377180051, + "y": 1.996698976059294, + "heading": -0.0006069291037716975, + "angularVelocity": -0.45955086917006693, + "velocityX": -2.2085844435447637, + "velocityY": -1.4936313545311521, "moduleForcesX": [ - -67.64804942069694, - -67.63218085033044, - -67.60757677396883, - -67.59149476249789 + -67.69752144856352, + -67.78734757334065, + -68.04263214008894, + -68.11934773122928 ], "moduleForcesY": [ - -17.65673640990495, - -17.716555475855195, - -17.811425350367866, - -17.871500295896368 + -17.47107619907958, + -17.127002103641317, + -16.07246012795104, + -15.752714784412063 ], - "timestamp": 10.021093006829378 + "timestamp": 9.881980310734749 }, { - "x": 7.810781225665186, - "y": 1.8953882630351646, - "heading": -0.1645573135688971, - "angularVelocity": -0.6171771107275482, - "velocityX": -2.4840400873691744, - "velocityY": -1.5268368766923714, + "x": 7.818124590832862, + "y": 1.8873638999769546, + "heading": -0.034213912395681174, + "angularVelocity": -0.468957985469511, + "velocityX": -2.4771295806372327, + "velocityY": -1.5256816291830964, "moduleForcesX": [ - -69.33792438793783, - -69.27727776528677, - -69.1346031312489, - -69.06017856287379 + -69.313687888199, + -69.3432482954395, + -69.47611214375348, + -69.49889577515765 ], "moduleForcesY": [ - -8.85711879468359, - -9.301819468635278, - -10.32983168125111, - -10.801199653575688 + -9.057792824867414, + -8.844692568248144, + -7.710678951592893, + -7.521600845904924 ], - "timestamp": 10.092697666875432 + "timestamp": 9.953643411339087 }, { - "x": 7.613573483725926, - "y": 1.7862509730258411, - "heading": -0.20709141412342455, - "angularVelocity": -0.5940130227162668, - "velocityX": -2.754118821490405, - "velocityY": -1.5241646275414062, + "x": 7.621240028444532, + "y": 1.7787112968796408, + "heading": -0.06841646549100246, + "angularVelocity": -0.4772686753278808, + "velocityX": -2.747363157998911, + "velocityY": -1.5161582764496822, "moduleForcesX": [ - -69.8380909495494, - -69.85117254072334, - -69.88578385231288, - -69.87027129628852 + -69.86517241058907, + -69.86591053003337, + -69.82282638095482, + -69.8236070877981 ], "moduleForcesY": [ - 2.6196094231982188, - 2.050557264342375, - -0.5954925448843501, - -1.3097488777935367 + 1.837312109452161, + 1.8863774422298984, + 3.0464717958262337, + 3.075434675486523 ], - "timestamp": 10.164302326921486 + "timestamp": 10.025306511943425 }, { - "x": 7.3974393349223595, - "y": 1.6810231576681, - "heading": -0.24689444053872592, - "angularVelocity": -0.5558720115380904, - "velocityX": -3.0184369098960024, - "velocityY": -1.469566579745805, + "x": 7.40547828858971, + "y": 1.6744202309428289, + "heading": -0.10310081581402811, + "angularVelocity": -0.48399176187648607, + "velocityX": -3.0107787415740312, + "velocityY": -1.4552965899789445, "moduleForcesX": [ - -67.81774479937387, - -67.81197181841603, - -68.89677208175078, - -68.95839961219598 + -68.17759957909404, + -68.20364257441437, + -67.95854331471489, + -67.98903555111266 ], "moduleForcesY": [ - 16.827330110890028, - 16.80465353313287, - 11.649075089581624, - 11.210500710885821 + 15.314544404106083, + 15.206518112264616, + 16.260066564096665, + 16.13994452069931 ], - "timestamp": 10.23590698696754 + "timestamp": 10.096969612547763 }, { - "x": 7.163840114735354, - "y": 1.5840259794682252, - "heading": -0.2828186940424167, - "angularVelocity": -0.5017027310874074, - "velocityX": -3.2623466131500303, - "velocityY": -1.3546210279818205, + "x": 7.172300767498466, + "y": 1.5786130838476118, + "heading": -0.13811594024177143, + "angularVelocity": -0.48860744417223123, + "velocityX": -3.253801735130716, + "velocityY": -1.3369104362952573, "moduleForcesX": [ - -61.83333462479733, - -61.05938989064099, - -64.89350871525448, - -64.5824879376315 + -62.90664616032858, + -62.994759663175806, + -62.62640903247994, + -62.71838864361929 ], "moduleForcesY": [ - 32.52596953901682, - 33.92307848948774, - 25.88066947280988, - 26.602254108081837 + 30.40188460343236, + 30.221851782690536, + 30.976021760761594, + 30.79224774785525 ], - "timestamp": 10.307511647013595 + "timestamp": 10.168632713152101 }, { - "x": 6.91551546736203, - "y": 1.4994886743390452, - "heading": -0.3139208387202008, - "angularVelocity": -0.4343592254719174, - "velocityX": -3.467995619469574, - "velocityY": -1.180611779663611, + "x": 6.924263851753144, + "y": 1.4952348436632368, + "heading": -0.17328998021723432, + "angularVelocity": -0.4908249807619052, + "velocityX": -3.461152443218535, + "velocityY": -1.1634751982713893, "moduleForcesX": [ - -51.68933538210916, - -48.85035357028174, - -57.056547684318936, - -55.184870641943775 + -53.63079242417267, + -53.732278169292705, + -53.450679135245664, + -53.553119173625326 ], "moduleForcesY": [ - 47.00644705748203, - 49.9244819592894, - 40.31096815949663, - 42.80215553528719 + 44.77877441586359, + 44.65783761257958, + 44.9941386909519, + 44.87304464918282 ], - "timestamp": 10.379116307059649 + "timestamp": 10.24029581375644 }, { - "x": 6.656064858498082, - "y": 1.43069889782314, - "heading": -0.3397354689772922, - "angularVelocity": -0.3605160647433842, - "velocityX": -3.6233760302342883, - "velocityY": -0.9606885427800527, + "x": 6.664645466021959, + "y": 1.4273879906699989, + "heading": -0.20846188348487105, + "angularVelocity": -0.49079516475048257, + "velocityX": -3.622762391548932, + "velocityY": -0.9467473835360443, "moduleForcesX": [ - -39.29968551732011, - -33.85969689804127, - -46.079653739591876, - -41.530113395690364 + -41.769412633151376, + -41.76713542840772, + -41.77205019669556, + -41.769773098851616 ], "moduleForcesY": [ - 57.77680095878208, - 61.10531694531754, - 52.513961690389884, - 56.154301989543136 + 56.01545952386056, + 56.01714957737264, + 56.01348552259297, + 56.01517566218937 ], - "timestamp": 10.450720967105703 + "timestamp": 10.311958914360778 }, { - "x": 6.389094964926698, - "y": 1.3796716923906485, - "heading": -0.36043754169192455, - "angularVelocity": -0.2891162768082043, - "velocityX": -3.728387138486177, - "velocityY": -0.7126240861931642, + "x": 6.396804785667352, + "y": 1.37708752891827, + "heading": -0.24350598675754967, + "angularVelocity": -0.4890118202694272, + "velocityX": -3.7374977930886835, + "velocityY": -0.7019018341035008, "moduleForcesX": [ - -27.32133306734179, - -20.310347551724792, - -33.900176274380996, - -27.121135359167035 + -29.670078278170966, + -29.48805559061184, + -29.820795736138948, + -29.638793031028708 ], "moduleForcesY": [ - 64.32684835868893, - 66.85977151354284, - 61.10135000376463, - 64.37959537846069 + 63.27577283382489, + 63.36048200304406, + 63.204474407441516, + 63.28970026112863 ], - "timestamp": 10.522325627151757 + "timestamp": 10.383622014965116 }, { - "x": 6.117512662620233, - "y": 1.34740277403393, - "heading": -0.37733465959682994, - "angularVelocity": -0.23597790833777565, - "velocityX": -3.7928020624885344, - "velocityY": -0.45065388671580686, + "x": 6.123724098762273, + "y": 1.3454463182894645, + "heading": -0.27833593889663377, + "angularVelocity": -0.4860235162218966, + "velocityX": -3.8106178019395767, + "velocityY": -0.4415272345457245, "moduleForcesX": [ - -17.461162262072158, - -11.425931826783211, - -21.98887484573413, - -15.772925216616537 + -18.964618645051026, + -18.60764038496825, + -19.189884865939113, + -18.832039003279412 ], "moduleForcesY": [ - 67.68404989930426, - 68.95306382811246, - 66.34075545396243, - 68.07770320581683 + 67.27850831701049, + 67.37773513616717, + 67.21397107913663, + 67.3147241815706 ], - "timestamp": 10.59393028719781 + "timestamp": 10.455285115569454 }, { - "x": 5.8435235121250875, - "y": 1.3343250077166011, - "heading": -0.3920716678293369, - "angularVelocity": -0.205810742248178, - "velocityX": -3.8264150729704207, - "velocityY": -0.18263848063684318, + "x": 5.847858828640413, + "y": 1.3329886237876336, + "heading": -0.3128943538499734, + "angularVelocity": -0.4822344367171772, + "velocityX": -3.849474384941141, + "velocityY": -0.17383694532855082, "moduleForcesX": [ - -9.461901729986893, - -5.721136386114701, - -11.732782343904876, - -7.862920839581364 + -10.164815803497616, + -9.673316441913729, + -10.413603531769702, + -9.919643964436528 ], "moduleForcesY": [ - 69.26711577967508, - 69.6728315865383, - 68.91304347288967, - 69.45744160036715 + 69.17032970505136, + 69.24042697051493, + 69.13258323678724, + 69.20483068344068 ], - "timestamp": 10.665534947243865 + "timestamp": 10.526948216173793 }, { - "x": 5.568929040137215, - "y": 1.3405893640009074, - "heading": -0.4055914386320038, - "angularVelocity": -0.18881132588258015, - "velocityX": -3.8348687335609193, - "velocityY": 0.08748531562439549, + "x": 5.571170150686736, + "y": 1.339901067236654, + "heading": -0.3471395996450313, + "angularVelocity": -0.4778644170607483, + "velocityX": -3.8609643682780583, + "velocityY": 0.09645749891823462, "moduleForcesX": [ - -2.762826914760814, - -0.5336705163664741, - -3.868314495467215, - -1.5820295641402107 + -3.140660151426149, + -2.5499567673882018, + -3.391519077751636, + -2.7966358407203566 ], "moduleForcesY": [ - 69.8660118858112, - 69.91723429353682, - 69.81049649840442, - 69.89820089852721 + 69.85390969745158, + 69.87766236501122, + 69.84141674708727, + 69.86745404796969 ], - "timestamp": 10.737139607289919 + "timestamp": 10.59861131677813 }, { "x": 5.295215606689453, "y": 1.3661898374557495, - "heading": -0.418290619060492, - "angularVelocity": -0.17735131233526324, - "velocityX": -3.8225645268299044, - "velocityY": 0.35752524260818264, + "heading": -0.3810366506112695, + "angularVelocity": -0.4730056427977949, + "velocityX": -3.850720128910756, + "velocityY": 0.3668383030792834, "moduleForcesX": [ - 2.720138806867678, - 4.271594537524151, - 2.07835169845561, - 3.6608431501275804 + 2.4358890124890213, + 3.105036958141336, + 2.1874776648542436, + 2.8624717837810634 ], "moduleForcesY": [ - 69.87725025164747, - 69.79888184516116, - 69.89739500886515, - 69.83163852773065 + 69.89157339518185, + 69.86477674457022, + 69.89901845264843, + 69.87435635433927 ], - "timestamp": 10.808744267335973 + "timestamp": 10.670274417382469 }, { - "x": 5.055367481611433, - "y": 1.4037520138273964, - "heading": -0.42898005555429164, - "angularVelocity": -0.16921534609514086, - "velocityX": -3.79683096661731, - "velocityY": 0.5946147562129904, + "x": 5.053706815736057, + "y": 1.4043054270449447, + "heading": -0.4105962505484791, + "angularVelocity": -0.46833496540702335, + "velocityX": -3.8264053470585537, + "velocityY": 0.6038939420574941, "moduleForcesX": [ - 7.128107876197704, - 8.397495650017172, - 6.681822586978301, - 7.973498881488917 + 6.887915896080987, + 7.623701240859441, + 6.643290267386372, + 7.386549953753599 ], "moduleForcesY": [ - 69.56442888170623, - 69.42212415607871, - 69.60712711861942, - 69.47048072990539 + 69.5924526319861, + 69.51542701820333, + 69.6153579444215, + 69.54014360764195 ], - "timestamp": 10.871914876506311 + "timestamp": 10.733390780926925 }, { - "x": 4.817922325323769, - "y": 1.4561844229991425, - "heading": -0.4391809724358013, - "angularVelocity": -0.16148200904637555, - "velocityX": -3.7587916185420474, - "velocityY": 0.8300127204783294, + "x": 4.814521797126242, + "y": 1.457279005501737, + "heading": -0.43977801159576735, + "angularVelocity": -0.46234857980584404, + "velocityX": -3.789588074752571, + "velocityY": 0.8393002302720002, "moduleForcesX": [ - 10.725498733701588, - 11.940280266617222, - 10.355733338830216, - 11.59193024794521 + 10.475140797961476, + 11.417395555766568, + 10.184857658397029, + 11.139880728378749 ], "moduleForcesY": [ - 69.09197337769128, - 68.89195807698471, - 69.14665294562548, - 68.94968761414044 + 69.13513408837922, + 68.98542996372927, + 69.17724110186298, + 69.02951613308481 ], - "timestamp": 10.93508548567665 + "timestamp": 10.79650714447138 }, { - "x": 4.583800693836467, - "y": 1.523306626333883, - "heading": -0.44902694770580176, - "angularVelocity": -0.15586323132409483, - "velocityX": -3.7061797339328746, - "velocityY": 1.0625543146773644, + "x": 4.578623056803542, + "y": 1.5249252632965482, + "heading": -0.46848135774782446, + "angularVelocity": -0.4547686929371364, + "velocityX": -3.7375210971485733, + "velocityY": 1.071770520289322, "moduleForcesX": [ - 15.088104493088409, - 15.974726148554316, - 14.871545105185097, - 15.770080616776701 + 14.840754027841374, + 16.029076302383867, + 14.51917987513575, + 15.728849295978776 ], "moduleForcesY": [ - 68.26065337301016, - 68.05830212763824, - 68.30673212222058, - 68.10456636002334 + 68.32109565083954, + 68.05166292944023, + 68.38835125483823, + 68.11984422324372 ], - "timestamp": 10.998256094846989 + "timestamp": 10.859623508015837 }, { - "x": 4.354069654505351, - "y": 1.6048331009047445, - "heading": -0.4589311264045249, - "angularVelocity": -0.15678460012973233, - "velocityX": -3.636676016716078, - "velocityY": 1.2905760391043033, + "x": 4.347193447367128, + "y": 1.6069250977069311, + "heading": -0.4965850522073392, + "angularVelocity": -0.4452679603399553, + "velocityX": -3.6667132965195126, + "velocityY": 1.299185025966005, "moduleForcesX": [ - 20.43945052990814, - 20.29380047894279, - 20.463831834196373, - 20.318512311359303 + 20.193590004699995, + 21.665646251529257, + 19.875228316396825, + 21.38196372017987 ], "moduleForcesY": [ - 66.83870936216607, - 66.88312038765154, - 66.83152320893446, - 66.87589139118117 + 66.92237625745865, + 66.45990723727093, + 67.0149915440133, + 66.54903999850195 ], - "timestamp": 11.061426704017327 + "timestamp": 10.922739871560292 }, { - "x": 4.129951329960104, - "y": 1.7003267532429973, - "heading": -0.46978399762811174, - "angularVelocity": -0.17180254181691199, - "velocityX": -3.5478259191852364, - "velocityY": 1.5116785098708787, + "x": 4.121687707159994, + "y": 1.7027378890121, + "heading": -0.5239423865994234, + "angularVelocity": -0.4334428166606177, + "velocityX": -3.572856982615943, + "velocityY": 1.5180340869556606, "moduleForcesX": [ - 27.12931598295055, - 24.762739453054976, - 27.29639832285452, - 25.017031498332177 + 26.76366004990429, + 28.54407768472358, + 26.51477620428529, + 28.34898574175057 ], "moduleForcesY": [ - 64.39426565175329, - 65.34133562291419, - 64.32896211579789, - 65.24961743862173 + 64.56136029572545, + 63.79374619278623, + 64.66009920375362, + 63.876720382176266 ], - "timestamp": 11.124597313187666 + "timestamp": 10.985856235104748 }, { - "x": 3.9129272992378854, - "y": 1.8090800592804388, - "heading": -0.48282197066425664, - "angularVelocity": -0.20639302370803844, - "velocityX": -3.435522208389926, - "velocityY": 1.721580770959322, + "x": 3.903869186822254, + "y": 1.8114606574002452, + "heading": -0.5503757290218597, + "angularVelocity": -0.4188033172066106, + "velocityX": -3.4510625787926257, + "velocityY": 1.7225765599053773, "moduleForcesX": [ - 35.59984400154966, - 30.289367075153166, - 35.35330491034905, - 30.469918831002943 + 34.722789021923006, + 36.79190026410112, + 34.65315726916901, + 36.79826370259493 ], "moduleForcesY": [ - 60.10095977176368, - 62.94451217757728, - 60.26121508533313, - 62.87162431394478 + 60.634978039609805, + 59.40218821323202, + 60.6689615218732, + 59.39227430417506 ], - "timestamp": 11.187767922358004 + "timestamp": 11.048972598649204 }, { - "x": 3.7049939746242946, - "y": 1.9297950624975384, - "heading": -0.498750181696931, - "angularVelocity": -0.2521459147199961, - "velocityX": -3.2916149985652714, - "velocityY": 1.910936190144909, + "x": 3.6957796336708695, + "y": 1.9316273259274328, + "heading": -0.5756742900350444, + "angularVelocity": -0.40082412218451013, + "velocityX": -3.296919237193045, + "velocityY": 1.903890873601256, "moduleForcesX": [ - 45.789906461187606, - 39.500956547308036, - 44.60405217605959, - 38.88284700733399 + 43.97155118149319, + 46.20214883747921, + 44.22377698479971, + 46.54083607831509 ], "moduleForcesY": [ - 52.72236020041571, - 57.580980046986554, - 53.75348320494377, - 58.023668192173425 + 54.28134738689487, + 52.39692915304774, + 54.06704722487376, + 52.087132813104375 ], - "timestamp": 11.250938531528343 + "timestamp": 11.11208896219366 }, { - "x": 3.508517364442358, - "y": 2.060224954573662, - "heading": -0.5175036294890961, - "angularVelocity": -0.2968698266245423, - "velocityX": -3.1102535302792567, - "velocityY": 2.064724304374236, + "x": 3.499550666767407, + "y": 2.061002715105782, + "heading": -0.5996083713418497, + "angularVelocity": -0.37920564434843623, + "velocityX": -3.109003052199785, + "velocityY": 2.0497915582101762, "moduleForcesX": [ - 56.310610811243905, - 51.69004210860725, - 54.5613125649118, - 50.14303242636139 + 53.763954059728796, + 55.84492515312586, + 54.42048149959161, + 56.552580490225075 ], "moduleForcesY": [ - 41.26969515333533, - 46.91561591928861, - 43.58578238706644, - 48.59522594128138 + 44.579593956853635, + 41.94760445118944, + 43.76152240959032, + 40.974101674150546 ], - "timestamp": 11.314109140698681 + "timestamp": 11.175205325738116 }, { - "x": 3.3256131531226463, - "y": 2.1971031258571245, - "heading": -0.5385715679309702, - "angularVelocity": -0.3335085527680234, - "velocityX": -2.8954004674311986, - "velocityY": 2.1668015091380983, + "x": 3.3170061238667126, + "y": 2.1965515433699756, + "heading": -0.621969243888579, + "angularVelocity": -0.3542801151872387, + "velocityX": -2.8921904344524014, + "velocityY": 2.1476019949837775, "moduleForcesX": [ - 64.77216448239771, - 62.673602768769214, - 63.38542095359247, - 61.15353995273158 + 62.44766102413855, + 63.94433819362826, + 63.329847528999416, + 64.77963805969534 ], "moduleForcesY": [ - 26.035622904980194, - 30.721312600843035, - 29.28306014997138, - 33.678552232388746 + 31.256641416018002, + 28.0802822128175, + 29.404706704310698, + 26.071343409245944 ], - "timestamp": 11.37727974986902 + "timestamp": 11.238321689282571 }, { - "x": 3.157514306984536, - "y": 2.3366007043510604, - "heading": -0.561190847527418, - "angularVelocity": -0.35806651057385414, - "velocityX": -2.6610293670721896, - "velocityY": 2.208267109120054, + "x": 3.149232244356222, + "y": 2.334780583324695, + "heading": -0.6426179790087161, + "angularVelocity": -0.32715343471259145, + "velocityX": -2.6581677094296006, + "velocityY": 2.1900666038429124, "moduleForcesX": [ - 69.18908773846705, - 68.77071008684689, - 68.72649352066082, - 68.18965051435607 + 68.07446024974723, + 68.73289751434758, + 68.68747354304334, + 69.20841989455995 ], "moduleForcesY": [ - 9.337187838928434, - 11.986015068259727, - 12.328677058413632, - 14.979951486247026 + 15.578092751741488, + 12.393772344765436, + 12.552654231475199, + 9.321777066848862 ], - "timestamp": 11.440450359039358 + "timestamp": 11.301438052827027 }, { - "x": 3.0044268603736923, - "y": 2.4751119466714426, - "heading": -0.5845404524188933, - "angularVelocity": -0.3696276670138402, - "velocityX": -2.4233967128296365, - "velocityY": 2.1926532629579243, + "x": 2.996456707624642, + "y": 2.4723242803752896, + "heading": -0.6615054771208647, + "angularVelocity": -0.2992488326556613, + "velocityX": -2.4205376886767986, + "velocityY": 2.179208200955956, "moduleForcesX": [ - 69.58635593128226, - 69.65294315110864, - 69.70772082110078, - 69.75414910427422 + 69.84418644116603, + 69.8030417146416, + 69.73398928648147, + 69.55639119192205 ], "moduleForcesY": [ - -5.863213664973564, - -4.950604413431918, - -4.219222524167902, - -3.2792456106314267 + 0.04124705741425996, + -2.629684927774557, + -3.7965442820408897, + -6.360953421225059 ], - "timestamp": 11.503620968209697 + "timestamp": 11.364554416371483 }, { - "x": 2.8658808016377564, - "y": 2.609745022810685, - "heading": -0.607898551213732, - "angularVelocity": -0.36976212674875547, - "velocityX": -2.193204411918028, - "velocityY": 2.1312613240155165, + "x": 2.858285394290842, + "y": 2.6063679764517347, + "heading": -0.6786531021168939, + "angularVelocity": -0.2716827147994899, + "velocityX": -2.18915199758744, + "velocityY": 2.1237550541395294, "moduleForcesX": [ - 67.49011979913459, - 67.49197422814143, - 67.49549991286162, - 67.49735359030318 + 68.5832050639289, + 68.18255030614512, + 67.65433881512726, + 67.18772346415568 ], "moduleForcesY": [ - -18.014094750436488, - -18.00696556606115, - -17.99399260732844, - -17.986857779229418 + -13.292687011674952, + -15.252960265172256, + -17.395421555860977, + -19.15158282474659 ], - "timestamp": 11.566791577380036 + "timestamp": 11.427670779915939 }, { - "x": 2.7411339659112834, - "y": 2.738363105075064, - "heading": -0.6306961855706249, - "angularVelocity": -0.36088989256727516, - "velocityX": -1.9747606895810539, - "velocityY": 2.0360430895570842, + "x": 2.734026522437346, + "y": 2.734745646026211, + "heading": -0.6941195885833025, + "angularVelocity": -0.2450471731552614, + "velocityX": -1.9687267275145863, + "velocityY": 2.0339839364169796, "moduleForcesX": [ - 64.39517539155403, - 64.27134897958376, - 63.8241376908432, - 63.70527604163203 + 65.71596180721829, + 65.23236893280917, + 64.13301722958752, + 63.6607820099661 ], "moduleForcesY": [ - -27.117529617984484, - -27.416726077312312, - -28.43391532243881, - -28.706015086897278 + -23.750213140906997, + -25.069078070391868, + -27.734992318593385, + -28.82188410507732 ], - "timestamp": 11.629962186550374 + "timestamp": 11.490787143460395 }, { - "x": 2.629408519345831, - "y": 2.8594244836840406, - "heading": -0.6525029140354126, - "angularVelocity": -0.34520370709084347, - "velocityX": -1.7686301910463869, - "velocityY": 1.9164193633551478, + "x": 2.622913491858143, + "y": 2.8558455619096765, + "heading": -0.7079766418555111, + "angularVelocity": -0.21954771304985882, + "velocityX": -1.7604472808535805, + "velocityY": 1.918677013104065, "moduleForcesX": [ - 61.165285278261656, - 60.992919135287345, - 59.86697681442411, - 59.72951823823286 + 62.3261455799643, + 61.90369851571434, + 60.3053013643231, + 59.94983415338624 ], "moduleForcesY": [ - -33.81189902733295, - -34.13067207401691, - -36.05965076270056, - -36.295299281822466 + -31.626927610118603, + -32.460334406565565, + -35.32513309689565, + -35.9385060308983 ], - "timestamp": 11.693132795720713 + "timestamp": 11.55390350700485 }, { - "x": 2.529987992544686, - "y": 2.971809130496302, - "heading": -0.6729951982943887, - "angularVelocity": -0.3243958626980939, - "velocityX": -1.5738415080509853, - "velocityY": 1.7790654275506226, + "x": 2.5242136717280004, + "y": 2.968472284388882, + "heading": -0.720296269771335, + "angularVelocity": -0.19518912725614584, + "velocityX": -1.5637754551658334, + "velocityY": 1.7844298396544638, "moduleForcesX": [ - 58.16870881285027, - 58.01912021029308, - 56.16470015343222, - 56.10022212287148 + 58.98754783726104, + 58.678775107744755, + 56.70105562415566, + 56.492209664989446 ], "moduleForcesY": [ - -38.76602349429578, - -38.998640161944856, - -41.61590645152988, - -41.71135809779262 + -37.51207015880541, + -38.003843268920306, + -40.88479342874241, + -41.18285690919068 ], - "timestamp": 11.756303404891051 + "timestamp": 11.617019870549306 }, { - "x": 2.4422441011606852, - "y": 3.074687163648499, - "heading": -0.6919282071615437, - "angularVelocity": -0.2997123047539793, - "velocityX": -1.3889986583381029, - "velocityY": 1.6285743402408528, + "x": 2.437268193420317, + "y": 3.0717284892697196, + "heading": -0.7311460366120948, + "angularVelocity": -0.17190101316781217, + "velocityX": -1.377542580482226, + "velocityY": 1.6359656843682122, "moduleForcesX": [ - 55.516392904604864, - 55.43040225099981, - 52.89236318545878, - 52.94888563341702 + 55.933543070193124, + 55.74620404374717, + 53.4983817914488, + 53.427889596621455 ], "moduleForcesY": [ - -42.49669388469859, - -42.617601177326456, - -45.721215937319954, - -45.66397764278443 + -41.94896951930148, + -42.20583852522306, + -45.01283639047294, + -45.10426074033776 ], - "timestamp": 11.81947401406139 + "timestamp": 11.680136234093762 }, { - "x": 2.36563547825889, - "y": 3.1674290736500192, - "heading": -0.709114876775145, - "angularVelocity": -0.2720674984668532, - "velocityX": -1.212725726535598, - "velocityY": 1.4681180254482455, + "x": 2.3614992683800686, + "y": 3.1649283314618284, + "heading": -0.7405881776451042, + "angularVelocity": -0.1495989392094639, + "velocityX": -1.2004640442708914, + "velocityY": 1.4766351696809092, "moduleForcesX": [ - 53.21000778523316, - 53.20542690598686, - 50.063878893766116, - 50.25772899182876 + 53.226119004537075, + 53.150380788764224, + 50.717908435297424, + 50.76595922311749 ], "moduleForcesY": [ - -45.3662355318544, - -45.379935036516486, - -48.8163206207447, - -48.624450752509695 + -45.3497145739425, + -45.44508539739661, + -48.13839282499532, + -48.093987559665536 ], - "timestamp": 11.882644623231728 + "timestamp": 11.743252597638218 }, { - "x": 2.29969733456388, - "y": 3.24954609830814, - "heading": -0.7244111189583969, - "angularVelocity": -0.24214175522680043, - "velocityX": -1.0438104770718435, - "velocityY": 1.2999245335230758, + "x": 2.2964046195038508, + "y": 3.2475371925356646, + "heading": -0.7486801929297746, + "angularVelocity": -0.12820788192226776, + "velocityX": -1.0313434618325024, + "velocityY": 1.3088342932756496, "moduleForcesX": [ - 51.212589224847115, - 51.29435553074885, - 47.635142275536005, - 47.96569255346641 + 50.85429251327673, + 50.87522935086129, + 48.32191532894057, + 48.467633279346366 ], "moduleForcesY": [ - -47.62154135295099, - -47.541251547195635, - -51.20041879059338, - -50.89807366778042 + -48.006099795729824, + -47.9893857552052, + -50.55448655081397, + -50.41998583087466 ], - "timestamp": 11.945815232402067 + "timestamp": 11.806368961182674 }, { - "x": 2.244029529169606, - "y": 3.32065049496387, - "heading": -0.7377053964323423, - "angularVelocity": -0.2104503605165108, - "velocityX": -0.8812295167863048, - "velocityY": 1.1255930184874117, + "x": 2.2415479359062194, + "y": 3.3191302814359016, + "heading": -0.7554758085003378, + "angularVelocity": -0.10766804658789833, + "velocityX": -0.8691356807810039, + "velocityY": 1.1343031328129516, "moduleForcesX": [ - 49.47830306207863, - 49.64543262889328, - 45.54785025385102, - 46.007190229326234 + 48.7813998909101, + 48.883997299800775, + 46.25717342000062, + 46.48203569715231 ], "moduleForcesY": [ - -49.430771057830185, - -49.27017896162637, - -53.075077496144715, - -52.68407579049555 + -50.12061198883146, + -50.02516024483403, + -52.45958599626199, + -52.26481927286433 ], - "timestamp": 12.008985841572406 + "timestamp": 11.86948532472713 }, { - "x": 2.198285645058137, - "y": 3.3804286588611365, - "heading": -0.748911194782175, - "angularVelocity": -0.177389429942277, - "velocityX": -0.7241323886575408, - "velocityY": 0.9462970942084137, + "x": 2.1965490021845606, + "y": 3.379364027011461, + "heading": -0.7610258706649285, + "angularVelocity": -0.08793380754076002, + "velocityX": -0.7129519382079801, + "velocityY": 0.9543285162988591, "moduleForcesX": [ - 47.963765452152025, - 48.213334157004304, - 43.74592423575202, - 44.32418492699512 + 46.96552536511235, + 47.136382750106904, + 44.47147307724585, + 44.76001886720274 ], "moduleForcesY": [ - -50.909740237006666, - -50.68021953336166, - -54.577452261696784, - -54.11506726002671 + -51.83392911935416, + -51.682542993841096, + -53.98932327657365, + -53.75408240792354 ], - "timestamp": 12.072156450742744 + "timestamp": 11.932601688271586 }, { - "x": 2.162163747962268, - "y": 3.42862257199927, - "heading": -0.7579610345845447, - "angularVelocity": -0.14326029020817274, - "velocityX": -0.5718149242231771, - "velocityY": 0.7629167071696146, + "x": 2.16107481240881, + "y": 3.4279560426439364, + "heading": -0.7653786869846049, + "angularVelocity": -0.06896494150222135, + "velocityX": -0.5620442589466436, + "velocityY": 0.7698798362844577, "moduleForcesX": [ - 46.6319076312275, - 46.9611493566907, - 42.180140742346175, - 42.868308982974696 + 45.36776130374439, + 45.59551423238422, + 42.91858789949486, + 43.2583272033556 ], "moduleForcesY": [ - -52.139192200259096, - -51.849151349231114, - -55.802822449668646, - -55.2816673356197 + -53.2445780347107, + -53.0530790830467, + -55.238217704660805, + -54.97579973773786 ], - "timestamp": 12.135327059913083 + "timestamp": 11.995718051816041 }, { - "x": 2.1353988201191023, - "y": 3.4650167267118044, - "heading": -0.7648020671777167, - "angularVelocity": -0.1082945484145205, - "velocityX": -0.4236927298103942, - "velocityY": 0.5761248021908162, + "x": 2.134832009400895, + "y": 3.4646708607022925, + "heading": -0.768580123623318, + "angularVelocity": -0.050722767582422834, + "velocityX": -0.4157844580103423, + "velocityY": 0.5817004655614504, "moduleForcesX": [ - 45.45179812579232, - 45.85909611829803, - 40.80962595285606, - 41.60067510940896 + 43.954428645312426, + 44.2297096749135, + 41.559591533194514, + 41.940634703058834 ], "moduleForcesY": [ - -53.17683717820275, - -52.83187682056265, - -56.8182410705635, - -56.24698678733785 + -54.42275064440647, + -54.20223828129335, + -56.273239100999206, + -55.992648881357695 ], - "timestamp": 12.198497669083421 + "timestamp": 12.058834415360497 }, { - "x": 2.1177566362723623, - "y": 3.4894287132169657, - "heading": -0.7693928270163053, - "angularVelocity": -0.07267240096117586, - "velocityX": -0.27927835552714286, - "velocityY": 0.3864453236367305, + "x": 2.1175606055664447, + "y": 3.489309593661584, + "heading": -0.770673719921477, + "angularVelocity": -0.03317042016662467, + "velocityX": -0.27364383599643854, + "velocityY": 0.39036997025244236, "moduleForcesX": [ - 44.397979595664026, - 44.88323764291386, - 39.60107624297137, - 40.49028312227595 + 42.69713344336191, + 43.01233196517679, + 40.36254825642123, + 40.77712883283335 ], "moduleForcesY": [ - -54.064737546429875, - -53.66807598097, - -57.67176826261024, - -57.055974307841915 + -55.41943823043682, + -55.177780690773446, + -57.14258045212568, + -56.849966899553834 ], - "timestamp": 12.26166827825376 + "timestamp": 12.121950778904953 }, { "x": 2.1090288162231445, "y": 3.501702308654785, "heading": -0.771700836029523, - "angularVelocity": -0.036536120888024486, - "velocityX": -0.1381626703279573, - "velocityY": 0.19429281431692114, + "angularVelocity": -0.016273372709797004, + "velocityX": -0.13517555296560768, + "velocityY": 0.19634710077161807, "moduleForcesX": [ - 43.44956308739193, - 44.01425554364353, - 38.52755610227539, - 39.51242168602051 + 41.5722477087897, + 41.92121432741254, + 39.30147273432098, + 39.743500474089345 ], "moduleForcesY": [ - -54.834133333236075, - -54.387070471211764, - -58.398462389422065, - -57.74131858284474 + -56.272332012188514, + -56.01516696263238, + -57.881532814470596, + -57.581128152106956 ], - "timestamp": 12.324838887424098 + "timestamp": 12.185067142449409 }, { "x": 2.1090288162231445, "y": 3.501702308654785, "heading": -0.771700836029523, - "angularVelocity": 1.2297405786038555e-20, - "velocityX": -1.8885005519347326e-21, - "velocityY": -1.1623354869164822e-22, + "angularVelocity": 7.034369509774633e-21, + "velocityX": 1.0745130979383846e-20, + "velocityY": 7.792911077792643e-21, "moduleForcesX": [ - 42.58937177633389, - 43.23643664704943, - 37.56731333436976, - 38.647309328509984 + 40.56018810627996, + 40.937946455779034, + 38.35525709358692, + 38.819933249113355 ], "moduleForcesY": [ - -55.508635741315956, - -55.01097400948289, - -59.024322031425896, - -58.327253062024916 + -57.00975798757109, + -56.74117440782203, + -58.516369460899966, + -58.211114035733786 ], - "timestamp": 12.388009496594437 + "timestamp": 12.248183505993865 } ], "trajectoryWaypoints": [ @@ -20141,10 +20141,10 @@ "controlIntervalCount": 34 }, { - "timestamp": 2.379141122264038, + "timestamp": 2.2648261210315326, "isStopPoint": false, - "x": 7.134490489959717, - "y": 0.7612675428390503, + "x": 7.107321739196777, + "y": 1.0795719623565674, "heading": -0.061776917324722916, "isInitialGuess": false, "translationConstrained": true, @@ -20152,10 +20152,10 @@ "controlIntervalCount": 14 }, { - "timestamp": 3.297718075690477, + "timestamp": 3.204592088409269, "isStopPoint": false, - "x": 8.828963279724121, - "y": 0.7209256887435913, + "x": 8.881949424743652, + "y": 1.0173043012619019, "heading": -0.030602705565700654, "isInitialGuess": false, "translationConstrained": true, @@ -20163,7 +20163,7 @@ "controlIntervalCount": 20 }, { - "timestamp": 4.7651738669070065, + "timestamp": 4.6551114717591044, "isStopPoint": false, "x": 5.302053451538086, "y": 1.5809556245803833, @@ -20174,7 +20174,7 @@ "controlIntervalCount": 24 }, { - "timestamp": 6.267535045844391, + "timestamp": 6.1324847610015265, "isStopPoint": true, "x": 2.1090288162231445, "y": 3.48003888130188, @@ -20185,7 +20185,7 @@ "controlIntervalCount": 24 }, { - "timestamp": 7.7765867524956835, + "timestamp": 7.6425499413735505, "isStopPoint": false, "x": 5.271278381347656, "y": 1.5317155122756958, @@ -20196,7 +20196,7 @@ "controlIntervalCount": 17 }, { - "timestamp": 8.31032118918129, + "timestamp": 8.170976551181077, "isStopPoint": false, "x": 7.231364727020264, "y": 1.9017281532287598, @@ -20207,7 +20207,7 @@ "controlIntervalCount": 15 }, { - "timestamp": 9.233441746322784, + "timestamp": 9.093686204087028, "isStopPoint": false, "x": 8.779803276062012, "y": 2.7246744632720947, @@ -20218,7 +20218,7 @@ "controlIntervalCount": 22 }, { - "timestamp": 10.808744267335973, + "timestamp": 10.670274417382469, "isStopPoint": false, "x": 5.295215606689453, "y": 1.3661898374557495, @@ -20229,7 +20229,7 @@ "controlIntervalCount": 25 }, { - "timestamp": 12.388009496594437, + "timestamp": 12.248183505993865, "isStopPoint": true, "x": 2.1090288162231445, "y": 3.501702308654785, diff --git a/src/main/deploy/choreo/line test.1.traj b/src/main/deploy/choreo/line test.1.traj deleted file mode 100644 index 1940583f..00000000 --- a/src/main/deploy/choreo/line test.1.traj +++ /dev/null @@ -1,373 +0,0 @@ -{ - "samples": [ - { - "x": 1.452175259590149, - "y": 5.548182010650635, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": 4.7403725809172957e-29, - "timestamp": 0 - }, - { - "x": 1.466318250541134, - "y": 5.5481975099299685, - "heading": -0.0157941308945737, - "angularVelocity": -0.25306635847793, - "velocityX": 0.22661045687664655, - "velocityY": 0.00024834200784383287, - "timestamp": 0.06241102527259989 - }, - { - "x": 1.4946025349312344, - "y": 5.548215799789452, - "heading": -0.0473993423008294, - "angularVelocity": -0.5064042974503729, - "velocityX": 0.4531937148431271, - "velocityY": 0.0002930549432895735, - "timestamp": 0.12482205054519978 - }, - { - "x": 1.5370283456921094, - "y": 5.548218111495675, - "heading": -0.0947949289781224, - "angularVelocity": -0.7594104802858671, - "velocityX": 0.6797807050293327, - "velocityY": 0.00003704002926447472, - "timestamp": 0.18723307581779966 - }, - { - "x": 1.5936000886657626, - "y": 5.548180976985819, - "heading": -0.1578825277578519, - "angularVelocity": -1.0108406087595598, - "velocityX": 0.906438289184867, - "velocityY": -0.0005949991964591386, - "timestamp": 0.24964410109039956 - }, - { - "x": 1.6643289227296565, - "y": 5.548079510624463, - "heading": -0.23644120556853737, - "angularVelocity": -1.2587307686055578, - "velocityX": 1.1332746699138267, - "velocityY": -0.0016257762303088456, - "timestamp": 0.3120551263629994 - }, - { - "x": 1.7492347150450995, - "y": 5.54789338877616, - "heading": -0.33009503374868415, - "angularVelocity": -1.5005974949338177, - "velocityX": 1.3604293783937302, - "velocityY": -0.0029821950135414175, - "timestamp": 0.3744661516355993 - }, - { - "x": 1.8483456904330084, - "y": 5.547615433446218, - "heading": -0.43832218913614734, - "angularVelocity": -1.7341031478800266, - "velocityX": 1.5880363278165355, - "velocityY": -0.004453625441981749, - "timestamp": 0.4368771769081992 - }, - { - "x": 1.961693677815499, - "y": 5.547260669213064, - "heading": -0.5605399629455426, - "angularVelocity": -1.9582721686711908, - "velocityX": 1.816153266010988, - "velocityY": -0.00568431990502835, - "timestamp": 0.4992882021807991 - }, - { - "x": 2.0893038321982407, - "y": 5.546870818800156, - "heading": -0.696281864374609, - "angularVelocity": -2.17496669596977, - "velocityX": 2.0446732580819043, - "velocityY": -0.006246499097583855, - "timestamp": 0.561699227453399 - }, - { - "x": 2.2311812523790926, - "y": 5.546508045018931, - "heading": -0.8454243630256707, - "angularVelocity": -2.3896819191761076, - "velocityX": 2.273274947222226, - "velocityY": -0.0058126553740190665, - "timestamp": 0.6241102527259988 - }, - { - "x": 2.3587205394296236, - "y": 5.547218412264921, - "heading": -0.9827928088578577, - "angularVelocity": -2.2010285078807508, - "velocityX": 2.0435377642260306, - "velocityY": 0.011382079418547606, - "timestamp": 0.6865212779985987 - }, - { - "x": 2.472038304431874, - "y": 5.547831922447711, - "heading": -1.1060218140283937, - "angularVelocity": -1.9744749366133947, - "velocityX": 1.8156690184984843, - "velocityY": 0.009830157095905711, - "timestamp": 0.7489323032711986 - }, - { - "x": 2.5711645214387526, - "y": 5.548341285454372, - "heading": -1.2146114450267442, - "angularVelocity": -1.739911025694353, - "velocityX": 1.5882805413255756, - "velocityY": 0.008161426679712673, - "timestamp": 0.8113433285437985 - }, - { - "x": 2.6561227019113978, - "y": 5.548755537852966, - "heading": -1.30816353035226, - "angularVelocity": -1.4989672884727516, - "velocityX": 1.3612687838272988, - "velocityY": 0.006637487476911759, - "timestamp": 0.8737543538163984 - }, - { - "x": 2.726928103569937, - "y": 5.549091162899927, - "heading": -1.3864150558606256, - "angularVelocity": -1.25380932558935, - "velocityX": 1.1345014978796688, - "velocityY": 0.005377656360364635, - "timestamp": 0.9361653790889983 - }, - { - "x": 2.783588770774515, - "y": 5.549365783831779, - "heading": -1.4492228855980942, - "angularVelocity": -1.006357922514942, - "velocityX": 0.9078631051861973, - "velocityY": 0.004400199014300529, - "timestamp": 0.9985764043615982 - }, - { - "x": 2.8261077167320017, - "y": 5.549594394204631, - "heading": -1.4965283766810025, - "angularVelocity": -0.757966895657864, - "velocityX": 0.6812729925338861, - "velocityY": 0.003662980569072177, - "timestamp": 1.0609874296341981 - }, - { - "x": 2.8544851092413706, - "y": 5.5497875455558034, - "heading": -1.52832129081398, - "angularVelocity": -0.5094118225394774, - "velocityX": 0.4546855684926289, - "velocityY": 0.003094827401618427, - "timestamp": 1.123398454906798 - }, - { - "x": 2.8687198943418704, - "y": 5.549950700866759, - "heading": -1.5446127785277006, - "angularVelocity": -0.2610354122555862, - "velocityX": 0.22808125703849524, - "velocityY": 0.00261420654819278, - "timestamp": 1.1858094801793977 - }, - { - "x": 2.868810653686523, - "y": 5.550084114074707, - "heading": -1.5454210598741147, - "angularVelocity": -0.01295093840237637, - "velocityX": 0.0014542196802219494, - "velocityY": 0.0021376544824072813, - "timestamp": 1.2482205054519975 - }, - { - "x": 2.854726594556137, - "y": 5.550182947210349, - "heading": -1.5307395591230892, - "angularVelocity": 0.23499685122626165, - "velocityX": -0.22543400732509913, - "velocityY": 0.00158195514736033, - "timestamp": 1.3106958138325855 - }, - { - "x": 2.826468525417828, - "y": 5.550236987413565, - "heading": -1.5005629948722483, - "angularVelocity": 0.4830158510803145, - "velocityX": -0.4523077975549147, - "velocityY": 0.0008649849782922644, - "timestamp": 1.3731711222131735 - }, - { - "x": 2.784035091739694, - "y": 5.550231524517414, - "heading": -1.4549270197774953, - "angularVelocity": 0.7304641829890326, - "velocityX": -0.6792032688605718, - "velocityY": -0.00008744088235959636, - "timestamp": 1.4356464305937615 - }, - { - "x": 2.7274206479760155, - "y": 5.550148748018171, - "heading": -1.3939413616466276, - "angularVelocity": 0.9761561761040737, - "velocityX": -0.9061891045975952, - "velocityY": -0.001324947428231499, - "timestamp": 1.4981217389743495 - }, - { - "x": 2.65661303732607, - "y": 5.549971095385583, - "heading": -1.3178257313501691, - "angularVelocity": 1.2183314059381936, - "velocityX": -1.1333695260202863, - "velocityY": -0.002843565515747548, - "timestamp": 1.5605970473549375 - }, - { - "x": 2.5715921751615904, - "y": 5.549686999649566, - "heading": -1.2269304838245711, - "angularVelocity": 1.4548987413019143, - "velocityX": -1.3608714285014007, - "velocityY": -0.004547328268952344, - "timestamp": 1.6230723557355256 - }, - { - "x": 2.4723310639104916, - "y": 5.5492986933544906, - "heading": -1.1217148662396161, - "angularVelocity": 1.6841152178762588, - "velocityX": -1.588805462856379, - "velocityY": -0.006215356194563731, - "timestamp": 1.6855476641161136 - }, - { - "x": 2.358801118789905, - "y": 5.54882973855317, - "heading": -1.0026530202844375, - "angularVelocity": 1.9057424291658058, - "velocityX": -1.8171970344945176, - "velocityY": -0.007506242279086287, - "timestamp": 1.7480229724967016 - }, - { - "x": 2.2309825227962774, - "y": 5.548327304081928, - "heading": -0.8700584227853703, - "angularVelocity": 2.1223520289500746, - "velocityX": -2.0459058035031332, - "velocityY": -0.00804212870740748, - "timestamp": 1.8104982808772896 - }, - { - "x": 2.088876812221134, - "y": 5.547853084619807, - "heading": -0.7238745865893556, - "angularVelocity": 2.3398657803954315, - "velocityX": -2.274589982131525, - "velocityY": -0.007590510140092997, - "timestamp": 1.8729735892578776 - }, - { - "x": 1.9614313385005593, - "y": 5.54812566903137, - "heading": -0.5810529386524688, - "angularVelocity": 2.286049507134354, - "velocityX": -2.0399334877429878, - "velocityY": 0.004363074279952781, - "timestamp": 1.9354488976384656 - }, - { - "x": 1.84821190371903, - "y": 5.548301120731397, - "heading": -0.45292055372392986, - "angularVelocity": 2.050928410700165, - "velocityX": -1.8122269056048297, - "velocityY": 0.002808336674295336, - "timestamp": 1.9979242060190536 - }, - { - "x": 1.7491837333691869, - "y": 5.548383822800345, - "heading": -0.3400916740028158, - "angularVelocity": 1.80597555487407, - "velocityX": -1.585076935483923, - "velocityY": 0.0013237560712066987, - "timestamp": 2.0603995143996414 - }, - { - "x": 1.6643210197252618, - "y": 5.548394053816421, - "heading": -0.2430258248809031, - "angularVelocity": 1.553667386941605, - "velocityX": -1.3583400521721993, - "velocityY": 0.0001637609515897562, - "timestamp": 2.122874822780229 - }, - { - "x": 1.5936081469644499, - "y": 5.5483582115844, - "heading": -0.1620062610422559, - "angularVelocity": 1.2968253528854596, - "velocityX": -1.1318531207730556, - "velocityY": -0.0005737023640135444, - "timestamp": 2.185350131160817 - }, - { - "x": 1.5370378237791984, - "y": 5.548301788911312, - "heading": -0.09717151543231023, - "angularVelocity": 1.0377659156801453, - "velocityX": -0.9054828964018549, - "velocityY": -0.0009031195621686598, - "timestamp": 2.247825439541405 - }, - { - "x": 1.494608182253064, - "y": 5.548245414161237, - "heading": -0.048565068035293965, - "angularVelocity": 0.7780105237779207, - "velocityX": -0.6791425705057237, - "velocityY": -0.0009023524905812985, - "timestamp": 2.3103007479219926 - }, - { - "x": 1.4663200930356874, - "y": 5.548203145973681, - "heading": -0.0161816029862198, - "angularVelocity": 0.5183402193296897, - "velocityX": -0.45278830870981024, - "velocityY": -0.0006765582864246184, - "timestamp": 2.3727760563025804 - }, - { - "x": 1.452175259590149, - "y": 5.548182010650635, - "heading": 9.25180642166272e-32, - "angularVelocity": 0.2590079729973767, - "velocityX": -0.22640678073274195, - "velocityY": -0.0003382988192744999, - "timestamp": 2.435251364683168 - }, - { - "x": 1.452175259590149, - "y": 5.548182010650635, - "heading": 8.192294602806875e-32, - "angularVelocity": -1.695752883323455e-31, - "velocityX": -4.5255833259766937e-32, - "velocityY": -2.0537668814453742e-32, - "timestamp": 2.497726673063756 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/source 3.1.traj b/src/main/deploy/choreo/source 3.1.traj index e28382ae..c36fb075 100644 --- a/src/main/deploy/choreo/source 3.1.traj +++ b/src/main/deploy/choreo/source 3.1.traj @@ -4,9 +4,9 @@ "x": 0.7778744697570801, "y": 4.333664894104004, "heading": -1.051650156247713, - "angularVelocity": -3.718899994914549e-23, - "velocityX": 1.067537172973605e-21, - "velocityY": 5.775370181920206e-22, + "angularVelocity": -1.3297634990832717e-18, + "velocityX": 8.096904896283762e-18, + "velocityY": -2.035450603469087e-18, "moduleForcesX": [ 0, 0, @@ -22,1936 +22,1936 @@ "timestamp": 0 }, { - "x": 0.7922205153045683, - "y": 4.322569747233587, - "heading": -1.038589316498916, - "angularVelocity": 0.18665078221022835, - "velocityX": 0.2050174930986984, - "velocityY": -0.15855931792528452, + "x": 0.7908719793717586, + "y": 4.323476837419605, + "heading": -1.0413708097378205, + "angularVelocity": 0.15431550267407224, + "velocityX": 0.19512108359903374, + "velocityY": -0.15294504247054264, "moduleForcesX": [ - 64.99353342135232, - 58.994889041922896, - 48.6051352797362, - 44.47522386087147 + 63.74590962361433, + 58.38725396448112, + 49.37860903101583, + 45.506342584710694 ], "moduleForcesY": [ - -25.906794419809312, - -37.63755378362518, - -50.312927149408814, - -54.02243790369699 + -28.834822023862102, + -38.568182065872634, + -49.55287419418438, + -53.15308295520182 ], - "timestamp": 0.06997473889011878 + "timestamp": 0.06661253297151565 }, { - "x": 0.8209322601683935, - "y": 4.300376239957637, - "heading": -1.0127877530474698, - "angularVelocity": 0.3687268271477519, - "velocityX": 0.4103158556820229, - "velocityY": -0.3171645600678816, + "x": 0.8168891188149916, + "y": 4.303112092011227, + "heading": -1.0210382023309228, + "angularVelocity": 0.3052369651757899, + "velocityX": 0.3905742400511663, + "velocityY": -0.30571942695958465, "moduleForcesX": [ - 64.75012154981113, - 59.119805707703335, - 48.51394542600652, - 44.98228840684379 + 63.57502695706841, + 58.50337248946953, + 49.388651702378716, + 45.92040304941029 ], "moduleForcesY": [ - -26.49930496933997, - -37.43568863955079, - -50.39606166150177, - -53.59728170965293 + -29.201580891943905, + -38.38697260898019, + -49.53833328341483, + -52.7922647589104 ], - "timestamp": 0.13994947778023756 + "timestamp": 0.1332250659430313 }, { - "x": 0.8640312929701923, - "y": 4.267081252649147, - "heading": -0.9746011298056743, - "angularVelocity": 0.5457201248261894, - "velocityX": 0.6159227384824854, - "velocityY": -0.47581438439909957, + "x": 0.8559503614500349, + "y": 4.272583897748778, + "heading": -0.9908994985401418, + "angularVelocity": 0.4524479470502735, + "velocityX": 0.5863947952819387, + "velocityY": -0.45829505200625403, "moduleForcesX": [ - 64.44611808173948, - 59.310850143621366, - 48.32509669282183, - 45.610751749821475 + 63.36650355926644, + 58.673994480374716, + 49.332465125535784, + 46.42312032557307 ], "moduleForcesY": [ - -27.21858213973262, - -37.12584717532736, - -50.57187006691617, - -53.05924040607812 + -29.64202317080124, + -38.12000070194584, + -49.58929691252429, + -52.34676597725229 ], - "timestamp": 0.20992421667035632 + "timestamp": 0.19983759891454694 }, { - "x": 0.9215421276125146, - "y": 4.222681267524096, - "heading": -0.9244509530719824, - "angularVelocity": 0.7166897301673768, - "velocityX": 0.8218799463136471, - "velocityY": -0.6345144809296546, + "x": 0.9080832434863456, + "y": 4.231907621482131, + "heading": -0.9512364839325822, + "angularVelocity": 0.5954287104578356, + "velocityX": 0.7826287293204035, + "velocityY": -0.6106399869832296, "moduleForcesX": [ - 64.05688808437033, - 59.54566542911579, - 48.099567813510134, - 46.36161309260976 + 63.1087538854026, + 58.88683733974907, + 49.24358469003159, + 47.01667690510743 ], "moduleForcesY": [ - -28.108700702243887, - -36.74020593171785, - -50.78046522808163, - -52.39939521356428 + -30.17612796676512, + -37.78365700090767, + -49.672009408886844, + -51.809713634059214 ], - "timestamp": 0.2798989555604751 + "timestamp": 0.2664501318860626 }, { - "x": 0.9934931064812429, - "y": 4.167171543523546, - "heading": -0.8628639745372912, - "angularVelocity": 0.880131594794532, - "velocityX": 1.0282421915387603, - "velocityY": -0.7932823313241175, + "x": 0.9733191777952942, + "y": 4.181101065602064, + "heading": -0.9023828510876672, + "angularVelocity": 0.7334000174682638, + "velocityX": 0.9793342393516886, + "velocityY": -0.7627176690877809, "moduleForcesX": [ - 63.55085157875546, - 59.79482586920611, - 47.91149012286905, - 47.23541305248315 + 62.78758091347448, + 59.12644232420397, + 49.162542801992586, + 47.70378434486226 ], "moduleForcesY": [ - -29.21888766916689, - -36.32352543051382, - -50.95113504071442, - -51.60695549029941 + -30.82641384591526, + -37.399521862180414, + -49.74596927329427, + -51.172358294771385 ], - "timestamp": 0.3498736944505939 + "timestamp": 0.33306266485757824 }, { - "x": 1.0799170133820089, - "y": 4.100544943426046, - "heading": -0.7905211436285549, - "angularVelocity": 1.0338420986798709, - "velocityX": 1.2350729459166292, - "velocityY": -0.9521521788330469, + "x": 1.051694367518301, + "y": 4.120184874713526, + "heading": -0.8447458342032448, + "angularVelocity": 0.8652578473343552, + "velocityX": 1.1765832378198438, + "velocityY": -0.9144854304607654, "moduleForcesX": [ - 62.890212743310435, - 60.01889414692214, - 47.84890703697811, - 48.23061564013138 + 62.38681753882747, + 59.37326562455472, + 49.13769712317514, + 48.48705032206493 ], "moduleForcesY": [ - -30.59654996800034, - -35.939691766596795, - -51.0018835950638, - -50.67037098511977 + -31.615322936234296, + -36.99658263811015, + -49.763347170131574, + -50.42430963220079 ], - "timestamp": 0.4198484333407127 + "timestamp": 0.3996751978290939 }, { - "x": 1.1808511971073097, - "y": 4.022790644493082, - "heading": -0.708315499162293, - "angularVelocity": 1.174790299615828, - "velocityX": 1.4424374470878363, - "velocityY": -1.1111766927070885, + "x": 1.1432508333296558, + "y": 4.049183171168427, + "heading": -0.778832566995066, + "angularVelocity": 0.9895024895144598, + "velocityX": 1.3744630586335072, + "velocityY": -1.065891062504106, "moduleForcesX": [ - 62.034245895880275, - 60.16310541057914, - 48.01415268705809, - 49.342246941586 + 61.889733856839435, + 59.602244362756366, + 49.22597947357009, + 49.36848790198351 ], "moduleForcesY": [ - -32.275569212980486, - -35.68171414287474, - -50.836548780375075, - -49.57842210438686 + -32.56119938527781, + -36.61435191109544, + -49.66762908534543, + -49.55361350286737 ], - "timestamp": 0.4898231722308315 + "timestamp": 0.46628773080060953 }, { - "x": 1.2963371243384598, - "y": 3.933893322355774, - "heading": -0.6174207883006573, - "angularVelocity": 1.298964630712914, - "velocityX": 1.650394543272217, - "velocityY": -1.2704202051672215, + "x": 1.2480376237140218, + "y": 3.968124696368845, + "heading": -0.7052817755202949, + "angularVelocity": 1.1041584547794074, + "velocityX": 1.57307920461716, + "velocityY": -1.2168652231591865, "moduleForcesX": [ - 60.94746004979524, - 60.146829265545456, - 48.525569264290525, - 50.56132114483972 + 61.28176481668813, + 59.78038532288985, + 49.4938549994705, + 50.34939808767109 ], "moduleForcesY": [ - -34.25932977118567, - -35.687927503268604, - -50.336093067995485, - -48.32077560597764 + -33.67250970561668, + -36.30739888304486, + -49.39059309707039, + -48.546399889287585 ], - "timestamp": 0.5597979111209502 + "timestamp": 0.5329002637721252 }, { - "x": 1.4264194135128296, - "y": 3.833833792551993, - "heading": -0.5193851332120467, - "angularVelocity": 1.4010149468732676, - "velocityX": 1.8589892758104873, - "velocityY": -1.429937880309981, + "x": 1.3661123895110021, + "y": 3.877044967671322, + "heading": -0.6249031875065618, + "angularVelocity": 1.206658633565307, + "velocityX": 1.7725608159574173, + "velocityY": -1.3673061905102768, "moduleForcesX": [ - 59.614996756079215, - 59.84049412454702, - 49.524716335836736, - 51.876090247246715 + 60.55523039747262, + 59.86237923234887, + 50.01933061446567, + 51.43105327265715 ], "moduleForcesY": [ - -36.50078923453568, - -36.17056672706365, - -49.33646413057858, - -46.88658430312503 + -34.9402966200205, + -36.15231999767806, + -48.845768467491496, + -47.385486747890475 ], - "timestamp": 0.629772650011069 + "timestamp": 0.5995127967436409 }, { - "x": 1.5711438836172125, - "y": 3.722592592486754, - "heading": -0.416299104221778, - "angularVelocity": 1.4731891912043358, - "velocityX": 2.0682388016001525, - "velocityY": -1.589733692896242, + "x": 1.4975436678185934, + "y": 3.7759903450547028, + "heading": -0.53873230694548, + "angularVelocity": 1.293613629704331, + "velocityX": 1.9730713192334703, + "velocityY": -1.5170511930514805, "moduleForcesX": [ - 58.065083972008736, - 59.00957277137866, - 51.197174245280834, - 53.27774962436911 + 59.71660813639446, + 59.781944702186415, + 50.89674120838264, + 52.617056525494455 ], "moduleForcesY": [ - -38.886690128517934, - -37.47125653206859, - -47.574170293153024, - -45.2567742927428 + -36.32951289772708, + -36.25934407553948, + -47.91447235046562, + -46.0464745895729 ], - "timestamp": 0.6997473889011877 + "timestamp": 0.6661253297151566 }, { - "x": 1.7305504220603747, - "y": 3.6001600451974833, - "heading": -0.3111713227790686, - "angularVelocity": 1.502367613086651, - "velocityX": 2.2780583532219847, - "velocityY": -1.749667797710917, + "x": 1.6424143533198352, + "y": 3.665025662326569, + "heading": -0.4481205140243463, + "angularVelocity": 1.3602814497368003, + "velocityX": 2.174826253239616, + "velocityY": -1.665822898156168, "moduleForcesX": [ - 56.39097412755216, - 57.161179416740495, - 53.820924588217835, - 54.780035432769175 + 58.79631756896231, + 59.433248892591386, + 52.247409534940125, + 53.9194587893842 ], "moduleForcesY": [ - -41.23803886906891, - -40.17856661949012, - -44.545308010292885, - -43.37339903624029 + -37.77070605252747, + -36.79353882986422, + -46.41555660113537, + -44.48747884319979 ], - "timestamp": 0.7697221277913064 + "timestamp": 0.7327378626866723 }, { - "x": 1.904638990279975, - "y": 3.4665697709891212, - "heading": -0.20885800973766258, - "angularVelocity": 1.462146406892186, - "velocityX": 2.4878773537543495, - "velocityY": -1.909121439068728, + "x": 1.800826604673265, + "y": 3.5442490372900517, + "heading": -0.3549074004558732, + "angularVelocity": 1.3993329694928751, + "velocityX": 2.3781148124358094, + "velocityY": -1.8131216401598682, "moduleForcesX": [ - 54.74964870144604, - 53.05199961529088, - 57.85147404335248, - 56.499407721715734 + 57.85964296595883, + 58.62699304555619, + 54.24098287937368, + 55.37454877680063 ], "moduleForcesY": [ - -43.34419920923391, - -45.385040535603636, - -39.0969722773908, - -41.00039456972551 + -39.15408809177324, + -38.01758274488999, + -44.03657792902701, + -42.620768436938704 ], - "timestamp": 0.8396968666814252 + "timestamp": 0.799350395658188 }, { - "x": 2.09320393495465, - "y": 3.3220757989813476, - "heading": -0.11854546488401964, - "angularVelocity": 1.2906449711153707, - "velocityX": 2.694757389102635, - "velocityY": -2.064944782926198, + "x": 1.9729064746231015, + "y": 3.4138237900863677, + "heading": -0.261785676023239, + "angularVelocity": 1.3979610184235784, + "velocityX": 2.58329569937567, + "velocityY": -1.9579685891760825, "moduleForcesX": [ - 53.2728143917394, - 42.775288784328325, - 63.8442089345851, - 59.14849524914464 + 57.01503970270268, + 56.97264868755776, + 57.13055506988329, + 57.088614242525495 ], "moduleForcesY": [ - -45.07889412240785, - -55.05640389507177, - -28.09637912144248, - -36.751236686389184 + -40.32815265808299, + -40.3871603414814, + -40.16401782504414, + -40.22274635092099 ], - "timestamp": 0.9096716055715439 + "timestamp": 0.8659629286297037 }, { - "x": 2.295164186316796, - "y": 3.1696331321710134, - "heading": -0.06408923152074174, - "angularVelocity": 0.778227032026378, - "velocityX": 2.886187995346207, - "velocityY": -2.178538558746386, + "x": 2.1587959216901558, + "y": 3.2740626515960307, + "heading": -0.1731498612166921, + "angularVelocity": 1.3306176908848175, + "velocityX": 2.7906076945992013, + "velocityY": -2.0981207628015075, "moduleForcesX": [ - 51.59206019363968, - 13.364464020811422, - 69.52592713911473, - 68.20078231069633 + 56.41042369707265, + 53.501719202644416, + 61.26419264721754, + 59.40078934825349 ], "moduleForcesY": [ - -46.885084851896735, - -68.25325997043356, - -3.8069250749386283, - -1.3257476748867494 + -41.1079010332853, + -44.78370054636255, + -33.42235417217702, + -36.56648525897984 ], - "timestamp": 0.9796463444616627 + "timestamp": 0.9325754616012194 }, { - "x": 2.5102036576315885, - "y": 3.0099910805069405, - "heading": -0.048739655045187506, - "angularVelocity": 0.21935882461323145, - "velocityX": 3.0731014466873368, - "velocityY": -2.281424042392765, + "x": 2.3585571947909116, + "y": 3.1257497783866914, + "heading": -0.09743941845228067, + "angularVelocity": 1.1365795502294775, + "velocityX": 2.9988541823830035, + "velocityY": -2.2265010290594955, "moduleForcesX": [ - 51.68312483431202, - 11.490644239292044, - 69.23100690631284, - 65.49577647910316 + 56.18321511797377, + 45.13645782831661, + 66.6575745567656, + 63.63924131102192 ], "moduleForcesY": [ - -46.34143932256032, - -68.12951985238924, - -1.4483890023414814, - 6.986075009746601 + -41.32675572742186, + -53.045678977052305, + -20.39623436243562, + -28.01878833525786 ], - "timestamp": 1.0496210833517814 + "timestamp": 0.9991879945727351 }, { - "x": 2.7349332678515874, - "y": 2.854041862643359, - "heading": -0.04856305611607661, - "angularVelocity": 0.002523752598608622, - "velocityX": 3.2115819763599447, - "velocityY": -2.228650229170103, + "x": 2.571323749679796, + "y": 2.9722890611298696, + "heading": -0.05631702040111217, + "angularVelocity": 0.6173372519665775, + "velocityX": 3.1940919433264323, + "velocityY": -2.303781441886374, "moduleForcesX": [ - 44.06076488703463, - 32.04578879681631, - 41.59848030725396, - 28.915624556417924 + 56.49315652197762, + 24.727674210970267, + 69.53856728119115, + 66.38848839184168 ], "moduleForcesY": [ - 2.892829937081654, - 5.007695054030006, - 21.07556807654082, - 26.89985763143924 + -40.73364522523241, + -64.83876276810156, + 2.4259673258224868, + 17.19340461150298 ], - "timestamp": 1.1195958222419002 + "timestamp": 1.0658005275442508 }, { - "x": 2.9682298777966594, - "y": 2.7112552443984246, - "heading": -0.048304755129702115, - "angularVelocity": 0.0036913461982344513, - "velocityX": 3.3340118683603395, - "velocityY": -2.040545209738506, + "x": 2.7978969801578932, + "y": 2.816123256954651, + "heading": -0.04923559160029977, + "angularVelocity": 0.1063077544857887, + "velocityX": 3.4013603802603107, + "velocityY": -2.3443907206170564, "moduleForcesX": [ - 32.3225255016905, - 32.43770393632479, - 32.3755224257375, - 32.49078432468219 + 61.70982060481134, + 40.01285570538606, + 68.37167087206099, + 60.43433119111175 ], "moduleForcesY": [ - 49.850764221083146, - 49.79054196126857, - 49.79062122677659, - 49.730235606563355 + -31.4520746032983, + -55.70693219082933, + 10.727828044445207, + 31.264613623300775 ], - "timestamp": 1.189570561132019 + "timestamp": 1.1324130605157665 }, { - "x": 3.206226948407469, - "y": 2.57644940649246, - "heading": -0.04804302634103678, - "angularVelocity": 0.0037403324802159203, - "velocityX": 3.4011855476093436, - "velocityY": -1.9264929036412723, + "x": 3.0347601843563274, + "y": 2.6710905971518515, + "heading": -0.04900748324484257, + "angularVelocity": 0.003424405968089362, + "velocityX": 3.55583542063653, + "velocityY": -2.1772578422264344, "moduleForcesX": [ - 17.778201901232073, - 17.782317553134607, - 17.77881545073154, - 17.782931249096187 + 50.41350702863919, + 41.75482316229609, + 44.362342200824024, + 35.27998738074212 ], "moduleForcesY": [ - 30.19121333723645, - 30.19026343520494, - 30.187987107924002, - 30.187037184259207 + 38.99730957905846, + 46.617569740155716, + 46.965805915927774, + 53.3083103983914 ], - "timestamp": 1.259545300022138 + "timestamp": 1.1990255934872822 }, { - "x": 3.4454066255541647, - "y": 2.443753454780269, - "heading": -0.04778008796546239, - "angularVelocity": 0.003757618531271475, - "velocityX": 3.418086025619602, - "velocityY": -1.896340791218425, + "x": 3.2779712543514585, + "y": 2.5369613930825095, + "heading": -0.04879559855232118, + "angularVelocity": 0.0031808532579297136, + "velocityX": 3.6511307879424364, + "velocityY": -2.0135730933201033, "moduleForcesX": [ - 4.472855661174314, - 4.474147280296156, - 4.472811619653854, - 4.474103244435602 + 26.514032908634963, + 26.48917929707793, + 26.505657516129617, + 26.480809194623138 ], "moduleForcesY": [ - 7.9818138726911165, - 7.981734606875763, - 7.980518679466535, - 7.980439415924407 + 45.5016606384836, + 45.51171619004381, + 45.5152401681101, + 45.525290455269825 ], - "timestamp": 1.3295200389122568 + "timestamp": 1.265638126458798 }, { - "x": 3.68491109766373, - "y": 2.3116446526478165, - "heading": -0.04751696829794281, - "angularVelocity": 0.003760209351159636, - "velocityX": 3.4227276286898194, - "velocityY": -1.8879499120375856, + "x": 3.5240170136884648, + "y": 2.4081056080213394, + "heading": -0.04858282682880418, + "angularVelocity": 0.003194169535753657, + "velocityX": 3.6936856828761946, + "velocityY": -1.93440752532666, "moduleForcesX": [ - 1.2285197916199417, - 1.228711642997039, - 1.2285106844862757, - 1.228702535899101 + 11.832044679638638, + 11.833159737293062, + 11.83210215812816, + 11.833217225479276 ], "moduleForcesY": [ - 2.2211300227139463, - 2.2211206994127397, - 2.22093420243456, - 2.220924879148784 + 22.01300562318127, + 22.0128485655156, + 22.01202572778171, + 22.011868671306686 ], - "timestamp": 1.3994947778023756 + "timestamp": 1.3322506594303136 }, { - "x": 3.924585147927277, - "y": 2.1798436167917243, - "heading": -0.047254057832202365, - "angularVelocity": 0.0037572196754216245, - "velocityX": 3.425151048293395, - "velocityY": -1.883551663737667, + "x": 3.7708350348315482, + "y": 2.2807352279621504, + "heading": -0.0483699449005901, + "angularVelocity": 0.0031958239496029865, + "velocityX": 3.7052790238231275, + "velocityY": -1.9121083432400703, "moduleForcesX": [ - 0.6415734362803132, - 0.6413521699115629, - 0.6415840632535661, - 0.6413627969097583 + 3.2235324585038754, + 3.2236618531632817, + 3.2235271302506407, + 3.2236565249546465 ], "moduleForcesY": [ - 1.164077957656933, - 1.1640884844019281, - 1.1643040474316328, - 1.1643145741874217 + 6.200483556552904, + 6.200476317722027, + 6.200352895038578, + 6.200345656224632 ], - "timestamp": 1.4694695166924945 + "timestamp": 1.3988631924018293 }, { - "x": 4.164406438877476, - "y": 2.0483104663276324, - "heading": -0.04699153890493271, - "angularVelocity": 0.0037516242494578765, - "velocityX": 3.427255246022282, - "velocityY": -1.8797233480304725, + "x": 4.017945860044819, + "y": 2.153933675847828, + "heading": -0.04815735530566025, + "angularVelocity": 0.0031914353868025967, + "velocityX": 3.709674654151622, + "velocityY": -1.90356899006594, "moduleForcesX": [ - 0.5571686890446169, - 0.556754586714119, - 0.5571884977126166, - 0.556774395458144 + 1.2223925594156837, + 1.2220511392203353, + 1.2224089657777908, + 1.222067545703039 ], "moduleForcesY": [ - 1.0131163405590924, - 1.013135903625798, - 1.013539512185854, - 1.0135590752854269 + 2.374233190641205, + 2.3742500204088812, + 2.3745815684863776, + 2.374598398301541 ], - "timestamp": 1.5394442555826133 + "timestamp": 1.465475725373345 }, { - "x": 4.404389812405626, - "y": 1.917073025680941, - "heading": -0.046729445162668845, - "angularVelocity": 0.0037455479851869126, - "velocityX": 3.4295715473121766, - "velocityY": -1.8754973970359978, + "x": 4.265335286953013, + "y": 2.027676292006606, + "heading": -0.04794522174606807, + "angularVelocity": 0.0031845892976762468, + "velocityX": 3.7138570757244884, + "velocityY": -1.895399832569214, "moduleForcesX": [ - 0.6133285579559523, - 0.6128788486139368, - 0.6133499263194478, - 0.6129002170763577 + 1.163199663544347, + 1.1626670888284039, + 1.1632251975919956, + 1.1626926239772695 ], "moduleForcesY": [ - 1.1183496176451213, - 1.1183707656322825, - 1.1188091442741797, - 1.1188302923040183 + 2.271195979291761, + 2.271222065843487, + 2.271739492202394, + 2.2717655728058856 ], - "timestamp": 1.6094189944727322 + "timestamp": 1.5320882583448607 }, { - "x": 4.6445839308598735, - "y": 1.7862215631603924, - "heading": -0.04646750224020687, - "angularVelocity": 0.0037433926387822392, - "velocityX": 3.432583275965109, - "velocityY": -1.8699814332429918, + "x": 4.513387263491877, + "y": 1.9027254137370513, + "heading": -0.04773334654928872, + "angularVelocity": 0.003180710706046184, + "velocityX": 3.7238033966511064, + "velocityY": -1.8757863227177534, "moduleForcesX": [ - 0.7972668572924495, - 0.7971073142412101, - 0.7972743634616845, - 0.7971148204266829 + 2.765776223844279, + 2.765473269227589, + 2.7657891163065877, + 2.7654861619049216 ], "moduleForcesY": [ - 1.4599652235465161, - 1.4599727135025415, - 1.4601282052544333, - 1.4601356952174342 + 5.453486682132146, + 5.453502875542226, + 5.453793409345764, + 5.453809602835306 ], - "timestamp": 1.679393733362851 + "timestamp": 1.5987007913163764 }, { - "x": 4.885064829086469, - "y": 1.6558983523007769, - "heading": -0.04620453225018068, - "angularVelocity": 0.003758070329338863, - "velocityX": 3.4366816088334584, - "velocityY": -1.8624322566499572, + "x": 4.7636905451889415, + "y": 1.7823482264471835, + "heading": -0.04752024111954168, + "angularVelocity": 0.003199179197076531, + "velocityX": 3.7576004173883883, + "velocityY": -1.8071252048220714, "moduleForcesX": [ - 1.0842918084117592, - 1.0853786132925534, - 1.084241424204242, - 1.085328230114811 + 9.396675512144679, + 9.39819901784068, + 9.396709254967268, + 9.398232777338956 ], "moduleForcesY": [ - 1.9988129858279566, - 1.9987618336162647, - 1.997703395532972, - 1.9976522437601878 + 19.092390606150673, + 19.092220862164694, + 19.091009053610517, + 19.090839312476135 ], - "timestamp": 1.7493684722529699 + "timestamp": 1.6653133242878921 }, { - "x": 5.125797171042743, - "y": 1.5260410914198634, - "heading": -0.04593987286096788, - "angularVelocity": 0.0037822133159851846, - "velocityX": 3.4402749588575916, - "velocityY": -1.8557734253903861, + "x": 5.0188400094891765, + "y": 1.6726490442267128, + "heading": -0.04723633820300304, + "angularVelocity": 0.004262004518879671, + "velocityX": 3.8303522313036735, + "velocityY": -1.646824964115687, "moduleForcesX": [ - 0.9502910191372389, - 0.9520784538822461, - 0.95020827131857, - 0.9519957085202748 + 20.160339598017387, + 20.271778827791216, + 20.186266828439518, + 20.297839903273452 ], "moduleForcesY": [ - 1.7635166253693095, - 1.7634333067623955, - 1.7616912237363618, - 1.7616079061721086 + 44.61714719861703, + 44.58388431235552, + 44.56094399930638, + 44.52760480062931 ], - "timestamp": 1.8193432111430887 + "timestamp": 1.7319258572594078 }, { - "x": 5.366788045768459, - "y": 1.396664893828562, - "heading": -0.04567411476495212, - "angularVelocity": 0.0037979147936955102, - "velocityX": 3.443969617437849, - "velocityY": -1.8488986117470256, + "x": 5.276040962243286, + "y": 1.5678524061254324, + "heading": -0.046947349952411, + "angularVelocity": 0.0043383465197998905, + "velocityX": 3.8611495657144865, + "velocityY": -1.5732270404143434, "moduleForcesX": [ - 0.977404742123205, - 0.9785672603177287, - 0.9773512911124315, - 0.978513810379268 + 8.560118396401565, + 8.566501267033127, + 8.560247116014803, + 8.56663029963911 ], "moduleForcesY": [ - 1.8203523461919868, - 1.820298421093803, - 1.8191652136181402, - 1.8191112889734293 + 20.46747803904423, + 20.466793414894244, + 20.461850439324866, + 20.461165848040086 ], - "timestamp": 1.8893179500332076 + "timestamp": 1.7985383902309235 }, { - "x": 5.608450195426062, - "y": 1.2685474906510974, - "heading": -0.045407399320727376, - "angularVelocity": 0.003811596133907239, - "velocityX": 3.453562721214088, - "velocityY": -1.830909342564999, + "x": 5.534216899574317, + "y": 1.4654824117182799, + "heading": -0.04665631309500156, + "angularVelocity": 0.004369100594536302, + "velocityX": 3.8757862194105366, + "velocityY": -1.536797804264961, "moduleForcesX": [ - 2.5387638123361675, - 2.539779903515735, - 2.538721732122775, - 2.5397378254255 + 4.068624748426005, + 4.071060153628479, + 4.068549709495257, + 4.070985139742783 ], "moduleForcesY": [ - 4.762218612064458, - 4.762167761549727, - 4.761187255842194, - 4.761136406193755 + 10.13063153345665, + 10.130481600946313, + 10.128231656616295, + 10.128081730271346 ], - "timestamp": 1.9592926889233264 + "timestamp": 1.8651509232024392 }, { - "x": 5.852933619648411, - "y": 1.1459010453590361, - "heading": -0.04513772147587357, - "angularVelocity": 0.0038539314205556937, - "velocityX": 3.493881193415528, - "velocityY": -1.7527245865776317, + "x": 5.794372355867246, + "y": 1.3682547977858872, + "heading": -0.04636219278537718, + "angularVelocity": 0.004415389964980432, + "velocityX": 3.905503134135057, + "velocityY": -1.459599411629766, "moduleForcesX": [ - 10.670372351980113, - 10.673724191649343, - 10.670507188995435, - 10.673859113242672 + 8.260963536224425, + 8.264869277780319, + 8.261045504013175, + 8.264951367768367 ], "moduleForcesY": [ - 20.696850436257613, - 20.696435019435945, - 20.693861640190523, - 20.69344623623891 + 21.467349064567532, + 21.466934299940117, + 21.46397136076802, + 21.463556605012805 ], - "timestamp": 2.029267427813445 + "timestamp": 1.931763456173955 }, { - "x": 6.1035014115315445, - "y": 1.0362251026771625, - "heading": -0.04485903801696482, - "angularVelocity": 0.003982629493571362, - "velocityX": 3.580832109664594, - "velocityY": -1.5673648008047245, + "x": 6.058520814972377, + "y": 1.282472529523383, + "heading": -0.04606005529713513, + "angularVelocity": 0.004535745373490557, + "velocityX": 3.965446850941467, + "velocityY": -1.2877797080496214, "moduleForcesX": [ - 23.006805416844802, - 23.020298874307237, - 23.01072211535244, - 23.02421749157213 + 16.659711714491884, + 16.673161829473102, + 16.66222092531276, + 16.675673239510985 ], "moduleForcesY": [ - 49.06916713204879, - 49.064472309009574, - 49.06330827580301, - 49.05861198170382 + 47.77989163980646, + 47.77656557923747, + 47.77432962004692, + 47.771002564051436 ], - "timestamp": 2.0992421667035637 + "timestamp": 1.9983759891454707 }, { - "x": 6.360440389666232, - "y": 0.9424571330795427, - "heading": -0.04456198298429893, - "angularVelocity": 0.004245175292934805, - "velocityX": 3.6718819135310827, - "velocityY": -1.3400260020242942, + "x": 6.326935018479887, + "y": 1.2111884138637838, + "heading": -0.04572387766988923, + "angularVelocity": 0.0050467624071529864, + "velocityX": 4.0294850163149665, + "velocityY": -1.0701306868195675, "moduleForcesX": [ - 24.07899931263681, - 24.111464518434406, - 24.089464997361308, - 24.121942503866567 + 17.76434909368371, + 17.833039573651952, + 17.77928266449683, + 17.84803536932326 ], "moduleForcesY": [ - 60.18510366880944, - 60.17310602326288, - 60.17798562768333, - 60.165977537276476 + 60.533843276425394, + 60.51552676552759, + 60.52159010023538, + 60.50324061772363 ], - "timestamp": 2.1692169055936823 + "timestamp": 2.064988522116986 }, { - "x": 6.622860129273645, - "y": 0.8657147607838669, - "heading": -0.04392950732433488, - "angularVelocity": 0.009038628367834598, - "velocityX": 3.7502067713248666, - "velocityY": -1.096715379191114, + "x": 6.598149001232439, + "y": 1.1556493567666084, + "heading": -0.04255962682130831, + "angularVelocity": 0.04750233488244815, + "velocityX": 4.071515834242889, + "velocityY": -0.8337628764383538, "moduleForcesX": [ - 20.32544719556, - 20.966458363180468, - 20.495346686232793, - 21.14167832393734 + 8.287572421549362, + 14.286479207247982, + 8.808436728764809, + 15.365145055742365 ], "moduleForcesY": [ - 64.54278261295427, - 64.34395175999101, - 64.46421735586846, - 64.26189706479809 + 66.4507233464468, + 65.45898479609045, + 66.07372493186959, + 64.91023150582662 ], - "timestamp": 2.239191644483801 + "timestamp": 2.1316010550885016 }, { - "x": 6.884679748309282, - "y": 0.806794359236595, - "heading": -0.030602705565700657, - "angularVelocity": 0.1904516111101361, - "velocityX": 3.7416305253638975, - "velocityY": -0.8420238857881919, + "x": 6.859559783326081, + "y": 1.113135958184906, + "heading": -0.030602705565701032, + "angularVelocity": 0.17949957346195378, + "velocityX": 3.9243483235418233, + "velocityY": -0.6382192161928673, "moduleForcesX": [ - -12.193365917081998, - 9.151089894849695, - -18.363615689541977, - 12.325518942868378 + -42.45988336627168, + -29.346972124084434, + -52.177609628795565, + -39.698593640879075 ], "moduleForcesY": [ - 67.8990355400377, - 68.35833703061081, - 66.03471322560178, - 67.3706211378861 + 54.43758266703957, + 62.32119709611513, + 44.88720682067673, + 55.84212799694645 ], - "timestamp": 2.3091663833739196 + "timestamp": 2.198213588060017 }, { - "x": 7.134490489959717, - "y": 0.7612675428390503, - "heading": -0.03060270556570066, - "angularVelocity": 1.4169381433025497e-21, - "velocityX": 3.570013202088712, - "velocityY": -0.6506178818192586, + "x": 7.107321739196777, + "y": 1.0795719623565674, + "heading": -0.030602705565701018, + "angularVelocity": 3.347783365519795e-18, + "velocityX": 3.7194495512825236, + "velocityY": -0.5038690818541787, "moduleForcesX": [ - -45.9398192805324, - -59.450192458631626, - -29.698787647568384, - -46.616491719965026 + -61.323422950505716, + -65.95546140653828, + -44.62846950333632, + -55.985729468867824 ], "moduleForcesY": [ - 51.92283929106931, - 36.09371888383255, - 62.89922393782059, - 51.7414031119671 + 32.44664959475204, + 22.20881577411158, + 53.33306981117861, + 41.43874553341409 ], - "timestamp": 2.379141122264038 + "timestamp": 2.2648261210315326 }, { - "x": 7.356199543080895, - "y": 0.7288568209198342, - "heading": -0.03060270556570066, - "angularVelocity": -2.149395300731945e-22, - "velocityX": 3.3790601126213313, - "velocityY": -0.49397070672899657, + "x": 7.342124626950215, + "y": 1.053916100516507, + "heading": -0.030602705565701004, + "angularVelocity": 7.672386391662901e-19, + "velocityX": 3.4979351696684997, + "velocityY": -0.3822037381956934, "moduleForcesX": [ - -53.90473620010748, - -53.90473620010748, - -53.90473620010747, - -53.90473620010747 + -61.122061696457, + -61.122061696457, + -61.122061696457, + -61.122061696457 ], "moduleForcesY": [ - 44.22041388953324, - 44.22041388953323, - 44.22041388953325, - 44.22041388953323 + 33.57089768760224, + 33.57089768760224, + 33.57089768760223, + 33.57089768760224 ], - "timestamp": 2.444753761794498 + "timestamp": 2.3319522615585138 }, { - "x": 7.563645437858805, - "y": 0.7041763764831177, - "heading": -0.03060270556570066, - "angularVelocity": -2.1492569533532087e-22, - "velocityX": 3.161675803053267, - "velocityY": -0.376153811420113, + "x": 7.56101179722364, + "y": 1.0341858544549376, + "heading": -0.03060270556570099, + "angularVelocity": 7.672403149471102e-19, + "velocityX": 3.260833537501588, + "velocityY": -0.2939279080649534, "moduleForcesX": [ - -61.36608679123134, - -61.36608679123133, - -61.36608679123134, - -61.36608679123134 + -65.42302348088484, + -65.42302348088484, + -65.42302348088484, + -65.42302348088484 ], "moduleForcesY": [ - 33.25889452354701, - 33.258894523547006, - 33.258894523547006, - 33.258894523547006 + 24.35778975736353, + 24.357789757363527, + 24.357789757363534, + 24.357789757363534 ], - "timestamp": 2.510366401324958 + "timestamp": 2.399078402085495 }, { - "x": 7.756067149920293, - "y": 0.6856463295872331, - "heading": -0.03060270556570066, - "angularVelocity": -2.148886089665622e-22, - "velocityX": 2.932692746984486, - "velocityY": -0.28241581238752267, + "x": 7.76353893371165, + "y": 1.0190536925576863, + "heading": -0.030602705565700973, + "angularVelocity": 7.672257598260093e-19, + "velocityX": 3.0171127804763933, + "velocityY": -0.22542874919449496, "moduleForcesX": [ - -64.6403326917135, - -64.6403326917135, - -64.6403326917135, - -64.6403326917135 + -67.24942660173558, + -67.24942660173558, + -67.24942660173558, + -67.24942660173558 ], "moduleForcesY": [ - 26.461588675373616, - 26.461588675373616, - 26.461588675373616, - 26.461588675373616 + 18.900848713116805, + 18.900848713116805, + 18.9008487131168, + 18.900848713116808 ], - "timestamp": 2.575979040855418 + "timestamp": 2.466204542612476 }, { - "x": 7.9330736647908635, - "y": 0.6722308870628335, - "heading": -0.03060270556570066, - "angularVelocity": -2.14835209295751e-22, - "velocityX": 2.697750252653663, - "velocityY": -0.2044643017016828, + "x": 7.949481024014891, + "y": 1.0076590456800256, + "heading": -0.03060270556570096, + "angularVelocity": 7.672013523245006e-19, + "velocityX": 2.7700399403791343, + "velocityY": -0.16974976943713838, "moduleForcesX": [ - -66.32264088747145, - -66.32264088747145, - -66.32264088747145, - -66.32264088747145 + -68.17436080640248, + -68.17436080640248, + -68.17436080640248, + -68.17436080640248 ], "moduleForcesY": [ - 22.005172221305003, - 22.005172221305003, - 22.005172221304996, - 22.005172221304996 + 15.36339993436348, + 15.363399934363485, + 15.363399934363473, + 15.363399934363477 ], - "timestamp": 2.641591680385878 + "timestamp": 2.5333306831394573 }, { - "x": 8.094438850207247, - "y": 0.6632084825286524, - "heading": -0.03060270556570066, - "angularVelocity": -2.1476865288778444e-22, - "velocityX": 2.4593612842152437, - "velocityY": -0.13751015960869134, + "x": 8.118709429041983, + "y": 0.9994031964377751, + "heading": -0.030602705565700945, + "angularVelocity": 7.671706888831663e-19, + "velocityX": 2.5210507217984657, + "velocityY": -0.12299007774670825, "moduleForcesX": [ - -67.29555668636523, - -67.29555668636523, - -67.29555668636523, - -67.29555668636523 + -68.70314364670283, + -68.70314364670283, + -68.70314364670283, + -68.70314364670283 ], "moduleForcesY": [ - 18.90069114406037, - 18.90069114406037, - 18.900691144060374, - 18.900691144060374 + 12.90231694942824, + 12.902316949428238, + 12.902316949428242, + 12.902316949428242 ], - "timestamp": 2.707204319916338 + "timestamp": 2.6004568236664385 }, { - "x": 8.240020092552498, - "y": 0.6580507593309295, - "heading": -0.03060270556570066, - "angularVelocity": -2.146966814878084e-22, - "velocityX": 2.2187987465078, - "velocityY": -0.07860868324507067, + "x": 8.271143977771962, + "y": 0.9938469245659062, + "heading": -0.030602705565700928, + "angularVelocity": 7.671359538796945e-19, + "velocityX": 2.270867169380966, + "velocityY": -0.08277359353968143, "moduleForcesX": [ - -67.9091402549072, - -67.9091402549072, - -67.9091402549072, - -67.9091402549072 + -69.0326940172405, + -69.0326940172405, + -69.0326940172405, + -69.0326940172405 ], "moduleForcesY": [ - 16.627479314598546, - 16.627479314598546, - 16.627479314598553, - 16.62747931459855 + 11.096861571799984, + 11.096861571799986, + 11.096861571799984, + 11.096861571799984 ], - "timestamp": 2.7728169594467977 + "timestamp": 2.6675829641934197 }, { - "x": 8.369721422210265, - "y": 0.6563552816375592, - "heading": -0.03060270556570066, - "angularVelocity": -2.146156534170985e-22, - "velocityX": 1.9767735391525214, - "velocityY": -0.02584071766512831, + "x": 8.406731406019375, + "y": 0.9906547367603562, + "heading": -0.030602705565700914, + "angularVelocity": 7.670985863150012e-19, + "velocityX": 2.019890123027612, + "velocityY": -0.047555062461336506, "moduleForcesX": [ - -68.32204177817843, - -68.32204177817843, - -68.32204177817843, - -68.32204177817843 + -69.25164136036881, + -69.25164136036881, + -69.25164136036881, + -69.25164136036881 ], "moduleForcesY": [ - 14.896031650164923, - 14.896031650164922, - 14.896031650164927, - 14.896031650164925 + 9.717785426549383, + 9.717785426549383, + 9.717785426549389, + 9.717785426549389 ], - "timestamp": 2.8384295989772577 + "timestamp": 2.734709104720401 }, { - "x": 8.483474950917365, - "y": 0.6578057698994572, - "heading": -0.03060270556570066, - "angularVelocity": -2.1453703946144053e-22, - "velocityX": 1.7337136490948615, - "velocityY": 0.022106842100517037, + "x": 8.5254345460703, + "y": 0.9895621634069518, + "heading": -0.0306027055657009, + "angularVelocity": 7.670595843658019e-19, + "velocityX": 1.7683593771223813, + "velocityY": -0.016276421448131698, "moduleForcesX": [ - -68.61412554743417, - -68.61412554743417, - -68.61412554743417, - -68.61412554743417 + -69.40442267327488, + -69.40442267327488, + -69.40442267327488, + -69.40442267327488 ], "moduleForcesY": [ - 13.535264434918018, - 13.535264434918016, - 13.53526443491802, - 13.53526443491802 + 8.630658704124505, + 8.630658704124507, + 8.630658704124507, + 8.630658704124508 ], - "timestamp": 2.9040422385077176 + "timestamp": 2.801835245247382 }, { - "x": 8.581230723210314, - "y": 0.6621472949390711, - "heading": -0.030602705565700657, - "angularVelocity": -2.1446182627185602e-22, - "velocityX": 1.4898923895230367, - "velocityY": 0.06616903497074665, + "x": 8.627226437405287, + "y": 0.9903554058889321, + "heading": -0.030602705565700883, + "angularVelocity": 7.670197234878886e-19, + "velocityX": 1.516426991569238, + "velocityY": 0.011817191868226978, "moduleForcesX": [ - -68.82905489435568, - -68.82905489435568, - -68.82905489435568, - -68.82905489435568 + -69.51524637407707, + -69.51524637407705, + -69.51524637407707, + -69.51524637407705 ], "moduleForcesY": [ - 12.438452238151038, - 12.438452238151038, - 12.438452238151038, - 12.438452238151038 + 7.751819786448097, + 7.751819786448097, + 7.751819786448097, + 7.751819786448097 ], - "timestamp": 2.9696548780381775 + "timestamp": 2.868961385774363 }, { - "x": 8.66295079020209, - "y": 0.6691700762270508, - "heading": -0.030602705565700657, - "angularVelocity": -2.1439007784276e-22, - "velocityX": 1.2454927522590922, - "velocityY": 0.10703396995207695, + "x": 8.712086900940639, + "y": 0.9928580419153559, + "heading": -0.03060270556570087, + "angularVelocity": 7.6697962024819e-19, + "velocityX": 1.2641939916274305, + "velocityY": 0.03728258479896153, "moduleForcesX": [ - -68.99232691579692, - -68.99232691579692, - -68.99232691579692, - -68.99232691579692 + -69.59819435728923, + -69.59819435728923, + -69.59819435728923, + -69.59819435728923 ], "moduleForcesY": [ - 11.535888453791287, - 11.535888453791287, - 11.535888453791285, - 11.535888453791285 + 7.026619700405902, + 7.026619700405903, + 7.0266197004059014, + 7.0266197004059014 ], - "timestamp": 3.0352675175686374 + "timestamp": 2.9360875263013444 }, { - "x": 8.728605558744404, - "y": 0.678698482418167, - "heading": -0.030602705565700657, - "angularVelocity": -2.1432640541955167e-22, - "velocityX": 1.0006420868319634, - "velocityY": 0.14522211359432977, + "x": 8.780000437865695, + "y": 0.996921987964414, + "heading": -0.03060270556570085, + "angularVelocity": 7.669398274852111e-19, + "velocityX": 1.011730100850266, + "velocityY": 0.06054192922688254, "moduleForcesX": [ - -69.11964904618539, - -69.11964904618539, - -69.11964904618539, - -69.11964904618539 + -69.66190364637255, + -69.66190364637255, + -69.66190364637255, + -69.66190364637255 ], "moduleForcesY": [ - 10.780248776015798, - 10.780248776015798, - 10.780248776015798, - 10.780248776015798 + 6.417908736725351, + 6.417908736725351, + 6.417908736725356, + 6.417908736725355 ], - "timestamp": 3.1008801570990974 + "timestamp": 3.0032136668283256 }, { - "x": 8.778171440675647, - "y": 0.6905833152058894, - "heading": -0.030602705565700657, - "angularVelocity": -2.1427532944253698e-22, - "velocityX": 0.7554319150387594, - "velocityY": 0.18113633093826434, + "x": 8.830954883407468, + "y": 1.0024211476734168, + "heading": -0.030602705565700838, + "angularVelocity": 7.669008842488089e-19, + "velocityX": 0.75908498748391, + "velocityY": 0.08192277502968102, "moduleForcesX": [ - -69.22113520637045, - -69.22113520637045, - -69.22113520637045, - -69.22113520637045 + -69.71190806731377, + -69.71190806731377, + -69.71190806731377, + -69.71190806731377 ], "moduleForcesY": [ - 10.138335112343585, - 10.138335112343585, - 10.138335112343585, - 10.138335112343585 + 5.89957801675687, + 5.899578016756869, + 5.899578016756865, + 5.899578016756865 ], - "timestamp": 3.1664927966295573 + "timestamp": 3.0703398073553068 }, { - "x": 8.811629282013929, - "y": 0.7046962431476756, - "heading": -0.030602705565700657, - "angularVelocity": -2.14229895305694e-22, - "velocityX": 0.5099298180611922, - "velocityY": 0.2150946531458243, + "x": 8.86494051215884, + "y": 1.0092468204121565, + "heading": -0.030602705565700824, + "angularVelocity": 7.668631929139065e-19, + "velocityX": 0.5062949915570888, + "velocityY": 0.10168427210552212, "moduleForcesX": [ - -69.303543666465, - -69.303543666465, - -69.303543666465, - -69.303543666465 + -69.7518852496981, + -69.7518852496981, + -69.7518852496981, + -69.7518852496981 ], "moduleForcesY": [ - 9.586199445644239, - 9.586199445644237, - 9.586199445644244, - 9.586199445644244 + 5.452754058783733, + 5.452754058783732, + 5.452754058783737, + 5.452754058783736 ], - "timestamp": 3.232105436160017 + "timestamp": 3.137465947882288 }, { - "x": 8.828963279724121, - "y": 0.7209256887435913, - "heading": -0.030602705565700657, - "angularVelocity": 3.635197207581808e-20, - "velocityX": 0.26418686756452614, - "velocityY": 0.24735242648455466, + "x": 8.881949424743652, + "y": 1.017304301261902, + "heading": -0.030602705565700813, + "angularVelocity": -6.192148052914439e-17, + "velocityX": 0.25338731604818493, + "velocityY": 0.12003491913118446, "moduleForcesX": [ - -69.3715349487538, - -69.3715349487538, - -69.3715349487538, - -69.3715349487538 + -69.78435636275017, + -69.78435636275017, + -69.78435636275017, + -69.78435636275017 ], "moduleForcesY": [ - 9.10614626386413, - 9.10614626386413, - 9.10614626386413, - 9.10614626386413 + 5.063460762339646, + 5.063460762339647, + 5.063460762339649, + 5.06346076233965 ], - "timestamp": 3.297718075690477 + "timestamp": 3.204592088409269 }, { - "x": 8.828203904774893, - "y": 0.741593975643098, - "heading": -0.03505435831192594, - "angularVelocity": -0.06067171185490822, - "velocityX": -0.010349544480640927, - "velocityY": 0.28168871625594366, + "x": 8.880530486218973, + "y": 1.027348591072611, + "heading": -0.034530809902176336, + "angularVelocity": -0.054161349122254336, + "velocityX": -0.019564557922623505, + "velocityY": 0.13849232110933965, "moduleForcesX": [ - -69.82238913204013, - -69.84216616588223, - -68.65753096355249, - -68.89031773540574 + -69.96707712828831, + -69.96918542605992, + -69.4046006596549, + -69.48963562370783 ], "moduleForcesY": [ - 4.5647052079056, - 4.326632215396829, - 13.498725767769074, - 12.28091309563698 + 0.7821062724142991, + 0.9492571184496107, + 8.889772333230777, + 8.233787400699313 ], - "timestamp": 3.3710908652513036 + "timestamp": 3.277118057576761 }, { - "x": 8.807299851592818, - "y": 0.7647796846719664, - "heading": -0.04385437316260252, - "angularVelocity": -0.11993567238412434, - "velocityX": -0.2849019821543826, - "velocityY": 0.31599873969146675, + "x": 8.859315047264229, + "y": 1.03873366112627, + "heading": -0.04230269871701783, + "angularVelocity": -0.10716008216259976, + "velocityX": -0.29252196417731824, + "velocityY": 0.15697921977944768, "moduleForcesX": [ - -69.81588658834606, - -69.83182481459568, - -68.68488260159317, - -68.89599182771352 + -69.96360527609723, + -69.96507878923691, + -69.41623345810719, + -69.49123274928873 ], "moduleForcesY": [ - 4.61919336637954, - 4.448281304435051, - 13.343438108461418, - 12.233541145410339 + 0.849054357453386, + 1.0605014457335615, + 8.776783714333206, + 8.198715576096298 ], - "timestamp": 3.44446365481213 + "timestamp": 3.3496440267442527 }, { - "x": 8.766249783637008, - "y": 0.7904802826298837, - "heading": -0.05688378166614004, - "angularVelocity": -0.17757820823666606, - "velocityX": -0.559472635584879, - "velocityY": 0.3502742380622083, + "x": 8.81830267354523, + "y": 1.0514616761704456, + "heading": -0.053822469418188024, + "angularVelocity": -0.15883649447922438, + "velocityX": -0.5654853591032976, + "velocityY": 0.17549596634529624, "moduleForcesX": [ - -69.80907759591098, - -69.8190745845742, - -68.71780064776569, - -68.90102631540906 + -69.95969350130241, + -69.9600439550638, + -69.43034005880952, + -69.49219040870635 ], "moduleForcesY": [ - 4.671002982456192, - 4.596843952033549, - 13.154721929440731, - 12.187023469399424 + 0.916057569851737, + 1.1945950847132014, + 8.639230967068034, + 8.165662210562154 ], - "timestamp": 3.5178364443729566 + "timestamp": 3.4221699959117444 }, { - "x": 8.705052173115664, - "y": 0.8186924642311597, - "heading": -0.07400373694683447, - "angularVelocity": -0.2333283957604187, - "velocityX": -0.8340641113366615, - "velocityY": 0.3845046885928745, + "x": 8.75749289682575, + "y": 1.0655348258499984, + "heading": -0.06897986858552595, + "angularVelocity": -0.2089927144919403, + "velocityX": -0.838455210147377, + "velocityY": 0.1940429040948802, "moduleForcesX": [ - -69.80139202754168, - -69.80353156229272, - -68.75655115056365, - -68.90652968620067 + -69.9551810729799, + -69.95383968280234, + -69.44688305083558, + -69.49295928173186 ], "moduleForcesY": [ - 4.726967109732889, - 4.773715711987706, - 12.929025319368964, - 12.134397256845864 + 0.9882804241447996, + 1.352618156589803, + 8.475469948022155, + 8.130018720639756 ], - "timestamp": 3.591209233933783 + "timestamp": 3.494695965079236 }, { - "x": 8.623705265045926, - "y": 0.8494119202761454, - "heading": -0.09504978733566269, - "angularVelocity": -0.28683726644168145, - "velocityX": -1.108679505803711, - "velocityY": 0.41867640890931357, + "x": 8.67688521696236, + "y": 1.080955321413936, + "heading": -0.08764661363140669, + "angularVelocity": -0.2573801530723142, + "velocityX": -1.1114319572514513, + "velocityY": 0.2126203309097312, "moduleForcesX": [ - -69.79205213254602, - -69.78467756927584, - -68.80147068719397, - -68.91395589602016 + -69.94983324196707, + -69.94615057660823, + -69.46581320027535, + -69.49411065050936 ], "moduleForcesY": [ - 4.795991300911855, - 4.980866247525471, - 12.661611125036274, - 12.06633404852973 + 1.0723396108261634, + 1.5360717401888286, + 8.283336243627176, + 8.085785369496797 ], - "timestamp": 3.6645820234946096 + "timestamp": 3.567221934246728 }, { - "x": 8.522207036579841, - "y": 0.8826330056369158, - "heading": -0.11982385908892228, - "angularVelocity": -0.3376465839931265, - "velocityX": -1.3833224697275863, - "velocityY": 0.45277119160407436, + "x": 8.576479109537585, + "y": 1.0977253883199334, + "heading": -0.1096713977003478, + "angularVelocity": -0.3036813478255371, + "velocityX": -1.3844159351099252, + "velocityY": 0.23122844270129664, "moduleForcesX": [ - -69.77997172662764, - -69.76180279178641, - -68.85297448408464, - -68.92524546744818 + -69.94330252963142, + -69.9365583049015, + -69.4870568794036, + -69.49637643933679 ], "moduleForcesY": [ - 4.889881316073384, - 5.2210669442672835, - 12.346127343474148, - 11.970039561538547 + 1.176823061341541, + 1.7470252667944042, + 8.060012105446882, + 8.025018376878974 ], - "timestamp": 3.737954813055436 + "timestamp": 3.6397479034142197 }, { - "x": 8.400555154994208, - "y": 0.9183482483752978, - "heading": -0.14808264155198017, - "angularVelocity": -0.3851398131678119, - "velocityX": -1.6579972264075493, - "velocityY": 0.48676413902423377, + "x": 8.456274044587177, + "y": 1.1158472524014584, + "heading": -0.13487291738284943, + "angularVelocity": -0.347482701327652, + "velocityX": -1.6574072202027315, + "velocityY": 0.24986724465107038, "moduleForcesX": [ - -69.76356912538407, - -69.73390347020135, - -68.91158189541561, - -68.94304262893307 + -69.93505805724766, + -69.92449408332656, + -69.51050132115871, + -69.50070530398553 ], "moduleForcesY": [ - 5.024739981390052, - 5.498335546770225, - 11.973827493944482, - 11.82738424849502 + 1.3131272162432313, + 1.98838442321693, + 7.801780109725148, + 7.936938199533164 ], - "timestamp": 3.8113276026162626 + "timestamp": 3.7122738725817115 }, { - "x": 8.258746945464983, - "y": 0.9565475945096078, - "heading": -0.1795200221320126, - "angularVelocity": -0.42846102442337525, - "velocityX": -1.932708438346409, - "velocityY": 0.5206200604195711, + "x": 8.316269527386241, + "y": 1.1353231154725127, + "heading": -0.16302980404777723, + "angularVelocity": -0.38823178770838385, + "velocityX": -1.930405326643809, + "velocityY": 0.26853640557469155, "moduleForcesX": [ - -69.74040444259873, - -69.6994878605935, - -68.97797470393978, - -68.97104071246528 + -69.92425253763372, + -69.90915580434691, + -69.53597671993317, + -69.50834196529713 ], "moduleForcesY": [ - 5.223448587988533, - 5.818826004851896, - 11.532106083145074, - 11.611544934432994 + 1.4968639656801608, + 2.264388307053663, + 7.503564176470051, + 7.80642632120717 ], - "timestamp": 3.884700392177089 + "timestamp": 3.7847998417492033 }, { - "x": 8.096779401992974, - "y": 0.9972171857084491, - "heading": -0.21373905317621672, - "angularVelocity": -0.46637222394054323, - "velocityX": -2.2074606191405195, - "velocityY": 0.5542871061911322, + "x": 8.156465184614108, + "y": 1.1561551126608645, + "heading": -0.1938654641731384, + "angularVelocity": -0.42516715708305197, + "velocityX": -2.2034085804916796, + "velocityY": 0.28723500599140817, "moduleForcesX": [ - -69.70643217846147, - -69.65617966490848, - -69.0531155698116, - -69.01454849786487 + -69.9094604865732, + -69.88935388472542, + -69.5632321514312, + -69.52093877070061 ], "moduleForcesY": [ - 5.520358534554248, - 6.192692191683197, - 11.001614243637365, - 11.280543987218508 + 1.7503444073984757, + 2.581564547481693, + 7.158055486956702, + 7.611351867199961 ], - "timestamp": 3.9580731817379156 + "timestamp": 3.857325810916695 }, { - "x": 7.914649343266423, - "y": 1.0403372462568932, - "heading": -0.2502040431261532, - "angularVelocity": -0.49698246677273733, - "velocityX": -2.482256158130169, - "velocityY": 0.5876846281372092, + "x": 7.976860949117206, + "y": 1.1783452360639273, + "heading": -0.2270240484884978, + "angularVelocity": -0.45719601814734345, + "velocityX": -2.4764127602640795, + "velocityY": 0.3059610737751436, "moduleForcesX": [ - -69.6543272195074, - -69.59982811388493, - -69.13847224033395, - -69.08142915869702 + -69.88812974316788, + -69.86320182607882, + -69.59189618603347, + -69.5407034064261 ], "moduleForcesY": [ - 5.970871351408315, - 6.638279683857717, - 10.350201343001592, - 10.763705620631843 + 2.107233171390243, + 2.950662034921671, + 6.753983199950573, + 7.317496840416227 ], - "timestamp": 4.031445971298742 + "timestamp": 3.929851780084187 }, { - "x": 7.712354049624925, - "y": 1.0858780921373514, - "heading": -0.28815099090474133, - "angularVelocity": -0.517180115485863, - "velocityX": -2.7570887634549326, - "velocityY": 0.6206775856968689, + "x": 7.777457476125625, + "y": 1.201895191338467, + "heading": -0.26202980984331203, + "angularVelocity": -0.4826651991962036, + "velocityX": -2.74940790561615, + "velocityY": 0.3247106594351006, "moduleForcesX": [ - -69.56928399894086, - -69.52224882009111, - -69.23640181351112, - -69.18354973211308 + -69.85533117072832, + -69.82743416827404, + -69.62139360036666, + -69.57054322743649 ], "moduleForcesY": [ - 6.673022110744875, - 7.1925990350588, - 9.51901410508811, - 9.929914946454016 + 2.6219385333270773, + 3.3908613060937713, + 6.272491751359185, + 6.868108041742767 ], - "timestamp": 4.104818760859568 + "timestamp": 4.0023777492516786 }, { - "x": 7.489893664384698, - "y": 1.133791594453917, - "heading": -0.3263979812876508, - "angularVelocity": -0.5212694053454582, - "velocityX": -3.03191941551856, - "velocityY": 0.6530145930576222, + "x": 7.558257164116274, + "y": 1.2268061069502, + "heading": -0.2982123205048736, + "angularVelocity": -0.49889041232006653, + "velocityX": -3.022369980373853, + "velocityY": 0.3434758045660868, "moduleForcesX": [ - -69.41666539245078, - -69.40439565809636, - -69.35057994409232, - -69.33787106983414 + -69.80056727463466, + -69.77570886950674, + -69.6507199544232, + -69.61392322278483 ], "moduleForcesY": [ - 7.822962646502443, - 7.941889554066643, - 8.38638483270236, - 8.500969214195738 + 3.39043477997722, + 3.9399463888437256, + 5.678903958822243, + 6.160009083534089 ], - "timestamp": 4.178191550420395 + "timestamp": 4.07490371841917 }, { - "x": 7.247281611648338, - "y": 1.183989156390833, - "heading": -0.3628608681557275, - "angularVelocity": -0.4969538038055478, - "velocityX": -3.3065671100760374, - "velocityY": 0.6841441116982726, + "x": 7.319267013948266, + "y": 1.2530778743434587, + "heading": -0.3345519884568234, + "angularVelocity": -0.5010573229066131, + "velocityX": -3.2952355261337996, + "velocityY": 0.3622394529136627, "moduleForcesX": [ - -69.09618755623467, - -69.18927818386388, - -69.48444757611301, - -69.55485789829521 + -69.69826365815047, + -69.69360285262502, + -69.67768220142874, + -69.67276264637083 ], "moduleForcesY": [ - 9.887138047237972, - 9.135257071526894, - 6.655220631175712, - 5.755332008623882 + 4.602649202702627, + 4.682797548651256, + 4.9016741603909, + 4.9806442793840855 ], - "timestamp": 4.251564339981221 + "timestamp": 4.147429687586662 }, { - "x": 6.9846108615466544, - "y": 1.2362643691091493, - "heading": -0.3928314758501629, - "angularVelocity": -0.40847033176501496, - "velocityX": -3.5799477118684186, - "velocityY": 0.7124604779402591, + "x": 7.060508520408783, + "y": 1.2807073809011449, + "heading": -0.3693010360245331, + "angularVelocity": -0.479125587246758, + "velocityX": -3.5678046982302343, + "velocityY": 0.38096018398438164, "moduleForcesX": [ - -68.19397798012086, - -68.604096270111, - -69.61287896515513, - -69.63437448602505 + -69.47236753682394, + -69.53951749408205, + -69.69564436263259, + -69.73202523829484 ], "moduleForcesY": [ - 14.31877505066931, - 11.837279257782388, - 3.3817824033703014, - -0.9454620249074384 + 6.700012628800592, + 5.851281299757406, + 3.768542257976925, + 2.8040874089418844 ], - "timestamp": 4.324937129542048 + "timestamp": 4.219955656754154 }, { - "x": 6.703415297553567, - "y": 1.290040410483288, - "heading": -0.3945237678570967, - "angularVelocity": -0.02306429968198066, - "velocityX": -3.8324229687352247, - "velocityY": 0.7329153177358495, + "x": 6.7820675423003, + "y": 1.3096827333084073, + "heading": -0.398737971474847, + "angularVelocity": -0.40588131104575736, + "velocityX": -3.839190035026568, + "velocityY": 0.39951692807238764, "moduleForcesX": [ - -63.16947604320132, - -61.34983762111749, - -69.05769594451374, - -61.35920225606092 + -68.80858735271256, + -69.12938539141281, + -69.67366590353845, + -69.61858416268272 ], "moduleForcesY": [ - 28.4635156424948, - 30.27498948987804, - -6.582220178863257, - -31.50206546085697 + 10.998366071640108, + 8.339611523596144, + 1.7520720276263266, + -2.1336448340952816 ], - "timestamp": 4.398309919102874 + "timestamp": 4.292481625921646 }, { - "x": 6.41959619572841, - "y": 1.3315714425952379, - "heading": -0.3945728781781997, - "angularVelocity": -0.0006693260730159474, - "velocityX": -3.868179246338573, - "velocityY": 0.566027710824874, + "x": 6.484732150451617, + "y": 1.3400281940076402, + "heading": -0.4090628910860552, + "angularVelocity": -0.142361690988409, + "velocityX": -4.099709321525832, + "velocityY": 0.4184082067095552, "moduleForcesX": [ - -9.773676203527769, - -7.669404194901154, - -10.354384240238268, - -8.307340543871192 + -65.20316881632876, + -65.7489206217446, + -69.31239778456624, + -65.86566076035609 ], "moduleForcesY": [ - -41.36655808090332, - -42.00286570902313, - -42.261025709276105, - -42.88386291727224 + 23.606521909357, + 20.444429351523326, + -3.7507078277888724, + -21.00209915007702 ], - "timestamp": 4.4716827086637005 + "timestamp": 4.365007595089137 }, { - "x": 6.135687490084942, - "y": 1.372487307589655, - "heading": -0.3946215087945329, - "angularVelocity": -0.0006627881619921617, - "velocityX": -3.8694004595273754, - "velocityY": 0.5576435793067078, + "x": 6.1842404767603805, + "y": 1.3742213267083638, + "heading": -0.40911116386317103, + "angularVelocity": -0.0006655929961390624, + "velocityX": -4.1432286550667365, + "velocityY": 0.4714605415577361, "moduleForcesX": [ - -0.30840180759639224, - -0.3079749898198458, - -0.30858337724113316, - -0.30815655961690325 + -13.325449562031311, + -4.164768682569861, + -18.043544953356022, + -8.922855853125498 ], "moduleForcesY": [ - -2.116157086062331, - -2.116334618026183, - -2.1165925122839986, - -2.116770044133725 + 19.90592667522208, + 16.82639877386449, + 10.611237303210721, + 6.85137196260812 ], - "timestamp": 4.545055498224527 + "timestamp": 4.437533564256629 }, { - "x": 5.85352801680754, - "y": 1.4241018905198637, - "heading": -0.39467053194899787, - "angularVelocity": -0.0006681380762323543, - "velocityX": -3.845560117943876, - "velocityY": 0.7034567342900305, + "x": 5.886052139153465, + "y": 1.4246963553310026, + "heading": -0.4091589633873657, + "angularVelocity": -0.0006590677069691712, + "velocityX": -4.111469878027867, + "velocityY": 0.6959580023818284, "moduleForcesX": [ - 6.0183308507069375, - 6.017872402285157, - 6.018488007158529, - 6.018029561236691 + 8.110433382725144, + 8.111187384355791, + 8.110199033033277, + 8.110953042580169 ], "moduleForcesY": [ - 36.80841670957, - 36.80855524647377, - 36.808665373492616, - 36.808803909707684 + 57.33325913716358, + 57.33311245310866, + 57.33314368701091, + 57.33299700322065 ], - "timestamp": 4.6184282877853535 + "timestamp": 4.510059533424121 }, { - "x": 5.5751319379159305, - "y": 1.493186559151982, - "heading": -0.39472121211754135, - "angularVelocity": -0.0006907215719449539, - "velocityX": -3.7942687003990416, - "velocityY": 0.941557068303184, + "x": 5.591555275132608, + "y": 1.4934998764417577, + "heading": -0.4092089626864391, + "angularVelocity": -0.0006893985650598928, + "velocityX": -4.060571232640143, + "velocityY": 0.94867427351207, "moduleForcesX": [ - 12.948916142731543, - 12.946169957519793, - 12.949515473189427, - 12.946769400230588 + 13.000242789451551, + 12.99626661282106, + 13.001163654267753, + 12.99718771199267 ], "moduleForcesY": [ - 60.104836688704204, - 60.10549713360235, - 60.105088950128234, - 60.105749386647936 + 64.53930421343945, + 64.54015945588058, + 64.53937921708196, + 64.54023445819007 ], - "timestamp": 4.69180107734618 + "timestamp": 4.582585502591613 }, { - "x": 5.302053451538086, - "y": 1.5809556245803833, - "heading": -0.39477498805993555, - "angularVelocity": -0.0007329139687353689, - "velocityX": -3.721795068885318, - "velocityY": 1.196207285476589, + "x": 5.302053451538088, + "y": 1.5809556245803824, + "heading": -0.4092627987960467, + "angularVelocity": -0.000742301140211202, + "velocityX": -3.991698793103366, + "velocityY": 1.2058542497602232, "moduleForcesX": [ - 18.297412052733495, - 18.291870609159098, - 18.298156444631005, - 18.292615475816596 + 17.591944196413664, + 17.584790178826015, + 17.593089676218266, + 17.5859364455025 ], "moduleForcesY": [ - 64.28225176659839, - 64.2838616989904, - 64.2823244352569, - 64.2839342864753 + 65.6787937062123, + 65.68074285605304, + 65.67871632504414, + 65.68066538032537 ], - "timestamp": 4.7651738669070065 + "timestamp": 4.6551114717591044 }, { - "x": 5.07390423214392, - "y": 1.6694230318569008, - "heading": -0.3948239257142182, - "angularVelocity": -0.0007817718663467028, - "velocityX": -3.644650395807531, - "velocityY": 1.4132538862180757, + "x": 5.060781264234282, + "y": 1.668503203961895, + "heading": -0.4093136867686553, + "angularVelocity": -0.0008266775577253413, + "velocityX": -3.9194782641972647, + "velocityY": 1.4222146294751892, "moduleForcesX": [ - 22.829580522567618, - 22.8220128584525, - 22.830066989794148, - 22.82250022697142 + 21.736557880357715, + 21.723118914680356, + 21.73784859212575, + 21.72441246515569 ], "moduleForcesY": [ - 64.21970113385147, - 64.22240309693508, - 64.21976226256318, - 64.22246395577116 + 65.09863070377499, + 65.10314246149298, + 65.09850389768496, + 65.10301496712766 ], - "timestamp": 4.827772249362731 + "timestamp": 4.716668692144205 }, { - "x": 4.851229056834639, - "y": 1.7708905614247654, - "heading": -0.3948711845379385, - "angularVelocity": -0.0007549527937680354, - "velocityX": -3.5572033425428917, - "velocityY": 1.6209289375749065, + "x": 4.824591541574993, + "y": 1.768967581170856, + "heading": -0.4093625786347868, + "angularVelocity": -0.0007942507121868569, + "velocityX": -3.836913382080745, + "velocityY": 1.632048630208832, "moduleForcesX": [ - 25.872414895839015, - 25.87639732222832, - 25.872346027139017, - 25.876328704106957 + 24.840611640815233, + 24.84563714723139, + 24.840370639682614, + 24.84539654710804 ], "moduleForcesY": [ - 61.44911151867199, - 61.44743545397816, - 61.44890652890951, - 61.447230357726816 + 63.138133963812095, + 63.136150247952436, + 63.13804254630454, + 63.13605868744737 ], - "timestamp": 4.890370631818455 + "timestamp": 4.778225912529306 }, { - "x": 4.634382563500368, - "y": 1.8842851362906166, - "heading": -0.3949168954426069, - "angularVelocity": -0.000730225013412799, - "velocityX": -3.4640910008760675, - "velocityY": 1.8114617409811675, + "x": 4.5940300417851905, + "y": 1.8817568503359232, + "heading": -0.40940943255755435, + "angularVelocity": -0.00076114422441599, + "velocityX": -3.7454826314023215, + "velocityY": 1.8322670916501005, "moduleForcesX": [ - 27.548950472331388, - 27.55233169388033, - 27.548972415747215, - 27.552353816038643 + 27.508225922613867, + 27.513128573012004, + 27.508193765462636, + 27.51309679763414 ], "moduleForcesY": [ - 56.377068082107755, - 56.375442909315176, - 56.3765916436959, - 56.3749663731007 + 60.24514716829222, + 60.242914588066654, + 60.24482580432994, + 60.242593041950194 ], - "timestamp": 4.95296901427418 + "timestamp": 4.839783132914407 }, { - "x": 4.422622805341516, - "y": 2.0069183796517893, - "heading": -0.39496118737413566, - "angularVelocity": -0.0007075571251397378, - "velocityX": -3.382831150767024, - "velocityY": 1.9590481183425312, + "x": 4.369349606253567, + "y": 2.0058517685581454, + "heading": -0.4094544926877705, + "angularVelocity": -0.0007320039783130824, + "velocityX": -3.6499444602278026, + "velocityY": 2.015927903272623, "moduleForcesX": [ - 24.042485271707772, - 24.045024480603296, - 24.042303530281014, - 24.044842834314085 + 28.744538248232754, + 28.74851367433131, + 28.744577870233446, + 28.74855354416703 ], "moduleForcesY": [ - 43.66989864479118, - 43.66861606069169, - 43.66891333264767, - 43.66763069286199 + 55.26315032046569, + 55.261123506293146, + 55.262531653571756, + 55.260504693222536 ], - "timestamp": 5.015567396729904 + "timestamp": 4.901340353299508 }, { - "x": 4.213243294089462, - "y": 2.1335730846638814, - "heading": -0.3950047652007233, - "angularVelocity": -0.0006961494031973518, - "velocityX": -3.344807054721382, - "velocityY": 2.023290379774191, + "x": 4.149973859162333, + "y": 2.13910073076137, + "heading": -0.4094996155694483, + "angularVelocity": -0.0007330233788204265, + "velocityX": -3.563769541880253, + "velocityY": 2.1646357871525597, "moduleForcesX": [ - 11.25048576449635, - 11.251433113415713, - 11.250173496493936, - 11.251120853755697 + 25.92928590684574, + 25.929168249602895, + 25.929292292013468, + 25.929174634983514 ], "moduleForcesY": [ - 19.00901574015739, - 19.008592674544687, - 19.00819786419031, - 19.007774795539515 + 44.74476438288918, + 44.74482729836071, + 44.74480699802511, + 44.744869913360205 ], - "timestamp": 5.078165779185628 + "timestamp": 4.962897573684609 }, { - "x": 4.004677598787759, - "y": 2.2615635179547406, - "heading": -0.39504821197194406, - "angularVelocity": -0.0006940558128879706, - "velocityX": -3.3318064639964438, - "velocityY": 2.0446284435765874, + "x": 3.9353204017277856, + "y": 2.278088242273816, + "heading": -0.41252140844778834, + "angularVelocity": -0.04908917036026353, + "velocityX": -3.4870557197299563, + "velocityY": 2.2578587961400522, "moduleForcesX": [ - 3.8466474195591016, - 3.8468089460712034, - 3.846580474426278, - 3.8467420010287405 + 24.953488052198967, + 20.4567569278476, + 25.589392523319265, + 21.330260691348453 ], "moduleForcesY": [ - 6.31375253757312, - 6.313684558372743, - 6.313590446713757, - 6.313522467489779 + 25.14686564166064, + 27.7645218324562, + 28.406596524112476, + 30.881756387142097 ], - "timestamp": 5.140764161641353 + "timestamp": 5.02445479406971 }, { - "x": 3.796955991966042, - "y": 2.3909094414143865, - "heading": -0.39510465976032694, - "angularVelocity": -0.0009017451596737262, - "velocityX": -3.318322274040202, - "velocityY": 2.066282200680384, + "x": 3.7318642433739195, + "y": 2.4104934513603675, + "heading": -0.43615839467618756, + "angularVelocity": -0.38398397589611083, + "velocityX": -3.305155058676315, + "velocityY": 2.150928977270674, "moduleForcesX": [ - 3.9944820139638706, - 3.9784542931865783, - 4.001115299988056, - 3.9850884886982674 + 49.89592032127635, + 34.025418039882794, + 66.93891398556973, + 68.06860648029641 ], "moduleForcesY": [ - 6.39563239881223, - 6.402385340358186, - 6.411708329457698, - 6.418461041024309 + -47.83606477558671, + -59.36994474295037, + -17.100092585775965, + -4.390644167479697 ], - "timestamp": 5.203362544097077 + "timestamp": 5.086012014454811 }, { - "x": 3.597726364612312, - "y": 2.517446428244699, - "heading": -0.42282084717405416, - "angularVelocity": -0.4427620383534821, - "velocityX": -3.1826641446309787, - "velocityY": 2.021409849045414, + "x": 3.540415993553555, + "y": 2.5354156131468453, + "heading": -0.46498951807167194, + "angularVelocity": -0.4683629835017646, + "velocityX": -3.110086008150945, + "velocityY": 2.0293665146828603, "moduleForcesX": [ - 43.79907538164046, - 18.529360915334063, - 57.13510334534536, - 41.093905736742414 + 55.83155696216655, + 54.18327481859195, + 62.40116195557468, + 62.36184788712686 ], "moduleForcesY": [ - -35.85922709226587, - -37.9829446448005, - -1.354929099210182, - 22.08867174295008 + -41.50061361131458, + -43.48191734914588, + -30.734501757153353, + -30.59101829583353 ], - "timestamp": 5.265960926552801 + "timestamp": 5.147569234839912 }, { - "x": 3.4109531206363775, - "y": 2.6365558877135373, - "heading": -0.45770016209152625, - "angularVelocity": -0.5571919520786697, - "velocityX": -2.9836752428553495, - "velocityY": 1.9027561862814057, + "x": 3.361022505593955, + "y": 2.652736487945131, + "heading": -0.4956638148847116, + "angularVelocity": -0.49830542414467005, + "velocityX": -2.9142558230751625, + "velocityY": 1.9058832426858676, "moduleForcesX": [ - 55.40246671070086, - 52.925950183673045, - 63.50311672291328, - 63.68070488263328 + 57.73901804404707, + 57.438460533401326, + 60.30587505828676, + 60.21056114711604 ], "moduleForcesY": [ - -41.613108609361404, - -44.404319792002745, - -27.690981599642193, - -26.723490845822106 + -39.07130416952509, + -39.474125567373356, + -34.97681089661358, + -35.097625013264334 ], - "timestamp": 5.328559309008526 + "timestamp": 5.209126455225013 }, { - "x": 3.236759101607606, - "y": 2.7480086789134943, - "heading": -0.49382230864067395, - "angularVelocity": -0.5770460055369145, - "velocityX": -2.7827239649839024, - "velocityY": 1.7804420310506774, + "x": 3.1936838062667205, + "y": 2.7624068712077934, + "heading": -0.5266594310996341, + "angularVelocity": -0.5035252732516518, + "velocityX": -2.7184252030934304, + "velocityY": 1.7816006404539642, "moduleForcesX": [ - 58.68329565175249, - 58.536897284256675, - 60.338571121696745, - 60.27603405724038 + 58.700566811459886, + 58.67734780171254, + 59.16682840463337, + 59.14969530624501 ], "moduleForcesY": [ - -37.48822564336687, - -37.68393200712494, - -34.759584038339426, - -34.83251521985918 + -37.749793809130026, + -37.78083859729802, + -37.014524694721786, + -37.03675261323169 ], - "timestamp": 5.39115769146425 + "timestamp": 5.270683675610114 }, { - "x": 3.0751440767985594, - "y": 2.851743750610944, - "heading": -0.529353385941278, - "angularVelocity": -0.5676037607798451, - "velocityX": -2.5817763729495224, - "velocityY": 1.6571525912961371, + "x": 3.038390509303798, + "y": 2.8643948429617936, + "heading": -0.5570773161987834, + "angularVelocity": -0.4941400035435474, + "velocityX": -2.5227470634868117, + "velocityY": 1.6567994967278303, "moduleForcesX": [ - 59.85882515009252, - 59.8712013872914, - 59.03484963172741, - 59.065559593293514 + 59.30031933124251, + 59.30307854027269, + 58.44288594465124, + 58.46463470158717 ], "moduleForcesY": [ - -35.82050595797904, - -35.81050714543758, - -37.16303360424269, - -37.12450292455551 + -36.885288676049825, + -36.88814111006628, + -38.229406656940725, + -38.203170518961 ], - "timestamp": 5.453756073919974 + "timestamp": 5.332240895995215 }, { - "x": 2.9260957467057915, - "y": 2.9477247616331526, - "heading": -0.563363095015486, - "angularVelocity": -0.5433001259779049, - "velocityX": -2.381025263682967, - "velocityY": 1.5332826066247958, + "x": 2.895129799571264, + "y": 2.9586740032068897, + "heading": -0.5863104114525834, + "angularVelocity": -0.4748930356337662, + "velocityX": -2.3272771063459046, + "velocityY": 1.5315694837305227, "moduleForcesX": [ - 60.486958658404426, - 60.41900268077107, - 58.32195697914738, - 58.369971326869816 + 59.717187192796516, + 59.657546410038414, + 57.934272823511364, + 57.95135152475748 ], "moduleForcesY": [ - -34.87489354988502, - -35.01332620759317, - -38.3857086444205, - -38.33172202686071 + -36.2638573648305, + -36.37433120370207, + -39.04885966973737, + -39.03512982178747 ], - "timestamp": 5.516354456375699 + "timestamp": 5.3937981163803155 }, { - "x": 2.789594973687008, - "y": 3.0359157910118872, - "heading": -0.595278042536244, - "angularVelocity": -0.5098366166782579, - "velocityX": -2.1805798754517425, - "velocityY": 1.4088387897420815, + "x": 2.763886371067874, + "y": 3.045218511804947, + "heading": -0.6139222009668763, + "angularVelocity": -0.4485548460699908, + "velocityX": -2.132055795929944, + "velocityY": 1.4059196964488687, "moduleForcesX": [ - 60.865075509815334, - 60.664615260538106, - 57.84868096481913, - 57.85768343206302 + 60.015428697215164, + 59.85831037769707, + 57.55335306922513, + 57.534003758004665 ], "moduleForcesY": [ - -34.288218926493336, - -34.66454967020803, - -39.16244097534043, - -39.169596867030975 + -35.80957668058033, + -36.086111428235434, + -39.64516852205637, + -39.68654602916551 ], - "timestamp": 5.578952838831423 + "timestamp": 5.455355336765416 }, { - "x": 2.6656099583026958, - "y": 3.1162638597823777, - "heading": -0.6247411596443208, - "angularVelocity": -0.4706689846006165, - "velocityX": -1.9806424786136814, - "velocityY": 1.2835486416493385, + "x": 2.644643305476117, + "y": 3.1240017980714323, + "heading": -0.6395960472514493, + "angularVelocity": -0.41707286527865206, + "velocityX": -1.937109324394044, + "velocityY": 1.2798382671215223, "moduleForcesX": [ - 61.0383972954406, - 60.7049969301487, - 57.47499394607686, - 57.4164365296211 + 60.22326407527744, + 59.95837931731987, + 57.26239607352692, + 57.186270668215975 ], "moduleForcesY": [ - -34.03127548799677, - -34.6446033347473, - -39.75325119619021, - -39.857347146466026 + -35.49033319006886, + -35.95108026507241, + -40.09166456022141, + -40.21383292620102 ], - "timestamp": 5.6415512212871475 + "timestamp": 5.516912557150517 }, { - "x": 2.554110374142435, - "y": 3.1887192741774415, - "heading": -0.6515321237552656, - "angularVelocity": -0.42798173147515556, - "velocityX": -1.7811895417445422, - "velocityY": 1.1574646428973032, + "x": 2.537383808831169, + "y": 3.1949980499443154, + "heading": -0.6631036099321576, + "angularVelocity": -0.38188148414867673, + "velocityX": -1.7424356716228648, + "velocityY": 1.1533375196074727, "moduleForcesX": [ - 61.11771507187923, - 60.66368506097603, - 57.21052652343248, - 57.0695180689212 + 60.36537448302678, + 59.99512146624315, + 57.04267536961116, + 56.89878422586106 ], "moduleForcesY": [ - -33.92654139856753, - -34.75226319319532, - -40.16455016546468, - -40.382680062539976 + -35.27267783600763, + -35.913635660706774, + -40.42480723386195, + -40.64046601274349 ], - "timestamp": 5.704149603742872 + "timestamp": 5.578469777535618 }, { - "x": 2.455069827309922, - "y": 3.2532389005840123, - "heading": -0.6755130530260514, - "angularVelocity": -0.3830918360829452, - "velocityX": -1.5821582435067625, - "velocityY": 1.0306915909880887, + "x": 2.4420919536077847, + "y": 3.2581828688425367, + "heading": -0.6842805929390149, + "angularVelocity": -0.344021105475332, + "velocityX": -1.5480207622638111, + "velocityY": 1.0264404159729446, "moduleForcesX": [ - 61.14927721542877, - 60.589796141080875, - 57.02529080592471, - 56.798052441013304 + 60.4598584765929, + 59.99281023891339, + 56.876528556020105, + 56.661344392247265 ], "moduleForcesY": [ - -33.89819653017852, - -34.90689829521632, - -40.450798746634256, - -40.78566638525383 + -35.130249715774205, + -35.93621486669289, + -40.675063944105, + -40.98709766240873 ], - "timestamp": 5.766747986198596 + "timestamp": 5.640026997920719 }, { - "x": 2.368465422506688, - "y": 3.3097852773291314, - "heading": -0.696597572734595, - "angularVelocity": -0.33682211714426863, - "velocityX": -1.3834926943118622, - "velocityY": 0.9033200943349304, + "x": 2.3587528835015927, + "y": 3.3135334333216804, + "heading": -0.7030081263323279, + "angularVelocity": -0.3042296789257516, + "velocityX": -1.3538471942824237, + "velocityY": 0.8991725768784151, "moduleForcesX": [ - 61.152198837351406, - 60.50298885718338, - 56.892015366210785, - 56.582333241123905 + 60.5201219775476, + 59.967443909712394, + 56.748515348358374, + 56.463990903711384 ], "moduleForcesY": [ - -33.915173903721346, - -35.07698541197515, - -40.656437065616224, - -41.10124961605489 + -35.04253435660417, + -35.99355809852384, + -40.867195346689, + -41.27154173850131 ], - "timestamp": 5.8293463686543205 + "timestamp": 5.70158421830582 }, { - "x": 2.2942772044503497, - "y": 3.3583257141932483, - "heading": -0.7147311185238733, - "angularVelocity": -0.28968074058629395, - "velocityX": -1.1851459278330674, - "velocityY": 0.7754263762078791, + "x": 2.2873528689676483, + "y": 3.3610285743692394, + "heading": -0.7191989284072005, + "angularVelocity": -0.26302035689049935, + "velocityX": -1.1598966634176864, + "velocityY": 0.7715608461595652, "moduleForcesX": [ - 61.13821655127134, - 60.41423668737451, - 56.79179682900103, - 56.40799267466763 + 60.55636984330847, + 59.92996721273085, + 56.64674423048133, + 56.29855167271921 ], "moduleForcesY": [ - -33.958143361421655, - -35.245265434945416, - -40.81102911719036, - -41.35348048483787 + -34.99335531824572, + -36.06824538837232, + -41.01955138323022, + -41.50757259528159 ], - "timestamp": 5.891944751110045 + "timestamp": 5.763141438690921 }, { - "x": 2.2324876517353642, - "y": 3.398831509029573, - "heading": -0.7298786663246292, - "angularVelocity": -0.2419798596468502, - "velocityX": -0.9870790632439912, - "velocityY": 0.6470741454857069, + "x": 2.2278792676988664, + "y": 3.4006487477183196, + "heading": -0.7327874449132579, + "angularVelocity": -0.2207461029112197, + "velocityX": -0.966151507438403, + "velocityY": 0.6436316178868581, "moduleForcesX": [ - 61.11437176518798, - 60.32958782211226, - 56.7120114962976, - 56.26499529096948 + 60.57586928152923, + 59.88760410989669, + 56.562363081650766, + 56.15861493971373 ], "moduleForcesY": [ - -34.01552925312056, - -35.40260151463775, - -40.933861723710784, - -41.55859600904434 + -34.97098554580845, + -36.148822416267066, + -41.14545732873801, + -41.70558761712785 ], - "timestamp": 5.954543133565769 + "timestamp": 5.824698659076022 }, { - "x": 2.1830812470403056, - "y": 3.4312772741579374, - "heading": -0.7420173390092132, - "angularVelocity": -0.19391351994070552, - "velocityX": -0.7892600855941332, - "velocityY": 0.5183163502876942, + "x": 2.180320435943854, + "y": 3.432375929705667, + "heading": -0.7437231910234625, + "angularVelocity": -0.17765172049338343, + "velocityX": -0.7725955047594213, + "velocityY": 0.5154095943394352, "moduleForcesX": [ - 61.08457488728973, - 60.252153365783734, - 56.64434890830058, - 56.14650396699435 + 60.58340632519516, + 59.84490600733586, + 56.48902920756478, + 56.03945201723916 ], "moduleForcesY": [ - -34.08103172001482, - -35.54464933149016, - -41.037463508837504, - -41.72744758205663 + -34.96766125833015, + -36.22817172523544, + -41.25431832732434, + -41.87309909683168 ], - "timestamp": 6.017141516021494 + "timestamp": 5.886255879461123 }, { - "x": 2.146044122527282, - "y": 3.455640363277112, - "heading": -0.7511321714311937, - "angularVelocity": -0.14560811421009826, - "velocityX": -0.5916626446253628, - "velocityY": 0.38919678374128486, + "x": 2.1446656199937997, + "y": 3.4561934671002783, + "heading": -0.7519665199163653, + "angularVelocity": -0.13391327355800717, + "velocityX": -0.5792141965962733, + "velocityY": 0.3869170382549771, "moduleForcesX": [ - 61.050689733359725, - 60.18330523590073, - 56.58339768822803, - 56.04798993012374 + 60.58183660174803, + 59.80457338750152, + 56.42234143923046, + 55.9377862653548 ], "moduleForcesY": [ - -34.15181538875617, - -35.66983583289213, - -41.129938480696744, - -41.867174962927784 + -34.97878115536029, + -36.30217808780585, + -41.352605880404866, + -42.01528814031888 ], - "timestamp": 6.079739898477218 + "timestamp": 5.947813099846224 }, { - "x": 2.1213637716998797, - "y": 3.4719003980089873, - "heading": -0.757213576537006, - "angularVelocity": -0.0971495567016255, - "velocityX": -0.3942649931067891, - "velocityY": 0.2597516755864367, + "x": 2.120904845915518, + "y": 3.472085915141707, + "heading": -0.7574858469261834, + "angularVelocity": -0.08966173221089468, + "velocityX": -0.3859949154564824, + "velocityY": 0.258173581295686, "moduleForcesX": [ - 61.01354989752815, - 60.123568909957186, - 56.52548269604857, - 55.96632136080857 + 60.57287518432625, + 59.76823373322807, + 56.35914684811399, + 55.851272292620784 ], "moduleForcesY": [ - -34.22674949510225, - -35.77786460913943, - -41.2167566576027, - -41.98268691331134 + -35.001611618471976, + -36.36844720693236, + -41.44491977809569, + -42.13584960752861 ], - "timestamp": 6.142338280932942 + "timestamp": 6.009370320231325 }, { - "x": 2.1090288162231445, - "y": 3.48003888130188, - "heading": -0.7602557704725093, - "angularVelocity": -0.04859860296957364, - "velocityX": -0.19704910882417112, - "velocityY": 0.13001107973754503, + "x": 2.109028816223142, + "y": 3.4800388813018817, + "heading": -0.7602557704725089, + "angularVelocity": -0.044997540970748186, + "velocityX": -0.19292667242090425, + "velocityY": 0.12919631702701284, "moduleForcesX": [ - 60.97345110924072, - 60.073045581715306, - 56.46803335710757, - 55.89926318732195 + 60.5575482023407, + 59.73687419838248, + 56.29713329942968, + 55.778188462220434 ], "moduleForcesY": [ - -34.30556428252632, - -35.869027517591256, - -41.3017196129008, - -42.07746912434651 + -35.034541448499795, + -36.42559455322768, + -41.5346063390199, + -42.237487649673916 ], - "timestamp": 6.204936663388667 + "timestamp": 6.070927540616426 }, { - "x": 2.1090288162231445, - "y": 3.48003888130188, - "heading": -0.7602557704725093, - "angularVelocity": -9.209824503951875e-20, - "velocityX": -1.2993679631111057e-18, - "velocityY": -2.0831148734143083e-18, + "x": 2.109028816223143, + "y": 3.4800388813018808, + "heading": -0.760255770472509, + "angularVelocity": -4.7854036756238116e-17, + "velocityX": -4.1684189280839564e-16, + "velocityY": 3.182458036241524e-16, "moduleForcesX": [ - 60.93038441390968, - 60.03160907225733, - 56.409223413469284, - 55.845190143852285 + 60.53644088216914, + 59.71107786211956, + 56.23458372965665, + 55.717252600818924 ], "moduleForcesY": [ - -34.38845870016777, - -35.943896076352594, - -41.38750873505681, - -42.15404676926127 + -35.07667248160167, + -36.472859348504336, + -41.62412921793918, + -42.32221267335896 ], - "timestamp": 6.267535045844391 + "timestamp": 6.1324847610015265 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/source 3.2.traj b/src/main/deploy/choreo/source 3.2.traj index 86fb3173..affe7841 100644 --- a/src/main/deploy/choreo/source 3.2.traj +++ b/src/main/deploy/choreo/source 3.2.traj @@ -1,2188 +1,2188 @@ { "samples": [ { - "x": 2.1090288162231445, - "y": 3.48003888130188, - "heading": -0.7602557704725093, - "angularVelocity": -9.209824503951875e-20, - "velocityX": -1.2993679631111057e-18, - "velocityY": -2.0831148734143083e-18, + "x": 2.109028816223143, + "y": 3.4800388813018808, + "heading": -0.760255770472509, + "angularVelocity": -4.7854036756238116e-17, + "velocityX": -4.1684189280839564e-16, + "velocityY": 3.182458036241524e-16, "moduleForcesX": [ - 60.93038441390968, - 60.03160907225733, - 56.409223413469284, - 55.845190143852285 + 60.53644088216914, + 59.71107786211956, + 56.23458372965665, + 55.717252600818924 ], "moduleForcesY": [ - -34.38845870016777, - -35.943896076352594, - -41.38750873505681, - -42.15404676926127 + -35.07667248160167, + -36.472859348504336, + -41.62412921793918, + -42.32221267335896 ], "timestamp": 0 }, { - "x": 2.117794568665661, - "y": 3.4681155240753, - "heading": -0.7530398045067034, - "angularVelocity": 0.11476292191713451, - "velocityX": 0.13941076882464276, - "velocityY": -0.18962940247615553, + "x": 2.1173647314297415, + "y": 3.4676489109609023, + "heading": -0.7574096677464409, + "angularVelocity": 0.045234117251022915, + "velocityX": 0.13248564867185347, + "velocityY": -0.19691818078357895, "moduleForcesX": [ - 48.13501730949312, - 48.836835407072094, - 31.251459327861657, - 36.04401089708163 + 41.520736384271714, + 42.51429487640815, + 35.1511276560287, + 36.81654326527437 ], "moduleForcesY": [ - -50.76492473541808, - -50.1079347694781, - -62.59287500981053, - -59.97406542598307 + -56.31022663437434, + -55.57003233589047, + -60.49287111022869, + -59.49929247213998 ], - "timestamp": 0.0628771544438047 + "timestamp": 0.062919382515501 }, { - "x": 2.1355122480176822, - "y": 3.4444032062204983, - "heading": -0.7387280504849648, - "angularVelocity": 0.22761453103812976, - "velocityX": 0.28178246151161096, - "velocityY": -0.37712135774201583, + "x": 2.1342750283552188, + "y": 3.4430345558261144, + "heading": -0.751664349766151, + "angularVelocity": 0.09131237069712948, + "velocityX": 0.2687613299655501, + "velocityY": -0.39120465190077036, "moduleForcesX": [ - 48.80717190467651, - 49.50131566328889, - 32.41122454849307, - 37.036459214586024 + 42.70173098288214, + 43.611128958075156, + 36.275170342746776, + 37.87745965146702 ], "moduleForcesY": [ - -50.11417943760879, - -49.44755209409215, - -61.99648850314001, - -59.36303177557035 + -55.41582346031395, + -54.70978545558125, + -59.821763435546124, + -58.826195167217094 ], - "timestamp": 0.12575430888760852 + "timestamp": 0.125838765031002 }, { - "x": 2.162391169686947, - "y": 3.4090581131486557, - "heading": -0.717466130764082, - "angularVelocity": 0.3381501581768513, - "velocityX": 0.42748311222143226, - "velocityY": -0.5621293359169338, + "x": 2.1600269154540004, + "y": 3.4063896499792783, + "heading": -0.7429636060218578, + "angularVelocity": 0.1382839976560426, + "velocityX": 0.4092838497332282, + "velocityY": -0.5824104494002036, "moduleForcesX": [ - 49.51581154621932, - 50.265844017076226, - 33.70048897709995, - 38.19652881261669 + 44.01151222332591, + 44.83593128986605, + 37.54047868266639, + 39.07824731715444 ], "moduleForcesY": [ - -49.40850610951917, - -48.66541972742287, - -61.300931724870296, - -58.619530919336384 + -54.37659249907092, + -53.70636995290632, + -59.03155043231922, + -58.03154166424591 ], - "timestamp": 0.18863146333141234 + "timestamp": 0.188758147546503 }, { - "x": 2.1986681969801216, - "y": 3.3622638578182182, - "heading": -0.6894300597240235, - "angularVelocity": 0.4458864477576762, - "velocityX": 0.576950843499077, - "velocityY": -0.7442171285321014, + "x": 2.1949215222317773, + "y": 3.357943659865962, + "heading": -0.7312478740657462, + "angularVelocity": 0.18620227166446401, + "velocityX": 0.5545923272400236, + "velocityY": -0.7699692555212343, "moduleForcesX": [ - 50.28381428848592, - 51.13744068398159, - 35.15534244893406, - 39.540816535356164 + 45.47389898312857, + 46.209380246970596, + 38.97509187538921, + 40.44329438249183 ], "moduleForcesY": [ - -48.620147379296554, - -47.74309558757011, - -60.4734352958337, - -57.716863658050855 + -53.154044492191325, + -52.52435244840178, + -58.08923297674164, + -57.08406886084209 ], - "timestamp": 0.25150861777521616 + "timestamp": 0.251677530062004 }, { - "x": 2.2446132442928994, - "y": 3.304239125606535, - "heading": -0.6548330373393313, - "angularVelocity": 0.550231999057986, - "velocityX": 0.7307113007768327, - "velocityY": -0.922826942869088, + "x": 2.2393001625829205, + "y": 3.297971653928642, + "heading": -0.7164541055664759, + "angularVelocity": 0.23512259510216332, + "velocityX": 0.7053254271878769, + "velocityY": -0.9531563015982434, "moduleForcesX": [ - 51.13905740321851, - 52.12476715621361, - 36.82116804073696, - 41.09052851792727 + 47.11716302884608, + 47.75573925943408, + 40.613851617327796, + 42.00243934560986 ], "moduleForcesY": [ - -47.71212916324656, - -46.6564142258722, - -59.46813562411414, - -56.61877073126656 + -51.69634105716302, + -51.116592706220615, + -56.94950428119139, + -55.94148520473914 ], - "timestamp": 0.31438577221902 + "timestamp": 0.314596912577505 }, { - "x": 2.3005361732090566, - "y": 3.235248258796662, - "heading": -0.6139347954947619, - "angularVelocity": 0.650446767293226, - "velocityX": 0.8893998052366954, - "velocityY": -1.0972326502391698, + "x": 2.293551835832764, + "y": 3.226807987294763, + "heading": -0.6985158525170084, + "angularVelocity": 0.28509900021743295, + "velocityX": 0.862241030996735, + "velocityY": -1.1310293233781603, "moduleForcesX": [ - 52.11606824369092, - 53.23896165126432, - 38.75432059140504, - 42.87287498290715 + 48.974244926718995, + 49.50301706537962, + 42.49983724647496, + 43.79203201526126 ], "moduleForcesY": [ - -46.633926917902656, - -45.372711713166105, - -58.220079562306445, - -55.27504395126715 + -49.932591819303816, + -49.41939613494972, + -55.54905272250912, + -54.54558483413394 ], - "timestamp": 0.3772629266628238 + "timestamp": 0.377516295093006 }, { - "x": 2.3667954166739076, - "y": 3.155616243498738, - "heading": -0.5670546584241174, - "angularVelocity": 0.7455829940991222, - "velocityX": 1.0537888371533939, - "velocityY": -1.2664697695423583, + "x": 2.3581219992537226, + "y": 3.144865400496988, + "heading": -0.6773637507894199, + "angularVelocity": 0.3361778339508891, + "velocityX": 1.0262364447879162, + "velocityY": -1.302342513892083, "moduleForcesX": [ - 53.25761026058277, - 54.49467377205829, - 41.023942389693836, - 44.92291476717264 + 51.081995362995904, + 51.48237998253363, + 44.68553797550306, + 45.85573965609478 ], "moduleForcesY": [ - -45.315096693303126, - -43.846496735504985, - -56.635631853416726, - -53.61440385265008 + -47.76441461323329, + -47.34537980089634, + -53.79777393977218, + -52.8148137116725 ], - "timestamp": 0.4401400811066276 + "timestamp": 0.440435677608507 }, { - "x": 2.4438086218512485, - "y": 3.065750442430627, - "heading": -0.5145902034841154, - "angularVelocity": 0.8343961396486453, - "velocityX": 1.2248201410922746, - "velocityY": -1.4292281809353713, + "x": 2.433522291274826, + "y": 3.0526620913362525, + "heading": -0.652926734850225, + "angularVelocity": 0.3883861373428849, + "velocityX": 1.1983635090908211, + "velocityY": -1.46541980347662, "moduleForcesX": [ - 54.61557385722636, - 55.91123819153664, - 43.7131279622274, - 47.285771912845235 + 53.47798671720776, + 53.72572515007777, + 47.232726000587874, + 48.24428581121922 ], "moduleForcesY": [ - -43.65585603926645, - -42.012507491987115, - -54.57686147012399, - -51.53256053951273 + -45.05355867588098, + -44.773023132097414, + -51.56510924673164, + -50.632864772568915 ], - "timestamp": 0.5030172355504314 + "timestamp": 0.503355060124008 }, { - "x": 2.532065289837757, - "y": 2.9661728933690776, - "heading": -0.45704452569034076, - "angularVelocity": 0.9152080481823572, - "velocityX": 1.4036364839854023, - "velocityY": -1.583684089116122, + "x": 2.520339981100692, + "y": 2.9508605578412346, + "heading": -0.6251343348430549, + "angularVelocity": 0.4417144430863272, + "velocityX": 1.3798242505713987, + "velocityY": -1.6179677775753416, "moduleForcesX": [ - 56.24963874793976, - 57.513648109663556, - 46.9169797801361, - 50.01853513267831 + 56.19197506453079, + 56.259276660576184, + 50.20804830120448, + 51.011888473748435 ], "moduleForcesY": [ - -41.51321004507581, - -39.77429977030871, - -51.835917665532385, - -48.87154151079314 + -41.604044996750225, + -41.53144010163077, + -48.65926448291237, + -47.83146963835682 ], - "timestamp": 0.5658943899942352 + "timestamp": 0.566274442639509 }, { - "x": 2.6321403093335114, - "y": 2.8575694307170187, - "heading": -0.3950667539653817, - "angularVelocity": 0.985696192411999, - "velocityX": 1.591595872634394, - "velocityY": -1.727232468020187, + "x": 2.6192438141173406, + "y": 2.8403235512778635, + "heading": -0.5939211843905473, + "angularVelocity": 0.4960816397843361, + "velocityX": 1.5719135989976136, + "velocityY": -1.7568037406619357, "moduleForcesX": [ - 58.220168914381084, - 59.332124325032076, - 50.73064435062025, - 53.18908968161254 + 59.2250417610776, + 59.087826967496994, + 53.66857872752665, + 54.204993983215175 ], "moduleForcesY": [ - -38.68115242041676, - -36.98517279536072, - -48.09349009166419, - -45.382842135502834 + -37.1392249820516, + -37.38030580647965, + -44.79597130253933, + -44.16473609829124 ], - "timestamp": 0.6287715444380391 + "timestamp": 0.62919382515501 }, { - "x": 2.7447041232794764, - "y": 2.740865636486064, - "heading": -0.32951256744957347, - "angularVelocity": 1.0425755919727022, - "velocityX": 1.7902180043240945, - "velocityY": -1.8560603650608418, + "x": 2.7309783267356824, + "y": 2.7221935960005026, + "heading": -0.5592365908276438, + "angularVelocity": 0.5512545129373857, + "velocityX": 1.7758361279342616, + "velocityY": -1.8774811600901864, "moduleForcesX": [ - 60.5677211979897, - 61.39663828373237, - 55.20909444556158, - 56.86245394897771 + 62.5038695327194, + 62.160031630846326, + 57.62319015145175, + 57.832996172310274 ], "moduleForcesY": [ - -34.86502705000924, - -33.416146228705585, - -42.85575978754388, - -40.660621351301806 + -31.280112248684674, + -31.988407393096097, + -39.55612203914035, + -39.27379472999048 ], - "timestamp": 0.6916486988818429 + "timestamp": 0.692113207670511 }, { - "x": 2.870516828572632, - "y": 2.6173440746550423, - "heading": -0.26153269734842044, - "angularVelocity": 1.081153730675094, - "velocityX": 2.0009287380458565, - "velocityY": -1.9644903291770046, + "x": 2.856329605423656, + "y": 2.598000279152458, + "heading": -0.5210635851729407, + "angularVelocity": 0.606697080113568, + "velocityX": 1.9922522071333428, + "velocityY": -1.9738483100568995, "moduleForcesX": [ - 63.264661428716025, - 63.71631261983922, - 60.25491380232913, - 61.043986331952446 + 65.79278778008134, + 65.30020556417972, + 61.939549113121835, + 61.79878070206867 ], "moduleForcesY": [ - -29.65704803224665, - -28.70294866760673, - -35.37956356936107, - -34.023166255951196 + -23.548304579936264, + -24.925035881299397, + -32.34851276785782, + -32.65108476149218 ], - "timestamp": 0.7545258533256467 + "timestamp": 0.755032590186012 }, { - "x": 3.0103732419935905, - "y": 2.488816613373573, - "heading": -0.1926918880041953, - "angularVelocity": 1.094846132163167, - "velocityX": 2.224280259787407, - "velocityY": -2.0441042922249113, + "x": 2.996031354964205, + "y": 2.46978535540666, + "heading": -0.47945305051497955, + "angularVelocity": 0.6613309443669427, + "velocityX": 2.220329315948571, + "velocityY": -2.0377651308674327, "moduleForcesX": [ - 66.11806816346805, - 66.21721889876862, - 65.35371655844693, - 65.48547608931766 + 68.5564837925553, + 68.09308895613952, + 66.1567792079864, + 65.75590846714256 ], "moduleForcesY": [ - -22.54079142004972, - -22.263089069440703, - -24.6715805356649, - -24.33345121959331 + -13.459642197003902, + -15.709687602642282, + -22.445832125017436, + -23.647305076742175 ], - "timestamp": 0.8174030077694505 + "timestamp": 0.817951972701513 }, { - "x": 3.1649283508936707, - "y": 2.3578354928773337, - "heading": -0.12509625596484528, - "angularVelocity": 1.075042797933275, - "velocityX": 2.458048718445309, - "velocityY": -2.0831273561099635, + "x": 3.1505724071472994, + "y": 2.340193763604278, + "heading": -0.43457360660126365, + "angularVelocity": 0.7132848753348665, + "velocityX": 2.456175601294578, + "velocityY": -2.059645003198423, "moduleForcesX": [ - 68.61737088139724, - 68.5714962678954, - 69.13779393676624, - 69.12206655275224 + 69.84706521344005, + 69.75290941394955, + 69.22551312159781, + 68.88502793630397 ], "moduleForcesY": [ - -13.001403954681635, - -13.198507429125556, - -9.864173290779956, - -9.916683464178158 + -0.8233024041036456, + -4.014815042921486, + -9.277060723605338, + -11.648512019585809 ], - "timestamp": 0.8802801622132543 + "timestamp": 0.880871355217014 }, { - "x": 3.3343139997886584, - "y": 2.227807798534294, - "heading": -0.061425277024888265, - "angularVelocity": 1.0126250067003624, - "velocityX": 2.6939140359218023, - "velocityY": -2.067964040250105, + "x": 3.3199095005462036, + "y": 2.212409184758706, + "heading": -0.3867586308994902, + "angularVelocity": 0.7599403202978621, + "velocityX": 2.691334317484535, + "velocityY": -2.03092550716134, "moduleForcesX": [ - 69.81884664866837, - 69.79428970681542, - 69.29948552018702, - 69.00682870384438 + 68.47925044891942, + 69.15340266140203, + 69.49958829473019, + 69.7686571831701 ], "moduleForcesY": [ - -0.8612358805856215, - -0.3297910827818419, - 8.567864185381797, - 10.490055875380158 + 13.736045870694008, + 9.89537651518505, + 6.815502213890215, + 3.370466430920167 ], - "timestamp": 0.9431573166570582 + "timestamp": 0.943790737732515 }, { - "x": 3.5176404379599324, - "y": 2.1026847463287157, - "heading": -0.004525789100840003, - "angularVelocity": 0.9049310266561369, - "velocityX": 2.9156287334077797, - "velocityY": -1.9899604763031347, + "x": 3.5032276259952, + "y": 2.089811754517775, + "heading": -0.3365088719529981, + "angularVelocity": 0.7986371915540097, + "velocityX": 2.9135398047467214, + "velocityY": -1.9484843197678885, "moduleForcesX": [ - 68.55777812799681, - 67.6445550813489, - 64.16468459069685, - 60.8787964212339 + 63.762980248590615, + 65.33381311946748, + 65.64375774359121, + 66.90783722378835 ], "moduleForcesY": [ - 13.196294200522606, - 17.05028282885496, - 27.549743552264584, - 34.11506286093019 + 28.499981635594498, + 24.717827613930762, + 23.81826750682116, + 20.038950866645536 ], - "timestamp": 1.006034471100862 + "timestamp": 1.006710120248016 }, { - "x": 3.712921078466066, - "y": 1.986111498522952, - "heading": 0.04367317548530425, - "angularVelocity": 0.7665576633119034, - "velocityX": 3.105748697337491, - "velocityY": -1.8539841511108714, + "x": 3.698983205941932, + "y": 1.9754559392748747, + "heading": -0.2844250046592506, + "angularVelocity": 0.8277873242146967, + "velocityX": 3.111212668028096, + "velocityY": -1.8174974176634262, "moduleForcesX": [ - 64.1546735583916, - 59.222021183604404, - 55.03127871584201, - 45.60985368543614 + 56.27400205759313, + 58.363284645856304, + 58.070669393657745, + 60.053109943523204 ], "moduleForcesY": [ - 27.543396744432503, - 36.83392520078611, - 43.0063108958907, - 52.83689467676779 + 41.37894474295054, + 38.388062514062604, + 38.80031879570978, + 35.67059242066261 ], - "timestamp": 1.0689116255446658 + "timestamp": 1.069629502763517 }, { - "x": 3.917974347119227, - "y": 1.880569748380944, - "heading": 0.0840407650336414, - "angularVelocity": 0.6420072585252761, - "velocityX": 3.261172845161521, - "velocityY": -1.678538907741677, + "x": 3.9052468507649114, + "y": 1.8717029681737345, + "heading": -0.23110382341189709, + "angularVelocity": 0.8474523924995162, + "velocityX": 3.2782210596577848, + "velocityY": -1.6489826656448716, "moduleForcesX": [ - 56.176200766123785, - 46.718354113326974, - 45.97750919549416, - 34.263779007829406 + 47.55707312914185, + 49.53638736807368, + 48.78953373366416, + 50.77045950153568 ], "moduleForcesY": [ - 41.47332187047582, - 51.820224724822296, - 52.59176748493925, - 60.84133045416032 + 51.17696939290848, + 49.268536466648314, + 49.99446973782349, + 47.98722716731246 ], - "timestamp": 1.1317887799884696 + "timestamp": 1.132548885279018 }, { - "x": 4.131094513594558, - "y": 1.787487140153957, - "heading": 0.11947354396561721, - "angularVelocity": 0.5635238942570701, - "velocityX": 3.3894690108122587, - "velocityY": -1.4803883707901997, + "x": 4.120080601641824, + "y": 1.7801799153533593, + "heading": -0.17706327947410902, + "angularVelocity": 0.8588854781668361, + "velocityX": 3.4144287862962694, + "velocityY": -1.454608248862383, "moduleForcesX": [ - 45.44840572457847, - 37.20679327325788, - 38.51657585799193, - 29.99924193711389 + 39.05701336414032, + 40.46649013860068, + 39.71798936608046, + 41.14398002340263 ], "moduleForcesY": [ - 53.034304370620895, - 59.079362573109876, - 58.2883307755835, - 63.07823138252855 + 57.94278647291951, + 56.96920228244361, + 57.48746696114792, + 56.477668387844396 ], - "timestamp": 1.1946659344322734 + "timestamp": 1.195468267794519 }, { - "x": 4.350770411068689, - "y": 1.7077754935808809, - "heading": 0.1521039131255066, - "angularVelocity": 0.5189542918812017, - "velocityX": 3.493731537588328, - "velocityY": -1.2677362275406037, + "x": 4.341754671827057, + "y": 1.7019368196307447, + "heading": -0.12271990769558694, + "angularVelocity": 0.8636984281454757, + "velocityX": 3.523144400386022, + "velocityY": -1.243545193777744, "moduleForcesX": [ - 35.38842380663489, - 29.894307911171417, - 31.489034487421396, - 26.080480439830673 + 31.542569411645978, + 32.211867496766956, + 31.792298515347188, + 32.46659837801001 ], "moduleForcesY": [ - 60.23788124052505, - 63.133637641285205, - 62.37825455380923, - 64.81765821899063 + 62.36899517370198, + 62.02650340460398, + 62.24055631529311, + 61.8920479920422 ], - "timestamp": 1.2575430888760772 + "timestamp": 1.25838765031002 }, { - "x": 4.575688712037729, - "y": 1.6420223189913672, - "heading": 0.18322659040704492, - "angularVelocity": 0.49497591862802676, - "velocityX": 3.5771068674880655, - "velocityY": -1.0457403037899842, + "x": 4.568810602625837, + "y": 1.6376320022464737, + "heading": -0.06839919997587875, + "angularVelocity": 0.8633382202427946, + "velocityX": 3.6086802146038512, + "velocityY": -1.0220192063777647, "moduleForcesX": [ - 27.158137877972838, - 23.927370915342486, - 25.160312496417824, - 21.995099103025833 + 25.215137722620277, + 25.161326804862117, + 25.198140713892162, + 25.144365342135142 ], "moduleForcesY": [ - 64.38875092826233, - 65.65351312146176, - 65.20196768922268, - 66.33295061137039 + 65.19836347228741, + 65.21911954217529, + 65.20503178610034, + 65.2257580831436 ], - "timestamp": 1.320420243319881 + "timestamp": 1.321307032825521 }, { - "x": 4.804716248215296, - "y": 1.5906097298688888, - "heading": 0.21347571274604327, - "angularVelocity": 0.48108287670736033, - "velocityX": 3.6424602576800607, - "velocityY": -0.8176672366499624, + "x": 4.800045847269579, + "y": 1.5876743304432248, + "heading": -0.014355888196568429, + "angularVelocity": 0.8589294684510949, + "velocityX": 3.675103527705061, + "velocityY": -0.793994947279417, "moduleForcesX": [ - 20.777362325555703, - 18.79970144179875, - 19.686842590719646, - 17.74181237154826 + 19.993448516674185, + 19.3060989973268, + 19.797835730589334, + 19.116494526678093 ], "moduleForcesY": [ - 66.73767220647805, - 67.31957624630218, - 67.070927016661, - 67.60968811178219 + 66.99591754938321, + 67.196927406319, + 67.05502467711538, + 67.25215990996023 ], - "timestamp": 1.3832973977636849 + "timestamp": 1.384226415341022 }, { - "x": 5.036866817289369, - "y": 1.55378761076971, - "heading": 0.24298831748702848, - "angularVelocity": 0.469369280496969, - "velocityX": 3.692129059077492, - "velocityY": -0.5856199986290461, + "x": 5.034473115872903, + "y": 1.5523169276578892, + "heading": 0.039207016322208126, + "angularVelocity": 0.8512941859462895, + "velocityX": 3.725835493467699, + "velocityY": -0.5619477078724477, "moduleForcesX": [ - 15.925448739090935, - 14.192665989132097, - 15.055141930627649, - 13.351354517579729 + 15.705471617855421, + 14.48469807799971, + 15.374313004019221, + 14.172732804713208 ], "moduleForcesY": [ - 68.07164789067083, - 68.45258203067056, - 68.27203655365994, - 68.62435165252099 + 68.13853193873344, + 68.40802253366346, + 68.21561194224486, + 68.47494827858137 ], - "timestamp": 1.4461745522074887 + "timestamp": 1.447145797856523 }, { "x": 5.271278381347656, "y": 1.5317155122756958, - "heading": 0.2715710097371584, - "angularVelocity": 0.45457992657214513, - "velocityX": 3.7280879857212588, - "velocityY": -0.3510352637497425, + "heading": 0.09212323355733296, + "angularVelocity": 0.8410161562232161, + "velocityX": 3.7636298388086282, + "velocityY": -0.3274255810936793, "moduleForcesX": [ - 12.231894678034537, - 9.978543755139432, - 11.179968921219166, - 8.979896356158925 + 12.17583308633574, + 10.508988015592593, + 11.72461754628835, + 10.093645719719404 ], "moduleForcesY": [ - 68.84172245876557, - 69.20285737211027, - 69.02334612966767, - 69.34261769004998 + 68.86508793588061, + 69.1385750114367, + 68.94526551565065, + 69.20238200518273 ], - "timestamp": 1.5090517066512925 + "timestamp": 1.510065180372024 }, { - "x": 5.388703997390188, - "y": 1.5243911275820383, - "heading": 0.2855249380175652, - "angularVelocity": 0.4444472090652179, - "velocityX": 3.7401286773236566, - "velocityY": -0.2332893125004083, + "x": 5.388681610311286, + "y": 1.5251584282304298, + "heading": 0.11807334458511697, + "angularVelocity": 0.8348404097837044, + "velocityX": 3.7769765097724286, + "velocityY": -0.21094779615682893, "moduleForcesX": [ - 9.343299160452919, - 6.163463777084662, - 7.986565873923746, - 4.920049004946797 + 9.224601000293218, + 7.170145322617407, + 8.707960432775863, + 6.708850729084163 ], "moduleForcesY": [ - 69.21664100426783, - 69.5686586634018, - 69.39371949997597, - 69.67546873681157 + 69.25444608627201, + 69.49615053644614, + 69.32560078879362, + 69.54657168705602 ], - "timestamp": 1.54044784998574 + "timestamp": 1.5411490985959961 }, { - "x": 5.50644545946061, - "y": 1.5207685343702697, - "heading": 0.2991263551425099, - "angularVelocity": 0.43321936009960066, - "velocityX": 3.7501887036309256, - "velocityY": -0.11538338238486952, + "x": 5.506442673711366, + "y": 1.5222273963184734, + "heading": 0.14379764901773748, + "angularVelocity": 0.8275759911368509, + "velocityX": 3.7884883929871296, + "velocityY": -0.0942941585046398, "moduleForcesX": [ - 8.420939086179335, - 4.879883420406134, - 6.919098093280834, - 3.519523756844695 + 8.369389483724206, + 5.949973696893992, + 7.73117703857559, + 5.38783266312984 ], "moduleForcesY": [ - 69.32400261131451, - 69.65903960081413, - 69.4987054257006, - 69.75025512359892 + 69.35385904945188, + 69.60180730909208, + 69.43322409042021, + 69.65302052985173 ], - "timestamp": 1.5718439933201873 + "timestamp": 1.5722330168199683 }, { - "x": 5.624430269287079, - "y": 1.5208521505351245, - "heading": 0.31232341935960334, - "angularVelocity": 0.4203402971031069, - "velocityX": 3.757939584159604, - "velocityY": 0.002663262298303611, + "x": 5.624497338635267, + "y": 1.522927459386341, + "heading": 0.16925769890783504, + "angularVelocity": 0.8190746644823769, + "velocityX": 3.7979338406848053, + "velocityY": 0.02252171244382533, "moduleForcesX": [ - 7.427158187557407, - 3.3353613300752003, - 5.7144874128058545, - 1.8133624389873595 + 7.409211713616261, + 4.57253345050813, + 6.632095741425086, + 3.899218959272906 ], "moduleForcesY": [ - 69.4251012893418, - 69.73687602026827, - 69.59766681374457, - 69.80441399722511 + 69.45271134204867, + 69.69519778275762, + 69.53780580291625, + 69.74287628352383 ], - "timestamp": 1.6032401366546347 + "timestamp": 1.6033169350439405 }, { - "x": 5.742576714793087, - "y": 1.5246451742965244, - "heading": 0.3250572744698019, - "angularVelocity": 0.4055866026139322, - "velocityX": 3.7630878495951756, - "velocityY": 0.12081177363075021, + "x": 5.742772793309857, + "y": 1.5272629148189605, + "heading": 0.19440986357494344, + "angularVelocity": 0.8091696962357507, + "velocityX": 3.8050368625463413, + "velocityY": 0.13947583446143447, "moduleForcesX": [ - 6.306930660430928, - 1.5778924322604664, - 4.3676425070670675, - -0.10369410469570728 + 6.320859466741637, + 3.0067323445032117, + 5.3877760442274845, + 2.2145608793254503 ], "moduleForcesY": [ - 69.52171601095, - 69.78371617293728, - 69.68348163684516, - 69.815527135674 + 69.54854425346966, + 69.76816296129176, + 69.63518331243104, + 69.80621969634359 ], - "timestamp": 1.6346362799890821 + "timestamp": 1.6344008532679126 }, { - "x": 5.860794105940623, - "y": 1.5321490223767453, - "heading": 0.33726992474980966, - "angularVelocity": 0.3889856836845344, - "velocityX": 3.7653475424743803, - "velocityY": 0.2390054090493234, + "x": 5.861185901308817, + "y": 1.5352368348891807, + "heading": 0.21920461001465913, + "angularVelocity": 0.7976712028728045, + "velocityX": 3.8094653043977926, + "velocityY": 0.2565287944964038, "moduleForcesX": [ - 5.00586577439091, - -0.3668746684666077, - 2.864339879121958, - -2.17095393844088 + 5.0743871573767105, + 1.212540305184361, + 3.969650176486514, + 0.29853628468642324 ], "moduleForcesY": [ - 69.611426474232, - 69.78350871775525, - 69.74815431030521, - 69.76783434978745 + 69.63726174457908, + 69.80849842797434, + 69.71904021769083, + 69.82888852399873 ], - "timestamp": 1.6660324233235295 + "timestamp": 1.6654847714918848 }, { - "x": 5.9789826775060435, - "y": 1.5433627560426824, - "heading": 0.3489121084818783, - "angularVelocity": 0.37081572752583847, - "velocityX": 3.764429608643845, - "velocityY": 0.3571691448367622, + "x": 5.979640967451708, + "y": 1.5468503445815898, + "heading": 0.24358557158644084, + "angularVelocity": 0.7843593396465376, + "velocityX": 3.810815138856516, + "velocityY": 0.3736179463840169, "moduleForcesX": [ - 3.4623816637274447, - -2.475568668701702, - 1.1828304235517055, - -4.335764900525163 + 3.6307956438747206, + -0.8621551207472431, + 2.3418002525812245, + -1.893133883598176 ], "moduleForcesY": [ - 69.68689216287859, - 69.72122437760291, - 69.7815156598602, - 69.65073511362182 + 69.7120826368421, + 69.79764527340737, + 69.77979201753804, + 69.79043163606336 ], - "timestamp": 1.697428566657977 + "timestamp": 1.696568689715857 }, { - "x": 6.097033194516696, - "y": 1.5582824506328423, - "heading": 0.3599519448622258, - "angularVelocity": 0.35163033442501535, - "velocityX": 3.7600324266938894, - "velocityY": 0.47520787605114534, + "x": 6.098026846695146, + "y": 1.5621015192437462, + "heading": 0.2674883023191033, + "angularVelocity": 0.7689741866013711, + "velocityX": 3.808589328746145, + "velocityY": 0.4906451803233273, "moduleForcesX": [ - 1.6038425315720386, - -4.721980038884405, - -0.7091635039964642, - -6.549079033729455 + 1.9386808198061933, + -3.28598819102792, + 0.4585755906663063, + -4.416447800806846 ], "moduleForcesY": [ - 69.73338633060467, - 69.58337205904242, - 69.76976708048174, - 69.45885860862715 + 69.76178277329687, + 69.70685348700545, + 69.80282869693546, + 69.6609062566512 ], - "timestamp": 1.7288247099924243 + "timestamp": 1.727652607939829 }, { - "x": 6.214826085217277, - "y": 1.5769004197748018, - "heading": 0.37038445324917163, - "angularVelocity": 0.3322863026777996, - "velocityX": 3.7518267592867267, - "velocityY": 0.5930017882652427, + "x": 6.216213161429717, + "y": 1.5809836737297318, + "heading": 0.2908385525285553, + "angularVelocity": 0.7512003487206494, + "velocityX": 3.8021691436385594, + "velocityY": 0.6074573465909969, "moduleForcesX": [ - -0.6554876781322178, - -7.078557663607612, - -2.860110163308246, - -8.769410640028871 + -0.07065192727705566, + -6.150375646776657, + -1.738667342074545, + -7.342704323857981 ], "moduleForcesY": [ - 69.7245742748119, - 69.35870703983915, - 69.69292267797944, - 69.1914612156941 + 69.76766630955797, + 69.49064187075653, + 69.76568210247977, + 69.39577088637856 ], - "timestamp": 1.7602208533268717 + "timestamp": 1.7587365261638013 }, { - "x": 6.332229852804532, - "y": 1.5992041212873251, - "heading": 0.3802420776714249, - "angularVelocity": 0.31397564717566995, - "velocityX": 3.7394327811735093, - "velocityY": 0.7103962188901063, + "x": 6.334045309853754, + "y": 1.6034826557006305, + "heading": 0.313549807262191, + "angularVelocity": 0.7306432403402956, + "velocityX": 3.790775267616168, + "velocityY": 0.7238142182972129, "moduleForcesX": [ - -3.4164906317976134, - -9.51983860988743, - -5.342993033888524, - -10.967733824511571 + -2.490383343681562, + -9.578064211771972, + -4.3259113321238125, + -10.762745909847968 ], "moduleForcesY": [ - 69.61603638629833, - 69.03834092530526, - 69.5200097527037, - 68.85058965896839 + 69.69820129934999, + 69.07536900988754, + 69.63337299442767, + 68.9276312183353 ], - "timestamp": 1.7916169966613191 + "timestamp": 1.7898204443877734 }, { - "x": 6.449098535030912, - "y": 1.625174456079489, - "heading": 0.3896069207325694, - "angularVelocity": 0.29828004546244535, - "velocityX": 3.722389752825253, - "velocityY": 0.827182323494794, + "x": 6.451337870714877, + "y": 1.6295724757424068, + "heading": 0.33551971883232135, + "angularVelocity": 0.706793506913393, + "velocityX": 3.7734162088570447, + "velocityY": 0.8393349851774983, "moduleForcesX": [ - -6.797799685550791, - -12.023870431612343, - -8.267497900609209, - -13.12862175589208 + -5.448866729736701, + -13.733572282788284, + -7.401927508185858, + -14.790647300726173 ], "moduleForcesY": [ - 69.33533463241886, - 68.61540564974591, - 69.19995880316955, - 68.43876233553452 + 69.49922387521676, + 68.33891344443161, + 69.35066212276465, + 68.15293380811582 ], - "timestamp": 1.8230131399957665 + "timestamp": 1.8209043626117456 }, { - "x": 6.5652678833030125, - "y": 1.6547829667795249, - "heading": 0.39862574645972415, - "angularVelocity": 0.287259031502107, - "velocityX": 3.7001152350021482, - "velocityY": 0.9430620310472863, + "x": 6.567866022044423, + "y": 1.659208124359565, + "heading": 0.35662502542728386, + "angularVelocity": 0.6789783206509007, + "velocityX": 3.7488244078469917, + "velocityY": 0.9534077526398532, "moduleForcesX": [ - -10.936219914888005, - -14.574762118658219, - -11.799686531958358, - -15.252283089212833 + -9.125353794114547, + -18.83172539656305, + -11.094164589642684, + -19.562885061791146 ], "moduleForcesY": [ - 68.76683316861762, - 68.08405084461447, - 68.64363965268268, - 67.95604052661037 + 69.0754447164273, + 67.07463735744341, + 68.82909121269832, + 66.91127860350836 ], - "timestamp": 1.854409283330214 + "timestamp": 1.8519882808357178 }, { - "x": 6.6805489880212905, - "y": 1.6879868491141017, - "heading": 0.40752627429499416, - "angularVelocity": 0.28349111992696274, - "velocityX": 3.671823748867664, - "velocityY": 1.057578377728512, + "x": 6.683354928219009, + "y": 1.6923136502791418, + "heading": 0.37671482456009425, + "angularVelocity": 0.6463084540390093, + "velocityX": 3.7153908764796526, + "velocityY": 1.0650370934911797, "moduleForcesX": [ - -15.976487504436525, - -17.19633335319417, - -16.19563370942626, - -17.393215864457243 + -13.771593248917489, + -25.130414558467773, + -15.562652582080137, + -25.223578565092154 ], "moduleForcesY": [ - 67.73039056976316, - 67.42967661497167, - 67.68616175746094, - 67.38710414645385 + 68.25443383699609, + 64.93026638806907, + 67.92556316929904, + 64.95632378013195 ], - "timestamp": 1.8858054266646613 + "timestamp": 1.88307219905969 }, { - "x": 6.794712373597259, - "y": 1.7247180908804576, - "heading": 0.4166128430796408, - "angularVelocity": 0.28941671873044894, - "velocityX": 3.6362232252491316, - "velocityY": 1.1699284646230703, + "x": 6.7974682626271505, + "y": 1.7287625418402501, + "heading": 0.3956032132276822, + "angularVelocity": 0.6076579063002795, + "velocityX": 3.671137389627324, + "velocityY": 1.1725964306841732, "moduleForcesX": [ - -22.036733716797762, - -20.18398244295977, - -21.84839807752641, - -19.94027338243488 + -19.7338671115633, + -32.869545661536705, + -20.995212284811174, + -31.878789101972526 ], "moduleForcesY": [ - 65.95765425448234, - 66.54948915551118, - 66.00569479257204, - 66.6085993405814 + 66.71548826157209, + 61.31804473588162, + 66.4083595377785, + 61.9239118979002 ], - "timestamp": 1.9172015699991087 + "timestamp": 1.914156117283662 }, { - "x": 6.907432636307125, - "y": 1.7648507330806484, - "heading": 0.4261265032765866, - "angularVelocity": 0.30302002687404705, - "velocityX": 3.590258252713168, - "velocityY": 1.278266625702327, + "x": 6.909800194013484, + "y": 1.7683468561362488, + "heading": 0.41306566672219286, + "angularVelocity": 0.5617841794810646, + "velocityX": 3.6138279150310604, + "velocityY": 1.2734660415324053, "moduleForcesX": [ - -29.201525856972616, - -25.15437815757134, - -29.219412722371683, - -24.891885489663608 + -27.444972321937637, + -42.075345805688336, + -27.57805811433052, + -39.49775701748281 ], "moduleForcesY": [ - 63.049097714214916, - 64.77103172702344, - 63.0005849187992, - 64.8334626863064 + 63.85363325543449, + 55.33672689274484, + 63.908626512967174, + 57.321971693817275 ], - "timestamp": 1.9485977133335561 + "timestamp": 1.9452400355076342 }, { - "x": 7.018202438708799, - "y": 1.808117637629141, - "heading": 0.4359499136905104, - "angularVelocity": 0.31288589522859334, - "velocityX": 3.5281340520616546, - "velocityY": 1.3780961593782959, + "x": 7.019881390468804, + "y": 1.810734119873736, + "heading": 0.42885115065317647, + "angularVelocity": 0.5078344312078966, + "velocityX": 3.5414195746540194, + "velocityY": 1.363639661900725, "moduleForcesX": [ - -37.84606166215224, - -35.12657257182429, - -38.23355604485398, - -35.39322800560654 + -37.27804152080681, + -52.1405235304012, + -35.411432044620646, + -47.75401943748761 ], "moduleForcesY": [ - 58.175724092585945, - 59.8531774353204, - 57.88611968366453, - 59.66067088624069 + 58.55809505279019, + 45.896730636123046, + 59.86837524976089, + 50.604050638682544 ], - "timestamp": 1.9799938566680035 + "timestamp": 1.9763239537316064 }, { - "x": 7.126380703050267, - "y": 1.854009244621213, - "heading": 0.4456272830552769, - "angularVelocity": 0.3082343350798942, - "velocityX": 3.4455908545549447, - "velocityY": 1.461695677179563, + "x": 7.127216489228573, + "y": 1.8554226010135964, + "heading": 0.44272434085459006, + "angularVelocity": 0.4463140747396045, + "velocityX": 3.4530749304632886, + "velocityY": 1.4376720726731453, "moduleForcesX": [ - -48.349849951021866, - -49.399500416665845, - -47.99842192068982, - -49.03597721530752 + -48.97728499858189, + -61.38705872437014, + -44.32215534808062, + -55.881419683225104 ], "moduleForcesY": [ - 49.65036150580679, - 48.61361186301433, - 50.011197849600876, - 49.001261315065825 + 49.06528866999657, + 32.434718317404084, + 53.54352333978887, + 41.41142967075846 ], - "timestamp": 2.011390000002451 + "timestamp": 2.0074078719555786 }, { "x": 7.231364727020264, "y": 1.90172815322876, "heading": 0.45451541638849, - "angularVelocity": 0.28309634207400236, - "velocityX": 3.343850958114625, - "velocityY": 1.5198971446658642, + "angularVelocity": 0.3793304128823317, + "velocityX": 3.3505505014284465, + "velocityY": 1.489694828094504, "moduleForcesX": [ - -59.49373787912882, - -62.82905712054162, - -57.10726545680866, - -60.65366662655575 + -60.49418247701689, + -67.53589230331421, + -53.57894617228146, + -62.756167691165814 ], "moduleForcesY": [ - 35.35799304336133, - 29.157332713084195, - 39.2405425695758, - 33.58676445334213 + 33.72227739407681, + 16.098766492768572, + 44.212603887497394, + 29.961682528729266 ], - "timestamp": 2.0427861433368983 + "timestamp": 2.0384917901795507 }, { - "x": 7.423388079224838, - "y": 1.9985849999652825, - "heading": 0.46645537399184306, - "angularVelocity": 0.19401513991291586, - "velocityX": 3.1202319792203808, - "velocityY": 1.5738493632366841, + "x": 7.4238027530368695, + "y": 1.9965622542854489, + "heading": 0.4692934758689833, + "angularVelocity": 0.24023905191549066, + "velocityX": 3.1283626232349375, + "velocityY": 1.5416675347118225, "moduleForcesX": [ - -67.89958925754117, - -69.39640044262892, - -64.37618878024341, - -67.53660965026158 + -68.24273916485633, + -69.7323377330834, + -62.01731512678528, + -67.61265076621554 ], "moduleForcesY": [ - 15.193911023893614, - 5.976626851451594, - 26.574607140432867, - 17.20645904856515 + 13.81064712566369, + -0.3213659067003291, + 31.862172293048193, + 17.244933344769986 ], - "timestamp": 2.104327513812998 + "timestamp": 2.1000057670399475 }, { - "x": 7.601264963854788, - "y": 2.094205845435507, - "heading": 0.47375371275159783, - "angularVelocity": 0.1185923989552555, - "velocityX": 2.8903627471056996, - "velocityY": 1.5537652920381468, + "x": 7.602133310590925, + "y": 2.090716538878092, + "heading": 0.47717310494630966, + "angularVelocity": 0.12809493841064415, + "velocityX": 2.899025012783137, + "velocityY": 1.5306161200782256, "moduleForcesX": [ - -68.97563128408632, - -68.37332903743547, - -69.6787002752127, - -69.70563687256657 + -69.13754121748958, + -68.20178675329157, + -69.06817284359947, + -69.80874265266795 ], "moduleForcesY": [ - -10.02818095513775, - -13.79457079772149, - 2.5400050436943973, - -2.895918692242975 + -9.046985055914966, + -14.8227009169618, + 9.835976008332125, + 0.7232885771622478 ], - "timestamp": 2.1658688842890976 + "timestamp": 2.161519743900344 }, { - "x": 7.765427995394615, - "y": 2.1860237902182353, - "heading": 0.4771503707617556, - "angularVelocity": 0.055193083674938345, - "velocityX": 2.667523168070835, - "velocityY": 1.491971076893497, + "x": 7.76664139791012, + "y": 2.1816852539691975, + "heading": 0.47965086497918497, + "angularVelocity": 0.04027962683177553, + "velocityX": 2.6743204669166074, + "velocityY": 1.4788300112274317, "moduleForcesX": [ - -65.89884406545639, - -65.46067825744251, - -68.71649541984166, - -68.19447216007241 + -66.40756278498199, + -65.73447233946021, + -69.52981390692969, + -68.96429053135994 ], "moduleForcesY": [ - -22.954179123046586, - -24.267429442756974, - -12.195515440032587, - -14.975245795855631 + -21.48510284383042, + -23.5840541324825, + -6.248641320167586, + -11.053848562321154 ], - "timestamp": 2.2274102547651973 + "timestamp": 2.223033720760741 }, { - "x": 7.916364373904703, - "y": 2.2725370209726456, - "heading": 0.4771759891464777, - "angularVelocity": 0.00041627907412384533, - "velocityX": 2.4526002157964135, - "velocityY": 1.4057735485108946, + "x": 7.917760773522142, + "y": 2.267858010889966, + "heading": 0.4778609500693658, + "angularVelocity": -0.029097694559421974, + "velocityX": 2.456667302700648, + "velocityY": 1.4008646704200718, "moduleForcesX": [ - -63.034541640882665, - -62.936246190035384, - -66.59623107773926, - -66.17286001113268 + -63.7079413269904, + -63.48861495951544, + -67.77169795111662, + -67.17513898955453 ], "moduleForcesY": [ - -30.054599009672646, - -30.30989854312364, - -21.022090416114956, - -22.384264891991062 + -28.621263222946787, + -29.166159121005602, + -16.89631000921434, + -19.21841268100821 ], - "timestamp": 2.288951625241297 + "timestamp": 2.2845476976211376 }, { - "x": 8.054483029258073, - "y": 2.3527934772526407, - "heading": 0.4742054330913605, - "angularVelocity": -0.04826925419658775, - "velocityX": 2.2443220598574287, - "velocityY": 1.3041057691616338, + "x": 8.055903927968282, + "y": 2.3481558395536353, + "heading": 0.47263055774144574, + "angularVelocity": -0.08502770581375621, + "velocityX": 2.245719777793735, + "velocityY": 1.3053590868608649, "moduleForcesX": [ - -60.81370264139378, - -60.97714530202473, - -64.58773267429666, - -64.36180927085024 + -61.53663246666227, + -61.636381591712215, + -65.62140779995624, + -65.27264016289558 ], "moduleForcesY": [ - -34.39454115337558, - -34.13615632978172, - -26.64149301034084, - -27.222875461504188 + -33.09645471160549, + -32.94683673534328, + -24.002173782199, + -24.982303085352363 ], - "timestamp": 2.3504929957173966 + "timestamp": 2.3460616744815344 }, { - "x": 8.180111081085983, - "y": 2.4261456194668596, - "heading": 0.4685113229121613, - "angularVelocity": -0.09252491673727926, - "velocityX": 2.0413593466641706, - "velocityY": 1.1919159688311711, + "x": 8.181425593296224, + "y": 2.421820395971198, + "heading": 0.4645721903392223, + "angularVelocity": -0.13100059228021374, + "velocityX": 2.0405389430894125, + "velocityY": 1.197525508466805, "moduleForcesX": [ - -59.113865720735134, - -59.466549474512234, - -62.90136023448186, - -62.859497948105854 + -59.837069189363575, + -60.1347503268217, + -63.62558289863267, + -63.52420699677533 ], "moduleForcesY": [ - -37.28394866924835, - -36.74169723968548, - -30.46123969045995, - -30.575354363117626 + -36.12028649557102, + -35.64612522470202, + -28.92915290640471, + -29.180149543959885 ], - "timestamp": 2.412034366193496 + "timestamp": 2.407575651341931 }, { - "x": 8.293510268919242, - "y": 2.4921279456843495, - "heading": 0.4602987892888278, - "angularVelocity": -0.1334473632907311, - "velocityX": 1.842649699803164, - "velocityY": 1.0721621196771174, + "x": 8.294623187946847, + "y": 2.488295400127025, + "heading": 0.4541517988040101, + "angularVelocity": -0.16939876215222985, + "velocityX": 1.8401930817693604, + "velocityY": 1.0806487817669281, "moduleForcesX": [ - -57.78992161833766, - -58.284723665890425, - -61.51620046633014, - -61.63027709798539 + -58.495272320583396, + -58.9108341498489, + -61.90125817659251, + -61.99096011956313 ], "moduleForcesY": [ - -39.335088546545315, - -38.615438325466826, - -33.20466598872263, - -33.01319912531834 + -38.28430915682556, + -37.65794710311188, + -32.488649405293295, + -32.336456191223455 ], - "timestamp": 2.473575736669596 + "timestamp": 2.469089628202328 }, { - "x": 8.39489265808716, - "y": 2.5503910578782976, - "heading": 0.4497269700993511, - "angularVelocity": -0.17178394156143129, - "velocityX": 1.6473859516549267, - "velocityY": 0.946730820961736, + "x": 8.395745589032531, + "y": 2.5471579028047797, + "heading": 0.4417334392214656, + "angularVelocity": -0.20187866589616352, + "velocityX": 1.6438930832772425, + "velocityY": 0.9568963946411755, "moduleForcesX": [ - -56.73532643582359, - -57.3433644167481, - -60.37384118279297, - -60.62016736060447 + -57.41823039591145, + -57.901215370721474, + -60.440306372625834, + -60.66569970555321 ], "moduleForcesY": [ - -40.86369626897553, - -40.01966277609491, - -35.26442066300551, - -34.85553891808558 + -39.90297584100897, + -39.210477609648265, + -35.155958840161816, + -34.77905570626861 ], - "timestamp": 2.5351171071456955 + "timestamp": 2.5306036050627245 }, { - "x": 8.484432685070649, - "y": 2.600663620222655, - "heading": 0.43692255880634423, - "angularVelocity": -0.20806184837855615, - "velocityX": 1.4549566623359964, - "velocityY": 0.8168905234874689, + "x": 8.48500235236776, + "y": 2.598076660395606, + "heading": 0.4276083189600344, + "angularVelocity": -0.2296245663564786, + "velocityX": 1.4509997221898334, + "velocityY": 0.8277591563683715, "moduleForcesX": [ - -55.87704936399228, - -56.580812311815635, - -59.42051911915581, - -59.78199094341051 + -56.53862920087126, + -57.05702405798682, + -59.20466714974995, + -59.52214725960545 ], "moduleForcesY": [ - -42.04664160695578, - -41.10591420167531, - -36.867255229715404, - -36.29138091153048 + -41.15627446217204, + -40.443127482255576, + -37.21736934315343, + -36.717258988355645 ], - "timestamp": 2.596658477621795 + "timestamp": 2.5921175819231213 }, { - "x": 8.56227596922161, - "y": 2.6427290793082396, - "heading": 0.42198862978658847, - "angularVelocity": -0.2426648757449358, - "velocityX": 1.2648935756345077, - "velocityY": 0.6835313994497664, + "x": 8.562571509547876, + "y": 2.640785806675407, + "heading": 0.41201408826118213, + "angularVelocity": -0.2535071132572452, + "velocityX": 1.2610005260455623, + "velocityY": 0.6942998723156388, "moduleForcesX": [ - -55.16507312556064, - -55.95401657048999, - -58.61387114486394, - -59.078804118808506 + -55.80869308264709, + -56.34173719772094, + -58.15451187186907, + -58.53176769808956 ], "moduleForcesY": [ - -42.989859151973775, - -41.96722249696166, - -38.151657125904684, - -37.43867247748044 + -42.15377542537939, + -41.44512997463475, + -38.852027537364954, + -38.28860744687602 ], - "timestamp": 2.658199848097895 + "timestamp": 2.653631558783518 }, { - "x": 8.628545730522124, - "y": 2.6764107009683005, - "heading": 0.405010585221208, - "angularVelocity": -0.27588018326589114, - "velocityX": 1.0768327189959002, - "velocityY": 0.5473004809527537, + "x": 8.62860576010961, + "y": 2.6750675306808085, + "heading": 0.3951480099492571, + "angularVelocity": -0.2741828633548061, + "velocityX": 1.0734836850427496, + "velocityY": 0.557299751293951, "moduleForcesX": [ - -54.56456662161311, - -55.43245939238377, - -57.921816727762916, - -58.48249206819515 + -55.19425623129184, + -55.728131941039244, + -57.25555323504394, + -57.66899798719165 ], "moduleForcesY": [ - -43.76032221451713, - -42.663599201968246, - -39.206371128583605, - -38.37439371125214 + -42.96566364835716, + -42.27592065616304, + -40.176385790672825, + -39.58618560604843 ], - "timestamp": 2.7197412185739944 + "timestamp": 2.7151455356439147 }, { - "x": 8.683347508309266, - "y": 2.7015615497885435, - "heading": 0.3860602892164142, - "angularVelocity": -0.30792775426009245, - "velocityX": 0.8904867955195352, - "velocityY": 0.4086819748352914, + "x": 8.683237297728128, + "y": 2.7007402779978236, + "heading": 0.3771761995999053, + "angularVelocity": -0.2921581609028107, + "velocityX": 0.8881158462978955, + "velocityY": 0.41734819674036067, "moduleForcesX": [ - -54.05076426253945, - -54.994086710555564, - -57.320209102597595, - -57.971713485412614 + -54.67049432361609, + -55.195762968597755, + -56.480010214696975, + -56.91239458884492 ], "moduleForcesY": [ - -44.402281739369485, - -43.23537505493689, - -40.090633053222774, - -39.150748009158825 + -43.638836888514874, + -42.97645664054756, + -41.268832050266276, + -40.67476273388533 ], - "timestamp": 2.781282589050094 + "timestamp": 2.7766595125043114 }, { - "x": 8.72677268008055, - "y": 2.7180575426494102, - "heading": 0.36519902233774076, - "angularVelocity": -0.3389795631342851, - "velocityX": 0.7056256861902239, - "velocityY": 0.26804721333388104, + "x": 8.726581564114948, + "y": 2.717650467677025, + "heading": 0.35824027480182863, + "angularVelocity": -0.30783125664352257, + "velocityX": 0.7046246820488491, + "velocityY": 0.2748999583879684, "moduleForcesX": [ - -53.60568984052227, - -54.62264590972197, - -56.790761924991, - -57.530148641518124 + -54.21904272949312, + -54.729099389545915, + -55.805815985531794, + -56.244420654225245 ], "moduleForcesY": [ - -44.94605887665676, - -43.71059919852908, - -40.84537555251459, - -39.80431908016251 + -44.20576368971399, + -43.57579607050465, + -42.183797695943305, + -41.60055887427103 ], - "timestamp": 2.8428239595261937 + "timestamp": 2.838173489364708 }, { - "x": 8.758901125853505, - "y": 2.725792489640272, - "heading": 0.34247964491820077, - "angularVelocity": -0.36917243219930057, - "velocityX": 0.522062565789479, - "velocityY": 0.1256869473497624, + "x": 8.758740183629461, + "y": 2.7256665220403926, + "heading": 0.338462250167195, + "angularVelocity": -0.32152082573882035, + "velocityX": 0.5227855709523735, + "velocityY": 0.13031273171557645, "moduleForcesX": [ - -53.2160444557517, - -54.305935154073225, - -56.31947803489789, - -57.14517521142788 + -53.82607120430835, + -54.316194914018055, + -55.21550784813393, + -55.65085738495609 ], "moduleForcesY": [ - -45.4130693587794, - -44.10935378815243, - -41.49973296414931, - -40.36148436994362 + -44.689609283680795, + -44.09507977862367, + -42.960129353038816, + -42.39731433858149 ], - "timestamp": 2.9043653300022934 + "timestamp": 2.899687466225105 }, { "x": 8.779803276062012, "y": 2.7246744632720947, "heading": 0.31794821481648144, - "angularVelocity": -0.398616896437924, - "velocityX": 0.33964388584139615, - "velocityY": -0.018167069721194994, + "angularVelocity": -0.33348576043569805, + "velocityX": 0.34241148934902593, + "velocityY": -0.016127371679270322, "moduleForcesX": [ - -52.8718209998866, - -54.034634846892985, - -55.89551210582599, - -56.80690426412013 + -53.48098782939392, + -53.947745103828254, + -54.695219377692204, + -55.12018458029143 ], "moduleForcesY": [ - -45.818828916724534, - -44.44637942204098, - -42.074976138022336, - -40.84174218658695 + -45.107332847724315, + -44.55002787941316, + -43.626228500899906, + -43.09016461444635 ], - "timestamp": 2.965906700478393 + "timestamp": 2.9612014430855016 }, { - "x": 8.789008750560857, - "y": 2.7112787087692207, - "heading": 0.2870074693343456, - "angularVelocity": -0.4321051934641618, - "velocityX": 0.12855971235566702, - "velocityY": -0.18707936737997807, + "x": 8.789390520920405, + "y": 2.711156481525927, + "heading": 0.29317696313872205, + "angularVelocity": -0.3456625720749201, + "velocityX": 0.13378216652004424, + "velocityY": -0.1886323872700164, "moduleForcesX": [ - -52.566704392180235, - -53.81516704620926, - -55.506512855841905, - -56.51638685989295 + -53.179448380447525, + -53.62389224585945, + -54.2363017447046, + -54.649105974722524 ], "moduleForcesY": [ - -46.18319326982662, - -44.72589340151746, - -42.6029828956687, - -41.258265442390176 + -45.47706110036372, + -44.95351001122035, + -44.210687438028955, + -43.700838292783004 ], - "timestamp": 3.037511360524447 + "timestamp": 3.03286454368984 }, { - "x": 8.782890992002454, - "y": 2.6860530746256153, - "heading": 0.25377121085635285, - "angularVelocity": -0.46416334434959966, - "velocityX": -0.08543799460076026, - "velocityY": -0.3522903974040376, + "x": 8.783794368286188, + "y": 2.6855639318912203, + "heading": 0.267539420337893, + "angularVelocity": -0.35775095669356133, + "velocityX": -0.07808973637788985, + "velocityY": -0.35712311383241235, "moduleForcesX": [ - -53.439272111965224, - -54.64572763634496, - -56.1717249078838, - -57.1626240496705 + -54.03803305894964, + -54.47570349158013, + -55.06023489691758, + -55.46707669488016 ], "moduleForcesY": [ - -45.16660023903551, - -43.7030817122355, - -41.717468923960176, - -40.353553056706815 + -44.44927461362763, + -43.91326452339743, + -43.17589421817186, + -42.65354228524133 ], - "timestamp": 3.109116020570501 + "timestamp": 3.104527644294178 }, { - "x": 8.761217284811817, - "y": 2.6493049073601336, - "heading": 0.2183564997566526, - "angularVelocity": -0.49458668020939406, - "velocityX": -0.302685707560046, - "velocityY": -0.5132091576420572, + "x": 8.76275609467282, + "y": 2.6482308380775303, + "heading": 0.2410424727336723, + "angularVelocity": -0.3697432483491591, + "velocityX": -0.2935719140806326, + "velocityY": -0.5209528125193889, "moduleForcesX": [ - -54.41587486021302, - -55.56438232577154, - -56.92221968880065, - -57.87959109885894 + -54.99526164627351, + -55.42219111728091, + -55.979540705697985, + -56.37649095420048 ], "moduleForcesY": [ - -43.98034800365324, - -42.52410083429765, - -40.68222445745944, - -39.312900897281004 + -43.254451016653434, + -42.70766979489076, + -41.972052319967815, + -41.439058648655944 ], - "timestamp": 3.180720680616555 + "timestamp": 3.1761907448985163 }, { - "x": 8.723726927916339, - "y": 2.601394409233509, - "heading": 0.1808983892167063, - "angularVelocity": -0.5231239212064431, - "velocityX": -0.5235742599904201, - "velocityY": -0.6690974874513724, + "x": 8.725986587983398, + "y": 2.5995490914917334, + "heading": 0.21369371175484705, + "angularVelocity": -0.38162960782036737, + "velocityX": -0.5130884148096344, + "velocityY": -0.679313986909043, "moduleForcesX": [ - -55.513204951308495, - -56.58432740025471, - -57.772952193024096, - -58.67869063733703 + -56.06650028997491, + -56.47788343064675, + -57.008812567039705, + -57.391121229420555 ], "moduleForcesY": [ - -42.58123461820781, - -41.151361111760814, - -39.45859689985431, - -38.10349107560392 + -41.850827206177, + -41.29576254132913, + -40.5570050084547, + -40.01607334967796 ], - "timestamp": 3.252325340662609 + "timestamp": 3.2478538455028545 }, { - "x": 8.670126749763783, - "y": 2.5427487589796156, - "heading": 0.14155435817055417, - "angularVelocity": -0.5494618788895446, - "velocityX": -0.748557120696901, - "velocityY": -0.8190200221071341, + "x": 8.673161820665543, + "y": 2.5399838352697053, + "heading": 0.1855016713653994, + "angularVelocity": -0.3933968827988571, + "velocityX": -0.7371264552102906, + "velocityY": -0.8311844689904736, "moduleForcesX": [ - -56.750175428910545, - -57.721068226165364, - -58.740871201237205, - -59.57336306357096 + -57.26911890755975, + -57.659157229516524, + -58.16431071641967, + -58.52626643425478 ], "moduleForcesY": [ - -40.9113473334207, - -39.5340460978986, - -37.9953221829711, - -36.68127355062254 + -40.18252449430719, + -39.62280351144781, + -38.874499091161304, + -38.32952205223615 ], - "timestamp": 3.323930000708663 + "timestamp": 3.3195169461071927 }, { - "x": 8.600086131198294, - "y": 2.4738813585896815, - "heading": 0.10051029066858429, - "angularVelocity": -0.5732038595752198, - "velocityX": -0.9781572668656089, - "velocityY": -0.9617726045433423, + "x": 8.603918051131696, + "y": 2.4700943215420597, + "heading": 0.156476193662526, + "angularVelocity": -0.4050268193547327, + "velocityX": -0.9662402121860534, + "velocityY": -0.9752510446556707, "moduleForcesX": [ - -58.14728541567019, - -58.992253649512634, - -59.84438084571835, - -60.57897527375518 + -58.62178890554708, + -58.98353550726006, + -59.46309693106811, + -59.79789639516721 ], "moduleForcesY": [ - -38.8916920572076, - -37.60212331585296, - -36.2234088942802, - -34.98615199145863 + -38.17360914029891, + -37.61450756539028, + -36.848345847349954, + -36.304918084781264 ], - "timestamp": 3.395534660754717 + "timestamp": 3.391180046711531 }, { - "x": 8.513231926199088, - "y": 2.395418529280188, - "heading": 0.0579886645280164, - "angularVelocity": -0.5938388103961255, - "velocityX": -1.2129686104695387, - "velocityY": -1.09577825324536, + "x": 8.517847357752716, + "y": 2.3905626117140066, + "heading": 0.12662900025486085, + "angularVelocity": -0.41649319044197497, + "velocityX": -1.2010461821096667, + "velocityY": -1.1098000108474018, "moduleForcesX": [ - -59.72456510756159, - -60.41656271939983, - -61.10169314907387, - -61.71200087220666 + -60.14239359593657, + -60.46764931194161, + -60.920705975153766, + -61.22039017159285 ], "moduleForcesY": [ - -36.41263890584794, - -35.257097835848604, - -34.04864509744792, - -32.934708796771055 + -35.71931021724835, + -35.16851680517709, + -34.373858499651185, + -33.84003668835228 ], - "timestamp": 3.4671393208007713 + "timestamp": 3.462843147315869 }, { - "x": 8.409144338572357, - "y": 2.3081371726052624, - "heading": 0.014259918978119247, - "angularVelocity": -0.6106969228227869, - "velocityX": -1.4536426478358366, - "velocityY": -1.2189340277404956, + "x": 8.414494950035653, + "y": 2.302233586652294, + "heading": 0.09597460073164957, + "angularVelocity": -0.4277570920697149, + "velocityX": -1.4421983816704484, + "velocityY": -1.2325593550492429, "moduleForcesX": [ - -61.49629524842574, - -62.01024906206618, - -62.5267777803112, - -62.98751725355034 + -61.84288712238317, + -62.122276292758905, + -62.54567545312701, + -62.801268264067 ], "moduleForcesY": [ - -33.31963140107074, - -32.35756349250855, - -31.340658297933924, - -30.409079395346843 + -32.67310530550032, + -32.14185463603263, + -31.305402017642134, + -30.792810837453125 ], - "timestamp": 3.5387439808468253 + "timestamp": 3.5345062479202074 }, { - "x": 8.287356319734036, - "y": 2.21301866214228, - "heading": -0.030341619606405787, - "angularVelocity": -0.6228859763573827, - "velocityX": -1.700839285599414, - "velocityY": -1.3283843593671696, + "x": 8.2933614907194, + "y": 2.2061711843005387, + "heading": 0.06453176165426741, + "angularVelocity": -0.4387591216710293, + "velocityX": -1.690318424610812, + "velocityY": -1.340472314784823, "moduleForcesX": [ - -63.458562590492434, - -63.778062663565834, - -64.12020804800913, - -64.4128163560726 + -63.717577250904, + -63.94109844594106, + -64.32744860294157, + -64.52959830151751 ], "moduleForcesY": [ - -29.392355415191965, - -28.696611629664407, - -27.917314517688634, - -27.239890506735552 + -28.828283743204985, + -28.332961805960878, + -27.43891241104092, + -26.96427075977155 ], - "timestamp": 3.6103486408928793 + "timestamp": 3.6061693485245456 }, { - "x": 8.147362398954167, - "y": 2.111326488188939, - "heading": -0.07539545756391505, - "angularVelocity": -0.6292025955926854, - "velocityX": -1.955095110986199, - "velocityY": -1.4201893268948602, + "x": 8.153917386350976, + "y": 2.103737230084854, + "heading": 0.032325911066374, + "angularVelocity": -0.4494063237049454, + "velocityX": -1.9458285113605962, + "velocityY": -1.4293821136938516, "moduleForcesX": [ - -65.56127142602155, - -65.69084909052835, - -65.8493885566579, - -65.97214747432078 + -65.71756976574333, + -65.87625105005283, + -66.21063177131644, + -66.35137198001577 ], "moduleForcesY": [ - -24.316794034856503, - -23.9674759446695, - -23.52399917574761, - -23.180580992000287 + -23.893867912535846, + -23.458019215236998, + -22.489715553229402, + -22.076652060585157 ], - "timestamp": 3.6819533009389334 + "timestamp": 3.677832449128884 }, { - "x": 7.988650071662026, - "y": 2.0047168985365014, - "heading": -0.12036455636704498, - "angularVelocity": -0.6280191648728856, - "velocityX": -2.216508355602279, - "velocityY": -1.4888638474628435, + "x": 7.995643377180051, + "y": 1.996698976059294, + "heading": -0.0006069291037716975, + "angularVelocity": -0.45955086917006693, + "velocityX": -2.2085844435447637, + "velocityY": -1.4936313545311521, "moduleForcesX": [ - -67.64804942069694, - -67.63218085033044, - -67.60757677396883, - -67.59149476249789 + -67.69752144856352, + -67.78734757334065, + -68.04263214008894, + -68.11934773122928 ], "moduleForcesY": [ - -17.65673640990495, - -17.716555475855195, - -17.811425350367866, - -17.871500295896368 + -17.47107619907958, + -17.127002103641317, + -16.07246012795104, + -15.752714784412063 ], - "timestamp": 3.7535579609849874 + "timestamp": 3.749495549733222 }, { - "x": 7.810781225665186, - "y": 1.8953882630351646, - "heading": -0.1645573135688971, - "angularVelocity": -0.6171771107275482, - "velocityX": -2.4840400873691744, - "velocityY": -1.5268368766923714, + "x": 7.818124590832862, + "y": 1.8873638999769546, + "heading": -0.034213912395681174, + "angularVelocity": -0.468957985469511, + "velocityX": -2.4771295806372327, + "velocityY": -1.5256816291830964, "moduleForcesX": [ - -69.33792438793783, - -69.27727776528677, - -69.1346031312489, - -69.06017856287379 + -69.313687888199, + -69.3432482954395, + -69.47611214375348, + -69.49889577515765 ], "moduleForcesY": [ - -8.85711879468359, - -9.301819468635278, - -10.32983168125111, - -10.801199653575688 + -9.057792824867414, + -8.844692568248144, + -7.710678951592893, + -7.521600845904924 ], - "timestamp": 3.8251626210310414 + "timestamp": 3.8211586503375603 }, { - "x": 7.613573483725926, - "y": 1.7862509730258411, - "heading": -0.20709141412342455, - "angularVelocity": -0.5940130227162668, - "velocityX": -2.754118821490405, - "velocityY": -1.5241646275414062, + "x": 7.621240028444532, + "y": 1.7787112968796408, + "heading": -0.06841646549100246, + "angularVelocity": -0.4772686753278808, + "velocityX": -2.747363157998911, + "velocityY": -1.5161582764496822, "moduleForcesX": [ - -69.8380909495494, - -69.85117254072334, - -69.88578385231288, - -69.87027129628852 + -69.86517241058907, + -69.86591053003337, + -69.82282638095482, + -69.8236070877981 ], "moduleForcesY": [ - 2.6196094231982188, - 2.050557264342375, - -0.5954925448843501, - -1.3097488777935367 + 1.837312109452161, + 1.8863774422298984, + 3.0464717958262337, + 3.075434675486523 ], - "timestamp": 3.8967672810770955 + "timestamp": 3.8928217509418985 }, { - "x": 7.3974393349223595, - "y": 1.6810231576681, - "heading": -0.24689444053872592, - "angularVelocity": -0.5558720115380904, - "velocityX": -3.0184369098960024, - "velocityY": -1.469566579745805, + "x": 7.40547828858971, + "y": 1.6744202309428289, + "heading": -0.10310081581402811, + "angularVelocity": -0.48399176187648607, + "velocityX": -3.0107787415740312, + "velocityY": -1.4552965899789445, "moduleForcesX": [ - -67.81774479937387, - -67.81197181841603, - -68.89677208175078, - -68.95839961219598 + -68.17759957909404, + -68.20364257441437, + -67.95854331471489, + -67.98903555111266 ], "moduleForcesY": [ - 16.827330110890028, - 16.80465353313287, - 11.649075089581624, - 11.210500710885821 + 15.314544404106083, + 15.206518112264616, + 16.260066564096665, + 16.13994452069931 ], - "timestamp": 3.9683719411231495 + "timestamp": 3.9644848515462368 }, { - "x": 7.163840114735354, - "y": 1.5840259794682252, - "heading": -0.2828186940424167, - "angularVelocity": -0.5017027310874074, - "velocityX": -3.2623466131500303, - "velocityY": -1.3546210279818205, + "x": 7.172300767498466, + "y": 1.5786130838476118, + "heading": -0.13811594024177143, + "angularVelocity": -0.48860744417223123, + "velocityX": -3.253801735130716, + "velocityY": -1.3369104362952573, "moduleForcesX": [ - -61.83333462479733, - -61.05938989064099, - -64.89350871525448, - -64.5824879376315 + -62.90664616032858, + -62.994759663175806, + -62.62640903247994, + -62.71838864361929 ], "moduleForcesY": [ - 32.52596953901682, - 33.92307848948774, - 25.88066947280988, - 26.602254108081837 + 30.40188460343236, + 30.221851782690536, + 30.976021760761594, + 30.79224774785525 ], - "timestamp": 4.0399766011692035 + "timestamp": 4.036147952150575 }, { - "x": 6.91551546736203, - "y": 1.4994886743390452, - "heading": -0.3139208387202008, - "angularVelocity": -0.4343592254719174, - "velocityX": -3.467995619469574, - "velocityY": -1.180611779663611, + "x": 6.924263851753144, + "y": 1.4952348436632368, + "heading": -0.17328998021723432, + "angularVelocity": -0.4908249807619052, + "velocityX": -3.461152443218535, + "velocityY": -1.1634751982713893, "moduleForcesX": [ - -51.68933538210916, - -48.85035357028174, - -57.056547684318936, - -55.184870641943775 + -53.63079242417267, + -53.732278169292705, + -53.450679135245664, + -53.553119173625326 ], "moduleForcesY": [ - 47.00644705748203, - 49.9244819592894, - 40.31096815949663, - 42.80215553528719 + 44.77877441586359, + 44.65783761257958, + 44.9941386909519, + 44.87304464918282 ], - "timestamp": 4.111581261215258 + "timestamp": 4.107811052754913 }, { - "x": 6.656064858498082, - "y": 1.43069889782314, - "heading": -0.3397354689772922, - "angularVelocity": -0.3605160647433842, - "velocityX": -3.6233760302342883, - "velocityY": -0.9606885427800527, + "x": 6.664645466021959, + "y": 1.4273879906699989, + "heading": -0.20846188348487105, + "angularVelocity": -0.49079516475048257, + "velocityX": -3.622762391548932, + "velocityY": -0.9467473835360443, "moduleForcesX": [ - -39.29968551732011, - -33.85969689804127, - -46.079653739591876, - -41.530113395690364 + -41.769412633151376, + -41.76713542840772, + -41.77205019669556, + -41.769773098851616 ], "moduleForcesY": [ - 57.77680095878208, - 61.10531694531754, - 52.513961690389884, - 56.154301989543136 + 56.01545952386056, + 56.01714957737264, + 56.01348552259297, + 56.01517566218937 ], - "timestamp": 4.183185921261312 + "timestamp": 4.179474153359251 }, { - "x": 6.389094964926698, - "y": 1.3796716923906485, - "heading": -0.36043754169192455, - "angularVelocity": -0.2891162768082043, - "velocityX": -3.728387138486177, - "velocityY": -0.7126240861931642, + "x": 6.396804785667352, + "y": 1.37708752891827, + "heading": -0.24350598675754967, + "angularVelocity": -0.4890118202694272, + "velocityX": -3.7374977930886835, + "velocityY": -0.7019018341035008, "moduleForcesX": [ - -27.32133306734179, - -20.310347551724792, - -33.900176274380996, - -27.121135359167035 + -29.670078278170966, + -29.48805559061184, + -29.820795736138948, + -29.638793031028708 ], "moduleForcesY": [ - 64.32684835868893, - 66.85977151354284, - 61.10135000376463, - 64.37959537846069 + 63.27577283382489, + 63.36048200304406, + 63.204474407441516, + 63.28970026112863 ], - "timestamp": 4.254790581307366 + "timestamp": 4.25113725396359 }, { - "x": 6.117512662620233, - "y": 1.34740277403393, - "heading": -0.37733465959682994, - "angularVelocity": -0.23597790833777565, - "velocityX": -3.7928020624885344, - "velocityY": -0.45065388671580686, + "x": 6.123724098762273, + "y": 1.3454463182894645, + "heading": -0.27833593889663377, + "angularVelocity": -0.4860235162218966, + "velocityX": -3.8106178019395767, + "velocityY": -0.4415272345457245, "moduleForcesX": [ - -17.461162262072158, - -11.425931826783211, - -21.98887484573413, - -15.772925216616537 + -18.964618645051026, + -18.60764038496825, + -19.189884865939113, + -18.832039003279412 ], "moduleForcesY": [ - 67.68404989930426, - 68.95306382811246, - 66.34075545396243, - 68.07770320581683 + 67.27850831701049, + 67.37773513616717, + 67.21397107913663, + 67.3147241815706 ], - "timestamp": 4.32639524135342 + "timestamp": 4.322800354567928 }, { - "x": 5.8435235121250875, - "y": 1.3343250077166011, - "heading": -0.3920716678293369, - "angularVelocity": -0.205810742248178, - "velocityX": -3.8264150729704207, - "velocityY": -0.18263848063684318, + "x": 5.847858828640413, + "y": 1.3329886237876336, + "heading": -0.3128943538499734, + "angularVelocity": -0.4822344367171772, + "velocityX": -3.849474384941141, + "velocityY": -0.17383694532855082, "moduleForcesX": [ - -9.461901729986893, - -5.721136386114701, - -11.732782343904876, - -7.862920839581364 + -10.164815803497616, + -9.673316441913729, + -10.413603531769702, + -9.919643964436528 ], "moduleForcesY": [ - 69.26711577967508, - 69.6728315865383, - 68.91304347288967, - 69.45744160036715 + 69.17032970505136, + 69.24042697051493, + 69.13258323678724, + 69.20483068344068 ], - "timestamp": 4.397999901399474 + "timestamp": 4.394463455172266 }, { - "x": 5.568929040137215, - "y": 1.3405893640009074, - "heading": -0.4055914386320038, - "angularVelocity": -0.18881132588258015, - "velocityX": -3.8348687335609193, - "velocityY": 0.08748531562439549, + "x": 5.571170150686736, + "y": 1.339901067236654, + "heading": -0.3471395996450313, + "angularVelocity": -0.4778644170607483, + "velocityX": -3.8609643682780583, + "velocityY": 0.09645749891823462, "moduleForcesX": [ - -2.762826914760814, - -0.5336705163664741, - -3.868314495467215, - -1.5820295641402107 + -3.140660151426149, + -2.5499567673882018, + -3.391519077751636, + -2.7966358407203566 ], "moduleForcesY": [ - 69.8660118858112, - 69.91723429353682, - 69.81049649840442, - 69.89820089852721 + 69.85390969745158, + 69.87766236501122, + 69.84141674708727, + 69.86745404796969 ], - "timestamp": 4.469604561445528 + "timestamp": 4.466126555776604 }, { "x": 5.295215606689453, "y": 1.3661898374557495, - "heading": -0.418290619060492, - "angularVelocity": -0.17735131233526324, - "velocityX": -3.8225645268299044, - "velocityY": 0.35752524260818264, + "heading": -0.3810366506112695, + "angularVelocity": -0.4730056427977949, + "velocityX": -3.850720128910756, + "velocityY": 0.3668383030792834, "moduleForcesX": [ - 2.720138806867678, - 4.271594537524151, - 2.07835169845561, - 3.6608431501275804 + 2.4358890124890213, + 3.105036958141336, + 2.1874776648542436, + 2.8624717837810634 ], "moduleForcesY": [ - 69.87725025164747, - 69.79888184516116, - 69.89739500886515, - 69.83163852773065 + 69.89157339518185, + 69.86477674457022, + 69.89901845264843, + 69.87435635433927 ], - "timestamp": 4.541209221491582 + "timestamp": 4.537789656380943 }, { - "x": 5.055367481611433, - "y": 1.4037520138273964, - "heading": -0.42898005555429164, - "angularVelocity": -0.16921534609514086, - "velocityX": -3.79683096661731, - "velocityY": 0.5946147562129904, + "x": 5.053706815736057, + "y": 1.4043054270449447, + "heading": -0.4105962505484791, + "angularVelocity": -0.46833496540702335, + "velocityX": -3.8264053470585537, + "velocityY": 0.6038939420574941, "moduleForcesX": [ - 7.128107876197704, - 8.397495650017172, - 6.681822586978301, - 7.973498881488917 + 6.887915896080987, + 7.623701240859441, + 6.643290267386372, + 7.386549953753599 ], "moduleForcesY": [ - 69.56442888170623, - 69.42212415607871, - 69.60712711861942, - 69.47048072990539 + 69.5924526319861, + 69.51542701820333, + 69.6153579444215, + 69.54014360764195 ], - "timestamp": 4.60437983066192 + "timestamp": 4.600906019925398 }, { - "x": 4.817922325323769, - "y": 1.4561844229991425, - "heading": -0.4391809724358013, - "angularVelocity": -0.16148200904637555, - "velocityX": -3.7587916185420474, - "velocityY": 0.8300127204783294, + "x": 4.814521797126242, + "y": 1.457279005501737, + "heading": -0.43977801159576735, + "angularVelocity": -0.46234857980584404, + "velocityX": -3.789588074752571, + "velocityY": 0.8393002302720002, "moduleForcesX": [ - 10.725498733701588, - 11.940280266617222, - 10.355733338830216, - 11.59193024794521 + 10.475140797961476, + 11.417395555766568, + 10.184857658397029, + 11.139880728378749 ], "moduleForcesY": [ - 69.09197337769128, - 68.89195807698471, - 69.14665294562548, - 68.94968761414044 + 69.13513408837922, + 68.98542996372927, + 69.17724110186298, + 69.02951613308481 ], - "timestamp": 4.667550439832259 + "timestamp": 4.664022383469854 }, { - "x": 4.583800693836467, - "y": 1.523306626333883, - "heading": -0.44902694770580176, - "angularVelocity": -0.15586323132409483, - "velocityX": -3.7061797339328746, - "velocityY": 1.0625543146773644, + "x": 4.578623056803542, + "y": 1.5249252632965482, + "heading": -0.46848135774782446, + "angularVelocity": -0.4547686929371364, + "velocityX": -3.7375210971485733, + "velocityY": 1.071770520289322, "moduleForcesX": [ - 15.088104493088409, - 15.974726148554316, - 14.871545105185097, - 15.770080616776701 + 14.840754027841374, + 16.029076302383867, + 14.51917987513575, + 15.728849295978776 ], "moduleForcesY": [ - 68.26065337301016, - 68.05830212763824, - 68.30673212222058, - 68.10456636002334 + 68.32109565083954, + 68.05166292944023, + 68.38835125483823, + 68.11984422324372 ], - "timestamp": 4.7307210490025975 + "timestamp": 4.72713874701431 }, { - "x": 4.354069654505351, - "y": 1.6048331009047445, - "heading": -0.4589311264045249, - "angularVelocity": -0.15678460012973233, - "velocityX": -3.636676016716078, - "velocityY": 1.2905760391043033, + "x": 4.347193447367128, + "y": 1.6069250977069311, + "heading": -0.4965850522073392, + "angularVelocity": -0.4452679603399553, + "velocityX": -3.6667132965195126, + "velocityY": 1.299185025966005, "moduleForcesX": [ - 20.43945052990814, - 20.29380047894279, - 20.463831834196373, - 20.318512311359303 + 20.193590004699995, + 21.665646251529257, + 19.875228316396825, + 21.38196372017987 ], "moduleForcesY": [ - 66.83870936216607, - 66.88312038765154, - 66.83152320893446, - 66.87589139118117 + 66.92237625745865, + 66.45990723727093, + 67.0149915440133, + 66.54903999850195 ], - "timestamp": 4.793891658172936 + "timestamp": 4.790255110558766 }, { - "x": 4.129951329960104, - "y": 1.7003267532429973, - "heading": -0.46978399762811174, - "angularVelocity": -0.17180254181691199, - "velocityX": -3.5478259191852364, - "velocityY": 1.5116785098708787, + "x": 4.121687707159994, + "y": 1.7027378890121, + "heading": -0.5239423865994234, + "angularVelocity": -0.4334428166606177, + "velocityX": -3.572856982615943, + "velocityY": 1.5180340869556606, "moduleForcesX": [ - 27.12931598295055, - 24.762739453054976, - 27.29639832285452, - 25.017031498332177 + 26.76366004990429, + 28.54407768472358, + 26.51477620428529, + 28.34898574175057 ], "moduleForcesY": [ - 64.39426565175329, - 65.34133562291419, - 64.32896211579789, - 65.24961743862173 + 64.56136029572545, + 63.79374619278623, + 64.66009920375362, + 63.876720382176266 ], - "timestamp": 4.857062267343275 + "timestamp": 4.853371474103222 }, { - "x": 3.9129272992378854, - "y": 1.8090800592804388, - "heading": -0.48282197066425664, - "angularVelocity": -0.20639302370803844, - "velocityX": -3.435522208389926, - "velocityY": 1.721580770959322, + "x": 3.903869186822254, + "y": 1.8114606574002452, + "heading": -0.5503757290218597, + "angularVelocity": -0.4188033172066106, + "velocityX": -3.4510625787926257, + "velocityY": 1.7225765599053773, "moduleForcesX": [ - 35.59984400154966, - 30.289367075153166, - 35.35330491034905, - 30.469918831002943 + 34.722789021923006, + 36.79190026410112, + 34.65315726916901, + 36.79826370259493 ], "moduleForcesY": [ - 60.10095977176368, - 62.94451217757728, - 60.26121508533313, - 62.87162431394478 + 60.634978039609805, + 59.40218821323202, + 60.6689615218732, + 59.39227430417506 ], - "timestamp": 4.920232876513613 + "timestamp": 4.9164878376476775 }, { - "x": 3.7049939746242946, - "y": 1.9297950624975384, - "heading": -0.498750181696931, - "angularVelocity": -0.2521459147199961, - "velocityX": -3.2916149985652714, - "velocityY": 1.910936190144909, + "x": 3.6957796336708695, + "y": 1.9316273259274328, + "heading": -0.5756742900350444, + "angularVelocity": -0.40082412218451013, + "velocityX": -3.296919237193045, + "velocityY": 1.903890873601256, "moduleForcesX": [ - 45.789906461187606, - 39.500956547308036, - 44.60405217605959, - 38.88284700733399 + 43.97155118149319, + 46.20214883747921, + 44.22377698479971, + 46.54083607831509 ], "moduleForcesY": [ - 52.72236020041571, - 57.580980046986554, - 53.75348320494377, - 58.023668192173425 + 54.28134738689487, + 52.39692915304774, + 54.06704722487376, + 52.087132813104375 ], - "timestamp": 4.983403485683952 + "timestamp": 4.979604201192133 }, { - "x": 3.508517364442358, - "y": 2.060224954573662, - "heading": -0.5175036294890961, - "angularVelocity": -0.2968698266245423, - "velocityX": -3.1102535302792567, - "velocityY": 2.064724304374236, + "x": 3.499550666767407, + "y": 2.061002715105782, + "heading": -0.5996083713418497, + "angularVelocity": -0.37920564434843623, + "velocityX": -3.109003052199785, + "velocityY": 2.0497915582101762, "moduleForcesX": [ - 56.310610811243905, - 51.69004210860725, - 54.5613125649118, - 50.14303242636139 + 53.763954059728796, + 55.84492515312586, + 54.42048149959161, + 56.552580490225075 ], "moduleForcesY": [ - 41.26969515333533, - 46.91561591928861, - 43.58578238706644, - 48.59522594128138 + 44.579593956853635, + 41.94760445118944, + 43.76152240959032, + 40.974101674150546 ], - "timestamp": 5.04657409485429 + "timestamp": 5.042720564736589 }, { - "x": 3.3256131531226463, - "y": 2.1971031258571245, - "heading": -0.5385715679309702, - "angularVelocity": -0.3335085527680234, - "velocityX": -2.8954004674311986, - "velocityY": 2.1668015091380983, + "x": 3.3170061238667126, + "y": 2.1965515433699756, + "heading": -0.621969243888579, + "angularVelocity": -0.3542801151872387, + "velocityX": -2.8921904344524014, + "velocityY": 2.1476019949837775, "moduleForcesX": [ - 64.77216448239771, - 62.673602768769214, - 63.38542095359247, - 61.15353995273158 + 62.44766102413855, + 63.94433819362826, + 63.329847528999416, + 64.77963805969534 ], "moduleForcesY": [ - 26.035622904980194, - 30.721312600843035, - 29.28306014997138, - 33.678552232388746 + 31.256641416018002, + 28.0802822128175, + 29.404706704310698, + 26.071343409245944 ], - "timestamp": 5.109744704024629 + "timestamp": 5.105836928281045 }, { - "x": 3.157514306984536, - "y": 2.3366007043510604, - "heading": -0.561190847527418, - "angularVelocity": -0.35806651057385414, - "velocityX": -2.6610293670721896, - "velocityY": 2.208267109120054, + "x": 3.149232244356222, + "y": 2.334780583324695, + "heading": -0.6426179790087161, + "angularVelocity": -0.32715343471259145, + "velocityX": -2.6581677094296006, + "velocityY": 2.1900666038429124, "moduleForcesX": [ - 69.18908773846705, - 68.77071008684689, - 68.72649352066082, - 68.18965051435607 + 68.07446024974723, + 68.73289751434758, + 68.68747354304334, + 69.20841989455995 ], "moduleForcesY": [ - 9.337187838928434, - 11.986015068259727, - 12.328677058413632, - 14.979951486247026 + 15.578092751741488, + 12.393772344765436, + 12.552654231475199, + 9.321777066848862 ], - "timestamp": 5.1729153131949674 + "timestamp": 5.168953291825501 }, { - "x": 3.0044268603736923, - "y": 2.4751119466714426, - "heading": -0.5845404524188933, - "angularVelocity": -0.3696276670138402, - "velocityX": -2.4233967128296365, - "velocityY": 2.1926532629579243, + "x": 2.996456707624642, + "y": 2.4723242803752896, + "heading": -0.6615054771208647, + "angularVelocity": -0.2992488326556613, + "velocityX": -2.4205376886767986, + "velocityY": 2.179208200955956, "moduleForcesX": [ - 69.58635593128226, - 69.65294315110864, - 69.70772082110078, - 69.75414910427422 + 69.84418644116603, + 69.8030417146416, + 69.73398928648147, + 69.55639119192205 ], "moduleForcesY": [ - -5.863213664973564, - -4.950604413431918, - -4.219222524167902, - -3.2792456106314267 + 0.04124705741425996, + -2.629684927774557, + -3.7965442820408897, + -6.360953421225059 ], - "timestamp": 5.236085922365306 + "timestamp": 5.232069655369957 }, { - "x": 2.8658808016377564, - "y": 2.609745022810685, - "heading": -0.607898551213732, - "angularVelocity": -0.36976212674875547, - "velocityX": -2.193204411918028, - "velocityY": 2.1312613240155165, + "x": 2.858285394290842, + "y": 2.6063679764517347, + "heading": -0.6786531021168939, + "angularVelocity": -0.2716827147994899, + "velocityX": -2.18915199758744, + "velocityY": 2.1237550541395294, "moduleForcesX": [ - 67.49011979913459, - 67.49197422814143, - 67.49549991286162, - 67.49735359030318 + 68.5832050639289, + 68.18255030614512, + 67.65433881512726, + 67.18772346415568 ], "moduleForcesY": [ - -18.014094750436488, - -18.00696556606115, - -17.99399260732844, - -17.986857779229418 + -13.292687011674952, + -15.252960265172256, + -17.395421555860977, + -19.15158282474659 ], - "timestamp": 5.299256531535645 + "timestamp": 5.295186018914412 }, { - "x": 2.7411339659112834, - "y": 2.738363105075064, - "heading": -0.6306961855706249, - "angularVelocity": -0.36088989256727516, - "velocityX": -1.9747606895810539, - "velocityY": 2.0360430895570842, + "x": 2.734026522437346, + "y": 2.734745646026211, + "heading": -0.6941195885833025, + "angularVelocity": -0.2450471731552614, + "velocityX": -1.9687267275145863, + "velocityY": 2.0339839364169796, "moduleForcesX": [ - 64.39517539155403, - 64.27134897958376, - 63.8241376908432, - 63.70527604163203 + 65.71596180721829, + 65.23236893280917, + 64.13301722958752, + 63.6607820099661 ], "moduleForcesY": [ - -27.117529617984484, - -27.416726077312312, - -28.43391532243881, - -28.706015086897278 + -23.750213140906997, + -25.069078070391868, + -27.734992318593385, + -28.82188410507732 ], - "timestamp": 5.362427140705983 + "timestamp": 5.358302382458868 }, { - "x": 2.629408519345831, - "y": 2.8594244836840406, - "heading": -0.6525029140354126, - "angularVelocity": -0.34520370709084347, - "velocityX": -1.7686301910463869, - "velocityY": 1.9164193633551478, + "x": 2.622913491858143, + "y": 2.8558455619096765, + "heading": -0.7079766418555111, + "angularVelocity": -0.21954771304985882, + "velocityX": -1.7604472808535805, + "velocityY": 1.918677013104065, "moduleForcesX": [ - 61.165285278261656, - 60.992919135287345, - 59.86697681442411, - 59.72951823823286 + 62.3261455799643, + 61.90369851571434, + 60.3053013643231, + 59.94983415338624 ], "moduleForcesY": [ - -33.81189902733295, - -34.13067207401691, - -36.05965076270056, - -36.295299281822466 + -31.626927610118603, + -32.460334406565565, + -35.32513309689565, + -35.9385060308983 ], - "timestamp": 5.425597749876322 + "timestamp": 5.421418746003324 }, { - "x": 2.529987992544686, - "y": 2.971809130496302, - "heading": -0.6729951982943887, - "angularVelocity": -0.3243958626980939, - "velocityX": -1.5738415080509853, - "velocityY": 1.7790654275506226, + "x": 2.5242136717280004, + "y": 2.968472284388882, + "heading": -0.720296269771335, + "angularVelocity": -0.19518912725614584, + "velocityX": -1.5637754551658334, + "velocityY": 1.7844298396544638, "moduleForcesX": [ - 58.16870881285027, - 58.01912021029308, - 56.16470015343222, - 56.10022212287148 + 58.98754783726104, + 58.678775107744755, + 56.70105562415566, + 56.492209664989446 ], "moduleForcesY": [ - -38.76602349429578, - -38.998640161944856, - -41.61590645152988, - -41.71135809779262 + -37.51207015880541, + -38.003843268920306, + -40.88479342874241, + -41.18285690919068 ], - "timestamp": 5.48876835904666 + "timestamp": 5.48453510954778 }, { - "x": 2.4422441011606852, - "y": 3.074687163648499, - "heading": -0.6919282071615437, - "angularVelocity": -0.2997123047539793, - "velocityX": -1.3889986583381029, - "velocityY": 1.6285743402408528, + "x": 2.437268193420317, + "y": 3.0717284892697196, + "heading": -0.7311460366120948, + "angularVelocity": -0.17190101316781217, + "velocityX": -1.377542580482226, + "velocityY": 1.6359656843682122, "moduleForcesX": [ - 55.516392904604864, - 55.43040225099981, - 52.89236318545878, - 52.94888563341702 + 55.933543070193124, + 55.74620404374717, + 53.4983817914488, + 53.427889596621455 ], "moduleForcesY": [ - -42.49669388469859, - -42.617601177326456, - -45.721215937319954, - -45.66397764278443 + -41.94896951930148, + -42.20583852522306, + -45.01283639047294, + -45.10426074033776 ], - "timestamp": 5.551938968216999 + "timestamp": 5.547651473092236 }, { - "x": 2.36563547825889, - "y": 3.1674290736500192, - "heading": -0.709114876775145, - "angularVelocity": -0.2720674984668532, - "velocityX": -1.212725726535598, - "velocityY": 1.4681180254482455, + "x": 2.3614992683800686, + "y": 3.1649283314618284, + "heading": -0.7405881776451042, + "angularVelocity": -0.1495989392094639, + "velocityX": -1.2004640442708914, + "velocityY": 1.4766351696809092, "moduleForcesX": [ - 53.21000778523316, - 53.20542690598686, - 50.063878893766116, - 50.25772899182876 + 53.226119004537075, + 53.150380788764224, + 50.717908435297424, + 50.76595922311749 ], "moduleForcesY": [ - -45.3662355318544, - -45.379935036516486, - -48.8163206207447, - -48.624450752509695 + -45.3497145739425, + -45.44508539739661, + -48.13839282499532, + -48.093987559665536 ], - "timestamp": 5.615109577387337 + "timestamp": 5.6107678366366915 }, { - "x": 2.29969733456388, - "y": 3.24954609830814, - "heading": -0.7244111189583969, - "angularVelocity": -0.24214175522680043, - "velocityX": -1.0438104770718435, - "velocityY": 1.2999245335230758, + "x": 2.2964046195038508, + "y": 3.2475371925356646, + "heading": -0.7486801929297746, + "angularVelocity": -0.12820788192226776, + "velocityX": -1.0313434618325024, + "velocityY": 1.3088342932756496, "moduleForcesX": [ - 51.212589224847115, - 51.29435553074885, - 47.635142275536005, - 47.96569255346641 + 50.85429251327673, + 50.87522935086129, + 48.32191532894057, + 48.467633279346366 ], "moduleForcesY": [ - -47.62154135295099, - -47.541251547195635, - -51.20041879059338, - -50.89807366778042 + -48.006099795729824, + -47.9893857552052, + -50.55448655081397, + -50.41998583087466 ], - "timestamp": 5.678280186557676 + "timestamp": 5.673884200181147 }, { - "x": 2.244029529169606, - "y": 3.32065049496387, - "heading": -0.7377053964323423, - "angularVelocity": -0.2104503605165108, - "velocityX": -0.8812295167863048, - "velocityY": 1.1255930184874117, + "x": 2.2415479359062194, + "y": 3.3191302814359016, + "heading": -0.7554758085003378, + "angularVelocity": -0.10766804658789833, + "velocityX": -0.8691356807810039, + "velocityY": 1.1343031328129516, "moduleForcesX": [ - 49.47830306207863, - 49.64543262889328, - 45.54785025385102, - 46.007190229326234 + 48.7813998909101, + 48.883997299800775, + 46.25717342000062, + 46.48203569715231 ], "moduleForcesY": [ - -49.430771057830185, - -49.27017896162637, - -53.075077496144715, - -52.68407579049555 + -50.12061198883146, + -50.02516024483403, + -52.45958599626199, + -52.26481927286433 ], - "timestamp": 5.7414507957280145 + "timestamp": 5.737000563725603 }, { - "x": 2.198285645058137, - "y": 3.3804286588611365, - "heading": -0.748911194782175, - "angularVelocity": -0.177389429942277, - "velocityX": -0.7241323886575408, - "velocityY": 0.9462970942084137, + "x": 2.1965490021845606, + "y": 3.379364027011461, + "heading": -0.7610258706649285, + "angularVelocity": -0.08793380754076002, + "velocityX": -0.7129519382079801, + "velocityY": 0.9543285162988591, "moduleForcesX": [ - 47.963765452152025, - 48.213334157004304, - 43.74592423575202, - 44.32418492699512 + 46.96552536511235, + 47.136382750106904, + 44.47147307724585, + 44.76001886720274 ], "moduleForcesY": [ - -50.909740237006666, - -50.68021953336166, - -54.577452261696784, - -54.11506726002671 + -51.83392911935416, + -51.682542993841096, + -53.98932327657365, + -53.75408240792354 ], - "timestamp": 5.804621404898353 + "timestamp": 5.800116927270059 }, { - "x": 2.162163747962268, - "y": 3.42862257199927, - "heading": -0.7579610345845447, - "angularVelocity": -0.14326029020817274, - "velocityX": -0.5718149242231771, - "velocityY": 0.7629167071696146, + "x": 2.16107481240881, + "y": 3.4279560426439364, + "heading": -0.7653786869846049, + "angularVelocity": -0.06896494150222135, + "velocityX": -0.5620442589466436, + "velocityY": 0.7698798362844577, "moduleForcesX": [ - 46.6319076312275, - 46.9611493566907, - 42.180140742346175, - 42.868308982974696 + 45.36776130374439, + 45.59551423238422, + 42.91858789949486, + 43.2583272033556 ], "moduleForcesY": [ - -52.139192200259096, - -51.849151349231114, - -55.802822449668646, - -55.2816673356197 + -53.2445780347107, + -53.0530790830467, + -55.238217704660805, + -54.97579973773786 ], - "timestamp": 5.867792014068692 + "timestamp": 5.863233290814515 }, { - "x": 2.1353988201191023, - "y": 3.4650167267118044, - "heading": -0.7648020671777167, - "angularVelocity": -0.1082945484145205, - "velocityX": -0.4236927298103942, - "velocityY": 0.5761248021908162, + "x": 2.134832009400895, + "y": 3.4646708607022925, + "heading": -0.768580123623318, + "angularVelocity": -0.050722767582422834, + "velocityX": -0.4157844580103423, + "velocityY": 0.5817004655614504, "moduleForcesX": [ - 45.45179812579232, - 45.85909611829803, - 40.80962595285606, - 41.60067510940896 + 43.954428645312426, + 44.2297096749135, + 41.559591533194514, + 41.940634703058834 ], "moduleForcesY": [ - -53.17683717820275, - -52.83187682056265, - -56.8182410705635, - -56.24698678733785 + -54.42275064440647, + -54.20223828129335, + -56.273239100999206, + -55.992648881357695 ], - "timestamp": 5.93096262323903 + "timestamp": 5.926349654358971 }, { - "x": 2.1177566362723623, - "y": 3.4894287132169657, - "heading": -0.7693928270163053, - "angularVelocity": -0.07267240096117586, - "velocityX": -0.27927835552714286, - "velocityY": 0.3864453236367305, + "x": 2.1175606055664447, + "y": 3.489309593661584, + "heading": -0.770673719921477, + "angularVelocity": -0.03317042016662467, + "velocityX": -0.27364383599643854, + "velocityY": 0.39036997025244236, "moduleForcesX": [ - 44.397979595664026, - 44.88323764291386, - 39.60107624297137, - 40.49028312227595 + 42.69713344336191, + 43.01233196517679, + 40.36254825642123, + 40.77712883283335 ], "moduleForcesY": [ - -54.064737546429875, - -53.66807598097, - -57.67176826261024, - -57.055974307841915 + -55.41943823043682, + -55.177780690773446, + -57.14258045212568, + -56.849966899553834 ], - "timestamp": 5.994133232409369 + "timestamp": 5.9894660179034265 }, { "x": 2.1090288162231445, "y": 3.501702308654785, "heading": -0.771700836029523, - "angularVelocity": -0.036536120888024486, - "velocityX": -0.1381626703279573, - "velocityY": 0.19429281431692114, + "angularVelocity": -0.016273372709797004, + "velocityX": -0.13517555296560768, + "velocityY": 0.19634710077161807, "moduleForcesX": [ - 43.44956308739193, - 44.01425554364353, - 38.52755610227539, - 39.51242168602051 + 41.5722477087897, + 41.92121432741254, + 39.30147273432098, + 39.743500474089345 ], "moduleForcesY": [ - -54.834133333236075, - -54.387070471211764, - -58.398462389422065, - -57.74131858284474 + -56.272332012188514, + -56.01516696263238, + -57.881532814470596, + -57.581128152106956 ], - "timestamp": 6.057303841579707 + "timestamp": 6.052582381447882 }, { "x": 2.1090288162231445, "y": 3.501702308654785, "heading": -0.771700836029523, - "angularVelocity": 1.2297405786038555e-20, - "velocityX": -1.8885005519347326e-21, - "velocityY": -1.1623354869164822e-22, + "angularVelocity": 7.034369509774633e-21, + "velocityX": 1.0745130979383846e-20, + "velocityY": 7.792911077792643e-21, "moduleForcesX": [ - 42.58937177633389, - 43.23643664704943, - 37.56731333436976, - 38.647309328509984 + 40.56018810627996, + 40.937946455779034, + 38.35525709358692, + 38.819933249113355 ], "moduleForcesY": [ - -55.508635741315956, - -55.01097400948289, - -59.024322031425896, - -58.327253062024916 + -57.00975798757109, + -56.74117440782203, + -58.516369460899966, + -58.211114035733786 ], - "timestamp": 6.120474450750046 + "timestamp": 6.115698744992338 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/source 3.traj b/src/main/deploy/choreo/source 3.traj index dae88667..0efa1f5d 100644 --- a/src/main/deploy/choreo/source 3.traj +++ b/src/main/deploy/choreo/source 3.traj @@ -4,9 +4,9 @@ "x": 0.7778744697570801, "y": 4.333664894104004, "heading": -1.051650156247713, - "angularVelocity": -3.718899994914549e-23, - "velocityX": 1.067537172973605e-21, - "velocityY": 5.775370181920206e-22, + "angularVelocity": -1.3297634990832717e-18, + "velocityX": 8.096904896283762e-18, + "velocityY": -2.035450603469087e-18, "moduleForcesX": [ 0, 0, @@ -22,4099 +22,4099 @@ "timestamp": 0 }, { - "x": 0.7922205153045683, - "y": 4.322569747233587, - "heading": -1.038589316498916, - "angularVelocity": 0.18665078221022835, - "velocityX": 0.2050174930986984, - "velocityY": -0.15855931792528452, + "x": 0.7908719793717586, + "y": 4.323476837419605, + "heading": -1.0413708097378205, + "angularVelocity": 0.15431550267407224, + "velocityX": 0.19512108359903374, + "velocityY": -0.15294504247054264, "moduleForcesX": [ - 64.99353342135232, - 58.994889041922896, - 48.6051352797362, - 44.47522386087147 + 63.74590962361433, + 58.38725396448112, + 49.37860903101583, + 45.506342584710694 ], "moduleForcesY": [ - -25.906794419809312, - -37.63755378362518, - -50.312927149408814, - -54.02243790369699 + -28.834822023862102, + -38.568182065872634, + -49.55287419418438, + -53.15308295520182 ], - "timestamp": 0.06997473889011878 + "timestamp": 0.06661253297151565 }, { - "x": 0.8209322601683935, - "y": 4.300376239957637, - "heading": -1.0127877530474698, - "angularVelocity": 0.3687268271477519, - "velocityX": 0.4103158556820229, - "velocityY": -0.3171645600678816, + "x": 0.8168891188149916, + "y": 4.303112092011227, + "heading": -1.0210382023309228, + "angularVelocity": 0.3052369651757899, + "velocityX": 0.3905742400511663, + "velocityY": -0.30571942695958465, "moduleForcesX": [ - 64.75012154981113, - 59.119805707703335, - 48.51394542600652, - 44.98228840684379 + 63.57502695706841, + 58.50337248946953, + 49.388651702378716, + 45.92040304941029 ], "moduleForcesY": [ - -26.49930496933997, - -37.43568863955079, - -50.39606166150177, - -53.59728170965293 + -29.201580891943905, + -38.38697260898019, + -49.53833328341483, + -52.7922647589104 ], - "timestamp": 0.13994947778023756 + "timestamp": 0.1332250659430313 }, { - "x": 0.8640312929701923, - "y": 4.267081252649147, - "heading": -0.9746011298056743, - "angularVelocity": 0.5457201248261894, - "velocityX": 0.6159227384824854, - "velocityY": -0.47581438439909957, + "x": 0.8559503614500349, + "y": 4.272583897748778, + "heading": -0.9908994985401418, + "angularVelocity": 0.4524479470502735, + "velocityX": 0.5863947952819387, + "velocityY": -0.45829505200625403, "moduleForcesX": [ - 64.44611808173948, - 59.310850143621366, - 48.32509669282183, - 45.610751749821475 + 63.36650355926644, + 58.673994480374716, + 49.332465125535784, + 46.42312032557307 ], "moduleForcesY": [ - -27.21858213973262, - -37.12584717532736, - -50.57187006691617, - -53.05924040607812 + -29.64202317080124, + -38.12000070194584, + -49.58929691252429, + -52.34676597725229 ], - "timestamp": 0.20992421667035632 + "timestamp": 0.19983759891454694 }, { - "x": 0.9215421276125146, - "y": 4.222681267524096, - "heading": -0.9244509530719824, - "angularVelocity": 0.7166897301673768, - "velocityX": 0.8218799463136471, - "velocityY": -0.6345144809296546, + "x": 0.9080832434863456, + "y": 4.231907621482131, + "heading": -0.9512364839325822, + "angularVelocity": 0.5954287104578356, + "velocityX": 0.7826287293204035, + "velocityY": -0.6106399869832296, "moduleForcesX": [ - 64.05688808437033, - 59.54566542911579, - 48.099567813510134, - 46.36161309260976 + 63.1087538854026, + 58.88683733974907, + 49.24358469003159, + 47.01667690510743 ], "moduleForcesY": [ - -28.108700702243887, - -36.74020593171785, - -50.78046522808163, - -52.39939521356428 + -30.17612796676512, + -37.78365700090767, + -49.672009408886844, + -51.809713634059214 ], - "timestamp": 0.2798989555604751 + "timestamp": 0.2664501318860626 }, { - "x": 0.9934931064812429, - "y": 4.167171543523546, - "heading": -0.8628639745372912, - "angularVelocity": 0.880131594794532, - "velocityX": 1.0282421915387603, - "velocityY": -0.7932823313241175, + "x": 0.9733191777952942, + "y": 4.181101065602064, + "heading": -0.9023828510876672, + "angularVelocity": 0.7334000174682638, + "velocityX": 0.9793342393516886, + "velocityY": -0.7627176690877809, "moduleForcesX": [ - 63.55085157875546, - 59.79482586920611, - 47.91149012286905, - 47.23541305248315 + 62.78758091347448, + 59.12644232420397, + 49.162542801992586, + 47.70378434486226 ], "moduleForcesY": [ - -29.21888766916689, - -36.32352543051382, - -50.95113504071442, - -51.60695549029941 + -30.82641384591526, + -37.399521862180414, + -49.74596927329427, + -51.172358294771385 ], - "timestamp": 0.3498736944505939 + "timestamp": 0.33306266485757824 }, { - "x": 1.0799170133820089, - "y": 4.100544943426046, - "heading": -0.7905211436285549, - "angularVelocity": 1.0338420986798709, - "velocityX": 1.2350729459166292, - "velocityY": -0.9521521788330469, + "x": 1.051694367518301, + "y": 4.120184874713526, + "heading": -0.8447458342032448, + "angularVelocity": 0.8652578473343552, + "velocityX": 1.1765832378198438, + "velocityY": -0.9144854304607654, "moduleForcesX": [ - 62.890212743310435, - 60.01889414692214, - 47.84890703697811, - 48.23061564013138 + 62.38681753882747, + 59.37326562455472, + 49.13769712317514, + 48.48705032206493 ], "moduleForcesY": [ - -30.59654996800034, - -35.939691766596795, - -51.0018835950638, - -50.67037098511977 + -31.615322936234296, + -36.99658263811015, + -49.763347170131574, + -50.42430963220079 ], - "timestamp": 0.4198484333407127 + "timestamp": 0.3996751978290939 }, { - "x": 1.1808511971073097, - "y": 4.022790644493082, - "heading": -0.708315499162293, - "angularVelocity": 1.174790299615828, - "velocityX": 1.4424374470878363, - "velocityY": -1.1111766927070885, + "x": 1.1432508333296558, + "y": 4.049183171168427, + "heading": -0.778832566995066, + "angularVelocity": 0.9895024895144598, + "velocityX": 1.3744630586335072, + "velocityY": -1.065891062504106, "moduleForcesX": [ - 62.034245895880275, - 60.16310541057914, - 48.01415268705809, - 49.342246941586 + 61.889733856839435, + 59.602244362756366, + 49.22597947357009, + 49.36848790198351 ], "moduleForcesY": [ - -32.275569212980486, - -35.68171414287474, - -50.836548780375075, - -49.57842210438686 + -32.56119938527781, + -36.61435191109544, + -49.66762908534543, + -49.55361350286737 ], - "timestamp": 0.4898231722308315 + "timestamp": 0.46628773080060953 }, { - "x": 1.2963371243384598, - "y": 3.933893322355774, - "heading": -0.6174207883006573, - "angularVelocity": 1.298964630712914, - "velocityX": 1.650394543272217, - "velocityY": -1.2704202051672215, + "x": 1.2480376237140218, + "y": 3.968124696368845, + "heading": -0.7052817755202949, + "angularVelocity": 1.1041584547794074, + "velocityX": 1.57307920461716, + "velocityY": -1.2168652231591865, "moduleForcesX": [ - 60.94746004979524, - 60.146829265545456, - 48.525569264290525, - 50.56132114483972 + 61.28176481668813, + 59.78038532288985, + 49.4938549994705, + 50.34939808767109 ], "moduleForcesY": [ - -34.25932977118567, - -35.687927503268604, - -50.336093067995485, - -48.32077560597764 + -33.67250970561668, + -36.30739888304486, + -49.39059309707039, + -48.546399889287585 ], - "timestamp": 0.5597979111209502 + "timestamp": 0.5329002637721252 }, { - "x": 1.4264194135128296, - "y": 3.833833792551993, - "heading": -0.5193851332120467, - "angularVelocity": 1.4010149468732676, - "velocityX": 1.8589892758104873, - "velocityY": -1.429937880309981, + "x": 1.3661123895110021, + "y": 3.877044967671322, + "heading": -0.6249031875065618, + "angularVelocity": 1.206658633565307, + "velocityX": 1.7725608159574173, + "velocityY": -1.3673061905102768, "moduleForcesX": [ - 59.614996756079215, - 59.84049412454702, - 49.524716335836736, - 51.876090247246715 + 60.55523039747262, + 59.86237923234887, + 50.01933061446567, + 51.43105327265715 ], "moduleForcesY": [ - -36.50078923453568, - -36.17056672706365, - -49.33646413057858, - -46.88658430312503 + -34.9402966200205, + -36.15231999767806, + -48.845768467491496, + -47.385486747890475 ], - "timestamp": 0.629772650011069 + "timestamp": 0.5995127967436409 }, { - "x": 1.5711438836172125, - "y": 3.722592592486754, - "heading": -0.416299104221778, - "angularVelocity": 1.4731891912043358, - "velocityX": 2.0682388016001525, - "velocityY": -1.589733692896242, + "x": 1.4975436678185934, + "y": 3.7759903450547028, + "heading": -0.53873230694548, + "angularVelocity": 1.293613629704331, + "velocityX": 1.9730713192334703, + "velocityY": -1.5170511930514805, "moduleForcesX": [ - 58.065083972008736, - 59.00957277137866, - 51.197174245280834, - 53.27774962436911 + 59.71660813639446, + 59.781944702186415, + 50.89674120838264, + 52.617056525494455 ], "moduleForcesY": [ - -38.886690128517934, - -37.47125653206859, - -47.574170293153024, - -45.2567742927428 + -36.32951289772708, + -36.25934407553948, + -47.91447235046562, + -46.0464745895729 ], - "timestamp": 0.6997473889011877 + "timestamp": 0.6661253297151566 }, { - "x": 1.7305504220603747, - "y": 3.6001600451974833, - "heading": -0.3111713227790686, - "angularVelocity": 1.502367613086651, - "velocityX": 2.2780583532219847, - "velocityY": -1.749667797710917, + "x": 1.6424143533198352, + "y": 3.665025662326569, + "heading": -0.4481205140243463, + "angularVelocity": 1.3602814497368003, + "velocityX": 2.174826253239616, + "velocityY": -1.665822898156168, "moduleForcesX": [ - 56.39097412755216, - 57.161179416740495, - 53.820924588217835, - 54.780035432769175 + 58.79631756896231, + 59.433248892591386, + 52.247409534940125, + 53.9194587893842 ], "moduleForcesY": [ - -41.23803886906891, - -40.17856661949012, - -44.545308010292885, - -43.37339903624029 + -37.77070605252747, + -36.79353882986422, + -46.41555660113537, + -44.48747884319979 ], - "timestamp": 0.7697221277913064 + "timestamp": 0.7327378626866723 }, { - "x": 1.904638990279975, - "y": 3.4665697709891212, - "heading": -0.20885800973766258, - "angularVelocity": 1.462146406892186, - "velocityX": 2.4878773537543495, - "velocityY": -1.909121439068728, + "x": 1.800826604673265, + "y": 3.5442490372900517, + "heading": -0.3549074004558732, + "angularVelocity": 1.3993329694928751, + "velocityX": 2.3781148124358094, + "velocityY": -1.8131216401598682, "moduleForcesX": [ - 54.74964870144604, - 53.05199961529088, - 57.85147404335248, - 56.499407721715734 + 57.85964296595883, + 58.62699304555619, + 54.24098287937368, + 55.37454877680063 ], "moduleForcesY": [ - -43.34419920923391, - -45.385040535603636, - -39.0969722773908, - -41.00039456972551 + -39.15408809177324, + -38.01758274488999, + -44.03657792902701, + -42.620768436938704 ], - "timestamp": 0.8396968666814252 + "timestamp": 0.799350395658188 }, { - "x": 2.09320393495465, - "y": 3.3220757989813476, - "heading": -0.11854546488401964, - "angularVelocity": 1.2906449711153707, - "velocityX": 2.694757389102635, - "velocityY": -2.064944782926198, + "x": 1.9729064746231015, + "y": 3.4138237900863677, + "heading": -0.261785676023239, + "angularVelocity": 1.3979610184235784, + "velocityX": 2.58329569937567, + "velocityY": -1.9579685891760825, "moduleForcesX": [ - 53.2728143917394, - 42.775288784328325, - 63.8442089345851, - 59.14849524914464 + 57.01503970270268, + 56.97264868755776, + 57.13055506988329, + 57.088614242525495 ], "moduleForcesY": [ - -45.07889412240785, - -55.05640389507177, - -28.09637912144248, - -36.751236686389184 + -40.32815265808299, + -40.3871603414814, + -40.16401782504414, + -40.22274635092099 ], - "timestamp": 0.9096716055715439 + "timestamp": 0.8659629286297037 }, { - "x": 2.295164186316796, - "y": 3.1696331321710134, - "heading": -0.06408923152074174, - "angularVelocity": 0.778227032026378, - "velocityX": 2.886187995346207, - "velocityY": -2.178538558746386, + "x": 2.1587959216901558, + "y": 3.2740626515960307, + "heading": -0.1731498612166921, + "angularVelocity": 1.3306176908848175, + "velocityX": 2.7906076945992013, + "velocityY": -2.0981207628015075, "moduleForcesX": [ - 51.59206019363968, - 13.364464020811422, - 69.52592713911473, - 68.20078231069633 + 56.41042369707265, + 53.501719202644416, + 61.26419264721754, + 59.40078934825349 ], "moduleForcesY": [ - -46.885084851896735, - -68.25325997043356, - -3.8069250749386283, - -1.3257476748867494 + -41.1079010332853, + -44.78370054636255, + -33.42235417217702, + -36.56648525897984 ], - "timestamp": 0.9796463444616627 + "timestamp": 0.9325754616012194 }, { - "x": 2.5102036576315885, - "y": 3.0099910805069405, - "heading": -0.048739655045187506, - "angularVelocity": 0.21935882461323145, - "velocityX": 3.0731014466873368, - "velocityY": -2.281424042392765, + "x": 2.3585571947909116, + "y": 3.1257497783866914, + "heading": -0.09743941845228067, + "angularVelocity": 1.1365795502294775, + "velocityX": 2.9988541823830035, + "velocityY": -2.2265010290594955, "moduleForcesX": [ - 51.68312483431202, - 11.490644239292044, - 69.23100690631284, - 65.49577647910316 + 56.18321511797377, + 45.13645782831661, + 66.6575745567656, + 63.63924131102192 ], "moduleForcesY": [ - -46.34143932256032, - -68.12951985238924, - -1.4483890023414814, - 6.986075009746601 + -41.32675572742186, + -53.045678977052305, + -20.39623436243562, + -28.01878833525786 ], - "timestamp": 1.0496210833517814 + "timestamp": 0.9991879945727351 }, { - "x": 2.7349332678515874, - "y": 2.854041862643359, - "heading": -0.04856305611607661, - "angularVelocity": 0.002523752598608622, - "velocityX": 3.2115819763599447, - "velocityY": -2.228650229170103, + "x": 2.571323749679796, + "y": 2.9722890611298696, + "heading": -0.05631702040111217, + "angularVelocity": 0.6173372519665775, + "velocityX": 3.1940919433264323, + "velocityY": -2.303781441886374, "moduleForcesX": [ - 44.06076488703463, - 32.04578879681631, - 41.59848030725396, - 28.915624556417924 + 56.49315652197762, + 24.727674210970267, + 69.53856728119115, + 66.38848839184168 ], "moduleForcesY": [ - 2.892829937081654, - 5.007695054030006, - 21.07556807654082, - 26.89985763143924 + -40.73364522523241, + -64.83876276810156, + 2.4259673258224868, + 17.19340461150298 ], - "timestamp": 1.1195958222419002 + "timestamp": 1.0658005275442508 }, { - "x": 2.9682298777966594, - "y": 2.7112552443984246, - "heading": -0.048304755129702115, - "angularVelocity": 0.0036913461982344513, - "velocityX": 3.3340118683603395, - "velocityY": -2.040545209738506, + "x": 2.7978969801578932, + "y": 2.816123256954651, + "heading": -0.04923559160029977, + "angularVelocity": 0.1063077544857887, + "velocityX": 3.4013603802603107, + "velocityY": -2.3443907206170564, "moduleForcesX": [ - 32.3225255016905, - 32.43770393632479, - 32.3755224257375, - 32.49078432468219 + 61.70982060481134, + 40.01285570538606, + 68.37167087206099, + 60.43433119111175 ], "moduleForcesY": [ - 49.850764221083146, - 49.79054196126857, - 49.79062122677659, - 49.730235606563355 + -31.4520746032983, + -55.70693219082933, + 10.727828044445207, + 31.264613623300775 ], - "timestamp": 1.189570561132019 + "timestamp": 1.1324130605157665 }, { - "x": 3.206226948407469, - "y": 2.57644940649246, - "heading": -0.04804302634103678, - "angularVelocity": 0.0037403324802159203, - "velocityX": 3.4011855476093436, - "velocityY": -1.9264929036412723, + "x": 3.0347601843563274, + "y": 2.6710905971518515, + "heading": -0.04900748324484257, + "angularVelocity": 0.003424405968089362, + "velocityX": 3.55583542063653, + "velocityY": -2.1772578422264344, "moduleForcesX": [ - 17.778201901232073, - 17.782317553134607, - 17.77881545073154, - 17.782931249096187 + 50.41350702863919, + 41.75482316229609, + 44.362342200824024, + 35.27998738074212 ], "moduleForcesY": [ - 30.19121333723645, - 30.19026343520494, - 30.187987107924002, - 30.187037184259207 + 38.99730957905846, + 46.617569740155716, + 46.965805915927774, + 53.3083103983914 ], - "timestamp": 1.259545300022138 + "timestamp": 1.1990255934872822 }, { - "x": 3.4454066255541647, - "y": 2.443753454780269, - "heading": -0.04778008796546239, - "angularVelocity": 0.003757618531271475, - "velocityX": 3.418086025619602, - "velocityY": -1.896340791218425, + "x": 3.2779712543514585, + "y": 2.5369613930825095, + "heading": -0.04879559855232118, + "angularVelocity": 0.0031808532579297136, + "velocityX": 3.6511307879424364, + "velocityY": -2.0135730933201033, "moduleForcesX": [ - 4.472855661174314, - 4.474147280296156, - 4.472811619653854, - 4.474103244435602 + 26.514032908634963, + 26.48917929707793, + 26.505657516129617, + 26.480809194623138 ], "moduleForcesY": [ - 7.9818138726911165, - 7.981734606875763, - 7.980518679466535, - 7.980439415924407 + 45.5016606384836, + 45.51171619004381, + 45.5152401681101, + 45.525290455269825 ], - "timestamp": 1.3295200389122568 + "timestamp": 1.265638126458798 }, { - "x": 3.68491109766373, - "y": 2.3116446526478165, - "heading": -0.04751696829794281, - "angularVelocity": 0.003760209351159636, - "velocityX": 3.4227276286898194, - "velocityY": -1.8879499120375856, + "x": 3.5240170136884648, + "y": 2.4081056080213394, + "heading": -0.04858282682880418, + "angularVelocity": 0.003194169535753657, + "velocityX": 3.6936856828761946, + "velocityY": -1.93440752532666, "moduleForcesX": [ - 1.2285197916199417, - 1.228711642997039, - 1.2285106844862757, - 1.228702535899101 + 11.832044679638638, + 11.833159737293062, + 11.83210215812816, + 11.833217225479276 ], "moduleForcesY": [ - 2.2211300227139463, - 2.2211206994127397, - 2.22093420243456, - 2.220924879148784 + 22.01300562318127, + 22.0128485655156, + 22.01202572778171, + 22.011868671306686 ], - "timestamp": 1.3994947778023756 + "timestamp": 1.3322506594303136 }, { - "x": 3.924585147927277, - "y": 2.1798436167917243, - "heading": -0.047254057832202365, - "angularVelocity": 0.0037572196754216245, - "velocityX": 3.425151048293395, - "velocityY": -1.883551663737667, + "x": 3.7708350348315482, + "y": 2.2807352279621504, + "heading": -0.0483699449005901, + "angularVelocity": 0.0031958239496029865, + "velocityX": 3.7052790238231275, + "velocityY": -1.9121083432400703, "moduleForcesX": [ - 0.6415734362803132, - 0.6413521699115629, - 0.6415840632535661, - 0.6413627969097583 + 3.2235324585038754, + 3.2236618531632817, + 3.2235271302506407, + 3.2236565249546465 ], "moduleForcesY": [ - 1.164077957656933, - 1.1640884844019281, - 1.1643040474316328, - 1.1643145741874217 + 6.200483556552904, + 6.200476317722027, + 6.200352895038578, + 6.200345656224632 ], - "timestamp": 1.4694695166924945 + "timestamp": 1.3988631924018293 }, { - "x": 4.164406438877476, - "y": 2.0483104663276324, - "heading": -0.04699153890493271, - "angularVelocity": 0.0037516242494578765, - "velocityX": 3.427255246022282, - "velocityY": -1.8797233480304725, + "x": 4.017945860044819, + "y": 2.153933675847828, + "heading": -0.04815735530566025, + "angularVelocity": 0.0031914353868025967, + "velocityX": 3.709674654151622, + "velocityY": -1.90356899006594, "moduleForcesX": [ - 0.5571686890446169, - 0.556754586714119, - 0.5571884977126166, - 0.556774395458144 + 1.2223925594156837, + 1.2220511392203353, + 1.2224089657777908, + 1.222067545703039 ], "moduleForcesY": [ - 1.0131163405590924, - 1.013135903625798, - 1.013539512185854, - 1.0135590752854269 + 2.374233190641205, + 2.3742500204088812, + 2.3745815684863776, + 2.374598398301541 ], - "timestamp": 1.5394442555826133 + "timestamp": 1.465475725373345 }, { - "x": 4.404389812405626, - "y": 1.917073025680941, - "heading": -0.046729445162668845, - "angularVelocity": 0.0037455479851869126, - "velocityX": 3.4295715473121766, - "velocityY": -1.8754973970359978, + "x": 4.265335286953013, + "y": 2.027676292006606, + "heading": -0.04794522174606807, + "angularVelocity": 0.0031845892976762468, + "velocityX": 3.7138570757244884, + "velocityY": -1.895399832569214, "moduleForcesX": [ - 0.6133285579559523, - 0.6128788486139368, - 0.6133499263194478, - 0.6129002170763577 + 1.163199663544347, + 1.1626670888284039, + 1.1632251975919956, + 1.1626926239772695 ], "moduleForcesY": [ - 1.1183496176451213, - 1.1183707656322825, - 1.1188091442741797, - 1.1188302923040183 + 2.271195979291761, + 2.271222065843487, + 2.271739492202394, + 2.2717655728058856 ], - "timestamp": 1.6094189944727322 + "timestamp": 1.5320882583448607 }, { - "x": 4.6445839308598735, - "y": 1.7862215631603924, - "heading": -0.04646750224020687, - "angularVelocity": 0.0037433926387822392, - "velocityX": 3.432583275965109, - "velocityY": -1.8699814332429918, + "x": 4.513387263491877, + "y": 1.9027254137370513, + "heading": -0.04773334654928872, + "angularVelocity": 0.003180710706046184, + "velocityX": 3.7238033966511064, + "velocityY": -1.8757863227177534, "moduleForcesX": [ - 0.7972668572924495, - 0.7971073142412101, - 0.7972743634616845, - 0.7971148204266829 + 2.765776223844279, + 2.765473269227589, + 2.7657891163065877, + 2.7654861619049216 ], "moduleForcesY": [ - 1.4599652235465161, - 1.4599727135025415, - 1.4601282052544333, - 1.4601356952174342 + 5.453486682132146, + 5.453502875542226, + 5.453793409345764, + 5.453809602835306 ], - "timestamp": 1.679393733362851 + "timestamp": 1.5987007913163764 }, { - "x": 4.885064829086469, - "y": 1.6558983523007769, - "heading": -0.04620453225018068, - "angularVelocity": 0.003758070329338863, - "velocityX": 3.4366816088334584, - "velocityY": -1.8624322566499572, + "x": 4.7636905451889415, + "y": 1.7823482264471835, + "heading": -0.04752024111954168, + "angularVelocity": 0.003199179197076531, + "velocityX": 3.7576004173883883, + "velocityY": -1.8071252048220714, "moduleForcesX": [ - 1.0842918084117592, - 1.0853786132925534, - 1.084241424204242, - 1.085328230114811 + 9.396675512144679, + 9.39819901784068, + 9.396709254967268, + 9.398232777338956 ], "moduleForcesY": [ - 1.9988129858279566, - 1.9987618336162647, - 1.997703395532972, - 1.9976522437601878 + 19.092390606150673, + 19.092220862164694, + 19.091009053610517, + 19.090839312476135 ], - "timestamp": 1.7493684722529699 + "timestamp": 1.6653133242878921 }, { - "x": 5.125797171042743, - "y": 1.5260410914198634, - "heading": -0.04593987286096788, - "angularVelocity": 0.0037822133159851846, - "velocityX": 3.4402749588575916, - "velocityY": -1.8557734253903861, + "x": 5.0188400094891765, + "y": 1.6726490442267128, + "heading": -0.04723633820300304, + "angularVelocity": 0.004262004518879671, + "velocityX": 3.8303522313036735, + "velocityY": -1.646824964115687, "moduleForcesX": [ - 0.9502910191372389, - 0.9520784538822461, - 0.95020827131857, - 0.9519957085202748 + 20.160339598017387, + 20.271778827791216, + 20.186266828439518, + 20.297839903273452 ], "moduleForcesY": [ - 1.7635166253693095, - 1.7634333067623955, - 1.7616912237363618, - 1.7616079061721086 + 44.61714719861703, + 44.58388431235552, + 44.56094399930638, + 44.52760480062931 ], - "timestamp": 1.8193432111430887 + "timestamp": 1.7319258572594078 }, { - "x": 5.366788045768459, - "y": 1.396664893828562, - "heading": -0.04567411476495212, - "angularVelocity": 0.0037979147936955102, - "velocityX": 3.443969617437849, - "velocityY": -1.8488986117470256, + "x": 5.276040962243286, + "y": 1.5678524061254324, + "heading": -0.046947349952411, + "angularVelocity": 0.0043383465197998905, + "velocityX": 3.8611495657144865, + "velocityY": -1.5732270404143434, "moduleForcesX": [ - 0.977404742123205, - 0.9785672603177287, - 0.9773512911124315, - 0.978513810379268 + 8.560118396401565, + 8.566501267033127, + 8.560247116014803, + 8.56663029963911 ], "moduleForcesY": [ - 1.8203523461919868, - 1.820298421093803, - 1.8191652136181402, - 1.8191112889734293 + 20.46747803904423, + 20.466793414894244, + 20.461850439324866, + 20.461165848040086 ], - "timestamp": 1.8893179500332076 + "timestamp": 1.7985383902309235 }, { - "x": 5.608450195426062, - "y": 1.2685474906510974, - "heading": -0.045407399320727376, - "angularVelocity": 0.003811596133907239, - "velocityX": 3.453562721214088, - "velocityY": -1.830909342564999, + "x": 5.534216899574317, + "y": 1.4654824117182799, + "heading": -0.04665631309500156, + "angularVelocity": 0.004369100594536302, + "velocityX": 3.8757862194105366, + "velocityY": -1.536797804264961, "moduleForcesX": [ - 2.5387638123361675, - 2.539779903515735, - 2.538721732122775, - 2.5397378254255 + 4.068624748426005, + 4.071060153628479, + 4.068549709495257, + 4.070985139742783 ], "moduleForcesY": [ - 4.762218612064458, - 4.762167761549727, - 4.761187255842194, - 4.761136406193755 + 10.13063153345665, + 10.130481600946313, + 10.128231656616295, + 10.128081730271346 ], - "timestamp": 1.9592926889233264 + "timestamp": 1.8651509232024392 }, { - "x": 5.852933619648411, - "y": 1.1459010453590361, - "heading": -0.04513772147587357, - "angularVelocity": 0.0038539314205556937, - "velocityX": 3.493881193415528, - "velocityY": -1.7527245865776317, + "x": 5.794372355867246, + "y": 1.3682547977858872, + "heading": -0.04636219278537718, + "angularVelocity": 0.004415389964980432, + "velocityX": 3.905503134135057, + "velocityY": -1.459599411629766, "moduleForcesX": [ - 10.670372351980113, - 10.673724191649343, - 10.670507188995435, - 10.673859113242672 + 8.260963536224425, + 8.264869277780319, + 8.261045504013175, + 8.264951367768367 ], "moduleForcesY": [ - 20.696850436257613, - 20.696435019435945, - 20.693861640190523, - 20.69344623623891 + 21.467349064567532, + 21.466934299940117, + 21.46397136076802, + 21.463556605012805 ], - "timestamp": 2.029267427813445 + "timestamp": 1.931763456173955 }, { - "x": 6.1035014115315445, - "y": 1.0362251026771625, - "heading": -0.04485903801696482, - "angularVelocity": 0.003982629493571362, - "velocityX": 3.580832109664594, - "velocityY": -1.5673648008047245, + "x": 6.058520814972377, + "y": 1.282472529523383, + "heading": -0.04606005529713513, + "angularVelocity": 0.004535745373490557, + "velocityX": 3.965446850941467, + "velocityY": -1.2877797080496214, "moduleForcesX": [ - 23.006805416844802, - 23.020298874307237, - 23.01072211535244, - 23.02421749157213 + 16.659711714491884, + 16.673161829473102, + 16.66222092531276, + 16.675673239510985 ], "moduleForcesY": [ - 49.06916713204879, - 49.064472309009574, - 49.06330827580301, - 49.05861198170382 + 47.77989163980646, + 47.77656557923747, + 47.77432962004692, + 47.771002564051436 ], - "timestamp": 2.0992421667035637 + "timestamp": 1.9983759891454707 }, { - "x": 6.360440389666232, - "y": 0.9424571330795427, - "heading": -0.04456198298429893, - "angularVelocity": 0.004245175292934805, - "velocityX": 3.6718819135310827, - "velocityY": -1.3400260020242942, + "x": 6.326935018479887, + "y": 1.2111884138637838, + "heading": -0.04572387766988923, + "angularVelocity": 0.0050467624071529864, + "velocityX": 4.0294850163149665, + "velocityY": -1.0701306868195675, "moduleForcesX": [ - 24.07899931263681, - 24.111464518434406, - 24.089464997361308, - 24.121942503866567 + 17.76434909368371, + 17.833039573651952, + 17.77928266449683, + 17.84803536932326 ], "moduleForcesY": [ - 60.18510366880944, - 60.17310602326288, - 60.17798562768333, - 60.165977537276476 + 60.533843276425394, + 60.51552676552759, + 60.52159010023538, + 60.50324061772363 ], - "timestamp": 2.1692169055936823 + "timestamp": 2.064988522116986 }, { - "x": 6.622860129273645, - "y": 0.8657147607838669, - "heading": -0.04392950732433488, - "angularVelocity": 0.009038628367834598, - "velocityX": 3.7502067713248666, - "velocityY": -1.096715379191114, + "x": 6.598149001232439, + "y": 1.1556493567666084, + "heading": -0.04255962682130831, + "angularVelocity": 0.04750233488244815, + "velocityX": 4.071515834242889, + "velocityY": -0.8337628764383538, "moduleForcesX": [ - 20.32544719556, - 20.966458363180468, - 20.495346686232793, - 21.14167832393734 + 8.287572421549362, + 14.286479207247982, + 8.808436728764809, + 15.365145055742365 ], "moduleForcesY": [ - 64.54278261295427, - 64.34395175999101, - 64.46421735586846, - 64.26189706479809 + 66.4507233464468, + 65.45898479609045, + 66.07372493186959, + 64.91023150582662 ], - "timestamp": 2.239191644483801 + "timestamp": 2.1316010550885016 }, { - "x": 6.884679748309282, - "y": 0.806794359236595, - "heading": -0.030602705565700657, - "angularVelocity": 0.1904516111101361, - "velocityX": 3.7416305253638975, - "velocityY": -0.8420238857881919, + "x": 6.859559783326081, + "y": 1.113135958184906, + "heading": -0.030602705565701032, + "angularVelocity": 0.17949957346195378, + "velocityX": 3.9243483235418233, + "velocityY": -0.6382192161928673, "moduleForcesX": [ - -12.193365917081998, - 9.151089894849695, - -18.363615689541977, - 12.325518942868378 + -42.45988336627168, + -29.346972124084434, + -52.177609628795565, + -39.698593640879075 ], "moduleForcesY": [ - 67.8990355400377, - 68.35833703061081, - 66.03471322560178, - 67.3706211378861 + 54.43758266703957, + 62.32119709611513, + 44.88720682067673, + 55.84212799694645 ], - "timestamp": 2.3091663833739196 + "timestamp": 2.198213588060017 }, { - "x": 7.134490489959717, - "y": 0.7612675428390503, - "heading": -0.03060270556570066, - "angularVelocity": 1.4169381433025497e-21, - "velocityX": 3.570013202088712, - "velocityY": -0.6506178818192586, + "x": 7.107321739196777, + "y": 1.0795719623565674, + "heading": -0.030602705565701018, + "angularVelocity": 3.347783365519795e-18, + "velocityX": 3.7194495512825236, + "velocityY": -0.5038690818541787, "moduleForcesX": [ - -45.9398192805324, - -59.450192458631626, - -29.698787647568384, - -46.616491719965026 + -61.323422950505716, + -65.95546140653828, + -44.62846950333632, + -55.985729468867824 ], "moduleForcesY": [ - 51.92283929106931, - 36.09371888383255, - 62.89922393782059, - 51.7414031119671 + 32.44664959475204, + 22.20881577411158, + 53.33306981117861, + 41.43874553341409 ], - "timestamp": 2.379141122264038 + "timestamp": 2.2648261210315326 }, { - "x": 7.356199543080895, - "y": 0.7288568209198342, - "heading": -0.03060270556570066, - "angularVelocity": -2.149395300731945e-22, - "velocityX": 3.3790601126213313, - "velocityY": -0.49397070672899657, + "x": 7.342124626950215, + "y": 1.053916100516507, + "heading": -0.030602705565701004, + "angularVelocity": 7.672386391662901e-19, + "velocityX": 3.4979351696684997, + "velocityY": -0.3822037381956934, "moduleForcesX": [ - -53.90473620010748, - -53.90473620010748, - -53.90473620010747, - -53.90473620010747 + -61.122061696457, + -61.122061696457, + -61.122061696457, + -61.122061696457 ], "moduleForcesY": [ - 44.22041388953324, - 44.22041388953323, - 44.22041388953325, - 44.22041388953323 + 33.57089768760224, + 33.57089768760224, + 33.57089768760223, + 33.57089768760224 ], - "timestamp": 2.444753761794498 + "timestamp": 2.3319522615585138 }, { - "x": 7.563645437858805, - "y": 0.7041763764831177, - "heading": -0.03060270556570066, - "angularVelocity": -2.1492569533532087e-22, - "velocityX": 3.161675803053267, - "velocityY": -0.376153811420113, + "x": 7.56101179722364, + "y": 1.0341858544549376, + "heading": -0.03060270556570099, + "angularVelocity": 7.672403149471102e-19, + "velocityX": 3.260833537501588, + "velocityY": -0.2939279080649534, "moduleForcesX": [ - -61.36608679123134, - -61.36608679123133, - -61.36608679123134, - -61.36608679123134 + -65.42302348088484, + -65.42302348088484, + -65.42302348088484, + -65.42302348088484 ], "moduleForcesY": [ - 33.25889452354701, - 33.258894523547006, - 33.258894523547006, - 33.258894523547006 + 24.35778975736353, + 24.357789757363527, + 24.357789757363534, + 24.357789757363534 ], - "timestamp": 2.510366401324958 + "timestamp": 2.399078402085495 }, { - "x": 7.756067149920293, - "y": 0.6856463295872331, - "heading": -0.03060270556570066, - "angularVelocity": -2.148886089665622e-22, - "velocityX": 2.932692746984486, - "velocityY": -0.28241581238752267, + "x": 7.76353893371165, + "y": 1.0190536925576863, + "heading": -0.030602705565700973, + "angularVelocity": 7.672257598260093e-19, + "velocityX": 3.0171127804763933, + "velocityY": -0.22542874919449496, "moduleForcesX": [ - -64.6403326917135, - -64.6403326917135, - -64.6403326917135, - -64.6403326917135 + -67.24942660173558, + -67.24942660173558, + -67.24942660173558, + -67.24942660173558 ], "moduleForcesY": [ - 26.461588675373616, - 26.461588675373616, - 26.461588675373616, - 26.461588675373616 + 18.900848713116805, + 18.900848713116805, + 18.9008487131168, + 18.900848713116808 ], - "timestamp": 2.575979040855418 + "timestamp": 2.466204542612476 }, { - "x": 7.9330736647908635, - "y": 0.6722308870628335, - "heading": -0.03060270556570066, - "angularVelocity": -2.14835209295751e-22, - "velocityX": 2.697750252653663, - "velocityY": -0.2044643017016828, + "x": 7.949481024014891, + "y": 1.0076590456800256, + "heading": -0.03060270556570096, + "angularVelocity": 7.672013523245006e-19, + "velocityX": 2.7700399403791343, + "velocityY": -0.16974976943713838, "moduleForcesX": [ - -66.32264088747145, - -66.32264088747145, - -66.32264088747145, - -66.32264088747145 + -68.17436080640248, + -68.17436080640248, + -68.17436080640248, + -68.17436080640248 ], "moduleForcesY": [ - 22.005172221305003, - 22.005172221305003, - 22.005172221304996, - 22.005172221304996 + 15.36339993436348, + 15.363399934363485, + 15.363399934363473, + 15.363399934363477 ], - "timestamp": 2.641591680385878 + "timestamp": 2.5333306831394573 }, { - "x": 8.094438850207247, - "y": 0.6632084825286524, - "heading": -0.03060270556570066, - "angularVelocity": -2.1476865288778444e-22, - "velocityX": 2.4593612842152437, - "velocityY": -0.13751015960869134, + "x": 8.118709429041983, + "y": 0.9994031964377751, + "heading": -0.030602705565700945, + "angularVelocity": 7.671706888831663e-19, + "velocityX": 2.5210507217984657, + "velocityY": -0.12299007774670825, "moduleForcesX": [ - -67.29555668636523, - -67.29555668636523, - -67.29555668636523, - -67.29555668636523 + -68.70314364670283, + -68.70314364670283, + -68.70314364670283, + -68.70314364670283 ], "moduleForcesY": [ - 18.90069114406037, - 18.90069114406037, - 18.900691144060374, - 18.900691144060374 + 12.90231694942824, + 12.902316949428238, + 12.902316949428242, + 12.902316949428242 ], - "timestamp": 2.707204319916338 + "timestamp": 2.6004568236664385 }, { - "x": 8.240020092552498, - "y": 0.6580507593309295, - "heading": -0.03060270556570066, - "angularVelocity": -2.146966814878084e-22, - "velocityX": 2.2187987465078, - "velocityY": -0.07860868324507067, + "x": 8.271143977771962, + "y": 0.9938469245659062, + "heading": -0.030602705565700928, + "angularVelocity": 7.671359538796945e-19, + "velocityX": 2.270867169380966, + "velocityY": -0.08277359353968143, "moduleForcesX": [ - -67.9091402549072, - -67.9091402549072, - -67.9091402549072, - -67.9091402549072 + -69.0326940172405, + -69.0326940172405, + -69.0326940172405, + -69.0326940172405 ], "moduleForcesY": [ - 16.627479314598546, - 16.627479314598546, - 16.627479314598553, - 16.62747931459855 + 11.096861571799984, + 11.096861571799986, + 11.096861571799984, + 11.096861571799984 ], - "timestamp": 2.7728169594467977 + "timestamp": 2.6675829641934197 }, { - "x": 8.369721422210265, - "y": 0.6563552816375592, - "heading": -0.03060270556570066, - "angularVelocity": -2.146156534170985e-22, - "velocityX": 1.9767735391525214, - "velocityY": -0.02584071766512831, + "x": 8.406731406019375, + "y": 0.9906547367603562, + "heading": -0.030602705565700914, + "angularVelocity": 7.670985863150012e-19, + "velocityX": 2.019890123027612, + "velocityY": -0.047555062461336506, "moduleForcesX": [ - -68.32204177817843, - -68.32204177817843, - -68.32204177817843, - -68.32204177817843 + -69.25164136036881, + -69.25164136036881, + -69.25164136036881, + -69.25164136036881 ], "moduleForcesY": [ - 14.896031650164923, - 14.896031650164922, - 14.896031650164927, - 14.896031650164925 + 9.717785426549383, + 9.717785426549383, + 9.717785426549389, + 9.717785426549389 ], - "timestamp": 2.8384295989772577 + "timestamp": 2.734709104720401 }, { - "x": 8.483474950917365, - "y": 0.6578057698994572, - "heading": -0.03060270556570066, - "angularVelocity": -2.1453703946144053e-22, - "velocityX": 1.7337136490948615, - "velocityY": 0.022106842100517037, + "x": 8.5254345460703, + "y": 0.9895621634069518, + "heading": -0.0306027055657009, + "angularVelocity": 7.670595843658019e-19, + "velocityX": 1.7683593771223813, + "velocityY": -0.016276421448131698, "moduleForcesX": [ - -68.61412554743417, - -68.61412554743417, - -68.61412554743417, - -68.61412554743417 + -69.40442267327488, + -69.40442267327488, + -69.40442267327488, + -69.40442267327488 ], "moduleForcesY": [ - 13.535264434918018, - 13.535264434918016, - 13.53526443491802, - 13.53526443491802 + 8.630658704124505, + 8.630658704124507, + 8.630658704124507, + 8.630658704124508 ], - "timestamp": 2.9040422385077176 + "timestamp": 2.801835245247382 }, { - "x": 8.581230723210314, - "y": 0.6621472949390711, - "heading": -0.030602705565700657, - "angularVelocity": -2.1446182627185602e-22, - "velocityX": 1.4898923895230367, - "velocityY": 0.06616903497074665, + "x": 8.627226437405287, + "y": 0.9903554058889321, + "heading": -0.030602705565700883, + "angularVelocity": 7.670197234878886e-19, + "velocityX": 1.516426991569238, + "velocityY": 0.011817191868226978, "moduleForcesX": [ - -68.82905489435568, - -68.82905489435568, - -68.82905489435568, - -68.82905489435568 + -69.51524637407707, + -69.51524637407705, + -69.51524637407707, + -69.51524637407705 ], "moduleForcesY": [ - 12.438452238151038, - 12.438452238151038, - 12.438452238151038, - 12.438452238151038 + 7.751819786448097, + 7.751819786448097, + 7.751819786448097, + 7.751819786448097 ], - "timestamp": 2.9696548780381775 + "timestamp": 2.868961385774363 }, { - "x": 8.66295079020209, - "y": 0.6691700762270508, - "heading": -0.030602705565700657, - "angularVelocity": -2.1439007784276e-22, - "velocityX": 1.2454927522590922, - "velocityY": 0.10703396995207695, + "x": 8.712086900940639, + "y": 0.9928580419153559, + "heading": -0.03060270556570087, + "angularVelocity": 7.6697962024819e-19, + "velocityX": 1.2641939916274305, + "velocityY": 0.03728258479896153, "moduleForcesX": [ - -68.99232691579692, - -68.99232691579692, - -68.99232691579692, - -68.99232691579692 + -69.59819435728923, + -69.59819435728923, + -69.59819435728923, + -69.59819435728923 ], "moduleForcesY": [ - 11.535888453791287, - 11.535888453791287, - 11.535888453791285, - 11.535888453791285 + 7.026619700405902, + 7.026619700405903, + 7.0266197004059014, + 7.0266197004059014 ], - "timestamp": 3.0352675175686374 + "timestamp": 2.9360875263013444 }, { - "x": 8.728605558744404, - "y": 0.678698482418167, - "heading": -0.030602705565700657, - "angularVelocity": -2.1432640541955167e-22, - "velocityX": 1.0006420868319634, - "velocityY": 0.14522211359432977, + "x": 8.780000437865695, + "y": 0.996921987964414, + "heading": -0.03060270556570085, + "angularVelocity": 7.669398274852111e-19, + "velocityX": 1.011730100850266, + "velocityY": 0.06054192922688254, "moduleForcesX": [ - -69.11964904618539, - -69.11964904618539, - -69.11964904618539, - -69.11964904618539 + -69.66190364637255, + -69.66190364637255, + -69.66190364637255, + -69.66190364637255 ], "moduleForcesY": [ - 10.780248776015798, - 10.780248776015798, - 10.780248776015798, - 10.780248776015798 + 6.417908736725351, + 6.417908736725351, + 6.417908736725356, + 6.417908736725355 ], - "timestamp": 3.1008801570990974 + "timestamp": 3.0032136668283256 }, { - "x": 8.778171440675647, - "y": 0.6905833152058894, - "heading": -0.030602705565700657, - "angularVelocity": -2.1427532944253698e-22, - "velocityX": 0.7554319150387594, - "velocityY": 0.18113633093826434, + "x": 8.830954883407468, + "y": 1.0024211476734168, + "heading": -0.030602705565700838, + "angularVelocity": 7.669008842488089e-19, + "velocityX": 0.75908498748391, + "velocityY": 0.08192277502968102, "moduleForcesX": [ - -69.22113520637045, - -69.22113520637045, - -69.22113520637045, - -69.22113520637045 + -69.71190806731377, + -69.71190806731377, + -69.71190806731377, + -69.71190806731377 ], "moduleForcesY": [ - 10.138335112343585, - 10.138335112343585, - 10.138335112343585, - 10.138335112343585 + 5.89957801675687, + 5.899578016756869, + 5.899578016756865, + 5.899578016756865 ], - "timestamp": 3.1664927966295573 + "timestamp": 3.0703398073553068 }, { - "x": 8.811629282013929, - "y": 0.7046962431476756, - "heading": -0.030602705565700657, - "angularVelocity": -2.14229895305694e-22, - "velocityX": 0.5099298180611922, - "velocityY": 0.2150946531458243, + "x": 8.86494051215884, + "y": 1.0092468204121565, + "heading": -0.030602705565700824, + "angularVelocity": 7.668631929139065e-19, + "velocityX": 0.5062949915570888, + "velocityY": 0.10168427210552212, "moduleForcesX": [ - -69.303543666465, - -69.303543666465, - -69.303543666465, - -69.303543666465 + -69.7518852496981, + -69.7518852496981, + -69.7518852496981, + -69.7518852496981 ], "moduleForcesY": [ - 9.586199445644239, - 9.586199445644237, - 9.586199445644244, - 9.586199445644244 + 5.452754058783733, + 5.452754058783732, + 5.452754058783737, + 5.452754058783736 ], - "timestamp": 3.232105436160017 + "timestamp": 3.137465947882288 }, { - "x": 8.828963279724121, - "y": 0.7209256887435913, - "heading": -0.030602705565700657, - "angularVelocity": 3.635197207581808e-20, - "velocityX": 0.26418686756452614, - "velocityY": 0.24735242648455466, + "x": 8.881949424743652, + "y": 1.017304301261902, + "heading": -0.030602705565700813, + "angularVelocity": -6.192148052914439e-17, + "velocityX": 0.25338731604818493, + "velocityY": 0.12003491913118446, "moduleForcesX": [ - -69.3715349487538, - -69.3715349487538, - -69.3715349487538, - -69.3715349487538 + -69.78435636275017, + -69.78435636275017, + -69.78435636275017, + -69.78435636275017 ], "moduleForcesY": [ - 9.10614626386413, - 9.10614626386413, - 9.10614626386413, - 9.10614626386413 + 5.063460762339646, + 5.063460762339647, + 5.063460762339649, + 5.06346076233965 ], - "timestamp": 3.297718075690477 + "timestamp": 3.204592088409269 }, { - "x": 8.828203904774893, - "y": 0.741593975643098, - "heading": -0.03505435831192594, - "angularVelocity": -0.06067171185490822, - "velocityX": -0.010349544480640927, - "velocityY": 0.28168871625594366, + "x": 8.880530486218973, + "y": 1.027348591072611, + "heading": -0.034530809902176336, + "angularVelocity": -0.054161349122254336, + "velocityX": -0.019564557922623505, + "velocityY": 0.13849232110933965, "moduleForcesX": [ - -69.82238913204013, - -69.84216616588223, - -68.65753096355249, - -68.89031773540574 + -69.96707712828831, + -69.96918542605992, + -69.4046006596549, + -69.48963562370783 ], "moduleForcesY": [ - 4.5647052079056, - 4.326632215396829, - 13.498725767769074, - 12.28091309563698 + 0.7821062724142991, + 0.9492571184496107, + 8.889772333230777, + 8.233787400699313 ], - "timestamp": 3.3710908652513036 + "timestamp": 3.277118057576761 }, { - "x": 8.807299851592818, - "y": 0.7647796846719664, - "heading": -0.04385437316260252, - "angularVelocity": -0.11993567238412434, - "velocityX": -0.2849019821543826, - "velocityY": 0.31599873969146675, + "x": 8.859315047264229, + "y": 1.03873366112627, + "heading": -0.04230269871701783, + "angularVelocity": -0.10716008216259976, + "velocityX": -0.29252196417731824, + "velocityY": 0.15697921977944768, "moduleForcesX": [ - -69.81588658834606, - -69.83182481459568, - -68.68488260159317, - -68.89599182771352 + -69.96360527609723, + -69.96507878923691, + -69.41623345810719, + -69.49123274928873 ], "moduleForcesY": [ - 4.61919336637954, - 4.448281304435051, - 13.343438108461418, - 12.233541145410339 + 0.849054357453386, + 1.0605014457335615, + 8.776783714333206, + 8.198715576096298 ], - "timestamp": 3.44446365481213 + "timestamp": 3.3496440267442527 }, { - "x": 8.766249783637008, - "y": 0.7904802826298837, - "heading": -0.05688378166614004, - "angularVelocity": -0.17757820823666606, - "velocityX": -0.559472635584879, - "velocityY": 0.3502742380622083, + "x": 8.81830267354523, + "y": 1.0514616761704456, + "heading": -0.053822469418188024, + "angularVelocity": -0.15883649447922438, + "velocityX": -0.5654853591032976, + "velocityY": 0.17549596634529624, "moduleForcesX": [ - -69.80907759591098, - -69.8190745845742, - -68.71780064776569, - -68.90102631540906 + -69.95969350130241, + -69.9600439550638, + -69.43034005880952, + -69.49219040870635 ], "moduleForcesY": [ - 4.671002982456192, - 4.596843952033549, - 13.154721929440731, - 12.187023469399424 + 0.916057569851737, + 1.1945950847132014, + 8.639230967068034, + 8.165662210562154 ], - "timestamp": 3.5178364443729566 + "timestamp": 3.4221699959117444 }, { - "x": 8.705052173115664, - "y": 0.8186924642311597, - "heading": -0.07400373694683447, - "angularVelocity": -0.2333283957604187, - "velocityX": -0.8340641113366615, - "velocityY": 0.3845046885928745, + "x": 8.75749289682575, + "y": 1.0655348258499984, + "heading": -0.06897986858552595, + "angularVelocity": -0.2089927144919403, + "velocityX": -0.838455210147377, + "velocityY": 0.1940429040948802, "moduleForcesX": [ - -69.80139202754168, - -69.80353156229272, - -68.75655115056365, - -68.90652968620067 + -69.9551810729799, + -69.95383968280234, + -69.44688305083558, + -69.49295928173186 ], "moduleForcesY": [ - 4.726967109732889, - 4.773715711987706, - 12.929025319368964, - 12.134397256845864 + 0.9882804241447996, + 1.352618156589803, + 8.475469948022155, + 8.130018720639756 ], - "timestamp": 3.591209233933783 + "timestamp": 3.494695965079236 }, { - "x": 8.623705265045926, - "y": 0.8494119202761454, - "heading": -0.09504978733566269, - "angularVelocity": -0.28683726644168145, - "velocityX": -1.108679505803711, - "velocityY": 0.41867640890931357, + "x": 8.67688521696236, + "y": 1.080955321413936, + "heading": -0.08764661363140669, + "angularVelocity": -0.2573801530723142, + "velocityX": -1.1114319572514513, + "velocityY": 0.2126203309097312, "moduleForcesX": [ - -69.79205213254602, - -69.78467756927584, - -68.80147068719397, - -68.91395589602016 + -69.94983324196707, + -69.94615057660823, + -69.46581320027535, + -69.49411065050936 ], "moduleForcesY": [ - 4.795991300911855, - 4.980866247525471, - 12.661611125036274, - 12.06633404852973 + 1.0723396108261634, + 1.5360717401888286, + 8.283336243627176, + 8.085785369496797 ], - "timestamp": 3.6645820234946096 + "timestamp": 3.567221934246728 }, { - "x": 8.522207036579841, - "y": 0.8826330056369158, - "heading": -0.11982385908892228, - "angularVelocity": -0.3376465839931265, - "velocityX": -1.3833224697275863, - "velocityY": 0.45277119160407436, + "x": 8.576479109537585, + "y": 1.0977253883199334, + "heading": -0.1096713977003478, + "angularVelocity": -0.3036813478255371, + "velocityX": -1.3844159351099252, + "velocityY": 0.23122844270129664, "moduleForcesX": [ - -69.77997172662764, - -69.76180279178641, - -68.85297448408464, - -68.92524546744818 + -69.94330252963142, + -69.9365583049015, + -69.4870568794036, + -69.49637643933679 ], "moduleForcesY": [ - 4.889881316073384, - 5.2210669442672835, - 12.346127343474148, - 11.970039561538547 + 1.176823061341541, + 1.7470252667944042, + 8.060012105446882, + 8.025018376878974 ], - "timestamp": 3.737954813055436 + "timestamp": 3.6397479034142197 }, { - "x": 8.400555154994208, - "y": 0.9183482483752978, - "heading": -0.14808264155198017, - "angularVelocity": -0.3851398131678119, - "velocityX": -1.6579972264075493, - "velocityY": 0.48676413902423377, + "x": 8.456274044587177, + "y": 1.1158472524014584, + "heading": -0.13487291738284943, + "angularVelocity": -0.347482701327652, + "velocityX": -1.6574072202027315, + "velocityY": 0.24986724465107038, "moduleForcesX": [ - -69.76356912538407, - -69.73390347020135, - -68.91158189541561, - -68.94304262893307 + -69.93505805724766, + -69.92449408332656, + -69.51050132115871, + -69.50070530398553 ], "moduleForcesY": [ - 5.024739981390052, - 5.498335546770225, - 11.973827493944482, - 11.82738424849502 + 1.3131272162432313, + 1.98838442321693, + 7.801780109725148, + 7.936938199533164 ], - "timestamp": 3.8113276026162626 + "timestamp": 3.7122738725817115 }, { - "x": 8.258746945464983, - "y": 0.9565475945096078, - "heading": -0.1795200221320126, - "angularVelocity": -0.42846102442337525, - "velocityX": -1.932708438346409, - "velocityY": 0.5206200604195711, + "x": 8.316269527386241, + "y": 1.1353231154725127, + "heading": -0.16302980404777723, + "angularVelocity": -0.38823178770838385, + "velocityX": -1.930405326643809, + "velocityY": 0.26853640557469155, "moduleForcesX": [ - -69.74040444259873, - -69.6994878605935, - -68.97797470393978, - -68.97104071246528 + -69.92425253763372, + -69.90915580434691, + -69.53597671993317, + -69.50834196529713 ], "moduleForcesY": [ - 5.223448587988533, - 5.818826004851896, - 11.532106083145074, - 11.611544934432994 + 1.4968639656801608, + 2.264388307053663, + 7.503564176470051, + 7.80642632120717 ], - "timestamp": 3.884700392177089 + "timestamp": 3.7847998417492033 }, { - "x": 8.096779401992974, - "y": 0.9972171857084491, - "heading": -0.21373905317621672, - "angularVelocity": -0.46637222394054323, - "velocityX": -2.2074606191405195, - "velocityY": 0.5542871061911322, + "x": 8.156465184614108, + "y": 1.1561551126608645, + "heading": -0.1938654641731384, + "angularVelocity": -0.42516715708305197, + "velocityX": -2.2034085804916796, + "velocityY": 0.28723500599140817, "moduleForcesX": [ - -69.70643217846147, - -69.65617966490848, - -69.0531155698116, - -69.01454849786487 + -69.9094604865732, + -69.88935388472542, + -69.5632321514312, + -69.52093877070061 ], "moduleForcesY": [ - 5.520358534554248, - 6.192692191683197, - 11.001614243637365, - 11.280543987218508 + 1.7503444073984757, + 2.581564547481693, + 7.158055486956702, + 7.611351867199961 ], - "timestamp": 3.9580731817379156 + "timestamp": 3.857325810916695 }, { - "x": 7.914649343266423, - "y": 1.0403372462568932, - "heading": -0.2502040431261532, - "angularVelocity": -0.49698246677273733, - "velocityX": -2.482256158130169, - "velocityY": 0.5876846281372092, + "x": 7.976860949117206, + "y": 1.1783452360639273, + "heading": -0.2270240484884978, + "angularVelocity": -0.45719601814734345, + "velocityX": -2.4764127602640795, + "velocityY": 0.3059610737751436, "moduleForcesX": [ - -69.6543272195074, - -69.59982811388493, - -69.13847224033395, - -69.08142915869702 + -69.88812974316788, + -69.86320182607882, + -69.59189618603347, + -69.5407034064261 ], "moduleForcesY": [ - 5.970871351408315, - 6.638279683857717, - 10.350201343001592, - 10.763705620631843 + 2.107233171390243, + 2.950662034921671, + 6.753983199950573, + 7.317496840416227 ], - "timestamp": 4.031445971298742 + "timestamp": 3.929851780084187 }, { - "x": 7.712354049624925, - "y": 1.0858780921373514, - "heading": -0.28815099090474133, - "angularVelocity": -0.517180115485863, - "velocityX": -2.7570887634549326, - "velocityY": 0.6206775856968689, + "x": 7.777457476125625, + "y": 1.201895191338467, + "heading": -0.26202980984331203, + "angularVelocity": -0.4826651991962036, + "velocityX": -2.74940790561615, + "velocityY": 0.3247106594351006, "moduleForcesX": [ - -69.56928399894086, - -69.52224882009111, - -69.23640181351112, - -69.18354973211308 + -69.85533117072832, + -69.82743416827404, + -69.62139360036666, + -69.57054322743649 ], "moduleForcesY": [ - 6.673022110744875, - 7.1925990350588, - 9.51901410508811, - 9.929914946454016 + 2.6219385333270773, + 3.3908613060937713, + 6.272491751359185, + 6.868108041742767 ], - "timestamp": 4.104818760859568 + "timestamp": 4.0023777492516786 }, { - "x": 7.489893664384698, - "y": 1.133791594453917, - "heading": -0.3263979812876508, - "angularVelocity": -0.5212694053454582, - "velocityX": -3.03191941551856, - "velocityY": 0.6530145930576222, + "x": 7.558257164116274, + "y": 1.2268061069502, + "heading": -0.2982123205048736, + "angularVelocity": -0.49889041232006653, + "velocityX": -3.022369980373853, + "velocityY": 0.3434758045660868, "moduleForcesX": [ - -69.41666539245078, - -69.40439565809636, - -69.35057994409232, - -69.33787106983414 + -69.80056727463466, + -69.77570886950674, + -69.6507199544232, + -69.61392322278483 ], "moduleForcesY": [ - 7.822962646502443, - 7.941889554066643, - 8.38638483270236, - 8.500969214195738 + 3.39043477997722, + 3.9399463888437256, + 5.678903958822243, + 6.160009083534089 ], - "timestamp": 4.178191550420395 + "timestamp": 4.07490371841917 }, { - "x": 7.247281611648338, - "y": 1.183989156390833, - "heading": -0.3628608681557275, - "angularVelocity": -0.4969538038055478, - "velocityX": -3.3065671100760374, - "velocityY": 0.6841441116982726, + "x": 7.319267013948266, + "y": 1.2530778743434587, + "heading": -0.3345519884568234, + "angularVelocity": -0.5010573229066131, + "velocityX": -3.2952355261337996, + "velocityY": 0.3622394529136627, "moduleForcesX": [ - -69.09618755623467, - -69.18927818386388, - -69.48444757611301, - -69.55485789829521 + -69.69826365815047, + -69.69360285262502, + -69.67768220142874, + -69.67276264637083 ], "moduleForcesY": [ - 9.887138047237972, - 9.135257071526894, - 6.655220631175712, - 5.755332008623882 + 4.602649202702627, + 4.682797548651256, + 4.9016741603909, + 4.9806442793840855 ], - "timestamp": 4.251564339981221 + "timestamp": 4.147429687586662 }, { - "x": 6.9846108615466544, - "y": 1.2362643691091493, - "heading": -0.3928314758501629, - "angularVelocity": -0.40847033176501496, - "velocityX": -3.5799477118684186, - "velocityY": 0.7124604779402591, + "x": 7.060508520408783, + "y": 1.2807073809011449, + "heading": -0.3693010360245331, + "angularVelocity": -0.479125587246758, + "velocityX": -3.5678046982302343, + "velocityY": 0.38096018398438164, "moduleForcesX": [ - -68.19397798012086, - -68.604096270111, - -69.61287896515513, - -69.63437448602505 + -69.47236753682394, + -69.53951749408205, + -69.69564436263259, + -69.73202523829484 ], "moduleForcesY": [ - 14.31877505066931, - 11.837279257782388, - 3.3817824033703014, - -0.9454620249074384 + 6.700012628800592, + 5.851281299757406, + 3.768542257976925, + 2.8040874089418844 ], - "timestamp": 4.324937129542048 + "timestamp": 4.219955656754154 }, { - "x": 6.703415297553567, - "y": 1.290040410483288, - "heading": -0.3945237678570967, - "angularVelocity": -0.02306429968198066, - "velocityX": -3.8324229687352247, - "velocityY": 0.7329153177358495, + "x": 6.7820675423003, + "y": 1.3096827333084073, + "heading": -0.398737971474847, + "angularVelocity": -0.40588131104575736, + "velocityX": -3.839190035026568, + "velocityY": 0.39951692807238764, "moduleForcesX": [ - -63.16947604320132, - -61.34983762111749, - -69.05769594451374, - -61.35920225606092 + -68.80858735271256, + -69.12938539141281, + -69.67366590353845, + -69.61858416268272 ], "moduleForcesY": [ - 28.4635156424948, - 30.27498948987804, - -6.582220178863257, - -31.50206546085697 + 10.998366071640108, + 8.339611523596144, + 1.7520720276263266, + -2.1336448340952816 ], - "timestamp": 4.398309919102874 + "timestamp": 4.292481625921646 }, { - "x": 6.41959619572841, - "y": 1.3315714425952379, - "heading": -0.3945728781781997, - "angularVelocity": -0.0006693260730159474, - "velocityX": -3.868179246338573, - "velocityY": 0.566027710824874, + "x": 6.484732150451617, + "y": 1.3400281940076402, + "heading": -0.4090628910860552, + "angularVelocity": -0.142361690988409, + "velocityX": -4.099709321525832, + "velocityY": 0.4184082067095552, "moduleForcesX": [ - -9.773676203527769, - -7.669404194901154, - -10.354384240238268, - -8.307340543871192 + -65.20316881632876, + -65.7489206217446, + -69.31239778456624, + -65.86566076035609 ], "moduleForcesY": [ - -41.36655808090332, - -42.00286570902313, - -42.261025709276105, - -42.88386291727224 + 23.606521909357, + 20.444429351523326, + -3.7507078277888724, + -21.00209915007702 ], - "timestamp": 4.4716827086637005 + "timestamp": 4.365007595089137 }, { - "x": 6.135687490084942, - "y": 1.372487307589655, - "heading": -0.3946215087945329, - "angularVelocity": -0.0006627881619921617, - "velocityX": -3.8694004595273754, - "velocityY": 0.5576435793067078, + "x": 6.1842404767603805, + "y": 1.3742213267083638, + "heading": -0.40911116386317103, + "angularVelocity": -0.0006655929961390624, + "velocityX": -4.1432286550667365, + "velocityY": 0.4714605415577361, "moduleForcesX": [ - -0.30840180759639224, - -0.3079749898198458, - -0.30858337724113316, - -0.30815655961690325 + -13.325449562031311, + -4.164768682569861, + -18.043544953356022, + -8.922855853125498 ], "moduleForcesY": [ - -2.116157086062331, - -2.116334618026183, - -2.1165925122839986, - -2.116770044133725 + 19.90592667522208, + 16.82639877386449, + 10.611237303210721, + 6.85137196260812 ], - "timestamp": 4.545055498224527 + "timestamp": 4.437533564256629 }, { - "x": 5.85352801680754, - "y": 1.4241018905198637, - "heading": -0.39467053194899787, - "angularVelocity": -0.0006681380762323543, - "velocityX": -3.845560117943876, - "velocityY": 0.7034567342900305, + "x": 5.886052139153465, + "y": 1.4246963553310026, + "heading": -0.4091589633873657, + "angularVelocity": -0.0006590677069691712, + "velocityX": -4.111469878027867, + "velocityY": 0.6959580023818284, "moduleForcesX": [ - 6.0183308507069375, - 6.017872402285157, - 6.018488007158529, - 6.018029561236691 + 8.110433382725144, + 8.111187384355791, + 8.110199033033277, + 8.110953042580169 ], "moduleForcesY": [ - 36.80841670957, - 36.80855524647377, - 36.808665373492616, - 36.808803909707684 + 57.33325913716358, + 57.33311245310866, + 57.33314368701091, + 57.33299700322065 ], - "timestamp": 4.6184282877853535 + "timestamp": 4.510059533424121 }, { - "x": 5.5751319379159305, - "y": 1.493186559151982, - "heading": -0.39472121211754135, - "angularVelocity": -0.0006907215719449539, - "velocityX": -3.7942687003990416, - "velocityY": 0.941557068303184, + "x": 5.591555275132608, + "y": 1.4934998764417577, + "heading": -0.4092089626864391, + "angularVelocity": -0.0006893985650598928, + "velocityX": -4.060571232640143, + "velocityY": 0.94867427351207, "moduleForcesX": [ - 12.948916142731543, - 12.946169957519793, - 12.949515473189427, - 12.946769400230588 + 13.000242789451551, + 12.99626661282106, + 13.001163654267753, + 12.99718771199267 ], "moduleForcesY": [ - 60.104836688704204, - 60.10549713360235, - 60.105088950128234, - 60.105749386647936 + 64.53930421343945, + 64.54015945588058, + 64.53937921708196, + 64.54023445819007 ], - "timestamp": 4.69180107734618 + "timestamp": 4.582585502591613 }, { - "x": 5.302053451538086, - "y": 1.5809556245803833, - "heading": -0.39477498805993555, - "angularVelocity": -0.0007329139687353689, - "velocityX": -3.721795068885318, - "velocityY": 1.196207285476589, + "x": 5.302053451538088, + "y": 1.5809556245803824, + "heading": -0.4092627987960467, + "angularVelocity": -0.000742301140211202, + "velocityX": -3.991698793103366, + "velocityY": 1.2058542497602232, "moduleForcesX": [ - 18.297412052733495, - 18.291870609159098, - 18.298156444631005, - 18.292615475816596 + 17.591944196413664, + 17.584790178826015, + 17.593089676218266, + 17.5859364455025 ], "moduleForcesY": [ - 64.28225176659839, - 64.2838616989904, - 64.2823244352569, - 64.2839342864753 + 65.6787937062123, + 65.68074285605304, + 65.67871632504414, + 65.68066538032537 ], - "timestamp": 4.7651738669070065 + "timestamp": 4.6551114717591044 }, { - "x": 5.07390423214392, - "y": 1.6694230318569008, - "heading": -0.3948239257142182, - "angularVelocity": -0.0007817718663467028, - "velocityX": -3.644650395807531, - "velocityY": 1.4132538862180757, + "x": 5.060781264234282, + "y": 1.668503203961895, + "heading": -0.4093136867686553, + "angularVelocity": -0.0008266775577253413, + "velocityX": -3.9194782641972647, + "velocityY": 1.4222146294751892, "moduleForcesX": [ - 22.829580522567618, - 22.8220128584525, - 22.830066989794148, - 22.82250022697142 + 21.736557880357715, + 21.723118914680356, + 21.73784859212575, + 21.72441246515569 ], "moduleForcesY": [ - 64.21970113385147, - 64.22240309693508, - 64.21976226256318, - 64.22246395577116 + 65.09863070377499, + 65.10314246149298, + 65.09850389768496, + 65.10301496712766 ], - "timestamp": 4.827772249362731 + "timestamp": 4.716668692144205 }, { - "x": 4.851229056834639, - "y": 1.7708905614247654, - "heading": -0.3948711845379385, - "angularVelocity": -0.0007549527937680354, - "velocityX": -3.5572033425428917, - "velocityY": 1.6209289375749065, + "x": 4.824591541574993, + "y": 1.768967581170856, + "heading": -0.4093625786347868, + "angularVelocity": -0.0007942507121868569, + "velocityX": -3.836913382080745, + "velocityY": 1.632048630208832, "moduleForcesX": [ - 25.872414895839015, - 25.87639732222832, - 25.872346027139017, - 25.876328704106957 + 24.840611640815233, + 24.84563714723139, + 24.840370639682614, + 24.84539654710804 ], "moduleForcesY": [ - 61.44911151867199, - 61.44743545397816, - 61.44890652890951, - 61.447230357726816 + 63.138133963812095, + 63.136150247952436, + 63.13804254630454, + 63.13605868744737 ], - "timestamp": 4.890370631818455 + "timestamp": 4.778225912529306 }, { - "x": 4.634382563500368, - "y": 1.8842851362906166, - "heading": -0.3949168954426069, - "angularVelocity": -0.000730225013412799, - "velocityX": -3.4640910008760675, - "velocityY": 1.8114617409811675, + "x": 4.5940300417851905, + "y": 1.8817568503359232, + "heading": -0.40940943255755435, + "angularVelocity": -0.00076114422441599, + "velocityX": -3.7454826314023215, + "velocityY": 1.8322670916501005, "moduleForcesX": [ - 27.548950472331388, - 27.55233169388033, - 27.548972415747215, - 27.552353816038643 + 27.508225922613867, + 27.513128573012004, + 27.508193765462636, + 27.51309679763414 ], "moduleForcesY": [ - 56.377068082107755, - 56.375442909315176, - 56.3765916436959, - 56.3749663731007 + 60.24514716829222, + 60.242914588066654, + 60.24482580432994, + 60.242593041950194 ], - "timestamp": 4.95296901427418 + "timestamp": 4.839783132914407 }, { - "x": 4.422622805341516, - "y": 2.0069183796517893, - "heading": -0.39496118737413566, - "angularVelocity": -0.0007075571251397378, - "velocityX": -3.382831150767024, - "velocityY": 1.9590481183425312, + "x": 4.369349606253567, + "y": 2.0058517685581454, + "heading": -0.4094544926877705, + "angularVelocity": -0.0007320039783130824, + "velocityX": -3.6499444602278026, + "velocityY": 2.015927903272623, "moduleForcesX": [ - 24.042485271707772, - 24.045024480603296, - 24.042303530281014, - 24.044842834314085 + 28.744538248232754, + 28.74851367433131, + 28.744577870233446, + 28.74855354416703 ], "moduleForcesY": [ - 43.66989864479118, - 43.66861606069169, - 43.66891333264767, - 43.66763069286199 + 55.26315032046569, + 55.261123506293146, + 55.262531653571756, + 55.260504693222536 ], - "timestamp": 5.015567396729904 + "timestamp": 4.901340353299508 }, { - "x": 4.213243294089462, - "y": 2.1335730846638814, - "heading": -0.3950047652007233, - "angularVelocity": -0.0006961494031973518, - "velocityX": -3.344807054721382, - "velocityY": 2.023290379774191, + "x": 4.149973859162333, + "y": 2.13910073076137, + "heading": -0.4094996155694483, + "angularVelocity": -0.0007330233788204265, + "velocityX": -3.563769541880253, + "velocityY": 2.1646357871525597, "moduleForcesX": [ - 11.25048576449635, - 11.251433113415713, - 11.250173496493936, - 11.251120853755697 + 25.92928590684574, + 25.929168249602895, + 25.929292292013468, + 25.929174634983514 ], "moduleForcesY": [ - 19.00901574015739, - 19.008592674544687, - 19.00819786419031, - 19.007774795539515 + 44.74476438288918, + 44.74482729836071, + 44.74480699802511, + 44.744869913360205 ], - "timestamp": 5.078165779185628 + "timestamp": 4.962897573684609 }, { - "x": 4.004677598787759, - "y": 2.2615635179547406, - "heading": -0.39504821197194406, - "angularVelocity": -0.0006940558128879706, - "velocityX": -3.3318064639964438, - "velocityY": 2.0446284435765874, + "x": 3.9353204017277856, + "y": 2.278088242273816, + "heading": -0.41252140844778834, + "angularVelocity": -0.04908917036026353, + "velocityX": -3.4870557197299563, + "velocityY": 2.2578587961400522, "moduleForcesX": [ - 3.8466474195591016, - 3.8468089460712034, - 3.846580474426278, - 3.8467420010287405 + 24.953488052198967, + 20.4567569278476, + 25.589392523319265, + 21.330260691348453 ], "moduleForcesY": [ - 6.31375253757312, - 6.313684558372743, - 6.313590446713757, - 6.313522467489779 + 25.14686564166064, + 27.7645218324562, + 28.406596524112476, + 30.881756387142097 ], - "timestamp": 5.140764161641353 + "timestamp": 5.02445479406971 }, { - "x": 3.796955991966042, - "y": 2.3909094414143865, - "heading": -0.39510465976032694, - "angularVelocity": -0.0009017451596737262, - "velocityX": -3.318322274040202, - "velocityY": 2.066282200680384, + "x": 3.7318642433739195, + "y": 2.4104934513603675, + "heading": -0.43615839467618756, + "angularVelocity": -0.38398397589611083, + "velocityX": -3.305155058676315, + "velocityY": 2.150928977270674, "moduleForcesX": [ - 3.9944820139638706, - 3.9784542931865783, - 4.001115299988056, - 3.9850884886982674 + 49.89592032127635, + 34.025418039882794, + 66.93891398556973, + 68.06860648029641 ], "moduleForcesY": [ - 6.39563239881223, - 6.402385340358186, - 6.411708329457698, - 6.418461041024309 + -47.83606477558671, + -59.36994474295037, + -17.100092585775965, + -4.390644167479697 ], - "timestamp": 5.203362544097077 + "timestamp": 5.086012014454811 }, { - "x": 3.597726364612312, - "y": 2.517446428244699, - "heading": -0.42282084717405416, - "angularVelocity": -0.4427620383534821, - "velocityX": -3.1826641446309787, - "velocityY": 2.021409849045414, + "x": 3.540415993553555, + "y": 2.5354156131468453, + "heading": -0.46498951807167194, + "angularVelocity": -0.4683629835017646, + "velocityX": -3.110086008150945, + "velocityY": 2.0293665146828603, "moduleForcesX": [ - 43.79907538164046, - 18.529360915334063, - 57.13510334534536, - 41.093905736742414 + 55.83155696216655, + 54.18327481859195, + 62.40116195557468, + 62.36184788712686 ], "moduleForcesY": [ - -35.85922709226587, - -37.9829446448005, - -1.354929099210182, - 22.08867174295008 + -41.50061361131458, + -43.48191734914588, + -30.734501757153353, + -30.59101829583353 ], - "timestamp": 5.265960926552801 + "timestamp": 5.147569234839912 }, { - "x": 3.4109531206363775, - "y": 2.6365558877135373, - "heading": -0.45770016209152625, - "angularVelocity": -0.5571919520786697, - "velocityX": -2.9836752428553495, - "velocityY": 1.9027561862814057, + "x": 3.361022505593955, + "y": 2.652736487945131, + "heading": -0.4956638148847116, + "angularVelocity": -0.49830542414467005, + "velocityX": -2.9142558230751625, + "velocityY": 1.9058832426858676, "moduleForcesX": [ - 55.40246671070086, - 52.925950183673045, - 63.50311672291328, - 63.68070488263328 + 57.73901804404707, + 57.438460533401326, + 60.30587505828676, + 60.21056114711604 ], "moduleForcesY": [ - -41.613108609361404, - -44.404319792002745, - -27.690981599642193, - -26.723490845822106 + -39.07130416952509, + -39.474125567373356, + -34.97681089661358, + -35.097625013264334 ], - "timestamp": 5.328559309008526 + "timestamp": 5.209126455225013 }, { - "x": 3.236759101607606, - "y": 2.7480086789134943, - "heading": -0.49382230864067395, - "angularVelocity": -0.5770460055369145, - "velocityX": -2.7827239649839024, - "velocityY": 1.7804420310506774, + "x": 3.1936838062667205, + "y": 2.7624068712077934, + "heading": -0.5266594310996341, + "angularVelocity": -0.5035252732516518, + "velocityX": -2.7184252030934304, + "velocityY": 1.7816006404539642, "moduleForcesX": [ - 58.68329565175249, - 58.536897284256675, - 60.338571121696745, - 60.27603405724038 + 58.700566811459886, + 58.67734780171254, + 59.16682840463337, + 59.14969530624501 ], "moduleForcesY": [ - -37.48822564336687, - -37.68393200712494, - -34.759584038339426, - -34.83251521985918 + -37.749793809130026, + -37.78083859729802, + -37.014524694721786, + -37.03675261323169 ], - "timestamp": 5.39115769146425 + "timestamp": 5.270683675610114 }, { - "x": 3.0751440767985594, - "y": 2.851743750610944, - "heading": -0.529353385941278, - "angularVelocity": -0.5676037607798451, - "velocityX": -2.5817763729495224, - "velocityY": 1.6571525912961371, + "x": 3.038390509303798, + "y": 2.8643948429617936, + "heading": -0.5570773161987834, + "angularVelocity": -0.4941400035435474, + "velocityX": -2.5227470634868117, + "velocityY": 1.6567994967278303, "moduleForcesX": [ - 59.85882515009252, - 59.8712013872914, - 59.03484963172741, - 59.065559593293514 + 59.30031933124251, + 59.30307854027269, + 58.44288594465124, + 58.46463470158717 ], "moduleForcesY": [ - -35.82050595797904, - -35.81050714543758, - -37.16303360424269, - -37.12450292455551 + -36.885288676049825, + -36.88814111006628, + -38.229406656940725, + -38.203170518961 ], - "timestamp": 5.453756073919974 + "timestamp": 5.332240895995215 }, { - "x": 2.9260957467057915, - "y": 2.9477247616331526, - "heading": -0.563363095015486, - "angularVelocity": -0.5433001259779049, - "velocityX": -2.381025263682967, - "velocityY": 1.5332826066247958, + "x": 2.895129799571264, + "y": 2.9586740032068897, + "heading": -0.5863104114525834, + "angularVelocity": -0.4748930356337662, + "velocityX": -2.3272771063459046, + "velocityY": 1.5315694837305227, "moduleForcesX": [ - 60.486958658404426, - 60.41900268077107, - 58.32195697914738, - 58.369971326869816 + 59.717187192796516, + 59.657546410038414, + 57.934272823511364, + 57.95135152475748 ], "moduleForcesY": [ - -34.87489354988502, - -35.01332620759317, - -38.3857086444205, - -38.33172202686071 + -36.2638573648305, + -36.37433120370207, + -39.04885966973737, + -39.03512982178747 ], - "timestamp": 5.516354456375699 + "timestamp": 5.3937981163803155 }, { - "x": 2.789594973687008, - "y": 3.0359157910118872, - "heading": -0.595278042536244, - "angularVelocity": -0.5098366166782579, - "velocityX": -2.1805798754517425, - "velocityY": 1.4088387897420815, + "x": 2.763886371067874, + "y": 3.045218511804947, + "heading": -0.6139222009668763, + "angularVelocity": -0.4485548460699908, + "velocityX": -2.132055795929944, + "velocityY": 1.4059196964488687, "moduleForcesX": [ - 60.865075509815334, - 60.664615260538106, - 57.84868096481913, - 57.85768343206302 + 60.015428697215164, + 59.85831037769707, + 57.55335306922513, + 57.534003758004665 ], "moduleForcesY": [ - -34.288218926493336, - -34.66454967020803, - -39.16244097534043, - -39.169596867030975 + -35.80957668058033, + -36.086111428235434, + -39.64516852205637, + -39.68654602916551 ], - "timestamp": 5.578952838831423 + "timestamp": 5.455355336765416 }, { - "x": 2.6656099583026958, - "y": 3.1162638597823777, - "heading": -0.6247411596443208, - "angularVelocity": -0.4706689846006165, - "velocityX": -1.9806424786136814, - "velocityY": 1.2835486416493385, + "x": 2.644643305476117, + "y": 3.1240017980714323, + "heading": -0.6395960472514493, + "angularVelocity": -0.41707286527865206, + "velocityX": -1.937109324394044, + "velocityY": 1.2798382671215223, "moduleForcesX": [ - 61.0383972954406, - 60.7049969301487, - 57.47499394607686, - 57.4164365296211 + 60.22326407527744, + 59.95837931731987, + 57.26239607352692, + 57.186270668215975 ], "moduleForcesY": [ - -34.03127548799677, - -34.6446033347473, - -39.75325119619021, - -39.857347146466026 + -35.49033319006886, + -35.95108026507241, + -40.09166456022141, + -40.21383292620102 ], - "timestamp": 5.6415512212871475 + "timestamp": 5.516912557150517 }, { - "x": 2.554110374142435, - "y": 3.1887192741774415, - "heading": -0.6515321237552656, - "angularVelocity": -0.42798173147515556, - "velocityX": -1.7811895417445422, - "velocityY": 1.1574646428973032, + "x": 2.537383808831169, + "y": 3.1949980499443154, + "heading": -0.6631036099321576, + "angularVelocity": -0.38188148414867673, + "velocityX": -1.7424356716228648, + "velocityY": 1.1533375196074727, "moduleForcesX": [ - 61.11771507187923, - 60.66368506097603, - 57.21052652343248, - 57.0695180689212 + 60.36537448302678, + 59.99512146624315, + 57.04267536961116, + 56.89878422586106 ], "moduleForcesY": [ - -33.92654139856753, - -34.75226319319532, - -40.16455016546468, - -40.382680062539976 + -35.27267783600763, + -35.913635660706774, + -40.42480723386195, + -40.64046601274349 ], - "timestamp": 5.704149603742872 + "timestamp": 5.578469777535618 }, { - "x": 2.455069827309922, - "y": 3.2532389005840123, - "heading": -0.6755130530260514, - "angularVelocity": -0.3830918360829452, - "velocityX": -1.5821582435067625, - "velocityY": 1.0306915909880887, + "x": 2.4420919536077847, + "y": 3.2581828688425367, + "heading": -0.6842805929390149, + "angularVelocity": -0.344021105475332, + "velocityX": -1.5480207622638111, + "velocityY": 1.0264404159729446, "moduleForcesX": [ - 61.14927721542877, - 60.589796141080875, - 57.02529080592471, - 56.798052441013304 + 60.4598584765929, + 59.99281023891339, + 56.876528556020105, + 56.661344392247265 ], "moduleForcesY": [ - -33.89819653017852, - -34.90689829521632, - -40.450798746634256, - -40.78566638525383 + -35.130249715774205, + -35.93621486669289, + -40.675063944105, + -40.98709766240873 ], - "timestamp": 5.766747986198596 + "timestamp": 5.640026997920719 }, { - "x": 2.368465422506688, - "y": 3.3097852773291314, - "heading": -0.696597572734595, - "angularVelocity": -0.33682211714426863, - "velocityX": -1.3834926943118622, - "velocityY": 0.9033200943349304, + "x": 2.3587528835015927, + "y": 3.3135334333216804, + "heading": -0.7030081263323279, + "angularVelocity": -0.3042296789257516, + "velocityX": -1.3538471942824237, + "velocityY": 0.8991725768784151, "moduleForcesX": [ - 61.152198837351406, - 60.50298885718338, - 56.892015366210785, - 56.582333241123905 + 60.5201219775476, + 59.967443909712394, + 56.748515348358374, + 56.463990903711384 ], "moduleForcesY": [ - -33.915173903721346, - -35.07698541197515, - -40.656437065616224, - -41.10124961605489 + -35.04253435660417, + -35.99355809852384, + -40.867195346689, + -41.27154173850131 ], - "timestamp": 5.8293463686543205 + "timestamp": 5.70158421830582 }, { - "x": 2.2942772044503497, - "y": 3.3583257141932483, - "heading": -0.7147311185238733, - "angularVelocity": -0.28968074058629395, - "velocityX": -1.1851459278330674, - "velocityY": 0.7754263762078791, + "x": 2.2873528689676483, + "y": 3.3610285743692394, + "heading": -0.7191989284072005, + "angularVelocity": -0.26302035689049935, + "velocityX": -1.1598966634176864, + "velocityY": 0.7715608461595652, "moduleForcesX": [ - 61.13821655127134, - 60.41423668737451, - 56.79179682900103, - 56.40799267466763 + 60.55636984330847, + 59.92996721273085, + 56.64674423048133, + 56.29855167271921 ], "moduleForcesY": [ - -33.958143361421655, - -35.245265434945416, - -40.81102911719036, - -41.35348048483787 + -34.99335531824572, + -36.06824538837232, + -41.01955138323022, + -41.50757259528159 ], - "timestamp": 5.891944751110045 + "timestamp": 5.763141438690921 }, { - "x": 2.2324876517353642, - "y": 3.398831509029573, - "heading": -0.7298786663246292, - "angularVelocity": -0.2419798596468502, - "velocityX": -0.9870790632439912, - "velocityY": 0.6470741454857069, + "x": 2.2278792676988664, + "y": 3.4006487477183196, + "heading": -0.7327874449132579, + "angularVelocity": -0.2207461029112197, + "velocityX": -0.966151507438403, + "velocityY": 0.6436316178868581, "moduleForcesX": [ - 61.11437176518798, - 60.32958782211226, - 56.7120114962976, - 56.26499529096948 + 60.57586928152923, + 59.88760410989669, + 56.562363081650766, + 56.15861493971373 ], "moduleForcesY": [ - -34.01552925312056, - -35.40260151463775, - -40.933861723710784, - -41.55859600904434 + -34.97098554580845, + -36.148822416267066, + -41.14545732873801, + -41.70558761712785 ], - "timestamp": 5.954543133565769 + "timestamp": 5.824698659076022 }, { - "x": 2.1830812470403056, - "y": 3.4312772741579374, - "heading": -0.7420173390092132, - "angularVelocity": -0.19391351994070552, - "velocityX": -0.7892600855941332, - "velocityY": 0.5183163502876942, + "x": 2.180320435943854, + "y": 3.432375929705667, + "heading": -0.7437231910234625, + "angularVelocity": -0.17765172049338343, + "velocityX": -0.7725955047594213, + "velocityY": 0.5154095943394352, "moduleForcesX": [ - 61.08457488728973, - 60.252153365783734, - 56.64434890830058, - 56.14650396699435 + 60.58340632519516, + 59.84490600733586, + 56.48902920756478, + 56.03945201723916 ], "moduleForcesY": [ - -34.08103172001482, - -35.54464933149016, - -41.037463508837504, - -41.72744758205663 + -34.96766125833015, + -36.22817172523544, + -41.25431832732434, + -41.87309909683168 ], - "timestamp": 6.017141516021494 + "timestamp": 5.886255879461123 }, { - "x": 2.146044122527282, - "y": 3.455640363277112, - "heading": -0.7511321714311937, - "angularVelocity": -0.14560811421009826, - "velocityX": -0.5916626446253628, - "velocityY": 0.38919678374128486, + "x": 2.1446656199937997, + "y": 3.4561934671002783, + "heading": -0.7519665199163653, + "angularVelocity": -0.13391327355800717, + "velocityX": -0.5792141965962733, + "velocityY": 0.3869170382549771, "moduleForcesX": [ - 61.050689733359725, - 60.18330523590073, - 56.58339768822803, - 56.04798993012374 + 60.58183660174803, + 59.80457338750152, + 56.42234143923046, + 55.9377862653548 ], "moduleForcesY": [ - -34.15181538875617, - -35.66983583289213, - -41.129938480696744, - -41.867174962927784 + -34.97878115536029, + -36.30217808780585, + -41.352605880404866, + -42.01528814031888 ], - "timestamp": 6.079739898477218 + "timestamp": 5.947813099846224 }, { - "x": 2.1213637716998797, - "y": 3.4719003980089873, - "heading": -0.757213576537006, - "angularVelocity": -0.0971495567016255, - "velocityX": -0.3942649931067891, - "velocityY": 0.2597516755864367, + "x": 2.120904845915518, + "y": 3.472085915141707, + "heading": -0.7574858469261834, + "angularVelocity": -0.08966173221089468, + "velocityX": -0.3859949154564824, + "velocityY": 0.258173581295686, "moduleForcesX": [ - 61.01354989752815, - 60.123568909957186, - 56.52548269604857, - 55.96632136080857 + 60.57287518432625, + 59.76823373322807, + 56.35914684811399, + 55.851272292620784 ], "moduleForcesY": [ - -34.22674949510225, - -35.77786460913943, - -41.2167566576027, - -41.98268691331134 + -35.001611618471976, + -36.36844720693236, + -41.44491977809569, + -42.13584960752861 ], - "timestamp": 6.142338280932942 + "timestamp": 6.009370320231325 }, { - "x": 2.1090288162231445, - "y": 3.48003888130188, - "heading": -0.7602557704725093, - "angularVelocity": -0.04859860296957364, - "velocityX": -0.19704910882417112, - "velocityY": 0.13001107973754503, + "x": 2.109028816223142, + "y": 3.4800388813018817, + "heading": -0.7602557704725089, + "angularVelocity": -0.044997540970748186, + "velocityX": -0.19292667242090425, + "velocityY": 0.12919631702701284, "moduleForcesX": [ - 60.97345110924072, - 60.073045581715306, - 56.46803335710757, - 55.89926318732195 + 60.5575482023407, + 59.73687419838248, + 56.29713329942968, + 55.778188462220434 ], "moduleForcesY": [ - -34.30556428252632, - -35.869027517591256, - -41.3017196129008, - -42.07746912434651 + -35.034541448499795, + -36.42559455322768, + -41.5346063390199, + -42.237487649673916 ], - "timestamp": 6.204936663388667 + "timestamp": 6.070927540616426 }, { - "x": 2.1090288162231445, - "y": 3.48003888130188, - "heading": -0.7602557704725093, - "angularVelocity": -9.209824503951875e-20, - "velocityX": -1.2993679631111057e-18, - "velocityY": -2.0831148734143083e-18, + "x": 2.109028816223143, + "y": 3.4800388813018808, + "heading": -0.760255770472509, + "angularVelocity": -4.7854036756238116e-17, + "velocityX": -4.1684189280839564e-16, + "velocityY": 3.182458036241524e-16, "moduleForcesX": [ - 60.93038441390968, - 60.03160907225733, - 56.409223413469284, - 55.845190143852285 + 60.53644088216914, + 59.71107786211956, + 56.23458372965665, + 55.717252600818924 ], "moduleForcesY": [ - -34.38845870016777, - -35.943896076352594, - -41.38750873505681, - -42.15404676926127 + -35.07667248160167, + -36.472859348504336, + -41.62412921793918, + -42.32221267335896 ], - "timestamp": 6.267535045844391 + "timestamp": 6.1324847610015265 }, { - "x": 2.117794568665661, - "y": 3.4681155240753, - "heading": -0.7530398045067034, - "angularVelocity": 0.11476292191713451, - "velocityX": 0.13941076882464276, - "velocityY": -0.18962940247615553, + "x": 2.1173647314297415, + "y": 3.4676489109609023, + "heading": -0.7574096677464409, + "angularVelocity": 0.045234117251022915, + "velocityX": 0.13248564867185347, + "velocityY": -0.19691818078357895, "moduleForcesX": [ - 48.13501730949312, - 48.836835407072094, - 31.251459327861657, - 36.04401089708163 + 41.520736384271714, + 42.51429487640815, + 35.1511276560287, + 36.81654326527437 ], "moduleForcesY": [ - -50.76492473541808, - -50.1079347694781, - -62.59287500981053, - -59.97406542598307 + -56.31022663437434, + -55.57003233589047, + -60.49287111022869, + -59.49929247213998 ], - "timestamp": 6.330412200288196 + "timestamp": 6.1954041435170275 }, { - "x": 2.1355122480176822, - "y": 3.4444032062204983, - "heading": -0.7387280504849648, - "angularVelocity": 0.22761453103812976, - "velocityX": 0.28178246151161096, - "velocityY": -0.37712135774201583, + "x": 2.1342750283552188, + "y": 3.4430345558261144, + "heading": -0.751664349766151, + "angularVelocity": 0.09131237069712948, + "velocityX": 0.2687613299655501, + "velocityY": -0.39120465190077036, "moduleForcesX": [ - 48.80717190467651, - 49.50131566328889, - 32.41122454849307, - 37.036459214586024 + 42.70173098288214, + 43.611128958075156, + 36.275170342746776, + 37.87745965146702 ], "moduleForcesY": [ - -50.11417943760879, - -49.44755209409215, - -61.99648850314001, - -59.36303177557035 + -55.41582346031395, + -54.70978545558125, + -59.821763435546124, + -58.826195167217094 ], - "timestamp": 6.3932893547319996 + "timestamp": 6.2583235260325285 }, { - "x": 2.162391169686947, - "y": 3.4090581131486557, - "heading": -0.717466130764082, - "angularVelocity": 0.3381501581768513, - "velocityX": 0.42748311222143226, - "velocityY": -0.5621293359169338, + "x": 2.1600269154540004, + "y": 3.4063896499792783, + "heading": -0.7429636060218578, + "angularVelocity": 0.1382839976560426, + "velocityX": 0.4092838497332282, + "velocityY": -0.5824104494002036, "moduleForcesX": [ - 49.51581154621932, - 50.265844017076226, - 33.70048897709995, - 38.19652881261669 + 44.01151222332591, + 44.83593128986605, + 37.54047868266639, + 39.07824731715444 ], "moduleForcesY": [ - -49.40850610951917, - -48.66541972742287, - -61.300931724870296, - -58.619530919336384 + -54.37659249907092, + -53.70636995290632, + -59.03155043231922, + -58.03154166424591 ], - "timestamp": 6.456166509175803 + "timestamp": 6.3212429085480295 }, { - "x": 2.1986681969801216, - "y": 3.3622638578182182, - "heading": -0.6894300597240235, - "angularVelocity": 0.4458864477576762, - "velocityX": 0.576950843499077, - "velocityY": -0.7442171285321014, + "x": 2.1949215222317773, + "y": 3.357943659865962, + "heading": -0.7312478740657462, + "angularVelocity": 0.18620227166446401, + "velocityX": 0.5545923272400236, + "velocityY": -0.7699692555212343, "moduleForcesX": [ - 50.28381428848592, - 51.13744068398159, - 35.15534244893406, - 39.540816535356164 + 45.47389898312857, + 46.209380246970596, + 38.97509187538921, + 40.44329438249183 ], "moduleForcesY": [ - -48.620147379296554, - -47.74309558757011, - -60.4734352958337, - -57.716863658050855 + -53.154044492191325, + -52.52435244840178, + -58.08923297674164, + -57.08406886084209 ], - "timestamp": 6.519043663619607 + "timestamp": 6.3841622910635305 }, { - "x": 2.2446132442928994, - "y": 3.304239125606535, - "heading": -0.6548330373393313, - "angularVelocity": 0.550231999057986, - "velocityX": 0.7307113007768327, - "velocityY": -0.922826942869088, + "x": 2.2393001625829205, + "y": 3.297971653928642, + "heading": -0.7164541055664759, + "angularVelocity": 0.23512259510216332, + "velocityX": 0.7053254271878769, + "velocityY": -0.9531563015982434, "moduleForcesX": [ - 51.13905740321851, - 52.12476715621361, - 36.82116804073696, - 41.09052851792727 + 47.11716302884608, + 47.75573925943408, + 40.613851617327796, + 42.00243934560986 ], "moduleForcesY": [ - -47.71212916324656, - -46.6564142258722, - -59.46813562411414, - -56.61877073126656 + -51.69634105716302, + -51.116592706220615, + -56.94950428119139, + -55.94148520473914 ], - "timestamp": 6.581920818063411 + "timestamp": 6.4470816735790315 }, { - "x": 2.3005361732090566, - "y": 3.235248258796662, - "heading": -0.6139347954947619, - "angularVelocity": 0.650446767293226, - "velocityX": 0.8893998052366954, - "velocityY": -1.0972326502391698, + "x": 2.293551835832764, + "y": 3.226807987294763, + "heading": -0.6985158525170084, + "angularVelocity": 0.28509900021743295, + "velocityX": 0.862241030996735, + "velocityY": -1.1310293233781603, "moduleForcesX": [ - 52.11606824369092, - 53.23896165126432, - 38.75432059140504, - 42.87287498290715 + 48.974244926718995, + 49.50301706537962, + 42.49983724647496, + 43.79203201526126 ], "moduleForcesY": [ - -46.633926917902656, - -45.372711713166105, - -58.220079562306445, - -55.27504395126715 + -49.932591819303816, + -49.41939613494972, + -55.54905272250912, + -54.54558483413394 ], - "timestamp": 6.644797972507215 + "timestamp": 6.5100010560945325 }, { - "x": 2.3667954166739076, - "y": 3.155616243498738, - "heading": -0.5670546584241174, - "angularVelocity": 0.7455829940991222, - "velocityX": 1.0537888371533939, - "velocityY": -1.2664697695423583, + "x": 2.3581219992537226, + "y": 3.144865400496988, + "heading": -0.6773637507894199, + "angularVelocity": 0.3361778339508891, + "velocityX": 1.0262364447879162, + "velocityY": -1.302342513892083, "moduleForcesX": [ - 53.25761026058277, - 54.49467377205829, - 41.023942389693836, - 44.92291476717264 + 51.081995362995904, + 51.48237998253363, + 44.68553797550306, + 45.85573965609478 ], "moduleForcesY": [ - -45.315096693303126, - -43.846496735504985, - -56.635631853416726, - -53.61440385265008 + -47.76441461323329, + -47.34537980089634, + -53.79777393977218, + -52.8148137116725 ], - "timestamp": 6.707675126951019 + "timestamp": 6.5729204386100335 }, { - "x": 2.4438086218512485, - "y": 3.065750442430627, - "heading": -0.5145902034841154, - "angularVelocity": 0.8343961396486453, - "velocityX": 1.2248201410922746, - "velocityY": -1.4292281809353713, + "x": 2.433522291274826, + "y": 3.0526620913362525, + "heading": -0.652926734850225, + "angularVelocity": 0.3883861373428849, + "velocityX": 1.1983635090908211, + "velocityY": -1.46541980347662, "moduleForcesX": [ - 54.61557385722636, - 55.91123819153664, - 43.7131279622274, - 47.285771912845235 + 53.47798671720776, + 53.72572515007777, + 47.232726000587874, + 48.24428581121922 ], "moduleForcesY": [ - -43.65585603926645, - -42.012507491987115, - -54.57686147012399, - -51.53256053951273 + -45.05355867588098, + -44.773023132097414, + -51.56510924673164, + -50.632864772568915 ], - "timestamp": 6.7705522813948225 + "timestamp": 6.6358398211255345 }, { - "x": 2.532065289837757, - "y": 2.9661728933690776, - "heading": -0.45704452569034076, - "angularVelocity": 0.9152080481823572, - "velocityX": 1.4036364839854023, - "velocityY": -1.583684089116122, + "x": 2.520339981100692, + "y": 2.9508605578412346, + "heading": -0.6251343348430549, + "angularVelocity": 0.4417144430863272, + "velocityX": 1.3798242505713987, + "velocityY": -1.6179677775753416, "moduleForcesX": [ - 56.24963874793976, - 57.513648109663556, - 46.9169797801361, - 50.01853513267831 + 56.19197506453079, + 56.259276660576184, + 50.20804830120448, + 51.011888473748435 ], "moduleForcesY": [ - -41.51321004507581, - -39.77429977030871, - -51.835917665532385, - -48.87154151079314 + -41.604044996750225, + -41.53144010163077, + -48.65926448291237, + -47.83146963835682 ], - "timestamp": 6.833429435838626 + "timestamp": 6.6987592036410355 }, { - "x": 2.6321403093335114, - "y": 2.8575694307170187, - "heading": -0.3950667539653817, - "angularVelocity": 0.985696192411999, - "velocityX": 1.591595872634394, - "velocityY": -1.727232468020187, + "x": 2.6192438141173406, + "y": 2.8403235512778635, + "heading": -0.5939211843905473, + "angularVelocity": 0.4960816397843361, + "velocityX": 1.5719135989976136, + "velocityY": -1.7568037406619357, "moduleForcesX": [ - 58.220168914381084, - 59.332124325032076, - 50.73064435062025, - 53.18908968161254 + 59.2250417610776, + 59.087826967496994, + 53.66857872752665, + 54.204993983215175 ], "moduleForcesY": [ - -38.68115242041676, - -36.98517279536072, - -48.09349009166419, - -45.382842135502834 + -37.1392249820516, + -37.38030580647965, + -44.79597130253933, + -44.16473609829124 ], - "timestamp": 6.89630659028243 + "timestamp": 6.7616785861565365 }, { - "x": 2.7447041232794764, - "y": 2.740865636486064, - "heading": -0.32951256744957347, - "angularVelocity": 1.0425755919727022, - "velocityX": 1.7902180043240945, - "velocityY": -1.8560603650608418, + "x": 2.7309783267356824, + "y": 2.7221935960005026, + "heading": -0.5592365908276438, + "angularVelocity": 0.5512545129373857, + "velocityX": 1.7758361279342616, + "velocityY": -1.8774811600901864, "moduleForcesX": [ - 60.5677211979897, - 61.39663828373237, - 55.20909444556158, - 56.86245394897771 + 62.5038695327194, + 62.160031630846326, + 57.62319015145175, + 57.832996172310274 ], "moduleForcesY": [ - -34.86502705000924, - -33.416146228705585, - -42.85575978754388, - -40.660621351301806 + -31.280112248684674, + -31.988407393096097, + -39.55612203914035, + -39.27379472999048 ], - "timestamp": 6.959183744726234 + "timestamp": 6.8245979686720375 }, { - "x": 2.870516828572632, - "y": 2.6173440746550423, - "heading": -0.26153269734842044, - "angularVelocity": 1.081153730675094, - "velocityX": 2.0009287380458565, - "velocityY": -1.9644903291770046, + "x": 2.856329605423656, + "y": 2.598000279152458, + "heading": -0.5210635851729407, + "angularVelocity": 0.606697080113568, + "velocityX": 1.9922522071333428, + "velocityY": -1.9738483100568995, "moduleForcesX": [ - 63.264661428716025, - 63.71631261983922, - 60.25491380232913, - 61.043986331952446 + 65.79278778008134, + 65.30020556417972, + 61.939549113121835, + 61.79878070206867 ], "moduleForcesY": [ - -29.65704803224665, - -28.70294866760673, - -35.37956356936107, - -34.023166255951196 + -23.548304579936264, + -24.925035881299397, + -32.34851276785782, + -32.65108476149218 ], - "timestamp": 7.022060899170038 + "timestamp": 6.8875173511875385 }, { - "x": 3.0103732419935905, - "y": 2.488816613373573, - "heading": -0.1926918880041953, - "angularVelocity": 1.094846132163167, - "velocityX": 2.224280259787407, - "velocityY": -2.0441042922249113, + "x": 2.996031354964205, + "y": 2.46978535540666, + "heading": -0.47945305051497955, + "angularVelocity": 0.6613309443669427, + "velocityX": 2.220329315948571, + "velocityY": -2.0377651308674327, "moduleForcesX": [ - 66.11806816346805, - 66.21721889876862, - 65.35371655844693, - 65.48547608931766 + 68.5564837925553, + 68.09308895613952, + 66.1567792079864, + 65.75590846714256 ], "moduleForcesY": [ - -22.54079142004972, - -22.263089069440703, - -24.6715805356649, - -24.33345121959331 + -13.459642197003902, + -15.709687602642282, + -22.445832125017436, + -23.647305076742175 ], - "timestamp": 7.0849380536138415 + "timestamp": 6.9504367337030395 }, { - "x": 3.1649283508936707, - "y": 2.3578354928773337, - "heading": -0.12509625596484528, - "angularVelocity": 1.075042797933275, - "velocityX": 2.458048718445309, - "velocityY": -2.0831273561099635, + "x": 3.1505724071472994, + "y": 2.340193763604278, + "heading": -0.43457360660126365, + "angularVelocity": 0.7132848753348665, + "velocityX": 2.456175601294578, + "velocityY": -2.059645003198423, "moduleForcesX": [ - 68.61737088139724, - 68.5714962678954, - 69.13779393676624, - 69.12206655275224 + 69.84706521344005, + 69.75290941394955, + 69.22551312159781, + 68.88502793630397 ], "moduleForcesY": [ - -13.001403954681635, - -13.198507429125556, - -9.864173290779956, - -9.916683464178158 + -0.8233024041036456, + -4.014815042921486, + -9.277060723605338, + -11.648512019585809 ], - "timestamp": 7.147815208057645 + "timestamp": 7.0133561162185405 }, { - "x": 3.3343139997886584, - "y": 2.227807798534294, - "heading": -0.061425277024888265, - "angularVelocity": 1.0126250067003624, - "velocityX": 2.6939140359218023, - "velocityY": -2.067964040250105, + "x": 3.3199095005462036, + "y": 2.212409184758706, + "heading": -0.3867586308994902, + "angularVelocity": 0.7599403202978621, + "velocityX": 2.691334317484535, + "velocityY": -2.03092550716134, "moduleForcesX": [ - 69.81884664866837, - 69.79428970681542, - 69.29948552018702, - 69.00682870384438 + 68.47925044891942, + 69.15340266140203, + 69.49958829473019, + 69.7686571831701 ], "moduleForcesY": [ - -0.8612358805856215, - -0.3297910827818419, - 8.567864185381797, - 10.490055875380158 + 13.736045870694008, + 9.89537651518505, + 6.815502213890215, + 3.370466430920167 ], - "timestamp": 7.210692362501449 + "timestamp": 7.0762754987340415 }, { - "x": 3.5176404379599324, - "y": 2.1026847463287157, - "heading": -0.004525789100840003, - "angularVelocity": 0.9049310266561369, - "velocityX": 2.9156287334077797, - "velocityY": -1.9899604763031347, + "x": 3.5032276259952, + "y": 2.089811754517775, + "heading": -0.3365088719529981, + "angularVelocity": 0.7986371915540097, + "velocityX": 2.9135398047467214, + "velocityY": -1.9484843197678885, "moduleForcesX": [ - 68.55777812799681, - 67.6445550813489, - 64.16468459069685, - 60.8787964212339 + 63.762980248590615, + 65.33381311946748, + 65.64375774359121, + 66.90783722378835 ], "moduleForcesY": [ - 13.196294200522606, - 17.05028282885496, - 27.549743552264584, - 34.11506286093019 + 28.499981635594498, + 24.717827613930762, + 23.81826750682116, + 20.038950866645536 ], - "timestamp": 7.273569516945253 + "timestamp": 7.1391948812495425 }, { - "x": 3.712921078466066, - "y": 1.986111498522952, - "heading": 0.04367317548530425, - "angularVelocity": 0.7665576633119034, - "velocityX": 3.105748697337491, - "velocityY": -1.8539841511108714, + "x": 3.698983205941932, + "y": 1.9754559392748747, + "heading": -0.2844250046592506, + "angularVelocity": 0.8277873242146967, + "velocityX": 3.111212668028096, + "velocityY": -1.8174974176634262, "moduleForcesX": [ - 64.1546735583916, - 59.222021183604404, - 55.03127871584201, - 45.60985368543614 + 56.27400205759313, + 58.363284645856304, + 58.070669393657745, + 60.053109943523204 ], "moduleForcesY": [ - 27.543396744432503, - 36.83392520078611, - 43.0063108958907, - 52.83689467676779 + 41.37894474295054, + 38.388062514062604, + 38.80031879570978, + 35.67059242066261 ], - "timestamp": 7.336446671389057 + "timestamp": 7.2021142637650435 }, { - "x": 3.917974347119227, - "y": 1.880569748380944, - "heading": 0.0840407650336414, - "angularVelocity": 0.6420072585252761, - "velocityX": 3.261172845161521, - "velocityY": -1.678538907741677, + "x": 3.9052468507649114, + "y": 1.8717029681737345, + "heading": -0.23110382341189709, + "angularVelocity": 0.8474523924995162, + "velocityX": 3.2782210596577848, + "velocityY": -1.6489826656448716, "moduleForcesX": [ - 56.176200766123785, - 46.718354113326974, - 45.97750919549416, - 34.263779007829406 + 47.55707312914185, + 49.53638736807368, + 48.78953373366416, + 50.77045950153568 ], "moduleForcesY": [ - 41.47332187047582, - 51.820224724822296, - 52.59176748493925, - 60.84133045416032 + 51.17696939290848, + 49.268536466648314, + 49.99446973782349, + 47.98722716731246 ], - "timestamp": 7.399323825832861 + "timestamp": 7.2650336462805445 }, { - "x": 4.131094513594558, - "y": 1.787487140153957, - "heading": 0.11947354396561721, - "angularVelocity": 0.5635238942570701, - "velocityX": 3.3894690108122587, - "velocityY": -1.4803883707901997, + "x": 4.120080601641824, + "y": 1.7801799153533593, + "heading": -0.17706327947410902, + "angularVelocity": 0.8588854781668361, + "velocityX": 3.4144287862962694, + "velocityY": -1.454608248862383, "moduleForcesX": [ - 45.44840572457847, - 37.20679327325788, - 38.51657585799193, - 29.99924193711389 + 39.05701336414032, + 40.46649013860068, + 39.71798936608046, + 41.14398002340263 ], "moduleForcesY": [ - 53.034304370620895, - 59.079362573109876, - 58.2883307755835, - 63.07823138252855 + 57.94278647291951, + 56.96920228244361, + 57.48746696114792, + 56.477668387844396 ], - "timestamp": 7.4622009802766645 + "timestamp": 7.3279530287960455 }, { - "x": 4.350770411068689, - "y": 1.7077754935808809, - "heading": 0.1521039131255066, - "angularVelocity": 0.5189542918812017, - "velocityX": 3.493731537588328, - "velocityY": -1.2677362275406037, + "x": 4.341754671827057, + "y": 1.7019368196307447, + "heading": -0.12271990769558694, + "angularVelocity": 0.8636984281454757, + "velocityX": 3.523144400386022, + "velocityY": -1.243545193777744, "moduleForcesX": [ - 35.38842380663489, - 29.894307911171417, - 31.489034487421396, - 26.080480439830673 + 31.542569411645978, + 32.211867496766956, + 31.792298515347188, + 32.46659837801001 ], "moduleForcesY": [ - 60.23788124052505, - 63.133637641285205, - 62.37825455380923, - 64.81765821899063 + 62.36899517370198, + 62.02650340460398, + 62.24055631529311, + 61.8920479920422 ], - "timestamp": 7.525078134720468 + "timestamp": 7.3908724113115465 }, { - "x": 4.575688712037729, - "y": 1.6420223189913672, - "heading": 0.18322659040704492, - "angularVelocity": 0.49497591862802676, - "velocityX": 3.5771068674880655, - "velocityY": -1.0457403037899842, + "x": 4.568810602625837, + "y": 1.6376320022464737, + "heading": -0.06839919997587875, + "angularVelocity": 0.8633382202427946, + "velocityX": 3.6086802146038512, + "velocityY": -1.0220192063777647, "moduleForcesX": [ - 27.158137877972838, - 23.927370915342486, - 25.160312496417824, - 21.995099103025833 + 25.215137722620277, + 25.161326804862117, + 25.198140713892162, + 25.144365342135142 ], "moduleForcesY": [ - 64.38875092826233, - 65.65351312146176, - 65.20196768922268, - 66.33295061137039 + 65.19836347228741, + 65.21911954217529, + 65.20503178610034, + 65.2257580831436 ], - "timestamp": 7.587955289164272 + "timestamp": 7.4537917938270475 }, { - "x": 4.804716248215296, - "y": 1.5906097298688888, - "heading": 0.21347571274604327, - "angularVelocity": 0.48108287670736033, - "velocityX": 3.6424602576800607, - "velocityY": -0.8176672366499624, + "x": 4.800045847269579, + "y": 1.5876743304432248, + "heading": -0.014355888196568429, + "angularVelocity": 0.8589294684510949, + "velocityX": 3.675103527705061, + "velocityY": -0.793994947279417, "moduleForcesX": [ - 20.777362325555703, - 18.79970144179875, - 19.686842590719646, - 17.74181237154826 + 19.993448516674185, + 19.3060989973268, + 19.797835730589334, + 19.116494526678093 ], "moduleForcesY": [ - 66.73767220647805, - 67.31957624630218, - 67.070927016661, - 67.60968811178219 + 66.99591754938321, + 67.196927406319, + 67.05502467711538, + 67.25215990996023 ], - "timestamp": 7.650832443608076 + "timestamp": 7.5167111763425485 }, { - "x": 5.036866817289369, - "y": 1.55378761076971, - "heading": 0.24298831748702848, - "angularVelocity": 0.469369280496969, - "velocityX": 3.692129059077492, - "velocityY": -0.5856199986290461, + "x": 5.034473115872903, + "y": 1.5523169276578892, + "heading": 0.039207016322208126, + "angularVelocity": 0.8512941859462895, + "velocityX": 3.725835493467699, + "velocityY": -0.5619477078724477, "moduleForcesX": [ - 15.925448739090935, - 14.192665989132097, - 15.055141930627649, - 13.351354517579729 + 15.705471617855421, + 14.48469807799971, + 15.374313004019221, + 14.172732804713208 ], "moduleForcesY": [ - 68.07164789067083, - 68.45258203067056, - 68.27203655365994, - 68.62435165252099 + 68.13853193873344, + 68.40802253366346, + 68.21561194224486, + 68.47494827858137 ], - "timestamp": 7.71370959805188 + "timestamp": 7.5796305588580495 }, { "x": 5.271278381347656, "y": 1.5317155122756958, - "heading": 0.2715710097371584, - "angularVelocity": 0.45457992657214513, - "velocityX": 3.7280879857212588, - "velocityY": -0.3510352637497425, + "heading": 0.09212323355733296, + "angularVelocity": 0.8410161562232161, + "velocityX": 3.7636298388086282, + "velocityY": -0.3274255810936793, "moduleForcesX": [ - 12.231894678034537, - 9.978543755139432, - 11.179968921219166, - 8.979896356158925 + 12.17583308633574, + 10.508988015592593, + 11.72461754628835, + 10.093645719719404 ], "moduleForcesY": [ - 68.84172245876557, - 69.20285737211027, - 69.02334612966767, - 69.34261769004998 + 68.86508793588061, + 69.1385750114367, + 68.94526551565065, + 69.20238200518273 ], - "timestamp": 7.7765867524956835 + "timestamp": 7.6425499413735505 }, { - "x": 5.388703997390188, - "y": 1.5243911275820383, - "heading": 0.2855249380175652, - "angularVelocity": 0.4444472090652179, - "velocityX": 3.7401286773236566, - "velocityY": -0.2332893125004083, + "x": 5.388681610311286, + "y": 1.5251584282304298, + "heading": 0.11807334458511697, + "angularVelocity": 0.8348404097837044, + "velocityX": 3.7769765097724286, + "velocityY": -0.21094779615682893, "moduleForcesX": [ - 9.343299160452919, - 6.163463777084662, - 7.986565873923746, - 4.920049004946797 + 9.224601000293218, + 7.170145322617407, + 8.707960432775863, + 6.708850729084163 ], "moduleForcesY": [ - 69.21664100426783, - 69.5686586634018, - 69.39371949997597, - 69.67546873681157 + 69.25444608627201, + 69.49615053644614, + 69.32560078879362, + 69.54657168705602 ], - "timestamp": 7.807982895830131 + "timestamp": 7.673633859597523 }, { - "x": 5.50644545946061, - "y": 1.5207685343702697, - "heading": 0.2991263551425099, - "angularVelocity": 0.43321936009960066, - "velocityX": 3.7501887036309256, - "velocityY": -0.11538338238486952, + "x": 5.506442673711366, + "y": 1.5222273963184734, + "heading": 0.14379764901773748, + "angularVelocity": 0.8275759911368509, + "velocityX": 3.7884883929871296, + "velocityY": -0.0942941585046398, "moduleForcesX": [ - 8.420939086179335, - 4.879883420406134, - 6.919098093280834, - 3.519523756844695 + 8.369389483724206, + 5.949973696893992, + 7.73117703857559, + 5.38783266312984 ], "moduleForcesY": [ - 69.32400261131451, - 69.65903960081413, - 69.4987054257006, - 69.75025512359892 + 69.35385904945188, + 69.60180730909208, + 69.43322409042021, + 69.65302052985173 ], - "timestamp": 7.839379039164578 + "timestamp": 7.704717777821495 }, { - "x": 5.624430269287079, - "y": 1.5208521505351245, - "heading": 0.31232341935960334, - "angularVelocity": 0.4203402971031069, - "velocityX": 3.757939584159604, - "velocityY": 0.002663262298303611, + "x": 5.624497338635267, + "y": 1.522927459386341, + "heading": 0.16925769890783504, + "angularVelocity": 0.8190746644823769, + "velocityX": 3.7979338406848053, + "velocityY": 0.02252171244382533, "moduleForcesX": [ - 7.427158187557407, - 3.3353613300752003, - 5.7144874128058545, - 1.8133624389873595 + 7.409211713616261, + 4.57253345050813, + 6.632095741425086, + 3.899218959272906 ], "moduleForcesY": [ - 69.4251012893418, - 69.73687602026827, - 69.59766681374457, - 69.80441399722511 + 69.45271134204867, + 69.69519778275762, + 69.53780580291625, + 69.74287628352383 ], - "timestamp": 7.870775182499026 + "timestamp": 7.735801696045467 }, { - "x": 5.742576714793087, - "y": 1.5246451742965244, - "heading": 0.3250572744698019, - "angularVelocity": 0.4055866026139322, - "velocityX": 3.7630878495951756, - "velocityY": 0.12081177363075021, + "x": 5.742772793309857, + "y": 1.5272629148189605, + "heading": 0.19440986357494344, + "angularVelocity": 0.8091696962357507, + "velocityX": 3.8050368625463413, + "velocityY": 0.13947583446143447, "moduleForcesX": [ - 6.306930660430928, - 1.5778924322604664, - 4.3676425070670675, - -0.10369410469570728 + 6.320859466741637, + 3.0067323445032117, + 5.3877760442274845, + 2.2145608793254503 ], "moduleForcesY": [ - 69.52171601095, - 69.78371617293728, - 69.68348163684516, - 69.815527135674 + 69.54854425346966, + 69.76816296129176, + 69.63518331243104, + 69.80621969634359 ], - "timestamp": 7.902171325833473 + "timestamp": 7.766885614269439 }, { - "x": 5.860794105940623, - "y": 1.5321490223767453, - "heading": 0.33726992474980966, - "angularVelocity": 0.3889856836845344, - "velocityX": 3.7653475424743803, - "velocityY": 0.2390054090493234, + "x": 5.861185901308817, + "y": 1.5352368348891807, + "heading": 0.21920461001465913, + "angularVelocity": 0.7976712028728045, + "velocityX": 3.8094653043977926, + "velocityY": 0.2565287944964038, "moduleForcesX": [ - 5.00586577439091, - -0.3668746684666077, - 2.864339879121958, - -2.17095393844088 + 5.0743871573767105, + 1.212540305184361, + 3.969650176486514, + 0.29853628468642324 ], "moduleForcesY": [ - 69.611426474232, - 69.78350871775525, - 69.74815431030521, - 69.76783434978745 + 69.63726174457908, + 69.80849842797434, + 69.71904021769083, + 69.82888852399873 ], - "timestamp": 7.9335674691679205 + "timestamp": 7.797969532493411 }, { - "x": 5.9789826775060435, - "y": 1.5433627560426824, - "heading": 0.3489121084818783, - "angularVelocity": 0.37081572752583847, - "velocityX": 3.764429608643845, - "velocityY": 0.3571691448367622, + "x": 5.979640967451708, + "y": 1.5468503445815898, + "heading": 0.24358557158644084, + "angularVelocity": 0.7843593396465376, + "velocityX": 3.810815138856516, + "velocityY": 0.3736179463840169, "moduleForcesX": [ - 3.4623816637274447, - -2.475568668701702, - 1.1828304235517055, - -4.335764900525163 + 3.6307956438747206, + -0.8621551207472431, + 2.3418002525812245, + -1.893133883598176 ], "moduleForcesY": [ - 69.68689216287859, - 69.72122437760291, - 69.7815156598602, - 69.65073511362182 + 69.7120826368421, + 69.79764527340737, + 69.77979201753804, + 69.79043163606336 ], - "timestamp": 7.964963612502368 + "timestamp": 7.8290534507173835 }, { - "x": 6.097033194516696, - "y": 1.5582824506328423, - "heading": 0.3599519448622258, - "angularVelocity": 0.35163033442501535, - "velocityX": 3.7600324266938894, - "velocityY": 0.47520787605114534, + "x": 6.098026846695146, + "y": 1.5621015192437462, + "heading": 0.2674883023191033, + "angularVelocity": 0.7689741866013711, + "velocityX": 3.808589328746145, + "velocityY": 0.4906451803233273, "moduleForcesX": [ - 1.6038425315720386, - -4.721980038884405, - -0.7091635039964642, - -6.549079033729455 + 1.9386808198061933, + -3.28598819102792, + 0.4585755906663063, + -4.416447800806846 ], "moduleForcesY": [ - 69.73338633060467, - 69.58337205904242, - 69.76976708048174, - 69.45885860862715 + 69.76178277329687, + 69.70685348700545, + 69.80282869693546, + 69.6609062566512 ], - "timestamp": 7.996359755836815 + "timestamp": 7.860137368941356 }, { - "x": 6.214826085217277, - "y": 1.5769004197748018, - "heading": 0.37038445324917163, - "angularVelocity": 0.3322863026777996, - "velocityX": 3.7518267592867267, - "velocityY": 0.5930017882652427, + "x": 6.216213161429717, + "y": 1.5809836737297318, + "heading": 0.2908385525285553, + "angularVelocity": 0.7512003487206494, + "velocityX": 3.8021691436385594, + "velocityY": 0.6074573465909969, "moduleForcesX": [ - -0.6554876781322178, - -7.078557663607612, - -2.860110163308246, - -8.769410640028871 + -0.07065192727705566, + -6.150375646776657, + -1.738667342074545, + -7.342704323857981 ], "moduleForcesY": [ - 69.7245742748119, - 69.35870703983915, - 69.69292267797944, - 69.1914612156941 + 69.76766630955797, + 69.49064187075653, + 69.76568210247977, + 69.39577088637856 ], - "timestamp": 8.027755899171263 + "timestamp": 7.891221287165328 }, { - "x": 6.332229852804532, - "y": 1.5992041212873251, - "heading": 0.3802420776714249, - "angularVelocity": 0.31397564717566995, - "velocityX": 3.7394327811735093, - "velocityY": 0.7103962188901063, + "x": 6.334045309853754, + "y": 1.6034826557006305, + "heading": 0.313549807262191, + "angularVelocity": 0.7306432403402956, + "velocityX": 3.790775267616168, + "velocityY": 0.7238142182972129, "moduleForcesX": [ - -3.4164906317976134, - -9.51983860988743, - -5.342993033888524, - -10.967733824511571 + -2.490383343681562, + -9.578064211771972, + -4.3259113321238125, + -10.762745909847968 ], "moduleForcesY": [ - 69.61603638629833, - 69.03834092530526, - 69.5200097527037, - 68.85058965896839 + 69.69820129934999, + 69.07536900988754, + 69.63337299442767, + 68.9276312183353 ], - "timestamp": 8.05915204250571 + "timestamp": 7.9223052053893 }, { - "x": 6.449098535030912, - "y": 1.625174456079489, - "heading": 0.3896069207325694, - "angularVelocity": 0.29828004546244535, - "velocityX": 3.722389752825253, - "velocityY": 0.827182323494794, + "x": 6.451337870714877, + "y": 1.6295724757424068, + "heading": 0.33551971883232135, + "angularVelocity": 0.706793506913393, + "velocityX": 3.7734162088570447, + "velocityY": 0.8393349851774983, "moduleForcesX": [ - -6.797799685550791, - -12.023870431612343, - -8.267497900609209, - -13.12862175589208 + -5.448866729736701, + -13.733572282788284, + -7.401927508185858, + -14.790647300726173 ], "moduleForcesY": [ - 69.33533463241886, - 68.61540564974591, - 69.19995880316955, - 68.43876233553452 + 69.49922387521676, + 68.33891344443161, + 69.35066212276465, + 68.15293380811582 ], - "timestamp": 8.090548185840158 + "timestamp": 7.953389123613272 }, { - "x": 6.5652678833030125, - "y": 1.6547829667795249, - "heading": 0.39862574645972415, - "angularVelocity": 0.287259031502107, - "velocityX": 3.7001152350021482, - "velocityY": 0.9430620310472863, + "x": 6.567866022044423, + "y": 1.659208124359565, + "heading": 0.35662502542728386, + "angularVelocity": 0.6789783206509007, + "velocityX": 3.7488244078469917, + "velocityY": 0.9534077526398532, "moduleForcesX": [ - -10.936219914888005, - -14.574762118658219, - -11.799686531958358, - -15.252283089212833 + -9.125353794114547, + -18.83172539656305, + -11.094164589642684, + -19.562885061791146 ], "moduleForcesY": [ - 68.76683316861762, - 68.08405084461447, - 68.64363965268268, - 67.95604052661037 + 69.0754447164273, + 67.07463735744341, + 68.82909121269832, + 66.91127860350836 ], - "timestamp": 8.121944329174605 + "timestamp": 7.984473041837244 }, { - "x": 6.6805489880212905, - "y": 1.6879868491141017, - "heading": 0.40752627429499416, - "angularVelocity": 0.28349111992696274, - "velocityX": 3.671823748867664, - "velocityY": 1.057578377728512, + "x": 6.683354928219009, + "y": 1.6923136502791418, + "heading": 0.37671482456009425, + "angularVelocity": 0.6463084540390093, + "velocityX": 3.7153908764796526, + "velocityY": 1.0650370934911797, "moduleForcesX": [ - -15.976487504436525, - -17.19633335319417, - -16.19563370942626, - -17.393215864457243 + -13.771593248917489, + -25.130414558467773, + -15.562652582080137, + -25.223578565092154 ], "moduleForcesY": [ - 67.73039056976316, - 67.42967661497167, - 67.68616175746094, - 67.38710414645385 + 68.25443383699609, + 64.93026638806907, + 67.92556316929904, + 64.95632378013195 ], - "timestamp": 8.153340472509052 + "timestamp": 8.015556960061216 }, { - "x": 6.794712373597259, - "y": 1.7247180908804576, - "heading": 0.4166128430796408, - "angularVelocity": 0.28941671873044894, - "velocityX": 3.6362232252491316, - "velocityY": 1.1699284646230703, + "x": 6.7974682626271505, + "y": 1.7287625418402501, + "heading": 0.3956032132276822, + "angularVelocity": 0.6076579063002795, + "velocityX": 3.671137389627324, + "velocityY": 1.1725964306841732, "moduleForcesX": [ - -22.036733716797762, - -20.18398244295977, - -21.84839807752641, - -19.94027338243488 + -19.7338671115633, + -32.869545661536705, + -20.995212284811174, + -31.878789101972526 ], "moduleForcesY": [ - 65.95765425448234, - 66.54948915551118, - 66.00569479257204, - 66.6085993405814 + 66.71548826157209, + 61.31804473588162, + 66.4083595377785, + 61.9239118979002 ], - "timestamp": 8.1847366158435 + "timestamp": 8.046640878285189 }, { - "x": 6.907432636307125, - "y": 1.7648507330806484, - "heading": 0.4261265032765866, - "angularVelocity": 0.30302002687404705, - "velocityX": 3.590258252713168, - "velocityY": 1.278266625702327, + "x": 6.909800194013484, + "y": 1.7683468561362488, + "heading": 0.41306566672219286, + "angularVelocity": 0.5617841794810646, + "velocityX": 3.6138279150310604, + "velocityY": 1.2734660415324053, "moduleForcesX": [ - -29.201525856972616, - -25.15437815757134, - -29.219412722371683, - -24.891885489663608 + -27.444972321937637, + -42.075345805688336, + -27.57805811433052, + -39.49775701748281 ], "moduleForcesY": [ - 63.049097714214916, - 64.77103172702344, - 63.0005849187992, - 64.8334626863064 + 63.85363325543449, + 55.33672689274484, + 63.908626512967174, + 57.321971693817275 ], - "timestamp": 8.216132759177947 + "timestamp": 8.07772479650916 }, { - "x": 7.018202438708799, - "y": 1.808117637629141, - "heading": 0.4359499136905104, - "angularVelocity": 0.31288589522859334, - "velocityX": 3.5281340520616546, - "velocityY": 1.3780961593782959, + "x": 7.019881390468804, + "y": 1.810734119873736, + "heading": 0.42885115065317647, + "angularVelocity": 0.5078344312078966, + "velocityX": 3.5414195746540194, + "velocityY": 1.363639661900725, "moduleForcesX": [ - -37.84606166215224, - -35.12657257182429, - -38.23355604485398, - -35.39322800560654 + -37.27804152080681, + -52.1405235304012, + -35.411432044620646, + -47.75401943748761 ], "moduleForcesY": [ - 58.175724092585945, - 59.8531774353204, - 57.88611968366453, - 59.66067088624069 + 58.55809505279019, + 45.896730636123046, + 59.86837524976089, + 50.604050638682544 ], - "timestamp": 8.247528902512395 + "timestamp": 8.108808714733133 }, { - "x": 7.126380703050267, - "y": 1.854009244621213, - "heading": 0.4456272830552769, - "angularVelocity": 0.3082343350798942, - "velocityX": 3.4455908545549447, - "velocityY": 1.461695677179563, + "x": 7.127216489228573, + "y": 1.8554226010135964, + "heading": 0.44272434085459006, + "angularVelocity": 0.4463140747396045, + "velocityX": 3.4530749304632886, + "velocityY": 1.4376720726731453, "moduleForcesX": [ - -48.349849951021866, - -49.399500416665845, - -47.99842192068982, - -49.03597721530752 + -48.97728499858189, + -61.38705872437014, + -44.32215534808062, + -55.881419683225104 ], "moduleForcesY": [ - 49.65036150580679, - 48.61361186301433, - 50.011197849600876, - 49.001261315065825 + 49.06528866999657, + 32.434718317404084, + 53.54352333978887, + 41.41142967075846 ], - "timestamp": 8.278925045846842 + "timestamp": 8.139892632957105 }, { "x": 7.231364727020264, "y": 1.90172815322876, "heading": 0.45451541638849, - "angularVelocity": 0.28309634207400236, - "velocityX": 3.343850958114625, - "velocityY": 1.5198971446658642, + "angularVelocity": 0.3793304128823317, + "velocityX": 3.3505505014284465, + "velocityY": 1.489694828094504, "moduleForcesX": [ - -59.49373787912882, - -62.82905712054162, - -57.10726545680866, - -60.65366662655575 + -60.49418247701689, + -67.53589230331421, + -53.57894617228146, + -62.756167691165814 ], "moduleForcesY": [ - 35.35799304336133, - 29.157332713084195, - 39.2405425695758, - 33.58676445334213 + 33.72227739407681, + 16.098766492768572, + 44.212603887497394, + 29.961682528729266 ], - "timestamp": 8.31032118918129 + "timestamp": 8.170976551181077 }, { - "x": 7.423388079224838, - "y": 1.9985849999652825, - "heading": 0.46645537399184306, - "angularVelocity": 0.19401513991291586, - "velocityX": 3.1202319792203808, - "velocityY": 1.5738493632366841, + "x": 7.4238027530368695, + "y": 1.9965622542854489, + "heading": 0.4692934758689833, + "angularVelocity": 0.24023905191549066, + "velocityX": 3.1283626232349375, + "velocityY": 1.5416675347118225, "moduleForcesX": [ - -67.89958925754117, - -69.39640044262892, - -64.37618878024341, - -67.53660965026158 + -68.24273916485633, + -69.7323377330834, + -62.01731512678528, + -67.61265076621554 ], "moduleForcesY": [ - 15.193911023893614, - 5.976626851451594, - 26.574607140432867, - 17.20645904856515 + 13.81064712566369, + -0.3213659067003291, + 31.862172293048193, + 17.244933344769986 ], - "timestamp": 8.371862559657389 + "timestamp": 8.232490528041474 }, { - "x": 7.601264963854788, - "y": 2.094205845435507, - "heading": 0.47375371275159783, - "angularVelocity": 0.1185923989552555, - "velocityX": 2.8903627471056996, - "velocityY": 1.5537652920381468, + "x": 7.602133310590925, + "y": 2.090716538878092, + "heading": 0.47717310494630966, + "angularVelocity": 0.12809493841064415, + "velocityX": 2.899025012783137, + "velocityY": 1.5306161200782256, "moduleForcesX": [ - -68.97563128408632, - -68.37332903743547, - -69.6787002752127, - -69.70563687256657 + -69.13754121748958, + -68.20178675329157, + -69.06817284359947, + -69.80874265266795 ], "moduleForcesY": [ - -10.02818095513775, - -13.79457079772149, - 2.5400050436943973, - -2.895918692242975 + -9.046985055914966, + -14.8227009169618, + 9.835976008332125, + 0.7232885771622478 ], - "timestamp": 8.433403930133489 + "timestamp": 8.29400450490187 }, { - "x": 7.765427995394615, - "y": 2.1860237902182353, - "heading": 0.4771503707617556, - "angularVelocity": 0.055193083674938345, - "velocityX": 2.667523168070835, - "velocityY": 1.491971076893497, + "x": 7.76664139791012, + "y": 2.1816852539691975, + "heading": 0.47965086497918497, + "angularVelocity": 0.04027962683177553, + "velocityX": 2.6743204669166074, + "velocityY": 1.4788300112274317, "moduleForcesX": [ - -65.89884406545639, - -65.46067825744251, - -68.71649541984166, - -68.19447216007241 + -66.40756278498199, + -65.73447233946021, + -69.52981390692969, + -68.96429053135994 ], "moduleForcesY": [ - -22.954179123046586, - -24.267429442756974, - -12.195515440032587, - -14.975245795855631 + -21.48510284383042, + -23.5840541324825, + -6.248641320167586, + -11.053848562321154 ], - "timestamp": 8.494945300609588 + "timestamp": 8.355518481762267 }, { - "x": 7.916364373904703, - "y": 2.2725370209726456, - "heading": 0.4771759891464777, - "angularVelocity": 0.00041627907412384533, - "velocityX": 2.4526002157964135, - "velocityY": 1.4057735485108946, + "x": 7.917760773522142, + "y": 2.267858010889966, + "heading": 0.4778609500693658, + "angularVelocity": -0.029097694559421974, + "velocityX": 2.456667302700648, + "velocityY": 1.4008646704200718, "moduleForcesX": [ - -63.034541640882665, - -62.936246190035384, - -66.59623107773926, - -66.17286001113268 + -63.7079413269904, + -63.48861495951544, + -67.77169795111662, + -67.17513898955453 ], "moduleForcesY": [ - -30.054599009672646, - -30.30989854312364, - -21.022090416114956, - -22.384264891991062 + -28.621263222946787, + -29.166159121005602, + -16.89631000921434, + -19.21841268100821 ], - "timestamp": 8.556486671085688 + "timestamp": 8.417032458622664 }, { - "x": 8.054483029258073, - "y": 2.3527934772526407, - "heading": 0.4742054330913605, - "angularVelocity": -0.04826925419658775, - "velocityX": 2.2443220598574287, - "velocityY": 1.3041057691616338, + "x": 8.055903927968282, + "y": 2.3481558395536353, + "heading": 0.47263055774144574, + "angularVelocity": -0.08502770581375621, + "velocityX": 2.245719777793735, + "velocityY": 1.3053590868608649, "moduleForcesX": [ - -60.81370264139378, - -60.97714530202473, - -64.58773267429666, - -64.36180927085024 + -61.53663246666227, + -61.636381591712215, + -65.62140779995624, + -65.27264016289558 ], "moduleForcesY": [ - -34.39454115337558, - -34.13615632978172, - -26.64149301034084, - -27.222875461504188 + -33.09645471160549, + -32.94683673534328, + -24.002173782199, + -24.982303085352363 ], - "timestamp": 8.618028041561788 + "timestamp": 8.47854643548306 }, { - "x": 8.180111081085983, - "y": 2.4261456194668596, - "heading": 0.4685113229121613, - "angularVelocity": -0.09252491673727926, - "velocityX": 2.0413593466641706, - "velocityY": 1.1919159688311711, + "x": 8.181425593296224, + "y": 2.421820395971198, + "heading": 0.4645721903392223, + "angularVelocity": -0.13100059228021374, + "velocityX": 2.0405389430894125, + "velocityY": 1.197525508466805, "moduleForcesX": [ - -59.113865720735134, - -59.466549474512234, - -62.90136023448186, - -62.859497948105854 + -59.837069189363575, + -60.1347503268217, + -63.62558289863267, + -63.52420699677533 ], "moduleForcesY": [ - -37.28394866924835, - -36.74169723968548, - -30.46123969045995, - -30.575354363117626 + -36.12028649557102, + -35.64612522470202, + -28.92915290640471, + -29.180149543959885 ], - "timestamp": 8.679569412037887 + "timestamp": 8.540060412343458 }, { - "x": 8.293510268919242, - "y": 2.4921279456843495, - "heading": 0.4602987892888278, - "angularVelocity": -0.1334473632907311, - "velocityX": 1.842649699803164, - "velocityY": 1.0721621196771174, + "x": 8.294623187946847, + "y": 2.488295400127025, + "heading": 0.4541517988040101, + "angularVelocity": -0.16939876215222985, + "velocityX": 1.8401930817693604, + "velocityY": 1.0806487817669281, "moduleForcesX": [ - -57.78992161833766, - -58.284723665890425, - -61.51620046633014, - -61.63027709798539 + -58.495272320583396, + -58.9108341498489, + -61.90125817659251, + -61.99096011956313 ], "moduleForcesY": [ - -39.335088546545315, - -38.615438325466826, - -33.20466598872263, - -33.01319912531834 + -38.28430915682556, + -37.65794710311188, + -32.488649405293295, + -32.336456191223455 ], - "timestamp": 8.741110782513987 + "timestamp": 8.601574389203854 }, { - "x": 8.39489265808716, - "y": 2.5503910578782976, - "heading": 0.4497269700993511, - "angularVelocity": -0.17178394156143129, - "velocityX": 1.6473859516549267, - "velocityY": 0.946730820961736, + "x": 8.395745589032531, + "y": 2.5471579028047797, + "heading": 0.4417334392214656, + "angularVelocity": -0.20187866589616352, + "velocityX": 1.6438930832772425, + "velocityY": 0.9568963946411755, "moduleForcesX": [ - -56.73532643582359, - -57.3433644167481, - -60.37384118279297, - -60.62016736060447 + -57.41823039591145, + -57.901215370721474, + -60.440306372625834, + -60.66569970555321 ], "moduleForcesY": [ - -40.86369626897553, - -40.01966277609491, - -35.26442066300551, - -34.85553891808558 + -39.90297584100897, + -39.210477609648265, + -35.155958840161816, + -34.77905570626861 ], - "timestamp": 8.802652152990087 + "timestamp": 8.663088366064251 }, { - "x": 8.484432685070649, - "y": 2.600663620222655, - "heading": 0.43692255880634423, - "angularVelocity": -0.20806184837855615, - "velocityX": 1.4549566623359964, - "velocityY": 0.8168905234874689, + "x": 8.48500235236776, + "y": 2.598076660395606, + "heading": 0.4276083189600344, + "angularVelocity": -0.2296245663564786, + "velocityX": 1.4509997221898334, + "velocityY": 0.8277591563683715, "moduleForcesX": [ - -55.87704936399228, - -56.580812311815635, - -59.42051911915581, - -59.78199094341051 + -56.53862920087126, + -57.05702405798682, + -59.20466714974995, + -59.52214725960545 ], "moduleForcesY": [ - -42.04664160695578, - -41.10591420167531, - -36.867255229715404, - -36.29138091153048 + -41.15627446217204, + -40.443127482255576, + -37.21736934315343, + -36.717258988355645 ], - "timestamp": 8.864193523466186 + "timestamp": 8.724602342924648 }, { - "x": 8.56227596922161, - "y": 2.6427290793082396, - "heading": 0.42198862978658847, - "angularVelocity": -0.2426648757449358, - "velocityX": 1.2648935756345077, - "velocityY": 0.6835313994497664, + "x": 8.562571509547876, + "y": 2.640785806675407, + "heading": 0.41201408826118213, + "angularVelocity": -0.2535071132572452, + "velocityX": 1.2610005260455623, + "velocityY": 0.6942998723156388, "moduleForcesX": [ - -55.16507312556064, - -55.95401657048999, - -58.61387114486394, - -59.078804118808506 + -55.80869308264709, + -56.34173719772094, + -58.15451187186907, + -58.53176769808956 ], "moduleForcesY": [ - -42.989859151973775, - -41.96722249696166, - -38.151657125904684, - -37.43867247748044 + -42.15377542537939, + -41.44512997463475, + -38.852027537364954, + -38.28860744687602 ], - "timestamp": 8.925734893942286 + "timestamp": 8.786116319785044 }, { - "x": 8.628545730522124, - "y": 2.6764107009683005, - "heading": 0.405010585221208, - "angularVelocity": -0.27588018326589114, - "velocityX": 1.0768327189959002, - "velocityY": 0.5473004809527537, + "x": 8.62860576010961, + "y": 2.6750675306808085, + "heading": 0.3951480099492571, + "angularVelocity": -0.2741828633548061, + "velocityX": 1.0734836850427496, + "velocityY": 0.557299751293951, "moduleForcesX": [ - -54.56456662161311, - -55.43245939238377, - -57.921816727762916, - -58.48249206819515 + -55.19425623129184, + -55.728131941039244, + -57.25555323504394, + -57.66899798719165 ], "moduleForcesY": [ - -43.76032221451713, - -42.663599201968246, - -39.206371128583605, - -38.37439371125214 + -42.96566364835716, + -42.27592065616304, + -40.176385790672825, + -39.58618560604843 ], - "timestamp": 8.987276264418385 + "timestamp": 8.847630296645441 }, { - "x": 8.683347508309266, - "y": 2.7015615497885435, - "heading": 0.3860602892164142, - "angularVelocity": -0.30792775426009245, - "velocityX": 0.8904867955195352, - "velocityY": 0.4086819748352914, + "x": 8.683237297728128, + "y": 2.7007402779978236, + "heading": 0.3771761995999053, + "angularVelocity": -0.2921581609028107, + "velocityX": 0.8881158462978955, + "velocityY": 0.41734819674036067, "moduleForcesX": [ - -54.05076426253945, - -54.994086710555564, - -57.320209102597595, - -57.971713485412614 + -54.67049432361609, + -55.195762968597755, + -56.480010214696975, + -56.91239458884492 ], "moduleForcesY": [ - -44.402281739369485, - -43.23537505493689, - -40.090633053222774, - -39.150748009158825 + -43.638836888514874, + -42.97645664054756, + -41.268832050266276, + -40.67476273388533 ], - "timestamp": 9.048817634894485 + "timestamp": 8.909144273505838 }, { - "x": 8.72677268008055, - "y": 2.7180575426494102, - "heading": 0.36519902233774076, - "angularVelocity": -0.3389795631342851, - "velocityX": 0.7056256861902239, - "velocityY": 0.26804721333388104, + "x": 8.726581564114948, + "y": 2.717650467677025, + "heading": 0.35824027480182863, + "angularVelocity": -0.30783125664352257, + "velocityX": 0.7046246820488491, + "velocityY": 0.2748999583879684, "moduleForcesX": [ - -53.60568984052227, - -54.62264590972197, - -56.790761924991, - -57.530148641518124 + -54.21904272949312, + -54.729099389545915, + -55.805815985531794, + -56.244420654225245 ], "moduleForcesY": [ - -44.94605887665676, - -43.71059919852908, - -40.84537555251459, - -39.80431908016251 + -44.20576368971399, + -43.57579607050465, + -42.183797695943305, + -41.60055887427103 ], - "timestamp": 9.110359005370585 + "timestamp": 8.970658250366235 }, { - "x": 8.758901125853505, - "y": 2.725792489640272, - "heading": 0.34247964491820077, - "angularVelocity": -0.36917243219930057, - "velocityX": 0.522062565789479, - "velocityY": 0.1256869473497624, + "x": 8.758740183629461, + "y": 2.7256665220403926, + "heading": 0.338462250167195, + "angularVelocity": -0.32152082573882035, + "velocityX": 0.5227855709523735, + "velocityY": 0.13031273171557645, "moduleForcesX": [ - -53.2160444557517, - -54.305935154073225, - -56.31947803489789, - -57.14517521142788 + -53.82607120430835, + -54.316194914018055, + -55.21550784813393, + -55.65085738495609 ], "moduleForcesY": [ - -45.4130693587794, - -44.10935378815243, - -41.49973296414931, - -40.36148436994362 + -44.689609283680795, + -44.09507977862367, + -42.960129353038816, + -42.39731433858149 ], - "timestamp": 9.171900375846684 + "timestamp": 9.032172227226631 }, { "x": 8.779803276062012, "y": 2.7246744632720947, "heading": 0.31794821481648144, - "angularVelocity": -0.398616896437924, - "velocityX": 0.33964388584139615, - "velocityY": -0.018167069721194994, + "angularVelocity": -0.33348576043569805, + "velocityX": 0.34241148934902593, + "velocityY": -0.016127371679270322, "moduleForcesX": [ - -52.8718209998866, - -54.034634846892985, - -55.89551210582599, - -56.80690426412013 + -53.48098782939392, + -53.947745103828254, + -54.695219377692204, + -55.12018458029143 ], "moduleForcesY": [ - -45.818828916724534, - -44.44637942204098, - -42.074976138022336, - -40.84174218658695 + -45.107332847724315, + -44.55002787941316, + -43.626228500899906, + -43.09016461444635 ], - "timestamp": 9.233441746322784 + "timestamp": 9.093686204087028 }, { - "x": 8.789008750560857, - "y": 2.7112787087692207, - "heading": 0.2870074693343456, - "angularVelocity": -0.4321051934641618, - "velocityX": 0.12855971235566702, - "velocityY": -0.18707936737997807, + "x": 8.789390520920405, + "y": 2.711156481525927, + "heading": 0.29317696313872205, + "angularVelocity": -0.3456625720749201, + "velocityX": 0.13378216652004424, + "velocityY": -0.1886323872700164, "moduleForcesX": [ - -52.566704392180235, - -53.81516704620926, - -55.506512855841905, - -56.51638685989295 + -53.179448380447525, + -53.62389224585945, + -54.2363017447046, + -54.649105974722524 ], "moduleForcesY": [ - -46.18319326982662, - -44.72589340151746, - -42.6029828956687, - -41.258265442390176 + -45.47706110036372, + -44.95351001122035, + -44.210687438028955, + -43.700838292783004 ], - "timestamp": 9.305046406368838 + "timestamp": 9.165349304691366 }, { - "x": 8.782890992002454, - "y": 2.6860530746256153, - "heading": 0.25377121085635285, - "angularVelocity": -0.46416334434959966, - "velocityX": -0.08543799460076026, - "velocityY": -0.3522903974040376, + "x": 8.783794368286188, + "y": 2.6855639318912203, + "heading": 0.267539420337893, + "angularVelocity": -0.35775095669356133, + "velocityX": -0.07808973637788985, + "velocityY": -0.35712311383241235, "moduleForcesX": [ - -53.439272111965224, - -54.64572763634496, - -56.1717249078838, - -57.1626240496705 + -54.03803305894964, + -54.47570349158013, + -55.06023489691758, + -55.46707669488016 ], "moduleForcesY": [ - -45.16660023903551, - -43.7030817122355, - -41.717468923960176, - -40.353553056706815 + -44.44927461362763, + -43.91326452339743, + -43.17589421817186, + -42.65354228524133 ], - "timestamp": 9.376651066414892 + "timestamp": 9.237012405295705 }, { - "x": 8.761217284811817, - "y": 2.6493049073601336, - "heading": 0.2183564997566526, - "angularVelocity": -0.49458668020939406, - "velocityX": -0.302685707560046, - "velocityY": -0.5132091576420572, + "x": 8.76275609467282, + "y": 2.6482308380775303, + "heading": 0.2410424727336723, + "angularVelocity": -0.3697432483491591, + "velocityX": -0.2935719140806326, + "velocityY": -0.5209528125193889, "moduleForcesX": [ - -54.41587486021302, - -55.56438232577154, - -56.92221968880065, - -57.87959109885894 + -54.99526164627351, + -55.42219111728091, + -55.979540705697985, + -56.37649095420048 ], "moduleForcesY": [ - -43.98034800365324, - -42.52410083429765, - -40.68222445745944, - -39.312900897281004 + -43.254451016653434, + -42.70766979489076, + -41.972052319967815, + -41.439058648655944 ], - "timestamp": 9.448255726460946 + "timestamp": 9.308675505900043 }, { - "x": 8.723726927916339, - "y": 2.601394409233509, - "heading": 0.1808983892167063, - "angularVelocity": -0.5231239212064431, - "velocityX": -0.5235742599904201, - "velocityY": -0.6690974874513724, + "x": 8.725986587983398, + "y": 2.5995490914917334, + "heading": 0.21369371175484705, + "angularVelocity": -0.38162960782036737, + "velocityX": -0.5130884148096344, + "velocityY": -0.679313986909043, "moduleForcesX": [ - -55.513204951308495, - -56.58432740025471, - -57.772952193024096, - -58.67869063733703 + -56.06650028997491, + -56.47788343064675, + -57.008812567039705, + -57.391121229420555 ], "moduleForcesY": [ - -42.58123461820781, - -41.151361111760814, - -39.45859689985431, - -38.10349107560392 + -41.850827206177, + -41.29576254132913, + -40.5570050084547, + -40.01607334967796 ], - "timestamp": 9.519860386507 + "timestamp": 9.380338606504381 }, { - "x": 8.670126749763783, - "y": 2.5427487589796156, - "heading": 0.14155435817055417, - "angularVelocity": -0.5494618788895446, - "velocityX": -0.748557120696901, - "velocityY": -0.8190200221071341, + "x": 8.673161820665543, + "y": 2.5399838352697053, + "heading": 0.1855016713653994, + "angularVelocity": -0.3933968827988571, + "velocityX": -0.7371264552102906, + "velocityY": -0.8311844689904736, "moduleForcesX": [ - -56.750175428910545, - -57.721068226165364, - -58.740871201237205, - -59.57336306357096 + -57.26911890755975, + -57.659157229516524, + -58.16431071641967, + -58.52626643425478 ], "moduleForcesY": [ - -40.9113473334207, - -39.5340460978986, - -37.9953221829711, - -36.68127355062254 + -40.18252449430719, + -39.62280351144781, + -38.874499091161304, + -38.32952205223615 ], - "timestamp": 9.591465046553054 + "timestamp": 9.45200170710872 }, { - "x": 8.600086131198294, - "y": 2.4738813585896815, - "heading": 0.10051029066858429, - "angularVelocity": -0.5732038595752198, - "velocityX": -0.9781572668656089, - "velocityY": -0.9617726045433423, + "x": 8.603918051131696, + "y": 2.4700943215420597, + "heading": 0.156476193662526, + "angularVelocity": -0.4050268193547327, + "velocityX": -0.9662402121860534, + "velocityY": -0.9752510446556707, "moduleForcesX": [ - -58.14728541567019, - -58.992253649512634, - -59.84438084571835, - -60.57897527375518 + -58.62178890554708, + -58.98353550726006, + -59.46309693106811, + -59.79789639516721 ], "moduleForcesY": [ - -38.8916920572076, - -37.60212331585296, - -36.2234088942802, - -34.98615199145863 + -38.17360914029891, + -37.61450756539028, + -36.848345847349954, + -36.304918084781264 ], - "timestamp": 9.663069706599108 + "timestamp": 9.523664807713057 }, { - "x": 8.513231926199088, - "y": 2.395418529280188, - "heading": 0.0579886645280164, - "angularVelocity": -0.5938388103961255, - "velocityX": -1.2129686104695387, - "velocityY": -1.09577825324536, + "x": 8.517847357752716, + "y": 2.3905626117140066, + "heading": 0.12662900025486085, + "angularVelocity": -0.41649319044197497, + "velocityX": -1.2010461821096667, + "velocityY": -1.1098000108474018, "moduleForcesX": [ - -59.72456510756159, - -60.41656271939983, - -61.10169314907387, - -61.71200087220666 + -60.14239359593657, + -60.46764931194161, + -60.920705975153766, + -61.22039017159285 ], "moduleForcesY": [ - -36.41263890584794, - -35.257097835848604, - -34.04864509744792, - -32.934708796771055 + -35.71931021724835, + -35.16851680517709, + -34.373858499651185, + -33.84003668835228 ], - "timestamp": 9.734674366645162 + "timestamp": 9.595327908317396 }, { - "x": 8.409144338572357, - "y": 2.3081371726052624, - "heading": 0.014259918978119247, - "angularVelocity": -0.6106969228227869, - "velocityX": -1.4536426478358366, - "velocityY": -1.2189340277404956, + "x": 8.414494950035653, + "y": 2.302233586652294, + "heading": 0.09597460073164957, + "angularVelocity": -0.4277570920697149, + "velocityX": -1.4421983816704484, + "velocityY": -1.2325593550492429, "moduleForcesX": [ - -61.49629524842574, - -62.01024906206618, - -62.5267777803112, - -62.98751725355034 + -61.84288712238317, + -62.122276292758905, + -62.54567545312701, + -62.801268264067 ], "moduleForcesY": [ - -33.31963140107074, - -32.35756349250855, - -31.340658297933924, - -30.409079395346843 + -32.67310530550032, + -32.14185463603263, + -31.305402017642134, + -30.792810837453125 ], - "timestamp": 9.806279026691216 + "timestamp": 9.666991008921734 }, { - "x": 8.287356319734036, - "y": 2.21301866214228, - "heading": -0.030341619606405787, - "angularVelocity": -0.6228859763573827, - "velocityX": -1.700839285599414, - "velocityY": -1.3283843593671696, + "x": 8.2933614907194, + "y": 2.2061711843005387, + "heading": 0.06453176165426741, + "angularVelocity": -0.4387591216710293, + "velocityX": -1.690318424610812, + "velocityY": -1.340472314784823, "moduleForcesX": [ - -63.458562590492434, - -63.778062663565834, - -64.12020804800913, - -64.4128163560726 + -63.717577250904, + -63.94109844594106, + -64.32744860294157, + -64.52959830151751 ], "moduleForcesY": [ - -29.392355415191965, - -28.696611629664407, - -27.917314517688634, - -27.239890506735552 + -28.828283743204985, + -28.332961805960878, + -27.43891241104092, + -26.96427075977155 ], - "timestamp": 9.87788368673727 + "timestamp": 9.738654109526072 }, { - "x": 8.147362398954167, - "y": 2.111326488188939, - "heading": -0.07539545756391505, - "angularVelocity": -0.6292025955926854, - "velocityX": -1.955095110986199, - "velocityY": -1.4201893268948602, + "x": 8.153917386350976, + "y": 2.103737230084854, + "heading": 0.032325911066374, + "angularVelocity": -0.4494063237049454, + "velocityX": -1.9458285113605962, + "velocityY": -1.4293821136938516, "moduleForcesX": [ - -65.56127142602155, - -65.69084909052835, - -65.8493885566579, - -65.97214747432078 + -65.71756976574333, + -65.87625105005283, + -66.21063177131644, + -66.35137198001577 ], "moduleForcesY": [ - -24.316794034856503, - -23.9674759446695, - -23.52399917574761, - -23.180580992000287 + -23.893867912535846, + -23.458019215236998, + -22.489715553229402, + -22.076652060585157 ], - "timestamp": 9.949488346783324 + "timestamp": 9.81031721013041 }, { - "x": 7.988650071662026, - "y": 2.0047168985365014, - "heading": -0.12036455636704498, - "angularVelocity": -0.6280191648728856, - "velocityX": -2.216508355602279, - "velocityY": -1.4888638474628435, + "x": 7.995643377180051, + "y": 1.996698976059294, + "heading": -0.0006069291037716975, + "angularVelocity": -0.45955086917006693, + "velocityX": -2.2085844435447637, + "velocityY": -1.4936313545311521, "moduleForcesX": [ - -67.64804942069694, - -67.63218085033044, - -67.60757677396883, - -67.59149476249789 + -67.69752144856352, + -67.78734757334065, + -68.04263214008894, + -68.11934773122928 ], "moduleForcesY": [ - -17.65673640990495, - -17.716555475855195, - -17.811425350367866, - -17.871500295896368 + -17.47107619907958, + -17.127002103641317, + -16.07246012795104, + -15.752714784412063 ], - "timestamp": 10.021093006829378 + "timestamp": 9.881980310734749 }, { - "x": 7.810781225665186, - "y": 1.8953882630351646, - "heading": -0.1645573135688971, - "angularVelocity": -0.6171771107275482, - "velocityX": -2.4840400873691744, - "velocityY": -1.5268368766923714, + "x": 7.818124590832862, + "y": 1.8873638999769546, + "heading": -0.034213912395681174, + "angularVelocity": -0.468957985469511, + "velocityX": -2.4771295806372327, + "velocityY": -1.5256816291830964, "moduleForcesX": [ - -69.33792438793783, - -69.27727776528677, - -69.1346031312489, - -69.06017856287379 + -69.313687888199, + -69.3432482954395, + -69.47611214375348, + -69.49889577515765 ], "moduleForcesY": [ - -8.85711879468359, - -9.301819468635278, - -10.32983168125111, - -10.801199653575688 + -9.057792824867414, + -8.844692568248144, + -7.710678951592893, + -7.521600845904924 ], - "timestamp": 10.092697666875432 + "timestamp": 9.953643411339087 }, { - "x": 7.613573483725926, - "y": 1.7862509730258411, - "heading": -0.20709141412342455, - "angularVelocity": -0.5940130227162668, - "velocityX": -2.754118821490405, - "velocityY": -1.5241646275414062, + "x": 7.621240028444532, + "y": 1.7787112968796408, + "heading": -0.06841646549100246, + "angularVelocity": -0.4772686753278808, + "velocityX": -2.747363157998911, + "velocityY": -1.5161582764496822, "moduleForcesX": [ - -69.8380909495494, - -69.85117254072334, - -69.88578385231288, - -69.87027129628852 + -69.86517241058907, + -69.86591053003337, + -69.82282638095482, + -69.8236070877981 ], "moduleForcesY": [ - 2.6196094231982188, - 2.050557264342375, - -0.5954925448843501, - -1.3097488777935367 + 1.837312109452161, + 1.8863774422298984, + 3.0464717958262337, + 3.075434675486523 ], - "timestamp": 10.164302326921486 + "timestamp": 10.025306511943425 }, { - "x": 7.3974393349223595, - "y": 1.6810231576681, - "heading": -0.24689444053872592, - "angularVelocity": -0.5558720115380904, - "velocityX": -3.0184369098960024, - "velocityY": -1.469566579745805, + "x": 7.40547828858971, + "y": 1.6744202309428289, + "heading": -0.10310081581402811, + "angularVelocity": -0.48399176187648607, + "velocityX": -3.0107787415740312, + "velocityY": -1.4552965899789445, "moduleForcesX": [ - -67.81774479937387, - -67.81197181841603, - -68.89677208175078, - -68.95839961219598 + -68.17759957909404, + -68.20364257441437, + -67.95854331471489, + -67.98903555111266 ], "moduleForcesY": [ - 16.827330110890028, - 16.80465353313287, - 11.649075089581624, - 11.210500710885821 + 15.314544404106083, + 15.206518112264616, + 16.260066564096665, + 16.13994452069931 ], - "timestamp": 10.23590698696754 + "timestamp": 10.096969612547763 }, { - "x": 7.163840114735354, - "y": 1.5840259794682252, - "heading": -0.2828186940424167, - "angularVelocity": -0.5017027310874074, - "velocityX": -3.2623466131500303, - "velocityY": -1.3546210279818205, + "x": 7.172300767498466, + "y": 1.5786130838476118, + "heading": -0.13811594024177143, + "angularVelocity": -0.48860744417223123, + "velocityX": -3.253801735130716, + "velocityY": -1.3369104362952573, "moduleForcesX": [ - -61.83333462479733, - -61.05938989064099, - -64.89350871525448, - -64.5824879376315 + -62.90664616032858, + -62.994759663175806, + -62.62640903247994, + -62.71838864361929 ], "moduleForcesY": [ - 32.52596953901682, - 33.92307848948774, - 25.88066947280988, - 26.602254108081837 + 30.40188460343236, + 30.221851782690536, + 30.976021760761594, + 30.79224774785525 ], - "timestamp": 10.307511647013595 + "timestamp": 10.168632713152101 }, { - "x": 6.91551546736203, - "y": 1.4994886743390452, - "heading": -0.3139208387202008, - "angularVelocity": -0.4343592254719174, - "velocityX": -3.467995619469574, - "velocityY": -1.180611779663611, + "x": 6.924263851753144, + "y": 1.4952348436632368, + "heading": -0.17328998021723432, + "angularVelocity": -0.4908249807619052, + "velocityX": -3.461152443218535, + "velocityY": -1.1634751982713893, "moduleForcesX": [ - -51.68933538210916, - -48.85035357028174, - -57.056547684318936, - -55.184870641943775 + -53.63079242417267, + -53.732278169292705, + -53.450679135245664, + -53.553119173625326 ], "moduleForcesY": [ - 47.00644705748203, - 49.9244819592894, - 40.31096815949663, - 42.80215553528719 + 44.77877441586359, + 44.65783761257958, + 44.9941386909519, + 44.87304464918282 ], - "timestamp": 10.379116307059649 + "timestamp": 10.24029581375644 }, { - "x": 6.656064858498082, - "y": 1.43069889782314, - "heading": -0.3397354689772922, - "angularVelocity": -0.3605160647433842, - "velocityX": -3.6233760302342883, - "velocityY": -0.9606885427800527, + "x": 6.664645466021959, + "y": 1.4273879906699989, + "heading": -0.20846188348487105, + "angularVelocity": -0.49079516475048257, + "velocityX": -3.622762391548932, + "velocityY": -0.9467473835360443, "moduleForcesX": [ - -39.29968551732011, - -33.85969689804127, - -46.079653739591876, - -41.530113395690364 + -41.769412633151376, + -41.76713542840772, + -41.77205019669556, + -41.769773098851616 ], "moduleForcesY": [ - 57.77680095878208, - 61.10531694531754, - 52.513961690389884, - 56.154301989543136 + 56.01545952386056, + 56.01714957737264, + 56.01348552259297, + 56.01517566218937 ], - "timestamp": 10.450720967105703 + "timestamp": 10.311958914360778 }, { - "x": 6.389094964926698, - "y": 1.3796716923906485, - "heading": -0.36043754169192455, - "angularVelocity": -0.2891162768082043, - "velocityX": -3.728387138486177, - "velocityY": -0.7126240861931642, + "x": 6.396804785667352, + "y": 1.37708752891827, + "heading": -0.24350598675754967, + "angularVelocity": -0.4890118202694272, + "velocityX": -3.7374977930886835, + "velocityY": -0.7019018341035008, "moduleForcesX": [ - -27.32133306734179, - -20.310347551724792, - -33.900176274380996, - -27.121135359167035 + -29.670078278170966, + -29.48805559061184, + -29.820795736138948, + -29.638793031028708 ], "moduleForcesY": [ - 64.32684835868893, - 66.85977151354284, - 61.10135000376463, - 64.37959537846069 + 63.27577283382489, + 63.36048200304406, + 63.204474407441516, + 63.28970026112863 ], - "timestamp": 10.522325627151757 + "timestamp": 10.383622014965116 }, { - "x": 6.117512662620233, - "y": 1.34740277403393, - "heading": -0.37733465959682994, - "angularVelocity": -0.23597790833777565, - "velocityX": -3.7928020624885344, - "velocityY": -0.45065388671580686, + "x": 6.123724098762273, + "y": 1.3454463182894645, + "heading": -0.27833593889663377, + "angularVelocity": -0.4860235162218966, + "velocityX": -3.8106178019395767, + "velocityY": -0.4415272345457245, "moduleForcesX": [ - -17.461162262072158, - -11.425931826783211, - -21.98887484573413, - -15.772925216616537 + -18.964618645051026, + -18.60764038496825, + -19.189884865939113, + -18.832039003279412 ], "moduleForcesY": [ - 67.68404989930426, - 68.95306382811246, - 66.34075545396243, - 68.07770320581683 + 67.27850831701049, + 67.37773513616717, + 67.21397107913663, + 67.3147241815706 ], - "timestamp": 10.59393028719781 + "timestamp": 10.455285115569454 }, { - "x": 5.8435235121250875, - "y": 1.3343250077166011, - "heading": -0.3920716678293369, - "angularVelocity": -0.205810742248178, - "velocityX": -3.8264150729704207, - "velocityY": -0.18263848063684318, + "x": 5.847858828640413, + "y": 1.3329886237876336, + "heading": -0.3128943538499734, + "angularVelocity": -0.4822344367171772, + "velocityX": -3.849474384941141, + "velocityY": -0.17383694532855082, "moduleForcesX": [ - -9.461901729986893, - -5.721136386114701, - -11.732782343904876, - -7.862920839581364 + -10.164815803497616, + -9.673316441913729, + -10.413603531769702, + -9.919643964436528 ], "moduleForcesY": [ - 69.26711577967508, - 69.6728315865383, - 68.91304347288967, - 69.45744160036715 + 69.17032970505136, + 69.24042697051493, + 69.13258323678724, + 69.20483068344068 ], - "timestamp": 10.665534947243865 + "timestamp": 10.526948216173793 }, { - "x": 5.568929040137215, - "y": 1.3405893640009074, - "heading": -0.4055914386320038, - "angularVelocity": -0.18881132588258015, - "velocityX": -3.8348687335609193, - "velocityY": 0.08748531562439549, + "x": 5.571170150686736, + "y": 1.339901067236654, + "heading": -0.3471395996450313, + "angularVelocity": -0.4778644170607483, + "velocityX": -3.8609643682780583, + "velocityY": 0.09645749891823462, "moduleForcesX": [ - -2.762826914760814, - -0.5336705163664741, - -3.868314495467215, - -1.5820295641402107 + -3.140660151426149, + -2.5499567673882018, + -3.391519077751636, + -2.7966358407203566 ], "moduleForcesY": [ - 69.8660118858112, - 69.91723429353682, - 69.81049649840442, - 69.89820089852721 + 69.85390969745158, + 69.87766236501122, + 69.84141674708727, + 69.86745404796969 ], - "timestamp": 10.737139607289919 + "timestamp": 10.59861131677813 }, { "x": 5.295215606689453, "y": 1.3661898374557495, - "heading": -0.418290619060492, - "angularVelocity": -0.17735131233526324, - "velocityX": -3.8225645268299044, - "velocityY": 0.35752524260818264, + "heading": -0.3810366506112695, + "angularVelocity": -0.4730056427977949, + "velocityX": -3.850720128910756, + "velocityY": 0.3668383030792834, "moduleForcesX": [ - 2.720138806867678, - 4.271594537524151, - 2.07835169845561, - 3.6608431501275804 + 2.4358890124890213, + 3.105036958141336, + 2.1874776648542436, + 2.8624717837810634 ], "moduleForcesY": [ - 69.87725025164747, - 69.79888184516116, - 69.89739500886515, - 69.83163852773065 + 69.89157339518185, + 69.86477674457022, + 69.89901845264843, + 69.87435635433927 ], - "timestamp": 10.808744267335973 + "timestamp": 10.670274417382469 }, { - "x": 5.055367481611433, - "y": 1.4037520138273964, - "heading": -0.42898005555429164, - "angularVelocity": -0.16921534609514086, - "velocityX": -3.79683096661731, - "velocityY": 0.5946147562129904, + "x": 5.053706815736057, + "y": 1.4043054270449447, + "heading": -0.4105962505484791, + "angularVelocity": -0.46833496540702335, + "velocityX": -3.8264053470585537, + "velocityY": 0.6038939420574941, "moduleForcesX": [ - 7.128107876197704, - 8.397495650017172, - 6.681822586978301, - 7.973498881488917 + 6.887915896080987, + 7.623701240859441, + 6.643290267386372, + 7.386549953753599 ], "moduleForcesY": [ - 69.56442888170623, - 69.42212415607871, - 69.60712711861942, - 69.47048072990539 + 69.5924526319861, + 69.51542701820333, + 69.6153579444215, + 69.54014360764195 ], - "timestamp": 10.871914876506311 + "timestamp": 10.733390780926925 }, { - "x": 4.817922325323769, - "y": 1.4561844229991425, - "heading": -0.4391809724358013, - "angularVelocity": -0.16148200904637555, - "velocityX": -3.7587916185420474, - "velocityY": 0.8300127204783294, + "x": 4.814521797126242, + "y": 1.457279005501737, + "heading": -0.43977801159576735, + "angularVelocity": -0.46234857980584404, + "velocityX": -3.789588074752571, + "velocityY": 0.8393002302720002, "moduleForcesX": [ - 10.725498733701588, - 11.940280266617222, - 10.355733338830216, - 11.59193024794521 + 10.475140797961476, + 11.417395555766568, + 10.184857658397029, + 11.139880728378749 ], "moduleForcesY": [ - 69.09197337769128, - 68.89195807698471, - 69.14665294562548, - 68.94968761414044 + 69.13513408837922, + 68.98542996372927, + 69.17724110186298, + 69.02951613308481 ], - "timestamp": 10.93508548567665 + "timestamp": 10.79650714447138 }, { - "x": 4.583800693836467, - "y": 1.523306626333883, - "heading": -0.44902694770580176, - "angularVelocity": -0.15586323132409483, - "velocityX": -3.7061797339328746, - "velocityY": 1.0625543146773644, + "x": 4.578623056803542, + "y": 1.5249252632965482, + "heading": -0.46848135774782446, + "angularVelocity": -0.4547686929371364, + "velocityX": -3.7375210971485733, + "velocityY": 1.071770520289322, "moduleForcesX": [ - 15.088104493088409, - 15.974726148554316, - 14.871545105185097, - 15.770080616776701 + 14.840754027841374, + 16.029076302383867, + 14.51917987513575, + 15.728849295978776 ], "moduleForcesY": [ - 68.26065337301016, - 68.05830212763824, - 68.30673212222058, - 68.10456636002334 + 68.32109565083954, + 68.05166292944023, + 68.38835125483823, + 68.11984422324372 ], - "timestamp": 10.998256094846989 + "timestamp": 10.859623508015837 }, { - "x": 4.354069654505351, - "y": 1.6048331009047445, - "heading": -0.4589311264045249, - "angularVelocity": -0.15678460012973233, - "velocityX": -3.636676016716078, - "velocityY": 1.2905760391043033, + "x": 4.347193447367128, + "y": 1.6069250977069311, + "heading": -0.4965850522073392, + "angularVelocity": -0.4452679603399553, + "velocityX": -3.6667132965195126, + "velocityY": 1.299185025966005, "moduleForcesX": [ - 20.43945052990814, - 20.29380047894279, - 20.463831834196373, - 20.318512311359303 + 20.193590004699995, + 21.665646251529257, + 19.875228316396825, + 21.38196372017987 ], "moduleForcesY": [ - 66.83870936216607, - 66.88312038765154, - 66.83152320893446, - 66.87589139118117 + 66.92237625745865, + 66.45990723727093, + 67.0149915440133, + 66.54903999850195 ], - "timestamp": 11.061426704017327 + "timestamp": 10.922739871560292 }, { - "x": 4.129951329960104, - "y": 1.7003267532429973, - "heading": -0.46978399762811174, - "angularVelocity": -0.17180254181691199, - "velocityX": -3.5478259191852364, - "velocityY": 1.5116785098708787, + "x": 4.121687707159994, + "y": 1.7027378890121, + "heading": -0.5239423865994234, + "angularVelocity": -0.4334428166606177, + "velocityX": -3.572856982615943, + "velocityY": 1.5180340869556606, "moduleForcesX": [ - 27.12931598295055, - 24.762739453054976, - 27.29639832285452, - 25.017031498332177 + 26.76366004990429, + 28.54407768472358, + 26.51477620428529, + 28.34898574175057 ], "moduleForcesY": [ - 64.39426565175329, - 65.34133562291419, - 64.32896211579789, - 65.24961743862173 + 64.56136029572545, + 63.79374619278623, + 64.66009920375362, + 63.876720382176266 ], - "timestamp": 11.124597313187666 + "timestamp": 10.985856235104748 }, { - "x": 3.9129272992378854, - "y": 1.8090800592804388, - "heading": -0.48282197066425664, - "angularVelocity": -0.20639302370803844, - "velocityX": -3.435522208389926, - "velocityY": 1.721580770959322, + "x": 3.903869186822254, + "y": 1.8114606574002452, + "heading": -0.5503757290218597, + "angularVelocity": -0.4188033172066106, + "velocityX": -3.4510625787926257, + "velocityY": 1.7225765599053773, "moduleForcesX": [ - 35.59984400154966, - 30.289367075153166, - 35.35330491034905, - 30.469918831002943 + 34.722789021923006, + 36.79190026410112, + 34.65315726916901, + 36.79826370259493 ], "moduleForcesY": [ - 60.10095977176368, - 62.94451217757728, - 60.26121508533313, - 62.87162431394478 + 60.634978039609805, + 59.40218821323202, + 60.6689615218732, + 59.39227430417506 ], - "timestamp": 11.187767922358004 + "timestamp": 11.048972598649204 }, { - "x": 3.7049939746242946, - "y": 1.9297950624975384, - "heading": -0.498750181696931, - "angularVelocity": -0.2521459147199961, - "velocityX": -3.2916149985652714, - "velocityY": 1.910936190144909, + "x": 3.6957796336708695, + "y": 1.9316273259274328, + "heading": -0.5756742900350444, + "angularVelocity": -0.40082412218451013, + "velocityX": -3.296919237193045, + "velocityY": 1.903890873601256, "moduleForcesX": [ - 45.789906461187606, - 39.500956547308036, - 44.60405217605959, - 38.88284700733399 + 43.97155118149319, + 46.20214883747921, + 44.22377698479971, + 46.54083607831509 ], "moduleForcesY": [ - 52.72236020041571, - 57.580980046986554, - 53.75348320494377, - 58.023668192173425 + 54.28134738689487, + 52.39692915304774, + 54.06704722487376, + 52.087132813104375 ], - "timestamp": 11.250938531528343 + "timestamp": 11.11208896219366 }, { - "x": 3.508517364442358, - "y": 2.060224954573662, - "heading": -0.5175036294890961, - "angularVelocity": -0.2968698266245423, - "velocityX": -3.1102535302792567, - "velocityY": 2.064724304374236, + "x": 3.499550666767407, + "y": 2.061002715105782, + "heading": -0.5996083713418497, + "angularVelocity": -0.37920564434843623, + "velocityX": -3.109003052199785, + "velocityY": 2.0497915582101762, "moduleForcesX": [ - 56.310610811243905, - 51.69004210860725, - 54.5613125649118, - 50.14303242636139 + 53.763954059728796, + 55.84492515312586, + 54.42048149959161, + 56.552580490225075 ], "moduleForcesY": [ - 41.26969515333533, - 46.91561591928861, - 43.58578238706644, - 48.59522594128138 + 44.579593956853635, + 41.94760445118944, + 43.76152240959032, + 40.974101674150546 ], - "timestamp": 11.314109140698681 + "timestamp": 11.175205325738116 }, { - "x": 3.3256131531226463, - "y": 2.1971031258571245, - "heading": -0.5385715679309702, - "angularVelocity": -0.3335085527680234, - "velocityX": -2.8954004674311986, - "velocityY": 2.1668015091380983, + "x": 3.3170061238667126, + "y": 2.1965515433699756, + "heading": -0.621969243888579, + "angularVelocity": -0.3542801151872387, + "velocityX": -2.8921904344524014, + "velocityY": 2.1476019949837775, "moduleForcesX": [ - 64.77216448239771, - 62.673602768769214, - 63.38542095359247, - 61.15353995273158 + 62.44766102413855, + 63.94433819362826, + 63.329847528999416, + 64.77963805969534 ], "moduleForcesY": [ - 26.035622904980194, - 30.721312600843035, - 29.28306014997138, - 33.678552232388746 + 31.256641416018002, + 28.0802822128175, + 29.404706704310698, + 26.071343409245944 ], - "timestamp": 11.37727974986902 + "timestamp": 11.238321689282571 }, { - "x": 3.157514306984536, - "y": 2.3366007043510604, - "heading": -0.561190847527418, - "angularVelocity": -0.35806651057385414, - "velocityX": -2.6610293670721896, - "velocityY": 2.208267109120054, + "x": 3.149232244356222, + "y": 2.334780583324695, + "heading": -0.6426179790087161, + "angularVelocity": -0.32715343471259145, + "velocityX": -2.6581677094296006, + "velocityY": 2.1900666038429124, "moduleForcesX": [ - 69.18908773846705, - 68.77071008684689, - 68.72649352066082, - 68.18965051435607 + 68.07446024974723, + 68.73289751434758, + 68.68747354304334, + 69.20841989455995 ], "moduleForcesY": [ - 9.337187838928434, - 11.986015068259727, - 12.328677058413632, - 14.979951486247026 + 15.578092751741488, + 12.393772344765436, + 12.552654231475199, + 9.321777066848862 ], - "timestamp": 11.440450359039358 + "timestamp": 11.301438052827027 }, { - "x": 3.0044268603736923, - "y": 2.4751119466714426, - "heading": -0.5845404524188933, - "angularVelocity": -0.3696276670138402, - "velocityX": -2.4233967128296365, - "velocityY": 2.1926532629579243, + "x": 2.996456707624642, + "y": 2.4723242803752896, + "heading": -0.6615054771208647, + "angularVelocity": -0.2992488326556613, + "velocityX": -2.4205376886767986, + "velocityY": 2.179208200955956, "moduleForcesX": [ - 69.58635593128226, - 69.65294315110864, - 69.70772082110078, - 69.75414910427422 + 69.84418644116603, + 69.8030417146416, + 69.73398928648147, + 69.55639119192205 ], "moduleForcesY": [ - -5.863213664973564, - -4.950604413431918, - -4.219222524167902, - -3.2792456106314267 + 0.04124705741425996, + -2.629684927774557, + -3.7965442820408897, + -6.360953421225059 ], - "timestamp": 11.503620968209697 + "timestamp": 11.364554416371483 }, { - "x": 2.8658808016377564, - "y": 2.609745022810685, - "heading": -0.607898551213732, - "angularVelocity": -0.36976212674875547, - "velocityX": -2.193204411918028, - "velocityY": 2.1312613240155165, + "x": 2.858285394290842, + "y": 2.6063679764517347, + "heading": -0.6786531021168939, + "angularVelocity": -0.2716827147994899, + "velocityX": -2.18915199758744, + "velocityY": 2.1237550541395294, "moduleForcesX": [ - 67.49011979913459, - 67.49197422814143, - 67.49549991286162, - 67.49735359030318 + 68.5832050639289, + 68.18255030614512, + 67.65433881512726, + 67.18772346415568 ], "moduleForcesY": [ - -18.014094750436488, - -18.00696556606115, - -17.99399260732844, - -17.986857779229418 + -13.292687011674952, + -15.252960265172256, + -17.395421555860977, + -19.15158282474659 ], - "timestamp": 11.566791577380036 + "timestamp": 11.427670779915939 }, { - "x": 2.7411339659112834, - "y": 2.738363105075064, - "heading": -0.6306961855706249, - "angularVelocity": -0.36088989256727516, - "velocityX": -1.9747606895810539, - "velocityY": 2.0360430895570842, + "x": 2.734026522437346, + "y": 2.734745646026211, + "heading": -0.6941195885833025, + "angularVelocity": -0.2450471731552614, + "velocityX": -1.9687267275145863, + "velocityY": 2.0339839364169796, "moduleForcesX": [ - 64.39517539155403, - 64.27134897958376, - 63.8241376908432, - 63.70527604163203 + 65.71596180721829, + 65.23236893280917, + 64.13301722958752, + 63.6607820099661 ], "moduleForcesY": [ - -27.117529617984484, - -27.416726077312312, - -28.43391532243881, - -28.706015086897278 + -23.750213140906997, + -25.069078070391868, + -27.734992318593385, + -28.82188410507732 ], - "timestamp": 11.629962186550374 + "timestamp": 11.490787143460395 }, { - "x": 2.629408519345831, - "y": 2.8594244836840406, - "heading": -0.6525029140354126, - "angularVelocity": -0.34520370709084347, - "velocityX": -1.7686301910463869, - "velocityY": 1.9164193633551478, + "x": 2.622913491858143, + "y": 2.8558455619096765, + "heading": -0.7079766418555111, + "angularVelocity": -0.21954771304985882, + "velocityX": -1.7604472808535805, + "velocityY": 1.918677013104065, "moduleForcesX": [ - 61.165285278261656, - 60.992919135287345, - 59.86697681442411, - 59.72951823823286 + 62.3261455799643, + 61.90369851571434, + 60.3053013643231, + 59.94983415338624 ], "moduleForcesY": [ - -33.81189902733295, - -34.13067207401691, - -36.05965076270056, - -36.295299281822466 + -31.626927610118603, + -32.460334406565565, + -35.32513309689565, + -35.9385060308983 ], - "timestamp": 11.693132795720713 + "timestamp": 11.55390350700485 }, { - "x": 2.529987992544686, - "y": 2.971809130496302, - "heading": -0.6729951982943887, - "angularVelocity": -0.3243958626980939, - "velocityX": -1.5738415080509853, - "velocityY": 1.7790654275506226, + "x": 2.5242136717280004, + "y": 2.968472284388882, + "heading": -0.720296269771335, + "angularVelocity": -0.19518912725614584, + "velocityX": -1.5637754551658334, + "velocityY": 1.7844298396544638, "moduleForcesX": [ - 58.16870881285027, - 58.01912021029308, - 56.16470015343222, - 56.10022212287148 + 58.98754783726104, + 58.678775107744755, + 56.70105562415566, + 56.492209664989446 ], "moduleForcesY": [ - -38.76602349429578, - -38.998640161944856, - -41.61590645152988, - -41.71135809779262 + -37.51207015880541, + -38.003843268920306, + -40.88479342874241, + -41.18285690919068 ], - "timestamp": 11.756303404891051 + "timestamp": 11.617019870549306 }, { - "x": 2.4422441011606852, - "y": 3.074687163648499, - "heading": -0.6919282071615437, - "angularVelocity": -0.2997123047539793, - "velocityX": -1.3889986583381029, - "velocityY": 1.6285743402408528, + "x": 2.437268193420317, + "y": 3.0717284892697196, + "heading": -0.7311460366120948, + "angularVelocity": -0.17190101316781217, + "velocityX": -1.377542580482226, + "velocityY": 1.6359656843682122, "moduleForcesX": [ - 55.516392904604864, - 55.43040225099981, - 52.89236318545878, - 52.94888563341702 + 55.933543070193124, + 55.74620404374717, + 53.4983817914488, + 53.427889596621455 ], "moduleForcesY": [ - -42.49669388469859, - -42.617601177326456, - -45.721215937319954, - -45.66397764278443 + -41.94896951930148, + -42.20583852522306, + -45.01283639047294, + -45.10426074033776 ], - "timestamp": 11.81947401406139 + "timestamp": 11.680136234093762 }, { - "x": 2.36563547825889, - "y": 3.1674290736500192, - "heading": -0.709114876775145, - "angularVelocity": -0.2720674984668532, - "velocityX": -1.212725726535598, - "velocityY": 1.4681180254482455, + "x": 2.3614992683800686, + "y": 3.1649283314618284, + "heading": -0.7405881776451042, + "angularVelocity": -0.1495989392094639, + "velocityX": -1.2004640442708914, + "velocityY": 1.4766351696809092, "moduleForcesX": [ - 53.21000778523316, - 53.20542690598686, - 50.063878893766116, - 50.25772899182876 + 53.226119004537075, + 53.150380788764224, + 50.717908435297424, + 50.76595922311749 ], "moduleForcesY": [ - -45.3662355318544, - -45.379935036516486, - -48.8163206207447, - -48.624450752509695 + -45.3497145739425, + -45.44508539739661, + -48.13839282499532, + -48.093987559665536 ], - "timestamp": 11.882644623231728 + "timestamp": 11.743252597638218 }, { - "x": 2.29969733456388, - "y": 3.24954609830814, - "heading": -0.7244111189583969, - "angularVelocity": -0.24214175522680043, - "velocityX": -1.0438104770718435, - "velocityY": 1.2999245335230758, + "x": 2.2964046195038508, + "y": 3.2475371925356646, + "heading": -0.7486801929297746, + "angularVelocity": -0.12820788192226776, + "velocityX": -1.0313434618325024, + "velocityY": 1.3088342932756496, "moduleForcesX": [ - 51.212589224847115, - 51.29435553074885, - 47.635142275536005, - 47.96569255346641 + 50.85429251327673, + 50.87522935086129, + 48.32191532894057, + 48.467633279346366 ], "moduleForcesY": [ - -47.62154135295099, - -47.541251547195635, - -51.20041879059338, - -50.89807366778042 + -48.006099795729824, + -47.9893857552052, + -50.55448655081397, + -50.41998583087466 ], - "timestamp": 11.945815232402067 + "timestamp": 11.806368961182674 }, { - "x": 2.244029529169606, - "y": 3.32065049496387, - "heading": -0.7377053964323423, - "angularVelocity": -0.2104503605165108, - "velocityX": -0.8812295167863048, - "velocityY": 1.1255930184874117, + "x": 2.2415479359062194, + "y": 3.3191302814359016, + "heading": -0.7554758085003378, + "angularVelocity": -0.10766804658789833, + "velocityX": -0.8691356807810039, + "velocityY": 1.1343031328129516, "moduleForcesX": [ - 49.47830306207863, - 49.64543262889328, - 45.54785025385102, - 46.007190229326234 + 48.7813998909101, + 48.883997299800775, + 46.25717342000062, + 46.48203569715231 ], "moduleForcesY": [ - -49.430771057830185, - -49.27017896162637, - -53.075077496144715, - -52.68407579049555 + -50.12061198883146, + -50.02516024483403, + -52.45958599626199, + -52.26481927286433 ], - "timestamp": 12.008985841572406 + "timestamp": 11.86948532472713 }, { - "x": 2.198285645058137, - "y": 3.3804286588611365, - "heading": -0.748911194782175, - "angularVelocity": -0.177389429942277, - "velocityX": -0.7241323886575408, - "velocityY": 0.9462970942084137, + "x": 2.1965490021845606, + "y": 3.379364027011461, + "heading": -0.7610258706649285, + "angularVelocity": -0.08793380754076002, + "velocityX": -0.7129519382079801, + "velocityY": 0.9543285162988591, "moduleForcesX": [ - 47.963765452152025, - 48.213334157004304, - 43.74592423575202, - 44.32418492699512 + 46.96552536511235, + 47.136382750106904, + 44.47147307724585, + 44.76001886720274 ], "moduleForcesY": [ - -50.909740237006666, - -50.68021953336166, - -54.577452261696784, - -54.11506726002671 + -51.83392911935416, + -51.682542993841096, + -53.98932327657365, + -53.75408240792354 ], - "timestamp": 12.072156450742744 + "timestamp": 11.932601688271586 }, { - "x": 2.162163747962268, - "y": 3.42862257199927, - "heading": -0.7579610345845447, - "angularVelocity": -0.14326029020817274, - "velocityX": -0.5718149242231771, - "velocityY": 0.7629167071696146, + "x": 2.16107481240881, + "y": 3.4279560426439364, + "heading": -0.7653786869846049, + "angularVelocity": -0.06896494150222135, + "velocityX": -0.5620442589466436, + "velocityY": 0.7698798362844577, "moduleForcesX": [ - 46.6319076312275, - 46.9611493566907, - 42.180140742346175, - 42.868308982974696 + 45.36776130374439, + 45.59551423238422, + 42.91858789949486, + 43.2583272033556 ], "moduleForcesY": [ - -52.139192200259096, - -51.849151349231114, - -55.802822449668646, - -55.2816673356197 + -53.2445780347107, + -53.0530790830467, + -55.238217704660805, + -54.97579973773786 ], - "timestamp": 12.135327059913083 + "timestamp": 11.995718051816041 }, { - "x": 2.1353988201191023, - "y": 3.4650167267118044, - "heading": -0.7648020671777167, - "angularVelocity": -0.1082945484145205, - "velocityX": -0.4236927298103942, - "velocityY": 0.5761248021908162, + "x": 2.134832009400895, + "y": 3.4646708607022925, + "heading": -0.768580123623318, + "angularVelocity": -0.050722767582422834, + "velocityX": -0.4157844580103423, + "velocityY": 0.5817004655614504, "moduleForcesX": [ - 45.45179812579232, - 45.85909611829803, - 40.80962595285606, - 41.60067510940896 + 43.954428645312426, + 44.2297096749135, + 41.559591533194514, + 41.940634703058834 ], "moduleForcesY": [ - -53.17683717820275, - -52.83187682056265, - -56.8182410705635, - -56.24698678733785 + -54.42275064440647, + -54.20223828129335, + -56.273239100999206, + -55.992648881357695 ], - "timestamp": 12.198497669083421 + "timestamp": 12.058834415360497 }, { - "x": 2.1177566362723623, - "y": 3.4894287132169657, - "heading": -0.7693928270163053, - "angularVelocity": -0.07267240096117586, - "velocityX": -0.27927835552714286, - "velocityY": 0.3864453236367305, + "x": 2.1175606055664447, + "y": 3.489309593661584, + "heading": -0.770673719921477, + "angularVelocity": -0.03317042016662467, + "velocityX": -0.27364383599643854, + "velocityY": 0.39036997025244236, "moduleForcesX": [ - 44.397979595664026, - 44.88323764291386, - 39.60107624297137, - 40.49028312227595 + 42.69713344336191, + 43.01233196517679, + 40.36254825642123, + 40.77712883283335 ], "moduleForcesY": [ - -54.064737546429875, - -53.66807598097, - -57.67176826261024, - -57.055974307841915 + -55.41943823043682, + -55.177780690773446, + -57.14258045212568, + -56.849966899553834 ], - "timestamp": 12.26166827825376 + "timestamp": 12.121950778904953 }, { "x": 2.1090288162231445, "y": 3.501702308654785, "heading": -0.771700836029523, - "angularVelocity": -0.036536120888024486, - "velocityX": -0.1381626703279573, - "velocityY": 0.19429281431692114, + "angularVelocity": -0.016273372709797004, + "velocityX": -0.13517555296560768, + "velocityY": 0.19634710077161807, "moduleForcesX": [ - 43.44956308739193, - 44.01425554364353, - 38.52755610227539, - 39.51242168602051 + 41.5722477087897, + 41.92121432741254, + 39.30147273432098, + 39.743500474089345 ], "moduleForcesY": [ - -54.834133333236075, - -54.387070471211764, - -58.398462389422065, - -57.74131858284474 + -56.272332012188514, + -56.01516696263238, + -57.881532814470596, + -57.581128152106956 ], - "timestamp": 12.324838887424098 + "timestamp": 12.185067142449409 }, { "x": 2.1090288162231445, "y": 3.501702308654785, "heading": -0.771700836029523, - "angularVelocity": 1.2297405786038555e-20, - "velocityX": -1.8885005519347326e-21, - "velocityY": -1.1623354869164822e-22, + "angularVelocity": 7.034369509774633e-21, + "velocityX": 1.0745130979383846e-20, + "velocityY": 7.792911077792643e-21, "moduleForcesX": [ - 42.58937177633389, - 43.23643664704943, - 37.56731333436976, - 38.647309328509984 + 40.56018810627996, + 40.937946455779034, + 38.35525709358692, + 38.819933249113355 ], "moduleForcesY": [ - -55.508635741315956, - -55.01097400948289, - -59.024322031425896, - -58.327253062024916 + -57.00975798757109, + -56.74117440782203, + -58.516369460899966, + -58.211114035733786 ], - "timestamp": 12.388009496594437 + "timestamp": 12.248183505993865 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/test.1.traj b/src/main/deploy/choreo/test.1.traj index 29e1ba4e..43454f70 100644 --- a/src/main/deploy/choreo/test.1.traj +++ b/src/main/deploy/choreo/test.1.traj @@ -1,1093 +1,2546 @@ { - "samples": [ - { - "x": 2.1199185848236084, - "y": 5.957952976226807, - "heading": 9.721358924197938e-33, - "angularVelocity": 0, - "velocityX": 7.529723953218568e-33, - "velocityY": 4.103114367587547e-33, - "timestamp": 0 - }, - { - "x": 2.12408359184855, - "y": 5.959490957092025, - "heading": 0.0006587462568403469, - "angularVelocity": 0.020752025490549227, - "velocityX": 0.13120732156333928, - "velocityY": 0.04844994227681366, - "timestamp": 0.031743708928096186 - }, - { - "x": 2.132419910947834, - "y": 5.962549984290152, - "heading": 0.0019689340924026384, - "angularVelocity": 0.04127393678319714, - "velocityX": 0.26261326671585045, - "velocityY": 0.0963664077520038, - "timestamp": 0.06348741785619237 - }, - { - "x": 2.144934423626495, - "y": 5.967111326975421, - "heading": 0.003922497731913535, - "angularVelocity": 0.06154175757899709, - "velocityX": 0.3942359951387039, - "velocityY": 0.1436928084112908, - "timestamp": 0.09523112678428855 - }, - { - "x": 2.161634668441711, - "y": 5.9731541575830684, - "heading": 0.006510477776343394, - "angularVelocity": 0.0815273366540852, - "velocityX": 0.5260962054889244, - "velocityY": 0.19036309277324892, - "timestamp": 0.12697483571238474 - }, - { - "x": 2.182528936666272, - "y": 5.980655180863507, - "heading": 0.00972285754365477, - "angularVelocity": 0.10119736715669446, - "velocityX": 0.6582176100432996, - "velocityY": 0.2362995230780409, - "timestamp": 0.15871854464048094 - }, - { - "x": 2.2076263864352867, - "y": 5.98958817047729, - "heading": 0.013548358655846408, - "angularVelocity": 0.12051210275577494, - "velocityX": 0.7906275169629459, - "velocityY": 0.2814097632390032, - "timestamp": 0.19046225356857713 - }, - { - "x": 2.2369371796137325, - "y": 5.999923382651596, - "heading": 0.017974182463991455, - "angularVelocity": 0.139423651412969, - "velocityX": 0.9233575460523492, - "velocityY": 0.32558300599701506, - "timestamp": 0.22220596249667332 - }, - { - "x": 2.270472646569135, - "y": 6.0116268035707865, - "heading": 0.022985678275806047, - "angularVelocity": 0.15787366949359927, - "velocityX": 1.0564445078350972, - "velocityY": 0.36868473516117956, - "timestamp": 0.2539496714247695 - }, - { - "x": 2.3082454850008034, - "y": 6.024659167637654, - "heading": 0.02856591078801465, - "angularVelocity": 0.17579018648543426, - "velocityX": 1.1899314764141191, - "velocityY": 0.41054950750668673, - "timestamp": 0.2856933803528657 - }, - { - "x": 2.350269999641707, - "y": 6.03897465321323, - "heading": 0.034695085760854436, - "angularVelocity": 0.1930831392993197, - "velocityX": 1.323869077053848, - "velocityY": 0.4509707926064986, - "timestamp": 0.31743708928096187 - }, - { - "x": 2.3965623891443495, - "y": 6.054519113336024, - "heading": 0.0413497715036912, - "angularVelocity": 0.20963793984853818, - "velocityX": 1.458316972585085, - "velocityY": 0.48968632361146647, - "timestamp": 0.34918079820905806 - }, - { - "x": 2.4471410825572004, - "y": 6.071227617313664, - "heading": 0.04850181812451799, - "angularVelocity": 0.2253059539144659, - "velocityX": 1.5933454256218078, - "velocityY": 0.5263563881424058, - "timestamp": 0.38092450713715426 - }, - { - "x": 2.5020271147547666, - "y": 6.089020938374403, - "heading": 0.05611681521560166, - "angularVelocity": 0.23988996082133435, - "velocityX": 1.7290365256905385, - "velocityY": 0.5605306267468754, - "timestamp": 0.41266821606525045 - }, - { - "x": 2.561244492876039, - "y": 6.107800369655392, - "heading": 0.06415181877617589, - "angularVelocity": 0.2531211327187972, - "velocityX": 1.8654839059735924, - "velocityY": 0.5915953716537398, - "timestamp": 0.44441192499334664 - }, - { - "x": 2.6248204026171256, - "y": 6.127439774433694, - "heading": 0.07255187184184077, - "angularVelocity": 0.26462103356269817, - "velocityX": 2.0027876983466375, - "velocityY": 0.6186865190448766, - "timestamp": 0.47615563392144283 - }, - { - "x": 2.6927848123865834, - "y": 6.14777283178745, - "heading": 0.08124443551167423, - "angularVelocity": 0.2738357918264941, - "velocityX": 2.141035564665967, - "velocityY": 0.6405381740302919, - "timestamp": 0.507899342849539 - }, - { - "x": 2.7651681706057807, - "y": 6.1685714629691, - "heading": 0.09012999755945103, - "angularVelocity": 0.27991568559003044, - "velocityX": 2.280242626441548, - "velocityY": 0.655204822749479, - "timestamp": 0.5396430517776352 - }, - { - "x": 2.8419931281714104, - "y": 6.189507110191248, - "heading": 0.09906528191141717, - "angularVelocity": 0.28148205278115124, - "velocityX": 2.420163243672891, - "velocityY": 0.65952114384502, - "timestamp": 0.5713867607057314 - }, - { - "x": 2.923246509702383, - "y": 6.21007727563667, - "heading": 0.10783160033295475, - "angularVelocity": 0.27615923650876134, - "velocityX": 2.559668806030865, - "velocityY": 0.6480076254481864, - "timestamp": 0.6031304696338275 - }, - { - "x": 3.0087812329232086, - "y": 6.229466168623038, - "heading": 0.11607630855981195, - "angularVelocity": 0.2597273130727852, - "velocityX": 2.694540937688452, - "velocityY": 0.6107948201729568, - "timestamp": 0.6348741785619237 - }, - { - "x": 3.0979818735027176, - "y": 6.246361893248638, - "heading": 0.12324903142117805, - "angularVelocity": 0.22595730314988358, - "velocityX": 2.810025784370449, - "velocityY": 0.5322542700937218, - "timestamp": 0.6666178874900199 - }, - { - "x": 3.1891159324545155, - "y": 6.259285555621032, - "heading": 0.12873166157260418, - "angularVelocity": 0.1727154871488241, - "velocityX": 2.8709329195972866, - "velocityY": 0.40712515357501006, - "timestamp": 0.6983615964181161 - }, - { - "x": 3.2799147655356333, - "y": 6.267810501767619, - "heading": 0.13233373871668666, - "angularVelocity": 0.1134737327714805, - "velocityX": 2.860372531980369, - "velocityY": 0.26855545348842214, - "timestamp": 0.7301053053462123 - }, - { - "x": 3.368930673043237, - "y": 6.2722939827484225, - "heading": 0.13423316054281098, - "angularVelocity": 0.05983616566124247, - "velocityX": 2.8042062667984893, - "velocityY": 0.14123998525071657, - "timestamp": 0.7618490142743085 - }, - { - "x": 3.455418033445333, - "y": 6.273149165824332, - "heading": 0.13460089895534874, - "angularVelocity": 0.011584607626349263, - "velocityX": 2.7245512047125473, - "velocityY": 0.026940238075252306, - "timestamp": 0.7935927232024047 - }, - { - "x": 3.53897038421859, - "y": 6.270691577134489, - "heading": 0.1335628529939654, - "angularVelocity": -0.03270084046371159, - "velocityX": 2.6320916362516953, - "velocityY": -0.07741970843436734, - "timestamp": 0.8253364321305009 - }, - { - "x": 3.619343411211179, - "y": 6.2651515676417615, - "heading": 0.13121159908477192, - "angularVelocity": -0.07406991774402583, - "velocityX": 2.531935608867881, - "velocityY": -0.17452306865816167, - "timestamp": 0.8570801410585971 - }, - { - "x": 3.696377587829814, - "y": 6.256700197632999, - "heading": 0.12761684069301102, - "angularVelocity": -0.11324317520377067, - "velocityX": 2.42675412608914, - "velocityY": -0.26623763555482466, - "timestamp": 0.8888238499866933 - }, - { - "x": 3.7699618707808793, - "y": 6.245468037234803, - "heading": 0.1228324760673318, - "angularVelocity": -0.15071851359642613, - "velocityX": 2.318074523608403, - "velocityY": -0.35383894250147446, - "timestamp": 0.9205675589147895 - }, - { - "x": 3.8400150685867502, - "y": 6.2315574365397515, - "heading": 0.11690123481863413, - "angularVelocity": -0.18684777075463518, - "velocityX": 2.2068372024374066, - "velocityY": -0.438215985616424, - "timestamp": 0.9523112678428857 - }, - { - "x": 3.906475492488988, - "y": 6.215050519883858, - "heading": 0.10985775018096655, - "angularVelocity": -0.22188600121123825, - "velocityX": 2.0936565431839025, - "velocityY": -0.5200059228516231, - "timestamp": 0.9840549767709819 - }, - { - "x": 3.9692948172312557, - "y": 6.196014512598508, - "heading": 0.10173064016138685, - "angularVelocity": -0.2560226984814519, - "velocityX": 1.9789535269668237, - "velocityY": -0.5996781071948685, - "timestamp": 1.015798685699078 - }, - { - "x": 4.028434236538794, - "y": 6.174505389403469, - "heading": 0.09254395303933619, - "angularVelocity": -0.2894018195183511, - "velocityX": 1.8630280236469257, - "velocityY": -0.6775869588445468, - "timestamp": 1.047542394627174 - }, - { - "x": 4.083861945125214, - "y": 6.150570441683336, - "heading": 0.08231819631194325, - "angularVelocity": -0.3221349071262287, - "velocityX": 1.7461005804951526, - "velocityY": -0.7540060228735362, - "timestamp": 1.07928610355527 - }, - { - "x": 4.135551426431915, - "y": 6.124250129459578, - "heading": 0.07107108601922003, - "angularVelocity": -0.35430989863852635, - "velocityX": 1.6283378046271129, - "velocityY": -0.8291505029665418, - "timestamp": 1.1110298124833662 - }, - { - "x": 4.183480250707313, - "y": 6.0955794475298, - "heading": 0.05881810384556654, - "angularVelocity": -0.38599718140717787, - "velocityX": 1.50986843988397, - "velocityY": -0.9031925662724984, - "timestamp": 1.1427735214114623 - }, - { - "x": 4.227629208104442, - "y": 6.064588953496888, - "heading": 0.04557291906848408, - "angularVelocity": -0.4172538504270615, - "velocityX": 1.3907939206829834, - "velocityY": -0.9762719946527232, - "timestamp": 1.1745172303395583 - }, - { - "x": 4.267981668593037, - "y": 6.031305555215571, - "heading": 0.031347713474676034, - "angularVelocity": -0.44812676508695654, - "velocityX": 1.2711955171966314, - "velocityY": -1.0485037635868417, - "timestamp": 1.2062609392676544 - }, - { - "x": 4.3045230996015675, - "y": 5.99575312358408, - "heading": 0.016153435256918744, - "angularVelocity": -0.47865478643897924, - "velocityX": 1.1511393042098552, - "velocityY": -1.1199835442045625, - "timestamp": 1.2380046481957505 - }, - { - "x": 4.337240695953369, - "y": 5.957952976226807, - "heading": 0, - "angularVelocity": -0.5088704440149149, - "velocityX": 1.030679698642281, - "velocityY": -1.1907917705171907, - "timestamp": 1.2697483571238466 - }, - { - "x": 4.3631357670929365, - "y": 5.922844781507193, - "heading": -0.015001401099503393, - "angularVelocity": -0.5352972829698707, - "velocityX": 0.924017772165235, - "velocityY": -1.252771065764447, - "timestamp": 1.2977727868692683 - }, - { - "x": 4.386013063556468, - "y": 5.886050856639685, - "heading": -0.030770580548153122, - "angularVelocity": -0.5626940348795785, - "velocityX": 0.8163340582253283, - "velocityY": -1.3129232316856283, - "timestamp": 1.32579721661469 - }, - { - "x": 4.40584258476598, - "y": 5.847627176742848, - "heading": -0.04733672158353509, - "angularVelocity": -0.5911321367061623, - "velocityX": 0.707579829086438, - "velocityY": -1.3710780289155786, - "timestamp": 1.3538216463601118 - }, - { - "x": 4.422592955723098, - "y": 5.807635111111906, - "heading": -0.06473120794982032, - "angularVelocity": -0.6206901094616508, - "velocityX": 0.5977060410962763, - "velocityY": -1.427042976225914, - "timestamp": 1.3818460761055336 - }, - { - "x": 4.436231449424729, - "y": 5.766142144918943, - "heading": -0.08298784256354313, - "angularVelocity": -0.6514542768423776, - "velocityX": 0.48666445046428625, - "velocityY": -1.480599839850087, - "timestamp": 1.4098705058509553 - }, - { - "x": 4.446724054726656, - "y": 5.723222715334596, - "heading": -0.10214308677500623, - "angularVelocity": -0.683519500145881, - "velocityX": 0.37440923498684786, - "velocityY": -1.5315005505636412, - "timestamp": 1.437894935596377 - }, - { - "x": 4.454035609213429, - "y": 5.678959179772936, - "heading": -0.12223631960196876, - "angularVelocity": -0.7169898909449176, - "velocityX": 0.2608993136770921, - "velocityY": -1.579462489112408, - "timestamp": 1.4659193653417988 - }, - { - "x": 4.458130023896409, - "y": 5.633442934952869, - "heading": -0.1433101145106275, - "angularVelocity": -0.7519794372302038, - "velocityX": 0.14610162348255104, - "velocityY": -1.624163104603462, - "timestamp": 1.4939437950872205 - }, - { - "x": 4.458970636242968, - "y": 5.586775705010787, - "heading": -0.1654105284090244, - "angularVelocity": -0.7886124391882843, - "velocityX": 0.029995698545707234, - "velocityY": -1.6652338822239472, - "timestamp": 1.5219682248326423 - }, - { - "x": 4.4565207406871075, - "y": 5.539071013689049, - "heading": -0.18858739291328477, - "angularVelocity": -0.8270235903032963, - "velocityX": -0.08741999669990082, - "velocityY": -1.7022537748348157, - "timestamp": 1.549992654578064 - }, - { - "x": 4.450744361777594, - "y": 5.490455847567595, - "heading": -0.2128945907543178, - "angularVelocity": -0.8673574471218202, - "velocityX": -0.2061194094576707, - "velocityY": -1.7347423859497373, - "timestamp": 1.5780170843234858 - }, - { - "x": 4.441607354345094, - "y": 5.441072501173689, - "heading": -0.2383902890539677, - "angularVelocity": -0.9097668902189089, - "velocityX": -0.3260372294994741, - "velocityY": -1.7621534797499967, - "timestamp": 1.6060415140689075 - }, - { - "x": 4.429078936103224, - "y": 5.39108056591739, - "heading": -0.26513708408252107, - "angularVelocity": -0.9544099655738234, - "velocityX": -0.44705345856029166, - "velocityY": -1.7838698489293998, - "timestamp": 1.6340659438143292 - }, - { - "x": 4.413133776945269, - "y": 5.340658976983783, - "heading": -0.29320198612106874, - "angularVelocity": -1.0014441790071809, - "velocityX": -0.5689735456815054, - "velocityY": -1.7992012466138827, - "timestamp": 1.662090373559751 - }, - { - "x": 4.3937547772479375, - "y": 5.290007958751612, - "heading": -0.3226561346138084, - "angularVelocity": -1.0510168720757636, - "velocityX": -0.691503801268173, - "velocityY": -1.8073880072598505, - "timestamp": 1.6901148033051727 - }, - { - "x": 4.370936649023499, - "y": 5.23935060545745, - "heading": -0.3535740801853513, - "angularVelocity": -1.10324976645044, - "velocityX": -0.8142227489273355, - "velocityY": -1.8076140622428558, - "timestamp": 1.7181392330505945 - }, - { - "x": 4.344690344443845, - "y": 5.188933704037928, - "heading": -0.3860324046425531, - "angularVelocity": -1.1582153411169682, - "velocityX": -0.9365508885668746, - "velocityY": -1.7990339813340146, - "timestamp": 1.7461636627960162 - }, - { - "x": 4.315048226527169, - "y": 5.139027274908771, - "heading": -0.42010739431834127, - "angularVelocity": -1.2159030526341073, - "velocityX": -1.0577242136931926, - "velocityY": -1.7808187207559585, - "timestamp": 1.774188092541438 - }, - { - "x": 4.2820696265426665, - "y": 5.089922231340629, - "heading": -0.45587149477143696, - "angularVelocity": -1.2761758500701854, - "velocityX": -1.1767804120934704, - "velocityY": -1.7522227575803986, - "timestamp": 1.8022125222868597 - }, - { - "x": 4.245846101927616, - "y": 5.041925630666271, - "heading": -0.4933884602431478, - "angularVelocity": -1.3387235998206146, - "velocityX": -1.292569552498, - "velocityY": -1.7126700207770804, - "timestamp": 1.8302369520322814 - }, - { - "x": 4.20650539300032, - "y": 4.995353304015392, - "heading": -0.5327075685418524, - "angularVelocity": -1.4030297371216962, - "velocityX": -1.4038005156455622, - "velocityY": -1.6618474336123745, - "timestamp": 1.8582613817777032 - }, - { - "x": 4.164212950532524, - "y": 4.950520223954836, - "heading": -0.5738579425633289, - "angularVelocity": -1.4683750711537225, - "velocityX": -1.50912767367571, - "velocityY": -1.5997856323152906, - "timestamp": 1.886285811523125 - }, - { - "x": 4.119170145989121, - "y": 4.907729664930321, - "heading": -0.6168445330043962, - "angularVelocity": -1.5338970616552692, - "velocityX": -1.6072692630172125, - "velocityY": -1.5269020427259838, - "timestamp": 1.9143102412685467 - }, - { - "x": 4.071608923365433, - "y": 4.867262744975817, - "heading": -0.6616471382328869, - "angularVelocity": -1.598698194235669, - "velocityX": -1.6971343594050328, - "velocityY": -1.4439872754632914, - "timestamp": 1.9423346710139684 - }, - { - "x": 4.021783518702593, - "y": 4.829370015961046, - "heading": -0.708222820088443, - "angularVelocity": -1.6619671578924535, - "velocityX": -1.7779275123691076, - "velocityY": -1.3521320276271316, - "timestamp": 1.9703591007593901 - }, - { - "x": 3.969960602751049, - "y": 4.79426629105861, - "heading": -0.7565108155122148, - "angularVelocity": -1.7230679040546606, - "velocityX": -1.8492050122807404, - "velocityY": -1.2526115685965424, - "timestamp": 1.9983835305048119 - }, - { - "x": 3.9164094697061533, - "y": 4.762129052357968, - "heading": -0.8064383857566846, - "angularVelocity": -1.7815731023973904, - "velocityX": -1.9108732463554607, - "velocityY": -1.1467579891038302, - "timestamp": 2.0264079602502334 - }, - { - "x": 3.8613936225255663, - "y": 4.733099950552934, - "heading": -0.8579262476213516, - "angularVelocity": -1.8372492262069025, - "velocityX": -1.9631388642109393, - "velocityY": -1.0358498663037214, - "timestamp": 2.054432389995655 - }, - { - "x": 3.805164500168681, - "y": 4.7072884154222505, - "heading": -0.910892878375897, - "angularVelocity": -1.8900163619991934, - "velocityX": -2.006432347336878, - "velocityY": -0.9210369440220643, - "timestamp": 2.082456819741077 - }, - { - "x": 3.747957470865572, - "y": 4.684776320796834, - "heading": -0.9652575650457059, - "angularVelocity": -1.9399034044105279, - "velocityX": -2.041327150018244, - "velocityY": -0.8033025053469364, - "timestamp": 2.1104812494864986 - }, - { - "x": 3.689989792880342, - "y": 4.665622858433567, - "heading": -1.0209423751702893, - "angularVelocity": -1.987009570950461, - "velocityX": -2.0684694929322522, - "velocityY": -0.683455918184957, - "timestamp": 2.1385056792319204 - }, - { - "x": 3.631460063326017, - "y": 4.6498690821793875, - "heading": -1.077873311902558, - "angularVelocity": -2.0314752967120566, - "velocityX": -2.088525264778511, - "velocityY": -0.562144400342476, - "timestamp": 2.166530108977342 - }, - { - "x": 3.572548670883797, - "y": 4.637541856462143, - "heading": -1.1359808949883092, - "angularVelocity": -2.073461747968023, - "velocityX": -2.1021441998063306, - "velocityY": -0.4398742750245512, - "timestamp": 2.194554538722764 - }, - { - "x": 3.5134188516985927, - "y": 4.628657132170972, - "heading": -1.1952003568393965, - "angularVelocity": -2.1131370874998674, - "velocityX": -2.1099383545837274, - "velocityY": -0.3170349716972457, - "timestamp": 2.2225789684681856 - }, - { - "x": 3.45421805464654, - "y": 4.623222581268303, - "heading": -1.2554715918974453, - "angularVelocity": -2.1506676712268407, - "velocityX": -2.112471068629758, - "velocityY": -0.19392190856467523, - "timestamp": 2.2506033982136073 - }, - { - "x": 3.3950794189302482, - "y": 4.621239671353537, - "heading": -1.3167389565753878, - "angularVelocity": -2.186212716351497, - "velocityX": -2.1102529562069527, - "velocityY": -0.07075647685889055, - "timestamp": 2.278627827959029 - }, - { - "x": 3.3361232418560975, - "y": 4.622705275847479, - "heading": -1.3789509864906324, - "angularVelocity": -2.219921350064421, - "velocityX": -2.103742256656607, - "velocityY": 0.052297388644645955, - "timestamp": 2.306652257704451 - }, - { - "x": 3.2774583674363726, - "y": 4.627612911455795, - "heading": -1.442060075578285, - "angularVelocity": -2.251931249304374, - "velocityX": -2.0933476596185394, - "velocityY": 0.1751199097679249, - "timestamp": 2.3346766874498726 - }, - { - "x": 3.219183461263326, - "y": 4.635953682667193, - "heading": -1.5060221459927505, - "angularVelocity": -2.2823683120585914, - "velocityX": -2.0794323632067755, - "velocityY": 0.2976250110052724, - "timestamp": 2.3627011171952943 - }, - { - "x": 3.1613881587982178, - "y": 4.647716999053955, - "heading": -1.5707963267948966, - "angularVelocity": -2.3113469708594327, - "velocityX": -2.0623185909625006, - "velocityY": 0.41975221239546007, - "timestamp": 2.390725546940716 - }, - { - "x": 3.10814561374089, - "y": 4.661599970136062, - "heading": -1.6316805279181992, - "angularVelocity": -2.337024785845805, - "velocityX": -2.0437017348473057, - "velocityY": 0.532894362108114, - "timestamp": 2.4167775603776476 - }, - { - "x": 3.055428663680876, - "y": 4.678424323710771, - "heading": -1.6931889627653025, - "angularVelocity": -2.3609858407297932, - "velocityX": -2.0235269027336233, - "velocityY": 0.6457985911698438, - "timestamp": 2.442829573814579 - }, - { - "x": 3.0032829116143174, - "y": 4.698182352912261, - "heading": -1.7552708867676743, - "angularVelocity": -2.3829990780811756, - "velocityX": -2.001601611053657, - "velocityY": 0.7584069941204404, - "timestamp": 2.4688815872515106 - }, - { - "x": 2.951759926605772, - "y": 4.720864384158654, - "heading": -1.817868495309071, - "angularVelocity": -2.4027934997397216, - "velocityX": -1.9776968537679434, - "velocityY": 0.8706440790575167, - "timestamp": 2.494933600688442 - }, - { - "x": 2.900918437449139, - "y": 4.746458145818845, - "heading": -1.8809155732314475, - "angularVelocity": -2.4200462691683855, - "velocityX": -1.951537806461976, - "velocityY": 0.9824101205132756, - "timestamp": 2.5209856141253737 - }, - { - "x": 2.850825842782793, - "y": 4.774947883106229, - "heading": -1.944335714599795, - "angularVelocity": -2.4343662159500172, - "velocityX": -1.9227916793305875, - "velocityY": 1.0935714184376848, - "timestamp": 2.5470376275623052 - }, - { - "x": 2.801560136398716, - "y": 4.806313093575505, - "heading": -2.0080399486258997, - "angularVelocity": -2.4452710413465035, - "velocityX": -1.8910517800605635, - "velocityY": 1.2039457351427163, - "timestamp": 2.5730896409992368 - }, - { - "x": 2.753212376704843, - "y": 4.84052668356083, - "heading": -2.0719235447338953, - "angularVelocity": -2.4521558098625915, - "velocityX": -1.855816626646186, - "velocityY": 1.313280068281472, - "timestamp": 2.5991416544361683 - }, - { - "x": 2.7058898612562734, - "y": 4.877552222226872, - "heading": -2.1358616797226424, - "angularVelocity": -2.454249271117854, - "velocityX": -1.816462883497472, - "velocityY": 1.421216012945561, - "timestamp": 2.6251936678731 - }, - { - "x": 2.659720184999156, - "y": 4.917339761356798, - "heading": -2.1997035219581984, - "angularVelocity": -2.450553097943996, - "velocityX": -1.7722114403511053, - "velocityY": 1.5272347078372241, - "timestamp": 2.6512456813100314 - }, - { - "x": 2.6148563151123536, - "y": 4.959819338743969, - "heading": -2.263264109416659, - "angularVelocity": -2.4397572038847217, - "velocityX": -1.7220883904196647, - "velocityY": 1.6305679209787758, - "timestamp": 2.677297694746963 - }, - { - "x": 2.57148256826981, - "y": 5.004890715522668, - "heading": -2.326313161086277, - "angularVelocity": -2.4201220309611453, - "velocityX": -1.664890391199128, - "velocityY": 1.7300534904071132, - "timestamp": 2.7033497081838944 - }, - { - "x": 2.5298205674140557, - "y": 5.052407085126601, - "heading": -2.3885596943982783, - "angularVelocity": -2.3893175651351455, - "velocityX": -1.5991854509290115, - "velocityY": 1.8239039266181656, - "timestamp": 2.729401721620826 - }, - { - "x": 2.490132038202787, - "y": 5.102149807083195, - "heading": -2.4496314755140496, - "angularVelocity": -2.3442249967978706, - "velocityX": -1.5234342369484943, - "velocityY": 1.9093619031408002, - "timestamp": 2.7554537350577575 - }, - { - "x": 2.452710284842023, - "y": 5.153792814807895, - "heading": -2.5090526704829195, - "angularVelocity": -2.2808676616384806, - "velocityX": -1.4364246146023367, - "velocityY": 1.9823038956169858, - "timestamp": 2.781505748494689 - }, - { - "x": 2.417845697613634, - "y": 5.206866100215564, - "heading": -2.5662496521434983, - "angularVelocity": -2.195491791797435, - "velocityX": -1.3382684341380409, - "velocityY": 2.037204745658009, - "timestamp": 2.8075577619316205 - }, - { - "x": 2.3857600871145697, - "y": 5.260751237983873, - "heading": -2.6206772315252245, - "angularVelocity": -2.08918898009516, - "velocityX": -1.231598109556386, - "velocityY": 2.0683674948483968, - "timestamp": 2.833609775368552 - }, - { - "x": 2.356544218134567, - "y": 5.314735552674231, - "heading": -2.6720662793567067, - "angularVelocity": -1.9725557088280796, - "velocityX": -1.1214438012912034, - "velocityY": 2.0721743761203633, - "timestamp": 2.8596617888054836 - }, - { - "x": 2.330129430188581, - "y": 5.368102945953001, - "heading": -2.7204400842018184, - "angularVelocity": -1.8568163632426362, - "velocityX": -1.0139250085193388, - "velocityY": 2.04849400250616, - "timestamp": 2.885713802242415 - }, - { - "x": 2.306309859520889, - "y": 5.420242360939423, - "heading": -2.7658578652974755, - "angularVelocity": -1.7433501332096546, - "velocityX": -0.9143082443649937, - "velocityY": 2.0013583638224843, - "timestamp": 2.9117658156793467 - }, - { - "x": 2.2848243549194245, - "y": 5.470705057059815, - "heading": -2.808343758268873, - "angularVelocity": -1.6308103430948317, - "velocityX": -0.8247157039696784, - "velocityY": 1.9369979307953407, - "timestamp": 2.937817829116278 - }, - { - "x": 2.2654228000679097, - "y": 5.519181079523684, - "heading": -2.8479456215051604, - "angularVelocity": -1.5201075852411496, - "velocityX": -0.7447238156269963, - "velocityY": 1.8607399608947834, - "timestamp": 2.9638698425532097 - }, - { - "x": 2.247888448425393, - "y": 5.565455623210483, - "heading": -2.8847464453032123, - "angularVelocity": -1.412590389112856, - "velocityX": -0.6730516888826009, - "velocityY": 1.7762367503310765, - "timestamp": 2.9899218559901413 - }, - { - "x": 2.232039117701984, - "y": 5.609376329385158, - "heading": -2.9188471410259273, - "angularVelocity": -1.308946650333392, - "velocityX": -0.6083725836308177, - "velocityY": 1.6858852879451771, - "timestamp": 3.015973869427073 - }, - { - "x": 2.2177225834921717, - "y": 5.650832312921057, - "heading": -2.9503511401259157, - "angularVelocity": -1.209273101914099, - "velocityX": -0.5495365739954405, - "velocityY": 1.591277527791001, - "timestamp": 3.0420258828640043 - }, - { - "x": 2.2048112517628295, - "y": 5.689740878391115, - "heading": -2.9793561711588477, - "angularVelocity": -1.1133508395866407, - "velocityX": -0.4955982293115555, - "velocityY": 1.4934955244149806, - "timestamp": 3.068077896300936 - }, - { - "x": 2.1931974509149272, - "y": 5.726038957702259, - "heading": -3.0059510066505495, - "angularVelocity": -1.0208360883923002, - "velocityX": -0.4457928319444466, - "velocityY": 1.3932926681086102, - "timestamp": 3.0941299097378674 - }, - { - "x": 2.1827895833104405, - "y": 5.7596774671860915, - "heading": -3.0302147916610416, - "angularVelocity": -0.931359300471352, - "velocityX": -0.399503386933393, - "velocityY": 1.2912057475045753, - "timestamp": 3.120181923174799 - }, - { - "x": 2.1735090671525814, - "y": 5.7906175048600845, - "heading": -3.0522175233133915, - "angularVelocity": -0.8445693345589079, - "velocityX": -0.3562302844779268, - "velocityY": 1.1876255840606498, - "timestamp": 3.1462339366117305 - }, - { - "x": 2.165287928866771, - "y": 5.818827731905624, - "heading": -3.0720209570909076, - "angularVelocity": -0.7601498373804071, - "velocityX": -0.31556633062986994, - "velocityY": 1.082842487926418, - "timestamp": 3.172285950048662 - }, - { - "x": 2.15806690849839, - "y": 5.844282532149564, - "heading": -3.0896796043938592, - "angularVelocity": -0.6778227466257346, - "velocityX": -0.27717705527303127, - "velocityY": 0.9770761214123779, - "timestamp": 3.1983379634855935 - }, - { - "x": 2.151793962845926, - "y": 5.866960693109083, - "heading": -3.1052416789306485, - "angularVelocity": -0.5973463269724832, - "velocityX": -0.2407854451499951, - "velocityY": 0.8704955190668501, - "timestamp": 3.224389976922525 - }, - { - "x": 2.1464230753720366, - "y": 5.8868444435667495, - "heading": -3.1187499404873535, - "angularVelocity": -0.5185112309805378, - "velocityX": -0.2061601682684892, - "velocityY": 0.7632327729986262, - "timestamp": 3.2504419903594566 - }, - { - "x": 2.1419133029864885, - "y": 5.90391873943064, - "heading": -3.1302424255201187, - "angularVelocity": -0.4411361548153143, - "velocityX": -0.17310648163399853, - "velocityY": 0.6553925632360712, - "timestamp": 3.276494003796388 - }, - { - "x": 2.1382280065351003, - "y": 5.918170725554471, - "heading": -3.139753071131935, - "angularVelocity": -0.36506374583446893, - "velocityX": -0.1414591797409874, - "velocityY": 0.5470589119083662, - "timestamp": 3.3025460172333196 - }, - { - "x": 2.135334224654641, - "y": 5.9295893243238895, - "heading": -3.1473122451070203, - "angularVelocity": -0.2901569966323474, - "velocityX": -0.11107709150637847, - "velocityY": 0.43830004913290593, - "timestamp": 3.328598030670251 - }, - { - "x": 2.133202160318781, - "y": 5.9381649169747, - "heading": -3.1529471958838693, - "angularVelocity": -0.2162961719058987, - "velocityX": -0.08183875465217869, - "velocityY": 0.32917197250689534, - "timestamp": 3.3546500441071827 - }, - { - "x": 2.131804756638863, - "y": 5.943889093714475, - "heading": -3.156682435526014, - "angularVelocity": -0.14337623658866083, - "velocityX": -0.053638989681216866, - "velocityY": 0.2197210881082821, - "timestamp": 3.3807020575441142 - }, - { - "x": 2.131117343902588, - "y": 5.946754455566406, - "heading": -3.158540067224243, - "angularVelocity": -0.07130472670475889, - "velocityX": -0.026386165427844832, - "velocityY": 0.10998619584120908, - "timestamp": 3.4067540709810458 - }, - { - "x": 2.131117343902588, - "y": 5.946754455566406, - "heading": -3.158540067224243, - "angularVelocity": -1.1257235159322888e-32, - "velocityX": -4.497825258620012e-34, - "velocityY": 8.02311483706782e-33, - "timestamp": 3.4328060844179773 - } - ] + "samples": [ + { + "x": 2.1199185848236084, + "y": 5.957952976226807, + "heading": 9.721358924197938e-33, + "angularVelocity": 0, + "velocityX": 7.529723953218568e-33, + "velocityY": 4.103114367587547e-33, + "timestamp": 0, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.12408359184855, + "y": 5.959490957092025, + "heading": 0.0006587462568403469, + "angularVelocity": 0.020752025490549227, + "velocityX": 0.13120732156333928, + "velocityY": 0.04844994227681366, + "timestamp": 0.031743708928096186, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.132419910947834, + "y": 5.962549984290152, + "heading": 0.0019689340924026384, + "angularVelocity": 0.04127393678319714, + "velocityX": 0.26261326671585045, + "velocityY": 0.0963664077520038, + "timestamp": 0.06348741785619237, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.144934423626495, + "y": 5.967111326975421, + "heading": 0.003922497731913535, + "angularVelocity": 0.06154175757899709, + "velocityX": 0.3942359951387039, + "velocityY": 0.1436928084112908, + "timestamp": 0.09523112678428855, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.161634668441711, + "y": 5.9731541575830684, + "heading": 0.006510477776343394, + "angularVelocity": 0.0815273366540852, + "velocityX": 0.5260962054889244, + "velocityY": 0.19036309277324892, + "timestamp": 0.12697483571238474, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.182528936666272, + "y": 5.980655180863507, + "heading": 0.00972285754365477, + "angularVelocity": 0.10119736715669446, + "velocityX": 0.6582176100432996, + "velocityY": 0.2362995230780409, + "timestamp": 0.15871854464048094, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.2076263864352867, + "y": 5.98958817047729, + "heading": 0.013548358655846408, + "angularVelocity": 0.12051210275577494, + "velocityX": 0.7906275169629459, + "velocityY": 0.2814097632390032, + "timestamp": 0.19046225356857713, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.2369371796137325, + "y": 5.999923382651596, + "heading": 0.017974182463991455, + "angularVelocity": 0.139423651412969, + "velocityX": 0.9233575460523492, + "velocityY": 0.32558300599701506, + "timestamp": 0.22220596249667332, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.270472646569135, + "y": 6.0116268035707865, + "heading": 0.022985678275806047, + "angularVelocity": 0.15787366949359927, + "velocityX": 1.0564445078350972, + "velocityY": 0.36868473516117956, + "timestamp": 0.2539496714247695, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.3082454850008034, + "y": 6.024659167637654, + "heading": 0.02856591078801465, + "angularVelocity": 0.17579018648543426, + "velocityX": 1.1899314764141191, + "velocityY": 0.41054950750668673, + "timestamp": 0.2856933803528657, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.350269999641707, + "y": 6.03897465321323, + "heading": 0.034695085760854436, + "angularVelocity": 0.1930831392993197, + "velocityX": 1.323869077053848, + "velocityY": 0.4509707926064986, + "timestamp": 0.31743708928096187, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.3965623891443495, + "y": 6.054519113336024, + "heading": 0.0413497715036912, + "angularVelocity": 0.20963793984853818, + "velocityX": 1.458316972585085, + "velocityY": 0.48968632361146647, + "timestamp": 0.34918079820905806, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.4471410825572004, + "y": 6.071227617313664, + "heading": 0.04850181812451799, + "angularVelocity": 0.2253059539144659, + "velocityX": 1.5933454256218078, + "velocityY": 0.5263563881424058, + "timestamp": 0.38092450713715426, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.5020271147547666, + "y": 6.089020938374403, + "heading": 0.05611681521560166, + "angularVelocity": 0.23988996082133435, + "velocityX": 1.7290365256905385, + "velocityY": 0.5605306267468754, + "timestamp": 0.41266821606525045, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.561244492876039, + "y": 6.107800369655392, + "heading": 0.06415181877617589, + "angularVelocity": 0.2531211327187972, + "velocityX": 1.8654839059735924, + "velocityY": 0.5915953716537398, + "timestamp": 0.44441192499334664, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.6248204026171256, + "y": 6.127439774433694, + "heading": 0.07255187184184077, + "angularVelocity": 0.26462103356269817, + "velocityX": 2.0027876983466375, + "velocityY": 0.6186865190448766, + "timestamp": 0.47615563392144283, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.6927848123865834, + "y": 6.14777283178745, + "heading": 0.08124443551167423, + "angularVelocity": 0.2738357918264941, + "velocityX": 2.141035564665967, + "velocityY": 0.6405381740302919, + "timestamp": 0.507899342849539, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.7651681706057807, + "y": 6.1685714629691, + "heading": 0.09012999755945103, + "angularVelocity": 0.27991568559003044, + "velocityX": 2.280242626441548, + "velocityY": 0.655204822749479, + "timestamp": 0.5396430517776352, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.8419931281714104, + "y": 6.189507110191248, + "heading": 0.09906528191141717, + "angularVelocity": 0.28148205278115124, + "velocityX": 2.420163243672891, + "velocityY": 0.65952114384502, + "timestamp": 0.5713867607057314, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.923246509702383, + "y": 6.21007727563667, + "heading": 0.10783160033295475, + "angularVelocity": 0.27615923650876134, + "velocityX": 2.559668806030865, + "velocityY": 0.6480076254481864, + "timestamp": 0.6031304696338275, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.0087812329232086, + "y": 6.229466168623038, + "heading": 0.11607630855981195, + "angularVelocity": 0.2597273130727852, + "velocityX": 2.694540937688452, + "velocityY": 0.6107948201729568, + "timestamp": 0.6348741785619237, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.0979818735027176, + "y": 6.246361893248638, + "heading": 0.12324903142117805, + "angularVelocity": 0.22595730314988358, + "velocityX": 2.810025784370449, + "velocityY": 0.5322542700937218, + "timestamp": 0.6666178874900199, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.1891159324545155, + "y": 6.259285555621032, + "heading": 0.12873166157260418, + "angularVelocity": 0.1727154871488241, + "velocityX": 2.8709329195972866, + "velocityY": 0.40712515357501006, + "timestamp": 0.6983615964181161, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.2799147655356333, + "y": 6.267810501767619, + "heading": 0.13233373871668666, + "angularVelocity": 0.1134737327714805, + "velocityX": 2.860372531980369, + "velocityY": 0.26855545348842214, + "timestamp": 0.7301053053462123, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.368930673043237, + "y": 6.2722939827484225, + "heading": 0.13423316054281098, + "angularVelocity": 0.05983616566124247, + "velocityX": 2.8042062667984893, + "velocityY": 0.14123998525071657, + "timestamp": 0.7618490142743085, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.455418033445333, + "y": 6.273149165824332, + "heading": 0.13460089895534874, + "angularVelocity": 0.011584607626349263, + "velocityX": 2.7245512047125473, + "velocityY": 0.026940238075252306, + "timestamp": 0.7935927232024047, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.53897038421859, + "y": 6.270691577134489, + "heading": 0.1335628529939654, + "angularVelocity": -0.03270084046371159, + "velocityX": 2.6320916362516953, + "velocityY": -0.07741970843436734, + "timestamp": 0.8253364321305009, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.619343411211179, + "y": 6.2651515676417615, + "heading": 0.13121159908477192, + "angularVelocity": -0.07406991774402583, + "velocityX": 2.531935608867881, + "velocityY": -0.17452306865816167, + "timestamp": 0.8570801410585971, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.696377587829814, + "y": 6.256700197632999, + "heading": 0.12761684069301102, + "angularVelocity": -0.11324317520377067, + "velocityX": 2.42675412608914, + "velocityY": -0.26623763555482466, + "timestamp": 0.8888238499866933, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.7699618707808793, + "y": 6.245468037234803, + "heading": 0.1228324760673318, + "angularVelocity": -0.15071851359642613, + "velocityX": 2.318074523608403, + "velocityY": -0.35383894250147446, + "timestamp": 0.9205675589147895, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.8400150685867502, + "y": 6.2315574365397515, + "heading": 0.11690123481863413, + "angularVelocity": -0.18684777075463518, + "velocityX": 2.2068372024374066, + "velocityY": -0.438215985616424, + "timestamp": 0.9523112678428857, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.906475492488988, + "y": 6.215050519883858, + "heading": 0.10985775018096655, + "angularVelocity": -0.22188600121123825, + "velocityX": 2.0936565431839025, + "velocityY": -0.5200059228516231, + "timestamp": 0.9840549767709819, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.9692948172312557, + "y": 6.196014512598508, + "heading": 0.10173064016138685, + "angularVelocity": -0.2560226984814519, + "velocityX": 1.9789535269668237, + "velocityY": -0.5996781071948685, + "timestamp": 1.015798685699078, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.028434236538794, + "y": 6.174505389403469, + "heading": 0.09254395303933619, + "angularVelocity": -0.2894018195183511, + "velocityX": 1.8630280236469257, + "velocityY": -0.6775869588445468, + "timestamp": 1.047542394627174, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.083861945125214, + "y": 6.150570441683336, + "heading": 0.08231819631194325, + "angularVelocity": -0.3221349071262287, + "velocityX": 1.7461005804951526, + "velocityY": -0.7540060228735362, + "timestamp": 1.07928610355527, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.135551426431915, + "y": 6.124250129459578, + "heading": 0.07107108601922003, + "angularVelocity": -0.35430989863852635, + "velocityX": 1.6283378046271129, + "velocityY": -0.8291505029665418, + "timestamp": 1.1110298124833662, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.183480250707313, + "y": 6.0955794475298, + "heading": 0.05881810384556654, + "angularVelocity": -0.38599718140717787, + "velocityX": 1.50986843988397, + "velocityY": -0.9031925662724984, + "timestamp": 1.1427735214114623, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.227629208104442, + "y": 6.064588953496888, + "heading": 0.04557291906848408, + "angularVelocity": -0.4172538504270615, + "velocityX": 1.3907939206829834, + "velocityY": -0.9762719946527232, + "timestamp": 1.1745172303395583, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.267981668593037, + "y": 6.031305555215571, + "heading": 0.031347713474676034, + "angularVelocity": -0.44812676508695654, + "velocityX": 1.2711955171966314, + "velocityY": -1.0485037635868417, + "timestamp": 1.2062609392676544, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.3045230996015675, + "y": 5.99575312358408, + "heading": 0.016153435256918744, + "angularVelocity": -0.47865478643897924, + "velocityX": 1.1511393042098552, + "velocityY": -1.1199835442045625, + "timestamp": 1.2380046481957505, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.337240695953369, + "y": 5.957952976226807, + "heading": 0, + "angularVelocity": -0.5088704440149149, + "velocityX": 1.030679698642281, + "velocityY": -1.1907917705171907, + "timestamp": 1.2697483571238466, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.3631357670929365, + "y": 5.922844781507193, + "heading": -0.015001401099503393, + "angularVelocity": -0.5352972829698707, + "velocityX": 0.924017772165235, + "velocityY": -1.252771065764447, + "timestamp": 1.2977727868692683, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.386013063556468, + "y": 5.886050856639685, + "heading": -0.030770580548153122, + "angularVelocity": -0.5626940348795785, + "velocityX": 0.8163340582253283, + "velocityY": -1.3129232316856283, + "timestamp": 1.32579721661469, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.40584258476598, + "y": 5.847627176742848, + "heading": -0.04733672158353509, + "angularVelocity": -0.5911321367061623, + "velocityX": 0.707579829086438, + "velocityY": -1.3710780289155786, + "timestamp": 1.3538216463601118, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.422592955723098, + "y": 5.807635111111906, + "heading": -0.06473120794982032, + "angularVelocity": -0.6206901094616508, + "velocityX": 0.5977060410962763, + "velocityY": -1.427042976225914, + "timestamp": 1.3818460761055336, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.436231449424729, + "y": 5.766142144918943, + "heading": -0.08298784256354313, + "angularVelocity": -0.6514542768423776, + "velocityX": 0.48666445046428625, + "velocityY": -1.480599839850087, + "timestamp": 1.4098705058509553, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.446724054726656, + "y": 5.723222715334596, + "heading": -0.10214308677500623, + "angularVelocity": -0.683519500145881, + "velocityX": 0.37440923498684786, + "velocityY": -1.5315005505636412, + "timestamp": 1.437894935596377, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.454035609213429, + "y": 5.678959179772936, + "heading": -0.12223631960196876, + "angularVelocity": -0.7169898909449176, + "velocityX": 0.2608993136770921, + "velocityY": -1.579462489112408, + "timestamp": 1.4659193653417988, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.458130023896409, + "y": 5.633442934952869, + "heading": -0.1433101145106275, + "angularVelocity": -0.7519794372302038, + "velocityX": 0.14610162348255104, + "velocityY": -1.624163104603462, + "timestamp": 1.4939437950872205, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.458970636242968, + "y": 5.586775705010787, + "heading": -0.1654105284090244, + "angularVelocity": -0.7886124391882843, + "velocityX": 0.029995698545707234, + "velocityY": -1.6652338822239472, + "timestamp": 1.5219682248326423, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.4565207406871075, + "y": 5.539071013689049, + "heading": -0.18858739291328477, + "angularVelocity": -0.8270235903032963, + "velocityX": -0.08741999669990082, + "velocityY": -1.7022537748348157, + "timestamp": 1.549992654578064, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.450744361777594, + "y": 5.490455847567595, + "heading": -0.2128945907543178, + "angularVelocity": -0.8673574471218202, + "velocityX": -0.2061194094576707, + "velocityY": -1.7347423859497373, + "timestamp": 1.5780170843234858, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.441607354345094, + "y": 5.441072501173689, + "heading": -0.2383902890539677, + "angularVelocity": -0.9097668902189089, + "velocityX": -0.3260372294994741, + "velocityY": -1.7621534797499967, + "timestamp": 1.6060415140689075, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.429078936103224, + "y": 5.39108056591739, + "heading": -0.26513708408252107, + "angularVelocity": -0.9544099655738234, + "velocityX": -0.44705345856029166, + "velocityY": -1.7838698489293998, + "timestamp": 1.6340659438143292, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.413133776945269, + "y": 5.340658976983783, + "heading": -0.29320198612106874, + "angularVelocity": -1.0014441790071809, + "velocityX": -0.5689735456815054, + "velocityY": -1.7992012466138827, + "timestamp": 1.662090373559751, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.3937547772479375, + "y": 5.290007958751612, + "heading": -0.3226561346138084, + "angularVelocity": -1.0510168720757636, + "velocityX": -0.691503801268173, + "velocityY": -1.8073880072598505, + "timestamp": 1.6901148033051727, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.370936649023499, + "y": 5.23935060545745, + "heading": -0.3535740801853513, + "angularVelocity": -1.10324976645044, + "velocityX": -0.8142227489273355, + "velocityY": -1.8076140622428558, + "timestamp": 1.7181392330505945, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.344690344443845, + "y": 5.188933704037928, + "heading": -0.3860324046425531, + "angularVelocity": -1.1582153411169682, + "velocityX": -0.9365508885668746, + "velocityY": -1.7990339813340146, + "timestamp": 1.7461636627960162, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.315048226527169, + "y": 5.139027274908771, + "heading": -0.42010739431834127, + "angularVelocity": -1.2159030526341073, + "velocityX": -1.0577242136931926, + "velocityY": -1.7808187207559585, + "timestamp": 1.774188092541438, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.2820696265426665, + "y": 5.089922231340629, + "heading": -0.45587149477143696, + "angularVelocity": -1.2761758500701854, + "velocityX": -1.1767804120934704, + "velocityY": -1.7522227575803986, + "timestamp": 1.8022125222868597, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.245846101927616, + "y": 5.041925630666271, + "heading": -0.4933884602431478, + "angularVelocity": -1.3387235998206146, + "velocityX": -1.292569552498, + "velocityY": -1.7126700207770804, + "timestamp": 1.8302369520322814, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.20650539300032, + "y": 4.995353304015392, + "heading": -0.5327075685418524, + "angularVelocity": -1.4030297371216962, + "velocityX": -1.4038005156455622, + "velocityY": -1.6618474336123745, + "timestamp": 1.8582613817777032, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.164212950532524, + "y": 4.950520223954836, + "heading": -0.5738579425633289, + "angularVelocity": -1.4683750711537225, + "velocityX": -1.50912767367571, + "velocityY": -1.5997856323152906, + "timestamp": 1.886285811523125, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.119170145989121, + "y": 4.907729664930321, + "heading": -0.6168445330043962, + "angularVelocity": -1.5338970616552692, + "velocityX": -1.6072692630172125, + "velocityY": -1.5269020427259838, + "timestamp": 1.9143102412685467, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.071608923365433, + "y": 4.867262744975817, + "heading": -0.6616471382328869, + "angularVelocity": -1.598698194235669, + "velocityX": -1.6971343594050328, + "velocityY": -1.4439872754632914, + "timestamp": 1.9423346710139684, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.021783518702593, + "y": 4.829370015961046, + "heading": -0.708222820088443, + "angularVelocity": -1.6619671578924535, + "velocityX": -1.7779275123691076, + "velocityY": -1.3521320276271316, + "timestamp": 1.9703591007593901, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.969960602751049, + "y": 4.79426629105861, + "heading": -0.7565108155122148, + "angularVelocity": -1.7230679040546606, + "velocityX": -1.8492050122807404, + "velocityY": -1.2526115685965424, + "timestamp": 1.9983835305048119, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.9164094697061533, + "y": 4.762129052357968, + "heading": -0.8064383857566846, + "angularVelocity": -1.7815731023973904, + "velocityX": -1.9108732463554607, + "velocityY": -1.1467579891038302, + "timestamp": 2.0264079602502334, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.8613936225255663, + "y": 4.733099950552934, + "heading": -0.8579262476213516, + "angularVelocity": -1.8372492262069025, + "velocityX": -1.9631388642109393, + "velocityY": -1.0358498663037214, + "timestamp": 2.054432389995655, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.805164500168681, + "y": 4.7072884154222505, + "heading": -0.910892878375897, + "angularVelocity": -1.8900163619991934, + "velocityX": -2.006432347336878, + "velocityY": -0.9210369440220643, + "timestamp": 2.082456819741077, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.747957470865572, + "y": 4.684776320796834, + "heading": -0.9652575650457059, + "angularVelocity": -1.9399034044105279, + "velocityX": -2.041327150018244, + "velocityY": -0.8033025053469364, + "timestamp": 2.1104812494864986, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.689989792880342, + "y": 4.665622858433567, + "heading": -1.0209423751702893, + "angularVelocity": -1.987009570950461, + "velocityX": -2.0684694929322522, + "velocityY": -0.683455918184957, + "timestamp": 2.1385056792319204, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.631460063326017, + "y": 4.6498690821793875, + "heading": -1.077873311902558, + "angularVelocity": -2.0314752967120566, + "velocityX": -2.088525264778511, + "velocityY": -0.562144400342476, + "timestamp": 2.166530108977342, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.572548670883797, + "y": 4.637541856462143, + "heading": -1.1359808949883092, + "angularVelocity": -2.073461747968023, + "velocityX": -2.1021441998063306, + "velocityY": -0.4398742750245512, + "timestamp": 2.194554538722764, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.5134188516985927, + "y": 4.628657132170972, + "heading": -1.1952003568393965, + "angularVelocity": -2.1131370874998674, + "velocityX": -2.1099383545837274, + "velocityY": -0.3170349716972457, + "timestamp": 2.2225789684681856, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.45421805464654, + "y": 4.623222581268303, + "heading": -1.2554715918974453, + "angularVelocity": -2.1506676712268407, + "velocityX": -2.112471068629758, + "velocityY": -0.19392190856467523, + "timestamp": 2.2506033982136073, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.3950794189302482, + "y": 4.621239671353537, + "heading": -1.3167389565753878, + "angularVelocity": -2.186212716351497, + "velocityX": -2.1102529562069527, + "velocityY": -0.07075647685889055, + "timestamp": 2.278627827959029, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.3361232418560975, + "y": 4.622705275847479, + "heading": -1.3789509864906324, + "angularVelocity": -2.219921350064421, + "velocityX": -2.103742256656607, + "velocityY": 0.052297388644645955, + "timestamp": 2.306652257704451, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.2774583674363726, + "y": 4.627612911455795, + "heading": -1.442060075578285, + "angularVelocity": -2.251931249304374, + "velocityX": -2.0933476596185394, + "velocityY": 0.1751199097679249, + "timestamp": 2.3346766874498726, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.219183461263326, + "y": 4.635953682667193, + "heading": -1.5060221459927505, + "angularVelocity": -2.2823683120585914, + "velocityX": -2.0794323632067755, + "velocityY": 0.2976250110052724, + "timestamp": 2.3627011171952943, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.1613881587982178, + "y": 4.647716999053955, + "heading": -1.5707963267948966, + "angularVelocity": -2.3113469708594327, + "velocityX": -2.0623185909625006, + "velocityY": 0.41975221239546007, + "timestamp": 2.390725546940716, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.10814561374089, + "y": 4.661599970136062, + "heading": -1.6316805279181992, + "angularVelocity": -2.337024785845805, + "velocityX": -2.0437017348473057, + "velocityY": 0.532894362108114, + "timestamp": 2.4167775603776476, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.055428663680876, + "y": 4.678424323710771, + "heading": -1.6931889627653025, + "angularVelocity": -2.3609858407297932, + "velocityX": -2.0235269027336233, + "velocityY": 0.6457985911698438, + "timestamp": 2.442829573814579, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.0032829116143174, + "y": 4.698182352912261, + "heading": -1.7552708867676743, + "angularVelocity": -2.3829990780811756, + "velocityX": -2.001601611053657, + "velocityY": 0.7584069941204404, + "timestamp": 2.4688815872515106, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.951759926605772, + "y": 4.720864384158654, + "heading": -1.817868495309071, + "angularVelocity": -2.4027934997397216, + "velocityX": -1.9776968537679434, + "velocityY": 0.8706440790575167, + "timestamp": 2.494933600688442, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.900918437449139, + "y": 4.746458145818845, + "heading": -1.8809155732314475, + "angularVelocity": -2.4200462691683855, + "velocityX": -1.951537806461976, + "velocityY": 0.9824101205132756, + "timestamp": 2.5209856141253737, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.850825842782793, + "y": 4.774947883106229, + "heading": -1.944335714599795, + "angularVelocity": -2.4343662159500172, + "velocityX": -1.9227916793305875, + "velocityY": 1.0935714184376848, + "timestamp": 2.5470376275623052, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.801560136398716, + "y": 4.806313093575505, + "heading": -2.0080399486258997, + "angularVelocity": -2.4452710413465035, + "velocityX": -1.8910517800605635, + "velocityY": 1.2039457351427163, + "timestamp": 2.5730896409992368, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.753212376704843, + "y": 4.84052668356083, + "heading": -2.0719235447338953, + "angularVelocity": -2.4521558098625915, + "velocityX": -1.855816626646186, + "velocityY": 1.313280068281472, + "timestamp": 2.5991416544361683, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.7058898612562734, + "y": 4.877552222226872, + "heading": -2.1358616797226424, + "angularVelocity": -2.454249271117854, + "velocityX": -1.816462883497472, + "velocityY": 1.421216012945561, + "timestamp": 2.6251936678731, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.659720184999156, + "y": 4.917339761356798, + "heading": -2.1997035219581984, + "angularVelocity": -2.450553097943996, + "velocityX": -1.7722114403511053, + "velocityY": 1.5272347078372241, + "timestamp": 2.6512456813100314, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.6148563151123536, + "y": 4.959819338743969, + "heading": -2.263264109416659, + "angularVelocity": -2.4397572038847217, + "velocityX": -1.7220883904196647, + "velocityY": 1.6305679209787758, + "timestamp": 2.677297694746963, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.57148256826981, + "y": 5.004890715522668, + "heading": -2.326313161086277, + "angularVelocity": -2.4201220309611453, + "velocityX": -1.664890391199128, + "velocityY": 1.7300534904071132, + "timestamp": 2.7033497081838944, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.5298205674140557, + "y": 5.052407085126601, + "heading": -2.3885596943982783, + "angularVelocity": -2.3893175651351455, + "velocityX": -1.5991854509290115, + "velocityY": 1.8239039266181656, + "timestamp": 2.729401721620826, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.490132038202787, + "y": 5.102149807083195, + "heading": -2.4496314755140496, + "angularVelocity": -2.3442249967978706, + "velocityX": -1.5234342369484943, + "velocityY": 1.9093619031408002, + "timestamp": 2.7554537350577575, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.452710284842023, + "y": 5.153792814807895, + "heading": -2.5090526704829195, + "angularVelocity": -2.2808676616384806, + "velocityX": -1.4364246146023367, + "velocityY": 1.9823038956169858, + "timestamp": 2.781505748494689, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.417845697613634, + "y": 5.206866100215564, + "heading": -2.5662496521434983, + "angularVelocity": -2.195491791797435, + "velocityX": -1.3382684341380409, + "velocityY": 2.037204745658009, + "timestamp": 2.8075577619316205, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.3857600871145697, + "y": 5.260751237983873, + "heading": -2.6206772315252245, + "angularVelocity": -2.08918898009516, + "velocityX": -1.231598109556386, + "velocityY": 2.0683674948483968, + "timestamp": 2.833609775368552, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.356544218134567, + "y": 5.314735552674231, + "heading": -2.6720662793567067, + "angularVelocity": -1.9725557088280796, + "velocityX": -1.1214438012912034, + "velocityY": 2.0721743761203633, + "timestamp": 2.8596617888054836, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.330129430188581, + "y": 5.368102945953001, + "heading": -2.7204400842018184, + "angularVelocity": -1.8568163632426362, + "velocityX": -1.0139250085193388, + "velocityY": 2.04849400250616, + "timestamp": 2.885713802242415, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.306309859520889, + "y": 5.420242360939423, + "heading": -2.7658578652974755, + "angularVelocity": -1.7433501332096546, + "velocityX": -0.9143082443649937, + "velocityY": 2.0013583638224843, + "timestamp": 2.9117658156793467, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.2848243549194245, + "y": 5.470705057059815, + "heading": -2.808343758268873, + "angularVelocity": -1.6308103430948317, + "velocityX": -0.8247157039696784, + "velocityY": 1.9369979307953407, + "timestamp": 2.937817829116278, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.2654228000679097, + "y": 5.519181079523684, + "heading": -2.8479456215051604, + "angularVelocity": -1.5201075852411496, + "velocityX": -0.7447238156269963, + "velocityY": 1.8607399608947834, + "timestamp": 2.9638698425532097, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.247888448425393, + "y": 5.565455623210483, + "heading": -2.8847464453032123, + "angularVelocity": -1.412590389112856, + "velocityX": -0.6730516888826009, + "velocityY": 1.7762367503310765, + "timestamp": 2.9899218559901413, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.232039117701984, + "y": 5.609376329385158, + "heading": -2.9188471410259273, + "angularVelocity": -1.308946650333392, + "velocityX": -0.6083725836308177, + "velocityY": 1.6858852879451771, + "timestamp": 3.015973869427073, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.2177225834921717, + "y": 5.650832312921057, + "heading": -2.9503511401259157, + "angularVelocity": -1.209273101914099, + "velocityX": -0.5495365739954405, + "velocityY": 1.591277527791001, + "timestamp": 3.0420258828640043, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.2048112517628295, + "y": 5.689740878391115, + "heading": -2.9793561711588477, + "angularVelocity": -1.1133508395866407, + "velocityX": -0.4955982293115555, + "velocityY": 1.4934955244149806, + "timestamp": 3.068077896300936, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.1931974509149272, + "y": 5.726038957702259, + "heading": -3.0059510066505495, + "angularVelocity": -1.0208360883923002, + "velocityX": -0.4457928319444466, + "velocityY": 1.3932926681086102, + "timestamp": 3.0941299097378674, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.1827895833104405, + "y": 5.7596774671860915, + "heading": -3.0302147916610416, + "angularVelocity": -0.931359300471352, + "velocityX": -0.399503386933393, + "velocityY": 1.2912057475045753, + "timestamp": 3.120181923174799, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.1735090671525814, + "y": 5.7906175048600845, + "heading": -3.0522175233133915, + "angularVelocity": -0.8445693345589079, + "velocityX": -0.3562302844779268, + "velocityY": 1.1876255840606498, + "timestamp": 3.1462339366117305, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.165287928866771, + "y": 5.818827731905624, + "heading": -3.0720209570909076, + "angularVelocity": -0.7601498373804071, + "velocityX": -0.31556633062986994, + "velocityY": 1.082842487926418, + "timestamp": 3.172285950048662, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.15806690849839, + "y": 5.844282532149564, + "heading": -3.0896796043938592, + "angularVelocity": -0.6778227466257346, + "velocityX": -0.27717705527303127, + "velocityY": 0.9770761214123779, + "timestamp": 3.1983379634855935, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.151793962845926, + "y": 5.866960693109083, + "heading": -3.1052416789306485, + "angularVelocity": -0.5973463269724832, + "velocityX": -0.2407854451499951, + "velocityY": 0.8704955190668501, + "timestamp": 3.224389976922525, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.1464230753720366, + "y": 5.8868444435667495, + "heading": -3.1187499404873535, + "angularVelocity": -0.5185112309805378, + "velocityX": -0.2061601682684892, + "velocityY": 0.7632327729986262, + "timestamp": 3.2504419903594566, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.1419133029864885, + "y": 5.90391873943064, + "heading": -3.1302424255201187, + "angularVelocity": -0.4411361548153143, + "velocityX": -0.17310648163399853, + "velocityY": 0.6553925632360712, + "timestamp": 3.276494003796388, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.1382280065351003, + "y": 5.918170725554471, + "heading": -3.139753071131935, + "angularVelocity": -0.36506374583446893, + "velocityX": -0.1414591797409874, + "velocityY": 0.5470589119083662, + "timestamp": 3.3025460172333196, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.135334224654641, + "y": 5.9295893243238895, + "heading": -3.1473122451070203, + "angularVelocity": -0.2901569966323474, + "velocityX": -0.11107709150637847, + "velocityY": 0.43830004913290593, + "timestamp": 3.328598030670251, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.133202160318781, + "y": 5.9381649169747, + "heading": -3.1529471958838693, + "angularVelocity": -0.2162961719058987, + "velocityX": -0.08183875465217869, + "velocityY": 0.32917197250689534, + "timestamp": 3.3546500441071827, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.131804756638863, + "y": 5.943889093714475, + "heading": -3.156682435526014, + "angularVelocity": -0.14337623658866083, + "velocityX": -0.053638989681216866, + "velocityY": 0.2197210881082821, + "timestamp": 3.3807020575441142, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.131117343902588, + "y": 5.946754455566406, + "heading": -3.158540067224243, + "angularVelocity": -0.07130472670475889, + "velocityX": -0.026386165427844832, + "velocityY": 0.10998619584120908, + "timestamp": 3.4067540709810458, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.131117343902588, + "y": 5.946754455566406, + "heading": -3.158540067224243, + "angularVelocity": -1.1257235159322888e-32, + "velocityX": -4.497825258620012e-34, + "velocityY": 8.02311483706782e-33, + "timestamp": 3.4328060844179773, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/test.traj b/src/main/deploy/choreo/test.traj index 29e1ba4e..43454f70 100644 --- a/src/main/deploy/choreo/test.traj +++ b/src/main/deploy/choreo/test.traj @@ -1,1093 +1,2546 @@ { - "samples": [ - { - "x": 2.1199185848236084, - "y": 5.957952976226807, - "heading": 9.721358924197938e-33, - "angularVelocity": 0, - "velocityX": 7.529723953218568e-33, - "velocityY": 4.103114367587547e-33, - "timestamp": 0 - }, - { - "x": 2.12408359184855, - "y": 5.959490957092025, - "heading": 0.0006587462568403469, - "angularVelocity": 0.020752025490549227, - "velocityX": 0.13120732156333928, - "velocityY": 0.04844994227681366, - "timestamp": 0.031743708928096186 - }, - { - "x": 2.132419910947834, - "y": 5.962549984290152, - "heading": 0.0019689340924026384, - "angularVelocity": 0.04127393678319714, - "velocityX": 0.26261326671585045, - "velocityY": 0.0963664077520038, - "timestamp": 0.06348741785619237 - }, - { - "x": 2.144934423626495, - "y": 5.967111326975421, - "heading": 0.003922497731913535, - "angularVelocity": 0.06154175757899709, - "velocityX": 0.3942359951387039, - "velocityY": 0.1436928084112908, - "timestamp": 0.09523112678428855 - }, - { - "x": 2.161634668441711, - "y": 5.9731541575830684, - "heading": 0.006510477776343394, - "angularVelocity": 0.0815273366540852, - "velocityX": 0.5260962054889244, - "velocityY": 0.19036309277324892, - "timestamp": 0.12697483571238474 - }, - { - "x": 2.182528936666272, - "y": 5.980655180863507, - "heading": 0.00972285754365477, - "angularVelocity": 0.10119736715669446, - "velocityX": 0.6582176100432996, - "velocityY": 0.2362995230780409, - "timestamp": 0.15871854464048094 - }, - { - "x": 2.2076263864352867, - "y": 5.98958817047729, - "heading": 0.013548358655846408, - "angularVelocity": 0.12051210275577494, - "velocityX": 0.7906275169629459, - "velocityY": 0.2814097632390032, - "timestamp": 0.19046225356857713 - }, - { - "x": 2.2369371796137325, - "y": 5.999923382651596, - "heading": 0.017974182463991455, - "angularVelocity": 0.139423651412969, - "velocityX": 0.9233575460523492, - "velocityY": 0.32558300599701506, - "timestamp": 0.22220596249667332 - }, - { - "x": 2.270472646569135, - "y": 6.0116268035707865, - "heading": 0.022985678275806047, - "angularVelocity": 0.15787366949359927, - "velocityX": 1.0564445078350972, - "velocityY": 0.36868473516117956, - "timestamp": 0.2539496714247695 - }, - { - "x": 2.3082454850008034, - "y": 6.024659167637654, - "heading": 0.02856591078801465, - "angularVelocity": 0.17579018648543426, - "velocityX": 1.1899314764141191, - "velocityY": 0.41054950750668673, - "timestamp": 0.2856933803528657 - }, - { - "x": 2.350269999641707, - "y": 6.03897465321323, - "heading": 0.034695085760854436, - "angularVelocity": 0.1930831392993197, - "velocityX": 1.323869077053848, - "velocityY": 0.4509707926064986, - "timestamp": 0.31743708928096187 - }, - { - "x": 2.3965623891443495, - "y": 6.054519113336024, - "heading": 0.0413497715036912, - "angularVelocity": 0.20963793984853818, - "velocityX": 1.458316972585085, - "velocityY": 0.48968632361146647, - "timestamp": 0.34918079820905806 - }, - { - "x": 2.4471410825572004, - "y": 6.071227617313664, - "heading": 0.04850181812451799, - "angularVelocity": 0.2253059539144659, - "velocityX": 1.5933454256218078, - "velocityY": 0.5263563881424058, - "timestamp": 0.38092450713715426 - }, - { - "x": 2.5020271147547666, - "y": 6.089020938374403, - "heading": 0.05611681521560166, - "angularVelocity": 0.23988996082133435, - "velocityX": 1.7290365256905385, - "velocityY": 0.5605306267468754, - "timestamp": 0.41266821606525045 - }, - { - "x": 2.561244492876039, - "y": 6.107800369655392, - "heading": 0.06415181877617589, - "angularVelocity": 0.2531211327187972, - "velocityX": 1.8654839059735924, - "velocityY": 0.5915953716537398, - "timestamp": 0.44441192499334664 - }, - { - "x": 2.6248204026171256, - "y": 6.127439774433694, - "heading": 0.07255187184184077, - "angularVelocity": 0.26462103356269817, - "velocityX": 2.0027876983466375, - "velocityY": 0.6186865190448766, - "timestamp": 0.47615563392144283 - }, - { - "x": 2.6927848123865834, - "y": 6.14777283178745, - "heading": 0.08124443551167423, - "angularVelocity": 0.2738357918264941, - "velocityX": 2.141035564665967, - "velocityY": 0.6405381740302919, - "timestamp": 0.507899342849539 - }, - { - "x": 2.7651681706057807, - "y": 6.1685714629691, - "heading": 0.09012999755945103, - "angularVelocity": 0.27991568559003044, - "velocityX": 2.280242626441548, - "velocityY": 0.655204822749479, - "timestamp": 0.5396430517776352 - }, - { - "x": 2.8419931281714104, - "y": 6.189507110191248, - "heading": 0.09906528191141717, - "angularVelocity": 0.28148205278115124, - "velocityX": 2.420163243672891, - "velocityY": 0.65952114384502, - "timestamp": 0.5713867607057314 - }, - { - "x": 2.923246509702383, - "y": 6.21007727563667, - "heading": 0.10783160033295475, - "angularVelocity": 0.27615923650876134, - "velocityX": 2.559668806030865, - "velocityY": 0.6480076254481864, - "timestamp": 0.6031304696338275 - }, - { - "x": 3.0087812329232086, - "y": 6.229466168623038, - "heading": 0.11607630855981195, - "angularVelocity": 0.2597273130727852, - "velocityX": 2.694540937688452, - "velocityY": 0.6107948201729568, - "timestamp": 0.6348741785619237 - }, - { - "x": 3.0979818735027176, - "y": 6.246361893248638, - "heading": 0.12324903142117805, - "angularVelocity": 0.22595730314988358, - "velocityX": 2.810025784370449, - "velocityY": 0.5322542700937218, - "timestamp": 0.6666178874900199 - }, - { - "x": 3.1891159324545155, - "y": 6.259285555621032, - "heading": 0.12873166157260418, - "angularVelocity": 0.1727154871488241, - "velocityX": 2.8709329195972866, - "velocityY": 0.40712515357501006, - "timestamp": 0.6983615964181161 - }, - { - "x": 3.2799147655356333, - "y": 6.267810501767619, - "heading": 0.13233373871668666, - "angularVelocity": 0.1134737327714805, - "velocityX": 2.860372531980369, - "velocityY": 0.26855545348842214, - "timestamp": 0.7301053053462123 - }, - { - "x": 3.368930673043237, - "y": 6.2722939827484225, - "heading": 0.13423316054281098, - "angularVelocity": 0.05983616566124247, - "velocityX": 2.8042062667984893, - "velocityY": 0.14123998525071657, - "timestamp": 0.7618490142743085 - }, - { - "x": 3.455418033445333, - "y": 6.273149165824332, - "heading": 0.13460089895534874, - "angularVelocity": 0.011584607626349263, - "velocityX": 2.7245512047125473, - "velocityY": 0.026940238075252306, - "timestamp": 0.7935927232024047 - }, - { - "x": 3.53897038421859, - "y": 6.270691577134489, - "heading": 0.1335628529939654, - "angularVelocity": -0.03270084046371159, - "velocityX": 2.6320916362516953, - "velocityY": -0.07741970843436734, - "timestamp": 0.8253364321305009 - }, - { - "x": 3.619343411211179, - "y": 6.2651515676417615, - "heading": 0.13121159908477192, - "angularVelocity": -0.07406991774402583, - "velocityX": 2.531935608867881, - "velocityY": -0.17452306865816167, - "timestamp": 0.8570801410585971 - }, - { - "x": 3.696377587829814, - "y": 6.256700197632999, - "heading": 0.12761684069301102, - "angularVelocity": -0.11324317520377067, - "velocityX": 2.42675412608914, - "velocityY": -0.26623763555482466, - "timestamp": 0.8888238499866933 - }, - { - "x": 3.7699618707808793, - "y": 6.245468037234803, - "heading": 0.1228324760673318, - "angularVelocity": -0.15071851359642613, - "velocityX": 2.318074523608403, - "velocityY": -0.35383894250147446, - "timestamp": 0.9205675589147895 - }, - { - "x": 3.8400150685867502, - "y": 6.2315574365397515, - "heading": 0.11690123481863413, - "angularVelocity": -0.18684777075463518, - "velocityX": 2.2068372024374066, - "velocityY": -0.438215985616424, - "timestamp": 0.9523112678428857 - }, - { - "x": 3.906475492488988, - "y": 6.215050519883858, - "heading": 0.10985775018096655, - "angularVelocity": -0.22188600121123825, - "velocityX": 2.0936565431839025, - "velocityY": -0.5200059228516231, - "timestamp": 0.9840549767709819 - }, - { - "x": 3.9692948172312557, - "y": 6.196014512598508, - "heading": 0.10173064016138685, - "angularVelocity": -0.2560226984814519, - "velocityX": 1.9789535269668237, - "velocityY": -0.5996781071948685, - "timestamp": 1.015798685699078 - }, - { - "x": 4.028434236538794, - "y": 6.174505389403469, - "heading": 0.09254395303933619, - "angularVelocity": -0.2894018195183511, - "velocityX": 1.8630280236469257, - "velocityY": -0.6775869588445468, - "timestamp": 1.047542394627174 - }, - { - "x": 4.083861945125214, - "y": 6.150570441683336, - "heading": 0.08231819631194325, - "angularVelocity": -0.3221349071262287, - "velocityX": 1.7461005804951526, - "velocityY": -0.7540060228735362, - "timestamp": 1.07928610355527 - }, - { - "x": 4.135551426431915, - "y": 6.124250129459578, - "heading": 0.07107108601922003, - "angularVelocity": -0.35430989863852635, - "velocityX": 1.6283378046271129, - "velocityY": -0.8291505029665418, - "timestamp": 1.1110298124833662 - }, - { - "x": 4.183480250707313, - "y": 6.0955794475298, - "heading": 0.05881810384556654, - "angularVelocity": -0.38599718140717787, - "velocityX": 1.50986843988397, - "velocityY": -0.9031925662724984, - "timestamp": 1.1427735214114623 - }, - { - "x": 4.227629208104442, - "y": 6.064588953496888, - "heading": 0.04557291906848408, - "angularVelocity": -0.4172538504270615, - "velocityX": 1.3907939206829834, - "velocityY": -0.9762719946527232, - "timestamp": 1.1745172303395583 - }, - { - "x": 4.267981668593037, - "y": 6.031305555215571, - "heading": 0.031347713474676034, - "angularVelocity": -0.44812676508695654, - "velocityX": 1.2711955171966314, - "velocityY": -1.0485037635868417, - "timestamp": 1.2062609392676544 - }, - { - "x": 4.3045230996015675, - "y": 5.99575312358408, - "heading": 0.016153435256918744, - "angularVelocity": -0.47865478643897924, - "velocityX": 1.1511393042098552, - "velocityY": -1.1199835442045625, - "timestamp": 1.2380046481957505 - }, - { - "x": 4.337240695953369, - "y": 5.957952976226807, - "heading": 0, - "angularVelocity": -0.5088704440149149, - "velocityX": 1.030679698642281, - "velocityY": -1.1907917705171907, - "timestamp": 1.2697483571238466 - }, - { - "x": 4.3631357670929365, - "y": 5.922844781507193, - "heading": -0.015001401099503393, - "angularVelocity": -0.5352972829698707, - "velocityX": 0.924017772165235, - "velocityY": -1.252771065764447, - "timestamp": 1.2977727868692683 - }, - { - "x": 4.386013063556468, - "y": 5.886050856639685, - "heading": -0.030770580548153122, - "angularVelocity": -0.5626940348795785, - "velocityX": 0.8163340582253283, - "velocityY": -1.3129232316856283, - "timestamp": 1.32579721661469 - }, - { - "x": 4.40584258476598, - "y": 5.847627176742848, - "heading": -0.04733672158353509, - "angularVelocity": -0.5911321367061623, - "velocityX": 0.707579829086438, - "velocityY": -1.3710780289155786, - "timestamp": 1.3538216463601118 - }, - { - "x": 4.422592955723098, - "y": 5.807635111111906, - "heading": -0.06473120794982032, - "angularVelocity": -0.6206901094616508, - "velocityX": 0.5977060410962763, - "velocityY": -1.427042976225914, - "timestamp": 1.3818460761055336 - }, - { - "x": 4.436231449424729, - "y": 5.766142144918943, - "heading": -0.08298784256354313, - "angularVelocity": -0.6514542768423776, - "velocityX": 0.48666445046428625, - "velocityY": -1.480599839850087, - "timestamp": 1.4098705058509553 - }, - { - "x": 4.446724054726656, - "y": 5.723222715334596, - "heading": -0.10214308677500623, - "angularVelocity": -0.683519500145881, - "velocityX": 0.37440923498684786, - "velocityY": -1.5315005505636412, - "timestamp": 1.437894935596377 - }, - { - "x": 4.454035609213429, - "y": 5.678959179772936, - "heading": -0.12223631960196876, - "angularVelocity": -0.7169898909449176, - "velocityX": 0.2608993136770921, - "velocityY": -1.579462489112408, - "timestamp": 1.4659193653417988 - }, - { - "x": 4.458130023896409, - "y": 5.633442934952869, - "heading": -0.1433101145106275, - "angularVelocity": -0.7519794372302038, - "velocityX": 0.14610162348255104, - "velocityY": -1.624163104603462, - "timestamp": 1.4939437950872205 - }, - { - "x": 4.458970636242968, - "y": 5.586775705010787, - "heading": -0.1654105284090244, - "angularVelocity": -0.7886124391882843, - "velocityX": 0.029995698545707234, - "velocityY": -1.6652338822239472, - "timestamp": 1.5219682248326423 - }, - { - "x": 4.4565207406871075, - "y": 5.539071013689049, - "heading": -0.18858739291328477, - "angularVelocity": -0.8270235903032963, - "velocityX": -0.08741999669990082, - "velocityY": -1.7022537748348157, - "timestamp": 1.549992654578064 - }, - { - "x": 4.450744361777594, - "y": 5.490455847567595, - "heading": -0.2128945907543178, - "angularVelocity": -0.8673574471218202, - "velocityX": -0.2061194094576707, - "velocityY": -1.7347423859497373, - "timestamp": 1.5780170843234858 - }, - { - "x": 4.441607354345094, - "y": 5.441072501173689, - "heading": -0.2383902890539677, - "angularVelocity": -0.9097668902189089, - "velocityX": -0.3260372294994741, - "velocityY": -1.7621534797499967, - "timestamp": 1.6060415140689075 - }, - { - "x": 4.429078936103224, - "y": 5.39108056591739, - "heading": -0.26513708408252107, - "angularVelocity": -0.9544099655738234, - "velocityX": -0.44705345856029166, - "velocityY": -1.7838698489293998, - "timestamp": 1.6340659438143292 - }, - { - "x": 4.413133776945269, - "y": 5.340658976983783, - "heading": -0.29320198612106874, - "angularVelocity": -1.0014441790071809, - "velocityX": -0.5689735456815054, - "velocityY": -1.7992012466138827, - "timestamp": 1.662090373559751 - }, - { - "x": 4.3937547772479375, - "y": 5.290007958751612, - "heading": -0.3226561346138084, - "angularVelocity": -1.0510168720757636, - "velocityX": -0.691503801268173, - "velocityY": -1.8073880072598505, - "timestamp": 1.6901148033051727 - }, - { - "x": 4.370936649023499, - "y": 5.23935060545745, - "heading": -0.3535740801853513, - "angularVelocity": -1.10324976645044, - "velocityX": -0.8142227489273355, - "velocityY": -1.8076140622428558, - "timestamp": 1.7181392330505945 - }, - { - "x": 4.344690344443845, - "y": 5.188933704037928, - "heading": -0.3860324046425531, - "angularVelocity": -1.1582153411169682, - "velocityX": -0.9365508885668746, - "velocityY": -1.7990339813340146, - "timestamp": 1.7461636627960162 - }, - { - "x": 4.315048226527169, - "y": 5.139027274908771, - "heading": -0.42010739431834127, - "angularVelocity": -1.2159030526341073, - "velocityX": -1.0577242136931926, - "velocityY": -1.7808187207559585, - "timestamp": 1.774188092541438 - }, - { - "x": 4.2820696265426665, - "y": 5.089922231340629, - "heading": -0.45587149477143696, - "angularVelocity": -1.2761758500701854, - "velocityX": -1.1767804120934704, - "velocityY": -1.7522227575803986, - "timestamp": 1.8022125222868597 - }, - { - "x": 4.245846101927616, - "y": 5.041925630666271, - "heading": -0.4933884602431478, - "angularVelocity": -1.3387235998206146, - "velocityX": -1.292569552498, - "velocityY": -1.7126700207770804, - "timestamp": 1.8302369520322814 - }, - { - "x": 4.20650539300032, - "y": 4.995353304015392, - "heading": -0.5327075685418524, - "angularVelocity": -1.4030297371216962, - "velocityX": -1.4038005156455622, - "velocityY": -1.6618474336123745, - "timestamp": 1.8582613817777032 - }, - { - "x": 4.164212950532524, - "y": 4.950520223954836, - "heading": -0.5738579425633289, - "angularVelocity": -1.4683750711537225, - "velocityX": -1.50912767367571, - "velocityY": -1.5997856323152906, - "timestamp": 1.886285811523125 - }, - { - "x": 4.119170145989121, - "y": 4.907729664930321, - "heading": -0.6168445330043962, - "angularVelocity": -1.5338970616552692, - "velocityX": -1.6072692630172125, - "velocityY": -1.5269020427259838, - "timestamp": 1.9143102412685467 - }, - { - "x": 4.071608923365433, - "y": 4.867262744975817, - "heading": -0.6616471382328869, - "angularVelocity": -1.598698194235669, - "velocityX": -1.6971343594050328, - "velocityY": -1.4439872754632914, - "timestamp": 1.9423346710139684 - }, - { - "x": 4.021783518702593, - "y": 4.829370015961046, - "heading": -0.708222820088443, - "angularVelocity": -1.6619671578924535, - "velocityX": -1.7779275123691076, - "velocityY": -1.3521320276271316, - "timestamp": 1.9703591007593901 - }, - { - "x": 3.969960602751049, - "y": 4.79426629105861, - "heading": -0.7565108155122148, - "angularVelocity": -1.7230679040546606, - "velocityX": -1.8492050122807404, - "velocityY": -1.2526115685965424, - "timestamp": 1.9983835305048119 - }, - { - "x": 3.9164094697061533, - "y": 4.762129052357968, - "heading": -0.8064383857566846, - "angularVelocity": -1.7815731023973904, - "velocityX": -1.9108732463554607, - "velocityY": -1.1467579891038302, - "timestamp": 2.0264079602502334 - }, - { - "x": 3.8613936225255663, - "y": 4.733099950552934, - "heading": -0.8579262476213516, - "angularVelocity": -1.8372492262069025, - "velocityX": -1.9631388642109393, - "velocityY": -1.0358498663037214, - "timestamp": 2.054432389995655 - }, - { - "x": 3.805164500168681, - "y": 4.7072884154222505, - "heading": -0.910892878375897, - "angularVelocity": -1.8900163619991934, - "velocityX": -2.006432347336878, - "velocityY": -0.9210369440220643, - "timestamp": 2.082456819741077 - }, - { - "x": 3.747957470865572, - "y": 4.684776320796834, - "heading": -0.9652575650457059, - "angularVelocity": -1.9399034044105279, - "velocityX": -2.041327150018244, - "velocityY": -0.8033025053469364, - "timestamp": 2.1104812494864986 - }, - { - "x": 3.689989792880342, - "y": 4.665622858433567, - "heading": -1.0209423751702893, - "angularVelocity": -1.987009570950461, - "velocityX": -2.0684694929322522, - "velocityY": -0.683455918184957, - "timestamp": 2.1385056792319204 - }, - { - "x": 3.631460063326017, - "y": 4.6498690821793875, - "heading": -1.077873311902558, - "angularVelocity": -2.0314752967120566, - "velocityX": -2.088525264778511, - "velocityY": -0.562144400342476, - "timestamp": 2.166530108977342 - }, - { - "x": 3.572548670883797, - "y": 4.637541856462143, - "heading": -1.1359808949883092, - "angularVelocity": -2.073461747968023, - "velocityX": -2.1021441998063306, - "velocityY": -0.4398742750245512, - "timestamp": 2.194554538722764 - }, - { - "x": 3.5134188516985927, - "y": 4.628657132170972, - "heading": -1.1952003568393965, - "angularVelocity": -2.1131370874998674, - "velocityX": -2.1099383545837274, - "velocityY": -0.3170349716972457, - "timestamp": 2.2225789684681856 - }, - { - "x": 3.45421805464654, - "y": 4.623222581268303, - "heading": -1.2554715918974453, - "angularVelocity": -2.1506676712268407, - "velocityX": -2.112471068629758, - "velocityY": -0.19392190856467523, - "timestamp": 2.2506033982136073 - }, - { - "x": 3.3950794189302482, - "y": 4.621239671353537, - "heading": -1.3167389565753878, - "angularVelocity": -2.186212716351497, - "velocityX": -2.1102529562069527, - "velocityY": -0.07075647685889055, - "timestamp": 2.278627827959029 - }, - { - "x": 3.3361232418560975, - "y": 4.622705275847479, - "heading": -1.3789509864906324, - "angularVelocity": -2.219921350064421, - "velocityX": -2.103742256656607, - "velocityY": 0.052297388644645955, - "timestamp": 2.306652257704451 - }, - { - "x": 3.2774583674363726, - "y": 4.627612911455795, - "heading": -1.442060075578285, - "angularVelocity": -2.251931249304374, - "velocityX": -2.0933476596185394, - "velocityY": 0.1751199097679249, - "timestamp": 2.3346766874498726 - }, - { - "x": 3.219183461263326, - "y": 4.635953682667193, - "heading": -1.5060221459927505, - "angularVelocity": -2.2823683120585914, - "velocityX": -2.0794323632067755, - "velocityY": 0.2976250110052724, - "timestamp": 2.3627011171952943 - }, - { - "x": 3.1613881587982178, - "y": 4.647716999053955, - "heading": -1.5707963267948966, - "angularVelocity": -2.3113469708594327, - "velocityX": -2.0623185909625006, - "velocityY": 0.41975221239546007, - "timestamp": 2.390725546940716 - }, - { - "x": 3.10814561374089, - "y": 4.661599970136062, - "heading": -1.6316805279181992, - "angularVelocity": -2.337024785845805, - "velocityX": -2.0437017348473057, - "velocityY": 0.532894362108114, - "timestamp": 2.4167775603776476 - }, - { - "x": 3.055428663680876, - "y": 4.678424323710771, - "heading": -1.6931889627653025, - "angularVelocity": -2.3609858407297932, - "velocityX": -2.0235269027336233, - "velocityY": 0.6457985911698438, - "timestamp": 2.442829573814579 - }, - { - "x": 3.0032829116143174, - "y": 4.698182352912261, - "heading": -1.7552708867676743, - "angularVelocity": -2.3829990780811756, - "velocityX": -2.001601611053657, - "velocityY": 0.7584069941204404, - "timestamp": 2.4688815872515106 - }, - { - "x": 2.951759926605772, - "y": 4.720864384158654, - "heading": -1.817868495309071, - "angularVelocity": -2.4027934997397216, - "velocityX": -1.9776968537679434, - "velocityY": 0.8706440790575167, - "timestamp": 2.494933600688442 - }, - { - "x": 2.900918437449139, - "y": 4.746458145818845, - "heading": -1.8809155732314475, - "angularVelocity": -2.4200462691683855, - "velocityX": -1.951537806461976, - "velocityY": 0.9824101205132756, - "timestamp": 2.5209856141253737 - }, - { - "x": 2.850825842782793, - "y": 4.774947883106229, - "heading": -1.944335714599795, - "angularVelocity": -2.4343662159500172, - "velocityX": -1.9227916793305875, - "velocityY": 1.0935714184376848, - "timestamp": 2.5470376275623052 - }, - { - "x": 2.801560136398716, - "y": 4.806313093575505, - "heading": -2.0080399486258997, - "angularVelocity": -2.4452710413465035, - "velocityX": -1.8910517800605635, - "velocityY": 1.2039457351427163, - "timestamp": 2.5730896409992368 - }, - { - "x": 2.753212376704843, - "y": 4.84052668356083, - "heading": -2.0719235447338953, - "angularVelocity": -2.4521558098625915, - "velocityX": -1.855816626646186, - "velocityY": 1.313280068281472, - "timestamp": 2.5991416544361683 - }, - { - "x": 2.7058898612562734, - "y": 4.877552222226872, - "heading": -2.1358616797226424, - "angularVelocity": -2.454249271117854, - "velocityX": -1.816462883497472, - "velocityY": 1.421216012945561, - "timestamp": 2.6251936678731 - }, - { - "x": 2.659720184999156, - "y": 4.917339761356798, - "heading": -2.1997035219581984, - "angularVelocity": -2.450553097943996, - "velocityX": -1.7722114403511053, - "velocityY": 1.5272347078372241, - "timestamp": 2.6512456813100314 - }, - { - "x": 2.6148563151123536, - "y": 4.959819338743969, - "heading": -2.263264109416659, - "angularVelocity": -2.4397572038847217, - "velocityX": -1.7220883904196647, - "velocityY": 1.6305679209787758, - "timestamp": 2.677297694746963 - }, - { - "x": 2.57148256826981, - "y": 5.004890715522668, - "heading": -2.326313161086277, - "angularVelocity": -2.4201220309611453, - "velocityX": -1.664890391199128, - "velocityY": 1.7300534904071132, - "timestamp": 2.7033497081838944 - }, - { - "x": 2.5298205674140557, - "y": 5.052407085126601, - "heading": -2.3885596943982783, - "angularVelocity": -2.3893175651351455, - "velocityX": -1.5991854509290115, - "velocityY": 1.8239039266181656, - "timestamp": 2.729401721620826 - }, - { - "x": 2.490132038202787, - "y": 5.102149807083195, - "heading": -2.4496314755140496, - "angularVelocity": -2.3442249967978706, - "velocityX": -1.5234342369484943, - "velocityY": 1.9093619031408002, - "timestamp": 2.7554537350577575 - }, - { - "x": 2.452710284842023, - "y": 5.153792814807895, - "heading": -2.5090526704829195, - "angularVelocity": -2.2808676616384806, - "velocityX": -1.4364246146023367, - "velocityY": 1.9823038956169858, - "timestamp": 2.781505748494689 - }, - { - "x": 2.417845697613634, - "y": 5.206866100215564, - "heading": -2.5662496521434983, - "angularVelocity": -2.195491791797435, - "velocityX": -1.3382684341380409, - "velocityY": 2.037204745658009, - "timestamp": 2.8075577619316205 - }, - { - "x": 2.3857600871145697, - "y": 5.260751237983873, - "heading": -2.6206772315252245, - "angularVelocity": -2.08918898009516, - "velocityX": -1.231598109556386, - "velocityY": 2.0683674948483968, - "timestamp": 2.833609775368552 - }, - { - "x": 2.356544218134567, - "y": 5.314735552674231, - "heading": -2.6720662793567067, - "angularVelocity": -1.9725557088280796, - "velocityX": -1.1214438012912034, - "velocityY": 2.0721743761203633, - "timestamp": 2.8596617888054836 - }, - { - "x": 2.330129430188581, - "y": 5.368102945953001, - "heading": -2.7204400842018184, - "angularVelocity": -1.8568163632426362, - "velocityX": -1.0139250085193388, - "velocityY": 2.04849400250616, - "timestamp": 2.885713802242415 - }, - { - "x": 2.306309859520889, - "y": 5.420242360939423, - "heading": -2.7658578652974755, - "angularVelocity": -1.7433501332096546, - "velocityX": -0.9143082443649937, - "velocityY": 2.0013583638224843, - "timestamp": 2.9117658156793467 - }, - { - "x": 2.2848243549194245, - "y": 5.470705057059815, - "heading": -2.808343758268873, - "angularVelocity": -1.6308103430948317, - "velocityX": -0.8247157039696784, - "velocityY": 1.9369979307953407, - "timestamp": 2.937817829116278 - }, - { - "x": 2.2654228000679097, - "y": 5.519181079523684, - "heading": -2.8479456215051604, - "angularVelocity": -1.5201075852411496, - "velocityX": -0.7447238156269963, - "velocityY": 1.8607399608947834, - "timestamp": 2.9638698425532097 - }, - { - "x": 2.247888448425393, - "y": 5.565455623210483, - "heading": -2.8847464453032123, - "angularVelocity": -1.412590389112856, - "velocityX": -0.6730516888826009, - "velocityY": 1.7762367503310765, - "timestamp": 2.9899218559901413 - }, - { - "x": 2.232039117701984, - "y": 5.609376329385158, - "heading": -2.9188471410259273, - "angularVelocity": -1.308946650333392, - "velocityX": -0.6083725836308177, - "velocityY": 1.6858852879451771, - "timestamp": 3.015973869427073 - }, - { - "x": 2.2177225834921717, - "y": 5.650832312921057, - "heading": -2.9503511401259157, - "angularVelocity": -1.209273101914099, - "velocityX": -0.5495365739954405, - "velocityY": 1.591277527791001, - "timestamp": 3.0420258828640043 - }, - { - "x": 2.2048112517628295, - "y": 5.689740878391115, - "heading": -2.9793561711588477, - "angularVelocity": -1.1133508395866407, - "velocityX": -0.4955982293115555, - "velocityY": 1.4934955244149806, - "timestamp": 3.068077896300936 - }, - { - "x": 2.1931974509149272, - "y": 5.726038957702259, - "heading": -3.0059510066505495, - "angularVelocity": -1.0208360883923002, - "velocityX": -0.4457928319444466, - "velocityY": 1.3932926681086102, - "timestamp": 3.0941299097378674 - }, - { - "x": 2.1827895833104405, - "y": 5.7596774671860915, - "heading": -3.0302147916610416, - "angularVelocity": -0.931359300471352, - "velocityX": -0.399503386933393, - "velocityY": 1.2912057475045753, - "timestamp": 3.120181923174799 - }, - { - "x": 2.1735090671525814, - "y": 5.7906175048600845, - "heading": -3.0522175233133915, - "angularVelocity": -0.8445693345589079, - "velocityX": -0.3562302844779268, - "velocityY": 1.1876255840606498, - "timestamp": 3.1462339366117305 - }, - { - "x": 2.165287928866771, - "y": 5.818827731905624, - "heading": -3.0720209570909076, - "angularVelocity": -0.7601498373804071, - "velocityX": -0.31556633062986994, - "velocityY": 1.082842487926418, - "timestamp": 3.172285950048662 - }, - { - "x": 2.15806690849839, - "y": 5.844282532149564, - "heading": -3.0896796043938592, - "angularVelocity": -0.6778227466257346, - "velocityX": -0.27717705527303127, - "velocityY": 0.9770761214123779, - "timestamp": 3.1983379634855935 - }, - { - "x": 2.151793962845926, - "y": 5.866960693109083, - "heading": -3.1052416789306485, - "angularVelocity": -0.5973463269724832, - "velocityX": -0.2407854451499951, - "velocityY": 0.8704955190668501, - "timestamp": 3.224389976922525 - }, - { - "x": 2.1464230753720366, - "y": 5.8868444435667495, - "heading": -3.1187499404873535, - "angularVelocity": -0.5185112309805378, - "velocityX": -0.2061601682684892, - "velocityY": 0.7632327729986262, - "timestamp": 3.2504419903594566 - }, - { - "x": 2.1419133029864885, - "y": 5.90391873943064, - "heading": -3.1302424255201187, - "angularVelocity": -0.4411361548153143, - "velocityX": -0.17310648163399853, - "velocityY": 0.6553925632360712, - "timestamp": 3.276494003796388 - }, - { - "x": 2.1382280065351003, - "y": 5.918170725554471, - "heading": -3.139753071131935, - "angularVelocity": -0.36506374583446893, - "velocityX": -0.1414591797409874, - "velocityY": 0.5470589119083662, - "timestamp": 3.3025460172333196 - }, - { - "x": 2.135334224654641, - "y": 5.9295893243238895, - "heading": -3.1473122451070203, - "angularVelocity": -0.2901569966323474, - "velocityX": -0.11107709150637847, - "velocityY": 0.43830004913290593, - "timestamp": 3.328598030670251 - }, - { - "x": 2.133202160318781, - "y": 5.9381649169747, - "heading": -3.1529471958838693, - "angularVelocity": -0.2162961719058987, - "velocityX": -0.08183875465217869, - "velocityY": 0.32917197250689534, - "timestamp": 3.3546500441071827 - }, - { - "x": 2.131804756638863, - "y": 5.943889093714475, - "heading": -3.156682435526014, - "angularVelocity": -0.14337623658866083, - "velocityX": -0.053638989681216866, - "velocityY": 0.2197210881082821, - "timestamp": 3.3807020575441142 - }, - { - "x": 2.131117343902588, - "y": 5.946754455566406, - "heading": -3.158540067224243, - "angularVelocity": -0.07130472670475889, - "velocityX": -0.026386165427844832, - "velocityY": 0.10998619584120908, - "timestamp": 3.4067540709810458 - }, - { - "x": 2.131117343902588, - "y": 5.946754455566406, - "heading": -3.158540067224243, - "angularVelocity": -1.1257235159322888e-32, - "velocityX": -4.497825258620012e-34, - "velocityY": 8.02311483706782e-33, - "timestamp": 3.4328060844179773 - } - ] + "samples": [ + { + "x": 2.1199185848236084, + "y": 5.957952976226807, + "heading": 9.721358924197938e-33, + "angularVelocity": 0, + "velocityX": 7.529723953218568e-33, + "velocityY": 4.103114367587547e-33, + "timestamp": 0, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.12408359184855, + "y": 5.959490957092025, + "heading": 0.0006587462568403469, + "angularVelocity": 0.020752025490549227, + "velocityX": 0.13120732156333928, + "velocityY": 0.04844994227681366, + "timestamp": 0.031743708928096186, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.132419910947834, + "y": 5.962549984290152, + "heading": 0.0019689340924026384, + "angularVelocity": 0.04127393678319714, + "velocityX": 0.26261326671585045, + "velocityY": 0.0963664077520038, + "timestamp": 0.06348741785619237, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.144934423626495, + "y": 5.967111326975421, + "heading": 0.003922497731913535, + "angularVelocity": 0.06154175757899709, + "velocityX": 0.3942359951387039, + "velocityY": 0.1436928084112908, + "timestamp": 0.09523112678428855, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.161634668441711, + "y": 5.9731541575830684, + "heading": 0.006510477776343394, + "angularVelocity": 0.0815273366540852, + "velocityX": 0.5260962054889244, + "velocityY": 0.19036309277324892, + "timestamp": 0.12697483571238474, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.182528936666272, + "y": 5.980655180863507, + "heading": 0.00972285754365477, + "angularVelocity": 0.10119736715669446, + "velocityX": 0.6582176100432996, + "velocityY": 0.2362995230780409, + "timestamp": 0.15871854464048094, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.2076263864352867, + "y": 5.98958817047729, + "heading": 0.013548358655846408, + "angularVelocity": 0.12051210275577494, + "velocityX": 0.7906275169629459, + "velocityY": 0.2814097632390032, + "timestamp": 0.19046225356857713, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.2369371796137325, + "y": 5.999923382651596, + "heading": 0.017974182463991455, + "angularVelocity": 0.139423651412969, + "velocityX": 0.9233575460523492, + "velocityY": 0.32558300599701506, + "timestamp": 0.22220596249667332, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.270472646569135, + "y": 6.0116268035707865, + "heading": 0.022985678275806047, + "angularVelocity": 0.15787366949359927, + "velocityX": 1.0564445078350972, + "velocityY": 0.36868473516117956, + "timestamp": 0.2539496714247695, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.3082454850008034, + "y": 6.024659167637654, + "heading": 0.02856591078801465, + "angularVelocity": 0.17579018648543426, + "velocityX": 1.1899314764141191, + "velocityY": 0.41054950750668673, + "timestamp": 0.2856933803528657, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.350269999641707, + "y": 6.03897465321323, + "heading": 0.034695085760854436, + "angularVelocity": 0.1930831392993197, + "velocityX": 1.323869077053848, + "velocityY": 0.4509707926064986, + "timestamp": 0.31743708928096187, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.3965623891443495, + "y": 6.054519113336024, + "heading": 0.0413497715036912, + "angularVelocity": 0.20963793984853818, + "velocityX": 1.458316972585085, + "velocityY": 0.48968632361146647, + "timestamp": 0.34918079820905806, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.4471410825572004, + "y": 6.071227617313664, + "heading": 0.04850181812451799, + "angularVelocity": 0.2253059539144659, + "velocityX": 1.5933454256218078, + "velocityY": 0.5263563881424058, + "timestamp": 0.38092450713715426, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.5020271147547666, + "y": 6.089020938374403, + "heading": 0.05611681521560166, + "angularVelocity": 0.23988996082133435, + "velocityX": 1.7290365256905385, + "velocityY": 0.5605306267468754, + "timestamp": 0.41266821606525045, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.561244492876039, + "y": 6.107800369655392, + "heading": 0.06415181877617589, + "angularVelocity": 0.2531211327187972, + "velocityX": 1.8654839059735924, + "velocityY": 0.5915953716537398, + "timestamp": 0.44441192499334664, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.6248204026171256, + "y": 6.127439774433694, + "heading": 0.07255187184184077, + "angularVelocity": 0.26462103356269817, + "velocityX": 2.0027876983466375, + "velocityY": 0.6186865190448766, + "timestamp": 0.47615563392144283, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.6927848123865834, + "y": 6.14777283178745, + "heading": 0.08124443551167423, + "angularVelocity": 0.2738357918264941, + "velocityX": 2.141035564665967, + "velocityY": 0.6405381740302919, + "timestamp": 0.507899342849539, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.7651681706057807, + "y": 6.1685714629691, + "heading": 0.09012999755945103, + "angularVelocity": 0.27991568559003044, + "velocityX": 2.280242626441548, + "velocityY": 0.655204822749479, + "timestamp": 0.5396430517776352, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.8419931281714104, + "y": 6.189507110191248, + "heading": 0.09906528191141717, + "angularVelocity": 0.28148205278115124, + "velocityX": 2.420163243672891, + "velocityY": 0.65952114384502, + "timestamp": 0.5713867607057314, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.923246509702383, + "y": 6.21007727563667, + "heading": 0.10783160033295475, + "angularVelocity": 0.27615923650876134, + "velocityX": 2.559668806030865, + "velocityY": 0.6480076254481864, + "timestamp": 0.6031304696338275, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.0087812329232086, + "y": 6.229466168623038, + "heading": 0.11607630855981195, + "angularVelocity": 0.2597273130727852, + "velocityX": 2.694540937688452, + "velocityY": 0.6107948201729568, + "timestamp": 0.6348741785619237, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.0979818735027176, + "y": 6.246361893248638, + "heading": 0.12324903142117805, + "angularVelocity": 0.22595730314988358, + "velocityX": 2.810025784370449, + "velocityY": 0.5322542700937218, + "timestamp": 0.6666178874900199, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.1891159324545155, + "y": 6.259285555621032, + "heading": 0.12873166157260418, + "angularVelocity": 0.1727154871488241, + "velocityX": 2.8709329195972866, + "velocityY": 0.40712515357501006, + "timestamp": 0.6983615964181161, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.2799147655356333, + "y": 6.267810501767619, + "heading": 0.13233373871668666, + "angularVelocity": 0.1134737327714805, + "velocityX": 2.860372531980369, + "velocityY": 0.26855545348842214, + "timestamp": 0.7301053053462123, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.368930673043237, + "y": 6.2722939827484225, + "heading": 0.13423316054281098, + "angularVelocity": 0.05983616566124247, + "velocityX": 2.8042062667984893, + "velocityY": 0.14123998525071657, + "timestamp": 0.7618490142743085, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.455418033445333, + "y": 6.273149165824332, + "heading": 0.13460089895534874, + "angularVelocity": 0.011584607626349263, + "velocityX": 2.7245512047125473, + "velocityY": 0.026940238075252306, + "timestamp": 0.7935927232024047, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.53897038421859, + "y": 6.270691577134489, + "heading": 0.1335628529939654, + "angularVelocity": -0.03270084046371159, + "velocityX": 2.6320916362516953, + "velocityY": -0.07741970843436734, + "timestamp": 0.8253364321305009, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.619343411211179, + "y": 6.2651515676417615, + "heading": 0.13121159908477192, + "angularVelocity": -0.07406991774402583, + "velocityX": 2.531935608867881, + "velocityY": -0.17452306865816167, + "timestamp": 0.8570801410585971, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.696377587829814, + "y": 6.256700197632999, + "heading": 0.12761684069301102, + "angularVelocity": -0.11324317520377067, + "velocityX": 2.42675412608914, + "velocityY": -0.26623763555482466, + "timestamp": 0.8888238499866933, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.7699618707808793, + "y": 6.245468037234803, + "heading": 0.1228324760673318, + "angularVelocity": -0.15071851359642613, + "velocityX": 2.318074523608403, + "velocityY": -0.35383894250147446, + "timestamp": 0.9205675589147895, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.8400150685867502, + "y": 6.2315574365397515, + "heading": 0.11690123481863413, + "angularVelocity": -0.18684777075463518, + "velocityX": 2.2068372024374066, + "velocityY": -0.438215985616424, + "timestamp": 0.9523112678428857, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.906475492488988, + "y": 6.215050519883858, + "heading": 0.10985775018096655, + "angularVelocity": -0.22188600121123825, + "velocityX": 2.0936565431839025, + "velocityY": -0.5200059228516231, + "timestamp": 0.9840549767709819, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.9692948172312557, + "y": 6.196014512598508, + "heading": 0.10173064016138685, + "angularVelocity": -0.2560226984814519, + "velocityX": 1.9789535269668237, + "velocityY": -0.5996781071948685, + "timestamp": 1.015798685699078, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.028434236538794, + "y": 6.174505389403469, + "heading": 0.09254395303933619, + "angularVelocity": -0.2894018195183511, + "velocityX": 1.8630280236469257, + "velocityY": -0.6775869588445468, + "timestamp": 1.047542394627174, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.083861945125214, + "y": 6.150570441683336, + "heading": 0.08231819631194325, + "angularVelocity": -0.3221349071262287, + "velocityX": 1.7461005804951526, + "velocityY": -0.7540060228735362, + "timestamp": 1.07928610355527, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.135551426431915, + "y": 6.124250129459578, + "heading": 0.07107108601922003, + "angularVelocity": -0.35430989863852635, + "velocityX": 1.6283378046271129, + "velocityY": -0.8291505029665418, + "timestamp": 1.1110298124833662, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.183480250707313, + "y": 6.0955794475298, + "heading": 0.05881810384556654, + "angularVelocity": -0.38599718140717787, + "velocityX": 1.50986843988397, + "velocityY": -0.9031925662724984, + "timestamp": 1.1427735214114623, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.227629208104442, + "y": 6.064588953496888, + "heading": 0.04557291906848408, + "angularVelocity": -0.4172538504270615, + "velocityX": 1.3907939206829834, + "velocityY": -0.9762719946527232, + "timestamp": 1.1745172303395583, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.267981668593037, + "y": 6.031305555215571, + "heading": 0.031347713474676034, + "angularVelocity": -0.44812676508695654, + "velocityX": 1.2711955171966314, + "velocityY": -1.0485037635868417, + "timestamp": 1.2062609392676544, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.3045230996015675, + "y": 5.99575312358408, + "heading": 0.016153435256918744, + "angularVelocity": -0.47865478643897924, + "velocityX": 1.1511393042098552, + "velocityY": -1.1199835442045625, + "timestamp": 1.2380046481957505, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.337240695953369, + "y": 5.957952976226807, + "heading": 0, + "angularVelocity": -0.5088704440149149, + "velocityX": 1.030679698642281, + "velocityY": -1.1907917705171907, + "timestamp": 1.2697483571238466, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.3631357670929365, + "y": 5.922844781507193, + "heading": -0.015001401099503393, + "angularVelocity": -0.5352972829698707, + "velocityX": 0.924017772165235, + "velocityY": -1.252771065764447, + "timestamp": 1.2977727868692683, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.386013063556468, + "y": 5.886050856639685, + "heading": -0.030770580548153122, + "angularVelocity": -0.5626940348795785, + "velocityX": 0.8163340582253283, + "velocityY": -1.3129232316856283, + "timestamp": 1.32579721661469, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.40584258476598, + "y": 5.847627176742848, + "heading": -0.04733672158353509, + "angularVelocity": -0.5911321367061623, + "velocityX": 0.707579829086438, + "velocityY": -1.3710780289155786, + "timestamp": 1.3538216463601118, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.422592955723098, + "y": 5.807635111111906, + "heading": -0.06473120794982032, + "angularVelocity": -0.6206901094616508, + "velocityX": 0.5977060410962763, + "velocityY": -1.427042976225914, + "timestamp": 1.3818460761055336, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.436231449424729, + "y": 5.766142144918943, + "heading": -0.08298784256354313, + "angularVelocity": -0.6514542768423776, + "velocityX": 0.48666445046428625, + "velocityY": -1.480599839850087, + "timestamp": 1.4098705058509553, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.446724054726656, + "y": 5.723222715334596, + "heading": -0.10214308677500623, + "angularVelocity": -0.683519500145881, + "velocityX": 0.37440923498684786, + "velocityY": -1.5315005505636412, + "timestamp": 1.437894935596377, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.454035609213429, + "y": 5.678959179772936, + "heading": -0.12223631960196876, + "angularVelocity": -0.7169898909449176, + "velocityX": 0.2608993136770921, + "velocityY": -1.579462489112408, + "timestamp": 1.4659193653417988, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.458130023896409, + "y": 5.633442934952869, + "heading": -0.1433101145106275, + "angularVelocity": -0.7519794372302038, + "velocityX": 0.14610162348255104, + "velocityY": -1.624163104603462, + "timestamp": 1.4939437950872205, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.458970636242968, + "y": 5.586775705010787, + "heading": -0.1654105284090244, + "angularVelocity": -0.7886124391882843, + "velocityX": 0.029995698545707234, + "velocityY": -1.6652338822239472, + "timestamp": 1.5219682248326423, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.4565207406871075, + "y": 5.539071013689049, + "heading": -0.18858739291328477, + "angularVelocity": -0.8270235903032963, + "velocityX": -0.08741999669990082, + "velocityY": -1.7022537748348157, + "timestamp": 1.549992654578064, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.450744361777594, + "y": 5.490455847567595, + "heading": -0.2128945907543178, + "angularVelocity": -0.8673574471218202, + "velocityX": -0.2061194094576707, + "velocityY": -1.7347423859497373, + "timestamp": 1.5780170843234858, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.441607354345094, + "y": 5.441072501173689, + "heading": -0.2383902890539677, + "angularVelocity": -0.9097668902189089, + "velocityX": -0.3260372294994741, + "velocityY": -1.7621534797499967, + "timestamp": 1.6060415140689075, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.429078936103224, + "y": 5.39108056591739, + "heading": -0.26513708408252107, + "angularVelocity": -0.9544099655738234, + "velocityX": -0.44705345856029166, + "velocityY": -1.7838698489293998, + "timestamp": 1.6340659438143292, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.413133776945269, + "y": 5.340658976983783, + "heading": -0.29320198612106874, + "angularVelocity": -1.0014441790071809, + "velocityX": -0.5689735456815054, + "velocityY": -1.7992012466138827, + "timestamp": 1.662090373559751, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.3937547772479375, + "y": 5.290007958751612, + "heading": -0.3226561346138084, + "angularVelocity": -1.0510168720757636, + "velocityX": -0.691503801268173, + "velocityY": -1.8073880072598505, + "timestamp": 1.6901148033051727, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.370936649023499, + "y": 5.23935060545745, + "heading": -0.3535740801853513, + "angularVelocity": -1.10324976645044, + "velocityX": -0.8142227489273355, + "velocityY": -1.8076140622428558, + "timestamp": 1.7181392330505945, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.344690344443845, + "y": 5.188933704037928, + "heading": -0.3860324046425531, + "angularVelocity": -1.1582153411169682, + "velocityX": -0.9365508885668746, + "velocityY": -1.7990339813340146, + "timestamp": 1.7461636627960162, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.315048226527169, + "y": 5.139027274908771, + "heading": -0.42010739431834127, + "angularVelocity": -1.2159030526341073, + "velocityX": -1.0577242136931926, + "velocityY": -1.7808187207559585, + "timestamp": 1.774188092541438, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.2820696265426665, + "y": 5.089922231340629, + "heading": -0.45587149477143696, + "angularVelocity": -1.2761758500701854, + "velocityX": -1.1767804120934704, + "velocityY": -1.7522227575803986, + "timestamp": 1.8022125222868597, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.245846101927616, + "y": 5.041925630666271, + "heading": -0.4933884602431478, + "angularVelocity": -1.3387235998206146, + "velocityX": -1.292569552498, + "velocityY": -1.7126700207770804, + "timestamp": 1.8302369520322814, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.20650539300032, + "y": 4.995353304015392, + "heading": -0.5327075685418524, + "angularVelocity": -1.4030297371216962, + "velocityX": -1.4038005156455622, + "velocityY": -1.6618474336123745, + "timestamp": 1.8582613817777032, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.164212950532524, + "y": 4.950520223954836, + "heading": -0.5738579425633289, + "angularVelocity": -1.4683750711537225, + "velocityX": -1.50912767367571, + "velocityY": -1.5997856323152906, + "timestamp": 1.886285811523125, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.119170145989121, + "y": 4.907729664930321, + "heading": -0.6168445330043962, + "angularVelocity": -1.5338970616552692, + "velocityX": -1.6072692630172125, + "velocityY": -1.5269020427259838, + "timestamp": 1.9143102412685467, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.071608923365433, + "y": 4.867262744975817, + "heading": -0.6616471382328869, + "angularVelocity": -1.598698194235669, + "velocityX": -1.6971343594050328, + "velocityY": -1.4439872754632914, + "timestamp": 1.9423346710139684, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 4.021783518702593, + "y": 4.829370015961046, + "heading": -0.708222820088443, + "angularVelocity": -1.6619671578924535, + "velocityX": -1.7779275123691076, + "velocityY": -1.3521320276271316, + "timestamp": 1.9703591007593901, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.969960602751049, + "y": 4.79426629105861, + "heading": -0.7565108155122148, + "angularVelocity": -1.7230679040546606, + "velocityX": -1.8492050122807404, + "velocityY": -1.2526115685965424, + "timestamp": 1.9983835305048119, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.9164094697061533, + "y": 4.762129052357968, + "heading": -0.8064383857566846, + "angularVelocity": -1.7815731023973904, + "velocityX": -1.9108732463554607, + "velocityY": -1.1467579891038302, + "timestamp": 2.0264079602502334, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.8613936225255663, + "y": 4.733099950552934, + "heading": -0.8579262476213516, + "angularVelocity": -1.8372492262069025, + "velocityX": -1.9631388642109393, + "velocityY": -1.0358498663037214, + "timestamp": 2.054432389995655, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.805164500168681, + "y": 4.7072884154222505, + "heading": -0.910892878375897, + "angularVelocity": -1.8900163619991934, + "velocityX": -2.006432347336878, + "velocityY": -0.9210369440220643, + "timestamp": 2.082456819741077, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.747957470865572, + "y": 4.684776320796834, + "heading": -0.9652575650457059, + "angularVelocity": -1.9399034044105279, + "velocityX": -2.041327150018244, + "velocityY": -0.8033025053469364, + "timestamp": 2.1104812494864986, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.689989792880342, + "y": 4.665622858433567, + "heading": -1.0209423751702893, + "angularVelocity": -1.987009570950461, + "velocityX": -2.0684694929322522, + "velocityY": -0.683455918184957, + "timestamp": 2.1385056792319204, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.631460063326017, + "y": 4.6498690821793875, + "heading": -1.077873311902558, + "angularVelocity": -2.0314752967120566, + "velocityX": -2.088525264778511, + "velocityY": -0.562144400342476, + "timestamp": 2.166530108977342, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.572548670883797, + "y": 4.637541856462143, + "heading": -1.1359808949883092, + "angularVelocity": -2.073461747968023, + "velocityX": -2.1021441998063306, + "velocityY": -0.4398742750245512, + "timestamp": 2.194554538722764, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.5134188516985927, + "y": 4.628657132170972, + "heading": -1.1952003568393965, + "angularVelocity": -2.1131370874998674, + "velocityX": -2.1099383545837274, + "velocityY": -0.3170349716972457, + "timestamp": 2.2225789684681856, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.45421805464654, + "y": 4.623222581268303, + "heading": -1.2554715918974453, + "angularVelocity": -2.1506676712268407, + "velocityX": -2.112471068629758, + "velocityY": -0.19392190856467523, + "timestamp": 2.2506033982136073, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.3950794189302482, + "y": 4.621239671353537, + "heading": -1.3167389565753878, + "angularVelocity": -2.186212716351497, + "velocityX": -2.1102529562069527, + "velocityY": -0.07075647685889055, + "timestamp": 2.278627827959029, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.3361232418560975, + "y": 4.622705275847479, + "heading": -1.3789509864906324, + "angularVelocity": -2.219921350064421, + "velocityX": -2.103742256656607, + "velocityY": 0.052297388644645955, + "timestamp": 2.306652257704451, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.2774583674363726, + "y": 4.627612911455795, + "heading": -1.442060075578285, + "angularVelocity": -2.251931249304374, + "velocityX": -2.0933476596185394, + "velocityY": 0.1751199097679249, + "timestamp": 2.3346766874498726, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.219183461263326, + "y": 4.635953682667193, + "heading": -1.5060221459927505, + "angularVelocity": -2.2823683120585914, + "velocityX": -2.0794323632067755, + "velocityY": 0.2976250110052724, + "timestamp": 2.3627011171952943, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.1613881587982178, + "y": 4.647716999053955, + "heading": -1.5707963267948966, + "angularVelocity": -2.3113469708594327, + "velocityX": -2.0623185909625006, + "velocityY": 0.41975221239546007, + "timestamp": 2.390725546940716, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.10814561374089, + "y": 4.661599970136062, + "heading": -1.6316805279181992, + "angularVelocity": -2.337024785845805, + "velocityX": -2.0437017348473057, + "velocityY": 0.532894362108114, + "timestamp": 2.4167775603776476, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.055428663680876, + "y": 4.678424323710771, + "heading": -1.6931889627653025, + "angularVelocity": -2.3609858407297932, + "velocityX": -2.0235269027336233, + "velocityY": 0.6457985911698438, + "timestamp": 2.442829573814579, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 3.0032829116143174, + "y": 4.698182352912261, + "heading": -1.7552708867676743, + "angularVelocity": -2.3829990780811756, + "velocityX": -2.001601611053657, + "velocityY": 0.7584069941204404, + "timestamp": 2.4688815872515106, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.951759926605772, + "y": 4.720864384158654, + "heading": -1.817868495309071, + "angularVelocity": -2.4027934997397216, + "velocityX": -1.9776968537679434, + "velocityY": 0.8706440790575167, + "timestamp": 2.494933600688442, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.900918437449139, + "y": 4.746458145818845, + "heading": -1.8809155732314475, + "angularVelocity": -2.4200462691683855, + "velocityX": -1.951537806461976, + "velocityY": 0.9824101205132756, + "timestamp": 2.5209856141253737, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.850825842782793, + "y": 4.774947883106229, + "heading": -1.944335714599795, + "angularVelocity": -2.4343662159500172, + "velocityX": -1.9227916793305875, + "velocityY": 1.0935714184376848, + "timestamp": 2.5470376275623052, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.801560136398716, + "y": 4.806313093575505, + "heading": -2.0080399486258997, + "angularVelocity": -2.4452710413465035, + "velocityX": -1.8910517800605635, + "velocityY": 1.2039457351427163, + "timestamp": 2.5730896409992368, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.753212376704843, + "y": 4.84052668356083, + "heading": -2.0719235447338953, + "angularVelocity": -2.4521558098625915, + "velocityX": -1.855816626646186, + "velocityY": 1.313280068281472, + "timestamp": 2.5991416544361683, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.7058898612562734, + "y": 4.877552222226872, + "heading": -2.1358616797226424, + "angularVelocity": -2.454249271117854, + "velocityX": -1.816462883497472, + "velocityY": 1.421216012945561, + "timestamp": 2.6251936678731, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.659720184999156, + "y": 4.917339761356798, + "heading": -2.1997035219581984, + "angularVelocity": -2.450553097943996, + "velocityX": -1.7722114403511053, + "velocityY": 1.5272347078372241, + "timestamp": 2.6512456813100314, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.6148563151123536, + "y": 4.959819338743969, + "heading": -2.263264109416659, + "angularVelocity": -2.4397572038847217, + "velocityX": -1.7220883904196647, + "velocityY": 1.6305679209787758, + "timestamp": 2.677297694746963, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.57148256826981, + "y": 5.004890715522668, + "heading": -2.326313161086277, + "angularVelocity": -2.4201220309611453, + "velocityX": -1.664890391199128, + "velocityY": 1.7300534904071132, + "timestamp": 2.7033497081838944, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.5298205674140557, + "y": 5.052407085126601, + "heading": -2.3885596943982783, + "angularVelocity": -2.3893175651351455, + "velocityX": -1.5991854509290115, + "velocityY": 1.8239039266181656, + "timestamp": 2.729401721620826, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.490132038202787, + "y": 5.102149807083195, + "heading": -2.4496314755140496, + "angularVelocity": -2.3442249967978706, + "velocityX": -1.5234342369484943, + "velocityY": 1.9093619031408002, + "timestamp": 2.7554537350577575, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.452710284842023, + "y": 5.153792814807895, + "heading": -2.5090526704829195, + "angularVelocity": -2.2808676616384806, + "velocityX": -1.4364246146023367, + "velocityY": 1.9823038956169858, + "timestamp": 2.781505748494689, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.417845697613634, + "y": 5.206866100215564, + "heading": -2.5662496521434983, + "angularVelocity": -2.195491791797435, + "velocityX": -1.3382684341380409, + "velocityY": 2.037204745658009, + "timestamp": 2.8075577619316205, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.3857600871145697, + "y": 5.260751237983873, + "heading": -2.6206772315252245, + "angularVelocity": -2.08918898009516, + "velocityX": -1.231598109556386, + "velocityY": 2.0683674948483968, + "timestamp": 2.833609775368552, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.356544218134567, + "y": 5.314735552674231, + "heading": -2.6720662793567067, + "angularVelocity": -1.9725557088280796, + "velocityX": -1.1214438012912034, + "velocityY": 2.0721743761203633, + "timestamp": 2.8596617888054836, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.330129430188581, + "y": 5.368102945953001, + "heading": -2.7204400842018184, + "angularVelocity": -1.8568163632426362, + "velocityX": -1.0139250085193388, + "velocityY": 2.04849400250616, + "timestamp": 2.885713802242415, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.306309859520889, + "y": 5.420242360939423, + "heading": -2.7658578652974755, + "angularVelocity": -1.7433501332096546, + "velocityX": -0.9143082443649937, + "velocityY": 2.0013583638224843, + "timestamp": 2.9117658156793467, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.2848243549194245, + "y": 5.470705057059815, + "heading": -2.808343758268873, + "angularVelocity": -1.6308103430948317, + "velocityX": -0.8247157039696784, + "velocityY": 1.9369979307953407, + "timestamp": 2.937817829116278, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.2654228000679097, + "y": 5.519181079523684, + "heading": -2.8479456215051604, + "angularVelocity": -1.5201075852411496, + "velocityX": -0.7447238156269963, + "velocityY": 1.8607399608947834, + "timestamp": 2.9638698425532097, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.247888448425393, + "y": 5.565455623210483, + "heading": -2.8847464453032123, + "angularVelocity": -1.412590389112856, + "velocityX": -0.6730516888826009, + "velocityY": 1.7762367503310765, + "timestamp": 2.9899218559901413, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.232039117701984, + "y": 5.609376329385158, + "heading": -2.9188471410259273, + "angularVelocity": -1.308946650333392, + "velocityX": -0.6083725836308177, + "velocityY": 1.6858852879451771, + "timestamp": 3.015973869427073, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.2177225834921717, + "y": 5.650832312921057, + "heading": -2.9503511401259157, + "angularVelocity": -1.209273101914099, + "velocityX": -0.5495365739954405, + "velocityY": 1.591277527791001, + "timestamp": 3.0420258828640043, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.2048112517628295, + "y": 5.689740878391115, + "heading": -2.9793561711588477, + "angularVelocity": -1.1133508395866407, + "velocityX": -0.4955982293115555, + "velocityY": 1.4934955244149806, + "timestamp": 3.068077896300936, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.1931974509149272, + "y": 5.726038957702259, + "heading": -3.0059510066505495, + "angularVelocity": -1.0208360883923002, + "velocityX": -0.4457928319444466, + "velocityY": 1.3932926681086102, + "timestamp": 3.0941299097378674, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.1827895833104405, + "y": 5.7596774671860915, + "heading": -3.0302147916610416, + "angularVelocity": -0.931359300471352, + "velocityX": -0.399503386933393, + "velocityY": 1.2912057475045753, + "timestamp": 3.120181923174799, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.1735090671525814, + "y": 5.7906175048600845, + "heading": -3.0522175233133915, + "angularVelocity": -0.8445693345589079, + "velocityX": -0.3562302844779268, + "velocityY": 1.1876255840606498, + "timestamp": 3.1462339366117305, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.165287928866771, + "y": 5.818827731905624, + "heading": -3.0720209570909076, + "angularVelocity": -0.7601498373804071, + "velocityX": -0.31556633062986994, + "velocityY": 1.082842487926418, + "timestamp": 3.172285950048662, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.15806690849839, + "y": 5.844282532149564, + "heading": -3.0896796043938592, + "angularVelocity": -0.6778227466257346, + "velocityX": -0.27717705527303127, + "velocityY": 0.9770761214123779, + "timestamp": 3.1983379634855935, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.151793962845926, + "y": 5.866960693109083, + "heading": -3.1052416789306485, + "angularVelocity": -0.5973463269724832, + "velocityX": -0.2407854451499951, + "velocityY": 0.8704955190668501, + "timestamp": 3.224389976922525, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.1464230753720366, + "y": 5.8868444435667495, + "heading": -3.1187499404873535, + "angularVelocity": -0.5185112309805378, + "velocityX": -0.2061601682684892, + "velocityY": 0.7632327729986262, + "timestamp": 3.2504419903594566, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.1419133029864885, + "y": 5.90391873943064, + "heading": -3.1302424255201187, + "angularVelocity": -0.4411361548153143, + "velocityX": -0.17310648163399853, + "velocityY": 0.6553925632360712, + "timestamp": 3.276494003796388, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.1382280065351003, + "y": 5.918170725554471, + "heading": -3.139753071131935, + "angularVelocity": -0.36506374583446893, + "velocityX": -0.1414591797409874, + "velocityY": 0.5470589119083662, + "timestamp": 3.3025460172333196, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.135334224654641, + "y": 5.9295893243238895, + "heading": -3.1473122451070203, + "angularVelocity": -0.2901569966323474, + "velocityX": -0.11107709150637847, + "velocityY": 0.43830004913290593, + "timestamp": 3.328598030670251, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.133202160318781, + "y": 5.9381649169747, + "heading": -3.1529471958838693, + "angularVelocity": -0.2162961719058987, + "velocityX": -0.08183875465217869, + "velocityY": 0.32917197250689534, + "timestamp": 3.3546500441071827, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.131804756638863, + "y": 5.943889093714475, + "heading": -3.156682435526014, + "angularVelocity": -0.14337623658866083, + "velocityX": -0.053638989681216866, + "velocityY": 0.2197210881082821, + "timestamp": 3.3807020575441142, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.131117343902588, + "y": 5.946754455566406, + "heading": -3.158540067224243, + "angularVelocity": -0.07130472670475889, + "velocityX": -0.026386165427844832, + "velocityY": 0.10998619584120908, + "timestamp": 3.4067540709810458, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + }, + { + "x": 2.131117343902588, + "y": 5.946754455566406, + "heading": -3.158540067224243, + "angularVelocity": -1.1257235159322888e-32, + "velocityX": -4.497825258620012e-34, + "velocityY": 8.02311483706782e-33, + "timestamp": 3.4328060844179773, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ] + } + ], + "eventMarkers": [] } \ No newline at end of file From bdcb3e5ee1d9f8d219769a975f1964dcfe13c9cc Mon Sep 17 00:00:00 2001 From: team computer Date: Sat, 28 Sep 2024 14:19:40 -0700 Subject: [PATCH 13/21] revert choreo swerve pid values --- .../java/frc/robot/subsystems/swerve/SwerveSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 2ecc5de7..936abef7 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -656,8 +656,8 @@ public Command runChoreoTraj(ChoreoTrajectory traj, boolean resetPose) { traj, this::getPose, Choreo.choreoSwerveController( - new PIDController(10, 0.0, 0.0), - new PIDController(10, 0.0, 0.0), + new PIDController(1.5, 0.0, 0.0), + new PIDController(1.5, 0.0, 0.0), new PIDController(3.0, 0.0, 0.0)), (ChassisSpeeds speeds) -> this.runVelocity(speeds), () -> { From 0d23d395e54a122fbe1fdbb146c85a4bd11e8692 Mon Sep 17 00:00:00 2001 From: team computer Date: Sun, 29 Sep 2024 10:17:13 -0700 Subject: [PATCH 14/21] add 3 center auto, works in match --- choreo.chor | 7404 ++++++++++++++---------- src/main/deploy/choreo/3 center.1.traj | 362 ++ src/main/deploy/choreo/3 center.2.traj | 551 ++ src/main/deploy/choreo/3 center.3.traj | 551 ++ src/main/deploy/choreo/3 center.traj | 1412 +++++ src/main/deploy/choreo/source 3.1.traj | 2742 ++++----- src/main/deploy/choreo/source 3.2.traj | 3077 +++++----- src/main/deploy/choreo/source 3.traj | 5797 +++++++++---------- src/main/java/frc/robot/Robot.java | 42 + 9 files changed, 13183 insertions(+), 8755 deletions(-) create mode 100644 src/main/deploy/choreo/3 center.1.traj create mode 100644 src/main/deploy/choreo/3 center.2.traj create mode 100644 src/main/deploy/choreo/3 center.3.traj create mode 100644 src/main/deploy/choreo/3 center.traj diff --git a/choreo.chor b/choreo.chor index 0f807e40..46e0d114 100644 --- a/choreo.chor +++ b/choreo.chor @@ -15980,12 +15980,12 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 14 }, { - "x": 8.779803276062012, - "y": 2.7246744632720947, - "heading": 0.31794821481648144, + "x": 8.628730773925781, + "y": 2.817955732345581, + "heading": 0.38460441295906894, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -16015,9 +16015,9 @@ "x": 0.7778744697570801, "y": 4.333664894104004, "heading": -1.051650156247713, - "angularVelocity": -1.3297634990832717e-18, - "velocityX": 8.096904896283762e-18, - "velocityY": -2.035450603469087e-18, + "angularVelocity": -1.595681461469011e-18, + "velocityX": 6.125258661649382e-18, + "velocityY": -3.9185398661174254e-18, "moduleForcesX": [ 0, 0, @@ -16033,4099 +16033,4078 @@ "timestamp": 0 }, { - "x": 0.7908719793717586, - "y": 4.323476837419605, - "heading": -1.0413708097378205, - "angularVelocity": 0.15431550267407224, - "velocityX": 0.19512108359903374, - "velocityY": -0.15294504247054264, + "x": 0.7908719793717547, + "y": 4.323476837419779, + "heading": -1.0413708097375791, + "angularVelocity": 0.15431550267789765, + "velocityX": 0.19512108359923522, + "velocityY": -0.1529450424681367, "moduleForcesX": [ - 63.74590962361433, - 58.38725396448112, - 49.37860903101583, - 45.506342584710694 + 63.74590962413147, + 58.38725396488212, + 49.378609031450075, + 45.50634258496312 ], "moduleForcesY": [ - -28.834822023862102, - -38.568182065872634, - -49.55287419418438, - -53.15308295520182 + -28.834822022718374, + -38.568182065265304, + -49.55287419375032, + -53.15308295498522 ], - "timestamp": 0.06661253297151565 + "timestamp": 0.06661253297142743 }, { - "x": 0.8168891188149916, - "y": 4.303112092011227, - "heading": -1.0210382023309228, - "angularVelocity": 0.3052369651757899, - "velocityX": 0.3905742400511663, - "velocityY": -0.30571942695958465, + "x": 0.8168891188150114, + "y": 4.303112092011752, + "heading": -1.0210382023301807, + "angularVelocity": 0.3052369651837109, + "velocityX": 0.3905742400520384, + "velocityY": -0.305719426954728, "moduleForcesX": [ - 63.57502695706841, - 58.50337248946953, - 49.388651702378716, - 45.92040304941029 + 63.5750269575728, + 58.50337248985279, + 49.38865170273007, + 45.92040304961696 ], "moduleForcesY": [ - -29.201580891943905, - -38.38697260898019, - -49.53833328341483, - -52.7922647589104 + -29.20158089084509, + -38.38697260839577, + -49.53833328306312, + -52.792264758730134 ], - "timestamp": 0.1332250659430313 + "timestamp": 0.13322506594285485 }, { - "x": 0.8559503614500349, - "y": 4.272583897748778, - "heading": -0.9908994985401418, - "angularVelocity": 0.4524479470502735, - "velocityX": 0.5863947952819387, - "velocityY": -0.45829505200625403, + "x": 0.8559503614500515, + "y": 4.272583897749817, + "heading": -0.9908994985386197, + "angularVelocity": 0.4524479470625842, + "velocityX": 0.5863947952826685, + "velocityY": -0.4582950519991483, "moduleForcesX": [ - 63.36650355926644, - 58.673994480374716, - 49.332465125535784, - 46.42312032557307 + 63.36650355975494, + 58.673994480740184, + 49.33246512578875, + 46.42312032573131 ], "moduleForcesY": [ - -29.64202317080124, - -38.12000070194584, - -49.58929691252429, - -52.34676597725229 + -29.642023169756015, + -38.120000701382985, + -49.589296912271145, + -52.34676597711144 ], - "timestamp": 0.19983759891454694 + "timestamp": 0.19983759891428227 }, { - "x": 0.9080832434863456, - "y": 4.231907621482131, - "heading": -0.9512364839325822, - "angularVelocity": 0.5954287104578356, - "velocityX": 0.7826287293204035, - "velocityY": -0.6106399869832296, + "x": 0.908083243486343, + "y": 4.231907621483826, + "heading": -0.9512364839299804, + "angularVelocity": 0.5954287104748315, + "velocityX": 0.7826287293211497, + "velocityY": -0.6106399869741947, "moduleForcesX": [ - 63.1087538854026, - 58.88683733974907, - 49.24358469003159, - 47.01667690510743 + 63.10875388586886, + 58.88683734009576, + 49.24358469017145, + 47.016676905215164 ], "moduleForcesY": [ - -30.17612796676512, - -37.78365700090767, - -49.672009408886844, - -51.809713634059214 + -30.176127965788748, + -37.78365700036693, + -49.6720094087466, + -51.80971363396091 ], - "timestamp": 0.2664501318860626 + "timestamp": 0.2664501318857097 }, { - "x": 0.9733191777952942, - "y": 4.181101065602064, - "heading": -0.9023828510876672, - "angularVelocity": 0.7334000174682638, - "velocityX": 0.9793342393516886, - "velocityY": -0.7627176690877809, + "x": 0.9733191777952241, + "y": 4.1811010656045475, + "heading": -0.9023828510836664, + "angularVelocity": 0.7334000174902384, + "velocityX": 0.9793342393519744, + "velocityY": -0.762717669076962, "moduleForcesX": [ - 62.78758091347448, - 59.12644232420397, - 49.162542801992586, - 47.70378434486226 + 62.78758091390919, + 59.1264423245301, + 49.16254280200449, + 47.70378434491597 ], "moduleForcesY": [ - -30.82641384591526, - -37.399521862180414, - -49.74596927329427, - -51.172358294771385 + -30.826413845028178, + -37.399521861664304, + -49.745969273280835, + -51.17235829472075 ], - "timestamp": 0.33306266485757824 + "timestamp": 0.33306266485713715 }, { - "x": 1.051694367518301, - "y": 4.120184874713526, - "heading": -0.8447458342032448, - "angularVelocity": 0.8652578473343552, - "velocityX": 1.1765832378198438, - "velocityY": -0.9144854304607654, + "x": 1.0516943675181405, + "y": 4.120184874716908, + "heading": -0.8447458341975047, + "angularVelocity": 0.8652578473616123, + "velocityX": 1.1765832378200387, + "velocityY": -0.9144854304484828, "moduleForcesX": [ - 62.38681753882747, - 59.37326562455472, - 49.13769712317514, - 48.48705032206493 + 62.386817539217134, + 59.37326562485723, + 49.13769712304434, + 48.487050322059 ], "moduleForcesY": [ - -31.615322936234296, - -36.99658263811015, - -49.763347170131574, - -50.42430963220079 + -31.615322935463205, + -36.99658263762404, + -49.76334717025892, + -50.42430963220592 ], - "timestamp": 0.3996751978290939 + "timestamp": 0.3996751978285646 }, { - "x": 1.1432508333296558, - "y": 4.049183171168427, - "heading": -0.778832566995066, - "angularVelocity": 0.9895024895144598, - "velocityX": 1.3744630586335072, - "velocityY": -1.065891062504106, + "x": 1.1432508333293583, + "y": 4.049183171172805, + "heading": -0.7788325669872294, + "angularVelocity": 0.9895024895472444, + "velocityX": 1.3744630586332751, + "velocityY": -1.0658910624905655, "moduleForcesX": [ - 61.889733856839435, - 59.602244362756366, - 49.22597947357009, - 49.36848790198351 + 61.88973385716236, + 59.60224436302891, + 49.225979473285065, + 49.36848790191129 ], "moduleForcesY": [ - -32.56119938527781, - -36.61435191109544, - -49.66762908534543, - -49.55361350286737 + -32.56119938466126, + -36.61435191065098, + -49.66762908562598, + -49.553613502938724 ], - "timestamp": 0.46628773080060953 + "timestamp": 0.46628773079999203 }, { - "x": 1.2480376237140218, - "y": 3.968124696368845, - "heading": -0.7052817755202949, - "angularVelocity": 1.1041584547794074, - "velocityX": 1.57307920461716, - "velocityY": -1.2168652231591865, + "x": 1.2480376237134958, + "y": 3.968124696374288, + "heading": -0.7052817755099939, + "angularVelocity": 1.1041584548178656, + "velocityX": 1.5730792046158117, + "velocityY": -1.2168652231448125, "moduleForcesX": [ - 61.28176481668813, - 59.78038532288985, - 49.4938549994705, - 50.34939808767109 + 61.28176481691346, + 59.78038532312139, + 49.49385499902187, + 50.34939808752191 ], "moduleForcesY": [ - -33.67250970561668, - -36.30739888304486, - -49.39059309707039, - -48.546399889287585 + -33.67250970520312, + -36.30739888266253, + -49.390593097517815, + -48.546399889441645 ], - "timestamp": 0.5329002637721252 + "timestamp": 0.5329002637714194 }, { - "x": 1.3661123895110021, - "y": 3.877044967671322, - "heading": -0.6249031875065618, - "angularVelocity": 1.206658633565307, - "velocityX": 1.7725608159574173, - "velocityY": -1.3673061905102768, + "x": 1.3661123895101355, + "y": 3.8770449676778536, + "heading": -0.6249031874934339, + "angularVelocity": 1.206658633609344, + "velocityX": 1.7725608159546524, + "velocityY": -1.367306190495742, "moduleForcesX": [ - 60.55523039747262, - 59.86237923234887, - 50.01933061446567, - 51.43105327265715 + 60.555230397553366, + 59.86237923251681, + 50.019330613853604, + 51.431053272417905 ], "moduleForcesY": [ - -34.9402966200205, - -36.15231999767806, - -48.845768467491496, - -47.385486747890475 + -34.940296619876236, + -36.15231999739851, + -48.845768468115836, + -47.38548674814936 ], - "timestamp": 0.5995127967436409 + "timestamp": 0.5995127967428469 }, { - "x": 1.4975436678185934, - "y": 3.7759903450547028, - "heading": -0.53873230694548, - "angularVelocity": 1.293613629704331, - "velocityX": 1.9730713192334703, - "velocityY": -1.5170511930514805, + "x": 1.4975436678172518, + "y": 3.775990345062295, + "heading": -0.5387323069291853, + "angularVelocity": 1.2936136297535843, + "velocityX": 1.9730713192289506, + "velocityY": -1.5170511930375694, "moduleForcesX": [ - 59.71660813639446, - 59.781944702186415, - 50.89674120838264, - 52.617056525494455 + 59.716608136271915, + 59.78194470225127, + 50.896741207611434, + 52.617056525140555 ], "moduleForcesY": [ - -36.32951289772708, - -36.25934407553948, - -47.91447235046562, - -46.0464745895729 + -36.329512897923145, + -36.259344075430484, + -47.91447235128193, + -46.04647458997633 ], - "timestamp": 0.6661253297151566 + "timestamp": 0.6661253297142743 }, { - "x": 1.6424143533198352, - "y": 3.665025662326569, - "heading": -0.4481205140243463, - "angularVelocity": 1.3602814497368003, - "velocityX": 2.174826253239616, - "velocityY": -1.665822898156168, + "x": 1.6424143533178284, + "y": 3.6650256623351347, + "heading": -0.4481205140046001, + "angularVelocity": 1.3602814497904157, + "velocityX": 2.1748262532325127, + "velocityY": -1.665822898143766, "moduleForcesX": [ - 58.79631756896231, - 59.433248892591386, - 52.247409534940125, - 53.9194587893842 + 58.79631756856442, + 59.43324889247598, + 52.247409534027724, + 53.91945878887585 ], "moduleForcesY": [ - -37.77070605252747, - -36.79353882986422, - -46.41555660113537, - -44.48747884319979 + -37.77070605314016, + -36.79353883004777, + -46.41555660215867, + -44.487478843814586 ], - "timestamp": 0.7327378626866723 + "timestamp": 0.7327378626857017 }, { - "x": 1.800826604673265, - "y": 3.5442490372900517, - "heading": -0.3549074004558732, - "angularVelocity": 1.3993329694928751, - "velocityX": 2.3781148124358094, - "velocityY": -1.8131216401598682, + "x": 1.8008266046703592, + "y": 3.544249037299425, + "heading": -0.3549074004325164, + "angularVelocity": 1.3993329695489327, + "velocityX": 2.378114812425462, + "velocityY": -1.8131216401501349, "moduleForcesX": [ - 57.85964296595883, - 58.62699304555619, - 54.24098287937368, - 55.37454877680063 + 57.85964296520334, + 58.62699304509392, + 54.240982878372066, + 55.374548776074526 ], "moduleForcesY": [ - -39.15408809177324, - -38.01758274488999, - -44.03657792902701, - -42.620768436938704 + -39.15408809288117, + -38.01758274559908, + -44.03657793025534, + -42.62076843787999 ], - "timestamp": 0.799350395658188 + "timestamp": 0.7993503956571292 }, { - "x": 1.9729064746231015, - "y": 3.4138237900863677, - "heading": -0.261785676023239, - "angularVelocity": 1.3979610184235784, - "velocityX": 2.58329569937567, - "velocityY": -1.9579685891760825, + "x": 1.9729064746189697, + "y": 3.413823790096245, + "heading": -0.2617856759964363, + "angularVelocity": 1.3979610184771598, + "velocityX": 2.583295699360691, + "velocityY": -1.9579685891711138, "moduleForcesX": [ - 57.01503970270268, - 56.97264868755776, - 57.13055506988329, - 57.088614242525495 + 57.01503970150166, + 56.972648686336136, + 57.13055506893403, + 57.08861424148898 ], "moduleForcesY": [ - -40.32815265808299, - -40.3871603414814, - -40.16401782504414, - -40.22274635092099 + -40.32815265976974, + -40.38716034320021, + -40.16401782638521, + -40.22274635238806 ], - "timestamp": 0.8659629286297037 + "timestamp": 0.8659629286285566 }, { - "x": 2.1587959216901558, - "y": 3.2740626515960307, - "heading": -0.1731498612166921, - "angularVelocity": 1.3306176908848175, - "velocityX": 2.7906076945992013, - "velocityY": -2.0981207628015075, + "x": 2.158795921684324, + "y": 3.274062651605914, + "heading": -0.17314986118757214, + "angularVelocity": 1.3306176909213672, + "velocityX": 2.790607694577369, + "velocityY": -2.0981207628041956, "moduleForcesX": [ - 56.41042369707265, - 53.501719202644416, - 61.26419264721754, - 59.40078934825349 + 56.41042369532152, + 53.50171919941402, + 61.2641926466866, + 59.4007893468711 ], "moduleForcesY": [ - -41.1079010332853, - -44.78370054636255, - -33.42235417217702, - -36.56648525897984 + -41.107901035672306, + -44.7837005502188, + -33.422354173129975, + -36.56648526121465 ], - "timestamp": 0.9325754616012194 + "timestamp": 0.9325754615999841 }, { - "x": 2.3585571947909116, - "y": 3.1257497783866914, - "heading": -0.09743941845228067, - "angularVelocity": 1.1365795502294775, - "velocityX": 2.9988541823830035, - "velocityY": -2.2265010290594955, + "x": 2.3585571947825676, + "y": 3.125749778395996, + "heading": -0.09743941842552141, + "angularVelocity": 1.1365795501955436, + "velocityX": 2.9988541823492687, + "velocityY": -2.226501029071138, "moduleForcesX": [ - 56.18321511797377, - 45.13645782831661, - 66.6575745567656, - 63.63924131102192 + 56.183215115418484, + 45.13645781804362, + 66.6575745572279, + 63.63924131085493 ], "moduleForcesY": [ - -41.32675572742186, - -53.045678977052305, - -20.39623436243562, - -28.01878833525786 + -41.32675573087057, + -53.04567898580249, + -20.39623436084957, + -28.018788335571973 ], - "timestamp": 0.9991879945727351 + "timestamp": 0.9991879945714115 }, { - "x": 2.571323749679796, - "y": 2.9722890611298696, - "heading": -0.05631702040111217, - "angularVelocity": 0.6173372519665775, - "velocityX": 3.1940919433264323, - "velocityY": -2.303781441886374, + "x": 2.5713237496680983, + "y": 2.9722890611360366, + "heading": -0.056317020373306215, + "angularVelocity": 0.6173372519831085, + "velocityX": 3.1940919432803097, + "velocityY": -2.30378144193652, "moduleForcesX": [ - 56.49315652197762, - 24.727674210970267, - 69.53856728119115, - 66.38848839184168 + 56.49315651832047, + 24.72767419303312, + 69.5385672809967, + 66.38848840011917 ], "moduleForcesY": [ - -40.73364522523241, - -64.83876276810156, - 2.4259673258224868, - 17.19340461150298 + -40.73364523028673, + -64.83876277492942, + 2.425967330179503, + 17.19340457572179 ], - "timestamp": 1.0658005275442508 + "timestamp": 1.0658005275428388 }, { - "x": 2.7978969801578932, - "y": 2.816123256954651, - "heading": -0.04923559160029977, - "angularVelocity": 0.1063077544857887, - "velocityX": 3.4013603802603107, - "velocityY": -2.3443907206170564, + "x": 2.797896980139407, + "y": 2.816123256947658, + "heading": -0.04923559157858403, + "angularVelocity": 0.10630775439450244, + "velocityX": 3.4013603801629086, + "velocityY": -2.3443907208177204, "moduleForcesX": [ - 61.70982060481134, - 40.01285570538606, - 68.37167087206099, - 60.43433119111175 + 61.70982059244306, + 40.01285562448143, + 68.37167087457641, + 60.434331225109716 ], "moduleForcesY": [ - -31.4520746032983, - -55.70693219082933, - 10.727828044445207, - 31.264613623300775 + -31.45207462723247, + -55.706932248574496, + 10.727828026787554, + 31.264613554872565 ], - "timestamp": 1.1324130605157665 + "timestamp": 1.1324130605142662 }, { - "x": 3.0347601843563274, - "y": 2.6710905971518515, - "heading": -0.04900748324484257, - "angularVelocity": 0.003424405968089362, - "velocityX": 3.55583542063653, - "velocityY": -2.1772578422264344, + "x": 3.034760184329724, + "y": 2.671090597132263, + "heading": -0.0490074832231963, + "angularVelocity": 0.003424405967050934, + "velocityX": 3.5558354205193843, + "velocityY": -2.177257842418406, "moduleForcesX": [ - 50.41350702863919, - 41.75482316229609, - 44.362342200824024, - 35.27998738074212 + 50.413506856910104, + 41.754823961326274, + 44.362341929459326, + 35.27998700294513 ], "moduleForcesY": [ - 38.99730957905846, - 46.617569740155716, - 46.965805915927774, - 53.3083103983914 + 38.99730976426598, + 46.61756909982145, + 46.965806150666, + 53.30831062845783 ], - "timestamp": 1.1990255934872822 + "timestamp": 1.1990255934856935 }, { - "x": 3.2779712543514585, - "y": 2.5369613930825095, - "heading": -0.04879559855232118, - "angularVelocity": 0.0031808532579297136, - "velocityX": 3.6511307879424364, - "velocityY": -2.0135730933201033, + "x": 3.277971254366929, + "y": 2.5369613931399093, + "heading": -0.048795598530848285, + "angularVelocity": 0.003180853255331212, + "velocityX": 3.651130788578886, + "velocityY": -2.0135730921670043, "moduleForcesX": [ - 26.514032908634963, - 26.48917929707793, - 26.505657516129617, - 26.480809194623138 + 26.51403343046492, + 26.489179612407455, + 26.505657599753675, + 26.480809112111523 ], "moduleForcesY": [ - 45.5016606384836, - 45.51171619004381, - 45.5152401681101, - 45.525290455269825 + 45.50166091112535, + 45.51171684473839, + 45.515240274553484, + 45.52529091768426 ], - "timestamp": 1.265638126458798 + "timestamp": 1.2656381264571208 }, { - "x": 3.5240170136884648, - "y": 2.4081056080213394, - "heading": -0.04858282682880418, - "angularVelocity": 0.003194169535753657, - "velocityX": 3.6936856828761946, - "velocityY": -1.93440752532666, + "x": 3.5240170136688564, + "y": 2.408105608012548, + "heading": -0.04858282680739443, + "angularVelocity": 0.0031941695348100155, + "velocityX": 3.6936856823544875, + "velocityY": -1.9344075263229015, "moduleForcesX": [ - 11.832044679638638, - 11.833159737293062, - 11.83210215812816, - 11.833217225479276 + 11.832044385798296, + 11.833159401629988, + 11.832101854528789, + 11.833216870497965 ], "moduleForcesY": [ - 22.01300562318127, - 22.0128485655156, - 22.01202572778171, - 22.011868671306686 + 22.01300500476856, + 22.012848038290926, + 22.012025057287687, + 22.01186809697094 ], - "timestamp": 1.3322506594303136 + "timestamp": 1.3322506594285481 }, { - "x": 3.7708350348315482, - "y": 2.2807352279621504, - "heading": -0.0483699449005901, - "angularVelocity": 0.0031958239496029865, - "velocityX": 3.7052790238231275, - "velocityY": -1.9121083432400703, + "x": 3.770835034817424, + "y": 2.2807352279647817, + "heading": -0.048369944879233955, + "angularVelocity": 0.0031958239488024064, + "velocityX": 3.7052790239103643, + "velocityY": -1.912108343071116, "moduleForcesX": [ - 3.2235324585038754, - 3.2236618531632817, - 3.2235271302506407, - 3.2236565249546465 + 3.223532949769158, + 3.2236620923869292, + 3.223527231378966, + 3.223656370634464 ], "moduleForcesY": [ - 6.200483556552904, - 6.200476317722027, - 6.200352895038578, - 6.200345656224632 + 6.200483740926388, + 6.200477036360204, + 6.200352815192096, + 6.2003461290433926 ], - "timestamp": 1.3988631924018293 + "timestamp": 1.3988631923999755 }, { - "x": 4.017945860044819, - "y": 2.153933675847828, - "heading": -0.04815735530566025, - "angularVelocity": 0.0031914353868025967, - "velocityX": 3.709674654151622, - "velocityY": -1.90356899006594, + "x": 4.017945860027259, + "y": 2.153933675844563, + "heading": -0.048157355284360694, + "angularVelocity": 0.0031914353859573896, + "velocityX": 3.7096746541049623, + "velocityY": -1.9035689901569823, "moduleForcesX": [ - 1.2223925594156837, - 1.2220511392203353, - 1.2224089657777908, - 1.222067545703039 + 1.2223938746517689, + 1.222051260087835, + 1.222408785272593, + 1.222066141189196 ], "moduleForcesY": [ - 2.374233190641205, - 2.3742500204088812, - 2.3745815684863776, - 2.374598398301541 + 2.3742328041770366, + 2.3742514629482714, + 2.374579889586726, + 2.3745987319670263 ], - "timestamp": 1.465475725373345 + "timestamp": 1.4654757253714028 }, { - "x": 4.265335286953013, - "y": 2.027676292006606, - "heading": -0.04794522174606807, - "angularVelocity": 0.0031845892976762468, - "velocityX": 3.7138570757244884, - "velocityY": -1.895399832569214, + "x": 4.265335287008856, + "y": 2.027676292147968, + "heading": -0.047945221724793284, + "angularVelocity": 0.003184589297308503, + "velocityX": 3.713857076831331, + "velocityY": -1.8953998304005573, "moduleForcesX": [ - 1.163199663544347, - 1.1626670888284039, - 1.1632251975919956, - 1.1626926239772695 + 1.1632026163804865, + 1.162667533168695, + 1.1632254607113321, + 1.1626902466338753 ], "moduleForcesY": [ - 2.271195979291761, - 2.271222065843487, - 2.271739492202394, - 2.2717655728058856 + 2.271196262649088, + 2.271225591627576, + 2.271736925412616, + 2.271766843769598 ], - "timestamp": 1.5320882583448607 + "timestamp": 1.5320882583428301 }, { - "x": 4.513387263491877, - "y": 1.9027254137370513, - "heading": -0.04773334654928872, - "angularVelocity": 0.003180710706046184, - "velocityX": 3.7238033966511064, - "velocityY": -1.8757863227177534, + "x": 4.513387263726315, + "y": 1.9027254142338006, + "heading": -0.04773334652797979, + "angularVelocity": 0.0031807107065629863, + "velocityX": 3.723803399337141, + "velocityY": -1.8757863173851013, "moduleForcesX": [ - 2.765776223844279, - 2.765473269227589, - 2.7657891163065877, - 2.7654861619049216 + 2.7657780823712406, + 2.765473685160751, + 2.7657896002600624, + 2.7654851599392303 ], "moduleForcesY": [ - 5.453486682132146, - 5.453502875542226, - 5.453793409345764, - 5.453809602835306 + 5.453487457755567, + 5.453505306304338, + 5.453792661463415, + 5.453810663508131 ], - "timestamp": 1.5987007913163764 + "timestamp": 1.5987007913142575 }, { - "x": 4.7636905451889415, - "y": 1.7823482264471835, - "heading": -0.04752024111954168, - "angularVelocity": 0.003199179197076531, - "velocityX": 3.7576004173883883, - "velocityY": -1.8071252048220714, + "x": 4.76369054518264, + "y": 1.7823482264444208, + "heading": -0.047520241098396636, + "angularVelocity": 0.0031991791946204317, + "velocityX": 3.7576004137793353, + "velocityY": -1.8071252123232402, "moduleForcesX": [ - 9.396675512144679, - 9.39819901784068, - 9.396709254967268, - 9.398232777338956 + 9.396673705872347, + 9.39819730006515, + 9.396707473785185, + 9.398231081199015 ], "moduleForcesY": [ - 19.092390606150673, - 19.092220862164694, - 19.091009053610517, - 19.090839312476135 + 19.092387038061002, + 19.09221720512043, + 19.09100557309194, + 19.090835744537614 ], - "timestamp": 1.6653133242878921 + "timestamp": 1.6653133242856848 }, { - "x": 5.0188400094891765, - "y": 1.6726490442267128, - "heading": -0.04723633820300304, - "angularVelocity": 0.004262004518879671, - "velocityX": 3.8303522313036735, - "velocityY": -1.646824964115687, + "x": 5.018840009680974, + "y": 1.6726490446859121, + "heading": -0.047236338181666296, + "angularVelocity": 0.004262004521763258, + "velocityX": 3.8303522342826466, + "velocityY": -1.6468249571828, "moduleForcesX": [ - 20.160339598017387, - 20.271778827791216, - 20.186266828439518, - 20.297839903273452 + 20.160341309415056, + 20.271780653378354, + 20.186268676149652, + 20.297841846829897 ], "moduleForcesY": [ - 44.61714719861703, - 44.58388431235552, - 44.56094399930638, - 44.52760480062931 + 44.61715118244674, + 44.583888259336774, + 44.56094806815956, + 44.52760885668282 ], - "timestamp": 1.7319258572594078 + "timestamp": 1.7319258572571121 }, { - "x": 5.276040962243286, - "y": 1.5678524061254324, - "heading": -0.046947349952411, - "angularVelocity": 0.0043383465197998905, - "velocityX": 3.8611495657144865, - "velocityY": -1.5732270404143434, + "x": 5.276040962308425, + "y": 1.567852406274765, + "heading": -0.04694734993114214, + "angularVelocity": 0.004338346518786609, + "velocityX": 3.861149563818171, + "velocityY": -1.5732270450682089, "moduleForcesX": [ - 8.560118396401565, - 8.566501267033127, - 8.560247116014803, - 8.56663029963911 + 8.560117120058553, + 8.56649998262021, + 8.560245714730028, + 8.566628839534616 ], "moduleForcesY": [ - 20.46747803904423, - 20.466793414894244, - 20.461850439324866, - 20.461165848040086 + 20.467474806427994, + 20.466790232197752, + 20.461847166644493, + 20.46116264960528 ], - "timestamp": 1.7985383902309235 + "timestamp": 1.7985383902285395 }, { - "x": 5.534216899574317, - "y": 1.4654824117182799, - "heading": -0.04665631309500156, - "angularVelocity": 0.004369100594536302, - "velocityX": 3.8757862194105366, - "velocityY": -1.536797804264961, + "x": 5.53421689953023, + "y": 1.4654824115931464, + "heading": -0.04665631307381442, + "angularVelocity": 0.004369100593315545, + "velocityX": 3.875786217775965, + "velocityY": -1.5367978083873335, "moduleForcesX": [ - 4.068624748426005, - 4.071060153628479, - 4.068549709495257, - 4.070985139742783 + 4.068624788601681, + 4.071060271920588, + 4.068549740960962, + 4.07098524097379 ], "moduleForcesY": [ - 10.13063153345665, - 10.130481600946313, - 10.128231656616295, - 10.128081730271346 + 10.130631677004772, + 10.130481677327632, + 10.1282318686583, + 10.1280818895711 ], - "timestamp": 1.8651509232024392 + "timestamp": 1.8651509231999668 }, { - "x": 5.794372355867246, - "y": 1.3682547977858872, - "heading": -0.04636219278537718, - "angularVelocity": 0.004415389964980432, - "velocityX": 3.905503134135057, - "velocityY": -1.459599411629766, + "x": 5.794372355842512, + "y": 1.3682547977136283, + "heading": -0.04636219276413434, + "angularVelocity": 0.00441538996582264, + "velocityX": 3.9055031344307545, + "velocityY": -1.4595994108379353, "moduleForcesX": [ - 8.260963536224425, - 8.264869277780319, - 8.261045504013175, - 8.264951367768367 + 8.260964021908269, + 8.26486985270304, + 8.261046002058745, + 8.26495195604506 ], "moduleForcesY": [ - 21.467349064567532, - 21.466934299940117, - 21.46397136076802, - 21.463556605012805 + 21.467350425085247, + 21.466935581870978, + 21.463972811998225, + 21.46355797712207 ], - "timestamp": 1.931763456173955 + "timestamp": 1.9317634561713941 }, { - "x": 6.058520814972377, - "y": 1.282472529523383, - "heading": -0.04606005529713513, - "angularVelocity": 0.004535745373490557, - "velocityX": 3.965446850941467, - "velocityY": -1.2877797080496214, + "x": 6.058520814952769, + "y": 1.2824725294681238, + "heading": -0.046060055275852295, + "angularVelocity": 0.004535745374097144, + "velocityX": 3.965446851023664, + "velocityY": -1.2877797077961244, "moduleForcesX": [ - 16.659711714491884, - 16.673161829473102, - 16.66222092531276, - 16.675673239510985 + 16.659711667705363, + 16.673161791773882, + 16.66222084343888, + 16.67567316842716 ], "moduleForcesY": [ - 47.77989163980646, - 47.77656557923747, - 47.77432962004692, - 47.771002564051436 + 47.779891485446186, + 47.776565422843, + 47.774329477432524, + 47.77100241879716 ], - "timestamp": 1.9983759891454707 + "timestamp": 1.9983759891428214 }, { - "x": 6.326935018479887, - "y": 1.2111884138637838, - "heading": -0.04572387766988923, - "angularVelocity": 0.0050467624071529864, - "velocityX": 4.0294850163149665, - "velocityY": -1.0701306868195675, + "x": 6.326935018464312, + "y": 1.2111884138255193, + "heading": -0.045723877648205694, + "angularVelocity": 0.005046762413175208, + "velocityX": 4.029485016380848, + "velocityY": -1.070130686565861, "moduleForcesX": [ - 17.76434909368371, - 17.833039573651952, - 17.77928266449683, - 17.84803536932326 + 17.764349079949987, + 17.833039578816862, + 17.779282668917745, + 17.84803535533965 ], "moduleForcesY": [ - 60.533843276425394, - 60.51552676552759, - 60.52159010023538, - 60.50324061772363 + 60.53384327945482, + 60.51552676309896, + 60.521590097018716, + 60.50324062036407 ], - "timestamp": 2.064988522116986 + "timestamp": 2.0649885221142488 }, { - "x": 6.598149001232439, - "y": 1.1556493567666084, - "heading": -0.04255962682130831, - "angularVelocity": 0.04750233488244815, - "velocityX": 4.071515834242889, - "velocityY": -0.8337628764383538, + "x": 6.598149001225702, + "y": 1.1556493567444743, + "heading": -0.04255962681817847, + "angularVelocity": 0.047502334603978215, + "velocityX": 4.0715158343809605, + "velocityY": -0.8337628761973023, "moduleForcesX": [ - 8.287572421549362, - 14.286479207247982, - 8.808436728764809, - 15.365145055742365 + 8.28757237649673, + 14.286479227047964, + 8.808436870157584, + 15.36514501967037 ], "moduleForcesY": [ - 66.4507233464468, - 65.45898479609045, - 66.07372493186959, - 64.91023150582662 + 66.45072335474919, + 65.45898479439751, + 66.07372490759722, + 64.91023150887932 ], - "timestamp": 2.1316010550885016 + "timestamp": 2.1316010550856763 }, { - "x": 6.859559783326081, - "y": 1.113135958184906, - "heading": -0.030602705565701032, - "angularVelocity": 0.17949957346195378, - "velocityX": 3.9243483235418233, - "velocityY": -0.6382192161928673, + "x": 6.859559783323042, + "y": 1.1131359581740934, + "heading": -0.0306027055657009, + "angularVelocity": 0.17949957341520645, + "velocityX": 3.9243483236025436, + "velocityY": -0.6382192160237583, "moduleForcesX": [ - -42.45988336627168, - -29.346972124084434, - -52.177609628795565, - -39.698593640879075 + -42.459883383315024, + -29.34697212286383, + -52.17760966362154, + -39.69859367485398 ], "moduleForcesY": [ - 54.43758266703957, - 62.32119709611513, - 44.88720682067673, - 55.84212799694645 + 54.43758265314552, + 62.32119709610292, + 44.88720677947976, + 55.842127971468855 ], - "timestamp": 2.198213588060017 + "timestamp": 2.198213588057104 }, { "x": 7.107321739196777, "y": 1.0795719623565674, - "heading": -0.030602705565701018, - "angularVelocity": 3.347783365519795e-18, - "velocityX": 3.7194495512825236, - "velocityY": -0.5038690818541787, + "heading": -0.030602705565700897, + "angularVelocity": 1.6075047383869973e-18, + "velocityX": 3.7194495513330783, + "velocityY": -0.5038690816925215, "moduleForcesX": [ - -61.323422950505716, - -65.95546140653828, - -44.62846950333632, - -55.985729468867824 + -61.32342295009295, + -65.955461406566, + -44.628469510881004, + -55.985729471196706 ], "moduleForcesY": [ - 32.44664959475204, - 22.20881577411158, - 53.33306981117861, - 41.43874553341409 + 32.44664959500376, + 22.208815773674722, + 53.33306980468184, + 41.43874553017478 ], - "timestamp": 2.2648261210315326 + "timestamp": 2.2648261210285314 }, { - "x": 7.342124626950215, - "y": 1.053916100516507, - "heading": -0.030602705565701004, - "angularVelocity": 7.672386391662901e-19, - "velocityX": 3.4979351696684997, - "velocityY": -0.3822037381956934, + "x": 7.342124626958182, + "y": 1.0539161005261706, + "heading": -0.03060270556570089, + "angularVelocity": 2.0024672081036984e-19, + "velocityX": 3.4979351697057512, + "velocityY": -0.38220373804281776, "moduleForcesX": [ - -61.122061696457, - -61.122061696457, - -61.122061696457, - -61.122061696457 + -61.122061698454715, + -61.122061698454715, + -61.122061698454715, + -61.122061698454715 ], "moduleForcesY": [ - 33.57089768760224, - 33.57089768760224, - 33.57089768760223, - 33.57089768760224 + 33.57089768389125, + 33.57089768389125, + 33.570897683891246, + 33.570897683891246 ], - "timestamp": 2.3319522615585138 + "timestamp": 2.3319522615570754 }, { - "x": 7.56101179722364, - "y": 1.0341858544549376, - "heading": -0.03060270556570099, - "angularVelocity": 7.672403149471102e-19, - "velocityX": 3.260833537501588, - "velocityY": -0.2939279080649534, + "x": 7.561011797238179, + "y": 1.0341858544731557, + "heading": -0.030602705565700883, + "angularVelocity": 2.0024561890342501e-19, + "velocityX": 3.2608335375235695, + "velocityY": -0.29392790793066653, "moduleForcesX": [ - -65.42302348088484, - -65.42302348088484, - -65.42302348088484, - -65.42302348088484 + -65.42302348307652, + -65.42302348307652, + -65.42302348307652, + -65.42302348307652 ], "moduleForcesY": [ - 24.35778975736353, - 24.357789757363527, - 24.357789757363534, - 24.357789757363534 + 24.3577897514352, + 24.3577897514352, + 24.3577897514352, + 24.357789751435195 ], - "timestamp": 2.399078402085495 + "timestamp": 2.3990784020856193 }, { - "x": 7.76353893371165, - "y": 1.0190536925576863, - "heading": -0.030602705565700973, - "angularVelocity": 7.672257598260093e-19, - "velocityX": 3.0171127804763933, - "velocityY": -0.22542874919449496, + "x": 7.7635389337314935, + "y": 1.0190536925830782, + "heading": -0.030602705565700876, + "angularVelocity": 2.0022790819956785e-19, + "velocityX": 3.0171127804851614, + "velocityY": -0.22542874908236665, "moduleForcesX": [ - -67.24942660173558, - -67.24942660173558, - -67.24942660173558, - -67.24942660173558 + -67.24942660363206, + -67.24942660363206, + -67.24942660363206, + -67.24942660363206 ], "moduleForcesY": [ - 18.900848713116805, - 18.900848713116805, - 18.9008487131168, - 18.900848713116808 + 18.90084870634776, + 18.900848706347762, + 18.90084870634775, + 18.90084870634776 ], - "timestamp": 2.466204542612476 + "timestamp": 2.466204542614163 }, { - "x": 7.949481024014891, - "y": 1.0076590456800256, - "heading": -0.03060270556570096, - "angularVelocity": 7.672013523245006e-19, - "velocityX": 2.7700399403791343, - "velocityY": -0.16974976943713838, + "x": 7.949481024038794, + "y": 1.0076590457110912, + "heading": -0.03060270556570087, + "angularVelocity": 2.0020051992032217e-19, + "velocityX": 2.7700399403750904, + "velocityY": -0.16974976934866481, "moduleForcesX": [ - -68.17436080640248, - -68.17436080640248, - -68.17436080640248, - -68.17436080640248 + -68.17436080799112, + -68.17436080799114, + -68.17436080799114, + -68.17436080799112 ], "moduleForcesY": [ - 15.36339993436348, - 15.363399934363485, - 15.363399934363473, - 15.363399934363477 + 15.363399927305814, + 15.363399927305815, + 15.363399927305815, + 15.363399927305814 ], - "timestamp": 2.5333306831394573 + "timestamp": 2.533330683142707 }, { - "x": 8.118709429041983, - "y": 0.9994031964377751, - "heading": -0.030602705565700945, - "angularVelocity": 7.671706888831663e-19, - "velocityX": 2.5210507217984657, - "velocityY": -0.12299007774670825, + "x": 8.118709429068772, + "y": 0.9994031964729742, + "heading": -0.030602705565700862, + "angularVelocity": 2.001673444928878e-19, + "velocityX": 2.521050721782785, + "velocityY": -0.12299007768226591, "moduleForcesX": [ - -68.70314364670283, - -68.70314364670283, - -68.70314364670283, - -68.70314364670283 + -68.7031436480408, + -68.7031436480408, + -68.7031436480408, + -68.7031436480408 ], "moduleForcesY": [ - 12.90231694942824, - 12.902316949428238, - 12.902316949428242, - 12.902316949428242 + 12.902316942304648, + 12.902316942304648, + 12.902316942304651, + 12.902316942304651 ], - "timestamp": 2.6004568236664385 + "timestamp": 2.600456823671251 }, { - "x": 8.271143977771962, - "y": 0.9938469245659062, - "heading": -0.030602705565700928, - "angularVelocity": 7.671359538796945e-19, - "velocityX": 2.270867169380966, - "velocityY": -0.08277359353968143, + "x": 8.271143977800422, + "y": 0.9938469246036682, + "heading": -0.030602705565700855, + "angularVelocity": 2.0013070310221862e-19, + "velocityX": 2.270867169353027, + "velocityY": -0.08277359349957805, "moduleForcesX": [ - -69.0326940172405, - -69.0326940172405, - -69.0326940172405, - -69.0326940172405 + -69.03269401838256, + -69.03269401838256, + -69.03269401838256, + -69.03269401838256 ], "moduleForcesY": [ - 11.096861571799984, - 11.096861571799986, - 11.096861571799984, - 11.096861571799984 + 11.096861564702607, + 11.096861564702607, + 11.096861564702607, + 11.096861564702605 ], - "timestamp": 2.6675829641934197 + "timestamp": 2.667582964199795 }, { - "x": 8.406731406019375, - "y": 0.9906547367603562, - "heading": -0.030602705565700914, - "angularVelocity": 7.670985863150012e-19, - "velocityX": 2.019890123027612, - "velocityY": -0.047555062461336506, + "x": 8.406731406048316, + "y": 0.9906547367990796, + "heading": -0.030602705565700848, + "angularVelocity": 2.0009206377793148e-19, + "velocityX": 2.0198901229876864, + "velocityY": -0.047555062445912684, "moduleForcesX": [ - -69.25164136036881, - -69.25164136036881, - -69.25164136036881, - -69.25164136036881 + -69.25164136135751, + -69.25164136135751, + -69.25164136135751, + -69.25164136135751 ], "moduleForcesY": [ - 9.717785426549383, - 9.717785426549383, - 9.717785426549389, - 9.717785426549389 + 9.71778541951536, + 9.717785419515362, + 9.717785419515362, + 9.717785419515362 ], - "timestamp": 2.734709104720401 + "timestamp": 2.734709104728339 }, { - "x": 8.5254345460703, - "y": 0.9895621634069518, - "heading": -0.0306027055657009, - "angularVelocity": 7.670595843658019e-19, - "velocityX": 1.7683593771223813, - "velocityY": -0.016276421448131698, + "x": 8.525434546098682, + "y": 0.9895621634450235, + "heading": -0.03060270556570084, + "angularVelocity": 2.0005241446773043e-19, + "velocityX": 1.7683593770728674, + "velocityY": -0.016276421457464545, "moduleForcesX": [ - -69.40442267327488, - -69.40442267327488, - -69.40442267327488, - -69.40442267327488 + -69.40442267414187, + -69.40442267414187, + -69.40442267414187, + -69.40442267414187 ], "moduleForcesY": [ - 8.630658704124505, - 8.630658704124507, - 8.630658704124507, - 8.630658704124508 + 8.63065869716772, + 8.63065869716772, + 8.630658697167716, + 8.630658697167716 ], - "timestamp": 2.801835245247382 + "timestamp": 2.801835245256883 }, { - "x": 8.627226437405287, - "y": 0.9903554058889321, - "heading": -0.030602705565700883, - "angularVelocity": 7.670197234878886e-19, - "velocityX": 1.516426991569238, - "velocityY": 0.011817191868226978, + "x": 8.627226437431862, + "y": 0.9903554059247525, + "heading": -0.030602705565700834, + "angularVelocity": 2.0001246831972175e-19, + "velocityX": 1.516426991507014, + "velocityY": 0.011817191834409059, "moduleForcesX": [ - -69.51524637407707, - -69.51524637407705, - -69.51524637407707, - -69.51524637407705 + -69.51524637484586, + -69.51524637484586, + -69.51524637484586, + -69.51524637484586 ], "moduleForcesY": [ - 7.751819786448097, - 7.751819786448097, - 7.751819786448097, - 7.751819786448097 + 7.751819779571535, + 7.751819779571536, + 7.751819779571532, + 7.751819779571534 ], - "timestamp": 2.868961385774363 + "timestamp": 2.868961385785427 }, { - "x": 8.712086900940639, - "y": 0.9928580419153559, - "heading": -0.03060270556570087, - "angularVelocity": 7.6697962024819e-19, - "velocityX": 1.2641939916274305, - "velocityY": 0.03728258479896153, + "x": 8.712086900964183, + "y": 0.992858041947287, + "heading": -0.030602705565700827, + "angularVelocity": 1.9997279446904402e-19, + "velocityX": 1.264193991552835, + "velocityY": 0.03728258474014907, "moduleForcesX": [ - -69.59819435728923, - -69.59819435728923, - -69.59819435728923, - -69.59819435728923 + -69.59819435797758, + -69.59819435797758, + -69.59819435797758, + -69.59819435797758 ], "moduleForcesY": [ - 7.026619700405902, - 7.026619700405903, - 7.0266197004059014, - 7.0266197004059014 + 7.026619693607424, + 7.026619693607423, + 7.026619693607425, + 7.026619693607425 ], - "timestamp": 2.9360875263013444 + "timestamp": 2.9360875263139707 }, { - "x": 8.780000437865695, - "y": 0.996921987964414, - "heading": -0.03060270556570085, - "angularVelocity": 7.669398274852111e-19, - "velocityX": 1.011730100850266, - "velocityY": 0.06054192922688254, + "x": 8.780000437885118, + "y": 0.9969219879908205, + "heading": -0.03060270556570082, + "angularVelocity": 1.9993387143813067e-19, + "velocityX": 1.0117301007653263, + "velocityY": 0.06054192914316773, "moduleForcesX": [ - -69.66190364637255, - -69.66190364637255, - -69.66190364637255, - -69.66190364637255 + -69.66190364699403, + -69.66190364699403, + -69.66190364699403, + -69.66190364699403 ], "moduleForcesY": [ - 6.417908736725351, - 6.417908736725351, - 6.417908736725356, - 6.417908736725355 + 6.417908730000572, + 6.417908730000572, + 6.41790873000057, + 6.41790873000057 ], - "timestamp": 3.0032136668283256 + "timestamp": 3.0032136668425147 }, { - "x": 8.830954883407468, - "y": 1.0024211476734168, - "heading": -0.030602705565700838, - "angularVelocity": 7.669008842488089e-19, - "velocityX": 0.75908498748391, - "velocityY": 0.08192277502968102, + "x": 8.830954883421578, + "y": 1.0024211476926257, + "heading": -0.030602705565700813, + "angularVelocity": 1.99896148062903e-19, + "velocityX": 0.7590849873871007, + "velocityY": 0.08192277492054269, "moduleForcesX": [ - -69.71190806731377, - -69.71190806731377, - -69.71190806731377, - -69.71190806731377 + -69.71190806787894, + -69.71190806787894, + -69.71190806787892, + -69.71190806787892 ], "moduleForcesY": [ - 5.89957801675687, - 5.899578016756869, - 5.899578016756865, - 5.899578016756865 + 5.89957801010069, + 5.899578010100691, + 5.8995780101006865, + 5.8995780101006865 ], - "timestamp": 3.0703398073553068 + "timestamp": 3.0703398073710586 }, { - "x": 8.86494051215884, - "y": 1.0092468204121565, - "heading": -0.030602705565700824, - "angularVelocity": 7.668631929139065e-19, - "velocityX": 0.5062949915570888, - "velocityY": 0.10168427210552212, + "x": 8.86494051216642, + "y": 1.009246820422566, + "heading": -0.030602705565700807, + "angularVelocity": 1.9985996963971072e-19, + "velocityX": 0.5062949914480063, + "velocityY": 0.10168427197206922, "moduleForcesX": [ - -69.7518852496981, - -69.7518852496981, - -69.7518852496981, - -69.7518852496981 + -69.75188525021527, + -69.75188525021527, + -69.75188525021527, + -69.75188525021527 ], "moduleForcesY": [ - 5.452754058783733, - 5.452754058783732, - 5.452754058783737, - 5.452754058783736 + 5.452754052190986, + 5.452754052190985, + 5.452754052190986, + 5.452754052190985 ], - "timestamp": 3.137465947882288 + "timestamp": 3.1374659478996025 }, { "x": 8.881949424743652, "y": 1.017304301261902, - "heading": -0.030602705565700813, - "angularVelocity": -6.192148052914439e-17, - "velocityX": 0.25338731604818493, - "velocityY": 0.12003491913118446, + "heading": -0.030602705565700807, + "angularVelocity": -6.178266080762978e-17, + "velocityX": 0.25338731592940406, + "velocityY": 0.12003491897331502, "moduleForcesX": [ - -69.78435636275017, - -69.78435636275017, - -69.78435636275017, - -69.78435636275017 + -69.78435636322597, + -69.78435636322598, + -69.78435636322598, + -69.78435636322598 ], "moduleForcesY": [ - 5.063460762339646, - 5.063460762339647, - 5.063460762339649, - 5.06346076233965 + 5.063460755805268, + 5.063460755805267, + 5.06346075580527, + 5.06346075580527 ], - "timestamp": 3.204592088409269 + "timestamp": 3.2045920884281465 }, { - "x": 8.880530486218973, - "y": 1.027348591072611, - "heading": -0.034530809902176336, - "angularVelocity": -0.054161349122254336, - "velocityX": -0.019564557922623505, - "velocityY": 0.13849232110933965, + "x": 8.880530486210139, + "y": 1.0273485910590279, + "heading": -0.03453080986256461, + "angularVelocity": -0.054161348577286314, + "velocityX": -0.01956455804488396, + "velocityY": 0.1384923209251697, "moduleForcesX": [ - -69.96707712828831, - -69.96918542605992, - -69.4046006596549, - -69.48963562370783 + -69.96707712791041, + -69.96918542564208, + -69.40460066617938, + -69.48963562834733 ], "moduleForcesY": [ - 0.7821062724142991, - 0.9492571184496107, - 8.889772333230777, - 8.233787400699313 + 0.7821063079634264, + 0.9492571479967624, + 8.889772282325291, + 8.233787361314777 ], - "timestamp": 3.277118057576761 + "timestamp": 3.277118057594007 }, { - "x": 8.859315047264229, - "y": 1.03873366112627, - "heading": -0.04230269871701783, - "angularVelocity": -0.10716008216259976, - "velocityX": -0.29252196417731824, - "velocityY": 0.15697921977944768, + "x": 8.859315047246675, + "y": 1.0387336610970788, + "heading": -0.04230269859815422, + "angularVelocity": -0.10716008107224526, + "velocityX": -0.292521964304125, + "velocityY": 0.1569792195677669, "moduleForcesX": [ - -69.96360527609723, - -69.96507878923691, - -69.41623345810719, - -69.49123274928873 + -69.96360527567995, + -69.96507878877915, + -69.41623346455654, + -69.49123275406077 ], "moduleForcesY": [ - 0.849054357453386, - 1.0605014457335615, - 8.776783714333206, - 8.198715576096298 + 0.8490543935707846, + 1.0605014747496089, + 8.776783663369878, + 8.198715535403977 ], - "timestamp": 3.3496440267442527 + "timestamp": 3.3496440267598677 }, { - "x": 8.81830267354523, - "y": 1.0514616761704456, - "heading": -0.053822469418188024, - "angularVelocity": -0.15883649447922438, - "velocityX": -0.5654853591032976, - "velocityY": 0.17549596634529624, + "x": 8.818302673519035, + "y": 1.0514616761236624, + "heading": -0.05382246917997862, + "angularVelocity": -0.15883649283719747, + "velocityX": -0.5654853592351373, + "velocityY": 0.17549596610668067, "moduleForcesX": [ - -69.95969350130241, - -69.9600439550638, - -69.43034005880952, - -69.49219040870635 + -69.95969350084123, + -69.96004395456353, + -69.43034006513656, + -69.49219041365508 ], "moduleForcesY": [ - 0.916057569851737, - 1.1945950847132014, - 8.639230967068034, - 8.165662210562154 + 0.9160576067978011, + 1.1945951128891326, + 8.639230916283404, + 8.165662168184191 ], - "timestamp": 3.4221699959117444 + "timestamp": 3.4221699959257283 }, { - "x": 8.75749289682575, - "y": 1.0655348258499984, - "heading": -0.06897986858552595, - "angularVelocity": -0.2089927144919403, - "velocityX": -0.838455210147377, - "velocityY": 0.1940429040948802, + "x": 8.757492896791033, + "y": 1.0655348257834631, + "heading": -0.06897986818711133, + "angularVelocity": -0.20899271228765146, + "velocityX": -0.8384552102837445, + "velocityY": 0.1940429038268972, "moduleForcesX": [ - -69.9551810729799, - -69.95383968280234, - -69.44688305083558, - -69.49295928173186 + -69.95518107246687, + -69.95383968225862, + -69.44688305699783, + -69.49295928690098 ], "moduleForcesY": [ - 0.9882804241447996, - 1.352618156589803, - 8.475469948022155, - 8.130018720639756 + 0.9882804621739782, + 1.3526181836391635, + 8.475469897619213, + 8.130018676173123 ], - "timestamp": 3.494695965079236 + "timestamp": 3.494695965091589 }, { - "x": 8.67688521696236, - "y": 1.080955321413936, - "heading": -0.08764661363140669, - "angularVelocity": -0.2573801530723142, - "velocityX": -1.1114319572514513, - "velocityY": 0.2126203309097312, + "x": 8.676885216919311, + "y": 1.0809553213253251, + "heading": -0.08764661303153386, + "angularVelocity": -0.2573801503002974, + "velocityX": -1.1114319573913516, + "velocityY": 0.212620330610125, "moduleForcesX": [ - -69.94983324196707, - -69.94615057660823, - -69.46581320027535, - -69.49411065050936 + -69.94983324139118, + -69.94615057602354, + -69.46581320622545, + -69.49411065593223 ], "moduleForcesY": [ - 1.0723396108261634, - 1.5360717401888286, - 8.283336243627176, - 8.085785369496797 + 1.072339650088805, + 1.5360717657814353, + 8.283336193853913, + 8.085785322585087 ], - "timestamp": 3.567221934246728 + "timestamp": 3.5672219342574496 }, { - "x": 8.576479109537585, - "y": 1.0977253883199334, - "heading": -0.1096713977003478, - "angularVelocity": -0.3036813478255371, - "velocityX": -1.3844159351099252, - "velocityY": 0.23122844270129664, + "x": 8.57647910948646, + "y": 1.0977253882067866, + "heading": -0.10967139685743804, + "angularVelocity": -0.30368134448125444, + "velocityX": -1.3844159352524572, + "velocityY": 0.2312284423681809, "moduleForcesX": [ - -69.94330252963142, - -69.9365583049015, - -69.4870568794036, - -69.49637643933679 + -69.94330252897542, + -69.93655830427998, + -69.48705688510655, + -69.49637644504644 ], "moduleForcesY": [ - 1.176823061341541, - 1.7470252667944042, - 8.060012105446882, - 8.025018376878974 + 1.1768231019990443, + 1.7470252906666641, + 8.060012056454902, + 8.025018327105174 ], - "timestamp": 3.6397479034142197 + "timestamp": 3.6397479034233102 }, { - "x": 8.456274044587177, - "y": 1.1158472524014584, - "heading": -0.13487291738284943, - "angularVelocity": -0.347482701327652, - "velocityX": -1.6574072202027315, - "velocityY": 0.24986724465107038, + "x": 8.456274044528195, + "y": 1.1158472522610625, + "heading": -0.13487291625482856, + "angularVelocity": -0.347482697404215, + "velocityX": -1.6574072203483379, + "velocityY": 0.2498672442809662, "moduleForcesX": [ - -69.93505805724766, - -69.92449408332656, - -69.51050132115871, - -69.50070530398553 + -69.93505805649114, + -69.92449408268004, + -69.510501326562, + -69.50070530998221 ], "moduleForcesY": [ - 1.3131272162432313, - 1.98838442321693, - 7.801780109725148, - 7.936938199533164 + 1.3131272581552487, + 1.9883844449565293, + 7.801780061823989, + 7.936938146668492 ], - "timestamp": 3.7122738725817115 + "timestamp": 3.712273872589171 }, { - "x": 8.316269527386241, - "y": 1.1353231154725127, - "heading": -0.16302980404777723, - "angularVelocity": -0.38823178770838385, - "velocityX": -1.930405326643809, - "velocityY": 0.26853640557469155, + "x": 8.316269527319614, + "y": 1.1353231153018994, + "heading": -0.16302980259206615, + "angularVelocity": -0.3882317831987629, + "velocityX": -1.9304053267926182, + "velocityY": 0.26853640516407645, "moduleForcesX": [ - -69.92425253763372, - -69.90915580434691, - -69.53597671993317, - -69.50834196529713 + -69.92425253674794, + -69.90915580369078, + -69.53597672500005, + -69.50834197156578 ], "moduleForcesY": [ - 1.4968639656801608, - 2.264388307053663, - 7.503564176470051, - 7.80642632120717 + 1.4968640086183072, + 2.2643883263150886, + 7.50356412984306, + 7.806426265009984 ], - "timestamp": 3.7847998417492033 + "timestamp": 3.7847998417550315 }, { - "x": 8.156465184614108, - "y": 1.1561551126608645, - "heading": -0.1938654641731384, - "angularVelocity": -0.42516715708305197, - "velocityX": -2.2034085804916796, - "velocityY": 0.28723500599140817, + "x": 8.156465184540169, + "y": 1.1561551124568015, + "heading": -0.19386546234654622, + "angularVelocity": -0.42516715197872557, + "velocityX": -2.203408580642062, + "velocityY": 0.28723500553664255, "moduleForcesX": [ - -69.9094604865732, - -69.88935388472542, - -69.5632321514312, - -69.52093877070061 + -69.90946048551345, + -69.88935388407138, + -69.56323215616419, + -69.52093877722275 ], "moduleForcesY": [ - 1.7503444073984757, - 2.581564547481693, - 7.158055486956702, - 7.611351867199961 + 1.7503444512317394, + 2.5815645641729006, + 7.1580554414148985, + 7.611351807216961 ], - "timestamp": 3.857325810916695 + "timestamp": 3.857325810920892 }, { - "x": 7.976860949117206, - "y": 1.1783452360639273, - "heading": -0.2270240484884978, - "angularVelocity": -0.45719601814734345, - "velocityX": -2.4764127602640795, - "velocityY": 0.3059610737751436, + "x": 7.976860949036181, + "y": 1.1783452358227742, + "heading": -0.22702404624732822, + "angularVelocity": -0.45719601244123276, + "velocityX": -2.4764127604175035, + "velocityY": 0.30596107327060007, "moduleForcesX": [ - -69.88812974316788, - -69.86320182607882, - -69.59189618603347, - -69.5407034064261 + -69.8881297418902, + -69.86320182545145, + -69.59189619041469, + -69.54070341308756 ], "moduleForcesY": [ - 2.107233171390243, - 2.950662034921671, - 6.753983199950573, - 7.317496840416227 + 2.1072332152187867, + 2.95066204873304, + 6.753983155440134, + 7.31749677667182 ], - "timestamp": 3.929851780084187 + "timestamp": 3.9298517800867527 }, { - "x": 7.777457476125625, - "y": 1.201895191338467, - "heading": -0.26202980984331203, - "angularVelocity": -0.4826651991962036, - "velocityX": -2.74940790561615, - "velocityY": 0.3247106594351006, + "x": 7.7774574760376565, + "y": 1.2018951910560491, + "heading": -0.26202980714373064, + "angularVelocity": -0.4826651928862579, + "velocityX": -2.7494079057737237, + "velocityY": 0.32471065887340905, "moduleForcesX": [ - -69.85533117072832, - -69.82743416827404, - -69.62139360036666, - -69.57054322743649 + -69.8553311691883, + -69.82743416769303, + -69.62139360441714, + -69.57054323404049 ], "moduleForcesY": [ - 2.6219385333270773, - 3.3908613060937713, - 6.272491751359185, - 6.868108041742767 + 2.621938575781362, + 3.3908613169810025, + 6.272491707304398, + 6.86810797438961 ], - "timestamp": 4.0023777492516786 + "timestamp": 4.002377749252613 }, { - "x": 7.558257164116274, - "y": 1.2268061069502, - "heading": -0.2982123205048736, - "angularVelocity": -0.49889041232006653, - "velocityX": -3.022369980373853, - "velocityY": 0.3434758045660868, + "x": 7.55825716402174, + "y": 1.226806106621595, + "heading": -0.29821231730362563, + "angularVelocity": -0.4988904054140694, + "velocityX": -3.0223699805323463, + "velocityY": 0.34347580393694493, "moduleForcesX": [ - -69.80056727463466, - -69.77570886950674, - -69.6507199544232, - -69.61392322278483 + -69.80056727283858, + -69.77570886897958, + -69.65071995819655, + -69.61392322895297 ], "moduleForcesY": [ - 3.39043477997722, - 3.9399463888437256, - 5.678903958822243, - 6.160009083534089 + 3.3904348183931043, + 3.9399463970685407, + 5.678903913889345, + 6.160009013377018 ], - "timestamp": 4.07490371841917 + "timestamp": 4.074903718418473 }, { - "x": 7.319267013948266, - "y": 1.2530778743434587, - "heading": -0.3345519884568234, - "angularVelocity": -0.5010573229066131, - "velocityX": -3.2952355261337996, - "velocityY": 0.3622394529136627, + "x": 7.319267013847545, + "y": 1.2530778739626474, + "heading": -0.33455198471356234, + "angularVelocity": -0.501057315444351, + "velocityX": -3.2952355262932205, + "velocityY": 0.3622394522019477, "moduleForcesX": [ - -69.69826365815047, - -69.69360285262502, - -69.67768220142874, - -69.67276264637083 + -69.69826365639196, + -69.69360285213232, + -69.677682204989, + -69.67276265133394 ], "moduleForcesY": [ - 4.602649202702627, - 4.682797548651256, - 4.9016741603909, - 4.9806442793840855 + 4.602649230854164, + 4.682797554881524, + 4.901674111951051, + 4.980644209624238 ], - "timestamp": 4.147429687586662 + "timestamp": 4.147429687584333 }, { - "x": 7.060508520408783, - "y": 1.2807073809011449, - "heading": -0.3693010360245331, - "angularVelocity": -0.479125587246758, - "velocityX": -3.5678046982302343, - "velocityY": 0.38096018398438164, + "x": 7.060508520302209, + "y": 1.2807073804603784, + "heading": -0.36930103170788803, + "angularVelocity": -0.47912557935144634, + "velocityX": -3.5678046983912037, + "velocityY": 0.38096018316622676, "moduleForcesX": [ - -69.47236753682394, - -69.53951749408205, - -69.69564436263259, - -69.73202523829484 + -69.47236753665878, + -69.53951749340459, + -69.69564436604739, + -69.73202524072254 ], "moduleForcesY": [ - 6.700012628800592, - 5.851281299757406, - 3.768542257976925, - 2.8040874089418844 + 6.7000126322527995, + 5.851281306796728, + 3.7685421990988033, + 2.8040873490370677 ], - "timestamp": 4.219955656754154 + "timestamp": 4.2199556567501935 }, { - "x": 6.7820675423003, - "y": 1.3096827333084073, - "heading": -0.398737971474847, - "angularVelocity": -0.40588131104575736, - "velocityX": -3.839190035026568, - "velocityY": 0.39951692807238764, + "x": 6.782067542187506, + "y": 1.3096827327970242, + "heading": -0.3987379665925305, + "angularVelocity": -0.405881303255151, + "velocityX": -3.8391900351986807, + "velocityY": 0.3995169271076376, "moduleForcesX": [ - -68.80858735271256, - -69.12938539141281, - -69.67366590353845, - -69.61858416268272 + -68.80858736495749, + -69.12938538991284, + -69.67366590614556, + -69.6185841628369 ], "moduleForcesY": [ - 10.998366071640108, - 8.339611523596144, - 1.7520720276263266, - -2.1336448340952816 + 10.99836599716967, + 8.33961153542166, + 1.752071940836427, + -2.13364483389887 ], - "timestamp": 4.292481625921646 + "timestamp": 4.292481625916054 }, { - "x": 6.484732150451617, - "y": 1.3400281940076402, - "heading": -0.4090628910860552, - "angularVelocity": -0.142361690988409, - "velocityX": -4.099709321525832, - "velocityY": 0.4184082067095552, + "x": 6.484732150295716, + "y": 1.3400281933976181, + "heading": -0.40906288611345654, + "angularVelocity": -0.14236168974674643, + "velocityX": -4.099709322212432, + "velocityY": 0.41840820535881235, "moduleForcesX": [ - -65.20316881632876, - -65.7489206217446, - -69.31239778456624, - -65.86566076035609 + -65.20316900335862, + -65.74892075037127, + -69.31239777710773, + -65.86566098161668 ], "moduleForcesY": [ - 23.606521909357, - 20.444429351523326, - -3.7507078277888724, - -21.00209915007702 + 23.606521394196665, + 20.444428940438716, + -3.750707987782145, + -21.00209845812331 ], - "timestamp": 4.365007595089137 + "timestamp": 4.365007595081914 }, { - "x": 6.1842404767603805, - "y": 1.3742213267083638, - "heading": -0.40911116386317103, - "angularVelocity": -0.0006655929961390624, - "velocityX": -4.1432286550667365, - "velocityY": 0.4714605415577361, + "x": 6.184240476626105, + "y": 1.3742213262250316, + "heading": -0.40911115889080796, + "angularVelocity": -0.0006655929994023552, + "velocityX": -4.143228654861732, + "velocityY": 0.4714605433153216, "moduleForcesX": [ - -13.325449562031311, - -4.164768682569861, - -18.043544953356022, - -8.922855853125498 + -13.325455797596309, + -4.164751278280683, + -18.043558746247182, + -8.922852319147749 ], "moduleForcesY": [ - 19.90592667522208, - 16.82639877386449, - 10.611237303210721, - 6.85137196260812 + 19.90590611882484, + 16.82641222119337, + 10.611221364447623, + 6.851398187145905 ], - "timestamp": 4.437533564256629 + "timestamp": 4.437533564247774 }, { - "x": 5.886052139153465, - "y": 1.4246963553310026, - "heading": -0.4091589633873657, - "angularVelocity": -0.0006590677069691712, - "velocityX": -4.111469878027867, - "velocityY": 0.6959580023818284, + "x": 5.886052139053873, + "y": 1.424696355011759, + "heading": -0.40915895841469946, + "angularVelocity": -0.0006590677028043638, + "velocityX": -4.111469877642081, + "velocityY": 0.6959580046600466, "moduleForcesX": [ - 8.110433382725144, - 8.111187384355791, - 8.110199033033277, - 8.110953042580169 + 8.110433400741913, + 8.11118742383485, + 8.110199088142462, + 8.110953115410458 ], "moduleForcesY": [ - 57.33325913716358, - 57.33311245310866, - 57.33314368701091, - 57.33299700322065 + 57.33325927042727, + 57.33311258568195, + 57.333143822794604, + 57.33299713881585 ], - "timestamp": 4.510059533424121 + "timestamp": 4.510059533413634 }, { - "x": 5.591555275132608, - "y": 1.4934998764417577, - "heading": -0.4092089626864391, - "angularVelocity": -0.0006893985650598928, - "velocityX": -4.060571232640143, - "velocityY": 0.94867427351207, + "x": 5.5915552750776545, + "y": 1.4934998762835463, + "heading": -0.4092089577134744, + "angularVelocity": -0.0006893985609604924, + "velocityX": -4.060571232115967, + "velocityY": 0.9486742757538188, "moduleForcesX": [ - 13.000242789451551, - 12.99626661282106, - 13.001163654267753, - 12.99718771199267 + 13.000242825406785, + 12.996266648691245, + 13.001163689713444, + 12.997187747343835 ], "moduleForcesY": [ - 64.53930421343945, - 64.54015945588058, - 64.53937921708196, - 64.54023445819007 + 64.53930420549774, + 64.54015944796213, + 64.53937920928017, + 64.5402344504135 ], - "timestamp": 4.582585502591613 + "timestamp": 4.582585502579494 }, { "x": 5.302053451538088, "y": 1.5809556245803824, - "heading": -0.4092627987960467, - "angularVelocity": -0.000742301140211202, - "velocityX": -3.991698793103366, - "velocityY": 1.2058542497602232, + "heading": -0.40926279382276776, + "angularVelocity": -0.0007423011358945286, + "velocityX": -3.9916987924354075, + "velocityY": 1.205854251968866, "moduleForcesX": [ - 17.591944196413664, - 17.584790178826015, - 17.593089676218266, - 17.5859364455025 + 17.591944233654857, + 17.584790216066136, + 17.59308971312774, + 17.585936482408147 ], "moduleForcesY": [ - 65.6787937062123, - 65.68074285605304, - 65.67871632504414, - 65.68066538032537 + 65.67879369894756, + 65.68074284879431, + 65.67871631788772, + 65.68066537317567 ], - "timestamp": 4.6551114717591044 + "timestamp": 4.655111471745355 }, { - "x": 5.060781264234282, - "y": 1.668503203961895, - "heading": -0.4093136867686553, - "angularVelocity": -0.0008266775577253413, - "velocityX": -3.9194782641972647, - "velocityY": 1.4222146294751892, + "x": 5.060781264318389, + "y": 1.6685032040805434, + "heading": -0.40931368179496846, + "angularVelocity": -0.0008266775512222434, + "velocityX": -3.9194782634197978, + "velocityY": 1.4222146316163005, "moduleForcesX": [ - 21.736557880357715, - 21.723118914680356, - 21.73784859212575, - 21.72441246515569 + 21.736557916865454, + 21.723118951579323, + 21.73784862823763, + 21.724412501654832 ], "moduleForcesY": [ - 65.09863070377499, - 65.10314246149298, - 65.09850389768496, - 65.10301496712766 + 65.09863069400774, + 65.10314245160757, + 65.09850388812552, + 65.10301495745126 ], - "timestamp": 4.716668692144205 + "timestamp": 4.716668692121208 }, { - "x": 4.824591541574993, - "y": 1.768967581170856, - "heading": -0.4093625786347868, - "angularVelocity": -0.0007942507121868569, - "velocityX": -3.836913382080745, - "velocityY": 1.632048630208832, + "x": 4.824591541749403, + "y": 1.7689675814031505, + "heading": -0.4093625736606763, + "angularVelocity": -0.0007942507054235842, + "velocityX": -3.8369133811902225, + "velocityY": 1.6320486323002081, "moduleForcesX": [ - 24.840611640815233, - 24.84563714723139, - 24.840370639682614, - 24.84539654710804 + 24.840611678634826, + 24.845637185091626, + 24.840370677624488, + 24.845396585090324 ], "moduleForcesY": [ - 63.138133963812095, - 63.136150247952436, - 63.13804254630454, - 63.13605868744737 + 63.13813395857038, + 63.136150242692494, + 63.13804254100993, + 63.13605868213461 ], - "timestamp": 4.778225912529306 + "timestamp": 4.778225912497061 }, { - "x": 4.5940300417851905, - "y": 1.8817568503359232, - "heading": -0.40940943255755435, - "angularVelocity": -0.00076114422441599, - "velocityX": -3.7454826314023215, - "velocityY": 1.8322670916501005, + "x": 4.5940300420581, + "y": 1.8817568506816633, + "heading": -0.4094094275830447, + "angularVelocity": -0.0007611442180464456, + "velocityX": -3.745482630364935, + "velocityY": 1.8322670937682957, "moduleForcesX": [ - 27.508225922613867, - 27.513128573012004, - 27.508193765462636, - 27.51309679763414 + 27.50822597067515, + 27.513128620991576, + 27.508193814141755, + 27.513096846198717 ], "moduleForcesY": [ - 60.24514716829222, - 60.242914588066654, - 60.24482580432994, - 60.242593041950194 + 60.245147185521205, + 60.24291460533314, + 60.24482582123695, + 60.24259305890976 ], - "timestamp": 4.839783132914407 + "timestamp": 4.839783132872914 }, { - "x": 4.369349606253567, - "y": 2.0058517685581454, - "heading": -0.4094544926877705, - "angularVelocity": -0.0007320039783130824, - "velocityX": -3.6499444602278026, - "velocityY": 2.015927903272623, + "x": 4.36934960664493, + "y": 2.0058517690384203, + "heading": -0.4094544877128489, + "angularVelocity": -0.0007320039717315139, + "velocityX": -3.6499444588518815, + "velocityY": 2.0159279057610187, "moduleForcesX": [ - 28.744538248232754, - 28.74851367433131, - 28.744577870233446, - 28.74855354416703 + 28.744538353811407, + 28.74851378007089, + 28.744577977045132, + 28.74855365043348 ], "moduleForcesY": [ - 55.26315032046569, - 55.261123506293146, - 55.262531653571756, - 55.260504693222536 + 55.2631504404134, + 55.261123626177344, + 55.2625317726504, + 55.26050481260123 ], - "timestamp": 4.901340353299508 + "timestamp": 4.901340353248767 }, { - "x": 4.149973859162333, - "y": 2.13910073076137, - "heading": -0.4094996155694483, - "angularVelocity": -0.0007330233788204265, - "velocityX": -3.563769541880253, - "velocityY": 2.1646357871525597, + "x": 4.149973859634745, + "y": 2.1391007313038166, + "heading": -0.40949961058910517, + "angularVelocity": -0.0007330232908571056, + "velocityX": -3.5637695410990893, + "velocityY": 2.1646357884876952, "moduleForcesX": [ - 25.92928590684574, - 25.929168249602895, - 25.929292292013468, - 25.929174634983514 + 25.92928571690531, + 25.929168021968326, + 25.929292070124113, + 25.92917457379335 ], "moduleForcesY": [ - 44.74476438288918, - 44.74482729836071, - 44.74480699802511, - 44.744869913360205 + 44.74476401967033, + 44.744826943195974, + 44.74480673058518, + 44.74486953719506 ], - "timestamp": 4.962897573684609 + "timestamp": 4.962897573624621 }, { - "x": 3.9353204017277856, - "y": 2.278088242273816, - "heading": -0.41252140844778834, - "angularVelocity": -0.04908917036026353, - "velocityX": -3.4870557197299563, - "velocityY": 2.2578587961400522, + "x": 3.9353204020821484, + "y": 2.2780882424747335, + "heading": -0.4125214036176848, + "angularVelocity": -0.04908917280838613, + "velocityX": -3.48705572217154, + "velocityY": 2.257858790931102, "moduleForcesX": [ - 24.953488052198967, - 20.4567569278476, - 25.589392523319265, - 21.330260691348453 + 24.953505556486252, + 20.45677256337718, + 25.589356554115078, + 21.330259655702005 ], "moduleForcesY": [ - 25.14686564166064, - 27.7645218324562, - 28.406596524112476, - 30.881756387142097 + 25.146827347937002, + 27.764534745643168, + 28.406606309810808, + 30.88176412187559 ], - "timestamp": 5.02445479406971 + "timestamp": 5.024454794000474 }, { - "x": 3.7318642433739195, - "y": 2.4104934513603675, - "heading": -0.43615839467618756, - "angularVelocity": -0.38398397589611083, - "velocityX": -3.305155058676315, - "velocityY": 2.150928977270674, + "x": 3.7318642436374203, + "y": 2.4104934515239713, + "heading": -0.4361583914201127, + "angularVelocity": -0.3839840015242407, + "velocityX": -3.3051550606489273, + "velocityY": 2.150928976987681, "moduleForcesX": [ - 49.89592032127635, - 34.025418039882794, - 66.93891398556973, - 68.06860648029641 + 49.89592057158015, + 34.025417576241814, + 66.93891416475311, + 68.0686068113683 ], "moduleForcesY": [ - -47.83606477558671, - -59.36994474295037, - -17.100092585775965, - -4.390644167479697 + -47.836064483916026, + -59.369944976556134, + -17.100091773085392, + -4.390638937539682 ], - "timestamp": 5.086012014454811 + "timestamp": 5.086012014376327 }, { - "x": 3.540415993553555, - "y": 2.5354156131468453, - "heading": -0.46498951807167194, - "angularVelocity": -0.4683629835017646, - "velocityX": -3.110086008150945, - "velocityY": 2.0293665146828603, + "x": 3.5404159937543107, + "y": 2.5354156132801298, + "heading": -0.4649895158550848, + "angularVelocity": -0.46836300045892293, + "velocityX": -3.1100860096374956, + "velocityY": 2.029366514495222, "moduleForcesX": [ - 55.83155696216655, - 54.18327481859195, - 62.40116195557468, - 62.36184788712686 + 55.83155730522792, + 54.18327547289772, + 62.40116176551297, + 62.3618475536798 ], "moduleForcesY": [ - -41.50061361131458, - -43.48191734914588, - -30.734501757153353, - -30.59101829583353 + -41.50061314279684, + -43.48191653513505, + -30.73450213526439, + -30.591018982325306 ], - "timestamp": 5.147569234839912 + "timestamp": 5.14756923475218 }, { - "x": 3.361022505593955, - "y": 2.652736487945131, - "heading": -0.4956638148847116, - "angularVelocity": -0.49830542414467005, - "velocityX": -2.9142558230751625, - "velocityY": 1.9058832426858676, + "x": 3.361022505744376, + "y": 2.652736488054298, + "heading": -0.4956638133831861, + "angularVelocity": -0.4983054358359662, + "velocityX": -2.9142558243306804, + "velocityY": 1.9058832425804164, "moduleForcesX": [ - 57.73901804404707, - 57.438460533401326, - 60.30587505828676, - 60.21056114711604 + 57.7390182838169, + 57.43846086192714, + 60.30587488496412, + 60.21056094689106 ], "moduleForcesY": [ - -39.07130416952509, - -39.474125567373356, - -34.97681089661358, - -35.097625013264334 + -39.071303811919336, + -39.47412508944996, + -34.976811192657685, + -35.09762535851428 ], - "timestamp": 5.209126455225013 + "timestamp": 5.209126455128033 }, { - "x": 3.1936838062667205, - "y": 2.7624068712077934, - "heading": -0.5266594310996341, - "angularVelocity": -0.5035252732516518, - "velocityX": -2.7184252030934304, - "velocityY": 1.7816006404539642, + "x": 3.1936838063800033, + "y": 2.762406871298198, + "heading": -0.5266594301029957, + "angularVelocity": -0.5035252815293935, + "velocityX": -2.7184252041051566, + "velocityY": 1.7816006404168396, "moduleForcesX": [ - 58.700566811459886, - 58.67734780171254, - 59.16682840463337, - 59.14969530624501 + 58.70056697929468, + 58.67734799260074, + 59.16682827716417, + 59.14969518046466 ], "moduleForcesY": [ - -37.749793809130026, - -37.78083859729802, - -37.014524694721786, - -37.03675261323169 + -37.749793546335056, + -37.78083830094146, + -37.01452489709346, + -37.03675281488077 ], - "timestamp": 5.270683675610114 + "timestamp": 5.270683675503887 }, { - "x": 3.038390509303798, - "y": 2.8643948429617936, - "heading": -0.5570773161987834, - "angularVelocity": -0.4941400035435474, - "velocityX": -2.5227470634868117, - "velocityY": 1.6567994967278303, + "x": 3.038390509385735, + "y": 2.864394843036019, + "heading": -0.5570773155618112, + "angularVelocity": -0.49414000946072106, + "velocityX": -2.5227470643750314, + "velocityY": 1.6567994967139286, "moduleForcesX": [ - 59.30031933124251, - 59.30307854027269, - 58.44288594465124, - 58.46463470158717 + 59.30031945388296, + 59.30307866344703, + 58.44288585276952, + 58.4646346161408 ], "moduleForcesY": [ - -36.885288676049825, - -36.88814111006628, - -38.229406656940725, - -38.203170518961 + -36.885288477772534, + -36.888140912141004, + -38.22940679661343, + -38.20317065012603 ], - "timestamp": 5.332240895995215 + "timestamp": 5.33224089587974 }, { - "x": 2.895129799571264, - "y": 2.9586740032068897, - "heading": -0.5863104114525834, - "angularVelocity": -0.4748930356337662, - "velocityX": -2.3272771063459046, - "velocityY": 1.5315694837305227, + "x": 2.8951297996314556, + "y": 2.958674003265299, + "heading": -0.5863104110705085, + "angularVelocity": -0.474893039846042, + "velocityX": -2.3272771070488116, + "velocityY": 1.5315694837037004, "moduleForcesX": [ - 59.717187192796516, - 59.657546410038414, - 57.934272823511364, - 57.95135152475748 + 59.71718728545176, + 59.65754649481836, + 57.93427275714158, + 57.95135146376398 ], "moduleForcesY": [ - -36.2638573648305, - -36.37433120370207, - -39.04885966973737, - -39.03512982178747 + -36.26385721152638, + -36.374331064721694, + -39.04885976771552, + -39.03512991256516 ], - "timestamp": 5.3937981163803155 + "timestamp": 5.393798116255593 }, { - "x": 2.763886371067874, - "y": 3.045218511804947, - "heading": -0.6139222009668763, - "angularVelocity": -0.4485548460699908, - "velocityX": -2.132055795929944, - "velocityY": 1.4059196964488687, + "x": 2.7638863711095008, + "y": 3.0452185118515818, + "heading": -0.6139222007620838, + "angularVelocity": -0.4485548490174231, + "velocityX": -2.1320557965518465, + "velocityY": 1.4059196964688137, "moduleForcesX": [ - 60.015428697215164, - 59.85831037769707, - 57.55335306922513, - 57.534003758004665 + 60.01542876878009, + 59.8583104385353, + 57.5533530210112, + 57.53400371300751 ], "moduleForcesY": [ - -35.80957668058033, - -36.086111428235434, - -39.64516852205637, - -39.68654602916551 + -35.80957656014014, + -36.08611132735859, + -39.645168591728385, + -39.68654609453473 ], - "timestamp": 5.455355336765416 + "timestamp": 5.455355336631446 }, { - "x": 2.644643305476117, - "y": 3.1240017980714323, - "heading": -0.6395960472514493, - "angularVelocity": -0.41707286527865206, - "velocityX": -1.937109324394044, - "velocityY": 1.2798382671215223, + "x": 2.6446433055054834, + "y": 3.1240017981097368, + "heading": -0.6395960471657398, + "angularVelocity": -0.417072867275884, + "velocityX": -1.9371093248842417, + "velocityY": 1.2798382671784732, "moduleForcesX": [ - 60.22326407527744, - 59.95837931731987, - 57.26239607352692, - 57.186270668215975 + 60.22326413098716, + 59.958379361963274, + 57.26239603856264, + 57.186270634486874 ], "moduleForcesY": [ - -35.49033319006886, - -35.95108026507241, - -40.09166456022141, - -40.21383292620102 + -35.49033309517584, + -35.95108019063276, + -40.0916646099415, + -40.21383297424875 ], - "timestamp": 5.516912557150517 + "timestamp": 5.516912557007299 }, { - "x": 2.537383808831169, - "y": 3.1949980499443154, - "heading": -0.6631036099321576, - "angularVelocity": -0.38188148414867673, - "velocityX": -1.7424356716228648, - "velocityY": 1.1533375196074727, + "x": 2.537383808849767, + "y": 3.194998049974314, + "heading": -0.6631036099218587, + "angularVelocity": -0.38188148543114175, + "velocityX": -1.7424356720595673, + "velocityY": 1.153337519645833, "moduleForcesX": [ - 60.36537448302678, - 59.99512146624315, - 57.04267536961116, - 56.89878422586106 + 60.36537452641838, + 59.995121499410935, + 57.04267534461395, + 56.89878420063114 ], "moduleForcesY": [ - -35.27267783600763, - -35.913635660706774, - -40.42480723386195, - -40.64046601274349 + -35.27267776148261, + -35.91363560529686, + -40.42480726897906, + -40.64046604811504 ], - "timestamp": 5.578469777535618 + "timestamp": 5.578469777383153 }, { - "x": 2.4420919536077847, - "y": 3.2581828688425367, - "heading": -0.6842805929390149, - "angularVelocity": -0.344021105475332, - "velocityX": -1.5480207622638111, - "velocityY": 1.0264404159729446, + "x": 2.4420919536194616, + "y": 3.258182868864464, + "heading": -0.6842805929718256, + "angularVelocity": -0.3440211062273634, + "velocityX": -1.5480207626088205, + "velocityY": 1.0264404159960439, "moduleForcesX": [ - 60.4598584765929, - 59.99281023891339, - 56.876528556020105, - 56.661344392247265 + 60.45985851021655, + 59.992810263691005, + 56.876528538625024, + 56.66134437370607 ], "moduleForcesY": [ - -35.130249715774205, - -35.93621486669289, - -40.675063944105, - -40.98709766240873 + -35.13024965770887, + -35.936214825314366, + -40.6750639683131, + -40.98709768806473 ], - "timestamp": 5.640026997920719 + "timestamp": 5.640026997759006 }, { - "x": 2.3587528835015927, - "y": 3.3135334333216804, - "heading": -0.7030081263323279, - "angularVelocity": -0.3042296789257516, - "velocityX": -1.3538471942824237, - "velocityY": 0.8991725768784151, + "x": 2.35875288350831, + "y": 3.313533433337192, + "heading": -0.7030081263847169, + "angularVelocity": -0.30422967928952566, + "velocityX": -1.353847194566388, + "velocityY": 0.8991725769092883, "moduleForcesX": [ - 60.5201219775476, - 59.967443909712394, - 56.748515348358374, - 56.463990903711384 + 60.52012200326446, + 59.96744392817741, + 56.74851533683041, + 56.463990890605494 ], "moduleForcesY": [ - -35.04253435660417, - -35.99355809852384, - -40.867195346689, - -41.27154173850131 + -35.042534312039635, + -35.99355806773702, + -40.86719536260796, + -41.2715417564386 ], - "timestamp": 5.70158421830582 + "timestamp": 5.701584218134859 }, { - "x": 2.2873528689676483, - "y": 3.3610285743692394, - "heading": -0.7191989284072005, - "angularVelocity": -0.26302035689049935, - "velocityX": -1.1598966634176864, - "velocityY": 0.7715608461595652, + "x": 2.287352868970426, + "y": 3.361028574379745, + "heading": -0.7191989284627818, + "angularVelocity": -0.26302035698188275, + "velocityX": -1.1598966636559505, + "velocityY": 0.7715608461941635, "moduleForcesX": [ - 60.55636984330847, - 59.92996721273085, - 56.64674423048133, - 56.29855167271921 + 60.556369862505825, + 59.92996722632186, + 56.646744223537, + 56.29855166413728 ], "moduleForcesY": [ - -34.99335531824572, - -36.06824538837232, - -41.01955138323022, - -41.50757259528159 + -34.99335528490936, + -36.068245365760546, + -41.019551392749705, + -41.507572606915744 ], - "timestamp": 5.763141438690921 + "timestamp": 5.763141438510712 }, { - "x": 2.2278792676988664, - "y": 3.4006487477183196, - "heading": -0.7327874449132579, - "angularVelocity": -0.2207461029112197, - "velocityX": -0.966151507438403, - "velocityY": 0.6436316178868581, + "x": 2.2278792676993477, + "y": 3.4006487477251475, + "heading": -0.7327874449614561, + "angularVelocity": -0.22074610282445042, + "velocityX": -0.9661515076208613, + "velocityY": 0.6436316179238191, "moduleForcesX": [ - 60.57586928152923, - 59.88760410989669, - 56.562363081650766, - 56.15861493971373 + 60.575869295269314, + 59.88760411963649, + 56.56236307833243, + 56.15861493495852 ], "moduleForcesY": [ - -34.97098554580845, - -36.148822416267066, - -41.14545732873801, - -41.70558761712785 + -34.970985521919275, + -36.1488224000973, + -41.14545733324237, + -41.70558762351516 ], - "timestamp": 5.824698659076022 + "timestamp": 5.8246986588865655 }, { - "x": 2.180320435943854, - "y": 3.432375929705667, - "heading": -0.7437231910234625, - "angularVelocity": -0.17765172049338343, - "velocityX": -0.7725955047594213, - "velocityY": 0.5154095943394352, + "x": 2.1803204359440898, + "y": 3.4323759297090946, + "heading": -0.7437231910585703, + "angularVelocity": -0.17765172030741577, + "velocityX": -0.772595504879486, + "velocityY": 0.5154095943616319, "moduleForcesX": [ - 60.58340632519516, - 59.84490600733586, - 56.48902920756478, - 56.03945201723916 + 60.583406334310894, + 59.84490601396566, + 56.489029207157884, + 56.03945201575598 ], "moduleForcesY": [ - -34.96766125833015, - -36.22817172523544, - -41.25431832732434, - -41.87309909683168 + -34.96766124246743, + -36.22817171424666, + -41.25431832783358, + -41.87309909879299 ], - "timestamp": 5.886255879461123 + "timestamp": 5.886255879262419 }, { - "x": 2.1446656199937997, - "y": 3.4561934671002783, - "heading": -0.7519665199163653, - "angularVelocity": -0.13391327355800717, - "velocityX": -0.5792141965962733, - "velocityY": 0.3869170382549771, + "x": 2.1446656199930714, + "y": 3.4561934671017123, + "heading": -0.7519665199367948, + "angularVelocity": -0.1339132733396743, + "velocityX": -0.5792141966989566, + "velocityY": 0.38691703828071905, "moduleForcesX": [ - 60.58183660174803, - 59.80457338750152, - 56.42234143923046, - 55.9377862653548 + 60.581836606907586, + 59.80457339156535, + 56.422341441204175, + 55.937786266687105 ], "moduleForcesY": [ - -34.97878115536029, - -36.30217808780585, - -41.352605880404866, - -42.01528814031888 + -34.97878114637003, + -36.302178081071816, + -41.352605877670875, + -42.01528813851514 ], - "timestamp": 5.947813099846224 + "timestamp": 5.947813099638272 }, { - "x": 2.120904845915518, - "y": 3.472085915141707, - "heading": -0.7574858469261834, - "angularVelocity": -0.08966173221089468, - "velocityX": -0.3859949154564824, - "velocityY": 0.258173581295686, + "x": 2.1209048459157667, + "y": 3.4720859151422694, + "heading": -0.7574858469338888, + "angularVelocity": -0.08966173201765831, + "velocityX": -0.3859949154986063, + "velocityY": 0.2581735813203214, "moduleForcesX": [ - 60.57287518432625, - 59.76823373322807, - 56.35914684811399, - 55.851272292620784 + 60.57287518607043, + 59.768233735123275, + 56.35914685208343, + 55.851272296387975 ], "moduleForcesY": [ - -35.001611618471976, - -36.36844720693236, - -41.44491977809569, - -42.13584960752861 + -35.00161161541115, + -36.368447203777166, + -41.44491977266188, + -42.13584960250002 ], - "timestamp": 6.009370320231325 + "timestamp": 6.009370320014125 }, { "x": 2.109028816223142, "y": 3.4800388813018817, "heading": -0.7602557704725089, - "angularVelocity": -0.044997540970748186, - "velocityX": -0.19292667242090425, - "velocityY": 0.12919631702701284, + "angularVelocity": -0.04499754085233033, + "velocityX": -0.19292667245392872, + "velocityY": 0.12919631703729303, "moduleForcesX": [ - 60.5575482023407, - 59.73687419838248, - 56.29713329942968, - 55.778188462220434 + 60.55754820112047, + 59.736874198400685, + 56.297133305118244, + 55.77818846809218 ], "moduleForcesY": [ - -35.034541448499795, - -36.42559455322768, - -41.5346063390199, - -42.237487649673916 + -35.03454145057582, + -36.425594553156486, + -41.53460633127721, + -42.237487641880264 ], - "timestamp": 6.070927540616426 + "timestamp": 6.070927540389978 }, { "x": 2.109028816223143, "y": 3.4800388813018808, "heading": -0.760255770472509, - "angularVelocity": -4.7854036756238116e-17, - "velocityX": -4.1684189280839564e-16, - "velocityY": 3.182458036241524e-16, + "angularVelocity": -4.742890180425531e-17, + "velocityX": -4.236195451911178e-16, + "velocityY": 3.1146529727541206e-16, "moduleForcesX": [ - 60.53644088216914, - 59.71107786211956, - 56.23458372965665, - 55.717252600818924 + 60.53644087836653, + 59.71107786047138, + 56.23458373687308, + 55.71725260850273 ], "moduleForcesY": [ - -35.07667248160167, - -36.472859348504336, - -41.62412921793918, - -42.32221267335896 + -35.0766724881386, + -36.47285935116105, + -41.62412920816004, + -42.32221266320002 ], - "timestamp": 6.1324847610015265 + "timestamp": 6.1324847607658315 }, { - "x": 2.1173647314297415, - "y": 3.4676489109609023, - "heading": -0.7574096677464409, - "angularVelocity": 0.045234117251022915, - "velocityX": 0.13248564867185347, - "velocityY": -0.19691818078357895, + "x": 2.11756098918411, + "y": 3.46792188759214, + "heading": -0.7574690093542815, + "angularVelocity": 0.04446117329426013, + "velocityX": 0.13612591984038372, + "velocityY": -0.1933196762400969, "moduleForcesX": [ - 41.520736384271714, - 42.51429487640815, - 35.1511276560287, - 36.81654326527437 + 42.74872403178063, + 43.59458842062485, + 36.532752148464645, + 38.029009807643504 ], "moduleForcesY": [ - -56.31022663437434, - -55.57003233589047, - -60.49287111022869, - -59.49929247213998 + -55.38361215410406, + -54.726449152918555, + -59.66836228131139, + -58.73145114027325 ], - "timestamp": 6.1954041435170275 + "timestamp": 6.1951632937812295 }, { - "x": 2.1342750283552188, - "y": 3.4430345558261144, - "heading": -0.751664349766151, - "angularVelocity": 0.09131237069712948, - "velocityX": 0.2687613299655501, - "velocityY": -0.39120465190077036, + "x": 2.134842785454668, + "y": 3.4438456887135054, + "heading": -0.7518451991808588, + "angularVelocity": 0.0897246617440947, + "velocityX": 0.27572113511834023, + "velocityY": -0.38412192692383823, "moduleForcesX": [ - 42.70173098288214, - 43.611128958075156, - 36.275170342746776, - 37.87745965146702 + 43.83189384298978, + 44.60279573474994, + 37.56581425139671, + 39.005385774472 ], "moduleForcesY": [ - -55.41582346031395, - -54.70978545558125, - -59.821763435546124, - -58.826195167217094 + -54.526207091523304, + -53.904217238652684, + -59.01959396027511, + -58.084181071273 ], - "timestamp": 6.2583235260325285 + "timestamp": 6.257841826796628 }, { - "x": 2.1600269154540004, - "y": 3.4063896499792783, - "heading": -0.7429636060218578, - "angularVelocity": 0.1382839976560426, - "velocityX": 0.4092838497332282, - "velocityY": -0.5824104494002036, + "x": 2.1611174297283973, + "y": 3.407994009108057, + "heading": -0.7433311964216099, + "angularVelocity": 0.13583602470653403, + "velocityX": 0.41919686070627105, + "velocityY": -0.5719929596412315, "moduleForcesX": [ - 44.01151222332591, - 44.83593128986605, - 37.54047868266639, - 39.07824731715444 + 45.03070879320651, + 45.72720818672459, + 38.72596873376568, + 40.10888786396711 ], "moduleForcesY": [ - -54.37659249907092, - -53.70636995290632, - -59.03155043231922, - -58.03154166424591 + -53.535601635367144, + -52.949460544128094, + -58.260527674539446, + -57.32381863218353 ], - "timestamp": 6.3212429085480295 + "timestamp": 6.320520359812026 }, { - "x": 2.1949215222317773, - "y": 3.357943659865962, - "heading": -0.7312478740657462, - "angularVelocity": 0.18620227166446401, - "velocityX": 0.5545923272400236, - "velocityY": -0.7699692555212343, + "x": 2.1966585475096383, + "y": 3.3605831084486266, + "heading": -0.7318707568809233, + "angularVelocity": 0.18284473151072553, + "velocityX": 0.5670381240816551, + "velocityY": -0.7564136934695477, "moduleForcesX": [ - 45.47389898312857, - 46.209380246970596, - 38.97509187538921, - 40.44329438249183 + 46.366588104731406, + 46.98653915444157, + 40.038346183061186, + 41.36150168519336 ], "moduleForcesY": [ - -53.154044492191325, - -52.52435244840178, - -58.08923297674164, - -57.08406886084209 + -52.37723663439366, + -51.83024165919106, + -57.36143656121796, + -56.42213294794304 ], - "timestamp": 6.3841622910635305 + "timestamp": 6.383198892827424 }, { - "x": 2.2393001625829205, - "y": 3.297971653928642, - "heading": -0.7164541055664759, - "angularVelocity": 0.23512259510216332, - "velocityX": 0.7053254271878769, - "velocityY": -0.9531563015982434, + "x": 2.2417757829173284, + "y": 3.3018706473116755, + "heading": -0.7174043905612114, + "angularVelocity": 0.23080256706323984, + "velocityX": 0.7198195815560299, + "velocityY": -0.9367236007665936, "moduleForcesX": [ - 47.11716302884608, - 47.75573925943408, - 40.613851617327796, - 42.00243934560986 + 47.865105493752495, + 48.40288266056825, + 41.53424661497753, + 42.79020355531207 ], "moduleForcesY": [ - -51.69634105716302, - -51.116592706220615, - -56.94950428119139, - -55.94148520473914 + -51.00479249750509, + -50.50425407309677, + -56.2817455416053, + -55.341130773916184 ], - "timestamp": 6.4470816735790315 + "timestamp": 6.445877425842822 }, { - "x": 2.293551835832764, - "y": 3.226807987294763, - "heading": -0.6985158525170084, - "angularVelocity": 0.28509900021743295, - "velocityX": 0.862241030996735, - "velocityY": -1.1310293233781603, + "x": 2.296821580243133, + "y": 3.232167800844977, + "heading": -0.6998693932081385, + "angularVelocity": 0.27976081298465, + "velocityX": 0.8782240853065545, + "velocityY": -1.1120688872787494, "moduleForcesX": [ - 48.974244926718995, - 49.50301706537962, - 42.49983724647496, - 43.79203201526126 + 49.55640607435229, + 50.00204429745789, + 43.25256094553968, + 44.42804273281315 ], "moduleForcesY": [ - -49.932591819303816, - -49.41939613494972, - -55.54905272250912, - -54.54558483413394 + -49.35518638216705, + -48.914538145342306, + -54.9650826996631, + -54.028779358711596 ], - "timestamp": 6.5100010560945325 + "timestamp": 6.50855595885822 }, { - "x": 2.3581219992537226, - "y": 3.144865400496988, - "heading": -0.6773637507894199, - "angularVelocity": 0.3361778339508891, - "velocityX": 1.0262364447879162, - "velocityY": -1.302342513892083, + "x": 2.3621992355615835, + "y": 3.1518561030599934, + "heading": -0.679200184492938, + "angularVelocity": 0.3297653553908588, + "velocityX": 1.043062946326279, + "velocityY": -1.2813270177407263, "moduleForcesX": [ - 51.081995362995904, - 51.48237998253363, - 44.68553797550306, - 45.85573965609478 + 51.47499199800596, + 51.81338905645372, + 45.24113729305585, + 46.315146275458055 ], "moduleForcesY": [ - -47.76441461323329, - -47.34537980089634, - -53.79777393977218, - -52.8148137116725 + -47.34111247918622, + -46.98312892385554, + -53.331646603367375, + -52.41249065479232 ], - "timestamp": 6.5729204386100335 + "timestamp": 6.571234491873618 }, { - "x": 2.433522291274826, - "y": 3.0526620913362525, - "heading": -0.652926734850225, - "angularVelocity": 0.3883861373428849, - "velocityX": 1.1983635090908211, - "velocityY": -1.46541980347662, + "x": 2.4383720926533567, + "y": 3.061411276771742, + "heading": -0.6553291916681563, + "angularVelocity": 0.38084798217784427, + "velocityX": 1.2152941912833248, + "velocityY": -1.4429952638813623, "moduleForcesX": [ - 53.47798671720776, - 53.72572515007777, - 47.232726000587874, - 48.24428581121922 + 53.657812871578834, + 53.86840420507071, + 47.55738728725876, + 48.499088173849444 ], "moduleForcesY": [ - -45.05355867588098, - -44.773023132097414, - -51.56510924673164, - -50.632864772568915 + -44.83992767026467, + -44.60161470848597, + -51.266287364667946, + -50.38906940154251 ], - "timestamp": 6.6358398211255345 + "timestamp": 6.633913024889016 }, { - "x": 2.520339981100692, - "y": 2.9508605578412346, - "heading": -0.6251343348430549, - "angularVelocity": 0.4417144430863272, - "velocityX": 1.3798242505713987, - "velocityY": -1.6179677775753416, + "x": 2.5258731212927574, + "y": 2.9614374657104525, + "heading": -0.6281884283597926, + "angularVelocity": 0.43301529251962634, + "velocityX": 1.3960286629220273, + "velocityY": -1.5950247437464637, "moduleForcesX": [ - 56.19197506453079, - 56.259276660576184, - 50.20804830120448, - 51.011888473748435 + 56.13852280957632, + 56.19641284805428, + 50.26589724732213, + 51.0329382483159 ], "moduleForcesY": [ - -41.604044996750225, - -41.53144010163077, - -48.65926448291237, - -47.83146963835682 + -41.67704853096463, + -41.61695338715726, + -48.600192852411276, + -47.809382613298865 ], - "timestamp": 6.6987592036410355 + "timestamp": 6.696591557904414 }, { - "x": 2.6192438141173406, - "y": 2.8403235512778635, - "heading": -0.5939211843905473, - "angularVelocity": 0.4960816397843361, - "velocityX": 1.5719135989976136, - "velocityY": -1.7568037406619357, + "x": 2.6253125642587176, + "y": 2.8527168842363753, + "heading": -0.5977125836977811, + "angularVelocity": 0.48622460028738235, + "velocityX": 1.5864992076558508, + "velocityY": -1.7345744426942504, "moduleForcesX": [ - 59.2250417610776, - 59.087826967496994, - 53.66857872752665, - 54.204993983215175 + 58.932237428484335, + 58.813020339679206, + 53.42902064875195, + 53.9678341870784 ], "moduleForcesY": [ - -37.1392249820516, - -37.38030580647965, - -44.79597130253933, - -44.16473609829124 + -37.60318829571993, + -37.811782678383274, + -45.08236639483354, + -44.45475051563535 ], - "timestamp": 6.7616785861565365 + "timestamp": 6.759270090919812 }, { - "x": 2.7309783267356824, - "y": 2.7221935960005026, - "heading": -0.5592365908276438, - "angularVelocity": 0.5512545129373857, - "velocityX": 1.7758361279342616, - "velocityY": -1.8774811600901864, + "x": 2.737377784774954, + "y": 2.7362816266480974, + "heading": -0.5638459295824587, + "angularVelocity": 0.5403230178824133, + "velocityX": 1.7879362378935268, + "velocityY": -1.8576576698065623, "moduleForcesX": [ - 62.5038695327194, - 62.160031630846326, - 57.62319015145175, - 57.832996172310274 + 62.00010213745141, + 61.69318591156156, + 57.07952464389426, + 57.33202712766898 ], "moduleForcesY": [ - -31.280112248684674, - -31.988407393096097, - -39.55612203914035, - -39.27379472999048 + -32.26809282018358, + -32.88021961976661, + -40.33774835466741, + -40.002144832707245 ], - "timestamp": 6.8245979686720375 + "timestamp": 6.82194862393521 }, { - "x": 2.856329605423656, - "y": 2.598000279152458, - "heading": -0.5210635851729407, - "angularVelocity": 0.606697080113568, - "velocityX": 1.9922522071333428, - "velocityY": -1.9738483100568995, + "x": 2.8628118504353286, + "y": 2.6135144018139433, + "heading": -0.526556868174763, + "angularVelocity": 0.5949255608540639, + "velocityX": 2.0012284848715227, + "velocityY": -1.9586805709698702, "moduleForcesX": [ - 65.79278778008134, - 65.30020556417972, - 61.939549113121835, - 61.79878070206867 + 65.17607152774222, + 64.71491642924042, + 61.14954805032546, + 61.07753901466829 ], "moduleForcesY": [ - -23.548304579936264, - -24.925035881299397, - -32.34851276785782, - -32.65108476149218 + -25.204925810974277, + -26.40709149039101, + -33.818839446517686, + -33.98135694151609 ], - "timestamp": 6.8875173511875385 + "timestamp": 6.884627156950608 }, { - "x": 2.996031354964205, - "y": 2.46978535540666, - "heading": -0.47945305051497955, - "angularVelocity": 0.6613309443669427, - "velocityX": 2.220329315948571, - "velocityY": -2.0377651308674327, + "x": 3.0023435972808152, + "y": 2.4862768905201404, + "heading": -0.4858660567958102, + "angularVelocity": 0.6491985281301392, + "velocityX": 2.226148892336199, + "velocityY": -2.0300014242921804, "moduleForcesX": [ - 68.5564837925553, - 68.09308895613952, - 66.1567792079864, - 65.75590846714256 + 68.03638305751167, + 67.55366556741266, + 65.31234174805479, + 64.96053229107252 ], "moduleForcesY": [ - -13.459642197003902, - -15.709687602642282, - -22.445832125017436, - -23.647305076742175 + -15.87553479112995, + -17.88364572686639, + -24.79397361190254, + -25.75031448328813 ], - "timestamp": 6.9504367337030395 + "timestamp": 6.947305689966006 }, { - "x": 3.1505724071472994, - "y": 2.340193763604278, - "heading": -0.43457360660126365, - "angularVelocity": 0.7132848753348665, - "velocityX": 2.456175601294578, - "velocityY": -2.059645003198423, + "x": 3.1565264589627544, + "y": 2.3570325928242064, + "heading": -0.4418928272775442, + "angularVelocity": 0.701567624555973, + "velocityX": 2.459899015888926, + "velocityY": -2.0620185489054506, "moduleForcesX": [ - 69.84706521344005, - 69.75290941394955, - 69.22551312159781, - 68.88502793630397 + 69.73996701133173, + 69.52946808363019, + 68.70982043228925, + 68.32066639322257 ], "moduleForcesY": [ - -0.8233024041036456, - -4.014815042921486, - -9.277060723605338, - -11.648512019585809 + -3.8907722279034003, + -6.8454111489966385, + -12.521345429829843, + -14.587709075119305 ], - "timestamp": 7.0133561162185405 + "timestamp": 7.009984222981404 }, { - "x": 3.3199095005462036, - "y": 2.212409184758706, - "heading": -0.3867586308994902, - "angularVelocity": 0.7599403202978621, - "velocityX": 2.691334317484535, - "velocityY": -2.03092550716134, + "x": 3.325459805009775, + "y": 2.2288592413825183, + "heading": -0.3949105310397784, + "angularVelocity": 0.7495755560555903, + "velocityX": 2.69523452320619, + "velocityY": -2.0449322164288706, "moduleForcesX": [ - 68.47925044891942, - 69.15340266140203, - 69.49958829473019, - 69.7686571831701 + 69.04574597152255, + 69.5267430838888, + 69.75682983590083, + 69.84457598668554 ], "moduleForcesY": [ - 13.736045870694008, - 9.89537651518505, - 6.815502213890215, - 3.370466430920167 + 10.479207074920234, + 6.736459804087273, + 3.110509612866868, + -0.12959966259523578 ], - "timestamp": 7.0762754987340415 + "timestamp": 7.072662755996802 }, { - "x": 3.5032276259952, - "y": 2.089811754517775, - "heading": -0.3365088719529981, - "angularVelocity": 0.7986371915540097, - "velocityX": 2.9135398047467214, - "velocityY": -1.9484843197678885, + "x": 3.508485431874634, + "y": 2.1051894273892637, + "heading": -0.3453726504925619, + "angularVelocity": 0.7903484361232042, + "velocityX": 2.920068770912297, + "velocityY": -1.973080862675469, "moduleForcesX": [ - 63.762980248590615, - 65.33381311946748, - 65.64375774359121, - 66.90783722378835 + 64.90348551855642, + 66.33308380991878, + 66.72276225524763, + 67.80174756382702 ], "moduleForcesY": [ - 28.499981635594498, - 24.717827613930762, - 23.81826750682116, - 20.038950866645536 + 25.769345424686897, + 21.867362447533644, + 20.5650741326706, + 16.728754630599152 ], - "timestamp": 7.1391948812495425 + "timestamp": 7.1353412890122 }, { - "x": 3.698983205941932, - "y": 1.9754559392748747, - "heading": -0.2844250046592506, - "angularVelocity": 0.8277873242146967, - "velocityX": 3.111212668028096, - "velocityY": -1.8174974176634262, + "x": 3.704119942311664, + "y": 1.9892732212302584, + "heading": -0.2938661408515115, + "angularVelocity": 0.8217567827951044, + "velocityX": 3.1212362674309677, + "velocityY": -1.8493765023270128, "moduleForcesX": [ - 56.27400205759313, - 58.363284645856304, - 58.070669393657745, - 60.053109943523204 + 57.471427969439794, + 59.58527298092468, + 59.37846559329102, + 61.351076031348384 ], "moduleForcesY": [ - 41.37894474295054, - 38.388062514062604, - 38.80031879570978, - 35.67059242066261 + 39.677051011352944, + 36.440949209828744, + 36.741332005058595, + 33.36307222006473 ], - "timestamp": 7.2021142637650435 + "timestamp": 7.198019822027598 }, { - "x": 3.9052468507649114, - "y": 1.8717029681737345, - "heading": -0.23110382341189709, - "angularVelocity": 0.8474523924995162, - "velocityX": 3.2782210596577848, - "velocityY": -1.6489826656448716, + "x": 3.9103619312555455, + "y": 1.8836924134378115, + "heading": -0.24100411082773987, + "angularVelocity": 0.8433833320697718, + "velocityX": 3.290472495474877, + "velocityY": -1.6844811566746316, "moduleForcesX": [ - 47.55707312914185, - 49.53638736807368, - 48.78953373366416, - 50.77045950153568 + 48.26509838932949, + 50.40336737122413, + 49.6190591353058, + 51.75496442543449 ], "moduleForcesY": [ - 51.17696939290848, - 49.268536466648314, - 49.99446973782349, - 47.98722716731246 + 50.492121778115546, + 48.363822792220766, + 49.151347533828364, + 46.9041391127918 ], - "timestamp": 7.2650336462805445 + "timestamp": 7.260698355042996 }, { - "x": 4.120080601641824, - "y": 1.7801799153533593, - "heading": -0.17706327947410902, - "angularVelocity": 0.8588854781668361, - "velocityX": 3.4144287862962694, - "velocityY": -1.454608248862383, + "x": 4.1251309979342885, + "y": 1.7902287074795913, + "heading": -0.18733070094754622, + "angularVelocity": 0.8563284317296923, + "velocityX": 3.426517123908772, + "velocityY": -1.491159755370459, "moduleForcesX": [ - 39.05701336414032, - 40.46649013860068, - 39.71798936608046, - 41.14398002340263 + 39.02721416882864, + 40.63159693937693, + 39.76181294529839, + 41.38836138220273 ], "moduleForcesY": [ - 57.94278647291951, - 56.96920228244361, - 57.48746696114792, - 56.477668387844396 + 57.948757981015774, + 56.837566924459644, + 57.44189927750841, + 56.28368962505613 ], - "timestamp": 7.3279530287960455 + "timestamp": 7.3233768880583945 }, { - "x": 4.341754671827057, - "y": 1.7019368196307447, - "heading": -0.12271990769558694, - "angularVelocity": 0.8636984281454757, - "velocityX": 3.523144400386022, - "velocityY": -1.243545193777744, + "x": 4.346552661405609, + "y": 1.7100097822136517, + "heading": -0.13328440238249356, + "angularVelocity": 0.862277656558669, + "velocityX": 3.5326554853624956, + "velocityY": -1.279846885475643, "moduleForcesX": [ - 31.542569411645978, - 32.211867496766956, - 31.792298515347188, - 32.46659837801001 + 30.796111969393834, + 31.63647803362233, + 31.08882111063717, + 31.937435866659744 ], "moduleForcesY": [ - 62.36899517370198, - 62.02650340460398, - 62.24055631529311, - 61.8920479920422 + 62.72964371688498, + 62.31072023606725, + 62.583051774162115, + 62.15497397097186 ], - "timestamp": 7.3908724113115465 + "timestamp": 7.3860554210737925 }, { - "x": 4.568810602625837, - "y": 1.6376320022464737, - "heading": -0.06839919997587875, - "angularVelocity": 0.8633382202427946, - "velocityX": 3.6086802146038512, - "velocityY": -1.0220192063777647, + "x": 4.5730514606078865, + "y": 1.6437195895696548, + "heading": -0.07920610917804985, + "angularVelocity": 0.8627881126566634, + "velocityX": 3.613658270314562, + "velocityY": -1.0576219553146122, "moduleForcesX": [ - 25.215137722620277, - 25.161326804862117, - 25.198140713892162, - 25.144365342135142 + 23.887085713336567, + 23.96483684337957, + 23.909031078260046, + 23.986859531626518 ], "moduleForcesY": [ - 65.19836347228741, - 65.21911954217529, - 65.20503178610034, - 65.2257580831436 + 65.68742061578509, + 65.65913506231513, + 65.67928603196187, + 65.6509428796385 ], - "timestamp": 7.4537917938270475 + "timestamp": 7.448733954089191 }, { - "x": 4.800045847269579, - "y": 1.5876743304432248, - "heading": -0.014355888196568429, - "angularVelocity": 0.8589294684510949, - "velocityX": 3.675103527705061, - "velocityY": -0.793994947279417, + "x": 4.803338854745861, + "y": 1.5917650272197101, + "heading": -0.025361992429283036, + "angularVelocity": 0.8590519617863268, + "velocityX": 3.6741031268456674, + "velocityY": -0.8289052064632888, "moduleForcesX": [ - 19.993448516674185, - 19.3060989973268, - 19.797835730589334, - 19.116494526678093 + 18.23081795339054, + 17.63640682889574, + 18.085112159973114, + 17.495364929313684 ], "moduleForcesY": [ - 66.99591754938321, - 67.196927406319, - 67.05502467711538, - 67.25215990996023 + 67.48937858084152, + 67.6469186921904, + 67.52952569073541, + 67.68449338706219 ], - "timestamp": 7.5167111763425485 + "timestamp": 7.511412487104589 }, { - "x": 5.034473115872903, - "y": 1.5523169276578892, - "heading": 0.039207016322208126, - "angularVelocity": 0.8512941859462895, - "velocityX": 3.725835493467699, - "velocityY": -0.5619477078724477, + "x": 5.0363666951223784, + "y": 1.554384768665583, + "heading": 0.028035090410861878, + "angularVelocity": 0.8519197924914251, + "velocityX": 3.7178253728317188, + "velocityY": -0.596380558953801, "moduleForcesX": [ - 15.705471617855421, - 14.48469807799971, - 15.374313004019221, - 14.172732804713208 + 13.629985184247289, + 12.466510567267608, + 13.36498947078768, + 12.219570464535467 ], "moduleForcesY": [ - 68.13853193873344, - 68.40802253366346, - 68.21561194224486, - 68.47494827858137 + 68.57759449105092, + 68.79825994640873, + 68.63134312576102, + 68.84417352906047 ], - "timestamp": 7.5796305588580495 + "timestamp": 7.574091020119987 }, { "x": 5.271278381347656, "y": 1.5317155122756958, - "heading": 0.09212323355733296, - "angularVelocity": 0.8410161562232161, - "velocityX": 3.7636298388086282, - "velocityY": -0.3274255810936793, + "heading": 0.08080945742074001, + "angularVelocity": 0.8419847190251427, + "velocityX": 3.7478810515167584, + "velocityY": -0.3616749674775882, "moduleForcesX": [ - 12.17583308633574, - 10.508988015592593, - 11.72461754628835, - 10.093645719719404 + 9.878633404360912, + 8.235261290860429, + 9.510066456350259, + 7.902786589676527 ], "moduleForcesY": [ - 68.86508793588061, - 69.1385750114367, - 68.94526551565065, - 69.20238200518273 + 69.22664704724305, + 69.44088567639092, + 69.28024531566139, + 69.48153670734771 ], - "timestamp": 7.6425499413735505 + "timestamp": 7.636769553135385 }, { - "x": 5.388681610311286, - "y": 1.5251584282304298, - "heading": 0.11807334458511697, - "angularVelocity": 0.8348404097837044, - "velocityX": 3.7769765097724286, - "velocityY": -0.21094779615682893, + "x": 5.391042214421232, + "y": 1.5240070453835066, + "heading": 0.1074471532788096, + "angularVelocity": 0.8357210579005186, + "velocityX": 3.757425484837848, + "velocityY": -0.24184254299832358, "moduleForcesX": [ - 9.224601000293218, - 7.170145322617407, - 8.707960432775863, - 6.708850729084163 + 6.772943150194192, + 4.71192047105511, + 6.351957415318363, + 4.348339414191522 ], "moduleForcesY": [ - 69.25444608627201, - 69.49615053644614, - 69.32560078879362, - 69.54657168705602 + 69.53223448731855, + 69.70137764950671, + 69.57638151835509, + 69.72949369202985 ], - "timestamp": 7.673633859597523 + "timestamp": 7.668643460830122 }, { - "x": 5.506442673711366, - "y": 1.5222273963184734, - "heading": 0.14379764901773748, - "angularVelocity": 0.8275759911368509, - "velocityX": 3.7884883929871296, - "velocityY": -0.0942941585046398, + "x": 5.51102563137661, + "y": 1.5201232868827126, + "heading": 0.13384807268108104, + "angularVelocity": 0.8282925223702723, + "velocityX": 3.764314627013447, + "velocityY": -0.12184757946812044, "moduleForcesX": [ - 8.369389483724206, - 5.949973696893992, - 7.73117703857559, - 5.38783266312984 + 5.467336768701578, + 3.0155789586234376, + 4.950575022268367, + 2.579688806707375 ], "moduleForcesY": [ - 69.35385904945188, - 69.60180730909208, - 69.43322409042021, - 69.65302052985173 + 69.63848507273565, + 69.78654461331922, + 69.68263747119076, + 69.80962731619984 ], - "timestamp": 7.704717777821495 + "timestamp": 7.700517368524859 }, { - "x": 5.624497338635267, - "y": 1.522927459386341, - "heading": 0.16925769890783504, - "angularVelocity": 0.8190746644823769, - "velocityX": 3.7979338406848053, - "velocityY": 0.02252171244382533, + "x": 5.631134254008104, + "y": 1.5200677662545983, + "heading": 0.15997015664506697, + "angularVelocity": 0.819544444131609, + "velocityX": 3.7682427828365683, + "velocityY": -0.0017418833186710159, "moduleForcesX": [ - 7.409211713616261, - 4.57253345050813, - 6.632095741425086, - 3.899218959272906 + 4.014268392430795, + 1.116698120742873, + 3.3923076891931627, + 0.60736396962066 ], "moduleForcesY": [ - 69.45271134204867, - 69.69519778275762, - 69.53780580291625, - 69.74287628352383 + 69.72782501035493, + 69.83295042566183, + 69.76768134726464, + 69.84622551429848 ], - "timestamp": 7.735801696045467 + "timestamp": 7.732391276219596 }, { - "x": 5.742772793309857, - "y": 1.5272629148189605, - "heading": 0.19440986357494344, - "angularVelocity": 0.8091696962357507, - "velocityX": 3.8050368625463413, - "velocityY": 0.13947583446143447, + "x": 5.751262381021374, + "y": 1.523841630833003, + "heading": 0.1857658669581749, + "angularVelocity": 0.8093049198786131, + "velocityX": 3.7688547059795785, + "velocityY": 0.11839980885141156, "moduleForcesX": [ - 6.320859466741637, - 3.0067323445032117, - 5.3877760442274845, - 2.2145608793254503 + 2.3863359481069573, + -1.019059720883274, + 1.6525549624055083, + -1.5974719300993867 ], "moduleForcesY": [ - 69.54854425346966, - 69.76816296129176, - 69.63518331243104, - 69.80621969634359 + 69.79190277644955, + 69.8235099520272, + 69.82158137742265, + 69.82135764239271 ], - "timestamp": 7.766885614269439 + "timestamp": 7.7642651839143335 }, { - "x": 5.861185901308817, - "y": 1.5352368348891807, - "heading": 0.21920461001465913, - "angularVelocity": 0.7976712028728045, - "velocityX": 3.8094653043977926, - "velocityY": 0.2565287944964038, + "x": 5.871291096439659, + "y": 1.5314426318940912, + "heading": 0.21118151569712723, + "angularVelocity": 0.7973810108996604, + "velocityX": 3.7657358039629982, + "velocityY": 0.23847095040510002, "moduleForcesX": [ - 5.0743871573767105, - 1.212540305184361, - 3.969650176486514, - 0.29853628468642324 + 0.549891385422086, + -3.4334671839024553, + -0.29767021184777925, + -4.068355861282044 ], "moduleForcesY": [ - 69.63726174457908, - 69.80849842797434, - 69.71904021769083, - 69.82888852399873 + 69.81865312665636, + 69.73422687541164, + 69.83062468569385, + 69.71085861910606 ], - "timestamp": 7.797969532493411 + "timestamp": 7.796139091609071 }, { - "x": 5.979640967451708, - "y": 1.5468503445815898, - "heading": 0.24358557158644084, - "angularVelocity": 0.7843593396465376, - "velocityX": 3.810815138856516, - "velocityY": 0.3736179463840169, + "x": 5.991086014880691, + "y": 1.5428637105440566, + "heading": 0.23615643982419934, + "angularVelocity": 0.7835538825757309, + "velocityX": 3.758400745472756, + "velocityY": 0.3583206288776096, "moduleForcesX": [ - 3.6307956438747206, - -0.8621551207472431, - 2.3418002525812245, - -1.893133883598176 + -1.5364034730888085, + -6.177058299089292, + -2.492386077354901, + -6.843823309273452 ], "moduleForcesY": [ - 69.7120826368421, - 69.79764527340737, - 69.77979201753804, - 69.79043163606336 + 69.79055227447756, + 69.53111319438398, + 69.77586096014748, + 69.48206624406794 ], - "timestamp": 7.8290534507173835 + "timestamp": 7.828012999303808 }, { - "x": 6.098026846695146, - "y": 1.5621015192437462, - "heading": 0.2674883023191033, - "angularVelocity": 0.7689741866013711, - "velocityX": 3.808589328746145, - "velocityY": 0.4906451803233273, + "x": 6.110494620280023, + "y": 1.558091016928009, + "heading": 0.260621989902633, + "angularVelocity": 0.7675729726252842, + "velocityX": 3.7462807053007396, + "velocityY": 0.47773578720836973, "moduleForcesX": [ - 1.9386808198061933, - -3.28598819102792, - 0.4585755906663063, - -4.416447800806846 + -3.9233308193331884, + -9.310207032464307, + -4.971223982333152, + -9.967160671911408 ], "moduleForcesY": [ - 69.76178277329687, - 69.70685348700545, - 69.80282869693546, - 69.6609062566512 + 69.68200654002352, + 69.16570334527854, + 69.63108366349697, + 69.09079688113802 ], - "timestamp": 7.860137368941356 + "timestamp": 7.859886906998545 }, { - "x": 6.216213161429717, - "y": 1.5809836737297318, - "heading": 0.2908385525285553, - "angularVelocity": 0.7512003487206494, - "velocityX": 3.8021691436385594, - "velocityY": 0.6074573465909969, + "x": 6.229343182975531, + "y": 1.5771011290944752, + "heading": 0.2845003140637755, + "angularVelocity": 0.7491495674088557, + "velocityX": 3.728710136006611, + "velocityY": 0.5964161140369018, "moduleForcesX": [ - -0.07065192727705566, - -6.150375646776657, - -1.738667342074545, - -7.342704323857981 + -6.673094685929394, + -12.903296056772389, + -7.779731069608852, + -13.485056064651221 ], "moduleForcesY": [ - 69.76766630955797, - 69.49064187075653, - 69.76568210247977, - 69.39577088637856 + 69.45541432263478, + 68.56862083714847, + 69.36005488296091, + 68.47745191194402 ], - "timestamp": 7.891221287165328 + "timestamp": 7.891760814693282 }, { - "x": 6.334045309853754, - "y": 1.6034826557006305, - "heading": 0.313549807262191, - "angularVelocity": 0.7306432403402956, - "velocityX": 3.790775267616168, - "velocityY": 0.7238142182972129, + "x": 6.347433309831943, + "y": 1.5998571523743064, + "heading": 0.30770295299816985, + "angularVelocity": 0.727950873065531, + "velocityX": 3.7049152550538524, + "velocityY": 0.7139389213826525, "moduleForcesX": [ - -2.490383343681562, - -9.578064211771972, - -4.3259113321238125, - -10.762745909847968 + -9.861215297744039, + -17.03459334936717, + -10.968990403175749, + -17.444220781953867 ], "moduleForcesY": [ - 69.69820129934999, - 69.07536900988754, - 69.63337299442767, - 68.9276312183353 + 69.05521867685782, + 67.64066622247339, + 68.91276556625236, + 67.56234354492842 ], - "timestamp": 7.9223052053893 + "timestamp": 7.9236347223880195 }, { - "x": 6.451337870714877, - "y": 1.6295724757424068, - "heading": 0.33551971883232135, - "angularVelocity": 0.706793506913393, - "velocityX": 3.7734162088570447, - "velocityY": 0.8393349851774983, + "x": 6.464538328080527, + "y": 1.626303286615654, + "heading": 0.33012934372254554, + "angularVelocity": 0.7035971534823224, + "velocityX": 3.674008827851357, + "velocityY": 0.8297110757371571, "moduleForcesX": [ - -5.448866729736701, - -13.733572282788284, - -7.401927508185858, - -14.790647300726173 + -13.57752083156378, + -21.783197519865176, + -14.593849038845681, + -21.884589883945374 ], "moduleForcesY": [ - 69.49922387521676, - 68.33891344443161, - 69.35066212276465, - 68.15293380811582 + 68.39900163770346, + 66.24134665518923, + 68.22058375084256, + 66.24082954639027 ], - "timestamp": 7.953389123613272 + "timestamp": 7.955508630082757 }, { - "x": 6.567866022044423, - "y": 1.659208124359565, - "heading": 0.35662502542728386, - "angularVelocity": 0.6789783206509007, - "velocityX": 3.7488244078469917, - "velocityY": 0.9534077526398532, + "x": 6.580399977943102, + "y": 1.656357384692337, + "heading": 0.3516655018085619, + "angularVelocity": 0.6756673292861429, + "velocityX": 3.6349998554367304, + "velocityY": 0.9429059770303924, "moduleForcesX": [ - -9.125353794114547, - -18.83172539656305, - -11.094164589642684, - -19.562885061791146 + -17.92431111597879, + -27.21254535403587, + -18.708578792359393, + -26.82734571095702 ], "moduleForcesY": [ - 69.0754447164273, - 67.07463735744341, - 68.82909121269832, - 66.91127860350836 + 67.36450849006661, + 64.17638992872675, + 67.19039419369011, + 64.37988045730229 ], - "timestamp": 7.984473041837244 + "timestamp": 7.987382537777494 }, { - "x": 6.683354928219009, - "y": 1.6923136502791418, - "heading": 0.37671482456009425, - "angularVelocity": 0.6463084540390093, - "velocityX": 3.7153908764796526, - "velocityY": 1.0650370934911797, + "x": 6.694726351538369, + "y": 1.6899010876153, + "heading": 0.37218347560916276, + "angularVelocity": 0.6437231982066761, + "velocityX": 3.586832674869755, + "velocityY": 1.0523875278869872, "moduleForcesX": [ - -13.771593248917489, - -25.130414558467773, - -15.562652582080137, - -25.223578565092154 + -23.00758940984267, + -33.338147011171934, + -23.358188320319844, + -32.256271677852126 ], "moduleForcesY": [ - 68.25443383699609, - 64.93026638806907, - 67.92556316929904, - 64.95632378013195 + 65.77194757070217, + 61.18992795027423, + 65.69849366449613, + 61.819461145008376 ], - "timestamp": 8.015556960061216 + "timestamp": 8.019256445472232 }, { - "x": 6.7974682626271505, - "y": 1.7287625418402501, - "heading": 0.3956032132276822, - "angularVelocity": 0.6076579063002795, - "velocityX": 3.671137389627324, - "velocityY": 1.1725964306841732, + "x": 6.807192685734502, + "y": 1.7267675051619156, + "heading": 0.3915426081860902, + "angularVelocity": 0.6073661492131278, + "velocityX": 3.5284764978691405, + "velocityY": 1.1566331276256518, "moduleForcesX": [ - -19.7338671115633, - -32.869545661536705, - -20.995212284811174, - -31.878789101972526 + -28.91315705633447, + -40.074653997125445, + -28.56271316758345, + -38.09306052597789 ], "moduleForcesY": [ - 66.71548826157209, - 61.31804473588162, - 66.4083595377785, - 61.9239118979002 + 63.36360268554179, + 56.97490897615298, + 63.58659281516764, + 58.38423738606962 ], - "timestamp": 8.046640878285189 + "timestamp": 8.05113035316697 }, { - "x": 6.909800194013484, - "y": 1.7683468561362488, - "heading": 0.41306566672219286, - "angularVelocity": 0.5617841794810646, - "velocityX": 3.6138279150310604, - "velocityY": 1.2734660415324053, + "x": 6.917447317500844, + "y": 1.7667274108508237, + "heading": 0.4095941832685751, + "angularVelocity": 0.5663433318364425, + "velocityX": 3.4590873771196318, + "velocityY": 1.2536870618943754, "moduleForcesX": [ - -27.444972321937637, - -42.075345805688336, - -27.57805811433052, - -39.49775701748281 + -35.656308905451695, + -47.1677975691104, + -34.292967625798184, + -44.171577156013576 ], "moduleForcesY": [ - 63.85363325543449, - 55.33672689274484, - 63.908626512967174, - 57.321971693817275 + 59.79015829574416, + 51.22561496941096, + 60.66465034630196, + 53.91255102553536 ], - "timestamp": 8.07772479650916 + "timestamp": 8.083004260861708 }, { - "x": 7.019881390468804, - "y": 1.810734119873736, - "heading": 0.42885115065317647, - "angularVelocity": 0.5078344312078966, - "velocityX": 3.5414195746540194, - "velocityY": 1.363639661900725, + "x": 7.025125269774396, + "y": 1.809476738261155, + "heading": 0.42619090648469543, + "angularVelocity": 0.5206993561966173, + "velocityX": 3.378247603174457, + "velocityY": 1.3412013305600805, "moduleForcesX": [ - -37.27804152080681, - -52.1405235304012, - -35.411432044620646, - -47.75401943748761 + -43.0950713666489, + -54.14575035838762, + -40.439391320133275, + -50.224432044019366 ], "moduleForcesY": [ - 58.55809505279019, - 45.896730636123046, - 59.86837524976089, - 50.604050638682544 + 54.6293050128275, + 43.75538755080409, + 56.72805998056306, + 48.30614376570568 ], - "timestamp": 8.108808714733133 + "timestamp": 8.114878168556446 }, { - "x": 7.127216489228573, - "y": 1.8554226010135964, - "heading": 0.44272434085459006, - "angularVelocity": 0.4463140747396045, - "velocityX": 3.4530749304632886, - "velocityY": 1.4376720726731453, + "x": 7.1298705644585345, + "y": 1.8546303708147, + "heading": 0.44120012056964497, + "angularVelocity": 0.4708934413908578, + "velocityX": 3.2862395062225853, + "velocityY": 1.4166330964496041, "moduleForcesX": [ - -48.97728499858189, - -61.38705872437014, - -44.32215534808062, - -55.881419683225104 + -50.81886431339406, + -60.36189206611623, + -46.781345213429454, + -55.90228668204932 ], "moduleForcesY": [ - 49.06528866999657, - 32.434718317404084, - 53.54352333978887, - 41.41142967075846 + 47.484904027140054, + 34.65982136246882, + 51.598757458793386, + 41.59074319154849 ], - "timestamp": 8.139892632957105 + "timestamp": 8.146752076251184 }, { "x": 7.231364727020264, "y": 1.90172815322876, "heading": 0.45451541638849, - "angularVelocity": 0.3793304128823317, - "velocityX": 3.3505505014284465, - "velocityY": 1.489694828094504, - "moduleForcesX": [ - -60.49418247701689, - -67.53589230331421, - -53.57894617228146, - -62.756167691165814 - ], - "moduleForcesY": [ - 33.72227739407681, - 16.098766492768572, - 44.212603887497394, - 29.961682528729266 - ], - "timestamp": 8.170976551181077 - }, - { - "x": 7.4238027530368695, - "y": 1.9965622542854489, - "heading": 0.4692934758689833, - "angularVelocity": 0.24023905191549066, - "velocityX": 3.1283626232349375, - "velocityY": 1.5416675347118225, + "angularVelocity": 0.4177490863802491, + "velocityX": 3.184239708973158, + "velocityY": 1.4776281234520827, "moduleForcesX": [ - -68.24273916485633, - -69.7323377330834, - -62.01731512678528, - -67.61265076621554 + -58.09323113814453, + -65.1805058005262, + -52.97746872707234, + -60.837974152120616 ], "moduleForcesY": [ - 13.81064712566369, - -0.3213659067003291, - 31.862172293048193, - 17.244933344769986 + 38.2048879415566, + 24.420633432131687, + 45.192733096216344, + 33.9590952401435 ], - "timestamp": 8.232490528041474 + "timestamp": 8.178625983945922 }, { - "x": 7.602133310590925, - "y": 2.090716538878092, - "heading": 0.47717310494630966, - "angularVelocity": 0.12809493841064415, - "velocityX": 2.899025012783137, - "velocityY": 1.5306161200782256, + "x": 7.424008457970693, + "y": 2.003914192060848, + "heading": 0.4743314475792421, + "angularVelocity": 0.30437239580938835, + "velocityX": 2.9589897877434668, + "velocityY": 1.569568051149872, "moduleForcesX": [ - -69.13754121748958, - -68.20178675329157, - -69.06817284359947, - -69.80874265266795 + -64.18282412471406, + -68.43086071098912, + -58.78460066435345, + -64.93261887331047 ], "moduleForcesY": [ - -9.046985055914966, - -14.8227009169618, - 9.835976008332125, - 0.7232885771622478 + 27.36299974626911, + 13.845305324141684, + 37.678868187130924, + 25.739033429406035 ], - "timestamp": 8.29400450490187 + "timestamp": 8.243730544883707 }, { - "x": 7.76664139791012, - "y": 2.1816852539691975, - "heading": 0.47965086497918497, - "angularVelocity": 0.04027962683177553, - "velocityX": 2.6743204669166074, - "velocityY": 1.4788300112274317, + "x": 7.60087719793961, + "y": 2.107850851805076, + "heading": 0.48752484756484954, + "angularVelocity": 0.20264939653329472, + "velocityX": 2.716687393651782, + "velocityY": 1.596457425518163, "moduleForcesX": [ - -66.40756278498199, - -65.73447233946021, - -69.52981390692969, - -68.96429053135994 + -69.61888985652718, + -69.76727726868289, + -67.16418021608261, + -69.18600572947932 ], "moduleForcesY": [ - -21.48510284383042, - -23.5840541324825, - -6.248641320167586, - -11.053848562321154 + 5.029388418504633, + -3.222941021531897, + 19.11676020734826, + 9.676481897144177 ], - "timestamp": 8.355518481762267 + "timestamp": 8.308835105821492 }, { - "x": 7.917760773522142, - "y": 2.267858010889966, - "heading": 0.4778609500693658, - "angularVelocity": -0.029097694559421974, - "velocityX": 2.456667302700648, - "velocityY": 1.4008646704200718, + "x": 7.761930148007023, + "y": 2.210081079106282, + "heading": 0.4950554503391814, + "angularVelocity": 0.11566935811959787, + "velocityX": 2.473758331944146, + "velocityY": 1.5702467819251287, "moduleForcesX": [ - -63.7079413269904, - -63.48861495951544, - -67.77169795111662, - -67.17513898955453 + -68.82435473996972, + -68.04450224754622, + -69.8239032049172, + -69.75673087129077 ], "moduleForcesY": [ - -28.621263222946787, - -29.166159121005602, - -16.89631000921434, - -19.21841268100821 + -11.841731474215324, + -15.852074510798255, + 1.93806565534906, + -4.071564113915693 ], - "timestamp": 8.417032458622664 + "timestamp": 8.373939666759277 }, { - "x": 8.055903927968282, - "y": 2.3481558395536353, - "heading": 0.47263055774144574, - "angularVelocity": -0.08502770581375621, - "velocityX": 2.245719777793735, - "velocityY": 1.3053590868608649, + "x": 7.907620343769395, + "y": 2.308071507252318, + "heading": 0.4978684590059931, + "angularVelocity": 0.04320755145710578, + "velocityX": 2.237787854856954, + "velocityY": 1.5051238612864044, "moduleForcesX": [ - -61.53663246666227, - -61.636381591712215, - -65.62140779995624, - -65.27264016289558 + -65.92623401375947, + -65.36976574658864, + -68.92142423550537, + -68.31330607927826 ], "moduleForcesY": [ - -33.09645471160549, - -32.94683673534328, - -24.002173782199, - -24.982303085352363 + -23.130100041160027, + -24.720666810010734, + -11.488057316014388, + -14.770045889808568 ], - "timestamp": 8.47854643548306 + "timestamp": 8.439044227697062 }, { - "x": 8.181425593296224, - "y": 2.421820395971198, - "heading": 0.4645721903392223, - "angularVelocity": -0.13100059228021374, - "velocityX": 2.0405389430894125, - "velocityY": 1.197525508466805, + "x": 8.038541023495547, + "y": 2.400018073701688, + "heading": 0.49676667390448254, + "angularVelocity": -0.01692331667152227, + "velocityX": 2.0109294623960547, + "velocityY": 1.412290707823597, "moduleForcesX": [ - -59.837069189363575, - -60.1347503268217, - -63.62558289863267, - -63.52420699677533 + -62.824360850795905, + -62.67121951693069, + -66.58034896128022, + -66.08539026982844 ], "moduleForcesY": [ - -36.12028649557102, - -35.64612522470202, - -28.92915290640471, - -29.180149543959885 + -30.62126190401479, + -30.968161395864133, + -21.258041017807635, + -22.795219386908013 ], - "timestamp": 8.540060412343458 + "timestamp": 8.504148788634847 }, { - "x": 8.294623187946847, - "y": 2.488295400127025, - "heading": 0.4541517988040101, - "angularVelocity": -0.16939876215222985, - "velocityX": 1.8401930817693604, - "velocityY": 1.0806487817669281, + "x": 8.155275443871254, + "y": 2.4846239308491906, + "heading": 0.49239437013062565, + "angularVelocity": -0.06715817925621288, + "velocityX": 1.7930298383742893, + "velocityY": 1.2995380957772373, "moduleForcesX": [ - -58.495272320583396, - -58.9108341498489, - -61.90125817659251, - -61.99096011956313 + -60.073321011059726, + -60.249556015844455, + -63.930532840470896, + -63.71297068980246 ], "moduleForcesY": [ - -38.28430915682556, - -37.65794710311188, - -32.488649405293295, - -32.336456191223455 + -35.75278795396041, + -35.47671823816927, + -28.283924388405126, + -28.797286948333827 ], - "timestamp": 8.601574389203854 + "timestamp": 8.569253349572632 }, { - "x": 8.395745589032531, - "y": 2.5471579028047797, - "heading": 0.4417334392214656, - "angularVelocity": -0.20187866589616352, - "velocityX": 1.6438930832772425, - "velocityY": 0.9568963946411755, + "x": 8.258347693307394, + "y": 2.5609347268737315, + "heading": 0.4852629357835075, + "angularVelocity": -0.10953816820810822, + "velocityX": 1.5831801635930889, + "velocityY": 1.172126728532337, "moduleForcesX": [ - -57.41823039591145, - -57.901215370721474, - -60.440306372625834, - -60.66569970555321 + -57.756771897763436, + -58.16002609551108, + -61.421389264955856, + -61.46747635960799 ], "moduleForcesY": [ - -39.90297584100897, - -39.210477609648265, - -35.155958840161816, - -34.77905570626861 + -39.40997001157865, + -38.82696454641265, + -33.4114065154027, + -33.34381170977727 ], - "timestamp": 8.663088366064251 + "timestamp": 8.634357910510417 }, { - "x": 8.48500235236776, - "y": 2.598076660395606, - "heading": 0.4276083189600344, - "angularVelocity": -0.2296245663564786, - "velocityX": 1.4509997221898334, - "velocityY": 0.8277591563683715, + "x": 8.348213779476872, + "y": 2.6282292552805555, + "heading": 0.47578115701955753, + "angularVelocity": -0.14563923982239507, + "velocityX": 1.380334724250048, + "velocityY": 1.0336376966143848, "moduleForcesX": [ - -56.53862920087126, - -57.05702405798682, - -59.20466714974995, - -59.52214725960545 + -55.82852902081254, + -56.37707964117926, + -59.18899393513469, + -59.44035011210898 ], "moduleForcesY": [ - -41.15627446217204, - -40.443127482255576, - -37.21736934315343, - -36.717258988355645 + -42.11500222204666, + -41.38806414610841, + -37.242958929245574, + -36.85233746127893 ], - "timestamp": 8.724602342924648 + "timestamp": 8.699462471448202 }, { - "x": 8.562571509547876, - "y": 2.640785806675407, - "heading": 0.41201408826118213, - "angularVelocity": -0.2535071132572452, - "velocityX": 1.2610005260455623, - "velocityY": 0.6942998723156388, + "x": 8.425266606479534, + "y": 2.685948485476612, + "heading": 0.4642800962886668, + "angularVelocity": -0.1766552230016766, + "velocityX": 1.183524255332804, + "velocityY": 0.8865620067880233, "moduleForcesX": [ - -55.80869308264709, - -56.34173719772094, - -58.15451187186907, - -58.53176769808956 + -54.21921582870304, + -54.855206644706676, + -57.24745392477798, + -57.645373900028495 ], "moduleForcesY": [ - -42.15377542537939, - -41.44512997463475, - -38.852027537364954, - -38.28860744687602 + -44.181208993738856, + -43.396520697801726, + -40.17831839961693, + -39.613797717067015 ], - "timestamp": 8.786116319785044 + "timestamp": 8.764567032385987 }, { - "x": 8.62860576010961, - "y": 2.6750675306808085, - "heading": 0.3951480099492571, - "angularVelocity": -0.2741828633548061, - "velocityX": 1.0734836850427496, - "velocityY": 0.557299751293951, + "x": 8.489844645461925, + "y": 2.7336490829588826, + "heading": 0.4510317174084265, + "angularVelocity": -0.20349386724073787, + "velocityX": 0.9919126717420388, + "velocityY": 0.7326767402341282, "moduleForcesX": [ - -55.19425623129184, - -55.728131941039244, - -57.25555323504394, - -57.66899798719165 + -52.865528585952326, + -53.5493743197174, + -55.569733234873816, + -56.06636357612068 ], "moduleForcesY": [ - -42.96566364835716, - -42.27592065616304, - -40.176385790672825, - -39.58618560604843 + -45.80304315675028, + -45.00724101650629, + -42.480063091401604, + -41.828690869314826 ], - "timestamp": 8.847630296645441 + "timestamp": 8.829671593323772 }, { - "x": 8.683237297728128, - "y": 2.7007402779978236, - "heading": 0.3771761995999053, - "angularVelocity": -0.2921581609028107, - "velocityX": 0.8881158462978955, - "velocityY": 0.41734819674036067, + "x": 8.542240663164481, + "y": 2.7709723355879694, + "heading": 0.43626250509958603, + "angularVelocity": -0.2268537272366222, + "velocityX": 0.8047979580513017, + "velocityY": 0.5732816885863476, "moduleForcesX": [ - -54.67049432361609, - -55.195762968597755, - -56.480010214696975, - -56.91239458884492 + -51.71605380731337, + -52.42081923342628, + -54.11889003994841, + -54.677868682944876 ], "moduleForcesY": [ - -43.638836888514874, - -42.97645664054756, - -41.268832050266276, - -40.67476273388533 + -47.105665338095726, + -46.3243865448859, + -44.322863344470825, + -43.63617258426661 ], - "timestamp": 8.909144273505838 + "timestamp": 8.894776154261557 }, { - "x": 8.726581564114948, - "y": 2.717650467677025, - "heading": 0.35824027480182863, - "angularVelocity": -0.30783125664352257, - "velocityX": 0.7046246820488491, - "velocityY": 0.2748999583879684, + "x": 8.582709383241658, + "y": 2.797622875392326, + "heading": 0.4201634326355504, + "angularVelocity": -0.247280255517275, + "velocityX": 0.6215957759986721, + "velocityY": 0.40934981237065615, "moduleForcesX": [ - -54.21904272949312, - -54.729099389545915, - -55.805815985531794, - -56.244420654225245 + -50.730544843076125, + -51.437804788630906, + -52.85923893372407, + -53.45364309208686 ], "moduleForcesY": [ - -44.20576368971399, - -43.57579607050465, - -42.183797695943305, - -41.60055887427103 + -48.17246735463719, + -47.41986096115254, + -45.825351350532394, + -45.13424388858939 ], - "timestamp": 8.970658250366235 + "timestamp": 8.959880715199342 }, { - "x": 8.758740183629461, - "y": 2.7256665220403926, - "heading": 0.338462250167195, - "angularVelocity": -0.32152082573882035, - "velocityX": 0.5227855709523735, - "velocityY": 0.13031273171557645, + "x": 8.611473874971457, + "y": 2.813353753747135, + "heading": 0.40289733126075, + "angularVelocity": -0.2652057110299845, + "velocityX": 0.4418199173063615, + "velocityY": 0.24162482824885853, "moduleForcesX": [ - -53.82607120430835, - -54.316194914018055, - -55.21550784813393, - -55.65085738495609 + -49.87778962282822, + -50.57482599449837, + -51.759729184855516, + -52.369784201574845 ], "moduleForcesY": [ - -44.689609283680795, - -44.09507977862367, - -42.960129353038816, - -42.39731433858149 + -49.06077660106482, + -48.34454115681274, + -47.069973438706235, + -46.393130116240336 ], - "timestamp": 9.032172227226631 + "timestamp": 9.024985276137127 }, { - "x": 8.779803276062012, - "y": 2.7246744632720947, - "heading": 0.31794821481648144, - "angularVelocity": -0.33348576043569805, - "velocityX": 0.34241148934902593, - "velocityY": -0.016127371679270322, + "x": 8.628730773925781, + "y": 2.817955732345581, + "heading": 0.38460441295906894, + "angularVelocity": -0.2809775235127061, + "velocityX": 0.2650643627074838, + "velocityY": 0.07068596319762284, "moduleForcesX": [ - -53.48098782939392, - -53.947745103828254, - -54.695219377692204, - -55.12018458029143 + -49.13353520650032, + -49.81148698796487, + -50.794430820642766, + -51.405616940966546 ], "moduleForcesY": [ - -45.107332847724315, - -44.55002787941316, - -43.626228500899906, - -43.09016461444635 + -49.81111505036111, + -49.135249239699505, + -48.11534331383063, + -47.464060182990515 ], - "timestamp": 9.093686204087028 + "timestamp": 9.090089837074911 }, { - "x": 8.789390520920405, - "y": 2.711156481525927, - "heading": 0.29317696313872205, - "angularVelocity": -0.3456625720749201, - "velocityX": 0.13378216652004424, - "velocityY": -0.1886323872700164, + "x": 8.63409093131169, + "y": 2.809578261213375, + "heading": 0.3636328313393472, + "angularVelocity": -0.29609942384481297, + "velocityX": 0.07568048716897836, + "velocityY": -0.11828217921293742, "moduleForcesX": [ - -53.179448380447525, - -53.62389224585945, - -54.2363017447046, - -54.649105974722524 + -48.48056297546824, + -49.13563036489729, + -49.94272883080661, + -50.54684256882197 ], "moduleForcesY": [ - -45.47706110036372, - -44.95351001122035, - -44.210687438028955, - -43.700838292783004 + -50.45598549008582, + -49.81982384689785, + -49.00837234110654, + -48.38670375127015 ], - "timestamp": 9.165349304691366 + "timestamp": 9.160915985087659 }, { - "x": 8.783794368286188, - "y": 2.6855639318912203, - "heading": 0.267539420337893, - "angularVelocity": -0.35775095669356133, - "velocityX": -0.07808973637788985, - "velocityY": -0.35712311383241235, + "x": 8.625782813087357, + "y": 2.7880784954449576, + "heading": 0.34158319665982656, + "angularVelocity": -0.31132054048107033, + "velocityX": -0.11730298000729854, + "velocityY": -0.3035568977229701, "moduleForcesX": [ - -54.03803305894964, - -54.47570349158013, - -55.06023489691758, - -55.46707669488016 + -49.434220030426786, + -50.08516326654055, + -50.87601640496041, + -51.47573234890385 ], "moduleForcesY": [ - -44.44927461362763, - -43.91326452339743, - -43.17589421817186, - -42.65354228524133 + -49.51809039841881, + -48.8612826879258, + -48.03470968086272, + -47.3932817354508 ], - "timestamp": 9.237012405295705 + "timestamp": 9.231742133100406 }, { - "x": 8.76275609467282, - "y": 2.6482308380775303, - "heading": 0.2410424727336723, - "angularVelocity": -0.3697432483491591, - "velocityX": -0.2935719140806326, - "velocityY": -0.5209528125193889, + "x": 8.603519939908493, + "y": 2.7537628223496577, + "heading": 0.3184472650429824, + "angularVelocity": -0.3266580530778029, + "velocityX": -0.314331271762177, + "velocityY": -0.4845057095173895, "moduleForcesX": [ - -54.99526164627351, - -55.42219111728091, - -55.979540705697985, - -56.37649095420048 + -50.50767113082232, + -51.15037434057846, + -51.926551544815524, + -52.51764005400783 ], "moduleForcesY": [ - -43.254451016653434, - -42.70766979489076, - -41.972052319967815, - -41.439058648655944 + -48.418122856235094, + -47.74052141391343, + -46.89226762522613, + -46.231320487911496 ], - "timestamp": 9.308675505900043 + "timestamp": 9.302568281113153 }, { - "x": 8.725986587983398, - "y": 2.5995490914917334, - "heading": 0.21369371175484705, - "angularVelocity": -0.38162960782036737, - "velocityX": -0.5130884148096344, - "velocityY": -0.679313986909043, + "x": 8.566978749430204, + "y": 2.7069943004295456, + "heading": 0.294215364928076, + "angularVelocity": -0.3421321192075174, + "velocityX": -0.5159279659217504, + "velocityY": -0.6603284695321164, "moduleForcesX": [ - -56.06650028997491, - -56.47788343064675, - -57.008812567039705, - -57.391121229420555 + -51.72253309664589, + -52.351547871619616, + -53.1150303533149, + -53.69192145893697 ], "moduleForcesY": [ - -41.850827206177, - -41.29576254132913, - -40.5570050084547, - -40.01607334967796 + -47.11270598236726, + -46.414844244282484, + -45.535980106736204, + -44.856574967933696 ], - "timestamp": 9.380338606504381 + "timestamp": 9.3733944291259 }, { - "x": 8.673161820665543, - "y": 2.5399838352697053, - "heading": 0.1855016713653994, - "angularVelocity": -0.3933968827988571, - "velocityX": -0.7371264552102906, - "velocityY": -0.8311844689904736, + "x": 8.515792047639485, + "y": 2.648208808083207, + "heading": 0.26887617344655823, + "angularVelocity": -0.35776605381613247, + "velocityX": -0.7227091014678351, + "velocityY": -0.8299970278739272, "moduleForcesX": [ - -57.26911890755975, - -57.659157229516524, - -58.16431071641967, - -58.52626643425478 + -53.10459348185788, + -53.71272642624142, + -54.46576355767823, + -55.021155079771624 ], "moduleForcesY": [ - -40.18252449430719, - -39.62280351144781, - -38.874499091161304, - -38.32952205223615 + -45.542660989830466, + -44.826209580052634, + -43.90453987460494, + -43.209067443671216 ], - "timestamp": 9.45200170710872 + "timestamp": 9.444220577138648 }, { - "x": 8.603918051131696, - "y": 2.4700943215420597, - "heading": 0.156476193662526, - "angularVelocity": -0.4050268193547327, - "velocityX": -0.9662402121860534, - "velocityY": -0.9752510446556707, + "x": 8.449541508295152, + "y": 2.577937395823057, + "heading": 0.24241650450925817, + "angularVelocity": -0.3735861638633528, + "velocityX": -0.9353966183846341, + "velocityY": -0.9921676419209313, "moduleForcesX": [ - -58.62178890554708, - -58.98353550726006, - -59.46309693106811, - -59.79789639516721 + -54.68387284740476, + -55.261648531274275, + -56.00637251373348, + -56.530738642405 ], "moduleForcesY": [ - -38.17360914029891, - -37.61450756539028, - -36.848345847349954, - -36.304918084781264 + -43.62575228611848, + -42.8942919991783, + -41.91307663268656, + -41.20610218263478 ], - "timestamp": 9.523664807713057 + "timestamp": 9.515046725151395 }, { - "x": 8.517847357752716, - "y": 2.3905626117140066, - "heading": 0.12662900025486085, - "angularVelocity": -0.41649319044197497, - "velocityX": -1.2010461821096667, - "velocityY": -1.1098000108474018, + "x": 8.367749643224476, + "y": 2.496837777925061, + "heading": 0.2148211681213936, + "angularVelocity": -0.38962074265140273, + "velocityX": -1.1548258286749533, + "velocityY": -1.1450519359516806, "moduleForcesX": [ - -60.14239359593657, - -60.46764931194161, - -60.920705975153766, - -61.22039017159285 + -56.49342338399157, + -57.02842944478959, + -57.76596137264449, + -58.246993802956545 ], "moduleForcesY": [ - -35.71931021724835, - -35.16851680517709, - -34.373858499651185, - -33.84003668835228 + -41.245553252699736, + -40.505899357042395, + -39.44204855581045, + -38.731729530962944 ], - "timestamp": 9.595327908317396 + "timestamp": 9.585872873164142 }, { - "x": 8.414494950035653, - "y": 2.302233586652294, - "heading": 0.09597460073164957, - "angularVelocity": -0.4277570920697149, - "velocityX": -1.4421983816704484, - "velocityY": -1.2325593550492429, + "x": 8.269872586703466, + "y": 2.4057394247705677, + "heading": 0.18607303639793232, + "angularVelocity": -0.4058971514063899, + "velocityX": -1.381933922248534, + "velocityY": -1.2862248718947424, "moduleForcesX": [ - -61.84288712238317, - -62.122276292758905, - -62.54567545312701, - -62.801268264067 + -58.56488116310085, + -59.04110442112179, + -59.76960478030494, + -60.19174438403556 ], "moduleForcesY": [ - -32.67310530550032, - -32.14185463603263, - -31.305402017642134, - -30.792810837453125 + -38.23433183892145, + -37.4988143615417, + -36.32045167969364, + -35.620919583734725 ], - "timestamp": 9.666991008921734 + "timestamp": 9.65669902117689 }, { - "x": 8.2933614907194, - "y": 2.2061711843005387, - "heading": 0.06453176165426741, - "angularVelocity": -0.4387591216710293, - "velocityX": -1.690318424610812, - "velocityY": -1.340472314784823, + "x": 8.155297198594102, + "y": 2.3057088684340563, + "heading": 0.15615361511012685, + "angularVelocity": -0.4224346816435758, + "velocityX": -1.6176989900501801, + "velocityY": -1.4123393569068392, "moduleForcesX": [ - -63.717577250904, - -63.94109844594106, - -64.32744860294157, - -64.52959830151751 + -60.91614610648466, + -61.31371636368788, + -62.02428527901857, + -62.36884971040699 ], "moduleForcesY": [ - -28.828283743204985, - -28.332961805960878, - -27.43891241104092, - -26.96427075977155 + -34.34726778188899, + -33.637620843364225, + -32.301189412117985, + -31.636486262630484 ], - "timestamp": 9.738654109526072 + "timestamp": 9.727525169189637 }, { - "x": 8.153917386350976, - "y": 2.103737230084854, - "heading": 0.032325911066374, - "angularVelocity": -0.4494063237049454, - "velocityX": -1.9458285113605962, - "velocityY": -1.4293821136938516, + "x": 8.023350887750192, + "y": 2.1981442323035503, + "heading": 0.12504476412995907, + "angularVelocity": -0.4392283337866834, + "velocityX": -1.8629604255784487, + "velocityY": -1.518713626938277, "moduleForcesX": [ - -65.71756976574333, - -65.87625105005283, - -66.21063177131644, - -66.35137198001577 + -63.520716542602926, + -63.81730819571486, + -64.48569991401658, + -64.73298646364212 ], "moduleForcesY": [ - -23.893867912535846, - -23.458019215236998, - -22.489715553229402, - -22.076652060585157 + -29.22638447390868, + -28.58008989329779, + -27.0282766874693, + -26.43848196643227 ], - "timestamp": 9.81031721013041 + "timestamp": 9.798351317202384 }, { - "x": 7.995643377180051, - "y": 1.996698976059294, - "heading": -0.0006069291037716975, - "angularVelocity": -0.45955086917006693, - "velocityX": -2.2085844435447637, - "velocityY": -1.4936313545311521, + "x": 7.8733430094963675, + "y": 2.084908220562927, + "heading": 0.09273289280550447, + "angularVelocity": -0.4562138734219929, + "velocityX": -2.1179731280434493, + "velocityY": -1.5987882288931434, "moduleForcesX": [ - -67.69752144856352, - -67.78734757334065, - -68.04263214008894, - -68.11934773122928 + -66.23669578701445, + -66.41381020137457, + -66.98496208031126, + -67.12159504763625 ], "moduleForcesY": [ - -17.47107619907958, - -17.127002103641317, - -16.07246012795104, - -15.752714784412063 + -22.361245953920218, + -21.84052143654894, + -20.00571217853413, + -19.554879879038108 ], - "timestamp": 9.881980310734749 + "timestamp": 9.869177465215131 }, { - "x": 7.818124590832862, - "y": 1.8873638999769546, - "heading": -0.034213912395681174, - "angularVelocity": -0.468957985469511, - "velocityX": -2.4771295806372327, - "velocityY": -1.5256816291830964, + "x": 7.704675957081329, + "y": 1.9684978477709927, + "heading": 0.059218072302814256, + "angularVelocity": -0.47319840825818243, + "velocityX": -2.38142348761764, + "velocityY": -1.6436072842897478, "moduleForcesX": [ - -69.313687888199, - -69.3432482954395, - -69.47611214375348, - -69.49889577515765 + -68.65865518215234, + -68.7193569333156, + -69.08393459750226, + -69.12136156007266 ], "moduleForcesY": [ - -9.057792824867414, - -8.844692568248144, - -7.710678951592893, - -7.521600845904924 + -13.088029266778213, + -12.787167251577607, + -10.613744042624726, + -10.394212606212953 ], - "timestamp": 9.953643411339087 + "timestamp": 9.940003613227878 }, { - "x": 7.621240028444532, - "y": 1.7787112968796408, - "heading": -0.06841646549100246, - "angularVelocity": -0.4772686753278808, - "velocityX": -2.747363157998911, - "velocityY": -1.5161582764496822, + "x": 7.5170858672429315, + "y": 1.85220323216817, + "heading": 0.024531257518575638, + "angularVelocity": -0.4897458884534555, + "velocityX": -2.6485993535132915, + "velocityY": -1.6419728993576328, "moduleForcesX": [ - -69.86517241058907, - -69.86591053003337, - -69.82282638095482, - -69.8236070877981 + -69.87540008016852, + -69.87946038423385, + -69.8592396435103, + -69.86628903728995 ], "moduleForcesY": [ - 1.837312109452161, - 1.8863774422298984, - 3.0464717958262337, - 3.075434675486523 + -0.7650699301565221, + -0.7967154045035195, + 1.692280326436183, + 1.5791599402263883 ], - "timestamp": 10.025306511943425 + "timestamp": 10.010829761240625 }, { - "x": 7.40547828858971, - "y": 1.6744202309428289, - "heading": -0.10310081581402811, - "angularVelocity": -0.48399176187648607, - "velocityX": -3.0107787415740312, - "velocityY": -1.4552965899789445, + "x": 7.3110452541555135, + "y": 1.7400985444349457, + "heading": -0.011240894625415731, + "angularVelocity": -0.5050698526983756, + "velocityX": -2.909103754313112, + "velocityY": -1.5828149755235674, "moduleForcesX": [ - -68.17759957909404, - -68.20364257441437, - -67.95854331471489, - -67.98903555111266 + -68.32136154860933, + -68.4142317509721, + -67.82249433370446, + -67.94358861469844 ], "moduleForcesY": [ - 15.314544404106083, - 15.206518112264616, - 16.260066564096665, - 16.13994452069931 + 14.610999455735499, + 14.191657426650783, + 16.77917087513115, + 16.30055641195774 ], - "timestamp": 10.096969612547763 + "timestamp": 10.081655909253373 }, { - "x": 7.172300767498466, - "y": 1.5786130838476118, - "heading": -0.13811594024177143, - "angularVelocity": -0.48860744417223123, - "velocityX": -3.253801735130716, - "velocityY": -1.3369104362952573, + "x": 7.0881669627233315, + "y": 1.636620998376319, + "heading": -0.04793860084545987, + "angularVelocity": -0.5181378240906105, + "velocityX": -3.146836270018103, + "velocityY": -1.4610076781248036, "moduleForcesX": [ - -62.90664616032858, - -62.994759663175806, - -62.62640903247994, - -62.71838864361929 + -62.398383827401425, + -62.73953495712387, + -61.584615770221966, + -61.958519697541895 ], "moduleForcesY": [ - 30.40188460343236, - 30.221851782690536, - 30.976021760761594, - 30.79224774785525 + 31.413609816398104, + 30.73509287925401, + 32.98416244544942, + 32.28414823175951 ], - "timestamp": 10.168632713152101 + "timestamp": 10.15248205726612 }, { - "x": 6.924263851753144, - "y": 1.4952348436632368, - "heading": -0.17328998021723432, - "angularVelocity": -0.4908249807619052, - "velocityX": -3.461152443218535, - "velocityY": -1.1634751982713893, + "x": 6.851189181350193, + "y": 1.5457607903673927, + "heading": -0.08534517045150107, + "angularVelocity": -0.528146322447312, + "velocityX": -3.3459080865231883, + "velocityY": -1.282862481700599, "moduleForcesX": [ - -53.63079242417267, - -53.732278169292705, - -53.450679135245664, - -53.553119173625326 + -52.16729788107013, + -52.743022249429835, + -51.36856497292788, + -51.96098925114158 ], "moduleForcesY": [ - 44.77877441586359, - 44.65783761257958, - 44.9941386909519, - 44.87304464918282 + 46.4697214226326, + 45.81902631486864, + 47.35399851175358, + 46.706752999418384 ], - "timestamp": 10.24029581375644 + "timestamp": 10.223308205278867 }, { - "x": 6.664645466021959, - "y": 1.4273879906699989, - "heading": -0.20846188348487105, - "angularVelocity": -0.49079516475048257, - "velocityX": -3.622762391548932, - "velocityY": -0.9467473835360443, + "x": 6.603389718482076, + "y": 1.470425326889423, + "heading": -0.12323379914298056, + "angularVelocity": -0.534952552899821, + "velocityX": -3.498700265663482, + "velocityY": -1.0636673826227476, "moduleForcesX": [ - -41.769412633151376, - -41.76713542840772, - -41.77205019669556, - -41.769773098851616 + -39.932337350479486, + -40.53983959192529, + -39.373177991520045, + -39.983518610928336 ], "moduleForcesY": [ - 56.01545952386056, - 56.01714957737264, - 56.01348552259297, - 56.01517566218937 + 57.340323299968865, + 56.91405451985326, + 57.72751484701259, + 57.308022362111764 ], - "timestamp": 10.311958914360778 + "timestamp": 10.294134353291614 }, { - "x": 6.396804785667352, - "y": 1.37708752891827, - "heading": -0.24350598675754967, - "angularVelocity": -0.4890118202694272, - "velocityX": -3.7374977930886835, - "velocityY": -0.7019018341035008, + "x": 6.347913177850161, + "y": 1.4123915991053673, + "heading": -0.1614061090640271, + "angularVelocity": -0.5389578706747766, + "velocityX": -3.607093535369666, + "velocityY": -0.8193828044073681, "moduleForcesX": [ - -29.670078278170966, - -29.48805559061184, - -29.820795736138948, - -29.638793031028708 + -28.27085895786839, + -28.72327567698898, + -27.969794241953274, + -28.42128687481258 ], "moduleForcesY": [ - 63.27577283382489, - 63.36048200304406, - 63.204474407441516, - 63.28970026112863 + 63.91704070638253, + 63.71563591935232, + 64.05032104573215, + 63.85186792328192 ], - "timestamp": 10.383622014965116 + "timestamp": 10.364960501304362 }, { - "x": 6.123724098762273, - "y": 1.3454463182894645, - "heading": -0.27833593889663377, - "angularVelocity": -0.4860235162218966, - "velocityX": -3.8106178019395767, - "velocityY": -0.4415272345457245, + "x": 6.087428151650664, + "y": 1.3726159298536926, + "heading": -0.19970318630453027, + "angularVelocity": -0.5407194703516945, + "velocityX": -3.677808740249677, + "velocityY": -0.561595828203393, "moduleForcesX": [ - -18.964618645051026, - -18.60764038496825, - -19.189884865939113, - -18.832039003279412 + -18.438441470262664, + -18.66329414572283, + -18.32289991687435, + -18.547288960002728 ], "moduleForcesY": [ - 67.27850831701049, - 67.37773513616717, - 67.21397107913663, - 67.3147241815706 + 67.42955829795206, + 67.36785699497005, + 67.46142450851352, + 67.4002635181382 ], - "timestamp": 10.455285115569454 + "timestamp": 10.435786649317109 }, { - "x": 5.847858828640413, - "y": 1.3329886237876336, - "heading": -0.3128943538499734, - "angularVelocity": -0.4822344367171772, - "velocityX": -3.849474384941141, - "velocityY": -0.17383694532855082, + "x": 5.824078839371321, + "y": 1.3515584546015937, + "heading": -0.2379996051703208, + "angularVelocity": -0.5407101747069203, + "velocityX": -3.7182498225365426, + "velocityY": -0.29731216285133905, "moduleForcesX": [ - -10.164815803497616, - -9.673316441913729, - -10.413603531769702, - -9.919643964436528 + -10.576255375139242, + -10.574993214493354, + -10.57678518795997, + -10.575523009234347 ], "moduleForcesY": [ - 69.17032970505136, - 69.24042697051493, - 69.13258323678724, - 69.20483068344068 + 69.11369299701585, + 69.11388541630474, + 69.11361014421324, + 69.11380257613486 ], - "timestamp": 10.526948216173793 + "timestamp": 10.506612797329856 }, { - "x": 5.571170150686736, - "y": 1.339901067236654, - "heading": -0.3471395996450313, - "angularVelocity": -0.4778644170607483, - "velocityX": -3.8609643682780583, - "velocityY": 0.09645749891823462, + "x": 5.5595602703116835, + "y": 1.3494038487343114, + "heading": -0.27619370702127155, + "angularVelocity": -0.5392655526610999, + "velocityX": -3.734758651734541, + "velocityY": -0.030421051091110783, "moduleForcesX": [ - -3.140660151426149, - -2.5499567673882018, - -3.391519077751636, - -2.7966358407203566 + -4.3819858465305295, + -4.179901202476201, + -4.454918349746145, + -4.252321785026191 ], "moduleForcesY": [ - 69.85390969745158, - 69.87766236501122, - 69.84141674708727, - 69.86745404796969 + 69.79184657492922, + 69.8041566069463, + 69.78698263696384, + 69.79953511767619 ], - "timestamp": 10.59861131677813 + "timestamp": 10.577438945342603 }, { "x": 5.295215606689453, "y": 1.3661898374557495, - "heading": -0.3810366506112695, - "angularVelocity": -0.4730056427977949, - "velocityX": -3.850720128910756, - "velocityY": 0.3668383030792834, + "heading": -0.3142003336934002, + "angularVelocity": -0.5366185757453372, + "velocityX": -3.7323032670738447, + "velocityY": 0.23700270581448382, "moduleForcesX": [ - 2.4358890124890213, - 3.105036958141336, - 2.1874776648542436, - 2.8624717837810634 + 0.5151198465758231, + 0.8900974969762288, + 0.3932026550090731, + 0.770045001718678 ], "moduleForcesY": [ - 69.89157339518185, - 69.86477674457022, - 69.89901845264843, - 69.87435635433927 + 69.9365194091976, + 69.93262774089895, + 69.93690494603392, + 69.93364443708154 ], - "timestamp": 10.670274417382469 + "timestamp": 10.64826509335535 }, { - "x": 5.053706815736057, - "y": 1.4043054270449447, - "heading": -0.4105962505484791, - "angularVelocity": -0.46833496540702335, - "velocityX": -3.8264053470585537, - "velocityY": 0.6038939420574941, + "x": 5.05792538929108, + "y": 1.3966823635324737, + "heading": -0.3482525379686159, + "angularVelocity": -0.5333202229677834, + "velocityX": -3.716401752678385, + "velocityY": 0.4775691075577662, "moduleForcesX": [ - 6.887915896080987, - 7.623701240859441, - 6.643290267386372, - 7.386549953753599 + 4.430042032874039, + 4.95053093134464, + 4.273296521707641, + 4.797518672081996 ], "moduleForcesY": [ - 69.5924526319861, - 69.51542701820333, - 69.6153579444215, - 69.54014360764195 + 69.79829505364833, + 69.76315167405717, + 69.80750234348231, + 69.77327260349615 ], - "timestamp": 10.733390780926925 + "timestamp": 10.712114545914499 }, { - "x": 4.814521797126242, - "y": 1.457279005501737, - "heading": -0.43977801159576735, - "angularVelocity": -0.46234857980584404, - "velocityX": -3.789588074752571, - "velocityY": 0.8393002302720002, + "x": 4.822357731816456, + "y": 1.442469918801244, + "heading": -0.3819972270935512, + "angularVelocity": -0.5285039694533855, + "velocityX": -3.6894232923360217, + "velocityY": 0.7171174291017512, "moduleForcesX": [ - 10.475140797961476, - 11.417395555766568, - 10.184857658397029, - 11.139880728378749 + 7.554810386078761, + 8.313127657144003, + 7.335080255073474, + 8.101550015304404 ], "moduleForcesY": [ - 69.13513408837922, - 68.98542996372927, - 69.17724110186298, - 69.02951613308481 + 69.52196945360694, + 69.43512984509572, + 69.54458000792314, + 69.45920878774324 ], - "timestamp": 10.79650714447138 + "timestamp": 10.775963998473648 }, { - "x": 4.578623056803542, - "y": 1.5249252632965482, - "heading": -0.46848135774782446, - "angularVelocity": -0.4547686929371364, - "velocityX": -3.7375210971485733, - "velocityY": 1.071770520289322, + "x": 4.589376593940337, + "y": 1.503428017324539, + "heading": -0.4153167376730134, + "angularVelocity": -0.5218448905038962, + "velocityX": -3.6489136325843017, + "velocityY": 0.9547160716346837, "moduleForcesX": [ - 14.840754027841374, - 16.029076302383867, - 14.51917987513575, - 15.728849295978776 + 11.364581723457395, + 12.409065316223296, + 11.08567451247928, + 12.146230097766022 ], "moduleForcesY": [ - 68.32109565083954, - 68.05166292944023, - 68.38835125483823, - 68.11984422324372 + 68.99256903562845, + 68.81203194141854, + 69.03650497066894, + 68.8574640226182 ], - "timestamp": 10.859623508015837 + "timestamp": 10.839813451032796 }, { - "x": 4.347193447367128, - "y": 1.6069250977069311, - "heading": -0.4965850522073392, - "angularVelocity": -0.4452679603399553, - "velocityX": -3.6667132965195126, - "velocityY": 1.299185025966005, + "x": 4.360049392260068, + "y": 1.5793325904196134, + "heading": -0.4480682781137704, + "angularVelocity": -0.5129494322667391, + "velocityX": -3.591686263368148, + "velocityY": 1.1888053859938605, "moduleForcesX": [ - 20.193590004699995, - 21.665646251529257, - 19.875228316396825, - 21.38196372017987 + 16.060474026737822, + 17.44417496465417, + 15.743033698279458, + 17.156332450571153 ], "moduleForcesY": [ - 66.92237625745865, - 66.45990723727093, - 67.0149915440133, - 66.54903999850195 + 68.04180346878879, + 67.69982630825619, + 68.1137623119266, + 67.77111438492656 ], - "timestamp": 10.922739871560292 + "timestamp": 10.903662903591945 }, { - "x": 4.121687707159994, - "y": 1.7027378890121, - "heading": -0.5239423865994234, - "angularVelocity": -0.4334428166606177, - "velocityX": -3.572856982615943, - "velocityY": 1.5180340869556606, + "x": 4.135705069539603, + "y": 1.6697908456139685, + "heading": -0.4800781004717692, + "angularVelocity": -0.5013327612847737, + "velocityX": -3.5136452033420373, + "velocityY": 1.4167428469423091, "moduleForcesX": [ - 26.76366004990429, - 28.54407768472358, - 26.51477620428529, - 28.34898574175057 + 21.89196611243463, + 23.664965264016004, + 21.586990023029227, + 23.41134616769712 ], "moduleForcesY": [ - 64.56136029572545, - 63.79374619278623, - 64.66009920375362, - 63.876720382176266 + 66.38195678091566, + 65.77026762364113, + 66.47840712872917, + 65.85754632928692 ], - "timestamp": 10.985856235104748 + "timestamp": 10.967512356151094 }, { - "x": 3.903869186822254, - "y": 1.8114606574002452, - "heading": -0.5503757290218597, - "angularVelocity": -0.4188033172066106, - "velocityX": -3.4510625787926257, - "velocityY": 1.7225765599053773, + "x": 3.9179949909601377, + "y": 1.774123115557712, + "heading": -0.5111344027979444, + "angularVelocity": -0.48639888176653956, + "velocityX": -3.409740723740524, + "velocityY": 1.6340354656462184, "moduleForcesX": [ - 34.722789021923006, - 36.79190026410112, - 34.65315726916901, - 36.79826370259493 + 29.124210645761526, + 31.309392995735184, + 28.932435627249138, + 31.199954435020395 ], "moduleForcesY": [ - 60.634978039609805, - 59.40218821323202, - 60.6689615218732, - 59.39227430417506 + 63.52533931919331, + 62.47695800696286, + 63.60772502954319, + 62.52636867386374 ], - "timestamp": 11.048972598649204 + "timestamp": 11.031361808710242 }, { - "x": 3.6957796336708695, - "y": 1.9316273259274328, - "heading": -0.5756742900350444, - "angularVelocity": -0.40082412218451013, - "velocityX": -3.296919237193045, - "velocityY": 1.903890873601256, + "x": 3.7089231153715345, + "y": 1.8911721418901661, + "heading": -0.5409813219232001, + "angularVelocity": -0.46745771387165014, + "velocityX": -3.2744505584433368, + "velocityY": 1.8332032874365511, "moduleForcesX": [ - 43.97155118149319, - 46.20214883747921, - 44.22377698479971, - 46.54083607831509 + 37.90825639910293, + 40.44135259696435, + 37.994172581878566, + 40.64072265092307 ], "moduleForcesY": [ - 54.28134738689487, - 52.39692915304774, - 54.06704722487376, - 52.087132813104375 + 58.68761411990541, + 56.972077301885136, + 58.62385597562841, + 56.821660386292294 ], - "timestamp": 11.11208896219366 + "timestamp": 11.095211261269391 }, { - "x": 3.499550666767407, - "y": 2.061002715105782, - "heading": -0.5996083713418497, - "angularVelocity": -0.37920564434843623, - "velocityX": -3.109003052199785, - "velocityY": 2.0497915582101762, + "x": 3.5107601533164168, + "y": 2.019044446471857, + "heading": -0.5693252783250675, + "angularVelocity": -0.4439185500550772, + "velocityX": -3.1035968847429976, + "velocityY": 2.0027157548960086, "moduleForcesX": [ - 53.763954059728796, - 55.84492515312586, - 54.42048149959161, - 56.552580490225075 + 47.952424172958146, + 50.57190975238657, + 48.49980640396711, + 51.2266283249439 ], "moduleForcesY": [ - 44.579593956853635, - 41.94760445118944, - 43.76152240959032, - 40.974101674150546 + 50.78756695761204, + 48.182517312375914, + 50.25197010379258, + 47.47244005809576 ], - "timestamp": 11.175205325738116 + "timestamp": 11.15906071382854 }, { - "x": 3.3170061238667126, - "y": 2.1965515433699756, - "heading": -0.621969243888579, - "angularVelocity": -0.3542801151872387, - "velocityX": -2.8921904344524014, - "velocityY": 2.1476019949837775, + "x": 3.3257171127383325, + "y": 2.154907115536406, + "heading": -0.5958715572695051, + "angularVelocity": -0.41576361081320273, + "velocityX": -2.898114755277928, + "velocityY": 2.1278595762225385, "moduleForcesX": [ - 62.44766102413855, - 63.94433819362826, - 63.329847528999416, - 64.77963805969534 + 58.0104977519177, + 60.19934179791254, + 59.013462507055486, + 61.2087388169446 ], "moduleForcesY": [ - 31.256641416018002, - 28.0802822128175, - 29.404706704310698, - 26.071343409245944 + 38.879821467287535, + 35.405230100188476, + 37.3182212633444, + 33.60787910396 ], - "timestamp": 11.238321689282571 + "timestamp": 11.222910166387688 }, { - "x": 3.149232244356222, - "y": 2.334780583324695, - "heading": -0.6426179790087161, - "angularVelocity": -0.32715343471259145, - "velocityX": -2.6581677094296006, - "velocityY": 2.1900666038429124, + "x": 3.155400510330475, + "y": 2.2951308910705395, + "heading": -0.620392443592049, + "angularVelocity": -0.38404223277918687, + "velocityX": -2.667471616143963, + "velocityY": 2.19616253411468, "moduleForcesX": [ - 68.07446024974723, - 68.73289751434758, - 68.68747354304334, - 69.20841989455995 + 65.83208211172878, + 67.07228199343204, + 66.81818884359254, + 67.90516981439036 ], "moduleForcesY": [ - 15.578092751741488, - 12.393772344765436, - 12.552654231475199, - 9.321777066848862 + 23.286937742879168, + 19.455710293987412, + 20.244285253522296, + 16.268687090512074 ], - "timestamp": 11.301438052827027 + "timestamp": 11.286759618946837 }, { - "x": 2.996456707624642, - "y": 2.4723242803752896, - "heading": -0.6615054771208647, - "angularVelocity": -0.2992488326556613, - "velocityX": -2.4205376886767986, - "velocityY": 2.179208200955956, + "x": 3.000433493753425, + "y": 2.4359044574852438, + "heading": -0.6427805397589214, + "angularVelocity": -0.3506388116034793, + "velocityX": -2.427068837175911, + "velocityY": 2.2047732716940165, "moduleForcesX": [ - 69.84418644116603, - 69.8030417146416, - 69.73398928648147, - 69.55639119192205 + 69.54650836600102, + 69.78388265544288, + 69.79679859116072, + 69.82517169644473 ], "moduleForcesY": [ - 0.04124705741425996, - -2.629684927774557, - -3.7965442820408897, - -6.360953421225059 + 6.348124333641362, + 2.931670853614007, + 2.0228290324436116, + -1.3111191685548196 ], - "timestamp": 11.364554416371483 + "timestamp": 11.350609071505986 }, { - "x": 2.858285394290842, - "y": 2.6063679764517347, - "heading": -0.6786531021168939, - "angularVelocity": -0.2716827147994899, - "velocityX": -2.18915199758744, - "velocityY": 2.1237550541395294, + "x": 2.8605755262449746, + "y": 2.5738952164278426, + "heading": -0.6630425145247281, + "angularVelocity": -0.3173398354048022, + "velocityX": -2.190433306830491, + "velocityY": 2.1611893823955435, "moduleForcesX": [ - 68.5832050639289, - 68.18255030614512, - 67.65433881512726, - 67.18772346415568 + 69.26890065109001, + 68.89986618517693, + 68.45222405005646, + 67.96002786407358 ], "moduleForcesY": [ - -13.292687011674952, - -15.252960265172256, - -17.395421555860977, - -19.15158282474659 + -8.993101394774568, + -11.54522411202305, + -13.87299037870192, + -16.161430765057244 ], - "timestamp": 11.427670779915939 + "timestamp": 11.414458524065134 }, { - "x": 2.734026522437346, - "y": 2.734745646026211, - "heading": -0.6941195885833025, - "angularVelocity": -0.2450471731552614, - "velocityX": -1.9687267275145863, - "velocityY": 2.0339839364169796, + "x": 2.735118463226934, + "y": 2.70652006019163, + "heading": -0.6812534516483567, + "angularVelocity": -0.28521680913017455, + "velocityX": -1.9648886245629291, + "velocityY": 2.0771492698536242, "moduleForcesX": [ - 65.71596180721829, - 65.23236893280917, - 64.13301722958752, - 63.6607820099661 + 66.56985010341027, + 66.01724983813499, + 64.83928204278061, + 64.28532521518495 ], "moduleForcesY": [ - -23.750213140906997, - -25.069078070391868, - -27.734992318593385, - -28.82188410507732 + -21.21291575912241, + -22.903571424790023, + -26.016891342337594, + -27.382914500304768 ], - "timestamp": 11.490787143460395 + "timestamp": 11.478307976624283 }, { - "x": 2.622913491858143, - "y": 2.8558455619096765, - "heading": -0.7079766418555111, - "angularVelocity": -0.21954771304985882, - "velocityX": -1.7604472808535805, - "velocityY": 1.918677013104065, + "x": 2.623211412340952, + "y": 2.8318836413124444, + "heading": -0.6975143593601079, + "angularVelocity": -0.2546757577394683, + "velocityX": -1.7526704834675835, + "velocityY": 1.9634245259140095, "moduleForcesX": [ - 62.3261455799643, - 61.90369851571434, - 60.3053013643231, - 59.94983415338624 + 62.957124910706376, + 62.45929870418039, + 60.62064580180965, + 60.211130480488805 ], "moduleForcesY": [ - -31.626927610118603, - -32.460334406565565, - -35.32513309689565, - -35.9385060308983 + -30.33692388884184, + -31.367391790541795, + -34.7676381916512, + -35.48902427322522 ], - "timestamp": 11.55390350700485 + "timestamp": 11.542157429183431 }, { - "x": 2.5242136717280004, - "y": 2.968472284388882, - "heading": -0.720296269771335, - "angularVelocity": -0.19518912725614584, - "velocityX": -1.5637754551658334, - "velocityY": 1.7844298396544638, + "x": 2.524027231935006, + "y": 2.9486107208127024, + "heading": -0.7119282315157714, + "angularVelocity": -0.22574777978418986, + "velocityX": -1.5534069037485454, + "velocityY": 1.8281610072087373, "moduleForcesX": [ - 58.98754783726104, - 58.678775107744755, - 56.70105562415566, - 56.492209664989446 + 59.28998740855844, + 58.93448494902304, + 56.604293732503145, + 56.387554803221356 ], "moduleForcesY": [ - -37.51207015880541, - -38.003843268920306, - -40.88479342874241, - -41.18285690919068 + -37.023359271023516, + -37.59971228683433, + -41.01043622517381, + -41.32007696242945 ], - "timestamp": 11.617019870549306 + "timestamp": 11.60600688174258 }, { - "x": 2.437268193420317, - "y": 3.0717284892697196, - "heading": -0.7311460366120948, - "angularVelocity": -0.17190101316781217, - "velocityX": -1.377542580482226, - "velocityY": 1.6359656843682122, + "x": 2.436823832988882, + "y": 3.0556924556358678, + "heading": -0.7245906689335784, + "angularVelocity": -0.19831708668256617, + "velocityX": -1.3657658045751015, + "velocityY": 1.6770971485459225, "moduleForcesX": [ - 55.933543070193124, - 55.74620404374717, - 53.4983817914488, - 53.427889596621455 + 55.924906891884625, + 55.72478848329192, + 53.0592023113592, + 53.02122971774716 ], "moduleForcesY": [ - -41.94896951930148, - -42.20583852522306, - -45.01283639047294, - -45.10426074033776 + -41.9548706445922, + -42.23018067659876, + -45.52453660860857, + -45.577986795152945 ], - "timestamp": 11.680136234093762 + "timestamp": 11.669856334301729 }, { - "x": 2.3614992683800686, - "y": 3.1649283314618284, - "heading": -0.7405881776451042, - "angularVelocity": -0.1495989392094639, - "velocityX": -1.2004640442708914, - "velocityY": 1.4766351696809092, + "x": 2.3609561392644562, + "y": 3.1523738866134026, + "heading": -0.7355878255117103, + "angularVelocity": -0.17223572227098613, + "velocityX": -1.188227787139511, + "velocityY": 1.5142092391156017, "moduleForcesX": [ - 53.226119004537075, - 53.150380788764224, - 50.717908435297424, - 50.76595922311749 + 52.96028064478196, + 52.90088037948619, + 50.017708559288, + 50.12810503784231 ], "moduleForcesY": [ - -45.3497145739425, - -45.44508539739661, - -48.13839282499532, - -48.093987559665536 + -45.65625782653057, + -45.73286478403445, + -48.86224974564624, + -48.75628870048946 ], - "timestamp": 11.743252597638218 + "timestamp": 11.733705786860877 }, { - "x": 2.2964046195038508, - "y": 3.2475371925356646, - "heading": -0.7486801929297746, - "angularVelocity": -0.12820788192226776, - "velocityX": -1.0313434618325024, - "velocityY": 1.3088342932756496, + "x": 2.295869220398745, + "y": 3.238077294744024, + "heading": -0.7449971922573848, + "angularVelocity": -0.14736800972503292, + "velocityX": -1.0193810010417363, + "velocityY": 1.3422731863085038, "moduleForcesX": [ - 50.85429251327673, - 50.87522935086129, - 48.32191532894057, - 48.467633279346366 + 50.38715799993498, + 50.44685956622395, + 47.42973162422406, + 47.658319113484225 ], "moduleForcesY": [ - -48.006099795729824, - -47.9893857552052, - -50.55448655081397, - -50.41998583087466 + -48.493835508606004, + -48.43806643870586, + -51.39038837291991, + -51.18442141820145 ], - "timestamp": 11.806368961182674 + "timestamp": 11.797555239420026 }, { - "x": 2.2415479359062194, - "y": 3.3191302814359016, - "heading": -0.7554758085003378, - "angularVelocity": -0.10766804658789833, - "velocityX": -0.8691356807810039, - "velocityY": 1.1343031328129516, + "x": 2.2410859959949136, + "y": 3.3123508545538702, + "heading": -0.7528891490451303, + "angularVelocity": -0.1236025756122906, + "velocityX": -0.8580061724582897, + "velocityY": 1.1632607145854852, "moduleForcesX": [ - 48.7813998909101, - 48.883997299800775, - 46.25717342000062, - 46.48203569715231 + 48.16035953471259, + 48.31828690116759, + 45.225962077158464, + 45.54734290961062 ], "moduleForcesY": [ - -50.12061198883146, - -50.02516024483403, - -52.45958599626199, - -52.26481927286433 + -50.71618060999772, + -50.57101713150958, + -53.34991243772539, + -53.08075358781461 ], - "timestamp": 11.86948532472713 + "timestamp": 11.861404691979175 }, { - "x": 2.1965490021845606, - "y": 3.379364027011461, - "heading": -0.7610258706649285, - "angularVelocity": -0.08793380754076002, - "velocityX": -0.7129519382079801, - "velocityY": 0.9543285162988591, + "x": 2.1961946939313863, + "y": 3.374834024346928, + "heading": -0.7593284846297825, + "angularVelocity": -0.10085185270284114, + "velocityX": -0.7030804535393855, + "velocityY": 0.9786014959983499, "moduleForcesX": [ - 46.96552536511235, - 47.136382750106904, - 44.47147307724585, - 44.76001886720274 + 46.22788871890863, + 46.46616016540548, + 43.34033970137869, + 43.73431169906387 ], "moduleForcesY": [ - -51.83392911935416, - -51.682542993841096, - -53.98932327657365, - -53.75408240792354 + -52.491972499861475, + -52.28563530920509, + -54.900801591760306, + -54.591680501964085 ], - "timestamp": 11.932601688271586 + "timestamp": 11.925254144538323 }, { - "x": 2.16107481240881, - "y": 3.4279560426439364, - "heading": -0.7653786869846049, - "angularVelocity": -0.06896494150222135, - "velocityX": -0.5620442589466436, - "velocityY": 0.7698798362844577, + "x": 2.1608377743767466, + "y": 3.4252338531970117, + "heading": -0.7643752716013872, + "angularVelocity": -0.07904197717168335, + "velocityX": -0.5537544667573492, + "velocityY": 0.7893541264648288, "moduleForcesX": [ - 45.36776130374439, - 45.59551423238422, - 42.91858789949486, - 43.2583272033556 + 44.54201907610667, + 44.84605364869624, + 41.71600811601532, + 42.16694704188603 ], "moduleForcesY": [ - -53.2445780347107, - -53.0530790830467, - -55.238217704660805, - -54.97579973773786 + -53.93680181997976, + -53.68810804227729, + -56.151644018849424, + -55.81741599524645 ], - "timestamp": 11.995718051816041 + "timestamp": 11.989103597097472 }, { - "x": 2.134832009400895, - "y": 3.4646708607022925, - "heading": -0.768580123623318, - "angularVelocity": -0.050722767582422834, - "velocityX": -0.4157844580103423, - "velocityY": 0.5817004655614504, + "x": 2.1347026814888364, + "y": 3.4633084503090976, + "heading": -0.7680853894338862, + "angularVelocity": -0.058107277099393444, + "velocityX": -0.40932368000649094, + "velocityY": 0.5963183016614753, "moduleForcesX": [ - 43.954428645312426, - 44.2297096749135, - 41.559591533194514, - 41.940634703058834 + 43.06208338047946, + 43.420291447119574, + 40.30623488983344, + 40.80225251311549 ], "moduleForcesY": [ - -54.42275064440647, - -54.20223828129335, - -56.273239100999206, - -55.992648881357695 + -55.13137229057452, + -54.85302592490151, + -57.17764132287779, + -56.82788011765582 ], - "timestamp": 12.058834415360497 + "timestamp": 12.05295304965662 }, { - "x": 2.1175606055664447, - "y": 3.489309593661584, - "heading": -0.770673719921477, - "angularVelocity": -0.03317042016662467, - "velocityX": -0.27364383599643854, - "velocityY": 0.39036997025244236, + "x": 2.1175142950444665, + "y": 3.488855217863188, + "heading": -0.7705109754915318, + "angularVelocity": -0.037989144157478276, + "velocityX": -0.26920178255948496, + "velocityY": 0.40010942193161164, "moduleForcesX": [ - 42.69713344336191, - 43.01233196517679, - 40.36254825642123, - 40.77712883283335 + 41.75436699647201, + 42.15764707113133, + 39.07345325528694, + 39.60555733885558 ], "moduleForcesY": [ - -55.41943823043682, - -55.177780690773446, - -57.14258045212568, - -56.849966899553834 + -56.13323789601473, + -55.83391387223054, + -58.03186064603064, + -57.67277468150624 ], - "timestamp": 12.121950778904953 + "timestamp": 12.11680250221577 }, { "x": 2.1090288162231445, "y": 3.501702308654785, "heading": -0.771700836029523, - "angularVelocity": -0.016273372709797004, - "velocityX": -0.13517555296560768, - "velocityY": 0.19634710077161807, + "angularVelocity": -0.018635407044235015, + "velocityX": -0.13289822357460562, + "velocityY": 0.20120909853840863, "moduleForcesX": [ - 41.5722477087897, - 41.92121432741254, - 39.30147273432098, - 39.743500474089345 + 40.59125684604141, + 41.032490008004295, + 37.98763054279593, + 38.5490223991702 ], "moduleForcesY": [ - -56.272332012188514, - -56.01516696263238, - -57.881532814470596, - -57.581128152106956 + -56.98424266070932, + -56.6699287318646, + -58.752507438065514, + -58.388136078887676 ], - "timestamp": 12.185067142449409 + "timestamp": 12.180651954774918 }, { "x": 2.1090288162231445, "y": 3.501702308654785, "heading": -0.771700836029523, - "angularVelocity": 7.034369509774633e-21, - "velocityX": 1.0745130979383846e-20, - "velocityY": 7.792911077792643e-21, + "angularVelocity": -4.8374542557573845e-23, + "velocityX": -1.6522222635939176e-22, + "velocityY": 1.1005265766974004e-22, "moduleForcesX": [ - 40.56018810627996, - 40.937946455779034, - 38.35525709358692, - 38.819933249113355 + 39.55019943140344, + 40.023804726295744, + 37.02474315966745, + 37.61025741647993 ], "moduleForcesY": [ - -57.00975798757109, - -56.74117440782203, - -58.516369460899966, - -58.211114035733786 + -57.7153729196056, + -57.39024948338779, + -59.367606493987516, + -59.000582824693424 ], - "timestamp": 12.248183505993865 + "timestamp": 12.244501407334067 } ], "trajectoryWaypoints": [ @@ -20141,7 +20120,7 @@ "controlIntervalCount": 34 }, { - "timestamp": 2.2648261210315326, + "timestamp": 2.2648261210285314, "isStopPoint": false, "x": 7.107321739196777, "y": 1.0795719623565674, @@ -20152,7 +20131,7 @@ "controlIntervalCount": 14 }, { - "timestamp": 3.204592088409269, + "timestamp": 3.2045920884281465, "isStopPoint": false, "x": 8.881949424743652, "y": 1.0173043012619019, @@ -20163,7 +20142,7 @@ "controlIntervalCount": 20 }, { - "timestamp": 4.6551114717591044, + "timestamp": 4.655111471745355, "isStopPoint": false, "x": 5.302053451538086, "y": 1.5809556245803833, @@ -20174,7 +20153,7 @@ "controlIntervalCount": 24 }, { - "timestamp": 6.1324847610015265, + "timestamp": 6.1324847607658315, "isStopPoint": true, "x": 2.1090288162231445, "y": 3.48003888130188, @@ -20185,7 +20164,7 @@ "controlIntervalCount": 24 }, { - "timestamp": 7.6425499413735505, + "timestamp": 7.636769553135385, "isStopPoint": false, "x": 5.271278381347656, "y": 1.5317155122756958, @@ -20196,7 +20175,7 @@ "controlIntervalCount": 17 }, { - "timestamp": 8.170976551181077, + "timestamp": 8.178625983945922, "isStopPoint": false, "x": 7.231364727020264, "y": 1.9017281532287598, @@ -20204,21 +20183,21 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 14 }, { - "timestamp": 9.093686204087028, + "timestamp": 9.090089837074911, "isStopPoint": false, - "x": 8.779803276062012, - "y": 2.7246744632720947, - "heading": 0.31794821481648144, + "x": 8.628730773925781, + "y": 2.817955732345581, + "heading": 0.38460441295906894, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 22 }, { - "timestamp": 10.670274417382469, + "timestamp": 10.64826509335535, "isStopPoint": false, "x": 5.295215606689453, "y": 1.3661898374557495, @@ -20229,7 +20208,7 @@ "controlIntervalCount": 25 }, { - "timestamp": 12.248183505993865, + "timestamp": 12.244501407334067, "isStopPoint": true, "x": 2.1090288162231445, "y": 3.501702308654785, @@ -28080,6 +28059,1579 @@ ], "eventMarkers": [], "isTrajectoryStale": true + }, + "3 center": { + "waypoints": [ + { + "x": 1.4015021324157715, + "y": 5.563943386077881, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 2.624587297439575, + "y": 4.260610580444336, + "heading": -0.350313284574424, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 2.05912709236145, + "y": 5.330090045928955, + "heading": 0.052583458296069426, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 11 + }, + { + "x": 2.9206175804138184, + "y": 5.5130615234375, + "heading": 0.14756797074007866, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 2.021008014678955, + "y": 7.007328033447266, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 10 + }, + { + "x": 2.8596270084381104, + "y": 6.999704360961914, + "heading": -0.027461342770395147, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.4015021324157717, + "y": 5.563943386077881, + "heading": 2.230586975435435e-18, + "angularVelocity": -4.993708780228108e-16, + "velocityX": 2.2186084345747425e-14, + "velocityY": -2.3644285540186748e-14, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ], + "timestamp": 0 + }, + { + "x": 1.4206212389589077, + "y": 5.543569823071968, + "heading": -0.005440740553276968, + "angularVelocity": -0.06321989027440103, + "velocityX": 0.22215869438518032, + "velocityY": -0.23673512918379025, + "moduleForcesX": [ + 47.940963091989595, + 43.779015007865034, + 51.71856759026114, + 47.81380065892401 + ], + "moduleForcesY": [ + -50.97920586080499, + -54.592033915440794, + -47.13866785517865, + -51.09102404890194 + ], + "timestamp": 0.08606058203599731 + }, + { + "x": 1.4588579982416516, + "y": 5.50282426541773, + "heading": -0.016323802947849745, + "angularVelocity": -0.12645815467526472, + "velocityX": 0.4443004964254837, + "velocityY": -0.4734520342542199, + "moduleForcesX": [ + 47.897364078006646, + 43.771363296078064, + 51.712153560329, + 47.85692298524638 + ], + "moduleForcesY": [ + -51.01338140036875, + -54.59132698392599, + -47.13783450766829, + -51.0426999647787 + ], + "timestamp": 0.17212116407199463 + }, + { + "x": 1.5162104171628858, + "y": 5.441708865073567, + "heading": -0.032652672648373536, + "angularVelocity": -0.18973691920489047, + "velocityX": 0.6664191378657773, + "velocityY": -0.7101439350063614, + "moduleForcesX": [ + 47.832382527825025, + 43.760444196177374, + 51.70298147300229, + 47.92205717348266 + ], + "moduleForcesY": [ + -51.06506924736401, + -54.59072757046299, + -47.13719675772626, + -50.9707235368353 + ], + "timestamp": 0.25818174610799194 + }, + { + "x": 1.5926756045431116, + "y": 5.360226735801809, + "heading": -0.05443308174346003, + "angularVelocity": -0.25308228900808055, + "velocityX": 0.8885041862345195, + "velocityY": -0.9467996533549011, + "moduleForcesX": [ + 47.74512096081025, + 43.74635348449958, + 51.68931736462727, + 48.00815391044375 + ], + "moduleForcesY": [ + -51.133333759147, + -54.588485401020556, + -47.13677120729667, + -50.87397800267328 + ], + "timestamp": 0.34424232814398925 + }, + { + "x": 1.6882490058265867, + "y": 5.258382764481358, + "heading": -0.08167367566723759, + "angularVelocity": -0.31652811634851347, + "velocityX": 1.1105363105122372, + "velocityY": -1.1833985890311751, + "moduleForcesX": [ + 47.63372466857229, + 43.72840022744862, + 51.668243433528644, + 48.113016067033115 + ], + "moduleForcesY": [ + -51.216216787013636, + -54.58157282722093, + -47.135721060383055, + -50.750174463810936 + ], + "timestamp": 0.43030291017998656 + }, + { + "x": 1.8029224178515728, + "y": 5.136185717697895, + "heading": -0.11438761196088189, + "angularVelocity": -0.3801268306536648, + "velocityX": 1.332473116574703, + "velocityY": -1.4198956586668827, + "moduleForcesX": [ + 47.49348736287193, + 43.70277282082887, + 51.63393088382914, + 48.231135604313586 + ], + "moduleForcesY": [ + -51.30877200765891, + -54.563746594487974, + -47.12995843648461, + -50.59351350853768 + ], + "timestamp": 0.5163634922159839 + }, + { + "x": 1.936676738585046, + "y": 4.9936559524582, + "heading": -0.1525981886270299, + "angularVelocity": -0.4439962612620674, + "velocityX": 1.5541879641384613, + "velocityY": -1.6561561857250287, + "moduleForcesX": [ + 47.30730481341717, + 43.65083702902553, + 51.569007669599586, + 48.34309714132929 + ], + "moduleForcesY": [ + -51.393363168260414, + -54.515933915159074, + -47.100223160042596, + -50.38283518179482 + ], + "timestamp": 0.6024240742519812 + }, + { + "x": 2.089417359472379, + "y": 4.830894184799883, + "heading": -0.19638882103609856, + "angularVelocity": -0.5088349552543711, + "velocityX": 1.7748034844634022, + "velocityY": -1.8912464178636816, + "moduleForcesX": [ + 46.92254775633583, + 43.388297546565106, + 51.333326631328006, + 48.27968404555044 + ], + "moduleForcesY": [ + -51.315242953431444, + -54.28258670469318, + -46.8588510495494, + -49.928189244812266 + ], + "timestamp": 0.6884846562879785 + }, + { + "x": 2.2231712411306024, + "y": 4.688364652999476, + "heading": -0.23481358247355089, + "angularVelocity": -0.4464850286666412, + "velocityX": 1.5541828604889207, + "velocityY": -1.6561534735218788, + "moduleForcesX": [ + -46.804641906327866, + -43.5737849154587, + -51.151878946250015, + -48.397943993853495 + ], + "moduleForcesY": [ + 51.411338017062235, + 54.12246522092687, + 47.04773571475752, + 49.80566610187949 + ], + "timestamp": 0.7745452383239758 + }, + { + "x": 2.3378438009906706, + "y": 4.5661682512486585, + "heading": -0.2677858501447284, + "angularVelocity": -0.383128569329872, + "velocityX": 1.3324632166436747, + "velocityY": -1.4198881637340148, + "moduleForcesX": [ + -46.8930408063945, + -43.75302961542786, + -51.4208655887188, + -48.80743987389535 + ], + "moduleForcesY": [ + 51.77006647658862, + 54.43088247512983, + 47.26280662177527, + 49.932717396293974 + ], + "timestamp": 0.8606058203599731 + }, + { + "x": 2.433416232746065, + "y": 4.4643250767817815, + "heading": -0.2952821236280862, + "angularVelocity": -0.3194990416382979, + "velocityX": 1.1105250446261894, + "velocityY": -1.1833893284845862, + "moduleForcesX": [ + -46.834172015543345, + -43.8038207106873, + -51.45084121327245, + -48.97366881140082 + ], + "moduleForcesY": [ + 51.91054751178551, + 54.480374777947134, + 47.331348947919075, + 49.87523949636651 + ], + "timestamp": 0.9466664023959704 + }, + { + "x": 2.5098805109805107, + "y": 4.382843730787292, + "heading": -0.3172895530520021, + "angularVelocity": -0.2557202020164514, + "velocityX": 0.888493622368971, + "velocityY": -0.9467905530509265, + "moduleForcesX": [ + -46.77185013607419, + -43.83510322729932, + -51.45045906865701, + -49.085367782981976 + ], + "moduleForcesY": [ + 52.003993622508574, + 54.49394996948038, + 47.37501122701898, + 49.81059264285791 + ], + "timestamp": 1.0327269844319678 + }, + { + "x": 2.5672321904712163, + "y": 4.3217289912490875, + "heading": -0.33380008105844566, + "angularVelocity": -0.1918477381378256, + "velocityX": 0.6664105458858951, + "velocityY": -0.7101362560644467, + "moduleForcesX": [ + -46.721839228792774, + -43.85751027569671, + -51.44391879316928, + -49.163980062749964 + ], + "moduleForcesY": [ + 52.06961690331074, + 54.49746975666322, + 47.406083494827506, + 49.75817477281888 + ], + "timestamp": 1.1187875664679652 + }, + { + "x": 2.6054684419481404, + "y": 4.280983903135222, + "heading": -0.3448087987822844, + "angularVelocity": -0.12791823461321336, + "velocityX": 0.444294595953872, + "velocityY": -0.47344657843923665, + "moduleForcesX": [ + -46.68832390905574, + -43.873154666936216, + -51.438399514205486, + -49.21567045651427 + ], + "moduleForcesY": [ + 52.11282780371421, + 54.49861617678228, + 47.42729008575955, + 49.72306941004055 + ], + "timestamp": 1.2048481485039626 + }, + { + "x": 2.6245872974433224, + "y": 4.260610580440736, + "heading": -0.3503132845744241, + "angularVelocity": -0.06396059220042477, + "velocityX": 0.2221557770547791, + "velocityY": -0.23673233693882528, + "moduleForcesX": [ + -46.672848976425186, + -43.88250079839491, + -51.436915163186654, + -49.242971039133984 + ], + "moduleForcesY": [ + 52.135797056459324, + 54.500625069799426, + 47.43940924267183, + 49.70711869156378 + ], + "timestamp": 1.29090873053996 + }, + { + "x": 2.624587297439575, + "y": 4.260610580444336, + "heading": -0.350313284574424, + "angularVelocity": 4.86945709754994e-16, + "velocityX": -2.1781193472847508e-14, + "velocityY": 2.3655888321777277e-14, + "moduleForcesX": [ + -46.676116985699565, + -43.88573472382722, + -51.440890855557896, + -49.24709237978015 + ], + "moduleForcesY": [ + 52.139555439371115, + 54.50503144060741, + 47.44278460160171, + 49.711156539011256 + ], + "timestamp": 1.3769693125759574 + }, + { + "x": 2.6097025403261815, + "y": 4.272883049188608, + "heading": -0.3486185943542768, + "angularVelocity": 0.023711121663719633, + "velocityX": -0.20825887874704344, + "velocityY": 0.17170925805626883, + "moduleForcesX": [ + -53.325527820192114, + -52.543899565580986, + -55.33489027006052, + -54.67599479757407 + ], + "moduleForcesY": [ + 45.30344546948554, + 46.204887640771894, + 42.82491499809031, + 43.659875176737565 + ], + "timestamp": 1.4484416893016012 + }, + { + "x": 2.580134116922562, + "y": 4.297663630247455, + "heading": -0.3451094282478584, + "angularVelocity": 0.04909821482314941, + "velocityX": -0.4137042155527122, + "velocityY": 0.34671550166182774, + "moduleForcesX": [ + -52.56862851207183, + -51.668428542826135, + -54.74294626604449, + -53.98380292140949 + ], + "moduleForcesY": [ + 46.169623323178776, + 47.17145541851404, + 43.5681756135052, + 44.50151083261666 + ], + "timestamp": 1.519914066027245 + }, + { + "x": 2.5361681561986593, + "y": 4.335274736263326, + "heading": -0.3396203018012238, + "angularVelocity": 0.07680067038585842, + "velocityX": -0.6151461937353943, + "velocityY": 0.5262327598304285, + "moduleForcesX": [ + -51.50047411154375, + -50.41891834157215, + -53.90421586456722, + -52.99033254736792 + ], + "moduleForcesY": [ + 47.344772853066466, + 48.49067038940593, + 44.586953730344504, + 45.66446768873032 + ], + "timestamp": 1.5913864427528888 + }, + { + "x": 2.4782427587314393, + "y": 4.386182986369333, + "heading": -0.3319081747833718, + "angularVelocity": 0.10790360375738635, + "velocityX": -0.810458532923371, + "velocityY": 0.7122786797293141, + "moduleForcesX": [ + -49.88208352874225, + -48.50022068908951, + -52.625503387992744, + -51.45217423757492 + ], + "moduleForcesY": [ + 49.02799707238364, + 50.389801569506666, + 46.06811701007498, + 47.36853338734092 + ], + "timestamp": 1.6628588194785325 + }, + { + "x": 2.407106964769289, + "y": 4.45111875799722, + "heading": -0.321583297914309, + "angularVelocity": 0.14445968277546212, + "velocityX": -0.9952907323070566, + "velocityY": 0.9085436161792976, + "moduleForcesX": [ + -47.15780192777166, + -45.2126943325825, + -50.44841035705035, + -48.77740463936025 + ], + "moduleForcesY": [ + 51.624929028521905, + 53.32888659245528, + 48.40981741843909, + 50.08380727693866 + ], + "timestamp": 1.7343311962041763 + }, + { + "x": 2.3242974518893162, + "y": 4.531358566520759, + "heading": -0.3079313440530546, + "angularVelocity": 0.1910102124288775, + "velocityX": -1.158622623127807, + "velocityY": 1.1226688178584086, + "moduleForcesX": [ + -41.72003068858196, + -38.48311349131453, + -45.98995227278055, + -43.116081797134584 + ], + "moduleForcesY": [ + 56.06234348401295, + 58.3198188676031, + 52.60707005821105, + 54.9720866974948 + ], + "timestamp": 1.80580357292982 + }, + { + "x": 2.2342218638275644, + "y": 4.629362025502949, + "heading": -0.2893536832656072, + "angularVelocity": 0.25992784399271474, + "velocityX": -1.260285334646795, + "velocityY": 1.3712074961147915, + "moduleForcesX": [ + -27.000280396790707, + -19.781400792593253, + -32.92142671337878, + -25.680048193575647 + ], + "moduleForcesY": [ + 64.36040574604013, + 66.91361057866158, + 61.51512045582401, + 64.8450531339154 + ], + "timestamp": 1.8772759496554638 + }, + { + "x": 2.1541934591617964, + "y": 4.743577081279941, + "heading": -0.2632851786920285, + "angularVelocity": 0.3647353812470825, + "velocityX": -1.119710973304069, + "velocityY": 1.598030752209193, + "moduleForcesX": [ + 28.898022383948128, + 40.38279327621932, + 31.518367214748288, + 44.919632243477565 + ], + "moduleForcesY": [ + 63.33426630830682, + 56.72995559800382, + 61.95891551538514, + 53.10093359505647 + ], + "timestamp": 1.9487483263811076 + }, + { + "x": 2.0933736359860933, + "y": 4.858226801594099, + "heading": -0.2335644163554314, + "angularVelocity": 0.41583565145382007, + "velocityX": -0.850955654977675, + "velocityY": 1.6041123219358235, + "moduleForcesX": [ + 69.47635686741319, + 69.65947112210823, + 69.73585524619895, + 69.7189941107446 + ], + "moduleForcesY": [ + 6.227625887406924, + 4.078499983627095, + -1.256444304150436, + -2.7455509544386563 + ], + "timestamp": 2.0202207031067516 + }, + { + "x": 2.0511871628096467, + "y": 4.967985225687469, + "heading": -0.2022159861765387, + "angularVelocity": 0.4386090349181448, + "velocityX": -0.5902486393714047, + "velocityY": 1.53567614863841, + "moduleForcesX": [ + 67.96496385579776, + 67.99440402036385, + 67.10639875219877, + 67.1820726301549 + ], + "moduleForcesY": [ + -16.191122120647563, + -16.095511247494485, + -19.44670942058393, + -19.20731800544632 + ], + "timestamp": 2.0916930798323956 + }, + { + "x": 2.0270139823956215, + "y": 5.070867612388492, + "heading": -0.1700681728530796, + "angularVelocity": 0.4497935397741312, + "velocityX": -0.3382171104112806, + "velocityY": 1.439470621969348, + "moduleForcesX": [ + 65.5463060074924, + 65.64862612212953, + 64.96948366218025, + 65.09044905349134 + ], + "moduleForcesY": [ + -24.323259893322444, + -24.052061862241104, + -25.825731707590776, + -25.525206862715248 + ], + "timestamp": 2.1631654565580396 + }, + { + "x": 2.020448891699325, + "y": 5.165875707863209, + "heading": -0.13755294679311533, + "angularVelocity": 0.45493416547221555, + "velocityX": -0.09185493341902934, + "velocityY": 1.3292981154257553, + "moduleForcesX": [ + 63.950064201059824, + 64.03180591913006, + 63.654913960217954, + 63.74125341653127 + ], + "moduleForcesY": [ + -28.315148832203725, + -28.131653555528967, + -28.97311608059264, + -28.784456776293474 + ], + "timestamp": 2.2346378332836836 + }, + { + "x": 2.031218238194587, + "y": 5.2524144264055215, + "heading": -0.10493277773470358, + "angularVelocity": 0.45640246697905146, + "velocityX": 0.150678441370407, + "velocityY": 1.2107995068082167, + "moduleForcesX": [ + 62.88066267543294, + 62.91207596743938, + 62.79228260986751, + 62.82409534697334 + ], + "moduleForcesY": [ + -30.650558779807053, + -30.586409677611073, + -30.831354646388686, + -30.76685591760273 + ], + "timestamp": 2.3061102100093276 + }, + { + "x": 2.05912709236145, + "y": 5.330090045928955, + "heading": -0.072383245243066, + "angularVelocity": 0.45541416114660316, + "velocityX": 0.3904844851873833, + "velocityY": 1.086792170543661, + "moduleForcesX": [ + 62.127913109347276, + 62.10220705945469, + 62.18868836322343, + 62.16316779871923 + ], + "moduleForcesY": [ + -32.17057458836276, + -32.21997133029195, + -32.052845514471784, + -32.1021124146997 + ], + "timestamp": 2.3775825867349716 + }, + { + "x": 2.11398267592862, + "y": 5.407564590406167, + "heading": -0.035074691263133305, + "angularVelocity": 0.4522495337757732, + "velocityX": 0.6649523885483815, + "velocityY": 0.939136547950644, + "moduleForcesX": [ + 61.580309305924985, + 61.49718284699979, + 61.749856139779475, + 61.66816132300872 + ], + "moduleForcesY": [ + -33.23357506637483, + -33.386760848755074, + -32.91728042389969, + -33.06968489028737 + ], + "timestamp": 2.4600780924096304 + }, + { + "x": 2.191082677171206, + "y": 5.472153160220583, + "heading": 0.0016521215955720662, + "angularVelocity": 0.44519774208758894, + "velocityX": 0.9345963836882241, + "velocityY": 0.7829344083945624, + "moduleForcesX": [ + 60.45923369475699, + 60.23455621610159, + 60.84356535380034, + 60.625874690348816 + ], + "moduleForcesY": [ + -35.214733524291056, + -35.59671720906772, + -34.545862040838756, + -34.92548722348504 + ], + "timestamp": 2.542573598084289 + }, + { + "x": 2.289728178771266, + "y": 5.522730614286461, + "heading": 0.037276639055293805, + "angularVelocity": 0.4318358578252923, + "velocityX": 1.1957681909289004, + "velocityY": 0.6130934487194334, + "moduleForcesX": [ + 58.52744287678723, + 57.99177342881693, + 59.27377627441346, + 58.761492424372356 + ], + "moduleForcesY": [ + -38.31446039097551, + -39.11845718880393, + -37.14787466755693, + -37.95082764754945 + ], + "timestamp": 2.625069103758948 + }, + { + "x": 2.408421432901453, + "y": 5.557250524578989, + "heading": 0.07081951118626838, + "angularVelocity": 0.40660241860066865, + "velocityX": 1.4387844903195497, + "velocityY": 0.41844595069491913, + "moduleForcesX": [ + 54.50941828585798, + 53.13358487259185, + 55.95915631243375, + 54.647136092353065 + ], + "moduleForcesY": [ + -43.802094987807024, + -45.45697038399386, + -41.92981110097996, + -43.62112365277983 + ], + "timestamp": 2.7075646094336068 + }, + { + "x": 2.542474709173094, + "y": 5.571243535937252, + "heading": 0.09992456939909769, + "angularVelocity": 0.3528078041928978, + "velocityX": 1.6249767211345831, + "velocityY": 0.1696214981769599, + "moduleForcesX": [ + 42.694250355720705, + 38.054672270282296, + 45.591148182962975, + 40.87638058379842 + ], + "moduleForcesY": [ + -55.307949027779564, + -58.58896847735615, + -52.92949409903056, + -56.63909921586958 + ], + "timestamp": 2.7900601151082656 + }, + { + "x": 2.668629243751066, + "y": 5.561025654990008, + "heading": 0.11917705216572184, + "angularVelocity": 0.2333761410288788, + "velocityX": 1.529229180843634, + "velocityY": -0.12385985482443941, + "moduleForcesX": [ + -12.382402533780613, + -24.607511319028973, + -17.558309914075192, + -31.441206870964095 + ], + "moduleForcesY": [ + -68.63720397945411, + -65.28047130453552, + -67.43128061515037, + -62.222247817779476 + ], + "timestamp": 2.8725556207829244 + }, + { + "x": 2.7698521306860013, + "y": 5.54500330305322, + "heading": 0.13176004586708878, + "angularVelocity": 0.15252944507008542, + "velocityX": 1.2270109278923758, + "velocityY": -0.19422090026996, + "moduleForcesX": [ + -66.46018133159996, + -66.9674196584085, + -68.9644976388019, + -69.02558396413289 + ], + "moduleForcesY": [ + -21.47227954276657, + -19.910515932777027, + -11.024044579675353, + -10.78336011685823 + ], + "timestamp": 2.955051126457583 + }, + { + "x": 2.845436141122318, + "y": 5.530116306121047, + "heading": 0.14009236993377866, + "angularVelocity": 0.10100336980206083, + "velocityX": 0.916219735867702, + "velocityY": -0.1804582814458009, + "moduleForcesX": [ + -69.91668485679588, + -69.92377181874357, + -69.57042974483447, + -69.70602424237894 + ], + "moduleForcesY": [ + 0.35481804279217377, + -0.526848643787446, + 6.976161517175589, + 5.555870742457722 + ], + "timestamp": 3.037546632132242 + }, + { + "x": 2.8956151191054844, + "y": 5.519017848354159, + "heading": 0.14518444674969433, + "angularVelocity": 0.06172550582309565, + "velocityX": 0.6082631723848511, + "velocityY": -0.13453409128336982, + "moduleForcesX": [ + -69.4187453897527, + -69.56564259987941, + -68.65234610284632, + -68.9344391693208 + ], + "moduleForcesY": [ + 8.590706564201634, + 7.346324469918923, + 13.40936902595393, + 11.897426701626598 + ], + "timestamp": 3.1200421378069008 + }, + { + "x": 2.9206175804135657, + "y": 5.513061523434405, + "heading": 0.14756797074007869, + "angularVelocity": 0.02889277386548866, + "velocityX": 0.3030766476923993, + "velocityY": -0.07220180908862774, + "moduleForcesX": [ + -68.79757631258838, + -69.02339707894248, + -67.96227334834312, + -68.30019620815122 + ], + "moduleForcesY": [ + 12.723388654681191, + 11.450987875380186, + 16.62115678249245, + 15.184154939108327 + ], + "timestamp": 3.2025376434815596 + }, + { + "x": 2.9206175804138184, + "y": 5.5130615234375, + "heading": 0.14756797074007869, + "angularVelocity": -2.1366473717756725e-18, + "velocityX": 2.759428296475241e-16, + "velocityY": 1.8508305640470853e-16, + "moduleForcesX": [ + -68.30769120451006, + -68.57184176455583, + -67.47562728593, + -67.83343378701302 + ], + "moduleForcesY": [ + 15.178954859633443, + 13.946231227068052, + 18.53346910587473, + 17.184707214005112 + ], + "timestamp": 3.2850331491562184 + }, + { + "x": 2.9008988708903125, + "y": 5.5307170603623375, + "heading": 0.14700286177553085, + "angularVelocity": -0.006751511016409849, + "velocityX": -0.23558480387769798, + "velocityY": 0.2109355181352119, + "moduleForcesX": [ + -52.09999330596725, + -52.529400631239085, + -51.73320472903666, + -52.16512079541669 + ], + "moduleForcesY": [ + 46.71445570132471, + 46.23149173401103, + 47.1208354788092, + 46.6426390261249 + ], + "timestamp": 3.3687342589037015 + }, + { + "x": 2.861623494471227, + "y": 5.566204209893025, + "heading": 0.14583871741812887, + "angularVelocity": -0.013908350330240071, + "velocityX": -0.4692336401967019, + "velocityY": 0.42397465993079464, + "moduleForcesX": [ + -51.6659111374572, + -52.12885129412831, + -51.276879306050894, + -51.74245743341866 + ], + "moduleForcesY": [ + 47.185653590179186, + 46.67421839348704, + 47.60875054548638, + 47.10282037910355 + ], + "timestamp": 3.4524353686511846 + }, + { + "x": 2.803016239597012, + "y": 5.619762027969135, + "heading": 0.14402910636298827, + "angularVelocity": -0.021619917114522248, + "velocityX": -0.7001968676942122, + "velocityY": 0.6398698673768739, + "moduleForcesX": [ + -51.06312930480856, + -51.57332722665041, + -50.64375367097991, + -51.156724254575956 + ], + "moduleForcesY": [ + 47.8258847940151, + 47.27589622960445, + 48.27053972557859, + 47.72716661234478 + ], + "timestamp": 3.5361364783986677 + }, + { + "x": 2.725409512823363, + "y": 5.691733398122384, + "heading": 0.14150668290126167, + "angularVelocity": -0.030136081460894748, + "velocityX": -0.9271887436445178, + "velocityY": 0.8598615995220361, + "moduleForcesX": [ + -50.170225601037366, + -50.75200909732681, + -49.70741389665356, + -50.29204690115321 + ], + "moduleForcesY": [ + 48.74540511502207, + 48.14020759141128, + 49.218329972809045, + 48.621580569011485 + ], + "timestamp": 3.619837588146151 + }, + { + "x": 2.6293437718673225, + "y": 5.782650350616714, + "heading": 0.1381651310810073, + "angularVelocity": -0.039922431498496204, + "velocityX": -1.147723623091234, + "velocityY": 1.0862096426984997, + "moduleForcesX": [ + -48.71431630344826, + -49.4167391577109, + -48.18511463537286, + -48.890119594937715 + ], + "moduleForcesY": [ + 50.17522088948343, + 49.48473182662626, + 50.68521987830446, + 50.00663478484992 + ], + "timestamp": 3.703538697893634 + }, + { + "x": 2.5158455258965873, + "y": 5.893444520165239, + "heading": 0.13381239728196173, + "angularVelocity": -0.05200329854838183, + "velocityX": -1.355994518259599, + "velocityY": 1.3236881799931342, + "moduleForcesX": [ + -45.93506137526542, + -46.87946903712664, + -45.29576298382204, + -46.24053944561288 + ], + "moduleForcesY": [ + 52.687688259487246, + 51.851007353746446, + 53.24100600936717, + 52.42425460814814 + ], + "timestamp": 3.787239807641117 + }, + { + "x": 2.3875470683794213, + "y": 6.02610991590263, + "heading": 0.12800509474442898, + "angularVelocity": -0.06938142821559826, + "velocityX": -1.5328166842349815, + "velocityY": 1.5849896874494018, + "moduleForcesX": [ + -38.731601936491195, + -40.35076624830184, + -37.91551455677665, + -39.51613010702398 + ], + "moduleForcesY": [ + 58.094777198180715, + 56.98539695236494, + 58.63708159838333, + 57.57358536077538 + ], + "timestamp": 3.8709409173886002 + }, + { + "x": 2.2585637323391317, + "y": 6.185053475715637, + "heading": 0.11910747265558115, + "angularVelocity": -0.10630231923689437, + "velocityX": -1.5409991149465732, + "velocityY": 1.8989420845305607, + "moduleForcesX": [ + 0.7933919130137743, + -3.975029098028374, + 0.1994458839598195, + -4.26048134968995 + ], + "moduleForcesY": [ + 69.51699522375354, + 69.4045334427663, + 69.55309629363511, + 69.42011317516652 + ], + "timestamp": 3.9546420271360834 + }, + { + "x": 2.155822904626011, + "y": 6.341651289925754, + "heading": 0.10835395117931683, + "angularVelocity": -0.1284752556892115, + "velocityX": -1.2274727064325863, + "velocityY": 1.8709168195383514, + "moduleForcesX": [ + 69.29850135005749, + 69.22376482749928, + 69.52084242344242, + 69.4745660602554 + ], + "moduleForcesY": [ + -7.254272274145535, + -7.823192974180219, + -4.62731192687223, + -5.1017659452285775 + ], + "timestamp": 4.038343136883566 + }, + { + "x": 2.0772115888518736, + "y": 6.487483095533371, + "heading": 0.0969715087930836, + "angularVelocity": -0.13598914543167476, + "velocityX": -0.939190842449076, + "velocityY": 1.7422923808692223, + "moduleForcesX": [ + 63.72237766804347, + 63.53223394842242, + 64.05032016117235, + 63.867552477264695 + ], + "moduleForcesY": [ + -28.628204478983974, + -29.043494705306433, + -27.884225907046652, + -28.295904148066843 + ], + "timestamp": 4.122044246631049 + }, + { + "x": 2.021637442637373, + "y": 6.620330494187104, + "heading": 0.08532200026076654, + "angularVelocity": -0.1391798575606646, + "velocityX": -0.6639594908779467, + "velocityY": 1.587164125330234, + "moduleForcesX": [ + 60.87885944738181, + 60.7694927196997, + 61.04027054010648, + 60.93220988562005 + ], + "moduleForcesY": [ + -34.375983359242426, + -34.56803319978518, + -34.087899420795566, + -34.2797476711687 + ], + "timestamp": 4.205745356378531 + }, + { + "x": 1.9885261521674162, + "y": 6.739204165667878, + "heading": 0.07357290264571206, + "angularVelocity": -0.14036967550974822, + "velocityX": -0.39558962325593, + "velocityY": 1.4202161922459862, + "moduleForcesX": [ + 59.37783973737232, + 59.33213553424243, + 59.44147378924915, + 59.395945957037256 + ], + "moduleForcesY": [ + -36.95821427423069, + -37.031312892639825, + -36.85560981002678, + -36.92870573976187 + ], + "timestamp": 4.289446466126014 + }, + { + "x": 1.977528756045062, + "y": 6.843547542559394, + "heading": 0.061820288525356495, + "angularVelocity": -0.14041168815821406, + "velocityX": -0.13138889263033743, + "velocityY": 1.246618800486609, + "moduleForcesX": [ + 58.46396442972924, + 58.46225971251114, + 58.46628338376228, + 58.46457888275662 + ], + "moduleForcesY": [ + -38.41537453860932, + -38.417962696829065, + -38.41184043411793, + -38.41442860346031 + ], + "timestamp": 4.373147575873497 + }, + { + "x": 1.9884117094887013, + "y": 6.933004553123894, + "heading": 0.05012639395544798, + "angularVelocity": -0.13971014966457962, + "velocityX": 0.1300216147344071, + "velocityY": 1.0687673173254597, + "moduleForcesX": [ + 57.85196007751825, + 57.88125766772754, + 57.812373545441886, + 57.841731008840526 + ], + "moduleForcesY": [ + -39.3486892907348, + -39.30566232094738, + -39.40689132743343, + -39.363869274238354 + ], + "timestamp": 4.4568486856209795 + }, + { + "x": 2.021008014678955, + "y": 7.007328033447266, + "heading": 0.03853480959994491, + "angularVelocity": -0.13848782161284204, + "velocityX": 0.38943695354526725, + "velocityY": 0.8879629018104452, + "moduleForcesX": [ + 57.41445936428743, + 57.46628160975792, + 57.34427766903865, + 57.39628164773986 + ], + "moduleForcesY": [ + -39.99658250377327, + -39.922209282283085, + -40.09723139207876, + -40.02287498111711 + ], + "timestamp": 4.540549795368462 + }, + { + "x": 2.0826299228686898, + "y": 7.070370365951682, + "heading": 0.02599725344124828, + "angularVelocity": -0.1367266901870693, + "velocityX": 0.6720097158166497, + "velocityY": 0.687499968419269, + "moduleForcesX": [ + 57.08917572518026, + 57.15769997977353, + 56.9954356988548, + 57.06427617407771 + ], + "moduleForcesY": [ + -40.47369675811929, + -40.37699177913841, + -40.605690518380186, + -40.509013116235465 + ], + "timestamp": 4.632247737749421 + }, + { + "x": 2.169672617334707, + "y": 7.114364526461086, + "heading": 0.013740199626283799, + "angularVelocity": -0.13366770831190322, + "velocityX": 0.9492327984450655, + "velocityY": 0.4797726026430948, + "moduleForcesX": [ + 56.01669479138227, + 56.14220393478205, + 55.84946987132304, + 55.97590888149232 + ], + "moduleForcesY": [ + -41.93127449705145, + -41.76334146364884, + -42.153954194749936, + -41.98616963230991 + ], + "timestamp": 4.72394568013038 + }, + { + "x": 2.2812482301938513, + "y": 7.138190544722294, + "heading": 0.0019682714125729057, + "angularVelocity": -0.12837723408034055, + "velocityX": 1.2167733535710465, + "velocityY": 0.25983154852058693, + "moduleForcesX": [ + 54.06938340379889, + 54.308966898728, + 53.770349274089426, + 54.012513868838 + ], + "moduleForcesY": [ + -44.39082318658512, + -44.09795887662825, + -44.753051093939305, + -44.46103930411709 + ], + "timestamp": 4.815643622511339 + }, + { + "x": 2.4153058153444134, + "y": 7.139605579002843, + "heading": -0.008889495736468035, + "angularVelocity": -0.1184079693298827, + "velocityX": 1.4619475818218677, + "velocityY": 0.015431471712328844, + "moduleForcesX": [ + 49.535744448116404, + 50.08883535472126, + 48.95285722529716, + 49.512750306129256 + ], + "moduleForcesY": [ + -49.35481713361861, + -48.79483073718232, + -49.9344054264043, + -49.38065310701591 + ], + "timestamp": 4.907341564892298 + }, + { + "x": 2.5635950141167547, + "y": 7.1126996745657065, + "heading": -0.017499861469921024, + "angularVelocity": -0.0938992251069123, + "velocityX": 1.6171485962309224, + "velocityY": -0.29341884600255425, + "moduleForcesX": [ + 30.82355947672089, + 33.05783383581679, + 29.658555665503656, + 31.855767521726126 + ], + "moduleForcesY": [ + -62.655847172898895, + -61.51032510158116, + -63.223044950314765, + -62.148500220510925 + ], + "timestamp": 4.999039507273257 + }, + { + "x": 2.6834005581658715, + "y": 7.072012772379466, + "heading": -0.022151846620554527, + "angularVelocity": -0.05073161981418278, + "velocityX": 1.3065237989836214, + "velocityY": -0.4437057267013505, + "moduleForcesX": [ + -64.12298736950346, + -63.25910030393463, + -62.330306323818874, + -61.2590262700717 + ], + "moduleForcesY": [ + -27.440487603067957, + -29.33631206040231, + -31.317162411294717, + -33.33134662941462 + ], + "timestamp": 5.090737449654216 + }, + { + "x": 2.7720201720116067, + "y": 7.037183326012226, + "heading": -0.02500990952650958, + "angularVelocity": -0.031168233787252347, + "velocityX": 0.9664296880719148, + "velocityY": -0.37982800389550603, + "moduleForcesX": [ + -68.53351263579651, + -68.45472035856257, + -68.92469478793043, + -68.86842514528999 + ], + "moduleForcesY": [ + 13.79021249565681, + 14.160173532909322, + 11.675894673290463, + 11.9841604417936 + ], + "timestamp": 5.182435392035175 + }, + { + "x": 2.830552574824699, + "y": 7.012504794671367, + "heading": -0.026692143130144577, + "angularVelocity": -0.018345380059196448, + "velocityX": 0.6383175162541149, + "velocityY": -0.26912851727456555, + "moduleForcesX": [ + -66.1245923787409, + -65.98169477222959, + -66.56070394354207, + -66.43347254576503 + ], + "moduleForcesY": [ + 22.81538982238073, + 23.221752664345384, + 21.508695963392192, + 21.894565620883082 + ], + "timestamp": 5.274133334416134 + }, + { + "x": 2.8596270084381104, + "y": 6.999704360961915, + "heading": -0.027461342770395154, + "angularVelocity": -0.008388406765443529, + "velocityX": 0.31706745919666984, + "velocityY": -0.1395934675707037, + "moduleForcesX": [ + -64.76590607807921, + -64.6158756095251, + -65.15697508947473, + -65.01741451245994 + ], + "moduleForcesY": [ + 26.47522061163077, + 26.837605390916714, + 25.49694542258738, + 25.848954107534034 + ], + "timestamp": 5.365831276797093 + }, + { + "x": 2.8596270084381104, + "y": 6.999704360961914, + "heading": -0.02746134277039515, + "angularVelocity": 1.6056171256353062e-18, + "velocityX": -1.8304903305133357e-17, + "velocityY": -4.784339232160718e-18, + "moduleForcesX": [ + -63.93945199318899, + -63.793171888386496, + -64.29137064962106, + -64.1528183330455 + ], + "moduleForcesY": [ + 28.437823121296837, + 28.763444348886257, + 27.63256619914302, + 27.951659076114336 + ], + "timestamp": 5.457529219178052 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.4015021324157715, + "y": 5.563943386077881, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 1.3769693125759574, + "isStopPoint": true, + "x": 2.624587297439575, + "y": 4.260610580444336, + "heading": -0.350313284574424, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "timestamp": 2.3775825867349716, + "isStopPoint": false, + "x": 2.05912709236145, + "y": 5.330090045928955, + "heading": 0.052583458296069426, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 11 + }, + { + "timestamp": 3.2850331491562184, + "isStopPoint": true, + "x": 2.9206175804138184, + "y": 5.5130615234375, + "heading": 0.14756797074007866, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "timestamp": 4.540549795368462, + "isStopPoint": false, + "x": 2.021008014678955, + "y": 7.007328033447266, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 10 + }, + { + "timestamp": 5.457529219178052, + "isStopPoint": true, + "x": 2.8596270084381104, + "y": 6.999704360961914, + "heading": -0.027461342770395147, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 5 + ], + "type": "StopPoint" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false } }, "splitTrajectoriesAtStopPoints": true, diff --git a/src/main/deploy/choreo/3 center.1.traj b/src/main/deploy/choreo/3 center.1.traj new file mode 100644 index 00000000..3f2af776 --- /dev/null +++ b/src/main/deploy/choreo/3 center.1.traj @@ -0,0 +1,362 @@ +{ + "samples": [ + { + "x": 1.4015021324157717, + "y": 5.563943386077881, + "heading": 2.230586975435435e-18, + "angularVelocity": -4.993708780228108e-16, + "velocityX": 2.2186084345747425e-14, + "velocityY": -2.3644285540186748e-14, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ], + "timestamp": 0 + }, + { + "x": 1.4206212389589077, + "y": 5.543569823071968, + "heading": -0.005440740553276968, + "angularVelocity": -0.06321989027440103, + "velocityX": 0.22215869438518032, + "velocityY": -0.23673512918379025, + "moduleForcesX": [ + 47.940963091989595, + 43.779015007865034, + 51.71856759026114, + 47.81380065892401 + ], + "moduleForcesY": [ + -50.97920586080499, + -54.592033915440794, + -47.13866785517865, + -51.09102404890194 + ], + "timestamp": 0.08606058203599731 + }, + { + "x": 1.4588579982416516, + "y": 5.50282426541773, + "heading": -0.016323802947849745, + "angularVelocity": -0.12645815467526472, + "velocityX": 0.4443004964254837, + "velocityY": -0.4734520342542199, + "moduleForcesX": [ + 47.897364078006646, + 43.771363296078064, + 51.712153560329, + 47.85692298524638 + ], + "moduleForcesY": [ + -51.01338140036875, + -54.59132698392599, + -47.13783450766829, + -51.0426999647787 + ], + "timestamp": 0.17212116407199463 + }, + { + "x": 1.5162104171628858, + "y": 5.441708865073567, + "heading": -0.032652672648373536, + "angularVelocity": -0.18973691920489047, + "velocityX": 0.6664191378657773, + "velocityY": -0.7101439350063614, + "moduleForcesX": [ + 47.832382527825025, + 43.760444196177374, + 51.70298147300229, + 47.92205717348266 + ], + "moduleForcesY": [ + -51.06506924736401, + -54.59072757046299, + -47.13719675772626, + -50.9707235368353 + ], + "timestamp": 0.25818174610799194 + }, + { + "x": 1.5926756045431116, + "y": 5.360226735801809, + "heading": -0.05443308174346003, + "angularVelocity": -0.25308228900808055, + "velocityX": 0.8885041862345195, + "velocityY": -0.9467996533549011, + "moduleForcesX": [ + 47.74512096081025, + 43.74635348449958, + 51.68931736462727, + 48.00815391044375 + ], + "moduleForcesY": [ + -51.133333759147, + -54.588485401020556, + -47.13677120729667, + -50.87397800267328 + ], + "timestamp": 0.34424232814398925 + }, + { + "x": 1.6882490058265867, + "y": 5.258382764481358, + "heading": -0.08167367566723759, + "angularVelocity": -0.31652811634851347, + "velocityX": 1.1105363105122372, + "velocityY": -1.1833985890311751, + "moduleForcesX": [ + 47.63372466857229, + 43.72840022744862, + 51.668243433528644, + 48.113016067033115 + ], + "moduleForcesY": [ + -51.216216787013636, + -54.58157282722093, + -47.135721060383055, + -50.750174463810936 + ], + "timestamp": 0.43030291017998656 + }, + { + "x": 1.8029224178515728, + "y": 5.136185717697895, + "heading": -0.11438761196088189, + "angularVelocity": -0.3801268306536648, + "velocityX": 1.332473116574703, + "velocityY": -1.4198956586668827, + "moduleForcesX": [ + 47.49348736287193, + 43.70277282082887, + 51.63393088382914, + 48.231135604313586 + ], + "moduleForcesY": [ + -51.30877200765891, + -54.563746594487974, + -47.12995843648461, + -50.59351350853768 + ], + "timestamp": 0.5163634922159839 + }, + { + "x": 1.936676738585046, + "y": 4.9936559524582, + "heading": -0.1525981886270299, + "angularVelocity": -0.4439962612620674, + "velocityX": 1.5541879641384613, + "velocityY": -1.6561561857250287, + "moduleForcesX": [ + 47.30730481341717, + 43.65083702902553, + 51.569007669599586, + 48.34309714132929 + ], + "moduleForcesY": [ + -51.393363168260414, + -54.515933915159074, + -47.100223160042596, + -50.38283518179482 + ], + "timestamp": 0.6024240742519812 + }, + { + "x": 2.089417359472379, + "y": 4.830894184799883, + "heading": -0.19638882103609856, + "angularVelocity": -0.5088349552543711, + "velocityX": 1.7748034844634022, + "velocityY": -1.8912464178636816, + "moduleForcesX": [ + 46.92254775633583, + 43.388297546565106, + 51.333326631328006, + 48.27968404555044 + ], + "moduleForcesY": [ + -51.315242953431444, + -54.28258670469318, + -46.8588510495494, + -49.928189244812266 + ], + "timestamp": 0.6884846562879785 + }, + { + "x": 2.2231712411306024, + "y": 4.688364652999476, + "heading": -0.23481358247355089, + "angularVelocity": -0.4464850286666412, + "velocityX": 1.5541828604889207, + "velocityY": -1.6561534735218788, + "moduleForcesX": [ + -46.804641906327866, + -43.5737849154587, + -51.151878946250015, + -48.397943993853495 + ], + "moduleForcesY": [ + 51.411338017062235, + 54.12246522092687, + 47.04773571475752, + 49.80566610187949 + ], + "timestamp": 0.7745452383239758 + }, + { + "x": 2.3378438009906706, + "y": 4.5661682512486585, + "heading": -0.2677858501447284, + "angularVelocity": -0.383128569329872, + "velocityX": 1.3324632166436747, + "velocityY": -1.4198881637340148, + "moduleForcesX": [ + -46.8930408063945, + -43.75302961542786, + -51.4208655887188, + -48.80743987389535 + ], + "moduleForcesY": [ + 51.77006647658862, + 54.43088247512983, + 47.26280662177527, + 49.932717396293974 + ], + "timestamp": 0.8606058203599731 + }, + { + "x": 2.433416232746065, + "y": 4.4643250767817815, + "heading": -0.2952821236280862, + "angularVelocity": -0.3194990416382979, + "velocityX": 1.1105250446261894, + "velocityY": -1.1833893284845862, + "moduleForcesX": [ + -46.834172015543345, + -43.8038207106873, + -51.45084121327245, + -48.97366881140082 + ], + "moduleForcesY": [ + 51.91054751178551, + 54.480374777947134, + 47.331348947919075, + 49.87523949636651 + ], + "timestamp": 0.9466664023959704 + }, + { + "x": 2.5098805109805107, + "y": 4.382843730787292, + "heading": -0.3172895530520021, + "angularVelocity": -0.2557202020164514, + "velocityX": 0.888493622368971, + "velocityY": -0.9467905530509265, + "moduleForcesX": [ + -46.77185013607419, + -43.83510322729932, + -51.45045906865701, + -49.085367782981976 + ], + "moduleForcesY": [ + 52.003993622508574, + 54.49394996948038, + 47.37501122701898, + 49.81059264285791 + ], + "timestamp": 1.0327269844319678 + }, + { + "x": 2.5672321904712163, + "y": 4.3217289912490875, + "heading": -0.33380008105844566, + "angularVelocity": -0.1918477381378256, + "velocityX": 0.6664105458858951, + "velocityY": -0.7101362560644467, + "moduleForcesX": [ + -46.721839228792774, + -43.85751027569671, + -51.44391879316928, + -49.163980062749964 + ], + "moduleForcesY": [ + 52.06961690331074, + 54.49746975666322, + 47.406083494827506, + 49.75817477281888 + ], + "timestamp": 1.1187875664679652 + }, + { + "x": 2.6054684419481404, + "y": 4.280983903135222, + "heading": -0.3448087987822844, + "angularVelocity": -0.12791823461321336, + "velocityX": 0.444294595953872, + "velocityY": -0.47344657843923665, + "moduleForcesX": [ + -46.68832390905574, + -43.873154666936216, + -51.438399514205486, + -49.21567045651427 + ], + "moduleForcesY": [ + 52.11282780371421, + 54.49861617678228, + 47.42729008575955, + 49.72306941004055 + ], + "timestamp": 1.2048481485039626 + }, + { + "x": 2.6245872974433224, + "y": 4.260610580440736, + "heading": -0.3503132845744241, + "angularVelocity": -0.06396059220042477, + "velocityX": 0.2221557770547791, + "velocityY": -0.23673233693882528, + "moduleForcesX": [ + -46.672848976425186, + -43.88250079839491, + -51.436915163186654, + -49.242971039133984 + ], + "moduleForcesY": [ + 52.135797056459324, + 54.500625069799426, + 47.43940924267183, + 49.70711869156378 + ], + "timestamp": 1.29090873053996 + }, + { + "x": 2.624587297439575, + "y": 4.260610580444336, + "heading": -0.350313284574424, + "angularVelocity": 4.86945709754994e-16, + "velocityX": -2.1781193472847508e-14, + "velocityY": 2.3655888321777277e-14, + "moduleForcesX": [ + -46.676116985699565, + -43.88573472382722, + -51.440890855557896, + -49.24709237978015 + ], + "moduleForcesY": [ + 52.139555439371115, + 54.50503144060741, + 47.44278460160171, + 49.711156539011256 + ], + "timestamp": 1.3769693125759574 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3 center.2.traj b/src/main/deploy/choreo/3 center.2.traj new file mode 100644 index 00000000..209de89b --- /dev/null +++ b/src/main/deploy/choreo/3 center.2.traj @@ -0,0 +1,551 @@ +{ + "samples": [ + { + "x": 2.624587297439575, + "y": 4.260610580444336, + "heading": -0.350313284574424, + "angularVelocity": 4.86945709754994e-16, + "velocityX": -2.1781193472847508e-14, + "velocityY": 2.3655888321777277e-14, + "moduleForcesX": [ + -46.676116985699565, + -43.88573472382722, + -51.440890855557896, + -49.24709237978015 + ], + "moduleForcesY": [ + 52.139555439371115, + 54.50503144060741, + 47.44278460160171, + 49.711156539011256 + ], + "timestamp": 0 + }, + { + "x": 2.6097025403261815, + "y": 4.272883049188608, + "heading": -0.3486185943542768, + "angularVelocity": 0.023711121663719633, + "velocityX": -0.20825887874704344, + "velocityY": 0.17170925805626883, + "moduleForcesX": [ + -53.325527820192114, + -52.543899565580986, + -55.33489027006052, + -54.67599479757407 + ], + "moduleForcesY": [ + 45.30344546948554, + 46.204887640771894, + 42.82491499809031, + 43.659875176737565 + ], + "timestamp": 0.07147237672564377 + }, + { + "x": 2.580134116922562, + "y": 4.297663630247455, + "heading": -0.3451094282478584, + "angularVelocity": 0.04909821482314941, + "velocityX": -0.4137042155527122, + "velocityY": 0.34671550166182774, + "moduleForcesX": [ + -52.56862851207183, + -51.668428542826135, + -54.74294626604449, + -53.98380292140949 + ], + "moduleForcesY": [ + 46.169623323178776, + 47.17145541851404, + 43.5681756135052, + 44.50151083261666 + ], + "timestamp": 0.14294475345128754 + }, + { + "x": 2.5361681561986593, + "y": 4.335274736263326, + "heading": -0.3396203018012238, + "angularVelocity": 0.07680067038585842, + "velocityX": -0.6151461937353943, + "velocityY": 0.5262327598304285, + "moduleForcesX": [ + -51.50047411154375, + -50.41891834157215, + -53.90421586456722, + -52.99033254736792 + ], + "moduleForcesY": [ + 47.344772853066466, + 48.49067038940593, + 44.586953730344504, + 45.66446768873032 + ], + "timestamp": 0.21441713017693131 + }, + { + "x": 2.4782427587314393, + "y": 4.386182986369333, + "heading": -0.3319081747833718, + "angularVelocity": 0.10790360375738635, + "velocityX": -0.810458532923371, + "velocityY": 0.7122786797293141, + "moduleForcesX": [ + -49.88208352874225, + -48.50022068908951, + -52.625503387992744, + -51.45217423757492 + ], + "moduleForcesY": [ + 49.02799707238364, + 50.389801569506666, + 46.06811701007498, + 47.36853338734092 + ], + "timestamp": 0.2858895069025751 + }, + { + "x": 2.407106964769289, + "y": 4.45111875799722, + "heading": -0.321583297914309, + "angularVelocity": 0.14445968277546212, + "velocityX": -0.9952907323070566, + "velocityY": 0.9085436161792976, + "moduleForcesX": [ + -47.15780192777166, + -45.2126943325825, + -50.44841035705035, + -48.77740463936025 + ], + "moduleForcesY": [ + 51.624929028521905, + 53.32888659245528, + 48.40981741843909, + 50.08380727693866 + ], + "timestamp": 0.35736188362821886 + }, + { + "x": 2.3242974518893162, + "y": 4.531358566520759, + "heading": -0.3079313440530546, + "angularVelocity": 0.1910102124288775, + "velocityX": -1.158622623127807, + "velocityY": 1.1226688178584086, + "moduleForcesX": [ + -41.72003068858196, + -38.48311349131453, + -45.98995227278055, + -43.116081797134584 + ], + "moduleForcesY": [ + 56.06234348401295, + 58.3198188676031, + 52.60707005821105, + 54.9720866974948 + ], + "timestamp": 0.42883426035386263 + }, + { + "x": 2.2342218638275644, + "y": 4.629362025502949, + "heading": -0.2893536832656072, + "angularVelocity": 0.25992784399271474, + "velocityX": -1.260285334646795, + "velocityY": 1.3712074961147915, + "moduleForcesX": [ + -27.000280396790707, + -19.781400792593253, + -32.92142671337878, + -25.680048193575647 + ], + "moduleForcesY": [ + 64.36040574604013, + 66.91361057866158, + 61.51512045582401, + 64.8450531339154 + ], + "timestamp": 0.5003066370795064 + }, + { + "x": 2.1541934591617964, + "y": 4.743577081279941, + "heading": -0.2632851786920285, + "angularVelocity": 0.3647353812470825, + "velocityX": -1.119710973304069, + "velocityY": 1.598030752209193, + "moduleForcesX": [ + 28.898022383948128, + 40.38279327621932, + 31.518367214748288, + 44.919632243477565 + ], + "moduleForcesY": [ + 63.33426630830682, + 56.72995559800382, + 61.95891551538514, + 53.10093359505647 + ], + "timestamp": 0.5717790138051502 + }, + { + "x": 2.0933736359860933, + "y": 4.858226801594099, + "heading": -0.2335644163554314, + "angularVelocity": 0.41583565145382007, + "velocityX": -0.850955654977675, + "velocityY": 1.6041123219358235, + "moduleForcesX": [ + 69.47635686741319, + 69.65947112210823, + 69.73585524619895, + 69.7189941107446 + ], + "moduleForcesY": [ + 6.227625887406924, + 4.078499983627095, + -1.256444304150436, + -2.7455509544386563 + ], + "timestamp": 0.6432513905307942 + }, + { + "x": 2.0511871628096467, + "y": 4.967985225687469, + "heading": -0.2022159861765387, + "angularVelocity": 0.4386090349181448, + "velocityX": -0.5902486393714047, + "velocityY": 1.53567614863841, + "moduleForcesX": [ + 67.96496385579776, + 67.99440402036385, + 67.10639875219877, + 67.1820726301549 + ], + "moduleForcesY": [ + -16.191122120647563, + -16.095511247494485, + -19.44670942058393, + -19.20731800544632 + ], + "timestamp": 0.7147237672564382 + }, + { + "x": 2.0270139823956215, + "y": 5.070867612388492, + "heading": -0.1700681728530796, + "angularVelocity": 0.4497935397741312, + "velocityX": -0.3382171104112806, + "velocityY": 1.439470621969348, + "moduleForcesX": [ + 65.5463060074924, + 65.64862612212953, + 64.96948366218025, + 65.09044905349134 + ], + "moduleForcesY": [ + -24.323259893322444, + -24.052061862241104, + -25.825731707590776, + -25.525206862715248 + ], + "timestamp": 0.7861961439820822 + }, + { + "x": 2.020448891699325, + "y": 5.165875707863209, + "heading": -0.13755294679311533, + "angularVelocity": 0.45493416547221555, + "velocityX": -0.09185493341902934, + "velocityY": 1.3292981154257553, + "moduleForcesX": [ + 63.950064201059824, + 64.03180591913006, + 63.654913960217954, + 63.74125341653127 + ], + "moduleForcesY": [ + -28.315148832203725, + -28.131653555528967, + -28.97311608059264, + -28.784456776293474 + ], + "timestamp": 0.8576685207077261 + }, + { + "x": 2.031218238194587, + "y": 5.2524144264055215, + "heading": -0.10493277773470358, + "angularVelocity": 0.45640246697905146, + "velocityX": 0.150678441370407, + "velocityY": 1.2107995068082167, + "moduleForcesX": [ + 62.88066267543294, + 62.91207596743938, + 62.79228260986751, + 62.82409534697334 + ], + "moduleForcesY": [ + -30.650558779807053, + -30.586409677611073, + -30.831354646388686, + -30.76685591760273 + ], + "timestamp": 0.9291408974333701 + }, + { + "x": 2.05912709236145, + "y": 5.330090045928955, + "heading": -0.072383245243066, + "angularVelocity": 0.45541416114660316, + "velocityX": 0.3904844851873833, + "velocityY": 1.086792170543661, + "moduleForcesX": [ + 62.127913109347276, + 62.10220705945469, + 62.18868836322343, + 62.16316779871923 + ], + "moduleForcesY": [ + -32.17057458836276, + -32.21997133029195, + -32.052845514471784, + -32.1021124146997 + ], + "timestamp": 1.0006132741590141 + }, + { + "x": 2.11398267592862, + "y": 5.407564590406167, + "heading": -0.035074691263133305, + "angularVelocity": 0.4522495337757732, + "velocityX": 0.6649523885483815, + "velocityY": 0.939136547950644, + "moduleForcesX": [ + 61.580309305924985, + 61.49718284699979, + 61.749856139779475, + 61.66816132300872 + ], + "moduleForcesY": [ + -33.23357506637483, + -33.386760848755074, + -32.91728042389969, + -33.06968489028737 + ], + "timestamp": 1.083108779833673 + }, + { + "x": 2.191082677171206, + "y": 5.472153160220583, + "heading": 0.0016521215955720662, + "angularVelocity": 0.44519774208758894, + "velocityX": 0.9345963836882241, + "velocityY": 0.7829344083945624, + "moduleForcesX": [ + 60.45923369475699, + 60.23455621610159, + 60.84356535380034, + 60.625874690348816 + ], + "moduleForcesY": [ + -35.214733524291056, + -35.59671720906772, + -34.545862040838756, + -34.92548722348504 + ], + "timestamp": 1.1656042855083317 + }, + { + "x": 2.289728178771266, + "y": 5.522730614286461, + "heading": 0.037276639055293805, + "angularVelocity": 0.4318358578252923, + "velocityX": 1.1957681909289004, + "velocityY": 0.6130934487194334, + "moduleForcesX": [ + 58.52744287678723, + 57.99177342881693, + 59.27377627441346, + 58.761492424372356 + ], + "moduleForcesY": [ + -38.31446039097551, + -39.11845718880393, + -37.14787466755693, + -37.95082764754945 + ], + "timestamp": 1.2480997911829905 + }, + { + "x": 2.408421432901453, + "y": 5.557250524578989, + "heading": 0.07081951118626838, + "angularVelocity": 0.40660241860066865, + "velocityX": 1.4387844903195497, + "velocityY": 0.41844595069491913, + "moduleForcesX": [ + 54.50941828585798, + 53.13358487259185, + 55.95915631243375, + 54.647136092353065 + ], + "moduleForcesY": [ + -43.802094987807024, + -45.45697038399386, + -41.92981110097996, + -43.62112365277983 + ], + "timestamp": 1.3305952968576493 + }, + { + "x": 2.542474709173094, + "y": 5.571243535937252, + "heading": 0.09992456939909769, + "angularVelocity": 0.3528078041928978, + "velocityX": 1.6249767211345831, + "velocityY": 0.1696214981769599, + "moduleForcesX": [ + 42.694250355720705, + 38.054672270282296, + 45.591148182962975, + 40.87638058379842 + ], + "moduleForcesY": [ + -55.307949027779564, + -58.58896847735615, + -52.92949409903056, + -56.63909921586958 + ], + "timestamp": 1.4130908025323081 + }, + { + "x": 2.668629243751066, + "y": 5.561025654990008, + "heading": 0.11917705216572184, + "angularVelocity": 0.2333761410288788, + "velocityX": 1.529229180843634, + "velocityY": -0.12385985482443941, + "moduleForcesX": [ + -12.382402533780613, + -24.607511319028973, + -17.558309914075192, + -31.441206870964095 + ], + "moduleForcesY": [ + -68.63720397945411, + -65.28047130453552, + -67.43128061515037, + -62.222247817779476 + ], + "timestamp": 1.495586308206967 + }, + { + "x": 2.7698521306860013, + "y": 5.54500330305322, + "heading": 0.13176004586708878, + "angularVelocity": 0.15252944507008542, + "velocityX": 1.2270109278923758, + "velocityY": -0.19422090026996, + "moduleForcesX": [ + -66.46018133159996, + -66.9674196584085, + -68.9644976388019, + -69.02558396413289 + ], + "moduleForcesY": [ + -21.47227954276657, + -19.910515932777027, + -11.024044579675353, + -10.78336011685823 + ], + "timestamp": 1.5780818138816257 + }, + { + "x": 2.845436141122318, + "y": 5.530116306121047, + "heading": 0.14009236993377866, + "angularVelocity": 0.10100336980206083, + "velocityX": 0.916219735867702, + "velocityY": -0.1804582814458009, + "moduleForcesX": [ + -69.91668485679588, + -69.92377181874357, + -69.57042974483447, + -69.70602424237894 + ], + "moduleForcesY": [ + 0.35481804279217377, + -0.526848643787446, + 6.976161517175589, + 5.555870742457722 + ], + "timestamp": 1.6605773195562845 + }, + { + "x": 2.8956151191054844, + "y": 5.519017848354159, + "heading": 0.14518444674969433, + "angularVelocity": 0.06172550582309565, + "velocityX": 0.6082631723848511, + "velocityY": -0.13453409128336982, + "moduleForcesX": [ + -69.4187453897527, + -69.56564259987941, + -68.65234610284632, + -68.9344391693208 + ], + "moduleForcesY": [ + 8.590706564201634, + 7.346324469918923, + 13.40936902595393, + 11.897426701626598 + ], + "timestamp": 1.7430728252309433 + }, + { + "x": 2.9206175804135657, + "y": 5.513061523434405, + "heading": 0.14756797074007869, + "angularVelocity": 0.02889277386548866, + "velocityX": 0.3030766476923993, + "velocityY": -0.07220180908862774, + "moduleForcesX": [ + -68.79757631258838, + -69.02339707894248, + -67.96227334834312, + -68.30019620815122 + ], + "moduleForcesY": [ + 12.723388654681191, + 11.450987875380186, + 16.62115678249245, + 15.184154939108327 + ], + "timestamp": 1.825568330905602 + }, + { + "x": 2.9206175804138184, + "y": 5.5130615234375, + "heading": 0.14756797074007869, + "angularVelocity": -2.1366473717756725e-18, + "velocityX": 2.759428296475241e-16, + "velocityY": 1.8508305640470853e-16, + "moduleForcesX": [ + -68.30769120451006, + -68.57184176455583, + -67.47562728593, + -67.83343378701302 + ], + "moduleForcesY": [ + 15.178954859633443, + 13.946231227068052, + 18.53346910587473, + 17.184707214005112 + ], + "timestamp": 1.908063836580261 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3 center.3.traj b/src/main/deploy/choreo/3 center.3.traj new file mode 100644 index 00000000..f2ffde16 --- /dev/null +++ b/src/main/deploy/choreo/3 center.3.traj @@ -0,0 +1,551 @@ +{ + "samples": [ + { + "x": 2.9206175804138184, + "y": 5.5130615234375, + "heading": 0.14756797074007869, + "angularVelocity": -2.1366473717756725e-18, + "velocityX": 2.759428296475241e-16, + "velocityY": 1.8508305640470853e-16, + "moduleForcesX": [ + -68.30769120451006, + -68.57184176455583, + -67.47562728593, + -67.83343378701302 + ], + "moduleForcesY": [ + 15.178954859633443, + 13.946231227068052, + 18.53346910587473, + 17.184707214005112 + ], + "timestamp": 0 + }, + { + "x": 2.9008988708903125, + "y": 5.5307170603623375, + "heading": 0.14700286177553085, + "angularVelocity": -0.006751511016409849, + "velocityX": -0.23558480387769798, + "velocityY": 0.2109355181352119, + "moduleForcesX": [ + -52.09999330596725, + -52.529400631239085, + -51.73320472903666, + -52.16512079541669 + ], + "moduleForcesY": [ + 46.71445570132471, + 46.23149173401103, + 47.1208354788092, + 46.6426390261249 + ], + "timestamp": 0.08370110974748313 + }, + { + "x": 2.861623494471227, + "y": 5.566204209893025, + "heading": 0.14583871741812887, + "angularVelocity": -0.013908350330240071, + "velocityX": -0.4692336401967019, + "velocityY": 0.42397465993079464, + "moduleForcesX": [ + -51.6659111374572, + -52.12885129412831, + -51.276879306050894, + -51.74245743341866 + ], + "moduleForcesY": [ + 47.185653590179186, + 46.67421839348704, + 47.60875054548638, + 47.10282037910355 + ], + "timestamp": 0.16740221949496625 + }, + { + "x": 2.803016239597012, + "y": 5.619762027969135, + "heading": 0.14402910636298827, + "angularVelocity": -0.021619917114522248, + "velocityX": -0.7001968676942122, + "velocityY": 0.6398698673768739, + "moduleForcesX": [ + -51.06312930480856, + -51.57332722665041, + -50.64375367097991, + -51.156724254575956 + ], + "moduleForcesY": [ + 47.8258847940151, + 47.27589622960445, + 48.27053972557859, + 47.72716661234478 + ], + "timestamp": 0.2511033292424494 + }, + { + "x": 2.725409512823363, + "y": 5.691733398122384, + "heading": 0.14150668290126167, + "angularVelocity": -0.030136081460894748, + "velocityX": -0.9271887436445178, + "velocityY": 0.8598615995220361, + "moduleForcesX": [ + -50.170225601037366, + -50.75200909732681, + -49.70741389665356, + -50.29204690115321 + ], + "moduleForcesY": [ + 48.74540511502207, + 48.14020759141128, + 49.218329972809045, + 48.621580569011485 + ], + "timestamp": 0.3348044389899325 + }, + { + "x": 2.6293437718673225, + "y": 5.782650350616714, + "heading": 0.1381651310810073, + "angularVelocity": -0.039922431498496204, + "velocityX": -1.147723623091234, + "velocityY": 1.0862096426984997, + "moduleForcesX": [ + -48.71431630344826, + -49.4167391577109, + -48.18511463537286, + -48.890119594937715 + ], + "moduleForcesY": [ + 50.17522088948343, + 49.48473182662626, + 50.68521987830446, + 50.00663478484992 + ], + "timestamp": 0.41850554873741563 + }, + { + "x": 2.5158455258965873, + "y": 5.893444520165239, + "heading": 0.13381239728196173, + "angularVelocity": -0.05200329854838183, + "velocityX": -1.355994518259599, + "velocityY": 1.3236881799931342, + "moduleForcesX": [ + -45.93506137526542, + -46.87946903712664, + -45.29576298382204, + -46.24053944561288 + ], + "moduleForcesY": [ + 52.687688259487246, + 51.851007353746446, + 53.24100600936717, + 52.42425460814814 + ], + "timestamp": 0.5022066584848988 + }, + { + "x": 2.3875470683794213, + "y": 6.02610991590263, + "heading": 0.12800509474442898, + "angularVelocity": -0.06938142821559826, + "velocityX": -1.5328166842349815, + "velocityY": 1.5849896874494018, + "moduleForcesX": [ + -38.731601936491195, + -40.35076624830184, + -37.91551455677665, + -39.51613010702398 + ], + "moduleForcesY": [ + 58.094777198180715, + 56.98539695236494, + 58.63708159838333, + 57.57358536077538 + ], + "timestamp": 0.5859077682323819 + }, + { + "x": 2.2585637323391317, + "y": 6.185053475715637, + "heading": 0.11910747265558115, + "angularVelocity": -0.10630231923689437, + "velocityX": -1.5409991149465732, + "velocityY": 1.8989420845305607, + "moduleForcesX": [ + 0.7933919130137743, + -3.975029098028374, + 0.1994458839598195, + -4.26048134968995 + ], + "moduleForcesY": [ + 69.51699522375354, + 69.4045334427663, + 69.55309629363511, + 69.42011317516652 + ], + "timestamp": 0.669608877979865 + }, + { + "x": 2.155822904626011, + "y": 6.341651289925754, + "heading": 0.10835395117931683, + "angularVelocity": -0.1284752556892115, + "velocityX": -1.2274727064325863, + "velocityY": 1.8709168195383514, + "moduleForcesX": [ + 69.29850135005749, + 69.22376482749928, + 69.52084242344242, + 69.4745660602554 + ], + "moduleForcesY": [ + -7.254272274145535, + -7.823192974180219, + -4.62731192687223, + -5.1017659452285775 + ], + "timestamp": 0.7533099877273477 + }, + { + "x": 2.0772115888518736, + "y": 6.487483095533371, + "heading": 0.0969715087930836, + "angularVelocity": -0.13598914543167476, + "velocityX": -0.939190842449076, + "velocityY": 1.7422923808692223, + "moduleForcesX": [ + 63.72237766804347, + 63.53223394842242, + 64.05032016117235, + 63.867552477264695 + ], + "moduleForcesY": [ + -28.628204478983974, + -29.043494705306433, + -27.884225907046652, + -28.295904148066843 + ], + "timestamp": 0.8370110974748304 + }, + { + "x": 2.021637442637373, + "y": 6.620330494187104, + "heading": 0.08532200026076654, + "angularVelocity": -0.1391798575606646, + "velocityX": -0.6639594908779467, + "velocityY": 1.587164125330234, + "moduleForcesX": [ + 60.87885944738181, + 60.7694927196997, + 61.04027054010648, + 60.93220988562005 + ], + "moduleForcesY": [ + -34.375983359242426, + -34.56803319978518, + -34.087899420795566, + -34.2797476711687 + ], + "timestamp": 0.9207122072223131 + }, + { + "x": 1.9885261521674162, + "y": 6.739204165667878, + "heading": 0.07357290264571206, + "angularVelocity": -0.14036967550974822, + "velocityX": -0.39558962325593, + "velocityY": 1.4202161922459862, + "moduleForcesX": [ + 59.37783973737232, + 59.33213553424243, + 59.44147378924915, + 59.395945957037256 + ], + "moduleForcesY": [ + -36.95821427423069, + -37.031312892639825, + -36.85560981002678, + -36.92870573976187 + ], + "timestamp": 1.0044133169697957 + }, + { + "x": 1.977528756045062, + "y": 6.843547542559394, + "heading": 0.061820288525356495, + "angularVelocity": -0.14041168815821406, + "velocityX": -0.13138889263033743, + "velocityY": 1.246618800486609, + "moduleForcesX": [ + 58.46396442972924, + 58.46225971251114, + 58.46628338376228, + 58.46457888275662 + ], + "moduleForcesY": [ + -38.41537453860932, + -38.417962696829065, + -38.41184043411793, + -38.41442860346031 + ], + "timestamp": 1.0881144267172784 + }, + { + "x": 1.9884117094887013, + "y": 6.933004553123894, + "heading": 0.05012639395544798, + "angularVelocity": -0.13971014966457962, + "velocityX": 0.1300216147344071, + "velocityY": 1.0687673173254597, + "moduleForcesX": [ + 57.85196007751825, + 57.88125766772754, + 57.812373545441886, + 57.841731008840526 + ], + "moduleForcesY": [ + -39.3486892907348, + -39.30566232094738, + -39.40689132743343, + -39.363869274238354 + ], + "timestamp": 1.171815536464761 + }, + { + "x": 2.021008014678955, + "y": 7.007328033447266, + "heading": 0.03853480959994491, + "angularVelocity": -0.13848782161284204, + "velocityX": 0.38943695354526725, + "velocityY": 0.8879629018104452, + "moduleForcesX": [ + 57.41445936428743, + 57.46628160975792, + 57.34427766903865, + 57.39628164773986 + ], + "moduleForcesY": [ + -39.99658250377327, + -39.922209282283085, + -40.09723139207876, + -40.02287498111711 + ], + "timestamp": 1.2555166462122438 + }, + { + "x": 2.0826299228686898, + "y": 7.070370365951682, + "heading": 0.02599725344124828, + "angularVelocity": -0.1367266901870693, + "velocityX": 0.6720097158166497, + "velocityY": 0.687499968419269, + "moduleForcesX": [ + 57.08917572518026, + 57.15769997977353, + 56.9954356988548, + 57.06427617407771 + ], + "moduleForcesY": [ + -40.47369675811929, + -40.37699177913841, + -40.605690518380186, + -40.509013116235465 + ], + "timestamp": 1.3472145885932028 + }, + { + "x": 2.169672617334707, + "y": 7.114364526461086, + "heading": 0.013740199626283799, + "angularVelocity": -0.13366770831190322, + "velocityX": 0.9492327984450655, + "velocityY": 0.4797726026430948, + "moduleForcesX": [ + 56.01669479138227, + 56.14220393478205, + 55.84946987132304, + 55.97590888149232 + ], + "moduleForcesY": [ + -41.93127449705145, + -41.76334146364884, + -42.153954194749936, + -41.98616963230991 + ], + "timestamp": 1.4389125309741617 + }, + { + "x": 2.2812482301938513, + "y": 7.138190544722294, + "heading": 0.0019682714125729057, + "angularVelocity": -0.12837723408034055, + "velocityX": 1.2167733535710465, + "velocityY": 0.25983154852058693, + "moduleForcesX": [ + 54.06938340379889, + 54.308966898728, + 53.770349274089426, + 54.012513868838 + ], + "moduleForcesY": [ + -44.39082318658512, + -44.09795887662825, + -44.753051093939305, + -44.46103930411709 + ], + "timestamp": 1.5306104733551207 + }, + { + "x": 2.4153058153444134, + "y": 7.139605579002843, + "heading": -0.008889495736468035, + "angularVelocity": -0.1184079693298827, + "velocityX": 1.4619475818218677, + "velocityY": 0.015431471712328844, + "moduleForcesX": [ + 49.535744448116404, + 50.08883535472126, + 48.95285722529716, + 49.512750306129256 + ], + "moduleForcesY": [ + -49.35481713361861, + -48.79483073718232, + -49.9344054264043, + -49.38065310701591 + ], + "timestamp": 1.6223084157360796 + }, + { + "x": 2.5635950141167547, + "y": 7.1126996745657065, + "heading": -0.017499861469921024, + "angularVelocity": -0.0938992251069123, + "velocityX": 1.6171485962309224, + "velocityY": -0.29341884600255425, + "moduleForcesX": [ + 30.82355947672089, + 33.05783383581679, + 29.658555665503656, + 31.855767521726126 + ], + "moduleForcesY": [ + -62.655847172898895, + -61.51032510158116, + -63.223044950314765, + -62.148500220510925 + ], + "timestamp": 1.7140063581170386 + }, + { + "x": 2.6834005581658715, + "y": 7.072012772379466, + "heading": -0.022151846620554527, + "angularVelocity": -0.05073161981418278, + "velocityX": 1.3065237989836214, + "velocityY": -0.4437057267013505, + "moduleForcesX": [ + -64.12298736950346, + -63.25910030393463, + -62.330306323818874, + -61.2590262700717 + ], + "moduleForcesY": [ + -27.440487603067957, + -29.33631206040231, + -31.317162411294717, + -33.33134662941462 + ], + "timestamp": 1.8057043004979976 + }, + { + "x": 2.7720201720116067, + "y": 7.037183326012226, + "heading": -0.02500990952650958, + "angularVelocity": -0.031168233787252347, + "velocityX": 0.9664296880719148, + "velocityY": -0.37982800389550603, + "moduleForcesX": [ + -68.53351263579651, + -68.45472035856257, + -68.92469478793043, + -68.86842514528999 + ], + "moduleForcesY": [ + 13.79021249565681, + 14.160173532909322, + 11.675894673290463, + 11.9841604417936 + ], + "timestamp": 1.8974022428789565 + }, + { + "x": 2.830552574824699, + "y": 7.012504794671367, + "heading": -0.026692143130144577, + "angularVelocity": -0.018345380059196448, + "velocityX": 0.6383175162541149, + "velocityY": -0.26912851727456555, + "moduleForcesX": [ + -66.1245923787409, + -65.98169477222959, + -66.56070394354207, + -66.43347254576503 + ], + "moduleForcesY": [ + 22.81538982238073, + 23.221752664345384, + 21.508695963392192, + 21.894565620883082 + ], + "timestamp": 1.9891001852599155 + }, + { + "x": 2.8596270084381104, + "y": 6.999704360961915, + "heading": -0.027461342770395154, + "angularVelocity": -0.008388406765443529, + "velocityX": 0.31706745919666984, + "velocityY": -0.1395934675707037, + "moduleForcesX": [ + -64.76590607807921, + -64.6158756095251, + -65.15697508947473, + -65.01741451245994 + ], + "moduleForcesY": [ + 26.47522061163077, + 26.837605390916714, + 25.49694542258738, + 25.848954107534034 + ], + "timestamp": 2.0807981276408745 + }, + { + "x": 2.8596270084381104, + "y": 6.999704360961914, + "heading": -0.02746134277039515, + "angularVelocity": 1.6056171256353062e-18, + "velocityX": -1.8304903305133357e-17, + "velocityY": -4.784339232160718e-18, + "moduleForcesX": [ + -63.93945199318899, + -63.793171888386496, + -64.29137064962106, + -64.1528183330455 + ], + "moduleForcesY": [ + 28.437823121296837, + 28.763444348886257, + 27.63256619914302, + 27.951659076114336 + ], + "timestamp": 2.1724960700218334 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3 center.traj b/src/main/deploy/choreo/3 center.traj new file mode 100644 index 00000000..c99f5997 --- /dev/null +++ b/src/main/deploy/choreo/3 center.traj @@ -0,0 +1,1412 @@ +{ + "samples": [ + { + "x": 1.4015021324157717, + "y": 5.563943386077881, + "heading": 2.230586975435435e-18, + "angularVelocity": -4.993708780228108e-16, + "velocityX": 2.2186084345747425e-14, + "velocityY": -2.3644285540186748e-14, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ], + "timestamp": 0 + }, + { + "x": 1.4206212389589077, + "y": 5.543569823071968, + "heading": -0.005440740553276968, + "angularVelocity": -0.06321989027440103, + "velocityX": 0.22215869438518032, + "velocityY": -0.23673512918379025, + "moduleForcesX": [ + 47.940963091989595, + 43.779015007865034, + 51.71856759026114, + 47.81380065892401 + ], + "moduleForcesY": [ + -50.97920586080499, + -54.592033915440794, + -47.13866785517865, + -51.09102404890194 + ], + "timestamp": 0.08606058203599731 + }, + { + "x": 1.4588579982416516, + "y": 5.50282426541773, + "heading": -0.016323802947849745, + "angularVelocity": -0.12645815467526472, + "velocityX": 0.4443004964254837, + "velocityY": -0.4734520342542199, + "moduleForcesX": [ + 47.897364078006646, + 43.771363296078064, + 51.712153560329, + 47.85692298524638 + ], + "moduleForcesY": [ + -51.01338140036875, + -54.59132698392599, + -47.13783450766829, + -51.0426999647787 + ], + "timestamp": 0.17212116407199463 + }, + { + "x": 1.5162104171628858, + "y": 5.441708865073567, + "heading": -0.032652672648373536, + "angularVelocity": -0.18973691920489047, + "velocityX": 0.6664191378657773, + "velocityY": -0.7101439350063614, + "moduleForcesX": [ + 47.832382527825025, + 43.760444196177374, + 51.70298147300229, + 47.92205717348266 + ], + "moduleForcesY": [ + -51.06506924736401, + -54.59072757046299, + -47.13719675772626, + -50.9707235368353 + ], + "timestamp": 0.25818174610799194 + }, + { + "x": 1.5926756045431116, + "y": 5.360226735801809, + "heading": -0.05443308174346003, + "angularVelocity": -0.25308228900808055, + "velocityX": 0.8885041862345195, + "velocityY": -0.9467996533549011, + "moduleForcesX": [ + 47.74512096081025, + 43.74635348449958, + 51.68931736462727, + 48.00815391044375 + ], + "moduleForcesY": [ + -51.133333759147, + -54.588485401020556, + -47.13677120729667, + -50.87397800267328 + ], + "timestamp": 0.34424232814398925 + }, + { + "x": 1.6882490058265867, + "y": 5.258382764481358, + "heading": -0.08167367566723759, + "angularVelocity": -0.31652811634851347, + "velocityX": 1.1105363105122372, + "velocityY": -1.1833985890311751, + "moduleForcesX": [ + 47.63372466857229, + 43.72840022744862, + 51.668243433528644, + 48.113016067033115 + ], + "moduleForcesY": [ + -51.216216787013636, + -54.58157282722093, + -47.135721060383055, + -50.750174463810936 + ], + "timestamp": 0.43030291017998656 + }, + { + "x": 1.8029224178515728, + "y": 5.136185717697895, + "heading": -0.11438761196088189, + "angularVelocity": -0.3801268306536648, + "velocityX": 1.332473116574703, + "velocityY": -1.4198956586668827, + "moduleForcesX": [ + 47.49348736287193, + 43.70277282082887, + 51.63393088382914, + 48.231135604313586 + ], + "moduleForcesY": [ + -51.30877200765891, + -54.563746594487974, + -47.12995843648461, + -50.59351350853768 + ], + "timestamp": 0.5163634922159839 + }, + { + "x": 1.936676738585046, + "y": 4.9936559524582, + "heading": -0.1525981886270299, + "angularVelocity": -0.4439962612620674, + "velocityX": 1.5541879641384613, + "velocityY": -1.6561561857250287, + "moduleForcesX": [ + 47.30730481341717, + 43.65083702902553, + 51.569007669599586, + 48.34309714132929 + ], + "moduleForcesY": [ + -51.393363168260414, + -54.515933915159074, + -47.100223160042596, + -50.38283518179482 + ], + "timestamp": 0.6024240742519812 + }, + { + "x": 2.089417359472379, + "y": 4.830894184799883, + "heading": -0.19638882103609856, + "angularVelocity": -0.5088349552543711, + "velocityX": 1.7748034844634022, + "velocityY": -1.8912464178636816, + "moduleForcesX": [ + 46.92254775633583, + 43.388297546565106, + 51.333326631328006, + 48.27968404555044 + ], + "moduleForcesY": [ + -51.315242953431444, + -54.28258670469318, + -46.8588510495494, + -49.928189244812266 + ], + "timestamp": 0.6884846562879785 + }, + { + "x": 2.2231712411306024, + "y": 4.688364652999476, + "heading": -0.23481358247355089, + "angularVelocity": -0.4464850286666412, + "velocityX": 1.5541828604889207, + "velocityY": -1.6561534735218788, + "moduleForcesX": [ + -46.804641906327866, + -43.5737849154587, + -51.151878946250015, + -48.397943993853495 + ], + "moduleForcesY": [ + 51.411338017062235, + 54.12246522092687, + 47.04773571475752, + 49.80566610187949 + ], + "timestamp": 0.7745452383239758 + }, + { + "x": 2.3378438009906706, + "y": 4.5661682512486585, + "heading": -0.2677858501447284, + "angularVelocity": -0.383128569329872, + "velocityX": 1.3324632166436747, + "velocityY": -1.4198881637340148, + "moduleForcesX": [ + -46.8930408063945, + -43.75302961542786, + -51.4208655887188, + -48.80743987389535 + ], + "moduleForcesY": [ + 51.77006647658862, + 54.43088247512983, + 47.26280662177527, + 49.932717396293974 + ], + "timestamp": 0.8606058203599731 + }, + { + "x": 2.433416232746065, + "y": 4.4643250767817815, + "heading": -0.2952821236280862, + "angularVelocity": -0.3194990416382979, + "velocityX": 1.1105250446261894, + "velocityY": -1.1833893284845862, + "moduleForcesX": [ + -46.834172015543345, + -43.8038207106873, + -51.45084121327245, + -48.97366881140082 + ], + "moduleForcesY": [ + 51.91054751178551, + 54.480374777947134, + 47.331348947919075, + 49.87523949636651 + ], + "timestamp": 0.9466664023959704 + }, + { + "x": 2.5098805109805107, + "y": 4.382843730787292, + "heading": -0.3172895530520021, + "angularVelocity": -0.2557202020164514, + "velocityX": 0.888493622368971, + "velocityY": -0.9467905530509265, + "moduleForcesX": [ + -46.77185013607419, + -43.83510322729932, + -51.45045906865701, + -49.085367782981976 + ], + "moduleForcesY": [ + 52.003993622508574, + 54.49394996948038, + 47.37501122701898, + 49.81059264285791 + ], + "timestamp": 1.0327269844319678 + }, + { + "x": 2.5672321904712163, + "y": 4.3217289912490875, + "heading": -0.33380008105844566, + "angularVelocity": -0.1918477381378256, + "velocityX": 0.6664105458858951, + "velocityY": -0.7101362560644467, + "moduleForcesX": [ + -46.721839228792774, + -43.85751027569671, + -51.44391879316928, + -49.163980062749964 + ], + "moduleForcesY": [ + 52.06961690331074, + 54.49746975666322, + 47.406083494827506, + 49.75817477281888 + ], + "timestamp": 1.1187875664679652 + }, + { + "x": 2.6054684419481404, + "y": 4.280983903135222, + "heading": -0.3448087987822844, + "angularVelocity": -0.12791823461321336, + "velocityX": 0.444294595953872, + "velocityY": -0.47344657843923665, + "moduleForcesX": [ + -46.68832390905574, + -43.873154666936216, + -51.438399514205486, + -49.21567045651427 + ], + "moduleForcesY": [ + 52.11282780371421, + 54.49861617678228, + 47.42729008575955, + 49.72306941004055 + ], + "timestamp": 1.2048481485039626 + }, + { + "x": 2.6245872974433224, + "y": 4.260610580440736, + "heading": -0.3503132845744241, + "angularVelocity": -0.06396059220042477, + "velocityX": 0.2221557770547791, + "velocityY": -0.23673233693882528, + "moduleForcesX": [ + -46.672848976425186, + -43.88250079839491, + -51.436915163186654, + -49.242971039133984 + ], + "moduleForcesY": [ + 52.135797056459324, + 54.500625069799426, + 47.43940924267183, + 49.70711869156378 + ], + "timestamp": 1.29090873053996 + }, + { + "x": 2.624587297439575, + "y": 4.260610580444336, + "heading": -0.350313284574424, + "angularVelocity": 4.86945709754994e-16, + "velocityX": -2.1781193472847508e-14, + "velocityY": 2.3655888321777277e-14, + "moduleForcesX": [ + -46.676116985699565, + -43.88573472382722, + -51.440890855557896, + -49.24709237978015 + ], + "moduleForcesY": [ + 52.139555439371115, + 54.50503144060741, + 47.44278460160171, + 49.711156539011256 + ], + "timestamp": 1.3769693125759574 + }, + { + "x": 2.6097025403261815, + "y": 4.272883049188608, + "heading": -0.3486185943542768, + "angularVelocity": 0.023711121663719633, + "velocityX": -0.20825887874704344, + "velocityY": 0.17170925805626883, + "moduleForcesX": [ + -53.325527820192114, + -52.543899565580986, + -55.33489027006052, + -54.67599479757407 + ], + "moduleForcesY": [ + 45.30344546948554, + 46.204887640771894, + 42.82491499809031, + 43.659875176737565 + ], + "timestamp": 1.4484416893016012 + }, + { + "x": 2.580134116922562, + "y": 4.297663630247455, + "heading": -0.3451094282478584, + "angularVelocity": 0.04909821482314941, + "velocityX": -0.4137042155527122, + "velocityY": 0.34671550166182774, + "moduleForcesX": [ + -52.56862851207183, + -51.668428542826135, + -54.74294626604449, + -53.98380292140949 + ], + "moduleForcesY": [ + 46.169623323178776, + 47.17145541851404, + 43.5681756135052, + 44.50151083261666 + ], + "timestamp": 1.519914066027245 + }, + { + "x": 2.5361681561986593, + "y": 4.335274736263326, + "heading": -0.3396203018012238, + "angularVelocity": 0.07680067038585842, + "velocityX": -0.6151461937353943, + "velocityY": 0.5262327598304285, + "moduleForcesX": [ + -51.50047411154375, + -50.41891834157215, + -53.90421586456722, + -52.99033254736792 + ], + "moduleForcesY": [ + 47.344772853066466, + 48.49067038940593, + 44.586953730344504, + 45.66446768873032 + ], + "timestamp": 1.5913864427528888 + }, + { + "x": 2.4782427587314393, + "y": 4.386182986369333, + "heading": -0.3319081747833718, + "angularVelocity": 0.10790360375738635, + "velocityX": -0.810458532923371, + "velocityY": 0.7122786797293141, + "moduleForcesX": [ + -49.88208352874225, + -48.50022068908951, + -52.625503387992744, + -51.45217423757492 + ], + "moduleForcesY": [ + 49.02799707238364, + 50.389801569506666, + 46.06811701007498, + 47.36853338734092 + ], + "timestamp": 1.6628588194785325 + }, + { + "x": 2.407106964769289, + "y": 4.45111875799722, + "heading": -0.321583297914309, + "angularVelocity": 0.14445968277546212, + "velocityX": -0.9952907323070566, + "velocityY": 0.9085436161792976, + "moduleForcesX": [ + -47.15780192777166, + -45.2126943325825, + -50.44841035705035, + -48.77740463936025 + ], + "moduleForcesY": [ + 51.624929028521905, + 53.32888659245528, + 48.40981741843909, + 50.08380727693866 + ], + "timestamp": 1.7343311962041763 + }, + { + "x": 2.3242974518893162, + "y": 4.531358566520759, + "heading": -0.3079313440530546, + "angularVelocity": 0.1910102124288775, + "velocityX": -1.158622623127807, + "velocityY": 1.1226688178584086, + "moduleForcesX": [ + -41.72003068858196, + -38.48311349131453, + -45.98995227278055, + -43.116081797134584 + ], + "moduleForcesY": [ + 56.06234348401295, + 58.3198188676031, + 52.60707005821105, + 54.9720866974948 + ], + "timestamp": 1.80580357292982 + }, + { + "x": 2.2342218638275644, + "y": 4.629362025502949, + "heading": -0.2893536832656072, + "angularVelocity": 0.25992784399271474, + "velocityX": -1.260285334646795, + "velocityY": 1.3712074961147915, + "moduleForcesX": [ + -27.000280396790707, + -19.781400792593253, + -32.92142671337878, + -25.680048193575647 + ], + "moduleForcesY": [ + 64.36040574604013, + 66.91361057866158, + 61.51512045582401, + 64.8450531339154 + ], + "timestamp": 1.8772759496554638 + }, + { + "x": 2.1541934591617964, + "y": 4.743577081279941, + "heading": -0.2632851786920285, + "angularVelocity": 0.3647353812470825, + "velocityX": -1.119710973304069, + "velocityY": 1.598030752209193, + "moduleForcesX": [ + 28.898022383948128, + 40.38279327621932, + 31.518367214748288, + 44.919632243477565 + ], + "moduleForcesY": [ + 63.33426630830682, + 56.72995559800382, + 61.95891551538514, + 53.10093359505647 + ], + "timestamp": 1.9487483263811076 + }, + { + "x": 2.0933736359860933, + "y": 4.858226801594099, + "heading": -0.2335644163554314, + "angularVelocity": 0.41583565145382007, + "velocityX": -0.850955654977675, + "velocityY": 1.6041123219358235, + "moduleForcesX": [ + 69.47635686741319, + 69.65947112210823, + 69.73585524619895, + 69.7189941107446 + ], + "moduleForcesY": [ + 6.227625887406924, + 4.078499983627095, + -1.256444304150436, + -2.7455509544386563 + ], + "timestamp": 2.0202207031067516 + }, + { + "x": 2.0511871628096467, + "y": 4.967985225687469, + "heading": -0.2022159861765387, + "angularVelocity": 0.4386090349181448, + "velocityX": -0.5902486393714047, + "velocityY": 1.53567614863841, + "moduleForcesX": [ + 67.96496385579776, + 67.99440402036385, + 67.10639875219877, + 67.1820726301549 + ], + "moduleForcesY": [ + -16.191122120647563, + -16.095511247494485, + -19.44670942058393, + -19.20731800544632 + ], + "timestamp": 2.0916930798323956 + }, + { + "x": 2.0270139823956215, + "y": 5.070867612388492, + "heading": -0.1700681728530796, + "angularVelocity": 0.4497935397741312, + "velocityX": -0.3382171104112806, + "velocityY": 1.439470621969348, + "moduleForcesX": [ + 65.5463060074924, + 65.64862612212953, + 64.96948366218025, + 65.09044905349134 + ], + "moduleForcesY": [ + -24.323259893322444, + -24.052061862241104, + -25.825731707590776, + -25.525206862715248 + ], + "timestamp": 2.1631654565580396 + }, + { + "x": 2.020448891699325, + "y": 5.165875707863209, + "heading": -0.13755294679311533, + "angularVelocity": 0.45493416547221555, + "velocityX": -0.09185493341902934, + "velocityY": 1.3292981154257553, + "moduleForcesX": [ + 63.950064201059824, + 64.03180591913006, + 63.654913960217954, + 63.74125341653127 + ], + "moduleForcesY": [ + -28.315148832203725, + -28.131653555528967, + -28.97311608059264, + -28.784456776293474 + ], + "timestamp": 2.2346378332836836 + }, + { + "x": 2.031218238194587, + "y": 5.2524144264055215, + "heading": -0.10493277773470358, + "angularVelocity": 0.45640246697905146, + "velocityX": 0.150678441370407, + "velocityY": 1.2107995068082167, + "moduleForcesX": [ + 62.88066267543294, + 62.91207596743938, + 62.79228260986751, + 62.82409534697334 + ], + "moduleForcesY": [ + -30.650558779807053, + -30.586409677611073, + -30.831354646388686, + -30.76685591760273 + ], + "timestamp": 2.3061102100093276 + }, + { + "x": 2.05912709236145, + "y": 5.330090045928955, + "heading": -0.072383245243066, + "angularVelocity": 0.45541416114660316, + "velocityX": 0.3904844851873833, + "velocityY": 1.086792170543661, + "moduleForcesX": [ + 62.127913109347276, + 62.10220705945469, + 62.18868836322343, + 62.16316779871923 + ], + "moduleForcesY": [ + -32.17057458836276, + -32.21997133029195, + -32.052845514471784, + -32.1021124146997 + ], + "timestamp": 2.3775825867349716 + }, + { + "x": 2.11398267592862, + "y": 5.407564590406167, + "heading": -0.035074691263133305, + "angularVelocity": 0.4522495337757732, + "velocityX": 0.6649523885483815, + "velocityY": 0.939136547950644, + "moduleForcesX": [ + 61.580309305924985, + 61.49718284699979, + 61.749856139779475, + 61.66816132300872 + ], + "moduleForcesY": [ + -33.23357506637483, + -33.386760848755074, + -32.91728042389969, + -33.06968489028737 + ], + "timestamp": 2.4600780924096304 + }, + { + "x": 2.191082677171206, + "y": 5.472153160220583, + "heading": 0.0016521215955720662, + "angularVelocity": 0.44519774208758894, + "velocityX": 0.9345963836882241, + "velocityY": 0.7829344083945624, + "moduleForcesX": [ + 60.45923369475699, + 60.23455621610159, + 60.84356535380034, + 60.625874690348816 + ], + "moduleForcesY": [ + -35.214733524291056, + -35.59671720906772, + -34.545862040838756, + -34.92548722348504 + ], + "timestamp": 2.542573598084289 + }, + { + "x": 2.289728178771266, + "y": 5.522730614286461, + "heading": 0.037276639055293805, + "angularVelocity": 0.4318358578252923, + "velocityX": 1.1957681909289004, + "velocityY": 0.6130934487194334, + "moduleForcesX": [ + 58.52744287678723, + 57.99177342881693, + 59.27377627441346, + 58.761492424372356 + ], + "moduleForcesY": [ + -38.31446039097551, + -39.11845718880393, + -37.14787466755693, + -37.95082764754945 + ], + "timestamp": 2.625069103758948 + }, + { + "x": 2.408421432901453, + "y": 5.557250524578989, + "heading": 0.07081951118626838, + "angularVelocity": 0.40660241860066865, + "velocityX": 1.4387844903195497, + "velocityY": 0.41844595069491913, + "moduleForcesX": [ + 54.50941828585798, + 53.13358487259185, + 55.95915631243375, + 54.647136092353065 + ], + "moduleForcesY": [ + -43.802094987807024, + -45.45697038399386, + -41.92981110097996, + -43.62112365277983 + ], + "timestamp": 2.7075646094336068 + }, + { + "x": 2.542474709173094, + "y": 5.571243535937252, + "heading": 0.09992456939909769, + "angularVelocity": 0.3528078041928978, + "velocityX": 1.6249767211345831, + "velocityY": 0.1696214981769599, + "moduleForcesX": [ + 42.694250355720705, + 38.054672270282296, + 45.591148182962975, + 40.87638058379842 + ], + "moduleForcesY": [ + -55.307949027779564, + -58.58896847735615, + -52.92949409903056, + -56.63909921586958 + ], + "timestamp": 2.7900601151082656 + }, + { + "x": 2.668629243751066, + "y": 5.561025654990008, + "heading": 0.11917705216572184, + "angularVelocity": 0.2333761410288788, + "velocityX": 1.529229180843634, + "velocityY": -0.12385985482443941, + "moduleForcesX": [ + -12.382402533780613, + -24.607511319028973, + -17.558309914075192, + -31.441206870964095 + ], + "moduleForcesY": [ + -68.63720397945411, + -65.28047130453552, + -67.43128061515037, + -62.222247817779476 + ], + "timestamp": 2.8725556207829244 + }, + { + "x": 2.7698521306860013, + "y": 5.54500330305322, + "heading": 0.13176004586708878, + "angularVelocity": 0.15252944507008542, + "velocityX": 1.2270109278923758, + "velocityY": -0.19422090026996, + "moduleForcesX": [ + -66.46018133159996, + -66.9674196584085, + -68.9644976388019, + -69.02558396413289 + ], + "moduleForcesY": [ + -21.47227954276657, + -19.910515932777027, + -11.024044579675353, + -10.78336011685823 + ], + "timestamp": 2.955051126457583 + }, + { + "x": 2.845436141122318, + "y": 5.530116306121047, + "heading": 0.14009236993377866, + "angularVelocity": 0.10100336980206083, + "velocityX": 0.916219735867702, + "velocityY": -0.1804582814458009, + "moduleForcesX": [ + -69.91668485679588, + -69.92377181874357, + -69.57042974483447, + -69.70602424237894 + ], + "moduleForcesY": [ + 0.35481804279217377, + -0.526848643787446, + 6.976161517175589, + 5.555870742457722 + ], + "timestamp": 3.037546632132242 + }, + { + "x": 2.8956151191054844, + "y": 5.519017848354159, + "heading": 0.14518444674969433, + "angularVelocity": 0.06172550582309565, + "velocityX": 0.6082631723848511, + "velocityY": -0.13453409128336982, + "moduleForcesX": [ + -69.4187453897527, + -69.56564259987941, + -68.65234610284632, + -68.9344391693208 + ], + "moduleForcesY": [ + 8.590706564201634, + 7.346324469918923, + 13.40936902595393, + 11.897426701626598 + ], + "timestamp": 3.1200421378069008 + }, + { + "x": 2.9206175804135657, + "y": 5.513061523434405, + "heading": 0.14756797074007869, + "angularVelocity": 0.02889277386548866, + "velocityX": 0.3030766476923993, + "velocityY": -0.07220180908862774, + "moduleForcesX": [ + -68.79757631258838, + -69.02339707894248, + -67.96227334834312, + -68.30019620815122 + ], + "moduleForcesY": [ + 12.723388654681191, + 11.450987875380186, + 16.62115678249245, + 15.184154939108327 + ], + "timestamp": 3.2025376434815596 + }, + { + "x": 2.9206175804138184, + "y": 5.5130615234375, + "heading": 0.14756797074007869, + "angularVelocity": -2.1366473717756725e-18, + "velocityX": 2.759428296475241e-16, + "velocityY": 1.8508305640470853e-16, + "moduleForcesX": [ + -68.30769120451006, + -68.57184176455583, + -67.47562728593, + -67.83343378701302 + ], + "moduleForcesY": [ + 15.178954859633443, + 13.946231227068052, + 18.53346910587473, + 17.184707214005112 + ], + "timestamp": 3.2850331491562184 + }, + { + "x": 2.9008988708903125, + "y": 5.5307170603623375, + "heading": 0.14700286177553085, + "angularVelocity": -0.006751511016409849, + "velocityX": -0.23558480387769798, + "velocityY": 0.2109355181352119, + "moduleForcesX": [ + -52.09999330596725, + -52.529400631239085, + -51.73320472903666, + -52.16512079541669 + ], + "moduleForcesY": [ + 46.71445570132471, + 46.23149173401103, + 47.1208354788092, + 46.6426390261249 + ], + "timestamp": 3.3687342589037015 + }, + { + "x": 2.861623494471227, + "y": 5.566204209893025, + "heading": 0.14583871741812887, + "angularVelocity": -0.013908350330240071, + "velocityX": -0.4692336401967019, + "velocityY": 0.42397465993079464, + "moduleForcesX": [ + -51.6659111374572, + -52.12885129412831, + -51.276879306050894, + -51.74245743341866 + ], + "moduleForcesY": [ + 47.185653590179186, + 46.67421839348704, + 47.60875054548638, + 47.10282037910355 + ], + "timestamp": 3.4524353686511846 + }, + { + "x": 2.803016239597012, + "y": 5.619762027969135, + "heading": 0.14402910636298827, + "angularVelocity": -0.021619917114522248, + "velocityX": -0.7001968676942122, + "velocityY": 0.6398698673768739, + "moduleForcesX": [ + -51.06312930480856, + -51.57332722665041, + -50.64375367097991, + -51.156724254575956 + ], + "moduleForcesY": [ + 47.8258847940151, + 47.27589622960445, + 48.27053972557859, + 47.72716661234478 + ], + "timestamp": 3.5361364783986677 + }, + { + "x": 2.725409512823363, + "y": 5.691733398122384, + "heading": 0.14150668290126167, + "angularVelocity": -0.030136081460894748, + "velocityX": -0.9271887436445178, + "velocityY": 0.8598615995220361, + "moduleForcesX": [ + -50.170225601037366, + -50.75200909732681, + -49.70741389665356, + -50.29204690115321 + ], + "moduleForcesY": [ + 48.74540511502207, + 48.14020759141128, + 49.218329972809045, + 48.621580569011485 + ], + "timestamp": 3.619837588146151 + }, + { + "x": 2.6293437718673225, + "y": 5.782650350616714, + "heading": 0.1381651310810073, + "angularVelocity": -0.039922431498496204, + "velocityX": -1.147723623091234, + "velocityY": 1.0862096426984997, + "moduleForcesX": [ + -48.71431630344826, + -49.4167391577109, + -48.18511463537286, + -48.890119594937715 + ], + "moduleForcesY": [ + 50.17522088948343, + 49.48473182662626, + 50.68521987830446, + 50.00663478484992 + ], + "timestamp": 3.703538697893634 + }, + { + "x": 2.5158455258965873, + "y": 5.893444520165239, + "heading": 0.13381239728196173, + "angularVelocity": -0.05200329854838183, + "velocityX": -1.355994518259599, + "velocityY": 1.3236881799931342, + "moduleForcesX": [ + -45.93506137526542, + -46.87946903712664, + -45.29576298382204, + -46.24053944561288 + ], + "moduleForcesY": [ + 52.687688259487246, + 51.851007353746446, + 53.24100600936717, + 52.42425460814814 + ], + "timestamp": 3.787239807641117 + }, + { + "x": 2.3875470683794213, + "y": 6.02610991590263, + "heading": 0.12800509474442898, + "angularVelocity": -0.06938142821559826, + "velocityX": -1.5328166842349815, + "velocityY": 1.5849896874494018, + "moduleForcesX": [ + -38.731601936491195, + -40.35076624830184, + -37.91551455677665, + -39.51613010702398 + ], + "moduleForcesY": [ + 58.094777198180715, + 56.98539695236494, + 58.63708159838333, + 57.57358536077538 + ], + "timestamp": 3.8709409173886002 + }, + { + "x": 2.2585637323391317, + "y": 6.185053475715637, + "heading": 0.11910747265558115, + "angularVelocity": -0.10630231923689437, + "velocityX": -1.5409991149465732, + "velocityY": 1.8989420845305607, + "moduleForcesX": [ + 0.7933919130137743, + -3.975029098028374, + 0.1994458839598195, + -4.26048134968995 + ], + "moduleForcesY": [ + 69.51699522375354, + 69.4045334427663, + 69.55309629363511, + 69.42011317516652 + ], + "timestamp": 3.9546420271360834 + }, + { + "x": 2.155822904626011, + "y": 6.341651289925754, + "heading": 0.10835395117931683, + "angularVelocity": -0.1284752556892115, + "velocityX": -1.2274727064325863, + "velocityY": 1.8709168195383514, + "moduleForcesX": [ + 69.29850135005749, + 69.22376482749928, + 69.52084242344242, + 69.4745660602554 + ], + "moduleForcesY": [ + -7.254272274145535, + -7.823192974180219, + -4.62731192687223, + -5.1017659452285775 + ], + "timestamp": 4.038343136883566 + }, + { + "x": 2.0772115888518736, + "y": 6.487483095533371, + "heading": 0.0969715087930836, + "angularVelocity": -0.13598914543167476, + "velocityX": -0.939190842449076, + "velocityY": 1.7422923808692223, + "moduleForcesX": [ + 63.72237766804347, + 63.53223394842242, + 64.05032016117235, + 63.867552477264695 + ], + "moduleForcesY": [ + -28.628204478983974, + -29.043494705306433, + -27.884225907046652, + -28.295904148066843 + ], + "timestamp": 4.122044246631049 + }, + { + "x": 2.021637442637373, + "y": 6.620330494187104, + "heading": 0.08532200026076654, + "angularVelocity": -0.1391798575606646, + "velocityX": -0.6639594908779467, + "velocityY": 1.587164125330234, + "moduleForcesX": [ + 60.87885944738181, + 60.7694927196997, + 61.04027054010648, + 60.93220988562005 + ], + "moduleForcesY": [ + -34.375983359242426, + -34.56803319978518, + -34.087899420795566, + -34.2797476711687 + ], + "timestamp": 4.205745356378531 + }, + { + "x": 1.9885261521674162, + "y": 6.739204165667878, + "heading": 0.07357290264571206, + "angularVelocity": -0.14036967550974822, + "velocityX": -0.39558962325593, + "velocityY": 1.4202161922459862, + "moduleForcesX": [ + 59.37783973737232, + 59.33213553424243, + 59.44147378924915, + 59.395945957037256 + ], + "moduleForcesY": [ + -36.95821427423069, + -37.031312892639825, + -36.85560981002678, + -36.92870573976187 + ], + "timestamp": 4.289446466126014 + }, + { + "x": 1.977528756045062, + "y": 6.843547542559394, + "heading": 0.061820288525356495, + "angularVelocity": -0.14041168815821406, + "velocityX": -0.13138889263033743, + "velocityY": 1.246618800486609, + "moduleForcesX": [ + 58.46396442972924, + 58.46225971251114, + 58.46628338376228, + 58.46457888275662 + ], + "moduleForcesY": [ + -38.41537453860932, + -38.417962696829065, + -38.41184043411793, + -38.41442860346031 + ], + "timestamp": 4.373147575873497 + }, + { + "x": 1.9884117094887013, + "y": 6.933004553123894, + "heading": 0.05012639395544798, + "angularVelocity": -0.13971014966457962, + "velocityX": 0.1300216147344071, + "velocityY": 1.0687673173254597, + "moduleForcesX": [ + 57.85196007751825, + 57.88125766772754, + 57.812373545441886, + 57.841731008840526 + ], + "moduleForcesY": [ + -39.3486892907348, + -39.30566232094738, + -39.40689132743343, + -39.363869274238354 + ], + "timestamp": 4.4568486856209795 + }, + { + "x": 2.021008014678955, + "y": 7.007328033447266, + "heading": 0.03853480959994491, + "angularVelocity": -0.13848782161284204, + "velocityX": 0.38943695354526725, + "velocityY": 0.8879629018104452, + "moduleForcesX": [ + 57.41445936428743, + 57.46628160975792, + 57.34427766903865, + 57.39628164773986 + ], + "moduleForcesY": [ + -39.99658250377327, + -39.922209282283085, + -40.09723139207876, + -40.02287498111711 + ], + "timestamp": 4.540549795368462 + }, + { + "x": 2.0826299228686898, + "y": 7.070370365951682, + "heading": 0.02599725344124828, + "angularVelocity": -0.1367266901870693, + "velocityX": 0.6720097158166497, + "velocityY": 0.687499968419269, + "moduleForcesX": [ + 57.08917572518026, + 57.15769997977353, + 56.9954356988548, + 57.06427617407771 + ], + "moduleForcesY": [ + -40.47369675811929, + -40.37699177913841, + -40.605690518380186, + -40.509013116235465 + ], + "timestamp": 4.632247737749421 + }, + { + "x": 2.169672617334707, + "y": 7.114364526461086, + "heading": 0.013740199626283799, + "angularVelocity": -0.13366770831190322, + "velocityX": 0.9492327984450655, + "velocityY": 0.4797726026430948, + "moduleForcesX": [ + 56.01669479138227, + 56.14220393478205, + 55.84946987132304, + 55.97590888149232 + ], + "moduleForcesY": [ + -41.93127449705145, + -41.76334146364884, + -42.153954194749936, + -41.98616963230991 + ], + "timestamp": 4.72394568013038 + }, + { + "x": 2.2812482301938513, + "y": 7.138190544722294, + "heading": 0.0019682714125729057, + "angularVelocity": -0.12837723408034055, + "velocityX": 1.2167733535710465, + "velocityY": 0.25983154852058693, + "moduleForcesX": [ + 54.06938340379889, + 54.308966898728, + 53.770349274089426, + 54.012513868838 + ], + "moduleForcesY": [ + -44.39082318658512, + -44.09795887662825, + -44.753051093939305, + -44.46103930411709 + ], + "timestamp": 4.815643622511339 + }, + { + "x": 2.4153058153444134, + "y": 7.139605579002843, + "heading": -0.008889495736468035, + "angularVelocity": -0.1184079693298827, + "velocityX": 1.4619475818218677, + "velocityY": 0.015431471712328844, + "moduleForcesX": [ + 49.535744448116404, + 50.08883535472126, + 48.95285722529716, + 49.512750306129256 + ], + "moduleForcesY": [ + -49.35481713361861, + -48.79483073718232, + -49.9344054264043, + -49.38065310701591 + ], + "timestamp": 4.907341564892298 + }, + { + "x": 2.5635950141167547, + "y": 7.1126996745657065, + "heading": -0.017499861469921024, + "angularVelocity": -0.0938992251069123, + "velocityX": 1.6171485962309224, + "velocityY": -0.29341884600255425, + "moduleForcesX": [ + 30.82355947672089, + 33.05783383581679, + 29.658555665503656, + 31.855767521726126 + ], + "moduleForcesY": [ + -62.655847172898895, + -61.51032510158116, + -63.223044950314765, + -62.148500220510925 + ], + "timestamp": 4.999039507273257 + }, + { + "x": 2.6834005581658715, + "y": 7.072012772379466, + "heading": -0.022151846620554527, + "angularVelocity": -0.05073161981418278, + "velocityX": 1.3065237989836214, + "velocityY": -0.4437057267013505, + "moduleForcesX": [ + -64.12298736950346, + -63.25910030393463, + -62.330306323818874, + -61.2590262700717 + ], + "moduleForcesY": [ + -27.440487603067957, + -29.33631206040231, + -31.317162411294717, + -33.33134662941462 + ], + "timestamp": 5.090737449654216 + }, + { + "x": 2.7720201720116067, + "y": 7.037183326012226, + "heading": -0.02500990952650958, + "angularVelocity": -0.031168233787252347, + "velocityX": 0.9664296880719148, + "velocityY": -0.37982800389550603, + "moduleForcesX": [ + -68.53351263579651, + -68.45472035856257, + -68.92469478793043, + -68.86842514528999 + ], + "moduleForcesY": [ + 13.79021249565681, + 14.160173532909322, + 11.675894673290463, + 11.9841604417936 + ], + "timestamp": 5.182435392035175 + }, + { + "x": 2.830552574824699, + "y": 7.012504794671367, + "heading": -0.026692143130144577, + "angularVelocity": -0.018345380059196448, + "velocityX": 0.6383175162541149, + "velocityY": -0.26912851727456555, + "moduleForcesX": [ + -66.1245923787409, + -65.98169477222959, + -66.56070394354207, + -66.43347254576503 + ], + "moduleForcesY": [ + 22.81538982238073, + 23.221752664345384, + 21.508695963392192, + 21.894565620883082 + ], + "timestamp": 5.274133334416134 + }, + { + "x": 2.8596270084381104, + "y": 6.999704360961915, + "heading": -0.027461342770395154, + "angularVelocity": -0.008388406765443529, + "velocityX": 0.31706745919666984, + "velocityY": -0.1395934675707037, + "moduleForcesX": [ + -64.76590607807921, + -64.6158756095251, + -65.15697508947473, + -65.01741451245994 + ], + "moduleForcesY": [ + 26.47522061163077, + 26.837605390916714, + 25.49694542258738, + 25.848954107534034 + ], + "timestamp": 5.365831276797093 + }, + { + "x": 2.8596270084381104, + "y": 6.999704360961914, + "heading": -0.02746134277039515, + "angularVelocity": 1.6056171256353062e-18, + "velocityX": -1.8304903305133357e-17, + "velocityY": -4.784339232160718e-18, + "moduleForcesX": [ + -63.93945199318899, + -63.793171888386496, + -64.29137064962106, + -64.1528183330455 + ], + "moduleForcesY": [ + 28.437823121296837, + 28.763444348886257, + 27.63256619914302, + 27.951659076114336 + ], + "timestamp": 5.457529219178052 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/source 3.1.traj b/src/main/deploy/choreo/source 3.1.traj index c36fb075..58099a83 100644 --- a/src/main/deploy/choreo/source 3.1.traj +++ b/src/main/deploy/choreo/source 3.1.traj @@ -4,9 +4,9 @@ "x": 0.7778744697570801, "y": 4.333664894104004, "heading": -1.051650156247713, - "angularVelocity": -1.3297634990832717e-18, - "velocityX": 8.096904896283762e-18, - "velocityY": -2.035450603469087e-18, + "angularVelocity": -1.595681461469011e-18, + "velocityX": 6.125258661649382e-18, + "velocityY": -3.9185398661174254e-18, "moduleForcesX": [ 0, 0, @@ -22,1936 +22,1936 @@ "timestamp": 0 }, { - "x": 0.7908719793717586, - "y": 4.323476837419605, - "heading": -1.0413708097378205, - "angularVelocity": 0.15431550267407224, - "velocityX": 0.19512108359903374, - "velocityY": -0.15294504247054264, + "x": 0.7908719793717547, + "y": 4.323476837419779, + "heading": -1.0413708097375791, + "angularVelocity": 0.15431550267789765, + "velocityX": 0.19512108359923522, + "velocityY": -0.1529450424681367, "moduleForcesX": [ - 63.74590962361433, - 58.38725396448112, - 49.37860903101583, - 45.506342584710694 + 63.74590962413147, + 58.38725396488212, + 49.378609031450075, + 45.50634258496312 ], "moduleForcesY": [ - -28.834822023862102, - -38.568182065872634, - -49.55287419418438, - -53.15308295520182 + -28.834822022718374, + -38.568182065265304, + -49.55287419375032, + -53.15308295498522 ], - "timestamp": 0.06661253297151565 + "timestamp": 0.06661253297142743 }, { - "x": 0.8168891188149916, - "y": 4.303112092011227, - "heading": -1.0210382023309228, - "angularVelocity": 0.3052369651757899, - "velocityX": 0.3905742400511663, - "velocityY": -0.30571942695958465, + "x": 0.8168891188150114, + "y": 4.303112092011752, + "heading": -1.0210382023301807, + "angularVelocity": 0.3052369651837109, + "velocityX": 0.3905742400520384, + "velocityY": -0.305719426954728, "moduleForcesX": [ - 63.57502695706841, - 58.50337248946953, - 49.388651702378716, - 45.92040304941029 + 63.5750269575728, + 58.50337248985279, + 49.38865170273007, + 45.92040304961696 ], "moduleForcesY": [ - -29.201580891943905, - -38.38697260898019, - -49.53833328341483, - -52.7922647589104 + -29.20158089084509, + -38.38697260839577, + -49.53833328306312, + -52.792264758730134 ], - "timestamp": 0.1332250659430313 + "timestamp": 0.13322506594285485 }, { - "x": 0.8559503614500349, - "y": 4.272583897748778, - "heading": -0.9908994985401418, - "angularVelocity": 0.4524479470502735, - "velocityX": 0.5863947952819387, - "velocityY": -0.45829505200625403, + "x": 0.8559503614500515, + "y": 4.272583897749817, + "heading": -0.9908994985386197, + "angularVelocity": 0.4524479470625842, + "velocityX": 0.5863947952826685, + "velocityY": -0.4582950519991483, "moduleForcesX": [ - 63.36650355926644, - 58.673994480374716, - 49.332465125535784, - 46.42312032557307 + 63.36650355975494, + 58.673994480740184, + 49.33246512578875, + 46.42312032573131 ], "moduleForcesY": [ - -29.64202317080124, - -38.12000070194584, - -49.58929691252429, - -52.34676597725229 + -29.642023169756015, + -38.120000701382985, + -49.589296912271145, + -52.34676597711144 ], - "timestamp": 0.19983759891454694 + "timestamp": 0.19983759891428227 }, { - "x": 0.9080832434863456, - "y": 4.231907621482131, - "heading": -0.9512364839325822, - "angularVelocity": 0.5954287104578356, - "velocityX": 0.7826287293204035, - "velocityY": -0.6106399869832296, + "x": 0.908083243486343, + "y": 4.231907621483826, + "heading": -0.9512364839299804, + "angularVelocity": 0.5954287104748315, + "velocityX": 0.7826287293211497, + "velocityY": -0.6106399869741947, "moduleForcesX": [ - 63.1087538854026, - 58.88683733974907, - 49.24358469003159, - 47.01667690510743 + 63.10875388586886, + 58.88683734009576, + 49.24358469017145, + 47.016676905215164 ], "moduleForcesY": [ - -30.17612796676512, - -37.78365700090767, - -49.672009408886844, - -51.809713634059214 + -30.176127965788748, + -37.78365700036693, + -49.6720094087466, + -51.80971363396091 ], - "timestamp": 0.2664501318860626 + "timestamp": 0.2664501318857097 }, { - "x": 0.9733191777952942, - "y": 4.181101065602064, - "heading": -0.9023828510876672, - "angularVelocity": 0.7334000174682638, - "velocityX": 0.9793342393516886, - "velocityY": -0.7627176690877809, + "x": 0.9733191777952241, + "y": 4.1811010656045475, + "heading": -0.9023828510836664, + "angularVelocity": 0.7334000174902384, + "velocityX": 0.9793342393519744, + "velocityY": -0.762717669076962, "moduleForcesX": [ - 62.78758091347448, - 59.12644232420397, - 49.162542801992586, - 47.70378434486226 + 62.78758091390919, + 59.1264423245301, + 49.16254280200449, + 47.70378434491597 ], "moduleForcesY": [ - -30.82641384591526, - -37.399521862180414, - -49.74596927329427, - -51.172358294771385 + -30.826413845028178, + -37.399521861664304, + -49.745969273280835, + -51.17235829472075 ], - "timestamp": 0.33306266485757824 + "timestamp": 0.33306266485713715 }, { - "x": 1.051694367518301, - "y": 4.120184874713526, - "heading": -0.8447458342032448, - "angularVelocity": 0.8652578473343552, - "velocityX": 1.1765832378198438, - "velocityY": -0.9144854304607654, + "x": 1.0516943675181405, + "y": 4.120184874716908, + "heading": -0.8447458341975047, + "angularVelocity": 0.8652578473616123, + "velocityX": 1.1765832378200387, + "velocityY": -0.9144854304484828, "moduleForcesX": [ - 62.38681753882747, - 59.37326562455472, - 49.13769712317514, - 48.48705032206493 + 62.386817539217134, + 59.37326562485723, + 49.13769712304434, + 48.487050322059 ], "moduleForcesY": [ - -31.615322936234296, - -36.99658263811015, - -49.763347170131574, - -50.42430963220079 + -31.615322935463205, + -36.99658263762404, + -49.76334717025892, + -50.42430963220592 ], - "timestamp": 0.3996751978290939 + "timestamp": 0.3996751978285646 }, { - "x": 1.1432508333296558, - "y": 4.049183171168427, - "heading": -0.778832566995066, - "angularVelocity": 0.9895024895144598, - "velocityX": 1.3744630586335072, - "velocityY": -1.065891062504106, + "x": 1.1432508333293583, + "y": 4.049183171172805, + "heading": -0.7788325669872294, + "angularVelocity": 0.9895024895472444, + "velocityX": 1.3744630586332751, + "velocityY": -1.0658910624905655, "moduleForcesX": [ - 61.889733856839435, - 59.602244362756366, - 49.22597947357009, - 49.36848790198351 + 61.88973385716236, + 59.60224436302891, + 49.225979473285065, + 49.36848790191129 ], "moduleForcesY": [ - -32.56119938527781, - -36.61435191109544, - -49.66762908534543, - -49.55361350286737 + -32.56119938466126, + -36.61435191065098, + -49.66762908562598, + -49.553613502938724 ], - "timestamp": 0.46628773080060953 + "timestamp": 0.46628773079999203 }, { - "x": 1.2480376237140218, - "y": 3.968124696368845, - "heading": -0.7052817755202949, - "angularVelocity": 1.1041584547794074, - "velocityX": 1.57307920461716, - "velocityY": -1.2168652231591865, + "x": 1.2480376237134958, + "y": 3.968124696374288, + "heading": -0.7052817755099939, + "angularVelocity": 1.1041584548178656, + "velocityX": 1.5730792046158117, + "velocityY": -1.2168652231448125, "moduleForcesX": [ - 61.28176481668813, - 59.78038532288985, - 49.4938549994705, - 50.34939808767109 + 61.28176481691346, + 59.78038532312139, + 49.49385499902187, + 50.34939808752191 ], "moduleForcesY": [ - -33.67250970561668, - -36.30739888304486, - -49.39059309707039, - -48.546399889287585 + -33.67250970520312, + -36.30739888266253, + -49.390593097517815, + -48.546399889441645 ], - "timestamp": 0.5329002637721252 + "timestamp": 0.5329002637714194 }, { - "x": 1.3661123895110021, - "y": 3.877044967671322, - "heading": -0.6249031875065618, - "angularVelocity": 1.206658633565307, - "velocityX": 1.7725608159574173, - "velocityY": -1.3673061905102768, + "x": 1.3661123895101355, + "y": 3.8770449676778536, + "heading": -0.6249031874934339, + "angularVelocity": 1.206658633609344, + "velocityX": 1.7725608159546524, + "velocityY": -1.367306190495742, "moduleForcesX": [ - 60.55523039747262, - 59.86237923234887, - 50.01933061446567, - 51.43105327265715 + 60.555230397553366, + 59.86237923251681, + 50.019330613853604, + 51.431053272417905 ], "moduleForcesY": [ - -34.9402966200205, - -36.15231999767806, - -48.845768467491496, - -47.385486747890475 + -34.940296619876236, + -36.15231999739851, + -48.845768468115836, + -47.38548674814936 ], - "timestamp": 0.5995127967436409 + "timestamp": 0.5995127967428469 }, { - "x": 1.4975436678185934, - "y": 3.7759903450547028, - "heading": -0.53873230694548, - "angularVelocity": 1.293613629704331, - "velocityX": 1.9730713192334703, - "velocityY": -1.5170511930514805, + "x": 1.4975436678172518, + "y": 3.775990345062295, + "heading": -0.5387323069291853, + "angularVelocity": 1.2936136297535843, + "velocityX": 1.9730713192289506, + "velocityY": -1.5170511930375694, "moduleForcesX": [ - 59.71660813639446, - 59.781944702186415, - 50.89674120838264, - 52.617056525494455 + 59.716608136271915, + 59.78194470225127, + 50.896741207611434, + 52.617056525140555 ], "moduleForcesY": [ - -36.32951289772708, - -36.25934407553948, - -47.91447235046562, - -46.0464745895729 + -36.329512897923145, + -36.259344075430484, + -47.91447235128193, + -46.04647458997633 ], - "timestamp": 0.6661253297151566 + "timestamp": 0.6661253297142743 }, { - "x": 1.6424143533198352, - "y": 3.665025662326569, - "heading": -0.4481205140243463, - "angularVelocity": 1.3602814497368003, - "velocityX": 2.174826253239616, - "velocityY": -1.665822898156168, + "x": 1.6424143533178284, + "y": 3.6650256623351347, + "heading": -0.4481205140046001, + "angularVelocity": 1.3602814497904157, + "velocityX": 2.1748262532325127, + "velocityY": -1.665822898143766, "moduleForcesX": [ - 58.79631756896231, - 59.433248892591386, - 52.247409534940125, - 53.9194587893842 + 58.79631756856442, + 59.43324889247598, + 52.247409534027724, + 53.91945878887585 ], "moduleForcesY": [ - -37.77070605252747, - -36.79353882986422, - -46.41555660113537, - -44.48747884319979 + -37.77070605314016, + -36.79353883004777, + -46.41555660215867, + -44.487478843814586 ], - "timestamp": 0.7327378626866723 + "timestamp": 0.7327378626857017 }, { - "x": 1.800826604673265, - "y": 3.5442490372900517, - "heading": -0.3549074004558732, - "angularVelocity": 1.3993329694928751, - "velocityX": 2.3781148124358094, - "velocityY": -1.8131216401598682, + "x": 1.8008266046703592, + "y": 3.544249037299425, + "heading": -0.3549074004325164, + "angularVelocity": 1.3993329695489327, + "velocityX": 2.378114812425462, + "velocityY": -1.8131216401501349, "moduleForcesX": [ - 57.85964296595883, - 58.62699304555619, - 54.24098287937368, - 55.37454877680063 + 57.85964296520334, + 58.62699304509392, + 54.240982878372066, + 55.374548776074526 ], "moduleForcesY": [ - -39.15408809177324, - -38.01758274488999, - -44.03657792902701, - -42.620768436938704 + -39.15408809288117, + -38.01758274559908, + -44.03657793025534, + -42.62076843787999 ], - "timestamp": 0.799350395658188 + "timestamp": 0.7993503956571292 }, { - "x": 1.9729064746231015, - "y": 3.4138237900863677, - "heading": -0.261785676023239, - "angularVelocity": 1.3979610184235784, - "velocityX": 2.58329569937567, - "velocityY": -1.9579685891760825, + "x": 1.9729064746189697, + "y": 3.413823790096245, + "heading": -0.2617856759964363, + "angularVelocity": 1.3979610184771598, + "velocityX": 2.583295699360691, + "velocityY": -1.9579685891711138, "moduleForcesX": [ - 57.01503970270268, - 56.97264868755776, - 57.13055506988329, - 57.088614242525495 + 57.01503970150166, + 56.972648686336136, + 57.13055506893403, + 57.08861424148898 ], "moduleForcesY": [ - -40.32815265808299, - -40.3871603414814, - -40.16401782504414, - -40.22274635092099 + -40.32815265976974, + -40.38716034320021, + -40.16401782638521, + -40.22274635238806 ], - "timestamp": 0.8659629286297037 + "timestamp": 0.8659629286285566 }, { - "x": 2.1587959216901558, - "y": 3.2740626515960307, - "heading": -0.1731498612166921, - "angularVelocity": 1.3306176908848175, - "velocityX": 2.7906076945992013, - "velocityY": -2.0981207628015075, + "x": 2.158795921684324, + "y": 3.274062651605914, + "heading": -0.17314986118757214, + "angularVelocity": 1.3306176909213672, + "velocityX": 2.790607694577369, + "velocityY": -2.0981207628041956, "moduleForcesX": [ - 56.41042369707265, - 53.501719202644416, - 61.26419264721754, - 59.40078934825349 + 56.41042369532152, + 53.50171919941402, + 61.2641926466866, + 59.4007893468711 ], "moduleForcesY": [ - -41.1079010332853, - -44.78370054636255, - -33.42235417217702, - -36.56648525897984 + -41.107901035672306, + -44.7837005502188, + -33.422354173129975, + -36.56648526121465 ], - "timestamp": 0.9325754616012194 + "timestamp": 0.9325754615999841 }, { - "x": 2.3585571947909116, - "y": 3.1257497783866914, - "heading": -0.09743941845228067, - "angularVelocity": 1.1365795502294775, - "velocityX": 2.9988541823830035, - "velocityY": -2.2265010290594955, + "x": 2.3585571947825676, + "y": 3.125749778395996, + "heading": -0.09743941842552141, + "angularVelocity": 1.1365795501955436, + "velocityX": 2.9988541823492687, + "velocityY": -2.226501029071138, "moduleForcesX": [ - 56.18321511797377, - 45.13645782831661, - 66.6575745567656, - 63.63924131102192 + 56.183215115418484, + 45.13645781804362, + 66.6575745572279, + 63.63924131085493 ], "moduleForcesY": [ - -41.32675572742186, - -53.045678977052305, - -20.39623436243562, - -28.01878833525786 + -41.32675573087057, + -53.04567898580249, + -20.39623436084957, + -28.018788335571973 ], - "timestamp": 0.9991879945727351 + "timestamp": 0.9991879945714115 }, { - "x": 2.571323749679796, - "y": 2.9722890611298696, - "heading": -0.05631702040111217, - "angularVelocity": 0.6173372519665775, - "velocityX": 3.1940919433264323, - "velocityY": -2.303781441886374, + "x": 2.5713237496680983, + "y": 2.9722890611360366, + "heading": -0.056317020373306215, + "angularVelocity": 0.6173372519831085, + "velocityX": 3.1940919432803097, + "velocityY": -2.30378144193652, "moduleForcesX": [ - 56.49315652197762, - 24.727674210970267, - 69.53856728119115, - 66.38848839184168 + 56.49315651832047, + 24.72767419303312, + 69.5385672809967, + 66.38848840011917 ], "moduleForcesY": [ - -40.73364522523241, - -64.83876276810156, - 2.4259673258224868, - 17.19340461150298 + -40.73364523028673, + -64.83876277492942, + 2.425967330179503, + 17.19340457572179 ], - "timestamp": 1.0658005275442508 + "timestamp": 1.0658005275428388 }, { - "x": 2.7978969801578932, - "y": 2.816123256954651, - "heading": -0.04923559160029977, - "angularVelocity": 0.1063077544857887, - "velocityX": 3.4013603802603107, - "velocityY": -2.3443907206170564, + "x": 2.797896980139407, + "y": 2.816123256947658, + "heading": -0.04923559157858403, + "angularVelocity": 0.10630775439450244, + "velocityX": 3.4013603801629086, + "velocityY": -2.3443907208177204, "moduleForcesX": [ - 61.70982060481134, - 40.01285570538606, - 68.37167087206099, - 60.43433119111175 + 61.70982059244306, + 40.01285562448143, + 68.37167087457641, + 60.434331225109716 ], "moduleForcesY": [ - -31.4520746032983, - -55.70693219082933, - 10.727828044445207, - 31.264613623300775 + -31.45207462723247, + -55.706932248574496, + 10.727828026787554, + 31.264613554872565 ], - "timestamp": 1.1324130605157665 + "timestamp": 1.1324130605142662 }, { - "x": 3.0347601843563274, - "y": 2.6710905971518515, - "heading": -0.04900748324484257, - "angularVelocity": 0.003424405968089362, - "velocityX": 3.55583542063653, - "velocityY": -2.1772578422264344, + "x": 3.034760184329724, + "y": 2.671090597132263, + "heading": -0.0490074832231963, + "angularVelocity": 0.003424405967050934, + "velocityX": 3.5558354205193843, + "velocityY": -2.177257842418406, "moduleForcesX": [ - 50.41350702863919, - 41.75482316229609, - 44.362342200824024, - 35.27998738074212 + 50.413506856910104, + 41.754823961326274, + 44.362341929459326, + 35.27998700294513 ], "moduleForcesY": [ - 38.99730957905846, - 46.617569740155716, - 46.965805915927774, - 53.3083103983914 + 38.99730976426598, + 46.61756909982145, + 46.965806150666, + 53.30831062845783 ], - "timestamp": 1.1990255934872822 + "timestamp": 1.1990255934856935 }, { - "x": 3.2779712543514585, - "y": 2.5369613930825095, - "heading": -0.04879559855232118, - "angularVelocity": 0.0031808532579297136, - "velocityX": 3.6511307879424364, - "velocityY": -2.0135730933201033, + "x": 3.277971254366929, + "y": 2.5369613931399093, + "heading": -0.048795598530848285, + "angularVelocity": 0.003180853255331212, + "velocityX": 3.651130788578886, + "velocityY": -2.0135730921670043, "moduleForcesX": [ - 26.514032908634963, - 26.48917929707793, - 26.505657516129617, - 26.480809194623138 + 26.51403343046492, + 26.489179612407455, + 26.505657599753675, + 26.480809112111523 ], "moduleForcesY": [ - 45.5016606384836, - 45.51171619004381, - 45.5152401681101, - 45.525290455269825 + 45.50166091112535, + 45.51171684473839, + 45.515240274553484, + 45.52529091768426 ], - "timestamp": 1.265638126458798 + "timestamp": 1.2656381264571208 }, { - "x": 3.5240170136884648, - "y": 2.4081056080213394, - "heading": -0.04858282682880418, - "angularVelocity": 0.003194169535753657, - "velocityX": 3.6936856828761946, - "velocityY": -1.93440752532666, + "x": 3.5240170136688564, + "y": 2.408105608012548, + "heading": -0.04858282680739443, + "angularVelocity": 0.0031941695348100155, + "velocityX": 3.6936856823544875, + "velocityY": -1.9344075263229015, "moduleForcesX": [ - 11.832044679638638, - 11.833159737293062, - 11.83210215812816, - 11.833217225479276 + 11.832044385798296, + 11.833159401629988, + 11.832101854528789, + 11.833216870497965 ], "moduleForcesY": [ - 22.01300562318127, - 22.0128485655156, - 22.01202572778171, - 22.011868671306686 + 22.01300500476856, + 22.012848038290926, + 22.012025057287687, + 22.01186809697094 ], - "timestamp": 1.3322506594303136 + "timestamp": 1.3322506594285481 }, { - "x": 3.7708350348315482, - "y": 2.2807352279621504, - "heading": -0.0483699449005901, - "angularVelocity": 0.0031958239496029865, - "velocityX": 3.7052790238231275, - "velocityY": -1.9121083432400703, + "x": 3.770835034817424, + "y": 2.2807352279647817, + "heading": -0.048369944879233955, + "angularVelocity": 0.0031958239488024064, + "velocityX": 3.7052790239103643, + "velocityY": -1.912108343071116, "moduleForcesX": [ - 3.2235324585038754, - 3.2236618531632817, - 3.2235271302506407, - 3.2236565249546465 + 3.223532949769158, + 3.2236620923869292, + 3.223527231378966, + 3.223656370634464 ], "moduleForcesY": [ - 6.200483556552904, - 6.200476317722027, - 6.200352895038578, - 6.200345656224632 + 6.200483740926388, + 6.200477036360204, + 6.200352815192096, + 6.2003461290433926 ], - "timestamp": 1.3988631924018293 + "timestamp": 1.3988631923999755 }, { - "x": 4.017945860044819, - "y": 2.153933675847828, - "heading": -0.04815735530566025, - "angularVelocity": 0.0031914353868025967, - "velocityX": 3.709674654151622, - "velocityY": -1.90356899006594, + "x": 4.017945860027259, + "y": 2.153933675844563, + "heading": -0.048157355284360694, + "angularVelocity": 0.0031914353859573896, + "velocityX": 3.7096746541049623, + "velocityY": -1.9035689901569823, "moduleForcesX": [ - 1.2223925594156837, - 1.2220511392203353, - 1.2224089657777908, - 1.222067545703039 + 1.2223938746517689, + 1.222051260087835, + 1.222408785272593, + 1.222066141189196 ], "moduleForcesY": [ - 2.374233190641205, - 2.3742500204088812, - 2.3745815684863776, - 2.374598398301541 + 2.3742328041770366, + 2.3742514629482714, + 2.374579889586726, + 2.3745987319670263 ], - "timestamp": 1.465475725373345 + "timestamp": 1.4654757253714028 }, { - "x": 4.265335286953013, - "y": 2.027676292006606, - "heading": -0.04794522174606807, - "angularVelocity": 0.0031845892976762468, - "velocityX": 3.7138570757244884, - "velocityY": -1.895399832569214, + "x": 4.265335287008856, + "y": 2.027676292147968, + "heading": -0.047945221724793284, + "angularVelocity": 0.003184589297308503, + "velocityX": 3.713857076831331, + "velocityY": -1.8953998304005573, "moduleForcesX": [ - 1.163199663544347, - 1.1626670888284039, - 1.1632251975919956, - 1.1626926239772695 + 1.1632026163804865, + 1.162667533168695, + 1.1632254607113321, + 1.1626902466338753 ], "moduleForcesY": [ - 2.271195979291761, - 2.271222065843487, - 2.271739492202394, - 2.2717655728058856 + 2.271196262649088, + 2.271225591627576, + 2.271736925412616, + 2.271766843769598 ], - "timestamp": 1.5320882583448607 + "timestamp": 1.5320882583428301 }, { - "x": 4.513387263491877, - "y": 1.9027254137370513, - "heading": -0.04773334654928872, - "angularVelocity": 0.003180710706046184, - "velocityX": 3.7238033966511064, - "velocityY": -1.8757863227177534, + "x": 4.513387263726315, + "y": 1.9027254142338006, + "heading": -0.04773334652797979, + "angularVelocity": 0.0031807107065629863, + "velocityX": 3.723803399337141, + "velocityY": -1.8757863173851013, "moduleForcesX": [ - 2.765776223844279, - 2.765473269227589, - 2.7657891163065877, - 2.7654861619049216 + 2.7657780823712406, + 2.765473685160751, + 2.7657896002600624, + 2.7654851599392303 ], "moduleForcesY": [ - 5.453486682132146, - 5.453502875542226, - 5.453793409345764, - 5.453809602835306 + 5.453487457755567, + 5.453505306304338, + 5.453792661463415, + 5.453810663508131 ], - "timestamp": 1.5987007913163764 + "timestamp": 1.5987007913142575 }, { - "x": 4.7636905451889415, - "y": 1.7823482264471835, - "heading": -0.04752024111954168, - "angularVelocity": 0.003199179197076531, - "velocityX": 3.7576004173883883, - "velocityY": -1.8071252048220714, + "x": 4.76369054518264, + "y": 1.7823482264444208, + "heading": -0.047520241098396636, + "angularVelocity": 0.0031991791946204317, + "velocityX": 3.7576004137793353, + "velocityY": -1.8071252123232402, "moduleForcesX": [ - 9.396675512144679, - 9.39819901784068, - 9.396709254967268, - 9.398232777338956 + 9.396673705872347, + 9.39819730006515, + 9.396707473785185, + 9.398231081199015 ], "moduleForcesY": [ - 19.092390606150673, - 19.092220862164694, - 19.091009053610517, - 19.090839312476135 + 19.092387038061002, + 19.09221720512043, + 19.09100557309194, + 19.090835744537614 ], - "timestamp": 1.6653133242878921 + "timestamp": 1.6653133242856848 }, { - "x": 5.0188400094891765, - "y": 1.6726490442267128, - "heading": -0.04723633820300304, - "angularVelocity": 0.004262004518879671, - "velocityX": 3.8303522313036735, - "velocityY": -1.646824964115687, + "x": 5.018840009680974, + "y": 1.6726490446859121, + "heading": -0.047236338181666296, + "angularVelocity": 0.004262004521763258, + "velocityX": 3.8303522342826466, + "velocityY": -1.6468249571828, "moduleForcesX": [ - 20.160339598017387, - 20.271778827791216, - 20.186266828439518, - 20.297839903273452 + 20.160341309415056, + 20.271780653378354, + 20.186268676149652, + 20.297841846829897 ], "moduleForcesY": [ - 44.61714719861703, - 44.58388431235552, - 44.56094399930638, - 44.52760480062931 + 44.61715118244674, + 44.583888259336774, + 44.56094806815956, + 44.52760885668282 ], - "timestamp": 1.7319258572594078 + "timestamp": 1.7319258572571121 }, { - "x": 5.276040962243286, - "y": 1.5678524061254324, - "heading": -0.046947349952411, - "angularVelocity": 0.0043383465197998905, - "velocityX": 3.8611495657144865, - "velocityY": -1.5732270404143434, + "x": 5.276040962308425, + "y": 1.567852406274765, + "heading": -0.04694734993114214, + "angularVelocity": 0.004338346518786609, + "velocityX": 3.861149563818171, + "velocityY": -1.5732270450682089, "moduleForcesX": [ - 8.560118396401565, - 8.566501267033127, - 8.560247116014803, - 8.56663029963911 + 8.560117120058553, + 8.56649998262021, + 8.560245714730028, + 8.566628839534616 ], "moduleForcesY": [ - 20.46747803904423, - 20.466793414894244, - 20.461850439324866, - 20.461165848040086 + 20.467474806427994, + 20.466790232197752, + 20.461847166644493, + 20.46116264960528 ], - "timestamp": 1.7985383902309235 + "timestamp": 1.7985383902285395 }, { - "x": 5.534216899574317, - "y": 1.4654824117182799, - "heading": -0.04665631309500156, - "angularVelocity": 0.004369100594536302, - "velocityX": 3.8757862194105366, - "velocityY": -1.536797804264961, + "x": 5.53421689953023, + "y": 1.4654824115931464, + "heading": -0.04665631307381442, + "angularVelocity": 0.004369100593315545, + "velocityX": 3.875786217775965, + "velocityY": -1.5367978083873335, "moduleForcesX": [ - 4.068624748426005, - 4.071060153628479, - 4.068549709495257, - 4.070985139742783 + 4.068624788601681, + 4.071060271920588, + 4.068549740960962, + 4.07098524097379 ], "moduleForcesY": [ - 10.13063153345665, - 10.130481600946313, - 10.128231656616295, - 10.128081730271346 + 10.130631677004772, + 10.130481677327632, + 10.1282318686583, + 10.1280818895711 ], - "timestamp": 1.8651509232024392 + "timestamp": 1.8651509231999668 }, { - "x": 5.794372355867246, - "y": 1.3682547977858872, - "heading": -0.04636219278537718, - "angularVelocity": 0.004415389964980432, - "velocityX": 3.905503134135057, - "velocityY": -1.459599411629766, + "x": 5.794372355842512, + "y": 1.3682547977136283, + "heading": -0.04636219276413434, + "angularVelocity": 0.00441538996582264, + "velocityX": 3.9055031344307545, + "velocityY": -1.4595994108379353, "moduleForcesX": [ - 8.260963536224425, - 8.264869277780319, - 8.261045504013175, - 8.264951367768367 + 8.260964021908269, + 8.26486985270304, + 8.261046002058745, + 8.26495195604506 ], "moduleForcesY": [ - 21.467349064567532, - 21.466934299940117, - 21.46397136076802, - 21.463556605012805 + 21.467350425085247, + 21.466935581870978, + 21.463972811998225, + 21.46355797712207 ], - "timestamp": 1.931763456173955 + "timestamp": 1.9317634561713941 }, { - "x": 6.058520814972377, - "y": 1.282472529523383, - "heading": -0.04606005529713513, - "angularVelocity": 0.004535745373490557, - "velocityX": 3.965446850941467, - "velocityY": -1.2877797080496214, + "x": 6.058520814952769, + "y": 1.2824725294681238, + "heading": -0.046060055275852295, + "angularVelocity": 0.004535745374097144, + "velocityX": 3.965446851023664, + "velocityY": -1.2877797077961244, "moduleForcesX": [ - 16.659711714491884, - 16.673161829473102, - 16.66222092531276, - 16.675673239510985 + 16.659711667705363, + 16.673161791773882, + 16.66222084343888, + 16.67567316842716 ], "moduleForcesY": [ - 47.77989163980646, - 47.77656557923747, - 47.77432962004692, - 47.771002564051436 + 47.779891485446186, + 47.776565422843, + 47.774329477432524, + 47.77100241879716 ], - "timestamp": 1.9983759891454707 + "timestamp": 1.9983759891428214 }, { - "x": 6.326935018479887, - "y": 1.2111884138637838, - "heading": -0.04572387766988923, - "angularVelocity": 0.0050467624071529864, - "velocityX": 4.0294850163149665, - "velocityY": -1.0701306868195675, + "x": 6.326935018464312, + "y": 1.2111884138255193, + "heading": -0.045723877648205694, + "angularVelocity": 0.005046762413175208, + "velocityX": 4.029485016380848, + "velocityY": -1.070130686565861, "moduleForcesX": [ - 17.76434909368371, - 17.833039573651952, - 17.77928266449683, - 17.84803536932326 + 17.764349079949987, + 17.833039578816862, + 17.779282668917745, + 17.84803535533965 ], "moduleForcesY": [ - 60.533843276425394, - 60.51552676552759, - 60.52159010023538, - 60.50324061772363 + 60.53384327945482, + 60.51552676309896, + 60.521590097018716, + 60.50324062036407 ], - "timestamp": 2.064988522116986 + "timestamp": 2.0649885221142488 }, { - "x": 6.598149001232439, - "y": 1.1556493567666084, - "heading": -0.04255962682130831, - "angularVelocity": 0.04750233488244815, - "velocityX": 4.071515834242889, - "velocityY": -0.8337628764383538, + "x": 6.598149001225702, + "y": 1.1556493567444743, + "heading": -0.04255962681817847, + "angularVelocity": 0.047502334603978215, + "velocityX": 4.0715158343809605, + "velocityY": -0.8337628761973023, "moduleForcesX": [ - 8.287572421549362, - 14.286479207247982, - 8.808436728764809, - 15.365145055742365 + 8.28757237649673, + 14.286479227047964, + 8.808436870157584, + 15.36514501967037 ], "moduleForcesY": [ - 66.4507233464468, - 65.45898479609045, - 66.07372493186959, - 64.91023150582662 + 66.45072335474919, + 65.45898479439751, + 66.07372490759722, + 64.91023150887932 ], - "timestamp": 2.1316010550885016 + "timestamp": 2.1316010550856763 }, { - "x": 6.859559783326081, - "y": 1.113135958184906, - "heading": -0.030602705565701032, - "angularVelocity": 0.17949957346195378, - "velocityX": 3.9243483235418233, - "velocityY": -0.6382192161928673, + "x": 6.859559783323042, + "y": 1.1131359581740934, + "heading": -0.0306027055657009, + "angularVelocity": 0.17949957341520645, + "velocityX": 3.9243483236025436, + "velocityY": -0.6382192160237583, "moduleForcesX": [ - -42.45988336627168, - -29.346972124084434, - -52.177609628795565, - -39.698593640879075 + -42.459883383315024, + -29.34697212286383, + -52.17760966362154, + -39.69859367485398 ], "moduleForcesY": [ - 54.43758266703957, - 62.32119709611513, - 44.88720682067673, - 55.84212799694645 + 54.43758265314552, + 62.32119709610292, + 44.88720677947976, + 55.842127971468855 ], - "timestamp": 2.198213588060017 + "timestamp": 2.198213588057104 }, { "x": 7.107321739196777, "y": 1.0795719623565674, - "heading": -0.030602705565701018, - "angularVelocity": 3.347783365519795e-18, - "velocityX": 3.7194495512825236, - "velocityY": -0.5038690818541787, + "heading": -0.030602705565700897, + "angularVelocity": 1.6075047383869973e-18, + "velocityX": 3.7194495513330783, + "velocityY": -0.5038690816925215, "moduleForcesX": [ - -61.323422950505716, - -65.95546140653828, - -44.62846950333632, - -55.985729468867824 + -61.32342295009295, + -65.955461406566, + -44.628469510881004, + -55.985729471196706 ], "moduleForcesY": [ - 32.44664959475204, - 22.20881577411158, - 53.33306981117861, - 41.43874553341409 + 32.44664959500376, + 22.208815773674722, + 53.33306980468184, + 41.43874553017478 ], - "timestamp": 2.2648261210315326 + "timestamp": 2.2648261210285314 }, { - "x": 7.342124626950215, - "y": 1.053916100516507, - "heading": -0.030602705565701004, - "angularVelocity": 7.672386391662901e-19, - "velocityX": 3.4979351696684997, - "velocityY": -0.3822037381956934, + "x": 7.342124626958182, + "y": 1.0539161005261706, + "heading": -0.03060270556570089, + "angularVelocity": 2.0024672081036984e-19, + "velocityX": 3.4979351697057512, + "velocityY": -0.38220373804281776, "moduleForcesX": [ - -61.122061696457, - -61.122061696457, - -61.122061696457, - -61.122061696457 + -61.122061698454715, + -61.122061698454715, + -61.122061698454715, + -61.122061698454715 ], "moduleForcesY": [ - 33.57089768760224, - 33.57089768760224, - 33.57089768760223, - 33.57089768760224 + 33.57089768389125, + 33.57089768389125, + 33.570897683891246, + 33.570897683891246 ], - "timestamp": 2.3319522615585138 + "timestamp": 2.3319522615570754 }, { - "x": 7.56101179722364, - "y": 1.0341858544549376, - "heading": -0.03060270556570099, - "angularVelocity": 7.672403149471102e-19, - "velocityX": 3.260833537501588, - "velocityY": -0.2939279080649534, + "x": 7.561011797238179, + "y": 1.0341858544731557, + "heading": -0.030602705565700883, + "angularVelocity": 2.0024561890342501e-19, + "velocityX": 3.2608335375235695, + "velocityY": -0.29392790793066653, "moduleForcesX": [ - -65.42302348088484, - -65.42302348088484, - -65.42302348088484, - -65.42302348088484 + -65.42302348307652, + -65.42302348307652, + -65.42302348307652, + -65.42302348307652 ], "moduleForcesY": [ - 24.35778975736353, - 24.357789757363527, - 24.357789757363534, - 24.357789757363534 + 24.3577897514352, + 24.3577897514352, + 24.3577897514352, + 24.357789751435195 ], - "timestamp": 2.399078402085495 + "timestamp": 2.3990784020856193 }, { - "x": 7.76353893371165, - "y": 1.0190536925576863, - "heading": -0.030602705565700973, - "angularVelocity": 7.672257598260093e-19, - "velocityX": 3.0171127804763933, - "velocityY": -0.22542874919449496, + "x": 7.7635389337314935, + "y": 1.0190536925830782, + "heading": -0.030602705565700876, + "angularVelocity": 2.0022790819956785e-19, + "velocityX": 3.0171127804851614, + "velocityY": -0.22542874908236665, "moduleForcesX": [ - -67.24942660173558, - -67.24942660173558, - -67.24942660173558, - -67.24942660173558 + -67.24942660363206, + -67.24942660363206, + -67.24942660363206, + -67.24942660363206 ], "moduleForcesY": [ - 18.900848713116805, - 18.900848713116805, - 18.9008487131168, - 18.900848713116808 + 18.90084870634776, + 18.900848706347762, + 18.90084870634775, + 18.90084870634776 ], - "timestamp": 2.466204542612476 + "timestamp": 2.466204542614163 }, { - "x": 7.949481024014891, - "y": 1.0076590456800256, - "heading": -0.03060270556570096, - "angularVelocity": 7.672013523245006e-19, - "velocityX": 2.7700399403791343, - "velocityY": -0.16974976943713838, + "x": 7.949481024038794, + "y": 1.0076590457110912, + "heading": -0.03060270556570087, + "angularVelocity": 2.0020051992032217e-19, + "velocityX": 2.7700399403750904, + "velocityY": -0.16974976934866481, "moduleForcesX": [ - -68.17436080640248, - -68.17436080640248, - -68.17436080640248, - -68.17436080640248 + -68.17436080799112, + -68.17436080799114, + -68.17436080799114, + -68.17436080799112 ], "moduleForcesY": [ - 15.36339993436348, - 15.363399934363485, - 15.363399934363473, - 15.363399934363477 + 15.363399927305814, + 15.363399927305815, + 15.363399927305815, + 15.363399927305814 ], - "timestamp": 2.5333306831394573 + "timestamp": 2.533330683142707 }, { - "x": 8.118709429041983, - "y": 0.9994031964377751, - "heading": -0.030602705565700945, - "angularVelocity": 7.671706888831663e-19, - "velocityX": 2.5210507217984657, - "velocityY": -0.12299007774670825, + "x": 8.118709429068772, + "y": 0.9994031964729742, + "heading": -0.030602705565700862, + "angularVelocity": 2.001673444928878e-19, + "velocityX": 2.521050721782785, + "velocityY": -0.12299007768226591, "moduleForcesX": [ - -68.70314364670283, - -68.70314364670283, - -68.70314364670283, - -68.70314364670283 + -68.7031436480408, + -68.7031436480408, + -68.7031436480408, + -68.7031436480408 ], "moduleForcesY": [ - 12.90231694942824, - 12.902316949428238, - 12.902316949428242, - 12.902316949428242 + 12.902316942304648, + 12.902316942304648, + 12.902316942304651, + 12.902316942304651 ], - "timestamp": 2.6004568236664385 + "timestamp": 2.600456823671251 }, { - "x": 8.271143977771962, - "y": 0.9938469245659062, - "heading": -0.030602705565700928, - "angularVelocity": 7.671359538796945e-19, - "velocityX": 2.270867169380966, - "velocityY": -0.08277359353968143, + "x": 8.271143977800422, + "y": 0.9938469246036682, + "heading": -0.030602705565700855, + "angularVelocity": 2.0013070310221862e-19, + "velocityX": 2.270867169353027, + "velocityY": -0.08277359349957805, "moduleForcesX": [ - -69.0326940172405, - -69.0326940172405, - -69.0326940172405, - -69.0326940172405 + -69.03269401838256, + -69.03269401838256, + -69.03269401838256, + -69.03269401838256 ], "moduleForcesY": [ - 11.096861571799984, - 11.096861571799986, - 11.096861571799984, - 11.096861571799984 + 11.096861564702607, + 11.096861564702607, + 11.096861564702607, + 11.096861564702605 ], - "timestamp": 2.6675829641934197 + "timestamp": 2.667582964199795 }, { - "x": 8.406731406019375, - "y": 0.9906547367603562, - "heading": -0.030602705565700914, - "angularVelocity": 7.670985863150012e-19, - "velocityX": 2.019890123027612, - "velocityY": -0.047555062461336506, + "x": 8.406731406048316, + "y": 0.9906547367990796, + "heading": -0.030602705565700848, + "angularVelocity": 2.0009206377793148e-19, + "velocityX": 2.0198901229876864, + "velocityY": -0.047555062445912684, "moduleForcesX": [ - -69.25164136036881, - -69.25164136036881, - -69.25164136036881, - -69.25164136036881 + -69.25164136135751, + -69.25164136135751, + -69.25164136135751, + -69.25164136135751 ], "moduleForcesY": [ - 9.717785426549383, - 9.717785426549383, - 9.717785426549389, - 9.717785426549389 + 9.71778541951536, + 9.717785419515362, + 9.717785419515362, + 9.717785419515362 ], - "timestamp": 2.734709104720401 + "timestamp": 2.734709104728339 }, { - "x": 8.5254345460703, - "y": 0.9895621634069518, - "heading": -0.0306027055657009, - "angularVelocity": 7.670595843658019e-19, - "velocityX": 1.7683593771223813, - "velocityY": -0.016276421448131698, + "x": 8.525434546098682, + "y": 0.9895621634450235, + "heading": -0.03060270556570084, + "angularVelocity": 2.0005241446773043e-19, + "velocityX": 1.7683593770728674, + "velocityY": -0.016276421457464545, "moduleForcesX": [ - -69.40442267327488, - -69.40442267327488, - -69.40442267327488, - -69.40442267327488 + -69.40442267414187, + -69.40442267414187, + -69.40442267414187, + -69.40442267414187 ], "moduleForcesY": [ - 8.630658704124505, - 8.630658704124507, - 8.630658704124507, - 8.630658704124508 + 8.63065869716772, + 8.63065869716772, + 8.630658697167716, + 8.630658697167716 ], - "timestamp": 2.801835245247382 + "timestamp": 2.801835245256883 }, { - "x": 8.627226437405287, - "y": 0.9903554058889321, - "heading": -0.030602705565700883, - "angularVelocity": 7.670197234878886e-19, - "velocityX": 1.516426991569238, - "velocityY": 0.011817191868226978, + "x": 8.627226437431862, + "y": 0.9903554059247525, + "heading": -0.030602705565700834, + "angularVelocity": 2.0001246831972175e-19, + "velocityX": 1.516426991507014, + "velocityY": 0.011817191834409059, "moduleForcesX": [ - -69.51524637407707, - -69.51524637407705, - -69.51524637407707, - -69.51524637407705 + -69.51524637484586, + -69.51524637484586, + -69.51524637484586, + -69.51524637484586 ], "moduleForcesY": [ - 7.751819786448097, - 7.751819786448097, - 7.751819786448097, - 7.751819786448097 + 7.751819779571535, + 7.751819779571536, + 7.751819779571532, + 7.751819779571534 ], - "timestamp": 2.868961385774363 + "timestamp": 2.868961385785427 }, { - "x": 8.712086900940639, - "y": 0.9928580419153559, - "heading": -0.03060270556570087, - "angularVelocity": 7.6697962024819e-19, - "velocityX": 1.2641939916274305, - "velocityY": 0.03728258479896153, + "x": 8.712086900964183, + "y": 0.992858041947287, + "heading": -0.030602705565700827, + "angularVelocity": 1.9997279446904402e-19, + "velocityX": 1.264193991552835, + "velocityY": 0.03728258474014907, "moduleForcesX": [ - -69.59819435728923, - -69.59819435728923, - -69.59819435728923, - -69.59819435728923 + -69.59819435797758, + -69.59819435797758, + -69.59819435797758, + -69.59819435797758 ], "moduleForcesY": [ - 7.026619700405902, - 7.026619700405903, - 7.0266197004059014, - 7.0266197004059014 + 7.026619693607424, + 7.026619693607423, + 7.026619693607425, + 7.026619693607425 ], - "timestamp": 2.9360875263013444 + "timestamp": 2.9360875263139707 }, { - "x": 8.780000437865695, - "y": 0.996921987964414, - "heading": -0.03060270556570085, - "angularVelocity": 7.669398274852111e-19, - "velocityX": 1.011730100850266, - "velocityY": 0.06054192922688254, + "x": 8.780000437885118, + "y": 0.9969219879908205, + "heading": -0.03060270556570082, + "angularVelocity": 1.9993387143813067e-19, + "velocityX": 1.0117301007653263, + "velocityY": 0.06054192914316773, "moduleForcesX": [ - -69.66190364637255, - -69.66190364637255, - -69.66190364637255, - -69.66190364637255 + -69.66190364699403, + -69.66190364699403, + -69.66190364699403, + -69.66190364699403 ], "moduleForcesY": [ - 6.417908736725351, - 6.417908736725351, - 6.417908736725356, - 6.417908736725355 + 6.417908730000572, + 6.417908730000572, + 6.41790873000057, + 6.41790873000057 ], - "timestamp": 3.0032136668283256 + "timestamp": 3.0032136668425147 }, { - "x": 8.830954883407468, - "y": 1.0024211476734168, - "heading": -0.030602705565700838, - "angularVelocity": 7.669008842488089e-19, - "velocityX": 0.75908498748391, - "velocityY": 0.08192277502968102, + "x": 8.830954883421578, + "y": 1.0024211476926257, + "heading": -0.030602705565700813, + "angularVelocity": 1.99896148062903e-19, + "velocityX": 0.7590849873871007, + "velocityY": 0.08192277492054269, "moduleForcesX": [ - -69.71190806731377, - -69.71190806731377, - -69.71190806731377, - -69.71190806731377 + -69.71190806787894, + -69.71190806787894, + -69.71190806787892, + -69.71190806787892 ], "moduleForcesY": [ - 5.89957801675687, - 5.899578016756869, - 5.899578016756865, - 5.899578016756865 + 5.89957801010069, + 5.899578010100691, + 5.8995780101006865, + 5.8995780101006865 ], - "timestamp": 3.0703398073553068 + "timestamp": 3.0703398073710586 }, { - "x": 8.86494051215884, - "y": 1.0092468204121565, - "heading": -0.030602705565700824, - "angularVelocity": 7.668631929139065e-19, - "velocityX": 0.5062949915570888, - "velocityY": 0.10168427210552212, + "x": 8.86494051216642, + "y": 1.009246820422566, + "heading": -0.030602705565700807, + "angularVelocity": 1.9985996963971072e-19, + "velocityX": 0.5062949914480063, + "velocityY": 0.10168427197206922, "moduleForcesX": [ - -69.7518852496981, - -69.7518852496981, - -69.7518852496981, - -69.7518852496981 + -69.75188525021527, + -69.75188525021527, + -69.75188525021527, + -69.75188525021527 ], "moduleForcesY": [ - 5.452754058783733, - 5.452754058783732, - 5.452754058783737, - 5.452754058783736 + 5.452754052190986, + 5.452754052190985, + 5.452754052190986, + 5.452754052190985 ], - "timestamp": 3.137465947882288 + "timestamp": 3.1374659478996025 }, { "x": 8.881949424743652, "y": 1.017304301261902, - "heading": -0.030602705565700813, - "angularVelocity": -6.192148052914439e-17, - "velocityX": 0.25338731604818493, - "velocityY": 0.12003491913118446, + "heading": -0.030602705565700807, + "angularVelocity": -6.178266080762978e-17, + "velocityX": 0.25338731592940406, + "velocityY": 0.12003491897331502, "moduleForcesX": [ - -69.78435636275017, - -69.78435636275017, - -69.78435636275017, - -69.78435636275017 + -69.78435636322597, + -69.78435636322598, + -69.78435636322598, + -69.78435636322598 ], "moduleForcesY": [ - 5.063460762339646, - 5.063460762339647, - 5.063460762339649, - 5.06346076233965 + 5.063460755805268, + 5.063460755805267, + 5.06346075580527, + 5.06346075580527 ], - "timestamp": 3.204592088409269 + "timestamp": 3.2045920884281465 }, { - "x": 8.880530486218973, - "y": 1.027348591072611, - "heading": -0.034530809902176336, - "angularVelocity": -0.054161349122254336, - "velocityX": -0.019564557922623505, - "velocityY": 0.13849232110933965, + "x": 8.880530486210139, + "y": 1.0273485910590279, + "heading": -0.03453080986256461, + "angularVelocity": -0.054161348577286314, + "velocityX": -0.01956455804488396, + "velocityY": 0.1384923209251697, "moduleForcesX": [ - -69.96707712828831, - -69.96918542605992, - -69.4046006596549, - -69.48963562370783 + -69.96707712791041, + -69.96918542564208, + -69.40460066617938, + -69.48963562834733 ], "moduleForcesY": [ - 0.7821062724142991, - 0.9492571184496107, - 8.889772333230777, - 8.233787400699313 + 0.7821063079634264, + 0.9492571479967624, + 8.889772282325291, + 8.233787361314777 ], - "timestamp": 3.277118057576761 + "timestamp": 3.277118057594007 }, { - "x": 8.859315047264229, - "y": 1.03873366112627, - "heading": -0.04230269871701783, - "angularVelocity": -0.10716008216259976, - "velocityX": -0.29252196417731824, - "velocityY": 0.15697921977944768, + "x": 8.859315047246675, + "y": 1.0387336610970788, + "heading": -0.04230269859815422, + "angularVelocity": -0.10716008107224526, + "velocityX": -0.292521964304125, + "velocityY": 0.1569792195677669, "moduleForcesX": [ - -69.96360527609723, - -69.96507878923691, - -69.41623345810719, - -69.49123274928873 + -69.96360527567995, + -69.96507878877915, + -69.41623346455654, + -69.49123275406077 ], "moduleForcesY": [ - 0.849054357453386, - 1.0605014457335615, - 8.776783714333206, - 8.198715576096298 + 0.8490543935707846, + 1.0605014747496089, + 8.776783663369878, + 8.198715535403977 ], - "timestamp": 3.3496440267442527 + "timestamp": 3.3496440267598677 }, { - "x": 8.81830267354523, - "y": 1.0514616761704456, - "heading": -0.053822469418188024, - "angularVelocity": -0.15883649447922438, - "velocityX": -0.5654853591032976, - "velocityY": 0.17549596634529624, + "x": 8.818302673519035, + "y": 1.0514616761236624, + "heading": -0.05382246917997862, + "angularVelocity": -0.15883649283719747, + "velocityX": -0.5654853592351373, + "velocityY": 0.17549596610668067, "moduleForcesX": [ - -69.95969350130241, - -69.9600439550638, - -69.43034005880952, - -69.49219040870635 + -69.95969350084123, + -69.96004395456353, + -69.43034006513656, + -69.49219041365508 ], "moduleForcesY": [ - 0.916057569851737, - 1.1945950847132014, - 8.639230967068034, - 8.165662210562154 + 0.9160576067978011, + 1.1945951128891326, + 8.639230916283404, + 8.165662168184191 ], - "timestamp": 3.4221699959117444 + "timestamp": 3.4221699959257283 }, { - "x": 8.75749289682575, - "y": 1.0655348258499984, - "heading": -0.06897986858552595, - "angularVelocity": -0.2089927144919403, - "velocityX": -0.838455210147377, - "velocityY": 0.1940429040948802, + "x": 8.757492896791033, + "y": 1.0655348257834631, + "heading": -0.06897986818711133, + "angularVelocity": -0.20899271228765146, + "velocityX": -0.8384552102837445, + "velocityY": 0.1940429038268972, "moduleForcesX": [ - -69.9551810729799, - -69.95383968280234, - -69.44688305083558, - -69.49295928173186 + -69.95518107246687, + -69.95383968225862, + -69.44688305699783, + -69.49295928690098 ], "moduleForcesY": [ - 0.9882804241447996, - 1.352618156589803, - 8.475469948022155, - 8.130018720639756 + 0.9882804621739782, + 1.3526181836391635, + 8.475469897619213, + 8.130018676173123 ], - "timestamp": 3.494695965079236 + "timestamp": 3.494695965091589 }, { - "x": 8.67688521696236, - "y": 1.080955321413936, - "heading": -0.08764661363140669, - "angularVelocity": -0.2573801530723142, - "velocityX": -1.1114319572514513, - "velocityY": 0.2126203309097312, + "x": 8.676885216919311, + "y": 1.0809553213253251, + "heading": -0.08764661303153386, + "angularVelocity": -0.2573801503002974, + "velocityX": -1.1114319573913516, + "velocityY": 0.212620330610125, "moduleForcesX": [ - -69.94983324196707, - -69.94615057660823, - -69.46581320027535, - -69.49411065050936 + -69.94983324139118, + -69.94615057602354, + -69.46581320622545, + -69.49411065593223 ], "moduleForcesY": [ - 1.0723396108261634, - 1.5360717401888286, - 8.283336243627176, - 8.085785369496797 + 1.072339650088805, + 1.5360717657814353, + 8.283336193853913, + 8.085785322585087 ], - "timestamp": 3.567221934246728 + "timestamp": 3.5672219342574496 }, { - "x": 8.576479109537585, - "y": 1.0977253883199334, - "heading": -0.1096713977003478, - "angularVelocity": -0.3036813478255371, - "velocityX": -1.3844159351099252, - "velocityY": 0.23122844270129664, + "x": 8.57647910948646, + "y": 1.0977253882067866, + "heading": -0.10967139685743804, + "angularVelocity": -0.30368134448125444, + "velocityX": -1.3844159352524572, + "velocityY": 0.2312284423681809, "moduleForcesX": [ - -69.94330252963142, - -69.9365583049015, - -69.4870568794036, - -69.49637643933679 + -69.94330252897542, + -69.93655830427998, + -69.48705688510655, + -69.49637644504644 ], "moduleForcesY": [ - 1.176823061341541, - 1.7470252667944042, - 8.060012105446882, - 8.025018376878974 + 1.1768231019990443, + 1.7470252906666641, + 8.060012056454902, + 8.025018327105174 ], - "timestamp": 3.6397479034142197 + "timestamp": 3.6397479034233102 }, { - "x": 8.456274044587177, - "y": 1.1158472524014584, - "heading": -0.13487291738284943, - "angularVelocity": -0.347482701327652, - "velocityX": -1.6574072202027315, - "velocityY": 0.24986724465107038, + "x": 8.456274044528195, + "y": 1.1158472522610625, + "heading": -0.13487291625482856, + "angularVelocity": -0.347482697404215, + "velocityX": -1.6574072203483379, + "velocityY": 0.2498672442809662, "moduleForcesX": [ - -69.93505805724766, - -69.92449408332656, - -69.51050132115871, - -69.50070530398553 + -69.93505805649114, + -69.92449408268004, + -69.510501326562, + -69.50070530998221 ], "moduleForcesY": [ - 1.3131272162432313, - 1.98838442321693, - 7.801780109725148, - 7.936938199533164 + 1.3131272581552487, + 1.9883844449565293, + 7.801780061823989, + 7.936938146668492 ], - "timestamp": 3.7122738725817115 + "timestamp": 3.712273872589171 }, { - "x": 8.316269527386241, - "y": 1.1353231154725127, - "heading": -0.16302980404777723, - "angularVelocity": -0.38823178770838385, - "velocityX": -1.930405326643809, - "velocityY": 0.26853640557469155, + "x": 8.316269527319614, + "y": 1.1353231153018994, + "heading": -0.16302980259206615, + "angularVelocity": -0.3882317831987629, + "velocityX": -1.9304053267926182, + "velocityY": 0.26853640516407645, "moduleForcesX": [ - -69.92425253763372, - -69.90915580434691, - -69.53597671993317, - -69.50834196529713 + -69.92425253674794, + -69.90915580369078, + -69.53597672500005, + -69.50834197156578 ], "moduleForcesY": [ - 1.4968639656801608, - 2.264388307053663, - 7.503564176470051, - 7.80642632120717 + 1.4968640086183072, + 2.2643883263150886, + 7.50356412984306, + 7.806426265009984 ], - "timestamp": 3.7847998417492033 + "timestamp": 3.7847998417550315 }, { - "x": 8.156465184614108, - "y": 1.1561551126608645, - "heading": -0.1938654641731384, - "angularVelocity": -0.42516715708305197, - "velocityX": -2.2034085804916796, - "velocityY": 0.28723500599140817, + "x": 8.156465184540169, + "y": 1.1561551124568015, + "heading": -0.19386546234654622, + "angularVelocity": -0.42516715197872557, + "velocityX": -2.203408580642062, + "velocityY": 0.28723500553664255, "moduleForcesX": [ - -69.9094604865732, - -69.88935388472542, - -69.5632321514312, - -69.52093877070061 + -69.90946048551345, + -69.88935388407138, + -69.56323215616419, + -69.52093877722275 ], "moduleForcesY": [ - 1.7503444073984757, - 2.581564547481693, - 7.158055486956702, - 7.611351867199961 + 1.7503444512317394, + 2.5815645641729006, + 7.1580554414148985, + 7.611351807216961 ], - "timestamp": 3.857325810916695 + "timestamp": 3.857325810920892 }, { - "x": 7.976860949117206, - "y": 1.1783452360639273, - "heading": -0.2270240484884978, - "angularVelocity": -0.45719601814734345, - "velocityX": -2.4764127602640795, - "velocityY": 0.3059610737751436, + "x": 7.976860949036181, + "y": 1.1783452358227742, + "heading": -0.22702404624732822, + "angularVelocity": -0.45719601244123276, + "velocityX": -2.4764127604175035, + "velocityY": 0.30596107327060007, "moduleForcesX": [ - -69.88812974316788, - -69.86320182607882, - -69.59189618603347, - -69.5407034064261 + -69.8881297418902, + -69.86320182545145, + -69.59189619041469, + -69.54070341308756 ], "moduleForcesY": [ - 2.107233171390243, - 2.950662034921671, - 6.753983199950573, - 7.317496840416227 + 2.1072332152187867, + 2.95066204873304, + 6.753983155440134, + 7.31749677667182 ], - "timestamp": 3.929851780084187 + "timestamp": 3.9298517800867527 }, { - "x": 7.777457476125625, - "y": 1.201895191338467, - "heading": -0.26202980984331203, - "angularVelocity": -0.4826651991962036, - "velocityX": -2.74940790561615, - "velocityY": 0.3247106594351006, + "x": 7.7774574760376565, + "y": 1.2018951910560491, + "heading": -0.26202980714373064, + "angularVelocity": -0.4826651928862579, + "velocityX": -2.7494079057737237, + "velocityY": 0.32471065887340905, "moduleForcesX": [ - -69.85533117072832, - -69.82743416827404, - -69.62139360036666, - -69.57054322743649 + -69.8553311691883, + -69.82743416769303, + -69.62139360441714, + -69.57054323404049 ], "moduleForcesY": [ - 2.6219385333270773, - 3.3908613060937713, - 6.272491751359185, - 6.868108041742767 + 2.621938575781362, + 3.3908613169810025, + 6.272491707304398, + 6.86810797438961 ], - "timestamp": 4.0023777492516786 + "timestamp": 4.002377749252613 }, { - "x": 7.558257164116274, - "y": 1.2268061069502, - "heading": -0.2982123205048736, - "angularVelocity": -0.49889041232006653, - "velocityX": -3.022369980373853, - "velocityY": 0.3434758045660868, + "x": 7.55825716402174, + "y": 1.226806106621595, + "heading": -0.29821231730362563, + "angularVelocity": -0.4988904054140694, + "velocityX": -3.0223699805323463, + "velocityY": 0.34347580393694493, "moduleForcesX": [ - -69.80056727463466, - -69.77570886950674, - -69.6507199544232, - -69.61392322278483 + -69.80056727283858, + -69.77570886897958, + -69.65071995819655, + -69.61392322895297 ], "moduleForcesY": [ - 3.39043477997722, - 3.9399463888437256, - 5.678903958822243, - 6.160009083534089 + 3.3904348183931043, + 3.9399463970685407, + 5.678903913889345, + 6.160009013377018 ], - "timestamp": 4.07490371841917 + "timestamp": 4.074903718418473 }, { - "x": 7.319267013948266, - "y": 1.2530778743434587, - "heading": -0.3345519884568234, - "angularVelocity": -0.5010573229066131, - "velocityX": -3.2952355261337996, - "velocityY": 0.3622394529136627, + "x": 7.319267013847545, + "y": 1.2530778739626474, + "heading": -0.33455198471356234, + "angularVelocity": -0.501057315444351, + "velocityX": -3.2952355262932205, + "velocityY": 0.3622394522019477, "moduleForcesX": [ - -69.69826365815047, - -69.69360285262502, - -69.67768220142874, - -69.67276264637083 + -69.69826365639196, + -69.69360285213232, + -69.677682204989, + -69.67276265133394 ], "moduleForcesY": [ - 4.602649202702627, - 4.682797548651256, - 4.9016741603909, - 4.9806442793840855 + 4.602649230854164, + 4.682797554881524, + 4.901674111951051, + 4.980644209624238 ], - "timestamp": 4.147429687586662 + "timestamp": 4.147429687584333 }, { - "x": 7.060508520408783, - "y": 1.2807073809011449, - "heading": -0.3693010360245331, - "angularVelocity": -0.479125587246758, - "velocityX": -3.5678046982302343, - "velocityY": 0.38096018398438164, + "x": 7.060508520302209, + "y": 1.2807073804603784, + "heading": -0.36930103170788803, + "angularVelocity": -0.47912557935144634, + "velocityX": -3.5678046983912037, + "velocityY": 0.38096018316622676, "moduleForcesX": [ - -69.47236753682394, - -69.53951749408205, - -69.69564436263259, - -69.73202523829484 + -69.47236753665878, + -69.53951749340459, + -69.69564436604739, + -69.73202524072254 ], "moduleForcesY": [ - 6.700012628800592, - 5.851281299757406, - 3.768542257976925, - 2.8040874089418844 + 6.7000126322527995, + 5.851281306796728, + 3.7685421990988033, + 2.8040873490370677 ], - "timestamp": 4.219955656754154 + "timestamp": 4.2199556567501935 }, { - "x": 6.7820675423003, - "y": 1.3096827333084073, - "heading": -0.398737971474847, - "angularVelocity": -0.40588131104575736, - "velocityX": -3.839190035026568, - "velocityY": 0.39951692807238764, + "x": 6.782067542187506, + "y": 1.3096827327970242, + "heading": -0.3987379665925305, + "angularVelocity": -0.405881303255151, + "velocityX": -3.8391900351986807, + "velocityY": 0.3995169271076376, "moduleForcesX": [ - -68.80858735271256, - -69.12938539141281, - -69.67366590353845, - -69.61858416268272 + -68.80858736495749, + -69.12938538991284, + -69.67366590614556, + -69.6185841628369 ], "moduleForcesY": [ - 10.998366071640108, - 8.339611523596144, - 1.7520720276263266, - -2.1336448340952816 + 10.99836599716967, + 8.33961153542166, + 1.752071940836427, + -2.13364483389887 ], - "timestamp": 4.292481625921646 + "timestamp": 4.292481625916054 }, { - "x": 6.484732150451617, - "y": 1.3400281940076402, - "heading": -0.4090628910860552, - "angularVelocity": -0.142361690988409, - "velocityX": -4.099709321525832, - "velocityY": 0.4184082067095552, + "x": 6.484732150295716, + "y": 1.3400281933976181, + "heading": -0.40906288611345654, + "angularVelocity": -0.14236168974674643, + "velocityX": -4.099709322212432, + "velocityY": 0.41840820535881235, "moduleForcesX": [ - -65.20316881632876, - -65.7489206217446, - -69.31239778456624, - -65.86566076035609 + -65.20316900335862, + -65.74892075037127, + -69.31239777710773, + -65.86566098161668 ], "moduleForcesY": [ - 23.606521909357, - 20.444429351523326, - -3.7507078277888724, - -21.00209915007702 + 23.606521394196665, + 20.444428940438716, + -3.750707987782145, + -21.00209845812331 ], - "timestamp": 4.365007595089137 + "timestamp": 4.365007595081914 }, { - "x": 6.1842404767603805, - "y": 1.3742213267083638, - "heading": -0.40911116386317103, - "angularVelocity": -0.0006655929961390624, - "velocityX": -4.1432286550667365, - "velocityY": 0.4714605415577361, + "x": 6.184240476626105, + "y": 1.3742213262250316, + "heading": -0.40911115889080796, + "angularVelocity": -0.0006655929994023552, + "velocityX": -4.143228654861732, + "velocityY": 0.4714605433153216, "moduleForcesX": [ - -13.325449562031311, - -4.164768682569861, - -18.043544953356022, - -8.922855853125498 + -13.325455797596309, + -4.164751278280683, + -18.043558746247182, + -8.922852319147749 ], "moduleForcesY": [ - 19.90592667522208, - 16.82639877386449, - 10.611237303210721, - 6.85137196260812 + 19.90590611882484, + 16.82641222119337, + 10.611221364447623, + 6.851398187145905 ], - "timestamp": 4.437533564256629 + "timestamp": 4.437533564247774 }, { - "x": 5.886052139153465, - "y": 1.4246963553310026, - "heading": -0.4091589633873657, - "angularVelocity": -0.0006590677069691712, - "velocityX": -4.111469878027867, - "velocityY": 0.6959580023818284, + "x": 5.886052139053873, + "y": 1.424696355011759, + "heading": -0.40915895841469946, + "angularVelocity": -0.0006590677028043638, + "velocityX": -4.111469877642081, + "velocityY": 0.6959580046600466, "moduleForcesX": [ - 8.110433382725144, - 8.111187384355791, - 8.110199033033277, - 8.110953042580169 + 8.110433400741913, + 8.11118742383485, + 8.110199088142462, + 8.110953115410458 ], "moduleForcesY": [ - 57.33325913716358, - 57.33311245310866, - 57.33314368701091, - 57.33299700322065 + 57.33325927042727, + 57.33311258568195, + 57.333143822794604, + 57.33299713881585 ], - "timestamp": 4.510059533424121 + "timestamp": 4.510059533413634 }, { - "x": 5.591555275132608, - "y": 1.4934998764417577, - "heading": -0.4092089626864391, - "angularVelocity": -0.0006893985650598928, - "velocityX": -4.060571232640143, - "velocityY": 0.94867427351207, + "x": 5.5915552750776545, + "y": 1.4934998762835463, + "heading": -0.4092089577134744, + "angularVelocity": -0.0006893985609604924, + "velocityX": -4.060571232115967, + "velocityY": 0.9486742757538188, "moduleForcesX": [ - 13.000242789451551, - 12.99626661282106, - 13.001163654267753, - 12.99718771199267 + 13.000242825406785, + 12.996266648691245, + 13.001163689713444, + 12.997187747343835 ], "moduleForcesY": [ - 64.53930421343945, - 64.54015945588058, - 64.53937921708196, - 64.54023445819007 + 64.53930420549774, + 64.54015944796213, + 64.53937920928017, + 64.5402344504135 ], - "timestamp": 4.582585502591613 + "timestamp": 4.582585502579494 }, { "x": 5.302053451538088, "y": 1.5809556245803824, - "heading": -0.4092627987960467, - "angularVelocity": -0.000742301140211202, - "velocityX": -3.991698793103366, - "velocityY": 1.2058542497602232, + "heading": -0.40926279382276776, + "angularVelocity": -0.0007423011358945286, + "velocityX": -3.9916987924354075, + "velocityY": 1.205854251968866, "moduleForcesX": [ - 17.591944196413664, - 17.584790178826015, - 17.593089676218266, - 17.5859364455025 + 17.591944233654857, + 17.584790216066136, + 17.59308971312774, + 17.585936482408147 ], "moduleForcesY": [ - 65.6787937062123, - 65.68074285605304, - 65.67871632504414, - 65.68066538032537 + 65.67879369894756, + 65.68074284879431, + 65.67871631788772, + 65.68066537317567 ], - "timestamp": 4.6551114717591044 + "timestamp": 4.655111471745355 }, { - "x": 5.060781264234282, - "y": 1.668503203961895, - "heading": -0.4093136867686553, - "angularVelocity": -0.0008266775577253413, - "velocityX": -3.9194782641972647, - "velocityY": 1.4222146294751892, + "x": 5.060781264318389, + "y": 1.6685032040805434, + "heading": -0.40931368179496846, + "angularVelocity": -0.0008266775512222434, + "velocityX": -3.9194782634197978, + "velocityY": 1.4222146316163005, "moduleForcesX": [ - 21.736557880357715, - 21.723118914680356, - 21.73784859212575, - 21.72441246515569 + 21.736557916865454, + 21.723118951579323, + 21.73784862823763, + 21.724412501654832 ], "moduleForcesY": [ - 65.09863070377499, - 65.10314246149298, - 65.09850389768496, - 65.10301496712766 + 65.09863069400774, + 65.10314245160757, + 65.09850388812552, + 65.10301495745126 ], - "timestamp": 4.716668692144205 + "timestamp": 4.716668692121208 }, { - "x": 4.824591541574993, - "y": 1.768967581170856, - "heading": -0.4093625786347868, - "angularVelocity": -0.0007942507121868569, - "velocityX": -3.836913382080745, - "velocityY": 1.632048630208832, + "x": 4.824591541749403, + "y": 1.7689675814031505, + "heading": -0.4093625736606763, + "angularVelocity": -0.0007942507054235842, + "velocityX": -3.8369133811902225, + "velocityY": 1.6320486323002081, "moduleForcesX": [ - 24.840611640815233, - 24.84563714723139, - 24.840370639682614, - 24.84539654710804 + 24.840611678634826, + 24.845637185091626, + 24.840370677624488, + 24.845396585090324 ], "moduleForcesY": [ - 63.138133963812095, - 63.136150247952436, - 63.13804254630454, - 63.13605868744737 + 63.13813395857038, + 63.136150242692494, + 63.13804254100993, + 63.13605868213461 ], - "timestamp": 4.778225912529306 + "timestamp": 4.778225912497061 }, { - "x": 4.5940300417851905, - "y": 1.8817568503359232, - "heading": -0.40940943255755435, - "angularVelocity": -0.00076114422441599, - "velocityX": -3.7454826314023215, - "velocityY": 1.8322670916501005, + "x": 4.5940300420581, + "y": 1.8817568506816633, + "heading": -0.4094094275830447, + "angularVelocity": -0.0007611442180464456, + "velocityX": -3.745482630364935, + "velocityY": 1.8322670937682957, "moduleForcesX": [ - 27.508225922613867, - 27.513128573012004, - 27.508193765462636, - 27.51309679763414 + 27.50822597067515, + 27.513128620991576, + 27.508193814141755, + 27.513096846198717 ], "moduleForcesY": [ - 60.24514716829222, - 60.242914588066654, - 60.24482580432994, - 60.242593041950194 + 60.245147185521205, + 60.24291460533314, + 60.24482582123695, + 60.24259305890976 ], - "timestamp": 4.839783132914407 + "timestamp": 4.839783132872914 }, { - "x": 4.369349606253567, - "y": 2.0058517685581454, - "heading": -0.4094544926877705, - "angularVelocity": -0.0007320039783130824, - "velocityX": -3.6499444602278026, - "velocityY": 2.015927903272623, + "x": 4.36934960664493, + "y": 2.0058517690384203, + "heading": -0.4094544877128489, + "angularVelocity": -0.0007320039717315139, + "velocityX": -3.6499444588518815, + "velocityY": 2.0159279057610187, "moduleForcesX": [ - 28.744538248232754, - 28.74851367433131, - 28.744577870233446, - 28.74855354416703 + 28.744538353811407, + 28.74851378007089, + 28.744577977045132, + 28.74855365043348 ], "moduleForcesY": [ - 55.26315032046569, - 55.261123506293146, - 55.262531653571756, - 55.260504693222536 + 55.2631504404134, + 55.261123626177344, + 55.2625317726504, + 55.26050481260123 ], - "timestamp": 4.901340353299508 + "timestamp": 4.901340353248767 }, { - "x": 4.149973859162333, - "y": 2.13910073076137, - "heading": -0.4094996155694483, - "angularVelocity": -0.0007330233788204265, - "velocityX": -3.563769541880253, - "velocityY": 2.1646357871525597, + "x": 4.149973859634745, + "y": 2.1391007313038166, + "heading": -0.40949961058910517, + "angularVelocity": -0.0007330232908571056, + "velocityX": -3.5637695410990893, + "velocityY": 2.1646357884876952, "moduleForcesX": [ - 25.92928590684574, - 25.929168249602895, - 25.929292292013468, - 25.929174634983514 + 25.92928571690531, + 25.929168021968326, + 25.929292070124113, + 25.92917457379335 ], "moduleForcesY": [ - 44.74476438288918, - 44.74482729836071, - 44.74480699802511, - 44.744869913360205 + 44.74476401967033, + 44.744826943195974, + 44.74480673058518, + 44.74486953719506 ], - "timestamp": 4.962897573684609 + "timestamp": 4.962897573624621 }, { - "x": 3.9353204017277856, - "y": 2.278088242273816, - "heading": -0.41252140844778834, - "angularVelocity": -0.04908917036026353, - "velocityX": -3.4870557197299563, - "velocityY": 2.2578587961400522, + "x": 3.9353204020821484, + "y": 2.2780882424747335, + "heading": -0.4125214036176848, + "angularVelocity": -0.04908917280838613, + "velocityX": -3.48705572217154, + "velocityY": 2.257858790931102, "moduleForcesX": [ - 24.953488052198967, - 20.4567569278476, - 25.589392523319265, - 21.330260691348453 + 24.953505556486252, + 20.45677256337718, + 25.589356554115078, + 21.330259655702005 ], "moduleForcesY": [ - 25.14686564166064, - 27.7645218324562, - 28.406596524112476, - 30.881756387142097 + 25.146827347937002, + 27.764534745643168, + 28.406606309810808, + 30.88176412187559 ], - "timestamp": 5.02445479406971 + "timestamp": 5.024454794000474 }, { - "x": 3.7318642433739195, - "y": 2.4104934513603675, - "heading": -0.43615839467618756, - "angularVelocity": -0.38398397589611083, - "velocityX": -3.305155058676315, - "velocityY": 2.150928977270674, + "x": 3.7318642436374203, + "y": 2.4104934515239713, + "heading": -0.4361583914201127, + "angularVelocity": -0.3839840015242407, + "velocityX": -3.3051550606489273, + "velocityY": 2.150928976987681, "moduleForcesX": [ - 49.89592032127635, - 34.025418039882794, - 66.93891398556973, - 68.06860648029641 + 49.89592057158015, + 34.025417576241814, + 66.93891416475311, + 68.0686068113683 ], "moduleForcesY": [ - -47.83606477558671, - -59.36994474295037, - -17.100092585775965, - -4.390644167479697 + -47.836064483916026, + -59.369944976556134, + -17.100091773085392, + -4.390638937539682 ], - "timestamp": 5.086012014454811 + "timestamp": 5.086012014376327 }, { - "x": 3.540415993553555, - "y": 2.5354156131468453, - "heading": -0.46498951807167194, - "angularVelocity": -0.4683629835017646, - "velocityX": -3.110086008150945, - "velocityY": 2.0293665146828603, + "x": 3.5404159937543107, + "y": 2.5354156132801298, + "heading": -0.4649895158550848, + "angularVelocity": -0.46836300045892293, + "velocityX": -3.1100860096374956, + "velocityY": 2.029366514495222, "moduleForcesX": [ - 55.83155696216655, - 54.18327481859195, - 62.40116195557468, - 62.36184788712686 + 55.83155730522792, + 54.18327547289772, + 62.40116176551297, + 62.3618475536798 ], "moduleForcesY": [ - -41.50061361131458, - -43.48191734914588, - -30.734501757153353, - -30.59101829583353 + -41.50061314279684, + -43.48191653513505, + -30.73450213526439, + -30.591018982325306 ], - "timestamp": 5.147569234839912 + "timestamp": 5.14756923475218 }, { - "x": 3.361022505593955, - "y": 2.652736487945131, - "heading": -0.4956638148847116, - "angularVelocity": -0.49830542414467005, - "velocityX": -2.9142558230751625, - "velocityY": 1.9058832426858676, + "x": 3.361022505744376, + "y": 2.652736488054298, + "heading": -0.4956638133831861, + "angularVelocity": -0.4983054358359662, + "velocityX": -2.9142558243306804, + "velocityY": 1.9058832425804164, "moduleForcesX": [ - 57.73901804404707, - 57.438460533401326, - 60.30587505828676, - 60.21056114711604 + 57.7390182838169, + 57.43846086192714, + 60.30587488496412, + 60.21056094689106 ], "moduleForcesY": [ - -39.07130416952509, - -39.474125567373356, - -34.97681089661358, - -35.097625013264334 + -39.071303811919336, + -39.47412508944996, + -34.976811192657685, + -35.09762535851428 ], - "timestamp": 5.209126455225013 + "timestamp": 5.209126455128033 }, { - "x": 3.1936838062667205, - "y": 2.7624068712077934, - "heading": -0.5266594310996341, - "angularVelocity": -0.5035252732516518, - "velocityX": -2.7184252030934304, - "velocityY": 1.7816006404539642, + "x": 3.1936838063800033, + "y": 2.762406871298198, + "heading": -0.5266594301029957, + "angularVelocity": -0.5035252815293935, + "velocityX": -2.7184252041051566, + "velocityY": 1.7816006404168396, "moduleForcesX": [ - 58.700566811459886, - 58.67734780171254, - 59.16682840463337, - 59.14969530624501 + 58.70056697929468, + 58.67734799260074, + 59.16682827716417, + 59.14969518046466 ], "moduleForcesY": [ - -37.749793809130026, - -37.78083859729802, - -37.014524694721786, - -37.03675261323169 + -37.749793546335056, + -37.78083830094146, + -37.01452489709346, + -37.03675281488077 ], - "timestamp": 5.270683675610114 + "timestamp": 5.270683675503887 }, { - "x": 3.038390509303798, - "y": 2.8643948429617936, - "heading": -0.5570773161987834, - "angularVelocity": -0.4941400035435474, - "velocityX": -2.5227470634868117, - "velocityY": 1.6567994967278303, + "x": 3.038390509385735, + "y": 2.864394843036019, + "heading": -0.5570773155618112, + "angularVelocity": -0.49414000946072106, + "velocityX": -2.5227470643750314, + "velocityY": 1.6567994967139286, "moduleForcesX": [ - 59.30031933124251, - 59.30307854027269, - 58.44288594465124, - 58.46463470158717 + 59.30031945388296, + 59.30307866344703, + 58.44288585276952, + 58.4646346161408 ], "moduleForcesY": [ - -36.885288676049825, - -36.88814111006628, - -38.229406656940725, - -38.203170518961 + -36.885288477772534, + -36.888140912141004, + -38.22940679661343, + -38.20317065012603 ], - "timestamp": 5.332240895995215 + "timestamp": 5.33224089587974 }, { - "x": 2.895129799571264, - "y": 2.9586740032068897, - "heading": -0.5863104114525834, - "angularVelocity": -0.4748930356337662, - "velocityX": -2.3272771063459046, - "velocityY": 1.5315694837305227, + "x": 2.8951297996314556, + "y": 2.958674003265299, + "heading": -0.5863104110705085, + "angularVelocity": -0.474893039846042, + "velocityX": -2.3272771070488116, + "velocityY": 1.5315694837037004, "moduleForcesX": [ - 59.717187192796516, - 59.657546410038414, - 57.934272823511364, - 57.95135152475748 + 59.71718728545176, + 59.65754649481836, + 57.93427275714158, + 57.95135146376398 ], "moduleForcesY": [ - -36.2638573648305, - -36.37433120370207, - -39.04885966973737, - -39.03512982178747 + -36.26385721152638, + -36.374331064721694, + -39.04885976771552, + -39.03512991256516 ], - "timestamp": 5.3937981163803155 + "timestamp": 5.393798116255593 }, { - "x": 2.763886371067874, - "y": 3.045218511804947, - "heading": -0.6139222009668763, - "angularVelocity": -0.4485548460699908, - "velocityX": -2.132055795929944, - "velocityY": 1.4059196964488687, + "x": 2.7638863711095008, + "y": 3.0452185118515818, + "heading": -0.6139222007620838, + "angularVelocity": -0.4485548490174231, + "velocityX": -2.1320557965518465, + "velocityY": 1.4059196964688137, "moduleForcesX": [ - 60.015428697215164, - 59.85831037769707, - 57.55335306922513, - 57.534003758004665 + 60.01542876878009, + 59.8583104385353, + 57.5533530210112, + 57.53400371300751 ], "moduleForcesY": [ - -35.80957668058033, - -36.086111428235434, - -39.64516852205637, - -39.68654602916551 + -35.80957656014014, + -36.08611132735859, + -39.645168591728385, + -39.68654609453473 ], - "timestamp": 5.455355336765416 + "timestamp": 5.455355336631446 }, { - "x": 2.644643305476117, - "y": 3.1240017980714323, - "heading": -0.6395960472514493, - "angularVelocity": -0.41707286527865206, - "velocityX": -1.937109324394044, - "velocityY": 1.2798382671215223, + "x": 2.6446433055054834, + "y": 3.1240017981097368, + "heading": -0.6395960471657398, + "angularVelocity": -0.417072867275884, + "velocityX": -1.9371093248842417, + "velocityY": 1.2798382671784732, "moduleForcesX": [ - 60.22326407527744, - 59.95837931731987, - 57.26239607352692, - 57.186270668215975 + 60.22326413098716, + 59.958379361963274, + 57.26239603856264, + 57.186270634486874 ], "moduleForcesY": [ - -35.49033319006886, - -35.95108026507241, - -40.09166456022141, - -40.21383292620102 + -35.49033309517584, + -35.95108019063276, + -40.0916646099415, + -40.21383297424875 ], - "timestamp": 5.516912557150517 + "timestamp": 5.516912557007299 }, { - "x": 2.537383808831169, - "y": 3.1949980499443154, - "heading": -0.6631036099321576, - "angularVelocity": -0.38188148414867673, - "velocityX": -1.7424356716228648, - "velocityY": 1.1533375196074727, + "x": 2.537383808849767, + "y": 3.194998049974314, + "heading": -0.6631036099218587, + "angularVelocity": -0.38188148543114175, + "velocityX": -1.7424356720595673, + "velocityY": 1.153337519645833, "moduleForcesX": [ - 60.36537448302678, - 59.99512146624315, - 57.04267536961116, - 56.89878422586106 + 60.36537452641838, + 59.995121499410935, + 57.04267534461395, + 56.89878420063114 ], "moduleForcesY": [ - -35.27267783600763, - -35.913635660706774, - -40.42480723386195, - -40.64046601274349 + -35.27267776148261, + -35.91363560529686, + -40.42480726897906, + -40.64046604811504 ], - "timestamp": 5.578469777535618 + "timestamp": 5.578469777383153 }, { - "x": 2.4420919536077847, - "y": 3.2581828688425367, - "heading": -0.6842805929390149, - "angularVelocity": -0.344021105475332, - "velocityX": -1.5480207622638111, - "velocityY": 1.0264404159729446, + "x": 2.4420919536194616, + "y": 3.258182868864464, + "heading": -0.6842805929718256, + "angularVelocity": -0.3440211062273634, + "velocityX": -1.5480207626088205, + "velocityY": 1.0264404159960439, "moduleForcesX": [ - 60.4598584765929, - 59.99281023891339, - 56.876528556020105, - 56.661344392247265 + 60.45985851021655, + 59.992810263691005, + 56.876528538625024, + 56.66134437370607 ], "moduleForcesY": [ - -35.130249715774205, - -35.93621486669289, - -40.675063944105, - -40.98709766240873 + -35.13024965770887, + -35.936214825314366, + -40.6750639683131, + -40.98709768806473 ], - "timestamp": 5.640026997920719 + "timestamp": 5.640026997759006 }, { - "x": 2.3587528835015927, - "y": 3.3135334333216804, - "heading": -0.7030081263323279, - "angularVelocity": -0.3042296789257516, - "velocityX": -1.3538471942824237, - "velocityY": 0.8991725768784151, + "x": 2.35875288350831, + "y": 3.313533433337192, + "heading": -0.7030081263847169, + "angularVelocity": -0.30422967928952566, + "velocityX": -1.353847194566388, + "velocityY": 0.8991725769092883, "moduleForcesX": [ - 60.5201219775476, - 59.967443909712394, - 56.748515348358374, - 56.463990903711384 + 60.52012200326446, + 59.96744392817741, + 56.74851533683041, + 56.463990890605494 ], "moduleForcesY": [ - -35.04253435660417, - -35.99355809852384, - -40.867195346689, - -41.27154173850131 + -35.042534312039635, + -35.99355806773702, + -40.86719536260796, + -41.2715417564386 ], - "timestamp": 5.70158421830582 + "timestamp": 5.701584218134859 }, { - "x": 2.2873528689676483, - "y": 3.3610285743692394, - "heading": -0.7191989284072005, - "angularVelocity": -0.26302035689049935, - "velocityX": -1.1598966634176864, - "velocityY": 0.7715608461595652, + "x": 2.287352868970426, + "y": 3.361028574379745, + "heading": -0.7191989284627818, + "angularVelocity": -0.26302035698188275, + "velocityX": -1.1598966636559505, + "velocityY": 0.7715608461941635, "moduleForcesX": [ - 60.55636984330847, - 59.92996721273085, - 56.64674423048133, - 56.29855167271921 + 60.556369862505825, + 59.92996722632186, + 56.646744223537, + 56.29855166413728 ], "moduleForcesY": [ - -34.99335531824572, - -36.06824538837232, - -41.01955138323022, - -41.50757259528159 + -34.99335528490936, + -36.068245365760546, + -41.019551392749705, + -41.507572606915744 ], - "timestamp": 5.763141438690921 + "timestamp": 5.763141438510712 }, { - "x": 2.2278792676988664, - "y": 3.4006487477183196, - "heading": -0.7327874449132579, - "angularVelocity": -0.2207461029112197, - "velocityX": -0.966151507438403, - "velocityY": 0.6436316178868581, + "x": 2.2278792676993477, + "y": 3.4006487477251475, + "heading": -0.7327874449614561, + "angularVelocity": -0.22074610282445042, + "velocityX": -0.9661515076208613, + "velocityY": 0.6436316179238191, "moduleForcesX": [ - 60.57586928152923, - 59.88760410989669, - 56.562363081650766, - 56.15861493971373 + 60.575869295269314, + 59.88760411963649, + 56.56236307833243, + 56.15861493495852 ], "moduleForcesY": [ - -34.97098554580845, - -36.148822416267066, - -41.14545732873801, - -41.70558761712785 + -34.970985521919275, + -36.1488224000973, + -41.14545733324237, + -41.70558762351516 ], - "timestamp": 5.824698659076022 + "timestamp": 5.8246986588865655 }, { - "x": 2.180320435943854, - "y": 3.432375929705667, - "heading": -0.7437231910234625, - "angularVelocity": -0.17765172049338343, - "velocityX": -0.7725955047594213, - "velocityY": 0.5154095943394352, + "x": 2.1803204359440898, + "y": 3.4323759297090946, + "heading": -0.7437231910585703, + "angularVelocity": -0.17765172030741577, + "velocityX": -0.772595504879486, + "velocityY": 0.5154095943616319, "moduleForcesX": [ - 60.58340632519516, - 59.84490600733586, - 56.48902920756478, - 56.03945201723916 + 60.583406334310894, + 59.84490601396566, + 56.489029207157884, + 56.03945201575598 ], "moduleForcesY": [ - -34.96766125833015, - -36.22817172523544, - -41.25431832732434, - -41.87309909683168 + -34.96766124246743, + -36.22817171424666, + -41.25431832783358, + -41.87309909879299 ], - "timestamp": 5.886255879461123 + "timestamp": 5.886255879262419 }, { - "x": 2.1446656199937997, - "y": 3.4561934671002783, - "heading": -0.7519665199163653, - "angularVelocity": -0.13391327355800717, - "velocityX": -0.5792141965962733, - "velocityY": 0.3869170382549771, + "x": 2.1446656199930714, + "y": 3.4561934671017123, + "heading": -0.7519665199367948, + "angularVelocity": -0.1339132733396743, + "velocityX": -0.5792141966989566, + "velocityY": 0.38691703828071905, "moduleForcesX": [ - 60.58183660174803, - 59.80457338750152, - 56.42234143923046, - 55.9377862653548 + 60.581836606907586, + 59.80457339156535, + 56.422341441204175, + 55.937786266687105 ], "moduleForcesY": [ - -34.97878115536029, - -36.30217808780585, - -41.352605880404866, - -42.01528814031888 + -34.97878114637003, + -36.302178081071816, + -41.352605877670875, + -42.01528813851514 ], - "timestamp": 5.947813099846224 + "timestamp": 5.947813099638272 }, { - "x": 2.120904845915518, - "y": 3.472085915141707, - "heading": -0.7574858469261834, - "angularVelocity": -0.08966173221089468, - "velocityX": -0.3859949154564824, - "velocityY": 0.258173581295686, + "x": 2.1209048459157667, + "y": 3.4720859151422694, + "heading": -0.7574858469338888, + "angularVelocity": -0.08966173201765831, + "velocityX": -0.3859949154986063, + "velocityY": 0.2581735813203214, "moduleForcesX": [ - 60.57287518432625, - 59.76823373322807, - 56.35914684811399, - 55.851272292620784 + 60.57287518607043, + 59.768233735123275, + 56.35914685208343, + 55.851272296387975 ], "moduleForcesY": [ - -35.001611618471976, - -36.36844720693236, - -41.44491977809569, - -42.13584960752861 + -35.00161161541115, + -36.368447203777166, + -41.44491977266188, + -42.13584960250002 ], - "timestamp": 6.009370320231325 + "timestamp": 6.009370320014125 }, { "x": 2.109028816223142, "y": 3.4800388813018817, "heading": -0.7602557704725089, - "angularVelocity": -0.044997540970748186, - "velocityX": -0.19292667242090425, - "velocityY": 0.12919631702701284, + "angularVelocity": -0.04499754085233033, + "velocityX": -0.19292667245392872, + "velocityY": 0.12919631703729303, "moduleForcesX": [ - 60.5575482023407, - 59.73687419838248, - 56.29713329942968, - 55.778188462220434 + 60.55754820112047, + 59.736874198400685, + 56.297133305118244, + 55.77818846809218 ], "moduleForcesY": [ - -35.034541448499795, - -36.42559455322768, - -41.5346063390199, - -42.237487649673916 + -35.03454145057582, + -36.425594553156486, + -41.53460633127721, + -42.237487641880264 ], - "timestamp": 6.070927540616426 + "timestamp": 6.070927540389978 }, { "x": 2.109028816223143, "y": 3.4800388813018808, "heading": -0.760255770472509, - "angularVelocity": -4.7854036756238116e-17, - "velocityX": -4.1684189280839564e-16, - "velocityY": 3.182458036241524e-16, + "angularVelocity": -4.742890180425531e-17, + "velocityX": -4.236195451911178e-16, + "velocityY": 3.1146529727541206e-16, "moduleForcesX": [ - 60.53644088216914, - 59.71107786211956, - 56.23458372965665, - 55.717252600818924 + 60.53644087836653, + 59.71107786047138, + 56.23458373687308, + 55.71725260850273 ], "moduleForcesY": [ - -35.07667248160167, - -36.472859348504336, - -41.62412921793918, - -42.32221267335896 + -35.0766724881386, + -36.47285935116105, + -41.62412920816004, + -42.32221266320002 ], - "timestamp": 6.1324847610015265 + "timestamp": 6.1324847607658315 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/source 3.2.traj b/src/main/deploy/choreo/source 3.2.traj index affe7841..eb5a96e6 100644 --- a/src/main/deploy/choreo/source 3.2.traj +++ b/src/main/deploy/choreo/source 3.2.traj @@ -4,2185 +4,2164 @@ "x": 2.109028816223143, "y": 3.4800388813018808, "heading": -0.760255770472509, - "angularVelocity": -4.7854036756238116e-17, - "velocityX": -4.1684189280839564e-16, - "velocityY": 3.182458036241524e-16, + "angularVelocity": -4.742890180425531e-17, + "velocityX": -4.236195451911178e-16, + "velocityY": 3.1146529727541206e-16, "moduleForcesX": [ - 60.53644088216914, - 59.71107786211956, - 56.23458372965665, - 55.717252600818924 + 60.53644087836653, + 59.71107786047138, + 56.23458373687308, + 55.71725260850273 ], "moduleForcesY": [ - -35.07667248160167, - -36.472859348504336, - -41.62412921793918, - -42.32221267335896 + -35.0766724881386, + -36.47285935116105, + -41.62412920816004, + -42.32221266320002 ], "timestamp": 0 }, { - "x": 2.1173647314297415, - "y": 3.4676489109609023, - "heading": -0.7574096677464409, - "angularVelocity": 0.045234117251022915, - "velocityX": 0.13248564867185347, - "velocityY": -0.19691818078357895, + "x": 2.11756098918411, + "y": 3.46792188759214, + "heading": -0.7574690093542815, + "angularVelocity": 0.04446117329426013, + "velocityX": 0.13612591984038372, + "velocityY": -0.1933196762400969, "moduleForcesX": [ - 41.520736384271714, - 42.51429487640815, - 35.1511276560287, - 36.81654326527437 + 42.74872403178063, + 43.59458842062485, + 36.532752148464645, + 38.029009807643504 ], "moduleForcesY": [ - -56.31022663437434, - -55.57003233589047, - -60.49287111022869, - -59.49929247213998 + -55.38361215410406, + -54.726449152918555, + -59.66836228131139, + -58.73145114027325 ], - "timestamp": 0.062919382515501 + "timestamp": 0.06267853301539805 }, { - "x": 2.1342750283552188, - "y": 3.4430345558261144, - "heading": -0.751664349766151, - "angularVelocity": 0.09131237069712948, - "velocityX": 0.2687613299655501, - "velocityY": -0.39120465190077036, + "x": 2.134842785454668, + "y": 3.4438456887135054, + "heading": -0.7518451991808588, + "angularVelocity": 0.0897246617440947, + "velocityX": 0.27572113511834023, + "velocityY": -0.38412192692383823, "moduleForcesX": [ - 42.70173098288214, - 43.611128958075156, - 36.275170342746776, - 37.87745965146702 + 43.83189384298978, + 44.60279573474994, + 37.56581425139671, + 39.005385774472 ], "moduleForcesY": [ - -55.41582346031395, - -54.70978545558125, - -59.821763435546124, - -58.826195167217094 + -54.526207091523304, + -53.904217238652684, + -59.01959396027511, + -58.084181071273 ], - "timestamp": 0.125838765031002 + "timestamp": 0.1253570660307961 }, { - "x": 2.1600269154540004, - "y": 3.4063896499792783, - "heading": -0.7429636060218578, - "angularVelocity": 0.1382839976560426, - "velocityX": 0.4092838497332282, - "velocityY": -0.5824104494002036, + "x": 2.1611174297283973, + "y": 3.407994009108057, + "heading": -0.7433311964216099, + "angularVelocity": 0.13583602470653403, + "velocityX": 0.41919686070627105, + "velocityY": -0.5719929596412315, "moduleForcesX": [ - 44.01151222332591, - 44.83593128986605, - 37.54047868266639, - 39.07824731715444 + 45.03070879320651, + 45.72720818672459, + 38.72596873376568, + 40.10888786396711 ], "moduleForcesY": [ - -54.37659249907092, - -53.70636995290632, - -59.03155043231922, - -58.03154166424591 + -53.535601635367144, + -52.949460544128094, + -58.260527674539446, + -57.32381863218353 ], - "timestamp": 0.188758147546503 + "timestamp": 0.18803559904619416 }, { - "x": 2.1949215222317773, - "y": 3.357943659865962, - "heading": -0.7312478740657462, - "angularVelocity": 0.18620227166446401, - "velocityX": 0.5545923272400236, - "velocityY": -0.7699692555212343, + "x": 2.1966585475096383, + "y": 3.3605831084486266, + "heading": -0.7318707568809233, + "angularVelocity": 0.18284473151072553, + "velocityX": 0.5670381240816551, + "velocityY": -0.7564136934695477, "moduleForcesX": [ - 45.47389898312857, - 46.209380246970596, - 38.97509187538921, - 40.44329438249183 + 46.366588104731406, + 46.98653915444157, + 40.038346183061186, + 41.36150168519336 ], "moduleForcesY": [ - -53.154044492191325, - -52.52435244840178, - -58.08923297674164, - -57.08406886084209 + -52.37723663439366, + -51.83024165919106, + -57.36143656121796, + -56.42213294794304 ], - "timestamp": 0.251677530062004 + "timestamp": 0.2507141320615922 }, { - "x": 2.2393001625829205, - "y": 3.297971653928642, - "heading": -0.7164541055664759, - "angularVelocity": 0.23512259510216332, - "velocityX": 0.7053254271878769, - "velocityY": -0.9531563015982434, + "x": 2.2417757829173284, + "y": 3.3018706473116755, + "heading": -0.7174043905612114, + "angularVelocity": 0.23080256706323984, + "velocityX": 0.7198195815560299, + "velocityY": -0.9367236007665936, "moduleForcesX": [ - 47.11716302884608, - 47.75573925943408, - 40.613851617327796, - 42.00243934560986 + 47.865105493752495, + 48.40288266056825, + 41.53424661497753, + 42.79020355531207 ], "moduleForcesY": [ - -51.69634105716302, - -51.116592706220615, - -56.94950428119139, - -55.94148520473914 + -51.00479249750509, + -50.50425407309677, + -56.2817455416053, + -55.341130773916184 ], - "timestamp": 0.314596912577505 + "timestamp": 0.31339266507699026 }, { - "x": 2.293551835832764, - "y": 3.226807987294763, - "heading": -0.6985158525170084, - "angularVelocity": 0.28509900021743295, - "velocityX": 0.862241030996735, - "velocityY": -1.1310293233781603, + "x": 2.296821580243133, + "y": 3.232167800844977, + "heading": -0.6998693932081385, + "angularVelocity": 0.27976081298465, + "velocityX": 0.8782240853065545, + "velocityY": -1.1120688872787494, "moduleForcesX": [ - 48.974244926718995, - 49.50301706537962, - 42.49983724647496, - 43.79203201526126 + 49.55640607435229, + 50.00204429745789, + 43.25256094553968, + 44.42804273281315 ], "moduleForcesY": [ - -49.932591819303816, - -49.41939613494972, - -55.54905272250912, - -54.54558483413394 + -49.35518638216705, + -48.914538145342306, + -54.9650826996631, + -54.028779358711596 ], - "timestamp": 0.377516295093006 + "timestamp": 0.3760711980923883 }, { - "x": 2.3581219992537226, - "y": 3.144865400496988, - "heading": -0.6773637507894199, - "angularVelocity": 0.3361778339508891, - "velocityX": 1.0262364447879162, - "velocityY": -1.302342513892083, + "x": 2.3621992355615835, + "y": 3.1518561030599934, + "heading": -0.679200184492938, + "angularVelocity": 0.3297653553908588, + "velocityX": 1.043062946326279, + "velocityY": -1.2813270177407263, "moduleForcesX": [ - 51.081995362995904, - 51.48237998253363, - 44.68553797550306, - 45.85573965609478 + 51.47499199800596, + 51.81338905645372, + 45.24113729305585, + 46.315146275458055 ], "moduleForcesY": [ - -47.76441461323329, - -47.34537980089634, - -53.79777393977218, - -52.8148137116725 + -47.34111247918622, + -46.98312892385554, + -53.331646603367375, + -52.41249065479232 ], - "timestamp": 0.440435677608507 + "timestamp": 0.43874973110778637 }, { - "x": 2.433522291274826, - "y": 3.0526620913362525, - "heading": -0.652926734850225, - "angularVelocity": 0.3883861373428849, - "velocityX": 1.1983635090908211, - "velocityY": -1.46541980347662, + "x": 2.4383720926533567, + "y": 3.061411276771742, + "heading": -0.6553291916681563, + "angularVelocity": 0.38084798217784427, + "velocityX": 1.2152941912833248, + "velocityY": -1.4429952638813623, "moduleForcesX": [ - 53.47798671720776, - 53.72572515007777, - 47.232726000587874, - 48.24428581121922 + 53.657812871578834, + 53.86840420507071, + 47.55738728725876, + 48.499088173849444 ], "moduleForcesY": [ - -45.05355867588098, - -44.773023132097414, - -51.56510924673164, - -50.632864772568915 + -44.83992767026467, + -44.60161470848597, + -51.266287364667946, + -50.38906940154251 ], - "timestamp": 0.503355060124008 + "timestamp": 0.5014282641231844 }, { - "x": 2.520339981100692, - "y": 2.9508605578412346, - "heading": -0.6251343348430549, - "angularVelocity": 0.4417144430863272, - "velocityX": 1.3798242505713987, - "velocityY": -1.6179677775753416, + "x": 2.5258731212927574, + "y": 2.9614374657104525, + "heading": -0.6281884283597926, + "angularVelocity": 0.43301529251962634, + "velocityX": 1.3960286629220273, + "velocityY": -1.5950247437464637, "moduleForcesX": [ - 56.19197506453079, - 56.259276660576184, - 50.20804830120448, - 51.011888473748435 + 56.13852280957632, + 56.19641284805428, + 50.26589724732213, + 51.0329382483159 ], "moduleForcesY": [ - -41.604044996750225, - -41.53144010163077, - -48.65926448291237, - -47.83146963835682 + -41.67704853096463, + -41.61695338715726, + -48.600192852411276, + -47.809382613298865 ], - "timestamp": 0.566274442639509 + "timestamp": 0.5641067971385825 }, { - "x": 2.6192438141173406, - "y": 2.8403235512778635, - "heading": -0.5939211843905473, - "angularVelocity": 0.4960816397843361, - "velocityX": 1.5719135989976136, - "velocityY": -1.7568037406619357, + "x": 2.6253125642587176, + "y": 2.8527168842363753, + "heading": -0.5977125836977811, + "angularVelocity": 0.48622460028738235, + "velocityX": 1.5864992076558508, + "velocityY": -1.7345744426942504, "moduleForcesX": [ - 59.2250417610776, - 59.087826967496994, - 53.66857872752665, - 54.204993983215175 + 58.932237428484335, + 58.813020339679206, + 53.42902064875195, + 53.9678341870784 ], "moduleForcesY": [ - -37.1392249820516, - -37.38030580647965, - -44.79597130253933, - -44.16473609829124 + -37.60318829571993, + -37.811782678383274, + -45.08236639483354, + -44.45475051563535 ], - "timestamp": 0.62919382515501 + "timestamp": 0.6267853301539805 }, { - "x": 2.7309783267356824, - "y": 2.7221935960005026, - "heading": -0.5592365908276438, - "angularVelocity": 0.5512545129373857, - "velocityX": 1.7758361279342616, - "velocityY": -1.8774811600901864, + "x": 2.737377784774954, + "y": 2.7362816266480974, + "heading": -0.5638459295824587, + "angularVelocity": 0.5403230178824133, + "velocityX": 1.7879362378935268, + "velocityY": -1.8576576698065623, "moduleForcesX": [ - 62.5038695327194, - 62.160031630846326, - 57.62319015145175, - 57.832996172310274 + 62.00010213745141, + 61.69318591156156, + 57.07952464389426, + 57.33202712766898 ], "moduleForcesY": [ - -31.280112248684674, - -31.988407393096097, - -39.55612203914035, - -39.27379472999048 + -32.26809282018358, + -32.88021961976661, + -40.33774835466741, + -40.002144832707245 ], - "timestamp": 0.692113207670511 + "timestamp": 0.6894638631693786 }, { - "x": 2.856329605423656, - "y": 2.598000279152458, - "heading": -0.5210635851729407, - "angularVelocity": 0.606697080113568, - "velocityX": 1.9922522071333428, - "velocityY": -1.9738483100568995, + "x": 2.8628118504353286, + "y": 2.6135144018139433, + "heading": -0.526556868174763, + "angularVelocity": 0.5949255608540639, + "velocityX": 2.0012284848715227, + "velocityY": -1.9586805709698702, "moduleForcesX": [ - 65.79278778008134, - 65.30020556417972, - 61.939549113121835, - 61.79878070206867 + 65.17607152774222, + 64.71491642924042, + 61.14954805032546, + 61.07753901466829 ], "moduleForcesY": [ - -23.548304579936264, - -24.925035881299397, - -32.34851276785782, - -32.65108476149218 + -25.204925810974277, + -26.40709149039101, + -33.818839446517686, + -33.98135694151609 ], - "timestamp": 0.755032590186012 + "timestamp": 0.7521423961847766 }, { - "x": 2.996031354964205, - "y": 2.46978535540666, - "heading": -0.47945305051497955, - "angularVelocity": 0.6613309443669427, - "velocityX": 2.220329315948571, - "velocityY": -2.0377651308674327, + "x": 3.0023435972808152, + "y": 2.4862768905201404, + "heading": -0.4858660567958102, + "angularVelocity": 0.6491985281301392, + "velocityX": 2.226148892336199, + "velocityY": -2.0300014242921804, "moduleForcesX": [ - 68.5564837925553, - 68.09308895613952, - 66.1567792079864, - 65.75590846714256 + 68.03638305751167, + 67.55366556741266, + 65.31234174805479, + 64.96053229107252 ], "moduleForcesY": [ - -13.459642197003902, - -15.709687602642282, - -22.445832125017436, - -23.647305076742175 + -15.87553479112995, + -17.88364572686639, + -24.79397361190254, + -25.75031448328813 ], - "timestamp": 0.817951972701513 + "timestamp": 0.8148209292001747 }, { - "x": 3.1505724071472994, - "y": 2.340193763604278, - "heading": -0.43457360660126365, - "angularVelocity": 0.7132848753348665, - "velocityX": 2.456175601294578, - "velocityY": -2.059645003198423, + "x": 3.1565264589627544, + "y": 2.3570325928242064, + "heading": -0.4418928272775442, + "angularVelocity": 0.701567624555973, + "velocityX": 2.459899015888926, + "velocityY": -2.0620185489054506, "moduleForcesX": [ - 69.84706521344005, - 69.75290941394955, - 69.22551312159781, - 68.88502793630397 + 69.73996701133173, + 69.52946808363019, + 68.70982043228925, + 68.32066639322257 ], "moduleForcesY": [ - -0.8233024041036456, - -4.014815042921486, - -9.277060723605338, - -11.648512019585809 + -3.8907722279034003, + -6.8454111489966385, + -12.521345429829843, + -14.587709075119305 ], - "timestamp": 0.880871355217014 + "timestamp": 0.8774994622155727 }, { - "x": 3.3199095005462036, - "y": 2.212409184758706, - "heading": -0.3867586308994902, - "angularVelocity": 0.7599403202978621, - "velocityX": 2.691334317484535, - "velocityY": -2.03092550716134, + "x": 3.325459805009775, + "y": 2.2288592413825183, + "heading": -0.3949105310397784, + "angularVelocity": 0.7495755560555903, + "velocityX": 2.69523452320619, + "velocityY": -2.0449322164288706, "moduleForcesX": [ - 68.47925044891942, - 69.15340266140203, - 69.49958829473019, - 69.7686571831701 + 69.04574597152255, + 69.5267430838888, + 69.75682983590083, + 69.84457598668554 ], "moduleForcesY": [ - 13.736045870694008, - 9.89537651518505, - 6.815502213890215, - 3.370466430920167 + 10.479207074920234, + 6.736459804087273, + 3.110509612866868, + -0.12959966259523578 ], - "timestamp": 0.943790737732515 + "timestamp": 0.9401779952309708 }, { - "x": 3.5032276259952, - "y": 2.089811754517775, - "heading": -0.3365088719529981, - "angularVelocity": 0.7986371915540097, - "velocityX": 2.9135398047467214, - "velocityY": -1.9484843197678885, + "x": 3.508485431874634, + "y": 2.1051894273892637, + "heading": -0.3453726504925619, + "angularVelocity": 0.7903484361232042, + "velocityX": 2.920068770912297, + "velocityY": -1.973080862675469, "moduleForcesX": [ - 63.762980248590615, - 65.33381311946748, - 65.64375774359121, - 66.90783722378835 + 64.90348551855642, + 66.33308380991878, + 66.72276225524763, + 67.80174756382702 ], "moduleForcesY": [ - 28.499981635594498, - 24.717827613930762, - 23.81826750682116, - 20.038950866645536 + 25.769345424686897, + 21.867362447533644, + 20.5650741326706, + 16.728754630599152 ], - "timestamp": 1.006710120248016 + "timestamp": 1.0028565282463688 }, { - "x": 3.698983205941932, - "y": 1.9754559392748747, - "heading": -0.2844250046592506, - "angularVelocity": 0.8277873242146967, - "velocityX": 3.111212668028096, - "velocityY": -1.8174974176634262, + "x": 3.704119942311664, + "y": 1.9892732212302584, + "heading": -0.2938661408515115, + "angularVelocity": 0.8217567827951044, + "velocityX": 3.1212362674309677, + "velocityY": -1.8493765023270128, "moduleForcesX": [ - 56.27400205759313, - 58.363284645856304, - 58.070669393657745, - 60.053109943523204 + 57.471427969439794, + 59.58527298092468, + 59.37846559329102, + 61.351076031348384 ], "moduleForcesY": [ - 41.37894474295054, - 38.388062514062604, - 38.80031879570978, - 35.67059242066261 + 39.677051011352944, + 36.440949209828744, + 36.741332005058595, + 33.36307222006473 ], - "timestamp": 1.069629502763517 + "timestamp": 1.065535061261767 }, { - "x": 3.9052468507649114, - "y": 1.8717029681737345, - "heading": -0.23110382341189709, - "angularVelocity": 0.8474523924995162, - "velocityX": 3.2782210596577848, - "velocityY": -1.6489826656448716, + "x": 3.9103619312555455, + "y": 1.8836924134378115, + "heading": -0.24100411082773987, + "angularVelocity": 0.8433833320697718, + "velocityX": 3.290472495474877, + "velocityY": -1.6844811566746316, "moduleForcesX": [ - 47.55707312914185, - 49.53638736807368, - 48.78953373366416, - 50.77045950153568 + 48.26509838932949, + 50.40336737122413, + 49.6190591353058, + 51.75496442543449 ], "moduleForcesY": [ - 51.17696939290848, - 49.268536466648314, - 49.99446973782349, - 47.98722716731246 + 50.492121778115546, + 48.363822792220766, + 49.151347533828364, + 46.9041391127918 ], - "timestamp": 1.132548885279018 + "timestamp": 1.128213594277165 }, { - "x": 4.120080601641824, - "y": 1.7801799153533593, - "heading": -0.17706327947410902, - "angularVelocity": 0.8588854781668361, - "velocityX": 3.4144287862962694, - "velocityY": -1.454608248862383, + "x": 4.1251309979342885, + "y": 1.7902287074795913, + "heading": -0.18733070094754622, + "angularVelocity": 0.8563284317296923, + "velocityX": 3.426517123908772, + "velocityY": -1.491159755370459, "moduleForcesX": [ - 39.05701336414032, - 40.46649013860068, - 39.71798936608046, - 41.14398002340263 + 39.02721416882864, + 40.63159693937693, + 39.76181294529839, + 41.38836138220273 ], "moduleForcesY": [ - 57.94278647291951, - 56.96920228244361, - 57.48746696114792, - 56.477668387844396 + 57.948757981015774, + 56.837566924459644, + 57.44189927750841, + 56.28368962505613 ], - "timestamp": 1.195468267794519 + "timestamp": 1.190892127292563 }, { - "x": 4.341754671827057, - "y": 1.7019368196307447, - "heading": -0.12271990769558694, - "angularVelocity": 0.8636984281454757, - "velocityX": 3.523144400386022, - "velocityY": -1.243545193777744, + "x": 4.346552661405609, + "y": 1.7100097822136517, + "heading": -0.13328440238249356, + "angularVelocity": 0.862277656558669, + "velocityX": 3.5326554853624956, + "velocityY": -1.279846885475643, "moduleForcesX": [ - 31.542569411645978, - 32.211867496766956, - 31.792298515347188, - 32.46659837801001 + 30.796111969393834, + 31.63647803362233, + 31.08882111063717, + 31.937435866659744 ], "moduleForcesY": [ - 62.36899517370198, - 62.02650340460398, - 62.24055631529311, - 61.8920479920422 + 62.72964371688498, + 62.31072023606725, + 62.583051774162115, + 62.15497397097186 ], - "timestamp": 1.25838765031002 + "timestamp": 1.253570660307961 }, { - "x": 4.568810602625837, - "y": 1.6376320022464737, - "heading": -0.06839919997587875, - "angularVelocity": 0.8633382202427946, - "velocityX": 3.6086802146038512, - "velocityY": -1.0220192063777647, + "x": 4.5730514606078865, + "y": 1.6437195895696548, + "heading": -0.07920610917804985, + "angularVelocity": 0.8627881126566634, + "velocityX": 3.613658270314562, + "velocityY": -1.0576219553146122, "moduleForcesX": [ - 25.215137722620277, - 25.161326804862117, - 25.198140713892162, - 25.144365342135142 + 23.887085713336567, + 23.96483684337957, + 23.909031078260046, + 23.986859531626518 ], "moduleForcesY": [ - 65.19836347228741, - 65.21911954217529, - 65.20503178610034, - 65.2257580831436 + 65.68742061578509, + 65.65913506231513, + 65.67928603196187, + 65.6509428796385 ], - "timestamp": 1.321307032825521 + "timestamp": 1.316249193323359 }, { - "x": 4.800045847269579, - "y": 1.5876743304432248, - "heading": -0.014355888196568429, - "angularVelocity": 0.8589294684510949, - "velocityX": 3.675103527705061, - "velocityY": -0.793994947279417, + "x": 4.803338854745861, + "y": 1.5917650272197101, + "heading": -0.025361992429283036, + "angularVelocity": 0.8590519617863268, + "velocityX": 3.6741031268456674, + "velocityY": -0.8289052064632888, "moduleForcesX": [ - 19.993448516674185, - 19.3060989973268, - 19.797835730589334, - 19.116494526678093 + 18.23081795339054, + 17.63640682889574, + 18.085112159973114, + 17.495364929313684 ], "moduleForcesY": [ - 66.99591754938321, - 67.196927406319, - 67.05502467711538, - 67.25215990996023 + 67.48937858084152, + 67.6469186921904, + 67.52952569073541, + 67.68449338706219 ], - "timestamp": 1.384226415341022 + "timestamp": 1.3789277263387572 }, { - "x": 5.034473115872903, - "y": 1.5523169276578892, - "heading": 0.039207016322208126, - "angularVelocity": 0.8512941859462895, - "velocityX": 3.725835493467699, - "velocityY": -0.5619477078724477, + "x": 5.0363666951223784, + "y": 1.554384768665583, + "heading": 0.028035090410861878, + "angularVelocity": 0.8519197924914251, + "velocityX": 3.7178253728317188, + "velocityY": -0.596380558953801, "moduleForcesX": [ - 15.705471617855421, - 14.48469807799971, - 15.374313004019221, - 14.172732804713208 + 13.629985184247289, + 12.466510567267608, + 13.36498947078768, + 12.219570464535467 ], "moduleForcesY": [ - 68.13853193873344, - 68.40802253366346, - 68.21561194224486, - 68.47494827858137 + 68.57759449105092, + 68.79825994640873, + 68.63134312576102, + 68.84417352906047 ], - "timestamp": 1.447145797856523 + "timestamp": 1.4416062593541552 }, { "x": 5.271278381347656, "y": 1.5317155122756958, - "heading": 0.09212323355733296, - "angularVelocity": 0.8410161562232161, - "velocityX": 3.7636298388086282, - "velocityY": -0.3274255810936793, + "heading": 0.08080945742074001, + "angularVelocity": 0.8419847190251427, + "velocityX": 3.7478810515167584, + "velocityY": -0.3616749674775882, "moduleForcesX": [ - 12.17583308633574, - 10.508988015592593, - 11.72461754628835, - 10.093645719719404 + 9.878633404360912, + 8.235261290860429, + 9.510066456350259, + 7.902786589676527 ], "moduleForcesY": [ - 68.86508793588061, - 69.1385750114367, - 68.94526551565065, - 69.20238200518273 + 69.22664704724305, + 69.44088567639092, + 69.28024531566139, + 69.48153670734771 ], - "timestamp": 1.510065180372024 + "timestamp": 1.5042847923695533 }, { - "x": 5.388681610311286, - "y": 1.5251584282304298, - "heading": 0.11807334458511697, - "angularVelocity": 0.8348404097837044, - "velocityX": 3.7769765097724286, - "velocityY": -0.21094779615682893, + "x": 5.391042214421232, + "y": 1.5240070453835066, + "heading": 0.1074471532788096, + "angularVelocity": 0.8357210579005186, + "velocityX": 3.757425484837848, + "velocityY": -0.24184254299832358, "moduleForcesX": [ - 9.224601000293218, - 7.170145322617407, - 8.707960432775863, - 6.708850729084163 + 6.772943150194192, + 4.71192047105511, + 6.351957415318363, + 4.348339414191522 ], "moduleForcesY": [ - 69.25444608627201, - 69.49615053644614, - 69.32560078879362, - 69.54657168705602 + 69.53223448731855, + 69.70137764950671, + 69.57638151835509, + 69.72949369202985 ], - "timestamp": 1.5411490985959961 + "timestamp": 1.5361587000642904 }, { - "x": 5.506442673711366, - "y": 1.5222273963184734, - "heading": 0.14379764901773748, - "angularVelocity": 0.8275759911368509, - "velocityX": 3.7884883929871296, - "velocityY": -0.0942941585046398, + "x": 5.51102563137661, + "y": 1.5201232868827126, + "heading": 0.13384807268108104, + "angularVelocity": 0.8282925223702723, + "velocityX": 3.764314627013447, + "velocityY": -0.12184757946812044, "moduleForcesX": [ - 8.369389483724206, - 5.949973696893992, - 7.73117703857559, - 5.38783266312984 + 5.467336768701578, + 3.0155789586234376, + 4.950575022268367, + 2.579688806707375 ], "moduleForcesY": [ - 69.35385904945188, - 69.60180730909208, - 69.43322409042021, - 69.65302052985173 + 69.63848507273565, + 69.78654461331922, + 69.68263747119076, + 69.80962731619984 ], - "timestamp": 1.5722330168199683 + "timestamp": 1.5680326077590276 }, { - "x": 5.624497338635267, - "y": 1.522927459386341, - "heading": 0.16925769890783504, - "angularVelocity": 0.8190746644823769, - "velocityX": 3.7979338406848053, - "velocityY": 0.02252171244382533, + "x": 5.631134254008104, + "y": 1.5200677662545983, + "heading": 0.15997015664506697, + "angularVelocity": 0.819544444131609, + "velocityX": 3.7682427828365683, + "velocityY": -0.0017418833186710159, "moduleForcesX": [ - 7.409211713616261, - 4.57253345050813, - 6.632095741425086, - 3.899218959272906 + 4.014268392430795, + 1.116698120742873, + 3.3923076891931627, + 0.60736396962066 ], "moduleForcesY": [ - 69.45271134204867, - 69.69519778275762, - 69.53780580291625, - 69.74287628352383 + 69.72782501035493, + 69.83295042566183, + 69.76768134726464, + 69.84622551429848 ], - "timestamp": 1.6033169350439405 + "timestamp": 1.5999065154537648 }, { - "x": 5.742772793309857, - "y": 1.5272629148189605, - "heading": 0.19440986357494344, - "angularVelocity": 0.8091696962357507, - "velocityX": 3.8050368625463413, - "velocityY": 0.13947583446143447, + "x": 5.751262381021374, + "y": 1.523841630833003, + "heading": 0.1857658669581749, + "angularVelocity": 0.8093049198786131, + "velocityX": 3.7688547059795785, + "velocityY": 0.11839980885141156, "moduleForcesX": [ - 6.320859466741637, - 3.0067323445032117, - 5.3877760442274845, - 2.2145608793254503 + 2.3863359481069573, + -1.019059720883274, + 1.6525549624055083, + -1.5974719300993867 ], "moduleForcesY": [ - 69.54854425346966, - 69.76816296129176, - 69.63518331243104, - 69.80621969634359 + 69.79190277644955, + 69.8235099520272, + 69.82158137742265, + 69.82135764239271 ], - "timestamp": 1.6344008532679126 + "timestamp": 1.631780423148502 }, { - "x": 5.861185901308817, - "y": 1.5352368348891807, - "heading": 0.21920461001465913, - "angularVelocity": 0.7976712028728045, - "velocityX": 3.8094653043977926, - "velocityY": 0.2565287944964038, + "x": 5.871291096439659, + "y": 1.5314426318940912, + "heading": 0.21118151569712723, + "angularVelocity": 0.7973810108996604, + "velocityX": 3.7657358039629982, + "velocityY": 0.23847095040510002, "moduleForcesX": [ - 5.0743871573767105, - 1.212540305184361, - 3.969650176486514, - 0.29853628468642324 + 0.549891385422086, + -3.4334671839024553, + -0.29767021184777925, + -4.068355861282044 ], "moduleForcesY": [ - 69.63726174457908, - 69.80849842797434, - 69.71904021769083, - 69.82888852399873 + 69.81865312665636, + 69.73422687541164, + 69.83062468569385, + 69.71085861910606 ], - "timestamp": 1.6654847714918848 + "timestamp": 1.6636543308432392 }, { - "x": 5.979640967451708, - "y": 1.5468503445815898, - "heading": 0.24358557158644084, - "angularVelocity": 0.7843593396465376, - "velocityX": 3.810815138856516, - "velocityY": 0.3736179463840169, + "x": 5.991086014880691, + "y": 1.5428637105440566, + "heading": 0.23615643982419934, + "angularVelocity": 0.7835538825757309, + "velocityX": 3.758400745472756, + "velocityY": 0.3583206288776096, "moduleForcesX": [ - 3.6307956438747206, - -0.8621551207472431, - 2.3418002525812245, - -1.893133883598176 + -1.5364034730888085, + -6.177058299089292, + -2.492386077354901, + -6.843823309273452 ], "moduleForcesY": [ - 69.7120826368421, - 69.79764527340737, - 69.77979201753804, - 69.79043163606336 + 69.79055227447756, + 69.53111319438398, + 69.77586096014748, + 69.48206624406794 ], - "timestamp": 1.696568689715857 + "timestamp": 1.6955282385379764 }, { - "x": 6.098026846695146, - "y": 1.5621015192437462, - "heading": 0.2674883023191033, - "angularVelocity": 0.7689741866013711, - "velocityX": 3.808589328746145, - "velocityY": 0.4906451803233273, + "x": 6.110494620280023, + "y": 1.558091016928009, + "heading": 0.260621989902633, + "angularVelocity": 0.7675729726252842, + "velocityX": 3.7462807053007396, + "velocityY": 0.47773578720836973, "moduleForcesX": [ - 1.9386808198061933, - -3.28598819102792, - 0.4585755906663063, - -4.416447800806846 + -3.9233308193331884, + -9.310207032464307, + -4.971223982333152, + -9.967160671911408 ], "moduleForcesY": [ - 69.76178277329687, - 69.70685348700545, - 69.80282869693546, - 69.6609062566512 + 69.68200654002352, + 69.16570334527854, + 69.63108366349697, + 69.09079688113802 ], - "timestamp": 1.727652607939829 + "timestamp": 1.7274021462327136 }, { - "x": 6.216213161429717, - "y": 1.5809836737297318, - "heading": 0.2908385525285553, - "angularVelocity": 0.7512003487206494, - "velocityX": 3.8021691436385594, - "velocityY": 0.6074573465909969, + "x": 6.229343182975531, + "y": 1.5771011290944752, + "heading": 0.2845003140637755, + "angularVelocity": 0.7491495674088557, + "velocityX": 3.728710136006611, + "velocityY": 0.5964161140369018, "moduleForcesX": [ - -0.07065192727705566, - -6.150375646776657, - -1.738667342074545, - -7.342704323857981 + -6.673094685929394, + -12.903296056772389, + -7.779731069608852, + -13.485056064651221 ], "moduleForcesY": [ - 69.76766630955797, - 69.49064187075653, - 69.76568210247977, - 69.39577088637856 + 69.45541432263478, + 68.56862083714847, + 69.36005488296091, + 68.47745191194402 ], - "timestamp": 1.7587365261638013 + "timestamp": 1.7592760539274508 }, { - "x": 6.334045309853754, - "y": 1.6034826557006305, - "heading": 0.313549807262191, - "angularVelocity": 0.7306432403402956, - "velocityX": 3.790775267616168, - "velocityY": 0.7238142182972129, + "x": 6.347433309831943, + "y": 1.5998571523743064, + "heading": 0.30770295299816985, + "angularVelocity": 0.727950873065531, + "velocityX": 3.7049152550538524, + "velocityY": 0.7139389213826525, "moduleForcesX": [ - -2.490383343681562, - -9.578064211771972, - -4.3259113321238125, - -10.762745909847968 + -9.861215297744039, + -17.03459334936717, + -10.968990403175749, + -17.444220781953867 ], "moduleForcesY": [ - 69.69820129934999, - 69.07536900988754, - 69.63337299442767, - 68.9276312183353 + 69.05521867685782, + 67.64066622247339, + 68.91276556625236, + 67.56234354492842 ], - "timestamp": 1.7898204443877734 + "timestamp": 1.791149961622188 }, { - "x": 6.451337870714877, - "y": 1.6295724757424068, - "heading": 0.33551971883232135, - "angularVelocity": 0.706793506913393, - "velocityX": 3.7734162088570447, - "velocityY": 0.8393349851774983, + "x": 6.464538328080527, + "y": 1.626303286615654, + "heading": 0.33012934372254554, + "angularVelocity": 0.7035971534823224, + "velocityX": 3.674008827851357, + "velocityY": 0.8297110757371571, "moduleForcesX": [ - -5.448866729736701, - -13.733572282788284, - -7.401927508185858, - -14.790647300726173 + -13.57752083156378, + -21.783197519865176, + -14.593849038845681, + -21.884589883945374 ], "moduleForcesY": [ - 69.49922387521676, - 68.33891344443161, - 69.35066212276465, - 68.15293380811582 + 68.39900163770346, + 66.24134665518923, + 68.22058375084256, + 66.24082954639027 ], - "timestamp": 1.8209043626117456 + "timestamp": 1.8230238693169252 }, { - "x": 6.567866022044423, - "y": 1.659208124359565, - "heading": 0.35662502542728386, - "angularVelocity": 0.6789783206509007, - "velocityX": 3.7488244078469917, - "velocityY": 0.9534077526398532, + "x": 6.580399977943102, + "y": 1.656357384692337, + "heading": 0.3516655018085619, + "angularVelocity": 0.6756673292861429, + "velocityX": 3.6349998554367304, + "velocityY": 0.9429059770303924, "moduleForcesX": [ - -9.125353794114547, - -18.83172539656305, - -11.094164589642684, - -19.562885061791146 + -17.92431111597879, + -27.21254535403587, + -18.708578792359393, + -26.82734571095702 ], "moduleForcesY": [ - 69.0754447164273, - 67.07463735744341, - 68.82909121269832, - 66.91127860350836 + 67.36450849006661, + 64.17638992872675, + 67.19039419369011, + 64.37988045730229 ], - "timestamp": 1.8519882808357178 + "timestamp": 1.8548977770116624 }, { - "x": 6.683354928219009, - "y": 1.6923136502791418, - "heading": 0.37671482456009425, - "angularVelocity": 0.6463084540390093, - "velocityX": 3.7153908764796526, - "velocityY": 1.0650370934911797, + "x": 6.694726351538369, + "y": 1.6899010876153, + "heading": 0.37218347560916276, + "angularVelocity": 0.6437231982066761, + "velocityX": 3.586832674869755, + "velocityY": 1.0523875278869872, "moduleForcesX": [ - -13.771593248917489, - -25.130414558467773, - -15.562652582080137, - -25.223578565092154 + -23.00758940984267, + -33.338147011171934, + -23.358188320319844, + -32.256271677852126 ], "moduleForcesY": [ - 68.25443383699609, - 64.93026638806907, - 67.92556316929904, - 64.95632378013195 + 65.77194757070217, + 61.18992795027423, + 65.69849366449613, + 61.819461145008376 ], - "timestamp": 1.88307219905969 + "timestamp": 1.8867716847064004 }, { - "x": 6.7974682626271505, - "y": 1.7287625418402501, - "heading": 0.3956032132276822, - "angularVelocity": 0.6076579063002795, - "velocityX": 3.671137389627324, - "velocityY": 1.1725964306841732, + "x": 6.807192685734502, + "y": 1.7267675051619156, + "heading": 0.3915426081860902, + "angularVelocity": 0.6073661492131278, + "velocityX": 3.5284764978691405, + "velocityY": 1.1566331276256518, "moduleForcesX": [ - -19.7338671115633, - -32.869545661536705, - -20.995212284811174, - -31.878789101972526 + -28.91315705633447, + -40.074653997125445, + -28.56271316758345, + -38.09306052597789 ], "moduleForcesY": [ - 66.71548826157209, - 61.31804473588162, - 66.4083595377785, - 61.9239118979002 + 63.36360268554179, + 56.97490897615298, + 63.58659281516764, + 58.38423738606962 ], - "timestamp": 1.914156117283662 + "timestamp": 1.9186455924011385 }, { - "x": 6.909800194013484, - "y": 1.7683468561362488, - "heading": 0.41306566672219286, - "angularVelocity": 0.5617841794810646, - "velocityX": 3.6138279150310604, - "velocityY": 1.2734660415324053, + "x": 6.917447317500844, + "y": 1.7667274108508237, + "heading": 0.4095941832685751, + "angularVelocity": 0.5663433318364425, + "velocityX": 3.4590873771196318, + "velocityY": 1.2536870618943754, "moduleForcesX": [ - -27.444972321937637, - -42.075345805688336, - -27.57805811433052, - -39.49775701748281 + -35.656308905451695, + -47.1677975691104, + -34.292967625798184, + -44.171577156013576 ], "moduleForcesY": [ - 63.85363325543449, - 55.33672689274484, - 63.908626512967174, - 57.321971693817275 + 59.79015829574416, + 51.22561496941096, + 60.66465034630196, + 53.91255102553536 ], - "timestamp": 1.9452400355076342 + "timestamp": 1.9505195000958766 }, { - "x": 7.019881390468804, - "y": 1.810734119873736, - "heading": 0.42885115065317647, - "angularVelocity": 0.5078344312078966, - "velocityX": 3.5414195746540194, - "velocityY": 1.363639661900725, + "x": 7.025125269774396, + "y": 1.809476738261155, + "heading": 0.42619090648469543, + "angularVelocity": 0.5206993561966173, + "velocityX": 3.378247603174457, + "velocityY": 1.3412013305600805, "moduleForcesX": [ - -37.27804152080681, - -52.1405235304012, - -35.411432044620646, - -47.75401943748761 + -43.0950713666489, + -54.14575035838762, + -40.439391320133275, + -50.224432044019366 ], "moduleForcesY": [ - 58.55809505279019, - 45.896730636123046, - 59.86837524976089, - 50.604050638682544 + 54.6293050128275, + 43.75538755080409, + 56.72805998056306, + 48.30614376570568 ], - "timestamp": 1.9763239537316064 + "timestamp": 1.9823934077906147 }, { - "x": 7.127216489228573, - "y": 1.8554226010135964, - "heading": 0.44272434085459006, - "angularVelocity": 0.4463140747396045, - "velocityX": 3.4530749304632886, - "velocityY": 1.4376720726731453, + "x": 7.1298705644585345, + "y": 1.8546303708147, + "heading": 0.44120012056964497, + "angularVelocity": 0.4708934413908578, + "velocityX": 3.2862395062225853, + "velocityY": 1.4166330964496041, "moduleForcesX": [ - -48.97728499858189, - -61.38705872437014, - -44.32215534808062, - -55.881419683225104 + -50.81886431339406, + -60.36189206611623, + -46.781345213429454, + -55.90228668204932 ], "moduleForcesY": [ - 49.06528866999657, - 32.434718317404084, - 53.54352333978887, - 41.41142967075846 + 47.484904027140054, + 34.65982136246882, + 51.598757458793386, + 41.59074319154849 ], - "timestamp": 2.0074078719555786 + "timestamp": 2.0142673154853528 }, { "x": 7.231364727020264, "y": 1.90172815322876, "heading": 0.45451541638849, - "angularVelocity": 0.3793304128823317, - "velocityX": 3.3505505014284465, - "velocityY": 1.489694828094504, + "angularVelocity": 0.4177490863802491, + "velocityX": 3.184239708973158, + "velocityY": 1.4776281234520827, "moduleForcesX": [ - -60.49418247701689, - -67.53589230331421, - -53.57894617228146, - -62.756167691165814 + -58.09323113814453, + -65.1805058005262, + -52.97746872707234, + -60.837974152120616 ], "moduleForcesY": [ - 33.72227739407681, - 16.098766492768572, - 44.212603887497394, - 29.961682528729266 + 38.2048879415566, + 24.420633432131687, + 45.192733096216344, + 33.9590952401435 ], - "timestamp": 2.0384917901795507 + "timestamp": 2.046141223180091 }, { - "x": 7.4238027530368695, - "y": 1.9965622542854489, - "heading": 0.4692934758689833, - "angularVelocity": 0.24023905191549066, - "velocityX": 3.1283626232349375, - "velocityY": 1.5416675347118225, + "x": 7.424008457970693, + "y": 2.003914192060848, + "heading": 0.4743314475792421, + "angularVelocity": 0.30437239580938835, + "velocityX": 2.9589897877434668, + "velocityY": 1.569568051149872, "moduleForcesX": [ - -68.24273916485633, - -69.7323377330834, - -62.01731512678528, - -67.61265076621554 + -64.18282412471406, + -68.43086071098912, + -58.78460066435345, + -64.93261887331047 ], "moduleForcesY": [ - 13.81064712566369, - -0.3213659067003291, - 31.862172293048193, - 17.244933344769986 + 27.36299974626911, + 13.845305324141684, + 37.678868187130924, + 25.739033429406035 ], - "timestamp": 2.1000057670399475 + "timestamp": 2.111245784117876 }, { - "x": 7.602133310590925, - "y": 2.090716538878092, - "heading": 0.47717310494630966, - "angularVelocity": 0.12809493841064415, - "velocityX": 2.899025012783137, - "velocityY": 1.5306161200782256, + "x": 7.60087719793961, + "y": 2.107850851805076, + "heading": 0.48752484756484954, + "angularVelocity": 0.20264939653329472, + "velocityX": 2.716687393651782, + "velocityY": 1.596457425518163, "moduleForcesX": [ - -69.13754121748958, - -68.20178675329157, - -69.06817284359947, - -69.80874265266795 + -69.61888985652718, + -69.76727726868289, + -67.16418021608261, + -69.18600572947932 ], "moduleForcesY": [ - -9.046985055914966, - -14.8227009169618, - 9.835976008332125, - 0.7232885771622478 + 5.029388418504633, + -3.222941021531897, + 19.11676020734826, + 9.676481897144177 ], - "timestamp": 2.161519743900344 + "timestamp": 2.1763503450556607 }, { - "x": 7.76664139791012, - "y": 2.1816852539691975, - "heading": 0.47965086497918497, - "angularVelocity": 0.04027962683177553, - "velocityX": 2.6743204669166074, - "velocityY": 1.4788300112274317, + "x": 7.761930148007023, + "y": 2.210081079106282, + "heading": 0.4950554503391814, + "angularVelocity": 0.11566935811959787, + "velocityX": 2.473758331944146, + "velocityY": 1.5702467819251287, "moduleForcesX": [ - -66.40756278498199, - -65.73447233946021, - -69.52981390692969, - -68.96429053135994 + -68.82435473996972, + -68.04450224754622, + -69.8239032049172, + -69.75673087129077 ], "moduleForcesY": [ - -21.48510284383042, - -23.5840541324825, - -6.248641320167586, - -11.053848562321154 + -11.841731474215324, + -15.852074510798255, + 1.93806565534906, + -4.071564113915693 ], - "timestamp": 2.223033720760741 + "timestamp": 2.2414549059934457 }, { - "x": 7.917760773522142, - "y": 2.267858010889966, - "heading": 0.4778609500693658, - "angularVelocity": -0.029097694559421974, - "velocityX": 2.456667302700648, - "velocityY": 1.4008646704200718, + "x": 7.907620343769395, + "y": 2.308071507252318, + "heading": 0.4978684590059931, + "angularVelocity": 0.04320755145710578, + "velocityX": 2.237787854856954, + "velocityY": 1.5051238612864044, "moduleForcesX": [ - -63.7079413269904, - -63.48861495951544, - -67.77169795111662, - -67.17513898955453 + -65.92623401375947, + -65.36976574658864, + -68.92142423550537, + -68.31330607927826 ], "moduleForcesY": [ - -28.621263222946787, - -29.166159121005602, - -16.89631000921434, - -19.21841268100821 + -23.130100041160027, + -24.720666810010734, + -11.488057316014388, + -14.770045889808568 ], - "timestamp": 2.2845476976211376 + "timestamp": 2.3065594669312306 }, { - "x": 8.055903927968282, - "y": 2.3481558395536353, - "heading": 0.47263055774144574, - "angularVelocity": -0.08502770581375621, - "velocityX": 2.245719777793735, - "velocityY": 1.3053590868608649, + "x": 8.038541023495547, + "y": 2.400018073701688, + "heading": 0.49676667390448254, + "angularVelocity": -0.01692331667152227, + "velocityX": 2.0109294623960547, + "velocityY": 1.412290707823597, "moduleForcesX": [ - -61.53663246666227, - -61.636381591712215, - -65.62140779995624, - -65.27264016289558 + -62.824360850795905, + -62.67121951693069, + -66.58034896128022, + -66.08539026982844 ], "moduleForcesY": [ - -33.09645471160549, - -32.94683673534328, - -24.002173782199, - -24.982303085352363 + -30.62126190401479, + -30.968161395864133, + -21.258041017807635, + -22.795219386908013 ], - "timestamp": 2.3460616744815344 + "timestamp": 2.3716640278690155 }, { - "x": 8.181425593296224, - "y": 2.421820395971198, - "heading": 0.4645721903392223, - "angularVelocity": -0.13100059228021374, - "velocityX": 2.0405389430894125, - "velocityY": 1.197525508466805, + "x": 8.155275443871254, + "y": 2.4846239308491906, + "heading": 0.49239437013062565, + "angularVelocity": -0.06715817925621288, + "velocityX": 1.7930298383742893, + "velocityY": 1.2995380957772373, "moduleForcesX": [ - -59.837069189363575, - -60.1347503268217, - -63.62558289863267, - -63.52420699677533 + -60.073321011059726, + -60.249556015844455, + -63.930532840470896, + -63.71297068980246 ], "moduleForcesY": [ - -36.12028649557102, - -35.64612522470202, - -28.92915290640471, - -29.180149543959885 + -35.75278795396041, + -35.47671823816927, + -28.283924388405126, + -28.797286948333827 ], - "timestamp": 2.407575651341931 + "timestamp": 2.4367685888068005 }, { - "x": 8.294623187946847, - "y": 2.488295400127025, - "heading": 0.4541517988040101, - "angularVelocity": -0.16939876215222985, - "velocityX": 1.8401930817693604, - "velocityY": 1.0806487817669281, + "x": 8.258347693307394, + "y": 2.5609347268737315, + "heading": 0.4852629357835075, + "angularVelocity": -0.10953816820810822, + "velocityX": 1.5831801635930889, + "velocityY": 1.172126728532337, "moduleForcesX": [ - -58.495272320583396, - -58.9108341498489, - -61.90125817659251, - -61.99096011956313 + -57.756771897763436, + -58.16002609551108, + -61.421389264955856, + -61.46747635960799 ], "moduleForcesY": [ - -38.28430915682556, - -37.65794710311188, - -32.488649405293295, - -32.336456191223455 + -39.40997001157865, + -38.82696454641265, + -33.4114065154027, + -33.34381170977727 ], - "timestamp": 2.469089628202328 + "timestamp": 2.5018731497445854 }, { - "x": 8.395745589032531, - "y": 2.5471579028047797, - "heading": 0.4417334392214656, - "angularVelocity": -0.20187866589616352, - "velocityX": 1.6438930832772425, - "velocityY": 0.9568963946411755, + "x": 8.348213779476872, + "y": 2.6282292552805555, + "heading": 0.47578115701955753, + "angularVelocity": -0.14563923982239507, + "velocityX": 1.380334724250048, + "velocityY": 1.0336376966143848, "moduleForcesX": [ - -57.41823039591145, - -57.901215370721474, - -60.440306372625834, - -60.66569970555321 + -55.82852902081254, + -56.37707964117926, + -59.18899393513469, + -59.44035011210898 ], "moduleForcesY": [ - -39.90297584100897, - -39.210477609648265, - -35.155958840161816, - -34.77905570626861 + -42.11500222204666, + -41.38806414610841, + -37.242958929245574, + -36.85233746127893 ], - "timestamp": 2.5306036050627245 + "timestamp": 2.5669777106823704 }, { - "x": 8.48500235236776, - "y": 2.598076660395606, - "heading": 0.4276083189600344, - "angularVelocity": -0.2296245663564786, - "velocityX": 1.4509997221898334, - "velocityY": 0.8277591563683715, + "x": 8.425266606479534, + "y": 2.685948485476612, + "heading": 0.4642800962886668, + "angularVelocity": -0.1766552230016766, + "velocityX": 1.183524255332804, + "velocityY": 0.8865620067880233, "moduleForcesX": [ - -56.53862920087126, - -57.05702405798682, - -59.20466714974995, - -59.52214725960545 + -54.21921582870304, + -54.855206644706676, + -57.24745392477798, + -57.645373900028495 ], "moduleForcesY": [ - -41.15627446217204, - -40.443127482255576, - -37.21736934315343, - -36.717258988355645 + -44.181208993738856, + -43.396520697801726, + -40.17831839961693, + -39.613797717067015 ], - "timestamp": 2.5921175819231213 + "timestamp": 2.6320822716201553 }, { - "x": 8.562571509547876, - "y": 2.640785806675407, - "heading": 0.41201408826118213, - "angularVelocity": -0.2535071132572452, - "velocityX": 1.2610005260455623, - "velocityY": 0.6942998723156388, + "x": 8.489844645461925, + "y": 2.7336490829588826, + "heading": 0.4510317174084265, + "angularVelocity": -0.20349386724073787, + "velocityX": 0.9919126717420388, + "velocityY": 0.7326767402341282, "moduleForcesX": [ - -55.80869308264709, - -56.34173719772094, - -58.15451187186907, - -58.53176769808956 + -52.865528585952326, + -53.5493743197174, + -55.569733234873816, + -56.06636357612068 ], "moduleForcesY": [ - -42.15377542537939, - -41.44512997463475, - -38.852027537364954, - -38.28860744687602 + -45.80304315675028, + -45.00724101650629, + -42.480063091401604, + -41.828690869314826 ], - "timestamp": 2.653631558783518 + "timestamp": 2.6971868325579402 }, { - "x": 8.62860576010961, - "y": 2.6750675306808085, - "heading": 0.3951480099492571, - "angularVelocity": -0.2741828633548061, - "velocityX": 1.0734836850427496, - "velocityY": 0.557299751293951, + "x": 8.542240663164481, + "y": 2.7709723355879694, + "heading": 0.43626250509958603, + "angularVelocity": -0.2268537272366222, + "velocityX": 0.8047979580513017, + "velocityY": 0.5732816885863476, "moduleForcesX": [ - -55.19425623129184, - -55.728131941039244, - -57.25555323504394, - -57.66899798719165 + -51.71605380731337, + -52.42081923342628, + -54.11889003994841, + -54.677868682944876 ], "moduleForcesY": [ - -42.96566364835716, - -42.27592065616304, - -40.176385790672825, - -39.58618560604843 + -47.105665338095726, + -46.3243865448859, + -44.322863344470825, + -43.63617258426661 ], - "timestamp": 2.7151455356439147 + "timestamp": 2.762291393495725 }, { - "x": 8.683237297728128, - "y": 2.7007402779978236, - "heading": 0.3771761995999053, - "angularVelocity": -0.2921581609028107, - "velocityX": 0.8881158462978955, - "velocityY": 0.41734819674036067, + "x": 8.582709383241658, + "y": 2.797622875392326, + "heading": 0.4201634326355504, + "angularVelocity": -0.247280255517275, + "velocityX": 0.6215957759986721, + "velocityY": 0.40934981237065615, "moduleForcesX": [ - -54.67049432361609, - -55.195762968597755, - -56.480010214696975, - -56.91239458884492 + -50.730544843076125, + -51.437804788630906, + -52.85923893372407, + -53.45364309208686 ], "moduleForcesY": [ - -43.638836888514874, - -42.97645664054756, - -41.268832050266276, - -40.67476273388533 + -48.17246735463719, + -47.41986096115254, + -45.825351350532394, + -45.13424388858939 ], - "timestamp": 2.7766595125043114 + "timestamp": 2.82739595443351 }, { - "x": 8.726581564114948, - "y": 2.717650467677025, - "heading": 0.35824027480182863, - "angularVelocity": -0.30783125664352257, - "velocityX": 0.7046246820488491, - "velocityY": 0.2748999583879684, + "x": 8.611473874971457, + "y": 2.813353753747135, + "heading": 0.40289733126075, + "angularVelocity": -0.2652057110299845, + "velocityX": 0.4418199173063615, + "velocityY": 0.24162482824885853, "moduleForcesX": [ - -54.21904272949312, - -54.729099389545915, - -55.805815985531794, - -56.244420654225245 + -49.87778962282822, + -50.57482599449837, + -51.759729184855516, + -52.369784201574845 ], "moduleForcesY": [ - -44.20576368971399, - -43.57579607050465, - -42.183797695943305, - -41.60055887427103 + -49.06077660106482, + -48.34454115681274, + -47.069973438706235, + -46.393130116240336 ], - "timestamp": 2.838173489364708 + "timestamp": 2.892500515371295 }, { - "x": 8.758740183629461, - "y": 2.7256665220403926, - "heading": 0.338462250167195, - "angularVelocity": -0.32152082573882035, - "velocityX": 0.5227855709523735, - "velocityY": 0.13031273171557645, + "x": 8.628730773925781, + "y": 2.817955732345581, + "heading": 0.38460441295906894, + "angularVelocity": -0.2809775235127061, + "velocityX": 0.2650643627074838, + "velocityY": 0.07068596319762284, "moduleForcesX": [ - -53.82607120430835, - -54.316194914018055, - -55.21550784813393, - -55.65085738495609 + -49.13353520650032, + -49.81148698796487, + -50.794430820642766, + -51.405616940966546 ], "moduleForcesY": [ - -44.689609283680795, - -44.09507977862367, - -42.960129353038816, - -42.39731433858149 + -49.81111505036111, + -49.135249239699505, + -48.11534331383063, + -47.464060182990515 ], - "timestamp": 2.899687466225105 + "timestamp": 2.95760507630908 }, { - "x": 8.779803276062012, - "y": 2.7246744632720947, - "heading": 0.31794821481648144, - "angularVelocity": -0.33348576043569805, - "velocityX": 0.34241148934902593, - "velocityY": -0.016127371679270322, + "x": 8.63409093131169, + "y": 2.809578261213375, + "heading": 0.3636328313393472, + "angularVelocity": -0.29609942384481297, + "velocityX": 0.07568048716897836, + "velocityY": -0.11828217921293742, "moduleForcesX": [ - -53.48098782939392, - -53.947745103828254, - -54.695219377692204, - -55.12018458029143 + -48.48056297546824, + -49.13563036489729, + -49.94272883080661, + -50.54684256882197 ], "moduleForcesY": [ - -45.107332847724315, - -44.55002787941316, - -43.626228500899906, - -43.09016461444635 + -50.45598549008582, + -49.81982384689785, + -49.00837234110654, + -48.38670375127015 ], - "timestamp": 2.9612014430855016 + "timestamp": 3.028431224321827 }, { - "x": 8.789390520920405, - "y": 2.711156481525927, - "heading": 0.29317696313872205, - "angularVelocity": -0.3456625720749201, - "velocityX": 0.13378216652004424, - "velocityY": -0.1886323872700164, + "x": 8.625782813087357, + "y": 2.7880784954449576, + "heading": 0.34158319665982656, + "angularVelocity": -0.31132054048107033, + "velocityX": -0.11730298000729854, + "velocityY": -0.3035568977229701, "moduleForcesX": [ - -53.179448380447525, - -53.62389224585945, - -54.2363017447046, - -54.649105974722524 + -49.434220030426786, + -50.08516326654055, + -50.87601640496041, + -51.47573234890385 ], "moduleForcesY": [ - -45.47706110036372, - -44.95351001122035, - -44.210687438028955, - -43.700838292783004 + -49.51809039841881, + -48.8612826879258, + -48.03470968086272, + -47.3932817354508 ], - "timestamp": 3.03286454368984 + "timestamp": 3.0992573723345744 }, { - "x": 8.783794368286188, - "y": 2.6855639318912203, - "heading": 0.267539420337893, - "angularVelocity": -0.35775095669356133, - "velocityX": -0.07808973637788985, - "velocityY": -0.35712311383241235, + "x": 8.603519939908493, + "y": 2.7537628223496577, + "heading": 0.3184472650429824, + "angularVelocity": -0.3266580530778029, + "velocityX": -0.314331271762177, + "velocityY": -0.4845057095173895, "moduleForcesX": [ - -54.03803305894964, - -54.47570349158013, - -55.06023489691758, - -55.46707669488016 + -50.50767113082232, + -51.15037434057846, + -51.926551544815524, + -52.51764005400783 ], "moduleForcesY": [ - -44.44927461362763, - -43.91326452339743, - -43.17589421817186, - -42.65354228524133 + -48.418122856235094, + -47.74052141391343, + -46.89226762522613, + -46.231320487911496 ], - "timestamp": 3.104527644294178 + "timestamp": 3.1700835203473217 }, { - "x": 8.76275609467282, - "y": 2.6482308380775303, - "heading": 0.2410424727336723, - "angularVelocity": -0.3697432483491591, - "velocityX": -0.2935719140806326, - "velocityY": -0.5209528125193889, + "x": 8.566978749430204, + "y": 2.7069943004295456, + "heading": 0.294215364928076, + "angularVelocity": -0.3421321192075174, + "velocityX": -0.5159279659217504, + "velocityY": -0.6603284695321164, "moduleForcesX": [ - -54.99526164627351, - -55.42219111728091, - -55.979540705697985, - -56.37649095420048 + -51.72253309664589, + -52.351547871619616, + -53.1150303533149, + -53.69192145893697 ], "moduleForcesY": [ - -43.254451016653434, - -42.70766979489076, - -41.972052319967815, - -41.439058648655944 + -47.11270598236726, + -46.414844244282484, + -45.535980106736204, + -44.856574967933696 ], - "timestamp": 3.1761907448985163 + "timestamp": 3.240909668360069 }, { - "x": 8.725986587983398, - "y": 2.5995490914917334, - "heading": 0.21369371175484705, - "angularVelocity": -0.38162960782036737, - "velocityX": -0.5130884148096344, - "velocityY": -0.679313986909043, + "x": 8.515792047639485, + "y": 2.648208808083207, + "heading": 0.26887617344655823, + "angularVelocity": -0.35776605381613247, + "velocityX": -0.7227091014678351, + "velocityY": -0.8299970278739272, "moduleForcesX": [ - -56.06650028997491, - -56.47788343064675, - -57.008812567039705, - -57.391121229420555 + -53.10459348185788, + -53.71272642624142, + -54.46576355767823, + -55.021155079771624 ], "moduleForcesY": [ - -41.850827206177, - -41.29576254132913, - -40.5570050084547, - -40.01607334967796 + -45.542660989830466, + -44.826209580052634, + -43.90453987460494, + -43.209067443671216 ], - "timestamp": 3.2478538455028545 + "timestamp": 3.311735816372816 }, { - "x": 8.673161820665543, - "y": 2.5399838352697053, - "heading": 0.1855016713653994, - "angularVelocity": -0.3933968827988571, - "velocityX": -0.7371264552102906, - "velocityY": -0.8311844689904736, + "x": 8.449541508295152, + "y": 2.577937395823057, + "heading": 0.24241650450925817, + "angularVelocity": -0.3735861638633528, + "velocityX": -0.9353966183846341, + "velocityY": -0.9921676419209313, "moduleForcesX": [ - -57.26911890755975, - -57.659157229516524, - -58.16431071641967, - -58.52626643425478 + -54.68387284740476, + -55.261648531274275, + -56.00637251373348, + -56.530738642405 ], "moduleForcesY": [ - -40.18252449430719, - -39.62280351144781, - -38.874499091161304, - -38.32952205223615 + -43.62575228611848, + -42.8942919991783, + -41.91307663268656, + -41.20610218263478 ], - "timestamp": 3.3195169461071927 + "timestamp": 3.3825619643855633 }, { - "x": 8.603918051131696, - "y": 2.4700943215420597, - "heading": 0.156476193662526, - "angularVelocity": -0.4050268193547327, - "velocityX": -0.9662402121860534, - "velocityY": -0.9752510446556707, + "x": 8.367749643224476, + "y": 2.496837777925061, + "heading": 0.2148211681213936, + "angularVelocity": -0.38962074265140273, + "velocityX": -1.1548258286749533, + "velocityY": -1.1450519359516806, "moduleForcesX": [ - -58.62178890554708, - -58.98353550726006, - -59.46309693106811, - -59.79789639516721 + -56.49342338399157, + -57.02842944478959, + -57.76596137264449, + -58.246993802956545 ], "moduleForcesY": [ - -38.17360914029891, - -37.61450756539028, - -36.848345847349954, - -36.304918084781264 + -41.245553252699736, + -40.505899357042395, + -39.44204855581045, + -38.731729530962944 ], - "timestamp": 3.391180046711531 + "timestamp": 3.4533881123983106 }, { - "x": 8.517847357752716, - "y": 2.3905626117140066, - "heading": 0.12662900025486085, - "angularVelocity": -0.41649319044197497, - "velocityX": -1.2010461821096667, - "velocityY": -1.1098000108474018, + "x": 8.269872586703466, + "y": 2.4057394247705677, + "heading": 0.18607303639793232, + "angularVelocity": -0.4058971514063899, + "velocityX": -1.381933922248534, + "velocityY": -1.2862248718947424, "moduleForcesX": [ - -60.14239359593657, - -60.46764931194161, - -60.920705975153766, - -61.22039017159285 + -58.56488116310085, + -59.04110442112179, + -59.76960478030494, + -60.19174438403556 ], "moduleForcesY": [ - -35.71931021724835, - -35.16851680517709, - -34.373858499651185, - -33.84003668835228 + -38.23433183892145, + -37.4988143615417, + -36.32045167969364, + -35.620919583734725 ], - "timestamp": 3.462843147315869 + "timestamp": 3.524214260411058 }, { - "x": 8.414494950035653, - "y": 2.302233586652294, - "heading": 0.09597460073164957, - "angularVelocity": -0.4277570920697149, - "velocityX": -1.4421983816704484, - "velocityY": -1.2325593550492429, + "x": 8.155297198594102, + "y": 2.3057088684340563, + "heading": 0.15615361511012685, + "angularVelocity": -0.4224346816435758, + "velocityX": -1.6176989900501801, + "velocityY": -1.4123393569068392, "moduleForcesX": [ - -61.84288712238317, - -62.122276292758905, - -62.54567545312701, - -62.801268264067 + -60.91614610648466, + -61.31371636368788, + -62.02428527901857, + -62.36884971040699 ], "moduleForcesY": [ - -32.67310530550032, - -32.14185463603263, - -31.305402017642134, - -30.792810837453125 + -34.34726778188899, + -33.637620843364225, + -32.301189412117985, + -31.636486262630484 ], - "timestamp": 3.5345062479202074 + "timestamp": 3.595040408423805 }, { - "x": 8.2933614907194, - "y": 2.2061711843005387, - "heading": 0.06453176165426741, - "angularVelocity": -0.4387591216710293, - "velocityX": -1.690318424610812, - "velocityY": -1.340472314784823, + "x": 8.023350887750192, + "y": 2.1981442323035503, + "heading": 0.12504476412995907, + "angularVelocity": -0.4392283337866834, + "velocityX": -1.8629604255784487, + "velocityY": -1.518713626938277, "moduleForcesX": [ - -63.717577250904, - -63.94109844594106, - -64.32744860294157, - -64.52959830151751 + -63.520716542602926, + -63.81730819571486, + -64.48569991401658, + -64.73298646364212 ], "moduleForcesY": [ - -28.828283743204985, - -28.332961805960878, - -27.43891241104092, - -26.96427075977155 + -29.22638447390868, + -28.58008989329779, + -27.0282766874693, + -26.43848196643227 ], - "timestamp": 3.6061693485245456 + "timestamp": 3.6658665564365522 }, { - "x": 8.153917386350976, - "y": 2.103737230084854, - "heading": 0.032325911066374, - "angularVelocity": -0.4494063237049454, - "velocityX": -1.9458285113605962, - "velocityY": -1.4293821136938516, + "x": 7.8733430094963675, + "y": 2.084908220562927, + "heading": 0.09273289280550447, + "angularVelocity": -0.4562138734219929, + "velocityX": -2.1179731280434493, + "velocityY": -1.5987882288931434, "moduleForcesX": [ - -65.71756976574333, - -65.87625105005283, - -66.21063177131644, - -66.35137198001577 + -66.23669578701445, + -66.41381020137457, + -66.98496208031126, + -67.12159504763625 ], "moduleForcesY": [ - -23.893867912535846, - -23.458019215236998, - -22.489715553229402, - -22.076652060585157 + -22.361245953920218, + -21.84052143654894, + -20.00571217853413, + -19.554879879038108 ], - "timestamp": 3.677832449128884 + "timestamp": 3.7366927044492995 }, { - "x": 7.995643377180051, - "y": 1.996698976059294, - "heading": -0.0006069291037716975, - "angularVelocity": -0.45955086917006693, - "velocityX": -2.2085844435447637, - "velocityY": -1.4936313545311521, + "x": 7.704675957081329, + "y": 1.9684978477709927, + "heading": 0.059218072302814256, + "angularVelocity": -0.47319840825818243, + "velocityX": -2.38142348761764, + "velocityY": -1.6436072842897478, "moduleForcesX": [ - -67.69752144856352, - -67.78734757334065, - -68.04263214008894, - -68.11934773122928 + -68.65865518215234, + -68.7193569333156, + -69.08393459750226, + -69.12136156007266 ], "moduleForcesY": [ - -17.47107619907958, - -17.127002103641317, - -16.07246012795104, - -15.752714784412063 + -13.088029266778213, + -12.787167251577607, + -10.613744042624726, + -10.394212606212953 ], - "timestamp": 3.749495549733222 + "timestamp": 3.8075188524620467 }, { - "x": 7.818124590832862, - "y": 1.8873638999769546, - "heading": -0.034213912395681174, - "angularVelocity": -0.468957985469511, - "velocityX": -2.4771295806372327, - "velocityY": -1.5256816291830964, + "x": 7.5170858672429315, + "y": 1.85220323216817, + "heading": 0.024531257518575638, + "angularVelocity": -0.4897458884534555, + "velocityX": -2.6485993535132915, + "velocityY": -1.6419728993576328, "moduleForcesX": [ - -69.313687888199, - -69.3432482954395, - -69.47611214375348, - -69.49889577515765 + -69.87540008016852, + -69.87946038423385, + -69.8592396435103, + -69.86628903728995 ], "moduleForcesY": [ - -9.057792824867414, - -8.844692568248144, - -7.710678951592893, - -7.521600845904924 + -0.7650699301565221, + -0.7967154045035195, + 1.692280326436183, + 1.5791599402263883 ], - "timestamp": 3.8211586503375603 + "timestamp": 3.878345000474794 }, { - "x": 7.621240028444532, - "y": 1.7787112968796408, - "heading": -0.06841646549100246, - "angularVelocity": -0.4772686753278808, - "velocityX": -2.747363157998911, - "velocityY": -1.5161582764496822, + "x": 7.3110452541555135, + "y": 1.7400985444349457, + "heading": -0.011240894625415731, + "angularVelocity": -0.5050698526983756, + "velocityX": -2.909103754313112, + "velocityY": -1.5828149755235674, "moduleForcesX": [ - -69.86517241058907, - -69.86591053003337, - -69.82282638095482, - -69.8236070877981 + -68.32136154860933, + -68.4142317509721, + -67.82249433370446, + -67.94358861469844 ], "moduleForcesY": [ - 1.837312109452161, - 1.8863774422298984, - 3.0464717958262337, - 3.075434675486523 + 14.610999455735499, + 14.191657426650783, + 16.77917087513115, + 16.30055641195774 ], - "timestamp": 3.8928217509418985 + "timestamp": 3.949171148487541 }, { - "x": 7.40547828858971, - "y": 1.6744202309428289, - "heading": -0.10310081581402811, - "angularVelocity": -0.48399176187648607, - "velocityX": -3.0107787415740312, - "velocityY": -1.4552965899789445, + "x": 7.0881669627233315, + "y": 1.636620998376319, + "heading": -0.04793860084545987, + "angularVelocity": -0.5181378240906105, + "velocityX": -3.146836270018103, + "velocityY": -1.4610076781248036, "moduleForcesX": [ - -68.17759957909404, - -68.20364257441437, - -67.95854331471489, - -67.98903555111266 + -62.398383827401425, + -62.73953495712387, + -61.584615770221966, + -61.958519697541895 ], "moduleForcesY": [ - 15.314544404106083, - 15.206518112264616, - 16.260066564096665, - 16.13994452069931 + 31.413609816398104, + 30.73509287925401, + 32.98416244544942, + 32.28414823175951 ], - "timestamp": 3.9644848515462368 + "timestamp": 4.019997296500288 }, { - "x": 7.172300767498466, - "y": 1.5786130838476118, - "heading": -0.13811594024177143, - "angularVelocity": -0.48860744417223123, - "velocityX": -3.253801735130716, - "velocityY": -1.3369104362952573, + "x": 6.851189181350193, + "y": 1.5457607903673927, + "heading": -0.08534517045150107, + "angularVelocity": -0.528146322447312, + "velocityX": -3.3459080865231883, + "velocityY": -1.282862481700599, "moduleForcesX": [ - -62.90664616032858, - -62.994759663175806, - -62.62640903247994, - -62.71838864361929 + -52.16729788107013, + -52.743022249429835, + -51.36856497292788, + -51.96098925114158 ], "moduleForcesY": [ - 30.40188460343236, - 30.221851782690536, - 30.976021760761594, - 30.79224774785525 + 46.4697214226326, + 45.81902631486864, + 47.35399851175358, + 46.706752999418384 ], - "timestamp": 4.036147952150575 + "timestamp": 4.090823444513036 }, { - "x": 6.924263851753144, - "y": 1.4952348436632368, - "heading": -0.17328998021723432, - "angularVelocity": -0.4908249807619052, - "velocityX": -3.461152443218535, - "velocityY": -1.1634751982713893, + "x": 6.603389718482076, + "y": 1.470425326889423, + "heading": -0.12323379914298056, + "angularVelocity": -0.534952552899821, + "velocityX": -3.498700265663482, + "velocityY": -1.0636673826227476, "moduleForcesX": [ - -53.63079242417267, - -53.732278169292705, - -53.450679135245664, - -53.553119173625326 + -39.932337350479486, + -40.53983959192529, + -39.373177991520045, + -39.983518610928336 ], "moduleForcesY": [ - 44.77877441586359, - 44.65783761257958, - 44.9941386909519, - 44.87304464918282 + 57.340323299968865, + 56.91405451985326, + 57.72751484701259, + 57.308022362111764 ], - "timestamp": 4.107811052754913 + "timestamp": 4.161649592525783 }, { - "x": 6.664645466021959, - "y": 1.4273879906699989, - "heading": -0.20846188348487105, - "angularVelocity": -0.49079516475048257, - "velocityX": -3.622762391548932, - "velocityY": -0.9467473835360443, + "x": 6.347913177850161, + "y": 1.4123915991053673, + "heading": -0.1614061090640271, + "angularVelocity": -0.5389578706747766, + "velocityX": -3.607093535369666, + "velocityY": -0.8193828044073681, "moduleForcesX": [ - -41.769412633151376, - -41.76713542840772, - -41.77205019669556, - -41.769773098851616 + -28.27085895786839, + -28.72327567698898, + -27.969794241953274, + -28.42128687481258 ], "moduleForcesY": [ - 56.01545952386056, - 56.01714957737264, - 56.01348552259297, - 56.01517566218937 + 63.91704070638253, + 63.71563591935232, + 64.05032104573215, + 63.85186792328192 ], - "timestamp": 4.179474153359251 + "timestamp": 4.23247574053853 }, { - "x": 6.396804785667352, - "y": 1.37708752891827, - "heading": -0.24350598675754967, - "angularVelocity": -0.4890118202694272, - "velocityX": -3.7374977930886835, - "velocityY": -0.7019018341035008, + "x": 6.087428151650664, + "y": 1.3726159298536926, + "heading": -0.19970318630453027, + "angularVelocity": -0.5407194703516945, + "velocityX": -3.677808740249677, + "velocityY": -0.561595828203393, "moduleForcesX": [ - -29.670078278170966, - -29.48805559061184, - -29.820795736138948, - -29.638793031028708 + -18.438441470262664, + -18.66329414572283, + -18.32289991687435, + -18.547288960002728 ], "moduleForcesY": [ - 63.27577283382489, - 63.36048200304406, - 63.204474407441516, - 63.28970026112863 + 67.42955829795206, + 67.36785699497005, + 67.46142450851352, + 67.4002635181382 ], - "timestamp": 4.25113725396359 + "timestamp": 4.303301888551277 }, { - "x": 6.123724098762273, - "y": 1.3454463182894645, - "heading": -0.27833593889663377, - "angularVelocity": -0.4860235162218966, - "velocityX": -3.8106178019395767, - "velocityY": -0.4415272345457245, + "x": 5.824078839371321, + "y": 1.3515584546015937, + "heading": -0.2379996051703208, + "angularVelocity": -0.5407101747069203, + "velocityX": -3.7182498225365426, + "velocityY": -0.29731216285133905, "moduleForcesX": [ - -18.964618645051026, - -18.60764038496825, - -19.189884865939113, - -18.832039003279412 + -10.576255375139242, + -10.574993214493354, + -10.57678518795997, + -10.575523009234347 ], "moduleForcesY": [ - 67.27850831701049, - 67.37773513616717, - 67.21397107913663, - 67.3147241815706 + 69.11369299701585, + 69.11388541630474, + 69.11361014421324, + 69.11380257613486 ], - "timestamp": 4.322800354567928 + "timestamp": 4.3741280365640245 }, { - "x": 5.847858828640413, - "y": 1.3329886237876336, - "heading": -0.3128943538499734, - "angularVelocity": -0.4822344367171772, - "velocityX": -3.849474384941141, - "velocityY": -0.17383694532855082, + "x": 5.5595602703116835, + "y": 1.3494038487343114, + "heading": -0.27619370702127155, + "angularVelocity": -0.5392655526610999, + "velocityX": -3.734758651734541, + "velocityY": -0.030421051091110783, "moduleForcesX": [ - -10.164815803497616, - -9.673316441913729, - -10.413603531769702, - -9.919643964436528 + -4.3819858465305295, + -4.179901202476201, + -4.454918349746145, + -4.252321785026191 ], "moduleForcesY": [ - 69.17032970505136, - 69.24042697051493, - 69.13258323678724, - 69.20483068344068 + 69.79184657492922, + 69.8041566069463, + 69.78698263696384, + 69.79953511767619 ], - "timestamp": 4.394463455172266 - }, - { - "x": 5.571170150686736, - "y": 1.339901067236654, - "heading": -0.3471395996450313, - "angularVelocity": -0.4778644170607483, - "velocityX": -3.8609643682780583, - "velocityY": 0.09645749891823462, - "moduleForcesX": [ - -3.140660151426149, - -2.5499567673882018, - -3.391519077751636, - -2.7966358407203566 - ], - "moduleForcesY": [ - 69.85390969745158, - 69.87766236501122, - 69.84141674708727, - 69.86745404796969 - ], - "timestamp": 4.466126555776604 + "timestamp": 4.444954184576772 }, { "x": 5.295215606689453, "y": 1.3661898374557495, - "heading": -0.3810366506112695, - "angularVelocity": -0.4730056427977949, - "velocityX": -3.850720128910756, - "velocityY": 0.3668383030792834, + "heading": -0.3142003336934002, + "angularVelocity": -0.5366185757453372, + "velocityX": -3.7323032670738447, + "velocityY": 0.23700270581448382, "moduleForcesX": [ - 2.4358890124890213, - 3.105036958141336, - 2.1874776648542436, - 2.8624717837810634 + 0.5151198465758231, + 0.8900974969762288, + 0.3932026550090731, + 0.770045001718678 ], "moduleForcesY": [ - 69.89157339518185, - 69.86477674457022, - 69.89901845264843, - 69.87435635433927 + 69.9365194091976, + 69.93262774089895, + 69.93690494603392, + 69.93364443708154 ], - "timestamp": 4.537789656380943 + "timestamp": 4.515780332589519 }, { - "x": 5.053706815736057, - "y": 1.4043054270449447, - "heading": -0.4105962505484791, - "angularVelocity": -0.46833496540702335, - "velocityX": -3.8264053470585537, - "velocityY": 0.6038939420574941, + "x": 5.05792538929108, + "y": 1.3966823635324737, + "heading": -0.3482525379686159, + "angularVelocity": -0.5333202229677834, + "velocityX": -3.716401752678385, + "velocityY": 0.4775691075577662, "moduleForcesX": [ - 6.887915896080987, - 7.623701240859441, - 6.643290267386372, - 7.386549953753599 + 4.430042032874039, + 4.95053093134464, + 4.273296521707641, + 4.797518672081996 ], "moduleForcesY": [ - 69.5924526319861, - 69.51542701820333, - 69.6153579444215, - 69.54014360764195 + 69.79829505364833, + 69.76315167405717, + 69.80750234348231, + 69.77327260349615 ], - "timestamp": 4.600906019925398 + "timestamp": 4.579629785148668 }, { - "x": 4.814521797126242, - "y": 1.457279005501737, - "heading": -0.43977801159576735, - "angularVelocity": -0.46234857980584404, - "velocityX": -3.789588074752571, - "velocityY": 0.8393002302720002, + "x": 4.822357731816456, + "y": 1.442469918801244, + "heading": -0.3819972270935512, + "angularVelocity": -0.5285039694533855, + "velocityX": -3.6894232923360217, + "velocityY": 0.7171174291017512, "moduleForcesX": [ - 10.475140797961476, - 11.417395555766568, - 10.184857658397029, - 11.139880728378749 + 7.554810386078761, + 8.313127657144003, + 7.335080255073474, + 8.101550015304404 ], "moduleForcesY": [ - 69.13513408837922, - 68.98542996372927, - 69.17724110186298, - 69.02951613308481 + 69.52196945360694, + 69.43512984509572, + 69.54458000792314, + 69.45920878774324 ], - "timestamp": 4.664022383469854 + "timestamp": 4.643479237707816 }, { - "x": 4.578623056803542, - "y": 1.5249252632965482, - "heading": -0.46848135774782446, - "angularVelocity": -0.4547686929371364, - "velocityX": -3.7375210971485733, - "velocityY": 1.071770520289322, + "x": 4.589376593940337, + "y": 1.503428017324539, + "heading": -0.4153167376730134, + "angularVelocity": -0.5218448905038962, + "velocityX": -3.6489136325843017, + "velocityY": 0.9547160716346837, "moduleForcesX": [ - 14.840754027841374, - 16.029076302383867, - 14.51917987513575, - 15.728849295978776 + 11.364581723457395, + 12.409065316223296, + 11.08567451247928, + 12.146230097766022 ], "moduleForcesY": [ - 68.32109565083954, - 68.05166292944023, - 68.38835125483823, - 68.11984422324372 + 68.99256903562845, + 68.81203194141854, + 69.03650497066894, + 68.8574640226182 ], - "timestamp": 4.72713874701431 + "timestamp": 4.707328690266965 }, { - "x": 4.347193447367128, - "y": 1.6069250977069311, - "heading": -0.4965850522073392, - "angularVelocity": -0.4452679603399553, - "velocityX": -3.6667132965195126, - "velocityY": 1.299185025966005, + "x": 4.360049392260068, + "y": 1.5793325904196134, + "heading": -0.4480682781137704, + "angularVelocity": -0.5129494322667391, + "velocityX": -3.591686263368148, + "velocityY": 1.1888053859938605, "moduleForcesX": [ - 20.193590004699995, - 21.665646251529257, - 19.875228316396825, - 21.38196372017987 + 16.060474026737822, + 17.44417496465417, + 15.743033698279458, + 17.156332450571153 ], "moduleForcesY": [ - 66.92237625745865, - 66.45990723727093, - 67.0149915440133, - 66.54903999850195 + 68.04180346878879, + 67.69982630825619, + 68.1137623119266, + 67.77111438492656 ], - "timestamp": 4.790255110558766 + "timestamp": 4.7711781428261135 }, { - "x": 4.121687707159994, - "y": 1.7027378890121, - "heading": -0.5239423865994234, - "angularVelocity": -0.4334428166606177, - "velocityX": -3.572856982615943, - "velocityY": 1.5180340869556606, + "x": 4.135705069539603, + "y": 1.6697908456139685, + "heading": -0.4800781004717692, + "angularVelocity": -0.5013327612847737, + "velocityX": -3.5136452033420373, + "velocityY": 1.4167428469423091, "moduleForcesX": [ - 26.76366004990429, - 28.54407768472358, - 26.51477620428529, - 28.34898574175057 + 21.89196611243463, + 23.664965264016004, + 21.586990023029227, + 23.41134616769712 ], "moduleForcesY": [ - 64.56136029572545, - 63.79374619278623, - 64.66009920375362, - 63.876720382176266 + 66.38195678091566, + 65.77026762364113, + 66.47840712872917, + 65.85754632928692 ], - "timestamp": 4.853371474103222 + "timestamp": 4.835027595385262 }, { - "x": 3.903869186822254, - "y": 1.8114606574002452, - "heading": -0.5503757290218597, - "angularVelocity": -0.4188033172066106, - "velocityX": -3.4510625787926257, - "velocityY": 1.7225765599053773, + "x": 3.9179949909601377, + "y": 1.774123115557712, + "heading": -0.5111344027979444, + "angularVelocity": -0.48639888176653956, + "velocityX": -3.409740723740524, + "velocityY": 1.6340354656462184, "moduleForcesX": [ - 34.722789021923006, - 36.79190026410112, - 34.65315726916901, - 36.79826370259493 + 29.124210645761526, + 31.309392995735184, + 28.932435627249138, + 31.199954435020395 ], "moduleForcesY": [ - 60.634978039609805, - 59.40218821323202, - 60.6689615218732, - 59.39227430417506 + 63.52533931919331, + 62.47695800696286, + 63.60772502954319, + 62.52636867386374 ], - "timestamp": 4.9164878376476775 + "timestamp": 4.898877047944411 }, { - "x": 3.6957796336708695, - "y": 1.9316273259274328, - "heading": -0.5756742900350444, - "angularVelocity": -0.40082412218451013, - "velocityX": -3.296919237193045, - "velocityY": 1.903890873601256, + "x": 3.7089231153715345, + "y": 1.8911721418901661, + "heading": -0.5409813219232001, + "angularVelocity": -0.46745771387165014, + "velocityX": -3.2744505584433368, + "velocityY": 1.8332032874365511, "moduleForcesX": [ - 43.97155118149319, - 46.20214883747921, - 44.22377698479971, - 46.54083607831509 + 37.90825639910293, + 40.44135259696435, + 37.994172581878566, + 40.64072265092307 ], "moduleForcesY": [ - 54.28134738689487, - 52.39692915304774, - 54.06704722487376, - 52.087132813104375 + 58.68761411990541, + 56.972077301885136, + 58.62385597562841, + 56.821660386292294 ], - "timestamp": 4.979604201192133 + "timestamp": 4.9627265005035595 }, { - "x": 3.499550666767407, - "y": 2.061002715105782, - "heading": -0.5996083713418497, - "angularVelocity": -0.37920564434843623, - "velocityX": -3.109003052199785, - "velocityY": 2.0497915582101762, + "x": 3.5107601533164168, + "y": 2.019044446471857, + "heading": -0.5693252783250675, + "angularVelocity": -0.4439185500550772, + "velocityX": -3.1035968847429976, + "velocityY": 2.0027157548960086, "moduleForcesX": [ - 53.763954059728796, - 55.84492515312586, - 54.42048149959161, - 56.552580490225075 + 47.952424172958146, + 50.57190975238657, + 48.49980640396711, + 51.2266283249439 ], "moduleForcesY": [ - 44.579593956853635, - 41.94760445118944, - 43.76152240959032, - 40.974101674150546 + 50.78756695761204, + 48.182517312375914, + 50.25197010379258, + 47.47244005809576 ], - "timestamp": 5.042720564736589 + "timestamp": 5.026575953062708 }, { - "x": 3.3170061238667126, - "y": 2.1965515433699756, - "heading": -0.621969243888579, - "angularVelocity": -0.3542801151872387, - "velocityX": -2.8921904344524014, - "velocityY": 2.1476019949837775, + "x": 3.3257171127383325, + "y": 2.154907115536406, + "heading": -0.5958715572695051, + "angularVelocity": -0.41576361081320273, + "velocityX": -2.898114755277928, + "velocityY": 2.1278595762225385, "moduleForcesX": [ - 62.44766102413855, - 63.94433819362826, - 63.329847528999416, - 64.77963805969534 + 58.0104977519177, + 60.19934179791254, + 59.013462507055486, + 61.2087388169446 ], "moduleForcesY": [ - 31.256641416018002, - 28.0802822128175, - 29.404706704310698, - 26.071343409245944 + 38.879821467287535, + 35.405230100188476, + 37.3182212633444, + 33.60787910396 ], - "timestamp": 5.105836928281045 + "timestamp": 5.090425405621857 }, { - "x": 3.149232244356222, - "y": 2.334780583324695, - "heading": -0.6426179790087161, - "angularVelocity": -0.32715343471259145, - "velocityX": -2.6581677094296006, - "velocityY": 2.1900666038429124, + "x": 3.155400510330475, + "y": 2.2951308910705395, + "heading": -0.620392443592049, + "angularVelocity": -0.38404223277918687, + "velocityX": -2.667471616143963, + "velocityY": 2.19616253411468, "moduleForcesX": [ - 68.07446024974723, - 68.73289751434758, - 68.68747354304334, - 69.20841989455995 + 65.83208211172878, + 67.07228199343204, + 66.81818884359254, + 67.90516981439036 ], "moduleForcesY": [ - 15.578092751741488, - 12.393772344765436, - 12.552654231475199, - 9.321777066848862 + 23.286937742879168, + 19.455710293987412, + 20.244285253522296, + 16.268687090512074 ], - "timestamp": 5.168953291825501 + "timestamp": 5.154274858181005 }, { - "x": 2.996456707624642, - "y": 2.4723242803752896, - "heading": -0.6615054771208647, - "angularVelocity": -0.2992488326556613, - "velocityX": -2.4205376886767986, - "velocityY": 2.179208200955956, + "x": 3.000433493753425, + "y": 2.4359044574852438, + "heading": -0.6427805397589214, + "angularVelocity": -0.3506388116034793, + "velocityX": -2.427068837175911, + "velocityY": 2.2047732716940165, "moduleForcesX": [ - 69.84418644116603, - 69.8030417146416, - 69.73398928648147, - 69.55639119192205 + 69.54650836600102, + 69.78388265544288, + 69.79679859116072, + 69.82517169644473 ], "moduleForcesY": [ - 0.04124705741425996, - -2.629684927774557, - -3.7965442820408897, - -6.360953421225059 + 6.348124333641362, + 2.931670853614007, + 2.0228290324436116, + -1.3111191685548196 ], - "timestamp": 5.232069655369957 + "timestamp": 5.218124310740154 }, { - "x": 2.858285394290842, - "y": 2.6063679764517347, - "heading": -0.6786531021168939, - "angularVelocity": -0.2716827147994899, - "velocityX": -2.18915199758744, - "velocityY": 2.1237550541395294, + "x": 2.8605755262449746, + "y": 2.5738952164278426, + "heading": -0.6630425145247281, + "angularVelocity": -0.3173398354048022, + "velocityX": -2.190433306830491, + "velocityY": 2.1611893823955435, "moduleForcesX": [ - 68.5832050639289, - 68.18255030614512, - 67.65433881512726, - 67.18772346415568 + 69.26890065109001, + 68.89986618517693, + 68.45222405005646, + 67.96002786407358 ], "moduleForcesY": [ - -13.292687011674952, - -15.252960265172256, - -17.395421555860977, - -19.15158282474659 + -8.993101394774568, + -11.54522411202305, + -13.87299037870192, + -16.161430765057244 ], - "timestamp": 5.295186018914412 + "timestamp": 5.281973763299303 }, { - "x": 2.734026522437346, - "y": 2.734745646026211, - "heading": -0.6941195885833025, - "angularVelocity": -0.2450471731552614, - "velocityX": -1.9687267275145863, - "velocityY": 2.0339839364169796, + "x": 2.735118463226934, + "y": 2.70652006019163, + "heading": -0.6812534516483567, + "angularVelocity": -0.28521680913017455, + "velocityX": -1.9648886245629291, + "velocityY": 2.0771492698536242, "moduleForcesX": [ - 65.71596180721829, - 65.23236893280917, - 64.13301722958752, - 63.6607820099661 + 66.56985010341027, + 66.01724983813499, + 64.83928204278061, + 64.28532521518495 ], "moduleForcesY": [ - -23.750213140906997, - -25.069078070391868, - -27.734992318593385, - -28.82188410507732 + -21.21291575912241, + -22.903571424790023, + -26.016891342337594, + -27.382914500304768 ], - "timestamp": 5.358302382458868 + "timestamp": 5.345823215858451 }, { - "x": 2.622913491858143, - "y": 2.8558455619096765, - "heading": -0.7079766418555111, - "angularVelocity": -0.21954771304985882, - "velocityX": -1.7604472808535805, - "velocityY": 1.918677013104065, + "x": 2.623211412340952, + "y": 2.8318836413124444, + "heading": -0.6975143593601079, + "angularVelocity": -0.2546757577394683, + "velocityX": -1.7526704834675835, + "velocityY": 1.9634245259140095, "moduleForcesX": [ - 62.3261455799643, - 61.90369851571434, - 60.3053013643231, - 59.94983415338624 + 62.957124910706376, + 62.45929870418039, + 60.62064580180965, + 60.211130480488805 ], "moduleForcesY": [ - -31.626927610118603, - -32.460334406565565, - -35.32513309689565, - -35.9385060308983 + -30.33692388884184, + -31.367391790541795, + -34.7676381916512, + -35.48902427322522 ], - "timestamp": 5.421418746003324 + "timestamp": 5.4096726684176 }, { - "x": 2.5242136717280004, - "y": 2.968472284388882, - "heading": -0.720296269771335, - "angularVelocity": -0.19518912725614584, - "velocityX": -1.5637754551658334, - "velocityY": 1.7844298396544638, + "x": 2.524027231935006, + "y": 2.9486107208127024, + "heading": -0.7119282315157714, + "angularVelocity": -0.22574777978418986, + "velocityX": -1.5534069037485454, + "velocityY": 1.8281610072087373, "moduleForcesX": [ - 58.98754783726104, - 58.678775107744755, - 56.70105562415566, - 56.492209664989446 + 59.28998740855844, + 58.93448494902304, + 56.604293732503145, + 56.387554803221356 ], "moduleForcesY": [ - -37.51207015880541, - -38.003843268920306, - -40.88479342874241, - -41.18285690919068 + -37.023359271023516, + -37.59971228683433, + -41.01043622517381, + -41.32007696242945 ], - "timestamp": 5.48453510954778 + "timestamp": 5.473522120976749 }, { - "x": 2.437268193420317, - "y": 3.0717284892697196, - "heading": -0.7311460366120948, - "angularVelocity": -0.17190101316781217, - "velocityX": -1.377542580482226, - "velocityY": 1.6359656843682122, + "x": 2.436823832988882, + "y": 3.0556924556358678, + "heading": -0.7245906689335784, + "angularVelocity": -0.19831708668256617, + "velocityX": -1.3657658045751015, + "velocityY": 1.6770971485459225, "moduleForcesX": [ - 55.933543070193124, - 55.74620404374717, - 53.4983817914488, - 53.427889596621455 + 55.924906891884625, + 55.72478848329192, + 53.0592023113592, + 53.02122971774716 ], "moduleForcesY": [ - -41.94896951930148, - -42.20583852522306, - -45.01283639047294, - -45.10426074033776 + -41.9548706445922, + -42.23018067659876, + -45.52453660860857, + -45.577986795152945 ], - "timestamp": 5.547651473092236 + "timestamp": 5.537371573535897 }, { - "x": 2.3614992683800686, - "y": 3.1649283314618284, - "heading": -0.7405881776451042, - "angularVelocity": -0.1495989392094639, - "velocityX": -1.2004640442708914, - "velocityY": 1.4766351696809092, + "x": 2.3609561392644562, + "y": 3.1523738866134026, + "heading": -0.7355878255117103, + "angularVelocity": -0.17223572227098613, + "velocityX": -1.188227787139511, + "velocityY": 1.5142092391156017, "moduleForcesX": [ - 53.226119004537075, - 53.150380788764224, - 50.717908435297424, - 50.76595922311749 + 52.96028064478196, + 52.90088037948619, + 50.017708559288, + 50.12810503784231 ], "moduleForcesY": [ - -45.3497145739425, - -45.44508539739661, - -48.13839282499532, - -48.093987559665536 + -45.65625782653057, + -45.73286478403445, + -48.86224974564624, + -48.75628870048946 ], - "timestamp": 5.6107678366366915 + "timestamp": 5.601221026095046 }, { - "x": 2.2964046195038508, - "y": 3.2475371925356646, - "heading": -0.7486801929297746, - "angularVelocity": -0.12820788192226776, - "velocityX": -1.0313434618325024, - "velocityY": 1.3088342932756496, + "x": 2.295869220398745, + "y": 3.238077294744024, + "heading": -0.7449971922573848, + "angularVelocity": -0.14736800972503292, + "velocityX": -1.0193810010417363, + "velocityY": 1.3422731863085038, "moduleForcesX": [ - 50.85429251327673, - 50.87522935086129, - 48.32191532894057, - 48.467633279346366 + 50.38715799993498, + 50.44685956622395, + 47.42973162422406, + 47.658319113484225 ], "moduleForcesY": [ - -48.006099795729824, - -47.9893857552052, - -50.55448655081397, - -50.41998583087466 + -48.493835508606004, + -48.43806643870586, + -51.39038837291991, + -51.18442141820145 ], - "timestamp": 5.673884200181147 + "timestamp": 5.6650704786541946 }, { - "x": 2.2415479359062194, - "y": 3.3191302814359016, - "heading": -0.7554758085003378, - "angularVelocity": -0.10766804658789833, - "velocityX": -0.8691356807810039, - "velocityY": 1.1343031328129516, + "x": 2.2410859959949136, + "y": 3.3123508545538702, + "heading": -0.7528891490451303, + "angularVelocity": -0.1236025756122906, + "velocityX": -0.8580061724582897, + "velocityY": 1.1632607145854852, "moduleForcesX": [ - 48.7813998909101, - 48.883997299800775, - 46.25717342000062, - 46.48203569715231 + 48.16035953471259, + 48.31828690116759, + 45.225962077158464, + 45.54734290961062 ], "moduleForcesY": [ - -50.12061198883146, - -50.02516024483403, - -52.45958599626199, - -52.26481927286433 + -50.71618060999772, + -50.57101713150958, + -53.34991243772539, + -53.08075358781461 ], - "timestamp": 5.737000563725603 + "timestamp": 5.728919931213343 }, { - "x": 2.1965490021845606, - "y": 3.379364027011461, - "heading": -0.7610258706649285, - "angularVelocity": -0.08793380754076002, - "velocityX": -0.7129519382079801, - "velocityY": 0.9543285162988591, + "x": 2.1961946939313863, + "y": 3.374834024346928, + "heading": -0.7593284846297825, + "angularVelocity": -0.10085185270284114, + "velocityX": -0.7030804535393855, + "velocityY": 0.9786014959983499, "moduleForcesX": [ - 46.96552536511235, - 47.136382750106904, - 44.47147307724585, - 44.76001886720274 + 46.22788871890863, + 46.46616016540548, + 43.34033970137869, + 43.73431169906387 ], "moduleForcesY": [ - -51.83392911935416, - -51.682542993841096, - -53.98932327657365, - -53.75408240792354 + -52.491972499861475, + -52.28563530920509, + -54.900801591760306, + -54.591680501964085 ], - "timestamp": 5.800116927270059 + "timestamp": 5.792769383772492 }, { - "x": 2.16107481240881, - "y": 3.4279560426439364, - "heading": -0.7653786869846049, - "angularVelocity": -0.06896494150222135, - "velocityX": -0.5620442589466436, - "velocityY": 0.7698798362844577, + "x": 2.1608377743767466, + "y": 3.4252338531970117, + "heading": -0.7643752716013872, + "angularVelocity": -0.07904197717168335, + "velocityX": -0.5537544667573492, + "velocityY": 0.7893541264648288, "moduleForcesX": [ - 45.36776130374439, - 45.59551423238422, - 42.91858789949486, - 43.2583272033556 + 44.54201907610667, + 44.84605364869624, + 41.71600811601532, + 42.16694704188603 ], "moduleForcesY": [ - -53.2445780347107, - -53.0530790830467, - -55.238217704660805, - -54.97579973773786 + -53.93680181997976, + -53.68810804227729, + -56.151644018849424, + -55.81741599524645 ], - "timestamp": 5.863233290814515 + "timestamp": 5.8566188363316405 }, { - "x": 2.134832009400895, - "y": 3.4646708607022925, - "heading": -0.768580123623318, - "angularVelocity": -0.050722767582422834, - "velocityX": -0.4157844580103423, - "velocityY": 0.5817004655614504, + "x": 2.1347026814888364, + "y": 3.4633084503090976, + "heading": -0.7680853894338862, + "angularVelocity": -0.058107277099393444, + "velocityX": -0.40932368000649094, + "velocityY": 0.5963183016614753, "moduleForcesX": [ - 43.954428645312426, - 44.2297096749135, - 41.559591533194514, - 41.940634703058834 + 43.06208338047946, + 43.420291447119574, + 40.30623488983344, + 40.80225251311549 ], "moduleForcesY": [ - -54.42275064440647, - -54.20223828129335, - -56.273239100999206, - -55.992648881357695 + -55.13137229057452, + -54.85302592490151, + -57.17764132287779, + -56.82788011765582 ], - "timestamp": 5.926349654358971 + "timestamp": 5.920468288890789 }, { - "x": 2.1175606055664447, - "y": 3.489309593661584, - "heading": -0.770673719921477, - "angularVelocity": -0.03317042016662467, - "velocityX": -0.27364383599643854, - "velocityY": 0.39036997025244236, + "x": 2.1175142950444665, + "y": 3.488855217863188, + "heading": -0.7705109754915318, + "angularVelocity": -0.037989144157478276, + "velocityX": -0.26920178255948496, + "velocityY": 0.40010942193161164, "moduleForcesX": [ - 42.69713344336191, - 43.01233196517679, - 40.36254825642123, - 40.77712883283335 + 41.75436699647201, + 42.15764707113133, + 39.07345325528694, + 39.60555733885558 ], "moduleForcesY": [ - -55.41943823043682, - -55.177780690773446, - -57.14258045212568, - -56.849966899553834 + -56.13323789601473, + -55.83391387223054, + -58.03186064603064, + -57.67277468150624 ], - "timestamp": 5.9894660179034265 + "timestamp": 5.984317741449938 }, { "x": 2.1090288162231445, "y": 3.501702308654785, "heading": -0.771700836029523, - "angularVelocity": -0.016273372709797004, - "velocityX": -0.13517555296560768, - "velocityY": 0.19634710077161807, + "angularVelocity": -0.018635407044235015, + "velocityX": -0.13289822357460562, + "velocityY": 0.20120909853840863, "moduleForcesX": [ - 41.5722477087897, - 41.92121432741254, - 39.30147273432098, - 39.743500474089345 + 40.59125684604141, + 41.032490008004295, + 37.98763054279593, + 38.5490223991702 ], "moduleForcesY": [ - -56.272332012188514, - -56.01516696263238, - -57.881532814470596, - -57.581128152106956 + -56.98424266070932, + -56.6699287318646, + -58.752507438065514, + -58.388136078887676 ], - "timestamp": 6.052582381447882 + "timestamp": 6.048167194009086 }, { "x": 2.1090288162231445, "y": 3.501702308654785, "heading": -0.771700836029523, - "angularVelocity": 7.034369509774633e-21, - "velocityX": 1.0745130979383846e-20, - "velocityY": 7.792911077792643e-21, + "angularVelocity": -4.8374542557573845e-23, + "velocityX": -1.6522222635939176e-22, + "velocityY": 1.1005265766974004e-22, "moduleForcesX": [ - 40.56018810627996, - 40.937946455779034, - 38.35525709358692, - 38.819933249113355 + 39.55019943140344, + 40.023804726295744, + 37.02474315966745, + 37.61025741647993 ], "moduleForcesY": [ - -57.00975798757109, - -56.74117440782203, - -58.516369460899966, - -58.211114035733786 + -57.7153729196056, + -57.39024948338779, + -59.367606493987516, + -59.000582824693424 ], - "timestamp": 6.115698744992338 + "timestamp": 6.112016646568235 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/source 3.traj b/src/main/deploy/choreo/source 3.traj index 0efa1f5d..0796f3db 100644 --- a/src/main/deploy/choreo/source 3.traj +++ b/src/main/deploy/choreo/source 3.traj @@ -4,9 +4,9 @@ "x": 0.7778744697570801, "y": 4.333664894104004, "heading": -1.051650156247713, - "angularVelocity": -1.3297634990832717e-18, - "velocityX": 8.096904896283762e-18, - "velocityY": -2.035450603469087e-18, + "angularVelocity": -1.595681461469011e-18, + "velocityX": 6.125258661649382e-18, + "velocityY": -3.9185398661174254e-18, "moduleForcesX": [ 0, 0, @@ -22,4099 +22,4078 @@ "timestamp": 0 }, { - "x": 0.7908719793717586, - "y": 4.323476837419605, - "heading": -1.0413708097378205, - "angularVelocity": 0.15431550267407224, - "velocityX": 0.19512108359903374, - "velocityY": -0.15294504247054264, + "x": 0.7908719793717547, + "y": 4.323476837419779, + "heading": -1.0413708097375791, + "angularVelocity": 0.15431550267789765, + "velocityX": 0.19512108359923522, + "velocityY": -0.1529450424681367, "moduleForcesX": [ - 63.74590962361433, - 58.38725396448112, - 49.37860903101583, - 45.506342584710694 + 63.74590962413147, + 58.38725396488212, + 49.378609031450075, + 45.50634258496312 ], "moduleForcesY": [ - -28.834822023862102, - -38.568182065872634, - -49.55287419418438, - -53.15308295520182 + -28.834822022718374, + -38.568182065265304, + -49.55287419375032, + -53.15308295498522 ], - "timestamp": 0.06661253297151565 + "timestamp": 0.06661253297142743 }, { - "x": 0.8168891188149916, - "y": 4.303112092011227, - "heading": -1.0210382023309228, - "angularVelocity": 0.3052369651757899, - "velocityX": 0.3905742400511663, - "velocityY": -0.30571942695958465, + "x": 0.8168891188150114, + "y": 4.303112092011752, + "heading": -1.0210382023301807, + "angularVelocity": 0.3052369651837109, + "velocityX": 0.3905742400520384, + "velocityY": -0.305719426954728, "moduleForcesX": [ - 63.57502695706841, - 58.50337248946953, - 49.388651702378716, - 45.92040304941029 + 63.5750269575728, + 58.50337248985279, + 49.38865170273007, + 45.92040304961696 ], "moduleForcesY": [ - -29.201580891943905, - -38.38697260898019, - -49.53833328341483, - -52.7922647589104 + -29.20158089084509, + -38.38697260839577, + -49.53833328306312, + -52.792264758730134 ], - "timestamp": 0.1332250659430313 + "timestamp": 0.13322506594285485 }, { - "x": 0.8559503614500349, - "y": 4.272583897748778, - "heading": -0.9908994985401418, - "angularVelocity": 0.4524479470502735, - "velocityX": 0.5863947952819387, - "velocityY": -0.45829505200625403, + "x": 0.8559503614500515, + "y": 4.272583897749817, + "heading": -0.9908994985386197, + "angularVelocity": 0.4524479470625842, + "velocityX": 0.5863947952826685, + "velocityY": -0.4582950519991483, "moduleForcesX": [ - 63.36650355926644, - 58.673994480374716, - 49.332465125535784, - 46.42312032557307 + 63.36650355975494, + 58.673994480740184, + 49.33246512578875, + 46.42312032573131 ], "moduleForcesY": [ - -29.64202317080124, - -38.12000070194584, - -49.58929691252429, - -52.34676597725229 + -29.642023169756015, + -38.120000701382985, + -49.589296912271145, + -52.34676597711144 ], - "timestamp": 0.19983759891454694 + "timestamp": 0.19983759891428227 }, { - "x": 0.9080832434863456, - "y": 4.231907621482131, - "heading": -0.9512364839325822, - "angularVelocity": 0.5954287104578356, - "velocityX": 0.7826287293204035, - "velocityY": -0.6106399869832296, + "x": 0.908083243486343, + "y": 4.231907621483826, + "heading": -0.9512364839299804, + "angularVelocity": 0.5954287104748315, + "velocityX": 0.7826287293211497, + "velocityY": -0.6106399869741947, "moduleForcesX": [ - 63.1087538854026, - 58.88683733974907, - 49.24358469003159, - 47.01667690510743 + 63.10875388586886, + 58.88683734009576, + 49.24358469017145, + 47.016676905215164 ], "moduleForcesY": [ - -30.17612796676512, - -37.78365700090767, - -49.672009408886844, - -51.809713634059214 + -30.176127965788748, + -37.78365700036693, + -49.6720094087466, + -51.80971363396091 ], - "timestamp": 0.2664501318860626 + "timestamp": 0.2664501318857097 }, { - "x": 0.9733191777952942, - "y": 4.181101065602064, - "heading": -0.9023828510876672, - "angularVelocity": 0.7334000174682638, - "velocityX": 0.9793342393516886, - "velocityY": -0.7627176690877809, + "x": 0.9733191777952241, + "y": 4.1811010656045475, + "heading": -0.9023828510836664, + "angularVelocity": 0.7334000174902384, + "velocityX": 0.9793342393519744, + "velocityY": -0.762717669076962, "moduleForcesX": [ - 62.78758091347448, - 59.12644232420397, - 49.162542801992586, - 47.70378434486226 + 62.78758091390919, + 59.1264423245301, + 49.16254280200449, + 47.70378434491597 ], "moduleForcesY": [ - -30.82641384591526, - -37.399521862180414, - -49.74596927329427, - -51.172358294771385 + -30.826413845028178, + -37.399521861664304, + -49.745969273280835, + -51.17235829472075 ], - "timestamp": 0.33306266485757824 + "timestamp": 0.33306266485713715 }, { - "x": 1.051694367518301, - "y": 4.120184874713526, - "heading": -0.8447458342032448, - "angularVelocity": 0.8652578473343552, - "velocityX": 1.1765832378198438, - "velocityY": -0.9144854304607654, + "x": 1.0516943675181405, + "y": 4.120184874716908, + "heading": -0.8447458341975047, + "angularVelocity": 0.8652578473616123, + "velocityX": 1.1765832378200387, + "velocityY": -0.9144854304484828, "moduleForcesX": [ - 62.38681753882747, - 59.37326562455472, - 49.13769712317514, - 48.48705032206493 + 62.386817539217134, + 59.37326562485723, + 49.13769712304434, + 48.487050322059 ], "moduleForcesY": [ - -31.615322936234296, - -36.99658263811015, - -49.763347170131574, - -50.42430963220079 + -31.615322935463205, + -36.99658263762404, + -49.76334717025892, + -50.42430963220592 ], - "timestamp": 0.3996751978290939 + "timestamp": 0.3996751978285646 }, { - "x": 1.1432508333296558, - "y": 4.049183171168427, - "heading": -0.778832566995066, - "angularVelocity": 0.9895024895144598, - "velocityX": 1.3744630586335072, - "velocityY": -1.065891062504106, + "x": 1.1432508333293583, + "y": 4.049183171172805, + "heading": -0.7788325669872294, + "angularVelocity": 0.9895024895472444, + "velocityX": 1.3744630586332751, + "velocityY": -1.0658910624905655, "moduleForcesX": [ - 61.889733856839435, - 59.602244362756366, - 49.22597947357009, - 49.36848790198351 + 61.88973385716236, + 59.60224436302891, + 49.225979473285065, + 49.36848790191129 ], "moduleForcesY": [ - -32.56119938527781, - -36.61435191109544, - -49.66762908534543, - -49.55361350286737 + -32.56119938466126, + -36.61435191065098, + -49.66762908562598, + -49.553613502938724 ], - "timestamp": 0.46628773080060953 + "timestamp": 0.46628773079999203 }, { - "x": 1.2480376237140218, - "y": 3.968124696368845, - "heading": -0.7052817755202949, - "angularVelocity": 1.1041584547794074, - "velocityX": 1.57307920461716, - "velocityY": -1.2168652231591865, + "x": 1.2480376237134958, + "y": 3.968124696374288, + "heading": -0.7052817755099939, + "angularVelocity": 1.1041584548178656, + "velocityX": 1.5730792046158117, + "velocityY": -1.2168652231448125, "moduleForcesX": [ - 61.28176481668813, - 59.78038532288985, - 49.4938549994705, - 50.34939808767109 + 61.28176481691346, + 59.78038532312139, + 49.49385499902187, + 50.34939808752191 ], "moduleForcesY": [ - -33.67250970561668, - -36.30739888304486, - -49.39059309707039, - -48.546399889287585 + -33.67250970520312, + -36.30739888266253, + -49.390593097517815, + -48.546399889441645 ], - "timestamp": 0.5329002637721252 + "timestamp": 0.5329002637714194 }, { - "x": 1.3661123895110021, - "y": 3.877044967671322, - "heading": -0.6249031875065618, - "angularVelocity": 1.206658633565307, - "velocityX": 1.7725608159574173, - "velocityY": -1.3673061905102768, + "x": 1.3661123895101355, + "y": 3.8770449676778536, + "heading": -0.6249031874934339, + "angularVelocity": 1.206658633609344, + "velocityX": 1.7725608159546524, + "velocityY": -1.367306190495742, "moduleForcesX": [ - 60.55523039747262, - 59.86237923234887, - 50.01933061446567, - 51.43105327265715 + 60.555230397553366, + 59.86237923251681, + 50.019330613853604, + 51.431053272417905 ], "moduleForcesY": [ - -34.9402966200205, - -36.15231999767806, - -48.845768467491496, - -47.385486747890475 + -34.940296619876236, + -36.15231999739851, + -48.845768468115836, + -47.38548674814936 ], - "timestamp": 0.5995127967436409 + "timestamp": 0.5995127967428469 }, { - "x": 1.4975436678185934, - "y": 3.7759903450547028, - "heading": -0.53873230694548, - "angularVelocity": 1.293613629704331, - "velocityX": 1.9730713192334703, - "velocityY": -1.5170511930514805, + "x": 1.4975436678172518, + "y": 3.775990345062295, + "heading": -0.5387323069291853, + "angularVelocity": 1.2936136297535843, + "velocityX": 1.9730713192289506, + "velocityY": -1.5170511930375694, "moduleForcesX": [ - 59.71660813639446, - 59.781944702186415, - 50.89674120838264, - 52.617056525494455 + 59.716608136271915, + 59.78194470225127, + 50.896741207611434, + 52.617056525140555 ], "moduleForcesY": [ - -36.32951289772708, - -36.25934407553948, - -47.91447235046562, - -46.0464745895729 + -36.329512897923145, + -36.259344075430484, + -47.91447235128193, + -46.04647458997633 ], - "timestamp": 0.6661253297151566 + "timestamp": 0.6661253297142743 }, { - "x": 1.6424143533198352, - "y": 3.665025662326569, - "heading": -0.4481205140243463, - "angularVelocity": 1.3602814497368003, - "velocityX": 2.174826253239616, - "velocityY": -1.665822898156168, + "x": 1.6424143533178284, + "y": 3.6650256623351347, + "heading": -0.4481205140046001, + "angularVelocity": 1.3602814497904157, + "velocityX": 2.1748262532325127, + "velocityY": -1.665822898143766, "moduleForcesX": [ - 58.79631756896231, - 59.433248892591386, - 52.247409534940125, - 53.9194587893842 + 58.79631756856442, + 59.43324889247598, + 52.247409534027724, + 53.91945878887585 ], "moduleForcesY": [ - -37.77070605252747, - -36.79353882986422, - -46.41555660113537, - -44.48747884319979 + -37.77070605314016, + -36.79353883004777, + -46.41555660215867, + -44.487478843814586 ], - "timestamp": 0.7327378626866723 + "timestamp": 0.7327378626857017 }, { - "x": 1.800826604673265, - "y": 3.5442490372900517, - "heading": -0.3549074004558732, - "angularVelocity": 1.3993329694928751, - "velocityX": 2.3781148124358094, - "velocityY": -1.8131216401598682, + "x": 1.8008266046703592, + "y": 3.544249037299425, + "heading": -0.3549074004325164, + "angularVelocity": 1.3993329695489327, + "velocityX": 2.378114812425462, + "velocityY": -1.8131216401501349, "moduleForcesX": [ - 57.85964296595883, - 58.62699304555619, - 54.24098287937368, - 55.37454877680063 + 57.85964296520334, + 58.62699304509392, + 54.240982878372066, + 55.374548776074526 ], "moduleForcesY": [ - -39.15408809177324, - -38.01758274488999, - -44.03657792902701, - -42.620768436938704 + -39.15408809288117, + -38.01758274559908, + -44.03657793025534, + -42.62076843787999 ], - "timestamp": 0.799350395658188 + "timestamp": 0.7993503956571292 }, { - "x": 1.9729064746231015, - "y": 3.4138237900863677, - "heading": -0.261785676023239, - "angularVelocity": 1.3979610184235784, - "velocityX": 2.58329569937567, - "velocityY": -1.9579685891760825, + "x": 1.9729064746189697, + "y": 3.413823790096245, + "heading": -0.2617856759964363, + "angularVelocity": 1.3979610184771598, + "velocityX": 2.583295699360691, + "velocityY": -1.9579685891711138, "moduleForcesX": [ - 57.01503970270268, - 56.97264868755776, - 57.13055506988329, - 57.088614242525495 + 57.01503970150166, + 56.972648686336136, + 57.13055506893403, + 57.08861424148898 ], "moduleForcesY": [ - -40.32815265808299, - -40.3871603414814, - -40.16401782504414, - -40.22274635092099 + -40.32815265976974, + -40.38716034320021, + -40.16401782638521, + -40.22274635238806 ], - "timestamp": 0.8659629286297037 + "timestamp": 0.8659629286285566 }, { - "x": 2.1587959216901558, - "y": 3.2740626515960307, - "heading": -0.1731498612166921, - "angularVelocity": 1.3306176908848175, - "velocityX": 2.7906076945992013, - "velocityY": -2.0981207628015075, + "x": 2.158795921684324, + "y": 3.274062651605914, + "heading": -0.17314986118757214, + "angularVelocity": 1.3306176909213672, + "velocityX": 2.790607694577369, + "velocityY": -2.0981207628041956, "moduleForcesX": [ - 56.41042369707265, - 53.501719202644416, - 61.26419264721754, - 59.40078934825349 + 56.41042369532152, + 53.50171919941402, + 61.2641926466866, + 59.4007893468711 ], "moduleForcesY": [ - -41.1079010332853, - -44.78370054636255, - -33.42235417217702, - -36.56648525897984 + -41.107901035672306, + -44.7837005502188, + -33.422354173129975, + -36.56648526121465 ], - "timestamp": 0.9325754616012194 + "timestamp": 0.9325754615999841 }, { - "x": 2.3585571947909116, - "y": 3.1257497783866914, - "heading": -0.09743941845228067, - "angularVelocity": 1.1365795502294775, - "velocityX": 2.9988541823830035, - "velocityY": -2.2265010290594955, + "x": 2.3585571947825676, + "y": 3.125749778395996, + "heading": -0.09743941842552141, + "angularVelocity": 1.1365795501955436, + "velocityX": 2.9988541823492687, + "velocityY": -2.226501029071138, "moduleForcesX": [ - 56.18321511797377, - 45.13645782831661, - 66.6575745567656, - 63.63924131102192 + 56.183215115418484, + 45.13645781804362, + 66.6575745572279, + 63.63924131085493 ], "moduleForcesY": [ - -41.32675572742186, - -53.045678977052305, - -20.39623436243562, - -28.01878833525786 + -41.32675573087057, + -53.04567898580249, + -20.39623436084957, + -28.018788335571973 ], - "timestamp": 0.9991879945727351 + "timestamp": 0.9991879945714115 }, { - "x": 2.571323749679796, - "y": 2.9722890611298696, - "heading": -0.05631702040111217, - "angularVelocity": 0.6173372519665775, - "velocityX": 3.1940919433264323, - "velocityY": -2.303781441886374, + "x": 2.5713237496680983, + "y": 2.9722890611360366, + "heading": -0.056317020373306215, + "angularVelocity": 0.6173372519831085, + "velocityX": 3.1940919432803097, + "velocityY": -2.30378144193652, "moduleForcesX": [ - 56.49315652197762, - 24.727674210970267, - 69.53856728119115, - 66.38848839184168 + 56.49315651832047, + 24.72767419303312, + 69.5385672809967, + 66.38848840011917 ], "moduleForcesY": [ - -40.73364522523241, - -64.83876276810156, - 2.4259673258224868, - 17.19340461150298 + -40.73364523028673, + -64.83876277492942, + 2.425967330179503, + 17.19340457572179 ], - "timestamp": 1.0658005275442508 + "timestamp": 1.0658005275428388 }, { - "x": 2.7978969801578932, - "y": 2.816123256954651, - "heading": -0.04923559160029977, - "angularVelocity": 0.1063077544857887, - "velocityX": 3.4013603802603107, - "velocityY": -2.3443907206170564, + "x": 2.797896980139407, + "y": 2.816123256947658, + "heading": -0.04923559157858403, + "angularVelocity": 0.10630775439450244, + "velocityX": 3.4013603801629086, + "velocityY": -2.3443907208177204, "moduleForcesX": [ - 61.70982060481134, - 40.01285570538606, - 68.37167087206099, - 60.43433119111175 + 61.70982059244306, + 40.01285562448143, + 68.37167087457641, + 60.434331225109716 ], "moduleForcesY": [ - -31.4520746032983, - -55.70693219082933, - 10.727828044445207, - 31.264613623300775 + -31.45207462723247, + -55.706932248574496, + 10.727828026787554, + 31.264613554872565 ], - "timestamp": 1.1324130605157665 + "timestamp": 1.1324130605142662 }, { - "x": 3.0347601843563274, - "y": 2.6710905971518515, - "heading": -0.04900748324484257, - "angularVelocity": 0.003424405968089362, - "velocityX": 3.55583542063653, - "velocityY": -2.1772578422264344, + "x": 3.034760184329724, + "y": 2.671090597132263, + "heading": -0.0490074832231963, + "angularVelocity": 0.003424405967050934, + "velocityX": 3.5558354205193843, + "velocityY": -2.177257842418406, "moduleForcesX": [ - 50.41350702863919, - 41.75482316229609, - 44.362342200824024, - 35.27998738074212 + 50.413506856910104, + 41.754823961326274, + 44.362341929459326, + 35.27998700294513 ], "moduleForcesY": [ - 38.99730957905846, - 46.617569740155716, - 46.965805915927774, - 53.3083103983914 + 38.99730976426598, + 46.61756909982145, + 46.965806150666, + 53.30831062845783 ], - "timestamp": 1.1990255934872822 + "timestamp": 1.1990255934856935 }, { - "x": 3.2779712543514585, - "y": 2.5369613930825095, - "heading": -0.04879559855232118, - "angularVelocity": 0.0031808532579297136, - "velocityX": 3.6511307879424364, - "velocityY": -2.0135730933201033, + "x": 3.277971254366929, + "y": 2.5369613931399093, + "heading": -0.048795598530848285, + "angularVelocity": 0.003180853255331212, + "velocityX": 3.651130788578886, + "velocityY": -2.0135730921670043, "moduleForcesX": [ - 26.514032908634963, - 26.48917929707793, - 26.505657516129617, - 26.480809194623138 + 26.51403343046492, + 26.489179612407455, + 26.505657599753675, + 26.480809112111523 ], "moduleForcesY": [ - 45.5016606384836, - 45.51171619004381, - 45.5152401681101, - 45.525290455269825 + 45.50166091112535, + 45.51171684473839, + 45.515240274553484, + 45.52529091768426 ], - "timestamp": 1.265638126458798 + "timestamp": 1.2656381264571208 }, { - "x": 3.5240170136884648, - "y": 2.4081056080213394, - "heading": -0.04858282682880418, - "angularVelocity": 0.003194169535753657, - "velocityX": 3.6936856828761946, - "velocityY": -1.93440752532666, + "x": 3.5240170136688564, + "y": 2.408105608012548, + "heading": -0.04858282680739443, + "angularVelocity": 0.0031941695348100155, + "velocityX": 3.6936856823544875, + "velocityY": -1.9344075263229015, "moduleForcesX": [ - 11.832044679638638, - 11.833159737293062, - 11.83210215812816, - 11.833217225479276 + 11.832044385798296, + 11.833159401629988, + 11.832101854528789, + 11.833216870497965 ], "moduleForcesY": [ - 22.01300562318127, - 22.0128485655156, - 22.01202572778171, - 22.011868671306686 + 22.01300500476856, + 22.012848038290926, + 22.012025057287687, + 22.01186809697094 ], - "timestamp": 1.3322506594303136 + "timestamp": 1.3322506594285481 }, { - "x": 3.7708350348315482, - "y": 2.2807352279621504, - "heading": -0.0483699449005901, - "angularVelocity": 0.0031958239496029865, - "velocityX": 3.7052790238231275, - "velocityY": -1.9121083432400703, + "x": 3.770835034817424, + "y": 2.2807352279647817, + "heading": -0.048369944879233955, + "angularVelocity": 0.0031958239488024064, + "velocityX": 3.7052790239103643, + "velocityY": -1.912108343071116, "moduleForcesX": [ - 3.2235324585038754, - 3.2236618531632817, - 3.2235271302506407, - 3.2236565249546465 + 3.223532949769158, + 3.2236620923869292, + 3.223527231378966, + 3.223656370634464 ], "moduleForcesY": [ - 6.200483556552904, - 6.200476317722027, - 6.200352895038578, - 6.200345656224632 + 6.200483740926388, + 6.200477036360204, + 6.200352815192096, + 6.2003461290433926 ], - "timestamp": 1.3988631924018293 + "timestamp": 1.3988631923999755 }, { - "x": 4.017945860044819, - "y": 2.153933675847828, - "heading": -0.04815735530566025, - "angularVelocity": 0.0031914353868025967, - "velocityX": 3.709674654151622, - "velocityY": -1.90356899006594, + "x": 4.017945860027259, + "y": 2.153933675844563, + "heading": -0.048157355284360694, + "angularVelocity": 0.0031914353859573896, + "velocityX": 3.7096746541049623, + "velocityY": -1.9035689901569823, "moduleForcesX": [ - 1.2223925594156837, - 1.2220511392203353, - 1.2224089657777908, - 1.222067545703039 + 1.2223938746517689, + 1.222051260087835, + 1.222408785272593, + 1.222066141189196 ], "moduleForcesY": [ - 2.374233190641205, - 2.3742500204088812, - 2.3745815684863776, - 2.374598398301541 + 2.3742328041770366, + 2.3742514629482714, + 2.374579889586726, + 2.3745987319670263 ], - "timestamp": 1.465475725373345 + "timestamp": 1.4654757253714028 }, { - "x": 4.265335286953013, - "y": 2.027676292006606, - "heading": -0.04794522174606807, - "angularVelocity": 0.0031845892976762468, - "velocityX": 3.7138570757244884, - "velocityY": -1.895399832569214, + "x": 4.265335287008856, + "y": 2.027676292147968, + "heading": -0.047945221724793284, + "angularVelocity": 0.003184589297308503, + "velocityX": 3.713857076831331, + "velocityY": -1.8953998304005573, "moduleForcesX": [ - 1.163199663544347, - 1.1626670888284039, - 1.1632251975919956, - 1.1626926239772695 + 1.1632026163804865, + 1.162667533168695, + 1.1632254607113321, + 1.1626902466338753 ], "moduleForcesY": [ - 2.271195979291761, - 2.271222065843487, - 2.271739492202394, - 2.2717655728058856 + 2.271196262649088, + 2.271225591627576, + 2.271736925412616, + 2.271766843769598 ], - "timestamp": 1.5320882583448607 + "timestamp": 1.5320882583428301 }, { - "x": 4.513387263491877, - "y": 1.9027254137370513, - "heading": -0.04773334654928872, - "angularVelocity": 0.003180710706046184, - "velocityX": 3.7238033966511064, - "velocityY": -1.8757863227177534, + "x": 4.513387263726315, + "y": 1.9027254142338006, + "heading": -0.04773334652797979, + "angularVelocity": 0.0031807107065629863, + "velocityX": 3.723803399337141, + "velocityY": -1.8757863173851013, "moduleForcesX": [ - 2.765776223844279, - 2.765473269227589, - 2.7657891163065877, - 2.7654861619049216 + 2.7657780823712406, + 2.765473685160751, + 2.7657896002600624, + 2.7654851599392303 ], "moduleForcesY": [ - 5.453486682132146, - 5.453502875542226, - 5.453793409345764, - 5.453809602835306 + 5.453487457755567, + 5.453505306304338, + 5.453792661463415, + 5.453810663508131 ], - "timestamp": 1.5987007913163764 + "timestamp": 1.5987007913142575 }, { - "x": 4.7636905451889415, - "y": 1.7823482264471835, - "heading": -0.04752024111954168, - "angularVelocity": 0.003199179197076531, - "velocityX": 3.7576004173883883, - "velocityY": -1.8071252048220714, + "x": 4.76369054518264, + "y": 1.7823482264444208, + "heading": -0.047520241098396636, + "angularVelocity": 0.0031991791946204317, + "velocityX": 3.7576004137793353, + "velocityY": -1.8071252123232402, "moduleForcesX": [ - 9.396675512144679, - 9.39819901784068, - 9.396709254967268, - 9.398232777338956 + 9.396673705872347, + 9.39819730006515, + 9.396707473785185, + 9.398231081199015 ], "moduleForcesY": [ - 19.092390606150673, - 19.092220862164694, - 19.091009053610517, - 19.090839312476135 + 19.092387038061002, + 19.09221720512043, + 19.09100557309194, + 19.090835744537614 ], - "timestamp": 1.6653133242878921 + "timestamp": 1.6653133242856848 }, { - "x": 5.0188400094891765, - "y": 1.6726490442267128, - "heading": -0.04723633820300304, - "angularVelocity": 0.004262004518879671, - "velocityX": 3.8303522313036735, - "velocityY": -1.646824964115687, + "x": 5.018840009680974, + "y": 1.6726490446859121, + "heading": -0.047236338181666296, + "angularVelocity": 0.004262004521763258, + "velocityX": 3.8303522342826466, + "velocityY": -1.6468249571828, "moduleForcesX": [ - 20.160339598017387, - 20.271778827791216, - 20.186266828439518, - 20.297839903273452 + 20.160341309415056, + 20.271780653378354, + 20.186268676149652, + 20.297841846829897 ], "moduleForcesY": [ - 44.61714719861703, - 44.58388431235552, - 44.56094399930638, - 44.52760480062931 + 44.61715118244674, + 44.583888259336774, + 44.56094806815956, + 44.52760885668282 ], - "timestamp": 1.7319258572594078 + "timestamp": 1.7319258572571121 }, { - "x": 5.276040962243286, - "y": 1.5678524061254324, - "heading": -0.046947349952411, - "angularVelocity": 0.0043383465197998905, - "velocityX": 3.8611495657144865, - "velocityY": -1.5732270404143434, + "x": 5.276040962308425, + "y": 1.567852406274765, + "heading": -0.04694734993114214, + "angularVelocity": 0.004338346518786609, + "velocityX": 3.861149563818171, + "velocityY": -1.5732270450682089, "moduleForcesX": [ - 8.560118396401565, - 8.566501267033127, - 8.560247116014803, - 8.56663029963911 + 8.560117120058553, + 8.56649998262021, + 8.560245714730028, + 8.566628839534616 ], "moduleForcesY": [ - 20.46747803904423, - 20.466793414894244, - 20.461850439324866, - 20.461165848040086 + 20.467474806427994, + 20.466790232197752, + 20.461847166644493, + 20.46116264960528 ], - "timestamp": 1.7985383902309235 + "timestamp": 1.7985383902285395 }, { - "x": 5.534216899574317, - "y": 1.4654824117182799, - "heading": -0.04665631309500156, - "angularVelocity": 0.004369100594536302, - "velocityX": 3.8757862194105366, - "velocityY": -1.536797804264961, + "x": 5.53421689953023, + "y": 1.4654824115931464, + "heading": -0.04665631307381442, + "angularVelocity": 0.004369100593315545, + "velocityX": 3.875786217775965, + "velocityY": -1.5367978083873335, "moduleForcesX": [ - 4.068624748426005, - 4.071060153628479, - 4.068549709495257, - 4.070985139742783 + 4.068624788601681, + 4.071060271920588, + 4.068549740960962, + 4.07098524097379 ], "moduleForcesY": [ - 10.13063153345665, - 10.130481600946313, - 10.128231656616295, - 10.128081730271346 + 10.130631677004772, + 10.130481677327632, + 10.1282318686583, + 10.1280818895711 ], - "timestamp": 1.8651509232024392 + "timestamp": 1.8651509231999668 }, { - "x": 5.794372355867246, - "y": 1.3682547977858872, - "heading": -0.04636219278537718, - "angularVelocity": 0.004415389964980432, - "velocityX": 3.905503134135057, - "velocityY": -1.459599411629766, + "x": 5.794372355842512, + "y": 1.3682547977136283, + "heading": -0.04636219276413434, + "angularVelocity": 0.00441538996582264, + "velocityX": 3.9055031344307545, + "velocityY": -1.4595994108379353, "moduleForcesX": [ - 8.260963536224425, - 8.264869277780319, - 8.261045504013175, - 8.264951367768367 + 8.260964021908269, + 8.26486985270304, + 8.261046002058745, + 8.26495195604506 ], "moduleForcesY": [ - 21.467349064567532, - 21.466934299940117, - 21.46397136076802, - 21.463556605012805 + 21.467350425085247, + 21.466935581870978, + 21.463972811998225, + 21.46355797712207 ], - "timestamp": 1.931763456173955 + "timestamp": 1.9317634561713941 }, { - "x": 6.058520814972377, - "y": 1.282472529523383, - "heading": -0.04606005529713513, - "angularVelocity": 0.004535745373490557, - "velocityX": 3.965446850941467, - "velocityY": -1.2877797080496214, + "x": 6.058520814952769, + "y": 1.2824725294681238, + "heading": -0.046060055275852295, + "angularVelocity": 0.004535745374097144, + "velocityX": 3.965446851023664, + "velocityY": -1.2877797077961244, "moduleForcesX": [ - 16.659711714491884, - 16.673161829473102, - 16.66222092531276, - 16.675673239510985 + 16.659711667705363, + 16.673161791773882, + 16.66222084343888, + 16.67567316842716 ], "moduleForcesY": [ - 47.77989163980646, - 47.77656557923747, - 47.77432962004692, - 47.771002564051436 + 47.779891485446186, + 47.776565422843, + 47.774329477432524, + 47.77100241879716 ], - "timestamp": 1.9983759891454707 + "timestamp": 1.9983759891428214 }, { - "x": 6.326935018479887, - "y": 1.2111884138637838, - "heading": -0.04572387766988923, - "angularVelocity": 0.0050467624071529864, - "velocityX": 4.0294850163149665, - "velocityY": -1.0701306868195675, + "x": 6.326935018464312, + "y": 1.2111884138255193, + "heading": -0.045723877648205694, + "angularVelocity": 0.005046762413175208, + "velocityX": 4.029485016380848, + "velocityY": -1.070130686565861, "moduleForcesX": [ - 17.76434909368371, - 17.833039573651952, - 17.77928266449683, - 17.84803536932326 + 17.764349079949987, + 17.833039578816862, + 17.779282668917745, + 17.84803535533965 ], "moduleForcesY": [ - 60.533843276425394, - 60.51552676552759, - 60.52159010023538, - 60.50324061772363 + 60.53384327945482, + 60.51552676309896, + 60.521590097018716, + 60.50324062036407 ], - "timestamp": 2.064988522116986 + "timestamp": 2.0649885221142488 }, { - "x": 6.598149001232439, - "y": 1.1556493567666084, - "heading": -0.04255962682130831, - "angularVelocity": 0.04750233488244815, - "velocityX": 4.071515834242889, - "velocityY": -0.8337628764383538, + "x": 6.598149001225702, + "y": 1.1556493567444743, + "heading": -0.04255962681817847, + "angularVelocity": 0.047502334603978215, + "velocityX": 4.0715158343809605, + "velocityY": -0.8337628761973023, "moduleForcesX": [ - 8.287572421549362, - 14.286479207247982, - 8.808436728764809, - 15.365145055742365 + 8.28757237649673, + 14.286479227047964, + 8.808436870157584, + 15.36514501967037 ], "moduleForcesY": [ - 66.4507233464468, - 65.45898479609045, - 66.07372493186959, - 64.91023150582662 + 66.45072335474919, + 65.45898479439751, + 66.07372490759722, + 64.91023150887932 ], - "timestamp": 2.1316010550885016 + "timestamp": 2.1316010550856763 }, { - "x": 6.859559783326081, - "y": 1.113135958184906, - "heading": -0.030602705565701032, - "angularVelocity": 0.17949957346195378, - "velocityX": 3.9243483235418233, - "velocityY": -0.6382192161928673, + "x": 6.859559783323042, + "y": 1.1131359581740934, + "heading": -0.0306027055657009, + "angularVelocity": 0.17949957341520645, + "velocityX": 3.9243483236025436, + "velocityY": -0.6382192160237583, "moduleForcesX": [ - -42.45988336627168, - -29.346972124084434, - -52.177609628795565, - -39.698593640879075 + -42.459883383315024, + -29.34697212286383, + -52.17760966362154, + -39.69859367485398 ], "moduleForcesY": [ - 54.43758266703957, - 62.32119709611513, - 44.88720682067673, - 55.84212799694645 + 54.43758265314552, + 62.32119709610292, + 44.88720677947976, + 55.842127971468855 ], - "timestamp": 2.198213588060017 + "timestamp": 2.198213588057104 }, { "x": 7.107321739196777, "y": 1.0795719623565674, - "heading": -0.030602705565701018, - "angularVelocity": 3.347783365519795e-18, - "velocityX": 3.7194495512825236, - "velocityY": -0.5038690818541787, + "heading": -0.030602705565700897, + "angularVelocity": 1.6075047383869973e-18, + "velocityX": 3.7194495513330783, + "velocityY": -0.5038690816925215, "moduleForcesX": [ - -61.323422950505716, - -65.95546140653828, - -44.62846950333632, - -55.985729468867824 + -61.32342295009295, + -65.955461406566, + -44.628469510881004, + -55.985729471196706 ], "moduleForcesY": [ - 32.44664959475204, - 22.20881577411158, - 53.33306981117861, - 41.43874553341409 + 32.44664959500376, + 22.208815773674722, + 53.33306980468184, + 41.43874553017478 ], - "timestamp": 2.2648261210315326 + "timestamp": 2.2648261210285314 }, { - "x": 7.342124626950215, - "y": 1.053916100516507, - "heading": -0.030602705565701004, - "angularVelocity": 7.672386391662901e-19, - "velocityX": 3.4979351696684997, - "velocityY": -0.3822037381956934, + "x": 7.342124626958182, + "y": 1.0539161005261706, + "heading": -0.03060270556570089, + "angularVelocity": 2.0024672081036984e-19, + "velocityX": 3.4979351697057512, + "velocityY": -0.38220373804281776, "moduleForcesX": [ - -61.122061696457, - -61.122061696457, - -61.122061696457, - -61.122061696457 + -61.122061698454715, + -61.122061698454715, + -61.122061698454715, + -61.122061698454715 ], "moduleForcesY": [ - 33.57089768760224, - 33.57089768760224, - 33.57089768760223, - 33.57089768760224 + 33.57089768389125, + 33.57089768389125, + 33.570897683891246, + 33.570897683891246 ], - "timestamp": 2.3319522615585138 + "timestamp": 2.3319522615570754 }, { - "x": 7.56101179722364, - "y": 1.0341858544549376, - "heading": -0.03060270556570099, - "angularVelocity": 7.672403149471102e-19, - "velocityX": 3.260833537501588, - "velocityY": -0.2939279080649534, + "x": 7.561011797238179, + "y": 1.0341858544731557, + "heading": -0.030602705565700883, + "angularVelocity": 2.0024561890342501e-19, + "velocityX": 3.2608335375235695, + "velocityY": -0.29392790793066653, "moduleForcesX": [ - -65.42302348088484, - -65.42302348088484, - -65.42302348088484, - -65.42302348088484 + -65.42302348307652, + -65.42302348307652, + -65.42302348307652, + -65.42302348307652 ], "moduleForcesY": [ - 24.35778975736353, - 24.357789757363527, - 24.357789757363534, - 24.357789757363534 + 24.3577897514352, + 24.3577897514352, + 24.3577897514352, + 24.357789751435195 ], - "timestamp": 2.399078402085495 + "timestamp": 2.3990784020856193 }, { - "x": 7.76353893371165, - "y": 1.0190536925576863, - "heading": -0.030602705565700973, - "angularVelocity": 7.672257598260093e-19, - "velocityX": 3.0171127804763933, - "velocityY": -0.22542874919449496, + "x": 7.7635389337314935, + "y": 1.0190536925830782, + "heading": -0.030602705565700876, + "angularVelocity": 2.0022790819956785e-19, + "velocityX": 3.0171127804851614, + "velocityY": -0.22542874908236665, "moduleForcesX": [ - -67.24942660173558, - -67.24942660173558, - -67.24942660173558, - -67.24942660173558 + -67.24942660363206, + -67.24942660363206, + -67.24942660363206, + -67.24942660363206 ], "moduleForcesY": [ - 18.900848713116805, - 18.900848713116805, - 18.9008487131168, - 18.900848713116808 + 18.90084870634776, + 18.900848706347762, + 18.90084870634775, + 18.90084870634776 ], - "timestamp": 2.466204542612476 + "timestamp": 2.466204542614163 }, { - "x": 7.949481024014891, - "y": 1.0076590456800256, - "heading": -0.03060270556570096, - "angularVelocity": 7.672013523245006e-19, - "velocityX": 2.7700399403791343, - "velocityY": -0.16974976943713838, + "x": 7.949481024038794, + "y": 1.0076590457110912, + "heading": -0.03060270556570087, + "angularVelocity": 2.0020051992032217e-19, + "velocityX": 2.7700399403750904, + "velocityY": -0.16974976934866481, "moduleForcesX": [ - -68.17436080640248, - -68.17436080640248, - -68.17436080640248, - -68.17436080640248 + -68.17436080799112, + -68.17436080799114, + -68.17436080799114, + -68.17436080799112 ], "moduleForcesY": [ - 15.36339993436348, - 15.363399934363485, - 15.363399934363473, - 15.363399934363477 + 15.363399927305814, + 15.363399927305815, + 15.363399927305815, + 15.363399927305814 ], - "timestamp": 2.5333306831394573 + "timestamp": 2.533330683142707 }, { - "x": 8.118709429041983, - "y": 0.9994031964377751, - "heading": -0.030602705565700945, - "angularVelocity": 7.671706888831663e-19, - "velocityX": 2.5210507217984657, - "velocityY": -0.12299007774670825, + "x": 8.118709429068772, + "y": 0.9994031964729742, + "heading": -0.030602705565700862, + "angularVelocity": 2.001673444928878e-19, + "velocityX": 2.521050721782785, + "velocityY": -0.12299007768226591, "moduleForcesX": [ - -68.70314364670283, - -68.70314364670283, - -68.70314364670283, - -68.70314364670283 + -68.7031436480408, + -68.7031436480408, + -68.7031436480408, + -68.7031436480408 ], "moduleForcesY": [ - 12.90231694942824, - 12.902316949428238, - 12.902316949428242, - 12.902316949428242 + 12.902316942304648, + 12.902316942304648, + 12.902316942304651, + 12.902316942304651 ], - "timestamp": 2.6004568236664385 + "timestamp": 2.600456823671251 }, { - "x": 8.271143977771962, - "y": 0.9938469245659062, - "heading": -0.030602705565700928, - "angularVelocity": 7.671359538796945e-19, - "velocityX": 2.270867169380966, - "velocityY": -0.08277359353968143, + "x": 8.271143977800422, + "y": 0.9938469246036682, + "heading": -0.030602705565700855, + "angularVelocity": 2.0013070310221862e-19, + "velocityX": 2.270867169353027, + "velocityY": -0.08277359349957805, "moduleForcesX": [ - -69.0326940172405, - -69.0326940172405, - -69.0326940172405, - -69.0326940172405 + -69.03269401838256, + -69.03269401838256, + -69.03269401838256, + -69.03269401838256 ], "moduleForcesY": [ - 11.096861571799984, - 11.096861571799986, - 11.096861571799984, - 11.096861571799984 + 11.096861564702607, + 11.096861564702607, + 11.096861564702607, + 11.096861564702605 ], - "timestamp": 2.6675829641934197 + "timestamp": 2.667582964199795 }, { - "x": 8.406731406019375, - "y": 0.9906547367603562, - "heading": -0.030602705565700914, - "angularVelocity": 7.670985863150012e-19, - "velocityX": 2.019890123027612, - "velocityY": -0.047555062461336506, + "x": 8.406731406048316, + "y": 0.9906547367990796, + "heading": -0.030602705565700848, + "angularVelocity": 2.0009206377793148e-19, + "velocityX": 2.0198901229876864, + "velocityY": -0.047555062445912684, "moduleForcesX": [ - -69.25164136036881, - -69.25164136036881, - -69.25164136036881, - -69.25164136036881 + -69.25164136135751, + -69.25164136135751, + -69.25164136135751, + -69.25164136135751 ], "moduleForcesY": [ - 9.717785426549383, - 9.717785426549383, - 9.717785426549389, - 9.717785426549389 + 9.71778541951536, + 9.717785419515362, + 9.717785419515362, + 9.717785419515362 ], - "timestamp": 2.734709104720401 + "timestamp": 2.734709104728339 }, { - "x": 8.5254345460703, - "y": 0.9895621634069518, - "heading": -0.0306027055657009, - "angularVelocity": 7.670595843658019e-19, - "velocityX": 1.7683593771223813, - "velocityY": -0.016276421448131698, + "x": 8.525434546098682, + "y": 0.9895621634450235, + "heading": -0.03060270556570084, + "angularVelocity": 2.0005241446773043e-19, + "velocityX": 1.7683593770728674, + "velocityY": -0.016276421457464545, "moduleForcesX": [ - -69.40442267327488, - -69.40442267327488, - -69.40442267327488, - -69.40442267327488 + -69.40442267414187, + -69.40442267414187, + -69.40442267414187, + -69.40442267414187 ], "moduleForcesY": [ - 8.630658704124505, - 8.630658704124507, - 8.630658704124507, - 8.630658704124508 + 8.63065869716772, + 8.63065869716772, + 8.630658697167716, + 8.630658697167716 ], - "timestamp": 2.801835245247382 + "timestamp": 2.801835245256883 }, { - "x": 8.627226437405287, - "y": 0.9903554058889321, - "heading": -0.030602705565700883, - "angularVelocity": 7.670197234878886e-19, - "velocityX": 1.516426991569238, - "velocityY": 0.011817191868226978, + "x": 8.627226437431862, + "y": 0.9903554059247525, + "heading": -0.030602705565700834, + "angularVelocity": 2.0001246831972175e-19, + "velocityX": 1.516426991507014, + "velocityY": 0.011817191834409059, "moduleForcesX": [ - -69.51524637407707, - -69.51524637407705, - -69.51524637407707, - -69.51524637407705 + -69.51524637484586, + -69.51524637484586, + -69.51524637484586, + -69.51524637484586 ], "moduleForcesY": [ - 7.751819786448097, - 7.751819786448097, - 7.751819786448097, - 7.751819786448097 + 7.751819779571535, + 7.751819779571536, + 7.751819779571532, + 7.751819779571534 ], - "timestamp": 2.868961385774363 + "timestamp": 2.868961385785427 }, { - "x": 8.712086900940639, - "y": 0.9928580419153559, - "heading": -0.03060270556570087, - "angularVelocity": 7.6697962024819e-19, - "velocityX": 1.2641939916274305, - "velocityY": 0.03728258479896153, + "x": 8.712086900964183, + "y": 0.992858041947287, + "heading": -0.030602705565700827, + "angularVelocity": 1.9997279446904402e-19, + "velocityX": 1.264193991552835, + "velocityY": 0.03728258474014907, "moduleForcesX": [ - -69.59819435728923, - -69.59819435728923, - -69.59819435728923, - -69.59819435728923 + -69.59819435797758, + -69.59819435797758, + -69.59819435797758, + -69.59819435797758 ], "moduleForcesY": [ - 7.026619700405902, - 7.026619700405903, - 7.0266197004059014, - 7.0266197004059014 + 7.026619693607424, + 7.026619693607423, + 7.026619693607425, + 7.026619693607425 ], - "timestamp": 2.9360875263013444 + "timestamp": 2.9360875263139707 }, { - "x": 8.780000437865695, - "y": 0.996921987964414, - "heading": -0.03060270556570085, - "angularVelocity": 7.669398274852111e-19, - "velocityX": 1.011730100850266, - "velocityY": 0.06054192922688254, + "x": 8.780000437885118, + "y": 0.9969219879908205, + "heading": -0.03060270556570082, + "angularVelocity": 1.9993387143813067e-19, + "velocityX": 1.0117301007653263, + "velocityY": 0.06054192914316773, "moduleForcesX": [ - -69.66190364637255, - -69.66190364637255, - -69.66190364637255, - -69.66190364637255 + -69.66190364699403, + -69.66190364699403, + -69.66190364699403, + -69.66190364699403 ], "moduleForcesY": [ - 6.417908736725351, - 6.417908736725351, - 6.417908736725356, - 6.417908736725355 + 6.417908730000572, + 6.417908730000572, + 6.41790873000057, + 6.41790873000057 ], - "timestamp": 3.0032136668283256 + "timestamp": 3.0032136668425147 }, { - "x": 8.830954883407468, - "y": 1.0024211476734168, - "heading": -0.030602705565700838, - "angularVelocity": 7.669008842488089e-19, - "velocityX": 0.75908498748391, - "velocityY": 0.08192277502968102, + "x": 8.830954883421578, + "y": 1.0024211476926257, + "heading": -0.030602705565700813, + "angularVelocity": 1.99896148062903e-19, + "velocityX": 0.7590849873871007, + "velocityY": 0.08192277492054269, "moduleForcesX": [ - -69.71190806731377, - -69.71190806731377, - -69.71190806731377, - -69.71190806731377 + -69.71190806787894, + -69.71190806787894, + -69.71190806787892, + -69.71190806787892 ], "moduleForcesY": [ - 5.89957801675687, - 5.899578016756869, - 5.899578016756865, - 5.899578016756865 + 5.89957801010069, + 5.899578010100691, + 5.8995780101006865, + 5.8995780101006865 ], - "timestamp": 3.0703398073553068 + "timestamp": 3.0703398073710586 }, { - "x": 8.86494051215884, - "y": 1.0092468204121565, - "heading": -0.030602705565700824, - "angularVelocity": 7.668631929139065e-19, - "velocityX": 0.5062949915570888, - "velocityY": 0.10168427210552212, + "x": 8.86494051216642, + "y": 1.009246820422566, + "heading": -0.030602705565700807, + "angularVelocity": 1.9985996963971072e-19, + "velocityX": 0.5062949914480063, + "velocityY": 0.10168427197206922, "moduleForcesX": [ - -69.7518852496981, - -69.7518852496981, - -69.7518852496981, - -69.7518852496981 + -69.75188525021527, + -69.75188525021527, + -69.75188525021527, + -69.75188525021527 ], "moduleForcesY": [ - 5.452754058783733, - 5.452754058783732, - 5.452754058783737, - 5.452754058783736 + 5.452754052190986, + 5.452754052190985, + 5.452754052190986, + 5.452754052190985 ], - "timestamp": 3.137465947882288 + "timestamp": 3.1374659478996025 }, { "x": 8.881949424743652, "y": 1.017304301261902, - "heading": -0.030602705565700813, - "angularVelocity": -6.192148052914439e-17, - "velocityX": 0.25338731604818493, - "velocityY": 0.12003491913118446, + "heading": -0.030602705565700807, + "angularVelocity": -6.178266080762978e-17, + "velocityX": 0.25338731592940406, + "velocityY": 0.12003491897331502, "moduleForcesX": [ - -69.78435636275017, - -69.78435636275017, - -69.78435636275017, - -69.78435636275017 + -69.78435636322597, + -69.78435636322598, + -69.78435636322598, + -69.78435636322598 ], "moduleForcesY": [ - 5.063460762339646, - 5.063460762339647, - 5.063460762339649, - 5.06346076233965 + 5.063460755805268, + 5.063460755805267, + 5.06346075580527, + 5.06346075580527 ], - "timestamp": 3.204592088409269 + "timestamp": 3.2045920884281465 }, { - "x": 8.880530486218973, - "y": 1.027348591072611, - "heading": -0.034530809902176336, - "angularVelocity": -0.054161349122254336, - "velocityX": -0.019564557922623505, - "velocityY": 0.13849232110933965, + "x": 8.880530486210139, + "y": 1.0273485910590279, + "heading": -0.03453080986256461, + "angularVelocity": -0.054161348577286314, + "velocityX": -0.01956455804488396, + "velocityY": 0.1384923209251697, "moduleForcesX": [ - -69.96707712828831, - -69.96918542605992, - -69.4046006596549, - -69.48963562370783 + -69.96707712791041, + -69.96918542564208, + -69.40460066617938, + -69.48963562834733 ], "moduleForcesY": [ - 0.7821062724142991, - 0.9492571184496107, - 8.889772333230777, - 8.233787400699313 + 0.7821063079634264, + 0.9492571479967624, + 8.889772282325291, + 8.233787361314777 ], - "timestamp": 3.277118057576761 + "timestamp": 3.277118057594007 }, { - "x": 8.859315047264229, - "y": 1.03873366112627, - "heading": -0.04230269871701783, - "angularVelocity": -0.10716008216259976, - "velocityX": -0.29252196417731824, - "velocityY": 0.15697921977944768, + "x": 8.859315047246675, + "y": 1.0387336610970788, + "heading": -0.04230269859815422, + "angularVelocity": -0.10716008107224526, + "velocityX": -0.292521964304125, + "velocityY": 0.1569792195677669, "moduleForcesX": [ - -69.96360527609723, - -69.96507878923691, - -69.41623345810719, - -69.49123274928873 + -69.96360527567995, + -69.96507878877915, + -69.41623346455654, + -69.49123275406077 ], "moduleForcesY": [ - 0.849054357453386, - 1.0605014457335615, - 8.776783714333206, - 8.198715576096298 + 0.8490543935707846, + 1.0605014747496089, + 8.776783663369878, + 8.198715535403977 ], - "timestamp": 3.3496440267442527 + "timestamp": 3.3496440267598677 }, { - "x": 8.81830267354523, - "y": 1.0514616761704456, - "heading": -0.053822469418188024, - "angularVelocity": -0.15883649447922438, - "velocityX": -0.5654853591032976, - "velocityY": 0.17549596634529624, + "x": 8.818302673519035, + "y": 1.0514616761236624, + "heading": -0.05382246917997862, + "angularVelocity": -0.15883649283719747, + "velocityX": -0.5654853592351373, + "velocityY": 0.17549596610668067, "moduleForcesX": [ - -69.95969350130241, - -69.9600439550638, - -69.43034005880952, - -69.49219040870635 + -69.95969350084123, + -69.96004395456353, + -69.43034006513656, + -69.49219041365508 ], "moduleForcesY": [ - 0.916057569851737, - 1.1945950847132014, - 8.639230967068034, - 8.165662210562154 + 0.9160576067978011, + 1.1945951128891326, + 8.639230916283404, + 8.165662168184191 ], - "timestamp": 3.4221699959117444 + "timestamp": 3.4221699959257283 }, { - "x": 8.75749289682575, - "y": 1.0655348258499984, - "heading": -0.06897986858552595, - "angularVelocity": -0.2089927144919403, - "velocityX": -0.838455210147377, - "velocityY": 0.1940429040948802, + "x": 8.757492896791033, + "y": 1.0655348257834631, + "heading": -0.06897986818711133, + "angularVelocity": -0.20899271228765146, + "velocityX": -0.8384552102837445, + "velocityY": 0.1940429038268972, "moduleForcesX": [ - -69.9551810729799, - -69.95383968280234, - -69.44688305083558, - -69.49295928173186 + -69.95518107246687, + -69.95383968225862, + -69.44688305699783, + -69.49295928690098 ], "moduleForcesY": [ - 0.9882804241447996, - 1.352618156589803, - 8.475469948022155, - 8.130018720639756 + 0.9882804621739782, + 1.3526181836391635, + 8.475469897619213, + 8.130018676173123 ], - "timestamp": 3.494695965079236 + "timestamp": 3.494695965091589 }, { - "x": 8.67688521696236, - "y": 1.080955321413936, - "heading": -0.08764661363140669, - "angularVelocity": -0.2573801530723142, - "velocityX": -1.1114319572514513, - "velocityY": 0.2126203309097312, + "x": 8.676885216919311, + "y": 1.0809553213253251, + "heading": -0.08764661303153386, + "angularVelocity": -0.2573801503002974, + "velocityX": -1.1114319573913516, + "velocityY": 0.212620330610125, "moduleForcesX": [ - -69.94983324196707, - -69.94615057660823, - -69.46581320027535, - -69.49411065050936 + -69.94983324139118, + -69.94615057602354, + -69.46581320622545, + -69.49411065593223 ], "moduleForcesY": [ - 1.0723396108261634, - 1.5360717401888286, - 8.283336243627176, - 8.085785369496797 + 1.072339650088805, + 1.5360717657814353, + 8.283336193853913, + 8.085785322585087 ], - "timestamp": 3.567221934246728 + "timestamp": 3.5672219342574496 }, { - "x": 8.576479109537585, - "y": 1.0977253883199334, - "heading": -0.1096713977003478, - "angularVelocity": -0.3036813478255371, - "velocityX": -1.3844159351099252, - "velocityY": 0.23122844270129664, + "x": 8.57647910948646, + "y": 1.0977253882067866, + "heading": -0.10967139685743804, + "angularVelocity": -0.30368134448125444, + "velocityX": -1.3844159352524572, + "velocityY": 0.2312284423681809, "moduleForcesX": [ - -69.94330252963142, - -69.9365583049015, - -69.4870568794036, - -69.49637643933679 + -69.94330252897542, + -69.93655830427998, + -69.48705688510655, + -69.49637644504644 ], "moduleForcesY": [ - 1.176823061341541, - 1.7470252667944042, - 8.060012105446882, - 8.025018376878974 + 1.1768231019990443, + 1.7470252906666641, + 8.060012056454902, + 8.025018327105174 ], - "timestamp": 3.6397479034142197 + "timestamp": 3.6397479034233102 }, { - "x": 8.456274044587177, - "y": 1.1158472524014584, - "heading": -0.13487291738284943, - "angularVelocity": -0.347482701327652, - "velocityX": -1.6574072202027315, - "velocityY": 0.24986724465107038, + "x": 8.456274044528195, + "y": 1.1158472522610625, + "heading": -0.13487291625482856, + "angularVelocity": -0.347482697404215, + "velocityX": -1.6574072203483379, + "velocityY": 0.2498672442809662, "moduleForcesX": [ - -69.93505805724766, - -69.92449408332656, - -69.51050132115871, - -69.50070530398553 + -69.93505805649114, + -69.92449408268004, + -69.510501326562, + -69.50070530998221 ], "moduleForcesY": [ - 1.3131272162432313, - 1.98838442321693, - 7.801780109725148, - 7.936938199533164 + 1.3131272581552487, + 1.9883844449565293, + 7.801780061823989, + 7.936938146668492 ], - "timestamp": 3.7122738725817115 + "timestamp": 3.712273872589171 }, { - "x": 8.316269527386241, - "y": 1.1353231154725127, - "heading": -0.16302980404777723, - "angularVelocity": -0.38823178770838385, - "velocityX": -1.930405326643809, - "velocityY": 0.26853640557469155, + "x": 8.316269527319614, + "y": 1.1353231153018994, + "heading": -0.16302980259206615, + "angularVelocity": -0.3882317831987629, + "velocityX": -1.9304053267926182, + "velocityY": 0.26853640516407645, "moduleForcesX": [ - -69.92425253763372, - -69.90915580434691, - -69.53597671993317, - -69.50834196529713 + -69.92425253674794, + -69.90915580369078, + -69.53597672500005, + -69.50834197156578 ], "moduleForcesY": [ - 1.4968639656801608, - 2.264388307053663, - 7.503564176470051, - 7.80642632120717 + 1.4968640086183072, + 2.2643883263150886, + 7.50356412984306, + 7.806426265009984 ], - "timestamp": 3.7847998417492033 + "timestamp": 3.7847998417550315 }, { - "x": 8.156465184614108, - "y": 1.1561551126608645, - "heading": -0.1938654641731384, - "angularVelocity": -0.42516715708305197, - "velocityX": -2.2034085804916796, - "velocityY": 0.28723500599140817, + "x": 8.156465184540169, + "y": 1.1561551124568015, + "heading": -0.19386546234654622, + "angularVelocity": -0.42516715197872557, + "velocityX": -2.203408580642062, + "velocityY": 0.28723500553664255, "moduleForcesX": [ - -69.9094604865732, - -69.88935388472542, - -69.5632321514312, - -69.52093877070061 + -69.90946048551345, + -69.88935388407138, + -69.56323215616419, + -69.52093877722275 ], "moduleForcesY": [ - 1.7503444073984757, - 2.581564547481693, - 7.158055486956702, - 7.611351867199961 + 1.7503444512317394, + 2.5815645641729006, + 7.1580554414148985, + 7.611351807216961 ], - "timestamp": 3.857325810916695 + "timestamp": 3.857325810920892 }, { - "x": 7.976860949117206, - "y": 1.1783452360639273, - "heading": -0.2270240484884978, - "angularVelocity": -0.45719601814734345, - "velocityX": -2.4764127602640795, - "velocityY": 0.3059610737751436, + "x": 7.976860949036181, + "y": 1.1783452358227742, + "heading": -0.22702404624732822, + "angularVelocity": -0.45719601244123276, + "velocityX": -2.4764127604175035, + "velocityY": 0.30596107327060007, "moduleForcesX": [ - -69.88812974316788, - -69.86320182607882, - -69.59189618603347, - -69.5407034064261 + -69.8881297418902, + -69.86320182545145, + -69.59189619041469, + -69.54070341308756 ], "moduleForcesY": [ - 2.107233171390243, - 2.950662034921671, - 6.753983199950573, - 7.317496840416227 + 2.1072332152187867, + 2.95066204873304, + 6.753983155440134, + 7.31749677667182 ], - "timestamp": 3.929851780084187 + "timestamp": 3.9298517800867527 }, { - "x": 7.777457476125625, - "y": 1.201895191338467, - "heading": -0.26202980984331203, - "angularVelocity": -0.4826651991962036, - "velocityX": -2.74940790561615, - "velocityY": 0.3247106594351006, + "x": 7.7774574760376565, + "y": 1.2018951910560491, + "heading": -0.26202980714373064, + "angularVelocity": -0.4826651928862579, + "velocityX": -2.7494079057737237, + "velocityY": 0.32471065887340905, "moduleForcesX": [ - -69.85533117072832, - -69.82743416827404, - -69.62139360036666, - -69.57054322743649 + -69.8553311691883, + -69.82743416769303, + -69.62139360441714, + -69.57054323404049 ], "moduleForcesY": [ - 2.6219385333270773, - 3.3908613060937713, - 6.272491751359185, - 6.868108041742767 + 2.621938575781362, + 3.3908613169810025, + 6.272491707304398, + 6.86810797438961 ], - "timestamp": 4.0023777492516786 + "timestamp": 4.002377749252613 }, { - "x": 7.558257164116274, - "y": 1.2268061069502, - "heading": -0.2982123205048736, - "angularVelocity": -0.49889041232006653, - "velocityX": -3.022369980373853, - "velocityY": 0.3434758045660868, + "x": 7.55825716402174, + "y": 1.226806106621595, + "heading": -0.29821231730362563, + "angularVelocity": -0.4988904054140694, + "velocityX": -3.0223699805323463, + "velocityY": 0.34347580393694493, "moduleForcesX": [ - -69.80056727463466, - -69.77570886950674, - -69.6507199544232, - -69.61392322278483 + -69.80056727283858, + -69.77570886897958, + -69.65071995819655, + -69.61392322895297 ], "moduleForcesY": [ - 3.39043477997722, - 3.9399463888437256, - 5.678903958822243, - 6.160009083534089 + 3.3904348183931043, + 3.9399463970685407, + 5.678903913889345, + 6.160009013377018 ], - "timestamp": 4.07490371841917 + "timestamp": 4.074903718418473 }, { - "x": 7.319267013948266, - "y": 1.2530778743434587, - "heading": -0.3345519884568234, - "angularVelocity": -0.5010573229066131, - "velocityX": -3.2952355261337996, - "velocityY": 0.3622394529136627, + "x": 7.319267013847545, + "y": 1.2530778739626474, + "heading": -0.33455198471356234, + "angularVelocity": -0.501057315444351, + "velocityX": -3.2952355262932205, + "velocityY": 0.3622394522019477, "moduleForcesX": [ - -69.69826365815047, - -69.69360285262502, - -69.67768220142874, - -69.67276264637083 + -69.69826365639196, + -69.69360285213232, + -69.677682204989, + -69.67276265133394 ], "moduleForcesY": [ - 4.602649202702627, - 4.682797548651256, - 4.9016741603909, - 4.9806442793840855 + 4.602649230854164, + 4.682797554881524, + 4.901674111951051, + 4.980644209624238 ], - "timestamp": 4.147429687586662 + "timestamp": 4.147429687584333 }, { - "x": 7.060508520408783, - "y": 1.2807073809011449, - "heading": -0.3693010360245331, - "angularVelocity": -0.479125587246758, - "velocityX": -3.5678046982302343, - "velocityY": 0.38096018398438164, + "x": 7.060508520302209, + "y": 1.2807073804603784, + "heading": -0.36930103170788803, + "angularVelocity": -0.47912557935144634, + "velocityX": -3.5678046983912037, + "velocityY": 0.38096018316622676, "moduleForcesX": [ - -69.47236753682394, - -69.53951749408205, - -69.69564436263259, - -69.73202523829484 + -69.47236753665878, + -69.53951749340459, + -69.69564436604739, + -69.73202524072254 ], "moduleForcesY": [ - 6.700012628800592, - 5.851281299757406, - 3.768542257976925, - 2.8040874089418844 + 6.7000126322527995, + 5.851281306796728, + 3.7685421990988033, + 2.8040873490370677 ], - "timestamp": 4.219955656754154 + "timestamp": 4.2199556567501935 }, { - "x": 6.7820675423003, - "y": 1.3096827333084073, - "heading": -0.398737971474847, - "angularVelocity": -0.40588131104575736, - "velocityX": -3.839190035026568, - "velocityY": 0.39951692807238764, + "x": 6.782067542187506, + "y": 1.3096827327970242, + "heading": -0.3987379665925305, + "angularVelocity": -0.405881303255151, + "velocityX": -3.8391900351986807, + "velocityY": 0.3995169271076376, "moduleForcesX": [ - -68.80858735271256, - -69.12938539141281, - -69.67366590353845, - -69.61858416268272 + -68.80858736495749, + -69.12938538991284, + -69.67366590614556, + -69.6185841628369 ], "moduleForcesY": [ - 10.998366071640108, - 8.339611523596144, - 1.7520720276263266, - -2.1336448340952816 + 10.99836599716967, + 8.33961153542166, + 1.752071940836427, + -2.13364483389887 ], - "timestamp": 4.292481625921646 + "timestamp": 4.292481625916054 }, { - "x": 6.484732150451617, - "y": 1.3400281940076402, - "heading": -0.4090628910860552, - "angularVelocity": -0.142361690988409, - "velocityX": -4.099709321525832, - "velocityY": 0.4184082067095552, + "x": 6.484732150295716, + "y": 1.3400281933976181, + "heading": -0.40906288611345654, + "angularVelocity": -0.14236168974674643, + "velocityX": -4.099709322212432, + "velocityY": 0.41840820535881235, "moduleForcesX": [ - -65.20316881632876, - -65.7489206217446, - -69.31239778456624, - -65.86566076035609 + -65.20316900335862, + -65.74892075037127, + -69.31239777710773, + -65.86566098161668 ], "moduleForcesY": [ - 23.606521909357, - 20.444429351523326, - -3.7507078277888724, - -21.00209915007702 + 23.606521394196665, + 20.444428940438716, + -3.750707987782145, + -21.00209845812331 ], - "timestamp": 4.365007595089137 + "timestamp": 4.365007595081914 }, { - "x": 6.1842404767603805, - "y": 1.3742213267083638, - "heading": -0.40911116386317103, - "angularVelocity": -0.0006655929961390624, - "velocityX": -4.1432286550667365, - "velocityY": 0.4714605415577361, + "x": 6.184240476626105, + "y": 1.3742213262250316, + "heading": -0.40911115889080796, + "angularVelocity": -0.0006655929994023552, + "velocityX": -4.143228654861732, + "velocityY": 0.4714605433153216, "moduleForcesX": [ - -13.325449562031311, - -4.164768682569861, - -18.043544953356022, - -8.922855853125498 + -13.325455797596309, + -4.164751278280683, + -18.043558746247182, + -8.922852319147749 ], "moduleForcesY": [ - 19.90592667522208, - 16.82639877386449, - 10.611237303210721, - 6.85137196260812 + 19.90590611882484, + 16.82641222119337, + 10.611221364447623, + 6.851398187145905 ], - "timestamp": 4.437533564256629 + "timestamp": 4.437533564247774 }, { - "x": 5.886052139153465, - "y": 1.4246963553310026, - "heading": -0.4091589633873657, - "angularVelocity": -0.0006590677069691712, - "velocityX": -4.111469878027867, - "velocityY": 0.6959580023818284, + "x": 5.886052139053873, + "y": 1.424696355011759, + "heading": -0.40915895841469946, + "angularVelocity": -0.0006590677028043638, + "velocityX": -4.111469877642081, + "velocityY": 0.6959580046600466, "moduleForcesX": [ - 8.110433382725144, - 8.111187384355791, - 8.110199033033277, - 8.110953042580169 + 8.110433400741913, + 8.11118742383485, + 8.110199088142462, + 8.110953115410458 ], "moduleForcesY": [ - 57.33325913716358, - 57.33311245310866, - 57.33314368701091, - 57.33299700322065 + 57.33325927042727, + 57.33311258568195, + 57.333143822794604, + 57.33299713881585 ], - "timestamp": 4.510059533424121 + "timestamp": 4.510059533413634 }, { - "x": 5.591555275132608, - "y": 1.4934998764417577, - "heading": -0.4092089626864391, - "angularVelocity": -0.0006893985650598928, - "velocityX": -4.060571232640143, - "velocityY": 0.94867427351207, + "x": 5.5915552750776545, + "y": 1.4934998762835463, + "heading": -0.4092089577134744, + "angularVelocity": -0.0006893985609604924, + "velocityX": -4.060571232115967, + "velocityY": 0.9486742757538188, "moduleForcesX": [ - 13.000242789451551, - 12.99626661282106, - 13.001163654267753, - 12.99718771199267 + 13.000242825406785, + 12.996266648691245, + 13.001163689713444, + 12.997187747343835 ], "moduleForcesY": [ - 64.53930421343945, - 64.54015945588058, - 64.53937921708196, - 64.54023445819007 + 64.53930420549774, + 64.54015944796213, + 64.53937920928017, + 64.5402344504135 ], - "timestamp": 4.582585502591613 + "timestamp": 4.582585502579494 }, { "x": 5.302053451538088, "y": 1.5809556245803824, - "heading": -0.4092627987960467, - "angularVelocity": -0.000742301140211202, - "velocityX": -3.991698793103366, - "velocityY": 1.2058542497602232, + "heading": -0.40926279382276776, + "angularVelocity": -0.0007423011358945286, + "velocityX": -3.9916987924354075, + "velocityY": 1.205854251968866, "moduleForcesX": [ - 17.591944196413664, - 17.584790178826015, - 17.593089676218266, - 17.5859364455025 + 17.591944233654857, + 17.584790216066136, + 17.59308971312774, + 17.585936482408147 ], "moduleForcesY": [ - 65.6787937062123, - 65.68074285605304, - 65.67871632504414, - 65.68066538032537 + 65.67879369894756, + 65.68074284879431, + 65.67871631788772, + 65.68066537317567 ], - "timestamp": 4.6551114717591044 + "timestamp": 4.655111471745355 }, { - "x": 5.060781264234282, - "y": 1.668503203961895, - "heading": -0.4093136867686553, - "angularVelocity": -0.0008266775577253413, - "velocityX": -3.9194782641972647, - "velocityY": 1.4222146294751892, + "x": 5.060781264318389, + "y": 1.6685032040805434, + "heading": -0.40931368179496846, + "angularVelocity": -0.0008266775512222434, + "velocityX": -3.9194782634197978, + "velocityY": 1.4222146316163005, "moduleForcesX": [ - 21.736557880357715, - 21.723118914680356, - 21.73784859212575, - 21.72441246515569 + 21.736557916865454, + 21.723118951579323, + 21.73784862823763, + 21.724412501654832 ], "moduleForcesY": [ - 65.09863070377499, - 65.10314246149298, - 65.09850389768496, - 65.10301496712766 + 65.09863069400774, + 65.10314245160757, + 65.09850388812552, + 65.10301495745126 ], - "timestamp": 4.716668692144205 + "timestamp": 4.716668692121208 }, { - "x": 4.824591541574993, - "y": 1.768967581170856, - "heading": -0.4093625786347868, - "angularVelocity": -0.0007942507121868569, - "velocityX": -3.836913382080745, - "velocityY": 1.632048630208832, + "x": 4.824591541749403, + "y": 1.7689675814031505, + "heading": -0.4093625736606763, + "angularVelocity": -0.0007942507054235842, + "velocityX": -3.8369133811902225, + "velocityY": 1.6320486323002081, "moduleForcesX": [ - 24.840611640815233, - 24.84563714723139, - 24.840370639682614, - 24.84539654710804 + 24.840611678634826, + 24.845637185091626, + 24.840370677624488, + 24.845396585090324 ], "moduleForcesY": [ - 63.138133963812095, - 63.136150247952436, - 63.13804254630454, - 63.13605868744737 + 63.13813395857038, + 63.136150242692494, + 63.13804254100993, + 63.13605868213461 ], - "timestamp": 4.778225912529306 + "timestamp": 4.778225912497061 }, { - "x": 4.5940300417851905, - "y": 1.8817568503359232, - "heading": -0.40940943255755435, - "angularVelocity": -0.00076114422441599, - "velocityX": -3.7454826314023215, - "velocityY": 1.8322670916501005, + "x": 4.5940300420581, + "y": 1.8817568506816633, + "heading": -0.4094094275830447, + "angularVelocity": -0.0007611442180464456, + "velocityX": -3.745482630364935, + "velocityY": 1.8322670937682957, "moduleForcesX": [ - 27.508225922613867, - 27.513128573012004, - 27.508193765462636, - 27.51309679763414 + 27.50822597067515, + 27.513128620991576, + 27.508193814141755, + 27.513096846198717 ], "moduleForcesY": [ - 60.24514716829222, - 60.242914588066654, - 60.24482580432994, - 60.242593041950194 + 60.245147185521205, + 60.24291460533314, + 60.24482582123695, + 60.24259305890976 ], - "timestamp": 4.839783132914407 + "timestamp": 4.839783132872914 }, { - "x": 4.369349606253567, - "y": 2.0058517685581454, - "heading": -0.4094544926877705, - "angularVelocity": -0.0007320039783130824, - "velocityX": -3.6499444602278026, - "velocityY": 2.015927903272623, + "x": 4.36934960664493, + "y": 2.0058517690384203, + "heading": -0.4094544877128489, + "angularVelocity": -0.0007320039717315139, + "velocityX": -3.6499444588518815, + "velocityY": 2.0159279057610187, "moduleForcesX": [ - 28.744538248232754, - 28.74851367433131, - 28.744577870233446, - 28.74855354416703 + 28.744538353811407, + 28.74851378007089, + 28.744577977045132, + 28.74855365043348 ], "moduleForcesY": [ - 55.26315032046569, - 55.261123506293146, - 55.262531653571756, - 55.260504693222536 + 55.2631504404134, + 55.261123626177344, + 55.2625317726504, + 55.26050481260123 ], - "timestamp": 4.901340353299508 + "timestamp": 4.901340353248767 }, { - "x": 4.149973859162333, - "y": 2.13910073076137, - "heading": -0.4094996155694483, - "angularVelocity": -0.0007330233788204265, - "velocityX": -3.563769541880253, - "velocityY": 2.1646357871525597, + "x": 4.149973859634745, + "y": 2.1391007313038166, + "heading": -0.40949961058910517, + "angularVelocity": -0.0007330232908571056, + "velocityX": -3.5637695410990893, + "velocityY": 2.1646357884876952, "moduleForcesX": [ - 25.92928590684574, - 25.929168249602895, - 25.929292292013468, - 25.929174634983514 + 25.92928571690531, + 25.929168021968326, + 25.929292070124113, + 25.92917457379335 ], "moduleForcesY": [ - 44.74476438288918, - 44.74482729836071, - 44.74480699802511, - 44.744869913360205 + 44.74476401967033, + 44.744826943195974, + 44.74480673058518, + 44.74486953719506 ], - "timestamp": 4.962897573684609 + "timestamp": 4.962897573624621 }, { - "x": 3.9353204017277856, - "y": 2.278088242273816, - "heading": -0.41252140844778834, - "angularVelocity": -0.04908917036026353, - "velocityX": -3.4870557197299563, - "velocityY": 2.2578587961400522, + "x": 3.9353204020821484, + "y": 2.2780882424747335, + "heading": -0.4125214036176848, + "angularVelocity": -0.04908917280838613, + "velocityX": -3.48705572217154, + "velocityY": 2.257858790931102, "moduleForcesX": [ - 24.953488052198967, - 20.4567569278476, - 25.589392523319265, - 21.330260691348453 + 24.953505556486252, + 20.45677256337718, + 25.589356554115078, + 21.330259655702005 ], "moduleForcesY": [ - 25.14686564166064, - 27.7645218324562, - 28.406596524112476, - 30.881756387142097 + 25.146827347937002, + 27.764534745643168, + 28.406606309810808, + 30.88176412187559 ], - "timestamp": 5.02445479406971 + "timestamp": 5.024454794000474 }, { - "x": 3.7318642433739195, - "y": 2.4104934513603675, - "heading": -0.43615839467618756, - "angularVelocity": -0.38398397589611083, - "velocityX": -3.305155058676315, - "velocityY": 2.150928977270674, + "x": 3.7318642436374203, + "y": 2.4104934515239713, + "heading": -0.4361583914201127, + "angularVelocity": -0.3839840015242407, + "velocityX": -3.3051550606489273, + "velocityY": 2.150928976987681, "moduleForcesX": [ - 49.89592032127635, - 34.025418039882794, - 66.93891398556973, - 68.06860648029641 + 49.89592057158015, + 34.025417576241814, + 66.93891416475311, + 68.0686068113683 ], "moduleForcesY": [ - -47.83606477558671, - -59.36994474295037, - -17.100092585775965, - -4.390644167479697 + -47.836064483916026, + -59.369944976556134, + -17.100091773085392, + -4.390638937539682 ], - "timestamp": 5.086012014454811 + "timestamp": 5.086012014376327 }, { - "x": 3.540415993553555, - "y": 2.5354156131468453, - "heading": -0.46498951807167194, - "angularVelocity": -0.4683629835017646, - "velocityX": -3.110086008150945, - "velocityY": 2.0293665146828603, + "x": 3.5404159937543107, + "y": 2.5354156132801298, + "heading": -0.4649895158550848, + "angularVelocity": -0.46836300045892293, + "velocityX": -3.1100860096374956, + "velocityY": 2.029366514495222, "moduleForcesX": [ - 55.83155696216655, - 54.18327481859195, - 62.40116195557468, - 62.36184788712686 + 55.83155730522792, + 54.18327547289772, + 62.40116176551297, + 62.3618475536798 ], "moduleForcesY": [ - -41.50061361131458, - -43.48191734914588, - -30.734501757153353, - -30.59101829583353 + -41.50061314279684, + -43.48191653513505, + -30.73450213526439, + -30.591018982325306 ], - "timestamp": 5.147569234839912 + "timestamp": 5.14756923475218 }, { - "x": 3.361022505593955, - "y": 2.652736487945131, - "heading": -0.4956638148847116, - "angularVelocity": -0.49830542414467005, - "velocityX": -2.9142558230751625, - "velocityY": 1.9058832426858676, + "x": 3.361022505744376, + "y": 2.652736488054298, + "heading": -0.4956638133831861, + "angularVelocity": -0.4983054358359662, + "velocityX": -2.9142558243306804, + "velocityY": 1.9058832425804164, "moduleForcesX": [ - 57.73901804404707, - 57.438460533401326, - 60.30587505828676, - 60.21056114711604 + 57.7390182838169, + 57.43846086192714, + 60.30587488496412, + 60.21056094689106 ], "moduleForcesY": [ - -39.07130416952509, - -39.474125567373356, - -34.97681089661358, - -35.097625013264334 + -39.071303811919336, + -39.47412508944996, + -34.976811192657685, + -35.09762535851428 ], - "timestamp": 5.209126455225013 + "timestamp": 5.209126455128033 }, { - "x": 3.1936838062667205, - "y": 2.7624068712077934, - "heading": -0.5266594310996341, - "angularVelocity": -0.5035252732516518, - "velocityX": -2.7184252030934304, - "velocityY": 1.7816006404539642, + "x": 3.1936838063800033, + "y": 2.762406871298198, + "heading": -0.5266594301029957, + "angularVelocity": -0.5035252815293935, + "velocityX": -2.7184252041051566, + "velocityY": 1.7816006404168396, "moduleForcesX": [ - 58.700566811459886, - 58.67734780171254, - 59.16682840463337, - 59.14969530624501 + 58.70056697929468, + 58.67734799260074, + 59.16682827716417, + 59.14969518046466 ], "moduleForcesY": [ - -37.749793809130026, - -37.78083859729802, - -37.014524694721786, - -37.03675261323169 + -37.749793546335056, + -37.78083830094146, + -37.01452489709346, + -37.03675281488077 ], - "timestamp": 5.270683675610114 + "timestamp": 5.270683675503887 }, { - "x": 3.038390509303798, - "y": 2.8643948429617936, - "heading": -0.5570773161987834, - "angularVelocity": -0.4941400035435474, - "velocityX": -2.5227470634868117, - "velocityY": 1.6567994967278303, + "x": 3.038390509385735, + "y": 2.864394843036019, + "heading": -0.5570773155618112, + "angularVelocity": -0.49414000946072106, + "velocityX": -2.5227470643750314, + "velocityY": 1.6567994967139286, "moduleForcesX": [ - 59.30031933124251, - 59.30307854027269, - 58.44288594465124, - 58.46463470158717 + 59.30031945388296, + 59.30307866344703, + 58.44288585276952, + 58.4646346161408 ], "moduleForcesY": [ - -36.885288676049825, - -36.88814111006628, - -38.229406656940725, - -38.203170518961 + -36.885288477772534, + -36.888140912141004, + -38.22940679661343, + -38.20317065012603 ], - "timestamp": 5.332240895995215 + "timestamp": 5.33224089587974 }, { - "x": 2.895129799571264, - "y": 2.9586740032068897, - "heading": -0.5863104114525834, - "angularVelocity": -0.4748930356337662, - "velocityX": -2.3272771063459046, - "velocityY": 1.5315694837305227, + "x": 2.8951297996314556, + "y": 2.958674003265299, + "heading": -0.5863104110705085, + "angularVelocity": -0.474893039846042, + "velocityX": -2.3272771070488116, + "velocityY": 1.5315694837037004, "moduleForcesX": [ - 59.717187192796516, - 59.657546410038414, - 57.934272823511364, - 57.95135152475748 + 59.71718728545176, + 59.65754649481836, + 57.93427275714158, + 57.95135146376398 ], "moduleForcesY": [ - -36.2638573648305, - -36.37433120370207, - -39.04885966973737, - -39.03512982178747 + -36.26385721152638, + -36.374331064721694, + -39.04885976771552, + -39.03512991256516 ], - "timestamp": 5.3937981163803155 + "timestamp": 5.393798116255593 }, { - "x": 2.763886371067874, - "y": 3.045218511804947, - "heading": -0.6139222009668763, - "angularVelocity": -0.4485548460699908, - "velocityX": -2.132055795929944, - "velocityY": 1.4059196964488687, + "x": 2.7638863711095008, + "y": 3.0452185118515818, + "heading": -0.6139222007620838, + "angularVelocity": -0.4485548490174231, + "velocityX": -2.1320557965518465, + "velocityY": 1.4059196964688137, "moduleForcesX": [ - 60.015428697215164, - 59.85831037769707, - 57.55335306922513, - 57.534003758004665 + 60.01542876878009, + 59.8583104385353, + 57.5533530210112, + 57.53400371300751 ], "moduleForcesY": [ - -35.80957668058033, - -36.086111428235434, - -39.64516852205637, - -39.68654602916551 + -35.80957656014014, + -36.08611132735859, + -39.645168591728385, + -39.68654609453473 ], - "timestamp": 5.455355336765416 + "timestamp": 5.455355336631446 }, { - "x": 2.644643305476117, - "y": 3.1240017980714323, - "heading": -0.6395960472514493, - "angularVelocity": -0.41707286527865206, - "velocityX": -1.937109324394044, - "velocityY": 1.2798382671215223, + "x": 2.6446433055054834, + "y": 3.1240017981097368, + "heading": -0.6395960471657398, + "angularVelocity": -0.417072867275884, + "velocityX": -1.9371093248842417, + "velocityY": 1.2798382671784732, "moduleForcesX": [ - 60.22326407527744, - 59.95837931731987, - 57.26239607352692, - 57.186270668215975 + 60.22326413098716, + 59.958379361963274, + 57.26239603856264, + 57.186270634486874 ], "moduleForcesY": [ - -35.49033319006886, - -35.95108026507241, - -40.09166456022141, - -40.21383292620102 + -35.49033309517584, + -35.95108019063276, + -40.0916646099415, + -40.21383297424875 ], - "timestamp": 5.516912557150517 + "timestamp": 5.516912557007299 }, { - "x": 2.537383808831169, - "y": 3.1949980499443154, - "heading": -0.6631036099321576, - "angularVelocity": -0.38188148414867673, - "velocityX": -1.7424356716228648, - "velocityY": 1.1533375196074727, + "x": 2.537383808849767, + "y": 3.194998049974314, + "heading": -0.6631036099218587, + "angularVelocity": -0.38188148543114175, + "velocityX": -1.7424356720595673, + "velocityY": 1.153337519645833, "moduleForcesX": [ - 60.36537448302678, - 59.99512146624315, - 57.04267536961116, - 56.89878422586106 + 60.36537452641838, + 59.995121499410935, + 57.04267534461395, + 56.89878420063114 ], "moduleForcesY": [ - -35.27267783600763, - -35.913635660706774, - -40.42480723386195, - -40.64046601274349 + -35.27267776148261, + -35.91363560529686, + -40.42480726897906, + -40.64046604811504 ], - "timestamp": 5.578469777535618 + "timestamp": 5.578469777383153 }, { - "x": 2.4420919536077847, - "y": 3.2581828688425367, - "heading": -0.6842805929390149, - "angularVelocity": -0.344021105475332, - "velocityX": -1.5480207622638111, - "velocityY": 1.0264404159729446, + "x": 2.4420919536194616, + "y": 3.258182868864464, + "heading": -0.6842805929718256, + "angularVelocity": -0.3440211062273634, + "velocityX": -1.5480207626088205, + "velocityY": 1.0264404159960439, "moduleForcesX": [ - 60.4598584765929, - 59.99281023891339, - 56.876528556020105, - 56.661344392247265 + 60.45985851021655, + 59.992810263691005, + 56.876528538625024, + 56.66134437370607 ], "moduleForcesY": [ - -35.130249715774205, - -35.93621486669289, - -40.675063944105, - -40.98709766240873 + -35.13024965770887, + -35.936214825314366, + -40.6750639683131, + -40.98709768806473 ], - "timestamp": 5.640026997920719 + "timestamp": 5.640026997759006 }, { - "x": 2.3587528835015927, - "y": 3.3135334333216804, - "heading": -0.7030081263323279, - "angularVelocity": -0.3042296789257516, - "velocityX": -1.3538471942824237, - "velocityY": 0.8991725768784151, + "x": 2.35875288350831, + "y": 3.313533433337192, + "heading": -0.7030081263847169, + "angularVelocity": -0.30422967928952566, + "velocityX": -1.353847194566388, + "velocityY": 0.8991725769092883, "moduleForcesX": [ - 60.5201219775476, - 59.967443909712394, - 56.748515348358374, - 56.463990903711384 + 60.52012200326446, + 59.96744392817741, + 56.74851533683041, + 56.463990890605494 ], "moduleForcesY": [ - -35.04253435660417, - -35.99355809852384, - -40.867195346689, - -41.27154173850131 + -35.042534312039635, + -35.99355806773702, + -40.86719536260796, + -41.2715417564386 ], - "timestamp": 5.70158421830582 + "timestamp": 5.701584218134859 }, { - "x": 2.2873528689676483, - "y": 3.3610285743692394, - "heading": -0.7191989284072005, - "angularVelocity": -0.26302035689049935, - "velocityX": -1.1598966634176864, - "velocityY": 0.7715608461595652, + "x": 2.287352868970426, + "y": 3.361028574379745, + "heading": -0.7191989284627818, + "angularVelocity": -0.26302035698188275, + "velocityX": -1.1598966636559505, + "velocityY": 0.7715608461941635, "moduleForcesX": [ - 60.55636984330847, - 59.92996721273085, - 56.64674423048133, - 56.29855167271921 + 60.556369862505825, + 59.92996722632186, + 56.646744223537, + 56.29855166413728 ], "moduleForcesY": [ - -34.99335531824572, - -36.06824538837232, - -41.01955138323022, - -41.50757259528159 + -34.99335528490936, + -36.068245365760546, + -41.019551392749705, + -41.507572606915744 ], - "timestamp": 5.763141438690921 + "timestamp": 5.763141438510712 }, { - "x": 2.2278792676988664, - "y": 3.4006487477183196, - "heading": -0.7327874449132579, - "angularVelocity": -0.2207461029112197, - "velocityX": -0.966151507438403, - "velocityY": 0.6436316178868581, + "x": 2.2278792676993477, + "y": 3.4006487477251475, + "heading": -0.7327874449614561, + "angularVelocity": -0.22074610282445042, + "velocityX": -0.9661515076208613, + "velocityY": 0.6436316179238191, "moduleForcesX": [ - 60.57586928152923, - 59.88760410989669, - 56.562363081650766, - 56.15861493971373 + 60.575869295269314, + 59.88760411963649, + 56.56236307833243, + 56.15861493495852 ], "moduleForcesY": [ - -34.97098554580845, - -36.148822416267066, - -41.14545732873801, - -41.70558761712785 + -34.970985521919275, + -36.1488224000973, + -41.14545733324237, + -41.70558762351516 ], - "timestamp": 5.824698659076022 + "timestamp": 5.8246986588865655 }, { - "x": 2.180320435943854, - "y": 3.432375929705667, - "heading": -0.7437231910234625, - "angularVelocity": -0.17765172049338343, - "velocityX": -0.7725955047594213, - "velocityY": 0.5154095943394352, + "x": 2.1803204359440898, + "y": 3.4323759297090946, + "heading": -0.7437231910585703, + "angularVelocity": -0.17765172030741577, + "velocityX": -0.772595504879486, + "velocityY": 0.5154095943616319, "moduleForcesX": [ - 60.58340632519516, - 59.84490600733586, - 56.48902920756478, - 56.03945201723916 + 60.583406334310894, + 59.84490601396566, + 56.489029207157884, + 56.03945201575598 ], "moduleForcesY": [ - -34.96766125833015, - -36.22817172523544, - -41.25431832732434, - -41.87309909683168 + -34.96766124246743, + -36.22817171424666, + -41.25431832783358, + -41.87309909879299 ], - "timestamp": 5.886255879461123 + "timestamp": 5.886255879262419 }, { - "x": 2.1446656199937997, - "y": 3.4561934671002783, - "heading": -0.7519665199163653, - "angularVelocity": -0.13391327355800717, - "velocityX": -0.5792141965962733, - "velocityY": 0.3869170382549771, + "x": 2.1446656199930714, + "y": 3.4561934671017123, + "heading": -0.7519665199367948, + "angularVelocity": -0.1339132733396743, + "velocityX": -0.5792141966989566, + "velocityY": 0.38691703828071905, "moduleForcesX": [ - 60.58183660174803, - 59.80457338750152, - 56.42234143923046, - 55.9377862653548 + 60.581836606907586, + 59.80457339156535, + 56.422341441204175, + 55.937786266687105 ], "moduleForcesY": [ - -34.97878115536029, - -36.30217808780585, - -41.352605880404866, - -42.01528814031888 + -34.97878114637003, + -36.302178081071816, + -41.352605877670875, + -42.01528813851514 ], - "timestamp": 5.947813099846224 + "timestamp": 5.947813099638272 }, { - "x": 2.120904845915518, - "y": 3.472085915141707, - "heading": -0.7574858469261834, - "angularVelocity": -0.08966173221089468, - "velocityX": -0.3859949154564824, - "velocityY": 0.258173581295686, + "x": 2.1209048459157667, + "y": 3.4720859151422694, + "heading": -0.7574858469338888, + "angularVelocity": -0.08966173201765831, + "velocityX": -0.3859949154986063, + "velocityY": 0.2581735813203214, "moduleForcesX": [ - 60.57287518432625, - 59.76823373322807, - 56.35914684811399, - 55.851272292620784 + 60.57287518607043, + 59.768233735123275, + 56.35914685208343, + 55.851272296387975 ], "moduleForcesY": [ - -35.001611618471976, - -36.36844720693236, - -41.44491977809569, - -42.13584960752861 + -35.00161161541115, + -36.368447203777166, + -41.44491977266188, + -42.13584960250002 ], - "timestamp": 6.009370320231325 + "timestamp": 6.009370320014125 }, { "x": 2.109028816223142, "y": 3.4800388813018817, "heading": -0.7602557704725089, - "angularVelocity": -0.044997540970748186, - "velocityX": -0.19292667242090425, - "velocityY": 0.12919631702701284, + "angularVelocity": -0.04499754085233033, + "velocityX": -0.19292667245392872, + "velocityY": 0.12919631703729303, "moduleForcesX": [ - 60.5575482023407, - 59.73687419838248, - 56.29713329942968, - 55.778188462220434 + 60.55754820112047, + 59.736874198400685, + 56.297133305118244, + 55.77818846809218 ], "moduleForcesY": [ - -35.034541448499795, - -36.42559455322768, - -41.5346063390199, - -42.237487649673916 + -35.03454145057582, + -36.425594553156486, + -41.53460633127721, + -42.237487641880264 ], - "timestamp": 6.070927540616426 + "timestamp": 6.070927540389978 }, { "x": 2.109028816223143, "y": 3.4800388813018808, "heading": -0.760255770472509, - "angularVelocity": -4.7854036756238116e-17, - "velocityX": -4.1684189280839564e-16, - "velocityY": 3.182458036241524e-16, + "angularVelocity": -4.742890180425531e-17, + "velocityX": -4.236195451911178e-16, + "velocityY": 3.1146529727541206e-16, "moduleForcesX": [ - 60.53644088216914, - 59.71107786211956, - 56.23458372965665, - 55.717252600818924 + 60.53644087836653, + 59.71107786047138, + 56.23458373687308, + 55.71725260850273 ], "moduleForcesY": [ - -35.07667248160167, - -36.472859348504336, - -41.62412921793918, - -42.32221267335896 + -35.0766724881386, + -36.47285935116105, + -41.62412920816004, + -42.32221266320002 ], - "timestamp": 6.1324847610015265 + "timestamp": 6.1324847607658315 }, { - "x": 2.1173647314297415, - "y": 3.4676489109609023, - "heading": -0.7574096677464409, - "angularVelocity": 0.045234117251022915, - "velocityX": 0.13248564867185347, - "velocityY": -0.19691818078357895, + "x": 2.11756098918411, + "y": 3.46792188759214, + "heading": -0.7574690093542815, + "angularVelocity": 0.04446117329426013, + "velocityX": 0.13612591984038372, + "velocityY": -0.1933196762400969, "moduleForcesX": [ - 41.520736384271714, - 42.51429487640815, - 35.1511276560287, - 36.81654326527437 + 42.74872403178063, + 43.59458842062485, + 36.532752148464645, + 38.029009807643504 ], "moduleForcesY": [ - -56.31022663437434, - -55.57003233589047, - -60.49287111022869, - -59.49929247213998 + -55.38361215410406, + -54.726449152918555, + -59.66836228131139, + -58.73145114027325 ], - "timestamp": 6.1954041435170275 + "timestamp": 6.1951632937812295 }, { - "x": 2.1342750283552188, - "y": 3.4430345558261144, - "heading": -0.751664349766151, - "angularVelocity": 0.09131237069712948, - "velocityX": 0.2687613299655501, - "velocityY": -0.39120465190077036, + "x": 2.134842785454668, + "y": 3.4438456887135054, + "heading": -0.7518451991808588, + "angularVelocity": 0.0897246617440947, + "velocityX": 0.27572113511834023, + "velocityY": -0.38412192692383823, "moduleForcesX": [ - 42.70173098288214, - 43.611128958075156, - 36.275170342746776, - 37.87745965146702 + 43.83189384298978, + 44.60279573474994, + 37.56581425139671, + 39.005385774472 ], "moduleForcesY": [ - -55.41582346031395, - -54.70978545558125, - -59.821763435546124, - -58.826195167217094 + -54.526207091523304, + -53.904217238652684, + -59.01959396027511, + -58.084181071273 ], - "timestamp": 6.2583235260325285 + "timestamp": 6.257841826796628 }, { - "x": 2.1600269154540004, - "y": 3.4063896499792783, - "heading": -0.7429636060218578, - "angularVelocity": 0.1382839976560426, - "velocityX": 0.4092838497332282, - "velocityY": -0.5824104494002036, + "x": 2.1611174297283973, + "y": 3.407994009108057, + "heading": -0.7433311964216099, + "angularVelocity": 0.13583602470653403, + "velocityX": 0.41919686070627105, + "velocityY": -0.5719929596412315, "moduleForcesX": [ - 44.01151222332591, - 44.83593128986605, - 37.54047868266639, - 39.07824731715444 + 45.03070879320651, + 45.72720818672459, + 38.72596873376568, + 40.10888786396711 ], "moduleForcesY": [ - -54.37659249907092, - -53.70636995290632, - -59.03155043231922, - -58.03154166424591 + -53.535601635367144, + -52.949460544128094, + -58.260527674539446, + -57.32381863218353 ], - "timestamp": 6.3212429085480295 + "timestamp": 6.320520359812026 }, { - "x": 2.1949215222317773, - "y": 3.357943659865962, - "heading": -0.7312478740657462, - "angularVelocity": 0.18620227166446401, - "velocityX": 0.5545923272400236, - "velocityY": -0.7699692555212343, + "x": 2.1966585475096383, + "y": 3.3605831084486266, + "heading": -0.7318707568809233, + "angularVelocity": 0.18284473151072553, + "velocityX": 0.5670381240816551, + "velocityY": -0.7564136934695477, "moduleForcesX": [ - 45.47389898312857, - 46.209380246970596, - 38.97509187538921, - 40.44329438249183 + 46.366588104731406, + 46.98653915444157, + 40.038346183061186, + 41.36150168519336 ], "moduleForcesY": [ - -53.154044492191325, - -52.52435244840178, - -58.08923297674164, - -57.08406886084209 + -52.37723663439366, + -51.83024165919106, + -57.36143656121796, + -56.42213294794304 ], - "timestamp": 6.3841622910635305 + "timestamp": 6.383198892827424 }, { - "x": 2.2393001625829205, - "y": 3.297971653928642, - "heading": -0.7164541055664759, - "angularVelocity": 0.23512259510216332, - "velocityX": 0.7053254271878769, - "velocityY": -0.9531563015982434, + "x": 2.2417757829173284, + "y": 3.3018706473116755, + "heading": -0.7174043905612114, + "angularVelocity": 0.23080256706323984, + "velocityX": 0.7198195815560299, + "velocityY": -0.9367236007665936, "moduleForcesX": [ - 47.11716302884608, - 47.75573925943408, - 40.613851617327796, - 42.00243934560986 + 47.865105493752495, + 48.40288266056825, + 41.53424661497753, + 42.79020355531207 ], "moduleForcesY": [ - -51.69634105716302, - -51.116592706220615, - -56.94950428119139, - -55.94148520473914 + -51.00479249750509, + -50.50425407309677, + -56.2817455416053, + -55.341130773916184 ], - "timestamp": 6.4470816735790315 + "timestamp": 6.445877425842822 }, { - "x": 2.293551835832764, - "y": 3.226807987294763, - "heading": -0.6985158525170084, - "angularVelocity": 0.28509900021743295, - "velocityX": 0.862241030996735, - "velocityY": -1.1310293233781603, + "x": 2.296821580243133, + "y": 3.232167800844977, + "heading": -0.6998693932081385, + "angularVelocity": 0.27976081298465, + "velocityX": 0.8782240853065545, + "velocityY": -1.1120688872787494, "moduleForcesX": [ - 48.974244926718995, - 49.50301706537962, - 42.49983724647496, - 43.79203201526126 + 49.55640607435229, + 50.00204429745789, + 43.25256094553968, + 44.42804273281315 ], "moduleForcesY": [ - -49.932591819303816, - -49.41939613494972, - -55.54905272250912, - -54.54558483413394 + -49.35518638216705, + -48.914538145342306, + -54.9650826996631, + -54.028779358711596 ], - "timestamp": 6.5100010560945325 + "timestamp": 6.50855595885822 }, { - "x": 2.3581219992537226, - "y": 3.144865400496988, - "heading": -0.6773637507894199, - "angularVelocity": 0.3361778339508891, - "velocityX": 1.0262364447879162, - "velocityY": -1.302342513892083, + "x": 2.3621992355615835, + "y": 3.1518561030599934, + "heading": -0.679200184492938, + "angularVelocity": 0.3297653553908588, + "velocityX": 1.043062946326279, + "velocityY": -1.2813270177407263, "moduleForcesX": [ - 51.081995362995904, - 51.48237998253363, - 44.68553797550306, - 45.85573965609478 + 51.47499199800596, + 51.81338905645372, + 45.24113729305585, + 46.315146275458055 ], "moduleForcesY": [ - -47.76441461323329, - -47.34537980089634, - -53.79777393977218, - -52.8148137116725 + -47.34111247918622, + -46.98312892385554, + -53.331646603367375, + -52.41249065479232 ], - "timestamp": 6.5729204386100335 + "timestamp": 6.571234491873618 }, { - "x": 2.433522291274826, - "y": 3.0526620913362525, - "heading": -0.652926734850225, - "angularVelocity": 0.3883861373428849, - "velocityX": 1.1983635090908211, - "velocityY": -1.46541980347662, + "x": 2.4383720926533567, + "y": 3.061411276771742, + "heading": -0.6553291916681563, + "angularVelocity": 0.38084798217784427, + "velocityX": 1.2152941912833248, + "velocityY": -1.4429952638813623, "moduleForcesX": [ - 53.47798671720776, - 53.72572515007777, - 47.232726000587874, - 48.24428581121922 + 53.657812871578834, + 53.86840420507071, + 47.55738728725876, + 48.499088173849444 ], "moduleForcesY": [ - -45.05355867588098, - -44.773023132097414, - -51.56510924673164, - -50.632864772568915 + -44.83992767026467, + -44.60161470848597, + -51.266287364667946, + -50.38906940154251 ], - "timestamp": 6.6358398211255345 + "timestamp": 6.633913024889016 }, { - "x": 2.520339981100692, - "y": 2.9508605578412346, - "heading": -0.6251343348430549, - "angularVelocity": 0.4417144430863272, - "velocityX": 1.3798242505713987, - "velocityY": -1.6179677775753416, + "x": 2.5258731212927574, + "y": 2.9614374657104525, + "heading": -0.6281884283597926, + "angularVelocity": 0.43301529251962634, + "velocityX": 1.3960286629220273, + "velocityY": -1.5950247437464637, "moduleForcesX": [ - 56.19197506453079, - 56.259276660576184, - 50.20804830120448, - 51.011888473748435 + 56.13852280957632, + 56.19641284805428, + 50.26589724732213, + 51.0329382483159 ], "moduleForcesY": [ - -41.604044996750225, - -41.53144010163077, - -48.65926448291237, - -47.83146963835682 + -41.67704853096463, + -41.61695338715726, + -48.600192852411276, + -47.809382613298865 ], - "timestamp": 6.6987592036410355 + "timestamp": 6.696591557904414 }, { - "x": 2.6192438141173406, - "y": 2.8403235512778635, - "heading": -0.5939211843905473, - "angularVelocity": 0.4960816397843361, - "velocityX": 1.5719135989976136, - "velocityY": -1.7568037406619357, + "x": 2.6253125642587176, + "y": 2.8527168842363753, + "heading": -0.5977125836977811, + "angularVelocity": 0.48622460028738235, + "velocityX": 1.5864992076558508, + "velocityY": -1.7345744426942504, "moduleForcesX": [ - 59.2250417610776, - 59.087826967496994, - 53.66857872752665, - 54.204993983215175 + 58.932237428484335, + 58.813020339679206, + 53.42902064875195, + 53.9678341870784 ], "moduleForcesY": [ - -37.1392249820516, - -37.38030580647965, - -44.79597130253933, - -44.16473609829124 + -37.60318829571993, + -37.811782678383274, + -45.08236639483354, + -44.45475051563535 ], - "timestamp": 6.7616785861565365 + "timestamp": 6.759270090919812 }, { - "x": 2.7309783267356824, - "y": 2.7221935960005026, - "heading": -0.5592365908276438, - "angularVelocity": 0.5512545129373857, - "velocityX": 1.7758361279342616, - "velocityY": -1.8774811600901864, + "x": 2.737377784774954, + "y": 2.7362816266480974, + "heading": -0.5638459295824587, + "angularVelocity": 0.5403230178824133, + "velocityX": 1.7879362378935268, + "velocityY": -1.8576576698065623, "moduleForcesX": [ - 62.5038695327194, - 62.160031630846326, - 57.62319015145175, - 57.832996172310274 + 62.00010213745141, + 61.69318591156156, + 57.07952464389426, + 57.33202712766898 ], "moduleForcesY": [ - -31.280112248684674, - -31.988407393096097, - -39.55612203914035, - -39.27379472999048 + -32.26809282018358, + -32.88021961976661, + -40.33774835466741, + -40.002144832707245 ], - "timestamp": 6.8245979686720375 + "timestamp": 6.82194862393521 }, { - "x": 2.856329605423656, - "y": 2.598000279152458, - "heading": -0.5210635851729407, - "angularVelocity": 0.606697080113568, - "velocityX": 1.9922522071333428, - "velocityY": -1.9738483100568995, + "x": 2.8628118504353286, + "y": 2.6135144018139433, + "heading": -0.526556868174763, + "angularVelocity": 0.5949255608540639, + "velocityX": 2.0012284848715227, + "velocityY": -1.9586805709698702, "moduleForcesX": [ - 65.79278778008134, - 65.30020556417972, - 61.939549113121835, - 61.79878070206867 + 65.17607152774222, + 64.71491642924042, + 61.14954805032546, + 61.07753901466829 ], "moduleForcesY": [ - -23.548304579936264, - -24.925035881299397, - -32.34851276785782, - -32.65108476149218 + -25.204925810974277, + -26.40709149039101, + -33.818839446517686, + -33.98135694151609 ], - "timestamp": 6.8875173511875385 + "timestamp": 6.884627156950608 }, { - "x": 2.996031354964205, - "y": 2.46978535540666, - "heading": -0.47945305051497955, - "angularVelocity": 0.6613309443669427, - "velocityX": 2.220329315948571, - "velocityY": -2.0377651308674327, + "x": 3.0023435972808152, + "y": 2.4862768905201404, + "heading": -0.4858660567958102, + "angularVelocity": 0.6491985281301392, + "velocityX": 2.226148892336199, + "velocityY": -2.0300014242921804, "moduleForcesX": [ - 68.5564837925553, - 68.09308895613952, - 66.1567792079864, - 65.75590846714256 + 68.03638305751167, + 67.55366556741266, + 65.31234174805479, + 64.96053229107252 ], "moduleForcesY": [ - -13.459642197003902, - -15.709687602642282, - -22.445832125017436, - -23.647305076742175 + -15.87553479112995, + -17.88364572686639, + -24.79397361190254, + -25.75031448328813 ], - "timestamp": 6.9504367337030395 + "timestamp": 6.947305689966006 }, { - "x": 3.1505724071472994, - "y": 2.340193763604278, - "heading": -0.43457360660126365, - "angularVelocity": 0.7132848753348665, - "velocityX": 2.456175601294578, - "velocityY": -2.059645003198423, + "x": 3.1565264589627544, + "y": 2.3570325928242064, + "heading": -0.4418928272775442, + "angularVelocity": 0.701567624555973, + "velocityX": 2.459899015888926, + "velocityY": -2.0620185489054506, "moduleForcesX": [ - 69.84706521344005, - 69.75290941394955, - 69.22551312159781, - 68.88502793630397 + 69.73996701133173, + 69.52946808363019, + 68.70982043228925, + 68.32066639322257 ], "moduleForcesY": [ - -0.8233024041036456, - -4.014815042921486, - -9.277060723605338, - -11.648512019585809 + -3.8907722279034003, + -6.8454111489966385, + -12.521345429829843, + -14.587709075119305 ], - "timestamp": 7.0133561162185405 + "timestamp": 7.009984222981404 }, { - "x": 3.3199095005462036, - "y": 2.212409184758706, - "heading": -0.3867586308994902, - "angularVelocity": 0.7599403202978621, - "velocityX": 2.691334317484535, - "velocityY": -2.03092550716134, + "x": 3.325459805009775, + "y": 2.2288592413825183, + "heading": -0.3949105310397784, + "angularVelocity": 0.7495755560555903, + "velocityX": 2.69523452320619, + "velocityY": -2.0449322164288706, "moduleForcesX": [ - 68.47925044891942, - 69.15340266140203, - 69.49958829473019, - 69.7686571831701 + 69.04574597152255, + 69.5267430838888, + 69.75682983590083, + 69.84457598668554 ], "moduleForcesY": [ - 13.736045870694008, - 9.89537651518505, - 6.815502213890215, - 3.370466430920167 + 10.479207074920234, + 6.736459804087273, + 3.110509612866868, + -0.12959966259523578 ], - "timestamp": 7.0762754987340415 + "timestamp": 7.072662755996802 }, { - "x": 3.5032276259952, - "y": 2.089811754517775, - "heading": -0.3365088719529981, - "angularVelocity": 0.7986371915540097, - "velocityX": 2.9135398047467214, - "velocityY": -1.9484843197678885, + "x": 3.508485431874634, + "y": 2.1051894273892637, + "heading": -0.3453726504925619, + "angularVelocity": 0.7903484361232042, + "velocityX": 2.920068770912297, + "velocityY": -1.973080862675469, "moduleForcesX": [ - 63.762980248590615, - 65.33381311946748, - 65.64375774359121, - 66.90783722378835 + 64.90348551855642, + 66.33308380991878, + 66.72276225524763, + 67.80174756382702 ], "moduleForcesY": [ - 28.499981635594498, - 24.717827613930762, - 23.81826750682116, - 20.038950866645536 + 25.769345424686897, + 21.867362447533644, + 20.5650741326706, + 16.728754630599152 ], - "timestamp": 7.1391948812495425 + "timestamp": 7.1353412890122 }, { - "x": 3.698983205941932, - "y": 1.9754559392748747, - "heading": -0.2844250046592506, - "angularVelocity": 0.8277873242146967, - "velocityX": 3.111212668028096, - "velocityY": -1.8174974176634262, + "x": 3.704119942311664, + "y": 1.9892732212302584, + "heading": -0.2938661408515115, + "angularVelocity": 0.8217567827951044, + "velocityX": 3.1212362674309677, + "velocityY": -1.8493765023270128, "moduleForcesX": [ - 56.27400205759313, - 58.363284645856304, - 58.070669393657745, - 60.053109943523204 + 57.471427969439794, + 59.58527298092468, + 59.37846559329102, + 61.351076031348384 ], "moduleForcesY": [ - 41.37894474295054, - 38.388062514062604, - 38.80031879570978, - 35.67059242066261 + 39.677051011352944, + 36.440949209828744, + 36.741332005058595, + 33.36307222006473 ], - "timestamp": 7.2021142637650435 + "timestamp": 7.198019822027598 }, { - "x": 3.9052468507649114, - "y": 1.8717029681737345, - "heading": -0.23110382341189709, - "angularVelocity": 0.8474523924995162, - "velocityX": 3.2782210596577848, - "velocityY": -1.6489826656448716, + "x": 3.9103619312555455, + "y": 1.8836924134378115, + "heading": -0.24100411082773987, + "angularVelocity": 0.8433833320697718, + "velocityX": 3.290472495474877, + "velocityY": -1.6844811566746316, "moduleForcesX": [ - 47.55707312914185, - 49.53638736807368, - 48.78953373366416, - 50.77045950153568 + 48.26509838932949, + 50.40336737122413, + 49.6190591353058, + 51.75496442543449 ], "moduleForcesY": [ - 51.17696939290848, - 49.268536466648314, - 49.99446973782349, - 47.98722716731246 + 50.492121778115546, + 48.363822792220766, + 49.151347533828364, + 46.9041391127918 ], - "timestamp": 7.2650336462805445 + "timestamp": 7.260698355042996 }, { - "x": 4.120080601641824, - "y": 1.7801799153533593, - "heading": -0.17706327947410902, - "angularVelocity": 0.8588854781668361, - "velocityX": 3.4144287862962694, - "velocityY": -1.454608248862383, + "x": 4.1251309979342885, + "y": 1.7902287074795913, + "heading": -0.18733070094754622, + "angularVelocity": 0.8563284317296923, + "velocityX": 3.426517123908772, + "velocityY": -1.491159755370459, "moduleForcesX": [ - 39.05701336414032, - 40.46649013860068, - 39.71798936608046, - 41.14398002340263 + 39.02721416882864, + 40.63159693937693, + 39.76181294529839, + 41.38836138220273 ], "moduleForcesY": [ - 57.94278647291951, - 56.96920228244361, - 57.48746696114792, - 56.477668387844396 + 57.948757981015774, + 56.837566924459644, + 57.44189927750841, + 56.28368962505613 ], - "timestamp": 7.3279530287960455 + "timestamp": 7.3233768880583945 }, { - "x": 4.341754671827057, - "y": 1.7019368196307447, - "heading": -0.12271990769558694, - "angularVelocity": 0.8636984281454757, - "velocityX": 3.523144400386022, - "velocityY": -1.243545193777744, + "x": 4.346552661405609, + "y": 1.7100097822136517, + "heading": -0.13328440238249356, + "angularVelocity": 0.862277656558669, + "velocityX": 3.5326554853624956, + "velocityY": -1.279846885475643, "moduleForcesX": [ - 31.542569411645978, - 32.211867496766956, - 31.792298515347188, - 32.46659837801001 + 30.796111969393834, + 31.63647803362233, + 31.08882111063717, + 31.937435866659744 ], "moduleForcesY": [ - 62.36899517370198, - 62.02650340460398, - 62.24055631529311, - 61.8920479920422 + 62.72964371688498, + 62.31072023606725, + 62.583051774162115, + 62.15497397097186 ], - "timestamp": 7.3908724113115465 + "timestamp": 7.3860554210737925 }, { - "x": 4.568810602625837, - "y": 1.6376320022464737, - "heading": -0.06839919997587875, - "angularVelocity": 0.8633382202427946, - "velocityX": 3.6086802146038512, - "velocityY": -1.0220192063777647, + "x": 4.5730514606078865, + "y": 1.6437195895696548, + "heading": -0.07920610917804985, + "angularVelocity": 0.8627881126566634, + "velocityX": 3.613658270314562, + "velocityY": -1.0576219553146122, "moduleForcesX": [ - 25.215137722620277, - 25.161326804862117, - 25.198140713892162, - 25.144365342135142 + 23.887085713336567, + 23.96483684337957, + 23.909031078260046, + 23.986859531626518 ], "moduleForcesY": [ - 65.19836347228741, - 65.21911954217529, - 65.20503178610034, - 65.2257580831436 + 65.68742061578509, + 65.65913506231513, + 65.67928603196187, + 65.6509428796385 ], - "timestamp": 7.4537917938270475 + "timestamp": 7.448733954089191 }, { - "x": 4.800045847269579, - "y": 1.5876743304432248, - "heading": -0.014355888196568429, - "angularVelocity": 0.8589294684510949, - "velocityX": 3.675103527705061, - "velocityY": -0.793994947279417, + "x": 4.803338854745861, + "y": 1.5917650272197101, + "heading": -0.025361992429283036, + "angularVelocity": 0.8590519617863268, + "velocityX": 3.6741031268456674, + "velocityY": -0.8289052064632888, "moduleForcesX": [ - 19.993448516674185, - 19.3060989973268, - 19.797835730589334, - 19.116494526678093 + 18.23081795339054, + 17.63640682889574, + 18.085112159973114, + 17.495364929313684 ], "moduleForcesY": [ - 66.99591754938321, - 67.196927406319, - 67.05502467711538, - 67.25215990996023 + 67.48937858084152, + 67.6469186921904, + 67.52952569073541, + 67.68449338706219 ], - "timestamp": 7.5167111763425485 + "timestamp": 7.511412487104589 }, { - "x": 5.034473115872903, - "y": 1.5523169276578892, - "heading": 0.039207016322208126, - "angularVelocity": 0.8512941859462895, - "velocityX": 3.725835493467699, - "velocityY": -0.5619477078724477, + "x": 5.0363666951223784, + "y": 1.554384768665583, + "heading": 0.028035090410861878, + "angularVelocity": 0.8519197924914251, + "velocityX": 3.7178253728317188, + "velocityY": -0.596380558953801, "moduleForcesX": [ - 15.705471617855421, - 14.48469807799971, - 15.374313004019221, - 14.172732804713208 + 13.629985184247289, + 12.466510567267608, + 13.36498947078768, + 12.219570464535467 ], "moduleForcesY": [ - 68.13853193873344, - 68.40802253366346, - 68.21561194224486, - 68.47494827858137 + 68.57759449105092, + 68.79825994640873, + 68.63134312576102, + 68.84417352906047 ], - "timestamp": 7.5796305588580495 + "timestamp": 7.574091020119987 }, { "x": 5.271278381347656, "y": 1.5317155122756958, - "heading": 0.09212323355733296, - "angularVelocity": 0.8410161562232161, - "velocityX": 3.7636298388086282, - "velocityY": -0.3274255810936793, + "heading": 0.08080945742074001, + "angularVelocity": 0.8419847190251427, + "velocityX": 3.7478810515167584, + "velocityY": -0.3616749674775882, "moduleForcesX": [ - 12.17583308633574, - 10.508988015592593, - 11.72461754628835, - 10.093645719719404 + 9.878633404360912, + 8.235261290860429, + 9.510066456350259, + 7.902786589676527 ], "moduleForcesY": [ - 68.86508793588061, - 69.1385750114367, - 68.94526551565065, - 69.20238200518273 + 69.22664704724305, + 69.44088567639092, + 69.28024531566139, + 69.48153670734771 ], - "timestamp": 7.6425499413735505 + "timestamp": 7.636769553135385 }, { - "x": 5.388681610311286, - "y": 1.5251584282304298, - "heading": 0.11807334458511697, - "angularVelocity": 0.8348404097837044, - "velocityX": 3.7769765097724286, - "velocityY": -0.21094779615682893, + "x": 5.391042214421232, + "y": 1.5240070453835066, + "heading": 0.1074471532788096, + "angularVelocity": 0.8357210579005186, + "velocityX": 3.757425484837848, + "velocityY": -0.24184254299832358, "moduleForcesX": [ - 9.224601000293218, - 7.170145322617407, - 8.707960432775863, - 6.708850729084163 + 6.772943150194192, + 4.71192047105511, + 6.351957415318363, + 4.348339414191522 ], "moduleForcesY": [ - 69.25444608627201, - 69.49615053644614, - 69.32560078879362, - 69.54657168705602 + 69.53223448731855, + 69.70137764950671, + 69.57638151835509, + 69.72949369202985 ], - "timestamp": 7.673633859597523 + "timestamp": 7.668643460830122 }, { - "x": 5.506442673711366, - "y": 1.5222273963184734, - "heading": 0.14379764901773748, - "angularVelocity": 0.8275759911368509, - "velocityX": 3.7884883929871296, - "velocityY": -0.0942941585046398, + "x": 5.51102563137661, + "y": 1.5201232868827126, + "heading": 0.13384807268108104, + "angularVelocity": 0.8282925223702723, + "velocityX": 3.764314627013447, + "velocityY": -0.12184757946812044, "moduleForcesX": [ - 8.369389483724206, - 5.949973696893992, - 7.73117703857559, - 5.38783266312984 + 5.467336768701578, + 3.0155789586234376, + 4.950575022268367, + 2.579688806707375 ], "moduleForcesY": [ - 69.35385904945188, - 69.60180730909208, - 69.43322409042021, - 69.65302052985173 + 69.63848507273565, + 69.78654461331922, + 69.68263747119076, + 69.80962731619984 ], - "timestamp": 7.704717777821495 + "timestamp": 7.700517368524859 }, { - "x": 5.624497338635267, - "y": 1.522927459386341, - "heading": 0.16925769890783504, - "angularVelocity": 0.8190746644823769, - "velocityX": 3.7979338406848053, - "velocityY": 0.02252171244382533, + "x": 5.631134254008104, + "y": 1.5200677662545983, + "heading": 0.15997015664506697, + "angularVelocity": 0.819544444131609, + "velocityX": 3.7682427828365683, + "velocityY": -0.0017418833186710159, "moduleForcesX": [ - 7.409211713616261, - 4.57253345050813, - 6.632095741425086, - 3.899218959272906 + 4.014268392430795, + 1.116698120742873, + 3.3923076891931627, + 0.60736396962066 ], "moduleForcesY": [ - 69.45271134204867, - 69.69519778275762, - 69.53780580291625, - 69.74287628352383 + 69.72782501035493, + 69.83295042566183, + 69.76768134726464, + 69.84622551429848 ], - "timestamp": 7.735801696045467 + "timestamp": 7.732391276219596 }, { - "x": 5.742772793309857, - "y": 1.5272629148189605, - "heading": 0.19440986357494344, - "angularVelocity": 0.8091696962357507, - "velocityX": 3.8050368625463413, - "velocityY": 0.13947583446143447, + "x": 5.751262381021374, + "y": 1.523841630833003, + "heading": 0.1857658669581749, + "angularVelocity": 0.8093049198786131, + "velocityX": 3.7688547059795785, + "velocityY": 0.11839980885141156, "moduleForcesX": [ - 6.320859466741637, - 3.0067323445032117, - 5.3877760442274845, - 2.2145608793254503 + 2.3863359481069573, + -1.019059720883274, + 1.6525549624055083, + -1.5974719300993867 ], "moduleForcesY": [ - 69.54854425346966, - 69.76816296129176, - 69.63518331243104, - 69.80621969634359 + 69.79190277644955, + 69.8235099520272, + 69.82158137742265, + 69.82135764239271 ], - "timestamp": 7.766885614269439 + "timestamp": 7.7642651839143335 }, { - "x": 5.861185901308817, - "y": 1.5352368348891807, - "heading": 0.21920461001465913, - "angularVelocity": 0.7976712028728045, - "velocityX": 3.8094653043977926, - "velocityY": 0.2565287944964038, + "x": 5.871291096439659, + "y": 1.5314426318940912, + "heading": 0.21118151569712723, + "angularVelocity": 0.7973810108996604, + "velocityX": 3.7657358039629982, + "velocityY": 0.23847095040510002, "moduleForcesX": [ - 5.0743871573767105, - 1.212540305184361, - 3.969650176486514, - 0.29853628468642324 + 0.549891385422086, + -3.4334671839024553, + -0.29767021184777925, + -4.068355861282044 ], "moduleForcesY": [ - 69.63726174457908, - 69.80849842797434, - 69.71904021769083, - 69.82888852399873 + 69.81865312665636, + 69.73422687541164, + 69.83062468569385, + 69.71085861910606 ], - "timestamp": 7.797969532493411 + "timestamp": 7.796139091609071 }, { - "x": 5.979640967451708, - "y": 1.5468503445815898, - "heading": 0.24358557158644084, - "angularVelocity": 0.7843593396465376, - "velocityX": 3.810815138856516, - "velocityY": 0.3736179463840169, + "x": 5.991086014880691, + "y": 1.5428637105440566, + "heading": 0.23615643982419934, + "angularVelocity": 0.7835538825757309, + "velocityX": 3.758400745472756, + "velocityY": 0.3583206288776096, "moduleForcesX": [ - 3.6307956438747206, - -0.8621551207472431, - 2.3418002525812245, - -1.893133883598176 + -1.5364034730888085, + -6.177058299089292, + -2.492386077354901, + -6.843823309273452 ], "moduleForcesY": [ - 69.7120826368421, - 69.79764527340737, - 69.77979201753804, - 69.79043163606336 + 69.79055227447756, + 69.53111319438398, + 69.77586096014748, + 69.48206624406794 ], - "timestamp": 7.8290534507173835 + "timestamp": 7.828012999303808 }, { - "x": 6.098026846695146, - "y": 1.5621015192437462, - "heading": 0.2674883023191033, - "angularVelocity": 0.7689741866013711, - "velocityX": 3.808589328746145, - "velocityY": 0.4906451803233273, + "x": 6.110494620280023, + "y": 1.558091016928009, + "heading": 0.260621989902633, + "angularVelocity": 0.7675729726252842, + "velocityX": 3.7462807053007396, + "velocityY": 0.47773578720836973, "moduleForcesX": [ - 1.9386808198061933, - -3.28598819102792, - 0.4585755906663063, - -4.416447800806846 + -3.9233308193331884, + -9.310207032464307, + -4.971223982333152, + -9.967160671911408 ], "moduleForcesY": [ - 69.76178277329687, - 69.70685348700545, - 69.80282869693546, - 69.6609062566512 + 69.68200654002352, + 69.16570334527854, + 69.63108366349697, + 69.09079688113802 ], - "timestamp": 7.860137368941356 + "timestamp": 7.859886906998545 }, { - "x": 6.216213161429717, - "y": 1.5809836737297318, - "heading": 0.2908385525285553, - "angularVelocity": 0.7512003487206494, - "velocityX": 3.8021691436385594, - "velocityY": 0.6074573465909969, + "x": 6.229343182975531, + "y": 1.5771011290944752, + "heading": 0.2845003140637755, + "angularVelocity": 0.7491495674088557, + "velocityX": 3.728710136006611, + "velocityY": 0.5964161140369018, "moduleForcesX": [ - -0.07065192727705566, - -6.150375646776657, - -1.738667342074545, - -7.342704323857981 + -6.673094685929394, + -12.903296056772389, + -7.779731069608852, + -13.485056064651221 ], "moduleForcesY": [ - 69.76766630955797, - 69.49064187075653, - 69.76568210247977, - 69.39577088637856 + 69.45541432263478, + 68.56862083714847, + 69.36005488296091, + 68.47745191194402 ], - "timestamp": 7.891221287165328 + "timestamp": 7.891760814693282 }, { - "x": 6.334045309853754, - "y": 1.6034826557006305, - "heading": 0.313549807262191, - "angularVelocity": 0.7306432403402956, - "velocityX": 3.790775267616168, - "velocityY": 0.7238142182972129, + "x": 6.347433309831943, + "y": 1.5998571523743064, + "heading": 0.30770295299816985, + "angularVelocity": 0.727950873065531, + "velocityX": 3.7049152550538524, + "velocityY": 0.7139389213826525, "moduleForcesX": [ - -2.490383343681562, - -9.578064211771972, - -4.3259113321238125, - -10.762745909847968 + -9.861215297744039, + -17.03459334936717, + -10.968990403175749, + -17.444220781953867 ], "moduleForcesY": [ - 69.69820129934999, - 69.07536900988754, - 69.63337299442767, - 68.9276312183353 + 69.05521867685782, + 67.64066622247339, + 68.91276556625236, + 67.56234354492842 ], - "timestamp": 7.9223052053893 + "timestamp": 7.9236347223880195 }, { - "x": 6.451337870714877, - "y": 1.6295724757424068, - "heading": 0.33551971883232135, - "angularVelocity": 0.706793506913393, - "velocityX": 3.7734162088570447, - "velocityY": 0.8393349851774983, + "x": 6.464538328080527, + "y": 1.626303286615654, + "heading": 0.33012934372254554, + "angularVelocity": 0.7035971534823224, + "velocityX": 3.674008827851357, + "velocityY": 0.8297110757371571, "moduleForcesX": [ - -5.448866729736701, - -13.733572282788284, - -7.401927508185858, - -14.790647300726173 + -13.57752083156378, + -21.783197519865176, + -14.593849038845681, + -21.884589883945374 ], "moduleForcesY": [ - 69.49922387521676, - 68.33891344443161, - 69.35066212276465, - 68.15293380811582 + 68.39900163770346, + 66.24134665518923, + 68.22058375084256, + 66.24082954639027 ], - "timestamp": 7.953389123613272 + "timestamp": 7.955508630082757 }, { - "x": 6.567866022044423, - "y": 1.659208124359565, - "heading": 0.35662502542728386, - "angularVelocity": 0.6789783206509007, - "velocityX": 3.7488244078469917, - "velocityY": 0.9534077526398532, + "x": 6.580399977943102, + "y": 1.656357384692337, + "heading": 0.3516655018085619, + "angularVelocity": 0.6756673292861429, + "velocityX": 3.6349998554367304, + "velocityY": 0.9429059770303924, "moduleForcesX": [ - -9.125353794114547, - -18.83172539656305, - -11.094164589642684, - -19.562885061791146 + -17.92431111597879, + -27.21254535403587, + -18.708578792359393, + -26.82734571095702 ], "moduleForcesY": [ - 69.0754447164273, - 67.07463735744341, - 68.82909121269832, - 66.91127860350836 + 67.36450849006661, + 64.17638992872675, + 67.19039419369011, + 64.37988045730229 ], - "timestamp": 7.984473041837244 + "timestamp": 7.987382537777494 }, { - "x": 6.683354928219009, - "y": 1.6923136502791418, - "heading": 0.37671482456009425, - "angularVelocity": 0.6463084540390093, - "velocityX": 3.7153908764796526, - "velocityY": 1.0650370934911797, + "x": 6.694726351538369, + "y": 1.6899010876153, + "heading": 0.37218347560916276, + "angularVelocity": 0.6437231982066761, + "velocityX": 3.586832674869755, + "velocityY": 1.0523875278869872, "moduleForcesX": [ - -13.771593248917489, - -25.130414558467773, - -15.562652582080137, - -25.223578565092154 + -23.00758940984267, + -33.338147011171934, + -23.358188320319844, + -32.256271677852126 ], "moduleForcesY": [ - 68.25443383699609, - 64.93026638806907, - 67.92556316929904, - 64.95632378013195 + 65.77194757070217, + 61.18992795027423, + 65.69849366449613, + 61.819461145008376 ], - "timestamp": 8.015556960061216 + "timestamp": 8.019256445472232 }, { - "x": 6.7974682626271505, - "y": 1.7287625418402501, - "heading": 0.3956032132276822, - "angularVelocity": 0.6076579063002795, - "velocityX": 3.671137389627324, - "velocityY": 1.1725964306841732, + "x": 6.807192685734502, + "y": 1.7267675051619156, + "heading": 0.3915426081860902, + "angularVelocity": 0.6073661492131278, + "velocityX": 3.5284764978691405, + "velocityY": 1.1566331276256518, "moduleForcesX": [ - -19.7338671115633, - -32.869545661536705, - -20.995212284811174, - -31.878789101972526 + -28.91315705633447, + -40.074653997125445, + -28.56271316758345, + -38.09306052597789 ], "moduleForcesY": [ - 66.71548826157209, - 61.31804473588162, - 66.4083595377785, - 61.9239118979002 + 63.36360268554179, + 56.97490897615298, + 63.58659281516764, + 58.38423738606962 ], - "timestamp": 8.046640878285189 + "timestamp": 8.05113035316697 }, { - "x": 6.909800194013484, - "y": 1.7683468561362488, - "heading": 0.41306566672219286, - "angularVelocity": 0.5617841794810646, - "velocityX": 3.6138279150310604, - "velocityY": 1.2734660415324053, + "x": 6.917447317500844, + "y": 1.7667274108508237, + "heading": 0.4095941832685751, + "angularVelocity": 0.5663433318364425, + "velocityX": 3.4590873771196318, + "velocityY": 1.2536870618943754, "moduleForcesX": [ - -27.444972321937637, - -42.075345805688336, - -27.57805811433052, - -39.49775701748281 + -35.656308905451695, + -47.1677975691104, + -34.292967625798184, + -44.171577156013576 ], "moduleForcesY": [ - 63.85363325543449, - 55.33672689274484, - 63.908626512967174, - 57.321971693817275 + 59.79015829574416, + 51.22561496941096, + 60.66465034630196, + 53.91255102553536 ], - "timestamp": 8.07772479650916 + "timestamp": 8.083004260861708 }, { - "x": 7.019881390468804, - "y": 1.810734119873736, - "heading": 0.42885115065317647, - "angularVelocity": 0.5078344312078966, - "velocityX": 3.5414195746540194, - "velocityY": 1.363639661900725, + "x": 7.025125269774396, + "y": 1.809476738261155, + "heading": 0.42619090648469543, + "angularVelocity": 0.5206993561966173, + "velocityX": 3.378247603174457, + "velocityY": 1.3412013305600805, "moduleForcesX": [ - -37.27804152080681, - -52.1405235304012, - -35.411432044620646, - -47.75401943748761 + -43.0950713666489, + -54.14575035838762, + -40.439391320133275, + -50.224432044019366 ], "moduleForcesY": [ - 58.55809505279019, - 45.896730636123046, - 59.86837524976089, - 50.604050638682544 + 54.6293050128275, + 43.75538755080409, + 56.72805998056306, + 48.30614376570568 ], - "timestamp": 8.108808714733133 + "timestamp": 8.114878168556446 }, { - "x": 7.127216489228573, - "y": 1.8554226010135964, - "heading": 0.44272434085459006, - "angularVelocity": 0.4463140747396045, - "velocityX": 3.4530749304632886, - "velocityY": 1.4376720726731453, + "x": 7.1298705644585345, + "y": 1.8546303708147, + "heading": 0.44120012056964497, + "angularVelocity": 0.4708934413908578, + "velocityX": 3.2862395062225853, + "velocityY": 1.4166330964496041, "moduleForcesX": [ - -48.97728499858189, - -61.38705872437014, - -44.32215534808062, - -55.881419683225104 + -50.81886431339406, + -60.36189206611623, + -46.781345213429454, + -55.90228668204932 ], "moduleForcesY": [ - 49.06528866999657, - 32.434718317404084, - 53.54352333978887, - 41.41142967075846 + 47.484904027140054, + 34.65982136246882, + 51.598757458793386, + 41.59074319154849 ], - "timestamp": 8.139892632957105 + "timestamp": 8.146752076251184 }, { "x": 7.231364727020264, "y": 1.90172815322876, "heading": 0.45451541638849, - "angularVelocity": 0.3793304128823317, - "velocityX": 3.3505505014284465, - "velocityY": 1.489694828094504, - "moduleForcesX": [ - -60.49418247701689, - -67.53589230331421, - -53.57894617228146, - -62.756167691165814 - ], - "moduleForcesY": [ - 33.72227739407681, - 16.098766492768572, - 44.212603887497394, - 29.961682528729266 - ], - "timestamp": 8.170976551181077 - }, - { - "x": 7.4238027530368695, - "y": 1.9965622542854489, - "heading": 0.4692934758689833, - "angularVelocity": 0.24023905191549066, - "velocityX": 3.1283626232349375, - "velocityY": 1.5416675347118225, + "angularVelocity": 0.4177490863802491, + "velocityX": 3.184239708973158, + "velocityY": 1.4776281234520827, "moduleForcesX": [ - -68.24273916485633, - -69.7323377330834, - -62.01731512678528, - -67.61265076621554 + -58.09323113814453, + -65.1805058005262, + -52.97746872707234, + -60.837974152120616 ], "moduleForcesY": [ - 13.81064712566369, - -0.3213659067003291, - 31.862172293048193, - 17.244933344769986 + 38.2048879415566, + 24.420633432131687, + 45.192733096216344, + 33.9590952401435 ], - "timestamp": 8.232490528041474 + "timestamp": 8.178625983945922 }, { - "x": 7.602133310590925, - "y": 2.090716538878092, - "heading": 0.47717310494630966, - "angularVelocity": 0.12809493841064415, - "velocityX": 2.899025012783137, - "velocityY": 1.5306161200782256, + "x": 7.424008457970693, + "y": 2.003914192060848, + "heading": 0.4743314475792421, + "angularVelocity": 0.30437239580938835, + "velocityX": 2.9589897877434668, + "velocityY": 1.569568051149872, "moduleForcesX": [ - -69.13754121748958, - -68.20178675329157, - -69.06817284359947, - -69.80874265266795 + -64.18282412471406, + -68.43086071098912, + -58.78460066435345, + -64.93261887331047 ], "moduleForcesY": [ - -9.046985055914966, - -14.8227009169618, - 9.835976008332125, - 0.7232885771622478 + 27.36299974626911, + 13.845305324141684, + 37.678868187130924, + 25.739033429406035 ], - "timestamp": 8.29400450490187 + "timestamp": 8.243730544883707 }, { - "x": 7.76664139791012, - "y": 2.1816852539691975, - "heading": 0.47965086497918497, - "angularVelocity": 0.04027962683177553, - "velocityX": 2.6743204669166074, - "velocityY": 1.4788300112274317, + "x": 7.60087719793961, + "y": 2.107850851805076, + "heading": 0.48752484756484954, + "angularVelocity": 0.20264939653329472, + "velocityX": 2.716687393651782, + "velocityY": 1.596457425518163, "moduleForcesX": [ - -66.40756278498199, - -65.73447233946021, - -69.52981390692969, - -68.96429053135994 + -69.61888985652718, + -69.76727726868289, + -67.16418021608261, + -69.18600572947932 ], "moduleForcesY": [ - -21.48510284383042, - -23.5840541324825, - -6.248641320167586, - -11.053848562321154 + 5.029388418504633, + -3.222941021531897, + 19.11676020734826, + 9.676481897144177 ], - "timestamp": 8.355518481762267 + "timestamp": 8.308835105821492 }, { - "x": 7.917760773522142, - "y": 2.267858010889966, - "heading": 0.4778609500693658, - "angularVelocity": -0.029097694559421974, - "velocityX": 2.456667302700648, - "velocityY": 1.4008646704200718, + "x": 7.761930148007023, + "y": 2.210081079106282, + "heading": 0.4950554503391814, + "angularVelocity": 0.11566935811959787, + "velocityX": 2.473758331944146, + "velocityY": 1.5702467819251287, "moduleForcesX": [ - -63.7079413269904, - -63.48861495951544, - -67.77169795111662, - -67.17513898955453 + -68.82435473996972, + -68.04450224754622, + -69.8239032049172, + -69.75673087129077 ], "moduleForcesY": [ - -28.621263222946787, - -29.166159121005602, - -16.89631000921434, - -19.21841268100821 + -11.841731474215324, + -15.852074510798255, + 1.93806565534906, + -4.071564113915693 ], - "timestamp": 8.417032458622664 + "timestamp": 8.373939666759277 }, { - "x": 8.055903927968282, - "y": 2.3481558395536353, - "heading": 0.47263055774144574, - "angularVelocity": -0.08502770581375621, - "velocityX": 2.245719777793735, - "velocityY": 1.3053590868608649, + "x": 7.907620343769395, + "y": 2.308071507252318, + "heading": 0.4978684590059931, + "angularVelocity": 0.04320755145710578, + "velocityX": 2.237787854856954, + "velocityY": 1.5051238612864044, "moduleForcesX": [ - -61.53663246666227, - -61.636381591712215, - -65.62140779995624, - -65.27264016289558 + -65.92623401375947, + -65.36976574658864, + -68.92142423550537, + -68.31330607927826 ], "moduleForcesY": [ - -33.09645471160549, - -32.94683673534328, - -24.002173782199, - -24.982303085352363 + -23.130100041160027, + -24.720666810010734, + -11.488057316014388, + -14.770045889808568 ], - "timestamp": 8.47854643548306 + "timestamp": 8.439044227697062 }, { - "x": 8.181425593296224, - "y": 2.421820395971198, - "heading": 0.4645721903392223, - "angularVelocity": -0.13100059228021374, - "velocityX": 2.0405389430894125, - "velocityY": 1.197525508466805, + "x": 8.038541023495547, + "y": 2.400018073701688, + "heading": 0.49676667390448254, + "angularVelocity": -0.01692331667152227, + "velocityX": 2.0109294623960547, + "velocityY": 1.412290707823597, "moduleForcesX": [ - -59.837069189363575, - -60.1347503268217, - -63.62558289863267, - -63.52420699677533 + -62.824360850795905, + -62.67121951693069, + -66.58034896128022, + -66.08539026982844 ], "moduleForcesY": [ - -36.12028649557102, - -35.64612522470202, - -28.92915290640471, - -29.180149543959885 + -30.62126190401479, + -30.968161395864133, + -21.258041017807635, + -22.795219386908013 ], - "timestamp": 8.540060412343458 + "timestamp": 8.504148788634847 }, { - "x": 8.294623187946847, - "y": 2.488295400127025, - "heading": 0.4541517988040101, - "angularVelocity": -0.16939876215222985, - "velocityX": 1.8401930817693604, - "velocityY": 1.0806487817669281, + "x": 8.155275443871254, + "y": 2.4846239308491906, + "heading": 0.49239437013062565, + "angularVelocity": -0.06715817925621288, + "velocityX": 1.7930298383742893, + "velocityY": 1.2995380957772373, "moduleForcesX": [ - -58.495272320583396, - -58.9108341498489, - -61.90125817659251, - -61.99096011956313 + -60.073321011059726, + -60.249556015844455, + -63.930532840470896, + -63.71297068980246 ], "moduleForcesY": [ - -38.28430915682556, - -37.65794710311188, - -32.488649405293295, - -32.336456191223455 + -35.75278795396041, + -35.47671823816927, + -28.283924388405126, + -28.797286948333827 ], - "timestamp": 8.601574389203854 + "timestamp": 8.569253349572632 }, { - "x": 8.395745589032531, - "y": 2.5471579028047797, - "heading": 0.4417334392214656, - "angularVelocity": -0.20187866589616352, - "velocityX": 1.6438930832772425, - "velocityY": 0.9568963946411755, + "x": 8.258347693307394, + "y": 2.5609347268737315, + "heading": 0.4852629357835075, + "angularVelocity": -0.10953816820810822, + "velocityX": 1.5831801635930889, + "velocityY": 1.172126728532337, "moduleForcesX": [ - -57.41823039591145, - -57.901215370721474, - -60.440306372625834, - -60.66569970555321 + -57.756771897763436, + -58.16002609551108, + -61.421389264955856, + -61.46747635960799 ], "moduleForcesY": [ - -39.90297584100897, - -39.210477609648265, - -35.155958840161816, - -34.77905570626861 + -39.40997001157865, + -38.82696454641265, + -33.4114065154027, + -33.34381170977727 ], - "timestamp": 8.663088366064251 + "timestamp": 8.634357910510417 }, { - "x": 8.48500235236776, - "y": 2.598076660395606, - "heading": 0.4276083189600344, - "angularVelocity": -0.2296245663564786, - "velocityX": 1.4509997221898334, - "velocityY": 0.8277591563683715, + "x": 8.348213779476872, + "y": 2.6282292552805555, + "heading": 0.47578115701955753, + "angularVelocity": -0.14563923982239507, + "velocityX": 1.380334724250048, + "velocityY": 1.0336376966143848, "moduleForcesX": [ - -56.53862920087126, - -57.05702405798682, - -59.20466714974995, - -59.52214725960545 + -55.82852902081254, + -56.37707964117926, + -59.18899393513469, + -59.44035011210898 ], "moduleForcesY": [ - -41.15627446217204, - -40.443127482255576, - -37.21736934315343, - -36.717258988355645 + -42.11500222204666, + -41.38806414610841, + -37.242958929245574, + -36.85233746127893 ], - "timestamp": 8.724602342924648 + "timestamp": 8.699462471448202 }, { - "x": 8.562571509547876, - "y": 2.640785806675407, - "heading": 0.41201408826118213, - "angularVelocity": -0.2535071132572452, - "velocityX": 1.2610005260455623, - "velocityY": 0.6942998723156388, + "x": 8.425266606479534, + "y": 2.685948485476612, + "heading": 0.4642800962886668, + "angularVelocity": -0.1766552230016766, + "velocityX": 1.183524255332804, + "velocityY": 0.8865620067880233, "moduleForcesX": [ - -55.80869308264709, - -56.34173719772094, - -58.15451187186907, - -58.53176769808956 + -54.21921582870304, + -54.855206644706676, + -57.24745392477798, + -57.645373900028495 ], "moduleForcesY": [ - -42.15377542537939, - -41.44512997463475, - -38.852027537364954, - -38.28860744687602 + -44.181208993738856, + -43.396520697801726, + -40.17831839961693, + -39.613797717067015 ], - "timestamp": 8.786116319785044 + "timestamp": 8.764567032385987 }, { - "x": 8.62860576010961, - "y": 2.6750675306808085, - "heading": 0.3951480099492571, - "angularVelocity": -0.2741828633548061, - "velocityX": 1.0734836850427496, - "velocityY": 0.557299751293951, + "x": 8.489844645461925, + "y": 2.7336490829588826, + "heading": 0.4510317174084265, + "angularVelocity": -0.20349386724073787, + "velocityX": 0.9919126717420388, + "velocityY": 0.7326767402341282, "moduleForcesX": [ - -55.19425623129184, - -55.728131941039244, - -57.25555323504394, - -57.66899798719165 + -52.865528585952326, + -53.5493743197174, + -55.569733234873816, + -56.06636357612068 ], "moduleForcesY": [ - -42.96566364835716, - -42.27592065616304, - -40.176385790672825, - -39.58618560604843 + -45.80304315675028, + -45.00724101650629, + -42.480063091401604, + -41.828690869314826 ], - "timestamp": 8.847630296645441 + "timestamp": 8.829671593323772 }, { - "x": 8.683237297728128, - "y": 2.7007402779978236, - "heading": 0.3771761995999053, - "angularVelocity": -0.2921581609028107, - "velocityX": 0.8881158462978955, - "velocityY": 0.41734819674036067, + "x": 8.542240663164481, + "y": 2.7709723355879694, + "heading": 0.43626250509958603, + "angularVelocity": -0.2268537272366222, + "velocityX": 0.8047979580513017, + "velocityY": 0.5732816885863476, "moduleForcesX": [ - -54.67049432361609, - -55.195762968597755, - -56.480010214696975, - -56.91239458884492 + -51.71605380731337, + -52.42081923342628, + -54.11889003994841, + -54.677868682944876 ], "moduleForcesY": [ - -43.638836888514874, - -42.97645664054756, - -41.268832050266276, - -40.67476273388533 + -47.105665338095726, + -46.3243865448859, + -44.322863344470825, + -43.63617258426661 ], - "timestamp": 8.909144273505838 + "timestamp": 8.894776154261557 }, { - "x": 8.726581564114948, - "y": 2.717650467677025, - "heading": 0.35824027480182863, - "angularVelocity": -0.30783125664352257, - "velocityX": 0.7046246820488491, - "velocityY": 0.2748999583879684, + "x": 8.582709383241658, + "y": 2.797622875392326, + "heading": 0.4201634326355504, + "angularVelocity": -0.247280255517275, + "velocityX": 0.6215957759986721, + "velocityY": 0.40934981237065615, "moduleForcesX": [ - -54.21904272949312, - -54.729099389545915, - -55.805815985531794, - -56.244420654225245 + -50.730544843076125, + -51.437804788630906, + -52.85923893372407, + -53.45364309208686 ], "moduleForcesY": [ - -44.20576368971399, - -43.57579607050465, - -42.183797695943305, - -41.60055887427103 + -48.17246735463719, + -47.41986096115254, + -45.825351350532394, + -45.13424388858939 ], - "timestamp": 8.970658250366235 + "timestamp": 8.959880715199342 }, { - "x": 8.758740183629461, - "y": 2.7256665220403926, - "heading": 0.338462250167195, - "angularVelocity": -0.32152082573882035, - "velocityX": 0.5227855709523735, - "velocityY": 0.13031273171557645, + "x": 8.611473874971457, + "y": 2.813353753747135, + "heading": 0.40289733126075, + "angularVelocity": -0.2652057110299845, + "velocityX": 0.4418199173063615, + "velocityY": 0.24162482824885853, "moduleForcesX": [ - -53.82607120430835, - -54.316194914018055, - -55.21550784813393, - -55.65085738495609 + -49.87778962282822, + -50.57482599449837, + -51.759729184855516, + -52.369784201574845 ], "moduleForcesY": [ - -44.689609283680795, - -44.09507977862367, - -42.960129353038816, - -42.39731433858149 + -49.06077660106482, + -48.34454115681274, + -47.069973438706235, + -46.393130116240336 ], - "timestamp": 9.032172227226631 + "timestamp": 9.024985276137127 }, { - "x": 8.779803276062012, - "y": 2.7246744632720947, - "heading": 0.31794821481648144, - "angularVelocity": -0.33348576043569805, - "velocityX": 0.34241148934902593, - "velocityY": -0.016127371679270322, + "x": 8.628730773925781, + "y": 2.817955732345581, + "heading": 0.38460441295906894, + "angularVelocity": -0.2809775235127061, + "velocityX": 0.2650643627074838, + "velocityY": 0.07068596319762284, "moduleForcesX": [ - -53.48098782939392, - -53.947745103828254, - -54.695219377692204, - -55.12018458029143 + -49.13353520650032, + -49.81148698796487, + -50.794430820642766, + -51.405616940966546 ], "moduleForcesY": [ - -45.107332847724315, - -44.55002787941316, - -43.626228500899906, - -43.09016461444635 + -49.81111505036111, + -49.135249239699505, + -48.11534331383063, + -47.464060182990515 ], - "timestamp": 9.093686204087028 + "timestamp": 9.090089837074911 }, { - "x": 8.789390520920405, - "y": 2.711156481525927, - "heading": 0.29317696313872205, - "angularVelocity": -0.3456625720749201, - "velocityX": 0.13378216652004424, - "velocityY": -0.1886323872700164, + "x": 8.63409093131169, + "y": 2.809578261213375, + "heading": 0.3636328313393472, + "angularVelocity": -0.29609942384481297, + "velocityX": 0.07568048716897836, + "velocityY": -0.11828217921293742, "moduleForcesX": [ - -53.179448380447525, - -53.62389224585945, - -54.2363017447046, - -54.649105974722524 + -48.48056297546824, + -49.13563036489729, + -49.94272883080661, + -50.54684256882197 ], "moduleForcesY": [ - -45.47706110036372, - -44.95351001122035, - -44.210687438028955, - -43.700838292783004 + -50.45598549008582, + -49.81982384689785, + -49.00837234110654, + -48.38670375127015 ], - "timestamp": 9.165349304691366 + "timestamp": 9.160915985087659 }, { - "x": 8.783794368286188, - "y": 2.6855639318912203, - "heading": 0.267539420337893, - "angularVelocity": -0.35775095669356133, - "velocityX": -0.07808973637788985, - "velocityY": -0.35712311383241235, + "x": 8.625782813087357, + "y": 2.7880784954449576, + "heading": 0.34158319665982656, + "angularVelocity": -0.31132054048107033, + "velocityX": -0.11730298000729854, + "velocityY": -0.3035568977229701, "moduleForcesX": [ - -54.03803305894964, - -54.47570349158013, - -55.06023489691758, - -55.46707669488016 + -49.434220030426786, + -50.08516326654055, + -50.87601640496041, + -51.47573234890385 ], "moduleForcesY": [ - -44.44927461362763, - -43.91326452339743, - -43.17589421817186, - -42.65354228524133 + -49.51809039841881, + -48.8612826879258, + -48.03470968086272, + -47.3932817354508 ], - "timestamp": 9.237012405295705 + "timestamp": 9.231742133100406 }, { - "x": 8.76275609467282, - "y": 2.6482308380775303, - "heading": 0.2410424727336723, - "angularVelocity": -0.3697432483491591, - "velocityX": -0.2935719140806326, - "velocityY": -0.5209528125193889, + "x": 8.603519939908493, + "y": 2.7537628223496577, + "heading": 0.3184472650429824, + "angularVelocity": -0.3266580530778029, + "velocityX": -0.314331271762177, + "velocityY": -0.4845057095173895, "moduleForcesX": [ - -54.99526164627351, - -55.42219111728091, - -55.979540705697985, - -56.37649095420048 + -50.50767113082232, + -51.15037434057846, + -51.926551544815524, + -52.51764005400783 ], "moduleForcesY": [ - -43.254451016653434, - -42.70766979489076, - -41.972052319967815, - -41.439058648655944 + -48.418122856235094, + -47.74052141391343, + -46.89226762522613, + -46.231320487911496 ], - "timestamp": 9.308675505900043 + "timestamp": 9.302568281113153 }, { - "x": 8.725986587983398, - "y": 2.5995490914917334, - "heading": 0.21369371175484705, - "angularVelocity": -0.38162960782036737, - "velocityX": -0.5130884148096344, - "velocityY": -0.679313986909043, + "x": 8.566978749430204, + "y": 2.7069943004295456, + "heading": 0.294215364928076, + "angularVelocity": -0.3421321192075174, + "velocityX": -0.5159279659217504, + "velocityY": -0.6603284695321164, "moduleForcesX": [ - -56.06650028997491, - -56.47788343064675, - -57.008812567039705, - -57.391121229420555 + -51.72253309664589, + -52.351547871619616, + -53.1150303533149, + -53.69192145893697 ], "moduleForcesY": [ - -41.850827206177, - -41.29576254132913, - -40.5570050084547, - -40.01607334967796 + -47.11270598236726, + -46.414844244282484, + -45.535980106736204, + -44.856574967933696 ], - "timestamp": 9.380338606504381 + "timestamp": 9.3733944291259 }, { - "x": 8.673161820665543, - "y": 2.5399838352697053, - "heading": 0.1855016713653994, - "angularVelocity": -0.3933968827988571, - "velocityX": -0.7371264552102906, - "velocityY": -0.8311844689904736, + "x": 8.515792047639485, + "y": 2.648208808083207, + "heading": 0.26887617344655823, + "angularVelocity": -0.35776605381613247, + "velocityX": -0.7227091014678351, + "velocityY": -0.8299970278739272, "moduleForcesX": [ - -57.26911890755975, - -57.659157229516524, - -58.16431071641967, - -58.52626643425478 + -53.10459348185788, + -53.71272642624142, + -54.46576355767823, + -55.021155079771624 ], "moduleForcesY": [ - -40.18252449430719, - -39.62280351144781, - -38.874499091161304, - -38.32952205223615 + -45.542660989830466, + -44.826209580052634, + -43.90453987460494, + -43.209067443671216 ], - "timestamp": 9.45200170710872 + "timestamp": 9.444220577138648 }, { - "x": 8.603918051131696, - "y": 2.4700943215420597, - "heading": 0.156476193662526, - "angularVelocity": -0.4050268193547327, - "velocityX": -0.9662402121860534, - "velocityY": -0.9752510446556707, + "x": 8.449541508295152, + "y": 2.577937395823057, + "heading": 0.24241650450925817, + "angularVelocity": -0.3735861638633528, + "velocityX": -0.9353966183846341, + "velocityY": -0.9921676419209313, "moduleForcesX": [ - -58.62178890554708, - -58.98353550726006, - -59.46309693106811, - -59.79789639516721 + -54.68387284740476, + -55.261648531274275, + -56.00637251373348, + -56.530738642405 ], "moduleForcesY": [ - -38.17360914029891, - -37.61450756539028, - -36.848345847349954, - -36.304918084781264 + -43.62575228611848, + -42.8942919991783, + -41.91307663268656, + -41.20610218263478 ], - "timestamp": 9.523664807713057 + "timestamp": 9.515046725151395 }, { - "x": 8.517847357752716, - "y": 2.3905626117140066, - "heading": 0.12662900025486085, - "angularVelocity": -0.41649319044197497, - "velocityX": -1.2010461821096667, - "velocityY": -1.1098000108474018, + "x": 8.367749643224476, + "y": 2.496837777925061, + "heading": 0.2148211681213936, + "angularVelocity": -0.38962074265140273, + "velocityX": -1.1548258286749533, + "velocityY": -1.1450519359516806, "moduleForcesX": [ - -60.14239359593657, - -60.46764931194161, - -60.920705975153766, - -61.22039017159285 + -56.49342338399157, + -57.02842944478959, + -57.76596137264449, + -58.246993802956545 ], "moduleForcesY": [ - -35.71931021724835, - -35.16851680517709, - -34.373858499651185, - -33.84003668835228 + -41.245553252699736, + -40.505899357042395, + -39.44204855581045, + -38.731729530962944 ], - "timestamp": 9.595327908317396 + "timestamp": 9.585872873164142 }, { - "x": 8.414494950035653, - "y": 2.302233586652294, - "heading": 0.09597460073164957, - "angularVelocity": -0.4277570920697149, - "velocityX": -1.4421983816704484, - "velocityY": -1.2325593550492429, + "x": 8.269872586703466, + "y": 2.4057394247705677, + "heading": 0.18607303639793232, + "angularVelocity": -0.4058971514063899, + "velocityX": -1.381933922248534, + "velocityY": -1.2862248718947424, "moduleForcesX": [ - -61.84288712238317, - -62.122276292758905, - -62.54567545312701, - -62.801268264067 + -58.56488116310085, + -59.04110442112179, + -59.76960478030494, + -60.19174438403556 ], "moduleForcesY": [ - -32.67310530550032, - -32.14185463603263, - -31.305402017642134, - -30.792810837453125 + -38.23433183892145, + -37.4988143615417, + -36.32045167969364, + -35.620919583734725 ], - "timestamp": 9.666991008921734 + "timestamp": 9.65669902117689 }, { - "x": 8.2933614907194, - "y": 2.2061711843005387, - "heading": 0.06453176165426741, - "angularVelocity": -0.4387591216710293, - "velocityX": -1.690318424610812, - "velocityY": -1.340472314784823, + "x": 8.155297198594102, + "y": 2.3057088684340563, + "heading": 0.15615361511012685, + "angularVelocity": -0.4224346816435758, + "velocityX": -1.6176989900501801, + "velocityY": -1.4123393569068392, "moduleForcesX": [ - -63.717577250904, - -63.94109844594106, - -64.32744860294157, - -64.52959830151751 + -60.91614610648466, + -61.31371636368788, + -62.02428527901857, + -62.36884971040699 ], "moduleForcesY": [ - -28.828283743204985, - -28.332961805960878, - -27.43891241104092, - -26.96427075977155 + -34.34726778188899, + -33.637620843364225, + -32.301189412117985, + -31.636486262630484 ], - "timestamp": 9.738654109526072 + "timestamp": 9.727525169189637 }, { - "x": 8.153917386350976, - "y": 2.103737230084854, - "heading": 0.032325911066374, - "angularVelocity": -0.4494063237049454, - "velocityX": -1.9458285113605962, - "velocityY": -1.4293821136938516, + "x": 8.023350887750192, + "y": 2.1981442323035503, + "heading": 0.12504476412995907, + "angularVelocity": -0.4392283337866834, + "velocityX": -1.8629604255784487, + "velocityY": -1.518713626938277, "moduleForcesX": [ - -65.71756976574333, - -65.87625105005283, - -66.21063177131644, - -66.35137198001577 + -63.520716542602926, + -63.81730819571486, + -64.48569991401658, + -64.73298646364212 ], "moduleForcesY": [ - -23.893867912535846, - -23.458019215236998, - -22.489715553229402, - -22.076652060585157 + -29.22638447390868, + -28.58008989329779, + -27.0282766874693, + -26.43848196643227 ], - "timestamp": 9.81031721013041 + "timestamp": 9.798351317202384 }, { - "x": 7.995643377180051, - "y": 1.996698976059294, - "heading": -0.0006069291037716975, - "angularVelocity": -0.45955086917006693, - "velocityX": -2.2085844435447637, - "velocityY": -1.4936313545311521, + "x": 7.8733430094963675, + "y": 2.084908220562927, + "heading": 0.09273289280550447, + "angularVelocity": -0.4562138734219929, + "velocityX": -2.1179731280434493, + "velocityY": -1.5987882288931434, "moduleForcesX": [ - -67.69752144856352, - -67.78734757334065, - -68.04263214008894, - -68.11934773122928 + -66.23669578701445, + -66.41381020137457, + -66.98496208031126, + -67.12159504763625 ], "moduleForcesY": [ - -17.47107619907958, - -17.127002103641317, - -16.07246012795104, - -15.752714784412063 + -22.361245953920218, + -21.84052143654894, + -20.00571217853413, + -19.554879879038108 ], - "timestamp": 9.881980310734749 + "timestamp": 9.869177465215131 }, { - "x": 7.818124590832862, - "y": 1.8873638999769546, - "heading": -0.034213912395681174, - "angularVelocity": -0.468957985469511, - "velocityX": -2.4771295806372327, - "velocityY": -1.5256816291830964, + "x": 7.704675957081329, + "y": 1.9684978477709927, + "heading": 0.059218072302814256, + "angularVelocity": -0.47319840825818243, + "velocityX": -2.38142348761764, + "velocityY": -1.6436072842897478, "moduleForcesX": [ - -69.313687888199, - -69.3432482954395, - -69.47611214375348, - -69.49889577515765 + -68.65865518215234, + -68.7193569333156, + -69.08393459750226, + -69.12136156007266 ], "moduleForcesY": [ - -9.057792824867414, - -8.844692568248144, - -7.710678951592893, - -7.521600845904924 + -13.088029266778213, + -12.787167251577607, + -10.613744042624726, + -10.394212606212953 ], - "timestamp": 9.953643411339087 + "timestamp": 9.940003613227878 }, { - "x": 7.621240028444532, - "y": 1.7787112968796408, - "heading": -0.06841646549100246, - "angularVelocity": -0.4772686753278808, - "velocityX": -2.747363157998911, - "velocityY": -1.5161582764496822, + "x": 7.5170858672429315, + "y": 1.85220323216817, + "heading": 0.024531257518575638, + "angularVelocity": -0.4897458884534555, + "velocityX": -2.6485993535132915, + "velocityY": -1.6419728993576328, "moduleForcesX": [ - -69.86517241058907, - -69.86591053003337, - -69.82282638095482, - -69.8236070877981 + -69.87540008016852, + -69.87946038423385, + -69.8592396435103, + -69.86628903728995 ], "moduleForcesY": [ - 1.837312109452161, - 1.8863774422298984, - 3.0464717958262337, - 3.075434675486523 + -0.7650699301565221, + -0.7967154045035195, + 1.692280326436183, + 1.5791599402263883 ], - "timestamp": 10.025306511943425 + "timestamp": 10.010829761240625 }, { - "x": 7.40547828858971, - "y": 1.6744202309428289, - "heading": -0.10310081581402811, - "angularVelocity": -0.48399176187648607, - "velocityX": -3.0107787415740312, - "velocityY": -1.4552965899789445, + "x": 7.3110452541555135, + "y": 1.7400985444349457, + "heading": -0.011240894625415731, + "angularVelocity": -0.5050698526983756, + "velocityX": -2.909103754313112, + "velocityY": -1.5828149755235674, "moduleForcesX": [ - -68.17759957909404, - -68.20364257441437, - -67.95854331471489, - -67.98903555111266 + -68.32136154860933, + -68.4142317509721, + -67.82249433370446, + -67.94358861469844 ], "moduleForcesY": [ - 15.314544404106083, - 15.206518112264616, - 16.260066564096665, - 16.13994452069931 + 14.610999455735499, + 14.191657426650783, + 16.77917087513115, + 16.30055641195774 ], - "timestamp": 10.096969612547763 + "timestamp": 10.081655909253373 }, { - "x": 7.172300767498466, - "y": 1.5786130838476118, - "heading": -0.13811594024177143, - "angularVelocity": -0.48860744417223123, - "velocityX": -3.253801735130716, - "velocityY": -1.3369104362952573, + "x": 7.0881669627233315, + "y": 1.636620998376319, + "heading": -0.04793860084545987, + "angularVelocity": -0.5181378240906105, + "velocityX": -3.146836270018103, + "velocityY": -1.4610076781248036, "moduleForcesX": [ - -62.90664616032858, - -62.994759663175806, - -62.62640903247994, - -62.71838864361929 + -62.398383827401425, + -62.73953495712387, + -61.584615770221966, + -61.958519697541895 ], "moduleForcesY": [ - 30.40188460343236, - 30.221851782690536, - 30.976021760761594, - 30.79224774785525 + 31.413609816398104, + 30.73509287925401, + 32.98416244544942, + 32.28414823175951 ], - "timestamp": 10.168632713152101 + "timestamp": 10.15248205726612 }, { - "x": 6.924263851753144, - "y": 1.4952348436632368, - "heading": -0.17328998021723432, - "angularVelocity": -0.4908249807619052, - "velocityX": -3.461152443218535, - "velocityY": -1.1634751982713893, + "x": 6.851189181350193, + "y": 1.5457607903673927, + "heading": -0.08534517045150107, + "angularVelocity": -0.528146322447312, + "velocityX": -3.3459080865231883, + "velocityY": -1.282862481700599, "moduleForcesX": [ - -53.63079242417267, - -53.732278169292705, - -53.450679135245664, - -53.553119173625326 + -52.16729788107013, + -52.743022249429835, + -51.36856497292788, + -51.96098925114158 ], "moduleForcesY": [ - 44.77877441586359, - 44.65783761257958, - 44.9941386909519, - 44.87304464918282 + 46.4697214226326, + 45.81902631486864, + 47.35399851175358, + 46.706752999418384 ], - "timestamp": 10.24029581375644 + "timestamp": 10.223308205278867 }, { - "x": 6.664645466021959, - "y": 1.4273879906699989, - "heading": -0.20846188348487105, - "angularVelocity": -0.49079516475048257, - "velocityX": -3.622762391548932, - "velocityY": -0.9467473835360443, + "x": 6.603389718482076, + "y": 1.470425326889423, + "heading": -0.12323379914298056, + "angularVelocity": -0.534952552899821, + "velocityX": -3.498700265663482, + "velocityY": -1.0636673826227476, "moduleForcesX": [ - -41.769412633151376, - -41.76713542840772, - -41.77205019669556, - -41.769773098851616 + -39.932337350479486, + -40.53983959192529, + -39.373177991520045, + -39.983518610928336 ], "moduleForcesY": [ - 56.01545952386056, - 56.01714957737264, - 56.01348552259297, - 56.01517566218937 + 57.340323299968865, + 56.91405451985326, + 57.72751484701259, + 57.308022362111764 ], - "timestamp": 10.311958914360778 + "timestamp": 10.294134353291614 }, { - "x": 6.396804785667352, - "y": 1.37708752891827, - "heading": -0.24350598675754967, - "angularVelocity": -0.4890118202694272, - "velocityX": -3.7374977930886835, - "velocityY": -0.7019018341035008, + "x": 6.347913177850161, + "y": 1.4123915991053673, + "heading": -0.1614061090640271, + "angularVelocity": -0.5389578706747766, + "velocityX": -3.607093535369666, + "velocityY": -0.8193828044073681, "moduleForcesX": [ - -29.670078278170966, - -29.48805559061184, - -29.820795736138948, - -29.638793031028708 + -28.27085895786839, + -28.72327567698898, + -27.969794241953274, + -28.42128687481258 ], "moduleForcesY": [ - 63.27577283382489, - 63.36048200304406, - 63.204474407441516, - 63.28970026112863 + 63.91704070638253, + 63.71563591935232, + 64.05032104573215, + 63.85186792328192 ], - "timestamp": 10.383622014965116 + "timestamp": 10.364960501304362 }, { - "x": 6.123724098762273, - "y": 1.3454463182894645, - "heading": -0.27833593889663377, - "angularVelocity": -0.4860235162218966, - "velocityX": -3.8106178019395767, - "velocityY": -0.4415272345457245, + "x": 6.087428151650664, + "y": 1.3726159298536926, + "heading": -0.19970318630453027, + "angularVelocity": -0.5407194703516945, + "velocityX": -3.677808740249677, + "velocityY": -0.561595828203393, "moduleForcesX": [ - -18.964618645051026, - -18.60764038496825, - -19.189884865939113, - -18.832039003279412 + -18.438441470262664, + -18.66329414572283, + -18.32289991687435, + -18.547288960002728 ], "moduleForcesY": [ - 67.27850831701049, - 67.37773513616717, - 67.21397107913663, - 67.3147241815706 + 67.42955829795206, + 67.36785699497005, + 67.46142450851352, + 67.4002635181382 ], - "timestamp": 10.455285115569454 + "timestamp": 10.435786649317109 }, { - "x": 5.847858828640413, - "y": 1.3329886237876336, - "heading": -0.3128943538499734, - "angularVelocity": -0.4822344367171772, - "velocityX": -3.849474384941141, - "velocityY": -0.17383694532855082, + "x": 5.824078839371321, + "y": 1.3515584546015937, + "heading": -0.2379996051703208, + "angularVelocity": -0.5407101747069203, + "velocityX": -3.7182498225365426, + "velocityY": -0.29731216285133905, "moduleForcesX": [ - -10.164815803497616, - -9.673316441913729, - -10.413603531769702, - -9.919643964436528 + -10.576255375139242, + -10.574993214493354, + -10.57678518795997, + -10.575523009234347 ], "moduleForcesY": [ - 69.17032970505136, - 69.24042697051493, - 69.13258323678724, - 69.20483068344068 + 69.11369299701585, + 69.11388541630474, + 69.11361014421324, + 69.11380257613486 ], - "timestamp": 10.526948216173793 + "timestamp": 10.506612797329856 }, { - "x": 5.571170150686736, - "y": 1.339901067236654, - "heading": -0.3471395996450313, - "angularVelocity": -0.4778644170607483, - "velocityX": -3.8609643682780583, - "velocityY": 0.09645749891823462, + "x": 5.5595602703116835, + "y": 1.3494038487343114, + "heading": -0.27619370702127155, + "angularVelocity": -0.5392655526610999, + "velocityX": -3.734758651734541, + "velocityY": -0.030421051091110783, "moduleForcesX": [ - -3.140660151426149, - -2.5499567673882018, - -3.391519077751636, - -2.7966358407203566 + -4.3819858465305295, + -4.179901202476201, + -4.454918349746145, + -4.252321785026191 ], "moduleForcesY": [ - 69.85390969745158, - 69.87766236501122, - 69.84141674708727, - 69.86745404796969 + 69.79184657492922, + 69.8041566069463, + 69.78698263696384, + 69.79953511767619 ], - "timestamp": 10.59861131677813 + "timestamp": 10.577438945342603 }, { "x": 5.295215606689453, "y": 1.3661898374557495, - "heading": -0.3810366506112695, - "angularVelocity": -0.4730056427977949, - "velocityX": -3.850720128910756, - "velocityY": 0.3668383030792834, + "heading": -0.3142003336934002, + "angularVelocity": -0.5366185757453372, + "velocityX": -3.7323032670738447, + "velocityY": 0.23700270581448382, "moduleForcesX": [ - 2.4358890124890213, - 3.105036958141336, - 2.1874776648542436, - 2.8624717837810634 + 0.5151198465758231, + 0.8900974969762288, + 0.3932026550090731, + 0.770045001718678 ], "moduleForcesY": [ - 69.89157339518185, - 69.86477674457022, - 69.89901845264843, - 69.87435635433927 + 69.9365194091976, + 69.93262774089895, + 69.93690494603392, + 69.93364443708154 ], - "timestamp": 10.670274417382469 + "timestamp": 10.64826509335535 }, { - "x": 5.053706815736057, - "y": 1.4043054270449447, - "heading": -0.4105962505484791, - "angularVelocity": -0.46833496540702335, - "velocityX": -3.8264053470585537, - "velocityY": 0.6038939420574941, + "x": 5.05792538929108, + "y": 1.3966823635324737, + "heading": -0.3482525379686159, + "angularVelocity": -0.5333202229677834, + "velocityX": -3.716401752678385, + "velocityY": 0.4775691075577662, "moduleForcesX": [ - 6.887915896080987, - 7.623701240859441, - 6.643290267386372, - 7.386549953753599 + 4.430042032874039, + 4.95053093134464, + 4.273296521707641, + 4.797518672081996 ], "moduleForcesY": [ - 69.5924526319861, - 69.51542701820333, - 69.6153579444215, - 69.54014360764195 + 69.79829505364833, + 69.76315167405717, + 69.80750234348231, + 69.77327260349615 ], - "timestamp": 10.733390780926925 + "timestamp": 10.712114545914499 }, { - "x": 4.814521797126242, - "y": 1.457279005501737, - "heading": -0.43977801159576735, - "angularVelocity": -0.46234857980584404, - "velocityX": -3.789588074752571, - "velocityY": 0.8393002302720002, + "x": 4.822357731816456, + "y": 1.442469918801244, + "heading": -0.3819972270935512, + "angularVelocity": -0.5285039694533855, + "velocityX": -3.6894232923360217, + "velocityY": 0.7171174291017512, "moduleForcesX": [ - 10.475140797961476, - 11.417395555766568, - 10.184857658397029, - 11.139880728378749 + 7.554810386078761, + 8.313127657144003, + 7.335080255073474, + 8.101550015304404 ], "moduleForcesY": [ - 69.13513408837922, - 68.98542996372927, - 69.17724110186298, - 69.02951613308481 + 69.52196945360694, + 69.43512984509572, + 69.54458000792314, + 69.45920878774324 ], - "timestamp": 10.79650714447138 + "timestamp": 10.775963998473648 }, { - "x": 4.578623056803542, - "y": 1.5249252632965482, - "heading": -0.46848135774782446, - "angularVelocity": -0.4547686929371364, - "velocityX": -3.7375210971485733, - "velocityY": 1.071770520289322, + "x": 4.589376593940337, + "y": 1.503428017324539, + "heading": -0.4153167376730134, + "angularVelocity": -0.5218448905038962, + "velocityX": -3.6489136325843017, + "velocityY": 0.9547160716346837, "moduleForcesX": [ - 14.840754027841374, - 16.029076302383867, - 14.51917987513575, - 15.728849295978776 + 11.364581723457395, + 12.409065316223296, + 11.08567451247928, + 12.146230097766022 ], "moduleForcesY": [ - 68.32109565083954, - 68.05166292944023, - 68.38835125483823, - 68.11984422324372 + 68.99256903562845, + 68.81203194141854, + 69.03650497066894, + 68.8574640226182 ], - "timestamp": 10.859623508015837 + "timestamp": 10.839813451032796 }, { - "x": 4.347193447367128, - "y": 1.6069250977069311, - "heading": -0.4965850522073392, - "angularVelocity": -0.4452679603399553, - "velocityX": -3.6667132965195126, - "velocityY": 1.299185025966005, + "x": 4.360049392260068, + "y": 1.5793325904196134, + "heading": -0.4480682781137704, + "angularVelocity": -0.5129494322667391, + "velocityX": -3.591686263368148, + "velocityY": 1.1888053859938605, "moduleForcesX": [ - 20.193590004699995, - 21.665646251529257, - 19.875228316396825, - 21.38196372017987 + 16.060474026737822, + 17.44417496465417, + 15.743033698279458, + 17.156332450571153 ], "moduleForcesY": [ - 66.92237625745865, - 66.45990723727093, - 67.0149915440133, - 66.54903999850195 + 68.04180346878879, + 67.69982630825619, + 68.1137623119266, + 67.77111438492656 ], - "timestamp": 10.922739871560292 + "timestamp": 10.903662903591945 }, { - "x": 4.121687707159994, - "y": 1.7027378890121, - "heading": -0.5239423865994234, - "angularVelocity": -0.4334428166606177, - "velocityX": -3.572856982615943, - "velocityY": 1.5180340869556606, + "x": 4.135705069539603, + "y": 1.6697908456139685, + "heading": -0.4800781004717692, + "angularVelocity": -0.5013327612847737, + "velocityX": -3.5136452033420373, + "velocityY": 1.4167428469423091, "moduleForcesX": [ - 26.76366004990429, - 28.54407768472358, - 26.51477620428529, - 28.34898574175057 + 21.89196611243463, + 23.664965264016004, + 21.586990023029227, + 23.41134616769712 ], "moduleForcesY": [ - 64.56136029572545, - 63.79374619278623, - 64.66009920375362, - 63.876720382176266 + 66.38195678091566, + 65.77026762364113, + 66.47840712872917, + 65.85754632928692 ], - "timestamp": 10.985856235104748 + "timestamp": 10.967512356151094 }, { - "x": 3.903869186822254, - "y": 1.8114606574002452, - "heading": -0.5503757290218597, - "angularVelocity": -0.4188033172066106, - "velocityX": -3.4510625787926257, - "velocityY": 1.7225765599053773, + "x": 3.9179949909601377, + "y": 1.774123115557712, + "heading": -0.5111344027979444, + "angularVelocity": -0.48639888176653956, + "velocityX": -3.409740723740524, + "velocityY": 1.6340354656462184, "moduleForcesX": [ - 34.722789021923006, - 36.79190026410112, - 34.65315726916901, - 36.79826370259493 + 29.124210645761526, + 31.309392995735184, + 28.932435627249138, + 31.199954435020395 ], "moduleForcesY": [ - 60.634978039609805, - 59.40218821323202, - 60.6689615218732, - 59.39227430417506 + 63.52533931919331, + 62.47695800696286, + 63.60772502954319, + 62.52636867386374 ], - "timestamp": 11.048972598649204 + "timestamp": 11.031361808710242 }, { - "x": 3.6957796336708695, - "y": 1.9316273259274328, - "heading": -0.5756742900350444, - "angularVelocity": -0.40082412218451013, - "velocityX": -3.296919237193045, - "velocityY": 1.903890873601256, + "x": 3.7089231153715345, + "y": 1.8911721418901661, + "heading": -0.5409813219232001, + "angularVelocity": -0.46745771387165014, + "velocityX": -3.2744505584433368, + "velocityY": 1.8332032874365511, "moduleForcesX": [ - 43.97155118149319, - 46.20214883747921, - 44.22377698479971, - 46.54083607831509 + 37.90825639910293, + 40.44135259696435, + 37.994172581878566, + 40.64072265092307 ], "moduleForcesY": [ - 54.28134738689487, - 52.39692915304774, - 54.06704722487376, - 52.087132813104375 + 58.68761411990541, + 56.972077301885136, + 58.62385597562841, + 56.821660386292294 ], - "timestamp": 11.11208896219366 + "timestamp": 11.095211261269391 }, { - "x": 3.499550666767407, - "y": 2.061002715105782, - "heading": -0.5996083713418497, - "angularVelocity": -0.37920564434843623, - "velocityX": -3.109003052199785, - "velocityY": 2.0497915582101762, + "x": 3.5107601533164168, + "y": 2.019044446471857, + "heading": -0.5693252783250675, + "angularVelocity": -0.4439185500550772, + "velocityX": -3.1035968847429976, + "velocityY": 2.0027157548960086, "moduleForcesX": [ - 53.763954059728796, - 55.84492515312586, - 54.42048149959161, - 56.552580490225075 + 47.952424172958146, + 50.57190975238657, + 48.49980640396711, + 51.2266283249439 ], "moduleForcesY": [ - 44.579593956853635, - 41.94760445118944, - 43.76152240959032, - 40.974101674150546 + 50.78756695761204, + 48.182517312375914, + 50.25197010379258, + 47.47244005809576 ], - "timestamp": 11.175205325738116 + "timestamp": 11.15906071382854 }, { - "x": 3.3170061238667126, - "y": 2.1965515433699756, - "heading": -0.621969243888579, - "angularVelocity": -0.3542801151872387, - "velocityX": -2.8921904344524014, - "velocityY": 2.1476019949837775, + "x": 3.3257171127383325, + "y": 2.154907115536406, + "heading": -0.5958715572695051, + "angularVelocity": -0.41576361081320273, + "velocityX": -2.898114755277928, + "velocityY": 2.1278595762225385, "moduleForcesX": [ - 62.44766102413855, - 63.94433819362826, - 63.329847528999416, - 64.77963805969534 + 58.0104977519177, + 60.19934179791254, + 59.013462507055486, + 61.2087388169446 ], "moduleForcesY": [ - 31.256641416018002, - 28.0802822128175, - 29.404706704310698, - 26.071343409245944 + 38.879821467287535, + 35.405230100188476, + 37.3182212633444, + 33.60787910396 ], - "timestamp": 11.238321689282571 + "timestamp": 11.222910166387688 }, { - "x": 3.149232244356222, - "y": 2.334780583324695, - "heading": -0.6426179790087161, - "angularVelocity": -0.32715343471259145, - "velocityX": -2.6581677094296006, - "velocityY": 2.1900666038429124, + "x": 3.155400510330475, + "y": 2.2951308910705395, + "heading": -0.620392443592049, + "angularVelocity": -0.38404223277918687, + "velocityX": -2.667471616143963, + "velocityY": 2.19616253411468, "moduleForcesX": [ - 68.07446024974723, - 68.73289751434758, - 68.68747354304334, - 69.20841989455995 + 65.83208211172878, + 67.07228199343204, + 66.81818884359254, + 67.90516981439036 ], "moduleForcesY": [ - 15.578092751741488, - 12.393772344765436, - 12.552654231475199, - 9.321777066848862 + 23.286937742879168, + 19.455710293987412, + 20.244285253522296, + 16.268687090512074 ], - "timestamp": 11.301438052827027 + "timestamp": 11.286759618946837 }, { - "x": 2.996456707624642, - "y": 2.4723242803752896, - "heading": -0.6615054771208647, - "angularVelocity": -0.2992488326556613, - "velocityX": -2.4205376886767986, - "velocityY": 2.179208200955956, + "x": 3.000433493753425, + "y": 2.4359044574852438, + "heading": -0.6427805397589214, + "angularVelocity": -0.3506388116034793, + "velocityX": -2.427068837175911, + "velocityY": 2.2047732716940165, "moduleForcesX": [ - 69.84418644116603, - 69.8030417146416, - 69.73398928648147, - 69.55639119192205 + 69.54650836600102, + 69.78388265544288, + 69.79679859116072, + 69.82517169644473 ], "moduleForcesY": [ - 0.04124705741425996, - -2.629684927774557, - -3.7965442820408897, - -6.360953421225059 + 6.348124333641362, + 2.931670853614007, + 2.0228290324436116, + -1.3111191685548196 ], - "timestamp": 11.364554416371483 + "timestamp": 11.350609071505986 }, { - "x": 2.858285394290842, - "y": 2.6063679764517347, - "heading": -0.6786531021168939, - "angularVelocity": -0.2716827147994899, - "velocityX": -2.18915199758744, - "velocityY": 2.1237550541395294, + "x": 2.8605755262449746, + "y": 2.5738952164278426, + "heading": -0.6630425145247281, + "angularVelocity": -0.3173398354048022, + "velocityX": -2.190433306830491, + "velocityY": 2.1611893823955435, "moduleForcesX": [ - 68.5832050639289, - 68.18255030614512, - 67.65433881512726, - 67.18772346415568 + 69.26890065109001, + 68.89986618517693, + 68.45222405005646, + 67.96002786407358 ], "moduleForcesY": [ - -13.292687011674952, - -15.252960265172256, - -17.395421555860977, - -19.15158282474659 + -8.993101394774568, + -11.54522411202305, + -13.87299037870192, + -16.161430765057244 ], - "timestamp": 11.427670779915939 + "timestamp": 11.414458524065134 }, { - "x": 2.734026522437346, - "y": 2.734745646026211, - "heading": -0.6941195885833025, - "angularVelocity": -0.2450471731552614, - "velocityX": -1.9687267275145863, - "velocityY": 2.0339839364169796, + "x": 2.735118463226934, + "y": 2.70652006019163, + "heading": -0.6812534516483567, + "angularVelocity": -0.28521680913017455, + "velocityX": -1.9648886245629291, + "velocityY": 2.0771492698536242, "moduleForcesX": [ - 65.71596180721829, - 65.23236893280917, - 64.13301722958752, - 63.6607820099661 + 66.56985010341027, + 66.01724983813499, + 64.83928204278061, + 64.28532521518495 ], "moduleForcesY": [ - -23.750213140906997, - -25.069078070391868, - -27.734992318593385, - -28.82188410507732 + -21.21291575912241, + -22.903571424790023, + -26.016891342337594, + -27.382914500304768 ], - "timestamp": 11.490787143460395 + "timestamp": 11.478307976624283 }, { - "x": 2.622913491858143, - "y": 2.8558455619096765, - "heading": -0.7079766418555111, - "angularVelocity": -0.21954771304985882, - "velocityX": -1.7604472808535805, - "velocityY": 1.918677013104065, + "x": 2.623211412340952, + "y": 2.8318836413124444, + "heading": -0.6975143593601079, + "angularVelocity": -0.2546757577394683, + "velocityX": -1.7526704834675835, + "velocityY": 1.9634245259140095, "moduleForcesX": [ - 62.3261455799643, - 61.90369851571434, - 60.3053013643231, - 59.94983415338624 + 62.957124910706376, + 62.45929870418039, + 60.62064580180965, + 60.211130480488805 ], "moduleForcesY": [ - -31.626927610118603, - -32.460334406565565, - -35.32513309689565, - -35.9385060308983 + -30.33692388884184, + -31.367391790541795, + -34.7676381916512, + -35.48902427322522 ], - "timestamp": 11.55390350700485 + "timestamp": 11.542157429183431 }, { - "x": 2.5242136717280004, - "y": 2.968472284388882, - "heading": -0.720296269771335, - "angularVelocity": -0.19518912725614584, - "velocityX": -1.5637754551658334, - "velocityY": 1.7844298396544638, + "x": 2.524027231935006, + "y": 2.9486107208127024, + "heading": -0.7119282315157714, + "angularVelocity": -0.22574777978418986, + "velocityX": -1.5534069037485454, + "velocityY": 1.8281610072087373, "moduleForcesX": [ - 58.98754783726104, - 58.678775107744755, - 56.70105562415566, - 56.492209664989446 + 59.28998740855844, + 58.93448494902304, + 56.604293732503145, + 56.387554803221356 ], "moduleForcesY": [ - -37.51207015880541, - -38.003843268920306, - -40.88479342874241, - -41.18285690919068 + -37.023359271023516, + -37.59971228683433, + -41.01043622517381, + -41.32007696242945 ], - "timestamp": 11.617019870549306 + "timestamp": 11.60600688174258 }, { - "x": 2.437268193420317, - "y": 3.0717284892697196, - "heading": -0.7311460366120948, - "angularVelocity": -0.17190101316781217, - "velocityX": -1.377542580482226, - "velocityY": 1.6359656843682122, + "x": 2.436823832988882, + "y": 3.0556924556358678, + "heading": -0.7245906689335784, + "angularVelocity": -0.19831708668256617, + "velocityX": -1.3657658045751015, + "velocityY": 1.6770971485459225, "moduleForcesX": [ - 55.933543070193124, - 55.74620404374717, - 53.4983817914488, - 53.427889596621455 + 55.924906891884625, + 55.72478848329192, + 53.0592023113592, + 53.02122971774716 ], "moduleForcesY": [ - -41.94896951930148, - -42.20583852522306, - -45.01283639047294, - -45.10426074033776 + -41.9548706445922, + -42.23018067659876, + -45.52453660860857, + -45.577986795152945 ], - "timestamp": 11.680136234093762 + "timestamp": 11.669856334301729 }, { - "x": 2.3614992683800686, - "y": 3.1649283314618284, - "heading": -0.7405881776451042, - "angularVelocity": -0.1495989392094639, - "velocityX": -1.2004640442708914, - "velocityY": 1.4766351696809092, + "x": 2.3609561392644562, + "y": 3.1523738866134026, + "heading": -0.7355878255117103, + "angularVelocity": -0.17223572227098613, + "velocityX": -1.188227787139511, + "velocityY": 1.5142092391156017, "moduleForcesX": [ - 53.226119004537075, - 53.150380788764224, - 50.717908435297424, - 50.76595922311749 + 52.96028064478196, + 52.90088037948619, + 50.017708559288, + 50.12810503784231 ], "moduleForcesY": [ - -45.3497145739425, - -45.44508539739661, - -48.13839282499532, - -48.093987559665536 + -45.65625782653057, + -45.73286478403445, + -48.86224974564624, + -48.75628870048946 ], - "timestamp": 11.743252597638218 + "timestamp": 11.733705786860877 }, { - "x": 2.2964046195038508, - "y": 3.2475371925356646, - "heading": -0.7486801929297746, - "angularVelocity": -0.12820788192226776, - "velocityX": -1.0313434618325024, - "velocityY": 1.3088342932756496, + "x": 2.295869220398745, + "y": 3.238077294744024, + "heading": -0.7449971922573848, + "angularVelocity": -0.14736800972503292, + "velocityX": -1.0193810010417363, + "velocityY": 1.3422731863085038, "moduleForcesX": [ - 50.85429251327673, - 50.87522935086129, - 48.32191532894057, - 48.467633279346366 + 50.38715799993498, + 50.44685956622395, + 47.42973162422406, + 47.658319113484225 ], "moduleForcesY": [ - -48.006099795729824, - -47.9893857552052, - -50.55448655081397, - -50.41998583087466 + -48.493835508606004, + -48.43806643870586, + -51.39038837291991, + -51.18442141820145 ], - "timestamp": 11.806368961182674 + "timestamp": 11.797555239420026 }, { - "x": 2.2415479359062194, - "y": 3.3191302814359016, - "heading": -0.7554758085003378, - "angularVelocity": -0.10766804658789833, - "velocityX": -0.8691356807810039, - "velocityY": 1.1343031328129516, + "x": 2.2410859959949136, + "y": 3.3123508545538702, + "heading": -0.7528891490451303, + "angularVelocity": -0.1236025756122906, + "velocityX": -0.8580061724582897, + "velocityY": 1.1632607145854852, "moduleForcesX": [ - 48.7813998909101, - 48.883997299800775, - 46.25717342000062, - 46.48203569715231 + 48.16035953471259, + 48.31828690116759, + 45.225962077158464, + 45.54734290961062 ], "moduleForcesY": [ - -50.12061198883146, - -50.02516024483403, - -52.45958599626199, - -52.26481927286433 + -50.71618060999772, + -50.57101713150958, + -53.34991243772539, + -53.08075358781461 ], - "timestamp": 11.86948532472713 + "timestamp": 11.861404691979175 }, { - "x": 2.1965490021845606, - "y": 3.379364027011461, - "heading": -0.7610258706649285, - "angularVelocity": -0.08793380754076002, - "velocityX": -0.7129519382079801, - "velocityY": 0.9543285162988591, + "x": 2.1961946939313863, + "y": 3.374834024346928, + "heading": -0.7593284846297825, + "angularVelocity": -0.10085185270284114, + "velocityX": -0.7030804535393855, + "velocityY": 0.9786014959983499, "moduleForcesX": [ - 46.96552536511235, - 47.136382750106904, - 44.47147307724585, - 44.76001886720274 + 46.22788871890863, + 46.46616016540548, + 43.34033970137869, + 43.73431169906387 ], "moduleForcesY": [ - -51.83392911935416, - -51.682542993841096, - -53.98932327657365, - -53.75408240792354 + -52.491972499861475, + -52.28563530920509, + -54.900801591760306, + -54.591680501964085 ], - "timestamp": 11.932601688271586 + "timestamp": 11.925254144538323 }, { - "x": 2.16107481240881, - "y": 3.4279560426439364, - "heading": -0.7653786869846049, - "angularVelocity": -0.06896494150222135, - "velocityX": -0.5620442589466436, - "velocityY": 0.7698798362844577, + "x": 2.1608377743767466, + "y": 3.4252338531970117, + "heading": -0.7643752716013872, + "angularVelocity": -0.07904197717168335, + "velocityX": -0.5537544667573492, + "velocityY": 0.7893541264648288, "moduleForcesX": [ - 45.36776130374439, - 45.59551423238422, - 42.91858789949486, - 43.2583272033556 + 44.54201907610667, + 44.84605364869624, + 41.71600811601532, + 42.16694704188603 ], "moduleForcesY": [ - -53.2445780347107, - -53.0530790830467, - -55.238217704660805, - -54.97579973773786 + -53.93680181997976, + -53.68810804227729, + -56.151644018849424, + -55.81741599524645 ], - "timestamp": 11.995718051816041 + "timestamp": 11.989103597097472 }, { - "x": 2.134832009400895, - "y": 3.4646708607022925, - "heading": -0.768580123623318, - "angularVelocity": -0.050722767582422834, - "velocityX": -0.4157844580103423, - "velocityY": 0.5817004655614504, + "x": 2.1347026814888364, + "y": 3.4633084503090976, + "heading": -0.7680853894338862, + "angularVelocity": -0.058107277099393444, + "velocityX": -0.40932368000649094, + "velocityY": 0.5963183016614753, "moduleForcesX": [ - 43.954428645312426, - 44.2297096749135, - 41.559591533194514, - 41.940634703058834 + 43.06208338047946, + 43.420291447119574, + 40.30623488983344, + 40.80225251311549 ], "moduleForcesY": [ - -54.42275064440647, - -54.20223828129335, - -56.273239100999206, - -55.992648881357695 + -55.13137229057452, + -54.85302592490151, + -57.17764132287779, + -56.82788011765582 ], - "timestamp": 12.058834415360497 + "timestamp": 12.05295304965662 }, { - "x": 2.1175606055664447, - "y": 3.489309593661584, - "heading": -0.770673719921477, - "angularVelocity": -0.03317042016662467, - "velocityX": -0.27364383599643854, - "velocityY": 0.39036997025244236, + "x": 2.1175142950444665, + "y": 3.488855217863188, + "heading": -0.7705109754915318, + "angularVelocity": -0.037989144157478276, + "velocityX": -0.26920178255948496, + "velocityY": 0.40010942193161164, "moduleForcesX": [ - 42.69713344336191, - 43.01233196517679, - 40.36254825642123, - 40.77712883283335 + 41.75436699647201, + 42.15764707113133, + 39.07345325528694, + 39.60555733885558 ], "moduleForcesY": [ - -55.41943823043682, - -55.177780690773446, - -57.14258045212568, - -56.849966899553834 + -56.13323789601473, + -55.83391387223054, + -58.03186064603064, + -57.67277468150624 ], - "timestamp": 12.121950778904953 + "timestamp": 12.11680250221577 }, { "x": 2.1090288162231445, "y": 3.501702308654785, "heading": -0.771700836029523, - "angularVelocity": -0.016273372709797004, - "velocityX": -0.13517555296560768, - "velocityY": 0.19634710077161807, + "angularVelocity": -0.018635407044235015, + "velocityX": -0.13289822357460562, + "velocityY": 0.20120909853840863, "moduleForcesX": [ - 41.5722477087897, - 41.92121432741254, - 39.30147273432098, - 39.743500474089345 + 40.59125684604141, + 41.032490008004295, + 37.98763054279593, + 38.5490223991702 ], "moduleForcesY": [ - -56.272332012188514, - -56.01516696263238, - -57.881532814470596, - -57.581128152106956 + -56.98424266070932, + -56.6699287318646, + -58.752507438065514, + -58.388136078887676 ], - "timestamp": 12.185067142449409 + "timestamp": 12.180651954774918 }, { "x": 2.1090288162231445, "y": 3.501702308654785, "heading": -0.771700836029523, - "angularVelocity": 7.034369509774633e-21, - "velocityX": 1.0745130979383846e-20, - "velocityY": 7.792911077792643e-21, + "angularVelocity": -4.8374542557573845e-23, + "velocityX": -1.6522222635939176e-22, + "velocityY": 1.1005265766974004e-22, "moduleForcesX": [ - 40.56018810627996, - 40.937946455779034, - 38.35525709358692, - 38.819933249113355 + 39.55019943140344, + 40.023804726295744, + 37.02474315966745, + 37.61025741647993 ], "moduleForcesY": [ - -57.00975798757109, - -56.74117440782203, - -58.516369460899966, - -58.211114035733786 + -57.7153729196056, + -57.39024948338779, + -59.367606493987516, + -59.000582824693424 ], - "timestamp": 12.248183505993865 + "timestamp": 12.244501407334067 } ], "eventMarkers": [] diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 32bbf7d0..47aadf7a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -453,6 +453,7 @@ public void robotInit() { autoChooser.addOption("Shoot Preload", teleopAutoAim()); autoChooser.addDefaultOption("Amp 4 Wing", autoAmp4Wing()); autoChooser.addOption("Source 3", autoSource3()); + autoChooser.addOption("Center 3", autoCenter3()); autoChooser.addOption("Amp 5", autoAmp5()); autoChooser.addOption("Source 4", autoSource4()); autoChooser.addOption("Line Test Repeatedly", lineTest()); @@ -839,6 +840,47 @@ private Command autoAmp5() { autoStaticAutoAim()); } + private Command autoCenter3() { + return Commands.sequence( + autoFenderShot(), + swerve + .runChoreoTraj(Choreo.getTrajectory("3 center.1"), true) + .asProxy() + .deadlineWith(autoIntake()), + autoIntake() + .raceWith( + Commands.sequence( + Commands.waitSeconds(0.25), + Commands.waitUntil( + () -> carriage.getBeambreak() || feeder.getFirstBeambreak()))) + .withTimeout(1.0), + autoStaticAutoAim().unless(() -> !feeder.getFirstBeambreak()), + swerve + .runChoreoTraj(Choreo.getTrajectory("3 center.2")) + .asProxy() + .deadlineWith(autoIntake()), + autoIntake() + .raceWith( + Commands.sequence( + Commands.waitSeconds(0.25), + Commands.waitUntil( + () -> carriage.getBeambreak() || feeder.getFirstBeambreak()))) + .withTimeout(1.0), + autoStaticAutoAim().unless(() -> !feeder.getFirstBeambreak()), + swerve + .runChoreoTraj(Choreo.getTrajectory("3 center.3")) + .asProxy() + .deadlineWith(autoIntake()), + autoIntake() + .raceWith( + Commands.sequence( + Commands.waitSeconds(0.25), + Commands.waitUntil( + () -> carriage.getBeambreak() || feeder.getFirstBeambreak()))) + .withTimeout(1.0), + autoStaticAutoAim()); + } + private Command autoSource3() { return Commands.sequence( autoFenderShot(), From 9c26d2cd1986b743d40da4f5ffa83bcac796ff26 Mon Sep 17 00:00:00 2001 From: team computer Date: Sun, 29 Sep 2024 10:18:08 -0700 Subject: [PATCH 15/21] rename 3 center to center 3 --- choreo.chor | 2 +- src/main/deploy/choreo/{3 center.1.traj => center 3.1.traj} | 0 src/main/deploy/choreo/{3 center.2.traj => center 3.2.traj} | 0 src/main/deploy/choreo/{3 center.3.traj => center 3.3.traj} | 0 src/main/deploy/choreo/{3 center.traj => center 3.traj} | 0 src/main/java/frc/robot/Robot.java | 6 +++--- 6 files changed, 4 insertions(+), 4 deletions(-) rename src/main/deploy/choreo/{3 center.1.traj => center 3.1.traj} (100%) rename src/main/deploy/choreo/{3 center.2.traj => center 3.2.traj} (100%) rename src/main/deploy/choreo/{3 center.3.traj => center 3.3.traj} (100%) rename src/main/deploy/choreo/{3 center.traj => center 3.traj} (100%) diff --git a/choreo.chor b/choreo.chor index 46e0d114..8e0e6cc2 100644 --- a/choreo.chor +++ b/choreo.chor @@ -28060,7 +28060,7 @@ "eventMarkers": [], "isTrajectoryStale": true }, - "3 center": { + "center 3": { "waypoints": [ { "x": 1.4015021324157715, diff --git a/src/main/deploy/choreo/3 center.1.traj b/src/main/deploy/choreo/center 3.1.traj similarity index 100% rename from src/main/deploy/choreo/3 center.1.traj rename to src/main/deploy/choreo/center 3.1.traj diff --git a/src/main/deploy/choreo/3 center.2.traj b/src/main/deploy/choreo/center 3.2.traj similarity index 100% rename from src/main/deploy/choreo/3 center.2.traj rename to src/main/deploy/choreo/center 3.2.traj diff --git a/src/main/deploy/choreo/3 center.3.traj b/src/main/deploy/choreo/center 3.3.traj similarity index 100% rename from src/main/deploy/choreo/3 center.3.traj rename to src/main/deploy/choreo/center 3.3.traj diff --git a/src/main/deploy/choreo/3 center.traj b/src/main/deploy/choreo/center 3.traj similarity index 100% rename from src/main/deploy/choreo/3 center.traj rename to src/main/deploy/choreo/center 3.traj diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 47aadf7a..f916c36c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -844,7 +844,7 @@ private Command autoCenter3() { return Commands.sequence( autoFenderShot(), swerve - .runChoreoTraj(Choreo.getTrajectory("3 center.1"), true) + .runChoreoTraj(Choreo.getTrajectory("center 3.1"), true) .asProxy() .deadlineWith(autoIntake()), autoIntake() @@ -856,7 +856,7 @@ private Command autoCenter3() { .withTimeout(1.0), autoStaticAutoAim().unless(() -> !feeder.getFirstBeambreak()), swerve - .runChoreoTraj(Choreo.getTrajectory("3 center.2")) + .runChoreoTraj(Choreo.getTrajectory("center 3.2")) .asProxy() .deadlineWith(autoIntake()), autoIntake() @@ -868,7 +868,7 @@ private Command autoCenter3() { .withTimeout(1.0), autoStaticAutoAim().unless(() -> !feeder.getFirstBeambreak()), swerve - .runChoreoTraj(Choreo.getTrajectory("3 center.3")) + .runChoreoTraj(Choreo.getTrajectory("center 3.3")) .asProxy() .deadlineWith(autoIntake()), autoIntake() From c61a316a8529d2283d90bfaf19f15545ab56dbb0 Mon Sep 17 00:00:00 2001 From: team computer Date: Sun, 29 Sep 2024 10:20:41 -0700 Subject: [PATCH 16/21] rename and generate source 3 -> source 4 --- choreo.chor | 2 +- .../deploy/choreo/{center 3.1.traj => center 4.1.traj} | 0 .../deploy/choreo/{center 3.2.traj => center 4.2.traj} | 0 .../deploy/choreo/{center 3.3.traj => center 4.3.traj} | 0 .../deploy/choreo/{center 3.traj => center 4.traj} | 0 src/main/java/frc/robot/Robot.java | 10 +++++----- 6 files changed, 6 insertions(+), 6 deletions(-) rename src/main/deploy/choreo/{center 3.1.traj => center 4.1.traj} (100%) rename src/main/deploy/choreo/{center 3.2.traj => center 4.2.traj} (100%) rename src/main/deploy/choreo/{center 3.3.traj => center 4.3.traj} (100%) rename src/main/deploy/choreo/{center 3.traj => center 4.traj} (100%) diff --git a/choreo.chor b/choreo.chor index 8e0e6cc2..02a137f0 100644 --- a/choreo.chor +++ b/choreo.chor @@ -28060,7 +28060,7 @@ "eventMarkers": [], "isTrajectoryStale": true }, - "center 3": { + "center 4": { "waypoints": [ { "x": 1.4015021324157715, diff --git a/src/main/deploy/choreo/center 3.1.traj b/src/main/deploy/choreo/center 4.1.traj similarity index 100% rename from src/main/deploy/choreo/center 3.1.traj rename to src/main/deploy/choreo/center 4.1.traj diff --git a/src/main/deploy/choreo/center 3.2.traj b/src/main/deploy/choreo/center 4.2.traj similarity index 100% rename from src/main/deploy/choreo/center 3.2.traj rename to src/main/deploy/choreo/center 4.2.traj diff --git a/src/main/deploy/choreo/center 3.3.traj b/src/main/deploy/choreo/center 4.3.traj similarity index 100% rename from src/main/deploy/choreo/center 3.3.traj rename to src/main/deploy/choreo/center 4.3.traj diff --git a/src/main/deploy/choreo/center 3.traj b/src/main/deploy/choreo/center 4.traj similarity index 100% rename from src/main/deploy/choreo/center 3.traj rename to src/main/deploy/choreo/center 4.traj diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f916c36c..99f3b3e0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -453,7 +453,7 @@ public void robotInit() { autoChooser.addOption("Shoot Preload", teleopAutoAim()); autoChooser.addDefaultOption("Amp 4 Wing", autoAmp4Wing()); autoChooser.addOption("Source 3", autoSource3()); - autoChooser.addOption("Center 3", autoCenter3()); + autoChooser.addOption("Center 4", autoCenter4()); autoChooser.addOption("Amp 5", autoAmp5()); autoChooser.addOption("Source 4", autoSource4()); autoChooser.addOption("Line Test Repeatedly", lineTest()); @@ -840,11 +840,11 @@ private Command autoAmp5() { autoStaticAutoAim()); } - private Command autoCenter3() { + private Command autoCenter4() { return Commands.sequence( autoFenderShot(), swerve - .runChoreoTraj(Choreo.getTrajectory("center 3.1"), true) + .runChoreoTraj(Choreo.getTrajectory("center 4.1"), true) .asProxy() .deadlineWith(autoIntake()), autoIntake() @@ -856,7 +856,7 @@ private Command autoCenter3() { .withTimeout(1.0), autoStaticAutoAim().unless(() -> !feeder.getFirstBeambreak()), swerve - .runChoreoTraj(Choreo.getTrajectory("center 3.2")) + .runChoreoTraj(Choreo.getTrajectory("center 4.2")) .asProxy() .deadlineWith(autoIntake()), autoIntake() @@ -868,7 +868,7 @@ private Command autoCenter3() { .withTimeout(1.0), autoStaticAutoAim().unless(() -> !feeder.getFirstBeambreak()), swerve - .runChoreoTraj(Choreo.getTrajectory("center 3.3")) + .runChoreoTraj(Choreo.getTrajectory("center 4.3")) .asProxy() .deadlineWith(autoIntake()), autoIntake() From 1f642c441d57ceb51b749fbe4549caacb76596fe Mon Sep 17 00:00:00 2001 From: team computer Date: Sun, 29 Sep 2024 11:45:06 -0700 Subject: [PATCH 17/21] [center 4] move first stop point shot away from wall, q72 --- choreo.chor | 2215 +++++++++++++----------- src/main/deploy/choreo/center 4.1.traj | 671 ++++--- src/main/deploy/choreo/center 4.2.traj | 765 ++++---- src/main/deploy/choreo/center 4.3.traj | 769 ++++---- src/main/deploy/choreo/center 4.traj | 2129 ++++++++++++----------- 5 files changed, 3502 insertions(+), 3047 deletions(-) diff --git a/choreo.chor b/choreo.chor index 02a137f0..4c01eec3 100644 --- a/choreo.chor +++ b/choreo.chor @@ -28078,6 +28078,15 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 2.075350046157837, + "y": 4.360164165496826, + "heading": -0.533708602141933, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, "controlIntervalCount": 14 }, { @@ -28087,30 +28096,30 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 11 + "controlIntervalCount": 10 }, { - "x": 2.9206175804138184, - "y": 5.5130615234375, - "heading": 0.14756797074007866, + "x": 2.810619354248047, + "y": 5.6322150230407715, + "heading": 0.18164838967891364, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 12 }, { - "x": 2.021008014678955, - "y": 7.007328033447266, + "x": 2.157177448272705, + "y": 6.508133888244629, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 10 + "controlIntervalCount": 12 }, { - "x": 2.8596270084381104, - "y": 6.999704360961914, - "heading": -0.027461342770395147, + "x": 2.8731672763824463, + "y": 6.999098300933838, + "heading": 0.4266275995625518, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -28121,10 +28130,10 @@ { "x": 1.4015021324157717, "y": 5.563943386077881, - "heading": 2.230586975435435e-18, - "angularVelocity": -4.993708780228108e-16, - "velocityX": 2.2186084345747425e-14, - "velocityY": -2.3644285540186748e-14, + "heading": 7.388244526996465e-19, + "angularVelocity": 3.0465745999077404e-18, + "velocityX": -3.4647017198299383e-17, + "velocityY": -1.192018758735892e-17, "moduleForcesX": [ 0, 0, @@ -28140,1390 +28149,1537 @@ "timestamp": 0 }, { - "x": 1.4206212389589077, - "y": 5.543569823071968, - "heading": -0.005440740553276968, - "angularVelocity": -0.06321989027440103, - "velocityX": 0.22215869438518032, - "velocityY": -0.23673512918379025, + "x": 1.4219202193105986, + "y": 5.548947800945218, + "heading": -0.0011051964959714333, + "angularVelocity": -0.01349618349862027, + "velocityX": 0.24933688120418268, + "velocityY": -0.18311962565685555, "moduleForcesX": [ - 47.940963091989595, - 43.779015007865034, - 51.71856759026114, - 47.81380065892401 + 56.284779515152344, + 55.67887470588661, + 57.10064703101138, + 56.51836963286635 ], "moduleForcesY": [ - -50.97920586080499, - -54.592033915440794, - -47.13866785517865, - -51.09102404890194 + -41.57896584233712, + -42.38577268175027, + -40.45035897321267, + -41.2588054071458 ], - "timestamp": 0.08606058203599731 + "timestamp": 0.08188955759860805 }, { - "x": 1.4588579982416516, - "y": 5.50282426541773, - "heading": -0.016323802947849745, - "angularVelocity": -0.12645815467526472, - "velocityX": 0.4443004964254837, - "velocityY": -0.4734520342542199, + "x": 1.4627096279184146, + "y": 5.518896687993435, + "heading": -0.003378676387590463, + "angularVelocity": -0.027762757038732957, + "velocityX": 0.4981026861537893, + "velocityY": -0.3669712455778003, "moduleForcesX": [ - 47.897364078006646, - 43.771363296078064, - 51.712153560329, - 47.85692298524638 + 56.14974361131909, + 55.50531984469861, + 57.01450005495424, + 56.39643729223106 ], "moduleForcesY": [ - -51.01338140036875, - -54.59132698392599, - -47.13783450766829, - -51.0426999647787 + -41.752695886724744, + -42.60428285649574, + -40.56281363910674, + -41.416368028381115 ], - "timestamp": 0.17212116407199463 + "timestamp": 0.1637791151972161 }, { - "x": 1.5162104171628858, - "y": 5.441708865073567, - "heading": -0.032652672648373536, - "angularVelocity": -0.18973691920489047, - "velocityX": 0.6664191378657773, - "velocityY": -0.7101439350063614, + "x": 1.5238075547381984, + "y": 5.473710157546218, + "heading": -0.0069048134518868666, + "angularVelocity": -0.04305966679635681, + "velocityX": 0.7461015618045014, + "velocityY": -0.5517984438105422, "moduleForcesX": [ - 47.832382527825025, - 43.760444196177374, - 51.70298147300229, - 47.92205717348266 + 55.968126398369286, + 55.27198555154887, + 56.89895032239671, + 56.233074234563546 ], "moduleForcesY": [ - -51.06506924736401, - -54.59072757046299, - -47.13719675772626, - -50.9707235368353 + -41.984633832597545, + -42.89526110767094, + -40.71292465128025, + -41.6259762326198 ], - "timestamp": 0.25818174610799194 + "timestamp": 0.24566867279582416 }, { - "x": 1.5926756045431116, - "y": 5.360226735801809, - "heading": -0.05443308174346003, - "angularVelocity": -0.25308228900808055, - "velocityX": 0.8885041862345195, - "velocityY": -0.9467996533549011, + "x": 1.605125212106644, + "y": 5.413276440660695, + "heading": -0.011802142574701541, + "angularVelocity": -0.05980407351594935, + "velocityX": 0.9930162007595612, + "velocityY": -0.7379905162231216, "moduleForcesX": [ - 47.74512096081025, - 43.74635348449958, - 51.68931736462727, - 48.00815391044375 + 55.71155946356058, + 54.941547780042846, + 56.7359549514754, + 56.00213234888038 ], "moduleForcesY": [ - -51.133333759147, - -54.588485401020556, - -47.13677120729667, - -50.87397800267328 + -42.30893410579651, + -43.30196365103173, + -40.92326397000725, + -41.91947734002761 ], - "timestamp": 0.34424232814398925 + "timestamp": 0.3275582303944322 }, { - "x": 1.6882490058265867, - "y": 5.258382764481358, - "heading": -0.08167367566723759, - "angularVelocity": -0.31652811634851347, - "velocityX": 1.1105363105122372, - "velocityY": -1.1833985890311751, + "x": 1.7065275334793777, + "y": 5.337428085536629, + "heading": -0.018249242256111287, + "angularVelocity": -0.07872920394823069, + "velocityX": 1.2382814652604819, + "velocityY": -0.9262274378844899, "moduleForcesX": [ - 47.63372466857229, - 43.72840022744862, - 51.668243433528644, - 48.113016067033115 + 55.322537771508856, + 54.4375833932818, + 56.48888780544757, + 55.64994626737733 ], "moduleForcesY": [ - -51.216216787013636, - -54.58157282722093, - -47.135721060383055, - -50.750174463810936 + -42.79328988841797, + -43.91048339133851, + -41.238988003610295, + -42.36091518217925 ], - "timestamp": 0.43030291017998656 + "timestamp": 0.40944778799304027 }, { - "x": 1.8029224178515728, - "y": 5.136185717697895, - "heading": -0.11438761196088189, - "angularVelocity": -0.3801268306536648, - "velocityX": 1.332473116574703, - "velocityY": -1.4198956586668827, + "x": 1.8277843888865313, + "y": 5.2458866793139265, + "heading": -0.02654556216924506, + "angularVelocity": -0.10131108478813869, + "velocityX": 1.4807364817078694, + "velocityY": -1.117864217454393, "moduleForcesX": [ - 47.49348736287193, - 43.70277282082887, - 51.63393088382914, - 48.231135604313586 + 54.66422556146924, + 53.57551056932187, + 56.07036150264626, + 55.04634059384568 ], "moduleForcesY": [ - -51.30877200765891, - -54.563746594487974, - -47.12995843648461, - -50.59351350853768 + -43.59337180528042, + -44.9199390004416, + -41.76546496247963, + -43.10085570220449 ], - "timestamp": 0.5163634922159839 + "timestamp": 0.4913373455916483 }, { - "x": 1.936676738585046, - "y": 4.9936559524582, - "heading": -0.1525981886270299, - "angularVelocity": -0.4439962612620674, - "velocityX": 1.5541879641384613, - "velocityY": -1.6561561857250287, + "x": 1.96841676914479, + "y": 5.138098820317635, + "heading": -0.03729581263726324, + "angularVelocity": -0.13127742758990868, + "velocityX": 1.717342044361563, + "velocityY": -1.3162588998792917, "moduleForcesX": [ - 47.30730481341717, - 43.65083702902553, - 51.569007669599586, - 48.34309714132929 + 53.31294529372992, + 51.76935791756607, + 55.208047286851695, + 53.77390871564015 ], "moduleForcesY": [ - -51.393363168260414, - -54.515933915159074, - -47.100223160042596, - -50.38283518179482 + -45.16334356265244, + -46.915928207338055, + -42.81846532880878, + -44.59597492097393 ], - "timestamp": 0.6024240742519812 + "timestamp": 0.5732269031902564 }, { - "x": 2.089417359472379, - "y": 4.830894184799883, - "heading": -0.19638882103609856, - "angularVelocity": -0.5088349552543711, - "velocityX": 1.7748034844634022, - "velocityY": -1.8912464178636816, + "x": 2.1268410825486406, + "y": 5.0124497352130994, + "heading": -0.05234368618465831, + "angularVelocity": -0.1837581492521726, + "velocityX": 1.9346094673069207, + "velocityY": -1.5343724986323788, "moduleForcesX": [ - 46.92254775633583, - 43.388297546565106, - 51.333326631328006, - 48.27968404555044 + 49.01689962945185, + 45.73289983149253, + 52.41775458712863, + 49.40090115837192 ], "moduleForcesY": [ - -51.315242953431444, - -54.28258670469318, - -46.8588510495494, - -49.928189244812266 + -49.59779004971247, + -52.616015053993074, + -45.96240551955732, + -49.157805609768765 ], - "timestamp": 0.6884846562879785 + "timestamp": 0.6551164607888644 }, { - "x": 2.2231712411306024, - "y": 4.688364652999476, - "heading": -0.23481358247355089, - "angularVelocity": -0.4464850286666412, - "velocityX": 1.5541828604889207, - "velocityY": -1.6561534735218788, + "x": 2.2651293964122, + "y": 4.873361744519432, + "heading": -0.08615107243119119, + "angularVelocity": -0.4128412368795543, + "velocityX": 1.6887173153556436, + "velocityY": -1.6984826243100626, "moduleForcesX": [ - -46.804641906327866, - -43.5737849154587, - -51.151878946250015, - -48.397943993853495 + -42.85767308274732, + -56.04462264975442, + -58.165526762903994, + -65.39829691272111 ], "moduleForcesY": [ - 51.411338017062235, - 54.12246522092687, - 47.04773571475752, - 49.80566610187949 + -53.371294254255105, + -39.88846466311047, + -34.7319701928346, + -20.48370015983788 ], - "timestamp": 0.7745452383239758 + "timestamp": 0.7370060183874725 }, { - "x": 2.3378438009906706, - "y": 4.5661682512486585, - "heading": -0.2677858501447284, - "angularVelocity": -0.383128569329872, - "velocityX": 1.3324632166436747, - "velocityY": -1.4198881637340148, + "x": 2.380781360334255, + "y": 4.745442480322685, + "heading": -0.12257922703942895, + "angularVelocity": -0.44484493110577494, + "velocityX": 1.4122919614360556, + "velocityY": -1.5620949477312027, "moduleForcesX": [ - -46.8930408063945, - -43.75302961542786, - -51.4208655887188, - -48.80743987389535 + -63.10751721072769, + -63.62818548216768, + -61.345490726061854, + -62.00924380931458 ], "moduleForcesY": [ - 51.77006647658862, - 54.43088247512983, - 47.26280662177527, - 49.932717396293974 + 29.679520572670825, + 28.58177154469767, + 33.18021447558313, + 31.952578589554413 ], - "timestamp": 0.8606058203599731 + "timestamp": 0.8188955759860805 }, { - "x": 2.433416232746065, - "y": 4.4643250767817815, - "heading": -0.2952821236280862, - "angularVelocity": -0.3194990416382979, - "velocityX": 1.1105250446261894, - "velocityY": -1.1833893284845862, + "x": 2.474640833839395, + "y": 4.630368598491031, + "heading": -0.1600449633075639, + "angularVelocity": -0.45751543135421574, + "velocityX": 1.1461714565026784, + "velocityY": -1.4052326719814436, "moduleForcesX": [ - -46.834172015543345, - -43.8038207106873, - -51.45084121327245, - -48.97366881140082 + -60.43135462935912, + -60.713068060914686, + -59.65856184219576, + -59.96434194640932 ], "moduleForcesY": [ - 51.91054751178551, - 54.480374777947134, - 47.331348947919075, - 49.87523949636651 + 35.07512707135166, + 34.59110445663475, + 36.37630093566144, + 35.87554621698124 ], - "timestamp": 0.9466664023959704 + "timestamp": 0.9007851335846886 }, { - "x": 2.5098805109805107, - "y": 4.382843730787292, - "heading": -0.3172895530520021, - "angularVelocity": -0.2557202020164514, - "velocityX": 0.888493622368971, - "velocityY": -0.9467905530509265, + "x": 2.547034756190077, + "y": 4.528712298571689, + "heading": -0.19799267869255702, + "angularVelocity": -0.46340115269638116, + "velocityX": 0.8840433929992599, + "velocityY": -1.2413829418592635, "moduleForcesX": [ - -46.77185013607419, - -43.83510322729932, - -51.45045906865701, - -49.085367782981976 + -59.40670461633923, + -59.54176328078391, + -59.033208502260386, + -59.17356671352153 ], "moduleForcesY": [ - 52.003993622508574, - 54.49394996948038, - 47.37501122701898, - 49.81059264285791 + 36.87137260564648, + 36.65461497196282, + 37.467114192996426, + 37.246739709029626 ], - "timestamp": 1.0327269844319678 + "timestamp": 0.9826746911832966 }, { - "x": 2.5672321904712163, - "y": 4.3217289912490875, - "heading": -0.33380008105844566, - "angularVelocity": -0.1918477381378256, - "velocityX": 0.6664105458858951, - "velocityY": -0.7101362560644467, + "x": 2.5981356234905864, + "y": 4.44076146673782, + "heading": -0.23613927516023567, + "angularVelocity": -0.46582980280169317, + "velocityX": 0.6240217776116697, + "velocityY": -1.0740176698103814, "moduleForcesX": [ - -46.721839228792774, - -43.85751027569671, - -51.44391879316928, - -49.163980062749964 + -58.86437819861236, + -58.918013476268975, + -58.70626834903893, + -58.760815319074105 ], "moduleForcesY": [ - 52.06961690331074, - 54.49746975666322, - 47.406083494827506, - 49.75817477281888 + 37.774145083580734, + 37.690963323617666, + 38.019592701279585, + 37.93575823083028 ], - "timestamp": 1.1187875664679652 + "timestamp": 1.0645642487819047 }, { - "x": 2.6054684419481404, - "y": 4.280983903135222, - "heading": -0.3448087987822844, - "angularVelocity": -0.12791823461321336, - "velocityX": 0.444294595953872, - "velocityY": -0.47344657843923665, + "x": 2.6280498749209062, + "y": 4.36668930468894, + "heading": -0.2743130502727876, + "angularVelocity": -0.46616169670453655, + "velocityX": 0.3652999516369684, + "velocityY": -0.9045373332203905, "moduleForcesX": [ - -46.68832390905574, - -43.873154666936216, - -51.438399514205486, - -49.21567045651427 + -58.52598156640852, + -58.532799162489525, + -58.50395042521087, + -58.51078511724211 ], "moduleForcesY": [ - 52.11282780371421, - 54.49861617678228, - 47.42729008575955, - 49.72306941004055 + 38.321865139050104, + 38.31150880550724, + 38.35550907746569, + 38.3451396295913 ], - "timestamp": 1.2048481485039626 + "timestamp": 1.1464538063805128 }, { - "x": 2.6245872974433224, - "y": 4.260610580440736, - "heading": -0.3503132845744241, - "angularVelocity": -0.06396059220042477, - "velocityX": 0.2221557770547791, - "velocityY": -0.23673233693882528, + "x": 2.6368497069838694, + "y": 4.3066114512341285, + "heading": -0.31239867185928216, + "angularVelocity": -0.46508520382019075, + "velocityX": 0.10745975825258396, + "velocityY": -0.733644865311249, "moduleForcesX": [ - -46.672848976425186, - -43.88250079839491, - -51.436915163186654, - -49.242971039133984 + -58.29266277626512, + -58.27259460380822, + -58.3652538106269, + -58.34536522287156 ], "moduleForcesY": [ - 52.135797056459324, - 54.500625069799426, - 47.43940924267183, - 49.70711869156378 + 38.69272698107036, + 38.7227879276336, + 38.58309676980216, + 38.61300918912735 ], - "timestamp": 1.29090873053996 + "timestamp": 1.2283433639791208 }, { "x": 2.624587297439575, "y": 4.260610580444336, "heading": -0.350313284574424, - "angularVelocity": 4.86945709754994e-16, - "velocityX": -2.1781193472847508e-14, - "velocityY": 2.3655888321777277e-14, + "angularVelocity": -0.4629969171525455, + "velocityX": -0.14974326280281844, + "velocityY": -0.5617428172622025, "moduleForcesX": [ - -46.676116985699565, - -43.88573472382722, - -51.440890855557896, - -49.24709237978015 + -58.12061439137176, + -58.08608612818397, + -58.26328189836039, + -58.2294247822304 ], "moduleForcesY": [ - 52.139555439371115, - 54.50503144060741, - 47.44278460160171, - 49.711156539011256 + 38.962670441077805, + 39.01386737811412, + 38.74894148261589, + 38.799539468823816 ], - "timestamp": 1.3769693125759574 + "timestamp": 1.3102329215777289 }, { - "x": 2.6097025403261815, - "y": 4.272883049188608, - "heading": -0.3486185943542768, - "angularVelocity": 0.023711121663719633, - "velocityX": -0.20825887874704344, - "velocityY": 0.17170925805626883, + "x": 2.587333207762282, + "y": 4.2275605158811125, + "heading": -0.3906561506913106, + "angularVelocity": -0.45994348660554407, + "velocityX": -0.42472876981164054, + "velocityY": -0.37679925575190565, "moduleForcesX": [ - -53.325527820192114, - -52.543899565580986, - -55.33489027006052, - -54.67599479757407 + -57.98909826227752, + -57.94886879340227, + -58.18608810349869, + -58.14709239976308 ], "moduleForcesY": [ - 45.30344546948554, - 46.204887640771894, - 42.82491499809031, - 43.659875176737565 + 39.171262180325584, + 39.23046156421673, + 38.877985037763544, + 38.93599048454856 ], - "timestamp": 1.4484416893016012 + "timestamp": 1.397945580397238 }, { - "x": 2.580134116922562, - "y": 4.297663630247455, - "heading": -0.3451094282478584, - "angularVelocity": 0.04909821482314941, - "velocityX": -0.4137042155527122, - "velocityY": 0.34671550166182774, + "x": 2.5262212871298537, + "y": 4.211106588413693, + "heading": -0.43034629999514007, + "angularVelocity": -0.45250195169094765, + "velocityX": -0.6967286302216157, + "velocityY": -0.18758897163586385, "moduleForcesX": [ - -52.56862851207183, - -51.668428542826135, - -54.74294626604449, - -53.98380292140949 + -57.23539499202029, + -57.144466338933306, + -57.72649841775235, + -57.64291104202704 ], "moduleForcesY": [ - 46.169623323178776, - 47.17145541851404, - 43.5681756135052, - 44.50151083261666 + 40.24658036814662, + 40.37467240535863, + 39.53878833074971, + 39.65961739205876 ], - "timestamp": 1.519914066027245 + "timestamp": 1.4856582392167472 }, { - "x": 2.5361681561986593, - "y": 4.335274736263326, - "heading": -0.3396203018012238, - "angularVelocity": 0.07680067038585842, - "velocityX": -0.6151461937353943, - "velocityY": 0.5262327598304285, + "x": 2.4417769879525806, + "y": 4.211962057230158, + "heading": -0.4686374057597009, + "angularVelocity": -0.4365516480734518, + "velocityX": -0.9627378797280479, + "velocityY": 0.009753082714672557, "moduleForcesX": [ - -51.50047411154375, - -50.41891834157215, - -53.90421586456722, - -52.99033254736792 + -55.72076727914722, + -55.51641572278753, + -56.81091067061447, + -56.64110655562729 ], "moduleForcesY": [ - 47.344772853066466, - 48.49067038940593, - 44.586953730344504, - 45.66446768873032 + 42.28603999562416, + 42.55126275898099, + 40.80924397426319, + 41.041748680703016 ], - "timestamp": 1.5913864427528888 + "timestamp": 1.5733708980362564 }, { - "x": 2.4782427587314393, - "y": 4.386182986369333, - "heading": -0.3319081747833718, - "angularVelocity": 0.10790360375738635, - "velocityX": -0.810458532923371, - "velocityY": 0.7122786797293141, + "x": 2.3355650057145154, + "y": 4.232002277874596, + "heading": -0.503478852500652, + "angularVelocity": -0.3972225584068319, + "velocityX": -1.2109082505028035, + "velocityY": 0.2284758085563137, "moduleForcesX": [ - -49.88208352874225, - -48.50022068908951, - -52.625503387992744, - -51.45217423757492 + -51.2927800550753, + -50.51860226979334, + -54.177916968511006, + -53.6319910684121 ], "moduleForcesY": [ - 49.02799707238364, - 50.389801569506666, - 46.06811701007498, - 47.36853338734092 + 47.48303091571001, + 48.29512533624039, + 44.16017520097052, + 44.809508828083096 ], - "timestamp": 1.6628588194785325 + "timestamp": 1.6610835568557656 }, { - "x": 2.407106964769289, - "y": 4.45111875799722, - "heading": -0.321583297914309, - "angularVelocity": 0.14445968277546212, - "velocityX": -0.9952907323070566, - "velocityY": 0.9085436161792976, + "x": 2.230262329026912, + "y": 4.2803026993216955, + "heading": -0.5182440105434433, + "angularVelocity": -0.1683355429137465, + "velocityX": -1.2005413825647344, + "velocityY": 0.5506664841443313, "moduleForcesX": [ - -47.15780192777166, - -45.2126943325825, - -50.44841035705035, - -48.77740463936025 + -2.1988028491138176, + 18.75163370930781, + -17.449445210541747, + 9.653164245490055 ], "moduleForcesY": [ - 51.624929028521905, - 53.32888659245528, - 48.40981741843909, - 50.08380727693866 + 69.51849752515592, + 66.91237895442454, + 67.17046621738278, + 68.54244686361893 ], - "timestamp": 1.7343311962041763 + "timestamp": 1.7487962156752748 }, { - "x": 2.3242974518893162, - "y": 4.531358566520759, - "heading": -0.3079313440530546, - "angularVelocity": 0.1910102124288775, - "velocityX": -1.158622623127807, - "velocityY": 1.1226688178584086, + "x": 2.152462213917938, + "y": 4.319517061002876, + "heading": -0.5266433649846194, + "angularVelocity": -0.0957598886434293, + "velocityX": -0.8869884479162654, + "velocityY": 0.4470775622235401, "moduleForcesX": [ - -41.72003068858196, - -38.48311349131453, - -45.98995227278055, - -43.116081797134584 + 67.79520676550965, + 67.25325195865238, + 65.1211910170664, + 64.67812621423997 ], "moduleForcesY": [ - 56.06234348401295, - 58.3198188676031, - 52.60707005821105, - 54.9720866974948 + -16.84436623215845, + -18.954280887505558, + -25.269117070430408, + -26.430366277427 ], - "timestamp": 1.80580357292982 + "timestamp": 1.836508874494784 }, { - "x": 2.2342218638275644, - "y": 4.629362025502949, - "heading": -0.2893536832656072, - "angularVelocity": 0.25992784399271474, - "velocityX": -1.260285334646795, - "velocityY": 1.3712074961147915, + "x": 2.1009759036389104, + "y": 4.3464600486777165, + "heading": -0.5314994814723972, + "angularVelocity": -0.05536391842561765, + "velocityX": -0.586988366011551, + "velocityY": 0.3071733092757232, "moduleForcesX": [ - -27.000280396790707, - -19.781400792593253, - -32.92142671337878, - -25.680048193575647 + 64.46794051526028, + 64.23813702837967, + 62.409666658641854, + 62.28438604366849 ], "moduleForcesY": [ - 64.36040574604013, - 66.91361057866158, - 61.51512045582401, - 64.8450531339154 + -27.11638680674935, + -27.668250188778345, + -31.56542143100178, + -31.82242904032056 ], - "timestamp": 1.8772759496554638 + "timestamp": 1.9242215333142931 }, { - "x": 2.1541934591617964, - "y": 4.743577081279941, - "heading": -0.2632851786920285, - "angularVelocity": 0.3647353812470825, - "velocityX": -1.119710973304069, - "velocityY": 1.598030752209193, + "x": 2.075350046157825, + "y": 4.360164165496833, + "heading": -0.533708602141933, + "angularVelocity": -0.025185881938447372, + "velocityX": -0.2921568884807754, + "velocityY": 0.15623875736483453, "moduleForcesX": [ - 28.898022383948128, - 40.38279327621932, - 31.518367214748288, - 44.919632243477565 + 63.13274035219152, + 63.00549492508412, + 61.47635196590758, + 61.41979412286576 ], "moduleForcesY": [ - 63.33426630830682, - 56.72995559800382, - 61.95891551538514, - 53.10093359505647 + -30.15395861597918, + -30.42427133590258, + -33.40120497268921, + -33.50998064492499 ], - "timestamp": 1.9487483263811076 + "timestamp": 2.0119341921338023 }, { - "x": 2.0933736359860933, - "y": 4.858226801594099, - "heading": -0.2335644163554314, - "angularVelocity": 0.41583565145382007, - "velocityX": -0.850955654977675, - "velocityY": 1.6041123219358235, + "x": 2.075350046157837, + "y": 4.360164165496826, + "heading": -0.533708602141933, + "angularVelocity": 1.9003761794839432e-18, + "velocityX": -1.0839332683477329e-17, + "velocityY": -8.204493501249472e-18, "moduleForcesX": [ - 69.47635686741319, - 69.65947112210823, - 69.73585524619895, - 69.7189941107446 + 62.43601779411841, + 62.3533183381444, + 61.007584822299386, + 60.978323649518536 ], "moduleForcesY": [ - 6.227625887406924, - 4.078499983627095, - -1.256444304150436, - -2.7455509544386563 + -31.598846071636423, + -31.764904680822614, + -34.275480740090025, + -34.33047069669531 ], - "timestamp": 2.0202207031067516 + "timestamp": 2.0996468509533113 }, { - "x": 2.0511871628096467, - "y": 4.967985225687469, - "heading": -0.2022159861765387, - "angularVelocity": 0.4386090349181448, - "velocityX": -0.5902486393714047, - "velocityY": 1.53567614863841, + "x": 2.0700552992840477, + "y": 4.371185945791571, + "heading": -0.5307171268562944, + "angularVelocity": 0.052514598698894153, + "velocityX": -0.09294795401590893, + "velocityY": 0.193484589996311, "moduleForcesX": [ - 67.96496385579776, - 67.99440402036385, - 67.10639875219877, - 67.1820726301549 + -29.15063286138222, + -24.04295915317239, + -36.04668160750333, + -31.647450844160158 ], "moduleForcesY": [ - -16.191122120647563, - -16.095511247494485, - -19.44670942058393, - -19.20731800544632 + 63.602098862195575, + 65.69767587500125, + 59.95949454907031, + 62.38598801471467 ], - "timestamp": 2.0916930798323956 + "timestamp": 2.156611492400752 }, { - "x": 2.0270139823956215, - "y": 5.070867612388492, - "heading": -0.1700681728530796, - "angularVelocity": 0.4497935397741312, - "velocityX": -0.3382171104112806, - "velocityY": 1.439470621969348, + "x": 2.0597833774444276, + "y": 4.393372035050221, + "heading": -0.524594516664425, + "angularVelocity": 0.10748088702565431, + "velocityX": -0.18032101280049811, + "velocityY": 0.3894712350488606, "moduleForcesX": [ - 65.5463060074924, - 65.64862612212953, - 64.96948366218025, - 65.09044905349134 + -27.476620846712287, + -21.825401194686584, + -34.64827459504736, + -29.68674215533022 ], "moduleForcesY": [ - -24.323259893322444, - -24.052061862241104, - -25.825731707590776, - -25.525206862715248 + 64.33617296186765, + 66.45985070451147, + 60.770089849061215, + 63.333306240651716 ], - "timestamp": 2.1631654565580396 + "timestamp": 2.2135761338481927 }, { - "x": 2.020448891699325, - "y": 5.165875707863209, - "heading": -0.13755294679311533, - "angularVelocity": 0.45493416547221555, - "velocityX": -0.09185493341902934, - "velocityY": 1.3292981154257553, + "x": 2.044957147315877, + "y": 4.426895638359724, + "heading": -0.5151629333116078, + "angularVelocity": 0.16556908133125822, + "velocityX": -0.2602707530816304, + "velocityY": 0.5884984519815477, "moduleForcesX": [ - 63.950064201059824, - 64.03180591913006, - 63.654913960217954, - 63.74125341653127 + -25.288712533466384, + -18.900927962794356, + -32.77675220463139, + -27.01590875565174 ], "moduleForcesY": [ - -28.315148832203725, - -28.131653555528967, - -28.97311608059264, - -28.784456776293474 + 65.21852387263795, + 67.34050489755609, + 61.788991765210525, + 64.50595442910968 ], - "timestamp": 2.2346378332836836 + "timestamp": 2.2705407752956335 }, { - "x": 2.031218238194587, - "y": 5.2524144264055215, - "heading": -0.10493277773470358, - "angularVelocity": 0.45640246697905146, - "velocityX": 0.150678441370407, - "velocityY": 1.2107995068082167, + "x": 2.026164201599616, + "y": 4.4719674608650095, + "heading": -0.5021887224153403, + "angularVelocity": 0.22775901974628154, + "velocityX": -0.32990545079793476, + "velocityY": 0.791224544911283, "moduleForcesX": [ - 62.88066267543294, - 62.91207596743938, - 62.79228260986751, - 62.82409534697334 + -22.307319507469934, + -14.900037659160601, + -30.148527798271417, + -23.210714805289566 ], "moduleForcesY": [ - -30.650558779807053, - -30.586409677611073, - -30.831354646388686, - -30.76685591760273 + 66.28610428285815, + 68.32491313667404, + 63.09885991865561, + 65.9548404457898 ], - "timestamp": 2.3061102100093276 + "timestamp": 2.327505416743074 }, { - "x": 2.05912709236145, - "y": 5.330090045928955, - "heading": -0.072383245243066, - "angularVelocity": 0.45541416114660316, - "velocityX": 0.3904844851873833, - "velocityY": 1.086792170543661, + "x": 2.0042669459715285, + "y": 4.528837148205876, + "heading": -0.4853528018471346, + "angularVelocity": 0.2955503649354062, + "velocityX": -0.38440083307276873, + "velocityY": 0.9983331044633778, + "moduleForcesX": [ + -18.02560615257705, + -9.175322282634115, + -26.214745646530282, + -17.46079465681506 + ], + "moduleForcesY": [ + 67.5606623787124, + 69.30954637447027, + 64.81204160137834, + 67.68228538813717 + ], + "timestamp": 2.384470058190515 + }, + { + "x": 1.9806218469741539, + "y": 4.597763043773167, + "heading": -0.4641981938958335, + "angularVelocity": 0.3713638392830007, + "velocityX": -0.41508378524932976, + "velocityY": 1.2099768174769732, + "moduleForcesX": [ + -11.453582503159748, + -0.552199333246785, + -19.79399970106862, + -8.106339127595051 + ], + "moduleForcesY": [ + 68.95738867398684, + 69.88879052102858, + 67.02262284825274, + 69.39413552946995 + ], + "timestamp": 2.4414346996379557 + }, + { + "x": 1.9575322526001036, + "y": 4.678830239834948, + "heading": -0.4380421531456962, + "angularVelocity": 0.4591627382447514, + "velocityX": -0.4053320408477288, + "velocityY": 1.4231143039244516, + "moduleForcesX": [ + -0.5334552598468943, + 13.009918369721786, + -8.032358249742447, + 8.238973565498672 + ], + "moduleForcesY": [ + 69.8666713444081, + 68.63601950178051, + 69.37382880885353, + 69.32921346604414 + ], + "timestamp": 2.4983993410853964 + }, + { + "x": 1.9389906812367639, + "y": 4.77112606241245, + "heading": -0.4060463560603266, + "angularVelocity": 0.5616781967264929, + "velocityX": -0.32549263705008313, + "velocityY": 1.6202300274752126, + "moduleForcesX": [ + 18.715982262905385, + 33.707081998178694, + 15.897712534120208, + 35.51802148738027 + ], + "moduleForcesY": [ + 67.2633143302821, + 61.14210942301303, + 67.92400233992704, + 60.03846848663157 + ], + "timestamp": 2.555363982532837 + }, + { + "x": 1.93014912520743, + "y": 4.87068791134505, + "heading": -0.3685234212138124, + "angularVelocity": 0.6587057145112619, + "velocityX": -0.1552112995829446, + "velocityY": 1.7477832986004502, + "moduleForcesX": [ + 47.983848434946395, + 57.21667950593689, + 53.72495772708293, + 62.54171738572598 + ], + "moduleForcesY": [ + 50.64853650489456, + 39.96838943911703, + 44.402717653560856, + 30.875613138399547 + ], + "timestamp": 2.612328623980278 + }, + { + "x": 1.933418171455475, + "y": 4.971531596301135, + "heading": -0.32709464429833723, + "angularVelocity": 0.7272717928664595, + "velocityX": 0.057387287358971396, + "velocityY": 1.7702856086460224, + "moduleForcesX": [ + 67.96928457704135, + 69.07682719509752, + 69.65789543520896, + 69.80083544623831 + ], + "moduleForcesY": [ + 15.813702501669626, + 10.130822196720576, + 3.8406949342180523, + -0.5188082796813483 + ], + "timestamp": 2.6692932654277186 + }, + { + "x": 1.9485966046329035, + "y": 5.069631244361437, + "heading": -0.28344891936809435, + "angularVelocity": 0.7661897594934095, + "velocityX": 0.2664535893100171, + "velocityY": 1.722114728850099, "moduleForcesX": [ - 62.127913109347276, - 62.10220705945469, - 62.18868836322343, - 62.16316779871923 + 68.8247416901388, + 68.711613893991, + 67.18557930818034, + 67.18883251525818 ], "moduleForcesY": [ - -32.17057458836276, - -32.21997133029195, - -32.052845514471784, - -32.1021124146997 + -11.868262004119803, + -12.595528771030496, + -19.06951150843806, + -19.117544695444614 ], - "timestamp": 2.3775825867349716 + "timestamp": 2.7262579068751593 }, { - "x": 2.11398267592862, - "y": 5.407564590406167, - "heading": -0.035074691263133305, - "angularVelocity": 0.4522495337757732, - "velocityX": 0.6649523885483815, - "velocityY": 0.939136547950644, + "x": 1.9749895957767274, + "y": 5.1628267728214325, + "heading": -0.238677619130099, + "angularVelocity": 0.7859489518476898, + "velocityX": 0.46332234300422453, + "velocityY": 1.6360241386928376, "moduleForcesX": [ - 61.580309305924985, - 61.49718284699979, - 61.749856139779475, - 61.66816132300872 + 64.61918569816321, + 64.82164567363998, + 63.15374635838673, + 63.45210887594756 ], "moduleForcesY": [ - -33.23357506637483, - -33.386760848755074, - -32.91728042389969, - -33.06968489028737 + -26.610781563662005, + -26.130445473862178, + -29.925633288523958, + -29.30220507461484 ], - "timestamp": 2.4600780924096304 + "timestamp": 2.7832225483226 }, { - "x": 2.191082677171206, - "y": 5.472153160220583, - "heading": 0.0016521215955720662, - "angularVelocity": 0.44519774208758894, - "velocityX": 0.9345963836882241, - "velocityY": 0.7829344083945624, + "x": 2.0119893346706585, + "y": 5.249897971911408, + "heading": -0.19345274501313633, + "angularVelocity": 0.7939113275853787, + "velocityX": 0.649521140724979, + "velocityY": 1.528513071925737, + "moduleForcesX": [ + 60.77468821171452, + 60.99722299740147, + 60.0777269685818, + 60.31974783111097 + ], + "moduleForcesY": [ + -34.556593215262986, + -34.166181049759004, + -35.755885616715915, + -35.34978878089667 + ], + "timestamp": 2.840187189770041 + }, + { + "x": 2.05912709236145, + "y": 5.330090045928955, + "heading": -0.14821270337422277, + "angularVelocity": 0.7941775896308426, + "velocityX": 0.8274915191783369, + "velocityY": 1.407751755824489, "moduleForcesX": [ - 60.45923369475699, - 60.23455621610159, - 60.84356535380034, - 60.625874690348816 + 57.8734037104444, + 57.88504600107734, + 57.84871177168228, + 57.86037646750411 ], "moduleForcesY": [ - -35.214733524291056, - -35.59671720906772, - -34.545862040838756, - -34.92548722348504 + -39.255748814527, + -39.23866862035294, + -39.292170409475375, + -39.27508028582055 ], - "timestamp": 2.542573598084289 + "timestamp": 2.8971518312174815 }, { - "x": 2.289728178771266, - "y": 5.522730614286461, - "heading": 0.037276639055293805, - "angularVelocity": 0.4318358578252923, - "velocityX": 1.1957681909289004, - "velocityY": 0.6130934487194334, + "x": 2.1419011012670555, + "y": 5.425958346458515, + "heading": -0.08687313324288204, + "angularVelocity": 0.7872633235969319, + "velocityX": 1.0623638414637935, + "velocityY": 1.2304226576887645, "moduleForcesX": [ - 58.52744287678723, - 57.99177342881693, - 59.27377627441346, - 58.761492424372356 + 55.744309007626626, + 55.4487335715346, + 56.215540263624625, + 55.92748705805922 ], "moduleForcesY": [ - -38.31446039097551, - -39.11845718880393, - -37.14787466755693, - -37.95082764754945 + -42.276636780585044, + -42.66266684218691, + -41.64740235699094, + -42.03249730192701 ], - "timestamp": 2.625069103758948 + "timestamp": 2.9750667655130183 }, { - "x": 2.408421432901453, - "y": 5.557250524578989, - "heading": 0.07081951118626838, - "angularVelocity": 0.40660241860066865, - "velocityX": 1.4387844903195497, - "velocityY": 0.41844595069491913, + "x": 2.2414261398578335, + "y": 5.506181529743673, + "heading": -0.02721344128448102, + "angularVelocity": 0.7657029104601152, + "velocityX": 1.2773550987449058, + "velocityY": 1.029625244640082, "moduleForcesX": [ - 54.50941828585798, - 53.13358487259185, - 55.95915631243375, - 54.647136092353065 + 51.02163650907591, + 49.70119985383043, + 52.48797262102768, + 51.22069659742051 ], "moduleForcesY": [ - -43.802094987807024, - -45.45697038399386, - -41.92981110097996, - -43.62112365277983 + -47.85047801815092, + -49.21787446602869, + -46.23470435722217, + -47.63176527887239 ], - "timestamp": 2.7075646094336068 + "timestamp": 3.052981699808555 }, { - "x": 2.542474709173094, - "y": 5.571243535937252, - "heading": 0.09992456939909769, - "angularVelocity": 0.3528078041928978, - "velocityX": 1.6249767211345831, - "velocityY": 0.1696214981769599, + "x": 2.354490568089435, + "y": 5.567942649448705, + "heading": 0.028816977553997255, + "angularVelocity": 0.719122968466496, + "velocityX": 1.4511265298991398, + "velocityY": 0.7926737057977623, "moduleForcesX": [ - 42.694250355720705, - 38.054672270282296, - 45.591148182962975, - 40.87638058379842 + 41.924684435294694, + 37.78420628587397, + 44.83708919222699, + 40.69029874809796 ], "moduleForcesY": [ - -55.307949027779564, - -58.58896847735615, - -52.92949409903056, - -56.63909921586958 + -55.96662276450357, + -58.83646046865295, + -53.653582227691004, + -56.85649586466601 ], - "timestamp": 2.7900601151082656 + "timestamp": 3.130896634104092 }, { - "x": 2.668629243751066, - "y": 5.561025654990008, - "heading": 0.11917705216572184, - "angularVelocity": 0.2333761410288788, - "velocityX": 1.529229180843634, - "velocityY": -0.12385985482443941, + "x": 2.473359530149637, + "y": 5.60763951076386, + "heading": 0.07772134675261151, + "angularVelocity": 0.6276636134109629, + "velocityX": 1.5256248771169323, + "velocityY": 0.5094897618033285, "moduleForcesX": [ - -12.382402533780613, - -24.607511319028973, - -17.558309914075192, - -31.441206870964095 + 22.049102020071857, + 11.270972185107798, + 25.093613830164124, + 12.425514812528533 ], "moduleForcesY": [ - -68.63720397945411, - -65.28047130453552, - -67.43128061515037, - -62.222247817779476 + -66.32736429857982, + -68.97836591275289, + -65.21334822172665, + -68.75568538466969 ], - "timestamp": 2.8725556207829244 + "timestamp": 3.2088115683996286 }, { - "x": 2.7698521306860013, - "y": 5.54500330305322, - "heading": 0.13176004586708878, - "angularVelocity": 0.15252944507008542, - "velocityX": 1.2270109278923758, - "velocityY": -0.19422090026996, + "x": 2.583125662677607, + "y": 5.626543406923644, + "heading": 0.11601498179594151, + "angularVelocity": 0.49148004024593883, + "velocityX": 1.4087945208503867, + "velocityY": 0.24262224348518271, "moduleForcesX": [ - -66.46018133159996, - -66.9674196584085, - -68.9644976388019, - -69.02558396413289 + -16.70068482620098, + -30.482853391136345, + -24.37959308564339, + -39.528854162952214 ], "moduleForcesY": [ - -21.47227954276657, - -19.910515932777027, - -11.024044579675353, - -10.78336011685823 + -67.844870324751, + -62.8857363057114, + -65.43315052803204, + -57.59599684491268 ], - "timestamp": 2.955051126457583 + "timestamp": 3.2867265026951653 }, { - "x": 2.845436141122318, - "y": 5.530116306121047, - "heading": 0.14009236993377866, - "angularVelocity": 0.10100336980206083, - "velocityX": 0.916219735867702, - "velocityY": -0.1804582814458009, + "x": 2.673631838796821, + "y": 5.633378148129679, + "heading": 0.14407534512325684, + "angularVelocity": 0.3601410125160406, + "velocityX": 1.1616024185545568, + "velocityY": 0.08772055406104393, "moduleForcesX": [ - -69.91668485679588, - -69.92377181874357, - -69.57042974483447, - -69.70602424237894 + -51.90525981906655, + -57.051226471868404, + -61.89152614197463, + -64.20272534700426 ], "moduleForcesY": [ - 0.35481804279217377, - -0.526848643787446, - 6.976161517175589, - 5.555870742457722 + -46.802489344466096, + -40.40908629215477, + -32.43095750906469, + -27.650831182455995 ], - "timestamp": 3.037546632132242 + "timestamp": 3.364641436990702 }, { - "x": 2.8956151191054844, - "y": 5.519017848354159, - "heading": 0.14518444674969433, - "angularVelocity": 0.06172550582309565, - "velocityX": 0.6082631723848511, - "velocityY": -0.13453409128336982, + "x": 2.7420691702187887, + "y": 5.6345006557115855, + "heading": 0.16357365067508123, + "angularVelocity": 0.2502511967457482, + "velocityX": 0.8783596115525151, + "velocityY": 0.01440683473656765, "moduleForcesX": [ - -69.4187453897527, - -69.56564259987941, - -68.65234610284632, - -68.9344391693208 + -65.04850079574194, + -65.99350180832509, + -69.15045925519797, + -69.13827378457684 ], "moduleForcesY": [ - 8.590706564201634, - 7.346324469918923, - 13.40936902595393, - 11.897426701626598 + -25.651238870662098, + -23.15939656800476, + -10.354928047129325, + -10.547196980897708 ], - "timestamp": 3.1200421378069008 + "timestamp": 3.442556371286239 }, { - "x": 2.9206175804135657, - "y": 5.513061523434405, - "heading": 0.14756797074007869, - "angularVelocity": 0.02889277386548866, - "velocityX": 0.3030766476923993, - "velocityY": -0.07220180908862774, + "x": 2.7877749257651523, + "y": 5.633422199584155, + "heading": 0.1758142407370851, + "angularVelocity": 0.15710197502797701, + "velocityX": 0.5866109746431721, + "velocityY": -0.013841455905480958, "moduleForcesX": [ - -68.79757631258838, - -69.02339707894248, - -67.96227334834312, - -68.30019620815122 + -68.75220978257724, + -68.781223473534, + -69.94627706176999, + -69.93907335246656 ], "moduleForcesY": [ - 12.723388654681191, - 11.450987875380186, - 16.62115678249245, - 15.184154939108327 + -12.86879255012935, + -12.770261545400503, + 0.3760238076675933, + -1.5977856312836116 ], - "timestamp": 3.2025376434815596 + "timestamp": 3.5204713055817756 }, { - "x": 2.9206175804138184, - "y": 5.5130615234375, - "heading": 0.14756797074007869, - "angularVelocity": -2.1366473717756725e-18, - "velocityX": 2.759428296475241e-16, - "velocityY": 1.8508305640470853e-16, + "x": 2.810619354248047, + "y": 5.6322150230407715, + "heading": 0.18164838967891364, + "angularVelocity": 0.0748784426833914, + "velocityX": 0.2931970448212678, + "velocityY": -0.015493519365684846, "moduleForcesX": [ - -68.30769120451006, - -68.57184176455583, - -67.47562728593, - -67.83343378701302 + -69.7650098576931, + -69.68462891027929, + -69.679901493562, + -69.87274192709826 ], "moduleForcesY": [ - 15.178954859633443, - 13.946231227068052, - 18.53346910587473, - 17.184707214005112 + -5.227252657372436, + -6.289797322807944, + 6.278310865228548, + 3.6678202564057916 ], - "timestamp": 3.2850331491562184 + "timestamp": 3.5983862398773123 }, { - "x": 2.9008988708903125, - "y": 5.5307170603623375, - "heading": 0.14700286177553085, - "angularVelocity": -0.006751511016409849, - "velocityX": -0.23558480387769798, - "velocityY": 0.2109355181352119, + "x": 2.810619354248047, + "y": 5.6322150230407715, + "heading": 0.18164838967891364, + "angularVelocity": 1.7820258276732997e-21, + "velocityX": -3.71309242331424e-21, + "velocityY": -1.8466663513371007e-20, "moduleForcesX": [ - -52.09999330596725, - -52.529400631239085, - -51.73320472903666, - -52.16512079541669 + -69.96925933574576, + -69.94810083182944, + -69.26326437970721, + -69.61542540763588 ], "moduleForcesY": [ - 46.71445570132471, - 46.23149173401103, - 47.1208354788092, - 46.6426390261249 + -0.3390303743865954, + -1.9603647789452194, + 9.931322934759566, + 7.100594511510579 ], - "timestamp": 3.3687342589037015 + "timestamp": 3.676301174172849 }, { - "x": 2.861623494471227, - "y": 5.566204209893025, - "heading": 0.14583871741812887, - "angularVelocity": -0.013908350330240071, - "velocityX": -0.4692336401967019, - "velocityY": 0.42397465993079464, + "x": 2.791342988452625, + "y": 5.642521810363919, + "heading": 0.18283388161538688, + "angularVelocity": 0.015584006009315527, + "velocityX": -0.25339944638282685, + "velocityY": 0.1354889313364025, "moduleForcesX": [ - -51.6659111374572, - -52.12885129412831, - -51.276879306050894, - -51.74245743341866 + -61.62075540054056, + -60.98102694031713, + -62.39838441045431, + -61.79343760463002 ], "moduleForcesY": [ - 47.185653590179186, - 46.67421839348704, - 47.60875054548638, - 47.10282037910355 + 33.15309974417089, + 34.31372499920319, + 31.663594414249264, + 32.826466274276484 ], - "timestamp": 3.4524353686511846 + "timestamp": 3.752372239325454 }, { - "x": 2.803016239597012, - "y": 5.619762027969135, - "heading": 0.14402910636298827, - "angularVelocity": -0.021619917114522248, - "velocityX": -0.7001968676942122, - "velocityY": 0.6398698673768739, + "x": 2.753067309241903, + "y": 5.663631883203031, + "heading": 0.18530131117107015, + "angularVelocity": 0.03243584864670181, + "velocityX": -0.5031568722475193, + "velocityY": 0.2775046306603334, "moduleForcesX": [ - -51.06312930480856, - -51.57332722665041, - -50.64375367097991, - -51.156724254575956 + -60.74636949447462, + -59.995692501352465, + -61.60810614324199, + -60.89635912716705 ], "moduleForcesY": [ - 47.8258847940151, - 47.27589622960445, - 48.27053972557859, - 47.72716661234478 + 34.71442361257027, + 35.99400899987649, + 33.15894228397032, + 34.44613293316394 ], - "timestamp": 3.5361364783986677 + "timestamp": 3.828443304478059 }, { - "x": 2.725409512823363, - "y": 5.691733398122384, - "heading": 0.14150668290126167, - "angularVelocity": -0.030136081460894748, - "velocityX": -0.9271887436445178, - "velocityY": 0.8598615995220361, + "x": 2.6962228110923627, + "y": 5.696261557261839, + "heading": 0.18919034306075003, + "angularVelocity": 0.05112366813686836, + "velocityX": -0.7472551887567889, + "velocityY": 0.42893673163836527, "moduleForcesX": [ - -50.170225601037366, - -50.75200909732681, - -49.70741389665356, - -50.29204690115321 + -59.39626570686199, + -58.46567557598025, + -60.37984931281294, + -59.49315400848136 ], "moduleForcesY": [ - 48.74540511502207, - 48.14020759141128, - 49.218329972809045, - 48.621580569011485 + 36.95663241834814, + 38.40914674537991, + 35.32399090200627, + 36.79466428895038 ], - "timestamp": 3.619837588146151 + "timestamp": 3.9045143696306637 }, { - "x": 2.6293437718673225, - "y": 5.782650350616714, - "heading": 0.1381651310810073, - "angularVelocity": -0.039922431498496204, - "velocityX": -1.147723623091234, - "velocityY": 1.0862096426984997, + "x": 2.621558016690057, + "y": 5.741526222171845, + "heading": 0.19471996442816977, + "angularVelocity": 0.07269020561664075, + "velocityX": -0.9815137234179877, + "velocityY": 0.5950313015757177, "moduleForcesX": [ - -48.71431630344826, - -49.4167391577109, - -48.18511463537286, - -48.890119594937715 + -57.07004054859922, + -55.81145405690308, + -58.24064731344431, + -57.02953301737525 ], "moduleForcesY": [ - 50.17522088948343, - 49.48473182662626, - 50.68521987830446, - 50.00663478484992 + 40.42700972423277, + 42.1440264982245, + 38.71744543220449, + 40.476187384360045 ], - "timestamp": 3.703538697893634 + "timestamp": 3.9805854347832685 }, { - "x": 2.5158455258965873, - "y": 5.893444520165239, - "heading": 0.13381239728196173, - "angularVelocity": -0.05200329854838183, - "velocityX": -1.355994518259599, - "velocityY": 1.3236881799931342, + "x": 2.5306316551845427, + "y": 5.801358526557014, + "heading": 0.2022754435822446, + "angularVelocity": 0.09932132722103867, + "velocityX": -1.1952818239511769, + "velocityY": 0.7865317024960887, "moduleForcesX": [ - -45.93506137526542, - -46.87946903712664, - -45.29576298382204, - -46.24053944561288 + -52.296776213679415, + -50.323936372341834, + -53.758106723210474, + -51.816584350831235 ], "moduleForcesY": [ - 52.687688259487246, - 51.851007353746446, - 53.24100600936717, - 52.42425460814814 + 46.39111947683355, + 48.51956857347833, + 44.682603452825234, + 46.914906732410614 ], - "timestamp": 3.787239807641117 + "timestamp": 4.056656499935873 }, { - "x": 2.3875470683794213, - "y": 6.02610991590263, - "heading": 0.12800509474442898, - "angularVelocity": -0.06938142821559826, - "velocityX": -1.5328166842349815, - "velocityY": 1.5849896874494018, + "x": 2.427863523047455, + "y": 5.879498749258094, + "heading": 0.2126425688241101, + "angularVelocity": 0.13628210964403, + "velocityX": -1.350949036022118, + "velocityY": 1.0272003230706037, "moduleForcesX": [ - -38.731601936491195, - -40.35076624830184, - -37.91551455677665, - -39.51613010702398 + -39.09732377550607, + -35.17129355710182, + -40.71904985918127, + -36.62147361504708 ], "moduleForcesY": [ - 58.094777198180715, - 56.98539695236494, - 58.63708159838333, - 57.57358536077538 + 57.87480113678438, + 60.336542269027305, + 56.73087731606344, + 59.45243630995032 ], - "timestamp": 3.8709409173886002 + "timestamp": 4.132727565088478 }, { - "x": 2.2585637323391317, - "y": 6.185053475715637, - "heading": 0.11910747265558115, - "angularVelocity": -0.10630231923689437, - "velocityX": -1.5409991149465732, - "velocityY": 1.8989420845305607, + "x": 2.3297978130131964, + "y": 5.978878854714064, + "heading": 0.22690067635521388, + "angularVelocity": 0.18743141695861396, + "velocityX": -1.2891328632973713, + "velocityY": 1.3064113833111803, "moduleForcesX": [ - 0.7933919130137743, - -3.975029098028374, - 0.1994458839598195, - -4.26048134968995 + 10.545576170240913, + 16.557527776184923, + 13.352744563742135, + 19.74884527271297 ], "moduleForcesY": [ - 69.51699522375354, - 69.4045334427663, - 69.55309629363511, - 69.42011317516652 + 68.92487436784589, + 67.74390307628403, + 68.40688100444298, + 66.8566796699288 ], - "timestamp": 3.9546420271360834 + "timestamp": 4.208798630241083 }, { - "x": 2.155822904626011, - "y": 6.341651289925754, - "heading": 0.10835395117931683, - "angularVelocity": -0.1284752556892115, - "velocityX": -1.2274727064325863, - "velocityY": 1.8709168195383514, + "x": 2.2518195232296936, + "y": 6.086732649237896, + "heading": 0.24339103584492305, + "angularVelocity": 0.2167757143485253, + "velocityX": -1.0250716172709238, + "velocityY": 1.4178031332605687, "moduleForcesX": [ - 69.29850135005749, - 69.22376482749928, - 69.52084242344242, - 69.4745660602554 + 63.3765480815569, + 63.68121368261706, + 64.96422299639283, + 65.1554779681156 ], "moduleForcesY": [ - -7.254272274145535, - -7.823192974180219, - -4.62731192687223, - -5.1017659452285775 + 29.25944062691299, + 28.61722194785657, + 25.535838612439772, + 25.07538960318053 ], - "timestamp": 4.038343136883566 + "timestamp": 4.284869695393688 }, { - "x": 2.0772115888518736, - "y": 6.487483095533371, - "heading": 0.0969715087930836, - "angularVelocity": -0.13598914543167476, - "velocityX": -0.939190842449076, - "velocityY": 1.7422923808692223, + "x": 2.195636505947272, + "y": 6.195898602093869, + "heading": 0.2608681511360735, + "angularVelocity": 0.229747214083171, + "velocityX": -0.7385596240793223, + "velocityY": 1.4350522453836667, "moduleForcesX": [ - 63.72237766804347, - 63.53223394842242, - 64.05032016117235, - 63.867552477264695 + 69.71870355427134, + 69.69809382001247, + 69.8203423782785, + 69.80580410665718 ], "moduleForcesY": [ - -28.628204478983974, - -29.043494705306433, - -27.884225907046652, - -28.295904148066843 + 4.902598216652911, + 5.226579532658753, + 3.1528242764653718, + 3.5174449089562745 ], - "timestamp": 4.122044246631049 + "timestamp": 4.360940760546293 }, { - "x": 2.021637442637373, - "y": 6.620330494187104, - "heading": 0.08532200026076654, - "angularVelocity": -0.1391798575606646, - "velocityX": -0.6639594908779467, - "velocityY": 1.587164125330234, + "x": 2.1612401984623397, + "y": 6.303431320252094, + "heading": 0.278796165903002, + "angularVelocity": 0.23567455945257776, + "velocityX": -0.45216019278723374, + "velocityY": 1.4135823909196665, "moduleForcesX": [ - 60.87885944738181, - 60.7694927196997, - 61.04027054010648, - 60.93220988562005 + 69.7517709773252, + 69.77145706900596, + 69.6934539088278, + 69.71663436820566 ], "moduleForcesY": [ - -34.375983359242426, - -34.56803319978518, - -34.087899420795566, - -34.2797476711687 + -4.983366244420907, + -4.712773441080035, + -5.745679950464318, + -5.468339576313232 ], - "timestamp": 4.205745356378531 + "timestamp": 4.437011825698898 }, { - "x": 1.9885261521674162, - "y": 6.739204165667878, - "heading": 0.07357290264571206, - "angularVelocity": -0.14036967550974822, - "velocityX": -0.39558962325593, - "velocityY": 1.4202161922459862, + "x": 2.1484700695340293, + "y": 6.407813780862964, + "heading": 0.29689533472311547, + "angularVelocity": 0.2379244826374525, + "velocityX": -0.1678710414096705, + "velocityY": 1.372170356777184, "moduleForcesX": [ - 59.37783973737232, - 59.33213553424243, - 59.44147378924915, - 59.395945957037256 + 69.23031154412588, + 69.24884860764138, + 69.18986060193748, + 69.20902830787257 ], "moduleForcesY": [ - -36.95821427423069, - -37.031312892639825, - -36.85560981002678, - -36.92870573976187 + -10.008624570555261, + -9.881273409427275, + -10.285342371577995, + -10.157228071808754 ], - "timestamp": 4.289446466126014 + "timestamp": 4.513082890851503 }, { - "x": 1.977528756045062, - "y": 6.843547542559394, - "heading": 0.061820288525356495, - "angularVelocity": -0.14041168815821406, - "velocityX": -0.13138889263033743, - "velocityY": 1.246618800486609, + "x": 2.157177448272705, + "y": 6.508133888244629, + "heading": 0.31499648569215616, + "angularVelocity": 0.2379505391797553, + "velocityX": 0.11446374151864286, + "velocityY": 1.318768275170261, "moduleForcesX": [ - 58.46396442972924, - 58.46225971251114, - 58.46628338376228, - 58.46457888275662 + 68.74379080687748, + 68.74410650557303, + 68.74320786149968, + 68.74352365244148 ], "moduleForcesY": [ - -38.41537453860932, - -38.417962696829065, - -38.41184043411793, - -38.41442860346031 + -13.001771747122337, + -13.000114222047666, + -13.004860154304824, + -13.00320254851976 ], - "timestamp": 4.373147575873497 + "timestamp": 4.5891539560041075 }, { - "x": 1.9884117094887013, - "y": 6.933004553123894, - "heading": 0.05012639395544798, - "angularVelocity": -0.13971014966457962, - "velocityX": 0.1300216147344071, - "velocityY": 1.0687673173254597, + "x": 2.184724710816574, + "y": 6.59922785852166, + "heading": 0.3320932322725307, + "angularVelocity": 0.23656836371790554, + "velocityX": 0.38117256954556666, + "velocityY": 1.2604708967109606, "moduleForcesX": [ - 57.85196007751825, - 57.88125766772754, - 57.812373545441886, - 57.841731008840526 + 68.34746493653803, + 68.32541439579168, + 68.38381933803426, + 68.36206742712406 ], "moduleForcesY": [ - -39.3486892907348, - -39.30566232094738, - -39.40689132743343, - -39.363869274238354 + -14.974761050304606, + -15.074567367293154, + -14.807552238846894, + -14.90716016282338 ], - "timestamp": 4.4568486856209795 + "timestamp": 4.6614237485988665 }, { - "x": 2.021008014678955, - "y": 7.007328033447266, - "heading": 0.03853480959994491, - "angularVelocity": -0.13848782161284204, - "velocityX": 0.38943695354526725, - "velocityY": 0.8879629018104452, + "x": 2.2313650052611718, + "y": 6.685359777525342, + "heading": 0.34895139183908297, + "angularVelocity": 0.23326702570023544, + "velocityX": 0.6453636127908321, + "velocityY": 1.1918107955097217, "moduleForcesX": [ - 57.41445936428743, - 57.46628160975792, - 57.34427766903865, - 57.39628164773986 + 67.69387597978809, + 67.62571914819677, + 67.79222902236084, + 67.72581400551105 ], "moduleForcesY": [ - -39.99658250377327, - -39.922209282283085, - -40.09723139207876, - -40.02287498111711 + -17.65912805639218, + -17.917221977463345, + -17.276880276884725, + -17.53424163554366 ], - "timestamp": 4.540549795368462 + "timestamp": 4.733693541193626 }, { - "x": 2.0826299228686898, - "y": 7.070370365951682, - "heading": 0.02599725344124828, - "angularVelocity": -0.1367266901870693, - "velocityX": 0.6720097158166497, - "velocityY": 0.687499968419269, + "x": 2.2967574046117547, + "y": 6.76538030405312, + "heading": 0.3653570894706221, + "angularVelocity": 0.2270062918753815, + "velocityX": 0.9048372356242996, + "velocityY": 1.1072472142888359, "moduleForcesX": [ - 57.08917572518026, - 57.15769997977353, - 56.9954356988548, - 57.06427617407771 + 66.47960800608301, + 66.30177808290506, + 66.69591112278235, + 66.52423830099455 ], "moduleForcesY": [ - -40.47369675811929, - -40.37699177913841, - -40.605690518380186, - -40.509013116235465 + -21.74308831253873, + -22.277371179768945, + -21.068430447014848, + -21.60216639071113 ], - "timestamp": 4.632247737749421 + "timestamp": 4.805963333788385 }, { - "x": 2.169672617334707, - "y": 7.114364526461086, - "heading": 0.013740199626283799, - "angularVelocity": -0.13366770831190322, - "velocityX": 0.9492327984450655, - "velocityY": 0.4797726026430948, + "x": 2.3801398957317264, + "y": 6.837336652626878, + "heading": 0.3809436958576124, + "angularVelocity": 0.21567249368473568, + "velocityX": 1.153766852321353, + "velocityY": 0.9956628625910408, "moduleForcesX": [ - 56.01669479138227, - 56.14220393478205, - 55.84946987132304, - 55.97590888149232 + 63.806559308723315, + 63.31882271721067, + 64.26880839630248, + 63.798069788806266 ], "moduleForcesY": [ - -41.93127449705145, - -41.76334146364884, - -42.153954194749936, - -41.98616963230991 + -28.59447611383761, + -29.655406522966526, + -27.53587934185067, + -28.605861210958697 ], - "timestamp": 4.72394568013038 + "timestamp": 4.878233126383144 }, { - "x": 2.2812482301938513, - "y": 7.138190544722294, - "heading": 0.0019682714125729057, - "angularVelocity": -0.12837723408034055, - "velocityX": 1.2167733535710465, - "velocityY": 0.25983154852058693, + "x": 2.479259804153409, + "y": 6.897447135971791, + "heading": 0.39498518087177925, + "angularVelocity": 0.19429258767770655, + "velocityX": 1.3715261226427125, + "velocityY": 0.831751153375685, "moduleForcesX": [ - 54.06938340379889, - 54.308966898728, - 53.770349274089426, - 54.012513868838 + 56.13643988602569, + 54.52558414545473, + 57.08952254401824, + 55.48617464288215 ], "moduleForcesY": [ - -44.39082318658512, - -44.09795887662825, - -44.753051093939305, - -44.46103930411709 + -41.60713844028672, + -43.692054420376515, + -40.280169475319, + -42.45608521234071 ], - "timestamp": 4.815643622511339 + "timestamp": 4.950502918977903 }, { - "x": 2.4153058153444134, - "y": 7.139605579002843, - "heading": -0.008889495736468035, - "angularVelocity": -0.1184079693298827, - "velocityX": 1.4619475818218677, - "velocityY": 0.015431471712328844, + "x": 2.584931627951079, + "y": 6.9390229938132615, + "heading": 0.4060308240467457, + "angularVelocity": 0.15283900476791354, + "velocityX": 1.4621852367863801, + "velocityY": 0.575286801701526, "moduleForcesX": [ - 49.535744448116404, - 50.08883535472126, - 48.95285722529716, - 49.512750306129256 + 26.148906673767343, + 20.715268485938005, + 26.009248473481666, + 20.06651880527857 ], "moduleForcesY": [ - -49.35481713361861, - -48.79483073718232, - -49.9344054264043, - -49.38065310701591 + -64.70640976971409, + -66.64626277199693, + -64.74052136771891, + -66.82336232717894 ], - "timestamp": 4.907341564892298 + "timestamp": 5.022772711572662 }, { - "x": 2.5635950141167547, - "y": 7.1126996745657065, - "heading": -0.017499861469921024, - "angularVelocity": -0.0938992251069123, - "velocityX": 1.6171485962309224, - "velocityY": -0.29341884600255425, + "x": 2.6792925575993936, + "y": 6.964515706893577, + "heading": 0.4138293757225624, + "angularVelocity": 0.10790887030139666, + "velocityX": 1.3056759437159087, + "velocityY": 0.3527436867469866, "moduleForcesX": [ - 30.82355947672089, - 33.05783383581679, - 29.658555665503656, - 31.855767521726126 + -36.39457522337274, + -39.28022118556542, + -41.130153635623216, + -43.6418492259847 ], "moduleForcesY": [ - -62.655847172898895, - -61.51032510158116, - -63.223044950314765, - -62.148500220510925 + -59.56113873853256, + -57.71718933439606, + -56.381055028824214, + -54.48253610093349 ], - "timestamp": 4.999039507273257 + "timestamp": 5.095042504167421 }, { - "x": 2.6834005581658715, - "y": 7.072012772379466, - "heading": -0.022151846620554527, - "angularVelocity": -0.05073161981418278, - "velocityX": 1.3065237989836214, - "velocityY": -0.4437057267013505, + "x": 2.756321629159372, + "y": 6.98065148897458, + "heading": 0.41934868952054644, + "angularVelocity": 0.0763709649608753, + "velocityX": 1.0658543326934196, + "velocityY": 0.22327145964679496, "moduleForcesX": [ - -64.12298736950346, - -63.25910030393463, - -62.330306323818874, - -61.2590262700717 + -60.27208113538249, + -60.51737707772543, + -62.48632553835477, + -62.5793289359259 ], "moduleForcesY": [ - -27.440487603067957, - -29.33631206040231, - -31.317162411294717, - -33.33134662941462 + -35.36566968241656, + -34.96009847227816, + -31.286074162611634, + -31.117684633539717 ], - "timestamp": 5.090737449654216 + "timestamp": 5.16731229676218 }, { - "x": 2.7720201720116067, - "y": 7.037183326012226, - "heading": -0.02500990952650958, - "angularVelocity": -0.031168233787252347, - "velocityX": 0.9664296880719148, - "velocityY": -0.37982800389550603, + "x": 2.814586704942504, + "y": 6.990746632959886, + "heading": 0.42314075402152007, + "angularVelocity": 0.052470947609287326, + "velocityX": 0.8062161754060119, + "velocityY": 0.13968690960429975, "moduleForcesX": [ - -68.53351263579651, - -68.45472035856257, - -68.92469478793043, - -68.86842514528999 + -66.06720809870912, + -65.97454972543122, + -67.12642001354656, + -67.00203153454379 ], "moduleForcesY": [ - 13.79021249565681, - 14.160173532909322, - 11.675894673290463, - 11.9841604417936 + -22.902929804587398, + -23.180877284771793, + -19.584025329151217, + -20.01956198271935 ], - "timestamp": 5.182435392035175 + "timestamp": 5.239582089356939 }, { - "x": 2.830552574824699, - "y": 7.012504794671367, - "heading": -0.026692143130144577, - "angularVelocity": -0.018345380059196448, - "velocityX": 0.6383175162541149, - "velocityY": -0.26912851727456555, + "x": 2.853604993354379, + "y": 6.996545885023089, + "heading": 0.42550601388588527, + "angularVelocity": 0.03272819499604782, + "velocityX": 0.5398976115880579, + "velocityY": 0.08024448189192064, "moduleForcesX": [ - -66.1245923787409, - -65.98169477222959, - -66.56070394354207, - -66.43347254576503 + -68.01213832568928, + -67.89244829561473, + -68.61715921102547, + -68.49693767599965 ], "moduleForcesY": [ - 22.81538982238073, - 23.221752664345384, - 21.508695963392192, - 21.894565620883082 + -16.33804890659345, + -16.838898924869497, + -13.577428279677779, + -14.183521442505858 ], - "timestamp": 5.274133334416134 + "timestamp": 5.311851881951698 }, { - "x": 2.8596270084381104, - "y": 6.999704360961915, - "heading": -0.027461342770395154, - "angularVelocity": -0.008388406765443529, - "velocityX": 0.31706745919666984, - "velocityY": -0.1395934675707037, + "x": 2.8731672763824463, + "y": 6.999098300933838, + "heading": 0.4266275995625518, + "angularVelocity": 0.015519425701905927, + "velocityX": 0.27068408979336367, + "velocityY": 0.035317880667813904, "moduleForcesX": [ - -64.76590607807921, - -64.6158756095251, - -65.15697508947473, - -65.01741451245994 + -68.85222618332939, + -68.74728425154547, + -69.24154633684182, + -69.1454169059897 ], "moduleForcesY": [ - 26.47522061163077, - 26.837605390916714, - 25.49694542258738, - 25.848954107534034 + -12.403847845099842, + -12.982110998649153, + -10.008033607336229, + -10.662885240301636 ], - "timestamp": 5.365831276797093 + "timestamp": 5.384121674546457 }, { - "x": 2.8596270084381104, - "y": 6.999704360961914, - "heading": -0.02746134277039515, - "angularVelocity": 1.6056171256353062e-18, - "velocityX": -1.8304903305133357e-17, - "velocityY": -4.784339232160718e-18, + "x": 2.8731672763824463, + "y": 6.999098300933838, + "heading": 0.4266275995625518, + "angularVelocity": 5.733280321426045e-22, + "velocityX": 4.854312894103748e-21, + "velocityY": -1.3121681595321098e-20, "moduleForcesX": [ - -63.93945199318899, - -63.793171888386496, - -64.29137064962106, - -64.1528183330455 + -69.27866045130386, + -69.19177764514303, + -69.54932705970307, + -69.47427349656373 ], "moduleForcesY": [ - 28.437823121296837, - 28.763444348886257, - 27.63256619914302, - 27.951659076114336 + -9.808624171483924, + -10.412359295598337, + -7.660954895057263, + -8.324479793607422 ], - "timestamp": 5.457529219178052 + "timestamp": 5.456391467141216 } ], "trajectoryWaypoints": [ @@ -29539,18 +29695,29 @@ "controlIntervalCount": 16 }, { - "timestamp": 1.3769693125759574, - "isStopPoint": true, + "timestamp": 1.3102329215777289, + "isStopPoint": false, "x": 2.624587297439575, "y": 4.260610580444336, "heading": -0.350313284574424, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 2.0996468509533113, + "isStopPoint": true, + "x": 2.075350046157837, + "y": 4.360164165496826, + "heading": -0.533708602141933, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, "controlIntervalCount": 14 }, { - "timestamp": 2.3775825867349716, + "timestamp": 2.8971518312174815, "isStopPoint": false, "x": 2.05912709236145, "y": 5.330090045928955, @@ -29558,36 +29725,36 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 11 + "controlIntervalCount": 10 }, { - "timestamp": 3.2850331491562184, + "timestamp": 3.676301174172849, "isStopPoint": true, - "x": 2.9206175804138184, - "y": 5.5130615234375, - "heading": 0.14756797074007866, + "x": 2.810619354248047, + "y": 5.6322150230407715, + "heading": 0.18164838967891364, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 12 }, { - "timestamp": 4.540549795368462, + "timestamp": 4.5891539560041075, "isStopPoint": false, - "x": 2.021008014678955, - "y": 7.007328033447266, + "x": 2.157177448272705, + "y": 6.508133888244629, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 10 + "controlIntervalCount": 12 }, { - "timestamp": 5.457529219178052, + "timestamp": 5.456391467141216, "isStopPoint": true, - "x": 2.8596270084381104, - "y": 6.999704360961914, - "heading": -0.027461342770395147, + "x": 2.8731672763824463, + "y": 6.999098300933838, + "heading": 0.4266275995625518, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -29609,19 +29776,13 @@ }, { "scope": [ - 5 - ], - "type": "StopPoint" - }, - { - "scope": [ - 3 + 4 ], "type": "StopPoint" }, { "scope": [ - 1 + 2 ], "type": "StopPoint" } diff --git a/src/main/deploy/choreo/center 4.1.traj b/src/main/deploy/choreo/center 4.1.traj index 3f2af776..cdfd7321 100644 --- a/src/main/deploy/choreo/center 4.1.traj +++ b/src/main/deploy/choreo/center 4.1.traj @@ -3,10 +3,10 @@ { "x": 1.4015021324157717, "y": 5.563943386077881, - "heading": 2.230586975435435e-18, - "angularVelocity": -4.993708780228108e-16, - "velocityX": 2.2186084345747425e-14, - "velocityY": -2.3644285540186748e-14, + "heading": 7.388244526996465e-19, + "angularVelocity": 3.0465745999077404e-18, + "velocityX": -3.4647017198299383e-17, + "velocityY": -1.192018758735892e-17, "moduleForcesX": [ 0, 0, @@ -22,340 +22,529 @@ "timestamp": 0 }, { - "x": 1.4206212389589077, - "y": 5.543569823071968, - "heading": -0.005440740553276968, - "angularVelocity": -0.06321989027440103, - "velocityX": 0.22215869438518032, - "velocityY": -0.23673512918379025, + "x": 1.4219202193105986, + "y": 5.548947800945218, + "heading": -0.0011051964959714333, + "angularVelocity": -0.01349618349862027, + "velocityX": 0.24933688120418268, + "velocityY": -0.18311962565685555, "moduleForcesX": [ - 47.940963091989595, - 43.779015007865034, - 51.71856759026114, - 47.81380065892401 + 56.284779515152344, + 55.67887470588661, + 57.10064703101138, + 56.51836963286635 ], "moduleForcesY": [ - -50.97920586080499, - -54.592033915440794, - -47.13866785517865, - -51.09102404890194 + -41.57896584233712, + -42.38577268175027, + -40.45035897321267, + -41.2588054071458 ], - "timestamp": 0.08606058203599731 + "timestamp": 0.08188955759860805 }, { - "x": 1.4588579982416516, - "y": 5.50282426541773, - "heading": -0.016323802947849745, - "angularVelocity": -0.12645815467526472, - "velocityX": 0.4443004964254837, - "velocityY": -0.4734520342542199, + "x": 1.4627096279184146, + "y": 5.518896687993435, + "heading": -0.003378676387590463, + "angularVelocity": -0.027762757038732957, + "velocityX": 0.4981026861537893, + "velocityY": -0.3669712455778003, "moduleForcesX": [ - 47.897364078006646, - 43.771363296078064, - 51.712153560329, - 47.85692298524638 + 56.14974361131909, + 55.50531984469861, + 57.01450005495424, + 56.39643729223106 ], "moduleForcesY": [ - -51.01338140036875, - -54.59132698392599, - -47.13783450766829, - -51.0426999647787 + -41.752695886724744, + -42.60428285649574, + -40.56281363910674, + -41.416368028381115 ], - "timestamp": 0.17212116407199463 + "timestamp": 0.1637791151972161 }, { - "x": 1.5162104171628858, - "y": 5.441708865073567, - "heading": -0.032652672648373536, - "angularVelocity": -0.18973691920489047, - "velocityX": 0.6664191378657773, - "velocityY": -0.7101439350063614, + "x": 1.5238075547381984, + "y": 5.473710157546218, + "heading": -0.0069048134518868666, + "angularVelocity": -0.04305966679635681, + "velocityX": 0.7461015618045014, + "velocityY": -0.5517984438105422, "moduleForcesX": [ - 47.832382527825025, - 43.760444196177374, - 51.70298147300229, - 47.92205717348266 + 55.968126398369286, + 55.27198555154887, + 56.89895032239671, + 56.233074234563546 ], "moduleForcesY": [ - -51.06506924736401, - -54.59072757046299, - -47.13719675772626, - -50.9707235368353 + -41.984633832597545, + -42.89526110767094, + -40.71292465128025, + -41.6259762326198 ], - "timestamp": 0.25818174610799194 + "timestamp": 0.24566867279582416 }, { - "x": 1.5926756045431116, - "y": 5.360226735801809, - "heading": -0.05443308174346003, - "angularVelocity": -0.25308228900808055, - "velocityX": 0.8885041862345195, - "velocityY": -0.9467996533549011, + "x": 1.605125212106644, + "y": 5.413276440660695, + "heading": -0.011802142574701541, + "angularVelocity": -0.05980407351594935, + "velocityX": 0.9930162007595612, + "velocityY": -0.7379905162231216, "moduleForcesX": [ - 47.74512096081025, - 43.74635348449958, - 51.68931736462727, - 48.00815391044375 + 55.71155946356058, + 54.941547780042846, + 56.7359549514754, + 56.00213234888038 ], "moduleForcesY": [ - -51.133333759147, - -54.588485401020556, - -47.13677120729667, - -50.87397800267328 + -42.30893410579651, + -43.30196365103173, + -40.92326397000725, + -41.91947734002761 ], - "timestamp": 0.34424232814398925 + "timestamp": 0.3275582303944322 }, { - "x": 1.6882490058265867, - "y": 5.258382764481358, - "heading": -0.08167367566723759, - "angularVelocity": -0.31652811634851347, - "velocityX": 1.1105363105122372, - "velocityY": -1.1833985890311751, + "x": 1.7065275334793777, + "y": 5.337428085536629, + "heading": -0.018249242256111287, + "angularVelocity": -0.07872920394823069, + "velocityX": 1.2382814652604819, + "velocityY": -0.9262274378844899, "moduleForcesX": [ - 47.63372466857229, - 43.72840022744862, - 51.668243433528644, - 48.113016067033115 + 55.322537771508856, + 54.4375833932818, + 56.48888780544757, + 55.64994626737733 ], "moduleForcesY": [ - -51.216216787013636, - -54.58157282722093, - -47.135721060383055, - -50.750174463810936 + -42.79328988841797, + -43.91048339133851, + -41.238988003610295, + -42.36091518217925 ], - "timestamp": 0.43030291017998656 + "timestamp": 0.40944778799304027 }, { - "x": 1.8029224178515728, - "y": 5.136185717697895, - "heading": -0.11438761196088189, - "angularVelocity": -0.3801268306536648, - "velocityX": 1.332473116574703, - "velocityY": -1.4198956586668827, + "x": 1.8277843888865313, + "y": 5.2458866793139265, + "heading": -0.02654556216924506, + "angularVelocity": -0.10131108478813869, + "velocityX": 1.4807364817078694, + "velocityY": -1.117864217454393, "moduleForcesX": [ - 47.49348736287193, - 43.70277282082887, - 51.63393088382914, - 48.231135604313586 + 54.66422556146924, + 53.57551056932187, + 56.07036150264626, + 55.04634059384568 ], "moduleForcesY": [ - -51.30877200765891, - -54.563746594487974, - -47.12995843648461, - -50.59351350853768 + -43.59337180528042, + -44.9199390004416, + -41.76546496247963, + -43.10085570220449 ], - "timestamp": 0.5163634922159839 + "timestamp": 0.4913373455916483 }, { - "x": 1.936676738585046, - "y": 4.9936559524582, - "heading": -0.1525981886270299, - "angularVelocity": -0.4439962612620674, - "velocityX": 1.5541879641384613, - "velocityY": -1.6561561857250287, + "x": 1.96841676914479, + "y": 5.138098820317635, + "heading": -0.03729581263726324, + "angularVelocity": -0.13127742758990868, + "velocityX": 1.717342044361563, + "velocityY": -1.3162588998792917, "moduleForcesX": [ - 47.30730481341717, - 43.65083702902553, - 51.569007669599586, - 48.34309714132929 + 53.31294529372992, + 51.76935791756607, + 55.208047286851695, + 53.77390871564015 ], "moduleForcesY": [ - -51.393363168260414, - -54.515933915159074, - -47.100223160042596, - -50.38283518179482 + -45.16334356265244, + -46.915928207338055, + -42.81846532880878, + -44.59597492097393 ], - "timestamp": 0.6024240742519812 + "timestamp": 0.5732269031902564 }, { - "x": 2.089417359472379, - "y": 4.830894184799883, - "heading": -0.19638882103609856, - "angularVelocity": -0.5088349552543711, - "velocityX": 1.7748034844634022, - "velocityY": -1.8912464178636816, + "x": 2.1268410825486406, + "y": 5.0124497352130994, + "heading": -0.05234368618465831, + "angularVelocity": -0.1837581492521726, + "velocityX": 1.9346094673069207, + "velocityY": -1.5343724986323788, "moduleForcesX": [ - 46.92254775633583, - 43.388297546565106, - 51.333326631328006, - 48.27968404555044 + 49.01689962945185, + 45.73289983149253, + 52.41775458712863, + 49.40090115837192 ], "moduleForcesY": [ - -51.315242953431444, - -54.28258670469318, - -46.8588510495494, - -49.928189244812266 + -49.59779004971247, + -52.616015053993074, + -45.96240551955732, + -49.157805609768765 ], - "timestamp": 0.6884846562879785 + "timestamp": 0.6551164607888644 }, { - "x": 2.2231712411306024, - "y": 4.688364652999476, - "heading": -0.23481358247355089, - "angularVelocity": -0.4464850286666412, - "velocityX": 1.5541828604889207, - "velocityY": -1.6561534735218788, + "x": 2.2651293964122, + "y": 4.873361744519432, + "heading": -0.08615107243119119, + "angularVelocity": -0.4128412368795543, + "velocityX": 1.6887173153556436, + "velocityY": -1.6984826243100626, "moduleForcesX": [ - -46.804641906327866, - -43.5737849154587, - -51.151878946250015, - -48.397943993853495 + -42.85767308274732, + -56.04462264975442, + -58.165526762903994, + -65.39829691272111 ], "moduleForcesY": [ - 51.411338017062235, - 54.12246522092687, - 47.04773571475752, - 49.80566610187949 + -53.371294254255105, + -39.88846466311047, + -34.7319701928346, + -20.48370015983788 ], - "timestamp": 0.7745452383239758 + "timestamp": 0.7370060183874725 }, { - "x": 2.3378438009906706, - "y": 4.5661682512486585, - "heading": -0.2677858501447284, - "angularVelocity": -0.383128569329872, - "velocityX": 1.3324632166436747, - "velocityY": -1.4198881637340148, + "x": 2.380781360334255, + "y": 4.745442480322685, + "heading": -0.12257922703942895, + "angularVelocity": -0.44484493110577494, + "velocityX": 1.4122919614360556, + "velocityY": -1.5620949477312027, "moduleForcesX": [ - -46.8930408063945, - -43.75302961542786, - -51.4208655887188, - -48.80743987389535 + -63.10751721072769, + -63.62818548216768, + -61.345490726061854, + -62.00924380931458 ], "moduleForcesY": [ - 51.77006647658862, - 54.43088247512983, - 47.26280662177527, - 49.932717396293974 + 29.679520572670825, + 28.58177154469767, + 33.18021447558313, + 31.952578589554413 ], - "timestamp": 0.8606058203599731 + "timestamp": 0.8188955759860805 }, { - "x": 2.433416232746065, - "y": 4.4643250767817815, - "heading": -0.2952821236280862, - "angularVelocity": -0.3194990416382979, - "velocityX": 1.1105250446261894, - "velocityY": -1.1833893284845862, + "x": 2.474640833839395, + "y": 4.630368598491031, + "heading": -0.1600449633075639, + "angularVelocity": -0.45751543135421574, + "velocityX": 1.1461714565026784, + "velocityY": -1.4052326719814436, "moduleForcesX": [ - -46.834172015543345, - -43.8038207106873, - -51.45084121327245, - -48.97366881140082 + -60.43135462935912, + -60.713068060914686, + -59.65856184219576, + -59.96434194640932 ], "moduleForcesY": [ - 51.91054751178551, - 54.480374777947134, - 47.331348947919075, - 49.87523949636651 + 35.07512707135166, + 34.59110445663475, + 36.37630093566144, + 35.87554621698124 ], - "timestamp": 0.9466664023959704 + "timestamp": 0.9007851335846886 }, { - "x": 2.5098805109805107, - "y": 4.382843730787292, - "heading": -0.3172895530520021, - "angularVelocity": -0.2557202020164514, - "velocityX": 0.888493622368971, - "velocityY": -0.9467905530509265, + "x": 2.547034756190077, + "y": 4.528712298571689, + "heading": -0.19799267869255702, + "angularVelocity": -0.46340115269638116, + "velocityX": 0.8840433929992599, + "velocityY": -1.2413829418592635, "moduleForcesX": [ - -46.77185013607419, - -43.83510322729932, - -51.45045906865701, - -49.085367782981976 + -59.40670461633923, + -59.54176328078391, + -59.033208502260386, + -59.17356671352153 ], "moduleForcesY": [ - 52.003993622508574, - 54.49394996948038, - 47.37501122701898, - 49.81059264285791 + 36.87137260564648, + 36.65461497196282, + 37.467114192996426, + 37.246739709029626 ], - "timestamp": 1.0327269844319678 + "timestamp": 0.9826746911832966 }, { - "x": 2.5672321904712163, - "y": 4.3217289912490875, - "heading": -0.33380008105844566, - "angularVelocity": -0.1918477381378256, - "velocityX": 0.6664105458858951, - "velocityY": -0.7101362560644467, + "x": 2.5981356234905864, + "y": 4.44076146673782, + "heading": -0.23613927516023567, + "angularVelocity": -0.46582980280169317, + "velocityX": 0.6240217776116697, + "velocityY": -1.0740176698103814, "moduleForcesX": [ - -46.721839228792774, - -43.85751027569671, - -51.44391879316928, - -49.163980062749964 + -58.86437819861236, + -58.918013476268975, + -58.70626834903893, + -58.760815319074105 ], "moduleForcesY": [ - 52.06961690331074, - 54.49746975666322, - 47.406083494827506, - 49.75817477281888 + 37.774145083580734, + 37.690963323617666, + 38.019592701279585, + 37.93575823083028 ], - "timestamp": 1.1187875664679652 + "timestamp": 1.0645642487819047 }, { - "x": 2.6054684419481404, - "y": 4.280983903135222, - "heading": -0.3448087987822844, - "angularVelocity": -0.12791823461321336, - "velocityX": 0.444294595953872, - "velocityY": -0.47344657843923665, + "x": 2.6280498749209062, + "y": 4.36668930468894, + "heading": -0.2743130502727876, + "angularVelocity": -0.46616169670453655, + "velocityX": 0.3652999516369684, + "velocityY": -0.9045373332203905, "moduleForcesX": [ - -46.68832390905574, - -43.873154666936216, - -51.438399514205486, - -49.21567045651427 + -58.52598156640852, + -58.532799162489525, + -58.50395042521087, + -58.51078511724211 ], "moduleForcesY": [ - 52.11282780371421, - 54.49861617678228, - 47.42729008575955, - 49.72306941004055 + 38.321865139050104, + 38.31150880550724, + 38.35550907746569, + 38.3451396295913 ], - "timestamp": 1.2048481485039626 + "timestamp": 1.1464538063805128 }, { - "x": 2.6245872974433224, - "y": 4.260610580440736, - "heading": -0.3503132845744241, - "angularVelocity": -0.06396059220042477, - "velocityX": 0.2221557770547791, - "velocityY": -0.23673233693882528, + "x": 2.6368497069838694, + "y": 4.3066114512341285, + "heading": -0.31239867185928216, + "angularVelocity": -0.46508520382019075, + "velocityX": 0.10745975825258396, + "velocityY": -0.733644865311249, "moduleForcesX": [ - -46.672848976425186, - -43.88250079839491, - -51.436915163186654, - -49.242971039133984 + -58.29266277626512, + -58.27259460380822, + -58.3652538106269, + -58.34536522287156 ], "moduleForcesY": [ - 52.135797056459324, - 54.500625069799426, - 47.43940924267183, - 49.70711869156378 + 38.69272698107036, + 38.7227879276336, + 38.58309676980216, + 38.61300918912735 ], - "timestamp": 1.29090873053996 + "timestamp": 1.2283433639791208 }, { "x": 2.624587297439575, "y": 4.260610580444336, "heading": -0.350313284574424, - "angularVelocity": 4.86945709754994e-16, - "velocityX": -2.1781193472847508e-14, - "velocityY": 2.3655888321777277e-14, + "angularVelocity": -0.4629969171525455, + "velocityX": -0.14974326280281844, + "velocityY": -0.5617428172622025, "moduleForcesX": [ - -46.676116985699565, - -43.88573472382722, - -51.440890855557896, - -49.24709237978015 + -58.12061439137176, + -58.08608612818397, + -58.26328189836039, + -58.2294247822304 ], "moduleForcesY": [ - 52.139555439371115, - 54.50503144060741, - 47.44278460160171, - 49.711156539011256 + 38.962670441077805, + 39.01386737811412, + 38.74894148261589, + 38.799539468823816 ], - "timestamp": 1.3769693125759574 + "timestamp": 1.3102329215777289 + }, + { + "x": 2.587333207762282, + "y": 4.2275605158811125, + "heading": -0.3906561506913106, + "angularVelocity": -0.45994348660554407, + "velocityX": -0.42472876981164054, + "velocityY": -0.37679925575190565, + "moduleForcesX": [ + -57.98909826227752, + -57.94886879340227, + -58.18608810349869, + -58.14709239976308 + ], + "moduleForcesY": [ + 39.171262180325584, + 39.23046156421673, + 38.877985037763544, + 38.93599048454856 + ], + "timestamp": 1.397945580397238 + }, + { + "x": 2.5262212871298537, + "y": 4.211106588413693, + "heading": -0.43034629999514007, + "angularVelocity": -0.45250195169094765, + "velocityX": -0.6967286302216157, + "velocityY": -0.18758897163586385, + "moduleForcesX": [ + -57.23539499202029, + -57.144466338933306, + -57.72649841775235, + -57.64291104202704 + ], + "moduleForcesY": [ + 40.24658036814662, + 40.37467240535863, + 39.53878833074971, + 39.65961739205876 + ], + "timestamp": 1.4856582392167472 + }, + { + "x": 2.4417769879525806, + "y": 4.211962057230158, + "heading": -0.4686374057597009, + "angularVelocity": -0.4365516480734518, + "velocityX": -0.9627378797280479, + "velocityY": 0.009753082714672557, + "moduleForcesX": [ + -55.72076727914722, + -55.51641572278753, + -56.81091067061447, + -56.64110655562729 + ], + "moduleForcesY": [ + 42.28603999562416, + 42.55126275898099, + 40.80924397426319, + 41.041748680703016 + ], + "timestamp": 1.5733708980362564 + }, + { + "x": 2.3355650057145154, + "y": 4.232002277874596, + "heading": -0.503478852500652, + "angularVelocity": -0.3972225584068319, + "velocityX": -1.2109082505028035, + "velocityY": 0.2284758085563137, + "moduleForcesX": [ + -51.2927800550753, + -50.51860226979334, + -54.177916968511006, + -53.6319910684121 + ], + "moduleForcesY": [ + 47.48303091571001, + 48.29512533624039, + 44.16017520097052, + 44.809508828083096 + ], + "timestamp": 1.6610835568557656 + }, + { + "x": 2.230262329026912, + "y": 4.2803026993216955, + "heading": -0.5182440105434433, + "angularVelocity": -0.1683355429137465, + "velocityX": -1.2005413825647344, + "velocityY": 0.5506664841443313, + "moduleForcesX": [ + -2.1988028491138176, + 18.75163370930781, + -17.449445210541747, + 9.653164245490055 + ], + "moduleForcesY": [ + 69.51849752515592, + 66.91237895442454, + 67.17046621738278, + 68.54244686361893 + ], + "timestamp": 1.7487962156752748 + }, + { + "x": 2.152462213917938, + "y": 4.319517061002876, + "heading": -0.5266433649846194, + "angularVelocity": -0.0957598886434293, + "velocityX": -0.8869884479162654, + "velocityY": 0.4470775622235401, + "moduleForcesX": [ + 67.79520676550965, + 67.25325195865238, + 65.1211910170664, + 64.67812621423997 + ], + "moduleForcesY": [ + -16.84436623215845, + -18.954280887505558, + -25.269117070430408, + -26.430366277427 + ], + "timestamp": 1.836508874494784 + }, + { + "x": 2.1009759036389104, + "y": 4.3464600486777165, + "heading": -0.5314994814723972, + "angularVelocity": -0.05536391842561765, + "velocityX": -0.586988366011551, + "velocityY": 0.3071733092757232, + "moduleForcesX": [ + 64.46794051526028, + 64.23813702837967, + 62.409666658641854, + 62.28438604366849 + ], + "moduleForcesY": [ + -27.11638680674935, + -27.668250188778345, + -31.56542143100178, + -31.82242904032056 + ], + "timestamp": 1.9242215333142931 + }, + { + "x": 2.075350046157825, + "y": 4.360164165496833, + "heading": -0.533708602141933, + "angularVelocity": -0.025185881938447372, + "velocityX": -0.2921568884807754, + "velocityY": 0.15623875736483453, + "moduleForcesX": [ + 63.13274035219152, + 63.00549492508412, + 61.47635196590758, + 61.41979412286576 + ], + "moduleForcesY": [ + -30.15395861597918, + -30.42427133590258, + -33.40120497268921, + -33.50998064492499 + ], + "timestamp": 2.0119341921338023 + }, + { + "x": 2.075350046157837, + "y": 4.360164165496826, + "heading": -0.533708602141933, + "angularVelocity": 1.9003761794839432e-18, + "velocityX": -1.0839332683477329e-17, + "velocityY": -8.204493501249472e-18, + "moduleForcesX": [ + 62.43601779411841, + 62.3533183381444, + 61.007584822299386, + 60.978323649518536 + ], + "moduleForcesY": [ + -31.598846071636423, + -31.764904680822614, + -34.275480740090025, + -34.33047069669531 + ], + "timestamp": 2.0996468509533113 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/center 4.2.traj b/src/main/deploy/choreo/center 4.2.traj index 209de89b..3ac2fda9 100644 --- a/src/main/deploy/choreo/center 4.2.traj +++ b/src/main/deploy/choreo/center 4.2.traj @@ -1,550 +1,529 @@ { "samples": [ { - "x": 2.624587297439575, - "y": 4.260610580444336, - "heading": -0.350313284574424, - "angularVelocity": 4.86945709754994e-16, - "velocityX": -2.1781193472847508e-14, - "velocityY": 2.3655888321777277e-14, + "x": 2.075350046157837, + "y": 4.360164165496826, + "heading": -0.533708602141933, + "angularVelocity": 1.9003761794839432e-18, + "velocityX": -1.0839332683477329e-17, + "velocityY": -8.204493501249472e-18, "moduleForcesX": [ - -46.676116985699565, - -43.88573472382722, - -51.440890855557896, - -49.24709237978015 + 62.43601779411841, + 62.3533183381444, + 61.007584822299386, + 60.978323649518536 ], "moduleForcesY": [ - 52.139555439371115, - 54.50503144060741, - 47.44278460160171, - 49.711156539011256 + -31.598846071636423, + -31.764904680822614, + -34.275480740090025, + -34.33047069669531 ], "timestamp": 0 }, { - "x": 2.6097025403261815, - "y": 4.272883049188608, - "heading": -0.3486185943542768, - "angularVelocity": 0.023711121663719633, - "velocityX": -0.20825887874704344, - "velocityY": 0.17170925805626883, + "x": 2.0700552992840477, + "y": 4.371185945791571, + "heading": -0.5307171268562944, + "angularVelocity": 0.052514598698894153, + "velocityX": -0.09294795401590893, + "velocityY": 0.193484589996311, "moduleForcesX": [ - -53.325527820192114, - -52.543899565580986, - -55.33489027006052, - -54.67599479757407 + -29.15063286138222, + -24.04295915317239, + -36.04668160750333, + -31.647450844160158 ], "moduleForcesY": [ - 45.30344546948554, - 46.204887640771894, - 42.82491499809031, - 43.659875176737565 + 63.602098862195575, + 65.69767587500125, + 59.95949454907031, + 62.38598801471467 ], - "timestamp": 0.07147237672564377 + "timestamp": 0.056964641447440734 }, { - "x": 2.580134116922562, - "y": 4.297663630247455, - "heading": -0.3451094282478584, - "angularVelocity": 0.04909821482314941, - "velocityX": -0.4137042155527122, - "velocityY": 0.34671550166182774, + "x": 2.0597833774444276, + "y": 4.393372035050221, + "heading": -0.524594516664425, + "angularVelocity": 0.10748088702565431, + "velocityX": -0.18032101280049811, + "velocityY": 0.3894712350488606, "moduleForcesX": [ - -52.56862851207183, - -51.668428542826135, - -54.74294626604449, - -53.98380292140949 + -27.476620846712287, + -21.825401194686584, + -34.64827459504736, + -29.68674215533022 ], "moduleForcesY": [ - 46.169623323178776, - 47.17145541851404, - 43.5681756135052, - 44.50151083261666 + 64.33617296186765, + 66.45985070451147, + 60.770089849061215, + 63.333306240651716 ], - "timestamp": 0.14294475345128754 + "timestamp": 0.11392928289488147 }, { - "x": 2.5361681561986593, - "y": 4.335274736263326, - "heading": -0.3396203018012238, - "angularVelocity": 0.07680067038585842, - "velocityX": -0.6151461937353943, - "velocityY": 0.5262327598304285, + "x": 2.044957147315877, + "y": 4.426895638359724, + "heading": -0.5151629333116078, + "angularVelocity": 0.16556908133125822, + "velocityX": -0.2602707530816304, + "velocityY": 0.5884984519815477, "moduleForcesX": [ - -51.50047411154375, - -50.41891834157215, - -53.90421586456722, - -52.99033254736792 + -25.288712533466384, + -18.900927962794356, + -32.77675220463139, + -27.01590875565174 ], "moduleForcesY": [ - 47.344772853066466, - 48.49067038940593, - 44.586953730344504, - 45.66446768873032 + 65.21852387263795, + 67.34050489755609, + 61.788991765210525, + 64.50595442910968 ], - "timestamp": 0.21441713017693131 + "timestamp": 0.1708939243423222 }, { - "x": 2.4782427587314393, - "y": 4.386182986369333, - "heading": -0.3319081747833718, - "angularVelocity": 0.10790360375738635, - "velocityX": -0.810458532923371, - "velocityY": 0.7122786797293141, + "x": 2.026164201599616, + "y": 4.4719674608650095, + "heading": -0.5021887224153403, + "angularVelocity": 0.22775901974628154, + "velocityX": -0.32990545079793476, + "velocityY": 0.791224544911283, "moduleForcesX": [ - -49.88208352874225, - -48.50022068908951, - -52.625503387992744, - -51.45217423757492 + -22.307319507469934, + -14.900037659160601, + -30.148527798271417, + -23.210714805289566 ], "moduleForcesY": [ - 49.02799707238364, - 50.389801569506666, - 46.06811701007498, - 47.36853338734092 + 66.28610428285815, + 68.32491313667404, + 63.09885991865561, + 65.9548404457898 ], - "timestamp": 0.2858895069025751 + "timestamp": 0.22785856578976293 }, { - "x": 2.407106964769289, - "y": 4.45111875799722, - "heading": -0.321583297914309, - "angularVelocity": 0.14445968277546212, - "velocityX": -0.9952907323070566, - "velocityY": 0.9085436161792976, + "x": 2.0042669459715285, + "y": 4.528837148205876, + "heading": -0.4853528018471346, + "angularVelocity": 0.2955503649354062, + "velocityX": -0.38440083307276873, + "velocityY": 0.9983331044633778, "moduleForcesX": [ - -47.15780192777166, - -45.2126943325825, - -50.44841035705035, - -48.77740463936025 + -18.02560615257705, + -9.175322282634115, + -26.214745646530282, + -17.46079465681506 ], "moduleForcesY": [ - 51.624929028521905, - 53.32888659245528, - 48.40981741843909, - 50.08380727693866 + 67.5606623787124, + 69.30954637447027, + 64.81204160137834, + 67.68228538813717 ], - "timestamp": 0.35736188362821886 + "timestamp": 0.28482320723720367 }, { - "x": 2.3242974518893162, - "y": 4.531358566520759, - "heading": -0.3079313440530546, - "angularVelocity": 0.1910102124288775, - "velocityX": -1.158622623127807, - "velocityY": 1.1226688178584086, + "x": 1.9806218469741539, + "y": 4.597763043773167, + "heading": -0.4641981938958335, + "angularVelocity": 0.3713638392830007, + "velocityX": -0.41508378524932976, + "velocityY": 1.2099768174769732, "moduleForcesX": [ - -41.72003068858196, - -38.48311349131453, - -45.98995227278055, - -43.116081797134584 + -11.453582503159748, + -0.552199333246785, + -19.79399970106862, + -8.106339127595051 ], "moduleForcesY": [ - 56.06234348401295, - 58.3198188676031, - 52.60707005821105, - 54.9720866974948 + 68.95738867398684, + 69.88879052102858, + 67.02262284825274, + 69.39413552946995 ], - "timestamp": 0.42883426035386263 + "timestamp": 0.3417878486846444 }, { - "x": 2.2342218638275644, - "y": 4.629362025502949, - "heading": -0.2893536832656072, - "angularVelocity": 0.25992784399271474, - "velocityX": -1.260285334646795, - "velocityY": 1.3712074961147915, + "x": 1.9575322526001036, + "y": 4.678830239834948, + "heading": -0.4380421531456962, + "angularVelocity": 0.4591627382447514, + "velocityX": -0.4053320408477288, + "velocityY": 1.4231143039244516, "moduleForcesX": [ - -27.000280396790707, - -19.781400792593253, - -32.92142671337878, - -25.680048193575647 + -0.5334552598468943, + 13.009918369721786, + -8.032358249742447, + 8.238973565498672 ], "moduleForcesY": [ - 64.36040574604013, - 66.91361057866158, - 61.51512045582401, - 64.8450531339154 + 69.8666713444081, + 68.63601950178051, + 69.37382880885353, + 69.32921346604414 ], - "timestamp": 0.5003066370795064 + "timestamp": 0.39875249013208514 }, { - "x": 2.1541934591617964, - "y": 4.743577081279941, - "heading": -0.2632851786920285, - "angularVelocity": 0.3647353812470825, - "velocityX": -1.119710973304069, - "velocityY": 1.598030752209193, + "x": 1.9389906812367639, + "y": 4.77112606241245, + "heading": -0.4060463560603266, + "angularVelocity": 0.5616781967264929, + "velocityX": -0.32549263705008313, + "velocityY": 1.6202300274752126, "moduleForcesX": [ - 28.898022383948128, - 40.38279327621932, - 31.518367214748288, - 44.919632243477565 + 18.715982262905385, + 33.707081998178694, + 15.897712534120208, + 35.51802148738027 ], "moduleForcesY": [ - 63.33426630830682, - 56.72995559800382, - 61.95891551538514, - 53.10093359505647 + 67.2633143302821, + 61.14210942301303, + 67.92400233992704, + 60.03846848663157 ], - "timestamp": 0.5717790138051502 + "timestamp": 0.45571713157952587 }, { - "x": 2.0933736359860933, - "y": 4.858226801594099, - "heading": -0.2335644163554314, - "angularVelocity": 0.41583565145382007, - "velocityX": -0.850955654977675, - "velocityY": 1.6041123219358235, + "x": 1.93014912520743, + "y": 4.87068791134505, + "heading": -0.3685234212138124, + "angularVelocity": 0.6587057145112619, + "velocityX": -0.1552112995829446, + "velocityY": 1.7477832986004502, "moduleForcesX": [ - 69.47635686741319, - 69.65947112210823, - 69.73585524619895, - 69.7189941107446 + 47.983848434946395, + 57.21667950593689, + 53.72495772708293, + 62.54171738572598 ], "moduleForcesY": [ - 6.227625887406924, - 4.078499983627095, - -1.256444304150436, - -2.7455509544386563 + 50.64853650489456, + 39.96838943911703, + 44.402717653560856, + 30.875613138399547 ], - "timestamp": 0.6432513905307942 + "timestamp": 0.5126817730269666 }, { - "x": 2.0511871628096467, - "y": 4.967985225687469, - "heading": -0.2022159861765387, - "angularVelocity": 0.4386090349181448, - "velocityX": -0.5902486393714047, - "velocityY": 1.53567614863841, + "x": 1.933418171455475, + "y": 4.971531596301135, + "heading": -0.32709464429833723, + "angularVelocity": 0.7272717928664595, + "velocityX": 0.057387287358971396, + "velocityY": 1.7702856086460224, "moduleForcesX": [ - 67.96496385579776, - 67.99440402036385, - 67.10639875219877, - 67.1820726301549 + 67.96928457704135, + 69.07682719509752, + 69.65789543520896, + 69.80083544623831 ], "moduleForcesY": [ - -16.191122120647563, - -16.095511247494485, - -19.44670942058393, - -19.20731800544632 + 15.813702501669626, + 10.130822196720576, + 3.8406949342180523, + -0.5188082796813483 ], - "timestamp": 0.7147237672564382 + "timestamp": 0.5696464144744073 }, { - "x": 2.0270139823956215, - "y": 5.070867612388492, - "heading": -0.1700681728530796, - "angularVelocity": 0.4497935397741312, - "velocityX": -0.3382171104112806, - "velocityY": 1.439470621969348, + "x": 1.9485966046329035, + "y": 5.069631244361437, + "heading": -0.28344891936809435, + "angularVelocity": 0.7661897594934095, + "velocityX": 0.2664535893100171, + "velocityY": 1.722114728850099, "moduleForcesX": [ - 65.5463060074924, - 65.64862612212953, - 64.96948366218025, - 65.09044905349134 + 68.8247416901388, + 68.711613893991, + 67.18557930818034, + 67.18883251525818 ], "moduleForcesY": [ - -24.323259893322444, - -24.052061862241104, - -25.825731707590776, - -25.525206862715248 + -11.868262004119803, + -12.595528771030496, + -19.06951150843806, + -19.117544695444614 ], - "timestamp": 0.7861961439820822 + "timestamp": 0.6266110559218481 }, { - "x": 2.020448891699325, - "y": 5.165875707863209, - "heading": -0.13755294679311533, - "angularVelocity": 0.45493416547221555, - "velocityX": -0.09185493341902934, - "velocityY": 1.3292981154257553, + "x": 1.9749895957767274, + "y": 5.1628267728214325, + "heading": -0.238677619130099, + "angularVelocity": 0.7859489518476898, + "velocityX": 0.46332234300422453, + "velocityY": 1.6360241386928376, "moduleForcesX": [ - 63.950064201059824, - 64.03180591913006, - 63.654913960217954, - 63.74125341653127 + 64.61918569816321, + 64.82164567363998, + 63.15374635838673, + 63.45210887594756 ], "moduleForcesY": [ - -28.315148832203725, - -28.131653555528967, - -28.97311608059264, - -28.784456776293474 + -26.610781563662005, + -26.130445473862178, + -29.925633288523958, + -29.30220507461484 ], - "timestamp": 0.8576685207077261 + "timestamp": 0.6835756973692888 }, { - "x": 2.031218238194587, - "y": 5.2524144264055215, - "heading": -0.10493277773470358, - "angularVelocity": 0.45640246697905146, - "velocityX": 0.150678441370407, - "velocityY": 1.2107995068082167, + "x": 2.0119893346706585, + "y": 5.249897971911408, + "heading": -0.19345274501313633, + "angularVelocity": 0.7939113275853787, + "velocityX": 0.649521140724979, + "velocityY": 1.528513071925737, "moduleForcesX": [ - 62.88066267543294, - 62.91207596743938, - 62.79228260986751, - 62.82409534697334 + 60.77468821171452, + 60.99722299740147, + 60.0777269685818, + 60.31974783111097 ], "moduleForcesY": [ - -30.650558779807053, - -30.586409677611073, - -30.831354646388686, - -30.76685591760273 + -34.556593215262986, + -34.166181049759004, + -35.755885616715915, + -35.34978878089667 ], - "timestamp": 0.9291408974333701 + "timestamp": 0.7405403388167295 }, { "x": 2.05912709236145, "y": 5.330090045928955, - "heading": -0.072383245243066, - "angularVelocity": 0.45541416114660316, - "velocityX": 0.3904844851873833, - "velocityY": 1.086792170543661, + "heading": -0.14821270337422277, + "angularVelocity": 0.7941775896308426, + "velocityX": 0.8274915191783369, + "velocityY": 1.407751755824489, "moduleForcesX": [ - 62.127913109347276, - 62.10220705945469, - 62.18868836322343, - 62.16316779871923 + 57.8734037104444, + 57.88504600107734, + 57.84871177168228, + 57.86037646750411 ], "moduleForcesY": [ - -32.17057458836276, - -32.21997133029195, - -32.052845514471784, - -32.1021124146997 + -39.255748814527, + -39.23866862035294, + -39.292170409475375, + -39.27508028582055 ], - "timestamp": 1.0006132741590141 + "timestamp": 0.7975049802641703 }, { - "x": 2.11398267592862, - "y": 5.407564590406167, - "heading": -0.035074691263133305, - "angularVelocity": 0.4522495337757732, - "velocityX": 0.6649523885483815, - "velocityY": 0.939136547950644, + "x": 2.1419011012670555, + "y": 5.425958346458515, + "heading": -0.08687313324288204, + "angularVelocity": 0.7872633235969319, + "velocityX": 1.0623638414637935, + "velocityY": 1.2304226576887645, "moduleForcesX": [ - 61.580309305924985, - 61.49718284699979, - 61.749856139779475, - 61.66816132300872 + 55.744309007626626, + 55.4487335715346, + 56.215540263624625, + 55.92748705805922 ], "moduleForcesY": [ - -33.23357506637483, - -33.386760848755074, - -32.91728042389969, - -33.06968489028737 + -42.276636780585044, + -42.66266684218691, + -41.64740235699094, + -42.03249730192701 ], - "timestamp": 1.083108779833673 + "timestamp": 0.875419914559707 }, { - "x": 2.191082677171206, - "y": 5.472153160220583, - "heading": 0.0016521215955720662, - "angularVelocity": 0.44519774208758894, - "velocityX": 0.9345963836882241, - "velocityY": 0.7829344083945624, + "x": 2.2414261398578335, + "y": 5.506181529743673, + "heading": -0.02721344128448102, + "angularVelocity": 0.7657029104601152, + "velocityX": 1.2773550987449058, + "velocityY": 1.029625244640082, "moduleForcesX": [ - 60.45923369475699, - 60.23455621610159, - 60.84356535380034, - 60.625874690348816 + 51.02163650907591, + 49.70119985383043, + 52.48797262102768, + 51.22069659742051 ], "moduleForcesY": [ - -35.214733524291056, - -35.59671720906772, - -34.545862040838756, - -34.92548722348504 + -47.85047801815092, + -49.21787446602869, + -46.23470435722217, + -47.63176527887239 ], - "timestamp": 1.1656042855083317 + "timestamp": 0.9533348488552438 }, { - "x": 2.289728178771266, - "y": 5.522730614286461, - "heading": 0.037276639055293805, - "angularVelocity": 0.4318358578252923, - "velocityX": 1.1957681909289004, - "velocityY": 0.6130934487194334, + "x": 2.354490568089435, + "y": 5.567942649448705, + "heading": 0.028816977553997255, + "angularVelocity": 0.719122968466496, + "velocityX": 1.4511265298991398, + "velocityY": 0.7926737057977623, "moduleForcesX": [ - 58.52744287678723, - 57.99177342881693, - 59.27377627441346, - 58.761492424372356 + 41.924684435294694, + 37.78420628587397, + 44.83708919222699, + 40.69029874809796 ], "moduleForcesY": [ - -38.31446039097551, - -39.11845718880393, - -37.14787466755693, - -37.95082764754945 + -55.96662276450357, + -58.83646046865295, + -53.653582227691004, + -56.85649586466601 ], - "timestamp": 1.2480997911829905 + "timestamp": 1.0312497831507805 }, { - "x": 2.408421432901453, - "y": 5.557250524578989, - "heading": 0.07081951118626838, - "angularVelocity": 0.40660241860066865, - "velocityX": 1.4387844903195497, - "velocityY": 0.41844595069491913, + "x": 2.473359530149637, + "y": 5.60763951076386, + "heading": 0.07772134675261151, + "angularVelocity": 0.6276636134109629, + "velocityX": 1.5256248771169323, + "velocityY": 0.5094897618033285, "moduleForcesX": [ - 54.50941828585798, - 53.13358487259185, - 55.95915631243375, - 54.647136092353065 + 22.049102020071857, + 11.270972185107798, + 25.093613830164124, + 12.425514812528533 ], "moduleForcesY": [ - -43.802094987807024, - -45.45697038399386, - -41.92981110097996, - -43.62112365277983 + -66.32736429857982, + -68.97836591275289, + -65.21334822172665, + -68.75568538466969 ], - "timestamp": 1.3305952968576493 + "timestamp": 1.1091647174463173 }, { - "x": 2.542474709173094, - "y": 5.571243535937252, - "heading": 0.09992456939909769, - "angularVelocity": 0.3528078041928978, - "velocityX": 1.6249767211345831, - "velocityY": 0.1696214981769599, + "x": 2.583125662677607, + "y": 5.626543406923644, + "heading": 0.11601498179594151, + "angularVelocity": 0.49148004024593883, + "velocityX": 1.4087945208503867, + "velocityY": 0.24262224348518271, "moduleForcesX": [ - 42.694250355720705, - 38.054672270282296, - 45.591148182962975, - 40.87638058379842 + -16.70068482620098, + -30.482853391136345, + -24.37959308564339, + -39.528854162952214 ], "moduleForcesY": [ - -55.307949027779564, - -58.58896847735615, - -52.92949409903056, - -56.63909921586958 + -67.844870324751, + -62.8857363057114, + -65.43315052803204, + -57.59599684491268 ], - "timestamp": 1.4130908025323081 + "timestamp": 1.187079651741854 }, { - "x": 2.668629243751066, - "y": 5.561025654990008, - "heading": 0.11917705216572184, - "angularVelocity": 0.2333761410288788, - "velocityX": 1.529229180843634, - "velocityY": -0.12385985482443941, + "x": 2.673631838796821, + "y": 5.633378148129679, + "heading": 0.14407534512325684, + "angularVelocity": 0.3601410125160406, + "velocityX": 1.1616024185545568, + "velocityY": 0.08772055406104393, "moduleForcesX": [ - -12.382402533780613, - -24.607511319028973, - -17.558309914075192, - -31.441206870964095 + -51.90525981906655, + -57.051226471868404, + -61.89152614197463, + -64.20272534700426 ], "moduleForcesY": [ - -68.63720397945411, - -65.28047130453552, - -67.43128061515037, - -62.222247817779476 + -46.802489344466096, + -40.40908629215477, + -32.43095750906469, + -27.650831182455995 ], - "timestamp": 1.495586308206967 + "timestamp": 1.2649945860373908 }, { - "x": 2.7698521306860013, - "y": 5.54500330305322, - "heading": 0.13176004586708878, - "angularVelocity": 0.15252944507008542, - "velocityX": 1.2270109278923758, - "velocityY": -0.19422090026996, + "x": 2.7420691702187887, + "y": 5.6345006557115855, + "heading": 0.16357365067508123, + "angularVelocity": 0.2502511967457482, + "velocityX": 0.8783596115525151, + "velocityY": 0.01440683473656765, "moduleForcesX": [ - -66.46018133159996, - -66.9674196584085, - -68.9644976388019, - -69.02558396413289 + -65.04850079574194, + -65.99350180832509, + -69.15045925519797, + -69.13827378457684 ], "moduleForcesY": [ - -21.47227954276657, - -19.910515932777027, - -11.024044579675353, - -10.78336011685823 + -25.651238870662098, + -23.15939656800476, + -10.354928047129325, + -10.547196980897708 ], - "timestamp": 1.5780818138816257 + "timestamp": 1.3429095203329275 }, { - "x": 2.845436141122318, - "y": 5.530116306121047, - "heading": 0.14009236993377866, - "angularVelocity": 0.10100336980206083, - "velocityX": 0.916219735867702, - "velocityY": -0.1804582814458009, + "x": 2.7877749257651523, + "y": 5.633422199584155, + "heading": 0.1758142407370851, + "angularVelocity": 0.15710197502797701, + "velocityX": 0.5866109746431721, + "velocityY": -0.013841455905480958, "moduleForcesX": [ - -69.91668485679588, - -69.92377181874357, - -69.57042974483447, - -69.70602424237894 + -68.75220978257724, + -68.781223473534, + -69.94627706176999, + -69.93907335246656 ], "moduleForcesY": [ - 0.35481804279217377, - -0.526848643787446, - 6.976161517175589, - 5.555870742457722 + -12.86879255012935, + -12.770261545400503, + 0.3760238076675933, + -1.5977856312836116 ], - "timestamp": 1.6605773195562845 + "timestamp": 1.4208244546284643 }, { - "x": 2.8956151191054844, - "y": 5.519017848354159, - "heading": 0.14518444674969433, - "angularVelocity": 0.06172550582309565, - "velocityX": 0.6082631723848511, - "velocityY": -0.13453409128336982, + "x": 2.810619354248047, + "y": 5.6322150230407715, + "heading": 0.18164838967891364, + "angularVelocity": 0.0748784426833914, + "velocityX": 0.2931970448212678, + "velocityY": -0.015493519365684846, "moduleForcesX": [ - -69.4187453897527, - -69.56564259987941, - -68.65234610284632, - -68.9344391693208 + -69.7650098576931, + -69.68462891027929, + -69.679901493562, + -69.87274192709826 ], "moduleForcesY": [ - 8.590706564201634, - 7.346324469918923, - 13.40936902595393, - 11.897426701626598 + -5.227252657372436, + -6.289797322807944, + 6.278310865228548, + 3.6678202564057916 ], - "timestamp": 1.7430728252309433 + "timestamp": 1.498739388924001 }, { - "x": 2.9206175804135657, - "y": 5.513061523434405, - "heading": 0.14756797074007869, - "angularVelocity": 0.02889277386548866, - "velocityX": 0.3030766476923993, - "velocityY": -0.07220180908862774, + "x": 2.810619354248047, + "y": 5.6322150230407715, + "heading": 0.18164838967891364, + "angularVelocity": 1.7820258276732997e-21, + "velocityX": -3.71309242331424e-21, + "velocityY": -1.8466663513371007e-20, "moduleForcesX": [ - -68.79757631258838, - -69.02339707894248, - -67.96227334834312, - -68.30019620815122 + -69.96925933574576, + -69.94810083182944, + -69.26326437970721, + -69.61542540763588 ], "moduleForcesY": [ - 12.723388654681191, - 11.450987875380186, - 16.62115678249245, - 15.184154939108327 + -0.3390303743865954, + -1.9603647789452194, + 9.931322934759566, + 7.100594511510579 ], - "timestamp": 1.825568330905602 - }, - { - "x": 2.9206175804138184, - "y": 5.5130615234375, - "heading": 0.14756797074007869, - "angularVelocity": -2.1366473717756725e-18, - "velocityX": 2.759428296475241e-16, - "velocityY": 1.8508305640470853e-16, - "moduleForcesX": [ - -68.30769120451006, - -68.57184176455583, - -67.47562728593, - -67.83343378701302 - ], - "moduleForcesY": [ - 15.178954859633443, - 13.946231227068052, - 18.53346910587473, - 17.184707214005112 - ], - "timestamp": 1.908063836580261 + "timestamp": 1.5766543232195378 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/center 4.3.traj b/src/main/deploy/choreo/center 4.3.traj index f2ffde16..4a67c14b 100644 --- a/src/main/deploy/choreo/center 4.3.traj +++ b/src/main/deploy/choreo/center 4.3.traj @@ -1,550 +1,529 @@ { "samples": [ { - "x": 2.9206175804138184, - "y": 5.5130615234375, - "heading": 0.14756797074007869, - "angularVelocity": -2.1366473717756725e-18, - "velocityX": 2.759428296475241e-16, - "velocityY": 1.8508305640470853e-16, + "x": 2.810619354248047, + "y": 5.6322150230407715, + "heading": 0.18164838967891364, + "angularVelocity": 1.7820258276732997e-21, + "velocityX": -3.71309242331424e-21, + "velocityY": -1.8466663513371007e-20, "moduleForcesX": [ - -68.30769120451006, - -68.57184176455583, - -67.47562728593, - -67.83343378701302 + -69.96925933574576, + -69.94810083182944, + -69.26326437970721, + -69.61542540763588 ], "moduleForcesY": [ - 15.178954859633443, - 13.946231227068052, - 18.53346910587473, - 17.184707214005112 + -0.3390303743865954, + -1.9603647789452194, + 9.931322934759566, + 7.100594511510579 ], "timestamp": 0 }, { - "x": 2.9008988708903125, - "y": 5.5307170603623375, - "heading": 0.14700286177553085, - "angularVelocity": -0.006751511016409849, - "velocityX": -0.23558480387769798, - "velocityY": 0.2109355181352119, + "x": 2.791342988452625, + "y": 5.642521810363919, + "heading": 0.18283388161538688, + "angularVelocity": 0.015584006009315527, + "velocityX": -0.25339944638282685, + "velocityY": 0.1354889313364025, "moduleForcesX": [ - -52.09999330596725, - -52.529400631239085, - -51.73320472903666, - -52.16512079541669 + -61.62075540054056, + -60.98102694031713, + -62.39838441045431, + -61.79343760463002 ], "moduleForcesY": [ - 46.71445570132471, - 46.23149173401103, - 47.1208354788092, - 46.6426390261249 + 33.15309974417089, + 34.31372499920319, + 31.663594414249264, + 32.826466274276484 ], - "timestamp": 0.08370110974748313 + "timestamp": 0.07607106515260487 }, { - "x": 2.861623494471227, - "y": 5.566204209893025, - "heading": 0.14583871741812887, - "angularVelocity": -0.013908350330240071, - "velocityX": -0.4692336401967019, - "velocityY": 0.42397465993079464, + "x": 2.753067309241903, + "y": 5.663631883203031, + "heading": 0.18530131117107015, + "angularVelocity": 0.03243584864670181, + "velocityX": -0.5031568722475193, + "velocityY": 0.2775046306603334, "moduleForcesX": [ - -51.6659111374572, - -52.12885129412831, - -51.276879306050894, - -51.74245743341866 + -60.74636949447462, + -59.995692501352465, + -61.60810614324199, + -60.89635912716705 ], "moduleForcesY": [ - 47.185653590179186, - 46.67421839348704, - 47.60875054548638, - 47.10282037910355 + 34.71442361257027, + 35.99400899987649, + 33.15894228397032, + 34.44613293316394 ], - "timestamp": 0.16740221949496625 + "timestamp": 0.15214213030520973 }, { - "x": 2.803016239597012, - "y": 5.619762027969135, - "heading": 0.14402910636298827, - "angularVelocity": -0.021619917114522248, - "velocityX": -0.7001968676942122, - "velocityY": 0.6398698673768739, + "x": 2.6962228110923627, + "y": 5.696261557261839, + "heading": 0.18919034306075003, + "angularVelocity": 0.05112366813686836, + "velocityX": -0.7472551887567889, + "velocityY": 0.42893673163836527, "moduleForcesX": [ - -51.06312930480856, - -51.57332722665041, - -50.64375367097991, - -51.156724254575956 + -59.39626570686199, + -58.46567557598025, + -60.37984931281294, + -59.49315400848136 ], "moduleForcesY": [ - 47.8258847940151, - 47.27589622960445, - 48.27053972557859, - 47.72716661234478 + 36.95663241834814, + 38.40914674537991, + 35.32399090200627, + 36.79466428895038 ], - "timestamp": 0.2511033292424494 + "timestamp": 0.2282131954578146 }, { - "x": 2.725409512823363, - "y": 5.691733398122384, - "heading": 0.14150668290126167, - "angularVelocity": -0.030136081460894748, - "velocityX": -0.9271887436445178, - "velocityY": 0.8598615995220361, + "x": 2.621558016690057, + "y": 5.741526222171845, + "heading": 0.19471996442816977, + "angularVelocity": 0.07269020561664075, + "velocityX": -0.9815137234179877, + "velocityY": 0.5950313015757177, "moduleForcesX": [ - -50.170225601037366, - -50.75200909732681, - -49.70741389665356, - -50.29204690115321 + -57.07004054859922, + -55.81145405690308, + -58.24064731344431, + -57.02953301737525 ], "moduleForcesY": [ - 48.74540511502207, - 48.14020759141128, - 49.218329972809045, - 48.621580569011485 + 40.42700972423277, + 42.1440264982245, + 38.71744543220449, + 40.476187384360045 ], - "timestamp": 0.3348044389899325 + "timestamp": 0.30428426061041947 }, { - "x": 2.6293437718673225, - "y": 5.782650350616714, - "heading": 0.1381651310810073, - "angularVelocity": -0.039922431498496204, - "velocityX": -1.147723623091234, - "velocityY": 1.0862096426984997, + "x": 2.5306316551845427, + "y": 5.801358526557014, + "heading": 0.2022754435822446, + "angularVelocity": 0.09932132722103867, + "velocityX": -1.1952818239511769, + "velocityY": 0.7865317024960887, "moduleForcesX": [ - -48.71431630344826, - -49.4167391577109, - -48.18511463537286, - -48.890119594937715 + -52.296776213679415, + -50.323936372341834, + -53.758106723210474, + -51.816584350831235 ], "moduleForcesY": [ - 50.17522088948343, - 49.48473182662626, - 50.68521987830446, - 50.00663478484992 + 46.39111947683355, + 48.51956857347833, + 44.682603452825234, + 46.914906732410614 ], - "timestamp": 0.41850554873741563 + "timestamp": 0.38035532576302433 }, { - "x": 2.5158455258965873, - "y": 5.893444520165239, - "heading": 0.13381239728196173, - "angularVelocity": -0.05200329854838183, - "velocityX": -1.355994518259599, - "velocityY": 1.3236881799931342, + "x": 2.427863523047455, + "y": 5.879498749258094, + "heading": 0.2126425688241101, + "angularVelocity": 0.13628210964403, + "velocityX": -1.350949036022118, + "velocityY": 1.0272003230706037, "moduleForcesX": [ - -45.93506137526542, - -46.87946903712664, - -45.29576298382204, - -46.24053944561288 + -39.09732377550607, + -35.17129355710182, + -40.71904985918127, + -36.62147361504708 ], "moduleForcesY": [ - 52.687688259487246, - 51.851007353746446, - 53.24100600936717, - 52.42425460814814 + 57.87480113678438, + 60.336542269027305, + 56.73087731606344, + 59.45243630995032 ], - "timestamp": 0.5022066584848988 + "timestamp": 0.4564263909156292 }, { - "x": 2.3875470683794213, - "y": 6.02610991590263, - "heading": 0.12800509474442898, - "angularVelocity": -0.06938142821559826, - "velocityX": -1.5328166842349815, - "velocityY": 1.5849896874494018, + "x": 2.3297978130131964, + "y": 5.978878854714064, + "heading": 0.22690067635521388, + "angularVelocity": 0.18743141695861396, + "velocityX": -1.2891328632973713, + "velocityY": 1.3064113833111803, "moduleForcesX": [ - -38.731601936491195, - -40.35076624830184, - -37.91551455677665, - -39.51613010702398 + 10.545576170240913, + 16.557527776184923, + 13.352744563742135, + 19.74884527271297 ], "moduleForcesY": [ - 58.094777198180715, - 56.98539695236494, - 58.63708159838333, - 57.57358536077538 + 68.92487436784589, + 67.74390307628403, + 68.40688100444298, + 66.8566796699288 ], - "timestamp": 0.5859077682323819 + "timestamp": 0.5324974560682341 }, { - "x": 2.2585637323391317, - "y": 6.185053475715637, - "heading": 0.11910747265558115, - "angularVelocity": -0.10630231923689437, - "velocityX": -1.5409991149465732, - "velocityY": 1.8989420845305607, + "x": 2.2518195232296936, + "y": 6.086732649237896, + "heading": 0.24339103584492305, + "angularVelocity": 0.2167757143485253, + "velocityX": -1.0250716172709238, + "velocityY": 1.4178031332605687, "moduleForcesX": [ - 0.7933919130137743, - -3.975029098028374, - 0.1994458839598195, - -4.26048134968995 + 63.3765480815569, + 63.68121368261706, + 64.96422299639283, + 65.1554779681156 ], "moduleForcesY": [ - 69.51699522375354, - 69.4045334427663, - 69.55309629363511, - 69.42011317516652 + 29.25944062691299, + 28.61722194785657, + 25.535838612439772, + 25.07538960318053 ], - "timestamp": 0.669608877979865 + "timestamp": 0.6085685212208389 }, { - "x": 2.155822904626011, - "y": 6.341651289925754, - "heading": 0.10835395117931683, - "angularVelocity": -0.1284752556892115, - "velocityX": -1.2274727064325863, - "velocityY": 1.8709168195383514, + "x": 2.195636505947272, + "y": 6.195898602093869, + "heading": 0.2608681511360735, + "angularVelocity": 0.229747214083171, + "velocityX": -0.7385596240793223, + "velocityY": 1.4350522453836667, "moduleForcesX": [ - 69.29850135005749, - 69.22376482749928, - 69.52084242344242, - 69.4745660602554 + 69.71870355427134, + 69.69809382001247, + 69.8203423782785, + 69.80580410665718 ], "moduleForcesY": [ - -7.254272274145535, - -7.823192974180219, - -4.62731192687223, - -5.1017659452285775 + 4.902598216652911, + 5.226579532658753, + 3.1528242764653718, + 3.5174449089562745 ], - "timestamp": 0.7533099877273477 + "timestamp": 0.6846395863734438 }, { - "x": 2.0772115888518736, - "y": 6.487483095533371, - "heading": 0.0969715087930836, - "angularVelocity": -0.13598914543167476, - "velocityX": -0.939190842449076, - "velocityY": 1.7422923808692223, + "x": 2.1612401984623397, + "y": 6.303431320252094, + "heading": 0.278796165903002, + "angularVelocity": 0.23567455945257776, + "velocityX": -0.45216019278723374, + "velocityY": 1.4135823909196665, "moduleForcesX": [ - 63.72237766804347, - 63.53223394842242, - 64.05032016117235, - 63.867552477264695 + 69.7517709773252, + 69.77145706900596, + 69.6934539088278, + 69.71663436820566 ], "moduleForcesY": [ - -28.628204478983974, - -29.043494705306433, - -27.884225907046652, - -28.295904148066843 + -4.983366244420907, + -4.712773441080035, + -5.745679950464318, + -5.468339576313232 ], - "timestamp": 0.8370110974748304 + "timestamp": 0.7607106515260487 }, { - "x": 2.021637442637373, - "y": 6.620330494187104, - "heading": 0.08532200026076654, - "angularVelocity": -0.1391798575606646, - "velocityX": -0.6639594908779467, - "velocityY": 1.587164125330234, + "x": 2.1484700695340293, + "y": 6.407813780862964, + "heading": 0.29689533472311547, + "angularVelocity": 0.2379244826374525, + "velocityX": -0.1678710414096705, + "velocityY": 1.372170356777184, "moduleForcesX": [ - 60.87885944738181, - 60.7694927196997, - 61.04027054010648, - 60.93220988562005 + 69.23031154412588, + 69.24884860764138, + 69.18986060193748, + 69.20902830787257 ], "moduleForcesY": [ - -34.375983359242426, - -34.56803319978518, - -34.087899420795566, - -34.2797476711687 + -10.008624570555261, + -9.881273409427275, + -10.285342371577995, + -10.157228071808754 ], - "timestamp": 0.9207122072223131 + "timestamp": 0.8367817166786535 }, { - "x": 1.9885261521674162, - "y": 6.739204165667878, - "heading": 0.07357290264571206, - "angularVelocity": -0.14036967550974822, - "velocityX": -0.39558962325593, - "velocityY": 1.4202161922459862, + "x": 2.157177448272705, + "y": 6.508133888244629, + "heading": 0.31499648569215616, + "angularVelocity": 0.2379505391797553, + "velocityX": 0.11446374151864286, + "velocityY": 1.318768275170261, "moduleForcesX": [ - 59.37783973737232, - 59.33213553424243, - 59.44147378924915, - 59.395945957037256 + 68.74379080687748, + 68.74410650557303, + 68.74320786149968, + 68.74352365244148 ], "moduleForcesY": [ - -36.95821427423069, - -37.031312892639825, - -36.85560981002678, - -36.92870573976187 + -13.001771747122337, + -13.000114222047666, + -13.004860154304824, + -13.00320254851976 ], - "timestamp": 1.0044133169697957 + "timestamp": 0.9128527818312584 }, { - "x": 1.977528756045062, - "y": 6.843547542559394, - "heading": 0.061820288525356495, - "angularVelocity": -0.14041168815821406, - "velocityX": -0.13138889263033743, - "velocityY": 1.246618800486609, + "x": 2.184724710816574, + "y": 6.59922785852166, + "heading": 0.3320932322725307, + "angularVelocity": 0.23656836371790554, + "velocityX": 0.38117256954556666, + "velocityY": 1.2604708967109606, "moduleForcesX": [ - 58.46396442972924, - 58.46225971251114, - 58.46628338376228, - 58.46457888275662 + 68.34746493653803, + 68.32541439579168, + 68.38381933803426, + 68.36206742712406 ], "moduleForcesY": [ - -38.41537453860932, - -38.417962696829065, - -38.41184043411793, - -38.41442860346031 + -14.974761050304606, + -15.074567367293154, + -14.807552238846894, + -14.90716016282338 ], - "timestamp": 1.0881144267172784 + "timestamp": 0.9851225744260175 }, { - "x": 1.9884117094887013, - "y": 6.933004553123894, - "heading": 0.05012639395544798, - "angularVelocity": -0.13971014966457962, - "velocityX": 0.1300216147344071, - "velocityY": 1.0687673173254597, + "x": 2.2313650052611718, + "y": 6.685359777525342, + "heading": 0.34895139183908297, + "angularVelocity": 0.23326702570023544, + "velocityX": 0.6453636127908321, + "velocityY": 1.1918107955097217, "moduleForcesX": [ - 57.85196007751825, - 57.88125766772754, - 57.812373545441886, - 57.841731008840526 + 67.69387597978809, + 67.62571914819677, + 67.79222902236084, + 67.72581400551105 ], "moduleForcesY": [ - -39.3486892907348, - -39.30566232094738, - -39.40689132743343, - -39.363869274238354 + -17.65912805639218, + -17.917221977463345, + -17.276880276884725, + -17.53424163554366 ], - "timestamp": 1.171815536464761 + "timestamp": 1.0573923670207765 }, { - "x": 2.021008014678955, - "y": 7.007328033447266, - "heading": 0.03853480959994491, - "angularVelocity": -0.13848782161284204, - "velocityX": 0.38943695354526725, - "velocityY": 0.8879629018104452, + "x": 2.2967574046117547, + "y": 6.76538030405312, + "heading": 0.3653570894706221, + "angularVelocity": 0.2270062918753815, + "velocityX": 0.9048372356242996, + "velocityY": 1.1072472142888359, "moduleForcesX": [ - 57.41445936428743, - 57.46628160975792, - 57.34427766903865, - 57.39628164773986 + 66.47960800608301, + 66.30177808290506, + 66.69591112278235, + 66.52423830099455 ], "moduleForcesY": [ - -39.99658250377327, - -39.922209282283085, - -40.09723139207876, - -40.02287498111711 + -21.74308831253873, + -22.277371179768945, + -21.068430447014848, + -21.60216639071113 ], - "timestamp": 1.2555166462122438 + "timestamp": 1.1296621596155356 }, { - "x": 2.0826299228686898, - "y": 7.070370365951682, - "heading": 0.02599725344124828, - "angularVelocity": -0.1367266901870693, - "velocityX": 0.6720097158166497, - "velocityY": 0.687499968419269, + "x": 2.3801398957317264, + "y": 6.837336652626878, + "heading": 0.3809436958576124, + "angularVelocity": 0.21567249368473568, + "velocityX": 1.153766852321353, + "velocityY": 0.9956628625910408, "moduleForcesX": [ - 57.08917572518026, - 57.15769997977353, - 56.9954356988548, - 57.06427617407771 + 63.806559308723315, + 63.31882271721067, + 64.26880839630248, + 63.798069788806266 ], "moduleForcesY": [ - -40.47369675811929, - -40.37699177913841, - -40.605690518380186, - -40.509013116235465 + -28.59447611383761, + -29.655406522966526, + -27.53587934185067, + -28.605861210958697 ], - "timestamp": 1.3472145885932028 + "timestamp": 1.2019319522102947 }, { - "x": 2.169672617334707, - "y": 7.114364526461086, - "heading": 0.013740199626283799, - "angularVelocity": -0.13366770831190322, - "velocityX": 0.9492327984450655, - "velocityY": 0.4797726026430948, + "x": 2.479259804153409, + "y": 6.897447135971791, + "heading": 0.39498518087177925, + "angularVelocity": 0.19429258767770655, + "velocityX": 1.3715261226427125, + "velocityY": 0.831751153375685, "moduleForcesX": [ - 56.01669479138227, - 56.14220393478205, - 55.84946987132304, - 55.97590888149232 + 56.13643988602569, + 54.52558414545473, + 57.08952254401824, + 55.48617464288215 ], "moduleForcesY": [ - -41.93127449705145, - -41.76334146364884, - -42.153954194749936, - -41.98616963230991 + -41.60713844028672, + -43.692054420376515, + -40.280169475319, + -42.45608521234071 ], - "timestamp": 1.4389125309741617 + "timestamp": 1.2742017448050538 }, { - "x": 2.2812482301938513, - "y": 7.138190544722294, - "heading": 0.0019682714125729057, - "angularVelocity": -0.12837723408034055, - "velocityX": 1.2167733535710465, - "velocityY": 0.25983154852058693, + "x": 2.584931627951079, + "y": 6.9390229938132615, + "heading": 0.4060308240467457, + "angularVelocity": 0.15283900476791354, + "velocityX": 1.4621852367863801, + "velocityY": 0.575286801701526, "moduleForcesX": [ - 54.06938340379889, - 54.308966898728, - 53.770349274089426, - 54.012513868838 + 26.148906673767343, + 20.715268485938005, + 26.009248473481666, + 20.06651880527857 ], "moduleForcesY": [ - -44.39082318658512, - -44.09795887662825, - -44.753051093939305, - -44.46103930411709 + -64.70640976971409, + -66.64626277199693, + -64.74052136771891, + -66.82336232717894 ], - "timestamp": 1.5306104733551207 + "timestamp": 1.3464715373998128 }, { - "x": 2.4153058153444134, - "y": 7.139605579002843, - "heading": -0.008889495736468035, - "angularVelocity": -0.1184079693298827, - "velocityX": 1.4619475818218677, - "velocityY": 0.015431471712328844, + "x": 2.6792925575993936, + "y": 6.964515706893577, + "heading": 0.4138293757225624, + "angularVelocity": 0.10790887030139666, + "velocityX": 1.3056759437159087, + "velocityY": 0.3527436867469866, "moduleForcesX": [ - 49.535744448116404, - 50.08883535472126, - 48.95285722529716, - 49.512750306129256 + -36.39457522337274, + -39.28022118556542, + -41.130153635623216, + -43.6418492259847 ], "moduleForcesY": [ - -49.35481713361861, - -48.79483073718232, - -49.9344054264043, - -49.38065310701591 + -59.56113873853256, + -57.71718933439606, + -56.381055028824214, + -54.48253610093349 ], - "timestamp": 1.6223084157360796 + "timestamp": 1.418741329994572 }, { - "x": 2.5635950141167547, - "y": 7.1126996745657065, - "heading": -0.017499861469921024, - "angularVelocity": -0.0938992251069123, - "velocityX": 1.6171485962309224, - "velocityY": -0.29341884600255425, + "x": 2.756321629159372, + "y": 6.98065148897458, + "heading": 0.41934868952054644, + "angularVelocity": 0.0763709649608753, + "velocityX": 1.0658543326934196, + "velocityY": 0.22327145964679496, "moduleForcesX": [ - 30.82355947672089, - 33.05783383581679, - 29.658555665503656, - 31.855767521726126 + -60.27208113538249, + -60.51737707772543, + -62.48632553835477, + -62.5793289359259 ], "moduleForcesY": [ - -62.655847172898895, - -61.51032510158116, - -63.223044950314765, - -62.148500220510925 + -35.36566968241656, + -34.96009847227816, + -31.286074162611634, + -31.117684633539717 ], - "timestamp": 1.7140063581170386 + "timestamp": 1.491011122589331 }, { - "x": 2.6834005581658715, - "y": 7.072012772379466, - "heading": -0.022151846620554527, - "angularVelocity": -0.05073161981418278, - "velocityX": 1.3065237989836214, - "velocityY": -0.4437057267013505, + "x": 2.814586704942504, + "y": 6.990746632959886, + "heading": 0.42314075402152007, + "angularVelocity": 0.052470947609287326, + "velocityX": 0.8062161754060119, + "velocityY": 0.13968690960429975, "moduleForcesX": [ - -64.12298736950346, - -63.25910030393463, - -62.330306323818874, - -61.2590262700717 + -66.06720809870912, + -65.97454972543122, + -67.12642001354656, + -67.00203153454379 ], "moduleForcesY": [ - -27.440487603067957, - -29.33631206040231, - -31.317162411294717, - -33.33134662941462 + -22.902929804587398, + -23.180877284771793, + -19.584025329151217, + -20.01956198271935 ], - "timestamp": 1.8057043004979976 + "timestamp": 1.56328091518409 }, { - "x": 2.7720201720116067, - "y": 7.037183326012226, - "heading": -0.02500990952650958, - "angularVelocity": -0.031168233787252347, - "velocityX": 0.9664296880719148, - "velocityY": -0.37982800389550603, + "x": 2.853604993354379, + "y": 6.996545885023089, + "heading": 0.42550601388588527, + "angularVelocity": 0.03272819499604782, + "velocityX": 0.5398976115880579, + "velocityY": 0.08024448189192064, "moduleForcesX": [ - -68.53351263579651, - -68.45472035856257, - -68.92469478793043, - -68.86842514528999 + -68.01213832568928, + -67.89244829561473, + -68.61715921102547, + -68.49693767599965 ], "moduleForcesY": [ - 13.79021249565681, - 14.160173532909322, - 11.675894673290463, - 11.9841604417936 + -16.33804890659345, + -16.838898924869497, + -13.577428279677779, + -14.183521442505858 ], - "timestamp": 1.8974022428789565 + "timestamp": 1.6355507077788491 }, { - "x": 2.830552574824699, - "y": 7.012504794671367, - "heading": -0.026692143130144577, - "angularVelocity": -0.018345380059196448, - "velocityX": 0.6383175162541149, - "velocityY": -0.26912851727456555, + "x": 2.8731672763824463, + "y": 6.999098300933838, + "heading": 0.4266275995625518, + "angularVelocity": 0.015519425701905927, + "velocityX": 0.27068408979336367, + "velocityY": 0.035317880667813904, "moduleForcesX": [ - -66.1245923787409, - -65.98169477222959, - -66.56070394354207, - -66.43347254576503 + -68.85222618332939, + -68.74728425154547, + -69.24154633684182, + -69.1454169059897 ], "moduleForcesY": [ - 22.81538982238073, - 23.221752664345384, - 21.508695963392192, - 21.894565620883082 + -12.403847845099842, + -12.982110998649153, + -10.008033607336229, + -10.662885240301636 ], - "timestamp": 1.9891001852599155 + "timestamp": 1.7078205003736082 }, { - "x": 2.8596270084381104, - "y": 6.999704360961915, - "heading": -0.027461342770395154, - "angularVelocity": -0.008388406765443529, - "velocityX": 0.31706745919666984, - "velocityY": -0.1395934675707037, + "x": 2.8731672763824463, + "y": 6.999098300933838, + "heading": 0.4266275995625518, + "angularVelocity": 5.733280321426045e-22, + "velocityX": 4.854312894103748e-21, + "velocityY": -1.3121681595321098e-20, "moduleForcesX": [ - -64.76590607807921, - -64.6158756095251, - -65.15697508947473, - -65.01741451245994 + -69.27866045130386, + -69.19177764514303, + -69.54932705970307, + -69.47427349656373 ], "moduleForcesY": [ - 26.47522061163077, - 26.837605390916714, - 25.49694542258738, - 25.848954107534034 + -9.808624171483924, + -10.412359295598337, + -7.660954895057263, + -8.324479793607422 ], - "timestamp": 2.0807981276408745 - }, - { - "x": 2.8596270084381104, - "y": 6.999704360961914, - "heading": -0.02746134277039515, - "angularVelocity": 1.6056171256353062e-18, - "velocityX": -1.8304903305133357e-17, - "velocityY": -4.784339232160718e-18, - "moduleForcesX": [ - -63.93945199318899, - -63.793171888386496, - -64.29137064962106, - -64.1528183330455 - ], - "moduleForcesY": [ - 28.437823121296837, - 28.763444348886257, - 27.63256619914302, - 27.951659076114336 - ], - "timestamp": 2.1724960700218334 + "timestamp": 1.7800902929683673 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/center 4.traj b/src/main/deploy/choreo/center 4.traj index c99f5997..197a6e5e 100644 --- a/src/main/deploy/choreo/center 4.traj +++ b/src/main/deploy/choreo/center 4.traj @@ -3,10 +3,10 @@ { "x": 1.4015021324157717, "y": 5.563943386077881, - "heading": 2.230586975435435e-18, - "angularVelocity": -4.993708780228108e-16, - "velocityX": 2.2186084345747425e-14, - "velocityY": -2.3644285540186748e-14, + "heading": 7.388244526996465e-19, + "angularVelocity": 3.0465745999077404e-18, + "velocityX": -3.4647017198299383e-17, + "velocityY": -1.192018758735892e-17, "moduleForcesX": [ 0, 0, @@ -22,1390 +22,1537 @@ "timestamp": 0 }, { - "x": 1.4206212389589077, - "y": 5.543569823071968, - "heading": -0.005440740553276968, - "angularVelocity": -0.06321989027440103, - "velocityX": 0.22215869438518032, - "velocityY": -0.23673512918379025, + "x": 1.4219202193105986, + "y": 5.548947800945218, + "heading": -0.0011051964959714333, + "angularVelocity": -0.01349618349862027, + "velocityX": 0.24933688120418268, + "velocityY": -0.18311962565685555, "moduleForcesX": [ - 47.940963091989595, - 43.779015007865034, - 51.71856759026114, - 47.81380065892401 + 56.284779515152344, + 55.67887470588661, + 57.10064703101138, + 56.51836963286635 ], "moduleForcesY": [ - -50.97920586080499, - -54.592033915440794, - -47.13866785517865, - -51.09102404890194 + -41.57896584233712, + -42.38577268175027, + -40.45035897321267, + -41.2588054071458 ], - "timestamp": 0.08606058203599731 + "timestamp": 0.08188955759860805 }, { - "x": 1.4588579982416516, - "y": 5.50282426541773, - "heading": -0.016323802947849745, - "angularVelocity": -0.12645815467526472, - "velocityX": 0.4443004964254837, - "velocityY": -0.4734520342542199, + "x": 1.4627096279184146, + "y": 5.518896687993435, + "heading": -0.003378676387590463, + "angularVelocity": -0.027762757038732957, + "velocityX": 0.4981026861537893, + "velocityY": -0.3669712455778003, "moduleForcesX": [ - 47.897364078006646, - 43.771363296078064, - 51.712153560329, - 47.85692298524638 + 56.14974361131909, + 55.50531984469861, + 57.01450005495424, + 56.39643729223106 ], "moduleForcesY": [ - -51.01338140036875, - -54.59132698392599, - -47.13783450766829, - -51.0426999647787 + -41.752695886724744, + -42.60428285649574, + -40.56281363910674, + -41.416368028381115 ], - "timestamp": 0.17212116407199463 + "timestamp": 0.1637791151972161 }, { - "x": 1.5162104171628858, - "y": 5.441708865073567, - "heading": -0.032652672648373536, - "angularVelocity": -0.18973691920489047, - "velocityX": 0.6664191378657773, - "velocityY": -0.7101439350063614, + "x": 1.5238075547381984, + "y": 5.473710157546218, + "heading": -0.0069048134518868666, + "angularVelocity": -0.04305966679635681, + "velocityX": 0.7461015618045014, + "velocityY": -0.5517984438105422, "moduleForcesX": [ - 47.832382527825025, - 43.760444196177374, - 51.70298147300229, - 47.92205717348266 + 55.968126398369286, + 55.27198555154887, + 56.89895032239671, + 56.233074234563546 ], "moduleForcesY": [ - -51.06506924736401, - -54.59072757046299, - -47.13719675772626, - -50.9707235368353 + -41.984633832597545, + -42.89526110767094, + -40.71292465128025, + -41.6259762326198 ], - "timestamp": 0.25818174610799194 + "timestamp": 0.24566867279582416 }, { - "x": 1.5926756045431116, - "y": 5.360226735801809, - "heading": -0.05443308174346003, - "angularVelocity": -0.25308228900808055, - "velocityX": 0.8885041862345195, - "velocityY": -0.9467996533549011, + "x": 1.605125212106644, + "y": 5.413276440660695, + "heading": -0.011802142574701541, + "angularVelocity": -0.05980407351594935, + "velocityX": 0.9930162007595612, + "velocityY": -0.7379905162231216, "moduleForcesX": [ - 47.74512096081025, - 43.74635348449958, - 51.68931736462727, - 48.00815391044375 + 55.71155946356058, + 54.941547780042846, + 56.7359549514754, + 56.00213234888038 ], "moduleForcesY": [ - -51.133333759147, - -54.588485401020556, - -47.13677120729667, - -50.87397800267328 + -42.30893410579651, + -43.30196365103173, + -40.92326397000725, + -41.91947734002761 ], - "timestamp": 0.34424232814398925 + "timestamp": 0.3275582303944322 }, { - "x": 1.6882490058265867, - "y": 5.258382764481358, - "heading": -0.08167367566723759, - "angularVelocity": -0.31652811634851347, - "velocityX": 1.1105363105122372, - "velocityY": -1.1833985890311751, + "x": 1.7065275334793777, + "y": 5.337428085536629, + "heading": -0.018249242256111287, + "angularVelocity": -0.07872920394823069, + "velocityX": 1.2382814652604819, + "velocityY": -0.9262274378844899, "moduleForcesX": [ - 47.63372466857229, - 43.72840022744862, - 51.668243433528644, - 48.113016067033115 + 55.322537771508856, + 54.4375833932818, + 56.48888780544757, + 55.64994626737733 ], "moduleForcesY": [ - -51.216216787013636, - -54.58157282722093, - -47.135721060383055, - -50.750174463810936 + -42.79328988841797, + -43.91048339133851, + -41.238988003610295, + -42.36091518217925 ], - "timestamp": 0.43030291017998656 + "timestamp": 0.40944778799304027 }, { - "x": 1.8029224178515728, - "y": 5.136185717697895, - "heading": -0.11438761196088189, - "angularVelocity": -0.3801268306536648, - "velocityX": 1.332473116574703, - "velocityY": -1.4198956586668827, + "x": 1.8277843888865313, + "y": 5.2458866793139265, + "heading": -0.02654556216924506, + "angularVelocity": -0.10131108478813869, + "velocityX": 1.4807364817078694, + "velocityY": -1.117864217454393, "moduleForcesX": [ - 47.49348736287193, - 43.70277282082887, - 51.63393088382914, - 48.231135604313586 + 54.66422556146924, + 53.57551056932187, + 56.07036150264626, + 55.04634059384568 ], "moduleForcesY": [ - -51.30877200765891, - -54.563746594487974, - -47.12995843648461, - -50.59351350853768 + -43.59337180528042, + -44.9199390004416, + -41.76546496247963, + -43.10085570220449 ], - "timestamp": 0.5163634922159839 + "timestamp": 0.4913373455916483 }, { - "x": 1.936676738585046, - "y": 4.9936559524582, - "heading": -0.1525981886270299, - "angularVelocity": -0.4439962612620674, - "velocityX": 1.5541879641384613, - "velocityY": -1.6561561857250287, + "x": 1.96841676914479, + "y": 5.138098820317635, + "heading": -0.03729581263726324, + "angularVelocity": -0.13127742758990868, + "velocityX": 1.717342044361563, + "velocityY": -1.3162588998792917, "moduleForcesX": [ - 47.30730481341717, - 43.65083702902553, - 51.569007669599586, - 48.34309714132929 + 53.31294529372992, + 51.76935791756607, + 55.208047286851695, + 53.77390871564015 ], "moduleForcesY": [ - -51.393363168260414, - -54.515933915159074, - -47.100223160042596, - -50.38283518179482 + -45.16334356265244, + -46.915928207338055, + -42.81846532880878, + -44.59597492097393 ], - "timestamp": 0.6024240742519812 + "timestamp": 0.5732269031902564 }, { - "x": 2.089417359472379, - "y": 4.830894184799883, - "heading": -0.19638882103609856, - "angularVelocity": -0.5088349552543711, - "velocityX": 1.7748034844634022, - "velocityY": -1.8912464178636816, + "x": 2.1268410825486406, + "y": 5.0124497352130994, + "heading": -0.05234368618465831, + "angularVelocity": -0.1837581492521726, + "velocityX": 1.9346094673069207, + "velocityY": -1.5343724986323788, "moduleForcesX": [ - 46.92254775633583, - 43.388297546565106, - 51.333326631328006, - 48.27968404555044 + 49.01689962945185, + 45.73289983149253, + 52.41775458712863, + 49.40090115837192 ], "moduleForcesY": [ - -51.315242953431444, - -54.28258670469318, - -46.8588510495494, - -49.928189244812266 + -49.59779004971247, + -52.616015053993074, + -45.96240551955732, + -49.157805609768765 ], - "timestamp": 0.6884846562879785 + "timestamp": 0.6551164607888644 }, { - "x": 2.2231712411306024, - "y": 4.688364652999476, - "heading": -0.23481358247355089, - "angularVelocity": -0.4464850286666412, - "velocityX": 1.5541828604889207, - "velocityY": -1.6561534735218788, + "x": 2.2651293964122, + "y": 4.873361744519432, + "heading": -0.08615107243119119, + "angularVelocity": -0.4128412368795543, + "velocityX": 1.6887173153556436, + "velocityY": -1.6984826243100626, "moduleForcesX": [ - -46.804641906327866, - -43.5737849154587, - -51.151878946250015, - -48.397943993853495 + -42.85767308274732, + -56.04462264975442, + -58.165526762903994, + -65.39829691272111 ], "moduleForcesY": [ - 51.411338017062235, - 54.12246522092687, - 47.04773571475752, - 49.80566610187949 + -53.371294254255105, + -39.88846466311047, + -34.7319701928346, + -20.48370015983788 ], - "timestamp": 0.7745452383239758 + "timestamp": 0.7370060183874725 }, { - "x": 2.3378438009906706, - "y": 4.5661682512486585, - "heading": -0.2677858501447284, - "angularVelocity": -0.383128569329872, - "velocityX": 1.3324632166436747, - "velocityY": -1.4198881637340148, + "x": 2.380781360334255, + "y": 4.745442480322685, + "heading": -0.12257922703942895, + "angularVelocity": -0.44484493110577494, + "velocityX": 1.4122919614360556, + "velocityY": -1.5620949477312027, "moduleForcesX": [ - -46.8930408063945, - -43.75302961542786, - -51.4208655887188, - -48.80743987389535 + -63.10751721072769, + -63.62818548216768, + -61.345490726061854, + -62.00924380931458 ], "moduleForcesY": [ - 51.77006647658862, - 54.43088247512983, - 47.26280662177527, - 49.932717396293974 + 29.679520572670825, + 28.58177154469767, + 33.18021447558313, + 31.952578589554413 ], - "timestamp": 0.8606058203599731 + "timestamp": 0.8188955759860805 }, { - "x": 2.433416232746065, - "y": 4.4643250767817815, - "heading": -0.2952821236280862, - "angularVelocity": -0.3194990416382979, - "velocityX": 1.1105250446261894, - "velocityY": -1.1833893284845862, + "x": 2.474640833839395, + "y": 4.630368598491031, + "heading": -0.1600449633075639, + "angularVelocity": -0.45751543135421574, + "velocityX": 1.1461714565026784, + "velocityY": -1.4052326719814436, "moduleForcesX": [ - -46.834172015543345, - -43.8038207106873, - -51.45084121327245, - -48.97366881140082 + -60.43135462935912, + -60.713068060914686, + -59.65856184219576, + -59.96434194640932 ], "moduleForcesY": [ - 51.91054751178551, - 54.480374777947134, - 47.331348947919075, - 49.87523949636651 + 35.07512707135166, + 34.59110445663475, + 36.37630093566144, + 35.87554621698124 ], - "timestamp": 0.9466664023959704 + "timestamp": 0.9007851335846886 }, { - "x": 2.5098805109805107, - "y": 4.382843730787292, - "heading": -0.3172895530520021, - "angularVelocity": -0.2557202020164514, - "velocityX": 0.888493622368971, - "velocityY": -0.9467905530509265, + "x": 2.547034756190077, + "y": 4.528712298571689, + "heading": -0.19799267869255702, + "angularVelocity": -0.46340115269638116, + "velocityX": 0.8840433929992599, + "velocityY": -1.2413829418592635, "moduleForcesX": [ - -46.77185013607419, - -43.83510322729932, - -51.45045906865701, - -49.085367782981976 + -59.40670461633923, + -59.54176328078391, + -59.033208502260386, + -59.17356671352153 ], "moduleForcesY": [ - 52.003993622508574, - 54.49394996948038, - 47.37501122701898, - 49.81059264285791 + 36.87137260564648, + 36.65461497196282, + 37.467114192996426, + 37.246739709029626 ], - "timestamp": 1.0327269844319678 + "timestamp": 0.9826746911832966 }, { - "x": 2.5672321904712163, - "y": 4.3217289912490875, - "heading": -0.33380008105844566, - "angularVelocity": -0.1918477381378256, - "velocityX": 0.6664105458858951, - "velocityY": -0.7101362560644467, + "x": 2.5981356234905864, + "y": 4.44076146673782, + "heading": -0.23613927516023567, + "angularVelocity": -0.46582980280169317, + "velocityX": 0.6240217776116697, + "velocityY": -1.0740176698103814, "moduleForcesX": [ - -46.721839228792774, - -43.85751027569671, - -51.44391879316928, - -49.163980062749964 + -58.86437819861236, + -58.918013476268975, + -58.70626834903893, + -58.760815319074105 ], "moduleForcesY": [ - 52.06961690331074, - 54.49746975666322, - 47.406083494827506, - 49.75817477281888 + 37.774145083580734, + 37.690963323617666, + 38.019592701279585, + 37.93575823083028 ], - "timestamp": 1.1187875664679652 + "timestamp": 1.0645642487819047 }, { - "x": 2.6054684419481404, - "y": 4.280983903135222, - "heading": -0.3448087987822844, - "angularVelocity": -0.12791823461321336, - "velocityX": 0.444294595953872, - "velocityY": -0.47344657843923665, + "x": 2.6280498749209062, + "y": 4.36668930468894, + "heading": -0.2743130502727876, + "angularVelocity": -0.46616169670453655, + "velocityX": 0.3652999516369684, + "velocityY": -0.9045373332203905, "moduleForcesX": [ - -46.68832390905574, - -43.873154666936216, - -51.438399514205486, - -49.21567045651427 + -58.52598156640852, + -58.532799162489525, + -58.50395042521087, + -58.51078511724211 ], "moduleForcesY": [ - 52.11282780371421, - 54.49861617678228, - 47.42729008575955, - 49.72306941004055 + 38.321865139050104, + 38.31150880550724, + 38.35550907746569, + 38.3451396295913 ], - "timestamp": 1.2048481485039626 + "timestamp": 1.1464538063805128 }, { - "x": 2.6245872974433224, - "y": 4.260610580440736, - "heading": -0.3503132845744241, - "angularVelocity": -0.06396059220042477, - "velocityX": 0.2221557770547791, - "velocityY": -0.23673233693882528, + "x": 2.6368497069838694, + "y": 4.3066114512341285, + "heading": -0.31239867185928216, + "angularVelocity": -0.46508520382019075, + "velocityX": 0.10745975825258396, + "velocityY": -0.733644865311249, "moduleForcesX": [ - -46.672848976425186, - -43.88250079839491, - -51.436915163186654, - -49.242971039133984 + -58.29266277626512, + -58.27259460380822, + -58.3652538106269, + -58.34536522287156 ], "moduleForcesY": [ - 52.135797056459324, - 54.500625069799426, - 47.43940924267183, - 49.70711869156378 + 38.69272698107036, + 38.7227879276336, + 38.58309676980216, + 38.61300918912735 ], - "timestamp": 1.29090873053996 + "timestamp": 1.2283433639791208 }, { "x": 2.624587297439575, "y": 4.260610580444336, "heading": -0.350313284574424, - "angularVelocity": 4.86945709754994e-16, - "velocityX": -2.1781193472847508e-14, - "velocityY": 2.3655888321777277e-14, + "angularVelocity": -0.4629969171525455, + "velocityX": -0.14974326280281844, + "velocityY": -0.5617428172622025, "moduleForcesX": [ - -46.676116985699565, - -43.88573472382722, - -51.440890855557896, - -49.24709237978015 + -58.12061439137176, + -58.08608612818397, + -58.26328189836039, + -58.2294247822304 ], "moduleForcesY": [ - 52.139555439371115, - 54.50503144060741, - 47.44278460160171, - 49.711156539011256 + 38.962670441077805, + 39.01386737811412, + 38.74894148261589, + 38.799539468823816 ], - "timestamp": 1.3769693125759574 + "timestamp": 1.3102329215777289 }, { - "x": 2.6097025403261815, - "y": 4.272883049188608, - "heading": -0.3486185943542768, - "angularVelocity": 0.023711121663719633, - "velocityX": -0.20825887874704344, - "velocityY": 0.17170925805626883, + "x": 2.587333207762282, + "y": 4.2275605158811125, + "heading": -0.3906561506913106, + "angularVelocity": -0.45994348660554407, + "velocityX": -0.42472876981164054, + "velocityY": -0.37679925575190565, "moduleForcesX": [ - -53.325527820192114, - -52.543899565580986, - -55.33489027006052, - -54.67599479757407 + -57.98909826227752, + -57.94886879340227, + -58.18608810349869, + -58.14709239976308 ], "moduleForcesY": [ - 45.30344546948554, - 46.204887640771894, - 42.82491499809031, - 43.659875176737565 + 39.171262180325584, + 39.23046156421673, + 38.877985037763544, + 38.93599048454856 ], - "timestamp": 1.4484416893016012 + "timestamp": 1.397945580397238 }, { - "x": 2.580134116922562, - "y": 4.297663630247455, - "heading": -0.3451094282478584, - "angularVelocity": 0.04909821482314941, - "velocityX": -0.4137042155527122, - "velocityY": 0.34671550166182774, + "x": 2.5262212871298537, + "y": 4.211106588413693, + "heading": -0.43034629999514007, + "angularVelocity": -0.45250195169094765, + "velocityX": -0.6967286302216157, + "velocityY": -0.18758897163586385, "moduleForcesX": [ - -52.56862851207183, - -51.668428542826135, - -54.74294626604449, - -53.98380292140949 + -57.23539499202029, + -57.144466338933306, + -57.72649841775235, + -57.64291104202704 ], "moduleForcesY": [ - 46.169623323178776, - 47.17145541851404, - 43.5681756135052, - 44.50151083261666 + 40.24658036814662, + 40.37467240535863, + 39.53878833074971, + 39.65961739205876 ], - "timestamp": 1.519914066027245 + "timestamp": 1.4856582392167472 }, { - "x": 2.5361681561986593, - "y": 4.335274736263326, - "heading": -0.3396203018012238, - "angularVelocity": 0.07680067038585842, - "velocityX": -0.6151461937353943, - "velocityY": 0.5262327598304285, + "x": 2.4417769879525806, + "y": 4.211962057230158, + "heading": -0.4686374057597009, + "angularVelocity": -0.4365516480734518, + "velocityX": -0.9627378797280479, + "velocityY": 0.009753082714672557, "moduleForcesX": [ - -51.50047411154375, - -50.41891834157215, - -53.90421586456722, - -52.99033254736792 + -55.72076727914722, + -55.51641572278753, + -56.81091067061447, + -56.64110655562729 ], "moduleForcesY": [ - 47.344772853066466, - 48.49067038940593, - 44.586953730344504, - 45.66446768873032 + 42.28603999562416, + 42.55126275898099, + 40.80924397426319, + 41.041748680703016 ], - "timestamp": 1.5913864427528888 + "timestamp": 1.5733708980362564 }, { - "x": 2.4782427587314393, - "y": 4.386182986369333, - "heading": -0.3319081747833718, - "angularVelocity": 0.10790360375738635, - "velocityX": -0.810458532923371, - "velocityY": 0.7122786797293141, + "x": 2.3355650057145154, + "y": 4.232002277874596, + "heading": -0.503478852500652, + "angularVelocity": -0.3972225584068319, + "velocityX": -1.2109082505028035, + "velocityY": 0.2284758085563137, "moduleForcesX": [ - -49.88208352874225, - -48.50022068908951, - -52.625503387992744, - -51.45217423757492 + -51.2927800550753, + -50.51860226979334, + -54.177916968511006, + -53.6319910684121 ], "moduleForcesY": [ - 49.02799707238364, - 50.389801569506666, - 46.06811701007498, - 47.36853338734092 + 47.48303091571001, + 48.29512533624039, + 44.16017520097052, + 44.809508828083096 ], - "timestamp": 1.6628588194785325 + "timestamp": 1.6610835568557656 }, { - "x": 2.407106964769289, - "y": 4.45111875799722, - "heading": -0.321583297914309, - "angularVelocity": 0.14445968277546212, - "velocityX": -0.9952907323070566, - "velocityY": 0.9085436161792976, + "x": 2.230262329026912, + "y": 4.2803026993216955, + "heading": -0.5182440105434433, + "angularVelocity": -0.1683355429137465, + "velocityX": -1.2005413825647344, + "velocityY": 0.5506664841443313, "moduleForcesX": [ - -47.15780192777166, - -45.2126943325825, - -50.44841035705035, - -48.77740463936025 + -2.1988028491138176, + 18.75163370930781, + -17.449445210541747, + 9.653164245490055 ], "moduleForcesY": [ - 51.624929028521905, - 53.32888659245528, - 48.40981741843909, - 50.08380727693866 + 69.51849752515592, + 66.91237895442454, + 67.17046621738278, + 68.54244686361893 ], - "timestamp": 1.7343311962041763 + "timestamp": 1.7487962156752748 }, { - "x": 2.3242974518893162, - "y": 4.531358566520759, - "heading": -0.3079313440530546, - "angularVelocity": 0.1910102124288775, - "velocityX": -1.158622623127807, - "velocityY": 1.1226688178584086, + "x": 2.152462213917938, + "y": 4.319517061002876, + "heading": -0.5266433649846194, + "angularVelocity": -0.0957598886434293, + "velocityX": -0.8869884479162654, + "velocityY": 0.4470775622235401, "moduleForcesX": [ - -41.72003068858196, - -38.48311349131453, - -45.98995227278055, - -43.116081797134584 + 67.79520676550965, + 67.25325195865238, + 65.1211910170664, + 64.67812621423997 ], "moduleForcesY": [ - 56.06234348401295, - 58.3198188676031, - 52.60707005821105, - 54.9720866974948 + -16.84436623215845, + -18.954280887505558, + -25.269117070430408, + -26.430366277427 ], - "timestamp": 1.80580357292982 + "timestamp": 1.836508874494784 }, { - "x": 2.2342218638275644, - "y": 4.629362025502949, - "heading": -0.2893536832656072, - "angularVelocity": 0.25992784399271474, - "velocityX": -1.260285334646795, - "velocityY": 1.3712074961147915, + "x": 2.1009759036389104, + "y": 4.3464600486777165, + "heading": -0.5314994814723972, + "angularVelocity": -0.05536391842561765, + "velocityX": -0.586988366011551, + "velocityY": 0.3071733092757232, "moduleForcesX": [ - -27.000280396790707, - -19.781400792593253, - -32.92142671337878, - -25.680048193575647 + 64.46794051526028, + 64.23813702837967, + 62.409666658641854, + 62.28438604366849 ], "moduleForcesY": [ - 64.36040574604013, - 66.91361057866158, - 61.51512045582401, - 64.8450531339154 + -27.11638680674935, + -27.668250188778345, + -31.56542143100178, + -31.82242904032056 ], - "timestamp": 1.8772759496554638 + "timestamp": 1.9242215333142931 }, { - "x": 2.1541934591617964, - "y": 4.743577081279941, - "heading": -0.2632851786920285, - "angularVelocity": 0.3647353812470825, - "velocityX": -1.119710973304069, - "velocityY": 1.598030752209193, + "x": 2.075350046157825, + "y": 4.360164165496833, + "heading": -0.533708602141933, + "angularVelocity": -0.025185881938447372, + "velocityX": -0.2921568884807754, + "velocityY": 0.15623875736483453, "moduleForcesX": [ - 28.898022383948128, - 40.38279327621932, - 31.518367214748288, - 44.919632243477565 + 63.13274035219152, + 63.00549492508412, + 61.47635196590758, + 61.41979412286576 ], "moduleForcesY": [ - 63.33426630830682, - 56.72995559800382, - 61.95891551538514, - 53.10093359505647 + -30.15395861597918, + -30.42427133590258, + -33.40120497268921, + -33.50998064492499 ], - "timestamp": 1.9487483263811076 + "timestamp": 2.0119341921338023 }, { - "x": 2.0933736359860933, - "y": 4.858226801594099, - "heading": -0.2335644163554314, - "angularVelocity": 0.41583565145382007, - "velocityX": -0.850955654977675, - "velocityY": 1.6041123219358235, + "x": 2.075350046157837, + "y": 4.360164165496826, + "heading": -0.533708602141933, + "angularVelocity": 1.9003761794839432e-18, + "velocityX": -1.0839332683477329e-17, + "velocityY": -8.204493501249472e-18, "moduleForcesX": [ - 69.47635686741319, - 69.65947112210823, - 69.73585524619895, - 69.7189941107446 + 62.43601779411841, + 62.3533183381444, + 61.007584822299386, + 60.978323649518536 ], "moduleForcesY": [ - 6.227625887406924, - 4.078499983627095, - -1.256444304150436, - -2.7455509544386563 + -31.598846071636423, + -31.764904680822614, + -34.275480740090025, + -34.33047069669531 ], - "timestamp": 2.0202207031067516 + "timestamp": 2.0996468509533113 }, { - "x": 2.0511871628096467, - "y": 4.967985225687469, - "heading": -0.2022159861765387, - "angularVelocity": 0.4386090349181448, - "velocityX": -0.5902486393714047, - "velocityY": 1.53567614863841, + "x": 2.0700552992840477, + "y": 4.371185945791571, + "heading": -0.5307171268562944, + "angularVelocity": 0.052514598698894153, + "velocityX": -0.09294795401590893, + "velocityY": 0.193484589996311, "moduleForcesX": [ - 67.96496385579776, - 67.99440402036385, - 67.10639875219877, - 67.1820726301549 + -29.15063286138222, + -24.04295915317239, + -36.04668160750333, + -31.647450844160158 ], "moduleForcesY": [ - -16.191122120647563, - -16.095511247494485, - -19.44670942058393, - -19.20731800544632 + 63.602098862195575, + 65.69767587500125, + 59.95949454907031, + 62.38598801471467 ], - "timestamp": 2.0916930798323956 + "timestamp": 2.156611492400752 }, { - "x": 2.0270139823956215, - "y": 5.070867612388492, - "heading": -0.1700681728530796, - "angularVelocity": 0.4497935397741312, - "velocityX": -0.3382171104112806, - "velocityY": 1.439470621969348, + "x": 2.0597833774444276, + "y": 4.393372035050221, + "heading": -0.524594516664425, + "angularVelocity": 0.10748088702565431, + "velocityX": -0.18032101280049811, + "velocityY": 0.3894712350488606, "moduleForcesX": [ - 65.5463060074924, - 65.64862612212953, - 64.96948366218025, - 65.09044905349134 + -27.476620846712287, + -21.825401194686584, + -34.64827459504736, + -29.68674215533022 ], "moduleForcesY": [ - -24.323259893322444, - -24.052061862241104, - -25.825731707590776, - -25.525206862715248 + 64.33617296186765, + 66.45985070451147, + 60.770089849061215, + 63.333306240651716 ], - "timestamp": 2.1631654565580396 + "timestamp": 2.2135761338481927 }, { - "x": 2.020448891699325, - "y": 5.165875707863209, - "heading": -0.13755294679311533, - "angularVelocity": 0.45493416547221555, - "velocityX": -0.09185493341902934, - "velocityY": 1.3292981154257553, + "x": 2.044957147315877, + "y": 4.426895638359724, + "heading": -0.5151629333116078, + "angularVelocity": 0.16556908133125822, + "velocityX": -0.2602707530816304, + "velocityY": 0.5884984519815477, "moduleForcesX": [ - 63.950064201059824, - 64.03180591913006, - 63.654913960217954, - 63.74125341653127 + -25.288712533466384, + -18.900927962794356, + -32.77675220463139, + -27.01590875565174 ], "moduleForcesY": [ - -28.315148832203725, - -28.131653555528967, - -28.97311608059264, - -28.784456776293474 + 65.21852387263795, + 67.34050489755609, + 61.788991765210525, + 64.50595442910968 ], - "timestamp": 2.2346378332836836 + "timestamp": 2.2705407752956335 }, { - "x": 2.031218238194587, - "y": 5.2524144264055215, - "heading": -0.10493277773470358, - "angularVelocity": 0.45640246697905146, - "velocityX": 0.150678441370407, - "velocityY": 1.2107995068082167, + "x": 2.026164201599616, + "y": 4.4719674608650095, + "heading": -0.5021887224153403, + "angularVelocity": 0.22775901974628154, + "velocityX": -0.32990545079793476, + "velocityY": 0.791224544911283, "moduleForcesX": [ - 62.88066267543294, - 62.91207596743938, - 62.79228260986751, - 62.82409534697334 + -22.307319507469934, + -14.900037659160601, + -30.148527798271417, + -23.210714805289566 ], "moduleForcesY": [ - -30.650558779807053, - -30.586409677611073, - -30.831354646388686, - -30.76685591760273 + 66.28610428285815, + 68.32491313667404, + 63.09885991865561, + 65.9548404457898 ], - "timestamp": 2.3061102100093276 + "timestamp": 2.327505416743074 }, { - "x": 2.05912709236145, - "y": 5.330090045928955, - "heading": -0.072383245243066, - "angularVelocity": 0.45541416114660316, - "velocityX": 0.3904844851873833, - "velocityY": 1.086792170543661, + "x": 2.0042669459715285, + "y": 4.528837148205876, + "heading": -0.4853528018471346, + "angularVelocity": 0.2955503649354062, + "velocityX": -0.38440083307276873, + "velocityY": 0.9983331044633778, + "moduleForcesX": [ + -18.02560615257705, + -9.175322282634115, + -26.214745646530282, + -17.46079465681506 + ], + "moduleForcesY": [ + 67.5606623787124, + 69.30954637447027, + 64.81204160137834, + 67.68228538813717 + ], + "timestamp": 2.384470058190515 + }, + { + "x": 1.9806218469741539, + "y": 4.597763043773167, + "heading": -0.4641981938958335, + "angularVelocity": 0.3713638392830007, + "velocityX": -0.41508378524932976, + "velocityY": 1.2099768174769732, + "moduleForcesX": [ + -11.453582503159748, + -0.552199333246785, + -19.79399970106862, + -8.106339127595051 + ], + "moduleForcesY": [ + 68.95738867398684, + 69.88879052102858, + 67.02262284825274, + 69.39413552946995 + ], + "timestamp": 2.4414346996379557 + }, + { + "x": 1.9575322526001036, + "y": 4.678830239834948, + "heading": -0.4380421531456962, + "angularVelocity": 0.4591627382447514, + "velocityX": -0.4053320408477288, + "velocityY": 1.4231143039244516, + "moduleForcesX": [ + -0.5334552598468943, + 13.009918369721786, + -8.032358249742447, + 8.238973565498672 + ], + "moduleForcesY": [ + 69.8666713444081, + 68.63601950178051, + 69.37382880885353, + 69.32921346604414 + ], + "timestamp": 2.4983993410853964 + }, + { + "x": 1.9389906812367639, + "y": 4.77112606241245, + "heading": -0.4060463560603266, + "angularVelocity": 0.5616781967264929, + "velocityX": -0.32549263705008313, + "velocityY": 1.6202300274752126, + "moduleForcesX": [ + 18.715982262905385, + 33.707081998178694, + 15.897712534120208, + 35.51802148738027 + ], + "moduleForcesY": [ + 67.2633143302821, + 61.14210942301303, + 67.92400233992704, + 60.03846848663157 + ], + "timestamp": 2.555363982532837 + }, + { + "x": 1.93014912520743, + "y": 4.87068791134505, + "heading": -0.3685234212138124, + "angularVelocity": 0.6587057145112619, + "velocityX": -0.1552112995829446, + "velocityY": 1.7477832986004502, + "moduleForcesX": [ + 47.983848434946395, + 57.21667950593689, + 53.72495772708293, + 62.54171738572598 + ], + "moduleForcesY": [ + 50.64853650489456, + 39.96838943911703, + 44.402717653560856, + 30.875613138399547 + ], + "timestamp": 2.612328623980278 + }, + { + "x": 1.933418171455475, + "y": 4.971531596301135, + "heading": -0.32709464429833723, + "angularVelocity": 0.7272717928664595, + "velocityX": 0.057387287358971396, + "velocityY": 1.7702856086460224, "moduleForcesX": [ - 62.127913109347276, - 62.10220705945469, - 62.18868836322343, - 62.16316779871923 + 67.96928457704135, + 69.07682719509752, + 69.65789543520896, + 69.80083544623831 ], "moduleForcesY": [ - -32.17057458836276, - -32.21997133029195, - -32.052845514471784, - -32.1021124146997 + 15.813702501669626, + 10.130822196720576, + 3.8406949342180523, + -0.5188082796813483 ], - "timestamp": 2.3775825867349716 + "timestamp": 2.6692932654277186 }, { - "x": 2.11398267592862, - "y": 5.407564590406167, - "heading": -0.035074691263133305, - "angularVelocity": 0.4522495337757732, - "velocityX": 0.6649523885483815, - "velocityY": 0.939136547950644, + "x": 1.9485966046329035, + "y": 5.069631244361437, + "heading": -0.28344891936809435, + "angularVelocity": 0.7661897594934095, + "velocityX": 0.2664535893100171, + "velocityY": 1.722114728850099, "moduleForcesX": [ - 61.580309305924985, - 61.49718284699979, - 61.749856139779475, - 61.66816132300872 + 68.8247416901388, + 68.711613893991, + 67.18557930818034, + 67.18883251525818 ], "moduleForcesY": [ - -33.23357506637483, - -33.386760848755074, - -32.91728042389969, - -33.06968489028737 + -11.868262004119803, + -12.595528771030496, + -19.06951150843806, + -19.117544695444614 ], - "timestamp": 2.4600780924096304 + "timestamp": 2.7262579068751593 }, { - "x": 2.191082677171206, - "y": 5.472153160220583, - "heading": 0.0016521215955720662, - "angularVelocity": 0.44519774208758894, - "velocityX": 0.9345963836882241, - "velocityY": 0.7829344083945624, + "x": 1.9749895957767274, + "y": 5.1628267728214325, + "heading": -0.238677619130099, + "angularVelocity": 0.7859489518476898, + "velocityX": 0.46332234300422453, + "velocityY": 1.6360241386928376, + "moduleForcesX": [ + 64.61918569816321, + 64.82164567363998, + 63.15374635838673, + 63.45210887594756 + ], + "moduleForcesY": [ + -26.610781563662005, + -26.130445473862178, + -29.925633288523958, + -29.30220507461484 + ], + "timestamp": 2.7832225483226 + }, + { + "x": 2.0119893346706585, + "y": 5.249897971911408, + "heading": -0.19345274501313633, + "angularVelocity": 0.7939113275853787, + "velocityX": 0.649521140724979, + "velocityY": 1.528513071925737, + "moduleForcesX": [ + 60.77468821171452, + 60.99722299740147, + 60.0777269685818, + 60.31974783111097 + ], + "moduleForcesY": [ + -34.556593215262986, + -34.166181049759004, + -35.755885616715915, + -35.34978878089667 + ], + "timestamp": 2.840187189770041 + }, + { + "x": 2.05912709236145, + "y": 5.330090045928955, + "heading": -0.14821270337422277, + "angularVelocity": 0.7941775896308426, + "velocityX": 0.8274915191783369, + "velocityY": 1.407751755824489, "moduleForcesX": [ - 60.45923369475699, - 60.23455621610159, - 60.84356535380034, - 60.625874690348816 + 57.8734037104444, + 57.88504600107734, + 57.84871177168228, + 57.86037646750411 ], "moduleForcesY": [ - -35.214733524291056, - -35.59671720906772, - -34.545862040838756, - -34.92548722348504 + -39.255748814527, + -39.23866862035294, + -39.292170409475375, + -39.27508028582055 ], - "timestamp": 2.542573598084289 + "timestamp": 2.8971518312174815 }, { - "x": 2.289728178771266, - "y": 5.522730614286461, - "heading": 0.037276639055293805, - "angularVelocity": 0.4318358578252923, - "velocityX": 1.1957681909289004, - "velocityY": 0.6130934487194334, + "x": 2.1419011012670555, + "y": 5.425958346458515, + "heading": -0.08687313324288204, + "angularVelocity": 0.7872633235969319, + "velocityX": 1.0623638414637935, + "velocityY": 1.2304226576887645, "moduleForcesX": [ - 58.52744287678723, - 57.99177342881693, - 59.27377627441346, - 58.761492424372356 + 55.744309007626626, + 55.4487335715346, + 56.215540263624625, + 55.92748705805922 ], "moduleForcesY": [ - -38.31446039097551, - -39.11845718880393, - -37.14787466755693, - -37.95082764754945 + -42.276636780585044, + -42.66266684218691, + -41.64740235699094, + -42.03249730192701 ], - "timestamp": 2.625069103758948 + "timestamp": 2.9750667655130183 }, { - "x": 2.408421432901453, - "y": 5.557250524578989, - "heading": 0.07081951118626838, - "angularVelocity": 0.40660241860066865, - "velocityX": 1.4387844903195497, - "velocityY": 0.41844595069491913, + "x": 2.2414261398578335, + "y": 5.506181529743673, + "heading": -0.02721344128448102, + "angularVelocity": 0.7657029104601152, + "velocityX": 1.2773550987449058, + "velocityY": 1.029625244640082, "moduleForcesX": [ - 54.50941828585798, - 53.13358487259185, - 55.95915631243375, - 54.647136092353065 + 51.02163650907591, + 49.70119985383043, + 52.48797262102768, + 51.22069659742051 ], "moduleForcesY": [ - -43.802094987807024, - -45.45697038399386, - -41.92981110097996, - -43.62112365277983 + -47.85047801815092, + -49.21787446602869, + -46.23470435722217, + -47.63176527887239 ], - "timestamp": 2.7075646094336068 + "timestamp": 3.052981699808555 }, { - "x": 2.542474709173094, - "y": 5.571243535937252, - "heading": 0.09992456939909769, - "angularVelocity": 0.3528078041928978, - "velocityX": 1.6249767211345831, - "velocityY": 0.1696214981769599, + "x": 2.354490568089435, + "y": 5.567942649448705, + "heading": 0.028816977553997255, + "angularVelocity": 0.719122968466496, + "velocityX": 1.4511265298991398, + "velocityY": 0.7926737057977623, "moduleForcesX": [ - 42.694250355720705, - 38.054672270282296, - 45.591148182962975, - 40.87638058379842 + 41.924684435294694, + 37.78420628587397, + 44.83708919222699, + 40.69029874809796 ], "moduleForcesY": [ - -55.307949027779564, - -58.58896847735615, - -52.92949409903056, - -56.63909921586958 + -55.96662276450357, + -58.83646046865295, + -53.653582227691004, + -56.85649586466601 ], - "timestamp": 2.7900601151082656 + "timestamp": 3.130896634104092 }, { - "x": 2.668629243751066, - "y": 5.561025654990008, - "heading": 0.11917705216572184, - "angularVelocity": 0.2333761410288788, - "velocityX": 1.529229180843634, - "velocityY": -0.12385985482443941, + "x": 2.473359530149637, + "y": 5.60763951076386, + "heading": 0.07772134675261151, + "angularVelocity": 0.6276636134109629, + "velocityX": 1.5256248771169323, + "velocityY": 0.5094897618033285, "moduleForcesX": [ - -12.382402533780613, - -24.607511319028973, - -17.558309914075192, - -31.441206870964095 + 22.049102020071857, + 11.270972185107798, + 25.093613830164124, + 12.425514812528533 ], "moduleForcesY": [ - -68.63720397945411, - -65.28047130453552, - -67.43128061515037, - -62.222247817779476 + -66.32736429857982, + -68.97836591275289, + -65.21334822172665, + -68.75568538466969 ], - "timestamp": 2.8725556207829244 + "timestamp": 3.2088115683996286 }, { - "x": 2.7698521306860013, - "y": 5.54500330305322, - "heading": 0.13176004586708878, - "angularVelocity": 0.15252944507008542, - "velocityX": 1.2270109278923758, - "velocityY": -0.19422090026996, + "x": 2.583125662677607, + "y": 5.626543406923644, + "heading": 0.11601498179594151, + "angularVelocity": 0.49148004024593883, + "velocityX": 1.4087945208503867, + "velocityY": 0.24262224348518271, "moduleForcesX": [ - -66.46018133159996, - -66.9674196584085, - -68.9644976388019, - -69.02558396413289 + -16.70068482620098, + -30.482853391136345, + -24.37959308564339, + -39.528854162952214 ], "moduleForcesY": [ - -21.47227954276657, - -19.910515932777027, - -11.024044579675353, - -10.78336011685823 + -67.844870324751, + -62.8857363057114, + -65.43315052803204, + -57.59599684491268 ], - "timestamp": 2.955051126457583 + "timestamp": 3.2867265026951653 }, { - "x": 2.845436141122318, - "y": 5.530116306121047, - "heading": 0.14009236993377866, - "angularVelocity": 0.10100336980206083, - "velocityX": 0.916219735867702, - "velocityY": -0.1804582814458009, + "x": 2.673631838796821, + "y": 5.633378148129679, + "heading": 0.14407534512325684, + "angularVelocity": 0.3601410125160406, + "velocityX": 1.1616024185545568, + "velocityY": 0.08772055406104393, "moduleForcesX": [ - -69.91668485679588, - -69.92377181874357, - -69.57042974483447, - -69.70602424237894 + -51.90525981906655, + -57.051226471868404, + -61.89152614197463, + -64.20272534700426 ], "moduleForcesY": [ - 0.35481804279217377, - -0.526848643787446, - 6.976161517175589, - 5.555870742457722 + -46.802489344466096, + -40.40908629215477, + -32.43095750906469, + -27.650831182455995 ], - "timestamp": 3.037546632132242 + "timestamp": 3.364641436990702 }, { - "x": 2.8956151191054844, - "y": 5.519017848354159, - "heading": 0.14518444674969433, - "angularVelocity": 0.06172550582309565, - "velocityX": 0.6082631723848511, - "velocityY": -0.13453409128336982, + "x": 2.7420691702187887, + "y": 5.6345006557115855, + "heading": 0.16357365067508123, + "angularVelocity": 0.2502511967457482, + "velocityX": 0.8783596115525151, + "velocityY": 0.01440683473656765, "moduleForcesX": [ - -69.4187453897527, - -69.56564259987941, - -68.65234610284632, - -68.9344391693208 + -65.04850079574194, + -65.99350180832509, + -69.15045925519797, + -69.13827378457684 ], "moduleForcesY": [ - 8.590706564201634, - 7.346324469918923, - 13.40936902595393, - 11.897426701626598 + -25.651238870662098, + -23.15939656800476, + -10.354928047129325, + -10.547196980897708 ], - "timestamp": 3.1200421378069008 + "timestamp": 3.442556371286239 }, { - "x": 2.9206175804135657, - "y": 5.513061523434405, - "heading": 0.14756797074007869, - "angularVelocity": 0.02889277386548866, - "velocityX": 0.3030766476923993, - "velocityY": -0.07220180908862774, + "x": 2.7877749257651523, + "y": 5.633422199584155, + "heading": 0.1758142407370851, + "angularVelocity": 0.15710197502797701, + "velocityX": 0.5866109746431721, + "velocityY": -0.013841455905480958, "moduleForcesX": [ - -68.79757631258838, - -69.02339707894248, - -67.96227334834312, - -68.30019620815122 + -68.75220978257724, + -68.781223473534, + -69.94627706176999, + -69.93907335246656 ], "moduleForcesY": [ - 12.723388654681191, - 11.450987875380186, - 16.62115678249245, - 15.184154939108327 + -12.86879255012935, + -12.770261545400503, + 0.3760238076675933, + -1.5977856312836116 ], - "timestamp": 3.2025376434815596 + "timestamp": 3.5204713055817756 }, { - "x": 2.9206175804138184, - "y": 5.5130615234375, - "heading": 0.14756797074007869, - "angularVelocity": -2.1366473717756725e-18, - "velocityX": 2.759428296475241e-16, - "velocityY": 1.8508305640470853e-16, + "x": 2.810619354248047, + "y": 5.6322150230407715, + "heading": 0.18164838967891364, + "angularVelocity": 0.0748784426833914, + "velocityX": 0.2931970448212678, + "velocityY": -0.015493519365684846, "moduleForcesX": [ - -68.30769120451006, - -68.57184176455583, - -67.47562728593, - -67.83343378701302 + -69.7650098576931, + -69.68462891027929, + -69.679901493562, + -69.87274192709826 ], "moduleForcesY": [ - 15.178954859633443, - 13.946231227068052, - 18.53346910587473, - 17.184707214005112 + -5.227252657372436, + -6.289797322807944, + 6.278310865228548, + 3.6678202564057916 ], - "timestamp": 3.2850331491562184 + "timestamp": 3.5983862398773123 }, { - "x": 2.9008988708903125, - "y": 5.5307170603623375, - "heading": 0.14700286177553085, - "angularVelocity": -0.006751511016409849, - "velocityX": -0.23558480387769798, - "velocityY": 0.2109355181352119, + "x": 2.810619354248047, + "y": 5.6322150230407715, + "heading": 0.18164838967891364, + "angularVelocity": 1.7820258276732997e-21, + "velocityX": -3.71309242331424e-21, + "velocityY": -1.8466663513371007e-20, "moduleForcesX": [ - -52.09999330596725, - -52.529400631239085, - -51.73320472903666, - -52.16512079541669 + -69.96925933574576, + -69.94810083182944, + -69.26326437970721, + -69.61542540763588 ], "moduleForcesY": [ - 46.71445570132471, - 46.23149173401103, - 47.1208354788092, - 46.6426390261249 + -0.3390303743865954, + -1.9603647789452194, + 9.931322934759566, + 7.100594511510579 ], - "timestamp": 3.3687342589037015 + "timestamp": 3.676301174172849 }, { - "x": 2.861623494471227, - "y": 5.566204209893025, - "heading": 0.14583871741812887, - "angularVelocity": -0.013908350330240071, - "velocityX": -0.4692336401967019, - "velocityY": 0.42397465993079464, + "x": 2.791342988452625, + "y": 5.642521810363919, + "heading": 0.18283388161538688, + "angularVelocity": 0.015584006009315527, + "velocityX": -0.25339944638282685, + "velocityY": 0.1354889313364025, "moduleForcesX": [ - -51.6659111374572, - -52.12885129412831, - -51.276879306050894, - -51.74245743341866 + -61.62075540054056, + -60.98102694031713, + -62.39838441045431, + -61.79343760463002 ], "moduleForcesY": [ - 47.185653590179186, - 46.67421839348704, - 47.60875054548638, - 47.10282037910355 + 33.15309974417089, + 34.31372499920319, + 31.663594414249264, + 32.826466274276484 ], - "timestamp": 3.4524353686511846 + "timestamp": 3.752372239325454 }, { - "x": 2.803016239597012, - "y": 5.619762027969135, - "heading": 0.14402910636298827, - "angularVelocity": -0.021619917114522248, - "velocityX": -0.7001968676942122, - "velocityY": 0.6398698673768739, + "x": 2.753067309241903, + "y": 5.663631883203031, + "heading": 0.18530131117107015, + "angularVelocity": 0.03243584864670181, + "velocityX": -0.5031568722475193, + "velocityY": 0.2775046306603334, "moduleForcesX": [ - -51.06312930480856, - -51.57332722665041, - -50.64375367097991, - -51.156724254575956 + -60.74636949447462, + -59.995692501352465, + -61.60810614324199, + -60.89635912716705 ], "moduleForcesY": [ - 47.8258847940151, - 47.27589622960445, - 48.27053972557859, - 47.72716661234478 + 34.71442361257027, + 35.99400899987649, + 33.15894228397032, + 34.44613293316394 ], - "timestamp": 3.5361364783986677 + "timestamp": 3.828443304478059 }, { - "x": 2.725409512823363, - "y": 5.691733398122384, - "heading": 0.14150668290126167, - "angularVelocity": -0.030136081460894748, - "velocityX": -0.9271887436445178, - "velocityY": 0.8598615995220361, + "x": 2.6962228110923627, + "y": 5.696261557261839, + "heading": 0.18919034306075003, + "angularVelocity": 0.05112366813686836, + "velocityX": -0.7472551887567889, + "velocityY": 0.42893673163836527, "moduleForcesX": [ - -50.170225601037366, - -50.75200909732681, - -49.70741389665356, - -50.29204690115321 + -59.39626570686199, + -58.46567557598025, + -60.37984931281294, + -59.49315400848136 ], "moduleForcesY": [ - 48.74540511502207, - 48.14020759141128, - 49.218329972809045, - 48.621580569011485 + 36.95663241834814, + 38.40914674537991, + 35.32399090200627, + 36.79466428895038 ], - "timestamp": 3.619837588146151 + "timestamp": 3.9045143696306637 }, { - "x": 2.6293437718673225, - "y": 5.782650350616714, - "heading": 0.1381651310810073, - "angularVelocity": -0.039922431498496204, - "velocityX": -1.147723623091234, - "velocityY": 1.0862096426984997, + "x": 2.621558016690057, + "y": 5.741526222171845, + "heading": 0.19471996442816977, + "angularVelocity": 0.07269020561664075, + "velocityX": -0.9815137234179877, + "velocityY": 0.5950313015757177, "moduleForcesX": [ - -48.71431630344826, - -49.4167391577109, - -48.18511463537286, - -48.890119594937715 + -57.07004054859922, + -55.81145405690308, + -58.24064731344431, + -57.02953301737525 ], "moduleForcesY": [ - 50.17522088948343, - 49.48473182662626, - 50.68521987830446, - 50.00663478484992 + 40.42700972423277, + 42.1440264982245, + 38.71744543220449, + 40.476187384360045 ], - "timestamp": 3.703538697893634 + "timestamp": 3.9805854347832685 }, { - "x": 2.5158455258965873, - "y": 5.893444520165239, - "heading": 0.13381239728196173, - "angularVelocity": -0.05200329854838183, - "velocityX": -1.355994518259599, - "velocityY": 1.3236881799931342, + "x": 2.5306316551845427, + "y": 5.801358526557014, + "heading": 0.2022754435822446, + "angularVelocity": 0.09932132722103867, + "velocityX": -1.1952818239511769, + "velocityY": 0.7865317024960887, "moduleForcesX": [ - -45.93506137526542, - -46.87946903712664, - -45.29576298382204, - -46.24053944561288 + -52.296776213679415, + -50.323936372341834, + -53.758106723210474, + -51.816584350831235 ], "moduleForcesY": [ - 52.687688259487246, - 51.851007353746446, - 53.24100600936717, - 52.42425460814814 + 46.39111947683355, + 48.51956857347833, + 44.682603452825234, + 46.914906732410614 ], - "timestamp": 3.787239807641117 + "timestamp": 4.056656499935873 }, { - "x": 2.3875470683794213, - "y": 6.02610991590263, - "heading": 0.12800509474442898, - "angularVelocity": -0.06938142821559826, - "velocityX": -1.5328166842349815, - "velocityY": 1.5849896874494018, + "x": 2.427863523047455, + "y": 5.879498749258094, + "heading": 0.2126425688241101, + "angularVelocity": 0.13628210964403, + "velocityX": -1.350949036022118, + "velocityY": 1.0272003230706037, "moduleForcesX": [ - -38.731601936491195, - -40.35076624830184, - -37.91551455677665, - -39.51613010702398 + -39.09732377550607, + -35.17129355710182, + -40.71904985918127, + -36.62147361504708 ], "moduleForcesY": [ - 58.094777198180715, - 56.98539695236494, - 58.63708159838333, - 57.57358536077538 + 57.87480113678438, + 60.336542269027305, + 56.73087731606344, + 59.45243630995032 ], - "timestamp": 3.8709409173886002 + "timestamp": 4.132727565088478 }, { - "x": 2.2585637323391317, - "y": 6.185053475715637, - "heading": 0.11910747265558115, - "angularVelocity": -0.10630231923689437, - "velocityX": -1.5409991149465732, - "velocityY": 1.8989420845305607, + "x": 2.3297978130131964, + "y": 5.978878854714064, + "heading": 0.22690067635521388, + "angularVelocity": 0.18743141695861396, + "velocityX": -1.2891328632973713, + "velocityY": 1.3064113833111803, "moduleForcesX": [ - 0.7933919130137743, - -3.975029098028374, - 0.1994458839598195, - -4.26048134968995 + 10.545576170240913, + 16.557527776184923, + 13.352744563742135, + 19.74884527271297 ], "moduleForcesY": [ - 69.51699522375354, - 69.4045334427663, - 69.55309629363511, - 69.42011317516652 + 68.92487436784589, + 67.74390307628403, + 68.40688100444298, + 66.8566796699288 ], - "timestamp": 3.9546420271360834 + "timestamp": 4.208798630241083 }, { - "x": 2.155822904626011, - "y": 6.341651289925754, - "heading": 0.10835395117931683, - "angularVelocity": -0.1284752556892115, - "velocityX": -1.2274727064325863, - "velocityY": 1.8709168195383514, + "x": 2.2518195232296936, + "y": 6.086732649237896, + "heading": 0.24339103584492305, + "angularVelocity": 0.2167757143485253, + "velocityX": -1.0250716172709238, + "velocityY": 1.4178031332605687, "moduleForcesX": [ - 69.29850135005749, - 69.22376482749928, - 69.52084242344242, - 69.4745660602554 + 63.3765480815569, + 63.68121368261706, + 64.96422299639283, + 65.1554779681156 ], "moduleForcesY": [ - -7.254272274145535, - -7.823192974180219, - -4.62731192687223, - -5.1017659452285775 + 29.25944062691299, + 28.61722194785657, + 25.535838612439772, + 25.07538960318053 ], - "timestamp": 4.038343136883566 + "timestamp": 4.284869695393688 }, { - "x": 2.0772115888518736, - "y": 6.487483095533371, - "heading": 0.0969715087930836, - "angularVelocity": -0.13598914543167476, - "velocityX": -0.939190842449076, - "velocityY": 1.7422923808692223, + "x": 2.195636505947272, + "y": 6.195898602093869, + "heading": 0.2608681511360735, + "angularVelocity": 0.229747214083171, + "velocityX": -0.7385596240793223, + "velocityY": 1.4350522453836667, "moduleForcesX": [ - 63.72237766804347, - 63.53223394842242, - 64.05032016117235, - 63.867552477264695 + 69.71870355427134, + 69.69809382001247, + 69.8203423782785, + 69.80580410665718 ], "moduleForcesY": [ - -28.628204478983974, - -29.043494705306433, - -27.884225907046652, - -28.295904148066843 + 4.902598216652911, + 5.226579532658753, + 3.1528242764653718, + 3.5174449089562745 ], - "timestamp": 4.122044246631049 + "timestamp": 4.360940760546293 }, { - "x": 2.021637442637373, - "y": 6.620330494187104, - "heading": 0.08532200026076654, - "angularVelocity": -0.1391798575606646, - "velocityX": -0.6639594908779467, - "velocityY": 1.587164125330234, + "x": 2.1612401984623397, + "y": 6.303431320252094, + "heading": 0.278796165903002, + "angularVelocity": 0.23567455945257776, + "velocityX": -0.45216019278723374, + "velocityY": 1.4135823909196665, "moduleForcesX": [ - 60.87885944738181, - 60.7694927196997, - 61.04027054010648, - 60.93220988562005 + 69.7517709773252, + 69.77145706900596, + 69.6934539088278, + 69.71663436820566 ], "moduleForcesY": [ - -34.375983359242426, - -34.56803319978518, - -34.087899420795566, - -34.2797476711687 + -4.983366244420907, + -4.712773441080035, + -5.745679950464318, + -5.468339576313232 ], - "timestamp": 4.205745356378531 + "timestamp": 4.437011825698898 }, { - "x": 1.9885261521674162, - "y": 6.739204165667878, - "heading": 0.07357290264571206, - "angularVelocity": -0.14036967550974822, - "velocityX": -0.39558962325593, - "velocityY": 1.4202161922459862, + "x": 2.1484700695340293, + "y": 6.407813780862964, + "heading": 0.29689533472311547, + "angularVelocity": 0.2379244826374525, + "velocityX": -0.1678710414096705, + "velocityY": 1.372170356777184, "moduleForcesX": [ - 59.37783973737232, - 59.33213553424243, - 59.44147378924915, - 59.395945957037256 + 69.23031154412588, + 69.24884860764138, + 69.18986060193748, + 69.20902830787257 ], "moduleForcesY": [ - -36.95821427423069, - -37.031312892639825, - -36.85560981002678, - -36.92870573976187 + -10.008624570555261, + -9.881273409427275, + -10.285342371577995, + -10.157228071808754 ], - "timestamp": 4.289446466126014 + "timestamp": 4.513082890851503 }, { - "x": 1.977528756045062, - "y": 6.843547542559394, - "heading": 0.061820288525356495, - "angularVelocity": -0.14041168815821406, - "velocityX": -0.13138889263033743, - "velocityY": 1.246618800486609, + "x": 2.157177448272705, + "y": 6.508133888244629, + "heading": 0.31499648569215616, + "angularVelocity": 0.2379505391797553, + "velocityX": 0.11446374151864286, + "velocityY": 1.318768275170261, "moduleForcesX": [ - 58.46396442972924, - 58.46225971251114, - 58.46628338376228, - 58.46457888275662 + 68.74379080687748, + 68.74410650557303, + 68.74320786149968, + 68.74352365244148 ], "moduleForcesY": [ - -38.41537453860932, - -38.417962696829065, - -38.41184043411793, - -38.41442860346031 + -13.001771747122337, + -13.000114222047666, + -13.004860154304824, + -13.00320254851976 ], - "timestamp": 4.373147575873497 + "timestamp": 4.5891539560041075 }, { - "x": 1.9884117094887013, - "y": 6.933004553123894, - "heading": 0.05012639395544798, - "angularVelocity": -0.13971014966457962, - "velocityX": 0.1300216147344071, - "velocityY": 1.0687673173254597, + "x": 2.184724710816574, + "y": 6.59922785852166, + "heading": 0.3320932322725307, + "angularVelocity": 0.23656836371790554, + "velocityX": 0.38117256954556666, + "velocityY": 1.2604708967109606, "moduleForcesX": [ - 57.85196007751825, - 57.88125766772754, - 57.812373545441886, - 57.841731008840526 + 68.34746493653803, + 68.32541439579168, + 68.38381933803426, + 68.36206742712406 ], "moduleForcesY": [ - -39.3486892907348, - -39.30566232094738, - -39.40689132743343, - -39.363869274238354 + -14.974761050304606, + -15.074567367293154, + -14.807552238846894, + -14.90716016282338 ], - "timestamp": 4.4568486856209795 + "timestamp": 4.6614237485988665 }, { - "x": 2.021008014678955, - "y": 7.007328033447266, - "heading": 0.03853480959994491, - "angularVelocity": -0.13848782161284204, - "velocityX": 0.38943695354526725, - "velocityY": 0.8879629018104452, + "x": 2.2313650052611718, + "y": 6.685359777525342, + "heading": 0.34895139183908297, + "angularVelocity": 0.23326702570023544, + "velocityX": 0.6453636127908321, + "velocityY": 1.1918107955097217, "moduleForcesX": [ - 57.41445936428743, - 57.46628160975792, - 57.34427766903865, - 57.39628164773986 + 67.69387597978809, + 67.62571914819677, + 67.79222902236084, + 67.72581400551105 ], "moduleForcesY": [ - -39.99658250377327, - -39.922209282283085, - -40.09723139207876, - -40.02287498111711 + -17.65912805639218, + -17.917221977463345, + -17.276880276884725, + -17.53424163554366 ], - "timestamp": 4.540549795368462 + "timestamp": 4.733693541193626 }, { - "x": 2.0826299228686898, - "y": 7.070370365951682, - "heading": 0.02599725344124828, - "angularVelocity": -0.1367266901870693, - "velocityX": 0.6720097158166497, - "velocityY": 0.687499968419269, + "x": 2.2967574046117547, + "y": 6.76538030405312, + "heading": 0.3653570894706221, + "angularVelocity": 0.2270062918753815, + "velocityX": 0.9048372356242996, + "velocityY": 1.1072472142888359, "moduleForcesX": [ - 57.08917572518026, - 57.15769997977353, - 56.9954356988548, - 57.06427617407771 + 66.47960800608301, + 66.30177808290506, + 66.69591112278235, + 66.52423830099455 ], "moduleForcesY": [ - -40.47369675811929, - -40.37699177913841, - -40.605690518380186, - -40.509013116235465 + -21.74308831253873, + -22.277371179768945, + -21.068430447014848, + -21.60216639071113 ], - "timestamp": 4.632247737749421 + "timestamp": 4.805963333788385 }, { - "x": 2.169672617334707, - "y": 7.114364526461086, - "heading": 0.013740199626283799, - "angularVelocity": -0.13366770831190322, - "velocityX": 0.9492327984450655, - "velocityY": 0.4797726026430948, + "x": 2.3801398957317264, + "y": 6.837336652626878, + "heading": 0.3809436958576124, + "angularVelocity": 0.21567249368473568, + "velocityX": 1.153766852321353, + "velocityY": 0.9956628625910408, "moduleForcesX": [ - 56.01669479138227, - 56.14220393478205, - 55.84946987132304, - 55.97590888149232 + 63.806559308723315, + 63.31882271721067, + 64.26880839630248, + 63.798069788806266 ], "moduleForcesY": [ - -41.93127449705145, - -41.76334146364884, - -42.153954194749936, - -41.98616963230991 + -28.59447611383761, + -29.655406522966526, + -27.53587934185067, + -28.605861210958697 ], - "timestamp": 4.72394568013038 + "timestamp": 4.878233126383144 }, { - "x": 2.2812482301938513, - "y": 7.138190544722294, - "heading": 0.0019682714125729057, - "angularVelocity": -0.12837723408034055, - "velocityX": 1.2167733535710465, - "velocityY": 0.25983154852058693, + "x": 2.479259804153409, + "y": 6.897447135971791, + "heading": 0.39498518087177925, + "angularVelocity": 0.19429258767770655, + "velocityX": 1.3715261226427125, + "velocityY": 0.831751153375685, "moduleForcesX": [ - 54.06938340379889, - 54.308966898728, - 53.770349274089426, - 54.012513868838 + 56.13643988602569, + 54.52558414545473, + 57.08952254401824, + 55.48617464288215 ], "moduleForcesY": [ - -44.39082318658512, - -44.09795887662825, - -44.753051093939305, - -44.46103930411709 + -41.60713844028672, + -43.692054420376515, + -40.280169475319, + -42.45608521234071 ], - "timestamp": 4.815643622511339 + "timestamp": 4.950502918977903 }, { - "x": 2.4153058153444134, - "y": 7.139605579002843, - "heading": -0.008889495736468035, - "angularVelocity": -0.1184079693298827, - "velocityX": 1.4619475818218677, - "velocityY": 0.015431471712328844, + "x": 2.584931627951079, + "y": 6.9390229938132615, + "heading": 0.4060308240467457, + "angularVelocity": 0.15283900476791354, + "velocityX": 1.4621852367863801, + "velocityY": 0.575286801701526, "moduleForcesX": [ - 49.535744448116404, - 50.08883535472126, - 48.95285722529716, - 49.512750306129256 + 26.148906673767343, + 20.715268485938005, + 26.009248473481666, + 20.06651880527857 ], "moduleForcesY": [ - -49.35481713361861, - -48.79483073718232, - -49.9344054264043, - -49.38065310701591 + -64.70640976971409, + -66.64626277199693, + -64.74052136771891, + -66.82336232717894 ], - "timestamp": 4.907341564892298 + "timestamp": 5.022772711572662 }, { - "x": 2.5635950141167547, - "y": 7.1126996745657065, - "heading": -0.017499861469921024, - "angularVelocity": -0.0938992251069123, - "velocityX": 1.6171485962309224, - "velocityY": -0.29341884600255425, + "x": 2.6792925575993936, + "y": 6.964515706893577, + "heading": 0.4138293757225624, + "angularVelocity": 0.10790887030139666, + "velocityX": 1.3056759437159087, + "velocityY": 0.3527436867469866, "moduleForcesX": [ - 30.82355947672089, - 33.05783383581679, - 29.658555665503656, - 31.855767521726126 + -36.39457522337274, + -39.28022118556542, + -41.130153635623216, + -43.6418492259847 ], "moduleForcesY": [ - -62.655847172898895, - -61.51032510158116, - -63.223044950314765, - -62.148500220510925 + -59.56113873853256, + -57.71718933439606, + -56.381055028824214, + -54.48253610093349 ], - "timestamp": 4.999039507273257 + "timestamp": 5.095042504167421 }, { - "x": 2.6834005581658715, - "y": 7.072012772379466, - "heading": -0.022151846620554527, - "angularVelocity": -0.05073161981418278, - "velocityX": 1.3065237989836214, - "velocityY": -0.4437057267013505, + "x": 2.756321629159372, + "y": 6.98065148897458, + "heading": 0.41934868952054644, + "angularVelocity": 0.0763709649608753, + "velocityX": 1.0658543326934196, + "velocityY": 0.22327145964679496, "moduleForcesX": [ - -64.12298736950346, - -63.25910030393463, - -62.330306323818874, - -61.2590262700717 + -60.27208113538249, + -60.51737707772543, + -62.48632553835477, + -62.5793289359259 ], "moduleForcesY": [ - -27.440487603067957, - -29.33631206040231, - -31.317162411294717, - -33.33134662941462 + -35.36566968241656, + -34.96009847227816, + -31.286074162611634, + -31.117684633539717 ], - "timestamp": 5.090737449654216 + "timestamp": 5.16731229676218 }, { - "x": 2.7720201720116067, - "y": 7.037183326012226, - "heading": -0.02500990952650958, - "angularVelocity": -0.031168233787252347, - "velocityX": 0.9664296880719148, - "velocityY": -0.37982800389550603, + "x": 2.814586704942504, + "y": 6.990746632959886, + "heading": 0.42314075402152007, + "angularVelocity": 0.052470947609287326, + "velocityX": 0.8062161754060119, + "velocityY": 0.13968690960429975, "moduleForcesX": [ - -68.53351263579651, - -68.45472035856257, - -68.92469478793043, - -68.86842514528999 + -66.06720809870912, + -65.97454972543122, + -67.12642001354656, + -67.00203153454379 ], "moduleForcesY": [ - 13.79021249565681, - 14.160173532909322, - 11.675894673290463, - 11.9841604417936 + -22.902929804587398, + -23.180877284771793, + -19.584025329151217, + -20.01956198271935 ], - "timestamp": 5.182435392035175 + "timestamp": 5.239582089356939 }, { - "x": 2.830552574824699, - "y": 7.012504794671367, - "heading": -0.026692143130144577, - "angularVelocity": -0.018345380059196448, - "velocityX": 0.6383175162541149, - "velocityY": -0.26912851727456555, + "x": 2.853604993354379, + "y": 6.996545885023089, + "heading": 0.42550601388588527, + "angularVelocity": 0.03272819499604782, + "velocityX": 0.5398976115880579, + "velocityY": 0.08024448189192064, "moduleForcesX": [ - -66.1245923787409, - -65.98169477222959, - -66.56070394354207, - -66.43347254576503 + -68.01213832568928, + -67.89244829561473, + -68.61715921102547, + -68.49693767599965 ], "moduleForcesY": [ - 22.81538982238073, - 23.221752664345384, - 21.508695963392192, - 21.894565620883082 + -16.33804890659345, + -16.838898924869497, + -13.577428279677779, + -14.183521442505858 ], - "timestamp": 5.274133334416134 + "timestamp": 5.311851881951698 }, { - "x": 2.8596270084381104, - "y": 6.999704360961915, - "heading": -0.027461342770395154, - "angularVelocity": -0.008388406765443529, - "velocityX": 0.31706745919666984, - "velocityY": -0.1395934675707037, + "x": 2.8731672763824463, + "y": 6.999098300933838, + "heading": 0.4266275995625518, + "angularVelocity": 0.015519425701905927, + "velocityX": 0.27068408979336367, + "velocityY": 0.035317880667813904, "moduleForcesX": [ - -64.76590607807921, - -64.6158756095251, - -65.15697508947473, - -65.01741451245994 + -68.85222618332939, + -68.74728425154547, + -69.24154633684182, + -69.1454169059897 ], "moduleForcesY": [ - 26.47522061163077, - 26.837605390916714, - 25.49694542258738, - 25.848954107534034 + -12.403847845099842, + -12.982110998649153, + -10.008033607336229, + -10.662885240301636 ], - "timestamp": 5.365831276797093 + "timestamp": 5.384121674546457 }, { - "x": 2.8596270084381104, - "y": 6.999704360961914, - "heading": -0.02746134277039515, - "angularVelocity": 1.6056171256353062e-18, - "velocityX": -1.8304903305133357e-17, - "velocityY": -4.784339232160718e-18, + "x": 2.8731672763824463, + "y": 6.999098300933838, + "heading": 0.4266275995625518, + "angularVelocity": 5.733280321426045e-22, + "velocityX": 4.854312894103748e-21, + "velocityY": -1.3121681595321098e-20, "moduleForcesX": [ - -63.93945199318899, - -63.793171888386496, - -64.29137064962106, - -64.1528183330455 + -69.27866045130386, + -69.19177764514303, + -69.54932705970307, + -69.47427349656373 ], "moduleForcesY": [ - 28.437823121296837, - 28.763444348886257, - 27.63256619914302, - 27.951659076114336 + -9.808624171483924, + -10.412359295598337, + -7.660954895057263, + -8.324479793607422 ], - "timestamp": 5.457529219178052 + "timestamp": 5.456391467141216 } ], "eventMarkers": [] From ce429eb4432b0cb8801ae4307d65a41046e5b04c Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 2 Nov 2024 16:18:48 -0700 Subject: [PATCH 18/21] TURNED OFF AUTO SIGNAL LOGGING + trace autoperiodic (#91) --- src/main/java/frc/robot/Robot.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 99f3b3e0..7f9eaada 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -141,6 +141,7 @@ public void setYaw(Rotation2d yaw) {} @Override public void robotInit() { + SignalLogger.enableAutoLogging(false); RoboRioDataJNI.setBrownoutVoltage(6.0); // Metadata about the current code running on the robot Logger.recordMetadata("Codebase", "Comp2024"); @@ -1042,4 +1043,9 @@ public void testInit() { private static double teleopAxisAdjustment(double x) { return MathUtil.applyDeadband(Math.abs(Math.pow(x, 2)) * Math.signum(x), 0.02); } + + @Override + public void autonomousPeriodic() { + Tracer.traceFunc("Auto Periodic", super::autonomousPeriodic); + } } From 3f42b2bc06d9fd839e85a4f0839254a39971573a Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 2 Nov 2024 16:21:44 -0700 Subject: [PATCH 19/21] Add tracer (#85) * responded to feedback * added credit --- src/main/java/frc/robot/Robot.java | 68 +++++------- .../swerve/PhoenixOdometryThread.java | 78 +++++++------- .../subsystems/swerve/SwerveSubsystem.java | 102 ++++++++++-------- src/main/java/frc/robot/utils/Tracer.java | 55 ++++------ 4 files changed, 146 insertions(+), 157 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7f9eaada..a18cba94 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -473,39 +473,38 @@ public void robotInit() { @Override public void robotPeriodic() { - Tracer.startTrace("RobotPeriodic"); - Tracer.traceFunc("CommandScheduler", CommandScheduler.getInstance()::run); - // Update ascope mechanism visualization - Logger.recordOutput( - "Mechanism Poses", - new Pose3d[] { - shooter.getMechanismPose(), elevator.getCarriagePose(), elevator.getFirstStagePose() + Tracer.trace( + "RobotPeriodic", + () -> { + Tracer.trace("CommandScheduler", CommandScheduler.getInstance()::run); + // Update ascope mechanism visualization + Logger.recordOutput( + "Mechanism Poses", + new Pose3d[] { + shooter.getMechanismPose(), elevator.getCarriagePose(), elevator.getFirstStagePose() + }); + Logger.recordOutput("Target", currentTarget); + Logger.recordOutput("AutoAim/Speaker", FieldConstants.getSpeaker()); + // Logger.recordOutput("Canivore Util", CANBus.getStatus("canivore").BusUtilization); + Logger.recordOutput( + "Angle to target", + Math.atan2( + FieldConstants.getSpeaker().getY() - swerve.getPose().getY(), + FieldConstants.getSpeaker().getX() - swerve.getPose().getX())); + Logger.recordOutput( + "AutoAim/Actual Distance", + swerve.getPose().minus(FieldConstants.getSpeaker()).getTranslation().getNorm()); }); - Logger.recordOutput("Target", currentTarget); - Logger.recordOutput("AutoAim/Speaker", FieldConstants.getSpeaker()); - // Logger.recordOutput("Canivore Util", CANBus.getStatus("canivore").BusUtilization); - Logger.recordOutput( - "Angle to target", - Math.atan2( - FieldConstants.getSpeaker().getY() - swerve.getPose().getY(), - FieldConstants.getSpeaker().getX() - swerve.getPose().getX())); - Logger.recordOutput( - "AutoAim/Actual Distance", - swerve.getPose().minus(FieldConstants.getSpeaker()).getTranslation().getNorm()); - - Tracer.endTrace(); } @Override public void loopFunc() { - Tracer.startTrace("Robot"); - Tracer.traceFunc("LoopFunc", super::loopFunc); - Tracer.endTrace(); + Tracer.trace("Robot/LoopFunc", super::loopFunc); } private void setUpLogging() { HashMap commandCounts = new HashMap<>(); - BiConsumer logCommandFunction = + final BiConsumer logCommandFunction = (Command command, Boolean active) -> { String name = command.getName(); int count = commandCounts.getOrDefault(name, 0) + (active ? 1 : -1); @@ -515,21 +514,12 @@ private void setUpLogging() { active.booleanValue()); Logger.recordOutput("Commands/CommandsAll/" + name, count > 0); }; - CommandScheduler.getInstance() - .onCommandInitialize( - (Command command) -> { - logCommandFunction.accept(command, true); - }); - CommandScheduler.getInstance() - .onCommandFinish( - (Command command) -> { - logCommandFunction.accept(command, false); - }); - CommandScheduler.getInstance() - .onCommandInterrupt( - (Command command) -> { - logCommandFunction.accept(command, false); - }); + + var scheduler = CommandScheduler.getInstance(); + + scheduler.onCommandInitialize(c -> logCommandFunction.accept(c, true)); + scheduler.onCommandFinish(c -> logCommandFunction.accept(c, false)); + scheduler.onCommandInterrupt(c -> logCommandFunction.accept(c, false)); } private Command shootWithDashboard() { diff --git a/src/main/java/frc/robot/subsystems/swerve/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/swerve/PhoenixOdometryThread.java index 880becce..7f369c19 100644 --- a/src/main/java/frc/robot/subsystems/swerve/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/swerve/PhoenixOdometryThread.java @@ -134,21 +134,23 @@ public void registerSignals(Registration... registrations) { } public List samplesSince(double timestamp) { - Tracer.startTrace("samples since"); - var readLock = journalLock.readLock(); - try { - readLock.lock(); - - return Tracer.traceFunc( - "stream timestamps", - () -> - journal.stream() - .filter(s -> s.timestamp > timestamp) - .collect(Collectors.toUnmodifiableList())); - } finally { - readLock.unlock(); - Tracer.endTrace(); - } + return Tracer.trace( + "samples since", + () -> { + var readLock = journalLock.readLock(); + try { + readLock.lock(); + + return Tracer.trace( + "stream timestamps", + () -> + journal.stream() + .filter(s -> s.timestamp > timestamp) + .collect(Collectors.toUnmodifiableList())); + } finally { + readLock.unlock(); + } + }); } @Override @@ -156,28 +158,30 @@ public void run() { while (true) { // Wait for updates from all signals var writeLock = journalLock.writeLock(); - Tracer.startTrace("Odometry Thread"); - Tracer.traceFunc( - "wait for all", - () -> BaseStatusSignal.waitForAll(2.0 / Module.ODOMETRY_FREQUENCY_HZ, signalArr)); - try { - writeLock.lock(); - var filteredSignals = - registeredSignals.stream() - .filter(s -> s.signal().getStatus().equals(StatusCode.OK)) - .collect(Collectors.toSet()); - journal.add( - new Samples( - timestampFor(filteredSignals), - filteredSignals.stream() - .collect( - Collectors.toUnmodifiableMap( - s -> new SignalID(s.type(), s.modID()), - s -> s.signal().getValueAsDouble())))); - } finally { - writeLock.unlock(); - } - Tracer.endTrace(); + Tracer.trace( + "Odometry Thread", + () -> { + Tracer.trace( + "wait for all", + () -> BaseStatusSignal.waitForAll(2.0 / Module.ODOMETRY_FREQUENCY_HZ, signalArr)); + try { + writeLock.lock(); + var filteredSignals = + registeredSignals.stream() + .filter(s -> s.signal().getStatus().equals(StatusCode.OK)) + .collect(Collectors.toSet()); + journal.add( + new Samples( + timestampFor(filteredSignals), + filteredSignals.stream() + .collect( + Collectors.toUnmodifiableMap( + s -> new SignalID(s.type(), s.modID()), + s -> s.signal().getValueAsDouble())))); + } finally { + writeLock.unlock(); + } + }); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 936abef7..61bdbe18 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -373,53 +373,59 @@ public static VisionIO[] createSimCameras() { } public void periodic() { - Tracer.startTrace("SwervePeriodic"); - for (var camera : cameras) { - Tracer.traceFunc("Update cam inputs", camera::updateInputs); - Tracer.traceFunc("Process cam inputs", camera::processInputs); - } - Tracer.traceFunc( - "Update odo inputs", - () -> odoThread.updateInputs(odoThreadInputs, lastOdometryUpdateTimestamp)); - Logger.processInputs("Async Odo", odoThreadInputs); - if (!odoThreadInputs.sampledStates.isEmpty()) { - lastOdometryUpdateTimestamp = - odoThreadInputs.sampledStates.get(odoThreadInputs.sampledStates.size() - 1).timestamp(); - } - Tracer.traceFunc("update gyro inputs", () -> gyroIO.updateInputs(gyroInputs)); - for (int i = 0; i < modules.length; i++) { - Tracer.traceFunc( - "SwerveModule update inputs from " + modules[i].getPrefix() + " Module", - modules[i]::updateInputs); - } - Logger.processInputs("Swerve/Gyro", gyroInputs); - for (int i = 0; i < modules.length; i++) { - Tracer.traceFunc( - "SwerveModule periodic from " + modules[i].getPrefix() + " Module", modules[i]::periodic); - } - - // Stop moving when disabled - if (DriverStation.isDisabled()) { - for (var module : modules) { - module.stop(); - } - } - // Log empty setpoint states when disabled - if (DriverStation.isDisabled()) { - Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); - Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); - } + Tracer.trace( + "SwervePeriodic", + () -> { + for (var camera : cameras) { + Tracer.trace("Update cam inputs", camera::updateInputs); + Tracer.trace("Process cam inputs", camera::processInputs); + } + Tracer.trace( + "Update odo inputs", + () -> odoThread.updateInputs(odoThreadInputs, lastOdometryUpdateTimestamp)); + Logger.processInputs("Async Odo", odoThreadInputs); + if (!odoThreadInputs.sampledStates.isEmpty()) { + lastOdometryUpdateTimestamp = + odoThreadInputs + .sampledStates + .get(odoThreadInputs.sampledStates.size() - 1) + .timestamp(); + } + Tracer.trace("update gyro inputs", () -> gyroIO.updateInputs(gyroInputs)); + for (int i = 0; i < modules.length; i++) { + Tracer.trace( + "SwerveModule update inputs from " + modules[i].getPrefix() + " Module", + modules[i]::updateInputs); + } + Logger.processInputs("Swerve/Gyro", gyroInputs); + for (int i = 0; i < modules.length; i++) { + Tracer.trace( + "SwerveModule periodic from " + modules[i].getPrefix() + " Module", + modules[i]::periodic); + } - Logger.recordOutput("ShotData/Angle", AutoAimStates.curShotData.getRotation()); - Logger.recordOutput("ShotData/Left RPM", AutoAimStates.curShotData.getLeftRPS()); - Logger.recordOutput("ShotData/Right RPM", AutoAimStates.curShotData.getRightRPS()); - Logger.recordOutput("ShotData/Flight Time", AutoAimStates.curShotData.getFlightTimeSeconds()); - Logger.recordOutput("ShotData/Lookahead", AutoAimStates.lookaheadTime); + // Stop moving when disabled + if (DriverStation.isDisabled()) { + for (var module : modules) { + module.stop(); + } + } + // Log empty setpoint states when disabled + if (DriverStation.isDisabled()) { + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); + } - Tracer.traceFunc("Update odometry", () -> updateOdometry()); - Tracer.traceFunc("update vision", () -> updateVision()); + Logger.recordOutput("ShotData/Angle", AutoAimStates.curShotData.getRotation()); + Logger.recordOutput("ShotData/Left RPM", AutoAimStates.curShotData.getLeftRPS()); + Logger.recordOutput("ShotData/Right RPM", AutoAimStates.curShotData.getRightRPS()); + Logger.recordOutput( + "ShotData/Flight Time", AutoAimStates.curShotData.getFlightTimeSeconds()); + Logger.recordOutput("ShotData/Lookahead", AutoAimStates.lookaheadTime); - Tracer.endTrace(); + Tracer.trace("Update odometry", this::updateOdometry); + Tracer.trace("update vision", this::updateVision); + }); } private void updateOdometry() { @@ -427,7 +433,6 @@ private void updateOdometry() { var sampleStates = odoThreadInputs.sampledStates; var i = 0; for (var sample : sampleStates) { - Tracer.startTrace("Sample " + i); i++; // Read wheel deltas from each module SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; @@ -476,7 +481,11 @@ private void updateOdometry() { Rotation2d.fromDegrees(sample.values().get(new SignalID(SignalType.GYRO, -1))); lastGyroRotation = rawGyroRotation; Logger.recordOutput("Odometry/Gyro Rotation", lastGyroRotation); - estimator.updateWithTime(sample.timestamp(), rawGyroRotation, lastModulePositions); + Tracer.trace( + "update estimator", + () -> + estimator.updateWithTime( + sample.timestamp(), rawGyroRotation, lastModulePositions)); } continue; } @@ -506,7 +515,6 @@ private void updateOdometry() { Logger.recordOutput("Odometry/Gyro Rotation", lastGyroRotation); // Apply update estimator.updateWithTime(sample.timestamp(), rawGyroRotation, modulePositions); - Tracer.endTrace(); } } diff --git a/src/main/java/frc/robot/utils/Tracer.java b/src/main/java/frc/robot/utils/Tracer.java index 784f4689..5dfe9fae 100644 --- a/src/main/java/frc/robot/utils/Tracer.java +++ b/src/main/java/frc/robot/utils/Tracer.java @@ -9,8 +9,11 @@ import org.littletonrobotics.junction.Logger; /** - * A Utility class for tracing code execution time. Will put info to NetworkTables under the - * "Tracer" table. + * + * Thanks to oh-yes-0-fps and FRC 3173! + *

+ * + * A Utility class for tracing code execution time. Will put info to Advantagekit under "Tracer". * *


  *
@@ -25,7 +28,6 @@
  * public void robotPeriodic() {
  *     Tracer.startTrace("RobotPeriodic");
  *     Tracer.traceFunc("CommandScheduler", scheduler::run);
- *     Tracer.traceFunc("Monologue", Monologue::updateAll);
  *     Tracer.endTrace();
  * }
  *
@@ -156,27 +158,6 @@ private static void endTrace(final TracerState state) {
     }
   }
 
-  /**
-   * Starts a trace, should be called at the beginning of a function thats not being called by user
-   * code. Should be paired with {@link Tracer#endTrace()} at the end of the function.
-   *
-   * 

Best used in periodic functions in Subsystems and Robot.java. - * - * @param name the name of the trace, should be unique to the function. - */ - public static void startTrace(String name) { - startTrace(name, threadLocalState.get()); - } - - /** - * Ends a trace, should only be called at the end of a function thats not being called by user - * code. If a {@link Tracer#startTrace(String)} is not paired with a {@link Tracer#endTrace()} - * there could be a crash. - */ - public static void endTrace() { - endTrace(threadLocalState.get()); - } - /** * Traces a function, should be used in place of {@link Tracer#startTrace(String)} and {@link * Tracer#endTrace()} for functions called by user code like {@code CommandScheduler.run()} and @@ -184,13 +165,16 @@ public static void endTrace() { * * @param name the name of the trace, should be unique to the function. * @param runnable the function to trace. - * @apiNote If you want to return a value then use {@link Tracer#traceFunc(String, Supplier)}. + * @apiNote If you want to return a value then use {@link Tracer#trace(String, Supplier)}. */ - public static void traceFunc(String name, Runnable runnable) { + public static void trace(String name, Runnable runnable) { final TracerState state = threadLocalState.get(); - startTrace(name, state); - runnable.run(); - endTrace(state); + try { + startTrace(name, state); + runnable.run(); + } finally { + endTrace(state); + } } /** @@ -201,11 +185,14 @@ public static void traceFunc(String name, Runnable runnable) { * @param name the name of the trace, should be unique to the function. * @param supplier the function to trace. */ - public static T traceFunc(String name, Supplier supplier) { + public static T trace(String name, Supplier supplier) { final TracerState state = threadLocalState.get(); - startTrace(name, state); - T ret = supplier.get(); - endTrace(state); - return ret; + try { + startTrace(name, state); + T ret = supplier.get(); + return ret; + } finally { + endTrace(state); + } } } From cb18dc66eedeabf209fe51db5cca50c599e88e60 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 2 Nov 2024 23:22:54 +0000 Subject: [PATCH 20/21] run spotless --- src/main/java/frc/robot/utils/Tracer.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/utils/Tracer.java b/src/main/java/frc/robot/utils/Tracer.java index 5dfe9fae..15d2832a 100644 --- a/src/main/java/frc/robot/utils/Tracer.java +++ b/src/main/java/frc/robot/utils/Tracer.java @@ -9,11 +9,11 @@ import org.littletonrobotics.junction.Logger; /** - * - * Thanks to oh-yes-0-fps and FRC 3173! - *

- * - * A Utility class for tracing code execution time. Will put info to Advantagekit under "Tracer". + * Thanks to oh-yes-0-fps and FRC + * 3173! + * + *

A Utility class for tracing code execution time. Will put info to Advantagekit under "Tracer". * *


  *

From 267d65ed33f3f6f4698fc372e9e3f86b54b28eb5 Mon Sep 17 00:00:00 2001
From: Lewis-Seiden 
Date: Sat, 2 Nov 2024 16:53:48 -0700
Subject: [PATCH 21/21] update Tracer.java doc comment, fix autonomousPeriodic
 tracer logging

---
 src/main/java/frc/robot/Robot.java        |  2 +-
 src/main/java/frc/robot/utils/Tracer.java | 18 ++++++++----------
 2 files changed, 9 insertions(+), 11 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index a18cba94..614e401e 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -1036,6 +1036,6 @@ private static double teleopAxisAdjustment(double x) {
 
   @Override
   public void autonomousPeriodic() {
-    Tracer.traceFunc("Auto Periodic", super::autonomousPeriodic);
+    Tracer.trace("Auto Periodic", super::autonomousPeriodic);
   }
 }
diff --git a/src/main/java/frc/robot/utils/Tracer.java b/src/main/java/frc/robot/utils/Tracer.java
index 15d2832a..f6def850 100644
--- a/src/main/java/frc/robot/utils/Tracer.java
+++ b/src/main/java/frc/robot/utils/Tracer.java
@@ -13,22 +13,24 @@
  * href="https://github.com/McQuaidRobotics/2024_Crescendo/blob/main/src/main/java/com/igknighters/util/Tracer.java">FRC
  * 3173!
  *
+ * 

A newer version of this class is being merged into WPILib for 2025, and should supersede this + * file once merged. The WPILib version may not support logging to akit the way we want, revisit + * then (might need to manually connect nt publishing to log) + * *

A Utility class for tracing code execution time. Will put info to Advantagekit under "Tracer". * *


  *
  * @Override
  * public void loopFunc() {
- *    Tracer.traceFunc("LoopFunc", super::loopFunc);
+ *    Tracer.trace("LoopFunc", super::loopFunc);
  * }
  *
  * 

* * @Override * public void robotPeriodic() { - * Tracer.startTrace("RobotPeriodic"); - * Tracer.traceFunc("CommandScheduler", scheduler::run); - * Tracer.endTrace(); + * Tracer.trace("CommandScheduler", scheduler::run); * } * *

@@ -159,9 +161,7 @@ private static void endTrace(final TracerState state) { } /** - * Traces a function, should be used in place of {@link Tracer#startTrace(String)} and {@link - * Tracer#endTrace()} for functions called by user code like {@code CommandScheduler.run()} and - * other expensive functions. + * Traces a function. * * @param name the name of the trace, should be unique to the function. * @param runnable the function to trace. @@ -178,9 +178,7 @@ public static void trace(String name, Runnable runnable) { } /** - * Traces a function, should be used in place of {@link Tracer#startTrace(String)} and {@link - * Tracer#endTrace()} for functions called by user code like {@code CommandScheduler.run()} and - * other expensive functions. + * Traces a function. * * @param name the name of the trace, should be unique to the function. * @param supplier the function to trace.