From da06f26953da3cae8ab632fbda94e3c070a92f16 Mon Sep 17 00:00:00 2001 From: Huck-Richardson Date: Tue, 25 Mar 2025 21:03:01 -0400 Subject: [PATCH 01/12] Started barge align. Finished finding servo target, starting work on moving. Still need to add joystick reading. --- .../robot/constants/BargeAlignConstants.java | 10 +++ .../robot/subsystems/vision/BargeAlign.java | 78 +++++++++++++++++++ 2 files changed, 88 insertions(+) create mode 100644 src/main/java/frc/robot/constants/BargeAlignConstants.java create mode 100644 src/main/java/frc/robot/subsystems/vision/BargeAlign.java diff --git a/src/main/java/frc/robot/constants/BargeAlignConstants.java b/src/main/java/frc/robot/constants/BargeAlignConstants.java new file mode 100644 index 00000000..9753ae44 --- /dev/null +++ b/src/main/java/frc/robot/constants/BargeAlignConstants.java @@ -0,0 +1,10 @@ +package frc.robot.constants; + +import edu.wpi.first.math.geometry.Translation2d; + +public class BargeAlignConstants { + + public static final Translation2d redBargePos = new Translation2d(); //TODO Sorry future contributor but these are empty + public static final Translation2d blueBargePos = new Translation2d(); + +} diff --git a/src/main/java/frc/robot/subsystems/vision/BargeAlign.java b/src/main/java/frc/robot/subsystems/vision/BargeAlign.java new file mode 100644 index 00000000..d567826a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/BargeAlign.java @@ -0,0 +1,78 @@ +package frc.robot.subsystems.vision; + +import java.time.OffsetDateTime; +import java.util.Set; + +import org.strykeforce.telemetry.measurable.MeasurableSubsystem; +import org.strykeforce.telemetry.measurable.Measure; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import frc.robot.constants.BargeAlignConstants; +import java.util.function.DoubleSupplier; +import frc.robot.subsystems.drive.DriveSubsystem; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +public class BargeAlign extends MeasurableSubsystem{ +private DoubleSupplier strStick; +private DriveSubsystem driveSubsystem; +//These are here to determine offsets for target positions. +private boolean blueAlliance; +private boolean redBare; +private Alliance alliance; +private double driveRadius = 1.5; //TODO This is a magic number that needs creating +private BargeAlignStates curState; + + public BargeAlign( + DoubleSupplier strStick, + DriveSubsystem driveSubsystem){ + if(alliance == Alliance.Blue){ + blueAlliance = true; + }else{ + blueAlliance = false; + } + } +public void setState(BargeAlignStates curState){ + this.curState = curState; +} +private boolean getOnBlueSide(){ + return + driveSubsystem.getPoseMeters().getMeasureX() + .compareTo(BargeAlignConstants.blueBargePos.getMeasureX()) <= 0; +} + +private Translation2d getTargetTranslation(){ + + Translation2d offset = new Translation2d( + getOnBlueSide() ? driveRadius : driveRadius * -1 , new Rotation2d( + getOnBlueSide() ? 0 : Math.PI)); + + Translation2d targetBarge = + blueAlliance ? BargeAlignConstants.blueBargePos : BargeAlignConstants.redBargePos; + + Translation2d targetPos = targetBarge.minus(offset); + return targetPos; +} + +@Override +public void periodic(){ + switch(curState){ + case DRIVE: + + break; + case DONE: + break; + } +} + +@Override +public Set getMeasures(){ + return Set.of(new Measure("BargeAlign/curState", () -> curState.ordinal())); +} + +public enum BargeAlignStates { + DRIVE, + DONE +} + +} From b62bdac51e35dcd5add4b70464dd685c9f486f61 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Wed, 26 Mar 2025 21:09:09 -0400 Subject: [PATCH 02/12] tuned max accel also added some ability to tell HP when to drop --- src/main/deploy/choreo/FiveMeterTestPath.traj | 163 ++++-------------- src/main/java/frc/robot/RobotContainer.java | 7 +- .../commands/auton/TestAutonCommand.java | 15 +- .../commands/drive/DriveAutonCommand.java | 2 +- .../frc/robot/constants/DriveConstants.java | 6 +- .../robot/constants/ElevatorConstants.java | 5 +- .../frc/robot/constants/LEDConstants.java | 3 + .../frc/robot/subsystems/drive/Swerve.java | 10 ++ .../frc/robot/subsystems/drive/SwerveFXS.java | 11 ++ .../frc/robot/subsystems/drive/SwerveIO.java | 1 + .../robot/subsystems/led/LEDSubsystem.java | 13 ++ .../subsystems/pathHandler/PathHandler.java | 3 + .../robotState/RobotStateSubsystem.java | 4 + 13 files changed, 96 insertions(+), 147 deletions(-) diff --git a/src/main/deploy/choreo/FiveMeterTestPath.traj b/src/main/deploy/choreo/FiveMeterTestPath.traj index 8ab66ea4..4b32a4a4 100644 --- a/src/main/deploy/choreo/FiveMeterTestPath.traj +++ b/src/main/deploy/choreo/FiveMeterTestPath.traj @@ -3,154 +3,55 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":0.0, "y":0.0, "heading":0.0, "intervals":120, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.0, "y":0.0, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":5.0, "y":0.0, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, - {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":true}], - "targetDt":0.05 + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":3.7}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":16.0}}, "enabled":true}], + "targetDt":0.08 }, "params":{ "waypoints":[ - {"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":120, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"5 m", "val":5.0}, "y":{"exp":"0 m", "val":0.0}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, - {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":true}], + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.7 m / s", "val":3.7}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"16 m / s ^ 2", "val":16.0}}}, "enabled":true}], "targetDt":{ - "exp":"0.05 s", - "val":0.05 + "exp":"0.08 s", + "val":0.08 } }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,6.00122], + "waypoints":[0.0,1.66277], "samples":[ - {"t":0.0, "x":0.0, "y":0.0, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.99959, "ay":0.0, "alpha":0.00745, "fx":[16.77217,16.43266,16.72304,16.72304], "fy":[-0.02404,0.02404,0.02404,-0.02404]}, - {"t":0.05001, "x":0.00125, "y":0.0, "heading":0.0, "vx":0.04999, "vy":0.0, "omega":0.00037, "ax":0.99979, "ay":0.0, "alpha":0.00019, "fx":[16.73708,16.54679,16.68995,16.68995], "fy":[-0.02306,0.02306,0.02306,-0.02306]}, - {"t":0.10002, "x":0.005, "y":0.0, "heading":0.00002, "vx":0.09999, "vy":0.0, "omega":0.00038, "ax":0.99977, "ay":0.0, "alpha":-0.00911, "fx":[16.68767,16.68767,16.64381,16.64381], "fy":[-0.02246,0.02247,0.02246,-0.02246]}, - {"t":0.15003, "x":0.01125, "y":0.0, "heading":0.00004, "vx":0.14999, "vy":0.0, "omega":-0.00007, "ax":0.99976, "ay":0.0, "alpha":0.00185, "fx":[16.74202,16.52663,16.6967,16.6967], "fy":[-0.02217,0.02217,0.02217,-0.02217]}, - {"t":0.20004, "x":0.02, "y":0.0, "heading":0.00003, "vx":0.19999, "vy":0.0, "omega":0.00002, "ax":0.99974, "ay":0.0, "alpha":-0.00885, "fx":[16.68655,16.68655,16.64397,16.64397], "fy":[-0.02181,0.02181,0.02181,-0.02181]}, - {"t":0.25005, "x":0.03125, "y":0.0, "heading":0.00004, "vx":0.24998, "vy":0.0, "omega":-0.00042, "ax":0.99973, "ay":0.0, "alpha":0.00355, "fx":[16.74892,16.50262,16.70417,16.70417], "fy":[-0.02189,0.02189,0.02189,-0.02189]}, - {"t":0.30006, "x":0.04501, "y":0.0, "heading":0.00001, "vx":0.29998, "vy":0.0, "omega":-0.00025, "ax":0.99971, "ay":0.0, "alpha":-0.0088, "fx":[16.6858,16.6858,16.64347,16.64347], "fy":[-0.02168,0.02168,0.02168,-0.02168]}, - {"t":0.35007, "x":0.06126, "y":0.0, "heading":0.0, "vx":0.34998, "vy":0.0, "omega":-0.00069, "ax":0.99968, "ay":0.0, "alpha":0.00549, "fx":[16.75816,16.47274,16.71305,16.71305], "fy":[-0.02207,0.02207,0.02207,-0.02207]}, - {"t":0.40008, "x":0.08001, "y":0.0, "heading":-0.00003, "vx":0.39997, "vy":0.0, "omega":-0.00041, "ax":0.99966, "ay":0.0, "alpha":-0.00889, "fx":[16.68519,16.68519,16.64241,16.64241], "fy":[-0.0219,0.02191,0.0219,-0.0219]}, - {"t":0.45009, "x":0.10126, "y":0.0, "heading":-0.00005, "vx":0.44996, "vy":0.0, "omega":-0.00086, "ax":0.99962, "ay":0.0, "alpha":0.00796, "fx":[16.77062,16.43332,16.72456,16.72456], "fy":[-0.02254,0.02254,0.02254,-0.02254]}, - {"t":0.5001, "x":0.12502, "y":0.0, "heading":-0.0001, "vx":0.49995, "vy":0.0, "omega":-0.00046, "ax":0.99959, "ay":0.0, "alpha":-0.00904, "fx":[16.68437,16.68437,16.64087,16.64087], "fy":[-0.02227,0.02228,0.02227,-0.02227]}, - {"t":0.55011, "x":0.15127, "y":0.0, "heading":-0.00012, "vx":0.54994, "vy":0.0, "omega":-0.00091, "ax":0.99954, "ay":0.0, "alpha":0.01125, "fx":[16.7868,16.38118,16.73965,16.73965], "fy":[-0.02307,0.02307,0.02307,-0.02307]}, - {"t":0.60012, "x":0.18002, "y":0.0, "heading":-0.00016, "vx":0.59993, "vy":0.0, "omega":-0.00035, "ax":0.99948, "ay":0.0, "alpha":-0.00914, "fx":[16.68282,16.68282,16.63883,16.63883], "fy":[-0.02252,0.02253,0.02251,-0.02252]}, - {"t":0.65013, "x":0.21127, "y":0.0, "heading":-0.00018, "vx":0.64991, "vy":0.0, "omega":-0.0008, "ax":0.9994, "ay":0.0, "alpha":0.01574, "fx":[16.80731,16.31171,16.75954,16.75954], "fy":[-0.02338,0.02338,0.02338,-0.02338]}, - {"t":0.70014, "x":0.24503, "y":0.0, "heading":-0.00022, "vx":0.6999, "vy":0.0, "omega":-0.00002, "ax":0.9993, "ay":0.0, "alpha":-0.00903, "fx":[16.67951,16.67951,16.63602,16.63602], "fy":[-0.02226,0.02227,0.02225,-0.02226]}, - {"t":0.75015, "x":0.28128, "y":0.0, "heading":-0.00022, "vx":0.74987, "vy":0.0, "omega":-0.00047, "ax":0.99914, "ay":0.0, "alpha":0.0219, "fx":[16.83194,16.21926,16.78492,16.78492], "fy":[-0.02301,0.02301,0.02301,-0.02301]}, - {"t":0.80016, "x":0.32003, "y":0.0, "heading":-0.00025, "vx":0.79984, "vy":0.0, "omega":0.00063, "ax":0.99891, "ay":0.0, "alpha":-0.00849, "fx":[16.67184,16.67184,16.63097,16.63097], "fy":[-0.02091,0.02092,0.0209,-0.02091]}, - {"t":0.85017, "x":0.36128, "y":0.0, "heading":-0.00022, "vx":0.84979, "vy":0.0, "omega":0.0002, "ax":0.99851, "ay":0.0, "alpha":0.0287, "fx":[16.84907,16.1183,16.80567,16.80567], "fy":[-0.02124,0.02124,0.02124,-0.02124]}, - {"t":0.90018, "x":0.40502, "y":0.0, "heading":-0.00021, "vx":0.89973, "vy":0.0, "omega":0.00164, "ax":0.99763, "ay":0.0, "alpha":-0.0072, "fx":[16.64737,16.64737,16.61271,16.61272], "fy":[-0.01773,0.01774,0.01771,-0.01773]}, - {"t":0.95019, "x":0.45127, "y":0.0, "heading":-0.00012, "vx":0.94962, "vy":0.0, "omega":0.00128, "ax":0.99423, "ay":0.0, "alpha":0.02853, "fx":[16.7637,16.0711,16.72914,16.72914], "fy":[-0.01692,0.01693,0.01693,-0.01692]}, - {"t":1.0002, "x":0.5, "y":0.0, "heading":-0.00006, "vx":0.99934, "vy":0.0, "omega":0.0027, "ax":0.00915, "ay":0.0, "alpha":-0.00505, "fx":[0.16466,0.16466,0.14036,0.14036], "fy":[-0.01247,0.01241,0.01237,-0.01247]}, - {"t":1.05021, "x":0.54999, "y":0.0, "heading":0.00008, "vx":0.9998, "vy":0.0, "omega":0.00245, "ax":0.0, "ay":0.0, "alpha":-0.00909, "fx":[-0.01709,0.08766,-0.03538,-0.03538], "fy":[-0.00888,0.00903,0.00903,-0.00888]}, - {"t":1.10022, "x":0.59999, "y":0.0, "heading":0.0002, "vx":0.9998, "vy":0.0, "omega":0.002, "ax":0.0, "ay":0.0, "alpha":-0.00279, "fx":[0.00672,0.00672,-0.00671,-0.00672], "fy":[-0.00694,0.00681,0.00679,-0.00694]}, - {"t":1.15023, "x":0.64999, "y":0.0, "heading":0.0003, "vx":0.9998, "vy":0.0, "omega":0.00186, "ax":0.0, "ay":0.0, "alpha":-0.02098, "fx":[-0.09134,0.28866,-0.09866,-0.09866], "fy":[-0.00351,0.00364,0.00364,-0.00351]}, - {"t":1.20024, "x":0.69999, "y":0.0, "heading":0.00039, "vx":0.9998, "vy":0.0, "omega":0.00081, "ax":0.0, "ay":0.0, "alpha":-0.00132, "fx":[0.00317,0.00318,-0.00317,-0.00318], "fy":[-0.00332,0.00319,0.00318,-0.00332]}, - {"t":1.25025, "x":0.74999, "y":0.0, "heading":0.00043, "vx":0.9998, "vy":0.0, "omega":0.00074, "ax":0.0, "ay":0.0, "alpha":-0.0107, "fx":[-0.04727,0.14836,-0.05054,-0.05055], "fy":[-0.00154,0.00166,0.00166,-0.00154]}, - {"t":1.30026, "x":0.79999, "y":0.0, "heading":0.00047, "vx":0.9998, "vy":0.0, "omega":0.00021, "ax":0.0, "ay":0.0, "alpha":-0.0006, "fx":[0.00144,0.00144,-0.00144,-0.00144], "fy":[-0.00154,0.00143,0.00142,-0.00154]}, - {"t":1.35027, "x":0.84999, "y":0.0, "heading":0.00048, "vx":0.9998, "vy":0.0, "omega":0.00018, "ax":0.0, "ay":0.0, "alpha":-0.00413, "fx":[-0.01783,0.05655,-0.01936,-0.01936], "fy":[-0.00069,0.0008,0.0008,-0.00069]}, - {"t":1.40028, "x":0.89999, "y":0.0, "heading":0.00049, "vx":0.9998, "vy":0.0, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00025, "fx":[0.0006,0.0006,-0.0006,-0.0006], "fy":[-0.00068,0.00058,0.00057,-0.00068]}, - {"t":1.45029, "x":0.94999, "y":0.0, "heading":0.00049, "vx":0.9998, "vy":0.0, "omega":-0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00149, "fx":[-0.00639,0.02032,-0.00696,-0.00697], "fy":[-0.00022,0.00033,0.00033,-0.00022]}, - {"t":1.5003, "x":0.99999, "y":0.0, "heading":0.00048, "vx":0.9998, "vy":0.0, "omega":-0.00012, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[0.00016,0.00016,-0.00016,-0.00016], "fy":[-0.00022,0.00012,0.00012,-0.00022]}, - {"t":1.55032, "x":1.04999, "y":0.0, "heading":0.00048, "vx":0.9998, "vy":0.0, "omega":-0.00012, "ax":0.0, "ay":0.0, "alpha":-0.00054, "fx":[-0.00266,0.00794,-0.00264,-0.00264], "fy":[0.00006,0.00003,0.00004,0.00006]}, - {"t":1.60033, "x":1.09999, "y":0.0, "heading":0.00047, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":0.00004, "fx":[-0.0001,-0.0001,0.0001,0.0001], "fy":[0.00004,-0.00013,-0.00014,0.00004]}, - {"t":1.65034, "x":1.14999, "y":0.0, "heading":0.00046, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":-0.00019, "fx":[-0.00152,0.00378,-0.00113,-0.00113], "fy":[0.00024,-0.00015,-0.00015,0.00025]}, - {"t":1.70035, "x":1.19999, "y":0.0, "heading":0.00046, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":0.0001, "fx":[-0.00025,-0.00025,0.00025,0.00025], "fy":[0.0002,-0.00029,-0.00029,0.0002]}, - {"t":1.75036, "x":1.24999, "y":0.0, "heading":0.00045, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":-0.00005, "fx":[-0.00115,0.00222,-0.00054,-0.00054], "fy":[0.00035,-0.00026,-0.00026,0.00035]}, - {"t":1.80037, "x":1.29999, "y":0.0, "heading":0.00044, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":0.00013, "fx":[-0.00033,-0.00033,0.00033,0.00033], "fy":[0.00028,-0.00037,-0.00038,0.00028]}, - {"t":1.85038, "x":1.34999, "y":0.0, "heading":0.00043, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":0.00002, "fx":[-0.00097,0.00146,-0.00024,-0.00025], "fy":[0.0004,-0.00031,-0.00031,0.0004]}, - {"t":1.90039, "x":1.39999, "y":0.0, "heading":0.00043, "vx":0.9998, "vy":0.0, "omega":-0.00014, "ax":0.0, "ay":0.0, "alpha":0.00015, "fx":[-0.00036,-0.00036,0.00036,0.00036], "fy":[0.00032,-0.0004,-0.00041,0.00032]}, - {"t":1.9504, "x":1.44999, "y":0.0, "heading":0.00042, "vx":0.9998, "vy":0.0, "omega":-0.00014, "ax":0.0, "ay":0.0, "alpha":0.00006, "fx":[-0.00083,0.00098,-0.00007,-0.00008], "fy":[0.00041,-0.00033,-0.00033,0.00041]}, - {"t":2.00041, "x":1.49999, "y":0.0, "heading":0.00041, "vx":0.9998, "vy":0.0, "omega":-0.00013, "ax":0.0, "ay":0.0, "alpha":0.00015, "fx":[-0.00036,-0.00036,0.00036,0.00036], "fy":[0.00032,-0.0004,-0.00041,0.00032]}, - {"t":2.05042, "x":1.54999, "y":0.0, "heading":0.00041, "vx":0.9998, "vy":0.0, "omega":-0.00013, "ax":0.0, "ay":0.0, "alpha":0.00008, "fx":[-0.00069,0.00063,0.00003,0.00003], "fy":[0.0004,-0.00032,-0.00032,0.0004]}, - {"t":2.10043, "x":1.59999, "y":0.0, "heading":0.0004, "vx":0.9998, "vy":0.0, "omega":-0.00012, "ax":0.0, "ay":0.0, "alpha":0.00014, "fx":[-0.00034,-0.00034,0.00034,0.00034], "fy":[0.0003,-0.00038,-0.00039,0.0003]}, - {"t":2.15044, "x":1.64999, "y":0.0, "heading":0.00039, "vx":0.9998, "vy":0.0, "omega":-0.00012, "ax":0.0, "ay":0.0, "alpha":0.00009, "fx":[-0.00056,0.00036,0.0001,0.0001], "fy":[0.00036,-0.00028,-0.00028,0.00036]}, - {"t":2.20045, "x":1.69999, "y":0.0, "heading":0.00039, "vx":0.9998, "vy":0.0, "omega":-0.00011, "ax":0.0, "ay":0.0, "alpha":0.00012, "fx":[-0.0003,-0.0003,0.0003,0.0003], "fy":[0.00026,-0.00034,-0.00035,0.00026]}, - {"t":2.25046, "x":1.74999, "y":0.0, "heading":0.00038, "vx":0.9998, "vy":0.0, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":0.00008, "fx":[-0.00044,0.00018,0.00013,0.00013], "fy":[0.00031,-0.00024,-0.00024,0.00032]}, - {"t":2.30047, "x":1.79999, "y":0.0, "heading":0.00038, "vx":0.9998, "vy":0.0, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":0.0001, "fx":[-0.00025,-0.00025,0.00025,0.00025], "fy":[0.00021,-0.00029,-0.00029,0.00021]}, - {"t":2.35048, "x":1.84999, "y":0.0, "heading":0.00037, "vx":0.9998, "vy":0.0, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":0.00007, "fx":[-0.00033,0.00007,0.00013,0.00013], "fy":[0.00026,-0.00018,-0.00018,0.00026]}, - {"t":2.40049, "x":1.89999, "y":0.0, "heading":0.00037, "vx":0.9998, "vy":0.0, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":0.00008, "fx":[-0.00019,-0.00019,0.00019,0.00019], "fy":[0.00016,-0.00023,-0.00023,0.00016]}, - {"t":2.4505, "x":1.94999, "y":0.0, "heading":0.00036, "vx":0.9998, "vy":0.0, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":0.00006, "fx":[-0.00022,0.0,0.00011,0.00011], "fy":[0.0002,-0.00012,-0.00012,0.0002]}, - {"t":2.50051, "x":1.99999, "y":0.0, "heading":0.00036, "vx":0.9998, "vy":0.0, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":0.00005, "fx":[-0.00013,-0.00013,0.00013,0.00013], "fy":[0.0001,-0.00016,-0.00017,0.0001]}, - {"t":2.55052, "x":2.04999, "y":0.0, "heading":0.00035, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":0.00003, "fx":[-0.00013,-0.00001,0.00007,0.00007], "fy":[0.00013,-0.00006,-0.00006,0.00013]}, - {"t":2.60053, "x":2.09999, "y":0.0, "heading":0.00035, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":0.00003, "fx":[-0.00006,-0.00006,0.00006,0.00006], "fy":[0.00003,-0.0001,-0.0001,0.00003]}, - {"t":2.65054, "x":2.14999, "y":0.0, "heading":0.00035, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[-0.00005,0.00001,0.00002,0.00002], "fy":[0.00006,0.0,0.0,0.00006]}, - {"t":2.70055, "x":2.19999, "y":0.0, "heading":0.00034, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-0.00003,-0.00003,-0.00004,-0.00003]}, - {"t":2.75056, "x":2.24999, "y":0.0, "heading":0.00034, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[0.00002,0.00005,-0.00004,-0.00004], "fy":[0.0,0.00007,0.00007,0.0]}, - {"t":2.80057, "x":2.29999, "y":0.0, "heading":0.00033, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[0.00007,0.00007,-0.00007,-0.00007], "fy":[-0.0001,0.00004,0.00003,-0.0001]}, - {"t":2.85058, "x":2.34999, "y":0.0, "heading":0.00033, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":-0.00004, "fx":[0.0001,0.00009,-0.0001,-0.0001], "fy":[-0.00007,0.00014,0.00014,-0.00007]}, - {"t":2.90059, "x":2.39999, "y":0.0, "heading":0.00033, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":-0.00006, "fx":[0.00014,0.00014,-0.00014,-0.00014], "fy":[-0.00017,0.00011,0.0001,-0.00016]}, - {"t":2.9506, "x":2.44999, "y":0.0, "heading":0.00032, "vx":0.9998, "vy":0.0, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[0.00017,0.00015,-0.00016,-0.00016], "fy":[-0.00014,0.0002,0.0002,-0.00014]}, - {"t":3.00061, "x":2.49999, "y":0.0, "heading":0.00032, "vx":0.9998, "vy":0.0, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":-0.00008, "fx":[0.0002,0.0002,-0.0002,-0.0002], "fy":[-0.00023,0.00018,0.00017,-0.00023]}, - {"t":3.05062, "x":2.55, "y":0.0, "heading":0.00031, "vx":0.9998, "vy":0.0, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":-0.00009, "fx":[0.00024,0.0002,-0.00022,-0.00022], "fy":[-0.00021,0.00027,0.00027,-0.00021]}, - {"t":3.10063, "x":2.6, "y":0.0, "heading":0.00031, "vx":0.9998, "vy":0.0, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":-0.00011, "fx":[0.00027,0.00027,-0.00027,-0.00027], "fy":[-0.0003,0.00024,0.00023,-0.0003]}, - {"t":3.15064, "x":2.65, "y":0.0, "heading":0.0003, "vx":0.9998, "vy":0.0, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":-0.00012, "fx":[0.00032,0.00024,-0.00028,-0.00028], "fy":[-0.00028,0.00033,0.00033,-0.00027]}, - {"t":3.20065, "x":2.7, "y":0.0, "heading":0.0003, "vx":0.9998, "vy":0.0, "omega":-0.00011, "ax":0.0, "ay":0.0, "alpha":-0.00014, "fx":[0.00034,0.00034,-0.00034,-0.00034], "fy":[-0.00036,0.00031,0.0003,-0.00036]}, - {"t":3.25066, "x":2.75, "y":0.0, "heading":0.00029, "vx":0.9998, "vy":0.0, "omega":-0.00012, "ax":0.0, "ay":0.0, "alpha":-0.00014, "fx":[0.00039,0.00027,-0.00033,-0.00033], "fy":[-0.00034,0.0004,0.0004,-0.00034]}, - {"t":3.30067, "x":2.8, "y":0.0, "heading":0.00029, "vx":0.9998, "vy":0.0, "omega":-0.00012, "ax":0.0, "ay":0.0, "alpha":-0.00016, "fx":[0.0004,0.0004,-0.0004,-0.0004], "fy":[-0.00042,0.00037,0.00037,-0.00042]}, - {"t":3.35068, "x":2.85, "y":0.0, "heading":0.00028, "vx":0.9998, "vy":0.0, "omega":-0.00013, "ax":0.0, "ay":0.0, "alpha":-0.00016, "fx":[0.00048,0.00026,-0.00037,-0.00037], "fy":[-0.00041,0.00046,0.00046,-0.00041]}, - {"t":3.40069, "x":2.9, "y":0.0, "heading":0.00028, "vx":0.9998, "vy":0.0, "omega":-0.00014, "ax":0.0, "ay":0.0, "alpha":-0.00019, "fx":[0.00046,0.00046,-0.00046,-0.00046], "fy":[-0.00049,0.00044,0.00043,-0.00049]}, - {"t":3.4507, "x":2.95, "y":0.0, "heading":0.00027, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":-0.00018, "fx":[0.00058,0.0002,-0.00039,-0.00039], "fy":[-0.00047,0.00052,0.00052,-0.00047]}, - {"t":3.50071, "x":3.0, "y":0.0, "heading":0.00026, "vx":0.9998, "vy":0.0, "omega":-0.00016, "ax":0.0, "ay":0.0, "alpha":-0.00021, "fx":[0.00052,0.00052,-0.00052,-0.00052], "fy":[-0.00054,0.0005,0.00049,-0.00054]}, - {"t":3.55072, "x":3.05, "y":0.0, "heading":0.00025, "vx":0.9998, "vy":0.0, "omega":-0.00017, "ax":0.0, "ay":0.0, "alpha":-0.00019, "fx":[0.0007,0.0001,-0.0004,-0.0004], "fy":[-0.00053,0.00058,0.00058,-0.00053]}, - {"t":3.60073, "x":3.1, "y":0.0, "heading":0.00024, "vx":0.9998, "vy":0.0, "omega":-0.00018, "ax":0.0, "ay":0.0, "alpha":-0.00024, "fx":[0.00058,0.00058,-0.00058,-0.00058], "fy":[-0.0006,0.00055,0.00054,-0.0006]}, - {"t":3.65074, "x":3.15, "y":0.0, "heading":0.00024, "vx":0.9998, "vy":0.0, "omega":-0.00019, "ax":0.0, "ay":0.0, "alpha":-0.0002, "fx":[0.00081,-0.00004,-0.00039,-0.00039], "fy":[-0.00058,0.00063,0.00063,-0.00058]}, - {"t":3.70075, "x":3.2, "y":0.0, "heading":0.00023, "vx":0.9998, "vy":0.0, "omega":-0.0002, "ax":0.0, "ay":0.0, "alpha":-0.00026, "fx":[0.00062,0.00062,-0.00062,-0.00062], "fy":[-0.00064,0.0006,0.00059,-0.00064]}, - {"t":3.75076, "x":3.25, "y":0.0, "heading":0.00022, "vx":0.9998, "vy":0.0, "omega":-0.00021, "ax":0.0, "ay":0.0, "alpha":-0.00021, "fx":[0.00093,-0.00022,-0.00035,-0.00035], "fy":[-0.00062,0.00067,0.00067,-0.00062]}, - {"t":3.80077, "x":3.3, "y":0.0, "heading":0.0002, "vx":0.9998, "vy":0.0, "omega":-0.00022, "ax":0.0, "ay":0.0, "alpha":-0.00027, "fx":[0.00066,0.00066,-0.00066,-0.00066], "fy":[-0.00068,0.00064,0.00063,-0.00068]}, - {"t":3.85078, "x":3.35, "y":0.0, "heading":0.00019, "vx":0.9998, "vy":0.0, "omega":-0.00024, "ax":0.0, "ay":0.0, "alpha":-0.0002, "fx":[0.00105,-0.00047,-0.00029,-0.00029], "fy":[-0.00066,0.0007,0.0007,-0.00066]}, - {"t":3.90079, "x":3.4, "y":0.0, "heading":0.00018, "vx":0.9998, "vy":0.0, "omega":-0.00025, "ax":0.0, "ay":0.0, "alpha":-0.00028, "fx":[0.00068,0.00068,-0.00068,-0.00068], "fy":[-0.0007,0.00066,0.00065,-0.0007]}, - {"t":3.9508, "x":3.45, "y":0.0, "heading":0.00017, "vx":0.9998, "vy":0.0, "omega":-0.00026, "ax":0.0, "ay":0.0, "alpha":-0.00018, "fx":[0.00117,-0.00078,-0.0002,-0.0002], "fy":[-0.00067,0.00071,0.00071,-0.00067]}, - {"t":4.00081, "x":3.5, "y":0.0, "heading":0.00016, "vx":0.9998, "vy":0.0, "omega":-0.00027, "ax":0.0, "ay":0.0, "alpha":-0.00028, "fx":[0.00068,0.00068,-0.00068,-0.00068], "fy":[-0.00069,0.00066,0.00065,-0.00069]}, - {"t":4.05082, "x":3.55, "y":0.0, "heading":0.00014, "vx":0.9998, "vy":0.0, "omega":-0.00028, "ax":0.0, "ay":0.0, "alpha":-0.00015, "fx":[0.00128,-0.00117,-0.00005,-0.00005], "fy":[-0.00066,0.00069,0.00069,-0.00066]}, - {"t":4.10083, "x":3.6, "y":0.0, "heading":0.00013, "vx":0.9998, "vy":0.0, "omega":-0.00029, "ax":0.0, "ay":0.0, "alpha":-0.00026, "fx":[0.00065,0.00065,-0.00065,-0.00065], "fy":[-0.00066,0.00063,0.00062,-0.00066]}, - {"t":4.15084, "x":3.65, "y":0.0, "heading":0.00011, "vx":0.9998, "vy":0.0, "omega":-0.00031, "ax":0.0, "ay":0.0, "alpha":-0.00009, "fx":[0.00139,-0.00172,0.00016,0.00016], "fy":[-0.00061,0.00064,0.00064,-0.00061]}, - {"t":4.20085, "x":3.7, "y":0.0, "heading":0.0001, "vx":0.9998, "vy":0.0, "omega":-0.00031, "ax":0.0, "ay":0.0, "alpha":-0.00023, "fx":[0.00057,0.00057,-0.00057,-0.00057], "fy":[-0.00058,0.00055,0.00054,-0.00058]}, - {"t":4.25086, "x":3.75, "y":0.0, "heading":0.00008, "vx":0.9998, "vy":0.0, "omega":-0.00032, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00159,-0.00272,0.00056,0.00056], "fy":[-0.00051,0.00054,0.00055,-0.00051]}, - {"t":4.30087, "x":3.8, "y":0.0, "heading":0.00007, "vx":0.9998, "vy":0.0, "omega":-0.00032, "ax":0.0, "ay":0.0, "alpha":-0.00018, "fx":[0.00044,0.00044,-0.00044,-0.00044], "fy":[-0.00044,0.00042,0.0004,-0.00044]}, - {"t":4.35088, "x":3.85, "y":0.0, "heading":0.00005, "vx":0.9998, "vy":0.0, "omega":-0.00033, "ax":0.0, "ay":0.0, "alpha":0.00023, "fx":[0.00216,-0.00509,0.00147,0.00147], "fy":[-0.00035,0.00038,0.00038,-0.00035]}, - {"t":4.40089, "x":3.9, "y":0.0, "heading":0.00003, "vx":0.9998, "vy":0.0, "omega":-0.00032, "ax":0.0, "ay":0.0, "alpha":-0.00009, "fx":[0.00022,0.00022,-0.00022,-0.00022], "fy":[-0.00022,0.0002,0.00018,-0.00022]}, - {"t":4.4509, "x":3.95, "y":0.0, "heading":0.00002, "vx":0.9998, "vy":0.0, "omega":-0.00032, "ax":0.0, "ay":0.0, "alpha":0.00078, "fx":[0.00408,-0.01188,0.0039,0.0039], "fy":[-0.0001,0.00013,0.00013,-0.0001]}, - {"t":4.50091, "x":4.0, "y":0.0, "heading":0.0, "vx":0.9998, "vy":0.0, "omega":-0.00028, "ax":0.0, "ay":0.0, "alpha":0.00006, "fx":[-0.00013,-0.00013,0.00013,0.00013], "fy":[0.00014,-0.00017,-0.00018,0.00014]}, - {"t":4.55093, "x":4.05, "y":0.0, "heading":-0.00001, "vx":0.9998, "vy":0.0, "omega":-0.00028, "ax":0.0, "ay":0.0, "alpha":0.00231, "fx":[0.01035,-0.03229,0.01097,0.01097], "fy":[0.00029,-0.00026,-0.00026,0.00029]}, - {"t":4.60094, "x":4.1, "y":0.0, "heading":-0.00003, "vx":0.9998, "vy":0.0, "omega":-0.00017, "ax":0.0, "ay":0.0, "alpha":0.00032, "fx":[-0.00075,-0.00075,0.00075,0.00075], "fy":[0.00078,-0.00079,-0.00081,0.00078]}, - {"t":4.65095, "x":4.15, "y":0.0, "heading":-0.00003, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":0.00642, "fx":[0.02846,-0.08921,0.03038,0.03037], "fy":[0.00092,-0.0009,-0.0009,0.00092]}, - {"t":4.70096, "x":4.2, "y":0.0, "heading":-0.00004, "vx":0.9998, "vy":0.0, "omega":0.00017, "ax":0.0, "ay":0.0, "alpha":0.00081, "fx":[-0.00194,-0.00194,0.00194,0.00194], "fy":[0.002,-0.00201,-0.00202,0.002]}, - {"t":4.75097, "x":4.25, "y":0.0, "heading":-0.00003, "vx":0.9998, "vy":0.0, "omega":0.00021, "ax":0.0, "ay":0.0, "alpha":0.01524, "fx":[0.06758,-0.21167,0.07205,0.07205], "fy":[0.00217,-0.00216,-0.00216,0.00217]}, - {"t":4.80098, "x":4.3, "y":0.0, "heading":-0.00002, "vx":0.9998, "vy":0.0, "omega":0.00097, "ax":0.0, "ay":0.0, "alpha":0.00183, "fx":[-0.00441,-0.00441,0.00441,0.00441], "fy":[0.00452,-0.00452,-0.00453,0.00452]}, - {"t":4.85099, "x":4.35, "y":0.0, "heading":0.00003, "vx":0.9998, "vy":0.0, "omega":0.00107, "ax":0.0, "ay":0.0, "alpha":0.02102, "fx":[0.08606,-0.28009,0.09701,0.09701], "fy":[0.00534,-0.00534,-0.00534,0.00534]}, - {"t":4.901, "x":4.4, "y":0.0, "heading":0.00008, "vx":0.9998, "vy":0.0, "omega":0.00212, "ax":0.0, "ay":0.0, "alpha":0.00372, "fx":[-0.00892,-0.00892,0.00898,0.00898], "fy":[0.00917,-0.00917,-0.00916,0.00917]}, - {"t":4.95101, "x":4.45, "y":0.0, "heading":0.00019, "vx":0.9998, "vy":0.0, "omega":0.0023, "ax":-0.00899, "ay":0.0, "alpha":-0.00661, "fx":[-0.2199,0.00956,-0.19446,-0.19446], "fy":[0.01242,-0.01243,-0.01243,0.01242]}, - {"t":5.00102, "x":4.49999, "y":0.0, "heading":0.0003, "vx":0.99935, "vy":0.0, "omega":0.00197, "ax":-0.99419, "ay":0.0, "alpha":0.006, "fx":[-16.58714,-16.58714,-16.55829,-16.55829], "fy":[0.01477,-0.01478,-0.01476,0.01477]}, - {"t":5.05103, "x":4.54872, "y":0.0, "heading":0.0004, "vx":0.94963, "vy":0.0, "omega":0.00227, "ax":-0.99763, "ay":0.0, "alpha":-0.02999, "fx":[-16.83337,-16.09669,-16.7949,-16.7949], "fy":[0.01881,-0.01881,-0.01881,0.01881]}, - {"t":5.10104, "x":4.59497, "y":0.0, "heading":0.00051, "vx":0.89974, "vy":0.0, "omega":0.00077, "ax":-0.99851, "ay":0.0, "alpha":0.00763, "fx":[-16.66302,-16.66302,-16.62628,-16.62628], "fy":[0.01882,-0.01883,-0.0188,0.01882]}, - {"t":5.15105, "x":4.63871, "y":0.0, "heading":0.00055, "vx":0.8498, "vy":0.0, "omega":0.00115, "ax":-0.99891, "ay":0.0, "alpha":-0.02646, "fx":[-16.84564,-16.1563,-16.80181,-16.80181], "fy":[0.02143,-0.02143,-0.02143,0.02143]}, - {"t":5.20106, "x":4.67996, "y":0.0, "heading":0.00061, "vx":0.79985, "vy":0.0, "omega":-0.00017, "ax":-0.99914, "ay":0.0, "alpha":0.00835, "fx":[-16.67535,-16.67535,-16.63516,-16.63516], "fy":[0.02059,-0.0206,-0.02058,0.02059]}, - {"t":5.25107, "x":4.71872, "y":0.0, "heading":0.0006, "vx":0.74988, "vy":0.0, "omega":0.00025, "ax":-0.99929, "ay":0.0, "alpha":-0.01953, "fx":[-16.81976,-16.26147,-16.77491,-16.77491], "fy":[0.02193,-0.02193,-0.02193,0.02193]}, - {"t":5.30108, "x":4.75497, "y":0.0, "heading":0.00061, "vx":0.69991, "vy":0.0, "omega":-0.00073, "ax":-0.9994, "ay":0.0, "alpha":0.00844, "fx":[-16.67982,-16.67982,-16.63922,-16.63922], "fy":[0.0208,-0.0208,-0.02079,0.0208]}, - {"t":5.35109, "x":4.78872, "y":0.0, "heading":0.00058, "vx":0.64993, "vy":0.0, "omega":-0.00031, "ax":-0.99948, "ay":0.0, "alpha":-0.0144, "fx":[-16.79597,-16.34261,-16.75235,-16.75235], "fy":[0.02133,-0.02133,-0.02133,0.02133]}, - {"t":5.4011, "x":4.81997, "y":0.0, "heading":0.00056, "vx":0.59994, "vy":0.0, "omega":-0.00103, "ax":-0.99954, "ay":0.0, "alpha":0.00815, "fx":[-16.68144,-16.68144,-16.64221,-16.64221], "fy":[0.0201,-0.02011,-0.0201,0.0201]}, - {"t":5.45111, "x":4.84873, "y":0.0, "heading":0.00051, "vx":0.54995, "vy":0.0, "omega":-0.00062, "ax":-0.99959, "ay":0.0, "alpha":-0.01077, "fx":[-16.77648,-16.40346,-16.73526,-16.73526], "fy":[0.02015,-0.02015,-0.02015,0.02015]}, - {"t":5.50112, "x":4.87498, "y":0.0, "heading":0.00048, "vx":0.49996, "vy":0.0, "omega":-0.00116, "ax":-0.99962, "ay":0.0, "alpha":0.00768, "fx":[-16.68174,-16.68174,-16.64478,-16.64478], "fy":[0.01895,-0.01895,-0.01894,0.01895]}, - {"t":5.55113, "x":4.89873, "y":0.0, "heading":0.00042, "vx":0.44997, "vy":0.0, "omega":-0.00077, "ax":-0.99966, "ay":0.0, "alpha":-0.00829, "fx":[-16.76126,-16.44805,-16.72294,-16.72294], "fy":[0.01873,-0.01873,-0.01873,0.01873]}, - {"t":5.60114, "x":4.91999, "y":0.0, "heading":0.00038, "vx":0.39998, "vy":0.0, "omega":-0.00119, "ax":-0.99968, "ay":0.0, "alpha":0.00714, "fx":[-16.68141,-16.68141,-16.64709,-16.64709], "fy":[0.0176,-0.0176,-0.0176,0.0176]}, - {"t":5.65115, "x":4.93874, "y":0.0, "heading":0.00032, "vx":0.34999, "vy":0.0, "omega":-0.00083, "ax":-0.99971, "ay":0.0, "alpha":-0.00654, "fx":[-16.74906,-16.48199,-16.71375,-16.71375], "fy":[0.01725,-0.01725,-0.01725,0.01725]}, - {"t":5.70116, "x":4.95499, "y":0.0, "heading":0.00028, "vx":0.29999, "vy":0.0, "omega":-0.00116, "ax":-0.99973, "ay":0.0, "alpha":0.00658, "fx":[-16.68079,-16.68079,-16.64915,-16.64915], "fy":[0.01623,-0.01623,-0.01623,0.01623]}, - {"t":5.75117, "x":4.96874, "y":0.0, "heading":0.00022, "vx":0.24999, "vy":0.0, "omega":-0.00083, "ax":-0.99974, "ay":0.0, "alpha":-0.00531, "fx":[-16.73944,-16.5076,-16.707,-16.707], "fy":[0.01585,-0.01585,-0.01585,0.01585]}, - {"t":5.80118, "x":4.98, "y":0.0, "heading":0.00018, "vx":0.2, "vy":0.0, "omega":-0.00109, "ax":-0.99976, "ay":0.0, "alpha":0.00606, "fx":[-16.68008,-16.68008,-16.65095,-16.65095], "fy":[0.01495,-0.01495,-0.01494,0.01495]}, - {"t":5.85119, "x":4.98875, "y":0.0, "heading":0.00013, "vx":0.15, "vy":0.0, "omega":-0.00079, "ax":-0.99977, "ay":0.0, "alpha":-0.00444, "fx":[-16.73178,-16.52731,-16.70193,-16.70193], "fy":[0.01458,-0.01458,-0.01458,0.01458]}, - {"t":5.9012, "x":4.995, "y":0.0, "heading":0.00009, "vx":0.1, "vy":0.0, "omega":-0.00101, "ax":-0.99979, "ay":0.0, "alpha":0.0056, "fx":[-16.67939,-16.67939,-16.65249,-16.65249], "fy":[0.01381,-0.01381,-0.01381,0.01381]}, - {"t":5.95121, "x":4.99875, "y":0.0, "heading":0.00004, "vx":0.05, "vy":0.0, "omega":-0.00073, "ax":-0.9998, "ay":0.0, "alpha":0.01467, "fx":[-16.72458,-16.72458,-16.69857,-16.51676], "fy":[0.01301,-0.01301,-0.01301,0.013]}, - {"t":6.00122, "x":5.0, "y":0.0, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":0.0, "y":0.0, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":11.95331, "ay":-0.00002, "alpha":-0.00111, "fx":[199.256,199.256,199.25599,199.25599], "fy":[-0.00578,0.00507,0.00507,-0.00578]}, + {"t":0.07918, "x":0.03747, "y":0.0, "heading":0.0, "vx":0.94646, "vy":0.0, "omega":-0.00009, "ax":11.94959, "ay":-0.00003, "alpha":-0.0012, "fx":[199.19396,199.19397,199.19396,199.19396], "fy":[-0.0063,0.00539,0.00543,-0.0063]}, + {"t":0.15836, "x":0.14987, "y":0.0, "heading":-0.00001, "vx":1.89262, "vy":0.0, "omega":-0.00018, "ax":11.93856, "ay":0.00037, "alpha":-0.00144, "fx":[199.01007,199.01007,199.01005,199.01005], "fy":[-0.00079,0.01328,0.01321,-0.00079]}, + {"t":0.23754, "x":0.33715, "y":0.0, "heading":-0.00002, "vx":2.83791, "vy":0.00003, "omega":-0.0003, "ax":10.8818, "ay":-0.00029, "alpha":0.00763, "fx":[181.45486,181.2459,181.43851,181.43851], "fy":[-0.01171,0.0016,0.00262,-0.01175]}, + {"t":0.31672, "x":0.59597, "y":0.0, "heading":-0.00005, "vx":3.69953, "vy":0.0, "omega":0.00031, "ax":0.00012, "ay":0.00054, "alpha":-0.00139, "fx":[0.02459,0.01426,-0.02048,-0.01014], "fy":[0.02473,0.01241,-0.01573,0.01439]}, + {"t":0.3959, "x":0.88889, "y":0.0, "heading":-0.00002, "vx":3.69954, "vy":0.00005, "omega":0.0002, "ax":0.0, "ay":-0.00114, "alpha":-0.00136, "fx":[-1.0006,1.63376,-0.31719,-0.31594], "fy":[0.29151,-0.32837,-0.32958,0.29036]}, + {"t":0.47508, "x":1.18182, "y":0.0, "heading":-0.00001, "vx":3.69954, "vy":-0.00004, "omega":0.00009, "ax":0.0, "ay":0.00077, "alpha":-0.00079, "fx":[0.0058,0.00568,-0.00581,-0.00569], "fy":[0.01475,0.00185,0.01986,0.01462]}, + {"t":0.55426, "x":1.47475, "y":0.0, "heading":0.0, "vx":3.69954, "vy":0.00002, "omega":0.00003, "ax":0.0, "ay":-0.00058, "alpha":-0.00042, "fx":[0.18252,-0.29271,0.05515,0.05504], "fy":[-0.06678,0.04752,0.04761,-0.06669]}, + {"t":0.63344, "x":1.76768, "y":0.0, "heading":0.0, "vx":3.69954, "vy":-0.00003, "omega":-0.00001, "ax":0.0, "ay":0.00031, "alpha":-0.0002, "fx":[0.0007,0.00071,-0.0007,-0.00071], "fy":[0.00482,0.0039,0.00684,0.00483]}, + {"t":0.71262, "x":2.06061, "y":0.0, "heading":0.0, "vx":3.69954, "vy":-0.00001, "omega":-0.00002, "ax":0.0, "ay":-0.00007, "alpha":-0.00011, "fx":[0.03438,-0.0548,0.01022,0.0102], "fy":[-0.01182,0.00963,0.00965,-0.0118]}, + {"t":0.7918, "x":2.35354, "y":0.0, "heading":0.0, "vx":3.69954, "vy":-0.00001, "omega":-0.00003, "ax":0.0, "ay":0.00003, "alpha":0.0, "fx":[0.00006,0.00006,-0.00006,-0.00006], "fy":[0.00052,-0.0001,0.00085,0.00052]}, + {"t":0.87098, "x":2.64646, "y":0.0, "heading":0.0, "vx":3.69954, "vy":-0.00001, "omega":-0.00003, "ax":0.0, "ay":0.00001, "alpha":0.00013, "fx":[-0.03328,0.05311,-0.00992,-0.00991], "fy":[0.01071,-0.01042,-0.01043,0.0107]}, + {"t":0.95016, "x":2.93939, "y":0.0, "heading":0.0, "vx":3.69954, "vy":-0.00001, "omega":-0.00002, "ax":0.0, "ay":-0.00028, "alpha":0.00019, "fx":[-0.0007,-0.0007,0.0007,0.0007], "fy":[-0.00441,-0.00408,-0.0057,-0.00441]}, + {"t":1.02934, "x":3.23232, "y":0.0, "heading":-0.00001, "vx":3.69954, "vy":-0.00003, "omega":-0.00001, "ax":0.0, "ay":0.00057, "alpha":0.00044, "fx":[-0.17435,0.2799,-0.05279,-0.05276], "fy":[0.06445,-0.0454,-0.04543,0.06442]}, + {"t":1.10851, "x":3.52525, "y":0.0, "heading":-0.00001, "vx":3.69954, "vy":0.00002, "omega":0.00003, "ax":0.0, "ay":-0.00088, "alpha":0.00079, "fx":[-0.00633,-0.00631,0.00633,0.00632], "fy":[-0.01717,-0.00238,-0.02203,-0.01715]}, + {"t":1.18769, "x":3.81818, "y":-0.00001, "heading":0.0, "vx":3.69954, "vy":-0.00005, "omega":0.00009, "ax":0.0, "ay":0.00136, "alpha":0.00134, "fx":[1.04108,-1.69797,0.32842,0.32844], "fy":[-0.29914,0.34461,0.34457,-0.29925]}, + {"t":1.26687, "x":4.11111, "y":-0.00001, "heading":0.0, "vx":3.69954, "vy":0.00005, "omega":0.0002, "ax":-0.00012, "ay":-0.00056, "alpha":0.00139, "fx":[-0.02221,-0.02285,0.01809,0.01874], "fy":[-0.02277,0.00063,0.00807,-0.02341]}, + {"t":1.34605, "x":4.40403, "y":0.0, "heading":0.00002, "vx":3.69953, "vy":0.00001, "omega":0.00031, "ax":-10.8818, "ay":0.00034, "alpha":-0.00757, "fx":[-181.45392,-181.24783,-181.43794,-181.43794], "fy":[0.01242,-0.00037,-0.00152,0.01232]}, + {"t":1.42523, "x":4.66285, "y":0.0, "heading":0.00004, "vx":2.83791, "vy":0.00004, "omega":-0.00029, "ax":-11.93856, "ay":-0.0005, "alpha":0.00142, "fx":[-199.01006,-199.01006,-199.01004,-199.01004], "fy":[-0.00141,-0.01549,-0.015,-0.00141]}, + {"t":1.50441, "x":4.85013, "y":0.0, "heading":0.00002, "vx":1.89262, "vy":0.0, "omega":-0.00018, "ax":-11.94959, "ay":0.00003, "alpha":0.00117, "fx":[-199.19397,-199.19397,-199.19396,-199.19396], "fy":[0.00614,-0.00525,-0.00529,0.00614]}, + {"t":1.58359, "x":4.96253, "y":0.0, "heading":0.00001, "vx":0.94646, "vy":0.0, "omega":-0.00009, "ax":-11.95332, "ay":0.00002, "alpha":0.00109, "fx":[-199.25604,-199.25604,-199.25604,-199.25604], "fy":[0.00563,-0.00496,-0.00494,0.00562]}, + {"t":1.66277, "x":5.0, "y":0.0, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 23ce1668..8743482b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,7 +6,6 @@ import static edu.wpi.first.units.Units.Rotations; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -227,11 +226,7 @@ public RobotContainer() { visionSubsystem, pathHandler); - testAutonCommand = - new TestAutonCommand( - driveSubsystem, - robotStateSubsystem, - new Pose2d(3.85576086490539, 5.073261807735684, Rotation2d.fromDegrees(300.0))); + testAutonCommand = new TestAutonCommand(driveSubsystem, robotStateSubsystem, elevatorSubsystem); configureTelemetry(); configureDriverBindings(); diff --git a/src/main/java/frc/robot/commands/auton/TestAutonCommand.java b/src/main/java/frc/robot/commands/auton/TestAutonCommand.java index 76680051..a1fbbff3 100644 --- a/src/main/java/frc/robot/commands/auton/TestAutonCommand.java +++ b/src/main/java/frc/robot/commands/auton/TestAutonCommand.java @@ -2,10 +2,13 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.commands.drive.DriveAutonCommand; import frc.robot.commands.drive.PrepOdomForAutoCommand; +import frc.robot.commands.elevator.ZeroElevatorCommand; import frc.robot.subsystems.drive.DriveSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem; import frc.robot.subsystems.robotState.RobotStateSubsystem; public class TestAutonCommand extends SequentialCommandGroup implements AutoCommandInterface { @@ -14,17 +17,21 @@ public class TestAutonCommand extends SequentialCommandGroup implements AutoComm private RobotStateSubsystem robotStateSubsystem; public TestAutonCommand( - DriveSubsystem driveSubsystem, RobotStateSubsystem robotStateSubsystem, Pose2d startPose) { + DriveSubsystem driveSubsystem, + RobotStateSubsystem robotStateSubsystem, + ElevatorSubsystem elevatorSubsystem) { this.driveSubsystem = driveSubsystem; this.robotStateSubsystem = robotStateSubsystem; - this.startPath = new DriveAutonCommand(driveSubsystem, "LTofetch", true, true, false); + this.startPath = new DriveAutonCommand(driveSubsystem, "FiveMeterTestPath", true, true, false); addCommands( new SequentialCommandGroup( - new PrepOdomForAutoCommand( - robotStateSubsystem, driveSubsystem, Rotation2d.fromDegrees(300.0), startPose), + new ParallelCommandGroup( + new PrepOdomForAutoCommand( + robotStateSubsystem, driveSubsystem, new Rotation2d(), new Pose2d()), + new ZeroElevatorCommand(elevatorSubsystem)), // new SetGyroOffsetCommand(driveSubsystem, Rotation2d.fromDegrees(180)), startPath)); } diff --git a/src/main/java/frc/robot/commands/drive/DriveAutonCommand.java b/src/main/java/frc/robot/commands/drive/DriveAutonCommand.java index 981bed66..60427592 100644 --- a/src/main/java/frc/robot/commands/drive/DriveAutonCommand.java +++ b/src/main/java/frc/robot/commands/drive/DriveAutonCommand.java @@ -130,7 +130,7 @@ public void initialize() { timer.reset(); // logger.info("Begin Trajectory: {}", trajectoryName); desiredState = mirrorToProcessor(trajectory.sampleAt(timer.get(), mirrorTrajectory).get()); - driveSubsystem.calculateController(desiredState); + // driveSubsystem.calculateController(desiredState); if (resetOdometry) { driveSubsystem.resetOdometry(initialPose); } diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index a67539b3..b7757de9 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -179,10 +179,10 @@ public static TalonFXConfiguration getDriveTalonConfig() { TalonFXConfiguration driveConfig = new TalonFXConfiguration(); CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs(); - currentConfig.SupplyCurrentLimit = 60; - currentConfig.SupplyCurrentLowerLimit = 60; + currentConfig.SupplyCurrentLimit = 80; + currentConfig.SupplyCurrentLowerLimit = 80; currentConfig.SupplyCurrentLowerTime = 1.0; - currentConfig.StatorCurrentLimit = 140; + currentConfig.StatorCurrentLimit = 160; currentConfig.SupplyCurrentLimitEnable = true; currentConfig.StatorCurrentLimitEnable = true; diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 09bbfeef..acdd87fd 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -120,8 +120,9 @@ public static TalonFXConfiguration getBothFXConfig() { MotionMagicConfigs motionMagic = new MotionMagicConfigs() - .withMotionMagicCruiseVelocity(75) - .withMotionMagicAcceleration(225); // was 300 + .withMotionMagicCruiseVelocity(70) + .withMotionMagicAcceleration(300) // was 300 + .withMotionMagicJerk(1500); fxConfig.MotionMagic = motionMagic; MotorOutputConfigs motorOut = diff --git a/src/main/java/frc/robot/constants/LEDConstants.java b/src/main/java/frc/robot/constants/LEDConstants.java index f340c9ad..32b7e6f3 100644 --- a/src/main/java/frc/robot/constants/LEDConstants.java +++ b/src/main/java/frc/robot/constants/LEDConstants.java @@ -52,6 +52,9 @@ public static final Color invertRedGreen(Color color) { public static final Color kGood = invertRedGreen(Color.kGreen); public static final Color kTooClose = invertRedGreen(Color.kBlue); + // HP Load Color + public static final Color kLoadCoral = invertRedGreen(Color.kYellow); + public static final Color[] kGameColors = { invertRedGreen(Color.kBlack), // a dummy color invertRedGreen(Color.kDarkRed), diff --git a/src/main/java/frc/robot/subsystems/drive/Swerve.java b/src/main/java/frc/robot/subsystems/drive/Swerve.java index 6fdcc594..604a3096 100644 --- a/src/main/java/frc/robot/subsystems/drive/Swerve.java +++ b/src/main/java/frc/robot/subsystems/drive/Swerve.java @@ -63,6 +63,8 @@ public class Swerve implements SwerveIO, Checkable { private StatusSignal drive11StatorCurrent; private StatusSignal drive12StatorCurrent; private StatusSignal drive13StatorCurrent; + private StatusSignal drive10Velocity; + private StatusSignal drive11Velocity; private StatusSignal drive12Velocity; private StatusSignal drive13Velocity; private boolean didZero = false; @@ -112,6 +114,8 @@ public Swerve() { drive11StatorCurrent = drives[1].getStatorCurrent(); drive12StatorCurrent = drives[2].getStatorCurrent(); drive13StatorCurrent = drives[3].getStatorCurrent(); + drive10Velocity = drives[0].getVelocity(); + drive11Velocity = drives[1].getVelocity(); drive12Velocity = drives[2].getVelocity(); drive13Velocity = drives[3].getVelocity(); didZero = true; @@ -345,6 +349,8 @@ public void updateInputs(SwerveIOInputs inputs) { drive11StatorCurrent, drive12StatorCurrent, drive13StatorCurrent, + drive10Velocity, + drive11Velocity, drive12Velocity, drive13Velocity); // inputs.avgDriveCurrent = getAvgDriveCurrent(); @@ -360,6 +366,10 @@ public void updateInputs(SwerveIOInputs inputs) { (Math.abs(drive12Velocity.getValueAsDouble()) + Math.abs(drive13Velocity.getValueAsDouble())) / 2; + inputs.avgRearDriveVel = + (Math.abs(drive10Velocity.getValueAsDouble()) + + Math.abs(drive11Velocity.getValueAsDouble())) + / 2; } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java b/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java index 2808e6fa..ed56aff7 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java @@ -194,6 +194,16 @@ public double getAvgDriveCurrent() { return sum / 4.0; } + public double getFrontDriveAvgVel() { + double sum = 0; + + for (int i = 0; i < 2; i++) { + sum += FastMath.abs(drives[i].getVelocity().getValueAsDouble()); + } + + return sum / 2.0; + } + public double getRearDriveAvgVel() { double sum = 0; @@ -310,6 +320,7 @@ public void updateInputs(SwerveIOInputs inputs) { } inputs.avgDriveCurrent = getAvgDriveCurrent(); inputs.avgRearDriveVel = getRearDriveAvgVel(); + inputs.avgFrontDriveVel = getFrontDriveAvgVel(); inputs.robotRelSpeed = getRobotRelSpeed(); inputs.fieldRelSpeed = getFieldRelSpeed(inputs.robotRelSpeed); inputs.fieldY = fieldY; diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveIO.java b/src/main/java/frc/robot/subsystems/drive/SwerveIO.java index f6e27bce..384e6249 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveIO.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveIO.java @@ -42,6 +42,7 @@ public static class SwerveIOInputs { public double[] azimuthCurrent = {0, 0, 0, 0}; public double avgDriveCurrent = 0; public double avgRearDriveVel = 0; + public double avgFrontDriveVel = 0; public boolean didZero = false; } diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java index 9f6e9db1..a786ffb7 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java @@ -45,6 +45,9 @@ public class LEDSubsystem extends MeasurableSubsystem { .blink(Seconds.of(0.25)); private LEDPattern currentLimiting = LEDPattern.solid(LEDConstants.kCurrentLimiting).blink(Seconds.of(0.5), Seconds.of(1)); + private LEDPattern loadCoral = + LEDPattern.steps( + Map.of(0, Color.kBlack, LEDConstants.kTopFirstIndex, LEDConstants.kLoadCoral)); // section booleans and states private boolean hasAlgae = false; @@ -52,6 +55,7 @@ public class LEDSubsystem extends MeasurableSubsystem { private ScoringLevel levelState = ScoringLevel.L4; private PlaceStates placeState = PlaceStates.MANUAL; private boolean shouldGetAlgae = false; + private boolean shouldLoadCoral = false; private boolean autoPlacing = false; private boolean isLimiting = false; @@ -115,6 +119,11 @@ public void setCurrentLimiting(boolean isCurrentLimiting) { buildBase(); } + public void setLoadCoral(boolean loadCoral) { + this.shouldLoadCoral = loadCoral; + buildBase(); + } + // getters public boolean getAlgeaLights() { return hasAlgae; @@ -212,6 +221,10 @@ private void buildBase() { } base = getAlgea.overlayOn(place.overlayOn(level.overlayOn(base))); + + if (shouldLoadCoral) { + base = loadCoral.overlayOn(base); + } } // game stuff diff --git a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java index 9d5e9281..b3505401 100644 --- a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java +++ b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java @@ -407,6 +407,9 @@ public void periodic() { if (!runningPath) { startPath(nextPath()); } + if (tagAlignSubsystem.getCurRadius(robotStateSubsystem.getAllianceColor()) + < .5) { // TODO magic num + } drivePath(); } case FETCH -> { diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index b31ad202..43f19af9 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -705,6 +705,10 @@ public boolean isElevatorFinished() { return elevatorSubsystem.isFinished(); } + public void setLEDLoadCoral(boolean isOn) { + ledSubsystem.setLoadCoral(isOn); + } + @Override public void periodic() { Logger.recordOutput("RobotState/state", curState); From 451f60005902ccac5351b2c5993914e77e0ba77e Mon Sep 17 00:00:00 2001 From: KaydenLee456 Date: Thu, 27 Mar 2025 20:00:55 -0400 Subject: [PATCH 03/12] Added a very small amount of stuff --- .../robot/constants/BargeAlignConstants.java | 4 +-- .../robot/subsystems/vision/BargeAlign.java | 34 +++++++++++++++---- 2 files changed, 29 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/constants/BargeAlignConstants.java b/src/main/java/frc/robot/constants/BargeAlignConstants.java index 9753ae44..dfd021f6 100644 --- a/src/main/java/frc/robot/constants/BargeAlignConstants.java +++ b/src/main/java/frc/robot/constants/BargeAlignConstants.java @@ -4,7 +4,7 @@ public class BargeAlignConstants { - public static final Translation2d redBargePos = new Translation2d(); //TODO Sorry future contributor but these are empty - public static final Translation2d blueBargePos = new Translation2d(); + public static final Translation2d redBargePos = new Translation2d(2, 3); //TODO random values now + public static final Translation2d blueBargePos = new Translation2d(3, 2); } diff --git a/src/main/java/frc/robot/subsystems/vision/BargeAlign.java b/src/main/java/frc/robot/subsystems/vision/BargeAlign.java index d567826a..b18e059d 100644 --- a/src/main/java/frc/robot/subsystems/vision/BargeAlign.java +++ b/src/main/java/frc/robot/subsystems/vision/BargeAlign.java @@ -12,35 +12,54 @@ import java.util.function.DoubleSupplier; import frc.robot.subsystems.drive.DriveSubsystem; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; public class BargeAlign extends MeasurableSubsystem{ private DoubleSupplier strStick; private DriveSubsystem driveSubsystem; + //These are here to determine offsets for target positions. + private boolean blueAlliance; -private boolean redBare; +private boolean redBarge; private Alliance alliance; private double driveRadius = 1.5; //TODO This is a magic number that needs creating private BargeAlignStates curState; public BargeAlign( - DoubleSupplier strStick, - DriveSubsystem driveSubsystem){ + DoubleSupplier strStick, + DriveSubsystem driveSubsystem){ + if(alliance == Alliance.Blue){ blueAlliance = true; - }else{ + } + else{ blueAlliance = false; } + curState = BargeAlignStates.DRIVE; } + public void setState(BargeAlignStates curState){ this.curState = curState; } -private boolean getOnBlueSide(){ + +private boolean getOnBlueSide() { return driveSubsystem.getPoseMeters().getMeasureX() .compareTo(BargeAlignConstants.blueBargePos.getMeasureX()) <= 0; } +private boolean getOnRedSide() { + return + driveSubsystem.getPoseMeters().getMeasureX() + .compareTo(BargeAlignConstants.redBargePos.getMeasureX()) <= 0; +} + +private double getYStickReading() { + return xboxController.XkY.value); +} //same joystick reading as the drive + private Translation2d getTargetTranslation(){ Translation2d offset = new Translation2d( @@ -60,7 +79,7 @@ public void periodic(){ case DRIVE: break; - case DONE: + case FINISHED: break; } } @@ -71,8 +90,9 @@ public Set getMeasures(){ } public enum BargeAlignStates { + //INIT, DRIVE, - DONE + FINISHED } } From 79cc0eca8464bf82452d14bae526331c854c2f85 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Thu, 27 Mar 2025 21:08:52 -0400 Subject: [PATCH 04/12] continued 4piece --- src/main/deploy/choreo/2025-project.chor | 4 +- src/main/deploy/choreo/JTofetch.traj | 198 ++++++++------- src/main/deploy/choreo/KTofetch.traj | 99 ++++---- src/main/deploy/choreo/LTofetch.traj | 97 ++++--- src/main/deploy/choreo/fetchToK.traj | 2 +- src/main/deploy/choreo/fetchToL.traj | 103 ++++---- src/main/deploy/choreo/startToG.traj | 2 +- src/main/deploy/choreo/startToH.traj | 2 +- src/main/deploy/choreo/startToI.traj | 2 +- src/main/deploy/choreo/startToJ.traj | 121 ++++----- src/main/deploy/choreo/startToJSlow.traj | 2 +- src/main/deploy/choreo/startToK.traj | 2 +- src/main/deploy/choreo/startToL.traj | 238 +++++++++--------- .../auton/NonProcessorDeepAutonCommand.java | 4 +- .../NonProcessorShallowAutonCommand.java | 4 +- .../NonProcessorShallowSlowAutonCommand.java | 4 +- .../auton/ProcessorShallowAutonCommand.java | 4 +- .../ProcessorShallowSlowAutonCommand.java | 4 +- .../pathHandler/SetPathHandlerCommand.java | 5 +- .../pathHandler/StartPathHandlerCommand.java | 3 +- .../frc/robot/constants/CoralConstants.java | 4 +- .../robot/constants/TagServoingConstants.java | 2 +- .../frc/robot/subsystems/auto/AutoSwitch.java | 17 +- .../robot/subsystems/led/LEDSubsystem.java | 96 +++---- .../subsystems/pathHandler/PathHandler.java | 34 ++- .../robotState/RobotStateSubsystem.java | 6 +- .../tagAlign/TagAlignSubsystem.java | 5 +- 27 files changed, 553 insertions(+), 511 deletions(-) diff --git a/src/main/deploy/choreo/2025-project.chor b/src/main/deploy/choreo/2025-project.chor index 9a010fe7..0e1713d2 100644 --- a/src/main/deploy/choreo/2025-project.chor +++ b/src/main/deploy/choreo/2025-project.chor @@ -250,8 +250,8 @@ "val":7.1008875 }, "y":{ - "exp":"5.0756788 m", - "val":5.0756788 + "exp":"6.1663548 m", + "val":6.1663548 }, "heading":{ "exp":"180 deg", diff --git a/src/main/deploy/choreo/JTofetch.traj b/src/main/deploy/choreo/JTofetch.traj index 2921f950..d1a89168 100644 --- a/src/main/deploy/choreo/JTofetch.traj +++ b/src/main/deploy/choreo/JTofetch.traj @@ -3,34 +3,34 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":5.1366723649053885, "y":5.197141990263579, "heading":4.1887902047863905, "intervals":86, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":4.392356872558594, "y":5.746496677398682, "heading":0.0, "intervals":63, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":3.3291568756103516, "y":6.2088751792907715, "heading":-1.4994887877871588, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.949252247810364, "y":6.8849358558654785, "heading":-1.081580366464561, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.1366723649053885, "y":5.197141990263579, "heading":4.1887902047863905, "intervals":87, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.392356872558594, "y":5.746496677398682, "heading":0.0, "intervals":64, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.3291568756103516, "y":6.2088751792907715, "heading":-1.4994887877871588, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.949252247810364, "y":6.8849358558654785, "heading":-1.081580366464561, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.69577419757843, "y":7.228199481964111, "heading":-0.942477796076938, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":0, "to":4, "data":{"type":"MaxVelocity", "props":{"max":3.7}}, "enabled":true}, - {"from":3, "to":4, "data":{"type":"MaxAcceleration", "props":{"max":4.5}}, "enabled":true}, + {"from":0, "to":4, "data":{"type":"MaxVelocity", "props":{"max":3.3}}, "enabled":true}, + {"from":0, "to":4, "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":true}, {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":1.0}}, "enabled":true}, {"from":1, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":1.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"J.x", "val":5.1366723649053885}, "y":{"exp":"J.y", "val":5.197141990263579}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":86, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"4.392356872558594 m", "val":4.392356872558594}, "y":{"exp":"5.746496677398682 m", "val":5.746496677398682}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":63, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"3.3291568756103516 m", "val":3.3291568756103516}, "y":{"exp":"6.2088751792907715 m", "val":6.2088751792907715}, "heading":{"exp":"-1.4994887877871588 rad", "val":-1.4994887877871588}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.9492522478103638 m", "val":1.949252247810364}, "y":{"exp":"6.8849358558654785 m", "val":6.8849358558654785}, "heading":{"exp":"-1.081580366464561 rad", "val":-1.081580366464561}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"J.x", "val":5.1366723649053885}, "y":{"exp":"J.y", "val":5.197141990263579}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":87, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.392356872558594 m", "val":4.392356872558594}, "y":{"exp":"5.746496677398682 m", "val":5.746496677398682}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":64, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"3.3291568756103516 m", "val":3.3291568756103516}, "y":{"exp":"6.2088751792907715 m", "val":6.2088751792907715}, "heading":{"exp":"-1.4994887877871588 rad", "val":-1.4994887877871588}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.9492522478103638 m", "val":1.949252247810364}, "y":{"exp":"6.8849358558654785 m", "val":6.8849358558654785}, "heading":{"exp":"-1.081580366464561 rad", "val":-1.081580366464561}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"fetch.x", "val":1.69577419757843}, "y":{"exp":"fetch.y", "val":7.228199481964111}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":0, "to":4, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.7 m / s", "val":3.7}}}, "enabled":true}, - {"from":3, "to":4, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"maxAccel + 1.5 m / s ^ 2", "val":4.5}}}, "enabled":true}, + {"from":0, "to":4, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.3 m / s", "val":3.3}}}, "enabled":true}, + {"from":0, "to":4, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":true}, {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"1 rad / s", "val":1.0}}}, "enabled":true}, {"from":1, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"1 rad / s", "val":1.0}}}, "enabled":true}], "targetDt":{ @@ -40,87 +40,101 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.41193,0.74905,1.23201,1.66838], + "waypoints":[0.0,0.55586,0.90819,1.4167,1.79691], "samples":[ - {"t":0.0, "x":5.13667, "y":5.19714, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-8.68224, "ay":6.58993, "alpha":17.80239, "fx":[-69.00042,-126.56109,-186.95451,-196.39925], "fy":[186.53501,153.71129,68.46368,30.6937]}, - {"t":0.02289, "x":5.1344, "y":5.19887, "heading":-2.0944, "vx":-0.1987, "vy":0.15081, "omega":0.40741, "ax":-8.98362, "ay":6.91521, "alpha":13.30824, "fx":[-94.15496,-131.81128,-183.21366,-189.8304], "fy":[175.15225,149.17103,77.80127,58.96811]}, - {"t":0.04577, "x":5.1275, "y":5.20413, "heading":-2.08507, "vx":-0.40429, "vy":0.30907, "omega":0.71197, "ax":-9.22988, "ay":7.15574, "alpha":8.53684, "fx":[-119.62547,-139.14453,-177.43714,-179.2238], "fy":[158.82086,142.26454,90.0575,85.98778]}, - {"t":0.06866, "x":5.11583, "y":5.21308, "heading":-2.06878, "vx":-0.61552, "vy":0.47283, "omega":0.90734, "ax":-9.38527, "ay":7.28932, "alpha":3.59135, "fx":[-143.07351,-148.81262,-167.71532,-166.19007], "fy":[138.05351,131.97649,106.91128,109.09671]}, - {"t":0.09154, "x":5.09929, "y":5.22581, "heading":-2.04801, "vx":-0.8303, "vy":0.63965, "omega":0.98953, "ax":-9.41913, "ay":7.31368, "alpha":0.25727, "fx":[-156.11144,-156.40748,-157.90204,-157.62834], "fy":[123.06861,122.70391,120.77238,121.1174]}, - {"t":0.11443, "x":5.07782, "y":5.24236, "heading":-2.02537, "vx":-1.04586, "vy":0.80702, "omega":0.99542, "ax":-9.41695, "ay":7.30874, "alpha":0.00756, "fx":[-156.94941,-156.95879,-157.00279,-156.99343], "fy":[121.86739,121.85569,121.79891,121.81059]}, - {"t":0.13731, "x":5.05142, "y":5.26274, "heading":-2.00259, "vx":-1.26137, "vy":0.97428, "omega":0.99559, "ax":-9.4138, "ay":7.30202, "alpha":0.00016, "fx":[-156.92288,-156.9231,-156.92405,-156.92382], "fy":[121.7219,121.72162,121.7204,121.72068]}, - {"t":0.1602, "x":5.02009, "y":5.28695, "heading":-1.9798, "vx":-1.4768, "vy":1.14139, "omega":0.9956, "ax":-9.40935, "ay":7.29274, "alpha":-0.00004, "fx":[-156.8495,-156.84945,-156.84924,-156.84929], "fy":[121.56637,121.56644,121.56671,121.56665]}, - {"t":0.18308, "x":4.98382, "y":5.31498, "heading":-1.95702, "vx":-1.69214, "vy":1.30829, "omega":0.99559, "ax":-9.40263, "ay":7.27912, "alpha":-0.00007, "fx":[-156.73765,-156.73753,-156.73714,-156.73725], "fy":[121.33912,121.33926,121.33978,121.33963]}, - {"t":0.20597, "x":4.94264, "y":5.34683, "heading":-1.93423, "vx":-1.90732, "vy":1.47487, "omega":0.99559, "ax":-9.39131, "ay":7.25718, "alpha":-0.00013, "fx":[-156.54917,-156.54893,-156.54818,-156.54841], "fy":[120.9731,120.97339,120.97438,120.97408]}, - {"t":0.22885, "x":4.89653, "y":5.38248, "heading":-1.91145, "vx":-2.12224, "vy":1.64096, "omega":0.99559, "ax":-9.36836, "ay":7.21597, "alpha":-0.00034, "fx":[-156.16737,-156.16672,-156.16483,-156.16548], "fy":[120.2851,120.28589,120.28836,120.28758]}, - {"t":0.25174, "x":4.84551, "y":5.42193, "heading":-1.88867, "vx":-2.33664, "vy":1.80609, "omega":0.99558, "ax":-9.30474, "ay":7.10276, "alpha":-0.00549, "fx":[-155.1259,-155.11487,-155.08501,-155.09608], "fy":[118.37334,118.38616,118.42584,118.41296]}, - {"t":0.27462, "x":4.7896, "y":5.46512, "heading":-1.86588, "vx":-2.54958, "vy":1.96864, "omega":0.99546, "ax":-8.27999, "ay":5.15171, "alpha":-7.03707, "fx":[-158.92792,-139.01946,-116.23307,-137.91348], "fy":[64.47479,62.46076,111.34365,105.22671]}, - {"t":0.29751, "x":4.72908, "y":5.51152, "heading":-1.8431, "vx":-2.73907, "vy":2.08654, "omega":0.83441, "ax":-5.02464, "ay":2.32388, "alpha":-13.44338, "fx":[-122.54638,-63.81315,-44.64993,-104.02386], "fy":[14.74617,-4.2407,64.96822,79.47786]}, - {"t":0.32039, "x":4.66508, "y":5.55988, "heading":-1.824, "vx":-2.85406, "vy":2.13972, "omega":0.52676, "ax":-2.81629, "ay":-0.17387, "alpha":-5.32988, "fx":[-62.14231,-37.5167,-31.58,-56.54567], "fy":[-11.85827,-19.37022,6.19605,13.43907]}, - {"t":0.34328, "x":4.59903, "y":5.6088, "heading":-1.81195, "vx":-2.91851, "vy":2.13574, "omega":0.40478, "ax":-2.21157, "ay":-2.38256, "alpha":-0.93805, "fx":[-39.48787,-34.92945,-34.21551,-38.83031], "fy":[-41.10603,-42.61114,-38.32498,-36.82267]}, - {"t":0.36616, "x":4.53166, "y":5.65706, "heading":-1.80269, "vx":-2.96912, "vy":2.08122, "omega":0.38331, "ax":-2.84985, "ay":-4.86485, "alpha":1.09287, "fx":[-44.23377,-49.3469,-50.75356,-45.68805], "fy":[-78.93671,-77.97002,-83.25129,-84.22087]}, - {"t":0.38905, "x":4.46296, "y":5.70341, "heading":-1.79391, "vx":-3.03434, "vy":1.96989, "omega":0.40832, "ax":-4.43873, "ay":-7.62043, "alpha":0.79232, "fx":[-71.07088,-74.36547,-76.30137,-74.22807], "fy":[-124.27326,-124.9462,-129.93153,-128.96488]}, - {"t":0.41193, "x":4.39236, "y":5.7465, "heading":-1.78457, "vx":-3.13593, "vy":1.79549, "omega":0.42646, "ax":-0.96346, "ay":-8.33718, "alpha":8.56284, "fx":[12.82375,-38.08947,-42.57159,3.5959], "fy":[-127.64185,-117.06781,-151.29784,-159.89974]}, - {"t":0.42879, "x":4.33936, "y":5.77558, "heading":-1.77738, "vx":-3.15217, "vy":1.65496, "omega":0.57079, "ax":-0.55056, "ay":-6.58601, "alpha":6.823, "fx":[11.5446,-23.94371,-29.34472,5.03335], "fy":[-98.12791,-91.13859,-121.65239,-128.22351]}, - {"t":0.44565, "x":4.28615, "y":5.80254, "heading":-1.76776, "vx":-3.16145, "vy":1.54395, "omega":0.68579, "ax":-0.24433, "ay":-4.91617, "alpha":5.15033, "fx":[10.95448,-14.68687,-18.92296,6.364], "fy":[-72.37971,-67.64061,-91.5799,-96.20048]}, - {"t":0.4625, "x":4.23283, "y":5.82786, "heading":-1.7562, "vx":-3.16556, "vy":1.46109, "omega":0.77261, "ax":-0.06146, "ay":-3.50996, "alpha":3.68335, "fx":[9.48496,-8.55189,-11.47052,6.43929], "fy":[-51.34954,-48.27023,-65.68886,-68.72862]}, - {"t":0.47936, "x":4.17946, "y":5.85199, "heading":-1.74318, "vx":-3.1666, "vy":1.40192, "omega":0.83469, "ax":0.02494, "ay":-2.38286, "alpha":2.43942, "fx":[7.25559,-4.61582,-6.40069,5.42401], "fy":[-34.81777,-32.98515,-44.63139,-46.44999]}, - {"t":0.49621, "x":4.12609, "y":5.87528, "heading":-1.72911, "vx":-3.16618, "vy":1.36176, "omega":0.87581, "ax":0.05768, "ay":-1.51246, "alpha":1.44728, "fx":[4.94933,-2.08377,-3.01779,3.99836], "fy":[-22.20963,-21.26364,-28.217,-29.15794]}, - {"t":0.51307, "x":4.07273, "y":5.89802, "heading":-1.71435, "vx":-3.16521, "vy":1.33627, "omega":0.9002, "ax":0.07577, "ay":-0.86676, "alpha":0.72967, "fx":[3.22973,-0.3216,-0.70065,2.84456], "fy":[-12.87722,-12.4966,-16.02081,-16.39955]}, - {"t":0.52992, "x":4.01939, "y":5.92042, "heading":-1.69917, "vx":-3.16393, "vy":1.32166, "omega":0.9125, "ax":0.10068, "ay":-0.40904, "alpha":0.27143, "fx":[2.37954,1.05072,0.97815,2.30466], "fy":[-6.19449,-6.12286,-7.44285,-7.51372]}, - {"t":0.54678, "x":3.96608, "y":5.94264, "heading":-1.68379, "vx":-3.16223, "vy":1.31476, "omega":0.91708, "ax":0.13506, "ay":-0.10089, "alpha":0.01853, "fx":[2.27002,2.17035,2.23334,2.33206], "fy":[-1.60118,-1.66543,-1.76243,-1.69785]}, - {"t":0.56363, "x":3.91279, "y":5.96479, "heading":-1.66833, "vx":-3.15996, "vy":1.31306, "omega":0.91739, "ax":0.17165, "ay":0.09275, "alpha":-0.09564, "fx":[2.58208,3.03976,3.14068,2.68256], "fy":[1.3679,1.26593,1.72422,1.82635]}, - {"t":0.58049, "x":3.85956, "y":5.98693, "heading":-1.65287, "vx":-3.15706, "vy":1.31462, "omega":0.91578, "ax":0.20195, "ay":0.19974, "alpha":-0.12716, "fx":[3.01331,3.62704,3.71977,3.10582], "fy":[3.06956,2.97601,3.58967,3.68329]}, - {"t":0.59735, "x":3.80637, "y":6.00912, "heading":-1.63743, "vx":-3.15366, "vy":1.31799, "omega":0.91363, "ax":0.21977, "ay":0.23976, "alpha":-0.11453, "fx":[3.352,3.9066,3.97502,3.4203], "fy":[3.75411,3.68506,4.23938,4.30847]}, - {"t":0.6142, "x":3.75324, "y":6.03137, "heading":-1.62203, "vx":-3.14995, "vy":1.32203, "omega":0.9117, "ax":0.22092, "ay":0.22498, "alpha":-0.08159, "fx":[3.46306,3.85888,3.90208,3.50618], "fy":[3.5743,3.53063,3.92615,3.96983]}, - {"t":0.63106, "x":3.70018, "y":6.05369, "heading":-1.60667, "vx":-3.14623, "vy":1.32582, "omega":0.91033, "ax":0.20176, "ay":0.16116, "alpha":-0.04313, "fx":[3.2467,3.45616,3.4799,3.2704], "fy":[2.59386,2.56976,2.77893,2.80304]}, - {"t":0.64791, "x":3.64718, "y":6.07606, "heading":-1.59132, "vx":-3.14283, "vy":1.32854, "omega":0.9096, "ax":0.15765, "ay":0.04866, "alpha":-0.01022, "fx":[2.59699,2.64666,2.65876,2.60907], "fy":[0.79262,0.78025,0.82965,0.84202]}, - {"t":0.66477, "x":3.59423, "y":6.09846, "heading":-1.57599, "vx":-3.14017, "vy":1.32936, "omega":0.90943, "ax":0.08133, "ay":-0.11695, "alpha":0.00515, "fx":[1.3646,1.33963,1.34672,1.37167], "fy":[-1.93315,-1.94046,-1.9657,-1.95839]}, - {"t":0.68162, "x":3.54131, "y":6.12085, "heading":-1.56066, "vx":-3.1388, "vy":1.32739, "omega":0.90952, "ax":-0.03857, "ay":-0.34452, "alpha":-0.01438, "fx":[-0.67996,-0.60972,-0.60596,-0.67622], "fy":[-5.77592,-5.77991,-5.70997,-5.70599]}, - {"t":0.69848, "x":3.4884, "y":6.14317, "heading":-1.54533, "vx":-3.13945, "vy":1.32158, "omega":0.90927, "ax":-0.21931, "ay":-0.64724, "alpha":-0.09651, "fx":[-3.88612,-3.41589,-3.42531,-3.89559], "fy":[-11.02845,-11.01953,-10.54979,-10.55873]}, - {"t":0.71534, "x":3.43545, "y":6.16536, "heading":-1.53001, "vx":-3.14315, "vy":1.31067, "omega":0.90765, "ax":-0.48591, "ay":-1.04279, "alpha":-0.28515, "fx":[-8.76608,-7.37771,-7.43361,-8.82218], "fy":[-18.10325,-18.04944,-16.66235,-16.71633]}, - {"t":0.73219, "x":3.3824, "y":6.1873, "heading":-1.51471, "vx":-3.15134, "vy":1.2931, "omega":0.90284, "ax":-0.87541, "ay":-1.55292, "alpha":-0.62521, "fx":[-16.02679,-12.98385,-13.15816,-16.20181], "fy":[-27.48737,-27.32291,-24.28487,-24.4505]}, - {"t":0.74905, "x":3.32916, "y":6.20888, "heading":-1.49949, "vx":-3.16609, "vy":1.26692, "omega":0.8923, "ax":-1.47642, "ay":-1.5881, "alpha":-1.9681, "fx":[-29.02476,-19.50706,-20.1782,-29.73515], "fy":[-31.59696,-30.93412,-21.33633,-22.02408]}, - {"t":0.76917, "x":3.26514, "y":6.23405, "heading":-1.48153, "vx":-3.19581, "vy":1.23496, "omega":0.8527, "ax":-0.87339, "ay":-0.95551, "alpha":-1.2622, "fx":[-17.34285,-11.22129,-11.77404,-17.89796], "fy":[-19.25963,-18.71983,-12.59489,-13.13755]}, - {"t":0.78929, "x":3.20066, "y":6.25871, "heading":-1.46437, "vx":-3.21338, "vy":1.21573, "omega":0.8273, "ax":-0.46709, "ay":-0.54387, "alpha":-0.73653, "fx":[-9.37878,-5.8118,-6.19318,-9.76077], "fy":[-11.03839,-10.66055,-7.09323,-7.47183]}, - {"t":0.80942, "x":3.1359, "y":6.28306, "heading":-1.44773, "vx":-3.22278, "vy":1.20479, "omega":0.81248, "ax":-0.21106, "ay":-0.28299, "alpha":-0.3951, "fx":[-4.35605,-2.44603,-2.68038,-4.59056], "fy":[-5.78903,-5.55552,-3.6454,-3.87913]}, - {"t":0.82954, "x":3.071, "y":6.30725, "heading":-1.43138, "vx":-3.22703, "vy":1.19909, "omega":0.80452, "ax":-0.05869, "ay":-0.12299, "alpha":-0.19005, "fx":[-1.37448,-0.45732,-0.58222,-1.49941], "fy":[-2.57111,-2.44639,-1.52914,-1.65395]}, - {"t":0.84966, "x":3.00605, "y":6.33135, "heading":-1.41519, "vx":-3.22821, "vy":1.19662, "omega":0.8007, "ax":0.0249, "ay":-0.02488, "alpha":-0.08183, "fx":[0.24592,0.6406,0.58411,0.18948], "fy":[-0.64019,-0.58388,-0.18931,-0.24565]}, - {"t":0.86979, "x":2.94109, "y":6.35543, "heading":-1.39907, "vx":-3.22771, "vy":1.19612, "omega":0.79905, "ax":0.06574, "ay":0.04464, "alpha":-0.04265, "fx":[1.0074,1.21352,1.18416,0.97818], "fy":[0.62699,0.6558,0.86132,0.83254]}, - {"t":0.88991, "x":2.87615, "y":6.37951, "heading":-1.38299, "vx":-3.22639, "vy":1.19702, "omega":0.7982, "ax":0.08515, "ay":0.1179, "alpha":-0.05337, "fx":[1.31182,1.56926,1.52673,1.26954], "fy":[1.81672,1.85795,2.11386,2.07274]}, - {"t":0.91003, "x":2.81124, "y":6.40362, "heading":-1.36693, "vx":-3.22467, "vy":1.19939, "omega":0.79712, "ax":0.10408, "ay":0.23079, "alpha":-0.09868, "fx":[1.54433,2.01791,1.92537,1.45222], "fy":[3.56682,3.65685,4.12729,4.03751]}, - {"t":0.93016, "x":2.74637, "y":6.4278, "heading":-1.35089, "vx":-3.22258, "vy":1.20403, "omega":0.79514, "ax":0.14797, "ay":0.42733, "alpha":-0.16114, "fx":[2.16573,2.9356,2.76721,1.99803], "fy":[6.65888,6.82338,7.58773,7.42374]}, - {"t":0.95028, "x":2.68155, "y":6.45212, "heading":-1.33489, "vx":-3.2196, "vy":1.21263, "omega":0.79189, "ax":0.25272, "ay":0.76388, "alpha":-0.21359, "fx":[3.82488,4.84108,4.6,3.58487], "fy":[12.10973,12.34822,13.35689,13.11939]}, - {"t":0.97041, "x":2.61681, "y":6.47668, "heading":-1.31895, "vx":-3.21451, "vy":1.22801, "omega":0.78759, "ax":0.47279, "ay":1.31165, "alpha":-0.20897, "fx":[7.50828,8.49633,8.25342,7.26701], "fy":[21.24197,21.50133,22.48628,22.22884]}, - {"t":0.99053, "x":2.55222, "y":6.50165, "heading":-1.3031, "vx":-3.205, "vy":1.2544, "omega":0.78339, "ax":0.89293, "ay":2.15398, "alpha":-0.06543, "fx":[14.74652,15.04213,15.02119,14.72897], "fy":[35.67304,35.81525,36.13602,35.99911]}, - {"t":1.01065, "x":2.48791, "y":6.52733, "heading":-1.28734, "vx":-3.18703, "vy":1.29775, "omega":0.78207, "ax":1.64456, "ay":3.36852, "alpha":0.35854, "fx":[27.8793,26.16029,26.94362,28.67279], "fy":[57.06246,56.86285,55.22829,55.45269]}, - {"t":1.03078, "x":2.42411, "y":6.55413, "heading":-1.2716, "vx":-3.15394, "vy":1.36553, "omega":0.78929, "ax":2.92081, "ay":4.96312, "alpha":1.31263, "fx":[50.20205,44.12622,47.17277,53.25308], "fy":[86.15311,85.43024,79.24194,80.10616]}, - {"t":1.0509, "x":2.36123, "y":6.58261, "heading":-1.25572, "vx":-3.09516, "vy":1.46541, "omega":0.8157, "ax":4.88878, "ay":6.64425, "alpha":3.18073, "fx":[83.98228,70.3246,78.98061,92.68696], "fy":[118.96834,118.20959,102.04052,103.8072]}, - {"t":1.07102, "x":2.29993, "y":6.61345, "heading":-1.2393, "vx":-2.99678, "vy":1.59911, "omega":0.87971, "ax":6.81528, "ay":7.47413, "alpha":5.22883, "fx":[111.73508,93.28838,115.73649,133.6698], "fy":[135.96093,142.58509,110.45032,109.36428]}, - {"t":1.09115, "x":2.24101, "y":6.64714, "heading":-1.2216, "vx":-2.85963, "vy":1.74952, "omega":0.98493, "ax":10.44812, "ay":4.02414, "alpha":0.52854, "fx":[173.41031,173.19725,174.93675,175.11601], "fy":[69.50815,69.16216,64.59652,65.05509]}, - {"t":1.11127, "x":2.18558, "y":6.68316, "heading":-1.20178, "vx":-2.64938, "vy":1.8305, "omega":0.99557, "ax":11.60237, "ay":-1.42063, "alpha":0.00279, "fx":[193.40824,193.40619,193.40362,193.40568], "fy":[-23.66345,-23.67568,-23.69909,-23.68683]}, - {"t":1.13139, "x":2.13461, "y":6.71971, "heading":-1.18174, "vx":-2.4159, "vy":1.80191, "omega":0.99563, "ax":11.48472, "ay":-2.71633, "alpha":0.00045, "fx":[191.44546,191.44483,191.44405,191.44468], "fy":[-45.27697,-45.27941,-45.28286,-45.28042]}, - {"t":1.15152, "x":2.08832, "y":6.75542, "heading":-1.16171, "vx":-2.18478, "vy":1.74725, "omega":0.99563, "ax":11.39975, "ay":-3.23067, "alpha":0.00018, "fx":[190.02872,190.02841,190.02806,190.02837], "fy":[-53.85251,-53.85355,-53.85483,-53.85379]}, - {"t":1.17164, "x":2.04666, "y":6.78993, "heading":-1.14167, "vx":-1.95538, "vy":1.68224, "omega":0.99564, "ax":11.345, "ay":-3.50608, "alpha":-0.00052, "fx":[189.11458,189.11559,189.11668,189.11566], "fy":[-58.44811,-58.44491,-58.44131,-58.44451]}, - {"t":1.19176, "x":2.00961, "y":6.82307, "heading":-1.12164, "vx":-1.72708, "vy":1.61168, "omega":0.99563, "ax":11.30743, "ay":-3.67796, "alpha":-0.0334, "fx":[188.41867,188.48927,188.56018,188.48984], "fy":[-61.5273,-61.31512,-61.09243,-61.30467]}, - {"t":1.21189, "x":1.97715, "y":6.85476, "heading":-1.1016, "vx":-1.49953, "vy":1.53767, "omega":0.99496, "ax":11.27422, "ay":-3.7935, "alpha":-1.33122, "fx":[184.9299,188.09957,190.74809,187.96604], "fy":[-71.84708,-63.21583,-54.55209,-63.32798]}, - {"t":1.23201, "x":1.94925, "y":6.88494, "heading":-1.08158, "vx":-1.27266, "vy":1.46133, "omega":0.96817, "ax":4.06353, "ay":-1.89095, "alpha":-6.25566, "fx":[60.71065,86.15103,75.47,48.61663], "fy":[-54.41434,-35.90502,-9.20285,-26.56253]}, - {"t":1.25928, "x":1.91605, "y":6.92409, "heading":-1.05518, "vx":-1.16183, "vy":1.40976, "omega":0.79755, "ax":3.73752, "ay":-2.49316, "alpha":-5.10662, "fx":[56.59425,77.50042,68.79133,46.32468], "fy":[-59.89066,-44.78199,-23.2343,-38.33216]}, - {"t":1.28656, "x":1.88576, "y":6.96161, "heading":-1.03342, "vx":-1.0599, "vy":1.34176, "omega":0.65828, "ax":3.46894, "ay":-2.85706, "alpha":-4.17658, "fx":[53.06887,70.71669,63.04109,44.47573], "fy":[-62.13188,-50.19194,-32.75233,-45.42732]}, - {"t":1.31383, "x":1.85814, "y":6.99714, "heading":-1.01547, "vx":-0.96529, "vy":1.26384, "omega":0.54437, "ax":3.26104, "ay":-3.09359, "alpha":-3.49652, "fx":[50.55794,65.39387,58.62852,42.85948], "fy":[-63.7264,-53.06302,-39.37011,-50.11509]}, - {"t":1.3411, "x":1.83303, "y":7.03046, "heading":-1.00062, "vx":-0.87635, "vy":1.17947, "omega":0.44901, "ax":3.09921, "ay":-3.25665, "alpha":-2.88742, "fx":[48.51019,61.00224,55.05126,42.08539], "fy":[-64.08359,-55.62473,-44.24557,-53.19354]}, - {"t":1.36838, "x":1.81028, "y":7.06142, "heading":-0.98838, "vx":-0.79182, "vy":1.09065, "omega":0.37026, "ax":2.97109, "ay":-3.37465, "alpha":-2.44331, "fx":[46.95238,57.5251,52.31651,41.31269], "fy":[-64.61623,-57.03051,-47.88715,-55.4812]}, - {"t":1.39565, "x":1.78979, "y":7.08991, "heading":-0.97828, "vx":-0.71079, "vy":0.99861, "omega":0.30363, "ax":2.86778, "ay":-3.46341, "alpha":-2.03961, "fx":[45.67188,54.59822,50.06108,40.88706], "fy":[-64.56072,-58.46795,-50.75887,-57.14615]}, - {"t":1.42292, "x":1.77147, "y":7.11585, "heading":-0.97, "vx":-0.63258, "vy":0.90415, "omega":0.248, "ax":2.78301, "ay":-3.53232, "alpha":-1.73181, "fx":[44.61239,52.19911,48.2655,40.48894], "fy":[-64.74032,-59.30656,-53.02092,-58.4605]}, - {"t":1.4502, "x":1.75525, "y":7.1392, "heading":-0.96323, "vx":-0.55668, "vy":0.80782, "omega":0.20077, "ax":2.71236, "ay":-3.58721, "alpha":-1.45721, "fx":[43.74103,50.19273,46.76206,40.15915], "fy":[-64.60185,-60.16405,-54.89724,-59.52485]}, - {"t":1.47747, "x":1.74108, "y":7.1599, "heading":-0.95776, "vx":-0.4827, "vy":0.70998, "omega":0.16102, "ax":2.65266, "ay":-3.63186, "alpha":-1.24691, "fx":[42.92975,48.47559,45.5481,39.92071], "fy":[-64.71817,-60.74642,-56.36443,-60.33675]}, - {"t":1.50474, "x":1.7289, "y":7.17791, "heading":-0.95337, "vx":-0.41036, "vy":0.61093, "omega":0.12702, "ax":2.60159, "ay":-3.66886, "alpha":-1.06107, "fx":[42.2643,47.02699,44.51738,39.66065], "fy":[-64.64457,-61.33575,-57.6183,-61.03377]}, - {"t":1.53202, "x":1.71868, "y":7.19321, "heading":-0.9499, "vx":-0.3394, "vy":0.51087, "omega":0.09808, "ax":2.55745, "ay":-3.69997, "alpha":-0.90839, "fx":[41.63183,45.78511,43.62598,39.48316], "fy":[-64.68789,-61.74414,-58.65666,-61.6181]}, - {"t":1.55929, "x":1.71037, "y":7.20576, "heading":-0.94723, "vx":-0.26965, "vy":0.40996, "omega":0.0733, "ax":2.51894, "ay":-3.72648, "alpha":-0.79003, "fx":[41.1128,44.75654,42.90276,39.18569], "fy":[-64.6847,-62.13714,-59.5212,-62.13118]}, - {"t":1.58656, "x":1.70395, "y":7.21556, "heading":-0.94523, "vx":-0.20096, "vy":0.30833, "omega":0.05176, "ax":2.48505, "ay":-3.74931, "alpha":-0.68503, "fx":[40.58631,43.83284,42.25127,39.02764], "fy":[-64.73385,-62.47158,-60.23497,-62.5567]}, - {"t":1.61384, "x":1.6994, "y":7.22257, "heading":-0.94382, "vx":-0.13318, "vy":0.20607, "omega":0.03307, "ax":2.455, "ay":-3.76919, "alpha":-0.62405, "fx":[40.15109,43.1473,41.73475,38.66175], "fy":[-64.82233,-62.72428,-60.81766,-62.95807]}, - {"t":1.64111, "x":1.69668, "y":7.22679, "heading":-0.94292, "vx":-0.06622, "vy":0.10327, "omega":0.01605, "ax":2.4282, "ay":-3.78663, "alpha":-0.58865, "fx":[39.68056,42.68152,41.25576,38.2897], "fy":[-64.91873,-62.96731,-61.29653,-63.3029]}, - {"t":1.66838, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":5.13667, "y":5.19714, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.74303, "ay":3.66641, "alpha":21.07046, "fx":[-1.83931,-86.47711,-141.61168,-86.32828], "fy":[108.57122,120.38114,36.52301,-21.00595]}, + {"t":0.02417, "x":5.13529, "y":5.19821, "heading":-2.0944, "vx":-0.11463, "vy":0.08861, "omega":0.50923, "ax":-4.74907, "ay":3.66257, "alpha":9.0189, "fx":[-47.26201,-81.36097,-108.18881,-79.84704], "fy":[79.2051,88.28514,47.13965,29.58303]}, + {"t":0.04834, "x":5.13113, "y":5.20142, "heading":-2.08209, "vx":-0.2294, "vy":0.17713, "omega":0.72719, "ax":-4.75351, "ay":3.65659, "alpha":2.32297, "fx":[-71.28405,-79.60753,-86.96156,-79.1015], "fy":[65.09326,68.41745,57.04606,53.25792]}, + {"t":0.0725, "x":5.1242, "y":5.20677, "heading":-2.06451, "vx":-0.34429, "vy":0.2655, "omega":0.78333, "ax":-4.75841, "ay":3.64997, "alpha":0.49169, "fx":[-77.67452,-79.4151,-80.93977,-79.25238], "fy":[61.741,62.43598,59.9936,59.20255]}, + {"t":0.09667, "x":5.11449, "y":5.21425, "heading":-2.04558, "vx":-0.45929, "vy":0.35371, "omega":0.79522, "ax":-4.7638, "ay":3.64267, "alpha":0.1226, "fx":[-78.33745,-79.52676,-79.85803,-79.91901], "fy":[62.05938,60.33404,59.81879,60.6741]}, + {"t":0.12084, "x":5.102, "y":5.22387, "heading":-2.02636, "vx":-0.57442, "vy":0.44174, "omega":0.79818, "ax":-4.76983, "ay":3.63448, "alpha":0.00166, "fx":[-79.5164,-79.50687,-79.49612,-79.52381], "fy":[60.58049,60.61604,60.5767,60.56704]}, + {"t":0.14501, "x":5.08672, "y":5.2356, "heading":-2.00707, "vx":-0.68969, "vy":0.52958, "omega":0.79822, "ax":-4.77654, "ay":3.62533, "alpha":-0.01105, "fx":[-79.53098,-79.557,-79.48931,-79.91329], "fy":[60.60928,60.31614,60.4342,60.37051]}, + {"t":0.16917, "x":5.06866, "y":5.24946, "heading":-1.98778, "vx":-0.80513, "vy":0.6172, "omega":0.79795, "ax":-4.78412, "ay":3.61495, "alpha":-0.02086, "fx":[-79.8671,-79.7027,-79.65368,-79.77254], "fy":[60.13793,60.30396,60.2789,60.31715]}, + {"t":0.19334, "x":5.0478, "y":5.26543, "heading":-1.9685, "vx":-0.92075, "vy":0.70456, "omega":0.79745, "ax":-4.7927, "ay":3.60315, "alpha":-0.01408, "fx":[-79.74406,-79.79615,-79.67545,-80.35214], "fy":[60.42101,59.8508,60.07227,59.90702]}, + {"t":0.21751, "x":5.02415, "y":5.28351, "heading":-1.94922, "vx":-1.03658, "vy":0.79164, "omega":0.79711, "ax":-4.80252, "ay":3.58955, "alpha":-0.02265, "fx":[-80.24602,-79.94013,-79.93821,-80.09863], "fy":[59.59868,60.02264,59.84154,59.88168]}, + {"t":0.24168, "x":4.9977, "y":5.30369, "heading":-1.92996, "vx":-1.15265, "vy":0.8784, "omega":0.79656, "ax":-4.81386, "ay":3.57376, "alpha":-0.01247, "fx":[-80.05479,-80.11254,-79.9759,-80.8358], "fy":[60.04754,59.33105,59.59553,59.31761]}, + {"t":0.26585, "x":4.96843, "y":5.32597, "heading":-1.91071, "vx":-1.26899, "vy":0.96477, "omega":0.79626, "ax":-4.8271, "ay":3.5552, "alpha":-0.02355, "fx":[-80.69351,-80.31441,-80.32995,-80.52363], "fy":[58.95563,59.54366,59.26002,59.29435]}, + {"t":0.29001, "x":4.93635, "y":5.35032, "heading":-1.89147, "vx":-1.38565, "vy":1.05069, "omega":0.79569, "ax":-4.84276, "ay":3.53303, "alpha":-0.01822, "fx":[-80.53271,-80.57882,-80.41641,-81.37777], "fy":[59.46721,58.57624,58.91556,58.61672]}, + {"t":0.31418, "x":4.90145, "y":5.37675, "heading":-1.87223, "vx":-1.50269, "vy":1.13607, "omega":0.79525, "ax":-4.86154, "ay":3.50616, "alpha":-0.03185, "fx":[-81.33039,-80.84361,-80.85735,-81.12687], "fy":[58.0394,58.81981,58.44376,58.48101]}, + {"t":0.33835, "x":4.86372, "y":5.40523, "heading":-1.85302, "vx":-1.62018, "vy":1.22081, "omega":0.79448, "ax":-4.88457, "ay":3.47279, "alpha":-0.03534, "fx":[-81.2578,-81.21738,-81.01509,-82.2032], "fy":[58.58649,57.45422,57.93536,57.58298]}, + {"t":0.36252, "x":4.82313, "y":5.43575, "heading":-1.83381, "vx":-1.73823, "vy":1.30474, "omega":0.79363, "ax":-4.91326, "ay":3.43051, "alpha":-0.05973, "fx":[-82.3077,-81.64178,-81.61046,-82.04683], "fy":[56.62903,57.60478,57.22645,57.27983]}, + {"t":0.38668, "x":4.77969, "y":5.46828, "heading":-1.81463, "vx":-1.85697, "vy":1.38765, "omega":0.79218, "ax":-4.95025, "ay":3.37485, "alpha":-0.09156, "fx":[-82.63306,-82.28893,-82.00894,-83.14213], "fy":[56.83418,55.60264,56.38375,56.20815]}, + {"t":0.41085, "x":4.73336, "y":5.5028, "heading":-1.79549, "vx":-1.97661, "vy":1.46921, "omega":0.78997, "ax":-4.99934, "ay":3.29883, "alpha":-0.13717, "fx":[-84.03274,-82.91222,-82.76679,-83.63459], "fy":[54.06703,55.45161,55.16989,55.27081]}, + {"t":0.43502, "x":4.68413, "y":5.53927, "heading":-1.7764, "vx":-2.09743, "vy":1.54893, "omega":0.78666, "ax":-5.06803, "ay":3.18803, "alpha":-0.25158, "fx":[-85.20698,-84.09511,-83.59396,-85.03054], "fy":[53.42328,51.87556,53.58782,53.68504]}, + {"t":0.45919, "x":4.63196, "y":5.57764, "heading":-1.75739, "vx":-2.21992, "vy":1.62598, "omega":0.78058, "ax":-5.1693, "ay":3.01409, "alpha":-0.4248, "fx":[-87.737,-85.26483,-84.71233,-86.96458], "fy":[48.29066,50.30658,51.05915,51.31751]}, + {"t":0.48336, "x":4.5768, "y":5.61781, "heading":-1.73852, "vx":-2.34485, "vy":1.69883, "omega":0.77031, "ax":-5.33373, "ay":2.69893, "alpha":-1.04079, "fx":[-91.46206,-87.12905,-85.88075,-91.17097], "fy":[43.6644,41.21753,47.32694,47.75043]}, + {"t":0.50752, "x":4.51858, "y":5.65966, "heading":-1.7199, "vx":-2.47375, "vy":1.76405, "omega":0.74516, "ax":-5.62343, "ay":1.98808, "alpha":-2.71888, "fx":[-100.58495,-89.00724,-86.8679,-98.49931], "fy":[25.55073,25.9823,39.55174,41.47689]}, + {"t":0.53169, "x":4.45715, "y":5.70287, "heading":-1.7019, "vx":-2.60966, "vy":1.8121, "omega":0.67945, "ax":-5.89582, "ay":-0.58694, "alpha":-8.69294, "fx":[-116.35586,-80.30115,-78.06481,-118.39989], "fy":[-25.86259,-39.48382,9.10452,17.10558]}, + {"t":0.55586, "x":4.39236, "y":5.7465, "heading":-1.68547, "vx":-2.75215, "vy":1.79792, "omega":0.46936, "ax":-3.56233, "ay":-4.29266, "alpha":2.41597, "fx":[-53.21753,-65.17613,-65.43434,-53.70158], "fy":[-65.93855,-65.53973,-76.12836,-78.61955]}, + {"t":0.56995, "x":4.35322, "y":5.77141, "heading":-1.67886, "vx":-2.80235, "vy":1.73742, "omega":0.50341, "ax":-2.97241, "ay":-4.88539, "alpha":0.98179, "fx":[-46.35587,-52.03559,-52.07768,-47.72575], "fy":[-80.85126,-77.71857,-83.10109,-84.07774]}, + {"t":0.58405, "x":4.31343, "y":5.79541, "heading":-1.67176, "vx":-2.84424, "vy":1.66857, "omega":0.51724, "ax":-2.78731, "ay":-4.88281, "alpha":0.28735, "fx":[-45.36757,-47.49137,-47.60833,-45.38513], "fy":[-79.73502,-82.31049,-81.55817,-81.97258]}, + {"t":0.59814, "x":4.27307, "y":5.81844, "heading":-1.66448, "vx":-2.88352, "vy":1.59975, "omega":0.52129, "ax":-2.58495, "ay":-4.7787, "alpha":0.06945, "fx":[-42.24246,-43.44439,-43.50777,-43.16474], "fy":[-81.19235,-78.40038,-79.46664,-79.5751]}, + {"t":0.61223, "x":4.23217, "y":5.84051, "heading":-1.65713, "vx":-2.91995, "vy":1.53241, "omega":0.52227, "ax":-2.19065, "ay":-4.29731, "alpha":-0.025, "fx":[-36.24833,-36.71564,-36.81716,-36.28702], "fy":[-70.83543,-73.08757,-71.27336,-71.3403]}, + {"t":0.62633, "x":4.1908, "y":5.86168, "heading":-1.64977, "vx":-2.95083, "vy":1.47184, "omega":0.52192, "ax":-1.81503, "ay":-3.70675, "alpha":-0.0362, "fx":[-29.63859,-30.18168,-30.22687,-30.97525], "fy":[-62.86403,-61.09247,-61.58831,-61.61444]}, + {"t":0.64042, "x":4.14904, "y":5.88206, "heading":-1.64241, "vx":-2.97641, "vy":1.4196, "omega":0.52141, "ax":-1.44573, "ay":-3.09516, "alpha":0.00119, "fx":[-23.8994,-24.25339,-24.31479,-23.93059], "fy":[-51.08915,-52.41509,-51.41804,-51.45699]}, + {"t":0.65451, "x":4.10695, "y":5.90175, "heading":-1.63506, "vx":-2.99678, "vy":1.37598, "omega":0.52143, "ax":-1.1254, "ay":-2.48688, "alpha":-0.00268, "fx":[-18.17924,-18.68711,-18.72396,-19.44949], "fy":[-41.93749,-40.96435,-41.44601,-41.47269]}, + {"t":0.6686, "x":4.0646, "y":5.9209, "heading":-1.62772, "vx":-3.01264, "vy":1.34094, "omega":0.52139, "ax":-0.91725, "ay":-2.08364, "alpha":0.03472, "fx":[-15.10724,-15.44403,-15.48035,-15.12888], "fy":[-34.39558,-35.0601,-34.72306,-34.75469]}, + {"t":0.6827, "x":4.02205, "y":5.93959, "heading":-1.62037, "vx":-3.02557, "vy":1.31157, "omega":0.52188, "ax":-0.67601, "ay":-1.58222, "alpha":0.02106, "fx":[-10.73636,-11.20801,-11.23568,-11.8948], "fy":[-26.45271,-26.03828,-26.49343,-26.51527]}, + {"t":0.69679, "x":3.97934, "y":5.95792, "heading":-1.61301, "vx":-3.03509, "vy":1.28927, "omega":0.52217, "ax":-0.57983, "ay":-1.36638, "alpha":0.04993, "fx":[-9.51217,-9.80142,-9.82244,-9.52597], "fy":[-22.56115,-22.79482,-22.86446,-22.8869]}, + {"t":0.71088, "x":3.93651, "y":5.97595, "heading":-1.60565, "vx":-3.04327, "vy":1.27002, "omega":0.52288, "ax":-0.4071, "ay":-0.99389, "alpha":0.03261, "fx":[-6.30271,-6.73009,-6.74925,-7.36254], "fy":[-16.40649,-16.33821,-16.7554,-16.77037]}, + {"t":0.72498, "x":3.89358, "y":5.99375, "heading":-1.59828, "vx":-3.049, "vy":1.25601, "omega":0.52334, "ax":-0.38153, "ay":-0.91814, "alpha":0.0585, "fx":[-6.22211,-6.48708,-6.49979,-6.231], "fy":[-15.1544,-15.15305,-15.44891,-15.46378]}, + {"t":0.73907, "x":3.85057, "y":6.01136, "heading":-1.59091, "vx":-3.05438, "vy":1.24307, "omega":0.52416, "ax":-0.26754, "ay":-0.67424, "alpha":0.04242, "fx":[-4.01441,-4.42947,-4.44172,-4.95357], "fy":[-10.97329,-11.05247,-11.46111,-11.47037]}, + {"t":0.75316, "x":3.8075, "y":6.02881, "heading":-1.58352, "vx":-3.05815, "vy":1.23357, "omega":0.52476, "ax":-0.2875, "ay":-0.7019, "alpha":0.06693, "fx":[-4.64462,-4.93383,-4.94154,-4.64984], "fy":[-11.54761,-11.49214,-11.87642,-11.8855]}, + {"t":0.76726, "x":3.76437, "y":6.04613, "heading":-1.57613, "vx":-3.0622, "vy":1.22367, "omega":0.5257, "ax":-0.22658, "ay":-0.58266, "alpha":0.05678, "fx":[-3.35017,-3.80881,-3.81454,-4.13447], "fy":[-9.44598,-9.49755,-9.9515,-9.95551]}, + {"t":0.78135, "x":3.7212, "y":6.06332, "heading":-1.56872, "vx":-3.0654, "vy":1.21546, "omega":0.5265, "ax":-0.27756, "ay":-0.69052, "alpha":0.08257, "fx":[-4.43292,-4.81949,-4.82175,-4.43313], "fy":[-11.27554,-11.32902,-11.71738,-11.72059]}, + {"t":0.79544, "x":3.67797, "y":6.08038, "heading":-1.5613, "vx":-3.06931, "vy":1.20573, "omega":0.52767, "ax":-0.2685, "ay":-0.69752, "alpha":0.09103, "fx":[-4.00886,-4.64168,-4.63805,-4.61416], "fy":[-11.41039,-11.28247,-11.91003,-11.90656]}, + {"t":0.80954, "x":3.63468, "y":6.0973, "heading":-1.55386, "vx":-3.07309, "vy":1.1959, "omega":0.52895, "ax":-0.34207, "ay":-0.87509, "alpha":0.13929, "fx":[-5.35109,-6.06488,-6.05419,-5.33856], "fy":[-14.1098,-14.42535,-14.91108,-14.90287]}, + {"t":0.82363, "x":3.59134, "y":6.11407, "heading":-1.54641, "vx":-3.07791, "vy":1.18357, "omega":0.53091, "ax":-0.38702, "ay":-1.01921, "alpha":0.21537, "fx":[-5.71209,-6.99819,-6.96727,-6.12821], "fy":[-16.68372,-16.25003,-17.52553,-17.49953]}, + {"t":0.83772, "x":3.54792, "y":6.13065, "heading":-1.53892, "vx":-3.08337, "vy":1.1692, "omega":0.53395, "ax":-0.47745, "ay":-1.26764, "alpha":0.38477, "fx":[-6.98971,-8.99549,-8.92828,-6.92224], "fy":[-19.89121,-20.64142,-22.02314,-21.96772]}, + {"t":0.85182, "x":3.50442, "y":6.147, "heading":-1.5314, "vx":-3.0901, "vy":1.15134, "omega":0.53937, "ax":-0.5797, "ay":-1.57043, "alpha":0.70914, "fx":[-7.74685,-11.54873,-11.39207,-7.96533], "fy":[-24.91011,-24.13391,-27.90198,-27.76749]}, + {"t":0.86591, "x":3.46082, "y":6.16307, "heading":-1.5238, "vx":-3.09827, "vy":1.12921, "omega":0.54936, "ax":-0.67944, "ay":-1.89823, "alpha":1.39404, "fx":[-7.91046,-15.09213,-14.73017,-7.57117], "fy":[-27.717,-29.20241,-34.97425,-34.6765]}, + {"t":0.88, "x":3.41708, "y":6.17879, "heading":-1.51606, "vx":-3.10784, "vy":1.10245, "omega":0.56901, "ax":-0.83907, "ay":-2.3935, "alpha":2.66235, "fx":[-7.48552,-21.03862,-20.23622,-7.18754], "fy":[-33.96754,-33.18485,-46.56496,-45.87642]}, + {"t":0.8941, "x":3.3732, "y":6.19409, "heading":-1.50804, "vx":-3.11967, "vy":1.06872, "omega":0.60653, "ax":-0.79647, "ay":-2.82233, "alpha":5.48932, "fx":[-0.00878,-28.31847,-26.3418,1.562], "fy":[-32.60627,-36.29486,-60.44665,-58.83978]}, + {"t":0.90819, "x":3.32916, "y":6.20888, "heading":-1.49949, "vx":-3.13089, "vy":1.02895, "omega":0.68389, "ax":-1.05961, "ay":-2.86269, "alpha":6.0743, "fx":[-3.63898,-34.03342,-31.52992,-1.45024], "fy":[-31.23343,-35.50371,-62.93179,-61.20971]}, + {"t":0.92514, "x":3.27593, "y":6.22591, "heading":-1.4879, "vx":-3.14885, "vy":0.98042, "omega":0.78686, "ax":-0.76186, "ay":-2.49421, "alpha":1.23744, "fx":[-9.88765,-16.30053,-15.74026,-8.87124], "fy":[-39.56469,-38.16379,-44.54408,-44.03677]}, + {"t":0.94209, "x":3.22245, "y":6.24217, "heading":-1.47456, "vx":-3.16177, "vy":0.93815, "omega":0.80783, "ax":-0.49009, "ay":-1.68517, "alpha":0.20074, "fx":[-7.61288,-8.84746,-8.72668,-7.49123], "fy":[-26.99967,-28.54547,-28.47467,-28.3443]}, + {"t":0.95904, "x":3.16879, "y":6.25783, "heading":-1.46087, "vx":-3.17007, "vy":0.90958, "omega":0.81123, "ax":-0.3027, "ay":-1.06131, "alpha":0.01983, "fx":[-5.05409,-5.29033,-5.26362,-4.57528], "fy":[-18.28265,-17.34602,-17.58199,-17.55539]}, + {"t":0.97599, "x":3.11501, "y":6.27309, "heading":-1.44711, "vx":-3.1752, "vy":0.89159, "omega":0.81157, "ax":-0.10272, "ay":-0.36971, "alpha":-0.00806, "fx":[-1.66616,-1.77179,-1.75862,-1.65281], "fy":[-5.97595,-6.5741,-6.05526,-6.04628]}, + {"t":0.99294, "x":3.06117, "y":6.28815, "heading":-1.43336, "vx":-3.17695, "vy":0.88532, "omega":0.81143, "ax":0.05466, "ay":0.19736, "alpha":-0.04074, "fx":[0.55579,0.91166,0.86268,1.31482], "fy":[2.90846,3.19662,3.55161,3.50262]}, + {"t":1.00989, "x":3.00733, "y":6.30319, "heading":-1.4196, "vx":-3.17602, "vy":0.88867, "omega":0.81074, "ax":0.25633, "ay":0.90684, "alpha":-0.05051, "fx":[4.16234,4.42368,4.38356,4.12225], "fy":[14.88783,15.13769,15.24377,15.19711]}, + {"t":1.02684, "x":2.95353, "y":6.31838, "heading":-1.40586, "vx":-3.17167, "vy":0.90404, "omega":0.80989, "ax":0.45351, "ay":1.56394, "alpha":-0.0738, "fx":[6.95345,7.77818,7.64005,7.8672], "fy":[26.03777,25.58153,26.39931,26.26222]}, + {"t":1.04379, "x":2.89983, "y":6.33393, "heading":-1.39213, "vx":-3.16399, "vy":0.93055, "omega":0.80864, "ax":0.72566, "ay":2.41266, "alpha":-0.08451, "fx":[11.84342,12.46176,12.35216,11.72858], "fy":[39.53253,40.83634,40.30985,40.19313]}, + {"t":1.06074, "x":2.84631, "y":6.35005, "heading":-1.37843, "vx":-3.15169, "vy":0.97145, "omega":0.8072, "ax":1.05062, "ay":3.29525, "alpha":-0.09126, "fx":[16.70054,17.98147,17.72673,17.64473], "fy":[55.44721,54.00534,55.2571,55.01112]}, + {"t":1.07769, "x":2.79304, "y":6.36699, "heading":-1.36474, "vx":-3.13388, "vy":1.0273, "omega":0.80566, "ax":1.45193, "ay":4.27118, "alpha":-0.11201, "fx":[23.81974,24.78769,24.60063,23.60354], "fy":[70.00216,72.59682,71.19707,70.99797]}, + {"t":1.09464, "x":2.74012, "y":6.38501, "heading":-1.35109, "vx":-3.10927, "vy":1.0997, "omega":0.80376, "ax":1.86887, "ay":5.05313, "alpha":-0.0819, "fx":[30.18223,31.81079,31.43372,31.18621], "fy":[85.33768,82.95221,84.49651,84.14677]}, + {"t":1.11159, "x":2.68769, "y":6.40438, "heading":-1.33746, "vx":-3.07759, "vy":1.18535, "omega":0.80237, "ax":2.10056, "ay":5.2247, "alpha":-0.09785, "fx":[34.64271,35.58967,35.43456,34.39433], "fy":[85.73825,88.91519,86.97618,86.7431]}, + {"t":1.12855, "x":2.63582, "y":6.42522, "heading":-1.32386, "vx":-3.04198, "vy":1.27392, "omega":0.80071, "ax":2.29733, "ay":5.26287, "alpha":-0.02495, "fx":[37.61851,38.84637,38.55602,38.16095], "fy":[89.06984,86.65566,87.76117,87.43167]}, + {"t":1.1455, "x":2.58459, "y":6.44757, "heading":-1.31029, "vx":-3.00304, "vy":1.36312, "omega":0.80029, "ax":2.48265, "ay":5.25491, "alpha":0.171, "fx":[41.40411,41.12192,41.46137,41.55113], "fy":[86.67646,90.51145,86.63696,86.56263]}, + {"t":1.16245, "x":2.53404, "y":6.47143, "heading":-1.29673, "vx":-2.96096, "vy":1.4522, "omega":0.80319, "ax":2.68382, "ay":5.20641, "alpha":0.41372, "fx":[44.43518,43.9934,44.29635,46.2271], "fy":[89.47369,86.389,85.69563,85.59482]}, + {"t":1.1794, "x":2.48424, "y":6.4968, "heading":-1.28311, "vx":-2.91547, "vy":1.54045, "omega":0.8102, "ax":3.74261, "ay":4.54875, "alpha":0.60173, "fx":[63.05708,60.7235,61.80328,63.96615], "fy":[76.62399,78.85426,73.66553,74.15811]}, + {"t":1.19635, "x":2.43536, "y":6.52356, "heading":-1.26938, "vx":-2.85203, "vy":1.61755, "omega":0.8204, "ax":5.44191, "ay":2.35406, "alpha":0.77131, "fx":[90.78279,88.42584,89.34564,94.30196], "fy":[41.53529,40.18824,37.39337,37.84708]}, + {"t":1.2133, "x":2.3878, "y":6.55132, "heading":-1.25547, "vx":-2.75978, "vy":1.65746, "omega":0.83347, "ax":5.86448, "ay":1.02369, "alpha":0.59302, "fx":[98.51603,96.05572,97.01498,99.44539], "fy":[18.78805,18.48065,15.08713,15.90214]}, + {"t":1.23025, "x":2.34186, "y":6.57956, "heading":-1.24134, "vx":-2.66038, "vy":1.67481, "omega":0.84353, "ax":5.95671, "ay":0.32977, "alpha":0.50062, "fx":[99.3889,97.64386,98.29021,101.85926], "fy":[6.50001,6.3498,4.32802,4.81047]}, + {"t":1.2472, "x":2.29762, "y":6.608, "heading":-1.22705, "vx":-2.55941, "vy":1.6804, "omega":0.85201, "ax":5.97282, "ay":-0.07752, "alpha":0.31788, "fx":[99.99455,98.59211,99.12951,100.53977], "fy":[0.05361,-1.29451,-2.25409,-1.67411]}, + {"t":1.26415, "x":2.2551, "y":6.63647, "heading":-1.2126, "vx":-2.45817, "vy":1.67908, "omega":0.8574, "ax":5.96841, "ay":-0.34168, "alpha":0.23069, "fx":[99.53994,98.55848,98.93371,100.92984], "fy":[-5.71726,-5.03337,-6.19698,-5.83469]}, + {"t":1.2811, "x":2.21429, "y":6.66488, "heading":-1.19807, "vx":-2.357, "vy":1.67329, "omega":0.86131, "ax":5.95841, "ay":-0.52586, "alpha":0.15795, "fx":[99.54234,98.81155,99.10225,99.83939], "fy":[-7.8391,-9.23466,-9.17988,-8.80975]}, + {"t":1.29805, "x":2.17519, "y":6.69317, "heading":-1.18347, "vx":-2.256, "vy":1.66438, "omega":0.86399, "ax":5.94776, "ay":-0.65818, "alpha":0.13182, "fx":[99.15788,98.58464,98.8173,100.0253], "fy":[-11.08422,-10.53378,-11.27897,-10.98891]}, + {"t":1.315, "x":2.1378, "y":6.72129, "heading":-1.16883, "vx":-2.15518, "vy":1.65322, "omega":0.86622, "ax":5.93724, "ay":-0.76229, "alpha":0.08238, "fx":[99.10494,98.64196,98.83956,99.29698], "fy":[-11.86125,-13.66619,-12.78849,-12.51185]}, + {"t":1.33195, "x":2.10212, "y":6.7492, "heading":-1.15414, "vx":-2.05454, "vy":1.6403, "omega":0.86762, "ax":5.92756, "ay":-0.84492, "alpha":0.0484, "fx":[98.83783,98.40237,98.59348,99.40446], "fy":[-14.68516,-13.57387,-14.17646,-13.90238]}, + {"t":1.3489, "x":2.06815, "y":6.77688, "heading":-1.13944, "vx":-1.95407, "vy":1.62598, "omega":0.86844, "ax":5.91925, "ay":-0.90929, "alpha":-0.01247, "fx":[98.70111,98.58834,98.64754,98.74749], "fy":[-14.85518,-15.9892,-14.90171,-14.88373]}, + {"t":1.36585, "x":2.03588, "y":6.80431, "heading":-1.12472, "vx":-1.85374, "vy":1.61057, "omega":0.86823, "ax":5.91177, "ay":-0.96299, "alpha":-0.47992, "fx":[97.99017,99.411,98.81529,97.96897], "fy":[-18.82611,-16.30643,-13.97203,-15.10576]}, + {"t":1.3828, "x":2.00531, "y":6.83148, "heading":-1.11, "vx":-1.75353, "vy":1.59424, "omega":0.86009, "ax":5.90483, "ay":-1.00967, "alpha":-2.57464, "fx":[96.11213,104.63896,100.92519,92.04631], "fy":[-26.94094,-20.90684,-6.34914,-13.12596]}, + {"t":1.39975, "x":1.97643, "y":6.85835, "heading":-1.09542, "vx":-1.65344, "vy":1.57713, "omega":0.81645, "ax":5.89888, "ay":-1.04795, "alpha":-10.12375, "fx":[89.70257,122.99426,108.97334,71.6556], "fy":[-59.79988,-27.81539,21.39311,-3.65306]}, + {"t":1.4167, "x":1.94925, "y":6.88494, "heading":-1.08158, "vx":-1.55345, "vy":1.55936, "omega":0.64485, "ax":5.80917, "ay":-1.45926, "alpha":-2.24162, "fx":[94.60041,102.60724,99.23986,90.89705], "fy":[-32.76151,-27.4342,-15.32744,-21.77748]}, + {"t":1.44386, "x":1.90921, "y":6.92675, "heading":-1.06407, "vx":-1.39568, "vy":1.51973, "omega":0.58397, "ax":5.58205, "ay":-2.18664, "alpha":-1.82953, "fx":[90.73323,97.66455,95.09826,88.70416], "fy":[-44.39296,-37.50934,-29.18844,-34.70995]}, + {"t":1.47102, "x":1.87336, "y":6.96721, "heading":-1.04821, "vx":-1.24409, "vy":1.46035, "omega":0.53428, "ax":5.29472, "ay":-2.81244, "alpha":-1.51314, "fx":[86.35957,92.23961,90.33379,84.10905], "fy":[-52.48215,-48.38655,-40.76686,-45.89241]}, + {"t":1.49818, "x":1.84153, "y":7.00584, "heading":-1.0337, "vx":-1.1003, "vy":1.38397, "omega":0.49319, "ax":4.98184, "ay":-3.33598, "alpha":-1.35494, "fx":[80.85839,86.7487,84.83224,79.74002], "fy":[-61.36216,-55.90902,-50.41645,-54.74877]}, + {"t":1.52533, "x":1.81348, "y":7.04219, "heading":-1.0203, "vx":-0.965, "vy":1.29337, "omega":0.45639, "ax":4.66646, "ay":-3.76499, "alpha":-1.23032, "fx":[76.07287,81.28204,79.676,74.11947], "fy":[-67.13275,-63.52211,-58.0023,-62.38506]}, + {"t":1.55249, "x":1.789, "y":7.07593, "heading":-1.00791, "vx":-0.83827, "vy":1.19112, "omega":0.42298, "ax":4.36215, "ay":-4.11412, "alpha":-1.23727, "fx":[70.58342,76.4563,74.50085,69.31895], "fy":[-73.46374,-68.54247,-64.08184,-68.23389]}, + {"t":1.57965, "x":1.76784, "y":7.10676, "heading":-0.99642, "vx":-0.7198, "vy":1.07939, "omega":0.38938, "ax":4.07752, "ay":-4.39674, "alpha":-1.23966, "fx":[66.14208,71.82085,69.92397,63.99441], "fy":[-77.55081,-73.58633,-68.79884,-73.23008]}, + {"t":1.60681, "x":1.74979, "y":7.13445, "heading":-0.98585, "vx":-0.60907, "vy":0.95999, "omega":0.35571, "ax":3.8155, "ay":-4.62627, "alpha":-1.34603, "fx":[61.42432,68.02895,65.6192,59.33781], "fy":[-81.89245,-76.9441,-72.4694,-77.16454]}, + {"t":1.63397, "x":1.73466, "y":7.15882, "heading":-0.97619, "vx":-0.50545, "vy":0.83435, "omega":0.31916, "ax":3.57707, "ay":-4.81327, "alpha":-1.33655, "fx":[58.14203,64.27202,61.661,54.43701], "fy":[-83.96725,-80.51558,-75.67872,-80.77815]}, + {"t":1.66112, "x":1.72225, "y":7.1797, "heading":-0.96752, "vx":-0.4083, "vy":0.70363, "omega":0.28286, "ax":3.36114, "ay":-4.9667, "alpha":-1.55456, "fx":[53.9596,60.97898,58.6788,50.49688], "fy":[-87.8869,-82.83418,-77.42815,-83.02086]}, + {"t":1.68828, "x":1.7124, "y":7.19698, "heading":-0.95984, "vx":-0.31702, "vy":0.56875, "omega":0.24064, "ax":3.16605, "ay":-5.09349, "alpha":-1.80493, "fx":[50.23993,59.23125,55.42876,46.20632], "fy":[-90.57147,-84.62129,-79.14609,-85.28518]}, + {"t":1.71544, "x":1.70496, "y":7.21055, "heading":-0.9533, "vx":-0.23104, "vy":0.43042, "omega":0.19162, "ax":2.98998, "ay":-5.19899, "alpha":-2.08694, "fx":[46.6333,57.09443,53.2789,42.35924], "fy":[-93.3736,-86.2946,-79.94906,-87.04131]}, + {"t":1.7426, "x":1.69979, "y":7.22032, "heading":-0.9481, "vx":-0.14984, "vy":0.28923, "omega":0.13495, "ax":2.8307, "ay":-5.28757, "alpha":-2.31677, "fx":[44.22467,55.39724,50.67066,38.45274], "fy":[-95.0983,-88.20915,-80.70123,-88.556]}, + {"t":1.76975, "x":1.69676, "y":7.22622, "heading":-0.94443, "vx":-0.07296, "vy":0.14563, "omega":0.07203, "ax":2.68662, "ay":-5.36235, "alpha":-2.65227, "fx":[41.27831,54.51895,48.80913,34.53247], "fy":[-97.23124,-88.98485,-81.24362,-90.09157]}, + {"t":1.79691, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/KTofetch.traj b/src/main/deploy/choreo/KTofetch.traj index 02145865..8a8bb434 100644 --- a/src/main/deploy/choreo/KTofetch.traj +++ b/src/main/deploy/choreo/KTofetch.traj @@ -3,28 +3,28 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":4.084460748154255, "y":5.3371419902635795, "heading":5.235987755982989, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.084460748154255, "y":5.3371419902635795, "heading":5.235987755982989, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.9105907678604128, "y":6.9139485359191895, "heading":-0.942477796076938, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.6957741975784302, "y":7.228199481964111, "heading":-0.942477796076938, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":4.0}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":5.5}}, "enabled":true}], + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":3.3}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"K.x", "val":4.084460748154255}, "y":{"exp":"K.y", "val":5.3371419902635795}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"K.x", "val":4.084460748154255}, "y":{"exp":"K.y", "val":5.3371419902635795}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.9105907678604126 m", "val":1.9105907678604128}, "y":{"exp":"6.9139485359191895 m", "val":6.9139485359191895}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.6957741975784302 m", "val":1.6957741975784302}, "y":{"exp":"7.228199481964111 m", "val":7.228199481964111}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"4 m / s", "val":4.0}}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"maxAccel + 2.5 m / s ^ 2", "val":5.5}}}, "enabled":true}], + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.3 m / s", "val":3.3}}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -32,51 +32,50 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.1365,1.50991], + "waypoints":[0.0,1.1307,1.48783], "samples":[ - {"t":0.0, "x":4.08446, "y":5.33714, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.72604, "ay":2.80721, "alpha":1.87951, "fx":[-76.23102,-84.2744,-81.32358,-73.29409], "fy":[53.82956,47.51038,39.7497,46.08957]}, - {"t":0.03666, "x":4.08128, "y":5.33903, "heading":-1.0472, "vx":-0.17326, "vy":0.10292, "omega":0.06891, "ax":-4.72725, "ay":2.80802, "alpha":1.45398, "fx":[-76.80787,-83.06882,-80.75736,-74.57004], "fy":[52.25991,47.3373,41.36646,46.26995]}, - {"t":0.07332, "x":4.07176, "y":5.34469, "heading":-1.04467, "vx":-0.34657, "vy":0.20586, "omega":0.12221, "ax":-4.72711, "ay":2.80806, "alpha":1.07322, "fx":[-77.35503,-81.84016,-80.31477,-75.68468], "fy":[50.82403,47.3296,42.72707,46.35538]}, - {"t":0.10998, "x":4.05587, "y":5.35412, "heading":-1.04019, "vx":-0.51987, "vy":0.30881, "omega":0.16156, "ax":-4.72696, "ay":2.80808, "alpha":0.78332, "fx":[-77.70587,-81.19411,-79.77842,-76.50645], "fy":[49.72725,47.02978,43.92819,46.55209]}, - {"t":0.14665, "x":4.03364, "y":5.36733, "heading":-1.03427, "vx":-0.69317, "vy":0.41176, "omega":0.19027, "ax":-4.72678, "ay":2.80812, "alpha":0.48694, "fx":[-78.1568,-80.15414,-79.4744,-77.38741], "fy":[48.61649,47.10328,44.9262,46.59424]}, - {"t":0.18331, "x":4.00505, "y":5.38431, "heading":-1.02729, "vx":-0.86646, "vy":0.51471, "omega":0.20813, "ax":-4.72659, "ay":2.80815, "alpha":0.26716, "fx":[-78.39452,-79.72155,-79.04958,-77.99407], "fy":[47.77422,46.82451,45.8901,46.75322]}, - {"t":0.21997, "x":3.97011, "y":5.40507, "heading":-1.01966, "vx":-1.03974, "vy":0.61766, "omega":0.21792, "ax":-4.72634, "ay":2.80821, "alpha":0.03451, "fx":[-78.75825,-78.82709,-78.85657,-78.70138], "fy":[46.90402,46.97507,46.61848,46.74862]}, - {"t":0.25663, "x":3.92881, "y":5.4296, "heading":-1.01167, "vx":-1.21302, "vy":0.72061, "omega":0.21919, "ax":-4.72607, "ay":2.80825, "alpha":-0.12416, "fx":[-78.8751,-78.63396,-78.49611,-79.12005], "fy":[46.27837,46.67271,47.41093,46.88682]}, - {"t":0.29329, "x":3.88116, "y":5.45791, "heading":-1.00364, "vx":-1.38628, "vy":0.82357, "omega":0.21463, "ax":-4.7257, "ay":2.80838, "alpha":-0.31109, "fx":[-79.19205,-77.77185,-78.42356,-79.71294], "fy":[45.58006,46.96909,47.89214,46.81597]}, - {"t":0.32995, "x":3.82717, "y":5.48999, "heading":-0.99577, "vx":-1.55953, "vy":0.92652, "omega":0.20323, "ax":-4.72533, "ay":2.80839, "alpha":-0.41302, "fx":[-79.19143,-77.82648,-78.10201,-79.9559], "fy":[45.16655,46.5912,48.54471,46.95542]}, - {"t":0.36661, "x":3.76682, "y":5.52584, "heading":-0.98832, "vx":-1.73277, "vy":1.02948, "omega":0.18809, "ax":-4.72475, "ay":2.8086, "alpha":-0.55747, "fx":[-79.46552,-77.00186,-78.13464,-80.43504], "fy":[44.64155,46.98521,48.80488,46.8405]}, - {"t":0.40328, "x":3.70011, "y":5.56547, "heading":-0.98142, "vx":-1.90599, "vy":1.13245, "omega":0.16765, "ax":-4.72416, "ay":2.8086, "alpha":-0.62599, "fx":[-79.45333,-77.00242,-77.94699,-80.59527], "fy":[44.38824,46.70956,49.24446,46.93002]}, - {"t":0.43994, "x":3.62706, "y":5.60888, "heading":-0.97528, "vx":-2.07918, "vy":1.23542, "omega":0.1447, "ax":-4.72325, "ay":2.80882, "alpha":-0.69827, "fx":[-79.55121,-76.63891,-77.92849,-80.81889], "fy":[44.21763,46.65045,49.46245,46.95613]}, - {"t":0.4766, "x":3.54766, "y":5.65606, "heading":-0.96997, "vx":-2.25234, "vy":1.33839, "omega":0.1191, "ax":-4.72209, "ay":2.80893, "alpha":-0.72475, "fx":[-79.52972,-76.57427,-77.87371,-80.88242], "fy":[44.0322,46.81069,49.55556,46.89591]}, - {"t":0.51326, "x":3.46192, "y":5.70701, "heading":-0.9656, "vx":-2.42546, "vy":1.44137, "omega":0.09253, "ax":-4.7202, "ay":2.80936, "alpha":-0.73527, "fx":[-79.52703,-76.42789,-77.87626,-80.90273], "fy":[44.07542,46.76009,49.56631,46.92106]}, - {"t":0.54992, "x":3.36982, "y":5.76174, "heading":-0.96221, "vx":-2.59851, "vy":1.54437, "omega":0.06557, "ax":-4.71731, "ay":2.80963, "alpha":-0.65599, "fx":[-79.25054,-76.96529,-77.72037,-80.60503], "fy":[44.23017,46.7249,49.46131,46.92433]}, - {"t":0.58658, "x":3.27139, "y":5.82025, "heading":-0.95981, "vx":-2.77145, "vy":1.64737, "omega":0.04152, "ax":-4.71131, "ay":2.81055, "alpha":-0.60112, "fx":[-79.20833,-76.61265,-77.92397,-80.39607], "fy":[44.58177,46.94863,48.99296,46.87865]}, - {"t":0.62324, "x":3.16662, "y":5.88253, "heading":-0.95829, "vx":-2.94418, "vy":1.75041, "omega":0.01949, "ax":-4.69593, "ay":2.80914, "alpha":-0.36912, "fx":[-78.63329,-77.03517,-77.96702,-79.48037], "fy":[45.34935,47.16876,48.01918,46.77096]}, - {"t":0.65991, "x":3.05552, "y":5.9486, "heading":-0.95757, "vx":-3.11634, "vy":1.8534, "omega":0.00595, "ax":-2.56727, "ay":3.73522, "alpha":-0.16229, "fx":[-42.84034,-42.4529,-42.56416,-43.3232], "fy":[61.76801,61.81571,63.00563,62.46764]}, - {"t":0.69657, "x":2.93955, "y":6.01905, "heading":-0.95735, "vx":-3.21046, "vy":1.99034, "omega":0.0, "ax":1.65701, "ay":2.59211, "alpha":-0.00007, "fx":[28.30266,26.15439,28.49056,27.53847], "fy":[42.49602,43.18284,43.82169,43.33666]}, - {"t":0.73323, "x":2.82296, "y":6.09376, "heading":-0.95735, "vx":-3.14971, "vy":2.08537, "omega":0.0, "ax":2.90419, "ay":4.15515, "alpha":0.00004, "fx":[48.31561,48.5178,48.28335,48.52886], "fy":[69.9594,67.73405,69.62782,69.73616]}, - {"t":0.76989, "x":2.70944, "y":6.17301, "heading":-0.95735, "vx":-3.04324, "vy":2.2377, "omega":0.0, "ax":3.7364, "ay":3.80206, "alpha":0.0029, "fx":[62.51384,61.88098,62.46425,62.27694], "fy":[63.108,63.51588,63.49258,63.3979]}, - {"t":0.80655, "x":2.60038, "y":6.2576, "heading":-0.95735, "vx":-2.90625, "vy":2.37709, "omega":0.00011, "ax":5.18404, "ay":-1.74236, "alpha":0.23607, "fx":[86.57554,85.67187,86.25529,87.15915], "fy":[-27.61663,-30.17682,-29.61978,-28.7642]}, - {"t":0.84321, "x":2.49732, "y":6.34358, "heading":-0.95735, "vx":-2.7162, "vy":2.31321, "omega":0.00876, "ax":4.98038, "ay":-2.29825, "alpha":0.39231, "fx":[83.26926,82.14489,82.54585,84.12215], "fy":[-36.66575,-38.12479,-39.98944,-38.46315]}, - {"t":0.87987, "x":2.40109, "y":6.42684, "heading":-0.95703, "vx":-2.53361, "vy":2.22896, "omega":0.02315, "ax":4.90171, "ay":-2.47303, "alpha":0.44788, "fx":[82.05032,80.66834,81.16545,82.95278], "fy":[-39.70083,-40.29131,-43.29665,-41.60814]}, - {"t":0.91654, "x":2.3115, "y":6.50689, "heading":-0.95618, "vx":-2.35391, "vy":2.13829, "omega":0.03957, "ax":4.86051, "ay":-2.55847, "alpha":0.43463, "fx":[81.42337,79.83332,80.55933,82.27334], "fy":[-40.94036,-42.51529,-44.38624,-42.75218]}, - {"t":0.9532, "x":2.22846, "y":6.58357, "heading":-0.95473, "vx":-2.17572, "vy":2.04449, "omega":0.0555, "ax":4.83536, "ay":-2.60882, "alpha":0.3343, "fx":[80.94018,79.60346,80.25925,81.60991], "fy":[-42.03621,-43.84551,-44.66007,-43.40913]}, - {"t":0.98986, "x":2.15195, "y":6.65677, "heading":-0.95269, "vx":-1.99844, "vy":1.94885, "omega":0.06776, "ax":4.81832, "ay":-2.64222, "alpha":0.19863, "fx":[80.54459,79.64241,80.17845,80.9106], "fy":[-43.30783,-44.05564,-44.75165,-44.06274]}, - {"t":1.02652, "x":2.08192, "y":6.72644, "heading":-0.95021, "vx":-1.8218, "vy":1.85198, "omega":0.07504, "ax":4.8061, "ay":-2.66581, "alpha":-0.03501, "fx":[80.06821,80.21506,80.14344,80.0346], "fy":[-44.48892,-44.65541,-44.24111,-44.36587]}, - {"t":1.06318, "x":2.01836, "y":6.79254, "heading":-0.94746, "vx":-1.6456, "vy":1.75425, "omega":0.07375, "ax":4.79681, "ay":-2.68355, "alpha":-0.31727, "fx":[79.59796,80.99476,80.2325,79.01703], "fy":[-45.94538,-44.65597,-43.58287,-44.74962]}, - {"t":1.09984, "x":1.96125, "y":6.85505, "heading":-0.94476, "vx":-1.46974, "vy":1.65587, "omega":0.06212, "ax":4.7896, "ay":-2.69723, "alpha":-0.64935, "fx":[79.1809,81.71581,80.52567,77.93874], "fy":[-47.34705,-45.32368,-42.36211,-44.813]}, - {"t":1.1365, "x":1.91059, "y":6.91395, "heading":-0.94248, "vx":-1.29415, "vy":1.55698, "omega":0.03832, "ax":4.64602, "ay":-2.93184, "alpha":-0.46307, "fx":[76.96809,78.79798,77.95565,76.06575], "fy":[-50.50423,-49.22673,-47.00994,-48.74851]}, - {"t":1.17045, "x":1.86934, "y":6.96511, "heading":-0.94118, "vx":-1.13644, "vy":1.45746, "omega":0.0226, "ax":4.36969, "ay":-3.33519, "alpha":-0.35588, "fx":[72.27995,73.86904,73.16892,72.04472], "fy":[-57.21945,-55.53847,-54.24555,-55.38072]}, - {"t":1.20439, "x":1.83328, "y":7.01266, "heading":-0.94041, "vx":-0.9881, "vy":1.34425, "omega":0.01052, "ax":4.09666, "ay":-3.66567, "alpha":-0.26851, "fx":[67.99719,69.07049,68.64714,67.4427], "fy":[-61.98571,-61.37489,-60.02432,-61.03523]}, - {"t":1.23834, "x":1.8021, "y":7.05618, "heading":-0.94005, "vx":-0.84904, "vy":1.21981, "omega":0.0014, "ax":3.83773, "ay":-3.93619, "alpha":-0.20097, "fx":[63.53208,64.57707,64.14658,63.6367], "fy":[-66.61125,-65.55094,-64.86396,-65.4317]}, - {"t":1.27229, "x":1.77549, "y":7.09532, "heading":-0.94001, "vx":-0.71877, "vy":1.0862, "omega":-0.00542, "ax":3.59701, "ay":-4.15754, "alpha":-0.13736, "fx":[59.81783,60.35132,60.16584,59.50663], "fy":[-69.71695,-69.50955,-68.74132,-69.24912]}, - {"t":1.30623, "x":1.75316, "y":7.1298, "heading":-0.94019, "vx":-0.59666, "vy":0.94507, "omega":-0.01008, "ax":3.3759, "ay":-4.3392, "alpha":-0.08434, "fx":[55.99739,56.52507,56.32776,56.24814], "fy":[-72.83959,-72.28472,-72.0054,-72.19962]}, - {"t":1.34018, "x":1.73485, "y":7.15938, "heading":-0.94053, "vx":-0.48207, "vy":0.79777, "omega":-0.01295, "ax":3.17428, "ay":-4.48896, "alpha":-0.02848, "fx":[52.90117,52.95031,52.98098,52.82254], "fy":[-74.87846,-75.00669,-74.65835,-74.77191]}, - {"t":1.37412, "x":1.72032, "y":7.18388, "heading":-0.94097, "vx":-0.37431, "vy":0.64539, "omega":-0.01391, "ax":2.99107, "ay":-4.61321, "alpha":0.01481, "fx":[49.74834,49.77016,49.81583,50.10473], "fy":[-77.02517,-76.85996,-76.91754,-76.79742]}, - {"t":1.40807, "x":1.70933, "y":7.20313, "heading":-0.94144, "vx":-0.27278, "vy":0.48879, "omega":-0.01341, "ax":2.82485, "ay":-4.71696, "alpha":0.0772, "fx":[47.20078,46.75265,47.02556,47.37631], "fy":[-78.34395,-78.80121,-78.80619,-78.56679]}, - {"t":1.44201, "x":1.7017, "y":7.217, "heading":-0.9419, "vx":-0.17689, "vy":0.32867, "omega":-0.01079, "ax":2.674, "ay":-4.8042, "alpha":0.13304, "fx":[44.74959,44.03548,44.44353,45.06855], "fy":[-79.62734,-80.24959,-80.43745,-80.02075]}, - {"t":1.47596, "x":1.69724, "y":7.22539, "heading":-0.94226, "vx":-0.08612, "vy":0.16559, "omega":-0.00627, "ax":2.53697, "ay":-4.87806, "alpha":0.1848, "fx":[42.49251,41.63472,42.05863,42.97432], "fy":[-80.73355,-81.31733,-81.90484,-81.30407]}, - {"t":1.50991, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":4.08446, "y":5.33714, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.0491, "ay":3.23526, "alpha":1.84644, "fx":[-81.36536,-89.31571,-87.04452,-78.93892], "fy":[61.00833,54.35776,46.784,53.57068]}, + {"t":0.03769, "x":4.08087, "y":5.33944, "heading":-1.0472, "vx":-0.1903, "vy":0.12194, "omega":0.06959, "ax":-5.05035, "ay":3.23614, "alpha":1.40718, "fx":[-82.03904,-88.09792,-86.38483,-80.22587], "fy":[59.35982,54.2681,48.48963,53.66198]}, + {"t":0.07538, "x":4.07012, "y":5.34633, "heading":-1.04457, "vx":-0.38065, "vy":0.24391, "omega":0.12263, "ax":-5.05018, "ay":3.23613, "alpha":1.03202, "fx":[-82.62053,-87.0158,-85.79512,-81.30488], "fy":[57.90804,54.2709,49.90518,53.6947]}, + {"t":0.11307, "x":4.05218, "y":5.35783, "heading":-1.03995, "vx":-0.57099, "vy":0.36588, "omega":0.16153, "ax":-5.04997, "ay":3.23612, "alpha":0.71159, "fx":[-83.09666,-86.20416,-85.24892,-82.17286], "fy":[56.68208,54.06875,51.20944,53.81825]}, + {"t":0.15076, "x":4.02707, "y":5.37391, "heading":-1.03386, "vx":-0.76132, "vy":0.48784, "omega":0.18835, "ax":-5.04973, "ay":3.23611, "alpha":0.42131, "fx":[-83.55372,-85.33469,-84.81791,-83.00026], "fy":[55.54228,54.11997,52.28249,53.8327]}, + {"t":0.18845, "x":3.99479, "y":5.3946, "heading":-1.02677, "vx":-0.95165, "vy":0.60981, "omega":0.20422, "ax":-5.04944, "ay":3.2361, "alpha":0.17665, "fx":[-83.89888,-84.72941,-84.39464,-83.66373], "fy":[54.61238,53.94097,53.29764,53.92565]}, + {"t":0.22614, "x":3.95534, "y":5.41988, "heading":-1.01907, "vx":-1.14196, "vy":0.73178, "omega":0.21088, "ax":-5.04906, "ay":3.23609, "alpha":-0.04986, "fx":[-84.25229,-84.0042,-84.09681,-84.30821], "fy":[53.70346,54.06143,54.10557,53.90556]}, + {"t":0.26383, "x":3.90871, "y":5.44976, "heading":-1.01112, "vx":-1.33226, "vy":0.85375, "omega":0.209, "ax":-5.04858, "ay":3.23607, "alpha":-0.23029, "fx":[-84.47243,-83.58684,-83.76873,-84.80146], "fy":[53.03043,53.88308,54.88867,53.97259]}, + {"t":0.30152, "x":3.85491, "y":5.48424, "heading":-1.00324, "vx":-1.52254, "vy":0.97572, "omega":0.20032, "ax":-5.04794, "ay":3.23603, "alpha":-0.402, "fx":[-84.73901,-82.96828,-83.59055,-85.28926], "fy":[52.31546,54.09175,55.45173,53.91355]}, + {"t":0.33921, "x":3.79395, "y":5.52331, "heading":-0.99569, "vx":-1.71279, "vy":1.09768, "omega":0.18517, "ax":-5.04703, "ay":3.23601, "alpha":-0.51676, "fx":[-84.82244,-82.77434,-83.33162,-85.59817], "fy":[51.9082,53.86384,56.02093,53.97818]}, + {"t":0.3769, "x":3.72581, "y":5.56698, "heading":-0.98871, "vx":-1.90302, "vy":1.21965, "omega":0.1657, "ax":-5.04568, "ay":3.23597, "alpha":-0.6303, "fx":[-84.99817,-82.25064,-83.27107,-85.9164], "fy":[51.41596,54.14706,56.31199,53.89325]}, + {"t":0.41459, "x":3.6505, "y":5.61525, "heading":-0.98247, "vx":-2.09319, "vy":1.34161, "omega":0.14194, "ax":-5.04338, "ay":3.23596, "alpha":-0.66291, "fx":[-84.8999,-82.36524,-83.0532,-85.9648], "fy":[51.31499,53.84202,56.63197,53.97847]}, + {"t":0.45228, "x":3.56802, "y":5.66811, "heading":-0.97712, "vx":-2.28327, "vy":1.46357, "omega":0.11695, "ax":-5.03893, "ay":3.23571, "alpha":-0.70264, "fx":[-84.948,-81.91585,-83.09539,-86.02699], "fy":[51.10598,54.21363,56.56236,53.86897]}, + {"t":0.48997, "x":3.47839, "y":5.72557, "heading":-0.97271, "vx":-2.47319, "vy":1.58553, "omega":0.09047, "ax":-5.0248, "ay":3.23615, "alpha":-0.59888, "fx":[-84.39202,-82.37812,-82.77244,-85.50176], "fy":[51.48314,53.89095,56.43416,53.97231]}, + {"t":0.52766, "x":3.3816, "y":5.78763, "heading":-0.9693, "vx":-2.66257, "vy":1.7075, "omega":0.0679, "ax":-3.09004, "ay":1.86924, "alpha":-0.57566, "fx":[-52.0024,-49.76498,-50.98281,-53.28791], "fy":[29.43455,30.04403,33.48519,31.6735]}, + {"t":0.56535, "x":3.27906, "y":5.85331, "heading":-0.96674, "vx":-2.77904, "vy":1.77795, "omega":0.0462, "ax":0.02284, "ay":0.03623, "alpha":-0.27111, "fx":[0.25629,1.1814,0.62506,-0.53967], "fy":[-0.36757,0.42586,1.57553,0.78191]}, + {"t":0.60304, "x":3.17433, "y":5.92035, "heading":-0.965, "vx":-2.77818, "vy":1.77931, "omega":0.03599, "ax":-0.02699, "ay":-0.04217, "alpha":-0.10424, "fx":[-0.51907,-0.08275,-0.38066,-0.81706], "fy":[-1.15027,-0.531,-0.4171,-0.71312]}, + {"t":0.64073, "x":3.0696, "y":5.98738, "heading":-0.96364, "vx":-2.77919, "vy":1.77773, "omega":0.03206, "ax":-0.02454, "ay":-0.03839, "alpha":-0.03947, "fx":[-0.47792,-0.18202,-0.43899,-0.53753], "fy":[-0.73135,-0.65967,-0.54842,-0.62027]}, + {"t":0.67842, "x":2.96484, "y":6.05435, "heading":-0.96244, "vx":-2.78012, "vy":1.77628, "omega":0.03057, "ax":-0.07265, "ay":-0.11391, "alpha":-0.01087, "fx":[-1.22394,-1.15791,-1.19756,-1.26498], "fy":[-1.97253,-1.78743,-1.90695,-1.92831]}, + {"t":0.71611, "x":2.86001, "y":6.12122, "heading":-0.96128, "vx":-2.78286, "vy":1.77199, "omega":0.03016, "ax":0.06686, "ay":0.10484, "alpha":0.01834, "fx":[1.14999,0.9699,1.14688,1.1912], "fy":[1.76563,1.74576,1.73035,1.74858]}, + {"t":0.7538, "x":2.75517, "y":6.18808, "heading":-0.96015, "vx":-2.78034, "vy":1.77594, "omega":0.03085, "ax":0.17419, "ay":0.27162, "alpha":-0.00324, "fx":[2.90226,2.88557,2.90553,2.92161], "fy":[4.80709,3.8197,4.72133,4.7628]}, + {"t":0.79149, "x":2.6505, "y":6.25521, "heading":-0.95898, "vx":-2.77377, "vy":1.78617, "omega":0.03073, "ax":0.78696, "ay":1.20032, "alpha":0.05782, "fx":[13.26078,12.63355,13.24798,13.33074], "fy":[20.06004,20.0203,19.95427,20.00043]}, + {"t":0.82918, "x":2.54652, "y":6.32338, "heading":-0.95783, "vx":-2.74411, "vy":1.83141, "omega":0.03291, "ax":1.98009, "ay":2.84317, "alpha":0.06029, "fx":[33.00962,32.8514,32.96067,33.20704], "fy":[47.6123,47.46758,47.15516,47.34231]}, + {"t":0.86687, "x":2.4445, "y":6.39443, "heading":-0.95659, "vx":-2.66948, "vy":1.93857, "omega":0.03518, "ax":3.46721, "ay":4.45852, "alpha":0.07437, "fx":[57.64241,57.93469,57.56224,58.04785], "fy":[74.71516,74.52649,73.86195,74.18223]}, + {"t":0.90456, "x":2.34635, "y":6.47066, "heading":-0.95526, "vx":-2.5388, "vy":2.10661, "omega":0.03798, "ax":5.5822, "ay":1.90084, "alpha":0.32201, "fx":[93.1644,92.20602,92.94234,93.89763], "fy":[32.19694,33.77498,29.92869,30.84356]}, + {"t":0.94225, "x":2.25462, "y":6.55141, "heading":-0.95383, "vx":-2.32841, "vy":2.17826, "omega":0.05012, "ax":5.48813, "ay":-2.37431, "alpha":0.27111, "fx":[91.63234,91.02072,91.08176,92.20343], "fy":[-38.36262,-39.43983,-40.83423,-39.67785]}, + {"t":0.97994, "x":2.17077, "y":6.63182, "heading":-0.95194, "vx":-2.12156, "vy":2.08877, "omega":0.06034, "ax":5.30464, "ay":-2.78038, "alpha":0.18256, "fx":[88.6087,87.89597,88.22003,88.97857], "fy":[-45.43241,-46.80969,-46.9349,-46.21332]}, + {"t":1.01763, "x":2.09457, "y":6.70857, "heading":-0.94967, "vx":-1.92163, "vy":1.98398, "omega":0.06722, "ax":5.22973, "ay":-2.92594, "alpha":-0.00629, "fx":[87.14342,87.22836,87.17494,87.16161], "fy":[-48.81935,-48.68033,-48.7811,-48.81516]}, + {"t":1.05532, "x":2.02586, "y":6.78127, "heading":-0.94713, "vx":-1.72452, "vy":1.8737, "omega":0.06698, "ax":5.1892, "ay":-3.00082, "alpha":-0.27755, "fx":[86.20053,87.23566,86.83271,85.73675], "fy":[-50.92398,-50.54217,-48.76343,-49.85919]}, + {"t":1.09301, "x":1.96455, "y":6.84976, "heading":-0.94461, "vx":-1.52894, "vy":1.7606, "omega":0.05652, "ax":5.16385, "ay":-3.04639, "alpha":-0.61567, "fx":[85.38647,87.81645,86.79065,84.32175], "fy":[-53.236,-50.71615,-48.34835,-50.82691]}, + {"t":1.1307, "x":1.91059, "y":6.91395, "heading":-0.94248, "vx":-1.33432, "vy":1.64578, "omega":0.03332, "ax":4.96768, "ay":-3.34869, "alpha":-0.38507, "fx":[82.46539,84.11325,83.21003,81.44663], "fy":[-56.9495,-55.95854,-54.35303,-56.02326]}, + {"t":1.16316, "x":1.86989, "y":6.96562, "heading":-0.9414, "vx":-1.17303, "vy":1.53706, "omega":0.02081, "ax":4.62511, "ay":-3.81573, "alpha":-0.34695, "fx":[76.61965,78.09166,77.51303,76.16879], "fy":[-65.12157,-63.47031,-62.3435,-63.49019]}, + {"t":1.19563, "x":1.83424, "y":7.01351, "heading":-0.94072, "vx":-1.02287, "vy":1.41317, "omega":0.00955, "ax":4.31452, "ay":-4.16428, "alpha":-0.24796, "fx":[71.60636,72.67972,72.27152,71.12643], "fy":[-70.22297,-69.58424,-68.45045,-69.4082]}, + {"t":1.2281, "x":1.80331, "y":7.0572, "heading":-0.94041, "vx":-0.88279, "vy":1.27797, "omega":0.0015, "ax":4.04245, "ay":-4.42934, "alpha":-0.19592, "fx":[67.05891,67.99274,67.62726,66.86403], "fy":[-74.6716,-73.74478,-73.15344,-73.76995]}, + {"t":1.26056, "x":1.77677, "y":7.09635, "heading":-0.94036, "vx":-0.75154, "vy":1.13417, "omega":-0.00486, "ax":3.80625, "ay":-4.63428, "alpha":-0.12862, "fx":[63.28423,63.85729,63.64067,63.01092], "fy":[-77.64038,-77.35314,-76.76598,-77.24509]}, + {"t":1.29303, "x":1.75438, "y":7.13073, "heading":-0.94052, "vx":-0.62797, "vy":0.9837, "omega":-0.00904, "ax":3.60147, "ay":-4.79546, "alpha":-0.08223, "fx":[59.85457,60.29557,60.12941,59.85958], "fy":[-80.32246,-79.88911,-79.6555,-79.88516]}, + {"t":1.3255, "x":1.73589, "y":7.16014, "heading":-0.94081, "vx":-0.51104, "vy":0.82801, "omega":-0.01171, "ax":3.42351, "ay":-4.92435, "alpha":-0.02695, "fx":[57.03782,57.14069,57.11966,56.97518], "fy":[-82.14951,-82.16114,-81.96642,-82.06905]}, + {"t":1.35797, "x":1.7211, "y":7.18443, "heading":-0.94119, "vx":-0.39989, "vy":0.66813, "omega":-0.01258, "ax":3.26819, "ay":-5.029, "alpha":0.02047, "fx":[54.43288,54.39132,54.44032,54.65221], "fy":[-83.84829,-83.81545,-83.87938,-83.78094]}, + {"t":1.39043, "x":1.70984, "y":7.20347, "heading":-0.9416, "vx":-0.29378, "vy":0.50486, "omega":-0.01192, "ax":3.13191, "ay":-5.11517, "alpha":0.0726, "fx":[52.3065,51.9214,52.12559,52.47637], "fy":[-85.02654,-85.33705,-85.47162,-85.23481]}, + {"t":1.4229, "x":1.70195, "y":7.21717, "heading":-0.94199, "vx":-0.1921, "vy":0.33878, "omega":-0.00956, "ax":3.01168, "ay":-5.18704, "alpha":0.12489, "fx":[50.36888,49.72203,50.05369,50.66824], "fy":[-86.06909,-86.5351,-86.83211,-86.42571]}, + {"t":1.45537, "x":1.69731, "y":7.22543, "heading":-0.9423, "vx":-0.09432, "vy":0.17038, "omega":-0.00551, "ax":2.90501, "ay":-5.24767, "alpha":0.16961, "fx":[48.62183,47.85212,48.17405,49.05247], "fy":[-86.92649,-87.50178,-88.02964,-87.44667]}, + {"t":1.48783, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/LTofetch.traj b/src/main/deploy/choreo/LTofetch.traj index 444e634f..0a8623c5 100644 --- a/src/main/deploy/choreo/LTofetch.traj +++ b/src/main/deploy/choreo/LTofetch.traj @@ -3,28 +3,28 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":3.798672364905389, "y":5.1721419902635795, "heading":5.235987755982989, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.798672364905389, "y":5.1721419902635795, "heading":5.235987755982989, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.9328655004501345, "y":6.905690670013428, "heading":-0.942477796076938, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.69577419757843, "y":7.228199481964111, "heading":-0.942477796076938, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":4.0}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":5.5}}, "enabled":true}], + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":3.3}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"L.x", "val":3.798672364905389}, "y":{"exp":"L.y", "val":5.1721419902635795}, "heading":{"exp":"L.heading", "val":5.235987755982989}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"L.x", "val":3.798672364905389}, "y":{"exp":"L.y", "val":5.1721419902635795}, "heading":{"exp":"L.heading", "val":5.235987755982989}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.9328655004501343 m", "val":1.9328655004501345}, "y":{"exp":"6.905690670013428 m", "val":6.905690670013428}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"fetch.x", "val":1.69577419757843}, "y":{"exp":"fetch.y", "val":7.228199481964111}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"4 m / s", "val":4.0}}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"maxAccel + 2.5 m / s ^ 2", "val":5.5}}}, "enabled":true}], + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.3 m / s", "val":3.3}}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -32,50 +32,49 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.08888,1.47081], + "waypoints":[0.0,1.08041,1.446], "samples":[ - {"t":0.0, "x":3.79867, "y":5.17214, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.20894, "ay":3.53558, "alpha":1.91433, "fx":[-67.4053,-76.13876,-72.88106,-64.21923], "fy":[65.74583,59.47893,52.11931,58.40167]}, - {"t":0.0363, "x":3.7959, "y":5.17447, "heading":-1.0472, "vx":-0.15277, "vy":0.12833, "omega":0.06948, "ax":-4.21006, "ay":3.53654, "alpha":1.48728, "fx":[-68.012,-74.85854,-72.27406,-65.57425], "fy":[64.24958,59.33732,53.67626,58.54671]}, - {"t":0.07259, "x":3.78758, "y":5.18146, "heading":-1.04468, "vx":-0.30557, "vy":0.25669, "omega":0.12346, "ax":-4.20997, "ay":3.53649, "alpha":1.10157, "fx":[-68.61165,-73.47462,-71.84717,-66.7792], "fy":[62.82311,59.5068,54.9321,58.54452]}, - {"t":0.10889, "x":3.77372, "y":5.1931, "heading":-1.04019, "vx":-0.45838, "vy":0.38505, "omega":0.16345, "ax":-4.20986, "ay":3.53644, "alpha":0.81454, "fx":[-68.96372,-72.87942,-71.22102,-67.64147], "fy":[61.82592,59.07461,56.13692,58.76525]}, - {"t":0.14518, "x":3.75431, "y":5.20941, "heading":-1.03426, "vx":-0.61118, "vy":0.51341, "omega":0.19301, "ax":-4.20974, "ay":3.53637, "alpha":0.50798, "fx":[-69.47607,-71.66448,-70.93857,-68.61834], "fy":[60.70372,59.31301,57.04854,58.73328]}, - {"t":0.18148, "x":3.72935, "y":5.23037, "heading":-1.02726, "vx":-0.76398, "vy":0.64176, "omega":0.21145, "ax":-4.20961, "ay":3.53629, "alpha":0.29314, "fx":[-69.69868,-71.31278,-70.43582,-69.24119], "fy":[59.94088,58.90069,58.0297,58.92195]}, - {"t":0.21778, "x":3.69885, "y":5.256, "heading":-1.01958, "vx":-0.91677, "vy":0.77011, "omega":0.22209, "ax":-4.20945, "ay":3.53621, "alpha":0.0467, "fx":[-70.13362,-70.21904,-70.27365,-70.05139], "fy":[59.03584,59.24058,58.67106,58.83988]}, - {"t":0.25407, "x":3.6628, "y":5.28628, "heading":-1.01152, "vx":-1.06955, "vy":0.89846, "omega":0.22378, "ax":-4.20926, "ay":3.5361, "alpha":-0.10134, "fx":[-70.20538,-70.18142,-69.8291,-70.44917], "fy":[58.48802,58.76073,59.50506,59.02652]}, - {"t":0.29037, "x":3.62121, "y":5.32122, "heading":-1.0034, "vx":-1.22233, "vy":1.02681, "omega":0.22011, "ax":-4.20903, "ay":3.53597, "alpha":-0.30799, "fx":[-70.61599,-69.05685,-69.81172,-71.16531], "fy":[57.72151,59.31212,59.88142,58.85692]}, - {"t":0.32666, "x":3.57407, "y":5.36081, "heading":-0.99541, "vx":-1.3751, "vy":1.15515, "omega":0.20893, "ax":-4.20875, "ay":3.53582, "alpha":-0.39254, "fx":[-70.53593,-69.35862,-69.38993,-71.34682], "fy":[57.39873,58.67791,60.61077,59.07434]}, - {"t":0.36296, "x":3.52139, "y":5.40507, "heading":-0.98783, "vx":-1.52786, "vy":1.28349, "omega":0.19468, "ax":-4.2084, "ay":3.53563, "alpha":-0.56165, "fx":[-70.91644,-68.24064,-69.49495,-71.9561], "fy":[56.81104,59.27793,60.78509,58.87495]}, - {"t":0.39925, "x":3.46316, "y":5.45399, "heading":-0.98076, "vx":-1.68061, "vy":1.41182, "omega":0.17429, "ax":-4.20795, "ay":3.53539, "alpha":-0.62489, "fx":[-70.87206,-68.34479,-69.26732,-72.09418], "fy":[56.60718,58.83178,61.27952,59.01422]}, - {"t":0.43555, "x":3.39939, "y":5.50756, "heading":-0.97443, "vx":-1.83334, "vy":1.54014, "omega":0.15161, "ax":-4.20737, "ay":3.53505, "alpha":-0.70572, "fx":[-71.0039,-67.91112,-69.25549,-72.3687], "fy":[56.43495,58.75824,61.48961,59.02764]}, - {"t":0.47185, "x":3.33008, "y":5.56579, "heading":-0.96893, "vx":-1.98605, "vy":1.66844, "omega":0.126, "ax":-4.20654, "ay":3.53459, "alpha":-0.74627, "fx":[-71.03057,-67.72173,-69.24667,-72.48524], "fy":[56.20343,58.96785,61.5705,58.93807]}, - {"t":0.50814, "x":3.25522, "y":5.62867, "heading":-0.96436, "vx":-2.13873, "vy":1.79673, "omega":0.09891, "ax":-4.20531, "ay":3.53391, "alpha":-0.75352, "fx":[-71.01044,-67.69034,-69.19997,-72.5015], "fy":[56.21448,58.85709,61.60682,58.95566]}, - {"t":0.54444, "x":3.17482, "y":5.69621, "heading":-0.96077, "vx":-2.29137, "vy":1.925, "omega":0.07156, "ax":-4.20331, "ay":3.53274, "alpha":-0.68107, "fx":[-70.73599,-68.2835,-69.02308,-72.22591], "fy":[56.28924,58.78197,61.54345,58.94181]}, - {"t":0.58073, "x":3.08889, "y":5.76841, "heading":-0.95817, "vx":-2.44393, "vy":2.05322, "omega":0.04684, "ax":-4.19927, "ay":3.53057, "alpha":-0.66236, "fx":[-70.81569,-67.76148,-69.28017,-72.14213], "fy":[56.32755,59.31249,61.04345,58.72785]}, - {"t":0.61703, "x":2.99742, "y":5.84526, "heading":-0.95647, "vx":-2.59634, "vy":2.18137, "omega":0.0228, "ax":-4.1887, "ay":3.52324, "alpha":-0.38769, "fx":[-69.83934,-69.73618,-68.64164,-71.07728], "fy":[56.89096,58.49906,60.70619,58.82689]}, - {"t":0.65333, "x":2.90042, "y":5.92675, "heading":-0.95564, "vx":-2.74838, "vy":2.30925, "omega":0.00873, "ax":-3.76583, "ay":3.5406, "alpha":-0.24045, "fx":[-63.05422,-61.77015,-62.5841,-63.68954], "fy":[57.41791,61.02952,59.23041,58.40225]}, - {"t":0.68962, "x":2.79819, "y":6.0129, "heading":-0.95533, "vx":-2.88506, "vy":2.43776, "omega":0.0, "ax":1.22873, "ay":1.4454, "alpha":0.0, "fx":[19.97044,21.7181,19.75398,20.48656], "fy":[24.67633,24.19188,23.51624,23.99198]}, - {"t":0.72592, "x":2.69428, "y":6.10234, "heading":-0.95533, "vx":-2.84046, "vy":2.49022, "omega":0.0, "ax":2.35527, "ay":2.58625, "alpha":0.00006, "fx":[39.24651,39.3688,39.29177,39.13749], "fy":[42.19271,45.41573,42.47697,42.36069]}, - {"t":0.76221, "x":2.59273, "y":6.19442, "heading":-0.95533, "vx":-2.75498, "vy":2.58409, "omega":0.0, "ax":5.13713, "ay":0.81067, "alpha":0.04027, "fx":[84.91,87.35527,84.58104,85.68736], "fy":[14.5711,13.68637,12.4562,13.34019]}, - {"t":0.79851, "x":2.49612, "y":6.28875, "heading":-0.95533, "vx":-2.56852, "vy":2.61351, "omega":0.00147, "ax":4.4515, "ay":-3.183, "alpha":0.36505, "fx":[74.61144,72.93749,73.87576,75.39305], "fy":[-51.09525,-54.57603,-53.9529,-52.61186]}, - {"t":0.8348, "x":2.40583, "y":6.38151, "heading":-0.95527, "vx":-2.40695, "vy":2.49798, "omega":0.01472, "ax":4.3401, "ay":-3.35545, "alpha":0.44857, "fx":[72.75538,71.18551,71.73741,73.71145], "fy":[-54.20216,-55.78871,-57.73329,-56.01081]}, - {"t":0.8711, "x":2.32133, "y":6.46997, "heading":-0.95474, "vx":-2.24942, "vy":2.37619, "omega":0.031, "ax":4.29964, "ay":-3.41462, "alpha":0.45363, "fx":[72.20595,70.16544,71.19313,73.12719], "fy":[-55.00244,-57.60172,-58.35851,-56.71771]}, - {"t":0.9074, "x":2.24251, "y":6.55396, "heading":-0.95361, "vx":-2.09336, "vy":2.25226, "omega":0.04746, "ax":4.27871, "ay":-3.44454, "alpha":0.3662, "fx":[71.69848,70.27675,70.86803,72.45311], "fy":[-56.0478,-57.34123,-58.82734,-57.45927]}, - {"t":0.94369, "x":2.16935, "y":6.63344, "heading":-0.95189, "vx":-1.93806, "vy":2.12723, "omega":0.06075, "ax":4.26591, "ay":-3.46263, "alpha":0.2192, "fx":[71.36703,70.36398,70.87903,71.83262], "fy":[-56.77967,-58.12868,-58.38131,-57.59172]}, - {"t":0.97999, "x":2.10182, "y":6.70837, "heading":-0.94969, "vx":-1.78323, "vy":2.00156, "omega":0.06871, "ax":4.25728, "ay":-3.47473, "alpha":0.02712, "fx":[71.03646,70.73918,71.03087,71.06045], "fy":[-57.86514,-57.9803,-57.93346,-57.9093]}, - {"t":1.01628, "x":2.0399, "y":6.77873, "heading":-0.94719, "vx":-1.62871, "vy":1.87544, "omega":0.06969, "ax":4.25104, "ay":-3.48341, "alpha":-0.26231, "fx":[70.59747,71.56816,71.21306,70.07278], "fy":[-58.84665,-58.63581,-56.90448,-57.88027]}, - {"t":1.05258, "x":1.98358, "y":6.84451, "heading":-0.94466, "vx":-1.47441, "vy":1.749, "omega":0.06017, "ax":4.24634, "ay":-3.48994, "alpha":-0.6311, "fx":[70.01923,72.97436,71.37289,68.7712], "fy":[-60.43121,-58.00889,-56.00671,-58.25592]}, - {"t":1.08888, "x":1.93287, "y":6.90569, "heading":-0.94248, "vx":-1.32029, "vy":1.62233, "omega":0.03727, "ax":4.13683, "ay":-3.61438, "alpha":-0.43513, "fx":[68.50148,70.26568,69.48769,67.58094], "fy":[-61.6546,-60.7174,-58.5236,-60.10457]}, - {"t":1.1236, "x":1.88952, "y":6.95984, "heading":-0.94118, "vx":-1.17665, "vy":1.49684, "omega":0.02216, "ax":3.94841, "ay":-3.8245, "alpha":-0.35251, "fx":[65.15228,66.83032,66.12301,65.16705], "fy":[-65.52318,-63.60728,-62.45126,-63.42855]}, - {"t":1.15832, "x":1.85104, "y":7.00951, "heading":-0.94041, "vx":-1.03955, "vy":1.36404, "omega":0.00992, "ax":3.78094, "ay":-3.99049, "alpha":-0.24599, "fx":[62.78756,63.70538,63.38119,62.23169], "fy":[-67.23166,-67.00793,-65.46771,-66.37106]}, - {"t":1.19304, "x":1.81723, "y":7.05447, "heading":-0.94007, "vx":-0.90827, "vy":1.22549, "omega":0.00138, "ax":3.63395, "ay":-4.12509, "alpha":-0.20116, "fx":[60.06843,61.14126,60.72243,60.37264], "fy":[-69.9199,-68.63995,-68.01036,-68.4826]}, - {"t":1.22776, "x":1.78788, "y":7.09453, "heading":-0.94002, "vx":-0.78209, "vy":1.08226, "omega":-0.00561, "ax":3.50464, "ay":-4.23574, "alpha":-0.12355, "fx":[58.31599,58.72931,58.62141,58.01568], "fy":[-70.92188,-70.98162,-70.03704,-70.49062]}, - {"t":1.26248, "x":1.76284, "y":7.12956, "heading":-0.94022, "vx":-0.66041, "vy":0.93518, "omega":-0.0099, "ax":3.3904, "ay":-4.32793, "alpha":-0.08293, "fx":[56.19932,56.7263,56.5446,56.59541], "fy":[-72.76123,-72.06689,-71.80955,-71.94014]}, - {"t":1.29721, "x":1.74195, "y":7.15942, "heading":-0.94056, "vx":-0.54269, "vy":0.78491, "omega":-0.01278, "ax":3.28905, "ay":-4.40562, "alpha":-0.022, "fx":[54.83525,54.81179,54.89728,54.76343], "fy":[-73.4356,-73.73269,-73.248,-73.34206]}, - {"t":1.33193, "x":1.72509, "y":7.18402, "heading":-0.941, "vx":-0.42848, "vy":0.63194, "omega":-0.01354, "ax":3.1987, "ay":-4.47181, "alpha":0.01686, "fx":[53.15456,53.20156,53.25056,53.67626], "fy":[-74.7579,-74.48609,-74.55037,-74.3775]}, - {"t":1.36665, "x":1.71214, "y":7.20326, "heading":-0.94147, "vx":-0.31742, "vy":0.47667, "omega":-0.01295, "ax":3.11776, "ay":-4.52875, "alpha":0.0779, "fx":[52.09442,51.61574,51.91853,52.25774], "fy":[-75.17457,-75.74675,-75.64564,-75.40129]}, - {"t":1.40137, "x":1.703, "y":7.21708, "heading":-0.94192, "vx":-0.20917, "vy":0.31942, "omega":-0.01025, "ax":3.04494, "ay":-4.57815, "alpha":0.13105, "fx":[50.94066,50.21882,50.63819,51.23289], "fy":[-75.83378,-76.54913,-76.65019,-76.22926]}, - {"t":1.43609, "x":1.69757, "y":7.22541, "heading":-0.94228, "vx":-0.10344, "vy":0.16046, "omega":-0.0057, "ax":2.97912, "ay":-4.62136, "alpha":0.16414, "fx":[49.80775,49.2056,49.38047,50.24849], "fy":[-76.46614,-77.00343,-77.64028,-77.03327]}, - {"t":1.47081, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":3.79867, "y":5.17214, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.52406, "ay":3.93603, "alpha":1.91869, "fx":[-72.32098,-81.31216,-78.52602,-69.49644], "fy":[72.49472,65.78651,58.69289,65.47304]}, + {"t":0.03726, "x":3.79553, "y":5.17487, "heading":-1.0472, "vx":-0.16855, "vy":0.14664, "omega":0.07148, "ax":-4.52521, "ay":3.93709, "alpha":1.46356, "fx":[-73.05677,-79.89957,-77.8237,-70.95197], "fy":[70.90526,65.77702,60.32978,65.50561]}, + {"t":0.07451, "x":3.78611, "y":5.18307, "heading":-1.04453, "vx":-0.33713, "vy":0.29332, "omega":0.12601, "ax":-4.52506, "ay":3.93703, "alpha":1.07098, "fx":[-73.71396,-78.56681,-77.23356,-72.20769], "fy":[69.48856,65.93597,61.64876,65.44044]}, + {"t":0.11177, "x":3.77041, "y":5.19673, "heading":-1.03984, "vx":-0.50572, "vy":0.43999, "omega":0.16591, "ax":-4.52488, "ay":3.93696, "alpha":0.75434, "fx":[-74.19475,-77.83151,-76.5756,-73.10835], "fy":[68.33546,65.63429,62.95288,65.58646]}, + {"t":0.14902, "x":3.74843, "y":5.21585, "heading":-1.03366, "vx":-0.67429, "vy":0.58666, "omega":0.19401, "ax":-4.52466, "ay":3.93688, "alpha":0.44312, "fx":[-74.74177,-76.69281,-76.16288,-74.09832], "fy":[67.19099,65.86352,63.93744,65.51185]}, + {"t":0.18628, "x":3.72017, "y":5.24044, "heading":-1.02643, "vx":-0.84286, "vy":0.73333, "omega":0.21052, "ax":-4.5244, "ay":3.93678, "alpha":0.2043, "fx":[-75.06401,-76.21505,-75.62498,-74.77452], "fy":[66.33048,65.54009,64.98614,65.64014]}, + {"t":0.22353, "x":3.68563, "y":5.27049, "heading":-1.01859, "vx":-1.01142, "vy":0.88, "omega":0.21813, "ax":-4.52407, "ay":3.93666, "alpha":-0.047, "fx":[-75.52614,-75.18561,-75.37109,-75.57355], "fy":[65.37016,65.89491,65.6987,65.52513]}, + {"t":0.26079, "x":3.64481, "y":5.30601, "heading":-1.01046, "vx":-1.17996, "vy":1.02666, "omega":0.21638, "ax":-4.52365, "ay":3.9365, "alpha":-0.2137, "fx":[-75.67374,-75.00646,-74.90766,-76.04038], "fy":[64.78351,65.48465,66.55531,65.65482]}, + {"t":0.29804, "x":3.59771, "y":5.34699, "heading":-1.0024, "vx":-1.34849, "vy":1.17332, "omega":0.20842, "ax":-4.52309, "ay":3.93629, "alpha":-0.41941, "fx":[-76.0867,-73.98109,-74.82125,-76.7018], "fy":[63.95301,66.04254,66.99536,65.47309]}, + {"t":0.3353, "x":3.54433, "y":5.39344, "heading":-0.99464, "vx":-1.517, "vy":1.31997, "omega":0.19279, "ax":-4.52229, "ay":3.936, "alpha":-0.50962, "fx":[-76.03152,-74.18321,-74.38959,-76.93345], "fy":[63.66862,65.4283,67.70013,65.64775]}, + {"t":0.37255, "x":3.48468, "y":5.44534, "heading":-0.98745, "vx":-1.68548, "vy":1.4666, "omega":0.17381, "ax":-4.52113, "ay":3.93553, "alpha":-0.66907, "fx":[-76.41196,-73.08309,-74.49888,-77.46642], "fy":[62.96274,66.29822,67.78706,65.36551]}, + {"t":0.40981, "x":3.41875, "y":5.50271, "heading":-0.98098, "vx":-1.85392, "vy":1.61322, "omega":0.14888, "ax":-4.51905, "ay":3.93492, "alpha":-0.64773, "fx":[-76.03709,-73.96019,-74.0169,-77.30738], "fy":[63.06057,65.31392,68.34175,65.65658]}, + {"t":0.44706, "x":3.34654, "y":5.56554, "heading":-0.97543, "vx":-2.02228, "vy":1.75982, "omega":0.12475, "ax":-4.515, "ay":3.93358, "alpha":-0.7776, "fx":[-76.45344,-72.60262,-74.31878,-77.67683], "fy":[62.3593,66.63379,68.05133,65.23888]}, + {"t":0.48432, "x":3.26807, "y":5.63384, "heading":-0.97078, "vx":-2.19049, "vy":1.90637, "omega":0.09578, "ax":-4.50235, "ay":3.93024, "alpha":-0.58015, "fx":[-75.4301,-74.30925,-73.58264,-76.88619], "fy":[63.09205,65.15179,68.21571,65.60149]}, + {"t":0.52158, "x":3.18334, "y":5.70759, "heading":-0.96722, "vx":-2.35822, "vy":2.05279, "omega":0.07416, "ax":-3.54141, "ay":2.98653, "alpha":-0.60738, "fx":[-59.71545,-56.9211,-58.47994,-61.01789], "fy":[47.15029,50.85955,51.57403,49.55253]}, + {"t":0.55883, "x":3.09302, "y":5.78614, "heading":-0.96445, "vx":-2.49016, "vy":2.16405, "omega":0.05154, "ax":0.03236, "ay":0.03795, "alpha":-0.38694, "fx":[0.1029,2.33844,0.52882,-0.8126], "fy":[-0.42727,0.44278,1.69319,0.82142]}, + {"t":0.59609, "x":3.00027, "y":5.86679, "heading":-0.96253, "vx":-2.48896, "vy":2.16547, "omega":0.03712, "ax":0.00458, "ay":0.00536, "alpha":-0.13065, "fx":[0.00628,0.45609,0.14616,-0.30293], "fy":[-0.16401,-0.55329,0.71736,0.35719]}, + {"t":0.63334, "x":2.90755, "y":5.94747, "heading":-0.96115, "vx":-2.48878, "vy":2.16567, "omega":0.03225, "ax":-0.06826, "ay":-0.07862, "alpha":-0.03486, "fx":[-1.15749,-1.01786,-1.10205,-1.27421], "fy":[-1.41769,-1.33083,-1.20335,-1.29022]}, + {"t":0.6706, "x":2.81478, "y":6.02809, "heading":-0.95995, "vx":-2.49133, "vy":2.16274, "omega":0.03095, "ax":-0.05701, "ay":-0.06574, "alpha":-0.01875, "fx":[-0.95812,-0.91301,-0.94251,-0.98776], "fy":[-1.00912,-1.49862,-0.92024,-0.95516]}, + {"t":0.70785, "x":2.72193, "y":6.10862, "heading":-0.9588, "vx":-2.49345, "vy":2.16029, "omega":0.03026, "ax":0.01715, "ay":0.01995, "alpha":0.03618, "fx":[0.4243,-0.13169,0.42894,0.42224], "fy":[0.30999,0.32724,0.35526,0.33789]}, + {"t":0.74511, "x":2.62904, "y":6.18912, "heading":-0.95767, "vx":-2.49281, "vy":2.16103, "omega":0.0316, "ax":0.11645, "ay":0.13375, "alpha":0.0078, "fx":[1.94077,1.9343,1.94219,1.94735], "fy":[2.27635,2.23641,2.18245,2.22271]}, + {"t":0.78236, "x":2.53625, "y":6.26972, "heading":-0.95649, "vx":-2.48847, "vy":2.16602, "omega":0.03189, "ax":0.76543, "ay":0.8681, "alpha":0.05576, "fx":[12.84313,12.42251,12.79173,12.98014], "fy":[14.5742,14.48927,14.36764,14.45235]}, + {"t":0.81962, "x":2.44408, "y":6.35102, "heading":-0.9553, "vx":-2.45996, "vy":2.19836, "omega":0.03397, "ax":1.95916, "ay":2.12147, "alpha":0.15564, "fx":[32.71966,32.28415,32.61334,33.0156], "fy":[34.95283,37.9189,34.11985,34.46414]}, + {"t":0.85687, "x":2.35379, "y":6.43439, "heading":-0.95404, "vx":-2.38697, "vy":2.27739, "omega":0.03977, "ax":4.13968, "ay":3.55109, "alpha":0.0782, "fx":[68.47291,70.09797,68.20464,69.25044], "fy":[60.12587,59.34015,58.28415,59.02977]}, + {"t":0.89413, "x":2.26773, "y":6.5217, "heading":-0.95255, "vx":-2.23274, "vy":2.40969, "omega":0.04268, "ax":4.76418, "ay":-3.60546, "alpha":0.29417, "fx":[79.82214,78.24132,79.17689,80.42582], "fy":[-58.01038,-62.71548,-60.4079,-59.27146]}, + {"t":0.93138, "x":2.18786, "y":6.60897, "heading":-0.95096, "vx":-2.05525, "vy":2.27537, "omega":0.05364, "ax":4.64624, "ay":-3.77694, "alpha":0.1697, "fx":[77.46579,77.47262,76.9498,77.91446], "fy":[-62.1936,-62.60508,-63.95564,-63.08467]}, + {"t":0.96864, "x":2.11451, "y":6.69112, "heading":-0.94897, "vx":-1.88215, "vy":2.13466, "omega":0.05996, "ax":4.60717, "ay":-3.83105, "alpha":0.04891, "fx":[76.93341,76.3642,76.86069,77.03876], "fy":[-63.30516,-65.12053,-63.57832,-63.44337]}, + {"t":1.0059, "x":2.04759, "y":6.76799, "heading":-0.94673, "vx":-1.71051, "vy":1.99193, "omega":0.06179, "ax":4.58753, "ay":-3.85775, "alpha":-0.25167, "fx":[76.09055,77.4986,76.63148,75.66707], "fy":[-65.20742,-64.04674,-63.55378,-64.41926]}, + {"t":1.04315, "x":1.98705, "y":6.83952, "heading":-0.94443, "vx":-1.5396, "vy":1.84821, "omega":0.05241, "ax":4.57574, "ay":-3.87362, "alpha":-0.54962, "fx":[75.66441,77.77456,77.05973,74.60276], "fy":[-66.3379,-65.37157,-62.22965,-64.34656]}, + {"t":1.08041, "x":1.93287, "y":6.90569, "heading":-0.94248, "vx":-1.36913, "vy":1.70389, "omega":0.03194, "ax":4.43957, "ay":-4.02254, "alpha":-0.37537, "fx":[73.63291,74.92127,74.60093,72.86714], "fy":[-68.10734,-68.077,-65.29103,-66.74012]}, + {"t":1.11364, "x":1.88981, "y":6.9601, "heading":-0.94142, "vx":-1.22158, "vy":1.5702, "omega":0.01946, "ax":4.22297, "ay":-4.25658, "alpha":-0.3302, "fx":[69.69023,71.32938,70.74445,69.81549], "fy":[-72.66788,-70.75684,-69.76216,-70.63381]}, + {"t":1.14688, "x":1.85155, "y":7.00993, "heading":-0.94077, "vx":-1.08123, "vy":1.42873, "omega":0.00849, "ax":4.04285, "ay":-4.42861, "alpha":-0.21694, "fx":[67.16802,67.97059,67.74588,66.68506], "fy":[-74.44436,-74.26096,-72.88271,-73.70347]}, + {"t":1.18011, "x":1.81784, "y":7.05497, "heading":-0.94049, "vx":-0.94686, "vy":1.28155, "omega":0.00127, "ax":3.89361, "ay":-4.56085, "alpha":-0.18504, "fx":[64.43579,65.44239,65.08018,64.66019], "fy":[-77.04637,-75.90484,-75.36707,-75.7905]}, + {"t":1.21335, "x":1.78853, "y":7.09505, "heading":-0.94045, "vx":-0.81746, "vy":1.12997, "omega":-0.00488, "ax":3.76862, "ay":-4.66504, "alpha":-0.11105, "fx":[62.71727,63.10283,63.01584,62.44856], "fy":[-78.0568,-78.04981,-77.26668,-77.68264]}, + {"t":1.24658, "x":1.76344, "y":7.13002, "heading":-0.94061, "vx":-0.69221, "vy":0.97492, "omega":-0.00857, "ax":3.6626, "ay":-4.74904, "alpha":-0.07464, "fx":[60.73979,61.25826,61.09884,61.11847], "fy":[-79.69924,-79.0975,-78.87148,-78.98888]}, + {"t":1.27982, "x":1.74246, "y":7.1598, "heading":-0.94089, "vx":-0.57048, "vy":0.81708, "omega":-0.01105, "ax":3.57181, "ay":-4.81796, "alpha":-0.02008, "fx":[59.54357,59.53744,59.60322,59.47714], "fy":[-80.33159,-80.51009,-80.16217,-80.24869]}, + {"t":1.31305, "x":1.72547, "y":7.1843, "heading":-0.94126, "vx":-0.45177, "vy":0.65696, "omega":-0.01171, "ax":3.49323, "ay":-4.87546, "alpha":0.01677, "fx":[58.07251,58.12914,58.16745,58.5525], "fy":[-81.44502,-81.2353,-81.28898,-81.11703]}, + {"t":1.34629, "x":1.71238, "y":7.20344, "heading":-0.94165, "vx":-0.33567, "vy":0.49492, "omega":-0.01116, "ax":3.42464, "ay":-4.92407, "alpha":0.06961, "fx":[57.19915,56.78066,57.02581,57.34255], "fy":[-81.82214,-82.24937,-82.24048,-82.0156]}, + {"t":1.37952, "x":1.70312, "y":7.21717, "heading":-0.94202, "vx":-0.22185, "vy":0.33127, "omega":-0.00884, "ax":3.36428, "ay":-4.96567, "alpha":0.11708, "fx":[56.25165,55.60837,55.95571,56.50826], "fy":[-82.367,-82.93954,-83.09036,-82.70413]}, + {"t":1.41276, "x":1.6976, "y":7.22544, "heading":-0.94231, "vx":-0.11004, "vy":0.16623, "omega":-0.00495, "ax":3.31081, "ay":-5.00162, "alpha":0.14901, "fx":[55.34671,54.7603,54.93302,55.71847], "fy":[-82.86221,-83.36118,-83.91745,-83.3576]}, + {"t":1.446, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/fetchToK.traj b/src/main/deploy/choreo/fetchToK.traj index 6c2af282..67544eb2 100644 --- a/src/main/deploy/choreo/fetchToK.traj +++ b/src/main/deploy/choreo/fetchToK.traj @@ -24,7 +24,7 @@ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.7 m / s", "val":3.7}}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.3 m / s", "val":3.3}}}, "enabled":true}, {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"maxAccel + 2.5 m / s ^ 2", "val":5.5}}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"3 m / s ^ 2", "val":3.0}}}, "enabled":true}], "targetDt":{ diff --git a/src/main/deploy/choreo/fetchToL.traj b/src/main/deploy/choreo/fetchToL.traj index eb1ba675..d80f868e 100644 --- a/src/main/deploy/choreo/fetchToL.traj +++ b/src/main/deploy/choreo/fetchToL.traj @@ -10,8 +10,8 @@ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":3.7}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":5.5}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":3.3}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":3.0}}, "enabled":true}], "targetDt":0.05 }, @@ -24,8 +24,8 @@ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.7 m / s", "val":3.7}}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"maxAccel + 2.5 m / s ^ 2", "val":5.5}}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.3 m / s", "val":3.3}}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"3 m / s ^ 2", "val":3.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", @@ -34,55 +34,54 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.16519,1.64871], + "waypoints":[0.0,1.15032,1.63379], "samples":[ - {"t":0.0, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.15229, "ay":-3.60231, "alpha":-0.82244, "fx":[68.21719,71.94694,70.14736,66.55539], "fy":[-62.96226,-59.90461,-57.14424,-60.18373]}, - {"t":0.03884, "x":1.69891, "y":7.22548, "heading":-0.94248, "vx":0.16127, "vy":-0.13991, "omega":-0.03194, "ax":4.15336, "ay":-3.60315, "alpha":-0.62044, "fx":[68.47816,71.28502,69.94133,67.23352], "fy":[-62.26883,-59.95511,-57.86576,-60.16125]}, - {"t":0.07768, "x":1.7083, "y":7.21733, "heading":-0.94372, "vx":0.32259, "vy":-0.27986, "omega":-0.05604, "ax":4.15333, "ay":-3.60302, "alpha":-0.44424, "fx":[68.70893,70.63041,69.78514,67.8116], "fy":[-61.63811,-60.08434,-58.43064,-60.0896]}, - {"t":0.11652, "x":1.72396, "y":7.20374, "heading":-0.9459, "vx":0.4839, "vy":-0.4198, "omega":-0.0733, "ax":4.1533, "ay":-3.60288, "alpha":-0.31524, "fx":[68.82926,70.3342,69.55465,68.21575], "fy":[-61.16427,-59.97989,-58.97991,-60.10916]}, - {"t":0.15536, "x":1.74589, "y":7.18472, "heading":-0.94874, "vx":0.64522, "vy":-0.55973, "omega":-0.08554, "ax":4.15325, "ay":-3.60272, "alpha":-0.17722, "fx":[69.02978,69.76703,69.46369,68.67052], "fy":[-60.66466,-60.14647,-59.37686,-60.03466]}, - {"t":0.1942, "x":1.77408, "y":7.16026, "heading":-0.95206, "vx":0.80653, "vy":-0.69966, "omega":-0.09242, "ax":4.15321, "ay":-3.60253, "alpha":-0.08541, "fx":[69.09799,69.6151,69.26303,68.95187], "fy":[-60.32724,-59.99751,-59.81238,-60.0728]}, - {"t":0.23304, "x":1.80854, "y":7.13037, "heading":-0.95565, "vx":0.96784, "vy":-0.83958, "omega":-0.09574, "ax":4.15315, "ay":-3.60231, "alpha":0.025, "fx":[69.27996,69.11039,69.21716,69.31677], "fy":[-59.91907,-60.19441,-60.08551,-59.99607]}, - {"t":0.27188, "x":1.84927, "y":7.09504, "heading":-0.95937, "vx":1.12915, "vy":-0.9795, "omega":-0.09477, "ax":4.15309, "ay":-3.60203, "alpha":0.08311, "fx":[69.28697,69.13215,69.01683,69.48428], "fy":[-59.69952,-59.96981,-60.44639,-60.06101]}, - {"t":0.31072, "x":1.89625, "y":7.05428, "heading":-0.96305, "vx":1.29045, "vy":-1.1194, "omega":-0.09154, "ax":4.15301, "ay":-3.60171, "alpha":0.17599, "fx":[69.47488,68.61086,69.03174,69.79732], "fy":[-59.349,-60.24571,-60.59683,-59.9633]}, - {"t":0.34956, "x":1.94951, "y":7.00809, "heading":-0.96661, "vx":1.45175, "vy":-1.25929, "omega":-0.0847, "ax":4.15291, "ay":-3.60129, "alpha":0.20102, "fx":[69.41137,68.82621,68.82004,69.85053], "fy":[-59.24287,-59.92376,-60.90054,-60.0601]}, - {"t":0.3884, "x":2.00903, "y":6.95646, "heading":-0.9699, "vx":1.61305, "vy":-1.39916, "omega":-0.0769, "ax":4.15278, "ay":-3.60076, "alpha":0.28051, "fx":[69.6241,68.2166,68.92095,70.13775], "fy":[-58.91219,-60.39795,-60.88419,-59.89766]}, - {"t":0.42724, "x":2.07481, "y":6.8994, "heading":-0.97289, "vx":1.77435, "vy":-1.53902, "omega":-0.066, "ax":4.15261, "ay":-3.60005, "alpha":0.26691, "fx":[69.46375,68.69095,68.68625,70.04727], "fy":[-58.96036,-59.86041,-61.1606,-60.06286]}, - {"t":0.46608, "x":2.14686, "y":6.83691, "heading":-0.97545, "vx":1.93563, "vy":-1.67884, "omega":-0.05564, "ax":4.15237, "ay":-3.59904, "alpha":0.32774, "fx":[69.66951,68.12542,68.82609,70.25135], "fy":[-58.7135,-60.25323,-61.08627,-59.92437]}, - {"t":0.50492, "x":2.22517, "y":6.76899, "heading":-0.97761, "vx":2.09691, "vy":-1.81863, "omega":-0.04291, "ax":4.15203, "ay":-3.59752, "alpha":0.32093, "fx":[69.61869,68.14811,68.82511,70.25732], "fy":[-58.81101,-60.04668,-61.07073,-59.94745]}, - {"t":0.54376, "x":2.30974, "y":6.69565, "heading":-0.97928, "vx":2.25817, "vy":-1.95835, "omega":-0.03044, "ax":4.15142, "ay":-3.595, "alpha":0.30498, "fx":[69.58898,68.23291,68.81722,70.16952], "fy":[-58.78619,-59.99943,-60.99347,-59.92886]}, - {"t":0.5826, "x":2.40058, "y":6.61687, "heading":-0.98046, "vx":2.41941, "vy":-2.09798, "omega":-0.0186, "ax":4.15027, "ay":-3.58983, "alpha":0.21764, "fx":[69.29553,68.94679,68.62143,69.86857], "fy":[-58.89253,-59.71434,-60.85726,-59.89854]}, - {"t":0.62144, "x":2.49768, "y":6.53268, "heading":-0.98118, "vx":2.58061, "vy":-2.23741, "omega":-0.01014, "ax":4.14702, "ay":-3.57374, "alpha":0.19199, "fx":[69.4144,68.33701,68.95805,69.80594], "fy":[-58.56374,-60.49416,-59.93888,-59.29321]}, - {"t":0.66028, "x":2.60104, "y":6.44308, "heading":-0.98157, "vx":2.74168, "vy":-2.37621, "omega":-0.00269, "ax":0.82944, "ay":-1.82409, "alpha":0.06235, "fx":[14.01718,13.24119,13.98863,14.05817], "fy":[-30.37816,-30.4101,-30.43198,-30.40637]}, - {"t":0.69912, "x":2.70815, "y":6.34941, "heading":-0.98168, "vx":2.77389, "vy":-2.44706, "omega":-0.00026, "ax":-1.25856, "ay":-1.39764, "alpha":-0.01938, "fx":[-20.99237,-20.90905,-20.96357,-21.05309], "fy":[-23.41516,-23.1865,-23.26526,-23.3248]}, - {"t":0.73796, "x":2.81494, "y":6.25332, "heading":-0.98169, "vx":2.72501, "vy":-2.50135, "omega":-0.00102, "ax":-4.74409, "ay":-0.00285, "alpha":-0.02331, "fx":[-78.87304,-79.62574,-78.68105,-79.14707], "fy":[-0.42154,-0.09632,0.32719,0.00035]}, - {"t":0.7768, "x":2.9172, "y":6.15616, "heading":-0.98173, "vx":2.54075, "vy":-2.50146, "omega":-0.00192, "ax":-4.25556, "ay":3.44525, "alpha":-0.21706, "fx":[-71.22695,-70.15265,-70.71307,-71.65999], "fy":[56.25609,58.37094,57.93633,57.15898]}, - {"t":0.81564, "x":3.01267, "y":6.06161, "heading":-0.9818, "vx":2.37547, "vy":-2.36764, "omega":-0.01035, "ax":-4.20633, "ay":3.52436, "alpha":-0.27827, "fx":[-70.40655,-69.37089,-69.70433,-70.98803], "fy":[57.6981,58.67579,59.82558,58.79778]}, - {"t":0.85448, "x":3.10176, "y":5.97231, "heading":-0.98221, "vx":2.21209, "vy":-2.23076, "omega":-0.02116, "ax":-4.18953, "ay":3.55068, "alpha":-0.32393, "fx":[-70.24522,-68.85534,-69.41191,-70.83746], "fy":[57.91179,59.31936,60.34167,59.17985]}, - {"t":0.89332, "x":3.18452, "y":5.88834, "heading":-0.98303, "vx":2.04937, "vy":-2.09285, "omega":-0.03374, "ax":-4.18103, "ay":3.56387, "alpha":-0.32736, "fx":[-70.07354,-68.79194,-69.21316,-70.70426], "fy":[58.1853,59.31374,60.66997,59.46312]}, - {"t":0.93216, "x":3.26096, "y":5.80974, "heading":-0.98434, "vx":1.88698, "vy":-1.95443, "omega":-0.04646, "ax":-4.17583, "ay":3.57186, "alpha":-0.33793, "fx":[-70.07798,-68.45671,-69.20994,-70.69197], "fy":[58.18007,59.93116,60.61888,59.43462]}, - {"t":0.97099, "x":3.3311, "y":5.73653, "heading":-0.98614, "vx":1.72479, "vy":-1.8157, "omega":-0.05958, "ax":-4.17235, "ay":3.5772, "alpha":-0.30062, "fx":[-69.8919,-68.78591,-69.05346,-70.47296], "fy":[58.49783,59.50509,60.82907,59.68859]}, - {"t":1.00983, "x":3.39495, "y":5.6687, "heading":-0.98846, "vx":1.56274, "vy":-1.67676, "omega":-0.07126, "ax":-4.16983, "ay":3.58104, "alpha":-0.29009, "fx":[-69.91898,-68.52857,-69.15501,-70.43365], "fy":[58.54107,59.98484,60.63471,59.61602]}, - {"t":1.04867, "x":3.4525, "y":5.60628, "heading":-0.99122, "vx":1.40079, "vy":-1.53768, "omega":-0.08253, "ax":-4.16794, "ay":3.58392, "alpha":-0.23423, "fx":[-69.75576,-68.86382,-69.09404,-70.19642], "fy":[58.86696,59.6511,60.66426,59.78629]}, - {"t":1.08751, "x":3.50376, "y":5.54926, "heading":-0.99443, "vx":1.2389, "vy":-1.39848, "omega":-0.09162, "ax":-4.16646, "ay":3.58616, "alpha":-0.19624, "fx":[-69.73919,-68.77366,-69.21574,-70.08268], "fy":[58.98854,60.01934,60.39683,59.71378]}, - {"t":1.12635, "x":3.54874, "y":5.49765, "heading":-0.99799, "vx":1.07708, "vy":-1.25919, "omega":-0.09925, "ax":-4.16527, "ay":3.58797, "alpha":-0.12378, "fx":[-69.57682,-69.12684,-69.21826,-69.80999], "fy":[59.34521,59.74357,60.30778,59.84213]}, - {"t":1.16519, "x":3.58743, "y":5.45145, "heading":-1.00184, "vx":0.9153, "vy":-1.11984, "omega":-0.10405, "ax":-2.2234, "ay":2.0056, "alpha":-0.11345, "fx":[-37.17775,-36.65869,-36.96374,-37.45198], "fy":[32.90852,33.69312,33.73426,33.39363]}, - {"t":1.19541, "x":3.61407, "y":5.41852, "heading":-1.00499, "vx":0.84811, "vy":-1.05923, "omega":-0.10748, "ax":-2.14707, "ay":2.09161, "alpha":-0.09804, "fx":[-35.78431,-35.40126,-35.62968,-36.34738], "fy":[34.74979,34.75197,35.13646,34.82609]}, - {"t":1.22563, "x":3.63872, "y":5.38747, "heading":-1.00824, "vx":0.78323, "vy":-0.99602, "omega":-0.11044, "ax":-2.08112, "ay":2.15757, "alpha":-0.06756, "fx":[-34.76446,-34.44458,-34.63161,-34.92466], "fy":[35.65341,36.12911,36.14051,35.93964]}, - {"t":1.25585, "x":3.66144, "y":5.35835, "heading":-1.01157, "vx":0.72034, "vy":-0.93082, "omega":-0.11249, "ax":-2.02512, "ay":2.2105, "alpha":-0.04327, "fx":[-33.72208,-33.55829,-33.66233,-34.08813], "fy":[36.88332,36.76916,36.94588,36.79337]}, - {"t":1.28607, "x":3.68229, "y":5.33123, "heading":-1.01497, "vx":0.65914, "vy":-0.86402, "omega":-0.11379, "ax":-1.9771, "ay":2.25377, "alpha":-0.01155, "fx":[-32.97623,-32.89812,-32.94967,-33.00536], "fy":[37.46455,37.72749,37.56246,37.52255]}, - {"t":1.31629, "x":3.7013, "y":5.30615, "heading":-1.01841, "vx":0.59939, "vy":-0.79591, "omega":-0.11414, "ax":-1.93556, "ay":2.28974, "alpha":0.02167, "fx":[-32.17926,-32.28893,-32.24253,-32.34842], "fy":[38.39695,38.13394,38.05662,38.08792]}, - {"t":1.34651, "x":3.71853, "y":5.28315, "heading":-1.02186, "vx":0.5409, "vy":-0.72671, "omega":-0.11349, "ax":-1.89931, "ay":2.32006, "alpha":0.05982, "fx":[-31.60731,-31.83981,-31.72371,-31.47133], "fy":[38.82145,38.85121,38.43289,38.59142]}, - {"t":1.37673, "x":3.73401, "y":5.26224, "heading":-1.02529, "vx":0.4835, "vy":-0.6566, "omega":-0.11168, "ax":-1.86743, "ay":2.34593, "alpha":0.10511, "fx":[-30.96907,-31.43707,-31.19982,-30.91055], "fy":[39.59311,39.12696,38.72102,38.98123]}, - {"t":1.40695, "x":3.74777, "y":5.24347, "heading":-1.02867, "vx":0.42707, "vy":-0.58571, "omega":-0.1085, "ax":-1.8392, "ay":2.36825, "alpha":0.15398, "fx":[-30.50695,-31.15126,-30.81978,-30.15628], "fy":[39.94622,39.7028,38.92393,39.33747]}, - {"t":1.43717, "x":3.75984, "y":5.22685, "heading":-1.03194, "vx":0.37148, "vy":-0.51414, "omega":-0.10385, "ax":-1.81404, "ay":2.38769, "alpha":0.21541, "fx":[-29.97136,-30.92031,-30.43377,-29.63105], "fy":[40.63779,39.90018,39.05572,39.61266]}, - {"t":1.46739, "x":3.77023, "y":5.21241, "heading":-1.03508, "vx":0.31666, "vy":-0.44198, "omega":-0.09734, "ax":-1.79148, "ay":2.40475, "alpha":0.28072, "fx":[-29.57505,-30.7778,-30.16103,-28.93849], "fy":[40.98331,40.38325,39.11343,39.86427]}, - {"t":1.49761, "x":3.77899, "y":5.20015, "heading":-1.03802, "vx":0.26253, "vy":-0.36931, "omega":-0.08886, "ax":-1.77115, "ay":2.41985, "alpha":0.36371, "fx":[-29.09954,-30.70533,-29.88554,-28.40628], "fy":[41.64906,40.54465,39.10296,40.05428]}, - {"t":1.52783, "x":3.78611, "y":5.19009, "heading":-1.04071, "vx":0.209, "vy":-0.29618, "omega":-0.07787, "ax":-1.75273, "ay":2.4333, "alpha":0.45448, "fx":[-28.73915,-30.70851,-29.70767,-27.71349], "fy":[42.03948,40.97402,39.01321,40.22085]}, - {"t":1.55805, "x":3.79163, "y":5.18225, "heading":-1.04306, "vx":0.15603, "vy":-0.22265, "omega":-0.06413, "ax":-1.73598, "ay":2.44534, "alpha":0.57103, "fx":[-28.29398,-30.81185,-29.53776,-27.10824], "fy":[42.74679,41.11624,38.84765,40.34018]}, - {"t":1.58827, "x":3.79555, "y":5.17664, "heading":-1.045, "vx":0.10357, "vy":-0.14875, "omega":-0.04687, "ax":-1.72068, "ay":2.4562, "alpha":0.69647, "fx":[-27.93866,-30.97618,-29.44562,-26.37087], "fy":[43.22594,41.52153,38.59309,40.43409]}, - {"t":1.61849, "x":3.79789, "y":5.17327, "heading":-1.04642, "vx":0.05157, "vy":-0.07452, "omega":-0.02583, "ax":-1.70664, "ay":2.46603, "alpha":0.85465, "fx":[-27.5118,-31.35309,-29.3693,-25.56151], "fy":[43.90116,41.68333,38.30884,40.53654]}, - {"t":1.64871, "x":3.79867, "y":5.17214, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.4841, "ay":-3.98182, "alpha":-0.7965, "fx":[73.67646,77.31805,75.7966,72.19983], "fy":[-69.23598,-66.14315,-63.50587,-66.61529]}, + {"t":0.03967, "x":1.6993, "y":7.22507, "heading":-0.94248, "vx":0.17787, "vy":-0.15794, "omega":-0.03159, "ax":4.48514, "ay":-3.98283, "alpha":-0.59639, "fx":[73.96011,76.68211,75.55546,72.86278], "fy":[-68.54283,-66.21402,-64.24006,-66.57071]}, + {"t":0.07933, "x":1.70989, "y":7.21567, "heading":-0.94373, "vx":0.35578, "vy":-0.31593, "omega":-0.05525, "ax":4.48497, "ay":-3.98279, "alpha":-0.42519, "fx":[74.19212,76.10004,75.34402,73.41316], "fy":[-67.91633,-66.32734,-64.82941,-66.49144]}, + {"t":0.119, "x":1.72753, "y":7.2, "heading":-0.94592, "vx":0.53368, "vy":-0.47391, "omega":-0.07212, "ax":4.48477, "ay":-3.98273, "alpha":-0.286, "fx":[74.35956,75.71338,75.11542,73.8474], "fy":[-67.41581,-66.28754,-65.38252,-66.47516]}, + {"t":0.15866, "x":1.75222, "y":7.17807, "heading":-0.94878, "vx":0.71157, "vy":-0.63189, "omega":-0.08346, "ax":4.48452, "ay":-3.98267, "alpha":-0.15252, "fx":[74.55096,75.22699,74.96664,74.2745], "fy":[-66.92175,-66.41559,-65.81514,-66.40438]}, + {"t":0.19833, "x":1.78398, "y":7.14987, "heading":-0.95209, "vx":0.88946, "vy":-0.78987, "omega":-0.08951, "ax":4.48421, "ay":-3.98258, "alpha":-0.04993, "fx":[74.66358,74.97647,74.77188,74.58684], "fy":[-66.55324,-66.33649,-66.25123,-66.41012]}, + {"t":0.238, "x":1.82279, "y":7.11541, "heading":-0.95564, "vx":1.06733, "vy":-0.94784, "omega":-0.09149, "ax":4.48381, "ay":-3.98249, "alpha":0.05585, "fx":[74.82993,74.54493,74.67362,74.92322], "fy":[-66.15127,-66.50033,-66.55459,-66.33854]}, + {"t":0.27766, "x":1.86865, "y":7.07468, "heading":-0.95927, "vx":1.24518, "vy":-1.10581, "omega":-0.08928, "ax":4.48328, "ay":-3.98235, "alpha":0.12277, "fx":[74.87205,74.47193,74.48137,75.11098], "fy":[-65.91163,-66.33061,-66.9122,-66.38102]}, + {"t":0.31733, "x":1.92157, "y":7.02768, "heading":-0.96281, "vx":1.42302, "vy":-1.26378, "omega":-0.08441, "ax":4.48253, "ay":-3.98216, "alpha":0.20889, "fx":[75.03609,74.0063,74.45564,75.38843], "fy":[-65.56638,-66.61503,-67.06429,-66.27726]}, + {"t":0.357, "x":1.98154, "y":6.97442, "heading":-0.96616, "vx":1.60082, "vy":-1.42173, "omega":-0.07612, "ax":4.48143, "ay":-3.98186, "alpha":0.24378, "fx":[75.01203,74.06398,74.2784,75.45856], "fy":[-65.45472,-66.35428,-67.34015,-66.35375]}, + {"t":0.39666, "x":2.04857, "y":6.91489, "heading":-0.96918, "vx":1.77858, "vy":-1.57968, "omega":-0.06645, "ax":4.47954, "ay":-3.98142, "alpha":0.29191, "fx":[75.08363,73.75637,74.25497,75.59246], "fy":[-65.27092,-66.49602,-67.4077,-66.29869]}, + {"t":0.43633, "x":2.12264, "y":6.8491, "heading":-0.97182, "vx":1.95627, "vy":-1.73761, "omega":-0.05487, "ax":4.47576, "ay":-3.98058, "alpha":0.28436, "fx":[74.94507,73.92804,74.0787,75.4834], "fy":[-65.25474,-66.29853,-67.51234,-66.35166]}, + {"t":0.47599, "x":2.20376, "y":6.77705, "heading":-0.97399, "vx":2.13381, "vy":-1.8955, "omega":-0.04359, "ax":4.46453, "ay":-3.97815, "alpha":0.28918, "fx":[74.83442,73.4757,74.02678,75.34964], "fy":[-65.11367,-66.72372,-67.24666,-66.17135]}, + {"t":0.51566, "x":2.29191, "y":6.69873, "heading":-0.97572, "vx":2.3109, "vy":-2.0533, "omega":-0.03212, "ax":3.92803, "ay":-3.46718, "alpha":0.25881, "fx":[65.85196,64.3975,65.32929,66.33454], "fy":[-56.88432,-58.02688,-58.53257,-57.74127]}, + {"t":0.55533, "x":2.38667, "y":6.61455, "heading":-0.977, "vx":2.46671, "vy":-2.19083, "omega":-0.02186, "ax":0.0781, "ay":0.087, "alpha":0.1325, "fx":[1.39361,0.835,1.21096,1.76768], "fy":[2.02648,1.22,1.08731,1.46694]}, + {"t":0.59499, "x":2.48457, "y":6.52772, "heading":-0.97786, "vx":2.46981, "vy":-2.18738, "omega":-0.0166, "ax":-0.01177, "ay":-0.01329, "alpha":0.0589, "fx":[-0.09162,-0.5608,-0.14025,0.00764], "fy":[-0.10173,-0.19824,-0.34146,-0.24484]}, + {"t":0.63466, "x":2.58253, "y":6.44094, "heading":-0.97852, "vx":2.46934, "vy":-2.18791, "omega":-0.01426, "ax":0.01698, "ay":0.01917, "alpha":0.01357, "fx":[0.29082,0.24371,0.27539,0.32249], "fy":[0.3229,0.43416,0.24505,0.27635]}, + {"t":0.67433, "x":2.68049, "y":6.35417, "heading":-0.97909, "vx":2.47001, "vy":-2.18715, "omega":-0.01372, "ax":-0.00335, "ay":-0.00379, "alpha":-0.01753, "fx":[-0.09286,0.06798,-0.08052,-0.11822], "fy":[-0.09084,-0.06842,-0.0355,-0.0577]}, + {"t":0.71399, "x":2.77847, "y":6.26742, "heading":-0.97963, "vx":2.46988, "vy":-2.1873, "omega":-0.01442, "ax":-0.09455, "ay":-0.10659, "alpha":-0.0127, "fx":[-1.58813,-1.51484,-1.56409,-1.63737], "fy":[-1.96143,-1.44199,-1.8243,-1.87929]}, + {"t":0.75366, "x":2.87636, "y":6.18057, "heading":-0.98021, "vx":2.46613, "vy":-2.19153, "omega":-0.01492, "ax":-0.46299, "ay":-0.51664, "alpha":-0.09661, "fx":[-7.92367,-7.03292,-7.85873,-8.05587], "fy":[-8.7651,-8.64585,-8.45727,-8.58016]}, + {"t":0.79332, "x":2.97382, "y":6.09323, "heading":-0.9808, "vx":2.44777, "vy":-2.21202, "omega":-0.01876, "ax":-1.48962, "ay":-1.603, "alpha":-0.143, "fx":[-24.91557,-24.3429,-24.72003,-25.34628], "fy":[-27.45415,-26.24861,-26.37811,-26.80419]}, + {"t":0.83299, "x":3.06974, "y":6.00423, "heading":-0.98154, "vx":2.38868, "vy":-2.2756, "omega":-0.02443, "ax":-5.47935, "ay":1.65125, "alpha":-0.16289, "fx":[-91.32201,-91.43051,-90.79449,-91.80563], "fy":[26.50621,27.73806,28.36227,27.49555]}, + {"t":0.87266, "x":3.16018, "y":5.91526, "heading":-0.98251, "vx":2.17133, "vy":-2.2101, "omega":-0.03089, "ax":-4.62028, "ay":3.79739, "alpha":-0.30475, "fx":[-77.43915,-76.06797,-76.59562,-77.96872], "fy":[62.0015,63.70671,64.31956,63.17485]}, + {"t":0.91232, "x":3.24268, "y":5.83059, "heading":-0.98374, "vx":1.98806, "vy":-2.05948, "omega":-0.04298, "ax":-4.55656, "ay":3.88814, "alpha":-0.32474, "fx":[-76.38418,-75.04287,-75.45378,-76.942], "fy":[63.58997,64.78513,66.06025,64.81833]}, + {"t":0.95199, "x":3.31795, "y":5.75195, "heading":-0.98544, "vx":1.80732, "vy":-1.90525, "omega":-0.05586, "ax":-4.53428, "ay":3.9191, "alpha":-0.32312, "fx":[-76.0497,-74.58338,-75.11532,-76.58897], "fy":[64.06083,65.53975,66.46599,65.25175]}, + {"t":0.99166, "x":3.38607, "y":5.67946, "heading":-0.98766, "vx":1.62746, "vy":-1.74979, "omega":-0.06868, "ax":-4.52296, "ay":3.93471, "alpha":-0.30009, "fx":[-75.82304,-74.49695,-74.94747,-76.31452], "fy":[64.48524,65.58341,66.70862,65.58137]}, + {"t":1.03132, "x":3.44707, "y":5.61315, "heading":-0.99038, "vx":1.44806, "vy":-1.59372, "omega":-0.08058, "ax":-4.51604, "ay":3.94417, "alpha":-0.25875, "fx":[-75.66436,-74.47385,-74.89892,-76.08377], "fy":[64.73929,65.90658,66.65661,65.68714]}, + {"t":1.07099, "x":3.50096, "y":5.55304, "heading":-0.99358, "vx":1.26892, "vy":-1.43727, "omega":-0.09084, "ax":-4.51138, "ay":3.95053, "alpha":-0.20252, "fx":[-75.48652,-74.63224,-74.87223,-75.81891], "fy":[65.11092,65.80573,66.63387,65.86293]}, + {"t":1.11065, "x":3.54774, "y":5.49913, "heading":-0.99718, "vx":1.08997, "vy":-1.28056, "omega":-0.09888, "ax":-4.50803, "ay":3.95507, "alpha":-0.14098, "fx":[-75.36553,-74.68372,-74.94615,-75.59149], "fy":[65.35563,66.09679,66.39266,65.87164]}, + {"t":1.15032, "x":3.58743, "y":5.45145, "heading":-1.0011, "vx":0.91115, "vy":-1.12368, "omega":-0.10447, "ax":-2.19966, "ay":2.0302, "alpha":-0.13051, "fx":[-36.78498,-36.24263,-36.54364,-37.09763], "fy":[33.37111,33.81099,34.2833,33.90449]}, + {"t":1.18054, "x":3.61396, "y":5.41842, "heading":-1.00426, "vx":0.84469, "vy":-1.06234, "omega":-0.10841, "ax":-2.11903, "ay":2.11952, "alpha":-0.10458, "fx":[-35.38929,-34.94342,-35.19945,-35.76081], "fy":[35.0737,35.23906,35.6661,35.34642]}, + {"t":1.21075, "x":3.63851, "y":5.38729, "heading":-1.00753, "vx":0.78066, "vy":-0.99829, "omega":-0.11157, "ax":-2.05256, "ay":2.1844, "alpha":-0.07876, "fx":[-34.29336,-33.94384,-34.14163,-34.48211], "fy":[36.10685,36.46268,36.65442,36.4279]}, + {"t":1.24097, "x":3.66117, "y":5.35812, "heading":-1.01091, "vx":0.71864, "vy":-0.93228, "omega":-0.11395, "ax":-1.99839, "ay":2.23443, "alpha":-0.05052, "fx":[-33.32135,-33.11362,-33.2368,-33.57671], "fy":[37.17252,37.18796,37.3936,37.23349]}, + {"t":1.27119, "x":3.68197, "y":5.33097, "heading":-1.01435, "vx":0.65825, "vy":-0.86477, "omega":-0.11548, "ax":-1.95349, "ay":2.27407, "alpha":-0.01852, "fx":[-32.58488,-32.48897,-32.54807,-32.63287], "fy":[37.81048,37.98792,37.94295,37.88947]}, + {"t":1.3014, "x":3.70097, "y":5.30588, "heading":-1.01784, "vx":0.59922, "vy":-0.79605, "omega":-0.11604, "ax":-1.91573, "ay":2.3062, "alpha":0.0181, "fx":[-31.87959,-31.96715,-31.93055,-31.96012], "fy":[38.59151,38.42484,38.36136,38.39536]}, + {"t":1.33162, "x":3.7182, "y":5.28288, "heading":-1.02134, "vx":0.54134, "vy":-0.72637, "omega":-0.11549, "ax":-1.88358, "ay":2.33273, "alpha":0.05769, "fx":[-31.34251,-31.57625,-31.45953,-31.21506], "fy":[39.05173,38.99961,38.66562,38.82517]}, + {"t":1.36184, "x":3.7337, "y":5.26199, "heading":-1.02483, "vx":0.48442, "vy":-0.65588, "omega":-0.11375, "ax":-1.85588, "ay":2.35499, "alpha":0.10652, "fx":[-30.79508,-31.26888,-31.029,-30.65365], "fy":[39.68747,39.29545,38.88201,39.16141]}, + {"t":1.39206, "x":3.74749, "y":5.24325, "heading":-1.02827, "vx":0.42834, "vy":-0.58472, "omega":-0.11053, "ax":-1.83179, "ay":2.37392, "alpha":0.15862, "fx":[-30.37275,-31.048,-30.70257,-30.01696], "fy":[40.08368,39.73717,39.01592,39.45166]}, + {"t":1.42227, "x":3.75959, "y":5.22667, "heading":-1.03161, "vx":0.37299, "vy":-0.51299, "omega":-0.10574, "ax":-1.81065, "ay":2.3902, "alpha":0.22573, "fx":[-29.91629,-30.91551,-30.40752,-29.49145], "fy":[40.66204,39.96088,39.0746,39.67674]}, + {"t":1.45249, "x":3.77004, "y":5.21226, "heading":-1.03481, "vx":0.31828, "vy":-0.44076, "omega":-0.09892, "ax":-1.79196, "ay":2.40436, "alpha":0.27732, "fx":[-29.59012,-30.82628,-30.17859,-28.88943], "fy":[40.86613,40.39206,39.12768,39.93202]}, + {"t":1.48271, "x":3.77884, "y":5.20003, "heading":-1.0378, "vx":0.26413, "vy":-0.36811, "omega":-0.09054, "ax":-1.77532, "ay":2.41677, "alpha":0.36341, "fx":[-29.18619,-30.89356,-29.97362,-28.3213], "fy":[41.47107,40.37956,39.1322,40.16251]}, + {"t":1.51292, "x":3.78601, "y":5.19002, "heading":-1.04053, "vx":0.21049, "vy":-0.29508, "omega":-0.07955, "ax":-1.76041, "ay":2.42773, "alpha":0.45612, "fx":[-28.85701,-30.93799,-29.8464,-27.73905], "fy":[41.89686,40.72935,38.97986,40.27049]}, + {"t":1.54314, "x":3.79156, "y":5.18221, "heading":-1.04293, "vx":0.15729, "vy":-0.22173, "omega":-0.06577, "ax":-1.74697, "ay":2.43749, "alpha":0.57746, "fx":[-28.46763,-31.1258,-29.73166,-27.15962], "fy":[42.56916,40.94303,38.84577,40.16934]}, + {"t":1.57336, "x":3.79552, "y":5.17662, "heading":-1.04492, "vx":0.10451, "vy":-0.14807, "omega":-0.04832, "ax":-1.73481, "ay":2.44623, "alpha":0.70645, "fx":[-28.14641,-31.3337,-29.69301,-26.50047], "fy":[43.08322,41.26648,38.53415,40.22622]}, + {"t":1.60357, "x":3.79789, "y":5.17326, "heading":-1.04638, "vx":0.05209, "vy":-0.07416, "omega":-0.02698, "ax":-1.72374, "ay":2.4541, "alpha":0.89276, "fx":[-27.75144,-31.75227,-29.71869,-25.71336], "fy":[43.82925,41.50413,37.97965,40.32185]}, + {"t":1.63379, "x":3.79867, "y":5.17214, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/startToG.traj b/src/main/deploy/choreo/startToG.traj index 100d2655..9d343805 100644 --- a/src/main/deploy/choreo/startToG.traj +++ b/src/main/deploy/choreo/startToG.traj @@ -15,7 +15,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":6.1663548}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"G.x", "val":5.827323}, "y":{"exp":"G.y", "val":3.7209}, "heading":{"exp":"G.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, diff --git a/src/main/deploy/choreo/startToH.traj b/src/main/deploy/choreo/startToH.traj index bc2f97b5..d88a07b2 100644 --- a/src/main/deploy/choreo/startToH.traj +++ b/src/main/deploy/choreo/startToH.traj @@ -15,7 +15,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":6.1663548}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"H.x", "val":5.827323}, "y":{"exp":"H.y", "val":4.0509}, "heading":{"exp":"H.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, diff --git a/src/main/deploy/choreo/startToI.traj b/src/main/deploy/choreo/startToI.traj index 8d6c56e4..a3b6f4d6 100644 --- a/src/main/deploy/choreo/startToI.traj +++ b/src/main/deploy/choreo/startToI.traj @@ -17,7 +17,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":6.1663548}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"6.0711771011352536 m", "val":6.0711771011352536}, "y":{"exp":"5.008563137054443 m", "val":5.008563137054443}, "heading":{"exp":"I.heading", "val":4.1887902047863905}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"I.x", "val":5.422460748154253}, "y":{"exp":"I.y", "val":5.032141990263579}, "heading":{"exp":"I.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ diff --git a/src/main/deploy/choreo/startToJ.traj b/src/main/deploy/choreo/startToJ.traj index 09f18911..e2130820 100644 --- a/src/main/deploy/choreo/startToJ.traj +++ b/src/main/deploy/choreo/startToJ.traj @@ -3,28 +3,28 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.821461200714111, "y":5.521196365356445, "heading":-2.1948197779926253, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.1008875, "y":6.1663548, "heading":3.141592653589793, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.498868942260742, "y":5.783792495727539, "heading":-2.1630637240599624, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":5.1366723649053885, "y":5.197141990263579, "heading":4.1887902047863905, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":3.7}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":3.3}}, "enabled":true}, {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":5.0}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":3.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.821461200714111 m", "val":5.821461200714111}, "y":{"exp":"5.521196365356445 m", "val":5.521196365356445}, "heading":{"exp":"-2.1948197779926253 rad", "val":-2.1948197779926253}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":6.1663548}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.498868942260742 m", "val":5.498868942260742}, "y":{"exp":"5.783792495727539 m", "val":5.783792495727539}, "heading":{"exp":"-2.1630637240599624 rad", "val":-2.1630637240599624}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"J.x", "val":5.1366723649053885}, "y":{"exp":"J.y", "val":5.197141990263579}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.7 m / s", "val":3.7}}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.3 m / s", "val":3.3}}}, "enabled":true}, {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"maxAccel + 2 m / s ^ 2", "val":5.0}}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"maxAccel", "val":3.0}}}, "enabled":true}], "targetDt":{ @@ -34,61 +34,62 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.8336,1.55921], + "waypoints":[0.0,0.93731,1.62847], "samples":[ - {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.04125, "ay":2.93812, "alpha":13.03994, "fx":[-41.14175,-33.62361,-91.70446,-102.99328], "fy":[17.37688,89.54633,75.46422,13.52059]}, - {"t":0.02874, "x":7.09922, "y":5.07689, "heading":-3.14159, "vx":-0.11617, "vy":0.08446, "omega":0.37483, "ax":-4.06232, "ay":2.91182, "alpha":10.9571, "fx":[-46.03885,-39.10651,-88.17145,-97.55063], "fy":[22.44002,82.51997,71.00251,18.19191]}, - {"t":0.05749, "x":7.0942, "y":5.08052, "heading":-3.13082, "vx":-0.23294, "vy":0.16816, "omega":0.68979, "ax":-4.0841, "ay":2.88098, "alpha":9.0422, "fx":[-50.04404,-44.6112,-85.22713,-92.43778], "fy":[27.10867,76.10242,66.51837,22.36865]}, - {"t":0.08623, "x":7.08582, "y":5.08655, "heading":-3.11099, "vx":-0.35033, "vy":0.25097, "omega":0.94971, "ax":-4.10848, "ay":2.84588, "alpha":7.28208, "fx":[-53.7166,-49.8113,-82.66982,-87.74776], "fy":[31.2475,70.23163,62.10617,26.17245]}, - {"t":0.11498, "x":7.07405, "y":5.09494, "heading":-3.08369, "vx":-0.46843, "vy":0.33277, "omega":1.15903, "ax":-4.13588, "ay":2.80565, "alpha":5.66019, "fx":[-57.08958,-54.78284,-80.36169,-83.53816], "fy":[34.79117,64.68255,57.88491,29.71662]}, - {"t":0.14372, "x":7.05888, "y":5.10566, "heading":-3.05037, "vx":-0.58732, "vy":0.41342, "omega":1.32174, "ax":-4.16689, "ay":2.75907, "alpha":4.21095, "fx":[-60.38119,-59.19276,-78.30932,-79.95677], "fy":[37.59359,59.56236,53.92336,32.89026]}, - {"t":0.17247, "x":7.04027, "y":5.11869, "heading":-3.01238, "vx":-0.70709, "vy":0.49273, "omega":1.44278, "ax":-4.20223, "ay":2.70456, "alpha":2.89802, "fx":[-63.54999,-63.27973,-76.3928,-76.97442], "fy":[39.66865,54.58113,50.30962,35.77569]}, - {"t":0.20121, "x":7.01821, "y":5.13397, "heading":-2.97091, "vx":-0.82789, "vy":0.57047, "omega":1.52608, "ax":-4.2429, "ay":2.63987, "alpha":1.74592, "fx":[-66.68546,-66.80922,-74.71846,-74.69546], "fy":[40.9826,49.86518,46.94195,38.2314]}, - {"t":0.22996, "x":6.99266, "y":5.15146, "heading":-2.92704, "vx":-0.94985, "vy":0.64636, "omega":1.57627, "ax":-4.29003, "ay":2.56202, "alpha":0.72489, "fx":[-69.76297,-69.99846,-73.23419,-73.05565], "fy":[41.54438,45.17985,43.8678,40.2386]}, - {"t":0.2587, "x":6.96358, "y":5.17109, "heading":-2.88173, "vx":-1.07317, "vy":0.72, "omega":1.59711, "ax":-4.34519, "ay":2.46665, "alpha":-0.14843, "fx":[-72.79408,-72.73924,-72.07035,-72.12499], "fy":[41.32076,40.62344,40.87738,41.65007]}, - {"t":0.28745, "x":6.93094, "y":5.19281, "heading":-2.83582, "vx":-1.19807, "vy":0.79091, "omega":1.59284, "ax":-4.41035, "ay":2.34728, "alpha":-0.89636, "fx":[-75.79453,-75.22223,-71.21082,-71.84633], "fy":[40.28088,35.95722,37.94996,42.32388]}, - {"t":0.31619, "x":6.89468, "y":5.21651, "heading":-2.79004, "vx":-1.32484, "vy":0.85838, "omega":1.56707, "ax":-4.4879, "ay":2.19421, "alpha":-1.51734, "fx":[-78.70626,-77.49168,-70.81451,-72.23211], "fy":[38.35107,31.07503,34.75984,42.11979]}, - {"t":0.34494, "x":6.85474, "y":5.24209, "heading":-2.74499, "vx":-1.45385, "vy":0.92145, "omega":1.52346, "ax":-4.58086, "ay":1.99122, "alpha":-2.14282, "fx":[-81.55513,-79.83527,-70.85148,-73.20106], "fy":[35.7404,24.30654,31.62446,41.09921]}, - {"t":0.37368, "x":6.81106, "y":5.2694, "heading":-2.7012, "vx":-1.58552, "vy":0.97869, "omega":1.46186, "ax":-4.69121, "ay":1.71264, "alpha":-2.29387, "fx":[-84.66162,-80.78068,-72.05368,-75.30462], "fy":[30.68745,20.03214,26.09222,37.38362]}, - {"t":0.40243, "x":6.76355, "y":5.29824, "heading":-2.65918, "vx":-1.72037, "vy":1.02792, "omega":1.39593, "ax":-4.81783, "ay":1.31042, "alpha":-2.5733, "fx":[-87.2956,-83.35904,-73.15623,-77.43294], "fy":[24.09659,12.08671,19.41908,31.7739]}, - {"t":0.43117, "x":6.7121, "y":5.32833, "heading":-2.61905, "vx":-1.85886, "vy":1.06559, "omega":1.32196, "ax":-4.94182, "ay":0.70044, "alpha":-2.64418, "fx":[-89.49598,-85.04485,-74.93627,-80.03377], "fy":[13.96931,1.4664,9.25642,22.0118]}, - {"t":0.45992, "x":6.65663, "y":5.35925, "heading":-2.58105, "vx":-2.00091, "vy":1.08572, "omega":1.24595, "ax":-4.98164, "ay":-0.26924, "alpha":-2.45982, "fx":[-90.03486,-84.54441,-75.88094,-81.7058], "fy":[-2.27879,-13.71033,-7.1071,5.14359]}, - {"t":0.48866, "x":6.59706, "y":5.39035, "heading":-2.54524, "vx":-2.14411, "vy":1.07798, "omega":1.17524, "ax":-4.65843, "ay":-1.77782, "alpha":-2.13999, "fx":[-83.988,-78.54378,-70.91337,-77.16995], "fy":[-27.3882,-37.43509,-32.06586,-21.65245]}, - {"t":0.51741, "x":6.5335, "y":5.4206, "heading":-2.51146, "vx":-2.27802, "vy":1.02688, "omega":1.11373, "ax":-3.45022, "ay":-3.59739, "alpha":-1.57021, "fx":[-62.94032,-57.64108,-51.94275,-57.52971], "fy":[-58.31433,-65.13032,-61.64981,-54.77291]}, - {"t":0.54615, "x":6.46659, "y":5.44863, "heading":-2.47944, "vx":-2.37719, "vy":0.92347, "omega":1.06859, "ax":-1.5795, "ay":-4.72909, "alpha":-0.91957, "fx":[-29.86685,-26.6862,-22.64461,-26.12038], "fy":[-78.20431,-81.47853,-79.47941,-76.16406]}, - {"t":0.5749, "x":6.39761, "y":5.47322, "heading":-2.44873, "vx":-2.4226, "vy":0.78753, "omega":1.04216, "ax":0.00383, "ay":-4.98865, "alpha":-0.36803, "fx":[-1.49449,-0.08942,1.62359,0.21542], "fy":[-83.12087,-84.05965,-83.32011,-82.13278]}, - {"t":0.60364, "x":6.32797, "y":5.4938, "heading":-2.41877, "vx":-2.42249, "vy":0.64413, "omega":1.03158, "ax":1.01535, "ay":-4.88664, "alpha":-0.1058, "fx":[16.82718,16.00262,17.59407,17.27794], "fy":[-81.43592,-81.80361,-81.40445,-81.1881]}, - {"t":0.63239, "x":6.25876, "y":5.5103, "heading":-2.38912, "vx":-2.3933, "vy":0.50367, "omega":1.02854, "ax":1.63982, "ay":-4.71575, "alpha":-0.04452, "fx":[27.22073,27.32424,27.43898,27.35611], "fy":[-78.56854,-78.85907,-78.54569,-78.464]}, - {"t":0.66113, "x":6.19064, "y":5.52283, "heading":-2.35955, "vx":-2.34616, "vy":0.36811, "omega":1.02726, "ax":2.04811, "ay":-4.55464, "alpha":-0.22039, "fx":[33.24322,34.14797,34.96397,34.20886], "fy":[-76.02022,-76.58294,-75.82462,-75.26719]}, - {"t":0.68988, "x":6.12404, "y":5.53153, "heading":-2.33002, "vx":-2.28729, "vy":0.23719, "omega":1.02092, "ax":2.32701, "ay":-4.41967, "alpha":-0.63759, "fx":[36.3864,38.58617,41.16013,39.02795], "fy":[-74.01006,-75.68739,-73.31642,-71.68135]}, - {"t":0.71862, "x":6.05926, "y":5.53652, "heading":-2.30068, "vx":-2.2204, "vy":0.11015, "omega":1.0026, "ax":2.52838, "ay":-4.30843, "alpha":-1.27951, "fx":[37.28224,41.88146,46.90507,42.51843], "fy":[-72.70712,-75.78164,-70.9774,-67.81152]}, - {"t":0.74737, "x":5.99648, "y":5.53791, "heading":-2.27186, "vx":-2.14772, "vy":-0.0137, "omega":0.96582, "ax":2.67916, "ay":-4.21694, "alpha":-2.14973, "fx":[36.57365,44.30803,52.59221,45.16768], "fy":[-72.05903,-76.9617,-68.68368,-63.47277]}, - {"t":0.77611, "x":5.93585, "y":5.53577, "heading":-2.24409, "vx":-2.07071, "vy":-0.13491, "omega":0.90402, "ax":2.79593, "ay":-4.14095, "alpha":-3.26587, "fx":[34.32151,46.4439,58.52837,47.13314], "fy":[-72.1355,-79.07884,-66.30628,-58.58983]}, - {"t":0.80486, "x":5.87748, "y":5.53018, "heading":-2.21811, "vx":-1.99034, "vy":-0.25394, "omega":0.81015, "ax":2.88876, "ay":-4.07715, "alpha":-4.63082, "fx":[30.78902,48.26728,64.90041,48.66004], "fy":[-72.94249,-82.09009,-63.81528,-53.00847]}, - {"t":0.8336, "x":5.82146, "y":5.5212, "heading":-2.19482, "vx":-1.9073, "vy":-0.37114, "omega":0.67703, "ax":1.90837, "ay":-2.30737, "alpha":-4.8111, "fx":[14.95327,33.74763,48.36391,30.18172], "fy":[-42.30903,-54.16986,-35.20457,-22.16779]}, - {"t":0.86515, "x":5.76224, "y":5.50834, "heading":-2.17346, "vx":-1.8471, "vy":-0.44393, "omega":0.52525, "ax":2.20987, "ay":-2.02455, "alpha":-3.65646, "fx":[24.27968,38.47353,49.22702,35.36963], "fy":[-36.82244,-45.93183,-30.91889,-21.31979]}, - {"t":0.8967, "x":5.70507, "y":5.49333, "heading":-2.15689, "vx":-1.77738, "vy":-0.50781, "omega":0.4099, "ax":2.47982, "ay":-1.683, "alpha":-2.80048, "fx":[31.87302,42.76667,50.65734,40.05293], "fy":[-30.66713,-37.43593,-25.70389,-18.41256]}, - {"t":0.92825, "x":5.65023, "y":5.47647, "heading":-2.14396, "vx":-1.69915, "vy":-0.5609, "omega":0.32155, "ax":2.69942, "ay":-1.30202, "alpha":-2.15759, "fx":[37.91416,46.23283,52.06991,43.77543], "fy":[-23.62048,-29.13995,-19.81567,-14.24005]}, - {"t":0.9598, "x":5.59797, "y":5.45813, "heading":-2.13381, "vx":-1.61399, "vy":-0.60198, "omega":0.25348, "ax":2.85677, "ay":-0.90636, "alpha":-1.665, "fx":[42.2034,48.6654,52.98066,46.63421], "fy":[-16.68864,-20.84923,-13.63661,-9.25997]}, - {"t":0.99134, "x":5.54847, "y":5.43868, "heading":-2.12582, "vx":-1.52386, "vy":-0.63057, "omega":0.20095, "ax":2.95157, "ay":-0.52112, "alpha":-1.29914, "fx":[45.0965,50.13436,53.36024,48.21411], "fy":[-9.8276,-13.25443,-7.54392,-4.12165]}, - {"t":1.02289, "x":5.50186, "y":5.41853, "heading":-2.11948, "vx":-1.43074, "vy":-0.64701, "omega":0.15997, "ax":2.99282, "ay":-0.16518, "alpha":-1.01302, "fx":[46.67785,50.64559,53.07977,49.15259], "fy":[-3.65602,-6.32352,-1.8741,0.84008]}, - {"t":1.05444, "x":5.45822, "y":5.39804, "heading":-2.11443, "vx":-1.33633, "vy":-0.65222, "omega":0.12801, "ax":2.99372, "ay":0.15138, "alpha":-0.79993, "fx":[47.43553,50.58213,52.43714,49.16088], "fy":[1.80219,-0.2836,3.20697,5.36786]}, - {"t":1.08599, "x":5.41755, "y":5.37754, "heading":-2.11039, "vx":-1.24188, "vy":-0.64745, "omega":0.10277, "ax":2.96731, "ay":0.42591, "alpha":-0.63346, "fx":[47.48757,50.00087,51.42965,48.93611], "fy":[6.59651,4.81825,7.64037,9.34403]}, - {"t":1.11754, "x":5.37984, "y":5.35732, "heading":-2.10715, "vx":-1.14827, "vy":-0.63401, "omega":0.08279, "ax":2.92419, "ay":0.6606, "alpha":-0.50301, "fx":[47.22464,49.22656,50.34269,48.18536], "fy":[10.54406,9.2677,11.42509,12.81083]}, - {"t":1.14908, "x":5.34507, "y":5.33765, "heading":-2.10454, "vx":-1.05602, "vy":-0.61317, "omega":0.06692, "ax":2.87207, "ay":0.85989, "alpha":-0.40404, "fx":[46.62193,48.25109,49.12131,47.50944], "fy":[14.04534,12.86022,14.67523,15.75525]}, - {"t":1.18063, "x":5.31319, "y":5.31873, "heading":-2.10243, "vx":-0.96541, "vy":-0.58604, "omega":0.05417, "ax":2.81609, "ay":1.0289, "alpha":-0.32258, "fx":[45.98511,47.27224,47.97073,46.54331], "fy":[16.80742,16.06065,17.41906,18.31823]}, - {"t":1.21218, "x":5.28413, "y":5.30076, "heading":-2.10072, "vx":-0.87656, "vy":-0.55358, "omega":0.044, "ax":2.75951, "ay":1.17254, "alpha":-0.26369, "fx":[45.18311,46.26079,46.807,45.74795], "fy":[19.37718,18.57066,19.7702,20.46462]}, - {"t":1.24373, "x":5.25785, "y":5.28387, "heading":-2.09933, "vx":-0.78951, "vy":-0.51659, "omega":0.03568, "ax":2.70426, "ay":1.29514, "alpha":-0.21143, "fx":[44.46518,45.30771,45.75944,44.78273], "fy":[21.35236,20.88596,21.76104,22.35788]}, - {"t":1.27528, "x":5.23429, "y":5.26822, "heading":-2.0982, "vx":-0.70419, "vy":-0.47573, "omega":0.02901, "ax":2.65143, "ay":1.40034, "alpha":-0.17574, "fx":[43.65201,44.38062,44.73522,44.02444], "fy":[23.23954,22.68963,23.49314,23.94935]}, - {"t":1.30682, "x":5.21339, "y":5.25391, "heading":-2.09729, "vx":-0.62055, "vy":-0.43155, "omega":0.02346, "ax":2.60154, "ay":1.49115, "alpha":-0.14228, "fx":[42.96232,43.52519,43.82745,43.15093], "fy":[24.68289,24.39235,24.97332,25.37875]}, - {"t":1.33837, "x":5.19511, "y":5.24104, "heading":-2.09655, "vx":-0.53847, "vy":-0.38451, "omega":0.01897, "ax":2.5548, "ay":1.57004, "alpha":-0.12207, "fx":[42.20755,42.71976,42.9581,42.46394], "fy":[26.10799,25.71252,26.27819,26.58882]}, - {"t":1.36992, "x":5.17939, "y":5.22969, "heading":-2.09595, "vx":-0.45787, "vy":-0.33498, "omega":0.01512, "ax":2.51121, "ay":1.639, "alpha":-0.10023, "fx":[41.58292,41.97458,42.18691,41.69847], "fy":[27.18625,27.00027,27.40522,27.69331]}, - {"t":1.40147, "x":5.1662, "y":5.21994, "heading":-2.09547, "vx":-0.37865, "vy":-0.28327, "omega":0.01196, "ax":2.47066, "ay":1.69962, "alpha":-0.09045, "fx":[40.90237,41.28696,41.45837,41.09144], "fy":[28.28992,27.98854,28.41151,28.63758]}, - {"t":1.43302, "x":5.15548, "y":5.21185, "heading":-2.0951, "vx":-0.3007, "vy":-0.22965, "omega":0.00911, "ax":2.43299, "ay":1.75324, "alpha":-0.07689, "fx":[40.34763,40.64579,40.8082,40.42527], "fy":[29.11856,28.98143,29.28975,29.51262]}, - {"t":1.46457, "x":5.14721, "y":5.20547, "heading":-2.09481, "vx":-0.22395, "vy":-0.17434, "omega":0.00668, "ax":2.39799, "ay":1.8009, "alpha":-0.0741, "fx":[39.73821,40.05977,40.20045,39.89461], "fy":[29.98603,29.7438,30.08291,30.2681]}, - {"t":1.49611, "x":5.14134, "y":5.20087, "heading":-2.0946, "vx":-0.1483, "vy":-0.11753, "omega":0.00434, "ax":2.36545, "ay":1.84351, "alpha":-0.07166, "fx":[39.20298,39.51579,39.65108,39.35411], "fy":[30.69849,30.46329,30.79046,30.96922]}, - {"t":1.52766, "x":5.13783, "y":5.19808, "heading":-2.09446, "vx":-0.07367, "vy":-0.05937, "omega":0.00208, "ax":2.33519, "ay":1.88176, "alpha":-0.06605, "fx":[38.72666,38.9581,39.15339,38.86814], "fy":[31.31356,31.14009,31.41455,31.60414]}, - {"t":1.55921, "x":5.13667, "y":5.19714, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.10089, "y":6.16635, "heading":-3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.9937, "ay":0.17023, "alpha":12.94941, "fx":[-57.0171,-57.04831,-109.14623,-109.75898], "fy":[-38.35351,44.46346,34.93277,-29.69215]}, + {"t":0.03024, "x":7.0986, "y":6.16643, "heading":-3.14159, "vx":-0.15099, "vy":0.00515, "omega":0.39154, "ax":-4.99609, "ay":0.1455, "alpha":10.7046, "fx":[-61.65965,-61.60264,-104.72136,-105.14601], "fy":[-30.99,36.15619,29.66604,-25.1308]}, + {"t":0.06047, "x":7.09176, "y":6.16665, "heading":-3.12975, "vx":-0.30205, "vy":0.00955, "omega":0.7152, "ax":-4.99672, "ay":0.11772, "alpha":8.70627, "fx":[-65.56727,-65.79136,-100.971,-100.84179], "fy":[-24.65868,29.52633,23.97226,-20.99034]}, + {"t":0.09071, "x":7.08034, "y":6.167, "heading":-3.10813, "vx":-0.45313, "vy":0.01311, "omega":0.97844, "ax":-4.99723, "ay":0.08661, "alpha":6.84721, "fx":[-68.81233,-70.01802,-97.54645,-96.82913], "fy":[-18.55848,22.81392,18.95923,-17.43996]}, + {"t":0.12094, "x":7.06435, "y":6.16743, "heading":-3.07855, "vx":-0.60423, "vy":0.01572, "omega":1.18547, "ax":-4.99759, "ay":0.05114, "alpha":5.19578, "fx":[-72.09472,-73.47235,-94.43222,-93.23027], "fy":[-13.81221,17.51006,13.65294,-13.94119]}, + {"t":0.15118, "x":7.0438, "y":6.16793, "heading":-3.0427, "vx":-0.75533, "vy":0.01727, "omega":1.34257, "ax":-4.99768, "ay":0.0109, "alpha":3.69147, "fx":[-74.90581,-76.80121,-91.48264,-90.04635], "fy":[-9.59475,11.96547,9.25803,-10.90172]}, + {"t":0.18142, "x":7.01868, "y":6.16846, "heading":-3.00211, "vx":-0.90644, "vy":0.0176, "omega":1.45419, "ax":-4.99739, "ay":-0.03574, "alpha":2.39871, "fx":[-77.81592,-79.24199,-88.75325,-87.40535], "fy":[-6.6269,7.30418,5.09194,-8.15226]}, + {"t":0.21165, "x":6.98899, "y":6.16897, "heading":-2.95814, "vx":-1.05754, "vy":0.01652, "omega":1.52671, "ax":-4.99651, "ay":-0.08991, "alpha":1.19445, "fx":[-80.28114,-81.58846,-86.10872,-85.17907], "fy":[-4.29287,2.47597,1.22446,-5.40291]}, + {"t":0.24189, "x":6.95473, "y":6.16943, "heading":-2.91198, "vx":-1.20861, "vy":0.0138, "omega":1.56283, "ax":-4.9947, "ay":-0.15401, "alpha":0.19279, "fx":[-82.81052,-82.98982,-83.70717,-83.52939], "fy":[-3.00424,-1.84137,-2.19188,-3.23156]}, + {"t":0.27212, "x":6.9159, "y":6.16978, "heading":-2.86472, "vx":-1.35963, "vy":0.00914, "omega":1.56866, "ax":-4.99146, "ay":-0.2306, "alpha":-0.71577, "fx":[-84.87228,-84.46986,-81.32342,-82.15534], "fy":[-2.44227,-6.25756,-5.28425,-1.39189]}, + {"t":0.30236, "x":6.87251, "y":6.16995, "heading":-2.8173, "vx":-1.51055, "vy":0.00217, "omega":1.54701, "ax":-4.98592, "ay":-0.32397, "alpha":-1.46094, "fx":[-86.92118,-84.91135,-79.26752,-81.3513], "fy":[-2.71564,-10.5002,-8.20782,-0.17836]}, + {"t":0.33259, "x":6.82456, "y":6.16987, "heading":-2.77052, "vx":-1.66131, "vy":-0.00762, "omega":1.50284, "ax":-4.97659, "ay":-0.43992, "alpha":-2.10154, "fx":[-88.41156,-85.66383,-77.16054,-80.59349], "fy":[-3.85595,-14.71466,-11.03592,0.27352]}, + {"t":0.36283, "x":6.77205, "y":6.16944, "heading":-2.72508, "vx":-1.81178, "vy":-0.02093, "omega":1.4393, "ax":-4.96078, "ay":-0.58742, "alpha":-2.59768, "fx":[-89.79739,-85.17805,-75.44951,-80.35059], "fy":[-5.81296,-19.1059,-14.09435,-0.15503]}, + {"t":0.39307, "x":6.715, "y":6.16854, "heading":-2.68156, "vx":-1.96177, "vy":-0.03869, "omega":1.36076, "ax":-4.9332, "ay":-0.78153, "alpha":-2.85974, "fx":[-89.66276,-84.45686,-74.24621,-80.57024], "fy":[-8.9981,-23.8803,-17.3753,-1.85712]}, + {"t":0.4233, "x":6.65343, "y":6.16701, "heading":-2.64042, "vx":-2.11093, "vy":-0.06232, "omega":1.27429, "ax":-4.88304, "ay":-1.04564, "alpha":-3.1747, "fx":[-90.53825,-83.5277,-71.99975,-79.52587], "fy":[-13.39102,-28.87666,-21.93575,-5.51803]}, + {"t":0.45354, "x":6.58737, "y":6.16465, "heading":-2.60189, "vx":-2.25858, "vy":-0.09393, "omega":1.1783, "ax":-4.78545, "ay":-1.42235, "alpha":-3.27135, "fx":[-89.27297,-82.34166,-69.4431,-78.02669], "fy":[-19.90638,-35.20209,-28.03889,-11.69209]}, + {"t":0.48377, "x":6.5169, "y":6.16116, "heading":-2.56626, "vx":-2.40327, "vy":-0.13694, "omega":1.07939, "ax":-4.57393, "ay":-1.99556, "alpha":-3.13476, "fx":[-85.83389,-77.21506,-66.46113,-75.47055], "fy":[-29.69925,-44.70584,-36.83716,-21.8176]}, + {"t":0.51401, "x":6.44214, "y":6.1561, "heading":-2.53363, "vx":-2.54156, "vy":-0.19728, "omega":0.98461, "ax":-4.07953, "ay":-2.86868, "alpha":-2.72006, "fx":[-76.68318,-69.5445,-58.5258,-67.26159], "fy":[-45.03881,-56.8372,-50.99832,-38.40353]}, + {"t":0.54425, "x":6.36343, "y":6.14883, "heading":-2.50386, "vx":-2.66491, "vy":-0.28401, "omega":0.90236, "ax":-2.79155, "ay":-4.12764, "alpha":-2.08333, "fx":[-53.97228,-46.76564,-39.08602,-46.31091], "fy":[-66.91054,-75.68099,-70.48095,-62.15043]}, + {"t":0.57448, "x":6.28158, "y":6.13835, "heading":-2.47657, "vx":-2.74932, "vy":-0.40882, "omega":0.83937, "ax":-0.21861, "ay":-4.97615, "alpha":-1.04024, "fx":[-7.53276,-4.37065,0.39792,-3.07083], "fy":[-82.56547,-86.05918,-83.36648,-79.80927]}, + {"t":0.60472, "x":6.19835, "y":6.12372, "heading":-2.45119, "vx":-2.75593, "vy":-0.55927, "omega":0.80792, "ax":2.23281, "ay":-4.45603, "alpha":-0.1593, "fx":[37.12889,37.00058,37.43876,37.31095], "fy":[-73.88089,-75.6129,-73.87797,-73.74769]}, + {"t":0.63495, "x":6.11604, "y":6.10477, "heading":-2.42676, "vx":-2.68842, "vy":-0.69401, "omega":0.8031, "ax":3.49076, "ay":-3.56316, "alpha":0.68442, "fx":[60.22954,59.3636,55.6713,57.49268], "fy":[-59.28731,-56.95909,-59.59605,-61.74253]}, + {"t":0.66519, "x":6.03635, "y":6.08216, "heading":-2.40248, "vx":-2.58287, "vy":-0.80174, "omega":0.8238, "ax":4.06044, "ay":-2.90212, "alpha":1.03133, "fx":[71.17048,68.31373,64.2058,67.05216], "fy":[-47.98586,-44.86379,-48.56774,-52.09032]}, + {"t":0.69542, "x":5.96011, "y":6.05659, "heading":-2.37757, "vx":-2.4601, "vy":-0.88949, "omega":0.85498, "ax":4.34818, "ay":-2.45381, "alpha":1.18752, "fx":[75.93359,73.88315,68.45082,71.66045], "fy":[-40.54955,-36.40591,-41.40308,-45.25675]}, + {"t":0.72566, "x":5.88772, "y":6.02857, "heading":-2.35172, "vx":-2.32863, "vy":-0.96368, "omega":0.89089, "ax":4.51038, "ay":-2.14402, "alpha":1.00554, "fx":[78.36517,75.57716,71.98805,74.81315], "fy":[-35.32673,-32.01402,-36.12945,-39.48907]}, + {"t":0.7559, "x":5.81937, "y":5.99846, "heading":-2.32479, "vx":-2.19225, "vy":-1.02851, "omega":0.92129, "ax":4.61172, "ay":-1.91869, "alpha":0.63308, "fx":[78.57034,77.5981,74.79266,76.5397], "fy":[-31.76034,-29.45481,-32.30041,-34.41893]}, + {"t":0.78613, "x":5.75519, "y":5.96648, "heading":-2.29693, "vx":-2.05281, "vy":-1.08652, "omega":0.94043, "ax":4.67874, "ay":-1.75084, "alpha":0.03157, "fx":[78.12786,77.95869,77.8592,78.02377], "fy":[-29.23015,-29.08837,-29.16875,-29.2552]}, + {"t":0.81637, "x":5.69527, "y":5.93283, "heading":-2.2685, "vx":-1.91135, "vy":-1.13946, "omega":0.94139, "ax":4.72643, "ay":-1.61929, "alpha":-0.79748, "fx":[76.14303,79.1467,81.10868,78.75083], "fy":[-27.66263,-29.90263,-26.42199,-23.98372]}, + {"t":0.8466, "x":5.63963, "y":5.89764, "heading":-2.24003, "vx":-1.76844, "vy":-1.18842, "omega":0.91727, "ax":4.76124, "ay":-1.51523, "alpha":-1.83121, "fx":[73.755,79.42555,84.8607,79.42906], "fy":[-27.01874,-32.094,-23.73542,-18.18461]}, + {"t":0.87684, "x":5.58834, "y":5.86101, "heading":-2.2123, "vx":-1.62448, "vy":-1.23423, "omega":0.8619, "ax":4.78777, "ay":-1.43034, "alpha":-3.15831, "fx":[69.98125,80.64614,89.09265,79.5196], "fy":[-26.93369,-35.55456,-21.18495,-11.69913]}, + {"t":0.90708, "x":5.54141, "y":5.82304, "heading":-2.18624, "vx":-1.47972, "vy":-1.27748, "omega":0.76641, "ax":4.80848, "ay":-1.36012, "alpha":-4.68682, "fx":[65.7958,81.40411,93.82603,79.59424], "fy":[-27.90818,-39.87928,-18.59356,-4.30943]}, + {"t":0.93731, "x":5.49887, "y":5.78379, "heading":-2.16306, "vx":-1.33433, "vy":-1.31861, "omega":0.6247, "ax":2.93167, "ay":-0.61079, "alpha":-4.69359, "fx":[33.5496,51.63447,63.76557,46.5288], "fy":[-14.01553,-26.61498,-6.72338,6.62785]}, + {"t":0.96873, "x":5.4584, "y":5.74207, "heading":-2.14344, "vx":-1.24223, "vy":-1.33779, "omega":0.47725, "ax":2.98955, "ay":-0.21467, "alpha":-3.62694, "fx":[37.8635,52.19657,61.2467,48.0306], "fy":[-6.41448,-16.36121,-0.7973,9.25924]}, + {"t":1.00014, "x":5.42085, "y":5.69993, "heading":-2.12844, "vx":-1.14831, "vy":-1.34454, "omega":0.3633, "ax":2.99064, "ay":0.19857, "alpha":-2.77164, "fx":[40.97855,52.02226,58.56304,47.84649], "fy":[0.91272,-6.36017,5.46032,13.22752]}, + {"t":1.03156, "x":5.38625, "y":5.65779, "heading":-2.11703, "vx":-1.05435, "vy":-1.3383, "omega":0.27623, "ax":2.93544, "ay":0.60561, "alpha":-2.19064, "fx":[41.7264,50.71421,55.76367,47.52536], "fy":[8.64176,2.25913,11.7254,17.75464]}, + {"t":1.06298, "x":5.35457, "y":5.61604, "heading":-2.10835, "vx":-0.96213, "vy":-1.31927, "omega":0.20741, "ax":2.83087, "ay":0.98501, "alpha":-1.68631, "fx":[41.78035,48.77934,52.53972,45.65754], "fy":[14.97561,10.65664,17.65424,22.39219]}, + {"t":1.09439, "x":5.32574, "y":5.57508, "heading":-2.10184, "vx":-0.8732, "vy":-1.28833, "omega":0.15443, "ax":2.69021, "ay":1.32194, "alpha":-1.35408, "fx":[40.31436,46.10365,49.09163,43.86851], "fy":[21.31421,17.18707,22.97167,26.67156]}, + {"t":1.12581, "x":5.29964, "y":5.53526, "heading":-2.09699, "vx":-0.78868, "vy":-1.2468, "omega":0.11189, "ax":2.52869, "ay":1.60976, "alpha":-1.04845, "fx":[38.73207,43.24786,45.55888,41.06946], "fy":[25.934,23.37389,27.54204,30.48552]}, + {"t":1.15722, "x":5.27611, "y":5.49689, "heading":-2.09347, "vx":-0.70924, "vy":-1.19623, "omega":0.07895, "ax":2.35951, "ay":1.84911, "alpha":-0.85403, "fx":[36.39714,40.18221,42.05168,38.69623], "fy":[30.49454,27.77049,31.36476,33.66536]}, + {"t":1.18864, "x":5.25499, "y":5.46022, "heading":-2.09099, "vx":-0.63511, "vy":-1.13813, "omega":0.05212, "ax":2.19244, "ay":2.04464, "alpha":-0.66093, "fx":[34.3433,37.2699,38.75469,35.81982], "fy":[33.49876,31.98799,34.50065,36.34535]}, + {"t":1.22006, "x":5.23612, "y":5.42547, "heading":-2.08935, "vx":-0.56624, "vy":-1.0739, "omega":0.03136, "ax":2.03336, "ay":2.2031, "alpha":-0.54238, "fx":[31.97475,34.44587,35.64759,33.51243], "fy":[36.58894,34.7862,37.0432,38.47994]}, + {"t":1.25147, "x":5.21933, "y":5.39282, "heading":-2.08837, "vx":-0.50236, "vy":-1.00469, "omega":0.01432, "ax":1.88564, "ay":2.33096, "alpha":-0.41191, "fx":[30.02369,31.8902,32.85173,30.96491], "fy":[38.46453,37.6169,39.09669,40.24566]}, + {"t":1.28289, "x":5.20448, "y":5.36241, "heading":-2.08792, "vx":-0.44312, "vy":-0.93146, "omega":0.00138, "ax":1.75023, "ay":2.43442, "alpha":-0.33596, "fx":[27.94886,29.51533,30.27568,28.96211], "fy":[40.56373,39.36703,40.75676,41.63499]}, + {"t":1.3143, "x":5.19142, "y":5.33435, "heading":-2.08787, "vx":-0.38813, "vy":-0.85498, "omega":-0.00918, "ax":1.62727, "ay":2.51841, "alpha":-0.24061, "fx":[26.27581,27.3942,27.98673,26.84642], "fy":[41.72298,41.31233,42.10324,42.78393]}, + {"t":1.34572, "x":5.18003, "y":5.30873, "heading":-2.08816, "vx":-0.33701, "vy":-0.77586, "omega":-0.01674, "ax":1.51605, "ay":2.587, "alpha":-0.18677, "fx":[24.55788,25.45435,25.88907,25.18577], "fy":[43.18555,42.42575,43.20003,43.68495]}, + {"t":1.37714, "x":5.1702, "y":5.28563, "heading":-2.08869, "vx":-0.28938, "vy":-0.69459, "omega":-0.0226, "ax":1.41564, "ay":2.64338, "alpha":-0.1102, "fx":[23.18177,23.72216,24.02465,23.46359], "fy":[43.90952,43.82035,44.09645,44.42943]}, + {"t":1.40855, "x":5.1618, "y":5.26511, "heading":-2.0894, "vx":-0.24491, "vy":-0.61154, "omega":-0.02607, "ax":1.32499, "ay":2.69006, "alpha":-0.06584, "fx":[21.79767,22.1449,22.30672,22.09862], "fy":[44.97063,44.55326,44.83492,45.00898]}, + {"t":1.43997, "x":5.15476, "y":5.24723, "heading":-2.09022, "vx":-0.20328, "vy":-0.52703, "omega":-0.02814, "ax":1.24304, "ay":2.72897, "alpha":0.00308, "fx":[20.68646,20.72267,20.76419,20.71017], "fy":[45.42688,45.61106,45.44347,45.4814]}, + {"t":1.47138, "x":5.14899, "y":5.23202, "heading":-2.0911, "vx":-0.16423, "vy":-0.4413, "omega":-0.02804, "ax":1.16884, "ay":2.76164, "alpha":0.04723, "fx":[19.59448,19.42724,19.32572,19.5883], "fy":[46.22999,46.12356,45.94954,45.83802]}, + {"t":1.5028, "x":5.14441, "y":5.21952, "heading":-2.09198, "vx":-0.12751, "vy":-0.35454, "omega":-0.02655, "ax":1.10145, "ay":2.78927, "alpha":0.11695, "fx":[18.71368,18.24275,18.01486,18.47116], "fy":[46.52323,46.97961,46.36695,46.11345]}, + {"t":1.53422, "x":5.14094, "y":5.20976, "heading":-2.09282, "vx":-0.09291, "vy":-0.26691, "omega":-0.02288, "ax":1.04014, "ay":2.81277, "alpha":0.16908, "fx":[17.88214,17.16219,16.76936,17.54118], "fy":[47.16045,47.37606,46.7145,46.29885]}, + {"t":1.56563, "x":5.13854, "y":5.20276, "heading":-2.09354, "vx":-0.06023, "vy":-0.17854, "omega":-0.01757, "ax":0.98415, "ay":2.8329, "alpha":0.24795, "fx":[17.20608,16.15328,15.61002,16.65211], "fy":[47.35474,48.12503,46.9984,46.41392]}, + {"t":1.59705, "x":5.13713, "y":5.19855, "heading":-2.09409, "vx":-0.02931, "vy":-0.08954, "omega":-0.00978, "ax":0.93294, "ay":2.85022, "alpha":0.31126, "fx":[16.62709,15.23401,14.47119,15.87451], "fy":[47.743,48.50509,47.28233,46.51683]}, + {"t":1.62847, "x":5.13667, "y":5.19714, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/startToJSlow.traj b/src/main/deploy/choreo/startToJSlow.traj index d70110ac..d74274b4 100644 --- a/src/main/deploy/choreo/startToJSlow.traj +++ b/src/main/deploy/choreo/startToJSlow.traj @@ -17,7 +17,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":6.1663548}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"5.7971601486206055 m", "val":5.7971601486206055}, "y":{"exp":"5.54304313659668 m", "val":5.54304313659668}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"J.x", "val":5.1366723649053885}, "y":{"exp":"J.y", "val":5.197141990263579}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ diff --git a/src/main/deploy/choreo/startToK.traj b/src/main/deploy/choreo/startToK.traj index 195d0036..05276ebe 100644 --- a/src/main/deploy/choreo/startToK.traj +++ b/src/main/deploy/choreo/startToK.traj @@ -17,7 +17,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":73, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":6.1663548}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":73, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"4.455554008483887 m", "val":4.455554008483887}, "y":{"exp":"5.959114074707031 m", "val":5.959114074707031}, "heading":{"exp":"-1.325818250323842 rad", "val":-1.325818250323842}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"K.x", "val":4.084460748154255}, "y":{"exp":"K.y", "val":5.3371419902635795}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ diff --git a/src/main/deploy/choreo/startToL.traj b/src/main/deploy/choreo/startToL.traj index 0a4a2728..0b652de6 100644 --- a/src/main/deploy/choreo/startToL.traj +++ b/src/main/deploy/choreo/startToL.traj @@ -3,7 +3,7 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":78, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.1008875, "y":6.1663548, "heading":3.141592653589793, "intervals":76, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":4.180931568145752, "y":5.788817405700684, "heading":-1.2827410322432835, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":3.798672364905389, "y":5.1721419902635795, "heading":5.235987755982989, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ @@ -17,7 +17,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":78, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":6.1663548}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":76, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"4.180931568145752 m", "val":4.180931568145752}, "y":{"exp":"5.788817405700684 m", "val":5.788817405700684}, "heading":{"exp":"-1.2827410322432835 rad", "val":-1.2827410322432835}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"L.x", "val":3.798672364905389}, "y":{"exp":"L.y", "val":5.1721419902635795}, "heading":{"exp":"L.heading", "val":5.235987755982989}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ @@ -34,125 +34,123 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,3.22915,4.4629], + "waypoints":[0.0,3.12876,4.35798], "samples":[ - {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.86898, "ay":0.87184, "alpha":3.77301, "fx":[-39.32034,-38.90387,-56.18911,-56.88497], "fy":[4.97691,24.70909,23.68532,4.76116]}, - {"t":0.0414, "x":7.09843, "y":5.07643, "heading":3.14159, "vx":-0.11877, "vy":0.03609, "omega":0.1562, "ax":-2.86958, "ay":0.87203, "alpha":2.87159, "fx":[-41.38453,-41.02911,-54.20171,-54.72291], "fy":[7.29982,22.25024,21.53938,7.05612]}, - {"t":0.0828, "x":7.09105, "y":5.07867, "heading":-3.13513, "vx":-0.23757, "vy":0.0722, "omega":0.27508, "ax":-2.86945, "ay":0.87199, "alpha":2.16926, "fx":[-42.95164,-42.68149,-52.68673,-53.00937], "fy":[9.13511,20.37738,19.79299,8.83738]}, - {"t":0.1242, "x":7.07876, "y":5.0824, "heading":-3.12374, "vx":-0.35637, "vy":0.1083, "omega":0.36489, "ax":-2.86924, "ay":0.87198, "alpha":1.61292, "fx":[-44.11529,-44.10282,-51.47463,-51.62298], "fy":[10.57393,18.9124,18.42683,10.22873]}, - {"t":0.1656, "x":7.06155, "y":5.08763, "heading":-3.10863, "vx":-0.47515, "vy":0.14439, "omega":0.43166, "ax":-2.86895, "ay":0.87192, "alpha":1.18755, "fx":[-45.08225,-45.07591,-50.55703,-50.58101], "fy":[11.67712,17.80475,17.33531,11.32056]}, - {"t":0.207, "x":7.03942, "y":5.09436, "heading":-3.09076, "vx":-0.59392, "vy":0.18049, "omega":0.48083, "ax":-2.86845, "ay":0.87187, "alpha":0.84743, "fx":[-45.75236,-45.9944,-49.78717,-49.72854], "fy":[12.5285,16.89842,16.52383,12.18357]}, - {"t":0.2484, "x":7.01237, "y":5.10258, "heading":-3.07086, "vx":-0.71268, "vy":0.21659, "omega":0.51591, "ax":-2.86748, "ay":0.87158, "alpha":0.61162, "fx":[-46.3439,-46.44464,-49.25411,-49.15549], "fy":[13.11724,16.30084,15.88643,12.81073]}, - {"t":0.2898, "x":6.98041, "y":5.11229, "heading":-3.0495, "vx":-0.83139, "vy":0.25267, "omega":0.54123, "ax":-2.86427, "ay":0.87153, "alpha":0.401, "fx":[-46.50831,-47.30316,-48.64224,-48.53045], "fy":[13.57251,15.73741,15.4955,13.30662]}, - {"t":0.33119, "x":6.94354, "y":5.1235, "heading":-3.02709, "vx":-0.94997, "vy":0.28875, "omega":0.55783, "ax":-0.1621, "ay":0.03786, "alpha":0.3384, "fx":[-1.73925,-1.93828,-3.66475,-3.46623], "fy":[-0.05078,1.35268,1.47345,-0.25103]}, - {"t":0.37259, "x":6.90407, "y":5.13549, "heading":-3.004, "vx":-0.95668, "vy":0.29032, "omega":0.57184, "ax":0.00239, "ay":0.00788, "alpha":0.27099, "fx":[0.73676,0.68477,-0.72002,-0.54207], "fy":[-0.42066,0.86185,0.68335,-0.59913]}, - {"t":0.41399, "x":6.86446, "y":5.14751, "heading":-2.98032, "vx":-0.95658, "vy":0.29064, "omega":0.58306, "ax":0.00111, "ay":0.00365, "alpha":0.19822, "fx":[0.57295,0.41716,-0.53599,-0.38021], "fy":[-0.33765,0.61594,0.45788,-0.4931]}, - {"t":0.45539, "x":6.82486, "y":5.15955, "heading":-2.95618, "vx":-0.95653, "vy":0.29079, "omega":0.59127, "ax":-0.00078, "ay":-0.00256, "alpha":0.14884, "fx":[0.40266,0.28928,-0.43855,-0.3052], "fy":[-0.33035,0.37873,0.24515,-0.46393]}, - {"t":0.49679, "x":6.78526, "y":5.17158, "heading":-2.93171, "vx":-0.95656, "vy":0.29069, "omega":0.59743, "ax":-0.00003, "ay":-0.00009, "alpha":0.1106, "fx":[0.32029,0.20726,-0.32123,-0.20819], "fy":[-0.2078,0.31512,0.20721,-0.32071]}, - {"t":0.53819, "x":6.74566, "y":5.18362, "heading":-2.90697, "vx":-0.95657, "vy":0.29068, "omega":0.60201, "ax":-0.0004, "ay":-0.00132, "alpha":0.08341, "fx":[0.23721,0.14527,-0.25192,-0.15729], "fy":[-0.17199,0.22267,0.12799,-0.26667]}, - {"t":0.57959, "x":6.70606, "y":5.19565, "heading":-2.88205, "vx":-0.95658, "vy":0.29063, "omega":0.60546, "ax":0.00021, "ay":0.0007, "alpha":0.06258, "fx":[0.19075,0.11199,-0.18364,-0.10487], "fy":[-0.0958,0.19607,0.12117,-0.17456]}, - {"t":0.62099, "x":6.66646, "y":5.20768, "heading":-2.85699, "vx":-0.95657, "vy":0.29066, "omega":0.60805, "ax":-0.00014, "ay":-0.00045, "alpha":0.04757, "fx":[0.14167,0.07614,-0.14606,-0.08081], "fy":[-0.08599,0.13626,0.07105,-0.1512]}, - {"t":0.66239, "x":6.62686, "y":5.21972, "heading":-2.83181, "vx":-0.95658, "vy":0.29064, "omega":0.61002, "ax":0.00014, "ay":0.00045, "alpha":0.03613, "fx":[0.11322,0.05932,-0.10862,-0.05473], "fy":[-0.04914,0.11732,0.06514,-0.10309]}, - {"t":0.70379, "x":6.58726, "y":5.23175, "heading":-2.80656, "vx":-0.95657, "vy":0.29066, "omega":0.61152, "ax":-0.00005, "ay":-0.00015, "alpha":0.02779, "fx":[0.08564,0.04063,-0.08698,-0.04236], "fy":[-0.0441,0.08359,0.03905,-0.08865]}, - {"t":0.74519, "x":6.54765, "y":5.24378, "heading":-2.78124, "vx":-0.95657, "vy":0.29065, "omega":0.61267, "ax":0.00005, "ay":0.00016, "alpha":0.02149, "fx":[0.0682,0.03127,-0.06658,-0.02965], "fy":[-0.02785,0.07005,0.0333,-0.06485]}, - {"t":0.78659, "x":6.50805, "y":5.25581, "heading":-2.75588, "vx":-0.95657, "vy":0.29066, "omega":0.61356, "ax":-0.00002, "ay":-0.00005, "alpha":0.0168, "fx":[0.05319,0.02217,-0.05363,-0.02278], "fy":[-0.02333,0.05236,0.0216,-0.05409]}, - {"t":0.82799, "x":6.46845, "y":5.26785, "heading":-2.73048, "vx":-0.95657, "vy":0.29066, "omega":0.61425, "ax":0.00001, "ay":0.00003, "alpha":0.01332, "fx":[0.04266,0.01685,-0.04233,-0.01652], "fy":[-0.01637,0.0436,0.01719,-0.04226]}, - {"t":0.86939, "x":6.42885, "y":5.27988, "heading":-2.70505, "vx":-0.95657, "vy":0.29066, "omega":0.6148, "ax":0.0, "ay":-0.00001, "alpha":0.01064, "fx":[0.03446,0.0125,-0.03457,-0.01264], "fy":[-0.0127,0.03414,0.01229,-0.03455]}, - {"t":0.91079, "x":6.38925, "y":5.29191, "heading":-2.67959, "vx":-0.95657, "vy":0.29066, "omega":0.61524, "ax":0.0, "ay":0.0, "alpha":0.00869, "fx":[0.02812,0.00943,-0.02814,-0.00945], "fy":[-0.00973,0.02883,0.00929,-0.02851]}, - {"t":0.95218, "x":6.34965, "y":5.30395, "heading":-2.65412, "vx":-0.95657, "vy":0.29066, "omega":0.6156, "ax":0.0, "ay":0.0, "alpha":0.00712, "fx":[0.02352,0.0073,-0.02352,-0.00725], "fy":[-0.00714,0.02341,0.00723,-0.02332]}, - {"t":0.99358, "x":6.31005, "y":5.31598, "heading":-2.62864, "vx":-0.95657, "vy":0.29066, "omega":0.6159, "ax":0.0, "ay":-0.00001, "alpha":0.00601, "fx":[0.01965,0.00548,-0.0197,-0.00554], "fy":[-0.00584,0.02026,0.0053,-0.0201]}, - {"t":1.03498, "x":6.27044, "y":5.32801, "heading":-2.60314, "vx":-0.95657, "vy":0.29066, "omega":0.61615, "ax":0.0, "ay":0.00001, "alpha":0.00506, "fx":[0.01697,0.00438,-0.01694,-0.00429], "fy":[-0.00415,0.0169,0.00435,-0.0167]}, - {"t":1.07638, "x":6.23084, "y":5.34005, "heading":-2.57763, "vx":-0.95657, "vy":0.29066, "omega":0.61636, "ax":0.0, "ay":0.0, "alpha":0.00438, "fx":[0.0145,0.00327,-0.01454,-0.0033], "fy":[-0.00352,0.01498,0.00315,-0.01486]}, - {"t":1.11778, "x":6.19124, "y":5.35208, "heading":-2.55212, "vx":-0.95657, "vy":0.29066, "omega":0.61654, "ax":0.0, "ay":0.0, "alpha":0.00378, "fx":[0.01286,0.00265,-0.01284,-0.00257], "fy":[-0.00246,0.01278,0.00262,-0.01262]}, - {"t":1.15918, "x":6.15164, "y":5.36411, "heading":-2.52659, "vx":-0.95657, "vy":0.29066, "omega":0.61669, "ax":0.0, "ay":0.0, "alpha":0.00334, "fx":[0.01119,0.00193,-0.01121,-0.00195], "fy":[-0.00208,0.01149,0.00189,-0.01145]}, - {"t":1.20058, "x":6.11204, "y":5.37614, "heading":-2.50106, "vx":-0.95657, "vy":0.29066, "omega":0.61683, "ax":0.0, "ay":0.0, "alpha":0.00295, "fx":[0.01012,0.00155,-0.01011,-0.00149], "fy":[-0.00141,0.01002,0.00152,-0.00991]}, - {"t":1.24198, "x":6.07244, "y":5.38818, "heading":-2.47552, "vx":-0.95657, "vy":0.29066, "omega":0.61695, "ax":0.0, "ay":0.0, "alpha":0.00262, "fx":[0.00889,0.00107,-0.00892,-0.00109], "fy":[-0.00113,0.00896,0.00108,-0.00909]}, - {"t":1.28338, "x":6.03284, "y":5.40021, "heading":-2.44998, "vx":-0.95657, "vy":0.29066, "omega":0.61706, "ax":0.0, "ay":0.0, "alpha":0.00235, "fx":[0.00816,0.00081,-0.00813,-0.00077], "fy":[-0.00071,0.00805,0.00082,-0.00793]}, - {"t":1.32478, "x":5.99323, "y":5.41224, "heading":-2.42444, "vx":-0.95657, "vy":0.29066, "omega":0.61716, "ax":0.0, "ay":-0.00001, "alpha":0.00208, "fx":[0.00715,0.00047,-0.00721,-0.00053], "fy":[-0.0005,0.00692,0.00054,-0.00732]}, - {"t":1.36618, "x":5.95363, "y":5.42428, "heading":-2.39889, "vx":-0.95657, "vy":0.29066, "omega":0.61725, "ax":0.0, "ay":0.00001, "alpha":0.0019, "fx":[0.00664,0.00032,-0.00657,-0.00026], "fy":[-0.00019,0.00655,0.00038,-0.00635]}, - {"t":1.40758, "x":5.91403, "y":5.43631, "heading":-2.37333, "vx":-0.95657, "vy":0.29066, "omega":0.61732, "ax":0.0, "ay":-0.00001, "alpha":0.00164, "fx":[0.00574,0.00005,-0.00584,-0.00016], "fy":[-0.00007,0.00511,0.00016,-0.0059]}, - {"t":1.44898, "x":5.87443, "y":5.44834, "heading":-2.34778, "vx":-0.95657, "vy":0.29066, "omega":0.61739, "ax":0.0, "ay":0.00001, "alpha":0.00152, "fx":[0.0054,-0.00002,-0.00526,0.0001], "fy":[0.00023,0.00536,0.00015,-0.00498]}, - {"t":1.49038, "x":5.83483, "y":5.46038, "heading":-2.32222, "vx":-0.95657, "vy":0.29066, "omega":0.61746, "ax":-0.00001, "ay":-0.00002, "alpha":0.00125, "fx":[0.00451,-0.00025,-0.0047,0.00005], "fy":[0.00021,0.00333,-0.0001,-0.00471]}, - {"t":1.53178, "x":5.79523, "y":5.47241, "heading":-2.29666, "vx":-0.95657, "vy":0.29066, "omega":0.61751, "ax":0.00001, "ay":0.00002, "alpha":0.0012, "fx":[0.00435,-0.00022,-0.00408,0.00037], "fy":[0.00058,0.0044,0.00011,-0.00371]}, - {"t":1.57317, "x":5.75562, "y":5.48444, "heading":-2.27109, "vx":-0.95657, "vy":0.29066, "omega":0.61756, "ax":-0.00001, "ay":-0.00003, "alpha":0.00089, "fx":[0.00339,-0.00047,-0.00372,0.00013], "fy":[0.00034,0.00144,-0.00028,-0.00369]}, - {"t":1.61457, "x":5.71602, "y":5.49648, "heading":-2.24552, "vx":-0.95657, "vy":0.29066, "omega":0.61759, "ax":0.00001, "ay":0.00004, "alpha":0.00091, "fx":[0.00343,-0.0003,-0.00298,0.00057], "fy":[0.00094,0.00365,0.00026,-0.00245]}, - {"t":1.65597, "x":5.67642, "y":5.50851, "heading":-2.21996, "vx":-0.95657, "vy":0.29066, "omega":0.61763, "ax":-0.00002, "ay":-0.00005, "alpha":0.00054, "fx":[0.00232,-0.00063,-0.00287,0.00008], "fy":[0.00031,-0.00069,-0.00043,-0.00281]}, - {"t":1.69737, "x":5.63682, "y":5.52054, "heading":-2.19439, "vx":-0.95657, "vy":0.29066, "omega":0.61765, "ax":0.00002, "ay":0.00006, "alpha":0.00064, "fx":[0.00262,-0.00024,-0.00188,0.00073], "fy":[0.00135,0.00312,0.00067,-0.0011]}, - {"t":1.73877, "x":5.59722, "y":5.53257, "heading":-2.16882, "vx":-0.95657, "vy":0.29066, "omega":0.61768, "ax":-0.00003, "ay":-0.00009, "alpha":0.00018, "fx":[0.00123,-0.00076,-0.00212,-0.00013], "fy":[0.00006,-0.00322,-0.00063,-0.00209]}, - {"t":1.78017, "x":5.55762, "y":5.54461, "heading":-2.14324, "vx":-0.95657, "vy":0.29066, "omega":0.61769, "ax":0.00003, "ay":0.0001, "alpha":0.00037, "fx":[0.00188,-0.00002,-0.0007,0.00087], "fy":[0.00193,0.00285,0.00142,0.0005]}, - {"t":1.82157, "x":5.51802, "y":5.55664, "heading":-2.11767, "vx":-0.95657, "vy":0.29066, "omega":0.6177, "ax":-0.00004, "ay":-0.00014, "alpha":-0.00022, "fx":[0.00001,-0.00089,-0.00144,-0.00054], "fy":[-0.00058,-0.00628,-0.00098,-0.0016]}, - {"t":1.86297, "x":5.47841, "y":5.56867, "heading":-2.0921, "vx":-0.95657, "vy":0.29065, "omega":0.61769, "ax":0.00005, "ay":0.00016, "alpha":0.00006, "fx":[0.00115,0.00046,0.00072,0.001], "fy":[0.00279,0.0029,0.0027,0.00258]}, - {"t":1.90437, "x":5.43881, "y":5.58071, "heading":-2.06653, "vx":-0.95657, "vy":0.29066, "omega":0.6177, "ax":-0.00007, "ay":-0.00022, "alpha":-0.00066, "fx":[-0.00153,-0.00102,-0.00074,-0.00125], "fy":[-0.00188,-0.00998,-0.00167,-0.00141]}, - {"t":1.94577, "x":5.39921, "y":5.59274, "heading":-2.04096, "vx":-0.95658, "vy":0.29065, "omega":0.61767, "ax":0.00008, "ay":0.00026, "alpha":-0.00034, "fx":[0.0003,0.00138,0.0026,0.00107], "fy":[0.00403,0.00327,0.00477,0.00553]}, - {"t":1.98717, "x":5.35961, "y":5.60477, "heading":-2.01538, "vx":-0.95657, "vy":0.29066, "omega":0.61766, "ax":-0.00011, "ay":-0.00035, "alpha":-0.00119, "fx":[-0.00368,-0.0011,0.00013,-0.00245], "fy":[-0.00438,-0.01428,-0.00299,-0.00172]}, - {"t":2.02857, "x":5.32001, "y":5.61681, "heading":-1.98981, "vx":-0.95658, "vy":0.29065, "omega":0.61761, "ax":0.00012, "ay":0.00041, "alpha":-0.00093, "fx":[-0.00094,0.00302,0.00527,0.00098], "fy":[0.00572,0.00389,0.00799,0.00982]}, - {"t":2.06997, "x":5.28041, "y":5.62884, "heading":-1.96425, "vx":-0.95657, "vy":0.29066, "omega":0.61757, "ax":-0.00016, "ay":-0.00054, "alpha":-0.00183, "fx":[-0.0069,-0.001,0.00146,-0.00445], "fy":[-0.00897,-0.01877,-0.00535,-0.00278]}, - {"t":2.11137, "x":5.24081, "y":5.64087, "heading":-1.93868, "vx":-0.95658, "vy":0.29064, "omega":0.61749, "ax":0.00019, "ay":0.00061, "alpha":-0.00188, "fx":[-0.00297,0.00583,0.00916,0.00041], "fy":[0.00766,0.00445,0.01279,0.01601]}, - {"t":2.15277, "x":5.2012, "y":5.6529, "heading":-1.91312, "vx":-0.95657, "vy":0.29067, "omega":0.61741, "ax":-0.00024, "ay":-0.0008, "alpha":-0.00261, "fx":[-0.01175,-0.00038,0.00367,-0.0077], "fy":[-0.01686,-0.02216,-0.0092,-0.00496]}, - {"t":2.19416, "x":5.1616, "y":5.66494, "heading":-1.88755, "vx":-0.95658, "vy":0.29063, "omega":0.61731, "ax":0.00026, "ay":0.00087, "alpha":-0.0034, "fx":[-0.00643,0.01047,0.01471,-0.00121], "fy":[0.00931,0.00432,0.01956,0.02455]}, - {"t":2.23556, "x":5.122, "y":5.67697, "heading":-1.862, "vx":-0.95657, "vy":0.29067, "omega":0.61717, "ax":-0.00033, "ay":-0.0011, "alpha":-0.00351, "fx":[-0.01867,0.00152,0.00757,-0.01262], "fy":[-0.02917,-0.02129,-0.01446,-0.00816]}, - {"t":2.27696, "x":5.0824, "y":5.689, "heading":-1.83645, "vx":-0.95658, "vy":0.29062, "omega":0.61702, "ax":0.00034, "ay":0.00114, "alpha":-0.00576, "fx":[-0.01217,0.01771,0.02225,-0.00481], "fy":[0.00941,0.00231,0.02843,0.03554]}, - {"t":2.31836, "x":5.0428, "y":5.70103, "heading":-1.8109, "vx":-0.95657, "vy":0.29067, "omega":0.61678, "ax":-0.00038, "ay":-0.00126, "alpha":-0.00453, "fx":[-0.02744,0.0064,0.01468,-0.01916], "fy":[-0.04522,-0.00962,-0.01885,-0.01031]}, - {"t":2.35976, "x":5.0032, "y":5.71307, "heading":-1.78537, "vx":-0.95658, "vy":0.29062, "omega":0.61659, "ax":0.0004, "ay":0.00131, "alpha":-0.00928, "fx":[-0.02138,0.02824,0.03164,-0.0119], "fy":[0.00528,-0.00397,0.03851,0.04776]}, - {"t":2.40116, "x":4.96359, "y":5.7251, "heading":-1.75984, "vx":-0.95657, "vy":0.29067, "omega":0.61621, "ax":-0.00026, "ay":-0.00086, "alpha":-0.0058, "fx":[-0.03641,0.01745,0.02775,-0.02611], "fy":[-0.05969,0.02258,-0.01521,-0.0047]}, - {"t":2.44256, "x":4.92399, "y":5.73713, "heading":-1.73433, "vx":-0.95658, "vy":0.29064, "omega":0.61597, "ax":0.00032, "ay":0.00104, "alpha":-0.01427, "fx":[-0.03637,0.04141,0.0414,-0.02536], "fy":[-0.01017,-0.02103,0.04486,0.05573]}, - {"t":2.48396, "x":4.88439, "y":5.74917, "heading":-1.70883, "vx":-0.95657, "vy":0.29068, "omega":0.61538, "ax":0.00018, "ay":0.00058, "alpha":-0.00833, "fx":[-0.04379,0.03827,0.04966,-0.0324], "fy":[-0.06385,0.07594,0.0075,0.019]}, - {"t":2.52536, "x":4.84479, "y":5.7612, "heading":-1.68336, "vx":-0.95656, "vy":0.29071, "omega":0.61504, "ax":-0.00024, "ay":-0.00079, "alpha":-0.02119, "fx":[-0.06321,0.05207,0.04708,-0.05202], "fy":[-0.05711,-0.06829,0.03064,0.04181]}, - {"t":2.56676, "x":4.80519, "y":5.77323, "heading":-1.65789, "vx":-0.95657, "vy":0.29067, "omega":0.61416, "ax":0.00069, "ay":0.00227, "alpha":-0.01561, "fx":[-0.05497,0.06732,0.07798,-0.04431], "fy":[-0.06087,0.10059,0.05056,0.06121]}, - {"t":2.60816, "x":4.76559, "y":5.78527, "heading":-1.63247, "vx":-0.95654, "vy":0.29077, "omega":0.61351, "ax":-0.00189, "ay":-0.00622, "alpha":-0.03126, "fx":[-0.11377,0.04934,0.04315,-0.10466], "fy":[-0.17356,-0.18274,-0.03373,-0.02455]}, - {"t":2.64956, "x":4.72599, "y":5.7973, "heading":-1.60707, "vx":-0.95662, "vy":0.29051, "omega":0.61222, "ax":0.00148, "ay":0.00487, "alpha":-0.03615, "fx":[-0.07237,0.11489,0.12166,-0.0656], "fy":[-0.01801,0.01489,0.16046,0.16717]}, - {"t":2.69096, "x":4.68638, "y":5.80933, "heading":-1.58172, "vx":-0.95656, "vy":0.29071, "omega":0.61072, "ax":-0.00424, "ay":-0.01396, "alpha":-0.04889, "fx":[-0.19011,0.04572,0.04927,-0.18755], "fy":[-0.35137,-0.35396,-0.1141,-0.11151]}, - {"t":2.73236, "x":4.64678, "y":5.82136, "heading":-1.55644, "vx":-0.95673, "vy":0.29013, "omega":0.6087, "ax":0.01441, "ay":0.04734, "alpha":-0.08221, "fx":[0.08425,0.40083,0.3962,0.07963], "fy":[0.71846,0.37322,1.03481,1.03032]}, - {"t":2.77376, "x":4.60718, "y":5.83341, "heading":-1.53124, "vx":-0.95614, "vy":0.29209, "omega":0.60529, "ax":-0.01899, "ay":-0.06246, "alpha":-0.08967, "fx":[-0.52584,-0.08647,-0.11075,-0.54313], "fy":[-1.26825,-1.25092,-0.81417,-0.83149]}, - {"t":2.81515, "x":4.56758, "y":5.84545, "heading":-1.50618, "vx":-0.95692, "vy":0.28951, "omega":0.60158, "ax":0.00169, "ay":0.0056, "alpha":-0.14174, "fx":[-0.25814,0.35666,0.31287,-0.29846], "fy":[-0.14081,-0.44908,0.50084,0.46223]}, - {"t":2.85655, "x":4.52797, "y":5.85744, "heading":-1.48128, "vx":-0.95685, "vy":0.28974, "omega":0.59571, "ax":-0.35302, "ay":-1.29457, "alpha":-0.17428, "fx":[-6.26678,-5.43375,-5.48823,-6.34991], "fy":[-22.03727,-21.96725,-21.12225,-21.19296]}, - {"t":2.89795, "x":4.48805, "y":5.86832, "heading":-1.45661, "vx":-0.97147, "vy":0.23614, "omega":0.5885, "ax":-0.51417, "ay":-2.85224, "alpha":-0.20133, "fx":[-9.06813,-7.94987,-8.04925,-9.2165], "fy":[-48.2982,-47.56101,-47.10552,-47.21689]}, - {"t":2.93935, "x":4.4474, "y":5.87566, "heading":-1.43225, "vx":-0.99275, "vy":0.11806, "omega":0.58016, "ax":-0.1683, "ay":-2.97015, "alpha":-0.28856, "fx":[-3.40504,-2.09101,-2.10224,-3.62375], "fy":[-50.293,-50.10737,-48.72853,-48.91518]}, - {"t":2.98075, "x":4.40615, "y":5.878, "heading":-1.40823, "vx":-0.99972, "vy":-0.0049, "omega":0.56822, "ax":0.20028, "ay":-2.98222, "alpha":-0.41283, "fx":[2.50572,4.47569,4.17919,2.19382], "fy":[-50.61993,-50.84497,-48.53766,-48.84583]}, - {"t":3.02215, "x":4.36494, "y":5.87524, "heading":-1.38471, "vx":-0.99143, "vy":-0.12836, "omega":0.55113, "ax":0.56886, "ay":-2.93923, "alpha":-0.50935, "fx":[8.41865,10.94844,10.58069,7.98286], "fy":[-50.39903,-49.90868,-47.58984,-48.08476]}, - {"t":3.06355, "x":4.32438, "y":5.86741, "heading":-1.36189, "vx":-0.96788, "vy":-0.25004, "omega":0.53004, "ax":0.92993, "ay":-2.84802, "alpha":-0.68717, "fx":[14.1093,17.48457,16.9139,13.4985], "fy":[-49.3192,-48.81815,-45.49603,-46.26695]}, - {"t":3.10495, "x":4.28511, "y":5.85461, "heading":-1.33995, "vx":-0.92938, "vy":-0.36795, "omega":0.50159, "ax":1.27835, "ay":-2.7109, "alpha":-0.88863, "fx":[19.54161,23.85315,23.15422,18.68918], "fy":[-47.79575,-46.63018,-42.58152,-43.75003]}, - {"t":3.14635, "x":4.24773, "y":5.83706, "heading":-1.31918, "vx":-0.87646, "vy":-0.48018, "omega":0.4648, "ax":1.61118, "ay":-2.52815, "alpha":-1.19176, "fx":[24.5568,30.31163,29.20316,23.3591], "fy":[-45.64367,-44.11627,-38.54856,-40.26373]}, - {"t":3.18775, "x":4.21282, "y":5.81501, "heading":-1.29994, "vx":-0.80976, "vy":-0.58484, "omega":0.41546, "ax":1.90557, "ay":-2.31496, "alpha":-1.57642, "fx":[28.79773,36.42743,34.74023,27.09404], "fy":[-43.39522,-40.92793,-33.78829,-36.24594]}, - {"t":3.22915, "x":4.18093, "y":5.78882, "heading":-1.28274, "vx":-0.73087, "vy":-0.68068, "omega":0.3502, "ax":0.67394, "ay":-0.73352, "alpha":-1.09358, "fx":[9.42425,14.52698,13.05525,7.93026], "fy":[-15.51989,-14.04656,-8.90435,-10.43913]}, - {"t":3.26162, "x":4.15756, "y":5.76633, "heading":-1.27137, "vx":-0.70899, "vy":-0.7045, "omega":0.3147, "ax":0.71291, "ay":-0.69821, "alpha":-0.85233, "fx":[10.43538,14.45579,13.24594,9.39835], "fy":[-14.28693,-13.00646,-8.99951,-10.26215]}, - {"t":3.29408, "x":4.13491, "y":5.74309, "heading":-1.26115, "vx":-0.68584, "vy":-0.72716, "omega":0.28702, "ax":0.73641, "ay":-0.67306, "alpha":-0.68516, "fx":[11.18771,14.35691,13.36794,10.18958], "fy":[-13.29791,-12.33845,-9.10509,-10.13668]}, - {"t":3.32655, "x":4.11304, "y":5.71913, "heading":-1.25183, "vx":-0.66193, "vy":-0.74902, "omega":0.26478, "ax":0.75857, "ay":-0.64759, "alpha":-0.54277, "fx":[11.78223,14.2718,13.46741,11.05883], "fy":[-12.57326,-11.58159,-9.09436,-9.93088]}, - {"t":3.35902, "x":4.09194, "y":5.69447, "heading":-1.24324, "vx":-0.6373, "vy":-0.77004, "omega":0.24716, "ax":0.79381, "ay":-0.60342, "alpha":-0.43142, "fx":[12.58285,14.53329,13.88396,11.92991], "fy":[-11.31146,-10.88356,-8.68241,-9.35751]}, - {"t":3.39148, "x":4.07167, "y":5.66915, "heading":-1.23521, "vx":-0.61153, "vy":-0.78963, "omega":0.23315, "ax":0.9051, "ay":-0.41784, "alpha":-0.33247, "fx":[14.55501,16.07522,15.55268,14.16745], "fy":[-8.11271,-7.4216,-5.89357,-6.4328]}, - {"t":3.42395, "x":4.05229, "y":5.64329, "heading":-1.22764, "vx":-0.58214, "vy":-0.8032, "omega":0.22235, "ax":0.98842, "ay":-0.12987, "alpha":-0.25667, "fx":[16.09713,17.27372,16.85618,15.67866], "fy":[-2.98429,-2.5136,-1.36706,-1.79432]}, - {"t":3.45642, "x":4.03391, "y":5.61714, "heading":-1.22042, "vx":-0.55005, "vy":-0.80742, "omega":0.21402, "ax":0.98617, "ay":0.14781, "alpha":-0.17832, "fx":[16.10989,17.01843,16.68541,15.94215], "fy":[1.95743,2.13154,3.05097,2.71587]}, - {"t":3.48889, "x":4.01658, "y":5.59101, "heading":-1.21348, "vx":-0.51803, "vy":-0.80262, "omega":0.20823, "ax":0.93018, "ay":0.36041, "alpha":-0.14699, "fx":[15.29058,15.98151,15.72087,15.02952], "fy":[5.47877,5.92969,6.44119,6.18166]}, - {"t":3.52135, "x":4.00025, "y":5.56514, "heading":-1.20672, "vx":-0.48783, "vy":-0.79092, "omega":0.20346, "ax":0.85949, "ay":0.50706, "alpha":-0.0992, "fx":[14.1451,14.67296,14.46894,14.02232], "fy":[8.22105,8.24117,8.77406,8.57321]}, - {"t":3.55382, "x":3.98486, "y":5.53973, "heading":-1.20011, "vx":-0.45993, "vy":-0.77445, "omega":0.20024, "ax":0.79307, "ay":0.60619, "alpha":-0.08239, "fx":[13.10187,13.49313,13.33871,12.94676], "fy":[9.78858,10.10248,10.34039,10.18789]}, - {"t":3.58629, "x":3.97035, "y":5.5149, "heading":-1.19361, "vx":-0.43418, "vy":-0.75477, "omega":0.19756, "ax":0.73623, "ay":0.67443, "alpha":-0.05219, "fx":[12.17067,12.46587,12.34705,12.1071], "fy":[11.14288,11.11671,11.41313,11.297]}, - {"t":3.61875, "x":3.95664, "y":5.49075, "heading":-1.18719, "vx":-0.41027, "vy":-0.73287, "omega":0.19587, "ax":0.689, "ay":0.72288, "alpha":-0.04386, "fx":[11.42311,11.63421,11.54782,11.3359], "fy":[11.8668,12.07946,12.17005,12.084]}, - {"t":3.65122, "x":3.94368, "y":5.46734, "heading":-1.18084, "vx":-0.3879, "vy":-0.7094, "omega":0.19444, "ax":0.64992, "ay":0.75841, "alpha":-0.02422, "fx":[10.77602,10.93436,10.8684,10.75669], "fy":[12.61827,12.56638,12.72446,12.66]}, - {"t":3.68369, "x":3.93143, "y":5.44471, "heading":-1.17452, "vx":-0.3668, "vy":-0.68478, "omega":0.19366, "ax":0.61743, "ay":0.78524, "alpha":-0.02207, "fx":[10.26084,10.37025,10.32405,10.21381], "fy":[12.98263,13.13449,13.14423,13.09717]}, - {"t":3.71616, "x":3.91985, "y":5.42289, "heading":-1.16823, "vx":-0.34676, "vy":-0.65929, "omega":0.19294, "ax":0.59017, "ay":0.80606, "alpha":-0.00984, "fx":[9.80345,9.88889,9.85216,9.80709], "fy":[13.44791,13.38824,13.47302,13.43704]}, - {"t":3.74862, "x":3.9089, "y":5.40191, "heading":-1.16197, "vx":-0.3276, "vy":-0.63312, "omega":0.19262, "ax":0.56708, "ay":0.82257, "alpha":-0.01198, "fx":[9.43576,9.49742,9.47061,9.4082], "fy":[13.6428,13.75856,13.73699,13.70885]}, - {"t":3.78109, "x":3.89856, "y":5.38178, "heading":-1.15572, "vx":-0.30918, "vy":-0.60641, "omega":0.19223, "ax":0.54733, "ay":0.83593, "alpha":-0.00518, "fx":[9.09854,9.15698,9.13108,9.10823], "fy":[13.95338,13.89821,13.95595,13.93052]}, - {"t":3.81356, "x":3.88881, "y":5.36254, "heading":-1.14947, "vx":-0.29141, "vy":-0.57927, "omega":0.19206, "ax":0.53027, "ay":0.84692, "alpha":-0.01082, "fx":[8.82451,8.87906,8.85453,8.79932], "fy":[14.05742,14.15683,14.14137,14.11567]}, - {"t":3.84603, "x":3.87963, "y":5.34418, "heading":-1.14324, "vx":-0.2742, "vy":-0.55177, "omega":0.19171, "ax":0.51541, "ay":0.85611, "alpha":-0.0086, "fx":[8.56529,8.63384,8.60238,8.56524], "fy":[14.27306,14.23524,14.30312,14.27235]}, - {"t":3.87849, "x":3.871, "y":5.32671, "heading":-1.13701, "vx":-0.25746, "vy":-0.52398, "omega":0.19143, "ax":0.50237, "ay":0.86388, "alpha":-0.01758, "fx":[8.35227,8.43531,8.39654,8.31293], "fy":[14.32216,14.42431,14.4474,14.40815]}, - {"t":3.91096, "x":3.8629, "y":5.31016, "heading":-1.1308, "vx":-0.24115, "vy":-0.49593, "omega":0.19086, "ax":0.49084, "ay":0.87053, "alpha":-0.01991, "fx":[8.1449,8.25922,8.20487,8.11906], "fy":[14.47205,14.46639,14.58003,14.52706]}, - {"t":3.94343, "x":3.85533, "y":5.29451, "heading":-1.1246, "vx":-0.22522, "vy":-0.46766, "omega":0.19022, "ax":0.48057, "ay":0.87628, "alpha":-0.03286, "fx":[7.97254,8.12156,8.04951,7.90001], "fy":[14.48248,14.60801,14.70484,14.63359]}, - {"t":3.97589, "x":3.84827, "y":5.27979, "heading":-1.11843, "vx":-0.20961, "vy":-0.43921, "omega":0.18915, "ax":0.47138, "ay":0.8813, "alpha":-0.04053, "fx":[7.80007,8.00114,7.90232,7.72731], "fy":[14.58073,14.62608,14.82631,14.73017]}, - {"t":4.00836, "x":3.84172, "y":5.266, "heading":-1.11229, "vx":-0.19431, "vy":-0.4106, "omega":0.18783, "ax":0.46311, "ay":0.88571, "alpha":-0.05887, "fx":[7.65448,7.91588,7.78533,7.52352], "fy":[14.5573,14.73273,14.94745,14.81974]}, - {"t":4.04083, "x":3.83565, "y":5.25313, "heading":-1.10619, "vx":-0.17927, "vy":-0.38184, "omega":0.18592, "ax":0.45562, "ay":0.88961, "alpha":-0.0736, "fx":[7.50491,7.84656,7.67331,7.35536], "fy":[14.60849,14.73217,15.07264,14.90416]}, - {"t":4.0733, "x":3.83007, "y":5.2412, "heading":-1.10015, "vx":-0.16448, "vy":-0.35296, "omega":0.18353, "ax":0.44882, "ay":0.89309, "alpha":-0.09983, "fx":[7.37559,7.81322,7.58789,7.1499], "fy":[14.54838,14.81015,15.20511,14.98571]}, - {"t":4.10576, "x":3.82497, "y":5.23021, "heading":-1.09419, "vx":-0.14991, "vy":-0.32397, "omega":0.18029, "ax":0.44261, "ay":0.8962, "alpha":-0.12463, "fx":[7.23932,7.79807,7.50618,6.96912], "fy":[14.54876,14.79282,15.3497,15.06588]}, - {"t":4.13823, "x":3.82033, "y":5.22017, "heading":-1.08834, "vx":-0.13554, "vy":-0.29487, "omega":0.17625, "ax":0.43693, "ay":0.89901, "alpha":-0.16273, "fx":[7.11736,7.82415,7.44954,6.74247], "fy":[14.4422,14.84445,15.51085,15.14691]}, - {"t":4.1707, "x":3.81616, "y":5.21107, "heading":-1.08262, "vx":-0.12135, "vy":-0.26568, "omega":0.17096, "ax":0.4317, "ay":0.90155, "alpha":-0.20244, "fx":[6.9855,7.8745,7.39686,6.52803], "fy":[14.37977,14.80894,15.69483,15.2304]}, - {"t":4.20316, "x":3.81245, "y":5.20292, "heading":-1.07707, "vx":-0.10734, "vy":-0.23641, "omega":0.16439, "ax":0.42688, "ay":0.90387, "alpha":-0.25865, "fx":[6.862,7.97691,7.36975,6.25468], "fy":[14.20835,14.8329,15.90803,15.3188]}, - {"t":4.23563, "x":3.80919, "y":5.19572, "heading":-1.07173, "vx":-0.09348, "vy":-0.20706, "omega":0.15599, "ax":0.42242, "ay":0.90598, "alpha":-0.32086, "fx":[6.72504,8.11481,7.34845,5.97754], "fy":[14.06056,14.7748,16.15926,15.41421]}, - {"t":4.2681, "x":3.80638, "y":5.18947, "heading":-1.06666, "vx":-0.07976, "vy":-0.17765, "omega":0.14558, "ax":0.41828, "ay":0.90791, "alpha":-0.40482, "fx":[6.58943,8.32309,7.35544,5.62184], "fy":[13.79365,14.76616,16.45822,15.51978]}, - {"t":4.30057, "x":3.80401, "y":5.18418, "heading":-1.06194, "vx":-0.06618, "vy":-0.14817, "omega":0.13243, "ax":0.41442, "ay":0.90969, "alpha":-0.50135, "fx":[6.43538,8.58561,7.37224,5.23979], "fy":[13.52315,14.67688,16.81777,15.63864]}, - {"t":4.33303, "x":3.80208, "y":5.17985, "heading":-1.05764, "vx":-0.05273, "vy":-0.11864, "omega":0.11615, "ax":0.41083, "ay":0.91133, "alpha":-0.62789, "fx":[6.27341,8.94754,7.4231,4.7495], "fy":[13.11231,14.62541,17.25278,15.77536]}, - {"t":4.3655, "x":3.80058, "y":5.17648, "heading":-1.05387, "vx":-0.03939, "vy":-0.08905, "omega":0.09577, "ax":0.40748, "ay":0.91285, "alpha":-0.77699, "fx":[6.08499,9.39394,7.49122,4.19957], "fy":[12.65954,14.49031,17.78235,15.93493]}, - {"t":4.39797, "x":3.79952, "y":5.17407, "heading":-1.05076, "vx":-0.02616, "vy":-0.05941, "omega":0.07054, "ax":0.40433, "ay":0.91426, "alpha":-0.96895, "fx":[5.87511,9.98441,7.60411,3.49641], "fy":[12.02957,14.37757,18.42926,16.12467]}, - {"t":4.43044, "x":3.79888, "y":5.17262, "heading":-1.04847, "vx":-0.01303, "vy":-0.02973, "omega":0.03908, "ax":0.40138, "ay":0.91557, "alpha":-1.20375, "fx":[5.62491,10.72215,7.74779,2.66838], "fy":[11.27558,14.17589,19.23493,16.36207]}, - {"t":4.4629, "x":3.79867, "y":5.17214, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.10089, "y":6.16635, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.98273, "ay":-0.30728, "alpha":3.8714, "fx":[-40.82092,-41.33439,-58.48722,-58.24], "fy":[-15.66273,5.20633,4.9747,-15.0074]}, + {"t":0.04117, "x":7.09836, "y":6.16609, "heading":3.14159, "vx":-0.12279, "vy":-0.01265, "omega":0.15938, "ax":-2.98336, "ay":-0.30735, "alpha":2.96383, "fx":[-42.91854,-43.26583,-56.46384,-56.27654], "fy":[-13.13377,2.72199,2.6241,-12.70578]}, + {"t":0.08234, "x":7.09078, "y":6.16531, "heading":-3.13503, "vx":-0.24561, "vy":-0.0253, "omega":0.28139, "ax":-2.98322, "ay":-0.30734, "alpha":2.30248, "fx":[-44.4086,-44.60572,-55.05709,-54.84391], "fy":[-11.22288,0.91475,0.82382,-11.0082]}, + {"t":0.1235, "x":7.07814, "y":6.16401, "heading":-3.12345, "vx":-0.36842, "vy":-0.03796, "omega":0.37618, "ax":-2.98302, "ay":-0.30731, "alpha":1.62387, "fx":[-45.97706,-46.48147,-53.34175,-53.10183], "fy":[-9.53153,-0.63661,-0.7888,-9.53422]}, + {"t":0.16467, "x":7.06044, "y":6.16219, "heading":-3.10796, "vx":-0.49123, "vy":-0.05061, "omega":0.44303, "ax":-2.98272, "ay":-0.30728, "alpha":1.2647, "fx":[-46.72322,-46.98968,-52.7184,-52.45095], "fy":[-8.36392,-1.74076,-1.91931,-8.46518]}, + {"t":0.20584, "x":7.03769, "y":6.15984, "heading":-3.08972, "vx":-0.61402, "vy":-0.06326, "omega":0.4951, "ax":-2.98223, "ay":-0.30723, "alpha":0.89603, "fx":[-47.32566,-48.03977,-51.87724,-51.60642], "fy":[-7.36459,-2.69071,-2.90652,-7.52391]}, + {"t":0.24701, "x":7.00989, "y":6.15698, "heading":-3.06934, "vx":-0.73679, "vy":-0.07591, "omega":0.53198, "ax":-2.98123, "ay":-0.30713, "alpha":0.64906, "fx":[-48.11291,-48.36144,-51.27731,-51.03079], "fy":[-6.71589,-3.27996,-3.57292,-6.91024]}, + {"t":0.28818, "x":6.97703, "y":6.15359, "heading":-3.04744, "vx":-0.85952, "vy":-0.08855, "omega":0.5587, "ax":-2.9782, "ay":-0.30682, "alpha":0.47167, "fx":[-48.51126,-48.50082,-50.89624,-50.67208], "fy":[-6.1755,-3.86308,-4.05715,-6.36225]}, + {"t":0.32934, "x":6.93912, "y":6.14969, "heading":-3.02444, "vx":-0.98213, "vy":-0.10118, "omega":0.57812, "ax":-0.30033, "ay":-0.03069, "alpha":0.32695, "fx":[-4.02003,-4.2278,-5.99268,-5.7852], "fy":[-1.12477,-0.06259,0.47599,-1.33514]}, + {"t":0.37051, "x":6.89843, "y":6.1455, "heading":-3.00064, "vx":-0.99449, "vy":-0.10244, "omega":0.59158, "ax":0.00005, "ay":-0.00048, "alpha":0.29144, "fx":[0.65706,0.8826,-0.86216,-0.67437], "fy":[-0.56879,0.73707,0.55258,-0.75319]}, + {"t":0.41168, "x":6.85749, "y":6.14128, "heading":-2.97628, "vx":-0.99449, "vy":-0.10246, "omega":0.60358, "ax":-0.00003, "ay":0.00025, "alpha":0.21684, "fx":[0.56117,0.40128,-0.56204,-0.40214], "fy":[-0.46636,0.7582,0.35428,-0.62944]}, + {"t":0.45285, "x":6.81655, "y":6.13706, "heading":-2.95144, "vx":-0.99449, "vy":-0.10245, "omega":0.61251, "ax":0.0, "ay":0.0, "alpha":0.15289, "fx":[0.42419,0.32819,-0.44603,-0.30636], "fy":[-0.28917,0.4261,0.28922,-0.42605]}, + {"t":0.49401, "x":6.77561, "y":6.13284, "heading":-2.92622, "vx":-0.99449, "vy":-0.10245, "omega":0.6188, "ax":0.00001, "ay":-0.00006, "alpha":0.11428, "fx":[0.32433,0.20855,-0.32412,-0.20834], "fy":[-0.21872,0.34444,0.2071,-0.33689]}, + {"t":0.53518, "x":6.73467, "y":6.12863, "heading":-2.90075, "vx":-0.99449, "vy":-0.10246, "omega":0.62351, "ax":-0.00001, "ay":0.00013, "alpha":0.08502, "fx":[0.25321,0.15224,-0.25279,-0.15358], "fy":[-0.14838,0.25003,0.15285,-0.24557]}, + {"t":0.57635, "x":6.69373, "y":6.12441, "heading":-2.87508, "vx":-0.99449, "vy":-0.10245, "omega":0.62701, "ax":0.00001, "ay":-0.00014, "alpha":0.06395, "fx":[0.19001,0.10903,-0.18952,-0.10854], "fy":[-0.11254,0.18865,0.10966,-0.19523]}, + {"t":0.61752, "x":6.65279, "y":6.12019, "heading":-2.84927, "vx":-0.99449, "vy":-0.10246, "omega":0.62964, "ax":-0.00002, "ay":0.00015, "alpha":0.04875, "fx":[0.1495,0.07969,-0.14956,-0.08065], "fy":[-0.07638,0.14873,0.08133,-0.14378]}, + {"t":0.65869, "x":6.61185, "y":6.11597, "heading":-2.82334, "vx":-0.99449, "vy":-0.10245, "omega":0.63165, "ax":0.00001, "ay":-0.00012, "alpha":0.03654, "fx":[0.11302,0.05718,-0.11261,-0.05676], "fy":[-0.05807,0.10655,0.05871,-0.11531]}, + {"t":0.69985, "x":6.5709, "y":6.11176, "heading":-2.79734, "vx":-0.99449, "vy":-0.10246, "omega":0.63315, "ax":-0.00001, "ay":0.00009, "alpha":0.02866, "fx":[0.09057,0.04233,-0.09077,-0.04276], "fy":[-0.03988,0.08956,0.04294,-0.0865]}, + {"t":0.74102, "x":6.52996, "y":6.10754, "heading":-2.77128, "vx":-0.99449, "vy":-0.10245, "omega":0.63433, "ax":0.00001, "ay":-0.00007, "alpha":0.0217, "fx":[0.06861,0.02987,-0.06836,-0.02962], "fy":[-0.03063,0.06544,0.03117,-0.0708]}, + {"t":0.78219, "x":6.48902, "y":6.10332, "heading":-2.74516, "vx":-0.99449, "vy":-0.10245, "omega":0.63522, "ax":-0.00001, "ay":0.00005, "alpha":0.01743, "fx":[0.05711,0.02254,-0.05721,-0.02278], "fy":[-0.02075,0.05527,0.02244,-0.05358]}, + {"t":0.82336, "x":6.44808, "y":6.0991, "heading":-2.71901, "vx":-0.99449, "vy":-0.10245, "omega":0.63594, "ax":0.0, "ay":-0.00004, "alpha":0.01333, "fx":[0.04277,0.01522,-0.04264,-0.01509], "fy":[-0.01589,0.0419,0.01668,-0.04519]}, + {"t":0.86453, "x":6.40714, "y":6.09488, "heading":-2.69283, "vx":-0.99449, "vy":-0.10245, "omega":0.63649, "ax":0.0, "ay":0.00002, "alpha":0.01105, "fx":[0.03797,0.0118,-0.03797,-0.01196], "fy":[-0.01039,0.03516,0.01117,-0.03437]}, + {"t":0.90569, "x":6.3662, "y":6.09067, "heading":-2.66663, "vx":-0.99449, "vy":-0.10245, "omega":0.63694, "ax":0.0, "ay":-0.00002, "alpha":0.00852, "fx":[0.02746,0.00705,-0.02741,-0.00699], "fy":[-0.00767,0.02824,0.00874,-0.03035]}, + {"t":0.94686, "x":6.32526, "y":6.08645, "heading":-2.64041, "vx":-0.99449, "vy":-0.10245, "omega":0.6373, "ax":0.0, "ay":0.00001, "alpha":0.00736, "fx":[0.02686,0.00566,-0.02681,-0.00577], "fy":[-0.00445,0.02311,0.0047,-0.02286]}, + {"t":0.98803, "x":6.28432, "y":6.08223, "heading":-2.61417, "vx":-0.99449, "vy":-0.10245, "omega":0.6376, "ax":0.0, "ay":0.0, "alpha":0.00569, "fx":[0.01812,0.00221,-0.01811,-0.0022], "fy":[-0.00281,0.02019,0.00404,-0.02162]}, + {"t":1.0292, "x":6.24338, "y":6.07801, "heading":-2.58792, "vx":-0.99449, "vy":-0.10245, "omega":0.63783, "ax":0.0, "ay":0.0, "alpha":0.00515, "fx":[0.02024,0.00183,-0.02014,-0.00192], "fy":[-0.00077,0.01563,0.00073,-0.01566]}, + {"t":1.07036, "x":6.20243, "y":6.0738, "heading":-2.56166, "vx":-0.99449, "vy":-0.10245, "omega":0.63804, "ax":0.0, "ay":0.0, "alpha":0.00396, "fx":[0.01216,-0.00086,-0.01217,0.00085], "fy":[0.00031,0.01528,0.00097,-0.01633]}, + {"t":1.11153, "x":6.16149, "y":6.06958, "heading":-2.5354, "vx":-0.99449, "vy":-0.10245, "omega":0.63821, "ax":0.0, "ay":-0.00001, "alpha":0.00376, "fx":[0.01607,-0.00081,-0.01594,0.00072], "fy":[0.00172,0.0107,-0.00189,-0.01088]}, + {"t":1.1527, "x":6.12055, "y":6.06536, "heading":-2.50912, "vx":-0.99449, "vy":-0.10245, "omega":0.63836, "ax":0.0, "ay":0.00001, "alpha":0.00286, "fx":[0.00812,-0.00296,-0.00814,0.00293], "fy":[0.00251,0.0121,-0.00124,-0.01294]}, + {"t":1.19387, "x":6.07961, "y":6.06114, "heading":-2.48284, "vx":-0.99449, "vy":-0.10245, "omega":0.63848, "ax":0.0, "ay":-0.00001, "alpha":0.00283, "fx":[0.01321,-0.00278,-0.01307,0.00269], "fy":[0.0035,0.00721,-0.00373,-0.00745]}, + {"t":1.23504, "x":6.03867, "y":6.05692, "heading":-2.45656, "vx":-0.99449, "vy":-0.10245, "omega":0.6386, "ax":0.0, "ay":0.00001, "alpha":0.00211, "fx":[0.00518,-0.00445,-0.0052,0.00442], "fy":[0.00415,0.00988,-0.00294,-0.01059]}, + {"t":1.2762, "x":5.99773, "y":6.05271, "heading":-2.43027, "vx":-0.99449, "vy":-0.10245, "omega":0.63868, "ax":0.0, "ay":-0.00001, "alpha":0.00216, "fx":[0.01104,-0.00433,-0.01088,0.00423], "fy":[0.0048,0.00453,-0.00505,-0.00478]}, + {"t":1.31737, "x":5.95679, "y":6.04849, "heading":-2.40397, "vx":-0.99449, "vy":-0.10245, "omega":0.63877, "ax":0.0, "ay":0.00001, "alpha":0.00158, "fx":[0.00288,-0.00551,-0.00291,0.00548], "fy":[0.00542,0.00818,-0.00428,-0.00878]}, + {"t":1.35854, "x":5.91585, "y":6.04427, "heading":-2.37768, "vx":-0.99449, "vy":-0.10245, "omega":0.63884, "ax":0.0, "ay":-0.00001, "alpha":0.00163, "fx":[0.00917,-0.00554,-0.00901,0.00542], "fy":[0.00572,0.00228,-0.00596,-0.00253]}, + {"t":1.39971, "x":5.87491, "y":6.04005, "heading":-2.35138, "vx":-0.99449, "vy":-0.10245, "omega":0.6389, "ax":0.0, "ay":0.00001, "alpha":0.00117, "fx":[0.00096,-0.0062,-0.00098,0.00618], "fy":[0.00635,0.00675,-0.00534,-0.00725]}, + {"t":1.44088, "x":5.83396, "y":6.03584, "heading":-2.32508, "vx":-0.99449, "vy":-0.10245, "omega":0.63895, "ax":0.0, "ay":-0.00001, "alpha":0.00119, "fx":[0.00743,-0.00642,-0.00727,0.0063], "fy":[0.00629,0.00031,-0.0065,-0.00053]}, + {"t":1.48204, "x":5.79302, "y":6.03162, "heading":-2.29877, "vx":-0.99449, "vy":-0.10245, "omega":0.639, "ax":0.0, "ay":0.00001, "alpha":0.00082, "fx":[-0.00073,-0.00655,0.00071,0.00653], "fy":[0.00694,0.00547,-0.00613,-0.00585]}, + {"t":1.52321, "x":5.75208, "y":6.0274, "heading":-2.27247, "vx":-0.99449, "vy":-0.10245, "omega":0.63904, "ax":0.0, "ay":0.0, "alpha":0.00077, "fx":[0.0057,-0.00698,-0.00554,0.00684], "fy":[0.00652,-0.00149,-0.00667,0.00134]}, + {"t":1.56438, "x":5.71114, "y":6.02318, "heading":-2.24616, "vx":-0.99449, "vy":-0.10245, "omega":0.63907, "ax":0.0, "ay":0.0, "alpha":0.00052, "fx":[-0.00229,-0.00657,0.00227,0.00655], "fy":[0.00715,0.00427,-0.00666,-0.0045]}, + {"t":1.60555, "x":5.6702, "y":6.01896, "heading":-2.21985, "vx":-0.99449, "vy":-0.10245, "omega":0.63909, "ax":0.0, "ay":0.0, "alpha":0.00037, "fx":[0.00392,-0.00718,-0.00376,0.00703], "fy":[0.00641,-0.00318,-0.00645,0.00313]}, + {"t":1.64671, "x":5.62926, "y":6.01475, "heading":-2.19354, "vx":-0.99449, "vy":-0.10245, "omega":0.6391, "ax":0.0, "ay":0.0, "alpha":0.00022, "fx":[-0.00379,-0.00623,0.00378,0.00623], "fy":[0.00693,0.00316,-0.00692,-0.00314]}, + {"t":1.68788, "x":5.58832, "y":6.01053, "heading":-2.16723, "vx":-0.99449, "vy":-0.10245, "omega":0.63911, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[0.00202,-0.007,-0.00186,0.00683], "fy":[0.00593,-0.00484,-0.00585,0.00491]}, + {"t":1.72905, "x":5.54738, "y":6.00631, "heading":-2.14092, "vx":-0.99449, "vy":-0.10245, "omega":0.63911, "ax":0.0, "ay":0.0, "alpha":-0.0001, "fx":[-0.00534,-0.00553,0.00535,0.00554], "fy":[0.00621,0.00225,-0.00692,-0.00171]}, + {"t":1.77022, "x":5.50644, "y":6.00209, "heading":-2.11461, "vx":-0.99449, "vy":-0.10245, "omega":0.63911, "ax":0.0, "ay":0.0, "alpha":-0.00058, "fx":[-0.0001,-0.00639,0.00026,0.0062], "fy":[0.00502,-0.00661,-0.00488,0.00676]}, + {"t":1.81139, "x":5.46549, "y":5.99788, "heading":-2.0883, "vx":-0.99449, "vy":-0.10245, "omega":0.63908, "ax":0.0, "ay":0.0, "alpha":-0.00047, "fx":[-0.00709,-0.00443,0.0071,0.00443], "fy":[0.00485,0.00176,-0.00664,-0.00012]}, + {"t":1.85255, "x":5.42455, "y":5.99366, "heading":-2.06199, "vx":-0.99449, "vy":-0.10245, "omega":0.63906, "ax":0.0, "ay":0.0, "alpha":-0.00121, "fx":[-0.00256,-0.00526,0.00275,0.00507], "fy":[0.00355,-0.00872,-0.00355,0.00871]}, + {"t":1.89372, "x":5.38361, "y":5.98944, "heading":-2.03568, "vx":-0.99449, "vy":-0.10245, "omega":0.63901, "ax":0.0, "ay":0.00001, "alpha":-0.00091, "fx":[-0.00925,-0.00283,0.00922,0.0028], "fy":[0.0027,0.00222,-0.00602,0.00174]}, + {"t":1.93489, "x":5.34267, "y":5.98522, "heading":-2.00937, "vx":-0.99449, "vy":-0.10245, "omega":0.63897, "ax":0.0, "ay":-0.00002, "alpha":-0.00206, "fx":[-0.00555,-0.00348,0.00584,0.00334], "fy":[0.0012,-0.01159,-0.00195,0.01083]}, + {"t":1.97606, "x":5.30173, "y":5.981, "heading":-1.98307, "vx":-0.99449, "vy":-0.10245, "omega":0.63889, "ax":0.0, "ay":0.00005, "alpha":-0.00145, "fx":[-0.01213,-0.00061,0.01196,0.00045], "fy":[-0.00051,0.0046,-0.00496,0.00407]}, + {"t":2.01723, "x":5.26079, "y":5.97679, "heading":-1.95676, "vx":-0.99449, "vy":-0.10245, "omega":0.63883, "ax":0.00001, "ay":-0.00008, "alpha":-0.00324, "fx":[-0.00934,-0.00079,0.00988,0.00081], "fy":[-0.00255,-0.01586,-0.00018,0.01312]}, + {"t":2.05839, "x":5.21985, "y":5.97257, "heading":-1.93046, "vx":-0.99449, "vy":-0.10246, "omega":0.6387, "ax":-0.00001, "ay":0.00014, "alpha":-0.0021, "fx":[-0.01616,0.00249,0.01569,-0.00296], "fy":[-0.00522,0.01058,-0.00329,0.0071]}, + {"t":2.09956, "x":5.17891, "y":5.96835, "heading":-1.90417, "vx":-0.99449, "vy":-0.10245, "omega":0.63861, "ax":0.00002, "ay":-0.00021, "alpha":-0.00493, "fx":[-0.0143,0.00321,0.01534,-0.00282], "fy":[-0.00856,-0.02248,0.00161,0.01553]}, + {"t":2.14073, "x":5.13797, "y":5.96413, "heading":-1.87788, "vx":-0.99449, "vy":-0.10246, "omega":0.63841, "ax":-0.00003, "ay":0.00031, "alpha":-0.00288, "fx":[-0.02193,0.00688,0.02086,-0.00795], "fy":[-0.0122,0.02272,-0.00081,0.01107]}, + {"t":2.1819, "x":5.09702, "y":5.95992, "heading":-1.8516, "vx":-0.99449, "vy":-0.10245, "omega":0.63829, "ax":0.00005, "ay":-0.00044, "alpha":-0.00736, "fx":[-0.02094,0.00916,0.02289,-0.00808], "fy":[-0.01806,-0.03269,0.00337,0.018]}, + {"t":2.22306, "x":5.05608, "y":5.9557, "heading":-1.82532, "vx":-0.99449, "vy":-0.10246, "omega":0.63799, "ax":-0.00006, "ay":0.00061, "alpha":-0.00387, "fx":[-0.03018,0.01325,0.0281,-0.01533], "fy":[-0.02273,0.04429,0.00271,0.01611]}, + {"t":2.26423, "x":5.01514, "y":5.95148, "heading":-1.79906, "vx":-0.99449, "vy":-0.10244, "omega":0.63783, "ax":0.00008, "ay":-0.00081, "alpha":-0.01089, "fx":[-0.03001,0.01802,0.03338,-0.01581], "fy":[-0.03251,-0.04786,0.00542,0.02078]}, + {"t":2.3054, "x":4.9742, "y":5.94726, "heading":-1.7728, "vx":-0.99449, "vy":-0.10247, "omega":0.63738, "ax":-0.00011, "ay":0.00102, "alpha":-0.00531, "fx":[-0.04186,0.02276,0.03834,-0.02628], "fy":[-0.03903,0.07774,0.00745,0.02217]}, + {"t":2.34657, "x":4.93326, "y":5.94304, "heading":-1.74656, "vx":-0.99449, "vy":-0.10243, "omega":0.63716, "ax":0.00013, "ay":-0.00131, "alpha":-0.01605, "fx":[-0.04268,0.03123,0.04801,-0.02756], "fy":[-0.05313,-0.0689,0.00949,0.02525]}, + {"t":2.38774, "x":4.89232, "y":5.93883, "heading":-1.72033, "vx":-0.99449, "vy":-0.10248, "omega":0.6365, "ax":-0.00015, "ay":0.00147, "alpha":-0.00806, "fx":[-0.05822,0.03746,0.05318,-0.04251], "fy":[-0.06464,0.11955,0.01383,0.02917]}, + {"t":2.4289, "x":4.85138, "y":5.93461, "heading":-1.69413, "vx":-0.9945, "vy":-0.10242, "omega":0.63617, "ax":0.00018, "ay":-0.00177, "alpha":-0.02375, "fx":[-0.06117,0.05094,0.06853,-0.04616], "fy":[-0.07975,-0.09497,0.02083,0.03605]}, + {"t":2.47007, "x":4.81044, "y":5.93039, "heading":-1.66794, "vx":-0.99449, "vy":-0.1025, "omega":0.63519, "ax":-0.00016, "ay":0.00157, "alpha":-0.01457, "fx":[-0.08115,0.06143,0.07575,-0.06683], "fy":[-0.10438,0.14663,0.02413,0.03844]}, + {"t":2.51124, "x":4.7695, "y":5.92617, "heading":-1.64179, "vx":-0.99449, "vy":-0.10243, "omega":0.63459, "ax":0.00017, "ay":-0.00167, "alpha":-0.03573, "fx":[-0.08983,0.08048,0.09814,-0.07729], "fy":[-0.1087,-0.12115,0.05291,0.06536]}, + {"t":2.55241, "x":4.72855, "y":5.92195, "heading":-1.61566, "vx":-0.99449, "vy":-0.1025, "omega":0.63312, "ax":-0.00005, "ay":0.00044, "alpha":-0.03119, "fx":[-0.11411,0.1031,0.11259,-0.10462], "fy":[-0.16241,0.08536,0.04843,0.05809]}, + {"t":2.59358, "x":4.68761, "y":5.91773, "heading":-1.5896, "vx":-0.99449, "vy":-0.10248, "omega":0.63183, "ax":-0.00002, "ay":0.00015, "alpha":-0.05556, "fx":[-0.13739,0.12526,0.1438,-0.13267], "fy":[-0.13098,-0.13556,0.13585,0.14044]}, + {"t":2.63474, "x":4.64667, "y":5.91352, "heading":-1.56359, "vx":-0.99449, "vy":-0.10248, "omega":0.62955, "ax":0.00035, "ay":-0.00335, "alpha":-0.07235, "fx":[-0.16557,0.18031,0.17707,-0.16881], "fy":[-0.23126,-0.2392,0.12528,0.12204]}, + {"t":2.67591, "x":4.60573, "y":5.90929, "heading":-1.53767, "vx":-0.99448, "vy":-0.10261, "omega":0.62657, "ax":-0.00054, "ay":0.00526, "alpha":-0.09108, "fx":[-0.2203,0.19785,0.22262,-0.23634], "fy":[-0.14455,-0.12876,0.31998,0.30419]}, + {"t":2.71708, "x":4.56479, "y":5.90507, "heading":-1.51188, "vx":-0.9945, "vy":-0.1024, "omega":0.62282, "ax":0.0007, "ay":-0.00676, "alpha":-0.16516, "fx":[-0.26473,0.32368,0.28799,-0.30046], "fy":[-0.20853,-1.06202,0.4281,0.3917]}, + {"t":2.75825, "x":4.52385, "y":5.90085, "heading":-1.48623, "vx":-0.99447, "vy":-0.10268, "omega":0.61602, "ax":-0.00068, "ay":0.00658, "alpha":-0.16079, "fx":[-0.35987,0.36011,0.38418,-0.42969], "fy":[-0.32323,-0.25442,0.54276,0.47392]}, + {"t":2.79941, "x":4.48291, "y":5.89663, "heading":-1.46087, "vx":-0.9945, "vy":-0.1024, "omega":0.6094, "ax":-0.00549, "ay":0.0539, "alpha":-0.30295, "fx":[-0.57102,0.50743,0.38855,-0.691], "fy":[0.72514,-0.88702,1.93906,1.81701]}, + {"t":2.84058, "x":4.44196, "y":5.89246, "heading":-1.43579, "vx":-0.99472, "vy":-0.10019, "omega":0.59693, "ax":0.02106, "ay":-0.20072, "alpha":-0.30254, "fx":[-0.27123,1.1124,1.03794,-0.47491], "fy":[-4.18785,-3.9831,-2.50384,-2.70883]}, + {"t":2.88175, "x":4.40103, "y":5.88817, "heading":-1.41121, "vx":-0.99386, "vy":-0.10845, "omega":0.58447, "ax":0.22937, "ay":-1.60355, "alpha":-0.38776, "fx":[2.97206,4.98464,4.67682,2.66028], "fy":[-28.03534,-27.08365,-25.73519,-26.06778]}, + {"t":2.92292, "x":4.36031, "y":5.88234, "heading":-1.38715, "vx":-0.98441, "vy":-0.17446, "omega":0.56851, "ax":0.68252, "ay":-2.84204, "alpha":-0.53829, "fx":[10.25605,12.94943,12.49402,9.80949], "fy":[-48.86527,-48.35011,-45.88475,-46.40139]}, + {"t":2.96409, "x":4.32036, "y":5.87275, "heading":-1.36375, "vx":-0.95632, "vy":-0.29146, "omega":0.54635, "ax":1.04162, "ay":-2.78988, "alpha":-0.72392, "fx":[15.92458,19.41118,18.81377,15.30385], "fy":[-48.46701,-47.98918,-44.38001,-45.18736]}, + {"t":3.00525, "x":4.28187, "y":5.85839, "heading":-1.34125, "vx":-0.91343, "vy":-0.40632, "omega":0.51655, "ax":1.38165, "ay":-2.65139, "alpha":-0.93032, "fx":[21.18556,25.69008,24.95036,20.29975], "fy":[-46.929,-45.72192,-41.46231,-42.67652]}, + {"t":3.04642, "x":4.24544, "y":5.83942, "heading":-1.31999, "vx":-0.85656, "vy":-0.51547, "omega":0.47825, "ax":1.70014, "ay":-2.46459, "alpha":-1.25612, "fx":[25.96142,31.90065,30.76924,24.73091], "fy":[-44.76446,-43.32874,-37.22343,-39.01765]}, + {"t":3.08759, "x":4.21162, "y":5.81611, "heading":-1.3003, "vx":-0.78656, "vy":-0.61693, "omega":0.42654, "ax":1.99806, "ay":-2.23264, "alpha":-1.64062, "fx":[30.26126,38.03872,36.43883,28.48799], "fy":[-42.28515,-39.6846,-32.16372,-34.73486]}, + {"t":3.12876, "x":4.18093, "y":5.78882, "heading":-1.28274, "vx":-0.70431, "vy":-0.70884, "omega":0.359, "ax":0.70613, "ay":-0.69573, "alpha":-1.15894, "fx":[9.89512,15.19247,13.65855,8.33711], "fy":[-14.92184,-13.88995,-7.98864,-9.58925]}, + {"t":3.16111, "x":4.15852, "y":5.76552, "heading":-1.27113, "vx":-0.68147, "vy":-0.73135, "omega":0.32151, "ax":0.7384, "ay":-0.66687, "alpha":-0.91943, "fx":[10.80304,15.01351,13.74055,9.67815], "fy":[-14.17165,-12.46028,-8.25738,-9.57664]}, + {"t":3.19345, "x":4.13686, "y":5.74152, "heading":-1.26073, "vx":-0.65758, "vy":-0.75292, "omega":0.29177, "ax":0.75908, "ay":-0.64199, "alpha":-0.7272, "fx":[11.52639,14.81732,13.78612,10.48442], "fy":[-12.8031,-12.12928,-8.3971,-9.47729]}, + {"t":3.2258, "x":4.11599, "y":5.71683, "heading":-1.25129, "vx":-0.63303, "vy":-0.77369, "omega":0.26824, "ax":0.7786, "ay":-0.61663, "alpha":-0.57837, "fx":[12.03549,14.66696,13.81169,11.40159], "fy":[-12.31163,-11.0608,-8.43127,-9.3121]}, + {"t":3.25815, "x":4.09592, "y":5.69148, "heading":-1.24261, "vx":-0.60784, "vy":-0.79364, "omega":0.24953, "ax":0.79713, "ay":-0.59059, "alpha":-0.45159, "fx":[12.60541,14.65952,13.97145,11.91502], "fy":[-11.22266,-10.59505,-8.4223,-9.13921]}, + {"t":3.2905, "x":4.07667, "y":5.66549, "heading":-1.23454, "vx":-0.58205, "vy":-0.81274, "omega":0.23493, "ax":0.8163, "ay":-0.56152, "alpha":-0.32593, "fx":[13.00439,14.64728,14.07958,12.69834], "fy":[-10.3043,-9.95136,-8.30215,-8.88295]}, + {"t":3.32285, "x":4.05827, "y":5.63891, "heading":-1.22694, "vx":-0.55565, "vy":-0.8309, "omega":0.22438, "ax":0.93469, "ay":-0.32539, "alpha":-0.28514, "fx":[15.17465,16.43648,15.98694,14.72493], "fy":[-6.25601,-5.98861,-4.49371,-4.95835]}, + {"t":3.35519, "x":4.04078, "y":5.61186, "heading":-1.21968, "vx":-0.52541, "vy":-0.84143, "omega":0.21516, "ax":0.91332, "ay":0.39008, "alpha":-0.18696, "fx":[14.83866,15.84754,15.47563,14.73681], "fy":[5.99964,6.11504,7.13198,6.76318]}, + {"t":3.38754, "x":4.02427, "y":5.58485, "heading":-1.21272, "vx":-0.49587, "vy":-0.82881, "omega":0.20911, "ax":0.7635, "ay":0.63911, "alpha":-0.16259, "fx":[12.49171,13.25032,12.9629,12.20399], "fy":[10.05263,10.58386,11.13456,10.84357]}, + {"t":3.41989, "x":4.00863, "y":5.55837, "heading":-1.20596, "vx":-0.47117, "vy":-0.80814, "omega":0.20385, "ax":0.67561, "ay":0.73312, "alpha":-0.09433, "fx":[11.00237,11.619,11.38099,11.04588], "fy":[12.05166,11.94309,12.56096,12.32708]}, + {"t":3.45224, "x":3.99374, "y":5.53261, "heading":-1.19936, "vx":-0.44932, "vy":-0.78442, "omega":0.2008, "ax":0.62294, "ay":0.77927, "alpha":-0.09232, "fx":[10.24819,10.69725,10.52093,10.07002], "fy":[12.5741,13.09746,13.23582,13.0529]}, + {"t":3.48459, "x":3.97953, "y":5.50765, "heading":-1.19287, "vx":-0.42917, "vy":-0.75921, "omega":0.19781, "ax":0.58858, "ay":0.80609, "alpha":-0.04649, "fx":[9.64957,10.02396,9.87425,9.69741], "fy":[13.43487,13.23813,13.61127,13.46435]}, + {"t":3.51693, "x":3.96595, "y":5.48351, "heading":-1.18647, "vx":-0.41013, "vy":-0.73314, "omega":0.19631, "ax":0.56457, "ay":0.82346, "alpha":-0.05145, "fx":[9.33423,9.59429,9.48895,9.22672], "fy":[13.44376,13.88542,13.8457,13.73163]}, + {"t":3.54928, "x":3.95298, "y":5.46022, "heading":-1.18012, "vx":-0.39186, "vy":-0.7065, "omega":0.19465, "ax":0.5469, "ay":0.83556, "alpha":-0.0219, "fx":[9.01998,9.24648,9.15297,9.04681], "fy":[13.99513,13.78692,14.01154,13.91988]}, + {"t":3.58163, "x":3.94059, "y":5.43781, "heading":-1.17382, "vx":-0.37417, "vy":-0.67947, "omega":0.19394, "ax":0.53338, "ay":0.84445, "alpha":-0.02857, "fx":[8.84816,8.9974,8.93517,8.78396], "fy":[13.88439,14.23166,14.13111,14.05933]}, + {"t":3.61398, "x":3.92877, "y":5.41627, "heading":-1.16755, "vx":-0.35692, "vy":-0.65216, "omega":0.19301, "ax":0.52271, "ay":0.85125, "alpha":-0.00909, "fx":[8.6509,8.7936,8.73288,8.67577], "fy":[14.27646,14.08711,14.22797,14.16842]}, + {"t":3.64632, "x":3.9175, "y":5.39562, "heading":-1.16131, "vx":-0.34001, "vy":-0.62462, "omega":0.19272, "ax":0.51408, "ay":0.85662, "alpha":-0.01753, "fx":[8.54329,8.63629,8.59636,8.5017], "fy":[14.14034,14.41389,14.30643,14.25693]}, + {"t":3.67867, "x":3.90677, "y":5.37586, "heading":-1.15507, "vx":-0.32338, "vy":-0.59691, "omega":0.19215, "ax":0.50695, "ay":0.86095, "alpha":-0.00501, "fx":[8.40357,8.50962,8.46304,8.42648], "fy":[14.43043,14.27103,14.37544,14.32975]}, + {"t":3.71102, "x":3.89657, "y":5.357, "heading":-1.14886, "vx":-0.30698, "vy":-0.56906, "omega":0.19199, "ax":0.50098, "ay":0.86453, "alpha":-0.01539, "fx":[8.32958,8.40823,8.37324,8.29316], "fy":[14.29261,14.52246,14.43703,14.39292]}, + {"t":3.74337, "x":3.8869, "y":5.33905, "heading":-1.14265, "vx":-0.29078, "vy":-0.5411, "omega":0.19149, "ax":0.49589, "ay":0.86753, "alpha":-0.00849, "fx":[8.22169,8.32996,8.28064,8.23285], "fy":[14.51212,14.38914,14.49597,14.44768]}, + {"t":3.77572, "x":3.87776, "y":5.322, "heading":-1.13645, "vx":-0.27474, "vy":-0.51303, "omega":0.19122, "ax":0.49151, "ay":0.87007, "alpha":-0.02132, "fx":[8.16645,8.26775,8.22075,8.11822], "fy":[14.37626,14.58827,14.55281,14.49755]}, + {"t":3.80806, "x":3.86913, "y":5.30586, "heading":-1.13027, "vx":-0.25884, "vy":-0.48489, "omega":0.19053, "ax":0.4877, "ay":0.87227, "alpha":-0.01967, "fx":[8.07705,8.22495,8.15494,8.06218], "fy":[14.54225,14.46468,14.61129,14.54294]}, + {"t":3.84041, "x":3.86101, "y":5.29063, "heading":-1.1241, "vx":-0.24306, "vy":-0.45667, "omega":0.18989, "ax":0.48436, "ay":0.87418, "alpha":-0.03594, "fx":[8.03221,8.19481,8.11638,7.9527], "fy":[14.40511,14.62496,14.67188,14.58642]}, + {"t":3.87276, "x":3.8534, "y":5.27631, "heading":-1.11796, "vx":-0.22739, "vy":-0.42839, "omega":0.18873, "ax":0.4814, "ay":0.87585, "alpha":-0.04015, "fx":[7.95302,8.18303,8.0702,7.89248], "fy":[14.52485,14.50916,14.73797,14.628]}, + {"t":3.90511, "x":3.8463, "y":5.26292, "heading":-1.11186, "vx":-0.21182, "vy":-0.40006, "omega":0.18743, "ax":0.47876, "ay":0.87733, "alpha":-0.06145, "fx":[7.91308,8.18415,8.04883,7.77682], "fy":[14.38072,14.63792,14.81036,14.66969]}, + {"t":3.93746, "x":3.83969, "y":5.25043, "heading":-1.10579, "vx":-0.19633, "vy":-0.37168, "omega":0.18544, "ax":0.4764, "ay":0.87865, "alpha":-0.0732, "fx":[7.83829,8.20548,8.01935,7.70205], "fy":[14.45526,14.52688,14.89286,14.71162]}, + {"t":3.9698, "x":3.83359, "y":5.23887, "heading":-1.09979, "vx":-0.18092, "vy":-0.34326, "omega":0.18307, "ax":0.47426, "ay":0.87983, "alpha":-0.10199, "fx":[7.7984,8.24206,8.0135,7.56901], "fy":[14.29431,14.6282,14.98724,14.75569]}, + {"t":4.00215, "x":3.82799, "y":5.22823, "heading":-1.09387, "vx":-0.16558, "vy":-0.3148, "omega":0.17978, "ax":0.47233, "ay":0.8809, "alpha":-0.12426, "fx":[7.72254,8.30436,8.00029,7.46689], "fy":[14.31823,14.51778,15.0982,14.80226]}, + {"t":4.0345, "x":3.82288, "y":5.2185, "heading":-1.08806, "vx":-0.1503, "vy":-0.2863, "omega":0.17576, "ax":0.47057, "ay":0.88186, "alpha":-0.16451, "fx":[7.67794,8.38689,8.01078,7.30112], "fy":[14.12608,14.59234,15.22904,14.85336]}, + {"t":4.06685, "x":3.81826, "y":5.2097, "heading":-1.08237, "vx":-0.13508, "vy":-0.25778, "omega":0.17043, "ax":0.46896, "ay":0.88274, "alpha":-0.20219, "fx":[7.59508,8.50535,8.01588,7.15313], "fy":[14.08629,14.4775,15.38592,14.90969]}, + {"t":4.0992, "x":3.81414, "y":5.20183, "heading":-1.07686, "vx":-0.11991, "vy":-0.22922, "omega":0.16389, "ax":0.46748, "ay":0.88354, "alpha":-0.26002, "fx":[7.53989,8.65237,8.04588,6.93282], "fy":[13.84126,14.52342,15.57456,14.9737]}, + {"t":4.13154, "x":3.81051, "y":5.19487, "heading":-1.07156, "vx":-0.10479, "vy":-0.20064, "omega":0.15548, "ax":0.46612, "ay":0.88428, "alpha":-0.32082, "fx":[7.44258,8.8523,8.07414,6.71124], "fy":[13.71486,14.39676,15.80363,15.04682]}, + {"t":4.16389, "x":3.80736, "y":5.18885, "heading":-1.06653, "vx":-0.08971, "vy":-0.17204, "omega":0.1451, "ax":0.46487, "ay":0.88496, "alpha":-0.4057, "fx":[7.36858,9.094,8.12983,6.40402], "fy":[13.38388,14.40882,16.0824,15.13219]}, + {"t":4.19624, "x":3.8047, "y":5.18374, "heading":-1.06183, "vx":-0.07467, "vy":-0.14341, "omega":0.13198, "ax":0.4637, "ay":0.88558, "alpha":-0.50154, "fx":[7.24622,9.41516,8.18986,6.06751], "fy":[13.13322,14.25964,16.4236,15.23261]}, + {"t":4.22859, "x":3.80253, "y":5.17957, "heading":-1.05756, "vx":-0.05967, "vy":-0.11476, "omega":0.11576, "ax":0.46262, "ay":0.88617, "alpha":-0.62812, "fx":[7.14111,9.79951,8.28223,5.62371], "fy":[12.66641,14.22725,16.84173,15.3524]}, + {"t":4.26094, "x":3.80084, "y":5.17632, "heading":-1.05382, "vx":-0.04471, "vy":-0.0861, "omega":0.09544, "ax":0.46161, "ay":0.8867, "alpha":-0.77733, "fx":[6.97766,10.30338,8.38878,5.10946], "fy":[12.23222,14.03949,17.35559,15.49645]}, + {"t":4.29328, "x":3.79964, "y":5.174, "heading":-1.05073, "vx":-0.02977, "vy":-0.05741, "omega":0.07029, "ax":0.46067, "ay":0.88721, "alpha":-0.96823, "fx":[6.82129,10.90512,8.53672,4.45331], "fy":[11.55328,13.94438,17.9878,15.67181]}, + {"t":4.32563, "x":3.79891, "y":5.17261, "heading":-1.04846, "vx":-0.01487, "vy":-0.02871, "omega":0.03897, "ax":0.45979, "ay":0.88768, "alpha":-1.20483, "fx":[6.5904,11.72881,8.71533,3.62309], "fy":[10.82873,13.71299,18.75232,15.89452]}, + {"t":4.35798, "x":3.79867, "y":5.17214, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/java/frc/robot/commands/auton/NonProcessorDeepAutonCommand.java b/src/main/java/frc/robot/commands/auton/NonProcessorDeepAutonCommand.java index 3204cf8c..03bae51c 100644 --- a/src/main/java/frc/robot/commands/auton/NonProcessorDeepAutonCommand.java +++ b/src/main/java/frc/robot/commands/auton/NonProcessorDeepAutonCommand.java @@ -29,7 +29,7 @@ public class NonProcessorDeepAutonCommand extends SequentialCommandGroup private RobotStateSubsystem robotStateSubsystem; private VisionSubsystem visionSubsystem; private List NodeNames; - private List NodeLevels; + private List NodeLevels; private char startNode; private boolean lastAlgae; @@ -45,7 +45,7 @@ public NonProcessorDeepAutonCommand( VisionSubsystem visionSubsystem, String startPathName, List NodeNames, - List NodeLevels, + List NodeLevels, char startNode, boolean startScoreLeft, boolean lastAlgae, diff --git a/src/main/java/frc/robot/commands/auton/NonProcessorShallowAutonCommand.java b/src/main/java/frc/robot/commands/auton/NonProcessorShallowAutonCommand.java index 5051fe49..9229a39e 100644 --- a/src/main/java/frc/robot/commands/auton/NonProcessorShallowAutonCommand.java +++ b/src/main/java/frc/robot/commands/auton/NonProcessorShallowAutonCommand.java @@ -29,7 +29,7 @@ public class NonProcessorShallowAutonCommand extends SequentialCommandGroup private RobotStateSubsystem robotStateSubsystem; private VisionSubsystem visionSubsystem; private List NodeNames; - private List NodeLevels; + private List NodeLevels; private char startNode; private boolean lastAlgae; @@ -45,7 +45,7 @@ public NonProcessorShallowAutonCommand( VisionSubsystem visionSubsystem, String startPathName, List NodeNames, - List NodeLevels, + List NodeLevels, char startNode, boolean startScoreLeft, boolean lastAlgae, diff --git a/src/main/java/frc/robot/commands/auton/NonProcessorShallowSlowAutonCommand.java b/src/main/java/frc/robot/commands/auton/NonProcessorShallowSlowAutonCommand.java index 6f5c4a88..3d1ad834 100644 --- a/src/main/java/frc/robot/commands/auton/NonProcessorShallowSlowAutonCommand.java +++ b/src/main/java/frc/robot/commands/auton/NonProcessorShallowSlowAutonCommand.java @@ -30,7 +30,7 @@ public class NonProcessorShallowSlowAutonCommand extends SequentialCommandGroup private RobotStateSubsystem robotStateSubsystem; private VisionSubsystem visionSubsystem; private List NodeNames; - private List NodeLevels; + private List NodeLevels; private char startNode; public NonProcessorShallowSlowAutonCommand( @@ -45,7 +45,7 @@ public NonProcessorShallowSlowAutonCommand( VisionSubsystem visionSubsystem, String startPathName, List NodeNames, - List NodeLevels, + List NodeLevels, char startNode, Boolean startScoreLeft, Pose2d startPose) { diff --git a/src/main/java/frc/robot/commands/auton/ProcessorShallowAutonCommand.java b/src/main/java/frc/robot/commands/auton/ProcessorShallowAutonCommand.java index 1115f505..612d177c 100644 --- a/src/main/java/frc/robot/commands/auton/ProcessorShallowAutonCommand.java +++ b/src/main/java/frc/robot/commands/auton/ProcessorShallowAutonCommand.java @@ -29,7 +29,7 @@ public class ProcessorShallowAutonCommand extends SequentialCommandGroup private RobotStateSubsystem robotStateSubsystem; private VisionSubsystem visionSubsystem; private List NodeNames; - private List NodeLevels; + private List NodeLevels; private char startNode; private boolean lastAlgae; @@ -45,7 +45,7 @@ public ProcessorShallowAutonCommand( VisionSubsystem visionSubsystem, String startPathName, List NodeNames, - List NodeLevels, + List NodeLevels, char startNode, boolean startScoreLeft, boolean lastAlgae, diff --git a/src/main/java/frc/robot/commands/auton/ProcessorShallowSlowAutonCommand.java b/src/main/java/frc/robot/commands/auton/ProcessorShallowSlowAutonCommand.java index 86d86e77..0723f02f 100644 --- a/src/main/java/frc/robot/commands/auton/ProcessorShallowSlowAutonCommand.java +++ b/src/main/java/frc/robot/commands/auton/ProcessorShallowSlowAutonCommand.java @@ -29,7 +29,7 @@ public class ProcessorShallowSlowAutonCommand extends SequentialCommandGroup private RobotStateSubsystem robotStateSubsystem; private VisionSubsystem visionSubsystem; private List NodeNames; - private List NodeLevels; + private List NodeLevels; private char startNode; public ProcessorShallowSlowAutonCommand( @@ -44,7 +44,7 @@ public ProcessorShallowSlowAutonCommand( VisionSubsystem visionSubsystem, String startPathName, List NodeNames, - List NodeLevels, + List NodeLevels, char startNode, boolean startScoreLeft, Pose2d startPose) { diff --git a/src/main/java/frc/robot/commands/pathHandler/SetPathHandlerCommand.java b/src/main/java/frc/robot/commands/pathHandler/SetPathHandlerCommand.java index 4333b1a9..05c04d49 100644 --- a/src/main/java/frc/robot/commands/pathHandler/SetPathHandlerCommand.java +++ b/src/main/java/frc/robot/commands/pathHandler/SetPathHandlerCommand.java @@ -2,13 +2,14 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.subsystems.pathHandler.PathHandler; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; import java.util.List; public class SetPathHandlerCommand extends InstantCommand { private PathHandler pathHandler; private String[][] pathNames; private List NodeNames; - private List NodeLevels; + private List NodeLevels; private Character startNode; private boolean mirrorToProcessor; @@ -16,7 +17,7 @@ public SetPathHandlerCommand( PathHandler pathHandler, String[][] pathNames, List NodeNames, - List NodeLevels, + List NodeLevels, Character startNode, boolean mirrorToProcessor) { this.pathHandler = pathHandler; diff --git a/src/main/java/frc/robot/commands/pathHandler/StartPathHandlerCommand.java b/src/main/java/frc/robot/commands/pathHandler/StartPathHandlerCommand.java index f4c9dfd8..eecbe962 100644 --- a/src/main/java/frc/robot/commands/pathHandler/StartPathHandlerCommand.java +++ b/src/main/java/frc/robot/commands/pathHandler/StartPathHandlerCommand.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.pathHandler.PathHandler; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; import java.util.List; public class StartPathHandlerCommand extends Command { @@ -11,7 +12,7 @@ public StartPathHandlerCommand( PathHandler pathHandler, String[][] pathNames, List NodeNames, - List NodeLevels, + List NodeLevels, Character startNode, boolean mirrorToProcessor) { pathHandler.setPathNames(pathNames); diff --git a/src/main/java/frc/robot/constants/CoralConstants.java b/src/main/java/frc/robot/constants/CoralConstants.java index 923bc4c5..82f01d4c 100644 --- a/src/main/java/frc/robot/constants/CoralConstants.java +++ b/src/main/java/frc/robot/constants/CoralConstants.java @@ -36,9 +36,9 @@ public static TalonFXSConfiguration getFXConfig() { new CurrentLimitsConfigs() .withStatorCurrentLimit(10) .withStatorCurrentLimitEnable(false) - .withSupplyCurrentLimit(10) + .withSupplyCurrentLimit(30) .withSupplyCurrentLowerLimit(8) - .withSupplyCurrentLowerTime(0.02) + .withSupplyCurrentLowerTime(0.5) .withSupplyCurrentLimitEnable(true); fxsConfig.CurrentLimits = current; diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/src/main/java/frc/robot/constants/TagServoingConstants.java index d21b502c..b517db8d 100644 --- a/src/main/java/frc/robot/constants/TagServoingConstants.java +++ b/src/main/java/frc/robot/constants/TagServoingConstants.java @@ -60,7 +60,7 @@ public class TagServoingConstants { // End conditions public static final double kInitialCloseEnough = 0.1; public static final double kCoralDriveXCloseEnough = 0.03; - public static final double kCoralDriveYCloseEnough = 0.015; // was 0.025 + public static final double kCoralDriveYCloseEnough = 0.02; // was 0.025 public static final double kAlgaeDriveXCloseEnough = 0.03; public static final double kAlgaeDriveYCloseEnough = kCoralDriveYCloseEnough; diff --git a/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java index ebee5a4b..6ba74aba 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java @@ -25,6 +25,7 @@ import frc.robot.subsystems.led.LEDSubsystem; import frc.robot.subsystems.pathHandler.PathHandler; import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; import frc.robot.subsystems.tagAlign.TagAlignSubsystem; import frc.robot.subsystems.vision.VisionSubsystem; import java.util.ArrayList; @@ -161,7 +162,7 @@ private AutoCommandInterface getAutoCommand(int switchPos) { visionSubsystem, "startToJSlow", new ArrayList(Arrays.asList('k', 'l')), - new ArrayList(Arrays.asList(4, 4)), + new ArrayList(Arrays.asList(ScoringLevel.L4, ScoringLevel.L4)), 'j', false, AutonConstants.kNonProcessorShallow); @@ -179,7 +180,8 @@ private AutoCommandInterface getAutoCommand(int switchPos) { visionSubsystem, "startToJ", new ArrayList(Arrays.asList('k', 'l', 'i')), - new ArrayList(Arrays.asList(4, 4, 4)), + new ArrayList( + Arrays.asList(ScoringLevel.L4, ScoringLevel.L4, ScoringLevel.L4)), 'j', false, false, @@ -198,7 +200,8 @@ private AutoCommandInterface getAutoCommand(int switchPos) { visionSubsystem, "startToJ", new ArrayList(Arrays.asList('k', 'l', 'a')), - new ArrayList(Arrays.asList(4, 4, 4)), + new ArrayList( + Arrays.asList(ScoringLevel.L4, ScoringLevel.L4, ScoringLevel.L4)), 'j', false, false, @@ -217,7 +220,8 @@ private AutoCommandInterface getAutoCommand(int switchPos) { visionSubsystem, "startDeepToK", new ArrayList(Arrays.asList('l', 'a', 'b')), - new ArrayList(Arrays.asList(4, 4, 4)), + new ArrayList( + Arrays.asList(ScoringLevel.L4, ScoringLevel.L4, ScoringLevel.L4)), 'k', true, false, @@ -237,7 +241,8 @@ private AutoCommandInterface getAutoCommand(int switchPos) { visionSubsystem, "startToJ", new ArrayList(Arrays.asList('k', 'l', 'k')), - new ArrayList(Arrays.asList(4, 4, 3)), + new ArrayList( + Arrays.asList(ScoringLevel.L4, ScoringLevel.L4, ScoringLevel.L3)), 'j', false, true, @@ -257,7 +262,7 @@ private AutoCommandInterface getAutoCommand(int switchPos) { visionSubsystem, "startPToESlow", new ArrayList(Arrays.asList('d', 'c')), - new ArrayList(Arrays.asList(4, 4)), + new ArrayList(Arrays.asList(ScoringLevel.L4, ScoringLevel.L4)), 'e', true, AutonConstants.kNonProcessorShallow); diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java index a786ffb7..2d680017 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java @@ -46,8 +46,7 @@ public class LEDSubsystem extends MeasurableSubsystem { private LEDPattern currentLimiting = LEDPattern.solid(LEDConstants.kCurrentLimiting).blink(Seconds.of(0.5), Seconds.of(1)); private LEDPattern loadCoral = - LEDPattern.steps( - Map.of(0, Color.kBlack, LEDConstants.kTopFirstIndex, LEDConstants.kLoadCoral)); + LEDPattern.steps(Map.of(LEDConstants.kTopFirstIndex, LEDConstants.kLoadCoral)); // section booleans and states private boolean hasAlgae = false; @@ -154,6 +153,9 @@ public boolean getCurrentLimiting() { } private void buildBase() { + if (shouldLoadCoral) { + base = loadCoral.overlayOn(base); + } algae = LEDPattern.steps( Map.of( @@ -166,51 +168,53 @@ private void buildBase() { case CORAL, SCORING -> coral = LEDPattern.solid(LEDConstants.kCoralInRobot); case NONE -> coral = LEDPattern.solid(LEDConstants.kCoralNotInRobot); } - switch (levelState) { - case L1 -> level = - LEDPattern.steps( - Map.of( - LEDConstants.kLevelStart / (double) LEDConstants.kTotalStripLength, - LEDConstants.kL1)); - case L2 -> level = - LEDPattern.steps( - Map.of( - LEDConstants.kLevelStart / (double) LEDConstants.kTotalStripLength, - LEDConstants.kL2)); - case L3 -> level = - LEDPattern.steps( - Map.of( - LEDConstants.kLevelStart / (double) LEDConstants.kTotalStripLength, - LEDConstants.kL3)); - case L4 -> level = - LEDPattern.steps( - Map.of( - LEDConstants.kLevelStart / (double) LEDConstants.kTotalStripLength, - LEDConstants.kL4)); - } - switch (placeState) { - case MANUAL -> place = - LEDPattern.steps( - Map.of( - LEDConstants.kPlaceStart / (double) LEDConstants.kTotalStripLength, - LEDConstants.kManual)); - case LEFT -> place = - LEDPattern.steps( - Map.of( - LEDConstants.kPlaceStart / (double) LEDConstants.kTotalStripLength, - LEDConstants.kLeft)); - case RIGHT -> place = + if (!shouldLoadCoral) { + switch (levelState) { + case L1 -> level = + LEDPattern.steps( + Map.of( + LEDConstants.kLevelStart / (double) LEDConstants.kTotalStripLength, + LEDConstants.kL1)); + case L2 -> level = + LEDPattern.steps( + Map.of( + LEDConstants.kLevelStart / (double) LEDConstants.kTotalStripLength, + LEDConstants.kL2)); + case L3 -> level = + LEDPattern.steps( + Map.of( + LEDConstants.kLevelStart / (double) LEDConstants.kTotalStripLength, + LEDConstants.kL3)); + case L4 -> level = + LEDPattern.steps( + Map.of( + LEDConstants.kLevelStart / (double) LEDConstants.kTotalStripLength, + LEDConstants.kL4)); + } + switch (placeState) { + case MANUAL -> place = + LEDPattern.steps( + Map.of( + LEDConstants.kPlaceStart / (double) LEDConstants.kTotalStripLength, + LEDConstants.kManual)); + case LEFT -> place = + LEDPattern.steps( + Map.of( + LEDConstants.kPlaceStart / (double) LEDConstants.kTotalStripLength, + LEDConstants.kLeft)); + case RIGHT -> place = + LEDPattern.steps( + Map.of( + LEDConstants.kPlaceStart / (double) LEDConstants.kTotalStripLength, + LEDConstants.kRight)); + } + getAlgea = LEDPattern.steps( Map.of( - LEDConstants.kPlaceStart / (double) LEDConstants.kTotalStripLength, - LEDConstants.kRight)); + LEDConstants.kGetAlgeaStart / (double) LEDConstants.kTotalStripLength, + shouldGetAlgae ? LEDConstants.kGetAlgea : LEDConstants.kNotGetAlgea)); + base = algae.overlayOn(coral); } - getAlgea = - LEDPattern.steps( - Map.of( - LEDConstants.kGetAlgeaStart / (double) LEDConstants.kTotalStripLength, - shouldGetAlgae ? LEDConstants.kGetAlgea : LEDConstants.kNotGetAlgea)); - base = algae.overlayOn(coral); if (isLimiting) { base = currentLimiting.overlayOn(base); @@ -221,10 +225,6 @@ private void buildBase() { } base = getAlgea.overlayOn(place.overlayOn(level.overlayOn(base))); - - if (shouldLoadCoral) { - base = loadCoral.overlayOn(base); - } } // game stuff diff --git a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java index b3505401..2efae0fd 100644 --- a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java +++ b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java @@ -13,6 +13,7 @@ import frc.robot.constants.TagServoingConstants; import frc.robot.subsystems.drive.DriveSubsystem; import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; import frc.robot.subsystems.tagAlign.TagAlignSubsystem; import frc.robot.subsystems.tagAlign.TagAlignSubsystem.TagAlignStates; import java.util.ArrayList; @@ -34,7 +35,8 @@ public class PathHandler extends MeasurableSubsystem { private String[][] pathNames = PathHandlerConstants.kShallowPathNames; // the array of paths, in string form private List nodeNames = new ArrayList<>(); // the list of nodes, in order, to score on - private List nodeLevels = new ArrayList<>(); // the list of levels, in order, to score on + private List nodeLevels = + new ArrayList<>(); // the list of levels, in order, to score on private Character startNode = 'a'; // the node that the robot starts in front of private boolean mirrorToProcessor = false; // whether the robot starts and fetches on the processor side. @@ -58,6 +60,7 @@ public class PathHandler extends MeasurableSubsystem { private boolean isPlacing = false; private boolean hasPreppedCoral = false; private boolean algaeOnLast = false; + private boolean hasLEDsOn = false; public PathHandler( DriveSubsystem driveSubsystem, @@ -75,7 +78,7 @@ public PathHandler( RobotStateSubsystem robotStateSubsystem, String[][] pathNames, List nodeNames, - List nodeLevels, + List nodeLevels, Character startNode, boolean mirrorToProcessor) { this.driveSubsystem = driveSubsystem; @@ -101,12 +104,12 @@ public void setNodeNames(List nodeNames) { } } - public void addNode(Character nodeName, int nodeLevel) { + public void addNode(Character nodeName, ScoringLevel nodeLevel) { nodeNames.add(nodeName); nodeLevels.add(nodeLevel); } - public void setNodeLevels(List nodeLevels) { + public void setNodeLevels(List nodeLevels) { if (!isHandling) { this.nodeLevels = nodeLevels; } @@ -135,6 +138,7 @@ public void setGetAlgaeLast(boolean algae) { public void startPathHandler() { teleop = false; nodeNames.add(0, startNode); + nodeLevels.add(0, ScoringLevel.L4); // a dummy level isHandling = true; robotStateSubsystem.setIsAutoPlacing(false); curState = PathStates.DRIVE_FETCH; @@ -286,6 +290,7 @@ private void advanceNodes() { (nodeNames.get(0) - 'a') % 2 == (mirrorToProcessor ? 1 : 0), false); nodeNames.remove(0); + nodeLevels.remove(0); } } @@ -407,12 +412,23 @@ public void periodic() { if (!runningPath) { startPath(nextPath()); } - if (tagAlignSubsystem.getCurRadius(robotStateSubsystem.getAllianceColor()) - < .5) { // TODO magic num + if (FastMath.sqrt( + FastMath.pow( + driveSubsystem.getPoseMeters().getX() - currPathFinalPose.getX(), 2) + + FastMath.pow( + driveSubsystem.getPoseMeters().getY() - currPathFinalPose.getY(), 2)) + < 0.5 + && !hasLEDsOn) { + robotStateSubsystem.setLEDLoadCoral(true); + hasLEDsOn = true; } drivePath(); } case FETCH -> { + if (hasLEDsOn) { + robotStateSubsystem.setLEDLoadCoral(false); + hasLEDsOn = false; + } if (robotStateSubsystem.hasCoral() || (robotStateSubsystem.hasCoralAuton())) { // waitingTimer.hasElapsed(PathHandlerConstants.kWaitingTime) // proceedToNext) { @@ -443,6 +459,7 @@ public void periodic() { robotStateSubsystem.getAllianceColor(), (next - 'a') % 2 == 0, targetHexant); } startPath(nextPath()); + robotStateSubsystem.setScoringLevel(nodeLevels.get(0)); } drivePath(); } @@ -454,6 +471,7 @@ public void periodic() { && !hasPreppedCoral) { hasPreppedCoral = true; if (algaeOnLast && nodeNames.size() == 1) { + robotStateSubsystem.setScoringLevel(nodeLevels.get(0)); robotStateSubsystem.toReefAlignAlgaeAuto(); } else { robotStateSubsystem.toPrepCoral(); @@ -464,6 +482,10 @@ public void periodic() { } } case PLACE -> { + if (algaeOnLast && nodeNames.size() == 1) { + killPathHandler(); + return; + } hasPreppedCoral = false; if (robotStateSubsystem.isElevatorFinished() && !isPlacing) { isPlacing = true; diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index 43f19af9..6cdef953 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -374,7 +374,7 @@ private void toPrestage() { public void toPrepCoral() { if (curState == RobotStates.REEF_ALIGN_CORAL) { - if (isAuto) isAutoReadyForEject = true; + if (isAuto) toReefAlign(false, false); if (elevatorSubsystem.isFinished()) { toPlaceCoral(); } @@ -386,7 +386,7 @@ public void toPrepCoral() { public void toPlaceCoralAuto() { isAutoReadyForEject = true; if (elevatorSubsystem.isFinished()) toPlaceCoral(); - else toReefAlign(getAlgaeOnCycle, false); + else toReefAlign(false, false); } public void toReefAlign() { @@ -394,7 +394,7 @@ public void toReefAlign() { } public void toReefAlignAlgaeAuto() { - toReefAlign(true, true); + toReefAlign(true, false); } private void toReefAlign(boolean getAlgae, boolean drive) { diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index 371a8f60..66d598c8 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -33,6 +33,7 @@ public class TagAlignSubsystem extends MeasurableSubsystem { // private ProfiledPIDController servoY; private TagAlignStates curState = TagAlignStates.DONE; + private boolean isAuto = false; // Set by start() private Pose2d targetPose; @@ -299,12 +300,14 @@ public double calculateAlignY() { } public void start(Alliance alliance, boolean scoreLeft, boolean algae) { + isAuto = false; setup(alliance, scoreLeft, algae); curState = TagAlignStates.DRIVE; } public void startAuto(Alliance alliance, boolean scoreLeft, boolean algae) { + isAuto = true; setup(alliance, scoreLeft, algae); tagAlign(); } @@ -422,7 +425,7 @@ public void periodic() { } } case TAG_ALIGN -> { - if (FastMath.abs(alignX.getError()) < driveXCloseEnough + if ((isAuto ? true : FastMath.abs(alignX.getError()) < driveXCloseEnough) && FastMath.abs(alignY.getError()) < driveYCloseEnough) { finalDrive = true; } From 1ba4a84fefe4475a04cee49f8f6ac11bcf41eb9b Mon Sep 17 00:00:00 2001 From: KaydenLee456 Date: Fri, 28 Mar 2025 19:14:22 -0400 Subject: [PATCH 05/12] Added some stuff --- .../robot/subsystems/vision/BargeAlign.java | 27 +++++++++++++++---- 1 file changed, 22 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/BargeAlign.java b/src/main/java/frc/robot/subsystems/vision/BargeAlign.java index b18e059d..3b40f378 100644 --- a/src/main/java/frc/robot/subsystems/vision/BargeAlign.java +++ b/src/main/java/frc/robot/subsystems/vision/BargeAlign.java @@ -16,18 +16,20 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; public class BargeAlign extends MeasurableSubsystem{ -private DoubleSupplier strStick; +private DoubleSupplier strStick; //note to Huck: what's strStick? + private DriveSubsystem driveSubsystem; //These are here to determine offsets for target positions. private boolean blueAlliance; private boolean redBarge; +private final XboxController xboxController = new XboxController(1); private Alliance alliance; private double driveRadius = 1.5; //TODO This is a magic number that needs creating private BargeAlignStates curState; - public BargeAlign( +public BargeAlign( DoubleSupplier strStick, DriveSubsystem driveSubsystem){ @@ -38,7 +40,7 @@ public BargeAlign( blueAlliance = false; } curState = BargeAlignStates.DRIVE; - } +} public void setState(BargeAlignStates curState){ this.curState = curState; @@ -57,15 +59,30 @@ private boolean getOnRedSide() { } private double getYStickReading() { - return xboxController.XkY.value); + return XboxController.Button.kLeftStick.kY.value; //just a placeholder, to remind me } //same joystick reading as the drive +/* + private void configureDriverBindings() { + driveSubsystem.setDefaultCommand( + new DriveTeleopCommand( + () -> flysky.getStr() + driveSubsystem, + robotStateSubsystem)); +*/ + private Translation2d getTargetTranslation(){ - Translation2d offset = new Translation2d( + Translation2d blueOffset = new Translation2d( getOnBlueSide() ? driveRadius : driveRadius * -1 , new Rotation2d( getOnBlueSide() ? 0 : Math.PI)); + /* + Translation2d redOffset = new Translation2d( + getOnRedSide() ? driveRadius : driveRadius * -1 , new Rotation2d( + getOnRedSide() ? 0 : Math.PI)); + */ + Translation2d targetBarge = blueAlliance ? BargeAlignConstants.blueBargePos : BargeAlignConstants.redBargePos; From 52261a31171c2aad4e1398fccf11a4dfbd52eb74 Mon Sep 17 00:00:00 2001 From: David Shen Date: Fri, 28 Mar 2025 20:58:36 -0400 Subject: [PATCH 06/12] earlier elevator staging, more lights, path adjustments, unsolved led stack overflow --- src/main/deploy/choreo/2025-project.chor | 24 ++- src/main/deploy/choreo/JTofetch.traj | 190 +++++++++--------- src/main/deploy/choreo/fetchToK.traj | 103 +++++----- src/main/deploy/choreo/startToG.traj | 2 +- src/main/deploy/choreo/startToH.traj | 2 +- src/main/deploy/choreo/startToI.traj | 2 +- src/main/deploy/choreo/startToJ.traj | 118 +++++------ src/main/deploy/choreo/startToJSlow.traj | 2 +- src/main/deploy/choreo/startToK.traj | 2 +- src/main/deploy/choreo/startToL.traj | 2 +- src/main/java/frc/robot/RobotContainer.java | 9 + .../frc/robot/constants/AutonConstants.java | 2 +- .../frc/robot/constants/LEDConstants.java | 4 +- .../robot/constants/PathHandlerConstants.java | 2 +- .../robot/subsystems/led/LEDSubsystem.java | 16 +- .../subsystems/pathHandler/PathHandler.java | 46 ++++- 16 files changed, 290 insertions(+), 236 deletions(-) diff --git a/src/main/deploy/choreo/2025-project.chor b/src/main/deploy/choreo/2025-project.chor index 0e1713d2..68359cc2 100644 --- a/src/main/deploy/choreo/2025-project.chor +++ b/src/main/deploy/choreo/2025-project.chor @@ -244,18 +244,32 @@ "val":0.942477796076938 } }, - "start":{ + "midStart":{ "x":{ - "exp":"7.1008875 m", - "val":7.1008875 + "exp":"7.1 m", + "val":7.1 }, "y":{ "exp":"6.1663548 m", "val":6.1663548 }, "heading":{ - "exp":"180 deg", - "val":3.141592653589793 + "exp":"3.14159 rad", + "val":3.14159 + } + }, + "start":{ + "x":{ + "exp":"7.1 m", + "val":7.1 + }, + "y":{ + "exp":"5.07 m", + "val":5.07 + }, + "heading":{ + "exp":"3.14159 rad", + "val":3.14159 } }, "startDeep":{ diff --git a/src/main/deploy/choreo/JTofetch.traj b/src/main/deploy/choreo/JTofetch.traj index d1a89168..35fe5f5a 100644 --- a/src/main/deploy/choreo/JTofetch.traj +++ b/src/main/deploy/choreo/JTofetch.traj @@ -15,7 +15,8 @@ {"from":0, "to":4, "data":{"type":"MaxVelocity", "props":{"max":3.3}}, "enabled":true}, {"from":0, "to":4, "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":true}, {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":1.0}}, "enabled":true}, - {"from":1, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":1.0}}, "enabled":true}], + {"from":1, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":1.0}}, "enabled":true}, + {"from":0, "to":0, "data":{"type":"MaxAcceleration", "props":{"max":10.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ @@ -32,7 +33,8 @@ {"from":0, "to":4, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.3 m / s", "val":3.3}}}, "enabled":true}, {"from":0, "to":4, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":true}, {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"1 rad / s", "val":1.0}}}, "enabled":true}, - {"from":1, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"1 rad / s", "val":1.0}}}, "enabled":true}], + {"from":1, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"1 rad / s", "val":1.0}}}, "enabled":true}, + {"from":0, "to":0, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"10 m / s ^ 2", "val":10.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -42,98 +44,98 @@ "sampleType":"Swerve", "waypoints":[0.0,0.55586,0.90819,1.4167,1.79691], "samples":[ - {"t":0.0, "x":5.13667, "y":5.19714, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.74303, "ay":3.66641, "alpha":21.07046, "fx":[-1.83931,-86.47711,-141.61168,-86.32828], "fy":[108.57122,120.38114,36.52301,-21.00595]}, - {"t":0.02417, "x":5.13529, "y":5.19821, "heading":-2.0944, "vx":-0.11463, "vy":0.08861, "omega":0.50923, "ax":-4.74907, "ay":3.66257, "alpha":9.0189, "fx":[-47.26201,-81.36097,-108.18881,-79.84704], "fy":[79.2051,88.28514,47.13965,29.58303]}, - {"t":0.04834, "x":5.13113, "y":5.20142, "heading":-2.08209, "vx":-0.2294, "vy":0.17713, "omega":0.72719, "ax":-4.75351, "ay":3.65659, "alpha":2.32297, "fx":[-71.28405,-79.60753,-86.96156,-79.1015], "fy":[65.09326,68.41745,57.04606,53.25792]}, - {"t":0.0725, "x":5.1242, "y":5.20677, "heading":-2.06451, "vx":-0.34429, "vy":0.2655, "omega":0.78333, "ax":-4.75841, "ay":3.64997, "alpha":0.49169, "fx":[-77.67452,-79.4151,-80.93977,-79.25238], "fy":[61.741,62.43598,59.9936,59.20255]}, - {"t":0.09667, "x":5.11449, "y":5.21425, "heading":-2.04558, "vx":-0.45929, "vy":0.35371, "omega":0.79522, "ax":-4.7638, "ay":3.64267, "alpha":0.1226, "fx":[-78.33745,-79.52676,-79.85803,-79.91901], "fy":[62.05938,60.33404,59.81879,60.6741]}, - {"t":0.12084, "x":5.102, "y":5.22387, "heading":-2.02636, "vx":-0.57442, "vy":0.44174, "omega":0.79818, "ax":-4.76983, "ay":3.63448, "alpha":0.00166, "fx":[-79.5164,-79.50687,-79.49612,-79.52381], "fy":[60.58049,60.61604,60.5767,60.56704]}, - {"t":0.14501, "x":5.08672, "y":5.2356, "heading":-2.00707, "vx":-0.68969, "vy":0.52958, "omega":0.79822, "ax":-4.77654, "ay":3.62533, "alpha":-0.01105, "fx":[-79.53098,-79.557,-79.48931,-79.91329], "fy":[60.60928,60.31614,60.4342,60.37051]}, - {"t":0.16917, "x":5.06866, "y":5.24946, "heading":-1.98778, "vx":-0.80513, "vy":0.6172, "omega":0.79795, "ax":-4.78412, "ay":3.61495, "alpha":-0.02086, "fx":[-79.8671,-79.7027,-79.65368,-79.77254], "fy":[60.13793,60.30396,60.2789,60.31715]}, - {"t":0.19334, "x":5.0478, "y":5.26543, "heading":-1.9685, "vx":-0.92075, "vy":0.70456, "omega":0.79745, "ax":-4.7927, "ay":3.60315, "alpha":-0.01408, "fx":[-79.74406,-79.79615,-79.67545,-80.35214], "fy":[60.42101,59.8508,60.07227,59.90702]}, - {"t":0.21751, "x":5.02415, "y":5.28351, "heading":-1.94922, "vx":-1.03658, "vy":0.79164, "omega":0.79711, "ax":-4.80252, "ay":3.58955, "alpha":-0.02265, "fx":[-80.24602,-79.94013,-79.93821,-80.09863], "fy":[59.59868,60.02264,59.84154,59.88168]}, - {"t":0.24168, "x":4.9977, "y":5.30369, "heading":-1.92996, "vx":-1.15265, "vy":0.8784, "omega":0.79656, "ax":-4.81386, "ay":3.57376, "alpha":-0.01247, "fx":[-80.05479,-80.11254,-79.9759,-80.8358], "fy":[60.04754,59.33105,59.59553,59.31761]}, - {"t":0.26585, "x":4.96843, "y":5.32597, "heading":-1.91071, "vx":-1.26899, "vy":0.96477, "omega":0.79626, "ax":-4.8271, "ay":3.5552, "alpha":-0.02355, "fx":[-80.69351,-80.31441,-80.32995,-80.52363], "fy":[58.95563,59.54366,59.26002,59.29435]}, - {"t":0.29001, "x":4.93635, "y":5.35032, "heading":-1.89147, "vx":-1.38565, "vy":1.05069, "omega":0.79569, "ax":-4.84276, "ay":3.53303, "alpha":-0.01822, "fx":[-80.53271,-80.57882,-80.41641,-81.37777], "fy":[59.46721,58.57624,58.91556,58.61672]}, - {"t":0.31418, "x":4.90145, "y":5.37675, "heading":-1.87223, "vx":-1.50269, "vy":1.13607, "omega":0.79525, "ax":-4.86154, "ay":3.50616, "alpha":-0.03185, "fx":[-81.33039,-80.84361,-80.85735,-81.12687], "fy":[58.0394,58.81981,58.44376,58.48101]}, - {"t":0.33835, "x":4.86372, "y":5.40523, "heading":-1.85302, "vx":-1.62018, "vy":1.22081, "omega":0.79448, "ax":-4.88457, "ay":3.47279, "alpha":-0.03534, "fx":[-81.2578,-81.21738,-81.01509,-82.2032], "fy":[58.58649,57.45422,57.93536,57.58298]}, - {"t":0.36252, "x":4.82313, "y":5.43575, "heading":-1.83381, "vx":-1.73823, "vy":1.30474, "omega":0.79363, "ax":-4.91326, "ay":3.43051, "alpha":-0.05973, "fx":[-82.3077,-81.64178,-81.61046,-82.04683], "fy":[56.62903,57.60478,57.22645,57.27983]}, - {"t":0.38668, "x":4.77969, "y":5.46828, "heading":-1.81463, "vx":-1.85697, "vy":1.38765, "omega":0.79218, "ax":-4.95025, "ay":3.37485, "alpha":-0.09156, "fx":[-82.63306,-82.28893,-82.00894,-83.14213], "fy":[56.83418,55.60264,56.38375,56.20815]}, - {"t":0.41085, "x":4.73336, "y":5.5028, "heading":-1.79549, "vx":-1.97661, "vy":1.46921, "omega":0.78997, "ax":-4.99934, "ay":3.29883, "alpha":-0.13717, "fx":[-84.03274,-82.91222,-82.76679,-83.63459], "fy":[54.06703,55.45161,55.16989,55.27081]}, - {"t":0.43502, "x":4.68413, "y":5.53927, "heading":-1.7764, "vx":-2.09743, "vy":1.54893, "omega":0.78666, "ax":-5.06803, "ay":3.18803, "alpha":-0.25158, "fx":[-85.20698,-84.09511,-83.59396,-85.03054], "fy":[53.42328,51.87556,53.58782,53.68504]}, - {"t":0.45919, "x":4.63196, "y":5.57764, "heading":-1.75739, "vx":-2.21992, "vy":1.62598, "omega":0.78058, "ax":-5.1693, "ay":3.01409, "alpha":-0.4248, "fx":[-87.737,-85.26483,-84.71233,-86.96458], "fy":[48.29066,50.30658,51.05915,51.31751]}, - {"t":0.48336, "x":4.5768, "y":5.61781, "heading":-1.73852, "vx":-2.34485, "vy":1.69883, "omega":0.77031, "ax":-5.33373, "ay":2.69893, "alpha":-1.04079, "fx":[-91.46206,-87.12905,-85.88075,-91.17097], "fy":[43.6644,41.21753,47.32694,47.75043]}, - {"t":0.50752, "x":4.51858, "y":5.65966, "heading":-1.7199, "vx":-2.47375, "vy":1.76405, "omega":0.74516, "ax":-5.62343, "ay":1.98808, "alpha":-2.71888, "fx":[-100.58495,-89.00724,-86.8679,-98.49931], "fy":[25.55073,25.9823,39.55174,41.47689]}, - {"t":0.53169, "x":4.45715, "y":5.70287, "heading":-1.7019, "vx":-2.60966, "vy":1.8121, "omega":0.67945, "ax":-5.89582, "ay":-0.58694, "alpha":-8.69294, "fx":[-116.35586,-80.30115,-78.06481,-118.39989], "fy":[-25.86259,-39.48382,9.10452,17.10558]}, - {"t":0.55586, "x":4.39236, "y":5.7465, "heading":-1.68547, "vx":-2.75215, "vy":1.79792, "omega":0.46936, "ax":-3.56233, "ay":-4.29266, "alpha":2.41597, "fx":[-53.21753,-65.17613,-65.43434,-53.70158], "fy":[-65.93855,-65.53973,-76.12836,-78.61955]}, - {"t":0.56995, "x":4.35322, "y":5.77141, "heading":-1.67886, "vx":-2.80235, "vy":1.73742, "omega":0.50341, "ax":-2.97241, "ay":-4.88539, "alpha":0.98179, "fx":[-46.35587,-52.03559,-52.07768,-47.72575], "fy":[-80.85126,-77.71857,-83.10109,-84.07774]}, - {"t":0.58405, "x":4.31343, "y":5.79541, "heading":-1.67176, "vx":-2.84424, "vy":1.66857, "omega":0.51724, "ax":-2.78731, "ay":-4.88281, "alpha":0.28735, "fx":[-45.36757,-47.49137,-47.60833,-45.38513], "fy":[-79.73502,-82.31049,-81.55817,-81.97258]}, - {"t":0.59814, "x":4.27307, "y":5.81844, "heading":-1.66448, "vx":-2.88352, "vy":1.59975, "omega":0.52129, "ax":-2.58495, "ay":-4.7787, "alpha":0.06945, "fx":[-42.24246,-43.44439,-43.50777,-43.16474], "fy":[-81.19235,-78.40038,-79.46664,-79.5751]}, - {"t":0.61223, "x":4.23217, "y":5.84051, "heading":-1.65713, "vx":-2.91995, "vy":1.53241, "omega":0.52227, "ax":-2.19065, "ay":-4.29731, "alpha":-0.025, "fx":[-36.24833,-36.71564,-36.81716,-36.28702], "fy":[-70.83543,-73.08757,-71.27336,-71.3403]}, - {"t":0.62633, "x":4.1908, "y":5.86168, "heading":-1.64977, "vx":-2.95083, "vy":1.47184, "omega":0.52192, "ax":-1.81503, "ay":-3.70675, "alpha":-0.0362, "fx":[-29.63859,-30.18168,-30.22687,-30.97525], "fy":[-62.86403,-61.09247,-61.58831,-61.61444]}, - {"t":0.64042, "x":4.14904, "y":5.88206, "heading":-1.64241, "vx":-2.97641, "vy":1.4196, "omega":0.52141, "ax":-1.44573, "ay":-3.09516, "alpha":0.00119, "fx":[-23.8994,-24.25339,-24.31479,-23.93059], "fy":[-51.08915,-52.41509,-51.41804,-51.45699]}, - {"t":0.65451, "x":4.10695, "y":5.90175, "heading":-1.63506, "vx":-2.99678, "vy":1.37598, "omega":0.52143, "ax":-1.1254, "ay":-2.48688, "alpha":-0.00268, "fx":[-18.17924,-18.68711,-18.72396,-19.44949], "fy":[-41.93749,-40.96435,-41.44601,-41.47269]}, - {"t":0.6686, "x":4.0646, "y":5.9209, "heading":-1.62772, "vx":-3.01264, "vy":1.34094, "omega":0.52139, "ax":-0.91725, "ay":-2.08364, "alpha":0.03472, "fx":[-15.10724,-15.44403,-15.48035,-15.12888], "fy":[-34.39558,-35.0601,-34.72306,-34.75469]}, - {"t":0.6827, "x":4.02205, "y":5.93959, "heading":-1.62037, "vx":-3.02557, "vy":1.31157, "omega":0.52188, "ax":-0.67601, "ay":-1.58222, "alpha":0.02106, "fx":[-10.73636,-11.20801,-11.23568,-11.8948], "fy":[-26.45271,-26.03828,-26.49343,-26.51527]}, - {"t":0.69679, "x":3.97934, "y":5.95792, "heading":-1.61301, "vx":-3.03509, "vy":1.28927, "omega":0.52217, "ax":-0.57983, "ay":-1.36638, "alpha":0.04993, "fx":[-9.51217,-9.80142,-9.82244,-9.52597], "fy":[-22.56115,-22.79482,-22.86446,-22.8869]}, - {"t":0.71088, "x":3.93651, "y":5.97595, "heading":-1.60565, "vx":-3.04327, "vy":1.27002, "omega":0.52288, "ax":-0.4071, "ay":-0.99389, "alpha":0.03261, "fx":[-6.30271,-6.73009,-6.74925,-7.36254], "fy":[-16.40649,-16.33821,-16.7554,-16.77037]}, - {"t":0.72498, "x":3.89358, "y":5.99375, "heading":-1.59828, "vx":-3.049, "vy":1.25601, "omega":0.52334, "ax":-0.38153, "ay":-0.91814, "alpha":0.0585, "fx":[-6.22211,-6.48708,-6.49979,-6.231], "fy":[-15.1544,-15.15305,-15.44891,-15.46378]}, - {"t":0.73907, "x":3.85057, "y":6.01136, "heading":-1.59091, "vx":-3.05438, "vy":1.24307, "omega":0.52416, "ax":-0.26754, "ay":-0.67424, "alpha":0.04242, "fx":[-4.01441,-4.42947,-4.44172,-4.95357], "fy":[-10.97329,-11.05247,-11.46111,-11.47037]}, - {"t":0.75316, "x":3.8075, "y":6.02881, "heading":-1.58352, "vx":-3.05815, "vy":1.23357, "omega":0.52476, "ax":-0.2875, "ay":-0.7019, "alpha":0.06693, "fx":[-4.64462,-4.93383,-4.94154,-4.64984], "fy":[-11.54761,-11.49214,-11.87642,-11.8855]}, - {"t":0.76726, "x":3.76437, "y":6.04613, "heading":-1.57613, "vx":-3.0622, "vy":1.22367, "omega":0.5257, "ax":-0.22658, "ay":-0.58266, "alpha":0.05678, "fx":[-3.35017,-3.80881,-3.81454,-4.13447], "fy":[-9.44598,-9.49755,-9.9515,-9.95551]}, - {"t":0.78135, "x":3.7212, "y":6.06332, "heading":-1.56872, "vx":-3.0654, "vy":1.21546, "omega":0.5265, "ax":-0.27756, "ay":-0.69052, "alpha":0.08257, "fx":[-4.43292,-4.81949,-4.82175,-4.43313], "fy":[-11.27554,-11.32902,-11.71738,-11.72059]}, - {"t":0.79544, "x":3.67797, "y":6.08038, "heading":-1.5613, "vx":-3.06931, "vy":1.20573, "omega":0.52767, "ax":-0.2685, "ay":-0.69752, "alpha":0.09103, "fx":[-4.00886,-4.64168,-4.63805,-4.61416], "fy":[-11.41039,-11.28247,-11.91003,-11.90656]}, - {"t":0.80954, "x":3.63468, "y":6.0973, "heading":-1.55386, "vx":-3.07309, "vy":1.1959, "omega":0.52895, "ax":-0.34207, "ay":-0.87509, "alpha":0.13929, "fx":[-5.35109,-6.06488,-6.05419,-5.33856], "fy":[-14.1098,-14.42535,-14.91108,-14.90287]}, - {"t":0.82363, "x":3.59134, "y":6.11407, "heading":-1.54641, "vx":-3.07791, "vy":1.18357, "omega":0.53091, "ax":-0.38702, "ay":-1.01921, "alpha":0.21537, "fx":[-5.71209,-6.99819,-6.96727,-6.12821], "fy":[-16.68372,-16.25003,-17.52553,-17.49953]}, - {"t":0.83772, "x":3.54792, "y":6.13065, "heading":-1.53892, "vx":-3.08337, "vy":1.1692, "omega":0.53395, "ax":-0.47745, "ay":-1.26764, "alpha":0.38477, "fx":[-6.98971,-8.99549,-8.92828,-6.92224], "fy":[-19.89121,-20.64142,-22.02314,-21.96772]}, - {"t":0.85182, "x":3.50442, "y":6.147, "heading":-1.5314, "vx":-3.0901, "vy":1.15134, "omega":0.53937, "ax":-0.5797, "ay":-1.57043, "alpha":0.70914, "fx":[-7.74685,-11.54873,-11.39207,-7.96533], "fy":[-24.91011,-24.13391,-27.90198,-27.76749]}, - {"t":0.86591, "x":3.46082, "y":6.16307, "heading":-1.5238, "vx":-3.09827, "vy":1.12921, "omega":0.54936, "ax":-0.67944, "ay":-1.89823, "alpha":1.39404, "fx":[-7.91046,-15.09213,-14.73017,-7.57117], "fy":[-27.717,-29.20241,-34.97425,-34.6765]}, - {"t":0.88, "x":3.41708, "y":6.17879, "heading":-1.51606, "vx":-3.10784, "vy":1.10245, "omega":0.56901, "ax":-0.83907, "ay":-2.3935, "alpha":2.66235, "fx":[-7.48552,-21.03862,-20.23622,-7.18754], "fy":[-33.96754,-33.18485,-46.56496,-45.87642]}, - {"t":0.8941, "x":3.3732, "y":6.19409, "heading":-1.50804, "vx":-3.11967, "vy":1.06872, "omega":0.60653, "ax":-0.79647, "ay":-2.82233, "alpha":5.48932, "fx":[-0.00878,-28.31847,-26.3418,1.562], "fy":[-32.60627,-36.29486,-60.44665,-58.83978]}, - {"t":0.90819, "x":3.32916, "y":6.20888, "heading":-1.49949, "vx":-3.13089, "vy":1.02895, "omega":0.68389, "ax":-1.05961, "ay":-2.86269, "alpha":6.0743, "fx":[-3.63898,-34.03342,-31.52992,-1.45024], "fy":[-31.23343,-35.50371,-62.93179,-61.20971]}, - {"t":0.92514, "x":3.27593, "y":6.22591, "heading":-1.4879, "vx":-3.14885, "vy":0.98042, "omega":0.78686, "ax":-0.76186, "ay":-2.49421, "alpha":1.23744, "fx":[-9.88765,-16.30053,-15.74026,-8.87124], "fy":[-39.56469,-38.16379,-44.54408,-44.03677]}, - {"t":0.94209, "x":3.22245, "y":6.24217, "heading":-1.47456, "vx":-3.16177, "vy":0.93815, "omega":0.80783, "ax":-0.49009, "ay":-1.68517, "alpha":0.20074, "fx":[-7.61288,-8.84746,-8.72668,-7.49123], "fy":[-26.99967,-28.54547,-28.47467,-28.3443]}, - {"t":0.95904, "x":3.16879, "y":6.25783, "heading":-1.46087, "vx":-3.17007, "vy":0.90958, "omega":0.81123, "ax":-0.3027, "ay":-1.06131, "alpha":0.01983, "fx":[-5.05409,-5.29033,-5.26362,-4.57528], "fy":[-18.28265,-17.34602,-17.58199,-17.55539]}, - {"t":0.97599, "x":3.11501, "y":6.27309, "heading":-1.44711, "vx":-3.1752, "vy":0.89159, "omega":0.81157, "ax":-0.10272, "ay":-0.36971, "alpha":-0.00806, "fx":[-1.66616,-1.77179,-1.75862,-1.65281], "fy":[-5.97595,-6.5741,-6.05526,-6.04628]}, - {"t":0.99294, "x":3.06117, "y":6.28815, "heading":-1.43336, "vx":-3.17695, "vy":0.88532, "omega":0.81143, "ax":0.05466, "ay":0.19736, "alpha":-0.04074, "fx":[0.55579,0.91166,0.86268,1.31482], "fy":[2.90846,3.19662,3.55161,3.50262]}, - {"t":1.00989, "x":3.00733, "y":6.30319, "heading":-1.4196, "vx":-3.17602, "vy":0.88867, "omega":0.81074, "ax":0.25633, "ay":0.90684, "alpha":-0.05051, "fx":[4.16234,4.42368,4.38356,4.12225], "fy":[14.88783,15.13769,15.24377,15.19711]}, - {"t":1.02684, "x":2.95353, "y":6.31838, "heading":-1.40586, "vx":-3.17167, "vy":0.90404, "omega":0.80989, "ax":0.45351, "ay":1.56394, "alpha":-0.0738, "fx":[6.95345,7.77818,7.64005,7.8672], "fy":[26.03777,25.58153,26.39931,26.26222]}, - {"t":1.04379, "x":2.89983, "y":6.33393, "heading":-1.39213, "vx":-3.16399, "vy":0.93055, "omega":0.80864, "ax":0.72566, "ay":2.41266, "alpha":-0.08451, "fx":[11.84342,12.46176,12.35216,11.72858], "fy":[39.53253,40.83634,40.30985,40.19313]}, - {"t":1.06074, "x":2.84631, "y":6.35005, "heading":-1.37843, "vx":-3.15169, "vy":0.97145, "omega":0.8072, "ax":1.05062, "ay":3.29525, "alpha":-0.09126, "fx":[16.70054,17.98147,17.72673,17.64473], "fy":[55.44721,54.00534,55.2571,55.01112]}, - {"t":1.07769, "x":2.79304, "y":6.36699, "heading":-1.36474, "vx":-3.13388, "vy":1.0273, "omega":0.80566, "ax":1.45193, "ay":4.27118, "alpha":-0.11201, "fx":[23.81974,24.78769,24.60063,23.60354], "fy":[70.00216,72.59682,71.19707,70.99797]}, - {"t":1.09464, "x":2.74012, "y":6.38501, "heading":-1.35109, "vx":-3.10927, "vy":1.0997, "omega":0.80376, "ax":1.86887, "ay":5.05313, "alpha":-0.0819, "fx":[30.18223,31.81079,31.43372,31.18621], "fy":[85.33768,82.95221,84.49651,84.14677]}, - {"t":1.11159, "x":2.68769, "y":6.40438, "heading":-1.33746, "vx":-3.07759, "vy":1.18535, "omega":0.80237, "ax":2.10056, "ay":5.2247, "alpha":-0.09785, "fx":[34.64271,35.58967,35.43456,34.39433], "fy":[85.73825,88.91519,86.97618,86.7431]}, - {"t":1.12855, "x":2.63582, "y":6.42522, "heading":-1.32386, "vx":-3.04198, "vy":1.27392, "omega":0.80071, "ax":2.29733, "ay":5.26287, "alpha":-0.02495, "fx":[37.61851,38.84637,38.55602,38.16095], "fy":[89.06984,86.65566,87.76117,87.43167]}, - {"t":1.1455, "x":2.58459, "y":6.44757, "heading":-1.31029, "vx":-3.00304, "vy":1.36312, "omega":0.80029, "ax":2.48265, "ay":5.25491, "alpha":0.171, "fx":[41.40411,41.12192,41.46137,41.55113], "fy":[86.67646,90.51145,86.63696,86.56263]}, - {"t":1.16245, "x":2.53404, "y":6.47143, "heading":-1.29673, "vx":-2.96096, "vy":1.4522, "omega":0.80319, "ax":2.68382, "ay":5.20641, "alpha":0.41372, "fx":[44.43518,43.9934,44.29635,46.2271], "fy":[89.47369,86.389,85.69563,85.59482]}, - {"t":1.1794, "x":2.48424, "y":6.4968, "heading":-1.28311, "vx":-2.91547, "vy":1.54045, "omega":0.8102, "ax":3.74261, "ay":4.54875, "alpha":0.60173, "fx":[63.05708,60.7235,61.80328,63.96615], "fy":[76.62399,78.85426,73.66553,74.15811]}, - {"t":1.19635, "x":2.43536, "y":6.52356, "heading":-1.26938, "vx":-2.85203, "vy":1.61755, "omega":0.8204, "ax":5.44191, "ay":2.35406, "alpha":0.77131, "fx":[90.78279,88.42584,89.34564,94.30196], "fy":[41.53529,40.18824,37.39337,37.84708]}, - {"t":1.2133, "x":2.3878, "y":6.55132, "heading":-1.25547, "vx":-2.75978, "vy":1.65746, "omega":0.83347, "ax":5.86448, "ay":1.02369, "alpha":0.59302, "fx":[98.51603,96.05572,97.01498,99.44539], "fy":[18.78805,18.48065,15.08713,15.90214]}, - {"t":1.23025, "x":2.34186, "y":6.57956, "heading":-1.24134, "vx":-2.66038, "vy":1.67481, "omega":0.84353, "ax":5.95671, "ay":0.32977, "alpha":0.50062, "fx":[99.3889,97.64386,98.29021,101.85926], "fy":[6.50001,6.3498,4.32802,4.81047]}, - {"t":1.2472, "x":2.29762, "y":6.608, "heading":-1.22705, "vx":-2.55941, "vy":1.6804, "omega":0.85201, "ax":5.97282, "ay":-0.07752, "alpha":0.31788, "fx":[99.99455,98.59211,99.12951,100.53977], "fy":[0.05361,-1.29451,-2.25409,-1.67411]}, - {"t":1.26415, "x":2.2551, "y":6.63647, "heading":-1.2126, "vx":-2.45817, "vy":1.67908, "omega":0.8574, "ax":5.96841, "ay":-0.34168, "alpha":0.23069, "fx":[99.53994,98.55848,98.93371,100.92984], "fy":[-5.71726,-5.03337,-6.19698,-5.83469]}, - {"t":1.2811, "x":2.21429, "y":6.66488, "heading":-1.19807, "vx":-2.357, "vy":1.67329, "omega":0.86131, "ax":5.95841, "ay":-0.52586, "alpha":0.15795, "fx":[99.54234,98.81155,99.10225,99.83939], "fy":[-7.8391,-9.23466,-9.17988,-8.80975]}, - {"t":1.29805, "x":2.17519, "y":6.69317, "heading":-1.18347, "vx":-2.256, "vy":1.66438, "omega":0.86399, "ax":5.94776, "ay":-0.65818, "alpha":0.13182, "fx":[99.15788,98.58464,98.8173,100.0253], "fy":[-11.08422,-10.53378,-11.27897,-10.98891]}, - {"t":1.315, "x":2.1378, "y":6.72129, "heading":-1.16883, "vx":-2.15518, "vy":1.65322, "omega":0.86622, "ax":5.93724, "ay":-0.76229, "alpha":0.08238, "fx":[99.10494,98.64196,98.83956,99.29698], "fy":[-11.86125,-13.66619,-12.78849,-12.51185]}, - {"t":1.33195, "x":2.10212, "y":6.7492, "heading":-1.15414, "vx":-2.05454, "vy":1.6403, "omega":0.86762, "ax":5.92756, "ay":-0.84492, "alpha":0.0484, "fx":[98.83783,98.40237,98.59348,99.40446], "fy":[-14.68516,-13.57387,-14.17646,-13.90238]}, - {"t":1.3489, "x":2.06815, "y":6.77688, "heading":-1.13944, "vx":-1.95407, "vy":1.62598, "omega":0.86844, "ax":5.91925, "ay":-0.90929, "alpha":-0.01247, "fx":[98.70111,98.58834,98.64754,98.74749], "fy":[-14.85518,-15.9892,-14.90171,-14.88373]}, - {"t":1.36585, "x":2.03588, "y":6.80431, "heading":-1.12472, "vx":-1.85374, "vy":1.61057, "omega":0.86823, "ax":5.91177, "ay":-0.96299, "alpha":-0.47992, "fx":[97.99017,99.411,98.81529,97.96897], "fy":[-18.82611,-16.30643,-13.97203,-15.10576]}, - {"t":1.3828, "x":2.00531, "y":6.83148, "heading":-1.11, "vx":-1.75353, "vy":1.59424, "omega":0.86009, "ax":5.90483, "ay":-1.00967, "alpha":-2.57464, "fx":[96.11213,104.63896,100.92519,92.04631], "fy":[-26.94094,-20.90684,-6.34914,-13.12596]}, - {"t":1.39975, "x":1.97643, "y":6.85835, "heading":-1.09542, "vx":-1.65344, "vy":1.57713, "omega":0.81645, "ax":5.89888, "ay":-1.04795, "alpha":-10.12375, "fx":[89.70257,122.99426,108.97334,71.6556], "fy":[-59.79988,-27.81539,21.39311,-3.65306]}, - {"t":1.4167, "x":1.94925, "y":6.88494, "heading":-1.08158, "vx":-1.55345, "vy":1.55936, "omega":0.64485, "ax":5.80917, "ay":-1.45926, "alpha":-2.24162, "fx":[94.60041,102.60724,99.23986,90.89705], "fy":[-32.76151,-27.4342,-15.32744,-21.77748]}, - {"t":1.44386, "x":1.90921, "y":6.92675, "heading":-1.06407, "vx":-1.39568, "vy":1.51973, "omega":0.58397, "ax":5.58205, "ay":-2.18664, "alpha":-1.82953, "fx":[90.73323,97.66455,95.09826,88.70416], "fy":[-44.39296,-37.50934,-29.18844,-34.70995]}, - {"t":1.47102, "x":1.87336, "y":6.96721, "heading":-1.04821, "vx":-1.24409, "vy":1.46035, "omega":0.53428, "ax":5.29472, "ay":-2.81244, "alpha":-1.51314, "fx":[86.35957,92.23961,90.33379,84.10905], "fy":[-52.48215,-48.38655,-40.76686,-45.89241]}, - {"t":1.49818, "x":1.84153, "y":7.00584, "heading":-1.0337, "vx":-1.1003, "vy":1.38397, "omega":0.49319, "ax":4.98184, "ay":-3.33598, "alpha":-1.35494, "fx":[80.85839,86.7487,84.83224,79.74002], "fy":[-61.36216,-55.90902,-50.41645,-54.74877]}, - {"t":1.52533, "x":1.81348, "y":7.04219, "heading":-1.0203, "vx":-0.965, "vy":1.29337, "omega":0.45639, "ax":4.66646, "ay":-3.76499, "alpha":-1.23032, "fx":[76.07287,81.28204,79.676,74.11947], "fy":[-67.13275,-63.52211,-58.0023,-62.38506]}, - {"t":1.55249, "x":1.789, "y":7.07593, "heading":-1.00791, "vx":-0.83827, "vy":1.19112, "omega":0.42298, "ax":4.36215, "ay":-4.11412, "alpha":-1.23727, "fx":[70.58342,76.4563,74.50085,69.31895], "fy":[-73.46374,-68.54247,-64.08184,-68.23389]}, - {"t":1.57965, "x":1.76784, "y":7.10676, "heading":-0.99642, "vx":-0.7198, "vy":1.07939, "omega":0.38938, "ax":4.07752, "ay":-4.39674, "alpha":-1.23966, "fx":[66.14208,71.82085,69.92397,63.99441], "fy":[-77.55081,-73.58633,-68.79884,-73.23008]}, - {"t":1.60681, "x":1.74979, "y":7.13445, "heading":-0.98585, "vx":-0.60907, "vy":0.95999, "omega":0.35571, "ax":3.8155, "ay":-4.62627, "alpha":-1.34603, "fx":[61.42432,68.02895,65.6192,59.33781], "fy":[-81.89245,-76.9441,-72.4694,-77.16454]}, - {"t":1.63397, "x":1.73466, "y":7.15882, "heading":-0.97619, "vx":-0.50545, "vy":0.83435, "omega":0.31916, "ax":3.57707, "ay":-4.81327, "alpha":-1.33655, "fx":[58.14203,64.27202,61.661,54.43701], "fy":[-83.96725,-80.51558,-75.67872,-80.77815]}, - {"t":1.66112, "x":1.72225, "y":7.1797, "heading":-0.96752, "vx":-0.4083, "vy":0.70363, "omega":0.28286, "ax":3.36114, "ay":-4.9667, "alpha":-1.55456, "fx":[53.9596,60.97898,58.6788,50.49688], "fy":[-87.8869,-82.83418,-77.42815,-83.02086]}, - {"t":1.68828, "x":1.7124, "y":7.19698, "heading":-0.95984, "vx":-0.31702, "vy":0.56875, "omega":0.24064, "ax":3.16605, "ay":-5.09349, "alpha":-1.80493, "fx":[50.23993,59.23125,55.42876,46.20632], "fy":[-90.57147,-84.62129,-79.14609,-85.28518]}, - {"t":1.71544, "x":1.70496, "y":7.21055, "heading":-0.9533, "vx":-0.23104, "vy":0.43042, "omega":0.19162, "ax":2.98998, "ay":-5.19899, "alpha":-2.08694, "fx":[46.6333,57.09443,53.2789,42.35924], "fy":[-93.3736,-86.2946,-79.94906,-87.04131]}, - {"t":1.7426, "x":1.69979, "y":7.22032, "heading":-0.9481, "vx":-0.14984, "vy":0.28923, "omega":0.13495, "ax":2.8307, "ay":-5.28757, "alpha":-2.31677, "fx":[44.22467,55.39724,50.67066,38.45274], "fy":[-95.0983,-88.20915,-80.70123,-88.556]}, - {"t":1.76975, "x":1.69676, "y":7.22622, "heading":-0.94443, "vx":-0.07296, "vy":0.14563, "omega":0.07203, "ax":2.68662, "ay":-5.36235, "alpha":-2.65227, "fx":[41.27831,54.51895,48.80913,34.53247], "fy":[-97.23124,-88.98485,-81.24362,-90.09157]}, + {"t":0.0, "x":5.13667, "y":5.19714, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.74303, "ay":3.66641, "alpha":21.06933, "fx":[-1.77954,-86.53735,-141.6162,-86.32336], "fy":[108.56338,120.33534,36.52755,-20.9572]}, + {"t":0.02417, "x":5.13529, "y":5.19821, "heading":-2.0944, "vx":-0.11463, "vy":0.08861, "omega":0.5092, "ax":-4.74907, "ay":3.66257, "alpha":9.01989, "fx":[-47.25507,-81.3647,-108.19351,-79.84552], "fy":[79.2136,88.2833,47.13152,29.58455]}, + {"t":0.04834, "x":5.13113, "y":5.20142, "heading":-2.08209, "vx":-0.2294, "vy":0.17713, "omega":0.72719, "ax":-4.75351, "ay":3.6566, "alpha":2.32306, "fx":[-71.28276,-79.60126,-86.95854,-79.11205], "fy":[65.08481,68.42544,57.04973,53.25474]}, + {"t":0.0725, "x":5.1242, "y":5.20677, "heading":-2.06451, "vx":-0.34429, "vy":0.2655, "omega":0.78333, "ax":-4.75841, "ay":3.64997, "alpha":0.49177, "fx":[-77.67607,-79.41353,-80.93912,-79.25301], "fy":[61.73947,62.43926,59.99295,59.20149]}, + {"t":0.09667, "x":5.11449, "y":5.21425, "heading":-2.04558, "vx":-0.45929, "vy":0.35371, "omega":0.79522, "ax":-4.7638, "ay":3.64267, "alpha":0.12267, "fx":[-78.33866,-79.53232,-79.86258,-79.90766], "fy":[62.06658,60.32724,59.81338,60.67915]}, + {"t":0.12084, "x":5.102, "y":5.22387, "heading":-2.02636, "vx":-0.57442, "vy":0.44174, "omega":0.79818, "ax":-4.76983, "ay":3.63448, "alpha":0.00151, "fx":[-79.51521,-79.50728,-79.49969,-79.52099], "fy":[60.5812,60.61164,60.57634,60.57114]}, + {"t":0.14501, "x":5.08672, "y":5.2356, "heading":-2.00707, "vx":-0.68969, "vy":0.52958, "omega":0.79822, "ax":-4.77654, "ay":3.62533, "alpha":-0.01104, "fx":[-79.53158,-79.55886,-79.49042,-79.90968], "fy":[60.61258,60.31322,60.43261,60.37177]}, + {"t":0.16917, "x":5.06866, "y":5.24946, "heading":-1.98778, "vx":-0.80513, "vy":0.6172, "omega":0.79795, "ax":-4.78412, "ay":3.61495, "alpha":-0.02074, "fx":[-79.87136,-79.6981,-79.65262,-79.77392], "fy":[60.13144,60.31374,60.27687,60.31593]}, + {"t":0.19334, "x":5.0478, "y":5.26543, "heading":-1.9685, "vx":-0.92075, "vy":0.70456, "omega":0.79745, "ax":-4.7927, "ay":3.60315, "alpha":-0.01422, "fx":[-79.74561,-79.79778,-79.67584,-80.34854], "fy":[60.42321,59.84792,60.07152,59.90849]}, + {"t":0.21751, "x":5.02415, "y":5.28351, "heading":-1.94923, "vx":-1.03658, "vy":0.79164, "omega":0.79711, "ax":-4.80252, "ay":3.58955, "alpha":-0.02263, "fx":[-80.24938,-79.93647,-79.9374,-80.09972], "fy":[59.59334,60.03016,59.84013,59.88094]}, + {"t":0.24168, "x":4.9977, "y":5.30369, "heading":-1.92996, "vx":-1.15265, "vy":0.8784, "omega":0.79656, "ax":-4.81386, "ay":3.57376, "alpha":-0.01251, "fx":[-80.05639,-80.11415,-79.9764,-80.83206], "fy":[60.05099,59.32785,59.59446,59.31846]}, + {"t":0.26585, "x":4.96843, "y":5.32597, "heading":-1.91071, "vx":-1.26899, "vy":0.96477, "omega":0.79626, "ax":-4.8271, "ay":3.5552, "alpha":-0.0236, "fx":[-80.69504,-80.31313,-80.32938,-80.52393], "fy":[58.95269,59.54704,59.2595,59.29446]}, + {"t":0.29001, "x":4.93635, "y":5.35032, "heading":-1.89147, "vx":-1.38565, "vy":1.05069, "omega":0.79569, "ax":-4.84276, "ay":3.53303, "alpha":-0.0182, "fx":[-80.53433,-80.58291,-80.4174,-81.37104], "fy":[59.47639,58.56776,58.91292,58.61867]}, + {"t":0.31418, "x":4.90145, "y":5.37675, "heading":-1.87224, "vx":-1.50269, "vy":1.13607, "omega":0.79525, "ax":-4.86154, "ay":3.50616, "alpha":-0.03176, "fx":[-81.33856,-80.83413,-80.8556,-81.1299], "fy":[58.02526,58.84034,58.44055,58.47784]}, + {"t":0.33835, "x":4.86372, "y":5.40523, "heading":-1.85302, "vx":-1.62018, "vy":1.22081, "omega":0.79448, "ax":-4.88457, "ay":3.47279, "alpha":-0.03461, "fx":[-81.25984,-81.22141,-81.01671,-82.19549], "fy":[58.60427,57.44421,57.93054,57.58005]}, + {"t":0.36252, "x":4.82313, "y":5.43575, "heading":-1.83382, "vx":-1.73823, "vy":1.30474, "omega":0.79364, "ax":-4.91326, "ay":3.43051, "alpha":-0.05781, "fx":[-82.31032,-81.6339,-81.61282,-82.04973], "fy":[56.6204,57.63203,57.21711,57.27055]}, + {"t":0.38668, "x":4.77969, "y":5.46828, "heading":-1.81463, "vx":-1.85697, "vy":1.38765, "omega":0.79225, "ax":-4.95025, "ay":3.37485, "alpha":-0.09628, "fx":[-82.63254,-82.26031,-82.00425,-83.17598], "fy":[56.67504,55.70486,56.42453,56.22426]}, + {"t":0.41085, "x":4.73336, "y":5.5028, "heading":-1.79549, "vx":-1.97661, "vy":1.46921, "omega":0.78992, "ax":-4.99934, "ay":3.29883, "alpha":-0.13504, "fx":[-84.03504,-82.90489,-82.76896,-83.63745], "fy":[54.05841,55.48117,55.1595,55.26025]}, + {"t":0.43502, "x":4.68413, "y":5.53927, "heading":-1.7764, "vx":-2.09743, "vy":1.54893, "omega":0.78666, "ax":-5.06803, "ay":3.18803, "alpha":-0.25246, "fx":[-85.19235,-84.08431,-83.58373,-85.06624], "fy":[53.42564,51.87867,53.59195,53.6754]}, + {"t":0.45919, "x":4.63196, "y":5.57764, "heading":-1.75739, "vx":-2.21992, "vy":1.62598, "omega":0.78056, "ax":-5.1693, "ay":3.01409, "alpha":-0.4245, "fx":[-87.74442,-85.25658,-84.70924,-86.96854], "fy":[48.27367,50.33361,51.05392,51.31263]}, + {"t":0.48336, "x":4.5768, "y":5.61781, "heading":-1.73852, "vx":-2.34485, "vy":1.69883, "omega":0.7703, "ax":-5.33373, "ay":2.69893, "alpha":-1.04018, "fx":[-91.45922,-87.12556,-85.87583,-91.18226], "fy":[43.69566,41.20315,47.32009,47.7403]}, + {"t":0.50752, "x":4.51858, "y":5.65966, "heading":-1.7199, "vx":-2.47375, "vy":1.76405, "omega":0.74516, "ax":-5.62343, "ay":1.98808, "alpha":-2.71846, "fx":[-100.58838,-89.00479,-86.864,-98.50225], "fy":[25.53923,26.00223,39.54611,41.47401]}, + {"t":0.53169, "x":4.45715, "y":5.70287, "heading":-1.7019, "vx":-2.60966, "vy":1.8121, "omega":0.67946, "ax":-5.89582, "ay":-0.58696, "alpha":-8.69347, "fx":[-116.35874,-80.29784,-78.05717,-118.40788], "fy":[-25.85395,-39.48629,9.0996,17.10337]}, + {"t":0.55586, "x":4.39236, "y":5.7465, "heading":-1.68548, "vx":-2.75215, "vy":1.79792, "omega":0.46936, "ax":-3.56235, "ay":-4.29265, "alpha":2.41623, "fx":[-53.21825,-65.17551,-65.43375,-53.70302], "fy":[-65.94085,-65.53348,-76.12981,-78.62145]}, + {"t":0.56995, "x":4.35322, "y":5.77141, "heading":-1.67886, "vx":-2.80235, "vy":1.73742, "omega":0.50341, "ax":-2.97241, "ay":-4.8854, "alpha":0.9819, "fx":[-46.35596,-52.03514,-52.07841,-47.72532], "fy":[-80.84946,-77.71957,-83.10181,-84.07796]}, + {"t":0.58405, "x":4.31343, "y":5.79541, "heading":-1.67177, "vx":-2.84424, "vy":1.66857, "omega":0.51725, "ax":-2.78731, "ay":-4.88281, "alpha":0.28718, "fx":[-45.3683,-47.49067,-47.60755,-45.38596], "fy":[-79.73588,-82.30994,-81.55825,-81.97236]}, + {"t":0.59814, "x":4.27307, "y":5.81844, "heading":-1.66448, "vx":-2.88352, "vy":1.59975, "omega":0.52129, "ax":-2.58495, "ay":-4.77871, "alpha":0.06959, "fx":[-42.24492,-43.44575,-43.50907,-43.1599], "fy":[-81.19277,-78.40107,-79.46632,-79.57487]}, + {"t":0.61223, "x":4.23217, "y":5.84051, "heading":-1.65713, "vx":-2.91995, "vy":1.53241, "omega":0.52227, "ax":-2.19063, "ay":-4.29727, "alpha":-0.02525, "fx":[-36.24851,-36.71479,-36.81632,-36.28705], "fy":[-70.83463,-73.0884,-71.2721,-71.33879]}, + {"t":0.62633, "x":4.1908, "y":5.86168, "heading":-1.64977, "vx":-2.95083, "vy":1.47184, "omega":0.52192, "ax":-1.81505, "ay":-3.7068, "alpha":-0.03588, "fx":[-29.64321,-30.18526,-30.23021,-30.96523], "fy":[-62.86755,-61.093,-61.58765,-61.61406]}, + {"t":0.64042, "x":4.14904, "y":5.88206, "heading":-1.64241, "vx":-2.97641, "vy":1.4196, "omega":0.52141, "ax":-1.44565, "ay":-3.095, "alpha":0.00085, "fx":[-23.8985,-24.25185,-24.3134,-23.92953], "fy":[-51.08547,-52.41627,-51.41417,-51.45297]}, + {"t":0.65451, "x":4.10695, "y":5.90175, "heading":-1.63507, "vx":-2.99678, "vy":1.37599, "omega":0.52143, "ax":-1.12546, "ay":-2.48701, "alpha":-0.00222, "fx":[-18.18602,-18.69279,-18.72942,-19.43548], "fy":[-41.94484,-40.96545,-41.44582,-41.4728]}, + {"t":0.6686, "x":4.0646, "y":5.9209, "heading":-1.62772, "vx":-3.01264, "vy":1.34094, "omega":0.52139, "ax":-0.91717, "ay":-2.08346, "alpha":0.03428, "fx":[-15.10604,-15.44246,-15.47902,-15.12756], "fy":[-34.39054,-35.06289,-34.71805,-34.74958]}, + {"t":0.6827, "x":4.02205, "y":5.93959, "heading":-1.62037, "vx":-3.02557, "vy":1.31157, "omega":0.52188, "ax":-0.6761, "ay":-1.58243, "alpha":0.02159, "fx":[-10.74463,-11.21519,-11.24267,-11.87829], "fy":[-26.46342,-26.04,-26.49389,-26.51598]}, + {"t":0.69679, "x":3.97934, "y":5.95792, "heading":-1.61301, "vx":-3.03509, "vy":1.28927, "omega":0.52218, "ax":-0.57975, "ay":-1.36619, "alpha":0.04942, "fx":[-9.51087,-9.80001,-9.82126,-9.5246], "fy":[-22.55541,-22.79912,-22.85898,-22.88135]}, + {"t":0.71088, "x":3.93651, "y":5.97595, "heading":-1.60565, "vx":-3.04327, "vy":1.27002, "omega":0.52288, "ax":-0.40721, "ay":-0.99414, "alpha":0.03314, "fx":[-6.31151,-6.73789,-6.7569,-7.34539], "fy":[-16.41939,-16.34026,-16.75632,-16.77147]}, + {"t":0.72498, "x":3.89358, "y":5.99375, "heading":-1.59829, "vx":-3.049, "vy":1.25601, "omega":0.52334, "ax":-0.38144, "ay":-0.91792, "alpha":0.05798, "fx":[-6.22047,-6.48557,-6.49847,-6.2293], "fy":[-15.1476,-15.1575,-15.44261,-15.45744]}, + {"t":0.73907, "x":3.85057, "y":6.01136, "heading":-1.59091, "vx":-3.05438, "vy":1.24307, "omega":0.52416, "ax":-0.26765, "ay":-0.67451, "alpha":0.04288, "fx":[-4.02268,-4.43696,-4.44912,-4.93775], "fy":[-10.98695,-11.05447,-11.46224,-11.47162]}, + {"t":0.75316, "x":3.8075, "y":6.02881, "heading":-1.58352, "vx":-3.05815, "vy":1.23356, "omega":0.52477, "ax":-0.28739, "ay":-0.70164, "alpha":0.06646, "fx":[-4.64263,-4.93226,-4.9401,-4.64781], "fy":[-11.54001,-11.49601,-11.86958,-11.87864]}, + {"t":0.76726, "x":3.76437, "y":6.04613, "heading":-1.57613, "vx":-3.0622, "vy":1.22368, "omega":0.5257, "ax":-0.22668, "ay":-0.58291, "alpha":0.05712, "fx":[-3.35692,-3.81528,-3.82097,-4.12159], "fy":[-9.45908,-9.49905,-9.95263,-9.95673]}, + {"t":0.78135, "x":3.7212, "y":6.06332, "heading":-1.56872, "vx":-3.0654, "vy":1.21546, "omega":0.52651, "ax":-0.27747, "ay":-0.69028, "alpha":0.08221, "fx":[-4.43092,-4.81832,-4.82067,-4.43111], "fy":[-11.26823,-11.33283,-11.71121,-11.7144]}, + {"t":0.79544, "x":3.67797, "y":6.08038, "heading":-1.5613, "vx":-3.06931, "vy":1.20573, "omega":0.52767, "ax":-0.26858, "ay":-0.69774, "alpha":0.09126, "fx":[-4.0135,-4.64677,-4.6431,-4.60498], "fy":[-11.42211,-11.28307,-11.91101,-11.90759]}, + {"t":0.80954, "x":3.63468, "y":6.0973, "heading":-1.55386, "vx":-3.07309, "vy":1.1959, "omega":0.52895, "ax":-0.342, "ay":-0.87491, "alpha":0.13908, "fx":[-5.34921,-6.0645,-6.05386,-5.33665], "fy":[-14.10343,-14.42967,-14.90632,-14.89808]}, + {"t":0.82363, "x":3.59134, "y":6.11407, "heading":-1.54641, "vx":-3.07791, "vy":1.18357, "omega":0.53091, "ax":-0.38708, "ay":-1.01937, "alpha":0.21549, "fx":[-5.71447,-7.00188,-6.97091,-6.12257], "fy":[-16.6938,-16.24937,-17.52614,-17.50014]}, + {"t":0.83772, "x":3.54792, "y":6.13065, "heading":-1.53892, "vx":-3.08337, "vy":1.1692, "omega":0.53395, "ax":-0.47741, "ay":-1.26753, "alpha":0.38475, "fx":[-6.9877,-8.99619,-8.92896,-6.92015], "fy":[-19.88569,-20.64648,-22.01982,-21.96433]}, + {"t":0.85182, "x":3.50442, "y":6.147, "heading":-1.5314, "vx":-3.0901, "vy":1.15134, "omega":0.53937, "ax":-0.57973, "ay":-1.57053, "alpha":0.70919, "fx":[-7.7471,-11.55126,-11.39447,-7.96255], "fy":[-24.91871,-24.13175,-27.90207,-27.76752]}, + {"t":0.86591, "x":3.46082, "y":6.16307, "heading":-1.5238, "vx":-3.09827, "vy":1.12921, "omega":0.54937, "ax":-0.67942, "ay":-1.89816, "alpha":1.39425, "fx":[-7.90785,-15.09415,-14.73202,-7.56835], "fy":[-27.71186,-29.20825,-34.97185,-34.67395]}, + {"t":0.88, "x":3.41708, "y":6.17879, "heading":-1.51606, "vx":-3.10784, "vy":1.10245, "omega":0.56902, "ax":-0.83909, "ay":-2.39354, "alpha":2.66214, "fx":[-7.4844,-21.03977,-20.23717,-7.18745], "fy":[-33.97553,-33.18158,-46.56397,-45.87534]}, + {"t":0.8941, "x":3.3732, "y":6.19409, "heading":-1.50804, "vx":-3.11967, "vy":1.06872, "omega":0.60653, "ax":-0.79646, "ay":-2.82229, "alpha":5.489, "fx":[-0.00697,-28.32004,-26.34325,1.56408], "fy":[-32.60336,-36.30346,-60.44252,-58.83571]}, + {"t":0.90819, "x":3.32916, "y":6.20888, "heading":-1.49949, "vx":-3.13089, "vy":1.02895, "omega":0.68389, "ax":-1.05961, "ay":-2.8627, "alpha":6.07446, "fx":[-3.63706,-34.03585,-31.53179,-1.44802], "fy":[-31.23004,-35.51046,-62.93033,-61.20839]}, + {"t":0.92514, "x":3.27593, "y":6.22591, "heading":-1.4879, "vx":-3.14885, "vy":0.98042, "omega":0.78686, "ax":-0.76184, "ay":-2.49413, "alpha":1.23739, "fx":[-9.88129,-16.29966,-15.73889,-8.87807], "fy":[-39.56593,-38.15801,-44.54369,-44.03591]}, + {"t":0.94209, "x":3.22245, "y":6.24217, "heading":-1.47456, "vx":-3.16177, "vy":0.93815, "omega":0.80783, "ax":-0.49013, "ay":-1.68531, "alpha":0.20081, "fx":[-7.613,-8.84876,-8.72787,-7.49122], "fy":[-26.99988,-28.55046,-28.47659,-28.34607]}, + {"t":0.95904, "x":3.16879, "y":6.25783, "heading":-1.46087, "vx":-3.17007, "vy":0.90958, "omega":0.81123, "ax":-0.30263, "ay":-1.06107, "alpha":0.01973, "fx":[-5.04638,-5.28735,-5.26012,-4.58499], "fy":[-18.27687,-17.33971,-17.58041,-17.55329]}, + {"t":0.97599, "x":3.11501, "y":6.27309, "heading":-1.44711, "vx":-3.1752, "vy":0.89159, "omega":0.81157, "ax":-0.1028, "ay":-0.36999, "alpha":-0.00797, "fx":[-1.66767,-1.77283,-1.75971,-1.65438], "fy":[-5.98118,-6.57657,-6.06066,-6.05166]}, + {"t":0.99294, "x":3.06117, "y":6.28815, "heading":-1.43336, "vx":-3.17695, "vy":0.88532, "omega":0.81143, "ax":0.05476, "ay":0.19769, "alpha":-0.04081, "fx":[0.56638,0.91702,0.86876,1.29912], "fy":[2.92245,3.20269,3.55247,3.50421]}, + {"t":1.00989, "x":3.00733, "y":6.30319, "heading":-1.4196, "vx":-3.17602, "vy":0.88867, "omega":0.81074, "ax":0.25621, "ay":0.90641, "alpha":-0.0504, "fx":[4.15949,4.42274,4.38238,4.1191], "fy":[14.87723,15.13792,15.23477,15.18789]}, + {"t":1.02684, "x":2.95353, "y":6.31838, "heading":-1.40586, "vx":-3.17167, "vy":0.90404, "omega":0.80989, "ax":0.45362, "ay":1.56434, "alpha":-0.07387, "fx":[6.96585,7.78549,7.64821,7.84705], "fy":[26.0581,25.58665,26.39937,26.26322]}, + {"t":1.04379, "x":2.89983, "y":6.33393, "heading":-1.39213, "vx":-3.16399, "vy":0.93055, "omega":0.80864, "ax":0.72555, "ay":2.41227, "alpha":-0.08442, "fx":[11.84027,12.4614,12.35149,11.72486], "fy":[39.52051,40.84079,40.30083,40.18369]}, + {"t":1.06074, "x":2.84631, "y":6.35005, "heading":-1.37843, "vx":-3.15169, "vy":0.97144, "omega":0.8072, "ax":1.05075, "ay":3.29563, "alpha":-0.09133, "fx":[16.71195,17.98928,17.73529,17.62519], "fy":[55.46934,54.00865,55.25671,55.01178]}, + {"t":1.07769, "x":2.79304, "y":6.36699, "heading":-1.36474, "vx":-3.13388, "vy":1.02731, "omega":0.80566, "ax":1.45184, "ay":4.27092, "alpha":-0.11199, "fx":[23.81703,24.78794,24.60067,23.59999], "fy":[69.99137,72.60443,71.19019,70.99051]}, + {"t":1.09464, "x":2.74012, "y":6.38501, "heading":-1.35109, "vx":-3.10927, "vy":1.0997, "omega":0.80376, "ax":1.8689, "ay":5.0532, "alpha":-0.08194, "fx":[30.18836,31.81614,31.43939,31.17068], "fy":[85.35298,82.9492,84.49214,84.14334]}, + {"t":1.11159, "x":2.68769, "y":6.40438, "heading":-1.33746, "vx":-3.07759, "vy":1.18535, "omega":0.80237, "ax":2.10056, "ay":5.2247, "alpha":-0.09789, "fx":[34.64185,35.59098,35.43608,34.39236], "fy":[85.73198,88.92607,86.97422,86.74051]}, + {"t":1.12855, "x":2.63582, "y":6.42522, "heading":-1.32386, "vx":-3.04198, "vy":1.27392, "omega":0.80071, "ax":2.29733, "ay":5.26287, "alpha":-0.0249, "fx":[37.61924,38.85021,38.55931,38.15313], "fy":[89.08147,86.6507,87.75772,87.4285]}, + {"t":1.1455, "x":2.58459, "y":6.44757, "heading":-1.31029, "vx":-3.00304, "vy":1.36312, "omega":0.80029, "ax":2.48265, "ay":5.25491, "alpha":0.171, "fx":[41.40404,41.12217,41.4626,41.54973], "fy":[86.67064,90.52084,86.63547,86.56058]}, + {"t":1.16245, "x":2.53404, "y":6.47143, "heading":-1.29673, "vx":-2.96096, "vy":1.4522, "omega":0.80319, "ax":2.68382, "ay":5.20641, "alpha":0.41383, "fx":[44.43064,43.99594,44.2967,46.22878], "fy":[89.48089,86.38368,85.69549,85.59309]}, + {"t":1.1794, "x":2.48424, "y":6.4968, "heading":-1.28311, "vx":-2.91547, "vy":1.54045, "omega":0.8102, "ax":3.74261, "ay":4.54875, "alpha":0.60164, "fx":[63.05762,60.72323,61.80396,63.96518], "fy":[76.61839,78.8615,73.66507,74.15696]}, + {"t":1.19635, "x":2.43536, "y":6.52356, "heading":-1.26938, "vx":-2.85203, "vy":1.61755, "omega":0.8204, "ax":5.44191, "ay":2.35406, "alpha":0.77137, "fx":[90.77698,88.42732,89.344,94.30796], "fy":[41.53925,40.18252,37.39641,37.8457]}, + {"t":1.2133, "x":2.3878, "y":6.55132, "heading":-1.25547, "vx":-2.75978, "vy":1.65745, "omega":0.83347, "ax":5.86448, "ay":1.02369, "alpha":0.59289, "fx":[98.51656,96.05542,97.01505,99.4451], "fy":[18.78327,18.48571,15.08743,15.90151]}, + {"t":1.23025, "x":2.34186, "y":6.57956, "heading":-1.24134, "vx":-2.66038, "vy":1.67481, "omega":0.84352, "ax":5.95671, "ay":0.32977, "alpha":0.50074, "fx":[99.38325,97.64393,98.28757,101.86749], "fy":[6.50205,6.34431,4.33275,4.80918]}, + {"t":1.2472, "x":2.29762, "y":6.608, "heading":-1.22705, "vx":-2.55941, "vy":1.6804, "omega":0.85201, "ax":5.97282, "ay":-0.07752, "alpha":0.31775, "fx":[99.99501,98.59162,99.12921,100.54009], "fy":[0.05034,-1.29214,-2.25316,-1.67413]}, + {"t":1.26415, "x":2.2551, "y":6.63647, "heading":-1.2126, "vx":-2.45817, "vy":1.67908, "omega":0.8574, "ax":5.96841, "ay":-0.34168, "alpha":0.2308, "fx":[99.53395,98.55728,98.93021,100.94054], "fy":[-5.71799,-5.03847,-6.19068,-5.83512]}, + {"t":1.2811, "x":2.21429, "y":6.66488, "heading":-1.19807, "vx":-2.357, "vy":1.67329, "omega":0.86131, "ax":5.95841, "ay":-0.52586, "alpha":0.15783, "fx":[99.5428,98.81077,99.10182,99.84015], "fy":[-7.84151,-9.23401,-9.17849,-8.80933]}, + {"t":1.29805, "x":2.17519, "y":6.69317, "heading":-1.18347, "vx":-2.256, "vy":1.66438, "omega":0.86399, "ax":5.94776, "ay":-0.65818, "alpha":0.13195, "fx":[99.15075,98.58254,98.81296,100.03886], "fy":[-11.08672,-10.5394,-11.27139,-10.98835]}, + {"t":1.315, "x":2.1378, "y":6.72129, "heading":-1.16883, "vx":-2.15518, "vy":1.65322, "omega":0.86622, "ax":5.93724, "ay":-0.76229, "alpha":0.08228, "fx":[99.10541,98.64081,98.83909,99.29813], "fy":[-11.8622,-13.66816,-12.78661,-12.51077]}, + {"t":1.33195, "x":2.10212, "y":6.7492, "heading":-1.15414, "vx":-2.05454, "vy":1.6403, "omega":0.86762, "ax":5.92756, "ay":-0.84492, "alpha":0.04839, "fx":[98.83066,98.39897,98.58859,99.41993], "fy":[-14.69309,-13.57676,-14.16851,-13.89947]}, + {"t":1.3489, "x":2.06815, "y":6.77688, "heading":-1.13944, "vx":-1.95407, "vy":1.62598, "omega":0.86844, "ax":5.91925, "ay":-0.90929, "alpha":-0.01244, "fx":[98.70154,98.58716,98.64718,98.7486], "fy":[-14.85435,-15.99255,-14.90038,-14.8825]}, + {"t":1.36585, "x":2.03588, "y":6.80431, "heading":-1.12472, "vx":-1.85374, "vy":1.61057, "omega":0.86823, "ax":5.91177, "ay":-0.96299, "alpha":-0.47989, "fx":[97.98758,99.40872,98.81293,97.97622], "fy":[-18.83222,-16.30511,-13.96953,-15.10344]}, + {"t":1.3828, "x":2.00531, "y":6.83148, "heading":-1.11, "vx":-1.75353, "vy":1.59424, "omega":0.86009, "ax":5.90483, "ay":-1.00967, "alpha":-2.57468, "fx":[96.11294,104.63794,100.92472,92.047], "fy":[-26.93624,-20.91792,-6.34689,-13.1218]}, + {"t":1.39975, "x":1.97643, "y":6.85835, "heading":-1.09542, "vx":-1.65344, "vy":1.57713, "omega":0.81645, "ax":5.89888, "ay":-1.04795, "alpha":-10.12355, "fx":[89.70279,122.99389,108.97245,71.65666], "fy":[-59.8,-27.81605,21.39074,-3.64986]}, + {"t":1.4167, "x":1.94925, "y":6.88494, "heading":-1.08158, "vx":-1.55345, "vy":1.55936, "omega":0.64485, "ax":5.80917, "ay":-1.45926, "alpha":-2.24118, "fx":[94.60118,102.60604,99.23946,90.89789], "fy":[-32.75799,-27.43698,-15.32897,-21.77666]}, + {"t":1.44386, "x":1.90921, "y":6.92675, "heading":-1.06407, "vx":-1.39568, "vy":1.51973, "omega":0.58399, "ax":5.58205, "ay":-2.18664, "alpha":-1.83041, "fx":[90.73269,97.66484,95.09853,88.70414], "fy":[-44.40302,-37.50533,-29.18633,-34.70599]}, + {"t":1.47102, "x":1.87336, "y":6.96721, "heading":-1.04821, "vx":-1.24409, "vy":1.46035, "omega":0.53428, "ax":5.29472, "ay":-2.81244, "alpha":-1.5126, "fx":[86.36118,92.23618,90.33497,84.1097], "fy":[-52.47574,-48.39418,-40.76609,-45.89192]}, + {"t":1.49818, "x":1.84153, "y":7.00584, "heading":-1.0337, "vx":-1.1003, "vy":1.38397, "omega":0.4932, "ax":4.98184, "ay":-3.33597, "alpha":-1.35584, "fx":[80.85752,86.74981,84.83365,79.73838], "fy":[-61.3697,-55.90597,-50.41423,-54.74648]}, + {"t":1.52533, "x":1.81348, "y":7.04219, "heading":-1.0203, "vx":-0.965, "vy":1.29337, "omega":0.45637, "ax":4.66646, "ay":-3.76499, "alpha":-1.23006, "fx":[76.07379,81.28016,79.67601,74.12045], "fy":[-67.13058,-63.52598,-58.00199,-62.38365]}, + {"t":1.55249, "x":1.789, "y":7.07593, "heading":-1.00791, "vx":-0.83827, "vy":1.19112, "omega":0.42297, "ax":4.36215, "ay":-4.11412, "alpha":-1.23708, "fx":[70.58414,76.45536,74.50012,69.3199], "fy":[-73.46369,-68.54282,-64.08255,-68.23287]}, + {"t":1.57965, "x":1.76784, "y":7.10676, "heading":-0.99642, "vx":-0.7198, "vy":1.07939, "omega":0.38937, "ax":4.07752, "ay":-4.39674, "alpha":-1.23949, "fx":[66.14295,71.81811,69.92463,63.99562], "fy":[-77.54911,-73.59178,-68.7971,-73.22806]}, + {"t":1.60681, "x":1.74979, "y":7.13445, "heading":-0.98585, "vx":-0.60907, "vy":0.95999, "omega":0.35571, "ax":3.8155, "ay":-4.62627, "alpha":-1.34594, "fx":[61.42434,68.02843,65.6189,59.33862], "fy":[-81.89254,-76.94431,-72.46956,-77.16407]}, + {"t":1.63397, "x":1.73466, "y":7.15882, "heading":-0.97619, "vx":-0.50545, "vy":0.83435, "omega":0.31916, "ax":3.57707, "ay":-4.81327, "alpha":-1.33645, "fx":[58.14235,64.27154,61.661,54.43718], "fy":[-83.96646,-80.51629,-75.67875,-80.7782]}, + {"t":1.66112, "x":1.72225, "y":7.1797, "heading":-0.96752, "vx":-0.4083, "vy":0.70363, "omega":0.28286, "ax":3.36114, "ay":-4.9667, "alpha":-1.55461, "fx":[53.95973,60.97885,58.6792,50.49649], "fy":[-87.88717,-82.83417,-77.42792,-83.02084]}, + {"t":1.68828, "x":1.7124, "y":7.19698, "heading":-0.95984, "vx":-0.31702, "vy":0.56875, "omega":0.24064, "ax":3.16605, "ay":-5.09349, "alpha":-1.80498, "fx":[50.23973,59.23132,55.42892,46.20629], "fy":[-90.57166,-84.62139,-79.14583,-85.28516]}, + {"t":1.71544, "x":1.70496, "y":7.21055, "heading":-0.9533, "vx":-0.23104, "vy":0.43042, "omega":0.19162, "ax":2.98998, "ay":-5.19899, "alpha":-2.0865, "fx":[46.63189,57.0931,53.27767,42.36319], "fy":[-93.37291,-86.29528,-79.94951,-87.04088]}, + {"t":1.7426, "x":1.69979, "y":7.22032, "heading":-0.9481, "vx":-0.14984, "vy":0.28923, "omega":0.13496, "ax":2.83069, "ay":-5.28757, "alpha":-2.31689, "fx":[44.22409,55.39793,50.67085,38.45243], "fy":[-95.09892,-88.20822,-80.70111,-88.55645]}, + {"t":1.76975, "x":1.69676, "y":7.22622, "heading":-0.94443, "vx":-0.07296, "vy":0.14563, "omega":0.07204, "ax":2.68662, "ay":-5.36235, "alpha":-2.6526, "fx":[41.27722,54.52163,48.80901,34.53099], "fy":[-97.23151,-88.98442,-81.24345,-90.0919]}, {"t":1.79691, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, diff --git a/src/main/deploy/choreo/fetchToK.traj b/src/main/deploy/choreo/fetchToK.traj index 67544eb2..c744ac96 100644 --- a/src/main/deploy/choreo/fetchToK.traj +++ b/src/main/deploy/choreo/fetchToK.traj @@ -10,7 +10,7 @@ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":3.7}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":3.3}}, "enabled":true}, {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":5.5}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":3.0}}, "enabled":true}], "targetDt":0.05 @@ -34,57 +34,58 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.12535,1.71439], + "waypoints":[0.0,1.14436,1.73324], "samples":[ - {"t":0.0, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.72302, "ay":-2.81267, "alpha":-0.77575, "fx":[77.853,81.2111,79.4868,76.37072], "fy":[-49.77171,-46.80404,-44.0297,-46.93812]}, - {"t":0.03881, "x":1.69933, "y":7.22608, "heading":-0.94248, "vx":0.18328, "vy":-0.10915, "omega":-0.0301, "ax":4.72408, "ay":-2.81355, "alpha":-0.58263, "fx":[78.08908,80.59589,79.32336,76.98426], "fy":[-49.07792,-46.84504,-44.7445,-46.9346]}, - {"t":0.07761, "x":1.71, "y":7.21973, "heading":-0.94365, "vx":0.3666, "vy":-0.21833, "omega":-0.05271, "ax":4.72387, "ay":-2.8137, "alpha":-0.41083, "fx":[78.30538,79.95576,79.20581,77.51154], "fy":[-48.45155,-46.95456,-45.31906,-46.88704]}, - {"t":0.11642, "x":1.72778, "y":7.20914, "heading":-0.94569, "vx":0.54991, "vy":-0.32751, "omega":-0.06865, "ax":4.72363, "ay":-2.81387, "alpha":-0.29755, "fx":[78.37695,79.77228,78.98059,77.83258], "fy":[-47.99057,-46.84278,-45.86177,-46.92865]}, - {"t":0.15522, "x":1.75268, "y":7.19431, "heading":-0.94836, "vx":0.73321, "vy":-0.43671, "omega":-0.0802, "ax":4.72335, "ay":-2.81408, "alpha":-0.16121, "fx":[78.56978,79.19682,78.92385,78.25335], "fy":[-47.49976,-46.99108,-46.26959,-46.87676]}, - {"t":0.19403, "x":1.78469, "y":7.17524, "heading":-0.95147, "vx":0.9165, "vy":-0.54591, "omega":-0.08646, "ax":4.72303, "ay":-2.81431, "alpha":-0.08458, "fx":[78.58505,79.17082,78.70753,78.45883], "fy":[-47.17465,-46.83404,-46.70888,-46.935]}, - {"t":0.23283, "x":1.82381, "y":7.15194, "heading":-0.95482, "vx":1.09978, "vy":-0.65512, "omega":-0.08974, "ax":4.72264, "ay":-2.81458, "alpha":0.02835, "fx":[78.77231,78.61122,78.70317,78.80987], "fy":[-46.77794,-47.02644,-46.98908,-46.8776]}, - {"t":0.27164, "x":1.87004, "y":7.1244, "heading":-0.95831, "vx":1.28304, "vy":-0.76434, "omega":-0.08864, "ax":4.72218, "ay":-2.81492, "alpha":0.07378, "fx":[78.72929,78.74605,78.47658,78.91368], "fy":[-46.56521,-46.81101,-47.35959,-46.95764]}, - {"t":0.31044, "x":1.92339, "y":7.09262, "heading":-0.96174, "vx":1.46629, "vy":-0.87357, "omega":-0.08578, "ax":4.72162, "ay":-2.81531, "alpha":0.17108, "fx":[78.92902,78.14065,78.53368,79.22514], "fy":[-46.21943,-47.12771,-47.5047,-46.86772]}, - {"t":0.34925, "x":1.98384, "y":7.0566, "heading":-0.96507, "vx":1.64951, "vy":-0.98282, "omega":-0.07914, "ax":4.7209, "ay":-2.81584, "alpha":0.17617, "fx":[78.77626,78.57624,78.24382,79.18425], "fy":[-46.14795,-46.73627,-47.86006,-47.0104]}, - {"t":0.38805, "x":2.0514, "y":7.01634, "heading":-0.96814, "vx":1.83271, "vy":-1.09209, "omega":-0.0723, "ax":4.72, "ay":-2.81648, "alpha":0.26712, "fx":[79.02375,77.79298,78.40735,79.49638], "fy":[-45.84918,-47.23714,-47.84209,-46.86882]}, - {"t":0.42686, "x":2.12608, "y":6.97184, "heading":-0.97095, "vx":2.01587, "vy":-1.20139, "omega":-0.06194, "ax":4.71876, "ay":-2.81738, "alpha":0.23375, "fx":[78.77265,78.46668,78.08541,79.31314], "fy":[-45.91934,-46.71857,-48.15532,-47.06458]}, - {"t":0.46566, "x":2.20786, "y":6.9231, "heading":-0.97335, "vx":2.19898, "vy":-1.31072, "omega":-0.05286, "ax":4.71703, "ay":-2.81865, "alpha":0.3143, "fx":[79.02359,77.66569,78.28392,79.54901], "fy":[-45.70396,-47.14878,-48.12166,-46.96803]}, - {"t":0.50447, "x":2.29674, "y":6.87012, "heading":-0.9754, "vx":2.38203, "vy":-1.42009, "omega":-0.04067, "ax":4.71442, "ay":-2.82056, "alpha":0.29993, "fx":[78.92578,77.66452,78.23369,79.52426], "fy":[-45.87957,-47.06817,-48.104,-47.01769]}, - {"t":0.54327, "x":2.39273, "y":6.81288, "heading":-0.97698, "vx":2.56497, "vy":-1.52955, "omega":-0.02903, "ax":4.71005, "ay":-2.82373, "alpha":0.29183, "fx":[78.84434,77.66547,78.17323,79.37432], "fy":[-46.02109,-46.86242,-48.21524,-47.18224]}, - {"t":0.58208, "x":2.49581, "y":6.7514, "heading":-0.97811, "vx":2.74775, "vy":-1.63912, "omega":-0.0177, "ax":4.7009, "ay":-2.83071, "alpha":0.17888, "fx":[78.32168,78.53519,77.71035,78.87986], "fy":[-46.24886,-46.97822,-48.23628,-47.28265]}, - {"t":0.62089, "x":2.60597, "y":6.68567, "heading":-0.9788, "vx":2.93017, "vy":-1.74897, "omega":-0.01076, "ax":4.67455, "ay":-2.84927, "alpha":0.1941, "fx":[78.18495,77.18179,77.75962,78.56361], "fy":[-46.61515,-48.00813,-48.00187,-47.35893]}, - {"t":0.65969, "x":2.7232, "y":6.61565, "heading":-0.97921, "vx":3.11157, "vy":-1.85954, "omega":-0.00323, "ax":0.19507, "ay":-3.31936, "alpha":0.09365, "fx":[3.40821,2.70219,3.30473,3.592], "fy":[-55.13828,-55.29521,-55.52909,-55.36575]}, - {"t":0.6985, "x":2.84409, "y":6.54099, "heading":-0.97934, "vx":3.11914, "vy":-1.98834, "omega":0.0004, "ax":-2.62003, "ay":-3.89227, "alpha":-0.03784, "fx":[-43.67589,-43.71377,-43.71157,-43.59706], "fy":[-63.74224,-68.03281,-63.91004,-63.84392]}, - {"t":0.7373, "x":2.96316, "y":6.4609, "heading":-0.97932, "vx":3.01746, "vy":-2.13939, "omega":-0.00107, "ax":-4.6758, "ay":-2.51312, "alpha":-0.01064, "fx":[-77.39407,-79.26569,-77.18856,-77.92536], "fy":[-42.61429,-41.99173,-41.21205,-41.75217]}, - {"t":0.77611, "x":3.07673, "y":6.37599, "heading":-0.97937, "vx":2.83602, "vy":-2.23691, "omega":-0.00148, "ax":-4.99951, "ay":2.23479, "alpha":-0.18211, "fx":[-83.53122,-82.71279,-83.18315,-83.93022], "fy":[35.96465,38.59674,37.5734,36.87654]}, - {"t":0.81491, "x":3.18302, "y":6.29087, "heading":-0.97942, "vx":2.64201, "vy":-2.15019, "omega":-0.00854, "ax":-4.87815, "ay":2.51392, "alpha":-0.24882, "fx":[-81.53656,-80.65398,-81.02645,-82.04873], "fy":[40.93543,41.79837,42.89522,41.99429]}, - {"t":0.85372, "x":3.28187, "y":6.20932, "heading":-0.97975, "vx":2.45271, "vy":-2.05263, "omega":-0.0182, "ax":-4.83132, "ay":2.61122, "alpha":-0.28522, "fx":[-80.85518,-79.68992,-80.23404,-81.36412], "fy":[42.17513,44.00245,44.49405,43.43973]}, - {"t":0.89252, "x":3.37341, "y":6.13164, "heading":-0.98046, "vx":2.26523, "vy":-1.9513, "omega":-0.02927, "ax":-4.80691, "ay":2.66009, "alpha":-0.28655, "fx":[-80.38365,-79.47498,-79.71873,-80.93791], "fy":[43.18787,44.16696,45.5606,44.45443]}, - {"t":0.93133, "x":3.45769, "y":6.05792, "heading":-0.9816, "vx":2.0787, "vy":-1.84808, "omega":-0.04039, "ax":-4.79183, "ay":2.68965, "alpha":-0.30903, "fx":[-80.25976,-78.93568,-79.54765,-80.76711], "fy":[43.37267,45.37841,45.8713,44.71839]}, - {"t":0.97013, "x":3.53475, "y":5.98823, "heading":-0.98316, "vx":1.89275, "vy":-1.7437, "omega":-0.05238, "ax":-4.78159, "ay":2.70947, "alpha":-0.26557, "fx":[-79.94074,-79.15529,-79.27936,-80.452], "fy":[44.08871,44.97013,46.3298,45.27384]}, - {"t":1.00894, "x":3.6046, "y":5.9226, "heading":-0.9852, "vx":1.7072, "vy":-1.63856, "omega":-0.06269, "ax":-4.77418, "ay":2.72369, "alpha":-0.25514, "fx":[-79.9152,-78.73344,-79.32198,-80.36277], "fy":[44.20259,45.96694,46.18278,45.25794]}, - {"t":1.04774, "x":3.66725, "y":5.86107, "heading":-0.98763, "vx":1.52194, "vy":-1.53287, "omega":-0.07259, "ax":-4.76862, "ay":2.7343, "alpha":-0.2071, "fx":[-79.69632,-79.00791,-79.18065,-80.07741], "fy":[44.75908,45.4498,46.45403,45.65518]}, - {"t":1.08655, "x":3.72272, "y":5.80365, "heading":-0.99045, "vx":1.33689, "vy":-1.42676, "omega":-0.08062, "ax":-4.76424, "ay":2.74262, "alpha":-0.16427, "fx":[-79.63916,-78.85141,-79.251,-79.92854], "fy":[44.92573,46.15077,46.19327,45.60264]}, - {"t":1.12535, "x":3.77101, "y":5.75034, "heading":-0.99357, "vx":1.15201, "vy":-1.32034, "omega":-0.087, "ax":-2.53426, "ay":1.5939, "alpha":-0.1576, "fx":[-42.38699,-41.70952,-42.1143,-42.769], "fy":[25.86086,26.84527,27.02593,26.5463]}, - {"t":1.15636, "x":3.80551, "y":5.71018, "heading":-0.99627, "vx":1.07344, "vy":-1.27092, "omega":-0.09188, "ax":-2.42628, "ay":1.75967, "alpha":-0.14685, "fx":[-40.44925,-39.85993,-40.21671,-41.25371], "fy":[29.15245,29.15001,29.75014,29.27853]}, - {"t":1.18736, "x":3.83762, "y":5.67162, "heading":-0.99912, "vx":0.99822, "vy":-1.21637, "omega":-0.09644, "ax":-2.3271, "ay":1.88927, "alpha":-0.1241, "fx":[-38.91354,-38.35921,-38.68449,-39.20933], "fy":[30.94094,31.7179,31.84435,31.46991]}, - {"t":1.21836, "x":3.86745, "y":5.63482, "heading":-1.00211, "vx":0.92608, "vy":-1.1578, "omega":-0.10028, "ax":-2.23908, "ay":1.99313, "alpha":-0.10631, "fx":[-37.31952,-36.87913,-37.14818,-37.95064], "fy":[33.15009,33.06706,33.51972,33.16101]}, - {"t":1.24936, "x":3.89508, "y":5.59989, "heading":-1.00522, "vx":0.85666, "vy":-1.09601, "omega":-0.10358, "ax":-2.16123, "ay":2.07755, "alpha":-0.08812, "fx":[-36.11932,-35.70861,-35.94905,-36.32975], "fy":[34.22387,34.84161,34.86283,34.59876]}, - {"t":1.28036, "x":3.9206, "y":5.56691, "heading":-1.00843, "vx":0.78966, "vy":-1.0316, "omega":-0.10631, "ax":-2.09235, "ay":2.14713, "alpha":-0.06552, "fx":[-34.85351,-34.58128,-34.75086,-35.32826], "fy":[35.81429,35.6711,35.95763,35.72338]}, - {"t":1.31136, "x":3.94408, "y":5.53596, "heading":-1.01173, "vx":0.7248, "vy":-0.96504, "omega":-0.10834, "ax":-2.03126, "ay":2.2052, "alpha":-0.04342, "fx":[-33.91193,-33.68845,-33.82266,-34.01747], "fy":[36.5195,36.96626,36.8428,36.70995]}, - {"t":1.34237, "x":3.96557, "y":5.5071, "heading":-1.01508, "vx":0.66182, "vy":-0.89667, "omega":-0.10969, "ax":-1.97689, "ay":2.25424, "alpha":-0.01632, "fx":[-32.88878,-32.83354,-32.87992,-33.21273], "fy":[37.73008,37.50074,37.5806,37.49666]}, - {"t":1.37337, "x":3.98514, "y":5.48038, "heading":-1.01848, "vx":0.60054, "vy":-0.82679, "omega":-0.11019, "ax":-1.9283, "ay":2.29608, "alpha":0.01427, "fx":[-32.13859,-32.16545,-32.16142,-32.10977], "fy":[38.24643,38.47846,38.17114,38.20192]}, - {"t":1.40437, "x":4.00283, "y":5.45585, "heading":-1.0219, "vx":0.54076, "vy":-0.7556, "omega":-0.10975, "ax":-1.8847, "ay":2.33212, "alpha":0.05267, "fx":[-31.29867,-31.53351,-31.42225,-31.41358], "fy":[39.23427,38.84542,38.65714,38.76453]}, - {"t":1.43537, "x":4.01869, "y":5.43355, "heading":-1.0253, "vx":0.48233, "vy":-0.6833, "omega":-0.10812, "ax":-1.84541, "ay":2.36344, "alpha":0.09094, "fx":[-30.67826,-31.03956,-30.85716,-30.47333], "fy":[39.63782,39.62648,39.04198,39.28342]}, - {"t":1.46637, "x":4.03276, "y":5.4135, "heading":-1.02865, "vx":0.42512, "vy":-0.61003, "omega":-0.1053, "ax":-1.80986, "ay":2.39087, "alpha":0.14252, "fx":[-29.96725,-30.59441,-30.27594,-29.84037], "fy":[40.48748,39.8904,39.34307,39.69759]}, - {"t":1.49737, "x":4.04507, "y":5.39574, "heading":-1.03192, "vx":0.36901, "vy":-0.53591, "omega":-0.10088, "ax":-1.77757, "ay":2.41506, "alpha":0.19477, "fx":[-29.43776,-30.25403,-29.83576,-28.9973], "fy":[40.85205,40.54195,39.55874,40.07875]}, - {"t":1.52838, "x":4.05565, "y":5.38029, "heading":-1.03505, "vx":0.3139, "vy":-0.46104, "omega":-0.09484, "ax":-1.74813, "ay":2.43653, "alpha":0.26463, "fx":[-28.81344,-29.97955,-29.38363,-28.38509], "fy":[41.63473,40.74088,39.70327,40.3844]}, - {"t":1.55938, "x":4.06454, "y":5.36716, "heading":-1.03799, "vx":0.25971, "vy":-0.3855, "omega":-0.08664, "ax":-1.72119, "ay":2.45571, "alpha":0.3374, "fx":[-28.34361,-29.78877,-29.05093,-27.58215], "fy":[42.01146,41.30239,39.76558,40.66231]}, - {"t":1.59038, "x":4.07177, "y":5.35639, "heading":-1.04067, "vx":0.20635, "vy":-0.30937, "omega":-0.07618, "ax":-1.69646, "ay":2.47292, "alpha":0.43418, "fx":[-27.77259,-29.69038,-28.71454,-26.93892], "fy":[42.7838,41.47136,39.75376,40.88053]}, - {"t":1.62138, "x":4.07735, "y":5.34799, "heading":-1.04303, "vx":0.15375, "vy":-0.23271, "omega":-0.06272, "ax":-1.67368, "ay":2.48845, "alpha":0.53651, "fx":[-27.3338,-29.65839,-28.47998,-26.12558], "fy":[43.22371,41.97614,39.65375,41.07129]}, - {"t":1.65238, "x":4.08131, "y":5.34197, "heading":-1.04498, "vx":0.10187, "vy":-0.15556, "omega":-0.04609, "ax":-1.65264, "ay":2.50252, "alpha":0.66491, "fx":[-26.8425,-29.73413,-28.27305,-25.34535], "fy":[43.88618,42.29614,39.4642,41.21677]}, - {"t":1.68339, "x":4.08368, "y":5.33835, "heading":-1.04641, "vx":0.05063, "vy":-0.07798, "omega":-0.02547, "ax":-1.63316, "ay":2.51533, "alpha":0.82163, "fx":[-26.31721,-30.05191,-28.0959,-24.43075], "fy":[44.5907,42.47974,39.26452,41.38234]}, - {"t":1.71439, "x":4.08446, "y":5.33714, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.669, "ay":-2.9013, "alpha":-0.74051, "fx":[76.99915,80.12854,78.60604,75.58611], "fy":[-51.14568,-48.29719,-45.59573,-48.41444]}, + {"t":0.03815, "x":1.69917, "y":7.22609, "heading":-0.94248, "vx":0.1781, "vy":-0.11067, "omega":-0.02825, "ax":4.67016, "ay":-2.90206, "alpha":-0.56106, "fx":[77.22128,79.57384,78.44619,76.1559], "fy":[-50.49411,-48.33124,-46.26806,-48.41064]}, + {"t":0.07629, "x":1.70936, "y":7.21976, "heading":-0.94356, "vx":0.35625, "vy":-0.22137, "omega":-0.04965, "ax":4.67003, "ay":-2.90204, "alpha":-0.40706, "fx":[77.40314,79.05332,78.30672,76.62552], "fy":[-49.89911,-48.43636,-46.8044,-48.36227]}, + {"t":0.11444, "x":1.72635, "y":7.2092, "heading":-0.94545, "vx":0.53439, "vy":-0.33207, "omega":-0.06518, "ax":4.66988, "ay":-2.902, "alpha":-0.29069, "fx":[77.50352,78.78699,78.12444,76.96374], "fy":[-49.46065,-48.32673,-47.31663,-48.39601]}, + {"t":0.15258, "x":1.75013, "y":7.19442, "heading":-0.94794, "vx":0.71252, "vy":-0.44277, "omega":-0.07626, "ax":4.6697, "ay":-2.90196, "alpha":-0.17086, "fx":[77.65847,78.33839,78.03864,77.33143], "fy":[-48.99553,-48.45917,-47.69853,-48.34421]}, + {"t":0.19073, "x":1.78071, "y":7.17542, "heading":-0.95084, "vx":0.89065, "vy":-0.55347, "omega":-0.08278, "ax":4.66949, "ay":-2.90192, "alpha":-0.08652, "fx":[77.71789,78.18639,77.8793,77.56918], "fy":[-48.67808,-48.32543,-48.10353,-48.38737]}, + {"t":0.22887, "x":1.81808, "y":7.1522, "heading":-0.954, "vx":1.06877, "vy":-0.66416, "omega":-0.08608, "ax":4.66923, "ay":-2.90186, "alpha":0.00892, "fx":[77.85982,77.77303,77.83633,77.86622], "fy":[-48.29007,-48.52996,-48.35372,-48.31701]}, + {"t":0.26702, "x":1.86224, "y":7.12475, "heading":-0.95729, "vx":1.24688, "vy":-0.77485, "omega":-0.08574, "ax":4.66891, "ay":-2.90179, "alpha":0.06501, "fx":[77.86941,77.76094,77.67437,78.00908], "fy":[-48.09,-48.3025,-48.70138,-48.39217]}, + {"t":0.30516, "x":1.9132, "y":7.09308, "heading":-0.96056, "vx":1.42498, "vy":-0.88554, "omega":-0.08326, "ax":4.66849, "ay":-2.90171, "alpha":0.1421, "fx":[78.00379,77.35876,77.67133,78.252], "fy":[-47.77499,-48.53961,-48.8506,-48.31504]}, + {"t":0.34331, "x":1.97096, "y":7.05719, "heading":-0.96373, "vx":1.60306, "vy":-0.99623, "omega":-0.07784, "ax":4.66793, "ay":-2.90158, "alpha":0.1669, "fx":[77.94324,77.53443,77.48414,78.2871], "fy":[-47.67631,-48.23567,-49.14479,-48.41529]}, + {"t":0.38145, "x":2.0355, "y":7.01708, "heading":-0.9667, "vx":1.78112, "vy":-1.10691, "omega":-0.07148, "ax":4.66716, "ay":-2.90142, "alpha":0.23177, "fx":[78.09417,77.04554,77.553,78.50435], "fy":[-47.40874,-48.60275,-49.15291,-48.29638]}, + {"t":0.4196, "x":2.10684, "y":6.97275, "heading":-0.96943, "vx":1.95915, "vy":-1.21759, "omega":-0.06263, "ax":4.66599, "ay":-2.90116, "alpha":0.22353, "fx":[77.95369,77.40229,77.34658,78.41698], "fy":[-47.43056,-48.19111,-49.39038,-48.43145]}, + {"t":0.45774, "x":2.18497, "y":6.92419, "heading":-0.97182, "vx":2.13713, "vy":-1.32825, "omega":-0.05411, "ax":4.66404, "ay":-2.90076, "alpha":0.2712, "fx":[78.08337,76.91788,77.43956,78.54814], "fy":[-47.25515,-48.49264,-49.33521,-48.33408]}, + {"t":0.49589, "x":2.26988, "y":6.87141, "heading":-0.97388, "vx":2.31505, "vy":-1.4389, "omega":-0.04376, "ax":4.66024, "ay":-2.89978, "alpha":0.26116, "fx":[77.9904,76.86525,77.39548,78.48492], "fy":[-47.34101,-48.39277,-49.28111,-48.33694]}, + {"t":0.53404, "x":2.36158, "y":6.81442, "heading":-0.97555, "vx":2.49281, "vy":-1.54952, "omega":-0.0338, "ax":4.64894, "ay":-2.89686, "alpha":0.23453, "fx":[77.77651,76.75632,77.23622,78.21341], "fy":[-47.30143,-48.52758,-49.08541,-48.24241]}, + {"t":0.57218, "x":2.46005, "y":6.7532, "heading":-0.97684, "vx":2.67015, "vy":-1.66002, "omega":-0.02485, "ax":3.48952, "ay":-2.08437, "alpha":0.2106, "fx":[58.4089,57.37542,58.03378,58.8562], "fy":[-34.04722,-34.70399,-35.43021,-34.80031]}, + {"t":0.61033, "x":2.56444, "y":6.68836, "heading":-0.97779, "vx":2.80326, "vy":-1.73953, "omega":-0.01682, "ax":0.06844, "ay":0.10963, "alpha":0.1041, "fx":[1.21534,0.76143,1.06655,1.52004], "fy":[2.43074,1.29682,1.62953,1.95269]}, + {"t":0.64847, "x":2.67143, "y":6.62209, "heading":-0.97843, "vx":2.80587, "vy":-1.73535, "omega":-0.01285, "ax":-0.03023, "ay":-0.04885, "alpha":-0.00015, "fx":[-0.68937,-0.04331,-0.77839,-0.50451], "fy":[-0.60104,-0.77368,-1.02664,-0.85595]}, + {"t":0.68662, "x":2.77843, "y":6.55586, "heading":-0.97892, "vx":2.80471, "vy":-1.73721, "omega":-0.01286, "ax":-0.04615, "ay":-0.07439, "alpha":0.02126, "fx":[-0.76223,-0.8073,-0.7774,-0.73018], "fy":[-1.36035,-0.74067,-1.447,-1.41236]}, + {"t":0.72476, "x":2.88539, "y":6.48954, "heading":-0.97941, "vx":2.80295, "vy":-1.74005, "omega":-0.01205, "ax":-0.47978, "ay":-0.76397, "alpha":-0.03181, "fx":[-8.01979,-7.88217,-7.9776,-8.11145], "fy":[-12.83811,-12.75024,-12.63593,-12.71581]}, + {"t":0.76291, "x":2.99196, "y":6.4226, "heading":-0.97987, "vx":2.78465, "vy":-1.76919, "omega":-0.01326, "ax":-1.36319, "ay":-2.0789, "alpha":-0.03456, "fx":[-22.73756,-22.60098,-22.68352,-22.8728], "fy":[-35.04932,-34.04508,-34.69175,-34.83124]}, + {"t":0.80105, "x":3.09719, "y":6.35361, "heading":-0.98038, "vx":2.73265, "vy":-1.84849, "omega":-0.01458, "ax":-2.82444, "ay":-3.92818, "alpha":-0.01193, "fx":[-46.86539,-47.56044,-46.77117,-47.13108], "fy":[-65.74372,-65.5792,-65.20095,-65.39969]}, + {"t":0.8392, "x":3.19937, "y":6.28024, "heading":-0.98093, "vx":2.62491, "vy":-1.99833, "omega":-0.01503, "ax":-5.24313, "ay":-1.17749, "alpha":-0.21225, "fx":[-87.52224,-86.84037,-87.29987,-87.93938], "fy":[-19.91948,-21.05425,-18.47634,-19.06252]}, + {"t":0.87734, "x":3.29569, "y":6.20315, "heading":-0.98151, "vx":2.42491, "vy":-2.04325, "omega":-0.02313, "ax":-4.95915, "ay":2.33176, "alpha":-0.22657, "fx":[-82.86083,-82.09957,-82.38807,-83.31814], "fy":[37.94375,38.805,39.7862,38.94263]}, + {"t":0.91549, "x":3.38458, "y":6.12691, "heading":-0.98239, "vx":2.23574, "vy":-1.9543, "omega":-0.03177, "ax":-4.83391, "ay":2.60127, "alpha":-0.25959, "fx":[-80.83046,-79.95529,-80.23875,-81.29136], "fy":[42.3775,43.07457,44.4825,43.51288]}, + {"t":0.95364, "x":3.46634, "y":6.05425, "heading":-0.9836, "vx":2.05135, "vy":-1.85508, "omega":-0.04167, "ax":-4.78479, "ay":2.69749, "alpha":-0.25924, "fx":[-80.01008,-79.14973,-79.39031,-80.49025], "fy":[43.93886,44.79658,46.0624,45.06556]}, + {"t":0.99178, "x":3.54111, "y":5.98545, "heading":-0.98519, "vx":1.86883, "vy":-1.75218, "omega":-0.05156, "ax":-4.75861, "ay":2.74691, "alpha":-0.2649, "fx":[-79.65367,-78.5316,-79.02035,-80.08963], "fy":[44.60732,46.09945,46.71852,45.73307]}, + {"t":1.02993, "x":3.60894, "y":5.92061, "heading":-0.98716, "vx":1.68732, "vy":-1.6474, "omega":-0.06167, "ax":-4.74237, "ay":2.77697, "alpha":-0.23304, "fx":[-79.30656,-78.44893,-78.73878,-79.71779], "fy":[45.38784,46.1711,47.24014,46.36419]}, + {"t":1.06807, "x":3.66985, "y":5.85979, "heading":-0.98951, "vx":1.50642, "vy":-1.54147, "omega":-0.07056, "ax":-4.73129, "ay":2.79723, "alpha":-0.20168, "fx":[-79.1264,-78.23063,-78.64036,-79.47582], "fy":[45.73479,46.91615,47.29921,46.56344]}, + {"t":1.10622, "x":3.72387, "y":5.80303, "heading":-0.9922, "vx":1.32594, "vy":-1.43477, "omega":-0.07825, "ax":-4.72326, "ay":2.81177, "alpha":-0.15767, "fx":[-78.91367,-78.31409,-78.52413,-79.18597], "fy":[46.26807,46.78806,47.50576,46.92172]}, + {"t":1.14436, "x":3.77101, "y":5.75034, "heading":-0.99518, "vx":1.14577, "vy":-1.32751, "omega":-0.08426, "ax":-2.50588, "ay":1.63694, "alpha":-0.15553, "fx":[-41.9141,-41.24427,-41.63986,-42.28896], "fy":[26.62255,27.48749,27.75319,27.28506]}, + {"t":1.17536, "x":3.80532, "y":5.70999, "heading":-0.9978, "vx":1.0681, "vy":-1.27678, "omega":-0.08908, "ax":-2.3922, "ay":1.8053, "alpha":-0.13624, "fx":[-39.9254,-39.34975,-39.69363,-40.53873], "fy":[29.86211,29.93276,30.51476,30.06451]}, + {"t":1.20635, "x":3.83728, "y":5.67128, "heading":-1.00056, "vx":0.99396, "vy":-1.22082, "omega":-0.09331, "ax":-2.29111, "ay":1.93247, "alpha":-0.12256, "fx":[-38.31235,-37.76734,-38.0834,-38.60353], "fy":[31.69979,32.3667,32.57577,32.21116]}, + {"t":1.23734, "x":3.86698, "y":5.63437, "heading":-1.00345, "vx":0.92295, "vy":-1.16093, "omega":-0.0971, "ax":-2.20392, "ay":2.03173, "alpha":-0.10653, "fx":[-36.76431,-36.32504,-36.58445,-37.27915], "fy":[33.69695,33.74888,34.18595,33.83985]}, + {"t":1.26834, "x":3.89453, "y":5.59937, "heading":-1.00646, "vx":0.85464, "vy":-1.09796, "omega":-0.10041, "ax":-2.12865, "ay":2.11077, "alpha":-0.08716, "fx":[-35.57428,-35.17343,-35.40495,-35.78135], "fy":[34.81148,35.32617,35.43043,35.17372]}, + {"t":1.29933, "x":3.91999, "y":5.56635, "heading":-1.00957, "vx":0.78867, "vy":-1.03254, "omega":-0.10311, "ax":-2.06339, "ay":2.17485, "alpha":-0.06199, "fx":[-34.40208,-34.13165,-34.29624,-34.75303], "fy":[36.21545,36.15438,36.4326,36.21254]}, + {"t":1.33032, "x":3.94345, "y":5.53539, "heading":-1.01277, "vx":0.72472, "vy":-0.96513, "omega":-0.10503, "ax":-2.00651, "ay":2.22765, "alpha":-0.04399, "fx":[-33.49706,-33.28333,-33.40784,-33.60177], "fy":[36.927,37.25365,37.2426,37.1125]}, + {"t":1.36132, "x":3.96495, "y":5.50655, "heading":-1.01602, "vx":0.66253, "vy":-0.89609, "omega":-0.10639, "ax":-1.95661, "ay":2.27178, "alpha":-0.01472, "fx":[-32.58049,-32.51963,-32.56577,-32.79736], "fy":[37.96352,37.81071,37.88988,37.81398]}, + {"t":1.39231, "x":3.98454, "y":5.47987, "heading":-1.01932, "vx":0.60189, "vy":-0.82568, "omega":-0.10685, "ax":-1.91259, "ay":2.30913, "alpha":0.01249, "fx":[-31.87597,-31.90501,-31.89746,-31.84914], "fy":[38.48193,38.63346,38.41196,38.44068]}, + {"t":1.4233, "x":4.00228, "y":5.45539, "heading":-1.02263, "vx":0.54261, "vy":-0.75411, "omega":-0.10646, "ax":-1.8735, "ay":2.34108, "alpha":0.0505, "fx":[-31.13697,-31.35837,-31.25191,-31.17438], "fy":[39.31732,39.01119,38.83068,38.93953]}, + {"t":1.4543, "x":4.01819, "y":5.43314, "heading":-1.02593, "vx":0.48454, "vy":-0.68156, "omega":-0.1049, "ax":-1.83862, "ay":2.3687, "alpha":0.08737, "fx":[-30.5655,-30.92283,-30.74095,-30.36609], "fy":[39.73415,39.65435,39.15887,39.39269]}, + {"t":1.48529, "x":4.03233, "y":5.41315, "heading":-1.02918, "vx":0.42756, "vy":-0.60814, "omega":-0.10219, "ax":-1.80731, "ay":2.39277, "alpha":0.13627, "fx":[-29.94621,-30.54894,-30.24139,-29.77126], "fy":[40.44607,39.93678,39.40796,39.75466]}, + {"t":1.51628, "x":4.04471, "y":5.39545, "heading":-1.03235, "vx":0.37154, "vy":-0.53398, "omega":-0.09796, "ax":-1.77907, "ay":2.41393, "alpha":0.18864, "fx":[-29.46487,-30.2696,-29.85597,-29.03476], "fy":[40.83291,40.45769,39.58021,40.08543]}, + {"t":1.54728, "x":4.05537, "y":5.38006, "heading":-1.03538, "vx":0.3164, "vy":-0.45917, "omega":-0.09212, "ax":-1.7535, "ay":2.43266, "alpha":0.25466, "fx":[-28.92548,-30.05319,-29.47605,-28.46496], "fy":[41.48788,40.68712,39.68271,40.34712]}, + {"t":1.57827, "x":4.06434, "y":5.367, "heading":-1.03824, "vx":0.26206, "vy":-0.38377, "omega":-0.08423, "ax":-1.73022, "ay":2.44934, "alpha":0.32785, "fx":[-28.49919,-29.91982,-29.19423,-27.7548], "fy":[41.88894,41.13382,39.71101,40.58323]}, + {"t":1.60926, "x":4.07163, "y":5.35628, "heading":-1.04085, "vx":0.20843, "vy":-0.30786, "omega":-0.07406, "ax":-1.70897, "ay":2.46428, "alpha":0.41931, "fx":[-28.00593,-29.86524,-28.91968,-27.15994], "fy":[42.54642,41.33274,39.66816,40.7661]}, + {"t":1.64026, "x":4.07727, "y":5.34793, "heading":-1.04315, "vx":0.15547, "vy":-0.23148, "omega":-0.06107, "ax":-1.68948, "ay":2.47774, "alpha":0.52226, "fx":[-27.6063,-29.8885,-28.73239,-26.42439], "fy":[43.00802,41.73677,39.54305,40.92303]}, + {"t":1.67125, "x":4.08127, "y":5.34194, "heading":-1.04504, "vx":0.1031, "vy":-0.15469, "omega":-0.04488, "ax":-1.67156, "ay":2.48992, "alpha":0.64778, "fx":[-27.16964,-30.00717,-28.57516,-25.70455], "fy":[43.62545,42.02827,39.33124,41.03815]}, + {"t":1.70224, "x":4.08367, "y":5.33834, "heading":-1.04643, "vx":0.05129, "vy":-0.07751, "omega":-0.0248, "ax":-1.65502, "ay":2.50099, "alpha":0.80031, "fx":[-26.70915,-30.31519,-28.44939,-24.88006], "fy":[44.30025,42.22982,39.07544,41.15599]}, + {"t":1.73324, "x":4.08446, "y":5.33714, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/startToG.traj b/src/main/deploy/choreo/startToG.traj index 9d343805..1fbeea49 100644 --- a/src/main/deploy/choreo/startToG.traj +++ b/src/main/deploy/choreo/startToG.traj @@ -15,7 +15,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":6.1663548}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"start.x", "val":7.1}, "y":{"exp":"start.y", "val":5.07}, "heading":{"exp":"start.heading", "val":3.14159}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"G.x", "val":5.827323}, "y":{"exp":"G.y", "val":3.7209}, "heading":{"exp":"G.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, diff --git a/src/main/deploy/choreo/startToH.traj b/src/main/deploy/choreo/startToH.traj index d88a07b2..0ece48b3 100644 --- a/src/main/deploy/choreo/startToH.traj +++ b/src/main/deploy/choreo/startToH.traj @@ -15,7 +15,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":6.1663548}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"start.x", "val":7.1}, "y":{"exp":"start.y", "val":5.07}, "heading":{"exp":"start.heading", "val":3.14159}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"H.x", "val":5.827323}, "y":{"exp":"H.y", "val":4.0509}, "heading":{"exp":"H.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, diff --git a/src/main/deploy/choreo/startToI.traj b/src/main/deploy/choreo/startToI.traj index a3b6f4d6..7365b35d 100644 --- a/src/main/deploy/choreo/startToI.traj +++ b/src/main/deploy/choreo/startToI.traj @@ -17,7 +17,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":6.1663548}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"start.x", "val":7.1}, "y":{"exp":"start.y", "val":5.07}, "heading":{"exp":"start.heading", "val":3.14159}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"6.0711771011352536 m", "val":6.0711771011352536}, "y":{"exp":"5.008563137054443 m", "val":5.008563137054443}, "heading":{"exp":"I.heading", "val":4.1887902047863905}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"I.x", "val":5.422460748154253}, "y":{"exp":"I.y", "val":5.032141990263579}, "heading":{"exp":"I.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ diff --git a/src/main/deploy/choreo/startToJ.traj b/src/main/deploy/choreo/startToJ.traj index e2130820..739460bd 100644 --- a/src/main/deploy/choreo/startToJ.traj +++ b/src/main/deploy/choreo/startToJ.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1008875, "y":6.1663548, "heading":3.141592653589793, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.498868942260742, "y":5.783792495727539, "heading":-2.1630637240599624, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.1, "y":6.1663548, "heading":3.14159, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.714269161224365, "y":5.753359317779541, "heading":-2.1630637240599624, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":5.1366723649053885, "y":5.197141990263579, "heading":4.1887902047863905, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -17,8 +17,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":6.1663548}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.498868942260742 m", "val":5.498868942260742}, "y":{"exp":"5.783792495727539 m", "val":5.783792495727539}, "heading":{"exp":"-2.1630637240599624 rad", "val":-2.1630637240599624}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"midStart.x", "val":7.1}, "y":{"exp":"midStart.y", "val":6.1663548}, "heading":{"exp":"midStart.heading", "val":3.14159}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.714269161224365 m", "val":5.714269161224365}, "y":{"exp":"5.753359317779541 m", "val":5.753359317779541}, "heading":{"exp":"-2.1630637240599624 rad", "val":-2.1630637240599624}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"J.x", "val":5.1366723649053885}, "y":{"exp":"J.y", "val":5.197141990263579}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -34,62 +34,62 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.93731,1.62847], + "waypoints":[0.0,0.82419,1.55964], "samples":[ - {"t":0.0, "x":7.10089, "y":6.16635, "heading":-3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.9937, "ay":0.17023, "alpha":12.94941, "fx":[-57.0171,-57.04831,-109.14623,-109.75898], "fy":[-38.35351,44.46346,34.93277,-29.69215]}, - {"t":0.03024, "x":7.0986, "y":6.16643, "heading":-3.14159, "vx":-0.15099, "vy":0.00515, "omega":0.39154, "ax":-4.99609, "ay":0.1455, "alpha":10.7046, "fx":[-61.65965,-61.60264,-104.72136,-105.14601], "fy":[-30.99,36.15619,29.66604,-25.1308]}, - {"t":0.06047, "x":7.09176, "y":6.16665, "heading":-3.12975, "vx":-0.30205, "vy":0.00955, "omega":0.7152, "ax":-4.99672, "ay":0.11772, "alpha":8.70627, "fx":[-65.56727,-65.79136,-100.971,-100.84179], "fy":[-24.65868,29.52633,23.97226,-20.99034]}, - {"t":0.09071, "x":7.08034, "y":6.167, "heading":-3.10813, "vx":-0.45313, "vy":0.01311, "omega":0.97844, "ax":-4.99723, "ay":0.08661, "alpha":6.84721, "fx":[-68.81233,-70.01802,-97.54645,-96.82913], "fy":[-18.55848,22.81392,18.95923,-17.43996]}, - {"t":0.12094, "x":7.06435, "y":6.16743, "heading":-3.07855, "vx":-0.60423, "vy":0.01572, "omega":1.18547, "ax":-4.99759, "ay":0.05114, "alpha":5.19578, "fx":[-72.09472,-73.47235,-94.43222,-93.23027], "fy":[-13.81221,17.51006,13.65294,-13.94119]}, - {"t":0.15118, "x":7.0438, "y":6.16793, "heading":-3.0427, "vx":-0.75533, "vy":0.01727, "omega":1.34257, "ax":-4.99768, "ay":0.0109, "alpha":3.69147, "fx":[-74.90581,-76.80121,-91.48264,-90.04635], "fy":[-9.59475,11.96547,9.25803,-10.90172]}, - {"t":0.18142, "x":7.01868, "y":6.16846, "heading":-3.00211, "vx":-0.90644, "vy":0.0176, "omega":1.45419, "ax":-4.99739, "ay":-0.03574, "alpha":2.39871, "fx":[-77.81592,-79.24199,-88.75325,-87.40535], "fy":[-6.6269,7.30418,5.09194,-8.15226]}, - {"t":0.21165, "x":6.98899, "y":6.16897, "heading":-2.95814, "vx":-1.05754, "vy":0.01652, "omega":1.52671, "ax":-4.99651, "ay":-0.08991, "alpha":1.19445, "fx":[-80.28114,-81.58846,-86.10872,-85.17907], "fy":[-4.29287,2.47597,1.22446,-5.40291]}, - {"t":0.24189, "x":6.95473, "y":6.16943, "heading":-2.91198, "vx":-1.20861, "vy":0.0138, "omega":1.56283, "ax":-4.9947, "ay":-0.15401, "alpha":0.19279, "fx":[-82.81052,-82.98982,-83.70717,-83.52939], "fy":[-3.00424,-1.84137,-2.19188,-3.23156]}, - {"t":0.27212, "x":6.9159, "y":6.16978, "heading":-2.86472, "vx":-1.35963, "vy":0.00914, "omega":1.56866, "ax":-4.99146, "ay":-0.2306, "alpha":-0.71577, "fx":[-84.87228,-84.46986,-81.32342,-82.15534], "fy":[-2.44227,-6.25756,-5.28425,-1.39189]}, - {"t":0.30236, "x":6.87251, "y":6.16995, "heading":-2.8173, "vx":-1.51055, "vy":0.00217, "omega":1.54701, "ax":-4.98592, "ay":-0.32397, "alpha":-1.46094, "fx":[-86.92118,-84.91135,-79.26752,-81.3513], "fy":[-2.71564,-10.5002,-8.20782,-0.17836]}, - {"t":0.33259, "x":6.82456, "y":6.16987, "heading":-2.77052, "vx":-1.66131, "vy":-0.00762, "omega":1.50284, "ax":-4.97659, "ay":-0.43992, "alpha":-2.10154, "fx":[-88.41156,-85.66383,-77.16054,-80.59349], "fy":[-3.85595,-14.71466,-11.03592,0.27352]}, - {"t":0.36283, "x":6.77205, "y":6.16944, "heading":-2.72508, "vx":-1.81178, "vy":-0.02093, "omega":1.4393, "ax":-4.96078, "ay":-0.58742, "alpha":-2.59768, "fx":[-89.79739,-85.17805,-75.44951,-80.35059], "fy":[-5.81296,-19.1059,-14.09435,-0.15503]}, - {"t":0.39307, "x":6.715, "y":6.16854, "heading":-2.68156, "vx":-1.96177, "vy":-0.03869, "omega":1.36076, "ax":-4.9332, "ay":-0.78153, "alpha":-2.85974, "fx":[-89.66276,-84.45686,-74.24621,-80.57024], "fy":[-8.9981,-23.8803,-17.3753,-1.85712]}, - {"t":0.4233, "x":6.65343, "y":6.16701, "heading":-2.64042, "vx":-2.11093, "vy":-0.06232, "omega":1.27429, "ax":-4.88304, "ay":-1.04564, "alpha":-3.1747, "fx":[-90.53825,-83.5277,-71.99975,-79.52587], "fy":[-13.39102,-28.87666,-21.93575,-5.51803]}, - {"t":0.45354, "x":6.58737, "y":6.16465, "heading":-2.60189, "vx":-2.25858, "vy":-0.09393, "omega":1.1783, "ax":-4.78545, "ay":-1.42235, "alpha":-3.27135, "fx":[-89.27297,-82.34166,-69.4431,-78.02669], "fy":[-19.90638,-35.20209,-28.03889,-11.69209]}, - {"t":0.48377, "x":6.5169, "y":6.16116, "heading":-2.56626, "vx":-2.40327, "vy":-0.13694, "omega":1.07939, "ax":-4.57393, "ay":-1.99556, "alpha":-3.13476, "fx":[-85.83389,-77.21506,-66.46113,-75.47055], "fy":[-29.69925,-44.70584,-36.83716,-21.8176]}, - {"t":0.51401, "x":6.44214, "y":6.1561, "heading":-2.53363, "vx":-2.54156, "vy":-0.19728, "omega":0.98461, "ax":-4.07953, "ay":-2.86868, "alpha":-2.72006, "fx":[-76.68318,-69.5445,-58.5258,-67.26159], "fy":[-45.03881,-56.8372,-50.99832,-38.40353]}, - {"t":0.54425, "x":6.36343, "y":6.14883, "heading":-2.50386, "vx":-2.66491, "vy":-0.28401, "omega":0.90236, "ax":-2.79155, "ay":-4.12764, "alpha":-2.08333, "fx":[-53.97228,-46.76564,-39.08602,-46.31091], "fy":[-66.91054,-75.68099,-70.48095,-62.15043]}, - {"t":0.57448, "x":6.28158, "y":6.13835, "heading":-2.47657, "vx":-2.74932, "vy":-0.40882, "omega":0.83937, "ax":-0.21861, "ay":-4.97615, "alpha":-1.04024, "fx":[-7.53276,-4.37065,0.39792,-3.07083], "fy":[-82.56547,-86.05918,-83.36648,-79.80927]}, - {"t":0.60472, "x":6.19835, "y":6.12372, "heading":-2.45119, "vx":-2.75593, "vy":-0.55927, "omega":0.80792, "ax":2.23281, "ay":-4.45603, "alpha":-0.1593, "fx":[37.12889,37.00058,37.43876,37.31095], "fy":[-73.88089,-75.6129,-73.87797,-73.74769]}, - {"t":0.63495, "x":6.11604, "y":6.10477, "heading":-2.42676, "vx":-2.68842, "vy":-0.69401, "omega":0.8031, "ax":3.49076, "ay":-3.56316, "alpha":0.68442, "fx":[60.22954,59.3636,55.6713,57.49268], "fy":[-59.28731,-56.95909,-59.59605,-61.74253]}, - {"t":0.66519, "x":6.03635, "y":6.08216, "heading":-2.40248, "vx":-2.58287, "vy":-0.80174, "omega":0.8238, "ax":4.06044, "ay":-2.90212, "alpha":1.03133, "fx":[71.17048,68.31373,64.2058,67.05216], "fy":[-47.98586,-44.86379,-48.56774,-52.09032]}, - {"t":0.69542, "x":5.96011, "y":6.05659, "heading":-2.37757, "vx":-2.4601, "vy":-0.88949, "omega":0.85498, "ax":4.34818, "ay":-2.45381, "alpha":1.18752, "fx":[75.93359,73.88315,68.45082,71.66045], "fy":[-40.54955,-36.40591,-41.40308,-45.25675]}, - {"t":0.72566, "x":5.88772, "y":6.02857, "heading":-2.35172, "vx":-2.32863, "vy":-0.96368, "omega":0.89089, "ax":4.51038, "ay":-2.14402, "alpha":1.00554, "fx":[78.36517,75.57716,71.98805,74.81315], "fy":[-35.32673,-32.01402,-36.12945,-39.48907]}, - {"t":0.7559, "x":5.81937, "y":5.99846, "heading":-2.32479, "vx":-2.19225, "vy":-1.02851, "omega":0.92129, "ax":4.61172, "ay":-1.91869, "alpha":0.63308, "fx":[78.57034,77.5981,74.79266,76.5397], "fy":[-31.76034,-29.45481,-32.30041,-34.41893]}, - {"t":0.78613, "x":5.75519, "y":5.96648, "heading":-2.29693, "vx":-2.05281, "vy":-1.08652, "omega":0.94043, "ax":4.67874, "ay":-1.75084, "alpha":0.03157, "fx":[78.12786,77.95869,77.8592,78.02377], "fy":[-29.23015,-29.08837,-29.16875,-29.2552]}, - {"t":0.81637, "x":5.69527, "y":5.93283, "heading":-2.2685, "vx":-1.91135, "vy":-1.13946, "omega":0.94139, "ax":4.72643, "ay":-1.61929, "alpha":-0.79748, "fx":[76.14303,79.1467,81.10868,78.75083], "fy":[-27.66263,-29.90263,-26.42199,-23.98372]}, - {"t":0.8466, "x":5.63963, "y":5.89764, "heading":-2.24003, "vx":-1.76844, "vy":-1.18842, "omega":0.91727, "ax":4.76124, "ay":-1.51523, "alpha":-1.83121, "fx":[73.755,79.42555,84.8607,79.42906], "fy":[-27.01874,-32.094,-23.73542,-18.18461]}, - {"t":0.87684, "x":5.58834, "y":5.86101, "heading":-2.2123, "vx":-1.62448, "vy":-1.23423, "omega":0.8619, "ax":4.78777, "ay":-1.43034, "alpha":-3.15831, "fx":[69.98125,80.64614,89.09265,79.5196], "fy":[-26.93369,-35.55456,-21.18495,-11.69913]}, - {"t":0.90708, "x":5.54141, "y":5.82304, "heading":-2.18624, "vx":-1.47972, "vy":-1.27748, "omega":0.76641, "ax":4.80848, "ay":-1.36012, "alpha":-4.68682, "fx":[65.7958,81.40411,93.82603,79.59424], "fy":[-27.90818,-39.87928,-18.59356,-4.30943]}, - {"t":0.93731, "x":5.49887, "y":5.78379, "heading":-2.16306, "vx":-1.33433, "vy":-1.31861, "omega":0.6247, "ax":2.93167, "ay":-0.61079, "alpha":-4.69359, "fx":[33.5496,51.63447,63.76557,46.5288], "fy":[-14.01553,-26.61498,-6.72338,6.62785]}, - {"t":0.96873, "x":5.4584, "y":5.74207, "heading":-2.14344, "vx":-1.24223, "vy":-1.33779, "omega":0.47725, "ax":2.98955, "ay":-0.21467, "alpha":-3.62694, "fx":[37.8635,52.19657,61.2467,48.0306], "fy":[-6.41448,-16.36121,-0.7973,9.25924]}, - {"t":1.00014, "x":5.42085, "y":5.69993, "heading":-2.12844, "vx":-1.14831, "vy":-1.34454, "omega":0.3633, "ax":2.99064, "ay":0.19857, "alpha":-2.77164, "fx":[40.97855,52.02226,58.56304,47.84649], "fy":[0.91272,-6.36017,5.46032,13.22752]}, - {"t":1.03156, "x":5.38625, "y":5.65779, "heading":-2.11703, "vx":-1.05435, "vy":-1.3383, "omega":0.27623, "ax":2.93544, "ay":0.60561, "alpha":-2.19064, "fx":[41.7264,50.71421,55.76367,47.52536], "fy":[8.64176,2.25913,11.7254,17.75464]}, - {"t":1.06298, "x":5.35457, "y":5.61604, "heading":-2.10835, "vx":-0.96213, "vy":-1.31927, "omega":0.20741, "ax":2.83087, "ay":0.98501, "alpha":-1.68631, "fx":[41.78035,48.77934,52.53972,45.65754], "fy":[14.97561,10.65664,17.65424,22.39219]}, - {"t":1.09439, "x":5.32574, "y":5.57508, "heading":-2.10184, "vx":-0.8732, "vy":-1.28833, "omega":0.15443, "ax":2.69021, "ay":1.32194, "alpha":-1.35408, "fx":[40.31436,46.10365,49.09163,43.86851], "fy":[21.31421,17.18707,22.97167,26.67156]}, - {"t":1.12581, "x":5.29964, "y":5.53526, "heading":-2.09699, "vx":-0.78868, "vy":-1.2468, "omega":0.11189, "ax":2.52869, "ay":1.60976, "alpha":-1.04845, "fx":[38.73207,43.24786,45.55888,41.06946], "fy":[25.934,23.37389,27.54204,30.48552]}, - {"t":1.15722, "x":5.27611, "y":5.49689, "heading":-2.09347, "vx":-0.70924, "vy":-1.19623, "omega":0.07895, "ax":2.35951, "ay":1.84911, "alpha":-0.85403, "fx":[36.39714,40.18221,42.05168,38.69623], "fy":[30.49454,27.77049,31.36476,33.66536]}, - {"t":1.18864, "x":5.25499, "y":5.46022, "heading":-2.09099, "vx":-0.63511, "vy":-1.13813, "omega":0.05212, "ax":2.19244, "ay":2.04464, "alpha":-0.66093, "fx":[34.3433,37.2699,38.75469,35.81982], "fy":[33.49876,31.98799,34.50065,36.34535]}, - {"t":1.22006, "x":5.23612, "y":5.42547, "heading":-2.08935, "vx":-0.56624, "vy":-1.0739, "omega":0.03136, "ax":2.03336, "ay":2.2031, "alpha":-0.54238, "fx":[31.97475,34.44587,35.64759,33.51243], "fy":[36.58894,34.7862,37.0432,38.47994]}, - {"t":1.25147, "x":5.21933, "y":5.39282, "heading":-2.08837, "vx":-0.50236, "vy":-1.00469, "omega":0.01432, "ax":1.88564, "ay":2.33096, "alpha":-0.41191, "fx":[30.02369,31.8902,32.85173,30.96491], "fy":[38.46453,37.6169,39.09669,40.24566]}, - {"t":1.28289, "x":5.20448, "y":5.36241, "heading":-2.08792, "vx":-0.44312, "vy":-0.93146, "omega":0.00138, "ax":1.75023, "ay":2.43442, "alpha":-0.33596, "fx":[27.94886,29.51533,30.27568,28.96211], "fy":[40.56373,39.36703,40.75676,41.63499]}, - {"t":1.3143, "x":5.19142, "y":5.33435, "heading":-2.08787, "vx":-0.38813, "vy":-0.85498, "omega":-0.00918, "ax":1.62727, "ay":2.51841, "alpha":-0.24061, "fx":[26.27581,27.3942,27.98673,26.84642], "fy":[41.72298,41.31233,42.10324,42.78393]}, - {"t":1.34572, "x":5.18003, "y":5.30873, "heading":-2.08816, "vx":-0.33701, "vy":-0.77586, "omega":-0.01674, "ax":1.51605, "ay":2.587, "alpha":-0.18677, "fx":[24.55788,25.45435,25.88907,25.18577], "fy":[43.18555,42.42575,43.20003,43.68495]}, - {"t":1.37714, "x":5.1702, "y":5.28563, "heading":-2.08869, "vx":-0.28938, "vy":-0.69459, "omega":-0.0226, "ax":1.41564, "ay":2.64338, "alpha":-0.1102, "fx":[23.18177,23.72216,24.02465,23.46359], "fy":[43.90952,43.82035,44.09645,44.42943]}, - {"t":1.40855, "x":5.1618, "y":5.26511, "heading":-2.0894, "vx":-0.24491, "vy":-0.61154, "omega":-0.02607, "ax":1.32499, "ay":2.69006, "alpha":-0.06584, "fx":[21.79767,22.1449,22.30672,22.09862], "fy":[44.97063,44.55326,44.83492,45.00898]}, - {"t":1.43997, "x":5.15476, "y":5.24723, "heading":-2.09022, "vx":-0.20328, "vy":-0.52703, "omega":-0.02814, "ax":1.24304, "ay":2.72897, "alpha":0.00308, "fx":[20.68646,20.72267,20.76419,20.71017], "fy":[45.42688,45.61106,45.44347,45.4814]}, - {"t":1.47138, "x":5.14899, "y":5.23202, "heading":-2.0911, "vx":-0.16423, "vy":-0.4413, "omega":-0.02804, "ax":1.16884, "ay":2.76164, "alpha":0.04723, "fx":[19.59448,19.42724,19.32572,19.5883], "fy":[46.22999,46.12356,45.94954,45.83802]}, - {"t":1.5028, "x":5.14441, "y":5.21952, "heading":-2.09198, "vx":-0.12751, "vy":-0.35454, "omega":-0.02655, "ax":1.10145, "ay":2.78927, "alpha":0.11695, "fx":[18.71368,18.24275,18.01486,18.47116], "fy":[46.52323,46.97961,46.36695,46.11345]}, - {"t":1.53422, "x":5.14094, "y":5.20976, "heading":-2.09282, "vx":-0.09291, "vy":-0.26691, "omega":-0.02288, "ax":1.04014, "ay":2.81277, "alpha":0.16908, "fx":[17.88214,17.16219,16.76936,17.54118], "fy":[47.16045,47.37606,46.7145,46.29885]}, - {"t":1.56563, "x":5.13854, "y":5.20276, "heading":-2.09354, "vx":-0.06023, "vy":-0.17854, "omega":-0.01757, "ax":0.98415, "ay":2.8329, "alpha":0.24795, "fx":[17.20608,16.15328,15.61002,16.65211], "fy":[47.35474,48.12503,46.9984,46.41392]}, - {"t":1.59705, "x":5.13713, "y":5.19855, "heading":-2.09409, "vx":-0.02931, "vy":-0.08954, "omega":-0.00978, "ax":0.93294, "ay":2.85022, "alpha":0.31126, "fx":[16.62709,15.23401,14.47119,15.87451], "fy":[47.743,48.50509,47.28233,46.51683]}, - {"t":1.62847, "x":5.13667, "y":5.19714, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.1, "y":6.16635, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.96001, "ay":-0.60085, "alpha":15.01487, "fx":[-51.02253,-53.72543,-114.42839,-111.54772], "fy":[-60.0677,37.73135,27.88346,-45.61063]}, + {"t":0.02747, "x":7.09813, "y":6.16613, "heading":3.14159, "vx":-0.13627, "vy":-0.01651, "omega":0.4125, "ax":-4.95971, "ay":-0.61787, "alpha":12.74542, "fx":[-55.70887,-58.14658,-109.69296,-107.1552], "fy":[-52.14038,29.34218,22.71617,-41.11619]}, + {"t":0.05495, "x":7.09251, "y":6.16544, "heading":-3.13026, "vx":-0.27252, "vy":-0.03348, "omega":0.76266, "ax":-4.95726, "ay":-0.6364, "alpha":10.6279, "fx":[-59.7193,-62.48663,-105.47682,-102.85767], "fy":[-44.55247,22.01203,17.22469,-37.11837]}, + {"t":0.08242, "x":7.08316, "y":6.16428, "heading":-3.10931, "vx":-0.40871, "vy":-0.05097, "omega":1.05464, "ax":-4.95446, "ay":-0.65698, "alpha":8.6584, "fx":[-63.50778,-66.60246,-101.55748,-98.68597], "fy":[-37.71899,15.62786,11.69787,-33.41323]}, + {"t":0.10989, "x":7.07006, "y":6.16263, "heading":-3.08034, "vx":-0.54483, "vy":-0.06901, "omega":1.29251, "ax":-4.95123, "ay":-0.67995, "alpha":6.82455, "fx":[-67.01439,-70.41843,-97.89213,-94.81355], "fy":[-31.51813,9.75434,6.28935,-29.86314]}, + {"t":0.13736, "x":7.05322, "y":6.16048, "heading":-3.04483, "vx":-0.68085, "vy":-0.0877, "omega":1.48, "ax":-4.94748, "ay":-0.70574, "alpha":5.15215, "fx":[-70.39939,-73.75575,-94.40962,-91.32338], "fy":[-26.26022,4.36864,1.26526,-26.4312]}, + {"t":0.16484, "x":7.03265, "y":6.15781, "heading":-3.00417, "vx":-0.81677, "vy":-0.10708, "omega":1.62154, "ax":-4.94306, "ay":-0.73492, "alpha":3.63788, "fx":[-73.5933,-76.61033,-91.10245,-88.28783], "fy":[-21.88549,-0.62486,-3.37448,-23.11799]}, + {"t":0.19231, "x":7.00834, "y":6.15459, "heading":-2.95962, "vx":-0.95258, "vy":-0.12727, "omega":1.72149, "ax":-4.93782, "ay":-0.76818, "alpha":2.27197, "fx":[-76.6316,-78.93718,-87.93555,-85.73977], "fy":[-18.41748,-5.43653,-7.46148,-19.90526]}, + {"t":0.21978, "x":6.98031, "y":6.1508, "heading":-2.91232, "vx":-1.08823, "vy":-0.14838, "omega":1.78391, "ax":-4.93149, "ay":-0.80646, "alpha":1.06908, "fx":[-79.46394,-80.74784,-84.93123,-83.67954], "fy":[-15.89257,-9.85356,-11.10074,-16.92627]}, + {"t":0.24726, "x":6.94855, "y":6.14642, "heading":-2.86331, "vx":-1.22371, "vy":-0.17053, "omega":1.81328, "ax":-4.92376, "ay":-0.85094, "alpha":-0.0097, "fx":[-82.06132,-82.09958,-82.06742,-82.07881], "fy":[-14.14521,-14.24941,-14.20759,-14.13667]}, + {"t":0.27473, "x":6.91307, "y":6.14141, "heading":-2.8135, "vx":-1.35898, "vy":-0.19391, "omega":1.81301, "ax":-4.91413, "ay":-0.90331, "alpha":-0.93084, "fx":[-84.41745,-82.93622,-79.39289,-80.91822], "fy":[-13.26426,-18.24035,-16.9476,-11.77882]}, + {"t":0.3022, "x":6.87388, "y":6.13574, "heading":-2.76369, "vx":-1.49399, "vy":-0.21873, "omega":1.78744, "ax":-4.9019, "ay":-0.96572, "alpha":-1.75033, "fx":[-86.4505,-83.4455,-76.86343,-80.08953], "fy":[-13.04195,-22.28991,-19.29744,-9.76283]}, + {"t":0.32968, "x":6.83099, "y":6.12937, "heading":-2.71458, "vx":-1.62866, "vy":-0.24526, "omega":1.73935, "ax":-4.88591, "ay":-1.04158, "alpha":-2.38076, "fx":[-88.16279,-83.51529,-74.5247,-79.58021], "fy":[-13.71585,-25.51941,-21.63847,-8.57656]}, + {"t":0.35715, "x":6.7844, "y":6.12224, "heading":-2.6668, "vx":-1.76289, "vy":-0.27387, "omega":1.67394, "ax":-4.86449, "ay":-1.13512, "alpha":-2.9832, "fx":[-89.64602,-82.2383,-72.89193,-79.57865], "fy":[-14.60252,-30.32705,-23.21917,-7.53912]}, + {"t":0.38462, "x":6.73414, "y":6.11429, "heading":-2.62081, "vx":-1.89653, "vy":-0.30506, "omega":1.59199, "ax":-4.83454, "ay":-1.25397, "alpha":-3.4229, "fx":[-90.39762,-82.37738,-70.50045,-79.08206], "fy":[-16.64824,-33.50489,-25.59408,-7.86504]}, + {"t":0.41209, "x":6.68021, "y":6.10543, "heading":-2.57707, "vx":-2.02935, "vy":-0.33951, "omega":1.49795, "ax":-4.79082, "ay":-1.40875, "alpha":-3.73244, "fx":[-90.64691,-81.53472,-68.54478,-78.71619], "fy":[-19.42046,-37.18112,-28.15245,-9.17867]}, + {"t":0.43957, "x":6.62265, "y":6.09557, "heading":-2.53592, "vx":-2.16097, "vy":-0.37821, "omega":1.39541, "ax":-4.72243, "ay":-1.6197, "alpha":-3.88782, "fx":[-90.25897,-79.69138,-66.70903,-78.22309], "fy":[-23.2475,-41.22919,-31.42542,-12.09657]}, + {"t":0.46704, "x":6.5615, "y":6.08457, "heading":-2.49759, "vx":-2.29071, "vy":-0.42271, "omega":1.2886, "ax":-4.60801, "ay":-1.91683, "alpha":-3.92904, "fx":[-88.58535,-77.42802,-64.3869,-76.85326], "fy":[-28.55093,-46.31664,-35.96252,-16.98013]}, + {"t":0.49451, "x":6.49683, "y":6.07224, "heading":-2.46218, "vx":-2.4173, "vy":-0.47537, "omega":1.18066, "ax":-4.39098, "ay":-2.36683, "alpha":-3.74445, "fx":[-84.92614,-72.98008,-60.95174,-73.92438], "fy":[-36.51043,-52.80477,-43.00543,-25.4953]}, + {"t":0.52199, "x":6.42876, "y":6.05828, "heading":-2.42975, "vx":-2.53794, "vy":-0.5404, "omega":1.07778, "ax":-3.9148, "ay":-3.08483, "alpha":-3.34832, "fx":[-76.18629,-65.06197,-53.58894,-66.19418], "fy":[-49.02729,-62.86795,-54.27749,-39.51748]}, + {"t":0.54946, "x":6.35756, "y":6.04227, "heading":-2.40014, "vx":-2.64549, "vy":-0.62514, "omega":0.9858, "ax":-2.67186, "ay":-4.19986, "alpha":-2.66305, "fx":[-54.15828,-43.92578,-34.70221,-45.36807], "fy":[-68.45281,-78.63732,-71.50597,-61.4428]}, + {"t":0.57693, "x":6.28387, "y":6.02351, "heading":-2.37305, "vx":-2.71889, "vy":-0.74053, "omega":0.91263, "ax":0.11778, "ay":-4.97112, "alpha":-1.55691, "fx":[-4.09463,1.54304,8.14593,2.25871], "fy":[-82.92397,-87.43216,-82.87551,-78.2331]}, + {"t":0.60441, "x":6.20922, "y":6.00129, "heading":-2.34798, "vx":-2.71566, "vy":-0.8771, "omega":0.86986, "ax":2.95448, "ay":-4.00537, "alpha":-0.53719, "fx":[47.47514,49.10909,50.95397,49.46077], "fy":[-66.84959,-68.89828,-66.34489,-64.97781]}, + {"t":0.63188, "x":6.13572, "y":5.97568, "heading":-2.32408, "vx":-2.63449, "vy":-0.98714, "omega":0.8551, "ax":4.15488, "ay":-2.7522, "alpha":0.12697, "fx":[69.91276,68.97568,68.83319,69.31801], "fy":[-45.63513,-45.69207,-45.83938,-46.34503]}, + {"t":0.65935, "x":6.06491, "y":5.94753, "heading":-2.30059, "vx":-2.52034, "vy":-1.06275, "omega":0.85859, "ax":4.58906, "ay":-1.95464, "alpha":0.36477, "fx":[77.49892,76.67657,75.41872,76.39552], "fy":[-32.50717,-30.98059,-32.91817,-33.92598]}, + {"t":0.68682, "x":5.99741, "y":5.91759, "heading":-2.277, "vx":-2.39427, "vy":-1.11645, "omega":0.86861, "ax":4.76973, "ay":-1.46835, "alpha":0.08042, "fx":[79.67303,79.77137,79.17394,79.41812], "fy":[-24.41037,-24.17209,-24.52064,-24.80366]}, + {"t":0.7143, "x":5.93343, "y":5.88636, "heading":-2.25314, "vx":-2.26323, "vy":-1.15679, "omega":0.87082, "ax":4.85858, "ay":-1.14798, "alpha":-0.53015, "fx":[79.38456,81.01497,82.5824,80.97878], "fy":[-19.5559,-21.1138,-18.82693,-17.04851]}, + {"t":0.74177, "x":5.87308, "y":5.85415, "heading":-2.22922, "vx":-2.12975, "vy":-1.18833, "omega":0.85626, "ax":4.90702, "ay":-0.92566, "alpha":-1.51957, "fx":[77.25311,82.2437,86.14757,81.54635], "fy":[-16.56681,-21.31943,-14.35315,-9.48217]}, + {"t":0.76924, "x":5.81643, "y":5.82115, "heading":-2.20569, "vx":-1.99494, "vy":-1.21376, "omega":0.81451, "ax":4.93586, "ay":-0.76281, "alpha":-2.81552, "fx":[73.94361,83.08674,90.39308,81.68994], "fy":[-15.14212,-23.475,-10.6597,-1.58621]}, + {"t":0.79672, "x":5.76348, "y":5.78752, "heading":-2.18332, "vx":-1.85934, "vy":-1.23472, "omega":0.73716, "ax":4.95417, "ay":-0.63838, "alpha":-4.48658, "fx":[69.35113,84.47586,95.19856,81.30878], "fy":[-14.77268,-27.84971,-7.09459,7.15113]}, + {"t":0.82419, "x":5.71427, "y":5.75336, "heading":-2.16306, "vx":-1.72323, "vy":-1.25225, "omega":0.6139, "ax":2.98964, "ay":-0.12672, "alpha":-4.76909, "fx":[34.26128,52.96579,64.91678,47.19956], "fy":[-5.76379,-18.81612,1.13613,14.99448]}, + {"t":0.85616, "x":5.6607, "y":5.71325, "heading":-2.14343, "vx":-1.62763, "vy":-1.25631, "omega":0.4614, "ax":2.98299, "ay":0.28337, "alpha":-3.55806, "fx":[38.35127,52.44257,60.88497,47.22155], "fy":[2.1727,-7.95114,7.23284,17.44038]}, + {"t":0.88814, "x":5.61017, "y":5.67323, "heading":-2.12868, "vx":-1.53225, "vy":-1.24724, "omega":0.34763, "ax":2.92925, "ay":0.63224, "alpha":-2.67703, "fx":[40.25531,51.09479,57.2391,46.72735], "fy":[8.49147,1.12593,12.47694,20.06227]}, + {"t":0.92012, "x":5.56268, "y":5.63367, "heading":-2.11756, "vx":-1.43858, "vy":-1.22703, "omega":0.26203, "ax":2.85196, "ay":0.92096, "alpha":-2.04845, "fx":[41.11305,49.45955,54.00584,45.58466], "fy":[13.85168,8.12153,16.83121,22.60356]}, + {"t":0.95209, "x":5.51813, "y":5.5949, "heading":-2.10919, "vx":-1.34739, "vy":-1.19758, "omega":0.19653, "ax":2.76487, "ay":1.15709, "alpha":-1.57189, "fx":[41.07969,47.61153,51.03425,44.63057], "fy":[18.13276,13.76121,20.45757,24.80084]}, + {"t":0.98407, "x":5.47646, "y":5.5572, "heading":-2.1029, "vx":-1.25898, "vy":-1.16058, "omega":0.14626, "ax":2.67638, "ay":1.34969, "alpha":-1.22, "fx":[40.82197,45.87948,48.50114,43.25355], "fy":[21.52217,18.27747,23.40299,26.79229]}, + {"t":1.01605, "x":5.43758, "y":5.52078, "heading":-2.09822, "vx":-1.1734, "vy":-1.11742, "omega":0.10725, "ax":2.59108, "ay":1.50738, "alpha":-0.95941, "fx":[40.147,44.19445,46.19933,42.22759], "fy":[24.47463,21.71675,25.87226,28.44578]}, + {"t":1.04802, "x":5.40138, "y":5.48582, "heading":-2.09479, "vx":-1.09054, "vy":-1.06922, "omega":0.07658, "ax":2.51117, "ay":1.63738, "alpha":-0.74917, "fx":[39.58178,42.69027,44.27653,40.89148], "fy":[26.59212,24.77381,27.87239,29.93874]}, + {"t":1.08, "x":5.36779, "y":5.45247, "heading":-2.09235, "vx":-1.01025, "vy":-1.01686, "omega":0.05262, "ax":2.43748, "ay":1.74545, "alpha":-0.59774, "fx":[38.73281,41.28434,42.50349,40.00607], "fy":[28.7123,26.95808,29.57404,31.13886]}, + {"t":1.11197, "x":5.33673, "y":5.42084, "heading":-2.09066, "vx":-0.93231, "vy":-0.96105, "omega":0.03351, "ax":2.37011, "ay":1.83612, "alpha":-0.46693, "fx":[38.11672,40.04487,41.03087,38.84178], "fy":[30.11352,29.07347,30.97952,32.2626]}, + {"t":1.14395, "x":5.30813, "y":5.39105, "heading":-2.08959, "vx":-0.85652, "vy":-0.90234, "omega":0.01858, "ax":2.30875, "ay":1.91289, "alpha":-0.3758, "fx":[37.29358,38.9078,39.65568,38.08616], "fy":[31.66567,30.52417,32.1996,33.15847]}, + {"t":1.17593, "x":5.28193, "y":5.36318, "heading":-2.089, "vx":-0.78269, "vy":-0.84117, "omega":0.00656, "ax":2.25296, "ay":1.97847, "alpha":-0.28793, "fx":[36.71898,37.89275,38.50269,37.10854], "fy":[32.63138,32.05809,33.21973,34.0111]}, + {"t":1.2079, "x":5.25805, "y":5.33729, "heading":-2.08879, "vx":-0.71065, "vy":-0.77791, "omega":-0.00265, "ax":2.20221, "ay":2.03495, "alpha":-0.2299, "fx":[35.98414,36.974,37.41576,36.46509], "fy":[33.80393,33.06692,34.1235,34.6921]}, + {"t":1.23988, "x":5.23645, "y":5.31346, "heading":-2.08887, "vx":-0.64024, "vy":-0.71284, "omega":-0.01, "ax":2.15599, "ay":2.08398, "alpha":-0.16583, "fx":[35.48294,36.13937,36.4915,35.64314], "fy":[34.50218,34.22522,34.88384,35.34478]}, + {"t":1.27185, "x":5.21708, "y":5.29173, "heading":-2.08919, "vx":-0.5713, "vy":-0.6462, "omega":-0.0153, "ax":2.11381, "ay":2.12686, "alpha":-0.12444, "fx":[34.84901,35.38355,35.60635,35.10571], "fy":[35.40761,34.9676,35.57403,35.86594]}, + {"t":1.30383, "x":5.1999, "y":5.27215, "heading":-2.08968, "vx":-0.5037, "vy":-0.57819, "omega":-0.01928, "ax":2.07523, "ay":2.16462, "alpha":-0.07227, "fx":[34.42549,34.68225,34.83584,34.42904], "fy":[35.92015,35.88202,36.16148,36.36874]}, + {"t":1.33581, "x":5.18485, "y":5.25477, "heading":-2.0903, "vx":-0.43735, "vy":-0.50898, "omega":-0.02159, "ax":2.03987, "ay":2.19806, "alpha":-0.03815, "fx":[33.8948,34.05476,34.09808,33.96716], "fy":[36.65055,36.45617,36.69371,36.76191]}, + {"t":1.36778, "x":5.17191, "y":5.23962, "heading":-2.09099, "vx":-0.37212, "vy":-0.43869, "omega":-0.02281, "ax":2.00737, "ay":2.22786, "alpha":0.00942, "fx":[33.55072,33.45783,33.43445,33.40474], "fy":[37.04762,37.20446,37.1544,37.14281]}, + {"t":1.39976, "x":5.16104, "y":5.22673, "heading":-2.09172, "vx":-0.30793, "vy":-0.36745, "omega":-0.02251, "ax":1.97743, "ay":2.25455, "alpha":0.04333, "fx":[33.1193,32.9234,32.7934,33.01497], "fy":[37.64551,37.67897,37.57403,37.43056]}, + {"t":1.43174, "x":5.1522, "y":5.21613, "heading":-2.09244, "vx":-0.2447, "vy":-0.29536, "omega":-0.02112, "ax":1.94977, "ay":2.27858, "alpha":0.0929, "fx":[32.85655,32.40365,32.19635,32.55021], "fy":[37.96378,38.32374,37.93911,37.70456]}, + {"t":1.46371, "x":5.14537, "y":5.20785, "heading":-2.09311, "vx":-0.18235, "vy":-0.2225, "omega":-0.01815, "ax":1.92416, "ay":2.3003, "alpha":0.13271, "fx":[32.52447,31.93701,31.61341,32.22424], "fy":[38.46642,38.74721,38.27144,37.89479]}, + {"t":1.49569, "x":5.14052, "y":5.20191, "heading":-2.09369, "vx":-0.12083, "vy":-0.14895, "omega":-0.01391, "ax":1.90039, "ay":2.32003, "alpha":0.18392, "fx":[32.2975,31.48487,31.04796,31.88388], "fy":[38.82964,39.24853,38.56424,38.053]}, + {"t":1.52766, "x":5.13763, "y":5.19834, "heading":-2.09414, "vx":-0.06006, "vy":-0.07476, "omega":-0.00803, "ax":1.87828, "ay":2.33802, "alpha":0.25108, "fx":[32.15449,30.99781,30.48879,31.5988], "fy":[39.13728,39.80954,38.80282,38.14507]}, + {"t":1.55964, "x":5.13667, "y":5.19714, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/startToJSlow.traj b/src/main/deploy/choreo/startToJSlow.traj index d74274b4..af5f88bf 100644 --- a/src/main/deploy/choreo/startToJSlow.traj +++ b/src/main/deploy/choreo/startToJSlow.traj @@ -17,7 +17,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":6.1663548}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"start.x", "val":7.1}, "y":{"exp":"start.y", "val":5.07}, "heading":{"exp":"start.heading", "val":3.14159}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"5.7971601486206055 m", "val":5.7971601486206055}, "y":{"exp":"5.54304313659668 m", "val":5.54304313659668}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"J.x", "val":5.1366723649053885}, "y":{"exp":"J.y", "val":5.197141990263579}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ diff --git a/src/main/deploy/choreo/startToK.traj b/src/main/deploy/choreo/startToK.traj index 05276ebe..c1f5352c 100644 --- a/src/main/deploy/choreo/startToK.traj +++ b/src/main/deploy/choreo/startToK.traj @@ -17,7 +17,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":6.1663548}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":73, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"start.x", "val":7.1}, "y":{"exp":"start.y", "val":5.07}, "heading":{"exp":"start.heading", "val":3.14159}, "intervals":73, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"4.455554008483887 m", "val":4.455554008483887}, "y":{"exp":"5.959114074707031 m", "val":5.959114074707031}, "heading":{"exp":"-1.325818250323842 rad", "val":-1.325818250323842}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"K.x", "val":4.084460748154255}, "y":{"exp":"K.y", "val":5.3371419902635795}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ diff --git a/src/main/deploy/choreo/startToL.traj b/src/main/deploy/choreo/startToL.traj index 0b652de6..8b48bf32 100644 --- a/src/main/deploy/choreo/startToL.traj +++ b/src/main/deploy/choreo/startToL.traj @@ -17,7 +17,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":6.1663548}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":76, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"start.x", "val":7.1}, "y":{"exp":"start.y", "val":5.07}, "heading":{"exp":"start.heading", "val":3.14159}, "intervals":76, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"4.180931568145752 m", "val":4.180931568145752}, "y":{"exp":"5.788817405700684 m", "val":5.788817405700684}, "heading":{"exp":"-1.2827410322432835 rad", "val":-1.2827410322432835}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"L.x", "val":3.798672364905389}, "y":{"exp":"L.y", "val":5.1721419902635795}, "heading":{"exp":"L.heading", "val":5.235987755982989}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8743482b..8fc52841 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -723,6 +723,15 @@ public void configTestDash() { driveSubsystem, climbSubsystem, climbAlignSubsystem, robotStateSubsystem)) .withPosition(0, 1) .withSize(1, 1); + + Shuffleboard.getTab("Test") + .add("Headlights On", new InstantCommand(() -> pathHandler.setHeadlights(true))) + .withPosition(2, 2) + .withSize(1, 1); + Shuffleboard.getTab("Test") + .add("Headlights Off", new InstantCommand(() -> pathHandler.setHeadlights(false))) + .withPosition(3, 2) + .withSize(1, 1); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/constants/AutonConstants.java b/src/main/java/frc/robot/constants/AutonConstants.java index 775acc7b..7cd1cdb8 100644 --- a/src/main/java/frc/robot/constants/AutonConstants.java +++ b/src/main/java/frc/robot/constants/AutonConstants.java @@ -10,7 +10,7 @@ public class AutonConstants { public static final double kMaxOmegaErrorDegrees = 0.5; public static final double kMaxOmegaErrorRadians = Units.degreesToRadians(kMaxOmegaErrorDegrees); public static final int kSwitchStableCounts = 3; - public static final double kElevatorStageRadius = 2.2; + public static final double kElevatorStageRadius = 2.3; public static final double kElevatorStageRadiusPathOne = 1.8; public static final double kInitPathPrestageTime = 0.6; diff --git a/src/main/java/frc/robot/constants/LEDConstants.java b/src/main/java/frc/robot/constants/LEDConstants.java index 32b7e6f3..51c5b045 100644 --- a/src/main/java/frc/robot/constants/LEDConstants.java +++ b/src/main/java/frc/robot/constants/LEDConstants.java @@ -10,7 +10,7 @@ public static final Color invertRedGreen(Color color) { public static final int kLEDPort = 0; // Auto/Operator = 27 // other = 21.5 - public static final int kTotalStripLength = 51; + public static final int kTotalStripLength = 51 + 10; public static final int kBottomStripLength = 34; public static final int kTopStripLength = 17; public static final int kTopFirstIndex = kBottomStripLength; @@ -53,7 +53,7 @@ public static final Color invertRedGreen(Color color) { public static final Color kTooClose = invertRedGreen(Color.kBlue); // HP Load Color - public static final Color kLoadCoral = invertRedGreen(Color.kYellow); + public static final Color kLoadCoral = invertRedGreen(Color.kBlue); public static final Color[] kGameColors = { invertRedGreen(Color.kBlack), // a dummy color diff --git a/src/main/java/frc/robot/constants/PathHandlerConstants.java b/src/main/java/frc/robot/constants/PathHandlerConstants.java index 34787f6e..bbb00ad9 100644 --- a/src/main/java/frc/robot/constants/PathHandlerConstants.java +++ b/src/main/java/frc/robot/constants/PathHandlerConstants.java @@ -158,5 +158,5 @@ public class PathHandlerConstants { public static final double kWaitingTime = 2.0; public static final double kServoRadius = 1.7; - public static final double kMaxServoErrorY = 0.4; + public static final double kMaxServoErrorY = 0.7; } diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java index 2d680017..c1de5721 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java @@ -46,7 +46,10 @@ public class LEDSubsystem extends MeasurableSubsystem { private LEDPattern currentLimiting = LEDPattern.solid(LEDConstants.kCurrentLimiting).blink(Seconds.of(0.5), Seconds.of(1)); private LEDPattern loadCoral = - LEDPattern.steps(Map.of(LEDConstants.kTopFirstIndex, LEDConstants.kLoadCoral)); + LEDPattern.steps( + Map.of( + LEDConstants.kTopFirstIndex / (double) LEDConstants.kTotalStripLength, + LEDConstants.kLoadCoral)); // section booleans and states private boolean hasAlgae = false; @@ -153,9 +156,7 @@ public boolean getCurrentLimiting() { } private void buildBase() { - if (shouldLoadCoral) { - base = loadCoral.overlayOn(base); - } + algae = LEDPattern.steps( Map.of( @@ -223,8 +224,11 @@ private void buildBase() { if (autoPlacing) { base = autoplace.overlayOn(base); } - - base = getAlgea.overlayOn(place.overlayOn(level.overlayOn(base))); + if (shouldLoadCoral) { + base = loadCoral.overlayOn(base); + } else { + base = getAlgea.overlayOn(place.overlayOn(level.overlayOn(base))); + } } // game stuff diff --git a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java index 2efae0fd..c2d52bbd 100644 --- a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java +++ b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java @@ -5,6 +5,7 @@ import choreo.trajectory.Trajectory; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DigitalOutput; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import frc.robot.constants.AutonConstants; @@ -59,9 +60,12 @@ public class PathHandler extends MeasurableSubsystem { private boolean teleop = false; private boolean isPlacing = false; private boolean hasPreppedCoral = false; + private boolean hasStagedAlgae = false; private boolean algaeOnLast = false; private boolean hasLEDsOn = false; + private DigitalOutput lights = new DigitalOutput(3); + public PathHandler( DriveSubsystem driveSubsystem, TagAlignSubsystem tagAlignSubsystem, @@ -184,6 +188,9 @@ private void startPath(Trajectory path) { if (curState == PathStates.PLACE) { curState = PathStates.DRIVE_FETCH; } else if (curState == PathStates.FETCH) { + robotStateSubsystem.setLEDLoadCoral(false); + hasLEDsOn = false; + setHeadlights(true); curState = PathStates.DRIVE_PLACE; } } @@ -223,6 +230,9 @@ private void drivePath() { pathTimer.reset(); driveSubsystem.drive(0, 0, 0); advanceNodes(); + robotStateSubsystem.setLEDLoadCoral(false); + hasLEDsOn = false; + setHeadlights(true); curState = PathStates.DRIVE_PLACE; } else if (shouldTransitionToServoing()) { @@ -400,6 +410,10 @@ public boolean isFinished() { return !isHandling; } + public void setHeadlights(boolean on) { + lights.set(on); + } + @Override public void periodic() { org.littletonrobotics.junction.Logger.recordOutput("PathHandler/State", curState); @@ -407,6 +421,10 @@ public void periodic() { org.littletonrobotics.junction.Logger.recordOutput( "PathHandler/curRadius", tagAlignSubsystem.getCurRadius(robotStateSubsystem.getAllianceColor())); + org.littletonrobotics.junction.Logger.recordOutput("PathHandler/hasLEDsOn", hasLEDsOn); + org.littletonrobotics.junction.Logger.recordOutput( + "PathHandler/hasStagedAlgae", hasStagedAlgae); + switch (curState) { case DRIVE_FETCH -> { if (!runningPath) { @@ -417,36 +435,41 @@ public void periodic() { driveSubsystem.getPoseMeters().getX() - currPathFinalPose.getX(), 2) + FastMath.pow( driveSubsystem.getPoseMeters().getY() - currPathFinalPose.getY(), 2)) - < 0.5 + < 1.7 && !hasLEDsOn) { robotStateSubsystem.setLEDLoadCoral(true); hasLEDsOn = true; + setHeadlights(false); } drivePath(); } case FETCH -> { - if (hasLEDsOn) { - robotStateSubsystem.setLEDLoadCoral(false); - hasLEDsOn = false; - } if (robotStateSubsystem.hasCoral() || (robotStateSubsystem.hasCoralAuton())) { // waitingTimer.hasElapsed(PathHandlerConstants.kWaitingTime) // proceedToNext) { advanceNodes(); + + robotStateSubsystem.setLEDLoadCoral(false); + hasLEDsOn = false; + setHeadlights(true); curState = PathStates.DRIVE_PLACE; } } case DRIVE_PLACE -> { + if (algaeOnLast + && nodeNames.size() == 1 + && !hasStagedAlgae + && robotStateSubsystem.hasCoral()) { + robotStateSubsystem.toReefAlignAlgaeAuto(); + hasStagedAlgae = true; + } if ( /*tagAlignSubsystem.getCurRadius(robotStateSubsystem.getAllianceColor()) <= AutonConstants.kElevatorStageRadius*/ shouldStageElevator() - && !hasPreppedCoral) { + && !hasPreppedCoral + && !hasStagedAlgae) { hasPreppedCoral = true; - if (algaeOnLast && nodeNames.size() == 1) { - robotStateSubsystem.toReefAlignAlgaeAuto(); - } else { - robotStateSubsystem.toPrepCoral(); - } + robotStateSubsystem.toPrepCoral(); } if (!runningPath) { if (nodeNames.size() > 0) { @@ -487,6 +510,7 @@ public void periodic() { return; } hasPreppedCoral = false; + hasStagedAlgae = false; if (robotStateSubsystem.isElevatorFinished() && !isPlacing) { isPlacing = true; robotStateSubsystem.toPlaceCoralAuto(); From 37a102622e1976a0e79bf39589e21c117725bbf3 Mon Sep 17 00:00:00 2001 From: David Shen Date: Sat, 29 Mar 2025 16:32:16 -0400 Subject: [PATCH 07/12] technically working 4 piece --- src/main/deploy/choreo/2025-project.chor | 22 +- src/main/deploy/choreo/CTofetchP.traj | 120 ++++----- src/main/deploy/choreo/CTofetchPSlow.traj | 97 ++++--- src/main/deploy/choreo/DTofetchP.traj | 118 ++++----- src/main/deploy/choreo/DTofetchPSlow.traj | 94 +++---- src/main/deploy/choreo/ETofetchP.traj | 194 +++++++------- src/main/deploy/choreo/ETofetchPSlow.traj | 2 +- src/main/deploy/choreo/JTofetch.traj | 197 +++++++-------- src/main/deploy/choreo/KTofetchSlow.traj | 95 ++++--- src/main/deploy/choreo/LTofetchSlow.traj | 94 +++---- src/main/deploy/choreo/fetchPToC.traj | 140 +++++------ src/main/deploy/choreo/fetchPToCSlow.traj | 139 +++++----- src/main/deploy/choreo/fetchPToD.traj | 144 +++++------ src/main/deploy/choreo/fetchPToDSlow.traj | 144 ++++++----- src/main/deploy/choreo/fetchToK.traj | 107 ++++---- src/main/deploy/choreo/fetchToKSlow.traj | 141 +++++------ src/main/deploy/choreo/fetchToLSlow.traj | 142 ++++++----- src/main/deploy/choreo/startPToE.traj | 135 +++++----- src/main/deploy/choreo/startPToESlow.traj | 142 ++++++----- src/main/deploy/choreo/startToG.traj | 94 +++---- src/main/deploy/choreo/startToH.traj | 86 +++---- src/main/deploy/choreo/startToI.traj | 118 ++++----- src/main/deploy/choreo/startToJSlow.traj | 2 +- src/main/deploy/choreo/startToK.traj | 230 ++++++++--------- src/main/deploy/choreo/startToL.traj | 238 +++++++++--------- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 14 +- .../frc/robot/constants/AutonConstants.java | 2 +- .../frc/robot/subsystems/auto/AutoSwitch.java | 23 ++ .../subsystems/pathHandler/PathHandler.java | 10 +- .../tagAlign/TagAlignSubsystem.java | 11 +- 31 files changed, 1519 insertions(+), 1577 deletions(-) diff --git a/src/main/deploy/choreo/2025-project.chor b/src/main/deploy/choreo/2025-project.chor index 68359cc2..92400604 100644 --- a/src/main/deploy/choreo/2025-project.chor +++ b/src/main/deploy/choreo/2025-project.chor @@ -232,12 +232,12 @@ }, "fetchP":{ "x":{ - "exp":"1.69577419757843 m", - "val":1.69577419757843 + "exp":"1.6421304941177368 m", + "val":1.6421304941177368 }, "y":{ - "exp":"0.8236 m", - "val":0.8236 + "exp":"0.8500130772590637 m", + "val":0.8500130772590637 }, "heading":{ "exp":"54 deg", @@ -258,6 +258,20 @@ "val":3.14159 } }, + "midStartP":{ + "x":{ + "exp":"7.1 m", + "val":7.1 + }, + "y":{ + "exp":"1.8818 m", + "val":1.8818 + }, + "heading":{ + "exp":"180 deg", + "val":3.141592653589793 + } + }, "start":{ "x":{ "exp":"7.1 m", diff --git a/src/main/deploy/choreo/CTofetchP.traj b/src/main/deploy/choreo/CTofetchP.traj index 847e5c9f..14c2b7fa 100644 --- a/src/main/deploy/choreo/CTofetchP.traj +++ b/src/main/deploy/choreo/CTofetchP.traj @@ -3,30 +3,28 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":3.5561852518457457, "y":3.0196580097364216, "heading":1.0471975511965976, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.9328655004501345, "y":1.1461, "heading":0.942477796076938, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.69577419757843, "y":0.8236, "heading":0.942477796076938, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.5561852518457457, "y":3.0196580097364216, "heading":1.0471975511965976, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.8771069049835205, "y":1.1704355478286743, "heading":0.942477796076938, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.6421304941177368, "y":0.8500130772590637, "heading":0.942477796076938, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":3.7}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":3.0}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":3.0}}, "enabled":true}], + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":3.3}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"C.x", "val":3.5561852518457457}, "y":{"exp":"C.y", "val":3.0196580097364216}, "heading":{"exp":"C.heading", "val":1.0471975511965976}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.9328655004501343 m", "val":1.9328655004501345}, "y":{"exp":"1.1461 m", "val":1.1461}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"fetchP.x", "val":1.69577419757843}, "y":{"exp":"fetchP.y", "val":0.8236}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"C.x", "val":3.5561852518457457}, "y":{"exp":"C.y", "val":3.0196580097364216}, "heading":{"exp":"C.heading", "val":1.0471975511965976}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.8771069049835205 m", "val":1.8771069049835205}, "y":{"exp":"1.1704355478286743 m", "val":1.1704355478286743}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"fetchP.x", "val":1.6421304941177368}, "y":{"exp":"0.8500130772590637 m", "val":0.8500130772590637}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.7 m / s", "val":3.7}}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"maxAccel", "val":3.0}}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"3 m / s ^ 2", "val":3.0}}}, "enabled":true}], + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.3 m / s", "val":3.3}}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -34,63 +32,49 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.44436,1.96122], + "waypoints":[0.0,1.06444,1.42852], "samples":[ - {"t":0.0, "x":3.55619, "y":3.01966, "heading":1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.01572, "ay":-2.21968, "alpha":-0.86558, "fx":[-30.7191,-34.69057,-36.361,-32.63378], "fy":[-36.40255,-34.07971,-37.61518,-39.90656]}, - {"t":0.03703, "x":3.5548, "y":3.01814, "heading":1.0472, "vx":-0.07465, "vy":-0.08221, "omega":-0.03206, "ax":-2.01609, "ay":-2.22041, "alpha":-0.6738, "fx":[-31.37558,-34.42844,-35.76699,-32.85826], "fy":[-36.54722,-34.73564,-37.48901,-39.28083]}, - {"t":0.07407, "x":3.55066, "y":3.01357, "heading":1.04601, "vx":-0.14932, "vy":-0.16444, "omega":-0.05701, "ax":-2.0159, "ay":-2.22054, "alpha":-0.50892, "fx":[-31.87791,-34.16324,-35.33012,-33.04492], "fy":[-36.61826,-35.39651,-37.33605,-38.71054]}, - {"t":0.1111, "x":3.54374, "y":3.00596, "heading":1.0439, "vx":-0.22398, "vy":-0.24668, "omega":-0.07586, "ax":-2.01568, "ay":-2.22068, "alpha":-0.39989, "fx":[-32.26498,-34.13141,-34.86122,-33.14434], "fy":[-36.74661,-35.66612,-37.29633,-38.36172]}, - {"t":0.14814, "x":3.53407, "y":2.9953, "heading":1.04109, "vx":-0.29863, "vy":-0.32892, "omega":-0.09067, "ax":-2.01545, "ay":-2.22084, "alpha":-0.28397, "fx":[-32.61427,-33.90363,-34.58421,-33.28431], "fy":[-36.77871,-36.15853,-37.17815,-37.96567]}, - {"t":0.18517, "x":3.52162, "y":2.98159, "heading":1.03773, "vx":-0.37327, "vy":-0.41117, "omega":-0.10119, "ax":-2.0152, "ay":-2.221, "alpha":-0.21688, "fx":[-32.85109,-33.94468,-34.24599,-33.32762], "fy":[-36.88316,-36.29061,-37.17059,-37.74796]}, - {"t":0.22221, "x":3.50642, "y":2.96484, "heading":1.03398, "vx":-0.4479, "vy":-0.49342, "omega":-0.10922, "ax":-2.01491, "ay":-2.22119, "alpha":-0.13048, "fx":[-33.10894,-33.72616,-34.07466,-33.4409], "fy":[-36.88271,-36.6911,-37.07132,-37.45958]}, - {"t":0.25924, "x":3.48845, "y":2.94504, "heading":1.02994, "vx":-0.52253, "vy":-0.57568, "omega":-0.11405, "ax":-2.0146, "ay":-2.2214, "alpha":-0.09211, "fx":[-33.24531,-33.82217,-33.81896,-33.44352], "fy":[-36.97651,-36.71991,-37.09094,-37.33104]}, - {"t":0.29628, "x":3.46771, "y":2.9222, "heading":1.02572, "vx":-0.59714, "vy":-0.65795, "omega":-0.11746, "ax":-2.01426, "ay":-2.22162, "alpha":-0.02424, "fx":[-33.44588,-33.60285,-33.71761,-33.54062], "fy":[-36.95205,-37.06735,-37.00238,-37.11183]}, - {"t":0.33331, "x":3.44422, "y":2.89631, "heading":1.02137, "vx":-0.67173, "vy":-0.74023, "omega":-0.11836, "ax":-2.01387, "ay":-2.22188, "alpha":-0.00546, "fx":[-33.51304,-33.7402,-33.51455,-33.51343], "fy":[-37.04107,-37.02298,-37.04258,-37.04399]}, - {"t":0.37035, "x":3.41796, "y":2.86737, "heading":1.01698, "vx":-0.74632, "vy":-0.82252, "omega":-0.11856, "ax":-2.01344, "ay":-2.22217, "alpha":0.05089, "fx":[-33.67616,-33.51517,-33.46052,-33.60044], "fy":[-36.99796,-37.3443,-36.95846,-36.86902]}, - {"t":0.40738, "x":3.38894, "y":2.83539, "heading":1.01259, "vx":-0.82089, "vy":-0.90482, "omega":-0.11668, "ax":-2.01295, "ay":-2.22249, "alpha":0.05505, "fx":[-33.69169,-33.68911,-33.28987,-33.5488], "fy":[-37.08811,-37.23941,-37.01697,-36.84692]}, - {"t":0.44442, "x":3.35716, "y":2.80035, "heading":1.00827, "vx":-0.89544, "vy":-0.98713, "omega":-0.11464, "ax":-2.01238, "ay":-2.22286, "alpha":0.10585, "fx":[-33.83143,-33.44643,-33.27219,-33.6319], "fy":[-37.02462,-37.56788,-36.92781,-36.69588]}, - {"t":0.48145, "x":3.32261, "y":2.76227, "heading":1.00402, "vx":-0.96996, "vy":-1.06945, "omega":-0.11072, "ax":-2.01174, "ay":-2.22329, "alpha":0.09697, "fx":[-33.802,-33.6653,-33.11647,-33.55489], "fy":[-37.12511,-37.39645,-37.00823,-36.71497]}, - {"t":0.51849, "x":3.28531, "y":2.72114, "heading":0.99992, "vx":-1.04447, "vy":-1.15179, "omega":-0.10713, "ax":-2.01098, "ay":-2.22379, "alpha":0.14358, "fx":[-33.92863,-33.39104,-33.12908,-33.63935], "fy":[-37.04977,-37.72632,-36.91754,-36.58447]}, - {"t":0.55552, "x":3.24525, "y":2.67696, "heading":0.99596, "vx":-1.11895, "vy":-1.23415, "omega":-0.10181, "ax":-2.01008, "ay":-2.22438, "alpha":0.12317, "fx":[-33.8339,-33.72804,-32.95407,-33.51233], "fy":[-37.15978,-37.50637,-37.01551,-36.63584]}, - {"t":0.59256, "x":3.20243, "y":2.62972, "heading":0.99219, "vx":-1.19339, "vy":-1.31653, "omega":-0.09725, "ax":-2.009, "ay":-2.22509, "alpha":0.1772, "fx":[-33.97727,-33.33265,-33.0203,-33.62634], "fy":[-37.04442,-37.93595,-36.8921,-36.4923]}, - {"t":0.62959, "x":3.15686, "y":2.57944, "heading":0.98858, "vx":-1.26779, "vy":-1.39893, "omega":-0.09068, "ax":-2.00769, "ay":-2.22596, "alpha":0.13958, "fx":[-33.82762,-33.75934,-32.82548,-33.45638], "fy":[-37.19511,-37.59107,-37.03598,-36.60033]}, - {"t":0.66663, "x":3.10853, "y":2.5261, "heading":0.98523, "vx":-1.34215, "vy":-1.48137, "omega":-0.08552, "ax":-2.00604, "ay":-2.22704, "alpha":0.19745, "fx":[-33.97616,-33.27168,-32.92546,-33.58568], "fy":[-37.05755,-38.08488,-36.8956,-36.45665]}, - {"t":0.70366, "x":3.05745, "y":2.46971, "heading":0.98206, "vx":-1.41644, "vy":-1.56385, "omega":-0.0782, "ax":-2.00392, "ay":-2.22843, "alpha":0.14747, "fx":[-33.76933,-33.77495,-32.70412,-33.3694], "fy":[-37.24088,-37.66274,-37.07488,-36.60879]}, - {"t":0.7407, "x":3.00361, "y":2.41027, "heading":0.97916, "vx":-1.49066, "vy":-1.64638, "omega":-0.07274, "ax":-2.00108, "ay":-2.23029, "alpha":0.20828, "fx":[-33.91286,-33.18806,-32.82507,-33.50249], "fy":[-37.08925,-38.21953,-36.92542,-36.47722]}, - {"t":0.77773, "x":2.94703, "y":2.34776, "heading":0.97647, "vx":-1.56477, "vy":-1.72898, "omega":-0.06503, "ax":-1.99711, "ay":-2.23289, "alpha":0.14412, "fx":[-33.59051,-33.8689,-32.52076,-33.18321], "fy":[-37.31853,-37.73357,-37.15181,-36.68073]}, - {"t":0.81477, "x":2.88771, "y":2.2822, "heading":0.97406, "vx":-1.63873, "vy":-1.81167, "omega":-0.05969, "ax":-1.99114, "ay":-2.23677, "alpha":0.19228, "fx":[-33.72892,-33.043,-32.66977,-33.3237], "fy":[-37.23275,-38.19468,-37.07528,-36.64079]}, - {"t":0.8518, "x":2.82566, "y":2.21357, "heading":0.97185, "vx":-1.71247, "vy":-1.89451, "omega":-0.05257, "ax":-1.98111, "ay":-2.24326, "alpha":0.1562, "fx":[-33.61516,-32.60404,-32.63898,-33.23846], "fy":[-37.46246,-37.93608,-37.30605,-36.87175]}, - {"t":0.88884, "x":2.76088, "y":2.14187, "heading":0.9699, "vx":-1.78584, "vy":-1.97759, "omega":-0.04678, "ax":-1.9607, "ay":-2.25635, "alpha":0.05674, "fx":[-33.13683,-32.63211,-32.19578,-32.77117], "fy":[-37.95327,-37.24142,-37.81566,-37.4389]}, - {"t":0.92587, "x":2.6934, "y":2.06708, "heading":0.96817, "vx":-1.85846, "vy":-2.06116, "omega":-0.04468, "ax":-1.89318, "ay":-2.29931, "alpha":0.06892, "fx":[-31.44092,-32.77033,-30.82397,-31.19832], "fy":[-38.38925,-38.62966,-38.2845,-38.00986]}, - {"t":0.96291, "x":2.62327, "y":1.98917, "heading":0.96652, "vx":-1.92857, "vy":-2.14631, "omega":-0.04213, "ax":2.06149, "ay":-1.4348, "alpha":0.08638, "fx":[34.27421,34.38408,34.45113,34.34673], "fy":[-23.71718,-24.6429,-23.68707,-23.6224]}, - {"t":0.99994, "x":2.55326, "y":1.9087, "heading":0.96495, "vx":-1.85222, "vy":-2.19945, "omega":-0.03893, "ax":2.10662, "ay":2.10587, "alpha":-0.0304, "fx":[34.91265,36.21606,34.5619,34.7748], "fy":[35.14593,35.25393,35.08771,34.92795]}, - {"t":1.03698, "x":2.48611, "y":1.82869, "heading":0.96351, "vx":-1.7742, "vy":-2.12146, "omega":-0.04006, "ax":2.06401, "ay":2.16235, "alpha":-0.0622, "fx":[34.73231,34.36968,34.05488,34.46766], "fy":[36.2171,36.00056,36.12156,35.84213]}, - {"t":1.07401, "x":2.42181, "y":1.7516, "heading":0.96203, "vx":-1.69776, "vy":-2.04138, "omega":-0.04236, "ax":2.05011, "ay":2.18042, "alpha":-0.11309, "fx":[34.56452,34.02685,33.82909,34.27726], "fy":[36.40106,36.73849,36.29113,35.95571]}, - {"t":1.11105, "x":2.36034, "y":1.67749, "heading":0.96046, "vx":-1.62184, "vy":-1.96062, "omega":-0.04655, "ax":2.04321, "ay":2.18934, "alpha":-0.13313, "fx":[34.44008,33.96728,33.68449,34.14523], "fy":[36.46533,37.10461,36.36358,36.04723]}, - {"t":1.14808, "x":2.30168, "y":1.60638, "heading":0.95874, "vx":-1.54617, "vy":-1.87954, "omega":-0.05148, "ax":2.0388, "ay":2.1949, "alpha":-0.09277, "fx":[34.23737,34.13265,33.58941,33.98366], "fy":[36.64022,36.91154,36.54809,36.25216]}, - {"t":1.18512, "x":2.24582, "y":1.53828, "heading":0.95683, "vx":-1.47066, "vy":-1.79825, "omega":-0.05492, "ax":2.03587, "ay":2.19859, "alpha":-0.10753, "fx":[34.19534,33.86124,33.69331,33.99792], "fy":[36.56202,37.25301,36.49626,36.28677]}, - {"t":1.22215, "x":2.19275, "y":1.47319, "heading":0.9548, "vx":-1.39526, "vy":-1.71683, "omega":-0.0589, "ax":2.03375, "ay":2.20124, "alpha":-0.03153, "fx":[33.9342,34.15465,33.68393,33.83409], "fy":[36.72067,36.80528,36.68222,36.5666]}, - {"t":1.25919, "x":2.14247, "y":1.41112, "heading":0.95262, "vx":-1.31994, "vy":-1.63531, "omega":-0.06007, "ax":2.03217, "ay":2.20323, "alpha":-0.01887, "fx":[33.84418,33.86616,33.92034,33.87033], "fy":[36.61701,37.00422,36.62163,36.66437]}, - {"t":1.29622, "x":2.09498, "y":1.35206, "heading":0.95039, "vx":-1.24468, "vy":-1.55371, "omega":-0.06076, "ax":2.03093, "ay":2.20478, "alpha":0.08144, "fx":[33.53251,34.09892,34.05424,33.73284], "fy":[36.73204,36.47416,36.78392,37.02015]}, - {"t":1.33326, "x":2.05027, "y":1.29603, "heading":0.94814, "vx":-1.16946, "vy":-1.47205, "omega":-0.05775, "ax":2.02994, "ay":2.20601, "alpha":0.13676, "fx":[33.29239,33.9455,34.39536,33.71932], "fy":[36.62325,36.48954,36.74035,37.23943]}, - {"t":1.37029, "x":2.00835, "y":1.24303, "heading":0.946, "vx":-1.09429, "vy":-1.39036, "omega":-0.05268, "ax":2.02913, "ay":2.20702, "alpha":0.27593, "fx":[32.85238,34.19221,34.6858,33.56808], "fy":[36.70019,35.8419,36.89059,37.72731]}, - {"t":1.40733, "x":1.96922, "y":1.19305, "heading":0.94405, "vx":-1.01914, "vy":-1.30862, "omega":-0.04246, "ax":2.02845, "ay":2.20787, "alpha":0.38887, "fx":[32.41881,34.10724,35.21417,33.51307], "fy":[36.58029,35.63923,36.86885,38.12785]}, - {"t":1.44436, "x":1.93287, "y":1.1461, "heading":0.94248, "vx":-0.94401, "vy":-1.22685, "omega":-0.02806, "ax":2.00352, "ay":2.22336, "alpha":0.26938, "fx":[32.36548,33.59927,34.4386,33.18756], "fy":[36.82565,36.39237,37.05666,37.97471]}, - {"t":1.47882, "x":1.90153, "y":1.10515, "heading":0.94151, "vx":-0.87498, "vy":-1.15024, "omega":-0.01878, "ax":1.96772, "ay":2.26012, "alpha":0.22661, "fx":[31.90949,32.90168,33.5295,32.86342], "fy":[37.9423,36.76429,37.6496,38.34448]}, - {"t":1.51328, "x":1.87255, "y":1.06685, "heading":0.94086, "vx":-0.80718, "vy":-1.07236, "omega":-0.01097, "ax":1.93383, "ay":2.28946, "alpha":0.1565, "fx":[31.60297,32.35225,32.88192,32.10683], "fy":[37.99552,37.84642,38.124,38.69109]}, - {"t":1.54774, "x":1.84588, "y":1.03126, "heading":0.94049, "vx":-0.74054, "vy":-0.99347, "omega":-0.00558, "ax":1.90355, "ay":2.31494, "alpha":0.13899, "fx":[31.14525,31.77392,32.16677,31.83886], "fy":[38.85601,37.99162,38.54052,38.96756]}, - {"t":1.58219, "x":1.82149, "y":0.9984, "heading":0.94029, "vx":-0.67495, "vy":-0.91371, "omega":-0.00079, "ax":1.87636, "ay":2.33724, "alpha":0.08998, "fx":[30.88245,31.34485,31.68564,31.19886], "fy":[38.82982,38.84444,38.90768,39.26086]}, - {"t":1.61665, "x":1.79935, "y":0.96831, "heading":0.94027, "vx":-0.6103, "vy":-0.83317, "omega":0.00231, "ax":1.85181, "ay":2.35692, "alpha":0.08321, "fx":[30.48238,30.87499,31.11629,31.0017], "fy":[39.52531,38.90107,39.23632,39.49223]}, - {"t":1.65111, "x":1.77942, "y":0.941, "heading":0.94035, "vx":-0.54649, "vy":-0.75196, "omega":0.00518, "ax":1.82956, "ay":2.3744, "alpha":0.04612, "fx":[30.26367,30.53249,30.74323,30.45237], "fy":[39.47793,39.58785,39.52277,39.73188]}, - {"t":1.68556, "x":1.76168, "y":0.9165, "heading":0.94052, "vx":-0.48345, "vy":-0.67015, "omega":0.00677, "ax":1.8093, "ay":2.39002, "alpha":0.04345, "fx":[29.92041,30.14479,30.27696,30.29876], "fy":[40.06716,39.59788,39.78112,39.91601]}, - {"t":1.72002, "x":1.74609, "y":0.89483, "heading":0.94076, "vx":-0.4211, "vy":-0.58779, "omega":0.00826, "ax":1.79079, "ay":2.40407, "alpha":0.01077, "fx":[29.74263,29.85993,29.97132,29.8323], "fy":[39.98891,40.19339,40.00874,40.1074]}, - {"t":1.75448, "x":1.73265, "y":0.876, "heading":0.94104, "vx":-0.3594, "vy":-0.50495, "omega":0.00864, "ax":1.7738, "ay":2.41675, "alpha":0.01008, "fx":[29.44647,29.53186,29.57249,29.72259], "fy":[40.50913,40.16282,40.2195,40.2528]}, - {"t":1.78894, "x":1.72132, "y":0.86003, "heading":0.94134, "vx":-0.29828, "vy":-0.42168, "omega":0.00898, "ax":1.75816, "ay":2.42826, "alpha":-0.02124, "fx":[29.31463,29.29145,29.3117,29.31291], "fy":[40.40914,40.69307,40.40597,40.40369]}, - {"t":1.82339, "x":1.71208, "y":0.84695, "heading":0.94165, "vx":-0.2377, "vy":-0.33801, "omega":0.00825, "ax":1.74372, "ay":2.43876, "alpha":-0.02345, "fx":[29.07007,29.01266,28.96071,29.22445], "fy":[40.86978,40.65033,40.57958,40.51181]}, - {"t":1.85785, "x":1.70493, "y":0.83675, "heading":0.94193, "vx":-0.17761, "vy":-0.25398, "omega":0.00744, "ax":1.73035, "ay":2.44836, "alpha":-0.05592, "fx":[28.97837,28.80226,28.71963,28.87594], "fy":[40.76479,41.12787,40.73614,40.62288]}, - {"t":1.89231, "x":1.69983, "y":0.82945, "heading":0.94219, "vx":-0.11799, "vy":-0.16961, "omega":0.00552, "ax":1.71793, "ay":2.45717, "alpha":-0.07775, "fx":[28.84738,28.579,28.4366,28.68518], "fy":[40.91987,41.34681,40.876,40.6969]}, - {"t":1.92676, "x":1.69679, "y":0.82506, "heading":0.94238, "vx":-0.0588, "vy":-0.08495, "omega":0.00284, "ax":1.70637, "ay":2.4653, "alpha":-0.08233, "fx":[28.69122,28.5211,28.10554,28.45944], "fy":[41.12853,41.37359,41.06839,40.8107]}, - {"t":1.96122, "x":1.69577, "y":0.8236, "heading":0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":3.55619, "y":3.01966, "heading":1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.11081, "ay":-4.36581, "alpha":-1.92362, "fx":[-62.32379,-71.99621,-74.46699,-65.31386], "fy":[-72.73314,-65.9803,-72.9743,-79.41641]}, + {"t":0.0367, "x":3.55342, "y":3.01672, "heading":1.0472, "vx":-0.15089, "vy":-0.16025, "omega":-0.07061, "ax":-4.11188, "ay":-4.36697, "alpha":-1.48089, "fx":[-63.79602,-71.1828,-73.14265,-66.05112], "fy":[-72.75116,-67.57461,-72.92986,-77.92541]}, + {"t":0.07341, "x":3.54511, "y":3.00789, "heading":1.04461, "vx":-0.30181, "vy":-0.32054, "omega":-0.12496, "ax":-4.11178, "ay":-4.36687, "alpha":-1.09435, "fx":[-65.04359,-70.42979,-71.99622,-66.69572], "fy":[-72.7175,-68.98988,-72.86123,-76.60567]}, + {"t":0.11011, "x":3.53126, "y":2.99319, "heading":1.04002, "vx":-0.45274, "vy":-0.48082, "omega":-0.16513, "ax":-4.11165, "ay":-4.36675, "alpha":-0.77503, "fx":[-66.05262,-69.9212,-70.95093,-67.23204], "fy":[-72.78219,-70.06267,-72.83947,-75.48201]}, + {"t":0.14682, "x":3.51187, "y":2.9726, "heading":1.03396, "vx":-0.60365, "vy":-0.6411, "omega":-0.19358, "ax":-4.1115, "ay":-4.3666, "alpha":-0.46974, "fx":[-67.02344,-69.30814,-70.06346,-67.75163], "fy":[-72.74618,-71.2001,-72.77709,-74.43343]}, + {"t":0.18352, "x":3.48695, "y":2.94612, "heading":1.02685, "vx":-0.75457, "vy":-0.80138, "omega":-0.21082, "ax":-4.11131, "ay":-4.36643, "alpha":-0.22702, "fx":[-67.79268,-68.9888,-69.2115,-68.14119], "fy":[-72.8005,-71.97648,-72.79802,-73.57044]}, + {"t":0.22023, "x":3.45648, "y":2.91377, "heading":1.01911, "vx":-0.90547, "vy":-0.96165, "omega":-0.21915, "ax":-4.11108, "ay":-4.36622, "alpha":0.01738, "fx":[-68.56271,-68.47075,-68.53066,-68.55487], "fy":[-72.7426,-72.91999,-72.74251,-72.72597]}, + {"t":0.25693, "x":3.42048, "y":2.87553, "heading":1.01107, "vx":-1.05637, "vy":-1.12191, "omega":-0.21851, "ax":-4.11079, "ay":-4.36595, "alpha":0.1947, "fx":[-69.13531,-68.32993,-67.82954,-68.80469], "fy":[-72.78488,-73.43114,-72.80836,-72.08863]}, + {"t":0.29364, "x":3.37893, "y":2.83141, "heading":1.00305, "vx":-1.20726, "vy":-1.28216, "omega":-0.21137, "ax":-4.1104, "ay":-4.36559, "alpha":0.39003, "fx":[-69.73741,-67.85833,-67.3474,-69.13068], "fy":[-72.69163,-74.25169,-72.73539,-71.41058]}, + {"t":0.33034, "x":3.33185, "y":2.78141, "heading":0.99529, "vx":-1.35813, "vy":-1.4424, "omega":-0.19705, "ax":-4.10988, "ay":-4.3651, "alpha":0.49874, "fx":[-70.09934,-67.93749,-66.77517,-69.22675], "fy":[-72.74489,-74.45522,-72.84959,-71.0069]}, + {"t":0.36705, "x":3.27923, "y":2.72552, "heading":0.98806, "vx":-1.50898, "vy":-1.60262, "omega":-0.17875, "ax":-4.10911, "ay":-4.36439, "alpha":0.64708, "fx":[-70.53697,-67.44908,-66.52242,-69.47904], "fy":[-72.60777,-75.18452,-72.7251,-70.49175]}, + {"t":0.40375, "x":3.22108, "y":2.66376, "heading":0.9815, "vx":-1.65981, "vy":-1.76282, "omega":-0.155, "ax":-4.10788, "ay":-4.36326, "alpha":0.67329, "fx":[-70.62631,-67.8056,-66.08816,-69.38567], "fy":[-72.69758,-74.99966,-72.87676,-70.35971]}, + {"t":0.44046, "x":3.15739, "y":2.59611, "heading":0.97581, "vx":-1.81059, "vy":-1.92297, "omega":-0.13028, "ax":-4.10562, "ay":-4.36118, "alpha":0.76638, "fx":[-70.83652,-67.23247,-66.12924,-69.55656], "fy":[-72.4985,-75.63916,-72.64299,-70.01466]}, + {"t":0.47716, "x":3.08816, "y":2.52259, "heading":0.97103, "vx":-1.96128, "vy":-2.08305, "omega":-0.10215, "ax":-4.10006, "ay":-4.35611, "alpha":0.67607, "fx":[-70.47784,-67.95707,-65.81453,-69.13493], "fy":[-72.5938,-74.85571,-72.77873,-70.22913]}, + {"t":0.51387, "x":3.01341, "y":2.4432, "heading":0.96728, "vx":-2.11177, "vy":-2.24294, "omega":-0.07734, "ax":-4.06579, "ay":-4.32455, "alpha":0.65707, "fx":[-69.74974,-66.96653,-65.81559,-68.5671], "fy":[-71.88787,-74.77541,-71.88937,-69.80006]}, + {"t":0.55057, "x":2.93316, "y":2.35796, "heading":0.96444, "vx":-2.26101, "vy":-2.40167, "omega":-0.05322, "ax":0.00604, "ay":-0.02747, "alpha":0.40032, "fx":[-1.35717,0.71123,1.31893,-0.27051], "fy":[-0.6954,-1.78918,-0.22321,0.87589]}, + {"t":0.58728, "x":2.85018, "y":2.26979, "heading":0.96249, "vx":-2.26079, "vy":-2.40268, "omega":-0.03853, "ax":-0.02387, "ay":0.02246, "alpha":0.15272, "fx":[-0.90864,-0.30506,0.1127,-0.49061], "fy":[0.27795,-0.14844,0.46802,0.90026]}, + {"t":0.62398, "x":2.76718, "y":2.18161, "heading":0.96107, "vx":-2.26166, "vy":-2.40186, "omega":-0.03292, "ax":-0.03374, "ay":0.03179, "alpha":0.05554, "fx":[-0.81411,-0.31948,-0.45056,-0.66558], "fy":[0.49908,0.35936,0.56075,0.70053]}, + {"t":0.66069, "x":2.68414, "y":2.09348, "heading":0.95986, "vx":-2.2629, "vy":-2.40069, "omega":-0.03088, "ax":-0.03149, "ay":0.02969, "alpha":0.01597, "fx":[-0.55839,-0.51849,-0.4913,-0.53128], "fy":[0.50953,0.39797,0.52281,0.54964]}, + {"t":0.69739, "x":2.60106, "y":2.00538, "heading":0.95873, "vx":-2.26406, "vy":-2.3996, "omega":-0.0303, "ax":-0.02604, "ay":0.02458, "alpha":-0.00297, "fx":[-0.41387,-0.45443,-0.44208,-0.42618], "fy":[0.40986,0.41386,0.40938,0.40553]}, + {"t":0.7341, "x":2.51794, "y":1.91732, "heading":0.95762, "vx":-2.26501, "vy":-2.3987, "omega":-0.0304, "ax":0.04863, "ay":-0.04587, "alpha":-0.01897, "fx":[0.87245,0.80026,0.74903,0.82088], "fy":[-0.74469,-0.70453,-0.77135,-0.83814]}, + {"t":0.7708, "x":2.43484, "y":1.82924, "heading":0.9565, "vx":-2.26323, "vy":-2.40038, "omega":-0.0311, "ax":0.34526, "ay":-0.32383, "alpha":-0.04236, "fx":[5.95402,5.57586,5.65987,5.83138], "fy":[-5.37665,-5.27485,-5.41962,-5.52134]}, + {"t":0.80751, "x":2.352, "y":1.74092, "heading":0.95536, "vx":-2.25056, "vy":-2.41227, "omega":-0.03266, "ax":1.08437, "ay":-0.9951, "alpha":-0.05762, "fx":[18.43827,18.01358,17.71566,18.13618], "fy":[-16.34028,-16.74949,-16.4735,-16.78831]}, + {"t":0.84421, "x":2.27012, "y":1.65171, "heading":0.95416, "vx":-2.21075, "vy":-2.44879, "omega":-0.03477, "ax":3.18005, "ay":-1.91494, "alpha":-0.21279, "fx":[53.47773,53.71022,51.99061,52.86107], "fy":[-31.78644,-31.12878,-32.05549,-32.71409]}, + {"t":0.88092, "x":2.19112, "y":1.56053, "heading":0.95289, "vx":-2.09403, "vy":-2.51908, "omega":-0.04258, "ax":4.21453, "ay":4.23432, "alpha":-0.39633, "fx":[71.16098,69.74593,69.47122,70.63878], "fy":[70.11904,72.88821,70.14141,69.18783]}, + {"t":0.91762, "x":2.11709, "y":1.47092, "heading":0.95132, "vx":-1.93934, "vy":-2.36366, "omega":-0.05713, "ax":4.16305, "ay":4.30326, "alpha":-0.17162, "fx":[69.91242,69.45209,68.68143,69.538], "fy":[71.7692,72.25678,71.80866,71.09842]}, + {"t":0.95433, "x":2.04872, "y":1.38706, "heading":0.94923, "vx":-1.78653, "vy":-2.20571, "omega":-0.06343, "ax":4.14682, "ay":4.32481, "alpha":-0.05253, "fx":[69.19157,68.98594,69.16232,69.16225], "fy":[71.92997,72.60162,71.90833,71.93008]}, + {"t":0.99103, "x":1.98593, "y":1.30902, "heading":0.9469, "vx":-1.63432, "vy":-2.04697, "omega":-0.06536, "ax":4.13858, "ay":4.33562, "alpha":0.28082, "fx":[68.05579,69.49308,69.83003,68.57336], "fy":[72.38771,71.22746,72.22636,73.24916]}, + {"t":1.02774, "x":1.92873, "y":1.2368, "heading":0.9445, "vx":-1.48242, "vy":-1.88783, "omega":-0.05505, "ax":4.13369, "ay":4.34201, "alpha":0.58518, "fx":[66.90563,69.65869,70.96066,68.1015], "fy":[72.43052,70.53271,72.11023,74.44367]}, + {"t":1.06444, "x":1.87711, "y":1.17044, "heading":0.94248, "vx":-1.33069, "vy":-1.72845, "omega":-0.03357, "ax":4.04925, "ay":4.41502, "alpha":0.39151, "fx":[66.09943,67.9433,68.99212,66.96157], "fy":[73.55942,72.5208,73.33638,74.96855]}, + {"t":1.09754, "x":1.83528, "y":1.11565, "heading":0.94137, "vx":-1.19667, "vy":-1.58233, "omega":-0.02061, "ax":3.92127, "ay":4.53598, "alpha":0.33193, "fx":[64.07694,65.81386,66.48029,65.09149], "fy":[75.97041,74.41101,75.4672,76.60201]}, + {"t":1.13064, "x":1.79782, "y":1.06576, "heading":0.94068, "vx":-1.06688, "vy":-1.43219, "omega":-0.00963, "ax":3.81761, "ay":4.62416, "alpha":0.24049, "fx":[62.77726,63.91987,64.54592,63.30761], "fy":[77.10119,76.38555,76.94461,77.8989]}, + {"t":1.16374, "x":1.7646, "y":1.02089, "heading":0.94037, "vx":-0.94052, "vy":-1.27914, "omega":-0.00167, "ax":3.73346, "ay":4.69283, "alpha":0.19306, "fx":[61.43279,62.48706,62.88985,62.13033], "fy":[78.45742,77.54235,78.13781,78.77139]}, + {"t":1.19684, "x":1.73552, "y":0.98112, "heading":0.94031, "vx":-0.81695, "vy":-1.12382, "omega":0.00472, "ax":3.66443, "ay":4.74731, "alpha":0.12617, "fx":[60.62161,61.22177,61.58269,60.91087], "fy":[79.13975,78.79334,79.05199,79.55656]}, + {"t":1.22993, "x":1.71048, "y":0.94652, "heading":0.94047, "vx":-0.69567, "vy":-0.96669, "omega":0.0089, "ax":3.60638, "ay":4.79187, "alpha":0.08482, "fx":[59.71036,60.22051,60.40144,60.13445], "fy":[80.02979,79.57204,79.82898,80.08158]}, + {"t":1.26303, "x":1.68943, "y":0.91715, "heading":0.94076, "vx":-0.5763, "vy":-0.80809, "omega":0.01171, "ax":3.55707, "ay":4.82884, "alpha":0.0269, "fx":[59.18323,59.30573,59.43375,59.25614], "fy":[80.4789,80.46323,80.45151,80.58437]}, + {"t":1.29613, "x":1.67231, "y":0.89305, "heading":0.94115, "vx":-0.45857, "vy":-0.64826, "omega":0.0126, "ax":3.51466, "ay":4.86002, "alpha":-0.01428, "fx":[58.55075,58.56077,58.5278,58.71139], "fy":[81.11479,81.03998,80.99616,80.90613]}, + {"t":1.32923, "x":1.65906, "y":0.87426, "heading":0.94157, "vx":-0.34224, "vy":-0.4874, "omega":0.01213, "ax":3.4778, "ay":4.88666, "alpha":-0.07152, "fx":[58.21372,57.85725,57.75377,58.06826], "fy":[81.42039,81.73564,81.45034,81.22664]}, + {"t":1.36233, "x":1.64963, "y":0.8608, "heading":0.94197, "vx":-0.22713, "vy":-0.32567, "omega":0.00976, "ax":3.44549, "ay":4.90966, "alpha":-0.12401, "fx":[57.86303,57.24986,57.02458,57.60093], "fy":[81.79076,82.28548,81.85019,81.44039]}, + {"t":1.39543, "x":1.644, "y":0.85271, "heading":0.94229, "vx":-0.11309, "vy":-0.16316, "omega":0.00565, "ax":3.41694, "ay":4.92972, "alpha":-0.17083, "fx":[57.55605,56.78613,56.32158,57.17096], "fy":[82.13772,82.72012,82.22775,81.61865]}, + {"t":1.42852, "x":1.64213, "y":0.85001, "heading":0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/CTofetchPSlow.traj b/src/main/deploy/choreo/CTofetchPSlow.traj index 8baeb58f..44a3890b 100644 --- a/src/main/deploy/choreo/CTofetchPSlow.traj +++ b/src/main/deploy/choreo/CTofetchPSlow.traj @@ -3,7 +3,7 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":3.6132737518457456, "y":3.1185381922643174, "heading":1.0471975511965976, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.5561852518457457, "y":3.0196580097364216, "heading":1.0471975511965976, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.9328655004501345, "y":1.1461, "heading":0.942477796076938, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.69577419757843, "y":0.8236, "heading":0.942477796076938, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ @@ -16,9 +16,9 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"C.x", "val":3.5561852518457457}, "y":{"exp":"C.y", "val":3.0196580097364216}, "heading":{"exp":"C.heading", "val":1.0471975511965976}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"C.x", "val":3.5561852518457457}, "y":{"exp":"C.y", "val":3.0196580097364216}, "heading":{"exp":"C.heading", "val":1.0471975511965976}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.9328655004501343 m", "val":1.9328655004501345}, "y":{"exp":"1.1461 m", "val":1.1461}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"fetchP.x", "val":1.69577419757843}, "y":{"exp":"fetchP.y", "val":0.8236}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"fetchP.x", "val":1.6421304941177368}, "y":{"exp":"fetchP.y", "val":0.8500130772590637}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -32,53 +32,52 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.14821,1.54848], + "waypoints":[0.0,1.11865,1.51893], "samples":[ - {"t":0.0, "x":3.61327, "y":3.11854, "heading":1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.31515, "ay":-3.73918, "alpha":-1.71088, "fx":[-49.5891,-58.05031,-60.70161,-52.70665], "fy":[-61.94533,-56.46385,-62.82903,-68.08303]}, - {"t":0.03588, "x":3.61114, "y":3.11613, "heading":1.0472, "vx":-0.11895, "vy":-0.13417, "omega":-0.06139, "ax":-3.31588, "ay":-3.74033, "alpha":-1.32292, "fx":[-50.9189,-57.40661,-59.48971,-53.28139], "fy":[-62.03736,-57.81178,-62.73169,-66.8169]}, - {"t":0.07176, "x":3.60474, "y":3.10891, "heading":1.04499, "vx":-0.23793, "vy":-0.26838, "omega":-0.10886, "ax":-3.31566, "ay":-3.74042, "alpha":-0.99329, "fx":[-52.02003,-56.7904,-58.47222,-53.79894], "fy":[-62.09944,-58.95904,-62.61107,-65.73438]}, - {"t":0.10764, "x":3.59407, "y":3.09687, "heading":1.04109, "vx":-0.3569, "vy":-0.40259, "omega":-0.1445, "ax":-3.31541, "ay":-3.74052, "alpha":-0.72673, "fx":[-52.88676,-56.44867,-57.57164,-54.15781], "fy":[-62.17305,-59.85742,-62.56012,-64.82026]}, - {"t":0.14353, "x":3.57912, "y":3.08002, "heading":1.0359, "vx":-0.47587, "vy":-0.5368, "omega":-0.17057, "ax":-3.31512, "ay":-3.74064, "alpha":-0.48016, "fx":[-53.68539,-55.96999,-56.83441,-54.55628], "fy":[-62.20608,-60.74415,-62.4764,-63.99204]}, - {"t":0.17941, "x":3.55992, "y":3.05835, "heading":1.02978, "vx":-0.59482, "vy":-0.67102, "omega":-0.1878, "ax":-3.31481, "ay":-3.74077, "alpha":-0.29457, "fx":[-54.29359,-55.78322,-56.1623,-54.78596], "fy":[-62.28243,-61.33773,-62.44506,-63.36209]}, - {"t":0.21529, "x":3.53644, "y":3.03186, "heading":1.02304, "vx":-0.71376, "vy":-0.80525, "omega":-0.19837, "ax":-3.31445, "ay":-3.74092, "alpha":-0.09451, "fx":[-54.9201,-55.36788,-55.5909,-55.1221], "fy":[-62.28009,-62.0889,-62.39902,-62.66933]}, - {"t":0.25117, "x":3.50869, "y":3.00056, "heading":1.01593, "vx":-0.83269, "vy":-0.93948, "omega":-0.20176, "ax":-3.31404, "ay":-3.74109, "alpha":0.01958, "fx":[-55.29133,-55.31548,-55.13246,-55.23432], "fy":[-62.3747,-62.42235,-62.36388,-62.28771]}, - {"t":0.28705, "x":3.47668, "y":2.96444, "heading":1.00869, "vx":-0.9516, "vy":-1.07372, "omega":-0.20106, "ax":-3.31355, "ay":-3.74129, "alpha":0.20036, "fx":[-55.83857,-54.908,-54.64485,-55.5499], "fy":[-62.33188,-63.13776,-62.34983,-61.64273]}, - {"t":0.32293, "x":3.4404, "y":2.92351, "heading":1.00147, "vx":-1.07049, "vy":-1.20796, "omega":-0.19387, "ax":-3.313, "ay":-3.74152, "alpha":0.24901, "fx":[-55.9901,-54.97443,-54.37351,-55.56662], "fy":[-62.45504,-63.2238,-62.30753,-61.49074]}, - {"t":0.35882, "x":3.39986, "y":2.87776, "heading":0.99452, "vx":-1.18937, "vy":-1.34221, "omega":-0.18494, "ax":-3.31233, "ay":-3.7418, "alpha":0.41586, "fx":[-56.48243,-54.56733,-53.95473,-55.85548], "fy":[-62.38693,-63.91998,-62.30734,-60.88158]}, - {"t":0.3947, "x":3.35505, "y":2.82719, "heading":0.98788, "vx":-1.30822, "vy":-1.47647, "omega":-0.17002, "ax":-3.31154, "ay":-3.74212, "alpha":0.41807, "fx":[-56.48163,-54.69007,-53.80919,-55.82646], "fy":[-62.51461,-63.82608,-62.27497,-60.90164]}, - {"t":0.43058, "x":3.30598, "y":2.7718, "heading":0.98178, "vx":-1.42704, "vy":-1.61075, "omega":-0.15502, "ax":-3.31054, "ay":-3.74254, "alpha":0.5552, "fx":[-56.87182,-54.33254,-53.50278,-56.03333], "fy":[-62.45583,-64.44483,-62.25008,-60.39486]}, - {"t":0.46646, "x":3.25264, "y":2.71159, "heading":0.97622, "vx":-1.54583, "vy":-1.74503, "omega":-0.13509, "ax":-3.30933, "ay":-3.74304, "alpha":0.52971, "fx":[-56.77277,-54.51295,-53.39591,-55.97785], "fy":[-62.56155,-64.23081,-62.27147,-60.51457]}, - {"t":0.50234, "x":3.19505, "y":2.64657, "heading":0.97137, "vx":-1.66458, "vy":-1.87934, "omega":-0.11609, "ax":-3.30768, "ay":-3.74374, "alpha":0.58914, "fx":[-57.18374,-54.33895,-53.11358,-55.91344], "fy":[-62.20299,-64.43692,-62.49522,-60.48997]}, - {"t":0.53822, "x":3.13319, "y":2.57673, "heading":0.96721, "vx":-1.78326, "vy":-2.01367, "omega":-0.09495, "ax":-3.30555, "ay":-3.7446, "alpha":0.56877, "fx":[-56.81303,-54.39957,-53.22225,-55.97267], "fy":[-62.60921,-64.41531,-62.26671,-60.39166]}, - {"t":0.57411, "x":3.06708, "y":2.50206, "heading":0.9638, "vx":-1.90187, "vy":-2.14803, "omega":-0.07454, "ax":-3.30239, "ay":-3.74594, "alpha":0.57165, "fx":[-56.80911,-54.23825,-53.2917,-55.858], "fy":[-62.65443,-64.54344,-62.12864,-60.44575]}, - {"t":0.60999, "x":2.99671, "y":2.42257, "heading":0.96112, "vx":-2.02036, "vy":-2.28244, "omega":-0.05403, "ax":-3.29766, "ay":-3.7479, "alpha":0.50275, "fx":[-56.4167,-54.42085,-53.33152,-55.71263], "fy":[-62.69731,-64.28716,-62.27582,-60.64231]}, - {"t":0.64587, "x":2.92209, "y":2.33826, "heading":0.95918, "vx":-2.13869, "vy":-2.41693, "omega":-0.03599, "ax":-3.28931, "ay":-3.75143, "alpha":0.4472, "fx":[-56.16809,-54.20281,-53.52139,-55.43247], "fy":[-62.71814,-64.29502,-62.11662,-61.00833]}, - {"t":0.68175, "x":2.84323, "y":2.24913, "heading":0.95789, "vx":-2.25672, "vy":-2.55153, "omega":-0.01994, "ax":-3.27114, "ay":-3.75924, "alpha":0.33433, "fx":[-55.54336,-53.3183,-53.95536,-55.29646], "fy":[-62.7271,-64.13903,-62.32608,-61.46643]}, - {"t":0.71763, "x":2.76015, "y":2.15515, "heading":0.95718, "vx":-2.37409, "vy":-2.68642, "omega":-0.00795, "ax":-3.20355, "ay":-3.7891, "alpha":0.22042, "fx":[-53.88001,-53.07558,-52.88291,-53.76805], "fy":[-63.30168,-64.29991,-62.58938,-62.4589]}, - {"t":0.75351, "x":2.6729, "y":2.05632, "heading":0.95689, "vx":-2.48904, "vy":-2.82238, "omega":-0.00004, "ax":1.39383, "ay":-1.69195, "alpha":0.00097, "fx":[23.16037,24.4242,22.82912,22.52448], "fy":[-28.51644,-28.15919,-27.88823,-28.25222]}, - {"t":0.7894, "x":2.58449, "y":1.95396, "heading":0.95689, "vx":-2.43902, "vy":-2.88309, "omega":0.0, "ax":3.90646, "ay":2.85521, "alpha":-0.16665, "fx":[65.55882,65.23172,64.65749,65.0271], "fy":[47.07598,48.53066,47.83878,46.93451]}, - {"t":0.82528, "x":2.49949, "y":1.85235, "heading":0.95689, "vx":-2.29885, "vy":-2.78064, "omega":-0.00598, "ax":3.4272, "ay":3.60557, "alpha":-0.27661, "fx":[58.19669,56.63148,56.37496,57.31618], "fy":[59.84702,61.20845,60.12307,59.23368]}, - {"t":0.86116, "x":2.41921, "y":1.7549, "heading":0.95668, "vx":-2.17588, "vy":-2.65127, "omega":-0.01591, "ax":3.3786, "ay":3.66717, "alpha":-0.41417, "fx":[57.64425,55.99942,55.01429,56.62033], "fy":[60.75629,62.8559,61.17734,59.73008]}, - {"t":0.89704, "x":2.34331, "y":1.66212, "heading":0.95611, "vx":-2.05465, "vy":-2.51968, "omega":-0.03077, "ax":3.36049, "ay":3.68964, "alpha":-0.35096, "fx":[57.17138,55.99771,54.74312,56.15898], "fy":[61.53759,62.71246,61.49912,60.26902]}, - {"t":0.93292, "x":2.27175, "y":1.57409, "heading":0.955, "vx":-1.93407, "vy":-2.38729, "omega":-0.04336, "ax":3.35103, "ay":3.70128, "alpha":-0.36384, "fx":[57.10527,55.59032,54.62069,56.12401], "fy":[61.57677,63.04766,61.64307,60.52702]}, - {"t":0.9688, "x":2.20451, "y":1.49081, "heading":0.95345, "vx":-1.81383, "vy":-2.25448, "omega":-0.05642, "ax":3.34519, "ay":3.70842, "alpha":-0.20416, "fx":[56.33223,55.80784,55.08058,55.83035], "fy":[61.98403,62.57147,61.69114,61.02397]}, - {"t":1.00468, "x":2.14158, "y":1.41231, "heading":0.95142, "vx":-1.6938, "vy":-2.12142, "omega":-0.06374, "ax":3.34126, "ay":3.71322, "alpha":-0.10879, "fx":[56.04476,55.61671,55.37968,55.74759], "fy":[61.91941,62.37926,61.7102,61.58182]}, - {"t":1.04057, "x":2.08295, "y":1.33858, "heading":0.94913, "vx":-1.57391, "vy":-1.98818, "omega":-0.06765, "ax":3.33838, "ay":3.71672, "alpha":0.10199, "fx":[55.21022,55.82383,56.0147,55.54776], "fy":[62.07412,61.63765,61.86597,62.24605]}, - {"t":1.07645, "x":2.02863, "y":1.26963, "heading":0.94671, "vx":-1.45412, "vy":-1.85482, "omega":-0.06399, "ax":3.33623, "ay":3.71933, "alpha":0.28106, "fx":[54.54169,55.81731,56.70581,55.38831], "fy":[61.98417,61.20596,61.8992,62.90863]}, - {"t":1.11233, "x":1.9786, "y":1.20547, "heading":0.94441, "vx":-1.33442, "vy":-1.72137, "omega":-0.0539, "ax":3.33451, "ay":3.7214, "alpha":0.52313, "fx":[53.85779,56.02402,57.21941,55.23761], "fy":[61.903,60.13087,62.20936,63.89266]}, - {"t":1.14821, "x":1.93287, "y":1.1461, "heading":0.94248, "vx":-1.21477, "vy":-1.58784, "omega":-0.03513, "ax":3.29178, "ay":3.75521, "alpha":0.3813, "fx":[53.44214,55.18259,56.31538,54.5493], "fy":[62.4454,61.50709,62.61245,63.82537]}, - {"t":1.18157, "x":1.89418, "y":1.09523, "heading":0.94131, "vx":-1.10497, "vy":-1.46258, "omega":-0.02241, "ax":3.22344, "ay":3.81849, "alpha":0.30536, "fx":[52.61579,53.97844,54.74058,53.59769], "fy":[63.68843,62.55508,63.74611,64.61966]}, - {"t":1.21492, "x":1.85911, "y":1.04856, "heading":0.94056, "vx":-0.99745, "vy":-1.33521, "omega":-0.01223, "ax":3.1634, "ay":3.86866, "alpha":0.25192, "fx":[51.81905,52.96733,53.65634,52.48699], "fy":[64.38648,63.73273,64.52398,65.31145]}, - {"t":1.24828, "x":1.8276, "y":1.00618, "heading":0.94015, "vx":-0.89193, "vy":-1.20617, "omega":-0.00382, "ax":3.11146, "ay":3.91081, "alpha":0.19311, "fx":[51.15087,52.04136,52.49388,51.77994], "fy":[65.22738,64.49146,65.24949,65.79669]}, - {"t":1.28163, "x":1.79958, "y":0.96812, "heading":0.94002, "vx":-0.78815, "vy":-1.07572, "omega":0.00262, "ax":3.06628, "ay":3.94654, "alpha":0.14496, "fx":[50.58671,51.25151,51.6535,50.96174], "fy":[65.71491,65.368,65.80122,66.26372]}, - {"t":1.31499, "x":1.775, "y":0.93444, "heading":0.94011, "vx":-0.68587, "vy":-0.94408, "omega":0.00745, "ax":3.02652, "ay":3.9773, "alpha":0.10026, "fx":[50.05821,50.54112,50.77337,50.42966], "fy":[66.35155,65.92943,66.31765,66.60002]}, - {"t":1.34835, "x":1.75381, "y":0.90516, "heading":0.94036, "vx":-0.58492, "vy":-0.81142, "omega":0.0108, "ax":2.99134, "ay":4.00398, "alpha":0.05349, "fx":[49.65988,49.91092,50.08063,49.80545], "fy":[66.70579,66.6166,66.73495,66.92056]}, - {"t":1.3817, "x":1.73596, "y":0.88032, "heading":0.94072, "vx":-0.48514, "vy":-0.67786, "omega":0.01258, "ax":2.95997, "ay":4.02737, "alpha":0.01485, "fx":[49.24359,49.34659,49.38216,49.39291], "fy":[67.20502,67.06183,67.11958,67.15089]}, - {"t":1.41506, "x":1.72142, "y":0.85995, "heading":0.94114, "vx":-0.38641, "vy":-0.54352, "omega":0.01308, "ax":2.93184, "ay":4.04802, "alpha":-0.0335, "fx":[48.97387,48.83164,48.78135,48.90289], "fy":[67.46763,67.62905,67.4489,67.36851]}, - {"t":1.44841, "x":1.71017, "y":0.84407, "heading":0.94158, "vx":-0.28861, "vy":-0.4085, "omega":0.01196, "ax":2.90649, "ay":4.06637, "alpha":-0.0702, "fx":[48.65022,48.38069,48.20154,48.56695], "fy":[67.87173,68.00087,67.74553,67.51972]}, - {"t":1.48177, "x":1.70216, "y":0.83271, "heading":0.94197, "vx":-0.19166, "vy":-0.27286, "omega":0.00962, "ax":2.8835, "ay":4.08281, "alpha":-0.12403, "fx":[48.48987,47.94176,47.6516,48.18306], "fy":[68.06897,68.4981,68.01337,67.65346]}, - {"t":1.51512, "x":1.69737, "y":0.82588, "heading":0.94229, "vx":-0.09548, "vy":-0.13668, "omega":0.00548, "ax":2.86259, "ay":4.09759, "alpha":-0.16432, "fx":[48.26265,47.62207,47.14205,47.84541], "fy":[68.34193,68.86405,68.2745,67.73879]}, - {"t":1.54848, "x":1.69577, "y":0.8236, "heading":0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":3.55619, "y":3.01966, "heading":1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.35664, "ay":-3.70196, "alpha":-1.68308, "fx":[-50.42003,-58.59166,-61.29087,-53.51205], "fy":[-61.24307,-55.91685,-62.26492,-67.41499]}, + {"t":0.03609, "x":3.554, "y":3.01725, "heading":1.0472, "vx":-0.12113, "vy":-0.13359, "omega":-0.06073, "ax":-3.35711, "ay":-3.70336, "alpha":-1.30562, "fx":[-51.68889,-57.98016,-60.12032,-54.05596], "fy":[-61.36607,-57.24355,-62.1531,-66.1704]}, + {"t":0.07217, "x":3.54744, "y":3.01002, "heading":1.04501, "vx":-0.24227, "vy":-0.26722, "omega":-0.10785, "ax":-3.35656, "ay":-3.70375, "alpha":-0.98296, "fx":[-52.73755,-57.41519,-59.13162,-54.52455], "fy":[-61.43611,-58.4016,-62.02769,-65.0933]}, + {"t":0.10826, "x":3.53652, "y":2.99796, "heading":1.04111, "vx":-0.36339, "vy":-0.40088, "omega":-0.14332, "ax":-3.35595, "ay":-3.70417, "alpha":-0.72183, "fx":[-53.57766,-57.05695,-58.24518,-54.88846], "fy":[-61.55328,-59.26757,-61.96209,-64.20433]}, + {"t":0.14434, "x":3.52122, "y":2.98108, "heading":1.03594, "vx":-0.48449, "vy":-0.53454, "omega":-0.16937, "ax":-3.35526, "ay":-3.70466, "alpha":-0.48005, "fx":[-54.34863,-56.61776,-57.51412,-55.2419], "fy":[-61.60071,-60.15603,-61.86597,-63.39678]}, + {"t":0.18043, "x":3.50155, "y":2.95938, "heading":1.02983, "vx":-0.60557, "vy":-0.66823, "omega":-0.18669, "ax":-3.35448, "ay":-3.70521, "alpha":-0.28925, "fx":[-54.95928,-56.39066,-56.82697,-55.4934], "fy":[-61.69666,-60.769,-61.84262,-62.74786]}, + {"t":0.21651, "x":3.47751, "y":2.93286, "heading":1.02309, "vx":-0.72662, "vy":-0.80193, "omega":-0.19713, "ax":-3.3536, "ay":-3.70582, "alpha":-0.10403, "fx":[-55.54111,-56.03456,-56.27918,-55.75673], "fy":[-61.71769,-61.47497,-61.77411,-62.1305]}, + {"t":0.2526, "x":3.44911, "y":2.90151, "heading":1.01598, "vx":-0.84763, "vy":-0.93566, "omega":-0.20088, "ax":-3.35258, "ay":-3.70654, "alpha":0.03468, "fx":[-55.98445,-55.91178,-55.73251,-55.91463], "fy":[-61.79472,-61.89904,-61.78921,-61.66216]}, + {"t":0.28868, "x":3.41634, "y":2.86533, "heading":1.00873, "vx":-0.96861, "vy":-1.06941, "omega":-0.19963, "ax":-3.35138, "ay":-3.70738, "alpha":0.17865, "fx":[-56.42356,-55.60583,-55.32689,-56.10741], "fy":[-61.79014,-62.4871,-61.73804,-61.18576]}, + {"t":0.32477, "x":3.37921, "y":2.82433, "heading":1.00153, "vx":-1.08955, "vy":-1.20319, "omega":-0.19318, "ax":-3.34997, "ay":-3.70837, "alpha":0.27416, "fx":[-56.72815,-55.57506,-54.88398,-56.18228], "fy":[-61.86038,-62.74501,-61.79087,-60.87094]}, + {"t":0.36085, "x":3.33771, "y":2.77849, "heading":0.99456, "vx":-1.21043, "vy":-1.33701, "omega":-0.18329, "ax":-3.34831, "ay":-3.70953, "alpha":0.38526, "fx":[-57.04777,-55.29019,-54.60138,-56.31938], "fy":[-61.83573,-63.25173,-61.74715,-60.50985]}, + {"t":0.39694, "x":3.29185, "y":2.72783, "heading":0.98794, "vx":-1.33126, "vy":-1.47087, "omega":-0.16939, "ax":-3.34626, "ay":-3.71096, "alpha":0.4416, "fx":[-57.21975,-55.34911,-54.23967,-56.31379], "fy":[-61.91227,-63.35437,-61.83527,-60.33793]}, + {"t":0.43302, "x":3.24163, "y":2.67234, "heading":0.98183, "vx":-1.45201, "vy":-1.60478, "omega":-0.15345, "ax":-3.34372, "ay":-3.71274, "alpha":0.52526, "fx":[-57.42908,-55.04783,-54.07178,-56.40409], "fy":[-61.87184,-63.81541,-61.78975,-60.08118]}, + {"t":0.46911, "x":3.18706, "y":2.61201, "heading":0.97629, "vx":-1.57267, "vy":-1.73875, "omega":-0.1345, "ax":-3.34044, "ay":-3.71503, "alpha":0.54207, "fx":[-57.45131,-55.21088,-53.7667,-56.30497], "fy":[-61.97631,-63.75688,-61.91984,-60.05811]}, + {"t":0.5052, "x":3.12813, "y":2.54685, "heading":0.97144, "vx":-1.69321, "vy":-1.87281, "omega":-0.11494, "ax":-3.33612, "ay":-3.71803, "alpha":0.55832, "fx":[-57.46882,-55.0472,-53.63885,-56.29125], "fy":[-62.07612,-63.78485,-62.01661,-60.03379]}, + {"t":0.54128, "x":3.06486, "y":2.47685, "heading":0.96729, "vx":-1.81359, "vy":-2.00698, "omega":-0.09479, "ax":-3.33018, "ay":-3.72215, "alpha":0.56224, "fx":[-57.34457,-55.13552,-53.46635,-56.10336], "fy":[-62.08673,-63.92685,-62.05943,-60.11278]}, + {"t":0.57737, "x":2.99725, "y":2.402, "heading":0.96387, "vx":-1.93376, "vy":-2.14129, "omega":-0.0745, "ax":-3.32145, "ay":-3.72815, "alpha":0.58036, "fx":[-57.20577,-54.63003,-53.58849,-56.0437], "fy":[-62.0512,-64.39751,-61.97865,-60.15885]}, + {"t":0.61345, "x":2.92531, "y":2.32231, "heading":0.96118, "vx":-2.05362, "vy":-2.27583, "omega":-0.05356, "ax":-3.30711, "ay":-3.73799, "alpha":0.4785, "fx":[-56.64796,-54.97392,-53.33348,-55.55655], "fy":[-62.36736,-63.916,-62.30895,-60.64943]}, + {"t":0.64954, "x":2.84905, "y":2.23775, "heading":0.95925, "vx":-2.17296, "vy":-2.41071, "omega":-0.03629, "ax":-3.28075, "ay":-3.75565, "alpha":0.45072, "fx":[-56.08467,-54.10566,-53.38507,-55.17867], "fy":[-62.46098,-64.48743,-62.38628,-61.08453]}, + {"t":0.68562, "x":2.7685, "y":2.14831, "heading":0.95794, "vx":-2.29134, "vy":-2.54624, "omega":-0.02003, "ax":-3.21102, "ay":-3.80098, "alpha":0.28272, "fx":[-54.30141,-53.96389,-52.23757,-53.60195], "fy":[-63.41355,-64.30694,-63.35724,-62.36403]}, + {"t":0.72171, "x":2.68372, "y":2.05396, "heading":0.95722, "vx":-2.40722, "vy":-2.6834, "omega":-0.00983, "ax":-2.24339, "ay":-4.31279, "alpha":0.27224, "fx":[-38.04024,-37.32709,-36.68231,-37.53544], "fy":[-71.67475,-73.36557,-71.55409,-70.97424]}, + {"t":0.75779, "x":2.5954, "y":1.95432, "heading":0.95686, "vx":-2.48817, "vy":-2.83903, "omega":0.0, "ax":4.33212, "ay":2.11002, "alpha":-0.09286, "fx":[72.29894,72.78957,71.70565,72.06317], "fy":[35.17462,35.6212,35.09121,34.80489]}, + {"t":0.79388, "x":2.50843, "y":1.85324, "heading":0.95686, "vx":-2.33184, "vy":-2.76288, "omega":-0.00335, "ax":3.5316, "ay":3.50344, "alpha":-0.33646, "fx":[59.75009,58.54782,58.02595,59.15674], "fy":[58.17727,60.09291,58.10104,57.23167]}, + {"t":0.82996, "x":2.42659, "y":1.75582, "heading":0.95674, "vx":-2.2044, "vy":-2.63646, "omega":-0.01549, "ax":3.45959, "ay":3.5911, "alpha":-0.34014, "fx":[58.69283,57.75691,56.32608,57.90277], "fy":[59.93278,60.99627,59.87565,58.64281]}, + {"t":0.86605, "x":2.34929, "y":1.66302, "heading":0.95618, "vx":-2.07956, "vy":-2.50688, "omega":-0.02777, "ax":3.4313, "ay":3.6241, "alpha":-0.41144, "fx":[58.45143,56.72512,55.98804,57.62812], "fy":[60.29041,62.12089,60.23916,58.99724]}, + {"t":0.90213, "x":2.27648, "y":1.57492, "heading":0.95518, "vx":-1.95574, "vy":-2.3761, "omega":-0.04261, "ax":3.41666, "ay":3.64097, "alpha":-0.32316, "fx":[57.94956,56.93789,55.72324,57.20566], "fy":[60.75228,61.77785,60.7006,59.54223]}, + {"t":0.93822, "x":2.20813, "y":1.49155, "heading":0.95364, "vx":-1.83245, "vy":-2.24471, "omega":-0.05428, "ax":3.40712, "ay":3.65177, "alpha":-0.2803, "fx":[57.66105,56.46703,55.96743,57.08458], "fy":[60.78742,62.03271,60.75224,59.92054]}, + {"t":0.97431, "x":2.14423, "y":1.41293, "heading":0.95169, "vx":-1.7095, "vy":-2.11294, "omega":-0.06439, "ax":3.40084, "ay":3.65887, "alpha":-0.10756, "fx":[56.99003,56.82356,56.22176,56.72611], "fy":[61.0338,61.33994,60.99716,60.5956]}, + {"t":1.01039, "x":2.08475, "y":1.33906, "heading":0.94936, "vx":-1.58678, "vy":-1.98091, "omega":-0.06827, "ax":3.39622, "ay":3.66406, "alpha":0.04337, "fx":[56.41975,56.65085,56.83487,56.54804], "fy":[61.01615,61.05942,61.00156,61.23533]}, + {"t":1.04648, "x":2.02971, "y":1.26997, "heading":0.9469, "vx":-1.46423, "vy":-1.84869, "omega":-0.06671, "ax":3.39274, "ay":3.66796, "alpha":0.30229, "fx":[55.51476,57.05401,57.47906,56.17374], "fy":[61.16252,60.06931,61.15931,62.18136]}, + {"t":1.08256, "x":1.97908, "y":1.20564, "heading":0.94449, "vx":-1.3418, "vy":-1.71633, "omega":-0.0558, "ax":3.39, "ay":3.67102, "alpha":0.56807, "fx":[54.5457,57.12301,58.47954,55.89051], "fy":[61.15344,59.3401,61.14805,63.13518]}, + {"t":1.11865, "x":1.93287, "y":1.1461, "heading":0.94248, "vx":-1.21947, "vy":-1.58386, "omega":-0.0353, "ax":3.34181, "ay":3.71089, "alpha":0.39575, "fx":[54.29452,56.11459,57.14413,55.27199], "fy":[61.78085,60.66988,61.77708,63.20722]}, + {"t":1.152, "x":1.89405, "y":1.09533, "heading":0.9413, "vx":-1.108, "vy":-1.46007, "omega":-0.0221, "ax":3.26434, "ay":3.78362, "alpha":0.31764, "fx":[53.23254,54.72171,55.46351,54.24211], "fy":[63.18356,61.95235,63.05285,64.09608]}, + {"t":1.18536, "x":1.8589, "y":1.04873, "heading":0.94056, "vx":-0.99911, "vy":-1.33386, "omega":-0.0115, "ax":3.19567, "ay":3.84209, "alpha":0.24358, "fx":[52.40519,53.52445,54.14907,53.00212], "fy":[64.02184,63.29155,64.00451,64.86505]}, + {"t":1.21872, "x":1.82735, "y":1.00638, "heading":0.94018, "vx":-0.89251, "vy":-1.2057, "omega":-0.00338, "ax":3.13611, "ay":3.89109, "alpha":0.18931, "fx":[51.54594,52.45563,52.9064,52.20181], "fy":[64.96278,64.19389,64.84161,65.45222]}, + {"t":1.25208, "x":1.79933, "y":0.96832, "heading":0.94007, "vx":-0.7879, "vy":-1.0759, "omega":0.00294, "ax":3.08389, "ay":3.93281, "alpha":0.13538, "fx":[50.91503,51.54381,51.91249,51.2565], "fy":[65.53653,65.16225,65.52432,66.00941]}, + {"t":1.28543, "x":1.77476, "y":0.93462, "heading":0.94016, "vx":-0.68503, "vy":-0.94471, "omega":0.00745, "ax":3.03782, "ay":3.96869, "alpha":0.09518, "fx":[50.24632,50.7246,50.95499,50.63024], "fy":[66.24851,65.81005,66.13417,66.4316]}, + {"t":1.31879, "x":1.7536, "y":0.90532, "heading":0.94041, "vx":-0.58369, "vy":-0.81233, "omega":0.01063, "ax":2.99686, "ay":3.99987, "alpha":0.04859, "fx":[49.767,49.99952,50.15795,49.90018], "fy":[66.65404,66.56482,66.64772,66.83689]}, + {"t":1.35215, "x":1.7358, "y":0.88044, "heading":0.94077, "vx":-0.48373, "vy":-0.6789, "omega":0.01225, "ax":2.96023, "ay":4.02719, "alpha":0.01372, "fx":[49.24579,49.34798,49.3829,49.40587], "fy":[67.21116,67.06202,67.11089,67.14104]}, + {"t":1.38551, "x":1.72131, "y":0.86004, "heading":0.94118, "vx":-0.38498, "vy":-0.54457, "omega":0.01271, "ax":2.9273, "ay":4.05131, "alpha":-0.0329, "fx":[48.89382,48.752,48.7111,48.82958], "fy":[67.51047,67.68901,67.50893,67.42542]}, + {"t":1.41886, "x":1.71009, "y":0.84413, "heading":0.9416, "vx":-0.28733, "vy":-0.40943, "omega":0.01161, "ax":2.89752, "ay":4.07277, "alpha":-0.06806, "fx":[48.49806,48.2214,48.05692,48.42502], "fy":[67.96899,68.09312,67.86957,67.63283]}, + {"t":1.45222, "x":1.70212, "y":0.83274, "heading":0.94199, "vx":-0.19068, "vy":-0.27357, "omega":0.00934, "ax":2.87049, "ay":4.09197, "alpha":-0.11978, "fx":[48.25448,47.71137,47.45518,47.97778], "fy":[68.18784,68.64839,68.18948,67.81891]}, + {"t":1.48558, "x":1.69736, "y":0.82589, "heading":0.9423, "vx":-0.09493, "vy":-0.13707, "omega":0.00534, "ax":2.84583, "ay":4.10925, "alpha":-0.16022, "fx":[47.99044,47.29394,46.86731,47.60296], "fy":[68.50165,69.02825,68.50104,67.96569]}, + {"t":1.51893, "x":1.69577, "y":0.8236, "heading":0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/DTofetchP.traj b/src/main/deploy/choreo/DTofetchP.traj index 9b907753..9f938579 100644 --- a/src/main/deploy/choreo/DTofetchP.traj +++ b/src/main/deploy/choreo/DTofetchP.traj @@ -3,28 +3,28 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":3.84197363509461, "y":2.854658009736421, "heading":1.0471975511965976, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.909953474998474, "y":1.1158, "heading":0.942477796076938, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.69577419757843, "y":0.8236, "heading":0.942477796076938, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.84197363509461, "y":2.854658009736421, "heading":1.0471975511965976, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.827253818511963, "y":1.1088320016860962, "heading":0.942477796076938, "intervals":10, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.6421304941177368, "y":0.8500130772590637, "heading":0.942477796076938, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":4.0}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":3.0}}, "enabled":true}], + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":3.3}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"D.x", "val":3.84197363509461}, "y":{"exp":"D.y", "val":2.854658009736421}, "heading":{"exp":"D.heading", "val":1.0471975511965976}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.9099534749984741 m", "val":1.909953474998474}, "y":{"exp":"1.1158 m", "val":1.1158}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"fetchP.x", "val":1.69577419757843}, "y":{"exp":"fetchP.y", "val":0.8236}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"D.x", "val":3.84197363509461}, "y":{"exp":"D.y", "val":2.854658009736421}, "heading":{"exp":"D.heading", "val":1.0471975511965976}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.827253818511963 m", "val":1.827253818511963}, "y":{"exp":"1.1088320016860962 m", "val":1.1088320016860962}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":10, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"fetchP.x", "val":1.6421304941177368}, "y":{"exp":"fetchP.y", "val":0.8500130772590637}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"4 m / s", "val":4.0}}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"maxAccel", "val":3.0}}}, "enabled":true}], + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.3 m / s", "val":3.3}}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -32,63 +32,49 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.50154,1.99377], + "waypoints":[0.0,1.13263,1.45877], "samples":[ - {"t":0.0, "x":3.84197, "y":2.85466, "heading":1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.33679, "ay":-1.87874, "alpha":-0.83799, "fx":[-36.20106,-40.02041,-41.57035,-38.02099], "fy":[-30.72653,-28.44775,-31.92276,-34.17349]}, - {"t":0.03754, "x":3.84033, "y":2.85333, "heading":1.0472, "vx":-0.08772, "vy":-0.07052, "omega":-0.03146, "ax":-2.33666, "ay":-1.88015, "alpha":-0.64911, "fx":[-36.8327,-39.74512,-40.99104,-38.23491], "fy":[-30.88283,-29.11356,-31.80834,-33.56005]}, - {"t":0.07508, "x":3.83539, "y":2.84936, "heading":1.04602, "vx":-0.17543, "vy":-0.1411, "omega":-0.05582, "ax":-2.33582, "ay":-1.88113, "alpha":-0.48814, "fx":[-37.31778,-39.46465,-40.5551,-38.41064], "fy":[-30.9766,-29.76041,-31.6787,-33.01436]}, - {"t":0.11262, "x":3.82716, "y":2.84274, "heading":1.04392, "vx":-0.26312, "vy":-0.21172, "omega":-0.07415, "ax":-2.33492, "ay":-1.88219, "alpha":-0.38102, "fx":[-37.66536,-39.43898,-40.096,-38.48741], "fy":[-31.11267,-30.06826,-31.6457,-32.67434]}, - {"t":0.15015, "x":3.81563, "y":2.83347, "heading":1.04114, "vx":-0.35077, "vy":-0.28237, "omega":-0.08845, "ax":-2.33393, "ay":-1.88335, "alpha":-0.26859, "fx":[-37.99798,-39.19127,-39.81689,-38.61559], "fy":[-31.16784,-30.5518,-31.55252,-32.30622]}, - {"t":0.18769, "x":3.80082, "y":2.82154, "heading":1.03782, "vx":-0.43838, "vy":-0.35307, "omega":-0.09853, "ax":-2.33284, "ay":-1.88462, "alpha":-0.20268, "fx":[-38.20062,-39.23132,-39.48037,-38.63707], "fy":[-31.28373,-30.72146,-31.55618,-32.10172]}, - {"t":0.22523, "x":3.78272, "y":2.80696, "heading":1.03412, "vx":-0.52595, "vy":-0.42382, "omega":-0.10614, "ax":-2.33165, "ay":-1.88602, "alpha":-0.11995, "fx":[-38.43961,-38.99272,-39.30193,-38.73539], "fy":[-31.31056,-31.11348,-31.48635,-31.84599]}, - {"t":0.26277, "x":3.76134, "y":2.78972, "heading":1.03013, "vx":-0.61348, "vy":-0.49462, "omega":-0.11064, "ax":-2.33032, "ay":-1.88757, "alpha":-0.08175, "fx":[-38.54282,-39.08655,-39.03837,-38.71374], "fy":[-31.41879,-31.18668,-31.52045,-31.73345]}, - {"t":0.30031, "x":3.73667, "y":2.76982, "heading":1.02598, "vx":-0.70095, "vy":-0.56547, "omega":-0.11371, "ax":-2.32885, "ay":-1.88928, "alpha":-0.01939, "fx":[-38.72239,-38.84208,-38.92577,-38.79311], "fy":[-31.43363,-31.50856,-31.47319,-31.55847]}, - {"t":0.33785, "x":3.70871, "y":2.74726, "heading":1.02171, "vx":-0.78838, "vy":-0.63639, "omega":-0.11444, "ax":-2.32721, "ay":-1.8912, "alpha":0.00143, "fx":[-38.75295,-38.97953,-38.705,-38.73613], "fy":[-31.53546,-31.5342,-31.52648,-31.50551]}, - {"t":0.37538, "x":3.67748, "y":2.72204, "heading":1.01742, "vx":-0.87573, "vy":-0.70739, "omega":-0.11439, "ax":-2.32535, "ay":-1.89336, "alpha":0.05372, "fx":[-38.89438,-38.71174,-38.63903,-38.80472], "fy":[-31.53353,-31.84909,-31.4856,-31.37729]}, - {"t":0.41292, "x":3.64297, "y":2.69415, "heading":1.01312, "vx":-0.96303, "vy":-0.77846, "omega":-0.11237, "ax":-2.32324, "ay":-1.8958, "alpha":0.05976, "fx":[-38.87343,-38.86881,-38.44551,-38.72173], "fy":[-31.64722,-31.81312,-31.56778,-31.38025]}, - {"t":0.45046, "x":3.60518, "y":2.6636, "heading":1.0089, "vx":-1.05024, "vy":-0.84962, "omega":-0.11013, "ax":-2.32083, "ay":-1.8986, "alpha":0.10429, "fx":[-38.98059,-38.58982,-38.40308,-38.77492], "fy":[-31.64169,-32.12242,-31.53851,-31.29219]}, - {"t":0.488, "x":3.56412, "y":2.63037, "heading":1.00477, "vx":-1.13736, "vy":-0.9209, "omega":-0.10621, "ax":-2.31804, "ay":-1.90181, "alpha":0.09908, "fx":[-38.90264,-38.7983,-38.20839,-38.65323], "fy":[-31.77075,-32.05007,-31.64754,-31.34098]}, - {"t":0.52554, "x":3.51979, "y":2.59446, "heading":1.00078, "vx":-1.22437, "vy":-0.99229, "omega":-0.10249, "ax":-2.31477, "ay":-1.90558, "alpha":0.13973, "fx":[-38.99159,-38.45902,-38.1912,-38.70278], "fy":[-31.76734,-32.37449,-31.63075,-31.28762]}, - {"t":0.56308, "x":3.4722, "y":2.55586, "heading":0.99694, "vx":-1.31127, "vy":-1.06382, "omega":-0.09725, "ax":-2.31091, "ay":-1.91, "alpha":0.12529, "fx":[-38.85962,-38.69994,-37.98599,-38.54168], "fy":[-31.92083,-32.27796,-31.77238,-31.38408]}, - {"t":0.60062, "x":3.42135, "y":2.51459, "heading":0.99329, "vx":-1.39801, "vy":-1.13552, "omega":-0.09254, "ax":-2.30625, "ay":-1.91533, "alpha":0.16875, "fx":[-38.92323,-38.29492,-37.97945,-38.57839], "fy":[-31.9116,-32.69086,-31.75699,-31.35116]}, - {"t":0.63815, "x":3.36724, "y":2.47061, "heading":0.98981, "vx":-1.48459, "vy":-1.20742, "omega":-0.08621, "ax":-2.30054, "ay":-1.92181, "alpha":0.1403, "fx":[-38.7204,-38.5803,-37.73685,-38.35789], "fy":[-32.1253,-32.52805,-31.96437,-31.52501]}, - {"t":0.67569, "x":3.30989, "y":2.42393, "heading":0.98658, "vx":-1.57095, "vy":-1.27956, "omega":-0.08094, "ax":-2.29335, "ay":-1.92992, "alpha":0.19211, "fx":[-38.75147,-38.06444,-37.72738,-38.37267], "fy":[-32.11725,-33.09606,-31.95525,-31.51513]}, - {"t":0.71323, "x":3.24931, "y":2.37454, "heading":0.98354, "vx":-1.65703, "vy":-1.352, "omega":-0.07373, "ax":-2.28406, "ay":-1.94032, "alpha":0.14653, "fx":[-38.44206,-38.3991,-37.40167,-38.05377], "fy":[-32.43794,-32.86044,-32.27232,-31.80593]}, - {"t":0.75077, "x":3.1855, "y":2.32242, "heading":0.98077, "vx":-1.74277, "vy":-1.42484, "omega":-0.06823, "ax":-2.27154, "ay":-1.95417, "alpha":0.19188, "fx":[-38.40367,-37.70678,-37.34453,-38.00679], "fy":[-32.53561,-33.46699,-32.37532,-31.92269]}, - {"t":0.78831, "x":3.11847, "y":2.26756, "heading":0.97821, "vx":-1.82804, "vy":-1.4982, "omega":-0.06103, "ax":-2.25386, "ay":-1.97344, "alpha":0.14676, "fx":[-37.93474,-37.9132,-36.89396,-37.54137], "fy":[-32.98783,-33.41654,-32.82411,-32.35683]}, - {"t":0.82585, "x":3.04826, "y":2.20993, "heading":0.97592, "vx":-1.91265, "vy":-1.57228, "omega":-0.05552, "ax":-2.22688, "ay":-2.00222, "alpha":0.18836, "fx":[-37.64289,-36.97252,-36.61559,-37.25292], "fy":[-33.3257,-34.26952,-33.17115,-32.73808]}, - {"t":0.86339, "x":2.9749, "y":2.14949, "heading":0.97383, "vx":-1.99624, "vy":-1.64744, "omega":-0.04845, "ax":-2.1808, "ay":-2.04966, "alpha":0.15437, "fx":[-36.92292,-35.97881,-35.95686,-36.55321], "fy":[-34.23394,-34.70654,-34.08064,-33.6464]}, - {"t":0.90092, "x":2.89842, "y":2.08621, "heading":0.97201, "vx":-2.07811, "vy":-1.72438, "omega":-0.04265, "ax":-2.08456, "ay":-2.14238, "alpha":0.06833, "fx":[-35.20243,-34.69335,-34.25925,-34.83914], "fy":[-36.01902,-35.45553,-35.88065,-35.4944]}, - {"t":0.93846, "x":2.81895, "y":2.01997, "heading":0.97041, "vx":-2.15636, "vy":-1.8048, "omega":-0.04009, "ax":-1.76662, "ay":-2.39766, "alpha":0.09339, "fx":[-29.67412,-29.67832,-29.02289,-29.41935], "fy":[-40.01817,-40.31404,-39.91103,-39.62796]}, - {"t":0.976, "x":2.73676, "y":1.95053, "heading":0.96891, "vx":-2.22268, "vy":-1.89481, "omega":-0.03658, "ax":2.31374, "ay":-1.7814, "alpha":-0.01672, "fx":[38.41983,38.63417,38.68963,38.53226], "fy":[-29.92008,-29.23049,-29.86647,-29.76349]}, - {"t":1.01354, "x":2.65495, "y":1.87815, "heading":0.96753, "vx":-2.13582, "vy":-1.96168, "omega":-0.03721, "ax":2.70954, "ay":1.24215, "alpha":-0.04148, "fx":[45.19992,45.48563,44.90007,45.08123], "fy":[20.73319,20.87984,20.67691,20.53455]}, - {"t":1.05108, "x":2.57668, "y":1.80538, "heading":0.96614, "vx":-2.03411, "vy":-1.91505, "omega":-0.03877, "ax":2.55918, "ay":1.54589, "alpha":-0.03724, "fx":[42.98685,42.63182,42.30317,42.71924], "fy":[26.03884,25.45244,25.93606,25.64931]}, - {"t":1.08862, "x":2.50213, "y":1.73458, "heading":0.96468, "vx":-1.93804, "vy":-1.85702, "omega":-0.04017, "ax":2.49781, "ay":1.64917, "alpha":-0.12339, "fx":[42.1159,41.23953,41.36911,41.82462], "fy":[27.5433,27.92507,27.42306,27.07218]}, - {"t":1.12615, "x":2.43114, "y":1.66604, "heading":0.96317, "vx":-1.84428, "vy":-1.79511, "omega":-0.0448, "ax":2.46505, "ay":1.70072, "alpha":-0.14461, "fx":[41.49279,40.992,40.69698,41.18302], "fy":[28.31363,29.03029,28.19956,27.85694]}, - {"t":1.16369, "x":2.36364, "y":1.59985, "heading":0.96149, "vx":-1.75174, "vy":-1.73127, "omega":-0.05023, "ax":2.44442, "ay":1.73201, "alpha":-0.10276, "fx":[40.96701,41.11184,40.2303,40.67997], "fy":[28.93745,29.24028,28.82726,28.48183]}, - {"t":1.20123, "x":2.29961, "y":1.53608, "heading":0.95961, "vx":-1.65998, "vy":-1.66625, "omega":-0.05408, "ax":2.43047, "ay":1.7527, "alpha":-0.11952, "fx":[40.84123,40.43108,40.19664,40.58993], "fy":[29.17716,29.79218,29.08917,28.80834]}, - {"t":1.23877, "x":2.23901, "y":1.47477, "heading":0.95758, "vx":-1.56875, "vy":-1.60046, "omega":-0.05857, "ax":2.4203, "ay":1.76754, "alpha":-0.06129, "fx":[40.44886,40.67144,39.99137,40.26957], "fy":[29.50685,29.68344,29.44063,29.22555]}, - {"t":1.27631, "x":2.18182, "y":1.41593, "heading":0.95538, "vx":-1.47789, "vy":-1.53411, "omega":-0.06087, "ax":2.41262, "ay":1.77864, "alpha":-0.06277, "fx":[40.3321,40.1743,40.11648,40.24623], "fy":[29.55498,30.08005,29.52534,29.43581]}, - {"t":1.31385, "x":2.12805, "y":1.3596, "heading":0.95309, "vx":-1.38733, "vy":-1.46734, "omega":-0.06323, "ax":2.4066, "ay":1.78727, "alpha":0.02485, "fx":[39.95864,40.42663,40.07879,40.0033], "fy":[29.79581,29.70814,29.80564,29.86189]}, - {"t":1.35139, "x":2.07766, "y":1.30577, "heading":0.95072, "vx":-1.29699, "vy":-1.40025, "omega":-0.06229, "ax":2.40174, "ay":1.79417, "alpha":0.06125, "fx":[39.76384,40.08446,40.31806,39.97723], "fy":[29.7964,29.851,29.86108,30.12352]}, - {"t":1.38892, "x":2.03067, "y":1.25448, "heading":0.94838, "vx":-1.20683, "vy":-1.3329, "omega":-0.06, "ax":2.39775, "ay":1.79983, "alpha":0.17447, "fx":[39.33122,40.33461,40.44613,39.76513], "fy":[29.9457,29.39234,30.07221,30.59872]}, - {"t":1.42646, "x":1.98705, "y":1.20571, "heading":0.94613, "vx":-1.11682, "vy":-1.26533, "omega":-0.05345, "ax":2.39441, "ay":1.80452, "alpha":0.25583, "fx":[38.99682,40.10385,40.83835,39.71583], "fy":[29.90172,29.33877,30.11083,30.97068]}, - {"t":1.464, "x":1.94682, "y":1.15948, "heading":0.94412, "vx":-1.02694, "vy":-1.1976, "omega":-0.04384, "ax":2.39157, "ay":1.80851, "alpha":0.41963, "fx":[38.42934,40.36477,41.16871,39.50229], "fy":[30.00203,28.6763,30.30508,31.60476]}, - {"t":1.50154, "x":1.90995, "y":1.1158, "heading":0.94248, "vx":-0.93716, "vy":-1.12971, "omega":-0.02809, "ax":2.34326, "ay":1.86809, "alpha":0.27288, "fx":[38.05726,39.2601,40.07479,38.85206], "fy":[30.93028,30.39472,31.15262,32.08333]}, - {"t":1.5367, "x":1.87845, "y":1.07723, "heading":0.94149, "vx":-0.85477, "vy":-1.06403, "omega":-0.0185, "ax":2.25685, "ay":1.97421, "alpha":0.22753, "fx":[36.76724,37.73653,38.34778,37.63059], "fy":[33.04728,32.03476,32.9246,33.62986]}, - {"t":1.57186, "x":1.84979, "y":1.04104, "heading":0.94084, "vx":-0.77543, "vy":-0.99461, "omega":-0.0105, "ax":2.1745, "ay":2.06468, "alpha":0.1654, "fx":[35.63154,36.3692,36.87287,36.11807], "fy":[34.29197,33.97727,34.41595,34.98345]}, - {"t":1.60702, "x":1.82387, "y":1.00735, "heading":0.94047, "vx":-0.69897, "vy":-0.92202, "omega":-0.00468, "ax":2.09764, "ay":2.14282, "alpha":0.13749, "fx":[34.41961,35.02125,35.39824,35.02779], "fy":[35.85088,35.1767,35.71431,36.1375]}, - {"t":1.64218, "x":1.80059, "y":0.97626, "heading":0.94031, "vx":-0.62522, "vy":-0.84668, "omega":0.00015, "ax":2.02615, "ay":2.21064, "alpha":0.09338, "fx":[33.40105,33.84304,34.15807,33.69758], "fy":[36.75344,36.65574,36.82561,37.16666]}, - {"t":1.67734, "x":1.77986, "y":0.94785, "heading":0.94031, "vx":-0.55398, "vy":-0.76895, "omega":0.00344, "ax":1.95979, "ay":2.26976, "alpha":0.07839, "fx":[32.33038,32.68969,32.9109,32.74419], "fy":[37.99762,37.49469,37.8055,38.04563]}, - {"t":1.7125, "x":1.7616, "y":0.92222, "heading":0.94043, "vx":-0.48507, "vy":-0.68915, "omega":0.00619, "ax":1.89825, "ay":2.32155, "alpha":0.0443, "fx":[31.44058,31.67385,31.85463,31.60253], "fy":[38.62519,38.66096,38.66313,38.84749]}, - {"t":1.74766, "x":1.74572, "y":0.89943, "heading":0.94065, "vx":-0.41833, "vy":-0.60753, "omega":0.00775, "ax":1.84118, "ay":2.36714, "alpha":0.03466, "fx":[30.50592,30.68396,30.78696,30.78973], "fy":[39.60776,39.27909,39.42228,39.52696]}, - {"t":1.78282, "x":1.73215, "y":0.87953, "heading":0.94092, "vx":-0.3536, "vy":-0.5243, "omega":0.00897, "ax":1.78824, "ay":2.40744, "alpha":0.0041, "fx":[29.74805,29.81042,29.87915,29.79886], "fy":[40.07451,40.22076,40.08509,40.14332]}, - {"t":1.81798, "x":1.72082, "y":0.86258, "heading":0.94124, "vx":-0.29072, "vy":-0.43965, "omega":0.00911, "ax":1.73908, "ay":2.44325, "alpha":-0.00439, "fx":[28.94657,28.9592,28.95563,29.09736], "fy":[40.87683,40.6855,40.68103,40.66763]}, - {"t":1.85313, "x":1.71167, "y":0.84864, "heading":0.94156, "vx":-0.22958, "vy":-0.35375, "omega":0.00896, "ax":1.69339, "ay":2.47519, "alpha":-0.03438, "fx":[28.30606,28.20112,28.15778,28.24697], "fy":[41.22392,41.4654,41.20745,41.14402]}, - {"t":1.88829, "x":1.70465, "y":0.83773, "heading":0.94187, "vx":-0.17004, "vy":-0.26672, "omega":0.00775, "ax":1.65086, "ay":2.5038, "alpha":-0.04622, "fx":[27.62743,27.46217,27.34359,27.64266], "fy":[41.88642,41.8411,41.67998,41.54137]}, - {"t":1.92345, "x":1.69969, "y":0.8299, "heading":0.94215, "vx":-0.112, "vy":-0.17869, "omega":0.00612, "ax":1.61121, "ay":2.52954, "alpha":-0.07974, "fx":[27.09855,26.79811,26.62458,26.91113], "fy":[42.1509,42.51222,42.10232,41.89958]}, - {"t":1.95861, "x":1.69675, "y":0.82518, "heading":0.94236, "vx":-0.05535, "vy":-0.08975, "omega":0.00332, "ax":1.5742, "ay":2.55278, "alpha":-0.09444, "fx":[26.54364,26.25795,25.88127,26.28157], "fy":[42.59005,42.87161,42.52143,42.23143]}, - {"t":1.99377, "x":1.69577, "y":0.8236, "heading":0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":3.84197, "y":2.85466, "heading":1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.68835, "ay":-3.73893, "alpha":-5.01168, "fx":[-60.58687,-88.67564,-93.60911,-69.73877], "fy":[-62.76837,-45.50037,-61.93169,-79.10416]}, + {"t":0.03775, "x":3.83863, "y":2.85199, "heading":1.0472, "vx":-0.17701, "vy":-0.14116, "omega":-0.18921, "ax":-4.68954, "ay":-3.73989, "alpha":-3.86694, "fx":[-65.06694,-86.00814,-90.05902,-71.55568], "fy":[-62.67071,-49.09795,-62.05047,-75.54941]}, + {"t":0.07551, "x":3.82861, "y":2.844, "heading":1.04005, "vx":-0.35406, "vy":-0.28236, "omega":-0.33521, "ax":-4.68942, "ay":-3.7398, "alpha":-2.52656, "fx":[-71.76404,-83.27156,-84.10977,-73.53618], "fy":[-61.67241,-52.09901,-62.66521,-72.92628]}, + {"t":0.11326, "x":3.8119, "y":2.83067, "heading":1.0274, "vx":-0.5311, "vy":-0.42355, "omega":-0.4306, "ax":-4.68927, "ay":-3.73971, "alpha":-1.69748, "fx":[-72.31871,-83.03716,-83.47937,-73.83621], "fy":[-62.25528,-57.02744,-62.43696,-67.63704]}, + {"t":0.15102, "x":3.7885, "y":2.81202, "heading":1.01114, "vx":-0.70814, "vy":-0.56474, "omega":-0.49468, "ax":-4.68909, "ay":-3.73959, "alpha":-0.32522, "fx":[-78.65419,-80.53848,-77.6794,-75.78775], "fy":[-61.37441,-60.52735,-62.75632,-64.6906]}, + {"t":0.18877, "x":3.75843, "y":2.78803, "heading":0.99247, "vx":-0.88518, "vy":-0.70593, "omega":-0.50696, "ax":-4.68887, "ay":-3.73945, "alpha":-0.39078, "fx":[-77.06998,-80.85927,-78.81885,-75.89685], "fy":[-61.61389,-61.09137,-63.15798,-63.47643]}, + {"t":0.22653, "x":3.72167, "y":2.75871, "heading":0.97333, "vx":-1.0622, "vy":-0.84711, "omega":-0.52172, "ax":-4.68861, "ay":-3.73926, "alpha":2.09238, "fx":[-85.69787,-77.05162,-70.64664,-79.23145], "fy":[-62.03201,-69.58097,-62.00258,-55.71131]}, + {"t":0.26428, "x":3.67822, "y":2.72407, "heading":0.95363, "vx":-1.23922, "vy":-0.98828, "omega":-0.44272, "ax":-4.68825, "ay":-3.73905, "alpha":-0.3364, "fx":[-77.91758,-78.18425,-77.67318,-78.82833], "fy":[-62.24133,-59.73898,-62.60007,-64.73241]}, + {"t":0.30204, "x":3.62809, "y":2.68409, "heading":0.93691, "vx":-1.41622, "vy":-1.12945, "omega":-0.45542, "ax":-4.68782, "ay":-3.7387, "alpha":4.99811, "fx":[-93.67061,-72.51493,-62.36499,-84.02397], "fy":[-63.27909,-80.97647,-60.1718,-44.86224]}, + {"t":0.33979, "x":3.57128, "y":2.63878, "heading":0.91972, "vx":-1.59321, "vy":-1.2706, "omega":-0.26672, "ax":-4.68713, "ay":-3.73831, "alpha":-1.44551, "fx":[-74.58579,-74.81042,-80.09459,-83.03812], "fy":[-65.69038,-53.81218,-59.40182,-70.35929]}, + {"t":0.37754, "x":3.50779, "y":2.58815, "heading":0.90965, "vx":-1.77017, "vy":-1.41174, "omega":-0.32129, "ax":-4.68618, "ay":-3.73763, "alpha":7.1547, "fx":[-100.0733,-69.78415,-55.09983,-87.50829], "fy":[-65.15536,-88.50389,-58.34583,-37.2132]}, + {"t":0.4153, "x":3.43762, "y":2.53218, "heading":0.89752, "vx":-1.94709, "vy":-1.55285, "omega":-0.05117, "ax":-4.68451, "ay":-3.7366, "alpha":-2.66694, "fx":[-69.58234,-70.81408,-85.10701,-86.85097], "fy":[-72.18566,-49.28362,-52.9818,-74.69855]}, + {"t":0.45305, "x":3.36077, "y":2.47089, "heading":0.89559, "vx":-2.12395, "vy":-1.69392, "omega":-0.15186, "ax":-4.68127, "ay":-3.73443, "alpha":6.59939, "fx":[-98.5836,-68.78927,-56.29513,-88.46985], "fy":[-67.36326,-85.70897,-55.96357,-39.9687]}, + {"t":0.49081, "x":3.27725, "y":2.40428, "heading":0.88985, "vx":-2.30069, "vy":-1.83492, "omega":0.0973, "ax":-4.67126, "ay":-3.72822, "alpha":-3.4513, "fx":[-65.92016,-67.92728,-89.25543,-88.36785], "fy":[-76.37272,-47.09629,-48.48917,-76.63255]}, + {"t":0.52856, "x":3.18706, "y":2.33235, "heading":0.89353, "vx":-2.47705, "vy":-1.97567, "omega":-0.03301, "ax":-2.78711, "ay":-2.05569, "alpha":2.20303, "fx":[-54.86265,-35.99732,-37.95307,-57.02616], "fy":[-45.32387,-37.30345,-26.42134,-28.02068]}, + {"t":0.56632, "x":3.09155, "y":2.25629, "heading":0.89228, "vx":-2.58228, "vy":-2.05328, "omega":0.05017, "ax":0.14422, "ay":-0.18133, "alpha":-1.12031, "fx":[8.52611,7.78983,-0.01323,-6.68622], "fy":[-14.85571,2.51347,8.81332,-8.56167]}, + {"t":0.60407, "x":2.99416, "y":2.17864, "heading":0.89418, "vx":-2.57683, "vy":-2.06013, "omega":0.00787, "ax":-0.05174, "ay":0.06478, "alpha":-1.62138, "fx":[6.94979,10.37379,-8.6655,-12.10797], "fy":[-8.93892,6.66392,11.46754,-4.87337]}, + {"t":0.64182, "x":2.89684, "y":2.10091, "heading":0.89447, "vx":-2.57878, "vy":-2.05768, "omega":-0.05334, "ax":-0.01551, "ay":0.01945, "alpha":0.58551, "fx":[-0.7562,9.23295,0.25136,-9.76228], "fy":[-9.70482,-1.09025,10.35218,1.73972]}, + {"t":0.67958, "x":2.79947, "y":2.02324, "heading":0.89246, "vx":-2.57937, "vy":-2.05695, "omega":-0.03124, "ax":-0.04629, "ay":0.0581, "alpha":-4.94546, "fx":[19.2364,11.10938,-20.78254,-12.64986], "fy":[-9.88665,17.88468,11.32615,-15.45024]}, + {"t":0.71733, "x":2.70205, "y":1.94562, "heading":0.89128, "vx":-2.58112, "vy":-2.05476, "omega":-0.21795, "ax":0.02502, "ay":-0.03142, "alpha":2.8459, "fx":[-6.5764,7.57791,7.96375,-7.29665], "fy":[-8.77135,-11.29683,7.7174,10.25547]}, + {"t":0.75509, "x":2.60462, "y":1.86802, "heading":0.88305, "vx":-2.58017, "vy":-2.05594, "omega":-0.11051, "ax":0.08663, "ay":-0.10853, "alpha":-5.44508, "fx":[22.47701,12.88268,-19.61419,-9.96931], "fy":[-12.45074,17.56396,7.56488,-19.91435]}, + {"t":0.79284, "x":2.50727, "y":1.79032, "heading":0.87888, "vx":-2.5769, "vy":-2.06004, "omega":-0.31608, "ax":0.66201, "ay":-0.81796, "alpha":4.7034, "fx":[-1.534,17.30065,24.02335,4.35152], "fy":[-20.88309,-32.05906,-6.49648,4.89856]}, + {"t":0.8306, "x":2.41045, "y":1.71196, "heading":0.86694, "vx":-2.55191, "vy":-2.09092, "omega":-0.13851, "ax":2.26997, "ay":-2.65965, "alpha":-2.85109, "fx":[46.80537,47.23303,28.80916,28.50979], "fy":[-54.26671,-30.07565,-38.70984,-54.28846]}, + {"t":0.86835, "x":2.31572, "y":1.63113, "heading":0.86172, "vx":-2.46621, "vy":-2.19134, "omega":-0.24615, "ax":4.2548, "ay":-3.66058, "alpha":7.14882, "fx":[47.69337,79.65814,90.15872,66.19166], "fy":[-66.26626,-87.73706,-56.89795,-33.17938]}, + {"t":0.90611, "x":2.22565, "y":1.54578, "heading":0.85242, "vx":-2.30557, "vy":-2.32954, "omega":0.02375, "ax":4.93501, "ay":3.37009, "alpha":4.3043, "fx":[70.76737,86.59262,92.99034,78.70633], "fy":[57.9999,34.93255,60.32545,71.45321]}, + {"t":0.94386, "x":2.14212, "y":1.46024, "heading":0.85332, "vx":-2.11925, "vy":-2.2023, "omega":0.18626, "ax":4.81732, "ay":3.55687, "alpha":6.02243, "fx":[60.81781,83.62996,98.70431,78.05723], "fy":[60.71664,36.46855,58.46813,81.51224]}, + {"t":0.98161, "x":2.06554, "y":1.37962, "heading":0.86035, "vx":-1.93738, "vy":-2.06802, "omega":0.41363, "ax":4.77641, "ay":3.61825, "alpha":3.86696, "fx":[65.6439,79.04659,93.34862,80.44274], "fy":[62.89978,47.52572,57.04611,73.78629]}, + {"t":1.01937, "x":1.9958, "y":1.30413, "heading":0.87597, "vx":-1.75705, "vy":-1.93141, "omega":0.55963, "ax":4.75579, "ay":3.64857, "alpha":1.95519, "fx":[72.61071,77.41698,84.90368,82.17544], "fy":[64.22686,52.70921,57.87295,68.47078]}, + {"t":1.05712, "x":1.93285, "y":1.23381, "heading":0.8971, "vx":-1.57749, "vy":-1.79366, "omega":0.63344, "ax":4.74328, "ay":3.66676, "alpha":-1.71774, "fx":[81.12662,72.41419,77.30663,85.42523], "fy":[63.67472,70.40695,57.88716,52.52357]}, + {"t":1.09488, "x":1.87668, "y":1.1687, "heading":0.92101, "vx":-1.39841, "vy":-1.65522, "omega":0.56859, "ax":4.73483, "ay":3.67894, "alpha":-6.04469, "fx":[97.33792,71.34563,56.63929,90.38647], "fy":[62.11436,81.39107,61.00284,40.79668]}, + {"t":1.13263, "x":1.82725, "y":1.10883, "heading":0.94248, "vx":-1.21965, "vy":-1.51633, "omega":0.34038, "ax":4.58417, "ay":3.85887, "alpha":-4.23824, "fx":[87.67322,71.21035,65.24245,81.53747], "fy":[63.70536,82.1563,64.44911,46.99149]}, + {"t":1.16525, "x":1.78991, "y":1.06143, "heading":0.95358, "vx":-1.07015, "vy":-1.39047, "omega":0.20215, "ax":4.32587, "ay":4.15262, "alpha":-3.70004, "fx":[82.86395,67.4805,59.22658,78.87], "fy":[68.68752,81.7681,71.22552,55.20737]}, + {"t":1.19786, "x":1.75731, "y":1.01829, "heading":0.96017, "vx":-0.92906, "vy":-1.25504, "omega":0.08148, "ax":4.09891, "ay":4.37725, "alpha":-2.52324, "fx":[75.18618,65.4159,61.6333,71.07192], "fy":[71.34954,83.68245,74.4451,62.38977]}, + {"t":1.23047, "x":1.72919, "y":0.97969, "heading":0.96283, "vx":-0.79538, "vy":-1.11228, "omega":-0.00082, "ax":3.90219, "ay":4.55387, "alpha":-2.10861, "fx":[71.41151,62.65329,57.54218,68.58329], "fy":[75.57939,82.85047,77.23914,67.97416]}, + {"t":1.26309, "x":1.70533, "y":0.94583, "heading":0.9628, "vx":-0.66811, "vy":-0.96376, "omega":-0.06959, "ax":3.7315, "ay":4.69506, "alpha":-1.27727, "fx":[65.89949,60.77131,58.65505,63.48312], "fy":[77.2238,83.67582,78.93426,73.22368]}, + {"t":1.2957, "x":1.68552, "y":0.9169, "heading":0.96053, "vx":-0.54641, "vy":-0.81063, "omega":-0.11125, "ax":3.58271, "ay":4.80981, "alpha":-0.66843, "fx":[61.5407,58.93563,57.31801,61.0941], "fy":[80.72138,82.15423,80.36229,77.47112]}, + {"t":1.32832, "x":1.66961, "y":0.89302, "heading":0.9569, "vx":-0.42956, "vy":-0.65377, "omega":-0.13305, "ax":3.45239, "ay":4.90442, "alpha":-0.01742, "fx":[57.44093,57.44331,57.77938,57.53496], "fy":[81.52527,82.2568,81.54814,81.68678]}, + {"t":1.36093, "x":1.65743, "y":0.8743, "heading":0.95257, "vx":-0.31697, "vy":-0.49381, "omega":-0.13361, "ax":3.33758, "ay":4.98344, "alpha":0.72865, "fx":[52.71584,56.41015,58.09594,55.32138], "fy":[84.10822,80.34442,82.62294,85.21092]}, + {"t":1.39355, "x":1.64887, "y":0.86085, "heading":0.94821, "vx":-0.20811, "vy":-0.33128, "omega":-0.10985, "ax":3.23586, "ay":5.05025, "alpha":1.34946, "fx":[49.2703,55.50638,58.64446,52.33962], "fy":[84.44603,79.77082,83.68316,88.84093]}, + {"t":1.42616, "x":1.6438, "y":0.85273, "heading":0.94463, "vx":-0.10258, "vy":-0.16657, "omega":-0.06584, "ax":3.14526, "ay":5.10731, "alpha":2.0187, "fx":[44.65016,55.40446,59.80274,49.86242], "fy":[85.74957,78.92826,84.63764,91.23005]}, + {"t":1.45877, "x":1.64213, "y":0.85001, "heading":0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/DTofetchPSlow.traj b/src/main/deploy/choreo/DTofetchPSlow.traj index e8455ad4..c1bbd5c9 100644 --- a/src/main/deploy/choreo/DTofetchPSlow.traj +++ b/src/main/deploy/choreo/DTofetchPSlow.traj @@ -3,7 +3,7 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":3.89906213509461, "y":2.953538192264317, "heading":1.0471975511965976, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.84197363509461, "y":2.854658009736421, "heading":1.0471975511965976, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.909953474998474, "y":1.1158, "heading":0.942477796076938, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.69577419757843, "y":0.8236, "heading":0.942477796076938, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ @@ -18,7 +18,7 @@ "waypoints":[ {"x":{"exp":"D.x", "val":3.84197363509461}, "y":{"exp":"D.y", "val":2.854658009736421}, "heading":{"exp":"D.heading", "val":1.0471975511965976}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.9099534749984741 m", "val":1.909953474998474}, "y":{"exp":"1.1158 m", "val":1.1158}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"fetchP.x", "val":1.69577419757843}, "y":{"exp":"fetchP.y", "val":0.8236}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"fetchP.x", "val":1.6421304941177368}, "y":{"exp":"fetchP.y", "val":0.8500130772590637}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -32,52 +32,52 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.19157,1.57273], + "waypoints":[0.0,1.16328,1.54453], "samples":[ - {"t":0.0, "x":3.89906, "y":2.95354, "heading":1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.84469, "ay":-3.19231, "alpha":-1.63358, "fx":[-58.97825,-66.54984,-69.02723,-61.80125], "fy":[-52.66189,-47.34948,-53.83362,-59.01205]}, - {"t":0.03724, "x":3.8964, "y":2.95133, "heading":1.0472, "vx":-0.14316, "vy":-0.11887, "omega":-0.06083, "ax":-3.84541, "ay":-3.19349, "alpha":-1.25752, "fx":[-60.18242,-65.98052,-67.91347,-62.32784], "fy":[-52.80688,-48.72096,-53.70173,-57.70605]}, - {"t":0.07447, "x":3.8884, "y":2.94468, "heading":1.04493, "vx":-0.28635, "vy":-0.23779, "omega":-0.10765, "ax":-3.84503, "ay":-3.19382, "alpha":-0.93791, "fx":[-61.17869,-65.43572,-66.982,-62.78253], "fy":[-52.90489,-49.89897,-53.57133,-56.58246]}, - {"t":0.11171, "x":3.87507, "y":2.93362, "heading":1.04092, "vx":-0.42953, "vy":-0.35671, "omega":-0.14258, "ax":-3.8446, "ay":-3.19419, "alpha":-0.67706, "fx":[-61.97008,-65.11619,-66.13668,-63.12744], "fy":[-53.0298,-50.8181,-53.48009,-55.65454]}, - {"t":0.14895, "x":3.85641, "y":2.91812, "heading":1.03561, "vx":-0.57269, "vy":-0.47565, "omega":-0.16779, "ax":-3.84412, "ay":-3.19461, "alpha":-0.43982, "fx":[-62.70603,-64.68851,-65.4524,-63.47152], "fy":[-53.09943,-51.70119,-53.38524,-54.82448]}, - {"t":0.18618, "x":3.83242, "y":2.89819, "heading":1.02937, "vx":-0.71583, "vy":-0.59461, "omega":-0.18417, "ax":-3.84357, "ay":-3.19508, "alpha":-0.24924, "fx":[-63.27541,-64.49957,-64.80111,-63.7059], "fy":[-53.19608,-52.36382,-53.339,-54.14322]}, - {"t":0.22342, "x":3.8031, "y":2.87384, "heading":1.02251, "vx":-0.85895, "vy":-0.71358, "omega":-0.19345, "ax":-3.84295, "ay":-3.19562, "alpha":-0.06836, "fx":[-63.83667,-64.14258,-64.29515,-63.96627], "fy":[-53.23126,-53.05848,-53.2718,-53.51647]}, - {"t":0.26066, "x":3.76845, "y":2.84505, "heading":1.01531, "vx":-1.00205, "vy":-0.83258, "omega":-0.19599, "ax":-3.84223, "ay":-3.19626, "alpha":0.06924, "fx":[-64.24222,-64.06202,-63.77988,-64.10816], "fy":[-53.30736,-53.52058,-53.26938,-53.02292]}, - {"t":0.29789, "x":3.72848, "y":2.81183, "heading":1.00801, "vx":-1.14512, "vy":-0.9516, "omega":-0.19342, "ax":-3.84139, "ay":-3.19698, "alpha":0.20925, "fx":[-64.67265,-63.74537,-63.41415,-64.30432], "fy":[-53.3095,-54.09599,-53.21674,-52.54644]}, - {"t":0.33513, "x":3.68317, "y":2.77418, "heading":1.00081, "vx":-1.28816, "vy":-1.07064, "omega":-0.18562, "ax":-3.84039, "ay":-3.19786, "alpha":0.30119, "fx":[-64.93455,-63.78231,-62.99449,-64.35837], "fy":[-53.3815,-54.37016,-53.25952,-52.21566]}, - {"t":0.37237, "x":3.63254, "y":2.7321, "heading":0.99389, "vx":-1.43117, "vy":-1.18972, "omega":-0.17441, "ax":-3.8392, "ay":-3.19888, "alpha":0.40498, "fx":[-65.25461,-63.47974,-62.75199,-64.50415], "fy":[-53.36823,-54.82844,-53.22262,-51.87618]}, - {"t":0.4096, "x":3.57659, "y":2.68558, "heading":0.9874, "vx":-1.57413, "vy":-1.30883, "omega":-0.15933, "ax":-3.83772, "ay":-3.20017, "alpha":0.45836, "fx":[-65.38375,-63.60777,-62.41972,-64.48063], "fy":[-53.43713,-54.96166,-53.29434,-51.68824]}, - {"t":0.44684, "x":3.51531, "y":2.63462, "heading":0.98147, "vx":-1.71703, "vy":-1.428, "omega":-0.14226, "ax":-3.83589, "ay":-3.20176, "alpha":0.53679, "fx":[-65.60582,-63.26254,-62.30887,-64.59263], "fy":[-53.38966,-55.40372,-53.23787,-51.45581]}, - {"t":0.48408, "x":3.44872, "y":2.57923, "heading":0.97617, "vx":-1.85987, "vy":-1.54722, "omega":-0.12227, "ax":-3.83349, "ay":-3.20385, "alpha":0.54748, "fx":[-65.5993,-63.45107,-62.06254,-64.49653], "fy":[-53.48767,-55.34995,-53.35671,-51.43252]}, - {"t":0.52131, "x":3.37681, "y":2.51939, "heading":0.97162, "vx":-2.00261, "vy":-1.66652, "omega":-0.10189, "ax":-3.83029, "ay":-3.20663, "alpha":0.57154, "fx":[-65.6545,-63.20012,-62.02037,-64.5212], "fy":[-53.51947,-55.50568,-53.38469,-51.40207]}, - {"t":0.55855, "x":3.29958, "y":2.45512, "heading":0.96782, "vx":-2.14524, "vy":-1.78593, "omega":-0.0806, "ax":-3.82579, "ay":-3.21052, "alpha":0.56625, "fx":[-65.5567,-63.19086,-61.94379,-64.40514], "fy":[-53.54936,-55.57366,-53.45203,-51.49631]}, - {"t":0.59579, "x":3.21705, "y":2.38639, "heading":0.96482, "vx":-2.2877, "vy":-1.90547, "omega":-0.05952, "ax":-3.81905, "ay":-3.21633, "alpha":0.53466, "fx":[-65.35242,-63.08288,-61.94598,-64.26543], "fy":[-53.656,-55.55113,-53.54482,-51.70662]}, - {"t":0.63302, "x":3.12921, "y":2.3132, "heading":0.9626, "vx":-2.42991, "vy":-2.02524, "omega":-0.03961, "ax":-3.80737, "ay":-3.22643, "alpha":0.45753, "fx":[-64.85121,-63.33888,-61.81859,-63.85954], "fy":[-53.83521,-55.40341,-53.75278,-52.14091]}, - {"t":0.67026, "x":3.03609, "y":2.23555, "heading":0.96113, "vx":-2.57168, "vy":-2.14538, "omega":-0.02257, "ax":-3.78399, "ay":-3.24635, "alpha":0.37655, "fx":[-64.25557,-62.65778,-61.91636,-63.47966], "fy":[-54.09875,-55.56244,-54.00346,-52.79598]}, - {"t":0.7075, "x":2.93771, "y":2.15342, "heading":0.96029, "vx":-2.71258, "vy":-2.26626, "omega":-0.00855, "ax":-3.70274, "ay":-3.31626, "alpha":0.1778, "fx":[-61.7718,-63.48989,-60.35947,-61.27054], "fy":[-55.30196,-56.02219,-55.22966,-54.56779]}, - {"t":0.74473, "x":2.83413, "y":2.06673, "heading":0.95997, "vx":-2.85046, "vy":-2.38975, "omega":-0.00193, "ax":1.12568, "ay":-3.65933, "alpha":0.05194, "fx":[18.74522,18.77275,18.77683,18.76364], "fy":[-60.84538,-61.50507,-60.83247,-60.81393]}, - {"t":0.78197, "x":2.72877, "y":1.97521, "heading":0.9599, "vx":-2.80854, "vy":-2.52601, "omega":0.0, "ax":3.93884, "ay":-2.41896, "alpha":-0.00188, "fx":[65.35835,66.77837,65.2038,65.29346], "fy":[-40.29509,-40.27031,-40.32807,-40.39799]}, - {"t":0.81921, "x":2.62692, "y":1.87947, "heading":0.9599, "vx":-2.66188, "vy":-2.61609, "omega":-0.00007, "ax":4.09641, "ay":2.81565, "alpha":-0.30239, "fx":[68.87598,68.20591,67.5907,68.46799], "fy":[46.71547,48.64154,46.6264,45.7588]}, - {"t":0.85644, "x":2.53064, "y":1.78401, "heading":0.9599, "vx":-2.50934, "vy":-2.51124, "omega":-0.01133, "ax":3.97687, "ay":3.00697, "alpha":-0.29424, "fx":[67.14178,66.24089,65.25923,66.5281], "fy":[50.17421,51.19268,50.11088,49.0211]}, - {"t":0.89368, "x":2.43996, "y":1.69258, "heading":0.95947, "vx":-2.36125, "vy":-2.39927, "omega":-0.02228, "ax":3.93617, "ay":3.06797, "alpha":-0.40137, "fx":[66.71093,65.20515,64.52806,66.01231], "fy":[51.02178,52.94134,50.93598,49.66711]}, - {"t":0.93092, "x":2.35476, "y":1.60537, "heading":0.95864, "vx":-2.21468, "vy":-2.28503, "omega":-0.03723, "ax":3.9157, "ay":3.09793, "alpha":-0.36761, "fx":[66.4193,64.85685,64.13319,65.68194], "fy":[51.6567,53.00095,51.60704,50.29908]}, - {"t":0.96815, "x":2.27501, "y":1.52243, "heading":0.95726, "vx":-2.06888, "vy":-2.16967, "omega":-0.05092, "ax":3.90333, "ay":3.11578, "alpha":-0.33832, "fx":[66.06611,64.73716,64.04033,65.42311], "fy":[51.91575,53.27432,51.86706,50.69723]}, - {"t":1.00539, "x":2.20068, "y":1.4438, "heading":0.95536, "vx":-1.92353, "vy":-2.05365, "omega":-0.06352, "ax":3.89505, "ay":3.12764, "alpha":-0.22969, "fx":[65.63806,64.74219,64.16825,65.16624], "fy":[52.15576,52.95776,52.13829,51.29293]}, - {"t":1.04263, "x":2.13175, "y":1.36949, "heading":0.953, "vx":-1.77849, "vy":-1.93719, "omega":-0.07207, "ax":3.8891, "ay":3.13611, "alpha":-0.11685, "fx":[65.1312,64.69742,64.54331,64.94553], "fy":[52.19917,52.85837,52.19638,51.8561]}, - {"t":1.07986, "x":2.06822, "y":1.29953, "heading":0.95031, "vx":-1.63367, "vy":-1.82041, "omega":-0.07642, "ax":3.88463, "ay":3.14244, "alpha":0.09523, "fx":[64.44617,64.88496,65.04284,64.64597], "fy":[52.37967,52.03317,52.39477,52.72415]}, - {"t":1.1171, "x":2.01008, "y":1.23393, "heading":0.94747, "vx":-1.48902, "vy":-1.7034, "omega":-0.07287, "ax":3.88115, "ay":3.14736, "alpha":0.31575, "fx":[63.64218,65.03307,65.76539,64.34685], "fy":[52.38726,51.44521,52.41472,53.61287]}, - {"t":1.15434, "x":1.95733, "y":1.17268, "heading":0.94475, "vx":-1.3445, "vy":-1.5862, "omega":-0.06112, "ax":3.87836, "ay":3.15129, "alpha":0.63629, "fx":[62.61483,65.37216,66.64972,63.96497], "fy":[52.53318,50.21257,52.54921,54.82696]}, - {"t":1.19157, "x":1.90995, "y":1.1158, "heading":0.94248, "vx":-1.20008, "vy":-1.46886, "omega":-0.03742, "ax":3.78538, "ay":3.25828, "alpha":0.43862, "fx":[61.63843,63.58822,64.5565,62.61875], "fy":[54.25853,52.8416,54.27003,55.88594]}, - {"t":1.22622, "x":1.87064, "y":1.06686, "heading":0.94118, "vx":-1.06892, "vy":-1.35595, "omega":-0.02222, "ax":3.62007, "ay":3.44526, "alpha":0.3446, "fx":[59.14265,60.70852,61.46287,60.06495], "fy":[57.57471,56.17141,57.39338,58.58402]}, - {"t":1.26087, "x":1.83578, "y":1.02194, "heading":0.94041, "vx":-0.94348, "vy":-1.23657, "omega":-0.01028, "ax":3.46825, "ay":3.59829, "alpha":0.2535, "fx":[56.93778,58.08012,58.70703,57.53162], "fy":[59.95011,59.18164,59.93085,60.86425]}, - {"t":1.29553, "x":1.80517, "y":0.98125, "heading":0.94005, "vx":-0.8233, "vy":-1.11189, "omega":-0.0015, "ax":3.33099, "ay":3.72593, "alpha":0.19573, "fx":[54.79857,55.72462,56.17281,55.40772], "fy":[62.21517,61.40773,62.08092,62.73398]}, - {"t":1.33018, "x":1.77864, "y":0.94496, "heading":0.94, "vx":-0.70788, "vy":-0.98278, "omega":0.00528, "ax":3.20704, "ay":3.83332, "alpha":0.13116, "fx":[52.9908,53.59554,53.94087,53.3118], "fy":[63.87912,63.50861,63.8654,64.34509]}, - {"t":1.36483, "x":1.75603, "y":0.91321, "heading":0.94019, "vx":-0.59675, "vy":-0.84995, "omega":0.00983, "ax":3.09502, "ay":3.92448, "alpha":0.08342, "fx":[51.24623,51.66755,51.86737,51.58865], "fy":[65.502,65.11487,65.39901,65.66063]}, - {"t":1.39948, "x":1.73721, "y":0.88611, "heading":0.94053, "vx":-0.48951, "vy":-0.71397, "omega":0.01272, "ax":2.99363, "ay":4.00249, "alpha":0.02766, "fx":[49.78827,49.92181,50.02957,49.86958], "fy":[66.69819,66.67512,66.69312,66.81217]}, - {"t":1.43413, "x":1.72205, "y":0.86378, "heading":0.94097, "vx":-0.38577, "vy":-0.57528, "omega":0.01368, "ax":2.90164, "ay":4.06979, "alpha":-0.01633, "fx":[48.38049,48.34127,48.30316,48.45105], "fy":[67.9131,67.87446,67.82301,67.75548]}, - {"t":1.46878, "x":1.71042, "y":0.84629, "heading":0.94144, "vx":-0.28523, "vy":-0.43425, "omega":0.01311, "ax":2.81798, "ay":4.12828, "alpha":-0.07242, "fx":[47.21516,46.88821,46.74238,47.05146], "fy":[68.79767,69.09055,68.79839,68.57896]}, - {"t":1.50343, "x":1.70223, "y":0.83372, "heading":0.9419, "vx":-0.18758, "vy":-0.2912, "omega":0.0106, "ax":2.74166, "ay":4.17944, "alpha":-0.12684, "fx":[46.13735,45.55638,45.27592,45.8392], "fy":[69.65014,70.11914,69.65221,69.25586]}, - {"t":1.53808, "x":1.69738, "y":0.82614, "heading":0.94226, "vx":-0.09258, "vy":-0.14638, "omega":0.00621, "ax":2.67186, "ay":4.2245, "alpha":-0.17913, "fx":[45.1681,44.37721,43.88611,44.72302], "fy":[70.4214,70.99907,70.42399,69.83703]}, - {"t":1.57273, "x":1.69577, "y":0.8236, "heading":0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":3.84197, "y":2.85466, "heading":1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.897, "ay":-3.12812, "alpha":-1.61874, "fx":[-59.90204,-67.41853,-69.81637,-62.70748], "fy":[-51.58272,-46.31425,-52.77595,-57.90413]}, + {"t":0.03635, "x":3.8394, "y":2.85259, "heading":1.0472, "vx":-0.14167, "vy":-0.11372, "omega":-0.05885, "ax":-3.89713, "ay":-3.1301, "alpha":-1.25269, "fx":[-61.06403,-66.86659,-68.72179,-63.20049], "fy":[-51.74464,-47.66283,-52.6578,-56.64384]}, + {"t":0.07271, "x":3.83167, "y":2.84639, "heading":1.04506, "vx":-0.28334, "vy":-0.2275, "omega":-0.10438, "ax":-3.89605, "ay":-3.13131, "alpha":-0.9369, "fx":[-62.04477,-66.26826,-67.82172,-63.64608], "fy":[-51.84043,-48.8573,-52.54007,-55.55222]}, + {"t":0.10906, "x":3.8188, "y":2.83605, "heading":1.04126, "vx":-0.42497, "vy":-0.34133, "omega":-0.13844, "ax":-3.89484, "ay":-3.13267, "alpha":-0.69121, "fx":[-62.75633,-66.03558,-66.9735,-63.93471], "fy":[-52.00252,-49.72671,-52.47159,-54.6798]}, + {"t":0.14541, "x":3.80078, "y":2.82157, "heading":1.03623, "vx":-0.56655, "vy":-0.45521, "omega":-0.16357, "ax":-3.89347, "ay":-3.1342, "alpha":-0.45054, "fx":[-63.49591,-65.50915,-66.32,-64.28418], "fy":[-52.06343,-50.67657,-52.37723,-53.86534]}, + {"t":0.18176, "x":3.77761, "y":2.80295, "heading":1.03028, "vx":-0.70809, "vy":-0.56915, "omega":-0.17995, "ax":-3.89193, "ay":-3.13593, "alpha":-0.27497, "fx":[-63.98624,-65.42427,-65.64046,-64.45568], "fy":[-52.2104,-51.27425,-52.37034,-53.24252]}, + {"t":0.21812, "x":3.7493, "y":2.78019, "heading":1.02374, "vx":-0.84957, "vy":-0.68315, "omega":-0.18994, "ax":-3.89017, "ay":-3.1379, "alpha":-0.08843, "fx":[-64.55588,-64.94402,-65.16195,-64.72745], "fy":[-52.24493,-52.04964,-52.30402,-52.63026]}, + {"t":0.25447, "x":3.71584, "y":2.75328, "heading":1.01684, "vx":-0.99099, "vy":-0.79722, "omega":-0.19316, "ax":-3.88814, "ay":-3.14017, "alpha":0.03495, "fx":[-64.87471,-64.99742,-64.58963,-64.79193], "fy":[-52.37877,-52.45013,-52.35354,-52.19807]}, + {"t":0.29082, "x":3.67725, "y":2.72223, "heading":1.00982, "vx":-1.13234, "vy":-0.91137, "omega":-0.19189, "ax":-3.88577, "ay":-3.14282, "alpha":0.18904, "fx":[-65.32723,-64.49019,-64.26221,-65.01602], "fy":[-52.37055,-53.18431,-52.28426,-51.71786]}, + {"t":0.32717, "x":3.63352, "y":2.68702, "heading":1.00284, "vx":-1.27359, "vy":-1.02562, "omega":-0.18502, "ax":-3.88297, "ay":-3.14593, "alpha":0.26136, "fx":[-65.48223,-64.72179,-63.74155,-64.9636], "fy":[-52.53103,-53.34286,-52.41598,-51.47501]}, + {"t":0.36353, "x":3.58465, "y":2.64766, "heading":0.99612, "vx":-1.41475, "vy":-1.13999, "omega":-0.17552, "ax":-3.87962, "ay":-3.14967, "alpha":0.38091, "fx":[-65.83289,-64.16365,-63.54125,-65.14779], "fy":[-52.52635,-53.97211,-52.37453,-51.14068]}, + {"t":0.39988, "x":3.53066, "y":2.60413, "heading":0.98973, "vx":-1.55578, "vy":-1.25448, "omega":-0.16167, "ax":-3.87552, "ay":-3.15422, "alpha":0.4159, "fx":[-65.8237,-64.56469,-63.04879,-64.97504], "fy":[-52.69871,-54.0118,-52.55666,-51.05004]}, + {"t":0.43623, "x":3.47154, "y":2.55645, "heading":0.98386, "vx":-1.69667, "vy":-1.36915, "omega":-0.14655, "ax":-3.87041, "ay":-3.15988, "alpha":0.49895, "fx":[-66.07474,-63.89984,-62.97195,-65.12469], "fy":[-52.73026,-54.51481,-52.56331,-50.88638]}, + {"t":0.47258, "x":3.40731, "y":2.50459, "heading":0.97853, "vx":-1.83737, "vy":-1.48402, "omega":-0.12841, "ax":-3.86384, "ay":-3.16712, "alpha":0.50965, "fx":[-65.93421,-64.25724,-62.5641,-64.87805], "fy":[-52.90567,-54.56958,-52.7683,-50.93402]}, + {"t":0.50894, "x":3.33796, "y":2.44855, "heading":0.97386, "vx":-1.97783, "vy":-1.59915, "omega":-0.10988, "ax":-3.85511, "ay":-3.17671, "alpha":0.53321, "fx":[-65.97001,-63.66968,-62.50906,-64.90284], "fy":[-53.10058,-54.76587,-52.93446,-51.01585]}, + {"t":0.54529, "x":3.26351, "y":2.38831, "heading":0.96987, "vx":-2.11797, "vy":-1.71463, "omega":-0.0905, "ax":-3.84293, "ay":-3.19, "alpha":0.56266, "fx":[-65.9051,-63.18408,-62.37701,-64.77323], "fy":[-53.17532,-55.24231,-53.0871,-51.19842]}, + {"t":0.58164, "x":3.18398, "y":2.32387, "heading":0.96658, "vx":-2.25767, "vy":-1.8306, "omega":-0.07005, "ax":-3.82477, "ay":-3.20965, "alpha":0.49244, "fx":[-65.37975,-63.2597,-62.05977,-64.3288], "fy":[-53.67188,-55.08186,-53.53324,-51.72634]}, + {"t":0.61799, "x":3.09938, "y":2.25521, "heading":0.96403, "vx":-2.39671, "vy":-1.94727, "omega":-0.05214, "ax":-3.7947, "ay":-3.24172, "alpha":0.45162, "fx":[-64.59931,-63.26388,-61.55963,-63.60044], "fy":[-54.09024,-55.6261,-54.01749,-52.41759]}, + {"t":0.65435, "x":3.00975, "y":2.18228, "heading":0.96214, "vx":-2.53466, "vy":-2.06512, "omega":-0.03573, "ax":-3.73509, "ay":-3.30372, "alpha":0.40283, "fx":[-63.47264,-61.79217,-61.08623,-62.69758], "fy":[-55.00303,-56.72264,-54.90983,-53.65021]}, + {"t":0.6907, "x":2.91514, "y":2.10502, "heading":0.96084, "vx":-2.67044, "vy":-2.18522, "omega":-0.02108, "ax":-3.56915, "ay":-3.46538, "alpha":0.22297, "fx":[-59.79708,-61.09258,-57.93752,-59.15711], "fy":[-57.84598,-58.50763,-57.79849,-56.91296]}, + {"t":0.72705, "x":2.8157, "y":2.02329, "heading":0.96007, "vx":-2.80019, "vy":-2.31119, "omega":-0.01298, "ax":-1.27306, "ay":-4.67512, "alpha":0.35718, "fx":[-21.73704,-21.14714,-20.67692,-21.32403], "fy":[-77.25523,-80.61148,-77.1336,-76.72778]}, + {"t":0.7634, "x":2.71307, "y":1.93619, "heading":0.9596, "vx":-2.84647, "vy":-2.48115, "omega":0.00001, "ax":4.68442, "ay":-1.19572, "alpha":-0.0254, "fx":[76.8152,82.89588,76.11629,76.52097], "fy":[-19.87754,-19.54013,-20.01664,-20.29396]}, + {"t":0.79976, "x":2.61269, "y":1.8452, "heading":0.9596, "vx":-2.67617, "vy":-2.52461, "omega":-0.00092, "ax":4.29909, "ay":2.50052, "alpha":-0.25091, "fx":[73.06497,71.66107,69.96336,71.96556], "fy":[42.70515,40.97673,42.36578,40.68206]}, + {"t":0.83611, "x":2.51824, "y":1.75508, "heading":0.95957, "vx":-2.51989, "vy":-2.43371, "omega":-0.01004, "ax":4.13157, "ay":2.79137, "alpha":-0.35746, "fx":[69.89333,68.93331,67.55774,69.10058], "fy":[46.62504,47.80547,46.50011,45.19255]}, + {"t":0.87246, "x":2.42937, "y":1.66845, "heading":0.9592, "vx":-2.3697, "vy":-2.33224, "omega":-0.02303, "ax":4.06208, "ay":2.89933, "alpha":-0.45503, "fx":[68.80812,67.22437,66.69438,68.1249], "fy":[48.03514,50.72575,47.90481,46.65607]}, + {"t":0.90881, "x":2.34591, "y":1.58558, "heading":0.95836, "vx":-2.22203, "vy":-2.22684, "omega":-0.03957, "ax":4.02753, "ay":2.95104, "alpha":-0.36097, "fx":[68.33275,66.45715,66.14633,67.6116], "fy":[49.18999,50.54931,49.13056,47.89978]}, + {"t":0.94517, "x":2.26779, "y":1.50658, "heading":0.95692, "vx":-2.07562, "vy":-2.11956, "omega":-0.0527, "ax":4.00627, "ay":2.98217, "alpha":-0.3244, "fx":[67.69649,66.4803,65.84897,67.1049], "fy":[49.67128,51.0743,49.59114,48.50838]}, + {"t":0.98152, "x":2.19498, "y":1.4315, "heading":0.95501, "vx":-1.92998, "vy":-2.01115, "omega":-0.06449, "ax":3.99198, "ay":3.00283, "alpha":-0.20152, "fx":[67.15923,66.44101,65.84904,66.72814], "fy":[50.09,50.76318,50.05854,49.31131]}, + {"t":1.01787, "x":2.12746, "y":1.36037, "heading":0.95266, "vx":-1.78486, "vy":-1.90199, "omega":-0.07181, "ax":3.98164, "ay":3.01763, "alpha":-0.11401, "fx":[66.6074,66.23289,66.17978,66.46809], "fy":[50.15378,51.01945,50.1406,49.89619]}, + {"t":1.05422, "x":2.06521, "y":1.29322, "heading":0.95005, "vx":-1.64012, "vy":-1.7923, "omega":-0.07596, "ax":3.97383, "ay":3.02874, "alpha":0.1158, "fx":[65.88191,66.37117,66.59666,66.11751], "fy":[50.48791,50.05241,50.50001,50.91055]}, + {"t":1.09058, "x":2.00821, "y":1.23007, "heading":0.94729, "vx":-1.49566, "vy":-1.68219, "omega":-0.07175, "ax":3.96771, "ay":3.0374, "alpha":0.3038, "fx":[65.06907,66.44061,67.2556,65.79402], "fy":[50.45986,49.81165,50.50351,51.75299]}, + {"t":1.12693, "x":1.95646, "y":1.17093, "heading":0.94468, "vx":-1.35143, "vy":-1.57178, "omega":-0.06071, "ax":3.96279, "ay":3.04433, "alpha":0.64156, "fx":[64.0246,66.77446,68.05378,65.37835], "fy":[50.74858,48.38624,50.77568,53.07976]}, + {"t":1.16328, "x":1.90995, "y":1.1158, "heading":0.94248, "vx":-1.20737, "vy":-1.46111, "omega":-0.03738, "ax":3.86282, "ay":3.16609, "alpha":0.42827, "fx":[62.91157,64.82378,65.90949,63.92071], "fy":[52.62015,51.50203,52.65794,54.32842]}, + {"t":1.19794, "x":1.87043, "y":1.06706, "heading":0.94118, "vx":-1.07349, "vy":-1.35137, "omega":-0.02254, "ax":3.68457, "ay":3.37618, "alpha":0.354, "fx":[60.12881,61.7876,62.56477,61.19888], "fy":[56.60125,54.92373,56.1972,57.39517]}, + {"t":1.2326, "x":1.83543, "y":1.02225, "heading":0.9404, "vx":-0.94578, "vy":-1.23436, "omega":-0.01027, "ax":3.51941, "ay":3.54826, "alpha":0.25001, "fx":[57.7732,58.90799,59.60263,58.3839], "fy":[59.06924,58.44708,59.04994,60.02472]}, + {"t":1.26726, "x":1.80477, "y":0.9816, "heading":0.94004, "vx":-0.8238, "vy":-1.11138, "omega":-0.00161, "ax":3.3692, "ay":3.6914, "alpha":0.20205, "fx":[55.35258,56.35793,56.8228,56.11859], "fy":[61.76511,60.7702,61.47521,62.12494]}, + {"t":1.30192, "x":1.77824, "y":0.9453, "heading":0.93999, "vx":-0.70703, "vy":-0.98343, "omega":0.0054, "ax":3.23294, "ay":3.81149, "alpha":0.12588, "fx":[53.41239,53.99491,54.41189,53.74674], "fy":[63.47471,63.24629,63.45594,63.96604]}, + {"t":1.33658, "x":1.75568, "y":0.9135, "heading":0.94018, "vx":-0.59497, "vy":-0.85133, "omega":0.00976, "ax":3.10946, "ay":3.91304, "alpha":0.08956, "fx":[51.42094,51.91431,52.12567,51.87189], "fy":[65.41249,64.87457,65.17816,65.44854]}, + {"t":1.37124, "x":1.73692, "y":0.88635, "heading":0.94051, "vx":-0.4872, "vy":-0.71571, "omega":0.01286, "ax":2.99749, "ay":3.9996, "alpha":0.02511, "fx":[49.84484,49.96828,50.1189,49.9346], "fy":[66.62825,66.68375,66.61735,66.75624]}, + {"t":1.4059, "x":1.72184, "y":0.86394, "heading":0.94096, "vx":-0.38331, "vy":-0.57708, "omega":0.01373, "ax":2.89579, "ay":4.07396, "alpha":-0.01412, "fx":[48.22234,48.23407,48.20259,48.42669], "fy":[68.0544,67.91655,67.87494,67.79776]}, + {"t":1.44056, "x":1.71029, "y":0.84639, "heading":0.94144, "vx":-0.28294, "vy":-0.43588, "omega":0.01325, "ax":2.80323, "ay":4.1383, "alpha":-0.07503, "fx":[46.96841,46.6294,46.50932,46.80702], "fy":[68.95171,69.29701,68.94674,68.73846]}, + {"t":1.47522, "x":1.70217, "y":0.83377, "heading":0.9419, "vx":-0.18579, "vy":-0.29245, "omega":0.01064, "ax":2.71879, "ay":4.19436, "alpha":-0.12959, "fx":[45.75888,45.16548,44.90021,45.45927], "fy":[69.88987,70.39868,69.88623,69.49695]}, + {"t":1.50988, "x":1.69736, "y":0.82615, "heading":0.94226, "vx":-0.09155, "vy":-0.14708, "omega":0.00615, "ax":2.64156, "ay":4.24351, "alpha":-0.17753, "fx":[44.64568,43.92613,43.36493,44.19757], "fy":[70.74504,71.30698,70.74198,70.15505]}, + {"t":1.54453, "x":1.69577, "y":0.8236, "heading":0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/ETofetchP.traj b/src/main/deploy/choreo/ETofetchP.traj index 2a15505a..e7e0fcae 100644 --- a/src/main/deploy/choreo/ETofetchP.traj +++ b/src/main/deploy/choreo/ETofetchP.traj @@ -3,32 +3,32 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":4.894185251845, "y":2.7146580097364, "heading":2.0943951023931953, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.975547790527344, "y":2.0208094120025635, "heading":0.0, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":1.949252247810364, "y":1.1668, "heading":0.942477796076938, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.69577419757843, "y":0.8236, "heading":0.942477796076938, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":4.894185251845, "y":2.7146580097364, "heading":2.0943951023931953, "intervals":88, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.975547790527344, "y":2.0208094120025635, "heading":0.0, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":2.121014356613159, "y":1.2148, "heading":1.081580366464561, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.6421304941177368, "y":0.8500130772590637, "heading":0.942477796076938, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":4.0}}, "enabled":true}, - {"from":0, "to":3, "data":{"type":"MaxVelocity", "props":{"max":3.0}}, "enabled":true}, - {"from":0, "to":3, "data":{"type":"MaxAcceleration", "props":{"max":3.0}}, "enabled":true}], + {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":1.0}}, "enabled":true}, + {"from":0, "to":3, "data":{"type":"MaxVelocity", "props":{"max":3.3}}, "enabled":true}, + {"from":0, "to":3, "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"E.x", "val":4.894185251845}, "y":{"exp":"E.y", "val":2.7146580097364}, "heading":{"exp":"E.heading", "val":2.0943951023931953}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.9755477905273438 m", "val":3.975547790527344}, "y":{"exp":"2.0208094120025635 m", "val":2.0208094120025635}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"1.9492522478103638 m", "val":1.949252247810364}, "y":{"exp":"1.1668 m", "val":1.1668}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"fetchP.x", "val":1.69577419757843}, "y":{"exp":"fetchP.y", "val":0.8236}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"E.x", "val":4.894185251845}, "y":{"exp":"E.y", "val":2.7146580097364}, "heading":{"exp":"E.heading", "val":2.0943951023931953}, "intervals":88, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.9755477905273438 m", "val":3.975547790527344}, "y":{"exp":"2.0208094120025635 m", "val":2.0208094120025635}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"2.121014356613159 m", "val":2.121014356613159}, "y":{"exp":"1.2148 m", "val":1.2148}, "heading":{"exp":"1.081580366464561 rad", "val":1.081580366464561}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"fetchP.x", "val":1.6421304941177368}, "y":{"exp":"fetchP.y", "val":0.8500130772590637}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"4 rad / s", "val":4.0}}}, "enabled":true}, - {"from":0, "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3 m / s", "val":3.0}}}, "enabled":true}, - {"from":0, "to":3, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"maxAccel", "val":3.0}}}, "enabled":true}], + {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"1 rad / s", "val":1.0}}}, "enabled":true}, + {"from":0, "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.3 m / s", "val":3.3}}}, "enabled":true}, + {"from":0, "to":3, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -36,97 +36,83 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.88133,1.78562,2.33107], + "waypoints":[0.0,0.62453,1.25013,1.69862], "samples":[ - {"t":0.0, "x":4.89419, "y":2.71466, "heading":2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.13351, "ay":-2.10601, "alpha":-8.49245, "fx":[-29.82745,-63.93452,-42.05324,-6.44341], "fy":[-5.93964,-27.32073,-61.33044,-45.83399]}, - {"t":0.02843, "x":4.89332, "y":2.71381, "heading":2.0944, "vx":-0.06066, "vy":-0.05987, "omega":-0.24144, "ax":-2.14942, "ay":-2.09126, "alpha":-6.56495, "fx":[-31.38674,-57.80556,-40.75436,-13.37263], "fy":[-12.47112,-28.61385,-55.28199,-43.07445]}, - {"t":0.05686, "x":4.89073, "y":2.71126, "heading":2.08753, "vx":-0.12176, "vy":-0.11933, "omega":-0.42808, "ax":-2.16535, "ay":-2.07472, "alpha":-4.97268, "fx":[-32.71842,-52.29913,-40.48218,-18.88172], "fy":[-17.71525,-29.58004,-50.07797,-40.9649]}, - {"t":0.08529, "x":4.88639, "y":2.70703, "heading":2.07536, "vx":-0.18332, "vy":-0.17831, "omega":-0.56945, "ax":-2.18212, "ay":-2.05702, "alpha":-3.91071, "fx":[-33.53715,-49.21554,-39.31516,-23.43169], "fy":[-21.04197,-30.0942,-47.12126,-38.90091]}, - {"t":0.11372, "x":4.8803, "y":2.70113, "heading":2.05917, "vx":-0.24536, "vy":-0.23679, "omega":-0.68064, "ax":-2.19979, "ay":-2.03806, "alpha":-2.97061, "fx":[-34.19512,-46.63555,-39.05809,-26.78913], "fy":[-24.31129,-30.42612,-43.58691,-37.56952]}, - {"t":0.14215, "x":4.87243, "y":2.69357, "heading":2.03982, "vx":-0.3079, "vy":-0.29474, "omega":-0.76509, "ax":-2.21843, "ay":-2.01769, "alpha":-2.18168, "fx":[-35.14629,-44.07328,-38.84718,-29.85419], "fy":[-26.30379,-31.08247,-40.75995,-36.38961]}, - {"t":0.17058, "x":4.86278, "y":2.68438, "heading":2.01807, "vx":-0.37097, "vy":-0.3521, "omega":-0.82711, "ax":-2.23812, "ay":-1.99577, "alpha":-1.6008, "fx":[-35.70328,-42.64382,-38.78354,-32.10304], "fy":[-28.18214,-31.09298,-38.43133,-35.36755]}, - {"t":0.19901, "x":4.85133, "y":2.67356, "heading":1.99455, "vx":-0.4346, "vy":-0.40884, "omega":-0.87263, "ax":-2.25893, "ay":-1.97211, "alpha":-1.12804, "fx":[-36.55502,-41.26103,-38.77013,-34.0352], "fy":[-29.08872,-31.46353,-36.51898,-34.4255]}, - {"t":0.22744, "x":4.83806, "y":2.66114, "heading":1.96975, "vx":-0.49882, "vy":-0.4649, "omega":-0.9047, "ax":-2.28096, "ay":-1.94652, "alpha":-0.76024, "fx":[-37.07013,-40.5457,-38.8183,-35.6558], "fy":[-30.12513,-31.23713,-34.89729,-33.53095]}, - {"t":0.25587, "x":4.82296, "y":2.64714, "heading":1.94403, "vx":-0.56367, "vy":-0.52024, "omega":-0.92631, "ax":-2.30429, "ay":-1.91877, "alpha":-0.47615, "fx":[-37.88406,-39.89374,-38.94974,-36.91795], "fy":[-30.34251,-31.40858,-33.50001,-32.68885]}, - {"t":0.2843, "x":4.80601, "y":2.63157, "heading":1.91769, "vx":-0.62918, "vy":-0.5748, "omega":-0.93985, "ax":-2.32902, "ay":-1.88858, "alpha":-0.23176, "fx":[-38.3881,-39.61162,-39.07661,-38.21843], "fy":[-30.84505,-30.99102,-32.26254,-31.82856]}, - {"t":0.31273, "x":4.78718, "y":2.61447, "heading":1.89097, "vx":-0.6954, "vy":-0.62849, "omega":-0.94644, "ax":-2.35527, "ay":-1.85565, "alpha":-0.05933, "fx":[-39.20049,-39.4277,-39.33309,-39.08356], "fy":[-30.61651,-30.99661,-31.11905,-30.99928]}, - {"t":0.34116, "x":4.76645, "y":2.59585, "heading":1.86406, "vx":-0.76236, "vy":-0.68124, "omega":-0.94812, "ax":-2.38314, "ay":-1.81962, "alpha":0.11614, "fx":[-39.72188,-39.41704,-39.52908,-40.23523], "fy":[-30.78234,-30.39769,-30.04743,-30.10118]}, - {"t":0.36959, "x":4.74382, "y":2.57575, "heading":1.83711, "vx":-0.83011, "vy":-0.73298, "omega":-0.94482, "ax":-2.41275, "ay":-1.78005, "alpha":0.2227, "fx":[-40.5564,-39.5417,-39.89508,-40.88455], "fy":[-30.20348,-30.27371,-28.9924,-29.22086]}, - {"t":0.39802, "x":4.71924, "y":2.55419, "heading":1.81025, "vx":-0.8987, "vy":-0.78358, "omega":-0.93849, "ax":-2.44422, "ay":-1.73646, "alpha":0.36145, "fx":[-41.10902,-39.69488,-40.13753,-42.03466], "fy":[-30.10047,-29.48405,-27.96104,-28.23829]}, - {"t":0.42645, "x":4.6927, "y":2.53121, "heading":1.78357, "vx":-0.96819, "vy":-0.83295, "omega":-0.92821, "ax":-2.47767, "ay":-1.68826, "alpha":0.4281, "fx":[-41.99543,-40.05596,-40.62113,-42.53355], "fy":[-29.24671,-29.21537,-26.86557,-27.24208]}, - {"t":0.45488, "x":4.66418, "y":2.50685, "heading":1.75718, "vx":-1.03863, "vy":-0.88095, "omega":-0.91604, "ax":-2.51319, "ay":-1.63475, "alpha":0.54861, "fx":[-42.60674,-40.33427,-40.91943,-43.71412], "fy":[-28.94869,-28.19266,-25.75272,-26.10809]}, - {"t":0.48331, "x":4.63363, "y":2.48114, "heading":1.73113, "vx":-1.11008, "vy":-0.92742, "omega":-0.90044, "ax":-2.55087, "ay":-1.57513, "alpha":0.59078, "fx":[-43.55369,-40.8725,-41.50308,-44.15799], "fy":[-27.7779,-27.76312,-24.54072,-24.9446]}, - {"t":0.51174, "x":4.60104, "y":2.45414, "heading":1.70553, "vx":-1.1826, "vy":-0.9722, "omega":-0.88365, "ax":-2.59077, "ay":-1.50841, "alpha":0.70554, "fx":[-44.22967,-41.23142,-41.83739,-45.44879], "fy":[-27.24697,-26.47387,-23.26166,-23.59521]}, - {"t":0.54017, "x":4.56637, "y":2.42589, "heading":1.68041, "vx":-1.25626, "vy":-1.01509, "omega":-0.86359, "ax":-2.63286, "ay":-1.43344, "alpha":0.72906, "fx":[-45.25494,-41.93723,-42.53303,-45.8287], "fy":[-25.74783,-25.79864,-21.83898,-22.19365]}, - {"t":0.5686, "x":4.5296, "y":2.39645, "heading":1.65586, "vx":-1.33111, "vy":-1.05584, "omega":-0.84286, "ax":-2.67704, "ay":-1.34886, "alpha":0.84309, "fx":[-46.00331,-42.35758,-42.8974,-47.24162], "fy":[-24.90534,-24.19478,-20.2919,-20.5474]}, - {"t":0.59703, "x":4.49067, "y":2.36589, "heading":1.6319, "vx":-1.40722, "vy":-1.09419, "omega":-0.81889, "ax":-2.72306, "ay":-1.25304, "alpha":0.8588, "fx":[-47.10053,-43.20241,-43.69451,-47.57105], "fy":[-22.99409,-23.24547,-18.52785,-18.78305]}, - {"t":0.62546, "x":4.44956, "y":2.33427, "heading":1.60862, "vx":-1.48463, "vy":-1.12981, "omega":-0.79448, "ax":-2.77045, "ay":-1.14409, "alpha":0.96746, "fx":[-47.91382,-43.67763,-44.08255,-49.0541], "fy":[-21.77776,-21.14803,-16.60606,-16.75378]}, - {"t":0.65389, "x":4.40623, "y":2.30169, "heading":1.58603, "vx":-1.5634, "vy":-1.16234, "omega":-0.76697, "ax":-2.8184, "ay":-1.01979, "alpha":0.97533, "fx":[-49.04218,-44.61186,-44.92693,-49.34485], "fy":[-19.34417,-19.72967,-14.40085,-14.52304]}, - {"t":0.68232, "x":4.36065, "y":2.26823, "heading":1.56422, "vx":-1.64353, "vy":-1.19133, "omega":-0.73925, "ax":-2.86567, "ay":-0.87765, "alpha":1.0826, "fx":[-49.86724,-45.08467,-45.29481,-50.83096], "fy":[-17.59381,-17.05857,-11.9233,-11.9446]}, - {"t":0.71075, "x":4.31277, "y":2.23401, "heading":1.54321, "vx":-1.725, "vy":-1.21628, "omega":-0.70847, "ax":-2.91035, "ay":-0.71493, "alpha":1.08939, "fx":[-50.94037,-46.00702,-46.09087,-51.01845], "fy":[-14.4506,-15.06326,-9.09274,-9.06362]}, - {"t":0.73918, "x":4.26255, "y":2.19914, "heading":1.52307, "vx":-1.80774, "vy":-1.23661, "omega":-0.6775, "ax":-2.94966, "ay":-0.52881, "alpha":1.18723, "fx":[-51.64295,-46.34863,-46.31226,-52.37372], "fy":[-11.95979,-11.59304,-5.90853,-5.79854]}, - {"t":0.76761, "x":4.20996, "y":2.16377, "heading":1.5038, "vx":-1.8916, "vy":-1.25164, "omega":-0.64374, "ax":-2.9797, "ay":-0.31671, "alpha":1.19514, "fx":[-52.48131,-47.05872,-46.85788,-52.28281], "fy":[-8.00526,-8.75302,-2.27262,-2.08641]}, - {"t":0.79604, "x":4.15498, "y":2.12806, "heading":1.4855, "vx":-1.97631, "vy":-1.26065, "omega":-0.60977, "ax":-2.9953, "ay":-0.07681, "alpha":1.29379, "fx":[-52.77554,-46.97263,-46.65244,-53.31994], "fy":[-4.57854,-4.40304,1.81229,2.04752]}, - {"t":0.82447, "x":4.09758, "y":2.09219, "heading":1.46817, "vx":-2.06147, "vy":-1.26283, "omega":-0.57298, "ax":-2.98998, "ay":0.19107, "alpha":1.30995, "fx":[-53.08801,-47.11718,-46.59318,-52.5679], "fy":[0.25211,-0.62075,6.38407,6.72482]}, - {"t":0.8529, "x":4.03777, "y":2.05636, "heading":1.45188, "vx":-2.14647, "vy":-1.2574, "omega":-0.53574, "ax":-2.95644, "ay":0.48465, "alpha":1.4182, "fx":[-52.59669,-46.16463,-45.53534,-52.83287], "fy":[4.59307,4.56335,11.39825,11.7608]}, - {"t":0.88133, "x":3.97555, "y":2.02081, "heading":1.43665, "vx":-2.23052, "vy":-1.24362, "omega":-0.49542, "ax":-2.91607, "ay":0.65493, "alpha":0.6402, "fx":[-50.07105,-48.28496,-46.34809,-49.73367], "fy":[9.35474,8.97441,12.50517,12.83499]}, - {"t":0.90338, "x":3.92564, "y":1.99354, "heading":1.42572, "vx":-2.29484, "vy":-1.22917, "omega":-0.4813, "ax":-2.91155, "ay":0.69669, "alpha":0.87334, "fx":[-50.81079,-46.81396,-46.25838,-50.2537], "fy":[9.74434,9.04095,13.64884,14.01996]}, - {"t":0.92544, "x":3.87432, "y":1.9666, "heading":1.4151, "vx":-2.35905, "vy":-1.21381, "omega":-0.46204, "ax":-2.89841, "ay":0.74652, "alpha":0.90272, "fx":[-50.56486,-47.33385,-45.42036,-49.94151], "fy":[10.36316,9.78499,14.55659,15.0718]}, - {"t":0.9475, "x":3.82158, "y":1.94001, "heading":1.40491, "vx":-2.42298, "vy":-1.19734, "omega":-0.44213, "ax":-2.88056, "ay":0.80924, "alpha":1.01887, "fx":[-50.76851,-46.04804,-45.26987,-49.98351], "fy":[11.30951,10.56353,15.76671,16.31911]}, - {"t":0.96955, "x":3.76744, "y":1.9138, "heading":1.39516, "vx":-2.48651, "vy":-1.17949, "omega":-0.41966, "ax":-2.85528, "ay":0.89027, "alpha":0.96929, "fx":[-50.04228,-46.72442,-44.38583,-49.23184], "fy":[12.64841,11.92571,17.07032,17.71697]}, - {"t":0.99161, "x":3.71191, "y":1.888, "heading":1.38591, "vx":-2.54949, "vy":-1.15986, "omega":-0.39828, "ax":-2.81735, "ay":0.99895, "alpha":1.00279, "fx":[-49.76934,-45.05102,-44.16559,-48.86921], "fy":[14.49945,13.86531,18.79593,19.44762]}, - {"t":1.01366, "x":3.65499, "y":1.86266, "heading":1.37712, "vx":-2.61163, "vy":-1.13783, "omega":-0.37616, "ax":-2.75617, "ay":1.15107, "alpha":0.887, "fx":[-48.19803,-45.35453,-42.88595,-47.33745], "fy":[17.21903,16.46603,21.19384,21.87235]}, - {"t":1.03572, "x":3.59672, "y":1.83784, "heading":1.36883, "vx":-2.67242, "vy":-1.11244, "omega":-0.3566, "ax":-2.64701, "ay":1.37618, "alpha":0.83061, "fx":[-46.55226,-42.5428,-41.70694,-45.69578], "fy":[21.11711,20.77577,24.6218,25.2462]}, - {"t":1.05778, "x":3.53713, "y":1.81364, "heading":1.36096, "vx":-2.7308, "vy":-1.08209, "omega":-0.33828, "ax":-2.42435, "ay":1.72901, "alpha":0.68175, "fx":[-42.25942,-39.64972,-38.20454,-41.53762], "fy":[27.38485,26.79387,30.27282,30.83562]}, - {"t":1.07983, "x":3.47631, "y":1.7902, "heading":1.3535, "vx":-2.78427, "vy":-1.04395, "omega":-0.32324, "ax":-1.90115, "ay":2.27964, "alpha":0.45944, "fx":[-33.25667,-30.71135,-30.13192,-32.66517], "fy":[36.64446,37.40579,38.75872,39.19307]}, - {"t":1.10189, "x":3.41444, "y":1.76773, "heading":1.34637, "vx":-2.8262, "vy":-0.99367, "omega":-0.31311, "ax":-1.06406, "ay":2.75736, "alpha":0.33086, "fx":[-18.76425,-16.83061,-16.93035,-18.42459], "fy":[45.35547,45.0718,46.575,46.85314]}, - {"t":1.12394, "x":3.35185, "y":1.74648, "heading":1.33946, "vx":-2.84967, "vy":-0.93286, "omega":-0.30581, "ax":-0.88824, "ay":2.79869, "alpha":-0.08234, "fx":[-15.02208,-14.65675,-14.60325,-14.94423], "fy":[45.8849,48.3325,46.16497,46.22869]}, - {"t":1.146, "x":3.28878, "y":1.72659, "heading":1.33272, "vx":-2.86926, "vy":-0.87113, "omega":-0.30763, "ax":-0.81176, "ay":2.77573, "alpha":-0.23823, "fx":[-12.72224,-14.50987,-13.93567,-12.95889], "fy":[46.65605,46.87666,45.88647,45.66097]}, - {"t":1.16805, "x":3.2253, "y":1.70805, "heading":1.32593, "vx":-2.88717, "vy":-0.80991, "omega":-0.31288, "ax":-0.693, "ay":2.59238, "alpha":-0.60242, "fx":[-10.01972,-12.49331,-13.08579,-10.60922], "fy":[43.73969,45.78677,41.94965,41.3791]}, - {"t":1.19011, "x":3.16145, "y":1.69082, "heading":1.31903, "vx":-2.90245, "vy":-0.75273, "omega":-0.32617, "ax":2.21285, "ay":-1.45266, "alpha":-0.75011, "fx":[38.80626,36.6834,34.16117,37.89751], "fy":[-22.75146,-21.75964,-25.67981,-26.66937]}, - {"t":1.21217, "x":3.09797, "y":1.67386, "heading":1.31184, "vx":-2.85365, "vy":-0.78477, "omega":-0.34271, "ax":2.8889, "ay":-0.58749, "alpha":-0.96954, "fx":[51.21267,46.35667,45.0935,49.96339], "fy":[-7.67835,-7.52089,-11.35815,-12.61509]}, - {"t":1.23422, "x":3.03573, "y":1.65641, "heading":1.30428, "vx":-2.78993, "vy":-0.79773, "omega":-0.3641, "ax":2.93892, "ay":-0.4447, "alpha":-1.18729, "fx":[52.25026,47.74007,45.20823,50.76297], "fy":[-5.2185,-3.62863,-9.6269,-11.17795]}, - {"t":1.25628, "x":2.97491, "y":1.63871, "heading":1.29625, "vx":-2.72511, "vy":-0.80753, "omega":-0.39028, "ax":2.95576, "ay":-0.38879, "alpha":-1.32918, "fx":[53.24846,47.04624,45.28147,51.50798], "fy":[-4.02202,-2.63572,-8.7665,-10.49946]}, - {"t":1.27833, "x":2.91553, "y":1.6208, "heading":1.28764, "vx":-2.65992, "vy":-0.81611, "omega":-0.4196, "ax":2.96398, "ay":-0.36018, "alpha":-1.41642, "fx":[53.33865,48.03826,44.82643,51.429], "fy":[-3.46376,-1.44866,-8.57535,-10.52855]}, - {"t":1.30039, "x":2.85758, "y":1.60271, "heading":1.27839, "vx":-2.59454, "vy":-0.82405, "omega":-0.45084, "ax":2.96893, "ay":-0.34203, "alpha":-1.51629, "fx":[53.96883,47.10749,44.99522,51.89096], "fy":[-3.09963,-1.0162,-8.29884,-10.39134]}, - {"t":1.32245, "x":2.80108, "y":1.58445, "heading":1.26844, "vx":-2.52906, "vy":-0.8316, "omega":-0.48428, "ax":2.97213, "ay":-0.33041, "alpha":-1.51461, "fx":[53.74437,48.46891,44.43451,51.52815], "fy":[-2.87501,-0.55635,-8.17903,-10.42097]}, - {"t":1.3445, "x":2.74602, "y":1.56603, "heading":1.25776, "vx":-2.46351, "vy":-0.83889, "omega":-0.51769, "ax":2.97446, "ay":-0.32153, "alpha":-1.58125, "fx":[54.26244,47.23131,44.88261,51.95486], "fy":[-2.75613,-0.34874,-7.99254,-10.34186]}, - {"t":1.36656, "x":2.69241, "y":1.54745, "heading":1.24634, "vx":-2.3979, "vy":-0.84598, "omega":-0.55256, "ax":2.97618, "ay":-0.31503, "alpha":-1.5444, "fx":[54.00929,48.38818,44.45644,51.59176], "fy":[-2.70895,-0.19832,-7.83487,-10.26352]}, - {"t":1.38861, "x":2.64025, "y":1.52872, "heading":1.23416, "vx":-2.33226, "vy":-0.85293, "omega":-0.58663, "ax":2.97753, "ay":-0.30973, "alpha":-1.56089, "fx":[54.26648,47.45444,44.97945,51.83549], "fy":[-2.72584,-0.08428,-7.66887,-10.17325]}, - {"t":1.41067, "x":2.58953, "y":1.50983, "heading":1.22122, "vx":-2.26659, "vy":-0.85976, "omega":-0.62105, "ax":2.9786, "ay":-0.30557, "alpha":-1.49501, "fx":[54.03571,48.29389,44.74559,51.53188], "fy":[-2.78334,-0.19892,-7.44309,-9.9498]}, - {"t":1.43272, "x":2.54026, "y":1.49079, "heading":1.20752, "vx":-2.2009, "vy":-0.8665, "omega":-0.65403, "ax":2.97948, "ay":-0.30203, "alpha":-1.45591, "fx":[54.01829,47.76854,45.29371,51.58552], "fy":[-2.87933,-0.21064,-7.25674,-9.79228]}, - {"t":1.45478, "x":2.49245, "y":1.47161, "heading":1.1931, "vx":-2.13518, "vy":-0.87316, "omega":-0.68614, "ax":2.98021, "ay":-0.29914, "alpha":-1.35271, "fx":[53.6997,48.58936,45.17432,51.25125], "fy":[-3.02526,-0.51248,-6.98122,-9.42745]}, - {"t":1.47684, "x":2.44608, "y":1.45228, "heading":1.17796, "vx":-2.06945, "vy":-0.87976, "omega":-0.71597, "ax":2.98083, "ay":-0.29663, "alpha":-1.26759, "fx":[53.50209,48.16478,45.85863,51.23056], "fy":[-3.19738,-0.65236,-6.768,-9.16104]}, - {"t":1.49889, "x":2.40116, "y":1.4328, "heading":1.16217, "vx":-2.0037, "vy":-0.8863, "omega":-0.74393, "ax":2.98136, "ay":-0.29447, "alpha":-1.11617, "fx":[53.04812,48.97133,45.90898,50.86307], "fy":[-3.40462,-1.1723,-6.43702,-8.62102]}, - {"t":1.52095, "x":2.35769, "y":1.41318, "heading":1.14576, "vx":-1.93795, "vy":-0.89279, "omega":-0.76855, "ax":2.98182, "ay":-0.29257, "alpha":-0.97691, "fx":[52.66385,48.63491,46.73628,50.78731], "fy":[-3.64367,-1.49699,-6.18532,-8.18219]}, - {"t":1.543, "x":2.31567, "y":1.39342, "heading":1.12881, "vx":-1.87218, "vy":-0.89925, "omega":-0.7901, "ax":2.98223, "ay":-0.29092, "alpha":-0.76416, "fx":[51.99206,49.48953,47.00163,50.366], "fy":[-3.90094,-2.24597,-5.81259,-7.43839]}, - {"t":1.56506, "x":2.27511, "y":1.37351, "heading":1.11139, "vx":-1.80641, "vy":-0.90566, "omega":-0.80695, "ax":2.98258, "ay":-0.28944, "alpha":-0.56119, "fx":[51.41845,49.1682,48.01412,50.27227], "fy":[-4.20517,-2.81682,-5.5223,-6.75474]}, - {"t":1.58712, "x":2.23599, "y":1.35347, "heading":1.09359, "vx":-1.74062, "vy":-0.91205, "omega":-0.81933, "ax":2.9829, "ay":-0.28812, "alpha":-0.27057, "fx":[50.46763,49.99814,48.60467,49.82375], "fy":[-4.49698,-3.84525,-5.11659,-5.75224]}, - {"t":1.60917, "x":2.19832, "y":1.33328, "heading":1.07552, "vx":-1.67483, "vy":-0.9184, "omega":-0.8253, "ax":2.98319, "ay":-0.28693, "alpha":0.01589, "fx":[49.64374,49.75083,49.81165,49.70693], "fy":[-4.85691,-4.75652,-4.78873,-4.72951]}, - {"t":1.63123, "x":2.16211, "y":1.31296, "heading":1.05731, "vx":-1.60904, "vy":-0.92473, "omega":-0.82495, "ax":2.98344, "ay":-0.28585, "alpha":0.40854, "fx":[48.29893,50.58047,50.8195,49.23132], "fy":[-5.17418,-6.11912,-4.36865,-3.39783]}, - {"t":1.65328, "x":2.12735, "y":1.29249, "heading":1.03912, "vx":-1.54323, "vy":-0.93103, "omega":-0.81594, "ax":2.98367, "ay":-0.28487, "alpha":0.80115, "fx":[47.17248,50.36689,52.28536,49.12094], "fy":[-5.58194,-7.47941,-4.01252,-1.92074]}, - {"t":1.67534, "x":2.09404, "y":1.27189, "heading":1.02112, "vx":-1.47743, "vy":-0.93732, "omega":-0.79827, "ax":2.98389, "ay":-0.28397, "alpha":1.32881, "fx":[45.29446,51.20323,53.83201,48.63007], "fy":[-5.91305,-9.27008,-3.59531,-0.1562]}, - {"t":1.6974, "x":2.06218, "y":1.25115, "heading":1.00352, "vx":-1.41161, "vy":-0.94358, "omega":-0.76896, "ax":2.98408, "ay":-0.28315, "alpha":1.86042, "fx":[43.77993,51.00221,55.63573,48.55475], "fy":[-6.35636,-11.23153,-3.21755,1.92549]}, - {"t":1.71945, "x":2.03177, "y":1.23026, "heading":0.98656, "vx":-1.3458, "vy":-0.94983, "omega":-0.72792, "ax":2.98426, "ay":-0.28239, "alpha":2.56326, "fx":[41.20793,51.82277,57.87592,48.07781], "fy":[-6.69974,-13.5607,-2.82406,4.25524]}, - {"t":1.74151, "x":2.00281, "y":1.20925, "heading":0.9705, "vx":-1.27998, "vy":-0.95605, "omega":-0.67139, "ax":2.98442, "ay":-0.2817, "alpha":3.27362, "fx":[39.17465,51.66355,60.10351,48.05355], "fy":[-7.18225,-16.2759,-2.43634,7.11157]}, - {"t":1.76356, "x":1.97531, "y":1.18809, "heading":0.95569, "vx":-1.21415, "vy":-0.96227, "omega":-0.59919, "ax":2.98457, "ay":-0.28104, "alpha":4.04823, "fx":[36.25182,52.97375,61.67014,48.10963], "fy":[-7.53129,-19.24908,-2.10941,10.15053]}, - {"t":1.78562, "x":1.94925, "y":1.1668, "heading":0.94248, "vx":-1.14833, "vy":-0.96847, "omega":-0.5099, "ax":2.99425, "ay":-0.13109, "alpha":4.87684, "fx":[34.06595,52.50221,65.28809,47.79451], "fy":[-5.26241,-19.60527,0.60964,15.51742]}, - {"t":1.81971, "x":1.91185, "y":1.13371, "heading":0.9251, "vx":-1.04625, "vy":-0.97293, "omega":-0.34365, "ax":2.9928, "ay":0.18503, "alpha":3.72743, "fx":[37.54044,52.26062,60.75489,48.99802], "fy":[1.2503,-10.75566,4.93621,16.90673]}, - {"t":1.8538, "x":1.87792, "y":1.10065, "heading":0.91338, "vx":-0.94422, "vy":-0.96663, "omega":-0.21658, "ax":2.95375, "ay":0.51598, "alpha":2.98793, "fx":[39.57151,50.77215,58.72022,47.88667], "fy":[7.29687,-2.17651,9.80198,19.48198]}, - {"t":1.88789, "x":1.84744, "y":1.06799, "heading":0.906, "vx":-0.84353, "vy":-0.94904, "omega":-0.11472, "ax":2.87549, "ay":0.84991, "alpha":2.26664, "fx":[40.35693,49.5252,54.33816,47.51217], "fy":[13.48028,5.56069,15.02767,22.6015]}, - {"t":1.92198, "x":1.82036, "y":1.03614, "heading":0.90209, "vx":-0.7455, "vy":-0.92006, "omega":-0.03744, "ax":2.75923, "ay":1.17367, "alpha":1.82161, "fx":[40.03727,46.99649,51.88828,45.05809], "fy":[18.98291,13.07206,20.07087,26.13247]}, - {"t":1.95607, "x":1.79655, "y":1.00545, "heading":0.90081, "vx":-0.65144, "vy":-0.88005, "omega":0.02466, "ax":2.61053, "ay":1.47519, "alpha":1.42633, "fx":[38.62056,44.24435,48.15529,43.0446], "fy":[24.39818,19.50433,24.90815,29.552]}, - {"t":1.99016, "x":1.77586, "y":0.97631, "heading":0.90165, "vx":-0.56244, "vy":-0.82976, "omega":0.07328, "ax":2.43811, "ay":1.74553, "alpha":1.03845, "fx":[37.17011,41.25368,44.10086,40.04393], "fy":[28.79554,25.50761,29.30713,32.77863]}, - {"t":2.02425, "x":1.7581, "y":0.94904, "heading":0.90415, "vx":-0.47933, "vy":-0.77026, "omega":0.10868, "ax":2.2519, "ay":1.98004, "alpha":0.75278, "fx":[34.86143,37.92048,40.01027,37.36011], "fy":[33.00326,30.33803,33.13356,35.55052]}, - {"t":2.05834, "x":1.74307, "y":0.92393, "heading":0.90785, "vx":-0.40256, "vy":-0.70275, "omega":0.13434, "ax":2.06111, "ay":2.17803, "alpha":0.45496, "fx":[32.78322,34.63678,35.9384,34.07215], "fy":[36.13608,34.83105,36.3674,37.89264]}, - {"t":2.09243, "x":1.73054, "y":0.90124, "heading":0.91243, "vx":-0.33229, "vy":-0.6285, "omega":0.14985, "ax":1.8731, "ay":2.34177, "alpha":0.21359, "fx":[30.39365,31.3095,31.91449,31.27718], "fy":[39.17659,38.23801,39.03228,39.6978]}, - {"t":2.12652, "x":1.7203, "y":0.88117, "heading":0.91754, "vx":-0.26844, "vy":-0.54867, "omega":0.15713, "ax":1.69302, "ay":2.47517, "alpha":-0.05615, "fx":[28.36167,28.18137,28.09268,28.25178], "fy":[41.20859,41.57117,41.188,41.07192]}, - {"t":2.16061, "x":1.71213, "y":0.8639, "heading":0.9229, "vx":-0.21072, "vy":-0.46429, "omega":0.15522, "ax":1.52396, "ay":2.58278, "alpha":-0.30184, "fx":[26.38485,25.17973,24.33958,25.71079], "fy":[43.34235,43.99324,42.90402,41.97494]}, - {"t":2.19471, "x":1.70584, "y":0.84958, "heading":0.92819, "vx":-0.15877, "vy":-0.37624, "omega":0.14493, "ax":1.36742, "ay":2.66903, "alpha":-0.58989, "fx":[24.76613,22.40217,20.82619,23.1825], "fy":[44.61835,46.60442,44.24244,42.50078]}, - {"t":2.2288, "x":1.70122, "y":0.8383, "heading":0.93313, "vx":-0.11215, "vy":-0.28526, "omega":0.12482, "ax":1.22377, "ay":2.73794, "alpha":-0.88602, "fx":[23.45567,19.80315,17.30391,21.03591], "fy":[46.14441,48.50649,45.27945,42.63031]}, - {"t":2.26289, "x":1.69811, "y":0.83017, "heading":0.93739, "vx":-0.07044, "vy":-0.19192, "omega":0.09462, "ax":1.09269, "ay":2.7929, "alpha":-1.16951, "fx":[22.41117,17.44472,13.96429,19.03815], "fy":[47.19138,50.11299,46.27574,42.64523]}, - {"t":2.29698, "x":1.69634, "y":0.82525, "heading":0.94061, "vx":-0.03318, "vy":-0.09671, "omega":0.05475, "ax":0.97343, "ay":2.83672, "alpha":-1.60593, "fx":[21.94894,15.10052,10.49591,17.36132], "fy":[47.91436,52.46266,46.70375,42.06657]}, - {"t":2.33107, "x":1.69577, "y":0.8236, "heading":0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":4.89419, "y":2.71466, "heading":2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.70082, "ay":-3.71939, "alpha":-17.78986, "fx":[-81.59535,-133.59766,-84.80008,-13.44888], "fy":[3.36383,-39.4942,-113.59489,-98.2767]}, + {"t":0.02154, "x":4.8931, "y":2.7138, "heading":2.0944, "vx":-0.10124, "vy":-0.0801, "omega":-0.38312, "ax":-4.70372, "ay":-3.72014, "alpha":-9.83015, "fx":[-78.66981,-110.28521,-81.21054,-43.46935], "fy":[-27.94922,-47.82996,-91.94008,-80.3323]}, + {"t":0.04307, "x":4.88982, "y":2.71121, "heading":2.08614, "vx":-0.20253, "vy":-0.16022, "omega":-0.59482, "ax":-4.70444, "ay":-3.71902, "alpha":-4.35004, "fx":[-77.82288,-92.94695,-79.2704,-63.64284], "fy":[-47.4686,-54.96912,-75.87567,-69.6634]}, + {"t":0.06461, "x":4.88437, "y":2.7069, "heading":2.07333, "vx":-0.30385, "vy":-0.24031, "omega":-0.6885, "ax":-4.70523, "ay":-3.71779, "alpha":-1.71619, "fx":[-78.20896,-84.19028,-78.81329,-72.52298], "fy":[-56.38788,-59.01089,-67.39528,-65.10122]}, + {"t":0.08614, "x":4.87674, "y":2.70086, "heading":2.05851, "vx":-0.40518, "vy":-0.32037, "omega":-0.72546, "ax":-4.70609, "ay":-3.71646, "alpha":-0.6148, "fx":[-78.14693,-80.6079,-78.48922,-76.54869], "fy":[-60.0273,-60.74965,-64.0329,-62.99633]}, + {"t":0.10768, "x":4.86692, "y":2.6931, "heading":2.04288, "vx":-0.50653, "vy":-0.40041, "omega":-0.7387, "ax":-4.70703, "ay":-3.71498, "alpha":-0.24154, "fx":[-78.4129,-79.27894,-78.57172,-77.59233], "fy":[-61.14022,-61.55002,-62.62767,-62.38998]}, + {"t":0.12921, "x":4.85492, "y":2.68361, "heading":2.02698, "vx":-0.60789, "vy":-0.48041, "omega":-0.7439, "ax":-4.70808, "ay":-3.71336, "alpha":-0.0691, "fx":[-78.29,-78.82295,-78.45191,-78.36063], "fy":[-61.80947,-61.63411,-62.18146,-61.97442]}, + {"t":0.15075, "x":4.84074, "y":2.6724, "heading":2.01096, "vx":-0.70929, "vy":-0.56038, "omega":-0.74539, "ax":-4.70924, "ay":-3.71155, "alpha":-0.0233, "fx":[-78.53642,-78.55937,-78.53165,-78.37545], "fy":[-61.72164,-61.94589,-61.88904,-61.92223]}, + {"t":0.17229, "x":4.82437, "y":2.65948, "heading":1.9949, "vx":-0.8107, "vy":-0.64031, "omega":-0.74589, "ax":-4.71054, "ay":-3.70952, "alpha":0.02677, "fx":[-78.34039,-78.54799,-78.42035,-78.78073], "fy":[-62.0555,-61.71531,-61.86756,-61.70538]}, + {"t":0.19382, "x":4.80582, "y":2.64483, "heading":1.97884, "vx":-0.91215, "vy":-0.7202, "omega":-0.74531, "ax":-4.71199, "ay":-3.70725, "alpha":0.0206, "fx":[-78.61612,-78.44601,-78.54881,-78.57566], "fy":[-61.74241,-62.00061,-61.68875,-61.76028]}, + {"t":0.21536, "x":4.78508, "y":2.62846, "heading":1.96279, "vx":-1.01362, "vy":-0.80004, "omega":-0.74487, "ax":-4.71364, "ay":-3.70467, "alpha":0.04753, "fx":[-78.33705,-78.58842,-78.49648,-78.87474], "fy":[-62.17158,-61.58887,-61.69141,-61.56809]}, + {"t":0.23689, "x":4.76216, "y":2.61037, "heading":1.94675, "vx":-1.11514, "vy":-0.87982, "omega":-0.74384, "ax":-4.71553, "ay":-3.70171, "alpha":0.03024, "fx":[-78.68764,-78.45315,-78.62234,-78.65952], "fy":[-61.59445,-62.04667,-61.5589,-61.62286]}, + {"t":0.25843, "x":4.73705, "y":2.59056, "heading":1.93073, "vx":-1.21669, "vy":-0.95954, "omega":-0.74319, "ax":-4.71771, "ay":-3.6983, "alpha":0.05265, "fx":[-78.3469,-78.65107,-78.54579,-79.02377], "fy":[-62.11132,-61.4606,-61.62596,-61.39789]}, + {"t":0.27996, "x":4.70975, "y":2.56904, "heading":1.91472, "vx":-1.31829, "vy":-1.03919, "omega":-0.74206, "ax":-4.72023, "ay":-3.69433, "alpha":0.03104, "fx":[-78.75619,-78.51813,-78.72456,-78.73723], "fy":[-61.40795,-62.02269,-61.43011,-61.47032]}, + {"t":0.3015, "x":4.68027, "y":2.5458, "heading":1.89874, "vx":-1.41994, "vy":-1.11875, "omega":-0.74139, "ax":-4.72322, "ay":-3.68964, "alpha":0.0575, "fx":[-78.40408,-78.71781,-78.5983,-79.21483], "fy":[-61.98658,-61.2893,-61.55572,-61.18652]}, + {"t":0.32304, "x":4.6486, "y":2.52086, "heading":1.88278, "vx":-1.52166, "vy":-1.1982, "omega":-0.74015, "ax":-4.7268, "ay":-3.68399, "alpha":0.03737, "fx":[-78.87964,-78.59269,-78.82779,-78.87374], "fy":[-61.14105,-61.99776,-61.25376,-61.24913]}, + {"t":0.34457, "x":4.61473, "y":2.4942, "heading":1.86684, "vx":-1.62345, "vy":-1.27754, "omega":-0.73935, "ax":-4.73116, "ay":-3.6771, "alpha":0.08119, "fx":[-78.43338,-78.84362,-78.75432,-79.4334], "fy":[-62.04328,-60.99967,-61.25823,-60.88073]}, + {"t":0.36611, "x":4.57867, "y":2.46583, "heading":1.85091, "vx":-1.72534, "vy":-1.35673, "omega":-0.7376, "ax":-4.73659, "ay":-3.66849, "alpha":0.06952, "fx":[-79.05719,-78.64987,-78.99401,-79.12563], "fy":[-60.88488,-61.94516,-60.88144,-60.89612]}, + {"t":0.38764, "x":4.54042, "y":2.43576, "heading":1.83503, "vx":-1.82735, "vy":-1.43573, "omega":-0.7361, "ax":-4.74356, "ay":-3.65738, "alpha":0.14178, "fx":[-78.574,-78.9529,-78.96694,-79.79764], "fy":[-62.16641,-60.63849,-60.68103,-60.38142]}, + {"t":0.40918, "x":4.49996, "y":2.40399, "heading":1.81918, "vx":-1.9295, "vy":-1.5145, "omega":-0.73305, "ax":-4.75279, "ay":-3.6426, "alpha":0.17924, "fx":[-79.42144,-78.59837,-79.19132,-79.69572], "fy":[-60.58957,-62.05465,-60.07977,-60.1574]}, + {"t":0.43071, "x":4.45731, "y":2.37053, "heading":1.80339, "vx":-2.03186, "vy":-1.59294, "omega":-0.72919, "ax":-4.76561, "ay":-3.6219, "alpha":0.28871, "fx":[-79.26173,-78.83709,-79.19769,-80.46498], "fy":[-62.03097,-60.39274,-59.57296,-59.50463]}, + {"t":0.45225, "x":4.41244, "y":2.33539, "heading":1.78769, "vx":-2.13449, "vy":-1.67094, "omega":-0.72297, "ax":-4.78464, "ay":-3.59083, "alpha":0.43467, "fx":[-80.24142,-78.42436,-79.43893,-80.92569], "fy":[-60.25836,-62.13272,-58.39782,-58.64044]}, + {"t":0.47379, "x":4.36537, "y":2.29857, "heading":1.77212, "vx":-2.23753, "vy":-1.74828, "omega":-0.71361, "ax":-4.81542, "ay":-3.53954, "alpha":0.68401, "fx":[-80.32381,-78.52278,-79.4034,-82.83279], "fy":[-61.89078,-59.71967,-57.14005,-57.25943]}, + {"t":0.49532, "x":4.31606, "y":2.2601, "heading":1.75675, "vx":-2.34123, "vy":-1.8245, "omega":-0.69888, "ax":-4.87802, "ay":-3.43268, "alpha":0.96332, "fx":[-82.5246,-78.66623,-80.24385,-83.8222], "fy":[-58.80187,-61.38329,-54.03399,-54.66521]}, + {"t":0.51686, "x":4.26451, "y":2.22001, "heading":1.7417, "vx":-2.44628, "vy":-1.89843, "omega":-0.67813, "ax":-5.06067, "ay":-3.09374, "alpha":1.25745, "fx":[-85.23866,-80.91923,-82.23983,-89.0383], "fy":[-55.76787,-53.52721,-48.01344,-48.97619]}, + {"t":0.53839, "x":4.21066, "y":2.17841, "heading":1.72709, "vx":-2.55527, "vy":-1.96505, "omega":-0.65105, "ax":-5.24666, "ay":1.21777, "alpha":1.14772, "fx":[-89.60848,-84.2254,-85.33881,-90.66457], "fy":[16.78958,18.25091,23.83244,22.32562]}, + {"t":0.55993, "x":4.15441, "y":2.13638, "heading":1.71307, "vx":-2.66826, "vy":-1.93883, "omega":-0.62634, "ax":-3.15403, "ay":4.49644, "alpha":0.40169, "fx":[-53.66397,-50.9293,-51.52439,-54.18695], "fy":[75.36221,73.29645,76.09367,75.06137]}, + {"t":0.58146, "x":4.09622, "y":2.09566, "heading":1.69958, "vx":-2.73618, "vy":-1.84199, "omega":-0.61769, "ax":-3.08437, "ay":4.77197, "alpha":-0.32854, "fx":[-50.74866,-51.28607,-52.17818,-51.44672], "fy":[79.84825,82.07339,78.04695,78.21742]}, + {"t":0.603, "x":4.03658, "y":2.0571, "heading":1.68628, "vx":-2.80261, "vy":-1.73923, "omega":-0.62476, "ax":-2.89718, "ay":5.01487, "alpha":-2.09428, "fx":[-42.61351,-53.52681,-53.38998,-43.64781], "fy":[90.76732,86.27839,77.40553,79.93063]}, + {"t":0.62453, "x":3.97555, "y":2.02081, "heading":1.67283, "vx":-2.865, "vy":-1.63123, "omega":-0.66986, "ax":-2.73663, "ay":4.86591, "alpha":-7.46206, "fx":[-28.87061,-63.27615,-64.53109,-25.79543], "fy":[101.76285,96.14677,59.04941,67.49047]}, + {"t":0.64611, "x":3.91311, "y":1.98675, "heading":1.65838, "vx":-2.92403, "vy":-1.52626, "omega":-0.83084, "ax":-2.51998, "ay":5.05045, "alpha":-2.75311, "fx":[-35.11547,-48.87117,-48.94182,-35.09884], "fy":[93.13297,87.99901,76.16828,79.45426]}, + {"t":0.66768, "x":3.84944, "y":1.955, "heading":1.64045, "vx":-2.9784, "vy":-1.41731, "omega":-0.89023, "ax":-2.13067, "ay":4.67916, "alpha":-0.75044, "fx":[-33.89192,-36.3333,-37.2171,-34.62645], "fy":[79.81265,81.07109,75.1682,75.94535]}, + {"t":0.68925, "x":3.7847, "y":1.92552, "heading":1.62125, "vx":-3.02436, "vy":-1.31637, "omega":-0.90641, "ax":-1.51171, "ay":3.59667, "alpha":-0.27871, "fx":[-24.24416,-25.50982,-25.78359,-25.2603], "fy":[61.63628,60.08922,59.01689,59.07684]}, + {"t":0.71082, "x":3.7191, "y":1.89796, "heading":1.6017, "vx":-3.05697, "vy":-1.23878, "omega":-0.91243, "ax":-1.00733, "ay":2.55043, "alpha":0.08115, "fx":[-17.13773,-15.74791,-16.46179,-17.81917], "fy":[42.23093,43.42584,42.36337,42.03777]}, + {"t":0.7324, "x":3.65292, "y":1.87183, "heading":1.58201, "vx":-3.0787, "vy":-1.18376, "omega":-0.91068, "ax":-0.61314, "ay":1.62306, "alpha":0.01814, "fx":[-9.99689,-9.87575,-10.30407,-10.7062], "fy":[27.13242,27.07195,27.19583,26.82235]}, + {"t":0.75397, "x":3.58637, "y":1.84667, "heading":1.56237, "vx":-3.09193, "vy":-1.14875, "omega":-0.91029, "ax":-0.40058, "ay":1.08928, "alpha":0.17504, "fx":[-7.17868,-5.64673,-6.18424,-7.70029], "fy":[17.82371,18.30241,18.4662,18.03876]}, + {"t":0.77554, "x":3.51957, "y":1.82214, "heading":1.54273, "vx":-3.10057, "vy":-1.12525, "omega":-0.90651, "ax":-0.20628, "ay":0.57341, "alpha":0.08401, "fx":[-3.42744,-3.0416,-3.43346,-3.85186], "fy":[9.08528,9.59102,9.97026,9.58698]}, + {"t":0.79711, "x":3.45264, "y":1.798, "heading":1.52318, "vx":-3.10502, "vy":-1.11288, "omega":-0.9047, "ax":-0.1323, "ay":0.36945, "alpha":0.20049, "fx":[-2.74968,-1.3059,-1.66419,-3.10194], "fy":[5.86323,5.91315,6.60199,6.25593]}, + {"t":0.81868, "x":3.38563, "y":1.77408, "heading":1.50366, "vx":-3.10787, "vy":-1.10491, "omega":-0.90037, "ax":-0.04515, "ay":0.12803, "alpha":0.11944, "fx":[-0.89191,-0.35401,-0.63333,-1.13148], "fy":[1.48678,2.08648,2.62116,2.34212]}, + {"t":0.84026, "x":3.31857, "y":1.75027, "heading":1.48424, "vx":-3.10885, "vy":-1.10215, "omega":-0.8978, "ax":-0.02581, "ay":0.07246, "alpha":0.20658, "fx":[-0.99404,0.33483,0.13284,-1.19467], "fy":[0.9241,0.77917,1.673,1.455]}, + {"t":0.86183, "x":3.2515, "y":1.72651, "heading":1.46487, "vx":-3.1094, "vy":-1.10059, "omega":-0.89334, "ax":0.01004, "ay":-0.02835, "alpha":0.14363, "fx":[-0.09878,0.55694,0.41031,-0.19916], "fy":[-1.12162,-0.64408,0.01114,-0.13549]}, + {"t":0.8834, "x":3.18443, "y":1.70277, "heading":1.4456, "vx":-3.10919, "vy":-1.1012, "omega":-0.89024, "ax":0.02211, "ay":-0.06197, "alpha":0.20794, "fx":[-0.20882,1.01451,0.94587,-0.27755], "fy":[-1.32763,-1.55617,-0.58308,-0.66548]}, + {"t":0.90497, "x":3.11736, "y":1.679, "heading":1.42639, "vx":-3.10871, "vy":-1.10254, "omega":-0.88575, "ax":0.04459, "ay":-0.12634, "alpha":0.17775, "fx":[0.30865,1.14019,1.13724,0.38705], "fy":[-2.73812,-2.44851,-1.61715,-1.62033]}, + {"t":0.92655, "x":3.05031, "y":1.65518, "heading":1.40729, "vx":-3.10775, "vy":-1.10526, "omega":-0.88192, "ax":0.0858, "ay":-0.23954, "alpha":0.22643, "fx":[0.80152,2.00097,2.05942,0.85937], "fy":[-4.32264,-4.65685,-3.52701,-3.46548]}, + {"t":0.94812, "x":2.98329, "y":1.63128, "heading":1.38826, "vx":-3.1059, "vy":-1.11043, "omega":-0.87704, "ax":0.13175, "ay":-0.36841, "alpha":0.24925, "fx":[1.45488,2.64466,2.81875,1.86628], "fy":[-6.87966,-6.74317,-5.55692,-5.38492]}, + {"t":0.96969, "x":2.91632, "y":1.60724, "heading":1.36934, "vx":-3.10305, "vy":-1.11838, "omega":-0.87166, "ax":0.24361, "ay":-0.66956, "alpha":0.29836, "fx":[3.25171,4.65338,4.87142,3.46712], "fy":[-11.5623,-12.19052,-10.56646,-10.32569]}, + {"t":0.99126, "x":2.84944, "y":1.58296, "heading":1.35054, "vx":-3.0978, "vy":-1.13282, "omega":-0.86522, "ax":0.36898, "ay":-0.99956, "alpha":0.39544, "fx":[4.86023,6.738,7.16911,5.83578], "fy":[-17.81554,-17.6627,-15.7985,-15.37222]}, + {"t":1.01283, "x":2.7827, "y":1.55829, "heading":1.33187, "vx":-3.08984, "vy":-1.15438, "omega":-0.85669, "ax":0.61547, "ay":-1.61831, "alpha":0.46709, "fx":[9.03143,11.02464,11.4931,9.48943], "fy":[-27.43607,-28.85567,-26.05273,-25.56111]}, + {"t":1.03441, "x":2.71618, "y":1.53301, "heading":1.31339, "vx":-3.07656, "vy":-1.18929, "omega":-0.84662, "ax":0.93363, "ay":-2.35633, "alpha":0.52985, "fx":[13.86438,16.23343,16.88841,15.26632], "fy":[-41.21123,-40.43049,-38.07762,-37.39629]}, + {"t":1.05598, "x":2.65003, "y":1.50681, "heading":1.29513, "vx":-3.05642, "vy":-1.24012, "omega":-0.83519, "ax":1.41794, "ay":-3.37882, "alpha":0.31494, "fx":[23.09462,23.98081,24.20166,23.26875], "fy":[-55.93935,-58.51142,-55.61494,-55.22743]}, + {"t":1.07755, "x":2.58443, "y":1.47927, "heading":1.27711, "vx":-3.02583, "vy":-1.31301, "omega":-0.82839, "ax":2.08292, "ay":-4.58929, "alpha":-0.60672, "fx":[36.84123,33.54315,32.33912,36.16143], "fy":[-77.46586,-73.8168,-77.17015,-77.5522]}, + {"t":1.09912, "x":2.51964, "y":1.44988, "heading":1.25924, "vx":-2.9809, "vy":-1.41201, "omega":-0.84148, "ax":2.51798, "ay":-5.05578, "alpha":-2.60081, "fx":[51.10905,38.31788,32.85568,45.61157], "fy":[-77.80557,-78.85534,-89.12897,-91.31977]}, + {"t":1.12069, "x":2.45592, "y":1.41824, "heading":1.24109, "vx":-2.92658, "vy":-1.52108, "omega":-0.89759, "ax":3.79597, "ay":-4.37543, "alpha":-5.49969, "fx":[79.48406,56.75411,46.1458,70.72412], "fy":[-64.22317,-54.49079,-84.08769,-88.9435]}, + {"t":1.14227, "x":2.39367, "y":1.38441, "heading":1.22173, "vx":-2.8447, "vy":-1.61547, "omega":-1.01623, "ax":5.93406, "ay":0.03235, "alpha":-8.21913, "fx":[121.24439,90.23682,75.21444,108.97586], "fy":[14.33477,29.83012,-15.66256,-26.34498]}, + {"t":1.16384, "x":2.33369, "y":1.34957, "heading":1.19981, "vx":-2.71668, "vy":-1.61477, "omega":-1.19353, "ax":5.88224, "ay":0.98929, "alpha":-8.14745, "fx":[119.16423,88.7519,74.96229,109.33826], "fy":[29.84328,45.62286,1.97751,-11.47963]}, + {"t":1.18541, "x":2.27645, "y":1.31497, "heading":1.17406, "vx":-2.58979, "vy":-1.59343, "omega":-1.36929, "ax":5.828, "ay":1.32294, "alpha":-5.06395, "fx":[111.19894,91.2071,82.54289,103.65064], "fy":[28.20492,40.76034,14.50761,4.73792]}, + {"t":1.20698, "x":2.22194, "y":1.2809, "heading":1.14452, "vx":-2.46407, "vy":-1.56489, "omega":-1.47853, "ax":5.79223, "ay":1.49528, "alpha":1.82785, "fx":[92.46013,98.03603,100.06547,95.65324], "fy":[22.96275,16.30351,27.40336,33.03284]}, + {"t":1.22856, "x":2.17013, "y":1.24749, "heading":1.11262, "vx":-2.33912, "vy":-1.53263, "omega":-1.4391, "ax":5.76795, "ay":1.5997, "alpha":10.68921, "fx":[66.97734,109.00093,122.18128,86.43611], "fy":[10.41643,-12.53597,38.06212,70.7224]}, + {"t":1.25013, "x":2.12101, "y":1.2148, "heading":1.08158, "vx":-2.21469, "vy":-1.49812, "omega":-1.20851, "ax":5.65526, "ay":1.94951, "alpha":9.86147, "fx":[67.23151,106.50274,118.58527,84.76214], "fy":[21.17003,-5.11244,41.06967,72.86232]}, + {"t":1.27816, "x":2.06116, "y":1.17357, "heading":1.04771, "vx":-2.05617, "vy":-1.44348, "omega":-0.93209, "ax":5.48245, "ay":2.419, "alpha":7.31729, "fx":[72.11181,100.52017,109.59924,83.32772], "fy":[36.46654,9.96947,44.87798,69.98022]}, + {"t":1.30619, "x":2.00567, "y":1.13406, "heading":1.02158, "vx":-1.90249, "vy":-1.37567, "omega":-0.72698, "ax":5.32853, "ay":2.74384, "alpha":5.65506, "fx":[73.02792,96.52172,103.6153,82.13105], "fy":[42.90035,23.59726,48.01166,68.44448]}, + {"t":1.33422, "x":1.95444, "y":1.09658, "heading":1.0012, "vx":-1.75313, "vy":-1.29876, "omega":-0.56846, "ax":5.20083, "ay":2.98047, "alpha":4.57384, "fx":[73.28798,92.88767,98.92177,81.68379], "fy":[49.7377,31.02249,50.75779,67.21403]}, + {"t":1.36225, "x":1.90734, "y":1.06134, "heading":0.98527, "vx":-1.60735, "vy":-1.21521, "omega":-0.44025, "ax":5.09492, "ay":3.15934, "alpha":3.56747, "fx":[74.15936,89.80365,95.14853,80.60766], "fy":[52.58333,39.04385,52.72523,66.30647]}, + {"t":1.39028, "x":1.86429, "y":1.02852, "heading":0.97293, "vx":-1.46454, "vy":-1.12666, "omega":-0.34026, "ax":5.0064, "ay":3.29876, "alpha":2.96755, "fx":[74.26269,87.44191,91.91252,80.20002], "fy":[55.76215,43.23606,54.98739,65.96934]}, + {"t":1.41831, "x":1.8252, "y":0.99824, "heading":0.96339, "vx":-1.3242, "vy":-1.03419, "omega":-0.25707, "ax":4.9317, "ay":3.41018, "alpha":2.38614, "fx":[74.94997,85.41432,89.2859,79.18596], "fy":[56.86746,47.94471,56.6446,65.92737]}, + {"t":1.44634, "x":1.79002, "y":0.97059, "heading":0.95618, "vx":-1.18596, "vy":-0.9386, "omega":-0.19019, "ax":4.868, "ay":3.50112, "alpha":1.97509, "fx":[74.8436,83.758,86.97248,79.01457], "fy":[59.08865,50.70394,58.1208,65.53431]}, + {"t":1.47437, "x":1.75869, "y":0.94565, "heading":0.95085, "vx":-1.04951, "vy":-0.84046, "omega":-0.13482, "ax":4.81315, "ay":3.57664, "alpha":1.57492, "fx":[75.36547,82.31087,85.04491,78.21036], "fy":[59.79222,53.8237,59.33556,65.53212]}, + {"t":1.5024, "x":1.73116, "y":0.9235, "heading":0.94707, "vx":-0.9146, "vy":-0.74021, "omega":-0.09068, "ax":4.76549, "ay":3.64032, "alpha":1.26283, "fx":[75.30382,81.07842,83.24952,78.12228], "fy":[61.22888,55.86924,60.43348,65.19766]}, + {"t":1.53043, "x":1.7074, "y":0.90418, "heading":0.94453, "vx":-0.78102, "vy":-0.63817, "omega":-0.05528, "ax":4.72375, "ay":3.69469, "alpha":0.9555, "fx":[75.76107,79.97994,81.73067,77.49859], "fy":[61.74309,58.1176,61.34964,65.14425]}, + {"t":1.55846, "x":1.68736, "y":0.88775, "heading":0.94298, "vx":-0.64861, "vy":-0.5346, "omega":-0.0285, "ax":4.6869, "ay":3.74163, "alpha":0.69547, "fx":[75.77346,79.01996,80.25321,77.46653], "fy":[62.73514,59.74953,62.1895,64.81055]}, + {"t":1.5865, "x":1.67102, "y":0.87423, "heading":0.94218, "vx":-0.51723, "vy":-0.42972, "omega":-0.009, "ax":4.65415, "ay":3.78256, "alpha":0.43551, "fx":[76.22402,78.13569,78.96462,77.00514], "fy":[63.15054,61.48101,62.90796,64.67411]}, + {"t":1.61453, "x":1.65835, "y":0.86367, "heading":0.94193, "vx":-0.38677, "vy":-0.32369, "omega":0.0032, "ax":4.62486, "ay":3.81854, "alpha":0.19954, "fx":[76.33092,77.36103,77.69182,76.99316], "fy":[63.87442,62.88626,63.55101,64.30124]}, + {"t":1.64256, "x":1.64933, "y":0.8561, "heading":0.94202, "vx":-0.25713, "vy":-0.21666, "omega":0.0088, "ax":4.59853, "ay":3.85042, "alpha":-0.04371, "fx":[76.81195,76.59466,76.51411,76.70029], "fy":[64.21589,64.33689,64.13058,64.05531]}, + {"t":1.67059, "x":1.64393, "y":0.85154, "heading":0.94227, "vx":-0.12823, "vy":-0.10873, "omega":0.00757, "ax":4.57473, "ay":3.87885, "alpha":-0.27015, "fx":[77.03121,76.14226,75.34241,76.51827], "fy":[64.67441,65.64378,64.69529,63.62092]}, + {"t":1.69862, "x":1.64213, "y":0.85001, "heading":0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/ETofetchPSlow.traj b/src/main/deploy/choreo/ETofetchPSlow.traj index 5ebe854c..8bf52156 100644 --- a/src/main/deploy/choreo/ETofetchPSlow.traj +++ b/src/main/deploy/choreo/ETofetchPSlow.traj @@ -21,7 +21,7 @@ {"x":{"exp":"E.x", "val":4.894185251845}, "y":{"exp":"E.y", "val":2.7146580097364}, "heading":{"exp":"E.heading", "val":2.0943951023931953}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"4.480600357055664 m", "val":4.480600357055664}, "y":{"exp":"2.3546712398529053 m", "val":2.3546712398529053}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"2.1380813121795654 m", "val":2.1380813121795654}, "y":{"exp":"1.227155 m", "val":1.227155}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"fetchP.x", "val":1.69577419757843}, "y":{"exp":"fetchP.y", "val":0.8236}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"fetchP.x", "val":1.6421304941177368}, "y":{"exp":"fetchP.y", "val":0.8500130772590637}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, diff --git a/src/main/deploy/choreo/JTofetch.traj b/src/main/deploy/choreo/JTofetch.traj index 35fe5f5a..d3198dd2 100644 --- a/src/main/deploy/choreo/JTofetch.traj +++ b/src/main/deploy/choreo/JTofetch.traj @@ -5,8 +5,8 @@ "waypoints":[ {"x":5.1366723649053885, "y":5.197141990263579, "heading":4.1887902047863905, "intervals":87, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":4.392356872558594, "y":5.746496677398682, "heading":0.0, "intervals":64, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":3.3291568756103516, "y":6.2088751792907715, "heading":-1.4994887877871588, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.949252247810364, "y":6.8849358558654785, "heading":-1.081580366464561, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.3291568756103516, "y":6.2088751792907715, "heading":-1.4994887877871588, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.121014356613159, "y":6.83516788482666, "heading":-1.081580366464561, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.69577419757843, "y":7.228199481964111, "heading":-0.942477796076938, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -23,8 +23,8 @@ "waypoints":[ {"x":{"exp":"J.x", "val":5.1366723649053885}, "y":{"exp":"J.y", "val":5.197141990263579}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":87, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"4.392356872558594 m", "val":4.392356872558594}, "y":{"exp":"5.746496677398682 m", "val":5.746496677398682}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":64, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"3.3291568756103516 m", "val":3.3291568756103516}, "y":{"exp":"6.2088751792907715 m", "val":6.2088751792907715}, "heading":{"exp":"-1.4994887877871588 rad", "val":-1.4994887877871588}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.9492522478103638 m", "val":1.949252247810364}, "y":{"exp":"6.8849358558654785 m", "val":6.8849358558654785}, "heading":{"exp":"-1.081580366464561 rad", "val":-1.081580366464561}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.3291568756103516 m", "val":3.3291568756103516}, "y":{"exp":"6.2088751792907715 m", "val":6.2088751792907715}, "heading":{"exp":"-1.4994887877871588 rad", "val":-1.4994887877871588}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.121014356613159 m", "val":2.121014356613159}, "y":{"exp":"6.83516788482666 m", "val":6.83516788482666}, "heading":{"exp":"-1.081580366464561 rad", "val":-1.081580366464561}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"fetch.x", "val":1.69577419757843}, "y":{"exp":"fetch.y", "val":7.228199481964111}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -42,101 +42,102 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.55586,0.90819,1.4167,1.79691], + "waypoints":[0.0,0.55589,0.90812,1.33578,1.77584], "samples":[ - {"t":0.0, "x":5.13667, "y":5.19714, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.74303, "ay":3.66641, "alpha":21.06933, "fx":[-1.77954,-86.53735,-141.6162,-86.32336], "fy":[108.56338,120.33534,36.52755,-20.9572]}, - {"t":0.02417, "x":5.13529, "y":5.19821, "heading":-2.0944, "vx":-0.11463, "vy":0.08861, "omega":0.5092, "ax":-4.74907, "ay":3.66257, "alpha":9.01989, "fx":[-47.25507,-81.3647,-108.19351,-79.84552], "fy":[79.2136,88.2833,47.13152,29.58455]}, - {"t":0.04834, "x":5.13113, "y":5.20142, "heading":-2.08209, "vx":-0.2294, "vy":0.17713, "omega":0.72719, "ax":-4.75351, "ay":3.6566, "alpha":2.32306, "fx":[-71.28276,-79.60126,-86.95854,-79.11205], "fy":[65.08481,68.42544,57.04973,53.25474]}, - {"t":0.0725, "x":5.1242, "y":5.20677, "heading":-2.06451, "vx":-0.34429, "vy":0.2655, "omega":0.78333, "ax":-4.75841, "ay":3.64997, "alpha":0.49177, "fx":[-77.67607,-79.41353,-80.93912,-79.25301], "fy":[61.73947,62.43926,59.99295,59.20149]}, - {"t":0.09667, "x":5.11449, "y":5.21425, "heading":-2.04558, "vx":-0.45929, "vy":0.35371, "omega":0.79522, "ax":-4.7638, "ay":3.64267, "alpha":0.12267, "fx":[-78.33866,-79.53232,-79.86258,-79.90766], "fy":[62.06658,60.32724,59.81338,60.67915]}, - {"t":0.12084, "x":5.102, "y":5.22387, "heading":-2.02636, "vx":-0.57442, "vy":0.44174, "omega":0.79818, "ax":-4.76983, "ay":3.63448, "alpha":0.00151, "fx":[-79.51521,-79.50728,-79.49969,-79.52099], "fy":[60.5812,60.61164,60.57634,60.57114]}, - {"t":0.14501, "x":5.08672, "y":5.2356, "heading":-2.00707, "vx":-0.68969, "vy":0.52958, "omega":0.79822, "ax":-4.77654, "ay":3.62533, "alpha":-0.01104, "fx":[-79.53158,-79.55886,-79.49042,-79.90968], "fy":[60.61258,60.31322,60.43261,60.37177]}, - {"t":0.16917, "x":5.06866, "y":5.24946, "heading":-1.98778, "vx":-0.80513, "vy":0.6172, "omega":0.79795, "ax":-4.78412, "ay":3.61495, "alpha":-0.02074, "fx":[-79.87136,-79.6981,-79.65262,-79.77392], "fy":[60.13144,60.31374,60.27687,60.31593]}, - {"t":0.19334, "x":5.0478, "y":5.26543, "heading":-1.9685, "vx":-0.92075, "vy":0.70456, "omega":0.79745, "ax":-4.7927, "ay":3.60315, "alpha":-0.01422, "fx":[-79.74561,-79.79778,-79.67584,-80.34854], "fy":[60.42321,59.84792,60.07152,59.90849]}, - {"t":0.21751, "x":5.02415, "y":5.28351, "heading":-1.94923, "vx":-1.03658, "vy":0.79164, "omega":0.79711, "ax":-4.80252, "ay":3.58955, "alpha":-0.02263, "fx":[-80.24938,-79.93647,-79.9374,-80.09972], "fy":[59.59334,60.03016,59.84013,59.88094]}, - {"t":0.24168, "x":4.9977, "y":5.30369, "heading":-1.92996, "vx":-1.15265, "vy":0.8784, "omega":0.79656, "ax":-4.81386, "ay":3.57376, "alpha":-0.01251, "fx":[-80.05639,-80.11415,-79.9764,-80.83206], "fy":[60.05099,59.32785,59.59446,59.31846]}, - {"t":0.26585, "x":4.96843, "y":5.32597, "heading":-1.91071, "vx":-1.26899, "vy":0.96477, "omega":0.79626, "ax":-4.8271, "ay":3.5552, "alpha":-0.0236, "fx":[-80.69504,-80.31313,-80.32938,-80.52393], "fy":[58.95269,59.54704,59.2595,59.29446]}, - {"t":0.29001, "x":4.93635, "y":5.35032, "heading":-1.89147, "vx":-1.38565, "vy":1.05069, "omega":0.79569, "ax":-4.84276, "ay":3.53303, "alpha":-0.0182, "fx":[-80.53433,-80.58291,-80.4174,-81.37104], "fy":[59.47639,58.56776,58.91292,58.61867]}, - {"t":0.31418, "x":4.90145, "y":5.37675, "heading":-1.87224, "vx":-1.50269, "vy":1.13607, "omega":0.79525, "ax":-4.86154, "ay":3.50616, "alpha":-0.03176, "fx":[-81.33856,-80.83413,-80.8556,-81.1299], "fy":[58.02526,58.84034,58.44055,58.47784]}, - {"t":0.33835, "x":4.86372, "y":5.40523, "heading":-1.85302, "vx":-1.62018, "vy":1.22081, "omega":0.79448, "ax":-4.88457, "ay":3.47279, "alpha":-0.03461, "fx":[-81.25984,-81.22141,-81.01671,-82.19549], "fy":[58.60427,57.44421,57.93054,57.58005]}, - {"t":0.36252, "x":4.82313, "y":5.43575, "heading":-1.83382, "vx":-1.73823, "vy":1.30474, "omega":0.79364, "ax":-4.91326, "ay":3.43051, "alpha":-0.05781, "fx":[-82.31032,-81.6339,-81.61282,-82.04973], "fy":[56.6204,57.63203,57.21711,57.27055]}, - {"t":0.38668, "x":4.77969, "y":5.46828, "heading":-1.81463, "vx":-1.85697, "vy":1.38765, "omega":0.79225, "ax":-4.95025, "ay":3.37485, "alpha":-0.09628, "fx":[-82.63254,-82.26031,-82.00425,-83.17598], "fy":[56.67504,55.70486,56.42453,56.22426]}, - {"t":0.41085, "x":4.73336, "y":5.5028, "heading":-1.79549, "vx":-1.97661, "vy":1.46921, "omega":0.78992, "ax":-4.99934, "ay":3.29883, "alpha":-0.13504, "fx":[-84.03504,-82.90489,-82.76896,-83.63745], "fy":[54.05841,55.48117,55.1595,55.26025]}, - {"t":0.43502, "x":4.68413, "y":5.53927, "heading":-1.7764, "vx":-2.09743, "vy":1.54893, "omega":0.78666, "ax":-5.06803, "ay":3.18803, "alpha":-0.25246, "fx":[-85.19235,-84.08431,-83.58373,-85.06624], "fy":[53.42564,51.87867,53.59195,53.6754]}, - {"t":0.45919, "x":4.63196, "y":5.57764, "heading":-1.75739, "vx":-2.21992, "vy":1.62598, "omega":0.78056, "ax":-5.1693, "ay":3.01409, "alpha":-0.4245, "fx":[-87.74442,-85.25658,-84.70924,-86.96854], "fy":[48.27367,50.33361,51.05392,51.31263]}, - {"t":0.48336, "x":4.5768, "y":5.61781, "heading":-1.73852, "vx":-2.34485, "vy":1.69883, "omega":0.7703, "ax":-5.33373, "ay":2.69893, "alpha":-1.04018, "fx":[-91.45922,-87.12556,-85.87583,-91.18226], "fy":[43.69566,41.20315,47.32009,47.7403]}, - {"t":0.50752, "x":4.51858, "y":5.65966, "heading":-1.7199, "vx":-2.47375, "vy":1.76405, "omega":0.74516, "ax":-5.62343, "ay":1.98808, "alpha":-2.71846, "fx":[-100.58838,-89.00479,-86.864,-98.50225], "fy":[25.53923,26.00223,39.54611,41.47401]}, - {"t":0.53169, "x":4.45715, "y":5.70287, "heading":-1.7019, "vx":-2.60966, "vy":1.8121, "omega":0.67946, "ax":-5.89582, "ay":-0.58696, "alpha":-8.69347, "fx":[-116.35874,-80.29784,-78.05717,-118.40788], "fy":[-25.85395,-39.48629,9.0996,17.10337]}, - {"t":0.55586, "x":4.39236, "y":5.7465, "heading":-1.68548, "vx":-2.75215, "vy":1.79792, "omega":0.46936, "ax":-3.56235, "ay":-4.29265, "alpha":2.41623, "fx":[-53.21825,-65.17551,-65.43375,-53.70302], "fy":[-65.94085,-65.53348,-76.12981,-78.62145]}, - {"t":0.56995, "x":4.35322, "y":5.77141, "heading":-1.67886, "vx":-2.80235, "vy":1.73742, "omega":0.50341, "ax":-2.97241, "ay":-4.8854, "alpha":0.9819, "fx":[-46.35596,-52.03514,-52.07841,-47.72532], "fy":[-80.84946,-77.71957,-83.10181,-84.07796]}, - {"t":0.58405, "x":4.31343, "y":5.79541, "heading":-1.67177, "vx":-2.84424, "vy":1.66857, "omega":0.51725, "ax":-2.78731, "ay":-4.88281, "alpha":0.28718, "fx":[-45.3683,-47.49067,-47.60755,-45.38596], "fy":[-79.73588,-82.30994,-81.55825,-81.97236]}, - {"t":0.59814, "x":4.27307, "y":5.81844, "heading":-1.66448, "vx":-2.88352, "vy":1.59975, "omega":0.52129, "ax":-2.58495, "ay":-4.77871, "alpha":0.06959, "fx":[-42.24492,-43.44575,-43.50907,-43.1599], "fy":[-81.19277,-78.40107,-79.46632,-79.57487]}, - {"t":0.61223, "x":4.23217, "y":5.84051, "heading":-1.65713, "vx":-2.91995, "vy":1.53241, "omega":0.52227, "ax":-2.19063, "ay":-4.29727, "alpha":-0.02525, "fx":[-36.24851,-36.71479,-36.81632,-36.28705], "fy":[-70.83463,-73.0884,-71.2721,-71.33879]}, - {"t":0.62633, "x":4.1908, "y":5.86168, "heading":-1.64977, "vx":-2.95083, "vy":1.47184, "omega":0.52192, "ax":-1.81505, "ay":-3.7068, "alpha":-0.03588, "fx":[-29.64321,-30.18526,-30.23021,-30.96523], "fy":[-62.86755,-61.093,-61.58765,-61.61406]}, - {"t":0.64042, "x":4.14904, "y":5.88206, "heading":-1.64241, "vx":-2.97641, "vy":1.4196, "omega":0.52141, "ax":-1.44565, "ay":-3.095, "alpha":0.00085, "fx":[-23.8985,-24.25185,-24.3134,-23.92953], "fy":[-51.08547,-52.41627,-51.41417,-51.45297]}, - {"t":0.65451, "x":4.10695, "y":5.90175, "heading":-1.63507, "vx":-2.99678, "vy":1.37599, "omega":0.52143, "ax":-1.12546, "ay":-2.48701, "alpha":-0.00222, "fx":[-18.18602,-18.69279,-18.72942,-19.43548], "fy":[-41.94484,-40.96545,-41.44582,-41.4728]}, - {"t":0.6686, "x":4.0646, "y":5.9209, "heading":-1.62772, "vx":-3.01264, "vy":1.34094, "omega":0.52139, "ax":-0.91717, "ay":-2.08346, "alpha":0.03428, "fx":[-15.10604,-15.44246,-15.47902,-15.12756], "fy":[-34.39054,-35.06289,-34.71805,-34.74958]}, - {"t":0.6827, "x":4.02205, "y":5.93959, "heading":-1.62037, "vx":-3.02557, "vy":1.31157, "omega":0.52188, "ax":-0.6761, "ay":-1.58243, "alpha":0.02159, "fx":[-10.74463,-11.21519,-11.24267,-11.87829], "fy":[-26.46342,-26.04,-26.49389,-26.51598]}, - {"t":0.69679, "x":3.97934, "y":5.95792, "heading":-1.61301, "vx":-3.03509, "vy":1.28927, "omega":0.52218, "ax":-0.57975, "ay":-1.36619, "alpha":0.04942, "fx":[-9.51087,-9.80001,-9.82126,-9.5246], "fy":[-22.55541,-22.79912,-22.85898,-22.88135]}, - {"t":0.71088, "x":3.93651, "y":5.97595, "heading":-1.60565, "vx":-3.04327, "vy":1.27002, "omega":0.52288, "ax":-0.40721, "ay":-0.99414, "alpha":0.03314, "fx":[-6.31151,-6.73789,-6.7569,-7.34539], "fy":[-16.41939,-16.34026,-16.75632,-16.77147]}, - {"t":0.72498, "x":3.89358, "y":5.99375, "heading":-1.59829, "vx":-3.049, "vy":1.25601, "omega":0.52334, "ax":-0.38144, "ay":-0.91792, "alpha":0.05798, "fx":[-6.22047,-6.48557,-6.49847,-6.2293], "fy":[-15.1476,-15.1575,-15.44261,-15.45744]}, - {"t":0.73907, "x":3.85057, "y":6.01136, "heading":-1.59091, "vx":-3.05438, "vy":1.24307, "omega":0.52416, "ax":-0.26765, "ay":-0.67451, "alpha":0.04288, "fx":[-4.02268,-4.43696,-4.44912,-4.93775], "fy":[-10.98695,-11.05447,-11.46224,-11.47162]}, - {"t":0.75316, "x":3.8075, "y":6.02881, "heading":-1.58352, "vx":-3.05815, "vy":1.23356, "omega":0.52477, "ax":-0.28739, "ay":-0.70164, "alpha":0.06646, "fx":[-4.64263,-4.93226,-4.9401,-4.64781], "fy":[-11.54001,-11.49601,-11.86958,-11.87864]}, - {"t":0.76726, "x":3.76437, "y":6.04613, "heading":-1.57613, "vx":-3.0622, "vy":1.22368, "omega":0.5257, "ax":-0.22668, "ay":-0.58291, "alpha":0.05712, "fx":[-3.35692,-3.81528,-3.82097,-4.12159], "fy":[-9.45908,-9.49905,-9.95263,-9.95673]}, - {"t":0.78135, "x":3.7212, "y":6.06332, "heading":-1.56872, "vx":-3.0654, "vy":1.21546, "omega":0.52651, "ax":-0.27747, "ay":-0.69028, "alpha":0.08221, "fx":[-4.43092,-4.81832,-4.82067,-4.43111], "fy":[-11.26823,-11.33283,-11.71121,-11.7144]}, - {"t":0.79544, "x":3.67797, "y":6.08038, "heading":-1.5613, "vx":-3.06931, "vy":1.20573, "omega":0.52767, "ax":-0.26858, "ay":-0.69774, "alpha":0.09126, "fx":[-4.0135,-4.64677,-4.6431,-4.60498], "fy":[-11.42211,-11.28307,-11.91101,-11.90759]}, - {"t":0.80954, "x":3.63468, "y":6.0973, "heading":-1.55386, "vx":-3.07309, "vy":1.1959, "omega":0.52895, "ax":-0.342, "ay":-0.87491, "alpha":0.13908, "fx":[-5.34921,-6.0645,-6.05386,-5.33665], "fy":[-14.10343,-14.42967,-14.90632,-14.89808]}, - {"t":0.82363, "x":3.59134, "y":6.11407, "heading":-1.54641, "vx":-3.07791, "vy":1.18357, "omega":0.53091, "ax":-0.38708, "ay":-1.01937, "alpha":0.21549, "fx":[-5.71447,-7.00188,-6.97091,-6.12257], "fy":[-16.6938,-16.24937,-17.52614,-17.50014]}, - {"t":0.83772, "x":3.54792, "y":6.13065, "heading":-1.53892, "vx":-3.08337, "vy":1.1692, "omega":0.53395, "ax":-0.47741, "ay":-1.26753, "alpha":0.38475, "fx":[-6.9877,-8.99619,-8.92896,-6.92015], "fy":[-19.88569,-20.64648,-22.01982,-21.96433]}, - {"t":0.85182, "x":3.50442, "y":6.147, "heading":-1.5314, "vx":-3.0901, "vy":1.15134, "omega":0.53937, "ax":-0.57973, "ay":-1.57053, "alpha":0.70919, "fx":[-7.7471,-11.55126,-11.39447,-7.96255], "fy":[-24.91871,-24.13175,-27.90207,-27.76752]}, - {"t":0.86591, "x":3.46082, "y":6.16307, "heading":-1.5238, "vx":-3.09827, "vy":1.12921, "omega":0.54937, "ax":-0.67942, "ay":-1.89816, "alpha":1.39425, "fx":[-7.90785,-15.09415,-14.73202,-7.56835], "fy":[-27.71186,-29.20825,-34.97185,-34.67395]}, - {"t":0.88, "x":3.41708, "y":6.17879, "heading":-1.51606, "vx":-3.10784, "vy":1.10245, "omega":0.56902, "ax":-0.83909, "ay":-2.39354, "alpha":2.66214, "fx":[-7.4844,-21.03977,-20.23717,-7.18745], "fy":[-33.97553,-33.18158,-46.56397,-45.87534]}, - {"t":0.8941, "x":3.3732, "y":6.19409, "heading":-1.50804, "vx":-3.11967, "vy":1.06872, "omega":0.60653, "ax":-0.79646, "ay":-2.82229, "alpha":5.489, "fx":[-0.00697,-28.32004,-26.34325,1.56408], "fy":[-32.60336,-36.30346,-60.44252,-58.83571]}, - {"t":0.90819, "x":3.32916, "y":6.20888, "heading":-1.49949, "vx":-3.13089, "vy":1.02895, "omega":0.68389, "ax":-1.05961, "ay":-2.8627, "alpha":6.07446, "fx":[-3.63706,-34.03585,-31.53179,-1.44802], "fy":[-31.23004,-35.51046,-62.93033,-61.20839]}, - {"t":0.92514, "x":3.27593, "y":6.22591, "heading":-1.4879, "vx":-3.14885, "vy":0.98042, "omega":0.78686, "ax":-0.76184, "ay":-2.49413, "alpha":1.23739, "fx":[-9.88129,-16.29966,-15.73889,-8.87807], "fy":[-39.56593,-38.15801,-44.54369,-44.03591]}, - {"t":0.94209, "x":3.22245, "y":6.24217, "heading":-1.47456, "vx":-3.16177, "vy":0.93815, "omega":0.80783, "ax":-0.49013, "ay":-1.68531, "alpha":0.20081, "fx":[-7.613,-8.84876,-8.72787,-7.49122], "fy":[-26.99988,-28.55046,-28.47659,-28.34607]}, - {"t":0.95904, "x":3.16879, "y":6.25783, "heading":-1.46087, "vx":-3.17007, "vy":0.90958, "omega":0.81123, "ax":-0.30263, "ay":-1.06107, "alpha":0.01973, "fx":[-5.04638,-5.28735,-5.26012,-4.58499], "fy":[-18.27687,-17.33971,-17.58041,-17.55329]}, - {"t":0.97599, "x":3.11501, "y":6.27309, "heading":-1.44711, "vx":-3.1752, "vy":0.89159, "omega":0.81157, "ax":-0.1028, "ay":-0.36999, "alpha":-0.00797, "fx":[-1.66767,-1.77283,-1.75971,-1.65438], "fy":[-5.98118,-6.57657,-6.06066,-6.05166]}, - {"t":0.99294, "x":3.06117, "y":6.28815, "heading":-1.43336, "vx":-3.17695, "vy":0.88532, "omega":0.81143, "ax":0.05476, "ay":0.19769, "alpha":-0.04081, "fx":[0.56638,0.91702,0.86876,1.29912], "fy":[2.92245,3.20269,3.55247,3.50421]}, - {"t":1.00989, "x":3.00733, "y":6.30319, "heading":-1.4196, "vx":-3.17602, "vy":0.88867, "omega":0.81074, "ax":0.25621, "ay":0.90641, "alpha":-0.0504, "fx":[4.15949,4.42274,4.38238,4.1191], "fy":[14.87723,15.13792,15.23477,15.18789]}, - {"t":1.02684, "x":2.95353, "y":6.31838, "heading":-1.40586, "vx":-3.17167, "vy":0.90404, "omega":0.80989, "ax":0.45362, "ay":1.56434, "alpha":-0.07387, "fx":[6.96585,7.78549,7.64821,7.84705], "fy":[26.0581,25.58665,26.39937,26.26322]}, - {"t":1.04379, "x":2.89983, "y":6.33393, "heading":-1.39213, "vx":-3.16399, "vy":0.93055, "omega":0.80864, "ax":0.72555, "ay":2.41227, "alpha":-0.08442, "fx":[11.84027,12.4614,12.35149,11.72486], "fy":[39.52051,40.84079,40.30083,40.18369]}, - {"t":1.06074, "x":2.84631, "y":6.35005, "heading":-1.37843, "vx":-3.15169, "vy":0.97144, "omega":0.8072, "ax":1.05075, "ay":3.29563, "alpha":-0.09133, "fx":[16.71195,17.98928,17.73529,17.62519], "fy":[55.46934,54.00865,55.25671,55.01178]}, - {"t":1.07769, "x":2.79304, "y":6.36699, "heading":-1.36474, "vx":-3.13388, "vy":1.02731, "omega":0.80566, "ax":1.45184, "ay":4.27092, "alpha":-0.11199, "fx":[23.81703,24.78794,24.60067,23.59999], "fy":[69.99137,72.60443,71.19019,70.99051]}, - {"t":1.09464, "x":2.74012, "y":6.38501, "heading":-1.35109, "vx":-3.10927, "vy":1.0997, "omega":0.80376, "ax":1.8689, "ay":5.0532, "alpha":-0.08194, "fx":[30.18836,31.81614,31.43939,31.17068], "fy":[85.35298,82.9492,84.49214,84.14334]}, - {"t":1.11159, "x":2.68769, "y":6.40438, "heading":-1.33746, "vx":-3.07759, "vy":1.18535, "omega":0.80237, "ax":2.10056, "ay":5.2247, "alpha":-0.09789, "fx":[34.64185,35.59098,35.43608,34.39236], "fy":[85.73198,88.92607,86.97422,86.74051]}, - {"t":1.12855, "x":2.63582, "y":6.42522, "heading":-1.32386, "vx":-3.04198, "vy":1.27392, "omega":0.80071, "ax":2.29733, "ay":5.26287, "alpha":-0.0249, "fx":[37.61924,38.85021,38.55931,38.15313], "fy":[89.08147,86.6507,87.75772,87.4285]}, - {"t":1.1455, "x":2.58459, "y":6.44757, "heading":-1.31029, "vx":-3.00304, "vy":1.36312, "omega":0.80029, "ax":2.48265, "ay":5.25491, "alpha":0.171, "fx":[41.40404,41.12217,41.4626,41.54973], "fy":[86.67064,90.52084,86.63547,86.56058]}, - {"t":1.16245, "x":2.53404, "y":6.47143, "heading":-1.29673, "vx":-2.96096, "vy":1.4522, "omega":0.80319, "ax":2.68382, "ay":5.20641, "alpha":0.41383, "fx":[44.43064,43.99594,44.2967,46.22878], "fy":[89.48089,86.38368,85.69549,85.59309]}, - {"t":1.1794, "x":2.48424, "y":6.4968, "heading":-1.28311, "vx":-2.91547, "vy":1.54045, "omega":0.8102, "ax":3.74261, "ay":4.54875, "alpha":0.60164, "fx":[63.05762,60.72323,61.80396,63.96518], "fy":[76.61839,78.8615,73.66507,74.15696]}, - {"t":1.19635, "x":2.43536, "y":6.52356, "heading":-1.26938, "vx":-2.85203, "vy":1.61755, "omega":0.8204, "ax":5.44191, "ay":2.35406, "alpha":0.77137, "fx":[90.77698,88.42732,89.344,94.30796], "fy":[41.53925,40.18252,37.39641,37.8457]}, - {"t":1.2133, "x":2.3878, "y":6.55132, "heading":-1.25547, "vx":-2.75978, "vy":1.65745, "omega":0.83347, "ax":5.86448, "ay":1.02369, "alpha":0.59289, "fx":[98.51656,96.05542,97.01505,99.4451], "fy":[18.78327,18.48571,15.08743,15.90151]}, - {"t":1.23025, "x":2.34186, "y":6.57956, "heading":-1.24134, "vx":-2.66038, "vy":1.67481, "omega":0.84352, "ax":5.95671, "ay":0.32977, "alpha":0.50074, "fx":[99.38325,97.64393,98.28757,101.86749], "fy":[6.50205,6.34431,4.33275,4.80918]}, - {"t":1.2472, "x":2.29762, "y":6.608, "heading":-1.22705, "vx":-2.55941, "vy":1.6804, "omega":0.85201, "ax":5.97282, "ay":-0.07752, "alpha":0.31775, "fx":[99.99501,98.59162,99.12921,100.54009], "fy":[0.05034,-1.29214,-2.25316,-1.67413]}, - {"t":1.26415, "x":2.2551, "y":6.63647, "heading":-1.2126, "vx":-2.45817, "vy":1.67908, "omega":0.8574, "ax":5.96841, "ay":-0.34168, "alpha":0.2308, "fx":[99.53395,98.55728,98.93021,100.94054], "fy":[-5.71799,-5.03847,-6.19068,-5.83512]}, - {"t":1.2811, "x":2.21429, "y":6.66488, "heading":-1.19807, "vx":-2.357, "vy":1.67329, "omega":0.86131, "ax":5.95841, "ay":-0.52586, "alpha":0.15783, "fx":[99.5428,98.81077,99.10182,99.84015], "fy":[-7.84151,-9.23401,-9.17849,-8.80933]}, - {"t":1.29805, "x":2.17519, "y":6.69317, "heading":-1.18347, "vx":-2.256, "vy":1.66438, "omega":0.86399, "ax":5.94776, "ay":-0.65818, "alpha":0.13195, "fx":[99.15075,98.58254,98.81296,100.03886], "fy":[-11.08672,-10.5394,-11.27139,-10.98835]}, - {"t":1.315, "x":2.1378, "y":6.72129, "heading":-1.16883, "vx":-2.15518, "vy":1.65322, "omega":0.86622, "ax":5.93724, "ay":-0.76229, "alpha":0.08228, "fx":[99.10541,98.64081,98.83909,99.29813], "fy":[-11.8622,-13.66816,-12.78661,-12.51077]}, - {"t":1.33195, "x":2.10212, "y":6.7492, "heading":-1.15414, "vx":-2.05454, "vy":1.6403, "omega":0.86762, "ax":5.92756, "ay":-0.84492, "alpha":0.04839, "fx":[98.83066,98.39897,98.58859,99.41993], "fy":[-14.69309,-13.57676,-14.16851,-13.89947]}, - {"t":1.3489, "x":2.06815, "y":6.77688, "heading":-1.13944, "vx":-1.95407, "vy":1.62598, "omega":0.86844, "ax":5.91925, "ay":-0.90929, "alpha":-0.01244, "fx":[98.70154,98.58716,98.64718,98.7486], "fy":[-14.85435,-15.99255,-14.90038,-14.8825]}, - {"t":1.36585, "x":2.03588, "y":6.80431, "heading":-1.12472, "vx":-1.85374, "vy":1.61057, "omega":0.86823, "ax":5.91177, "ay":-0.96299, "alpha":-0.47989, "fx":[97.98758,99.40872,98.81293,97.97622], "fy":[-18.83222,-16.30511,-13.96953,-15.10344]}, - {"t":1.3828, "x":2.00531, "y":6.83148, "heading":-1.11, "vx":-1.75353, "vy":1.59424, "omega":0.86009, "ax":5.90483, "ay":-1.00967, "alpha":-2.57468, "fx":[96.11294,104.63794,100.92472,92.047], "fy":[-26.93624,-20.91792,-6.34689,-13.1218]}, - {"t":1.39975, "x":1.97643, "y":6.85835, "heading":-1.09542, "vx":-1.65344, "vy":1.57713, "omega":0.81645, "ax":5.89888, "ay":-1.04795, "alpha":-10.12355, "fx":[89.70279,122.99389,108.97245,71.65666], "fy":[-59.8,-27.81605,21.39074,-3.64986]}, - {"t":1.4167, "x":1.94925, "y":6.88494, "heading":-1.08158, "vx":-1.55345, "vy":1.55936, "omega":0.64485, "ax":5.80917, "ay":-1.45926, "alpha":-2.24118, "fx":[94.60118,102.60604,99.23946,90.89789], "fy":[-32.75799,-27.43698,-15.32897,-21.77666]}, - {"t":1.44386, "x":1.90921, "y":6.92675, "heading":-1.06407, "vx":-1.39568, "vy":1.51973, "omega":0.58399, "ax":5.58205, "ay":-2.18664, "alpha":-1.83041, "fx":[90.73269,97.66484,95.09853,88.70414], "fy":[-44.40302,-37.50533,-29.18633,-34.70599]}, - {"t":1.47102, "x":1.87336, "y":6.96721, "heading":-1.04821, "vx":-1.24409, "vy":1.46035, "omega":0.53428, "ax":5.29472, "ay":-2.81244, "alpha":-1.5126, "fx":[86.36118,92.23618,90.33497,84.1097], "fy":[-52.47574,-48.39418,-40.76609,-45.89192]}, - {"t":1.49818, "x":1.84153, "y":7.00584, "heading":-1.0337, "vx":-1.1003, "vy":1.38397, "omega":0.4932, "ax":4.98184, "ay":-3.33597, "alpha":-1.35584, "fx":[80.85752,86.74981,84.83365,79.73838], "fy":[-61.3697,-55.90597,-50.41423,-54.74648]}, - {"t":1.52533, "x":1.81348, "y":7.04219, "heading":-1.0203, "vx":-0.965, "vy":1.29337, "omega":0.45637, "ax":4.66646, "ay":-3.76499, "alpha":-1.23006, "fx":[76.07379,81.28016,79.67601,74.12045], "fy":[-67.13058,-63.52598,-58.00199,-62.38365]}, - {"t":1.55249, "x":1.789, "y":7.07593, "heading":-1.00791, "vx":-0.83827, "vy":1.19112, "omega":0.42297, "ax":4.36215, "ay":-4.11412, "alpha":-1.23708, "fx":[70.58414,76.45536,74.50012,69.3199], "fy":[-73.46369,-68.54282,-64.08255,-68.23287]}, - {"t":1.57965, "x":1.76784, "y":7.10676, "heading":-0.99642, "vx":-0.7198, "vy":1.07939, "omega":0.38937, "ax":4.07752, "ay":-4.39674, "alpha":-1.23949, "fx":[66.14295,71.81811,69.92463,63.99562], "fy":[-77.54911,-73.59178,-68.7971,-73.22806]}, - {"t":1.60681, "x":1.74979, "y":7.13445, "heading":-0.98585, "vx":-0.60907, "vy":0.95999, "omega":0.35571, "ax":3.8155, "ay":-4.62627, "alpha":-1.34594, "fx":[61.42434,68.02843,65.6189,59.33862], "fy":[-81.89254,-76.94431,-72.46956,-77.16407]}, - {"t":1.63397, "x":1.73466, "y":7.15882, "heading":-0.97619, "vx":-0.50545, "vy":0.83435, "omega":0.31916, "ax":3.57707, "ay":-4.81327, "alpha":-1.33645, "fx":[58.14235,64.27154,61.661,54.43718], "fy":[-83.96646,-80.51629,-75.67875,-80.7782]}, - {"t":1.66112, "x":1.72225, "y":7.1797, "heading":-0.96752, "vx":-0.4083, "vy":0.70363, "omega":0.28286, "ax":3.36114, "ay":-4.9667, "alpha":-1.55461, "fx":[53.95973,60.97885,58.6792,50.49649], "fy":[-87.88717,-82.83417,-77.42792,-83.02084]}, - {"t":1.68828, "x":1.7124, "y":7.19698, "heading":-0.95984, "vx":-0.31702, "vy":0.56875, "omega":0.24064, "ax":3.16605, "ay":-5.09349, "alpha":-1.80498, "fx":[50.23973,59.23132,55.42892,46.20629], "fy":[-90.57166,-84.62139,-79.14583,-85.28516]}, - {"t":1.71544, "x":1.70496, "y":7.21055, "heading":-0.9533, "vx":-0.23104, "vy":0.43042, "omega":0.19162, "ax":2.98998, "ay":-5.19899, "alpha":-2.0865, "fx":[46.63189,57.0931,53.27767,42.36319], "fy":[-93.37291,-86.29528,-79.94951,-87.04088]}, - {"t":1.7426, "x":1.69979, "y":7.22032, "heading":-0.9481, "vx":-0.14984, "vy":0.28923, "omega":0.13496, "ax":2.83069, "ay":-5.28757, "alpha":-2.31689, "fx":[44.22409,55.39793,50.67085,38.45243], "fy":[-95.09892,-88.20822,-80.70111,-88.55645]}, - {"t":1.76975, "x":1.69676, "y":7.22622, "heading":-0.94443, "vx":-0.07296, "vy":0.14563, "omega":0.07204, "ax":2.68662, "ay":-5.36235, "alpha":-2.6526, "fx":[41.27722,54.52163,48.80901,34.53099], "fy":[-97.23151,-88.98442,-81.24345,-90.0919]}, - {"t":1.79691, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":5.13667, "y":5.19714, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.73739, "ay":3.6737, "alpha":20.8906, "fx":[-2.17441,-86.37282,-141.20679,-86.12589], "fy":[108.15293,119.89176,36.7192,-19.80841]}, + {"t":0.02417, "x":5.13529, "y":5.19821, "heading":-2.0944, "vx":-0.1145, "vy":0.08879, "omega":0.50491, "ax":-4.74376, "ay":3.66945, "alpha":8.99922, "fx":[-47.12793,-81.29265,-108.12011,-79.76385], "fy":[79.34756,88.21605,47.23017,29.87791]}, + {"t":0.04834, "x":5.13114, "y":5.20143, "heading":-2.08219, "vx":-0.22915, "vy":0.17748, "omega":0.72241, "ax":-4.74856, "ay":3.66301, "alpha":2.33657, "fx":[-70.95946,-79.69079,-87.43554,-78.53917], "fy":[65.01069,68.4888,56.49769,54.24557]}, + {"t":0.07251, "x":5.12421, "y":5.20679, "heading":-2.06473, "vx":-0.34392, "vy":0.26601, "omega":0.77888, "ax":-4.75385, "ay":3.65592, "alpha":0.54256, "fx":[-77.51524,-79.1857,-81.15132,-79.12518], "fy":[62.10424,62.80558,59.38841,59.47124]}, + {"t":0.09668, "x":5.11451, "y":5.21429, "heading":-2.04591, "vx":-0.45881, "vy":0.35437, "omega":0.79199, "ax":-4.75968, "ay":3.64805, "alpha":0.11048, "fx":[-78.53502,-79.47035,-79.49837,-79.86288], "fy":[61.6988,60.56658,60.92627,60.05354]}, + {"t":0.12085, "x":5.10203, "y":5.22392, "heading":-2.02677, "vx":-0.57385, "vy":0.44254, "omega":0.79466, "ax":-4.76619, "ay":3.63926, "alpha":0.02057, "fx":[-79.39885,-79.43805,-79.49431,-79.46905], "fy":[60.7067,60.76063,60.61388,60.57773]}, + {"t":0.14501, "x":5.08677, "y":5.23568, "heading":-2.00756, "vx":-0.68904, "vy":0.5305, "omega":0.79516, "ax":-4.77345, "ay":3.62941, "alpha":-0.00538, "fx":[-79.46201,-79.51105,-79.45816,-79.85309], "fy":[60.685,60.40201,60.49212,60.42275]}, + {"t":0.16918, "x":5.06872, "y":5.24956, "heading":-1.98834, "vx":-0.80441, "vy":0.61821, "omega":0.79503, "ax":-4.78162, "ay":3.61826, "alpha":-0.01802, "fx":[-79.81269,-79.66926,-79.62105,-79.72636], "fy":[60.20203,60.36101,60.33236,60.36325]}, + {"t":0.19335, "x":5.04788, "y":5.26556, "heading":-1.96913, "vx":-0.91998, "vy":0.70566, "omega":0.79459, "ax":-4.79088, "ay":3.60557, "alpha":-0.01249, "fx":[-79.71072,-79.76109,-79.64611,-80.32902], "fy":[60.45778,59.905,60.11482,59.93456]}, + {"t":0.21752, "x":5.02425, "y":5.28367, "heading":-1.94992, "vx":-1.03577, "vy":0.79281, "omega":0.79429, "ax":-4.80148, "ay":3.59096, "alpha":-0.02206, "fx":[-80.23015,-79.92224,-79.91844,-80.0823], "fy":[59.61875,60.05602,59.86196,59.9017]}, + {"t":0.24169, "x":4.99781, "y":5.30388, "heading":-1.93072, "vx":-1.15182, "vy":0.8796, "omega":0.79376, "ax":-4.8137, "ay":3.574, "alpha":-0.01218, "fx":[-80.06553,-80.11737,-79.97754,-80.80755], "fy":[60.07351,59.32016,59.59149,59.32204]}, + {"t":0.26586, "x":4.96857, "y":5.32618, "heading":-1.91154, "vx":-1.26816, "vy":0.96598, "omega":0.79346, "ax":-4.82795, "ay":3.55404, "alpha":-0.0243, "fx":[-80.7151,-80.32658,-80.33574,-80.54126], "fy":[58.92737,59.53503,59.23854,59.27586]}, + {"t":0.29003, "x":4.93651, "y":5.35056, "heading":-1.89236, "vx":-1.38485, "vy":1.05187, "omega":0.79288, "ax":-4.84481, "ay":3.53024, "alpha":-0.01747, "fx":[-80.58896,-80.62207,-80.45177,-81.37953], "fy":[59.47858,58.49548,58.85496,58.56039]}, + {"t":0.3142, "x":4.90162, "y":5.37702, "heading":-1.8732, "vx":-1.50194, "vy":1.1372, "omega":0.79246, "ax":-4.86501, "ay":3.50136, "alpha":-0.03338, "fx":[-81.4094,-80.88709,-80.8991,-81.19417], "fy":[57.92796,58.77803,58.35717,58.40079]}, + {"t":0.33837, "x":4.8639, "y":5.40553, "heading":-1.85405, "vx":-1.61952, "vy":1.22182, "omega":0.79165, "ax":-4.88971, "ay":3.46557, "alpha":-0.03633, "fx":[-81.36417,-81.30532,-81.08939,-82.27752], "fy":[58.54062,57.28328,57.80369,57.45026]}, + {"t":0.36254, "x":4.82333, "y":5.43607, "heading":-1.83491, "vx":-1.7377, "vy":1.30558, "omega":0.79077, "ax":-4.92051, "ay":3.42015, "alpha":-0.06087, "fx":[-82.46135,-81.73322,-81.71011,-82.18535], "fy":[56.40125,57.50967,57.0379,57.1002]}, + {"t":0.3867, "x":4.7799, "y":5.46862, "heading":-1.8158, "vx":-1.85663, "vy":1.38824, "omega":0.7893, "ax":-4.96003, "ay":3.36051, "alpha":-0.09742, "fx":[-82.86487,-82.44332,-82.17266,-83.24473], "fy":[56.52134,55.39037,56.16381,55.99693]}, + {"t":0.41087, "x":4.73357, "y":5.50316, "heading":-1.79672, "vx":-1.97651, "vy":1.46946, "omega":0.78695, "ax":-5.01245, "ay":3.27896, "alpha":-0.13929, "fx":[-84.28745,-83.10317,-82.95364,-83.876], "fy":[53.66803,55.21117,54.81927,54.93658]}, + {"t":0.43504, "x":4.68434, "y":5.53963, "heading":-1.77771, "vx":-2.09765, "vy":1.54871, "omega":0.78358, "ax":-5.08523, "ay":3.16068, "alpha":-0.26236, "fx":[-85.52387,-84.3545,-83.84175,-85.35304], "fy":[53.00748,51.35183,53.14212,53.24633]}, + {"t":0.45921, "x":4.63216, "y":5.57798, "heading":-1.75877, "vx":-2.22056, "vy":1.6251, "omega":0.77724, "ax":-5.19221, "ay":2.97472, "alpha":-0.43817, "fx":[-88.14942,-85.62481,-85.05777,-87.37488], "fy":[47.59107,49.62156,50.42105,50.71467]}, + {"t":0.48338, "x":4.57697, "y":5.61813, "heading":-1.73998, "vx":-2.34605, "vy":1.697, "omega":0.76665, "ax":-5.36282, "ay":2.64131, "alpha":-1.08575, "fx":[-91.92378,-87.50686,-86.28541,-91.8663], "fy":[42.43522,40.23378,46.46912,46.97943]}, + {"t":0.50755, "x":4.5187, "y":5.65991, "heading":-1.72145, "vx":-2.47566, "vy":1.76084, "omega":0.74041, "ax":-5.65667, "ay":1.89413, "alpha":-2.79057, "fx":[-101.09188,-89.45924,-87.45527,-99.16953], "fy":[24.26012,23.57962,38.15703,40.30023]}, + {"t":0.53172, "x":4.45722, "y":5.70303, "heading":-1.70356, "vx":-2.61238, "vy":1.80662, "omega":0.67296, "ax":-5.89276, "ay":-0.65963, "alpha":-8.60514, "fx":[-116.11016,-80.14528,-78.23374,-118.42874], "fy":[-26.34592,-40.63356,7.21199,15.78464]}, + {"t":0.55589, "x":4.39236, "y":5.7465, "heading":-1.68729, "vx":-2.7548, "vy":1.79067, "omega":0.46498, "ax":-3.66171, "ay":-4.25244, "alpha":2.3492, "fx":[-55.15545,-66.68918,-66.81546,-55.49543], "fy":[-65.56584,-64.81243,-75.32246,-77.84394]}, + {"t":0.56998, "x":4.35318, "y":5.7713, "heading":-1.68074, "vx":-2.80639, "vy":1.73076, "omega":0.49808, "ax":-2.9759, "ay":-4.91099, "alpha":0.93735, "fx":[-46.46975,-51.95335,-51.92642,-48.07789], "fy":[-81.18464,-78.32194,-83.48801,-84.461]}, + {"t":0.58407, "x":4.31334, "y":5.7952, "heading":-1.67372, "vx":-2.84832, "vy":1.66156, "omega":0.51129, "ax":-2.79415, "ay":-4.92208, "alpha":0.28041, "fx":[-45.55591,-47.55587,-47.65216,-45.54449], "fy":[-80.54275,-82.77352,-82.22919,-82.64963]}, + {"t":0.59816, "x":4.27293, "y":5.81812, "heading":-1.66652, "vx":-2.88769, "vy":1.59222, "omega":0.51524, "ax":-2.61599, "ay":-4.8704, "alpha":0.05869, "fx":[-42.83843,-43.95119,-44.00183,-43.63779], "fy":[-82.69926,-80.01329,-80.97247,-81.06362]}, + {"t":0.61225, "x":4.23199, "y":5.84007, "heading":-1.65926, "vx":-2.92455, "vy":1.52359, "omega":0.51607, "ax":-2.30821, "ay":-4.56538, "alpha":-0.0506, "fx":[-38.25522,-38.62739,-38.73561,-38.28883], "fy":[-75.31613,-77.69696,-75.66864,-75.72914]}, + {"t":0.62633, "x":4.19055, "y":5.86109, "heading":-1.65199, "vx":-2.95707, "vy":1.45927, "omega":0.51535, "ax":-1.9555, "ay":-4.04679, "alpha":-0.05662, "fx":[-32.02118,-32.50337,-32.54441,-33.31991], "fy":[-68.70644,-66.75096,-67.17935,-67.19516]}, + {"t":0.64042, "x":4.1487, "y":5.88125, "heading":-1.64473, "vx":-2.98462, "vy":1.40225, "omega":0.51456, "ax":-1.5659, "ay":-3.4115, "alpha":-0.01835, "fx":[-25.91986,-26.23776,-26.30291,-25.95097], "fy":[-56.33363,-57.86495,-56.61833,-56.65551]}, + {"t":0.65451, "x":4.10649, "y":5.90067, "heading":-1.63748, "vx":-3.00668, "vy":1.35419, "omega":0.5143, "ax":-1.2379, "ay":-2.79275, "alpha":-0.01555, "fx":[-20.05714,-20.54745,-20.58438,-21.35164], "fy":[-47.17493,-46.02949,-46.49287,-46.51789]}, + {"t":0.6686, "x":4.064, "y":5.91947, "heading":-1.63023, "vx":-3.02413, "vy":1.31484, "omega":0.51408, "ax":-0.99623, "ay":-2.32284, "alpha":0.02936, "fx":[-16.40973,-16.77147,-16.81063,-16.43492], "fy":[-38.34397,-39.15834,-38.67352,-38.70699]}, + {"t":0.68269, "x":4.0213, "y":5.93776, "heading":-1.62299, "vx":-3.03816, "vy":1.28211, "omega":0.51449, "ax":-0.74582, "ay":-1.79285, "alpha":0.01962, "fx":[-11.8759,-12.37415,-12.40493,-13.07479], "fy":[-30.03714,-29.50442,-29.98872,-30.01374]}, + {"t":0.69678, "x":3.97842, "y":5.95565, "heading":-1.61574, "vx":-3.04867, "vy":1.25685, "omega":0.51477, "ax":-0.61353, "ay":-1.49424, "alpha":0.05958, "fx":[-10.0298,-10.40249,-10.42721,-10.0497], "fy":[-24.64466,-24.96067,-25.00105,-25.027]}, + {"t":0.71087, "x":3.9354, "y":5.97321, "heading":-1.60849, "vx":-3.05731, "vy":1.2358, "omega":0.51561, "ax":-0.43088, "ay":-1.08307, "alpha":0.04632, "fx":[-6.65483,-7.16087,-7.18497,-7.72931], "fy":[-17.89388,-17.76755,-18.26753,-18.28833]}, + {"t":0.72496, "x":3.89228, "y":5.99051, "heading":-1.60122, "vx":-3.06338, "vy":1.22054, "omega":0.51626, "ax":-0.36249, "ay":-0.90464, "alpha":0.08657, "fx":[-5.82449,-6.24367,-6.26106,-5.84068], "fy":[-14.86545,-14.87118,-15.28179,-15.30111]}, + {"t":0.73905, "x":3.84909, "y":6.00762, "heading":-1.59395, "vx":-3.06849, "vy":1.20779, "omega":0.51748, "ax":-0.23369, "ay":-0.6066, "alpha":0.07815, "fx":[-3.38232,-3.95085,-3.96876,-4.28022], "fy":[-9.75134,-9.84881,-10.41529,-10.43145]}, + {"t":0.75314, "x":3.80583, "y":6.02458, "heading":-1.58666, "vx":-3.07178, "vy":1.19924, "omega":0.51858, "ax":-0.19879, "ay":-0.50069, "alpha":0.12072, "fx":[-3.03815,-3.57723,-3.58935,-3.05013], "fy":[-8.10242,-7.95464,-8.65725,-8.67081]}, + {"t":0.76723, "x":3.76253, "y":6.04142, "heading":-1.57935, "vx":-3.07459, "vy":1.19219, "omega":0.52028, "ax":-0.11164, "ay":-0.29791, "alpha":0.1273, "fx":[-1.32454,-2.05925,-2.06981,-1.99059], "fy":[-4.45861,-4.64249,-5.37652,-5.38647]}, + {"t":0.78132, "x":3.7192, "y":6.05819, "heading":-1.57202, "vx":-3.07616, "vy":1.18799, "omega":0.52208, "ax":-0.09057, "ay":-0.22648, "alpha":0.17981, "fx":[-1.11007,-1.90526,-1.90941,-1.11429], "fy":[-3.39134,-3.20298,-4.25074,-4.25604]}, + {"t":0.79541, "x":3.67585, "y":6.07491, "heading":-1.56467, "vx":-3.07743, "vy":1.1848, "omega":0.52461, "ax":-0.03489, "ay":-0.09802, "alpha":0.23237, "fx":[0.1149,-1.07345,-1.07031,-0.2975], "fy":[-0.89478,-1.08955,-2.27732,-2.27434]}, + {"t":0.8095, "x":3.63249, "y":6.09159, "heading":-1.55727, "vx":-3.07793, "vy":1.18342, "omega":0.52788, "ax":-0.01429, "ay":-0.03117, "alpha":0.34728, "fx":[0.53791,-1.03199,-1.01432,0.55543], "fy":[0.3075,0.46746,-1.43583,-1.41732]}, + {"t":0.82359, "x":3.58912, "y":6.10826, "heading":-1.54984, "vx":-3.07813, "vy":1.18298, "omega":0.53278, "ax":0.01886, "ay":0.04406, "alpha":0.56886, "fx":[1.72336,-1.05491,-1.00169,1.59066], "fy":[2.26596,2.05829,-0.71975,-0.66669]}, + {"t":0.83768, "x":3.54575, "y":6.12493, "heading":-1.54233, "vx":-3.07786, "vy":1.1836, "omega":0.54079, "ax":0.04878, "ay":0.13031, "alpha":0.98613, "fx":[3.08014,-1.58239,-1.45411,3.20912], "fy":[4.64524,4.64049,-0.3652,-0.23177]}, + {"t":0.85177, "x":3.50239, "y":6.14162, "heading":-1.53471, "vx":-3.07717, "vy":1.18544, "omega":0.55468, "ax":0.06847, "ay":0.17464, "alpha":1.85767, "fx":[5.45481,-3.53657,-3.21911,5.86662], "fy":[7.6379,7.22685,-1.77008,-1.4503]}, + {"t":0.86586, "x":3.45904, "y":6.15834, "heading":-1.52689, "vx":-3.07621, "vy":1.1879, "omega":0.58086, "ax":0.1279, "ay":0.33163, "alpha":3.60234, "fx":[10.52348,-7.05415,-6.28058,11.33941], "fy":[14.51014,14.00862,-3.58951,-2.81712]}, + {"t":0.87995, "x":3.41571, "y":6.17511, "heading":-1.51871, "vx":-3.07441, "vy":1.19257, "omega":0.63161, "ax":0.13265, "ay":0.33746, "alpha":7.17257, "fx":[18.62675,-16.09329,-14.3166,20.62782], "fy":[24.0001,22.1862,-12.79691,-10.8879]}, + {"t":0.89404, "x":3.37241, "y":6.19195, "heading":-1.50981, "vx":-3.07254, "vy":1.19733, "omega":0.73267, "ax":0.40559, "ay":0.57268, "alpha":15.56991, "fx":[42.51595,-34.04535,-29.40868,47.98187], "fy":[47.91504,45.59351,-30.58565,-24.73767]}, + {"t":0.90812, "x":3.32916, "y":6.20888, "heading":-1.49949, "vx":-3.06682, "vy":1.20539, "omega":0.95204, "ax":-0.02159, "ay":0.38354, "alpha":1.78447, "fx":[3.4799,-4.84493,-4.27013,4.19554], "fy":[11.12903,10.60123,1.59443,2.24927]}, + {"t":0.92287, "x":3.28393, "y":6.22669, "heading":-1.48545, "vx":-3.06714, "vy":1.21105, "omega":0.97836, "ax":0.15252, "ay":0.38949, "alpha":-0.03985, "fx":[2.18291,2.53711,2.51136,2.93853], "fy":[6.16072,6.37756,6.72975,6.70222]}, + {"t":0.93762, "x":3.23872, "y":6.24459, "heading":-1.47102, "vx":-3.06489, "vy":1.21679, "omega":0.97777, "ax":0.17202, "ay":0.42697, "alpha":0.00688, "fx":[2.89239,2.83132,2.84281,2.90346], "fy":[7.24587,6.98036,7.11433,7.1293]}, + {"t":0.95236, "x":3.19354, "y":6.26258, "heading":-1.4566, "vx":-3.06236, "vy":1.22309, "omega":0.97787, "ax":0.11193, "ay":0.28567, "alpha":-0.00358, "fx":[1.54958,1.72974,1.71053,2.47345], "fy":[4.39609,4.77213,4.94957,4.93001]}, + {"t":0.96711, "x":3.14839, "y":6.28065, "heading":-1.44218, "vx":-3.06071, "vy":1.2273, "omega":0.97782, "ax":0.16341, "ay":0.3982, "alpha":-0.00348, "fx":[2.74021,2.70141,2.70789,2.74603], "fy":[6.73534,6.4476,6.68034,6.68765]}, + {"t":0.98186, "x":3.10327, "y":6.29879, "heading":-1.42776, "vx":-3.0583, "vy":1.23318, "omega":0.97777, "ax":0.12272, "ay":0.31233, "alpha":-0.00447, "fx":[1.7484,1.92433,1.89932,2.61083], "fy":[4.8509,5.21818,5.39099,5.36577]}, + {"t":0.9966, "x":3.05819, "y":6.31701, "heading":-1.41334, "vx":-3.05649, "vy":1.23778, "omega":0.9777, "ax":0.20587, "ay":0.49636, "alpha":-0.00197, "fx":[3.44844,3.40813,3.41579,3.45497], "fy":[8.35046,8.11837,8.31053,8.31671]}, + {"t":1.01135, "x":3.01313, "y":6.33532, "heading":-1.39893, "vx":-3.05345, "vy":1.2451, "omega":0.97767, "ax":0.19436, "ay":0.48511, "alpha":-0.00472, "fx":[2.93853,3.14376,3.10803,3.76941], "fy":[7.79381,8.06199,8.26316,8.22747]}, + {"t":1.0261, "x":2.96813, "y":6.35373, "heading":-1.38451, "vx":-3.05058, "vy":1.25225, "omega":0.9776, "ax":0.31376, "ay":0.74938, "alpha":-0.00177, "fx":[5.23159,5.22724,5.22995,5.23202], "fy":[12.50106,12.45749,12.50436,12.50461]}, + {"t":1.04084, "x":2.92318, "y":6.37228, "heading":-1.37009, "vx":-3.04596, "vy":1.26331, "omega":0.97758, "ax":0.34489, "ay":0.83644, "alpha":-0.00491, "fx":[5.43442,5.70499,5.64931,6.2081], "fy":[13.79566,13.83403,14.09882,14.04382]}, + {"t":1.05559, "x":2.8783, "y":6.391, "heading":-1.35568, "vx":-3.04087, "vy":1.27564, "omega":0.97751, "ax":0.51001, "ay":1.19626, "alpha":-0.00202, "fx":[8.47523,8.54139,8.53024,8.45939], "fy":[19.81663,20.15556,19.9034,19.88883]}, + {"t":1.07034, "x":2.83351, "y":6.40994, "heading":-1.34126, "vx":-3.03335, "vy":1.29328, "omega":0.97748, "ax":0.60677, "ay":1.41576, "alpha":-0.00469, "fx":[9.79238,10.16621,10.0775,10.42192], "fy":[23.72667,23.34353,23.70839,23.62179]}, + {"t":1.08509, "x":2.78884, "y":6.42917, "heading":-1.32685, "vx":-3.0244, "vy":1.31416, "omega":0.97741, "ax":0.82872, "ay":1.87869, "alpha":-0.00336, "fx":[13.75052,13.91863,13.88303,13.70526], "fy":[30.98648,31.91745,31.20249,31.16086]}, + {"t":1.09983, "x":2.74433, "y":6.44875, "heading":-1.31243, "vx":-3.01218, "vy":1.34186, "omega":0.97736, "ax":1.03298, "ay":2.28232, "alpha":-0.00372, "fx":[16.88872,17.42079,17.27789,17.28998], "fy":[38.59784,37.56247,38.07928,37.94141]}, + {"t":1.11458, "x":2.70002, "y":6.46879, "heading":-1.29802, "vx":-2.99695, "vy":1.37552, "omega":0.9773, "ax":1.32161, "ay":2.83219, "alpha":-0.00611, "fx":[21.92154,22.22083,22.14983,21.8301], "fy":[46.58813,48.36547,46.98856,46.90309]}, + {"t":1.12933, "x":2.65597, "y":6.48938, "heading":-1.28361, "vx":-2.97746, "vy":1.41729, "omega":0.97721, "ax":1.69432, "ay":3.46784, "alpha":-0.00031, "fx":[27.91392,28.65901,28.43582,27.96517], "fy":[58.97511,57.01099,57.72794,57.51517]}, + {"t":1.14407, "x":2.61225, "y":6.51066, "heading":-1.2692, "vx":-2.95247, "vy":1.46843, "omega":0.97721, "ax":2.06284, "ay":4.06168, "alpha":-0.00584, "fx":[34.2225,34.69104,34.57364,34.059], "fy":[66.61895,69.78616,67.28784,67.13209]}, + {"t":1.15882, "x":2.56893, "y":6.53276, "heading":-1.25479, "vx":-2.92205, "vy":1.52832, "omega":0.97712, "ax":2.52707, "ay":4.68314, "alpha":0.00731, "fx":[41.91513,42.80571,42.51194,41.26734], "fy":[80.02382,76.94539,77.78365,77.50961]}, + {"t":1.17357, "x":2.52612, "y":6.5558, "heading":-1.24038, "vx":-2.88479, "vy":1.59738, "omega":0.97723, "ax":2.77357, "ay":4.86005, "alpha":-0.00278, "fx":[46.10605,46.48357,46.39888,45.94802], "fy":[80.00679,82.98658,80.60804,80.45722]}, + {"t":1.18831, "x":2.48388, "y":6.57989, "heading":-1.22597, "vx":-2.84389, "vy":1.66905, "omega":0.97719, "ax":3.05932, "ay":4.81581, "alpha":0.01937, "fx":[50.37579,51.35195,50.98512,51.27665], "fy":[81.3074,79.42643,80.38307,79.99221]}, + {"t":1.20306, "x":2.44227, "y":6.60502, "heading":-1.21156, "vx":-2.79877, "vy":1.74007, "omega":0.97747, "ax":5.16451, "ay":2.65696, "alpha":0.0875, "fx":[86.16985,85.89528,86.02684,86.26737], "fy":[44.19227,45.16443,43.85915,43.94488]}, + {"t":1.21781, "x":2.40156, "y":6.63097, "heading":-1.19714, "vx":-2.72261, "vy":1.77925, "omega":0.97876, "ax":5.88872, "ay":0.35253, "alpha":0.0479, "fx":[97.00277,97.85772,97.49914,100.28887], "fy":[5.28886,5.56936,6.5544,6.09335]}, + {"t":1.23255, "x":2.36205, "y":6.65725, "heading":-1.18271, "vx":-2.63577, "vy":1.78445, "omega":0.97947, "ax":5.90358, "ay":-0.62267, "alpha":0.02058, "fx":[98.45352,98.29988,98.36397,98.52185], "fy":[-10.0991,-10.78026,-10.3553,-10.28395]}, + {"t":1.2473, "x":2.32383, "y":6.6835, "heading":-1.16826, "vx":-2.54871, "vy":1.77527, "omega":0.97977, "ax":5.85378, "ay":-1.08767, "alpha":0.01109, "fx":[96.9193,97.19311,97.07294,99.13312], "fy":[-19.221,-17.95498,-17.58488,-17.76287]}, + {"t":1.26205, "x":2.28688, "y":6.70956, "heading":-1.15382, "vx":-2.46239, "vy":1.75923, "omega":0.97994, "ax":5.80973, "ay":-1.34832, "alpha":0.00813, "fx":[96.88561,96.74253,96.80846,96.94483], "fy":[-22.0974,-23.19492,-22.34324,-22.26788]}, + {"t":1.27679, "x":2.2512, "y":6.73535, "heading":-1.13937, "vx":-2.37672, "vy":1.73934, "omega":0.98006, "ax":5.77677, "ay":-1.50915, "alpha":0.00159, "fx":[95.85313,95.91326,95.89605,97.52122], "fy":[-26.28823,-24.83123,-24.74469,-24.76298]}, + {"t":1.29154, "x":2.21678, "y":6.76084, "heading":-1.12491, "vx":-2.29153, "vy":1.71709, "omega":0.98008, "ax":5.74938, "ay":-1.62715, "alpha":0.0061, "fx":[95.92004,95.63171,95.77841,96.02719], "fy":[-26.46056,-28.58635,-26.78264,-26.6654]}, + {"t":1.30629, "x":2.18361, "y":6.78598, "heading":-1.11046, "vx":-2.20674, "vy":1.69309, "omega":0.98017, "ax":5.72853, "ay":-1.71075, "alpha":-0.13517, "fx":[95.12445,95.25091,95.31352,96.27858], "fy":[-30.28777,-28.35626,-27.50581,-27.91997]}, + {"t":1.32103, "x":2.15169, "y":6.81077, "heading":-1.09601, "vx":-2.12227, "vy":1.66787, "omega":0.97818, "ax":5.71194, "ay":-1.77388, "alpha":-14.58868, "fx":[83.2826,130.93636,113.22619,53.41604], "fy":[-87.95292,-44.85177,24.93007,-10.40404]}, + {"t":1.33578, "x":2.12101, "y":6.83517, "heading":-1.08158, "vx":-2.03803, "vy":1.64171, "omega":0.76304, "ax":5.58429, "ay":-2.14614, "alpha":-4.62939, "fx":[88.15925,104.97982,98.71024,80.50013], "fy":[-53.33798,-41.24374,-17.47161,-31.04697]}, + {"t":1.36328, "x":2.06707, "y":6.87951, "heading":-1.06059, "vx":-1.88445, "vy":1.58268, "omega":0.63572, "ax":5.36377, "ay":-2.67216, "alpha":-3.42826, "fx":[84.80456,98.24852,93.72537,80.86743], "fy":[-58.93579,-46.35717,-31.03549,-41.84612]}, + {"t":1.39079, "x":2.01727, "y":6.92203, "heading":-1.04311, "vx":-1.73692, "vy":1.50919, "omega":0.54143, "ax":5.16368, "ay":-3.04284, "alpha":-2.59924, "fx":[82.71688,93.00509,89.7902,78.79205], "fy":[-60.39591,-52.81901,-40.36919,-49.30685]}, + {"t":1.41829, "x":1.97145, "y":6.96239, "heading":-1.02822, "vx":-1.5949, "vy":1.4255, "omega":0.46994, "ax":4.99417, "ay":-3.31514, "alpha":-2.10181, "fx":[79.98162,89.02889,86.053,77.93793], "fy":[-64.03048,-55.71251,-47.19239,-54.112]}, + {"t":1.44579, "x":1.92948, "y":7.00034, "heading":-1.01529, "vx":-1.45754, "vy":1.33432, "omega":0.41213, "ax":4.85169, "ay":-3.52143, "alpha":-1.67582, "fx":[78.62809,85.557,83.36613,75.9501], "fy":[-64.76315,-59.73881,-52.13815,-58.16214]}, + {"t":1.4733, "x":1.89122, "y":7.03571, "heading":-1.00396, "vx":-1.3241, "vy":1.23746, "omega":0.36604, "ax":4.73125, "ay":-3.68248, "alpha":-1.44807, "fx":[76.47996,83.04938,80.8057,75.13594], "fy":[-67.3669,-61.43245,-55.94862,-60.79283]}, + {"t":1.5008, "x":1.8566, "y":7.06835, "heading":-0.99389, "vx":-1.19397, "vy":1.13618, "omega":0.32621, "ax":4.62894, "ay":-3.81096, "alpha":-1.23349, "fx":[75.4884,80.73625,79.00242,73.42161], "fy":[-67.9194,-64.09926,-58.77508,-63.31353]}, + {"t":1.52831, "x":1.82551, "y":7.09816, "heading":-0.98492, "vx":-1.06666, "vy":1.03137, "omega":0.29229, "ax":4.54116, "ay":-3.91568, "alpha":-1.15942, "fx":[73.79262,79.17679,77.28141,72.54499], "fy":[-69.93391,-65.13501,-60.9986,-65.02226]}, + {"t":1.55581, "x":1.79789, "y":7.12504, "heading":-0.97688, "vx":-0.94176, "vy":0.92367, "omega":0.2604, "ax":4.46529, "ay":-4.00242, "alpha":-1.06313, "fx":[72.93728,77.63735,76.00929,71.15311], "fy":[-70.52602,-66.90996,-62.72598,-66.71187]}, + {"t":1.58331, "x":1.77367, "y":7.14893, "heading":-0.96972, "vx":-0.81895, "vy":0.81359, "omega":0.23116, "ax":4.39915, "ay":-4.07538, "alpha":-1.0573, "fx":[71.70097,76.53044,74.88016,70.21549], "fy":[-71.95367,-67.79943,-64.01278,-67.97257]}, + {"t":1.61082, "x":1.75281, "y":7.16977, "heading":-0.96336, "vx":-0.69796, "vy":0.7015, "omega":0.20208, "ax":4.34106, "ay":-4.13752, "alpha":-1.06592, "fx":[70.79524,75.66645,73.95791,69.03421], "fy":[-72.81521,-68.87781,-65.04959,-69.13927]}, + {"t":1.63832, "x":1.73526, "y":7.1875, "heading":-0.9578, "vx":-0.57856, "vy":0.5877, "omega":0.17276, "ax":4.28971, "ay":-4.19102, "alpha":-0.92921, "fx":[70.18752,75.17577,72.33058,68.33541], "fy":[-73.04015,-68.86782,-66.96232,-70.57889]}, + {"t":1.66582, "x":1.72097, "y":7.20207, "heading":-0.95305, "vx":-0.46058, "vy":0.47243, "omega":0.1472, "ax":4.24396, "ay":-4.23758, "alpha":-1.15264, "fx":[69.06634,74.47783,72.43408,67.00073], "fy":[-74.72026,-70.32224,-66.55568,-70.95546]}, + {"t":1.69333, "x":1.70991, "y":7.21347, "heading":-0.949, "vx":-0.34385, "vy":0.35588, "omega":0.1155, "ax":4.20303, "ay":-4.27839, "alpha":-1.31478, "fx":[68.23809,74.33646,72.11043,65.56521], "fy":[-75.60567,-70.3663,-66.20842,-73.09411]}, + {"t":1.72083, "x":1.70204, "y":7.22164, "heading":-0.94582, "vx":-0.22825, "vy":0.23821, "omega":0.07934, "ax":4.16616, "ay":-4.31449, "alpha":-1.34455, "fx":[67.59545,73.61578,71.56358,65.01656], "fy":[-76.53945,-71.90404,-66.95622,-72.28216]}, + {"t":1.74834, "x":1.69734, "y":7.22656, "heading":-0.94364, "vx":-0.11367, "vy":0.11955, "omega":0.04236, "ax":4.13282, "ay":-4.3466, "alpha":-1.54013, "fx":[66.70378,74.26644,70.94272,63.65584], "fy":[-77.68938,-71.71467,-67.26101,-73.15766]}, + {"t":1.77584, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/KTofetchSlow.traj b/src/main/deploy/choreo/KTofetchSlow.traj index 47d8f8fb..7fb41705 100644 --- a/src/main/deploy/choreo/KTofetchSlow.traj +++ b/src/main/deploy/choreo/KTofetchSlow.traj @@ -3,7 +3,7 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":4.141549248154255, "y":5.238261807735684, "heading":5.235987755982989, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.084460748154255, "y":5.3371419902635795, "heading":5.235987755982989, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.909953474998474, "y":6.936121463775635, "heading":-0.942477796076938, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.6957741975784302, "y":7.228199481964111, "heading":-0.942477796076938, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ @@ -16,7 +16,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"K.x", "val":4.084460748154255}, "y":{"exp":"K.y", "val":5.3371419902635795}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"K.x", "val":4.084460748154255}, "y":{"exp":"K.y", "val":5.3371419902635795}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.9099534749984741 m", "val":1.909953474998474}, "y":{"exp":"6.936121463775635 m", "val":6.936121463775635}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.6957741975784302 m", "val":1.6957741975784302}, "y":{"exp":"7.228199481964111 m", "val":7.228199481964111}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ @@ -32,53 +32,52 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.22106,1.60269], + "waypoints":[0.0,1.19423,1.57606], "samples":[ - {"t":0.0, "x":4.14155, "y":5.23826, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.19674, "ay":2.71289, "alpha":1.64131, "fx":[-67.78543,-74.85365,-72.15878,-65.03245], "fy":[51.22378,45.94723,39.1882,44.53085]}, - {"t":0.037, "x":4.13868, "y":5.24012, "heading":-1.0472, "vx":-0.15529, "vy":0.10038, "omega":0.06073, "ax":-4.19763, "ay":2.71392, "alpha":1.26674, "fx":[-68.28416,-73.75371,-71.66956,-66.18252], "fy":[49.88039,45.78865,40.58231,44.70729]}, - {"t":0.074, "x":4.13006, "y":5.24569, "heading":-1.04495, "vx":-0.31061, "vy":0.2008, "omega":0.1076, "ax":-4.19734, "ay":2.71422, "alpha":0.94644, "fx":[-68.71849,-72.75614,-71.2545,-67.14112], "fy":[48.72398,45.67289,41.74425,44.838]}, - {"t":0.11101, "x":4.11569, "y":5.25498, "heading":-1.04097, "vx":-0.46592, "vy":0.30123, "omega":0.14262, "ax":-4.197, "ay":2.71457, "alpha":0.68726, "fx":[-69.04274,-72.05861,-70.84381,-67.9029], "fy":[47.76055,45.52033,42.74294,44.97843]}, - {"t":0.14801, "x":4.09558, "y":5.26798, "heading":-1.03569, "vx":-0.62121, "vy":0.40168, "omega":0.16805, "ax":-4.19663, "ay":2.71495, "alpha":0.44835, "fx":[-69.37467,-71.27997,-70.55164,-68.61703], "fy":[46.90282,45.46261,43.59179,45.07072]}, - {"t":0.18501, "x":4.06972, "y":5.28471, "heading":-1.02947, "vx":-0.7765, "vy":0.50214, "omega":0.18464, "ax":-4.1962, "ay":2.7154, "alpha":0.25728, "fx":[-69.59895,-70.78257,-70.24171,-69.17163], "fy":[46.18905,45.34256,44.34868,45.17739]}, - {"t":0.22201, "x":4.03811, "y":5.30514, "heading":-1.02264, "vx":-0.93177, "vy":0.60261, "omega":0.19416, "ax":-4.19572, "ay":2.7159, "alpha":0.07569, "fx":[-69.84805,-70.15623,-70.04228,-69.71626], "fy":[45.54536,45.32772,44.9816,45.23621]}, - {"t":0.25901, "x":4.00076, "y":5.3293, "heading":-1.01546, "vx":-1.08702, "vy":0.70311, "omega":0.19696, "ax":-4.19516, "ay":2.71649, "alpha":-0.06428, "fx":[-69.98896,-69.8131,-69.80487,-70.11823], "fy":[45.01636,45.2405,45.56075,45.31264]}, - {"t":0.29602, "x":3.95767, "y":5.35718, "heading":-1.00817, "vx":-1.24224, "vy":0.80362, "omega":0.19459, "ax":-4.19451, "ay":2.71716, "alpha":-0.20289, "fx":[-70.17496,-69.29048,-69.67939,-70.53696], "fy":[44.53291,45.27632,46.02533,45.34069]}, - {"t":0.33302, "x":3.90883, "y":5.38877, "heading":-1.00097, "vx":-1.39745, "vy":0.90416, "omega":0.18708, "ax":-4.19373, "ay":2.71797, "alpha":-0.3003, "fx":[-70.24087,-69.09033,-69.48973,-70.80906], "fy":[44.1615,45.19748,46.46856,45.40171]}, - {"t":0.37002, "x":3.85425, "y":5.42409, "heading":-0.99405, "vx":-1.55263, "vy":1.00473, "omega":0.17597, "ax":-4.19281, "ay":2.71892, "alpha":-0.40396, "fx":[-70.37938,-68.63786,-69.42718,-71.12425], "fy":[43.8118,45.28857,46.7875,45.40478]}, - {"t":0.40702, "x":3.79393, "y":5.46313, "heading":-0.98754, "vx":-1.70777, "vy":1.10534, "omega":0.16102, "ax":-4.19166, "ay":2.72013, "alpha":-0.4629, "fx":[-70.37747,-68.56824,-69.27127,-71.27475], "fy":[43.58825,45.20978,47.1126,45.46249]}, - {"t":0.44402, "x":3.72787, "y":5.50589, "heading":-0.98158, "vx":-1.86287, "vy":1.20599, "omega":0.14389, "ax":-4.19026, "ay":2.72157, "alpha":-0.533, "fx":[-70.46918,-68.18835,-69.25665,-71.48411], "fy":[43.37974,45.327,47.30286,45.45971]}, - {"t":0.48103, "x":3.65608, "y":5.55238, "heading":-0.97625, "vx":-2.01792, "vy":1.30669, "omega":0.12417, "ax":-4.18838, "ay":2.72353, "alpha":-0.5573, "fx":[-70.41789,-68.18906,-69.14422,-71.52228], "fy":[43.29281,45.30535,47.49361,45.50824]}, - {"t":0.51803, "x":3.57854, "y":5.60259, "heading":-0.97166, "vx":-2.17289, "vy":1.40747, "omega":0.10355, "ax":-4.18596, "ay":2.72603, "alpha":-0.587, "fx":[-70.43014,-67.96385,-69.13247,-71.58517], "fy":[43.28027,45.33144,47.60131,45.55368]}, - {"t":0.55503, "x":3.49527, "y":5.65654, "heading":-0.96783, "vx":-2.32778, "vy":1.50833, "omega":0.08183, "ax":-4.18241, "ay":2.72975, "alpha":-0.56685, "fx":[-70.29461,-68.09191,-69.02487,-71.46388], "fy":[43.34523,45.42218,47.64693,45.59999]}, - {"t":0.59203, "x":3.40628, "y":5.71422, "heading":-0.9648, "vx":-2.48254, "vy":1.60934, "omega":0.06085, "ax":-4.17719, "ay":2.73515, "alpha":-0.55993, "fx":[-70.24325,-67.86687,-69.04153,-71.3751], "fy":[43.52478,45.56167,47.6183,45.67001]}, - {"t":0.62903, "x":3.31156, "y":5.77564, "heading":-0.96255, "vx":-2.6371, "vy":1.71055, "omega":0.04013, "ax":-4.16825, "ay":2.74443, "alpha":-0.46658, "fx":[-69.87707,-68.31525,-68.80949,-70.92941], "fy":[43.90732,45.65003,47.59812,45.83794]}, - {"t":0.66603, "x":3.21113, "y":5.84081, "heading":-0.96106, "vx":-2.79134, "vy":1.8121, "omega":0.02287, "ax":-4.1501, "ay":2.76318, "alpha":-0.4126, "fx":[-69.61428,-67.82665,-68.77459,-70.50546], "fy":[44.53909,46.11969,47.48807,46.09687]}, - {"t":0.70304, "x":3.105, "y":5.90975, "heading":-0.96022, "vx":-2.9449, "vy":1.91434, "omega":0.0076, "ax":-4.08926, "ay":2.82703, "alpha":-0.15809, "fx":[-68.0741,-68.20372,-67.7204,-68.66563], "fy":[46.20559,47.22722,47.90392,47.16391]}, - {"t":0.74004, "x":2.99324, "y":5.98252, "heading":-0.95993, "vx":-3.09621, "vy":2.01894, "omega":0.00175, "ax":0.08187, "ay":4.03515, "alpha":-0.04733, "fx":[1.34064,1.53929,1.41684,1.16223], "fy":[66.8772,67.79923,67.27045,67.10897]}, - {"t":0.77704, "x":2.87873, "y":6.05999, "heading":-0.95987, "vx":-3.09318, "vy":2.16825, "omega":0.0, "ax":2.66158, "ay":3.60709, "alpha":-0.0001, "fx":[44.24647,44.73798,44.16449,44.32041], "fy":[60.29988,60.20025,59.91637,60.09725]}, - {"t":0.81404, "x":2.7661, "y":6.14269, "heading":-0.95987, "vx":-2.9947, "vy":2.30172, "omega":0.0, "ax":4.54617, "ay":1.66043, "alpha":0.07276, "fx":[75.80997,75.59343,75.76582,75.96059], "fy":[27.54048,28.72455,27.14854,27.30101]}, - {"t":0.85104, "x":2.6584, "y":6.22899, "heading":-0.95987, "vx":-2.82648, "vy":2.36316, "omega":0.00269, "ax":4.51623, "ay":-2.0848, "alpha":0.19507, "fx":[75.34907,74.87359,75.04107,75.87013], "fy":[-33.88368,-34.75006,-35.54774,-34.82866]}, - {"t":0.88805, "x":2.5569, "y":6.31501, "heading":-0.95977, "vx":-2.65937, "vy":2.28602, "omega":0.00991, "ax":4.37712, "ay":-2.38884, "alpha":0.33825, "fx":[73.27992,71.90919,72.64979,74.01877], "fy":[-38.25023,-40.4535,-40.89698,-39.68227]}, - {"t":0.92505, "x":2.4615, "y":6.39796, "heading":-0.9594, "vx":-2.49741, "vy":2.19763, "omega":0.02242, "ax":4.32442, "ay":-2.49175, "alpha":0.39007, "fx":[72.43573,70.99183,71.66785,73.24831], "fy":[-40.0307,-41.41935,-43.0567,-41.63843]}, - {"t":0.96205, "x":2.37205, "y":6.47757, "heading":-0.95857, "vx":-2.3374, "vy":2.10543, "omega":0.03686, "ax":4.29598, "ay":-2.54485, "alpha":0.38755, "fx":[72.00047,70.43135,71.22594,72.79007], "fy":[-40.8524,-42.60611,-43.81152,-42.41544]}, - {"t":0.99905, "x":2.2885, "y":6.55373, "heading":-0.95721, "vx":-2.17844, "vy":2.01126, "omega":0.0512, "ax":4.2786, "ay":-2.57659, "alpha":0.33873, "fx":[71.65621,70.31268,70.97657,72.34334], "fy":[-41.66673,-42.88974,-44.23289,-43.0126]}, - {"t":1.03605, "x":2.21083, "y":6.62639, "heading":-0.95532, "vx":-2.02012, "vy":1.91593, "omega":0.06373, "ax":4.26668, "ay":-2.59803, "alpha":0.24106, "fx":[71.36729,70.38792,70.87592,71.86269], "fy":[-42.32437,-43.45696,-44.16397,-43.28618]}, - {"t":1.07306, "x":2.139, "y":6.6955, "heading":-0.95296, "vx":-1.86224, "vy":1.81979, "omega":0.07265, "ax":4.25805, "ay":-2.61338, "alpha":0.10104, "fx":[71.07042,70.70028,70.863,71.28505], "fy":[-43.17969,-43.5274,-43.96154,-43.58662]}, - {"t":1.11006, "x":2.07301, "y":6.76105, "heading":-0.95027, "vx":-1.70469, "vy":1.72309, "omega":0.07639, "ax":4.25149, "ay":-2.62497, "alpha":-0.08688, "fx":[70.78586,71.11682,70.96097,70.61783], "fy":[-44.00836,-43.96726,-43.37072,-43.6814]}, - {"t":1.14706, "x":2.01284, "y":6.82301, "heading":-0.94744, "vx":-1.54738, "vy":1.62596, "omega":0.07318, "ax":4.24637, "ay":-2.63397, "alpha":-0.32894, "fx":[70.44358,71.8134,71.10174,69.78095], "fy":[-45.13809,-43.91351,-42.69116,-43.88546]}, - {"t":1.18406, "x":1.95849, "y":6.88137, "heading":-0.94474, "vx":-1.39025, "vy":1.5285, "omega":0.061, "ax":4.2422, "ay":-2.64125, "alpha":-0.62445, "fx":[70.0968,72.60144,71.35105,68.81267], "fy":[-46.31248,-44.23369,-41.63915,-43.92827]}, - {"t":1.22106, "x":1.90995, "y":6.93612, "heading":-0.94248, "vx":-1.23328, "vy":1.43077, "omega":0.0379, "ax":4.12768, "ay":-2.81215, "alpha":-0.44898, "fx":[68.35686,70.17554,69.27178,67.42163], "fy":[-48.46933,-47.09511,-45.15416,-46.79016]}, - {"t":1.25576, "x":1.86965, "y":6.98407, "heading":-0.94116, "vx":-1.09008, "vy":1.33321, "omega":0.02232, "ax":3.91254, "ay":-3.10913, "alpha":-0.34564, "fx":[64.81862,66.28046,65.56624,64.21564], "fy":[-53.19113,-51.80522,-50.56172,-51.75259]}, - {"t":1.29045, "x":1.83419, "y":7.02845, "heading":-0.94039, "vx":-0.95434, "vy":1.22534, "omega":0.01033, "ax":3.70481, "ay":-3.3542, "alpha":-0.25997, "fx":[61.47812,62.5854,62.04871,60.9174], "fy":[-56.80339,-56.00576,-54.95407,-55.88867]}, - {"t":1.32514, "x":1.80331, "y":7.06894, "heading":-0.94003, "vx":-0.82581, "vy":1.10897, "omega":0.00131, "ax":3.51014, "ay":-3.55763, "alpha":-0.19168, "fx":[58.25267,59.12611,58.70387,57.96705], "fy":[-60.05762,-59.27988,-58.6175,-59.2606]}, - {"t":1.35984, "x":1.77677, "y":7.10528, "heading":-0.93998, "vx":-0.70403, "vy":0.98555, "omega":-0.00534, "ax":3.33, "ay":-3.72697, "alpha":-0.13307, "fx":[55.36534,55.94383,55.66459,55.06441], "fy":[-62.55815,-62.1974,-61.64271,-62.10867]}, - {"t":1.39453, "x":1.75435, "y":7.13722, "heading":-0.94017, "vx":-0.5885, "vy":0.85625, "omega":-0.00996, "ax":3.16452, "ay":-3.86862, "alpha":-0.08101, "fx":[52.62171,53.01023,52.82585,52.54647], "fy":[-64.83261,-64.4668,-64.20104,-64.45197]}, - {"t":1.42922, "x":1.73584, "y":7.1646, "heading":-0.94052, "vx":-0.47871, "vy":0.72203, "omega":-0.01277, "ax":3.01314, "ay":-3.98781, "alpha":-0.02908, "fx":[50.19871,50.31566,50.26601,50.12976], "fy":[-66.55275,-66.53425,-66.35585,-66.45647]}, - {"t":1.46392, "x":1.72104, "y":7.18725, "heading":-0.94096, "vx":-0.37417, "vy":0.58368, "omega":-0.01378, "ax":2.8749, "ay":-4.08871, "alpha":0.01864, "fx":[47.90764,47.84201,47.89137,48.052], "fy":[-68.15665,-68.14002,-68.20652,-68.12429]}, - {"t":1.49861, "x":1.70979, "y":7.20504, "heading":-0.94144, "vx":-0.27443, "vy":0.44183, "omega":-0.01313, "ax":2.74872, "ay":-4.1747, "alpha":0.07196, "fx":[45.90331,45.55091,45.74738,46.078], "fy":[-69.3394,-69.64783,-69.80347,-69.57012]}, - {"t":1.5333, "x":1.70192, "y":7.21786, "heading":-0.94189, "vx":-0.17907, "vy":0.29699, "omega":-0.01063, "ax":2.63347, "ay":-4.24844, "alpha":0.1259, "fx":[44.04085,43.43494,43.76754,44.35167], "fy":[-70.39792,-70.87406,-71.20556,-70.80037]}, - {"t":1.568, "x":1.6973, "y":7.2256, "heading":-0.94226, "vx":-0.08771, "vy":0.1496, "omega":-0.00626, "ax":2.52807, "ay":-4.31209, "alpha":0.18056, "fx":[42.32963,41.51305,41.9325,42.79173], "fy":[-71.29249,-71.87938,-72.46907,-71.88065]}, - {"t":1.60269, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":4.08446, "y":5.33714, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.2526, "ay":2.6245, "alpha":1.63794, "fx":[-68.75278,-75.75655,-73.04666,-65.99936], "fy":[49.76526,44.49877,37.70777,43.02474]}, + {"t":0.03732, "x":4.0815, "y":5.33897, "heading":-1.0472, "vx":-0.15871, "vy":0.09795, "omega":0.06113, "ax":-4.25248, "ay":2.62717, "alpha":1.2612, "fx":[-69.23044,-74.6351,-72.54865,-67.13276], "fy":[48.43607,44.35948,39.1408,43.23834]}, + {"t":0.07464, "x":4.07262, "y":5.34445, "heading":-1.04492, "vx":-0.31741, "vy":0.19599, "omega":0.1082, "ax":-4.25104, "ay":2.62934, "alpha":0.93915, "fx":[-69.64552,-73.60769,-72.12151,-68.07641], "fy":[47.28287,44.31441,40.33062,43.3916]}, + {"t":0.11196, "x":4.05781, "y":5.3536, "heading":-1.04088, "vx":-0.47605, "vy":0.29412, "omega":0.14324, "ax":-4.24944, "ay":2.63176, "alpha":0.68007, "fx":[-69.93858,-72.90556,-71.69263,-68.80739], "fy":[46.36516,44.14291,41.38311,43.58973]}, + {"t":0.14928, "x":4.03708, "y":5.36641, "heading":-1.03553, "vx":-0.63464, "vy":0.39233, "omega":0.16862, "ax":-4.24762, "ay":2.6345, "alpha":0.44025, "fx":[-70.24723,-72.09151,-71.38306,-69.5015], "fy":[45.5235,44.16867,42.26141,43.70952]}, + {"t":0.1866, "x":4.01044, "y":5.38289, "heading":-1.02924, "vx":-0.79316, "vy":0.49065, "omega":0.18505, "ax":-4.24557, "ay":2.63758, "alpha":0.24932, "fx":[-70.43563,-71.58354,-71.04673,-70.02075], "fy":[44.86631,44.04024,43.08161,43.88053]}, + {"t":0.22392, "x":3.97788, "y":5.40303, "heading":-1.02233, "vx":-0.9516, "vy":0.58909, "omega":0.19436, "ax":-4.24322, "ay":2.64112, "alpha":0.06758, "fx":[-70.65429,-70.91628,-70.82281,-70.53629], "fy":[44.25258,44.12447,43.75135,43.97612]}, + {"t":0.26124, "x":3.93942, "y":5.42686, "heading":-1.01508, "vx":-1.10996, "vy":0.68765, "omega":0.19688, "ax":-4.2405, "ay":2.64519, "alpha":-0.07213, "fx":[-70.75125,-70.55396,-70.54782,-70.89542], "fy":[43.79771,44.04006,44.40806,44.13039]}, + {"t":0.29856, "x":3.89504, "y":5.45436, "heading":-1.00773, "vx":-1.26821, "vy":0.78637, "omega":0.19419, "ax":-4.23732, "ay":2.64995, "alpha":-0.21045, "fx":[-70.89713,-69.97717,-70.38887,-71.27322], "fy":[43.36017,44.20457,44.92082,44.20785]}, + {"t":0.33588, "x":3.84476, "y":5.48555, "heading":-1.00049, "vx":-1.42635, "vy":0.88526, "omega":0.18633, "ax":-4.23356, "ay":2.65556, "alpha":-0.30678, "fx":[-70.90321,-69.75196,-70.14311,-71.48725], "fy":[43.0924,44.13751,45.46574,44.37197]}, + {"t":0.3732, "x":3.78858, "y":5.52044, "heading":-0.99353, "vx":-1.58434, "vy":0.98437, "omega":0.17489, "ax":-4.22899, "ay":2.66236, "alpha":-0.40999, "fx":[-70.9857,-69.21936,-70.03288,-71.74298], "fy":[42.81603,44.40255,45.8542,44.448]}, + {"t":0.41052, "x":3.72651, "y":5.55903, "heading":-0.987, "vx":-1.74217, "vy":1.08373, "omega":0.15958, "ax":-4.22348, "ay":2.67052, "alpha":-0.46814, "fx":[-70.90481,-69.09187,-69.80059,-71.81606], "fy":[42.73821,44.36738,46.31385,44.64545]}, + {"t":0.44784, "x":3.65855, "y":5.60134, "heading":-0.98105, "vx":-1.89979, "vy":1.18339, "omega":0.14211, "ax":-4.21648, "ay":2.68083, "alpha":-0.53482, "fx":[-70.89976,-68.62494,-69.69957,-71.92268], "fy":[42.66387,44.69433,46.62415,44.77004]}, + {"t":0.48516, "x":3.58471, "y":5.64737, "heading":-0.97575, "vx":-2.05715, "vy":1.28344, "omega":0.12215, "ax":-4.20746, "ay":2.69404, "alpha":-0.56592, "fx":[-70.75778,-68.41865,-69.50025,-71.86886], "fy":[42.78204,44.843,46.9965,45.01181]}, + {"t":0.52248, "x":3.50501, "y":5.69714, "heading":-0.97119, "vx":-2.21417, "vy":1.38398, "omega":0.10103, "ax":-4.1952, "ay":2.71188, "alpha":-0.58603, "fx":[-70.57483,-68.124,-69.29675,-71.73206], "fy":[43.02381,45.13282,47.35686,45.30937]}, + {"t":0.5598, "x":3.41946, "y":5.75068, "heading":-0.96742, "vx":-2.37073, "vy":1.48518, "omega":0.07916, "ax":-4.17834, "ay":2.73608, "alpha":-0.56877, "fx":[-70.22826,-67.99977,-68.97147,-71.40413], "fy":[43.4537,45.52199,47.75027,45.71058]}, + {"t":0.59711, "x":3.32807, "y":5.80801, "heading":-0.96446, "vx":-2.52666, "vy":1.58729, "omega":0.05794, "ax":-4.15228, "ay":2.77295, "alpha":-0.55781, "fx":[-69.82487,-67.44276,-68.64039,-70.95798], "fy":[44.10605,46.33332,48.19969,46.25565]}, + {"t":0.63443, "x":3.23089, "y":5.86918, "heading":-0.9623, "vx":-2.68163, "vy":1.69078, "omega":0.03712, "ax":-4.10814, "ay":2.8338, "alpha":-0.46851, "fx":[-68.87338,-67.29197,-67.81255,-69.94489], "fy":[45.4048,47.1315,49.08258,47.33369]}, + {"t":0.67175, "x":3.12795, "y":5.93425, "heading":-0.96091, "vx":-2.83494, "vy":1.79654, "omega":0.01964, "ax":-4.01543, "ay":2.95578, "alpha":-0.42101, "fx":[-67.37215,-65.5045,-66.54676,-68.31768], "fy":[47.68991,49.47078,50.65638,49.26893]}, + {"t":0.70907, "x":3.01935, "y":6.00335, "heading":-0.96018, "vx":-2.9848, "vy":1.90684, "omega":0.00392, "ax":-3.69134, "ay":3.331, "alpha":-0.07589, "fx":[-61.04051,-62.53101,-60.84579,-61.71392], "fy":[54.49989,55.63271,56.34519,55.62687]}, + {"t":0.74639, "x":2.90539, "y":6.07684, "heading":-0.96004, "vx":-3.12255, "vy":2.03116, "omega":0.00109, "ax":1.48801, "ay":4.61198, "alpha":-0.02914, "fx":[24.74263,24.85681,24.7121,24.90618], "fy":[78.32252,73.19595,77.92392,78.07579]}, + {"t":0.78371, "x":2.7899, "y":6.15585, "heading":-0.95999, "vx":-3.06702, "vy":2.20327, "omega":0.0, "ax":4.56489, "ay":1.80131, "alpha":-0.01113, "fx":[75.77203,77.12386,75.49845,75.98388], "fy":[30.38047,30.27513,29.50166,29.9504]}, + {"t":0.82103, "x":2.67861, "y":6.23933, "heading":-0.95999, "vx":-2.89666, "vy":2.2705, "omega":-0.00041, "ax":4.70962, "ay":-1.60774, "alpha":0.30063, "fx":[78.60219,78.03452,78.26844,79.12315], "fy":[-27.79886,-21.13044,-29.51085,-28.76056]}, + {"t":0.85835, "x":2.57379, "y":6.32295, "heading":-0.96001, "vx":-2.7209, "vy":2.2105, "omega":0.01081, "ax":4.53996, "ay":-2.06423, "alpha":0.32076, "fx":[75.87699,74.9609,75.26479,76.61291], "fy":[-33.07688,-34.27102,-35.75754,-34.53356]}, + {"t":0.89567, "x":2.47541, "y":6.404, "heading":-0.95961, "vx":-2.55147, "vy":2.13346, "omega":0.02278, "ax":4.46361, "ay":-2.23357, "alpha":0.38165, "fx":[74.78277,73.20123,74.05183,75.58892], "fy":[-35.3703,-38.166,-38.39827,-36.99578]}, + {"t":0.93299, "x":2.3833, "y":6.48207, "heading":-0.95876, "vx":-2.38489, "vy":2.05011, "omega":0.03702, "ax":4.42195, "ay":-2.3195, "alpha":0.38213, "fx":[74.04818,72.65748,73.29487,74.84637], "fy":[-37.17774,-38.55364,-40.16427,-38.7643]}, + {"t":0.97031, "x":2.29737, "y":6.55696, "heading":-0.95737, "vx":-2.21987, "vy":1.96354, "omega":0.05128, "ax":4.3959, "ay":-2.37122, "alpha":0.33477, "fx":[73.60348,72.26944,72.94409,74.29299], "fy":[-38.14022,-39.73835,-40.7231,-39.50685]}, + {"t":1.00763, "x":2.21759, "y":6.62859, "heading":-0.95546, "vx":-2.05581, "vy":1.87505, "omega":0.06378, "ax":4.37757, "ay":-2.4067, "alpha":0.24494, "fx":[73.20891,72.23712,72.73247,73.70958], "fy":[-39.18522,-40.08509,-41.0413,-40.16222]}, + {"t":1.04495, "x":2.14392, "y":6.69689, "heading":-0.95308, "vx":-1.89244, "vy":1.78523, "omega":0.07292, "ax":4.36436, "ay":-2.43186, "alpha":0.10091, "fx":[72.8494,72.44156,72.64825,73.06792], "fy":[-40.07233,-40.72904,-40.86255,-40.48801]}, + {"t":1.08227, "x":2.07633, "y":6.76182, "heading":-0.95036, "vx":-1.72957, "vy":1.69448, "omega":0.07668, "ax":4.3542, "ay":-2.45097, "alpha":-0.08964, "fx":[72.47355,72.90669,72.64015,72.30956], "fy":[-41.18329,-40.83229,-40.55207,-40.85802]}, + {"t":1.11959, "x":2.01482, "y":6.82335, "heading":-0.9475, "vx":-1.56707, "vy":1.60301, "omega":0.07334, "ax":4.34626, "ay":-2.46578, "alpha":-0.3261, "fx":[72.13676,73.41021,72.77643,71.47674], "fy":[-42.24358,-41.39242,-39.79703,-40.98027]}, + {"t":1.15691, "x":1.95936, "y":6.88146, "heading":-0.94476, "vx":-1.40487, "vy":1.51098, "omega":0.06117, "ax":4.33979, "ay":-2.47775, "alpha":-0.62889, "fx":[71.71999,74.27754,72.93945,70.43161], "fy":[-43.66673,-41.3476,-38.95011,-41.2471]}, + {"t":1.19423, "x":1.90995, "y":6.93612, "heading":-0.94248, "vx":-1.24291, "vy":1.41852, "omega":0.0377, "ax":4.22165, "ay":-2.66908, "alpha":-0.44516, "fx":[69.93833,71.71734,70.82496,69.01063], "fy":[-46.06803,-44.75274,-42.76474,-44.38379]}, + {"t":1.22894, "x":1.86935, "y":6.98375, "heading":-0.94117, "vx":-1.09637, "vy":1.32587, "omega":0.02225, "ax":3.99684, "ay":-3.0, "alpha":-0.34519, "fx":[66.21307,67.66707,66.95863,65.66305], "fy":[-51.41591,-49.98186,-48.73625,-49.90018]}, + {"t":1.26365, "x":1.8337, "y":7.02797, "heading":-0.9404, "vx":-0.95763, "vy":1.22173, "omega":0.01026, "ax":3.77528, "ay":-3.27468, "alpha":-0.25734, "fx":[62.6614,63.7382,63.222,62.10711], "fy":[-55.46378,-54.71643,-53.62049,-54.54879]}, + {"t":1.29837, "x":1.80274, "y":7.0684, "heading":-0.94004, "vx":-0.82658, "vy":1.10806, "omega":0.00133, "ax":3.56483, "ay":-3.50282, "alpha":-0.19367, "fx":[59.15097,60.03105,59.61248,58.90147], "fy":[-59.18864,-58.35714,-57.6958,-58.31998]}, + {"t":1.33308, "x":1.77619, "y":7.10476, "heading":-0.93999, "vx":-0.70284, "vy":0.98647, "omega":-0.00539, "ax":3.36812, "ay":-3.69255, "alpha":-0.13137, "fx":[56.00634,56.56393,56.30149,55.70819], "fy":[-61.97329,-61.65045,-61.06288,-61.52527]}, + {"t":1.36779, "x":1.75382, "y":7.13678, "heading":-0.94018, "vx":-0.58592, "vy":0.85829, "omega":-0.00995, "ax":3.1863, "ay":-3.8507, "alpha":-0.08012, "fx":[52.96703,53.3664,53.18312,52.93957], "fy":[-64.55275,-64.16505,-63.90024,-64.13949]}, + {"t":1.4025, "x":1.73541, "y":7.16425, "heading":-0.94053, "vx":-0.47532, "vy":0.72463, "omega":-0.01273, "ax":3.01926, "ay":-3.98317, "alpha":-0.02831, "fx":[50.30429,50.40898,50.36983,50.23534], "fy":[-66.46807,-66.47602,-66.27343,-66.37246]}, + {"t":1.43721, "x":1.72073, "y":7.187, "heading":-0.94097, "vx":-0.37052, "vy":0.58636, "omega":-0.01372, "ax":2.86636, "ay":-4.0947, "alpha":0.01975, "fx":[47.7468,47.69384,47.74293,47.93964], "fy":[-68.27119,-68.23824,-68.30407,-68.21348]}, + {"t":1.47193, "x":1.70959, "y":7.20489, "heading":-0.94145, "vx":-0.27102, "vy":0.44423, "omega":-0.01303, "ax":2.72661, "ay":-4.18917, "alpha":0.07193, "fx":[45.53689,45.17656,45.38152,45.71022], "fy":[-69.57592,-69.90772,-70.03705,-69.80517]}, + {"t":1.50664, "x":1.70183, "y":7.21779, "heading":-0.9419, "vx":-0.17637, "vy":0.29881, "omega":-0.01053, "ax":2.5989, "ay":-4.26968, "alpha":0.12545, "fx":[43.46575,42.8547,43.19442,43.77504], "fy":[-70.74923,-71.2447,-71.55081,-71.14904]}, + {"t":1.54135, "x":1.69727, "y":7.22559, "heading":-0.94226, "vx":-0.08616, "vy":0.15061, "omega":-0.00618, "ax":2.48212, "ay":-4.3387, "alpha":0.17801, "fx":[41.55748,40.76329,41.16394,42.01804], "fy":[-71.74359,-72.31824,-72.90799,-72.32642]}, + {"t":1.57606, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/LTofetchSlow.traj b/src/main/deploy/choreo/LTofetchSlow.traj index 9b15b1a0..11ac6e5d 100644 --- a/src/main/deploy/choreo/LTofetchSlow.traj +++ b/src/main/deploy/choreo/LTofetchSlow.traj @@ -3,7 +3,7 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":3.85576086490539, "y":5.073261807735684, "heading":5.235987755982989, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.798672364905389, "y":5.1721419902635795, "heading":5.235987755982989, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.9328655004501345, "y":6.905690670013428, "heading":-0.942477796076938, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.69577419757843, "y":7.228199481964111, "heading":-0.942477796076938, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ @@ -32,53 +32,53 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.16782,1.56838], + "waypoints":[0.0,1.13941,1.54008], "samples":[ - {"t":0.0, "x":3.85576, "y":5.07326, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.78118, "ay":3.2672, "alpha":1.68859, "fx":[-60.66762,-68.27667,-65.44776,-57.7296], "fy":[60.40453,55.07957,48.46585,53.90076]}, - {"t":0.03649, "x":3.85324, "y":5.07544, "heading":-1.0472, "vx":-0.13799, "vy":0.11923, "omega":0.06162, "ax":-3.78203, "ay":3.26825, "alpha":1.30687, "fx":[-61.20302,-67.10421,-64.91551,-58.95581], "fy":[59.09001,54.95,49.83791,54.04264]}, - {"t":0.07299, "x":3.84569, "y":5.08197, "heading":-1.04495, "vx":-0.27601, "vy":0.23851, "omega":0.10932, "ax":-3.7818, "ay":3.26839, "alpha":0.98282, "fx":[-61.66601,-66.06446,-64.46085,-59.97169], "fy":[57.95456,54.86201,50.97246,54.14109]}, - {"t":0.10948, "x":3.8331, "y":5.09285, "heading":-1.04096, "vx":-0.41403, "vy":0.35778, "omega":0.14518, "ax":-3.78154, "ay":3.26855, "alpha":0.71632, "fx":[-62.02652,-65.30197,-64.02267,-60.79444], "fy":[57.00734,54.71807,51.95738,54.25809]}, - {"t":0.14598, "x":3.81547, "y":5.10808, "heading":-1.03566, "vx":-0.55203, "vy":0.47707, "omega":0.17133, "ax":-3.78124, "ay":3.26873, "alpha":0.47419, "fx":[-62.38032,-64.49341,-63.7007,-61.55166], "fy":[56.15953,54.68019,52.7868,54.32638]}, - {"t":0.18247, "x":3.79281, "y":5.12767, "heading":-1.02941, "vx":-0.69003, "vy":0.59636, "omega":0.18863, "ax":-3.78091, "ay":3.26894, "alpha":0.27746, "fx":[-62.63223,-63.94903,-63.37057,-62.15207], "fy":[55.45588,54.56014,53.53335,54.41719]}, - {"t":0.21897, "x":3.76511, "y":5.15161, "heading":-1.02252, "vx":-0.82801, "vy":0.71566, "omega":0.19876, "ax":-3.78053, "ay":3.26917, "alpha":0.0926, "fx":[-62.90127,-63.29103,-63.15204,-62.7342], "fy":[54.80691,54.58221,54.14411,54.44897]}, - {"t":0.25546, "x":3.73237, "y":5.1799, "heading":-1.01527, "vx":-0.96598, "vy":0.83496, "omega":0.20214, "ax":-3.78009, "ay":3.26944, "alpha":-0.05183, "fx":[-63.06123,-62.91947,-62.89851,-63.17003], "fy":[54.28922,54.46889,54.7219,54.52026]}, - {"t":0.29195, "x":3.6946, "y":5.21255, "heading":-1.00789, "vx":-1.10393, "vy":0.95428, "omega":0.20024, "ax":-3.77958, "ay":3.26976, "alpha":-0.19366, "fx":[-63.2633,-62.37163,-62.76015,-63.62007], "fy":[53.79684,54.52776,55.17015,54.52649]}, - {"t":0.32845, "x":3.6518, "y":5.24955, "heading":-1.00059, "vx":-1.24186, "vy":1.0736, "omega":0.19318, "ax":-3.77897, "ay":3.27013, "alpha":-0.29442, "fx":[-63.34237,-62.15513,-62.55881,-63.91835], "fy":[53.42769,54.42622,55.61103,54.58129]}, - {"t":0.36494, "x":3.60396, "y":5.29091, "heading":-0.99354, "vx":-1.37977, "vy":1.19295, "omega":0.18243, "ax":-3.77824, "ay":3.27058, "alpha":-0.402, "fx":[-63.49785,-61.67175,-62.49232,-64.26409], "fy":[53.05813,54.5479,55.9112,54.55903]}, - {"t":0.40144, "x":3.55109, "y":5.33662, "heading":-0.98688, "vx":-1.51766, "vy":1.3123, "omega":0.16776, "ax":-3.77735, "ay":3.27113, "alpha":-0.46223, "fx":[-63.49803,-61.61753,-62.32252,-64.42847], "fy":[52.83505,54.41829,56.24248,54.61713]}, - {"t":0.43793, "x":3.49319, "y":5.38669, "heading":-0.98076, "vx":-1.65551, "vy":1.43168, "omega":0.15089, "ax":-3.77624, "ay":3.27182, "alpha":-0.53695, "fx":[-63.60816,-61.2016,-62.31402,-64.66868], "fy":[52.60079,54.55773,56.41215,54.58784]}, - {"t":0.47443, "x":3.43026, "y":5.44112, "heading":-0.97525, "vx":-1.79332, "vy":1.55108, "omega":0.1313, "ax":-3.7748, "ay":3.27271, "alpha":-0.55819, "fx":[-63.54166,-61.27416,-62.18037,-64.70018], "fy":[52.50851,54.45323,56.61515,54.64102]}, - {"t":0.51092, "x":3.3623, "y":5.49991, "heading":-0.97046, "vx":-1.93108, "vy":1.67052, "omega":0.11093, "ax":-3.77289, "ay":3.27388, "alpha":-0.59795, "fx":[-63.59796,-60.97411,-62.19452,-64.80257], "fy":[52.42914,54.52578,56.69603,54.64507]}, - {"t":0.54741, "x":3.28931, "y":5.56305, "heading":-0.96641, "vx":-2.06877, "vy":1.79, "omega":0.0891, "ax":-3.7702, "ay":3.27554, "alpha":-0.59075, "fx":[-63.51466,-60.95016,-62.16717,-64.75783], "fy":[52.46825,54.61512,56.68253,54.64069]}, - {"t":0.58391, "x":3.2113, "y":5.63056, "heading":-0.96316, "vx":-2.20636, "vy":1.90954, "omega":0.06755, "ax":-3.76614, "ay":3.27805, "alpha":-0.57213, "fx":[-63.42535,-60.93186,-62.13342,-64.62862], "fy":[52.59902,54.61002,56.66111,54.70371]}, - {"t":0.6204, "x":3.12828, "y":5.70243, "heading":-0.96069, "vx":-2.3438, "vy":2.02917, "omega":0.04667, "ax":-3.75935, "ay":3.28225, "alpha":-0.49544, "fx":[-63.13669,-61.26556,-61.99193,-64.27224], "fy":[52.85523,54.67088,56.56005,54.76773]}, - {"t":0.6569, "x":3.04024, "y":5.77867, "heading":-0.95899, "vx":-2.48099, "vy":2.14895, "omega":0.02859, "ax":-3.74595, "ay":3.2904, "alpha":-0.44492, "fx":[-62.94248,-60.9245,-61.98923,-63.91654], "fy":[53.20811,55.05436,56.31219,54.82318]}, - {"t":0.69339, "x":2.9472, "y":5.85928, "heading":-0.95795, "vx":-2.6177, "vy":2.26903, "omega":0.01235, "ax":-3.70453, "ay":3.31607, "alpha":-0.23921, "fx":[-61.77946,-61.55417,-61.13744,-62.53987], "fy":[54.16789,55.20915,56.39269,55.33924]}, - {"t":0.72989, "x":2.8492, "y":5.9443, "heading":-0.9575, "vx":-2.75289, "vy":2.39005, "omega":0.00362, "ax":-1.19698, "ay":4.08694, "alpha":-0.09904, "fx":[-19.97652,-19.71297,-19.8146,-20.30832], "fy":[67.74019,68.10684,68.49019,68.17231]}, - {"t":0.76638, "x":2.74794, "y":6.03424, "heading":-0.95736, "vx":-2.79658, "vy":2.5392, "omega":0.0, "ax":2.80273, "ay":2.92292, "alpha":-0.00009, "fx":[46.63753,46.99936,46.57096,46.67289], "fy":[48.85062,48.79184,48.553,48.6993]}, - {"t":0.80287, "x":2.64775, "y":6.12885, "heading":-0.95736, "vx":-2.69429, "vy":2.64587, "omega":0.0, "ax":4.60492, "ay":-1.63862, "alpha":0.13039, "fx":[76.8395,76.39604,76.63783,77.17392], "fy":[-26.77353,-27.35649,-27.78595,-27.34374]}, - {"t":0.83937, "x":2.55249, "y":6.22432, "heading":-0.95736, "vx":-2.52624, "vy":2.58607, "omega":0.00476, "ax":4.00997, "ay":-2.94772, "alpha":0.28983, "fx":[67.10369,66.01224,66.52035,67.74098], "fy":[-48.04449,-49.05915,-50.24192,-49.20297]}, - {"t":0.87586, "x":2.46297, "y":6.31673, "heading":-0.95719, "vx":-2.3799, "vy":2.47849, "omega":0.01534, "ax":3.9141, "ay":-3.09068, "alpha":0.36447, "fx":[65.6322,64.08888,64.85346,66.41045], "fy":[-50.00774,-51.91342,-52.73117,-51.42848]}, - {"t":0.91236, "x":2.37872, "y":6.40513, "heading":-0.95663, "vx":-2.23706, "vy":2.3657, "omega":0.02864, "ax":3.87613, "ay":-3.14449, "alpha":0.38555, "fx":[65.01141,63.42661,64.18628,65.82834], "fy":[-50.99562,-52.38595,-53.82897,-52.45772]}, - {"t":0.94885, "x":2.29966, "y":6.48937, "heading":-0.95559, "vx":-2.0956, "vy":2.25094, "omega":0.04271, "ax":3.85556, "ay":-3.17296, "alpha":0.34732, "fx":[64.63719,63.19076,63.87497,65.37818], "fy":[-51.559,-53.01155,-54.11832,-52.87823]}, - {"t":0.98535, "x":2.22575, "y":6.5694, "heading":-0.95403, "vx":-1.95489, "vy":2.13515, "omega":0.05538, "ax":3.84278, "ay":-3.19045, "alpha":0.26116, "fx":[64.31914,63.27176,63.75674,64.8813], "fy":[-52.22382,-53.13713,-54.15439,-53.21758]}, - {"t":1.02184, "x":2.15697, "y":6.6452, "heading":-0.95201, "vx":-1.81465, "vy":2.01872, "omega":0.06491, "ax":3.83397, "ay":-3.20239, "alpha":0.15256, "fx":[64.1409,63.58402,63.69071,64.22598], "fy":[-52.61173,-53.35623,-53.98883,-53.57247]}, - {"t":1.05833, "x":2.0933, "y":6.71674, "heading":-0.94964, "vx":-1.67474, "vy":1.90185, "omega":0.07048, "ax":3.8276, "ay":-3.21098, "alpha":-0.0604, "fx":[63.71407,64.03549,63.85897,63.60842], "fy":[-53.73406,-53.49097,-53.33286,-53.54427]}, - {"t":1.09483, "x":2.03473, "y":6.784, "heading":-0.94706, "vx":-1.53505, "vy":1.78466, "omega":0.06828, "ax":3.82275, "ay":-3.2175, "alpha":-0.29758, "fx":[63.39522,64.64524,64.06262,62.79026], "fy":[-54.65039,-53.80887,-52.49801,-53.57948]}, - {"t":1.13132, "x":1.98125, "y":6.84699, "heading":-0.94457, "vx":-1.39554, "vy":1.66724, "omega":0.05742, "ax":3.81893, "ay":-3.22261, "alpha":-0.60241, "fx":[62.99134,65.60952,64.31444,61.72351], "fy":[-55.87827,-53.69262,-51.56361,-53.74297]}, - {"t":1.16782, "x":1.93287, "y":6.90569, "heading":-0.94248, "vx":-1.25617, "vy":1.54964, "omega":0.03543, "ax":3.73275, "ay":-3.3175, "alpha":-0.38147, "fx":[61.82346,63.63341,62.51497,60.92103], "fy":[-56.61402,-55.33712,-54.15274,-55.10045]}, - {"t":1.2012, "x":1.89301, "y":6.95557, "heading":-0.9413, "vx":-1.13158, "vy":1.4389, "omega":0.0227, "ax":3.58394, "ay":-3.48243, "alpha":-0.32984, "fx":[59.31902,60.7878,60.09077,58.77243], "fy":[-59.33145,-58.00437,-56.86967,-57.99656]}, - {"t":1.23458, "x":1.85724, "y":7.00166, "heading":-0.94054, "vx":-1.01194, "vy":1.32266, "omega":0.01169, "ax":3.44914, "ay":-3.61628, "alpha":-0.24954, "fx":[57.22135,58.30049,57.78778,56.67228], "fy":[-61.11223,-60.38598,-59.3696,-60.25854]}, - {"t":1.26796, "x":1.82538, "y":7.04379, "heading":-0.94015, "vx":-0.89681, "vy":1.20195, "omega":0.00336, "ax":3.32884, "ay":-3.72755, "alpha":-0.19248, "fx":[55.21754,56.11152,55.689,54.94263], "fy":[-62.89852,-62.0975,-61.45817,-62.09169]}, - {"t":1.30134, "x":1.7973, "y":7.08184, "heading":-0.94004, "vx":-0.7857, "vy":1.07752, "omega":-0.00307, "ax":3.22133, "ay":-3.82105, "alpha":-0.13757, "fx":[53.54751,54.14863,53.86148,53.23455], "fy":[-64.13686,-63.77006,-63.19633,-63.67702]}, - {"t":1.33472, "x":1.77287, "y":7.11568, "heading":-0.94014, "vx":-0.67817, "vy":0.94997, "omega":-0.00766, "ax":3.12497, "ay":-3.90043, "alpha":-0.09192, "fx":[51.93951,52.38892,52.17538,51.86316], "fy":[-65.40702,-64.99681,-64.69074,-64.97876]}, - {"t":1.3681, "x":1.75197, "y":7.14521, "heading":-0.94039, "vx":-0.57386, "vy":0.81978, "omega":-0.01073, "ax":3.03833, "ay":-3.96845, "alpha":-0.05019, "fx":[50.59466,50.8073,50.71039,50.47747], "fy":[-66.29973,-66.21472,-65.96053,-66.1339]}, - {"t":1.40148, "x":1.73451, "y":7.17037, "heading":-0.94075, "vx":-0.47244, "vy":0.68731, "omega":-0.0124, "ax":2.96014, "ay":-4.02725, "alpha":-0.01101, "fx":[49.2916,49.36641,49.34278,49.37589], "fy":[-67.23419,-67.11401,-67.08151,-67.09947]}, - {"t":1.43486, "x":1.72039, "y":7.19107, "heading":-0.94117, "vx":-0.37363, "vy":0.55288, "omega":-0.01277, "ax":2.88934, "ay":-4.07847, "alpha":0.03099, "fx":[48.20227,48.04309,48.1346,48.2757], "fy":[-67.86655,-68.04415,-68.06776,-67.96594]}, - {"t":1.46824, "x":1.70953, "y":7.20725, "heading":-0.94159, "vx":-0.27718, "vy":0.41674, "omega":-0.01173, "ax":2.82499, "ay":-4.12341, "alpha":0.07056, "fx":[47.14094,46.82712,47.00214,47.39498], "fy":[-68.559,-68.72154,-68.95601,-68.70447]}, - {"t":1.50162, "x":1.70185, "y":7.21886, "heading":-0.94198, "vx":-0.18289, "vy":0.27911, "omega":-0.00938, "ax":2.76631, "ay":-4.16311, "alpha":0.11782, "fx":[46.24874,45.68421,45.98643,46.53277], "fy":[-68.9968,-69.45352,-69.76242,-69.37521]}, - {"t":1.535, "x":1.69729, "y":7.22586, "heading":-0.9423, "vx":-0.09055, "vy":0.14014, "omega":-0.00545, "ax":2.71261, "ay":-4.19838, "alpha":0.16316, "fx":[45.39283,44.6579,45.02247,45.79859], "fy":[-69.447,-69.988,-70.52449,-69.98051]}, - {"t":1.56838, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":3.79867, "y":5.17214, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.83558, "ay":3.20306, "alpha":1.70576, "fx":[-61.49991,-69.17664,-66.45367,-58.6187], "fy":[59.42561,53.99203,47.2835,52.87285]}, + {"t":0.03561, "x":3.79624, "y":5.17417, "heading":-1.0472, "vx":-0.13657, "vy":0.11405, "omega":0.06074, "ax":-3.83532, "ay":3.20549, "alpha":1.32387, "fx":[-62.02741,-67.99101,-65.88409,-59.82955], "fy":[58.13473,53.89945,48.6853,53.01663]}, + {"t":0.07121, "x":3.78895, "y":5.18027, "heading":-1.04503, "vx":-0.27313, "vy":0.22819, "omega":0.10787, "ax":-3.83382, "ay":3.20717, "alpha":1.00096, "fx":[-62.49832,-66.96241,-65.3675,-60.80353], "fy":[57.02636,53.81131,49.8541,53.15597]}, + {"t":0.10682, "x":3.77679, "y":5.19042, "heading":-1.04119, "vx":-0.40964, "vy":0.34238, "omega":0.14352, "ax":-3.83215, "ay":3.20902, "alpha":0.7352, "fx":[-62.82154,-66.16686,-64.92149,-61.61075], "fy":[56.10632,53.73854,50.86865,53.2577]}, + {"t":0.14243, "x":3.75978, "y":5.20465, "heading":-1.03608, "vx":-0.54609, "vy":0.45665, "omega":0.16969, "ax":-3.83029, "ay":3.21109, "alpha":0.49504, "fx":[-63.16604,-65.37433,-64.54803,-62.30767], "fy":[55.28071,53.71017,51.74868,53.36999]}, + {"t":0.17803, "x":3.7379, "y":5.22294, "heading":-1.03004, "vx":-0.68248, "vy":0.57098, "omega":0.18732, "ax":-3.82818, "ay":3.21343, "alpha":0.29987, "fx":[-63.38037,-64.77745,-64.20369,-62.89409], "fy":[54.62808,53.66276,52.50831,53.4662]}, + {"t":0.21364, "x":3.71118, "y":5.24531, "heading":-1.02337, "vx":-0.81879, "vy":0.6854, "omega":0.198, "ax":-3.82579, "ay":3.21608, "alpha":0.11715, "fx":[-63.6238,-64.13772,-63.93126,-63.40349], "fy":[54.0061,53.69576,53.18942,53.55055]}, + {"t":0.24925, "x":3.6796, "y":5.27176, "heading":-1.01632, "vx":-0.95501, "vy":0.79991, "omega":0.20217, "ax":-3.82306, "ay":3.2191, "alpha":-0.02723, "fx":[-63.75137,-63.68729,-63.65143,-63.82395], "fy":[53.54985,53.66648,53.77948,53.64751]}, + {"t":0.28485, "x":3.64317, "y":5.30228, "heading":-1.00912, "vx":-1.09114, "vy":0.91454, "omega":0.2012, "ax":-3.81989, "ay":3.22259, "alpha":-0.16662, "fx":[-63.90949,-63.15039,-63.45311,-64.18993], "fy":[53.08845,53.76364,54.31048,53.71381]}, + {"t":0.32046, "x":3.60189, "y":5.33688, "heading":-1.00196, "vx":-1.22715, "vy":1.02928, "omega":0.19527, "ax":-3.81619, "ay":3.22667, "alpha":-0.27169, "fx":[-63.96463,-62.81773,-63.20294,-64.47076], "fy":[52.7933,53.74472,54.78794,53.82237]}, + {"t":0.35607, "x":3.55578, "y":5.37558, "heading":-0.99501, "vx":-1.36303, "vy":1.14417, "omega":0.18559, "ax":-3.81178, "ay":3.23152, "alpha":-0.37512, "fx":[-64.04127,-62.35341,-63.05931,-64.70817], "fy":[52.4786,53.90104,55.20019,53.8916]}, + {"t":0.39167, "x":3.50483, "y":5.41837, "heading":-0.9884, "vx":-1.49875, "vy":1.25924, "omega":0.17224, "ax":-3.80653, "ay":3.23727, "alpha":-0.44568, "fx":[-64.02486,-62.12282,-62.81144,-64.85313], "fy":[52.33634,53.89597,55.5983,54.02405]}, + {"t":0.42728, "x":3.44905, "y":5.46526, "heading":-0.98227, "vx":-1.63429, "vy":1.3745, "omega":0.15637, "ax":-3.80003, "ay":3.24436, "alpha":-0.51612, "fx":[-64.01277,-61.7007,-62.7051,-64.9603], "fy":[52.17783,54.12259,55.90809,54.11947]}, + {"t":0.46289, "x":3.38845, "y":5.51626, "heading":-0.9767, "vx":-1.7696, "vy":1.49002, "omega":0.13799, "ax":-3.79191, "ay":3.25319, "alpha":-0.55024, "fx":[-63.89627,-61.57602,-62.43135,-64.93386], "fy":[52.21086,54.13952,56.25747,54.30855]}, + {"t":0.49849, "x":3.32304, "y":5.57137, "heading":-0.97178, "vx":-1.90462, "vy":1.60586, "omega":0.1184, "ax":-3.78133, "ay":3.26463, "alpha":-0.58903, "fx":[-63.7731,-61.13924,-62.32847,-64.89105], "fy":[52.25028,54.47798,56.48588,54.46542]}, + {"t":0.5341, "x":3.25283, "y":5.63062, "heading":-0.96757, "vx":-2.03926, "vy":1.7221, "omega":0.09742, "ax":-3.76727, "ay":3.27971, "alpha":-0.58087, "fx":[-63.48715,-61.10936,-61.98417,-64.61383], "fy":[52.52101,54.55384,56.83574,54.77448]}, + {"t":0.56971, "x":3.17783, "y":5.69402, "heading":-0.9641, "vx":-2.1734, "vy":1.83888, "omega":0.07674, "ax":-3.74719, "ay":3.30105, "alpha":-0.58145, "fx":[-63.16051,-60.54934,-61.80526,-64.34064], "fy":[52.90602,55.11679,57.01865,55.06635]}, + {"t":0.60531, "x":3.09806, "y":5.76159, "heading":-0.96137, "vx":-2.30682, "vy":1.95642, "omega":0.05604, "ax":-3.71695, "ay":3.3327, "alpha":-0.51883, "fx":[-62.51317,-60.54237,-61.21023,-63.57319], "fy":[53.58925,55.40854,57.53751,55.6829]}, + {"t":0.64092, "x":3.01357, "y":5.83336, "heading":-0.95937, "vx":-2.43917, "vy":2.07509, "omega":0.03756, "ax":-3.66529, "ay":3.3855, "alpha":-0.47185, "fx":[-61.63115,-59.49359,-60.59308,-62.67685], "fy":[54.70905,56.61898,57.97542,56.4353]}, + {"t":0.67653, "x":2.9244, "y":5.90939, "heading":-0.95803, "vx":-2.56968, "vy":2.19563, "omega":0.02076, "ax":-3.55804, "ay":3.49031, "alpha":-0.32758, "fx":[-59.62081,-58.30537,-58.97275,-60.34419], "fy":[56.88417,58.28018,59.3196,58.24301]}, + {"t":0.71213, "x":2.83064, "y":5.98979, "heading":-0.9573, "vx":-2.69637, "vy":2.31991, "omega":0.0091, "ax":-3.18393, "ay":3.81292, "alpha":-0.20549, "fx":[-53.22859,-52.55848,-52.72521,-53.78581], "fy":[62.90803,63.2056,64.37145,63.75292]}, + {"t":0.74774, "x":2.73262, "y":6.07481, "heading":-0.95697, "vx":-2.80974, "vy":2.45568, "omega":0.00178, "ax":3.11423, "ay":3.66404, "alpha":-0.0724, "fx":[51.45803,52.63557,51.84736,51.70972], "fy":[61.06605,60.99818,61.05058,61.19638]}, + {"t":0.78335, "x":2.63454, "y":6.16457, "heading":-0.95691, "vx":-2.69885, "vy":2.58614, "omega":-0.0008, "ax":4.39616, "ay":-2.31235, "alpha":0.18455, "fx":[73.33626,72.74758,73.14513,73.89872], "fy":[-37.62153,-39.13725,-39.17052,-38.25372]}, + {"t":0.81895, "x":2.54123, "y":6.25519, "heading":-0.95694, "vx":-2.54232, "vy":2.50381, "omega":0.00578, "ax":4.13999, "ay":-2.77513, "alpha":0.29616, "fx":[69.1108,68.03606,68.83167,70.0678], "fy":[-45.15458,-46.47702,-47.27449,-46.13421]}, + {"t":0.85456, "x":2.45334, "y":6.34258, "heading":-0.95673, "vx":-2.39491, "vy":2.40499, "omega":0.01632, "ax":4.04907, "ay":-2.91557, "alpha":0.38306, "fx":[67.83969,66.22076,67.15028,68.77367], "fy":[-47.06443,-49.08987,-49.87059,-48.37964]}, + {"t":0.89017, "x":2.37063, "y":6.42636, "heading":-0.95615, "vx":-2.25073, "vy":2.30118, "omega":0.02996, "ax":4.00338, "ay":-2.98251, "alpha":0.38794, "fx":[66.99506,65.56972,66.36907,68.00351], "fy":[-48.2895,-49.68192,-51.16184,-49.73455]}, + {"t":0.92577, "x":2.29302, "y":6.50641, "heading":-0.95508, "vx":-2.10819, "vy":2.19498, "omega":0.04377, "ax":3.97626, "ay":-3.0212, "alpha":0.34192, "fx":[66.59093,65.08752,65.99374,67.45705], "fy":[-49.0923,-50.6097,-51.45382,-50.29171]}, + {"t":0.96138, "x":2.22048, "y":6.58265, "heading":-0.95352, "vx":-1.9666, "vy":2.08741, "omega":0.05595, "ax":3.95753, "ay":-3.04741, "alpha":0.24525, "fx":[66.1358,65.27611,65.74116,66.72771], "fy":[-49.87033,-50.68714,-51.74338,-50.89432]}, + {"t":0.99699, "x":2.15296, "y":6.65505, "heading":-0.95153, "vx":-1.82569, "vy":1.9789, "omega":0.06468, "ax":3.94453, "ay":-3.06544, "alpha":0.10534, "fx":[65.82645,65.33122,65.69577,66.16016], "fy":[-50.70408,-51.2496,-51.35097,-51.09285]}, + {"t":1.03259, "x":2.09046, "y":6.72356, "heading":-0.94923, "vx":-1.68524, "vy":1.86975, "omega":0.06843, "ax":3.93449, "ay":-3.07921, "alpha":-0.0758, "fx":[65.48835,65.91336,65.63371,65.30896], "fy":[-51.56392,-51.24721,-51.10945,-51.39545]}, + {"t":1.0682, "x":2.03295, "y":6.78819, "heading":-0.94679, "vx":-1.54514, "vy":1.76011, "omega":0.06573, "ax":3.92683, "ay":-3.08967, "alpha":-0.28924, "fx":[65.18338,66.38162,65.74923,64.51942], "fy":[-52.47312,-51.66552,-50.42559,-51.44927]}, + {"t":1.1038, "x":1.98042, "y":6.8489, "heading":-0.94445, "vx":-1.40532, "vy":1.6501, "omega":0.05543, "ax":3.92054, "ay":-3.09821, "alpha":-0.55001, "fx":[64.89547,67.13187,65.80707,63.57943], "fy":[-53.61513,-51.73002,-49.66533,-51.57248]}, + {"t":1.13941, "x":1.93287, "y":6.90569, "heading":-0.94248, "vx":-1.26573, "vy":1.53978, "omega":0.03585, "ax":3.82761, "ay":-3.2078, "alpha":-0.39438, "fx":[63.49133,65.18373,64.15523,62.38731], "fy":[-54.68225,-53.6172,-52.10386,-53.48648]}, + {"t":1.1728, "x":1.89274, "y":6.95531, "heading":-0.94128, "vx":-1.13793, "vy":1.43267, "omega":0.02268, "ax":3.66571, "ay":-3.39632, "alpha":-0.32195, "fx":[60.76885,62.12314,61.40164,60.12866], "fy":[-57.83837,-56.64768,-55.46434,-56.50996]}, + {"t":1.20619, "x":1.85679, "y":7.00126, "heading":-0.94052, "vx":-1.01553, "vy":1.31928, "omega":0.01193, "ax":3.51661, "ay":-3.55075, "alpha":-0.25273, "fx":[58.36206,59.44404,58.89378,57.78094], "fy":[-60.03471,-59.30875,-58.28532,-59.12816]}, + {"t":1.23958, "x":1.82484, "y":7.04333, "heading":-0.94013, "vx":-0.89812, "vy":1.20072, "omega":0.00349, "ax":3.382, "ay":-3.67942, "alpha":-0.19156, "fx":[56.13629,56.99162,56.55797,55.81923], "fy":[-62.06818,-61.35032,-60.648,-61.27027]}, + {"t":1.27297, "x":1.79674, "y":7.08137, "heading":-0.94001, "vx":-0.78519, "vy":1.07787, "omega":-0.0029, "ax":3.26042, "ay":-3.78778, "alpha":-0.1427, "fx":[54.19788,54.81663,54.5111,53.87274], "fy":[-63.60832,-63.22255,-62.63047,-63.10033]}, + {"t":1.30636, "x":1.77234, "y":7.11524, "heading":-0.94011, "vx":-0.67633, "vy":0.9514, "omega":-0.00767, "ax":3.15064, "ay":-3.87974, "alpha":-0.09455, "fx":[52.37961,52.82368,52.60277,52.27232], "fy":[-65.05726,-64.67395,-64.33192,-64.6308]}, + {"t":1.33975, "x":1.75151, "y":7.14485, "heading":-0.94036, "vx":-0.57114, "vy":0.82186, "omega":-0.01082, "ax":3.05126, "ay":-3.95853, "alpha":-0.05304, "fx":[50.80744,51.03407,50.92548,50.68521], "fy":[-66.14894,-66.04596,-65.78978,-65.96245]}, + {"t":1.37313, "x":1.73414, "y":7.17008, "heading":-0.94072, "vx":-0.46926, "vy":0.68969, "omega":-0.0126, "ax":2.96114, "ay":-4.02652, "alpha":-0.01095, "fx":[49.31739,49.38446,49.35795,49.38339], "fy":[-67.21185,-67.10726,-67.06885,-67.09284]}, + {"t":1.40652, "x":1.72013, "y":7.19087, "heading":-0.94114, "vx":-0.37039, "vy":0.55524, "omega":-0.01296, "ax":2.87919, "ay":-4.08564, "alpha":0.03084, "fx":[48.03093,47.88014,47.96456,48.10336], "fy":[-67.9857,-68.15238,-68.19146,-68.09331]}, + {"t":1.43991, "x":1.70936, "y":7.20713, "heading":-0.94158, "vx":-0.27426, "vy":0.41883, "omega":-0.01193, "ax":2.80443, "ay":-4.13743, "alpha":0.07196, "fx":[46.80113,46.4814,46.66544,47.04596], "fy":[-68.78055,-68.94629,-69.19729,-68.95155]}, + {"t":1.4733, "x":1.70177, "y":7.21881, "heading":-0.94197, "vx":-0.18062, "vy":0.28068, "omega":-0.00953, "ax":2.73611, "ay":-4.18302, "alpha":0.11925, "fx":[45.73921,45.18419,45.48709,46.02777], "fy":[-69.31736,-69.76729,-70.10335,-69.72778]}, + {"t":1.50669, "x":1.69726, "y":7.22585, "heading":-0.94229, "vx":-0.08926, "vy":0.14102, "omega":-0.00555, "ax":2.6734, "ay":-4.22346, "alpha":0.16612, "fx":[44.71874,43.97966,44.40593,45.1531], "fy":[-69.85726,-70.39369,-70.94824,-70.41283]}, + {"t":1.54008, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/fetchPToC.traj b/src/main/deploy/choreo/fetchPToC.traj index bb897eec..03a69205 100644 --- a/src/main/deploy/choreo/fetchPToC.traj +++ b/src/main/deploy/choreo/fetchPToC.traj @@ -3,30 +3,30 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":1.69577419757843, "y":0.8236, "heading":0.942477796076938, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.2043471336364746, "y":2.569765567779541, "heading":0.0, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":1.6421304941177368, "y":0.8500130772590637, "heading":0.942477796076938, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.2383909225463867, "y":2.551051378250122, "heading":0.0, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":3.5561852518457457, "y":3.0196580097364216, "heading":1.0471975511965976, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":3.0}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":3.0}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":true}], + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":3.3}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":3.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"fetchP.x", "val":1.69577419757843}, "y":{"exp":"fetchP.y", "val":0.8236}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.2043471336364746 m", "val":3.2043471336364746}, "y":{"exp":"2.569765567779541 m", "val":2.569765567779541}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"fetchP.x", "val":1.6421304941177368}, "y":{"exp":"fetchP.y", "val":0.8500130772590637}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.2383909225463867 m", "val":3.2383909225463867}, "y":{"exp":"2.551051378250122 m", "val":2.551051378250122}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"C.x", "val":3.5561852518457457}, "y":{"exp":"C.y", "val":3.0196580097364216}, "heading":{"exp":"C.heading", "val":1.0471975511965976}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3 m / s", "val":3.0}}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"maxAccel", "val":3.0}}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":true}], + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.3 m / s", "val":3.3}}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"3 m / s ^ 2", "val":3.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -34,78 +34,56 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.4695,2.53854], + "waypoints":[0.0,1.03801,1.6527], "samples":[ - {"t":0.0, "x":1.69577, "y":0.8236, "heading":0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":1.98965, "ay":2.2432, "alpha":0.37009, "fx":[31.87162,33.58916,34.35522,32.84976], "fy":[37.27235,36.12739,37.52395,38.64875]}, - {"t":0.03972, "x":1.69734, "y":0.82537, "heading":0.94248, "vx":0.07902, "vy":0.08909, "omega":0.0147, "ax":1.98995, "ay":2.24393, "alpha":0.2835, "fx":[32.18971,33.46349,34.09512,32.93757], "fy":[37.31246,36.43464,37.50385,38.36982]}, - {"t":0.07943, "x":1.70205, "y":0.83068, "heading":0.94306, "vx":0.15805, "vy":0.17821, "omega":0.02596, "ax":1.98971, "ay":2.24408, "alpha":0.20402, "fx":[32.43802,33.31833,33.90224,33.01141], "fy":[37.29655,36.79075,37.44375,38.10022]}, - {"t":0.11915, "x":1.7099, "y":0.83953, "heading":0.94409, "vx":0.23708, "vy":0.26734, "omega":0.03406, "ax":1.98945, "ay":2.24426, "alpha":0.16337, "fx":[32.58438,33.38473,33.67307,33.01034], "fy":[37.35809,36.85124,37.46942,37.96403]}, - {"t":0.15886, "x":1.72088, "y":0.85191, "heading":0.94545, "vx":0.31609, "vy":0.35647, "omega":0.04055, "ax":1.98916, "ay":2.24445, "alpha":0.10511, "fx":[32.76015,33.23587,33.56361,33.07355], "fy":[37.3283,37.14632,37.41082,37.77006]}, - {"t":0.19858, "x":1.73501, "y":0.86784, "heading":0.94706, "vx":0.39509, "vy":0.44561, "omega":0.04472, "ax":1.98883, "ay":2.24466, "alpha":0.08523, "fx":[32.83605,33.32849,33.3937,33.05319], "fy":[37.39128,37.12551,37.45009,37.70297]}, - {"t":0.2383, "x":1.75227, "y":0.88731, "heading":0.94883, "vx":0.47408, "vy":0.53476, "omega":0.04811, "ax":1.98847, "ay":2.2449, "alpha":0.03969, "fx":[32.96608,33.17555,33.33587,33.10959], "fy":[37.35165,37.38827,37.39058,37.55536]}, - {"t":0.27801, "x":1.77266, "y":0.91032, "heading":0.95074, "vx":0.55306, "vy":0.62392, "omega":0.04969, "ax":1.98805, "ay":2.24517, "alpha":0.03403, "fx":[32.98925,33.3052,33.19598,33.06916], "fy":[37.41864,37.30938,37.44117,37.53476]}, - {"t":0.31773, "x":1.7962, "y":0.93687, "heading":0.95272, "vx":0.63201, "vy":0.71309, "omega":0.05104, "ax":1.98758, "ay":2.24549, "alpha":-0.00539, "fx":[33.09609,33.12649,33.17783,33.12754], "fy":[37.3672,37.57018,37.37546,37.41195]}, - {"t":0.35745, "x":1.82286, "y":0.96696, "heading":0.95474, "vx":0.71095, "vy":0.80227, "omega":0.05082, "ax":1.98704, "ay":2.24584, "alpha":0.00028, "fx":[33.07748,33.29731,33.05036,33.06663], "fy":[37.44367,37.43625,37.44027,37.42837]}, - {"t":0.39716, "x":1.85267, "y":1.0006, "heading":0.95676, "vx":0.78987, "vy":0.89147, "omega":0.05083, "ax":1.9864, "ay":2.24626, "alpha":-0.03768, "fx":[33.17319,33.08169,33.06361,33.13086], "fy":[37.37687,37.72143,37.36302,37.31519]}, - {"t":0.43688, "x":1.88561, "y":1.03777, "heading":0.95878, "vx":0.86876, "vy":0.98068, "omega":0.04934, "ax":1.98566, "ay":2.24675, "alpha":-0.02159, "fx":[33.11745,33.29952,32.93573,33.04705], "fy":[37.46924,37.52668,37.44707,37.36607]}, - {"t":0.47659, "x":1.92168, "y":1.07849, "heading":0.96074, "vx":0.94763, "vy":1.06991, "omega":0.04848, "ax":1.98477, "ay":2.24733, "alpha":-0.06217, "fx":[33.20952,33.03509,32.97609,33.11984], "fy":[37.38108,37.86581,37.35126,37.24971]}, - {"t":0.51631, "x":1.96088, "y":1.12276, "heading":0.96267, "vx":1.02645, "vy":1.15917, "omega":0.04601, "ax":1.98368, "ay":2.24805, "alpha":-0.03538, "fx":[33.12233,33.28871,32.8423,33.01431], "fy":[37.49835,37.59629,37.46271,37.3384]}, - {"t":0.55603, "x":2.00321, "y":1.17057, "heading":0.96449, "vx":1.10524, "vy":1.24845, "omega":0.04461, "ax":1.98232, "ay":2.24894, "alpha":-0.08239, "fx":[33.20834,32.97874,32.90013,33.09025], "fy":[37.3816,38.02512,37.34058,37.20754]}, - {"t":0.59574, "x":2.04867, "y":1.22193, "heading":0.96626, "vx":1.18397, "vy":1.33777, "omega":0.04133, "ax":1.9806, "ay":2.25006, "alpha":-0.04238, "fx":[33.07719,33.29909,32.73939,32.94705], "fy":[37.53804,37.65532,37.49286,37.34355]}, - {"t":0.63546, "x":2.09725, "y":1.27683, "heading":0.96791, "vx":1.26263, "vy":1.42714, "omega":0.03965, "ax":1.97828, "ay":2.25159, "alpha":-0.09818, "fx":[33.16262,32.90052,32.81519,33.02952], "fy":[37.39451,38.19302,37.34605,37.19784]}, - {"t":0.67518, "x":2.14896, "y":1.33529, "heading":0.96948, "vx":1.3412, "vy":1.51656, "omega":0.03575, "ax":1.9752, "ay":2.25357, "alpha":-0.04372, "fx":[32.95876,33.33372,32.59274,32.81754], "fy":[37.60225,37.72208,37.55026,37.38898]}, - {"t":0.71489, "x":2.20378, "y":1.3973, "heading":0.9709, "vx":1.41965, "vy":1.60606, "omega":0.03401, "ax":1.9704, "ay":2.25671, "alpha":-0.11801, "fx":[33.04079,32.75904,32.68031,32.90222], "fy":[37.41937,38.47214,37.36648,37.21536]}, - {"t":0.75461, "x":2.26172, "y":1.46287, "heading":0.97225, "vx":1.4979, "vy":1.69569, "omega":0.02933, "ax":1.96275, "ay":2.26166, "alpha":-0.04605, "fx":[32.74852,33.15549,32.36739,32.60091], "fy":[37.73928,37.87062,37.68064,37.51235]}, - {"t":0.79432, "x":2.32276, "y":1.532, "heading":0.97342, "vx":1.57586, "vy":1.78552, "omega":0.0275, "ax":1.94792, "ay":2.27118, "alpha":-0.02298, "fx":[32.65239,32.4538,32.27197,32.50519], "fy":[37.99963,37.70865,37.94241,37.78749]}, - {"t":0.83404, "x":2.38688, "y":1.6047, "heading":0.97451, "vx":1.65322, "vy":1.87572, "omega":0.02659, "ax":1.90843, "ay":2.29587, "alpha":-0.04983, "fx":[32.05201,31.47452,31.77839,31.94529], "fy":[38.2852,38.44317,38.23865,38.11694]}, - {"t":0.87376, "x":2.45405, "y":1.68101, "heading":0.97556, "vx":1.72902, "vy":1.9669, "omega":0.02461, "ax":1.45191, "ay":2.52496, "alpha":-0.06038, "fx":[24.26393,24.20434,24.13092,24.21134], "fy":[41.95269,42.5918,41.93207,41.883]}, - {"t":0.91347, "x":2.52386, "y":1.76112, "heading":0.97654, "vx":1.78668, "vy":2.06718, "omega":0.02221, "ax":-2.08868, "ay":-2.12338, "alpha":0.00737, "fx":[-34.65209,-35.51154,-34.50912,-34.59639], "fy":[-35.41265,-35.45754,-35.38783,-35.32483]}, - {"t":0.95319, "x":2.59318, "y":1.84154, "heading":0.97742, "vx":1.70372, "vy":1.98285, "omega":0.0225, "ax":-2.03832, "ay":-2.18808, "alpha":0.01047, "fx":[-34.13154,-33.97363,-33.80156,-34.00461], "fy":[-36.62995,-36.24458,-36.57877,-36.44365]}, - {"t":0.9929, "x":2.65923, "y":1.91857, "heading":0.97832, "vx":1.62277, "vy":1.89595, "omega":0.02292, "ax":-2.02284, "ay":-2.20705, "alpha":0.05558, "fx":[-33.88144,-33.76071,-33.50139,-33.73561], "fy":[-36.82194,-36.98619,-36.76042,-36.5932]}, - {"t":1.03262, "x":2.72209, "y":1.99213, "heading":0.97923, "vx":1.54243, "vy":1.80829, "omega":0.02512, "ax":-2.01543, "ay":-2.21601, "alpha":0.0877, "fx":[-33.80715,-33.53051,-33.39662,-33.65095], "fy":[-36.87721,-37.42672,-36.8133,-36.64224]}, - {"t":1.07234, "x":2.78176, "y":2.0622, "heading":0.98023, "vx":1.46239, "vy":1.72028, "omega":0.02861, "ax":-2.01116, "ay":-2.22117, "alpha":0.05471, "fx":[-33.61595,-33.84074,-33.18971,-33.45378], "fy":[-37.06765,-37.22008,-37.00089,-36.81497]}, - {"t":1.11205, "x":2.83825, "y":2.12877, "heading":0.98136, "vx":1.38251, "vy":1.63207, "omega":0.03078, "ax":-2.00835, "ay":-2.22455, "alpha":0.09324, "fx":[-33.69751,-33.40342,-33.27421,-33.53781], "fy":[-37.00879,-37.61176,-36.94303,-36.76519]}, - {"t":1.15177, "x":2.89158, "y":2.19184, "heading":0.98258, "vx":1.30275, "vy":1.54372, "omega":0.03448, "ax":-2.00634, "ay":-2.22695, "alpha":0.05419, "fx":[-33.54786,-33.70952,-33.13121,-33.39054], "fy":[-37.16223,-37.31223,-37.0976,-36.91684]}, - {"t":1.19149, "x":2.94173, "y":2.25139, "heading":0.98395, "vx":1.22306, "vy":1.45527, "omega":0.03664, "ax":-2.0049, "ay":-2.22869, "alpha":0.08427, "fx":[-33.62501,-33.35064,-33.23032,-33.47712], "fy":[-37.09282,-37.61616,-37.03106,-36.86461]}, - {"t":1.2312, "x":2.98873, "y":2.30743, "heading":0.98541, "vx":1.14343, "vy":1.36675, "omega":0.03998, "ax":-2.00376, "ay":-2.23005, "alpha":0.0482, "fx":[-33.49994,-33.61316,-33.13209,-33.36192], "fy":[-37.20918,-37.34181,-37.1517,-36.99289]}, - {"t":1.27092, "x":3.03256, "y":2.35995, "heading":0.987, "vx":1.06385, "vy":1.27819, "omega":0.0419, "ax":-2.00286, "ay":-2.23114, "alpha":0.07246, "fx":[-33.55674,-33.32507,-33.22996,-33.43491], "fy":[-37.13511,-37.60436,-37.08302,-36.94538]}, - {"t":1.31063, "x":3.07323, "y":2.40896, "heading":0.98866, "vx":0.98431, "vy":1.18957, "omega":0.04478, "ax":-2.00212, "ay":-2.23202, "alpha":0.03573, "fx":[-33.43824,-33.56721,-33.15822,-33.33359], "fy":[-37.23511,-37.33146,-37.19026,-37.07004]}, - {"t":1.35035, "x":3.11075, "y":2.45444, "heading":0.99044, "vx":0.90479, "vy":1.10093, "omega":0.04619, "ax":-2.0015, "ay":-2.23276, "alpha":0.05013, "fx":[-33.47795,-33.3209,-33.26034,-33.39693], "fy":[-37.17522,-37.51296,-37.1393,-37.04845]}, - {"t":1.39007, "x":3.1451, "y":2.49641, "heading":0.99227, "vx":0.8253, "vy":1.01225, "omega":0.04819, "ax":-2.00097, "ay":-2.23339, "alpha":0.01697, "fx":[-33.37159,-33.50317,-33.22812,-33.31771], "fy":[-37.2462,-37.28972,-37.22156,-37.16056]}, - {"t":1.42978, "x":3.1763, "y":2.53485, "heading":0.99419, "vx":0.74583, "vy":0.92355, "omega":0.04886, "ax":-2.00053, "ay":-2.23391, "alpha":0.02323, "fx":[-33.37797,-33.32827,-33.32698,-33.35812], "fy":[-37.18876,-37.42667,-37.17855,-37.15905]}, - {"t":1.4695, "x":3.20435, "y":2.56977, "heading":0.99613, "vx":0.66637, "vy":0.83483, "omega":0.04978, "ax":-0.66121, "ay":-0.74658, "alpha":0.00345, "fx":[-10.98646,-11.19599,-10.93838,-10.96756], "fy":[-12.45047,-12.46837,-12.44029,-12.42114]}, - {"t":1.50291, "x":3.22624, "y":2.59724, "heading":0.99779, "vx":0.64429, "vy":0.80988, "omega":0.0499, "ax":-0.65532, "ay":-0.75373, "alpha":0.02094, "fx":[-10.9497,-10.9173,-10.8987,-10.92972], "fy":[-12.52299,-12.73082,-12.51204,-12.49174]}, - {"t":1.53631, "x":3.2474, "y":2.62387, "heading":0.99946, "vx":0.62239, "vy":0.7847, "omega":0.0506, "ax":-0.64973, "ay":-0.75871, "alpha":0.00562, "fx":[-10.81848,-10.9441,-10.76314,-10.79686], "fy":[-12.65338,-12.67365,-12.64179,-12.62034]}, - {"t":1.56972, "x":3.26783, "y":2.64967, "heading":1.00115, "vx":0.60069, "vy":0.75936, "omega":0.05078, "ax":-0.64505, "ay":-0.76281, "alpha":0.01994, "fx":[-10.77992,-10.74567,-10.72592,-10.7589], "fy":[-12.68018,-12.86816,-12.6682,-12.6464]}, - {"t":1.60313, "x":3.28754, "y":2.67461, "heading":1.00285, "vx":0.57914, "vy":0.73387, "omega":0.05145, "ax":-0.64107, "ay":-0.76626, "alpha":0.00598, "fx":[-10.67957,-10.78479,-10.62321,-10.65767], "fy":[-12.77937,-12.79972,-12.76759,-12.74617]}, - {"t":1.63654, "x":3.30653, "y":2.6987, "heading":1.00456, "vx":0.55772, "vy":0.70827, "omega":0.05165, "ax":-0.63765, "ay":-0.7692, "alpha":0.01861, "fx":[-10.6556,-10.62254,-10.60361,-10.63547], "fy":[-12.79035,-12.96247,-12.77848,-12.75729]}, - {"t":1.66994, "x":3.3248, "y":2.72193, "heading":1.00629, "vx":0.53642, "vy":0.68258, "omega":0.05227, "ax":-0.63468, "ay":-0.77173, "alpha":0.00569, "fx":[-10.576,-10.66424,-10.52324,-10.5556], "fy":[-12.87007,-12.88882,-12.85902,-12.83932]}, - {"t":1.70335, "x":3.34237, "y":2.7443, "heading":1.00804, "vx":0.51522, "vy":0.6568, "omega":0.05246, "ax":-0.63207, "ay":-0.77393, "alpha":0.01672, "fx":[-10.55964,-10.53022,-10.51357,-10.54189], "fy":[-12.87238,-13.02752,-12.86154,-12.84259]}, - {"t":1.73676, "x":3.35923, "y":2.76581, "heading":1.00979, "vx":0.4941, "vy":0.63094, "omega":0.05302, "ax":-0.62977, "ay":-0.77586, "alpha":0.00477, "fx":[-10.49419,-10.57175,-10.44897,-10.47679], "fy":[-12.93819,-12.95388,-12.92873,-12.91219]}, - {"t":1.77017, "x":3.37538, "y":2.78646, "heading":1.01156, "vx":0.47306, "vy":0.60502, "omega":0.05318, "ax":-0.62772, "ay":-0.77757, "alpha":0.01421, "fx":[-10.48214,-10.45875,-10.44582,-10.46821], "fy":[-12.93597,-13.07218,-12.92709,-12.91189]}, - {"t":1.80357, "x":3.39084, "y":2.80624, "heading":1.01334, "vx":0.45209, "vy":0.57905, "omega":0.05365, "ax":-0.62588, "ay":-0.7791, "alpha":0.00313, "fx":[-10.42583,-10.50111,-10.39241,-10.41301], "fy":[-12.99086,-13.00188,-12.98398,-12.97218]}, - {"t":1.83698, "x":3.40559, "y":2.82515, "heading":1.01513, "vx":0.43118, "vy":0.55302, "omega":0.05376, "ax":-0.62422, "ay":-0.78047, "alpha":0.01143, "fx":[-10.41662,-10.40222,-10.3948,-10.40828], "fy":[-12.9849,-13.1066,-12.97915,-12.96953]}, - {"t":1.87039, "x":3.41965, "y":2.84318, "heading":1.01692, "vx":0.41033, "vy":0.52694, "omega":0.05414, "ax":-0.62272, "ay":-0.7817, "alpha":0.00069, "fx":[-10.36702,-10.44322,-10.35083,-10.3608], "fy":[-13.03229,-13.0366,-13.02933,-13.0243]}, - {"t":1.9038, "x":3.43301, "y":2.86035, "heading":1.01873, "vx":0.38952, "vy":0.50083, "omega":0.05416, "ax":-0.62136, "ay":-0.78282, "alpha":0.00764, "fx":[-10.3584,-10.357,-10.35744,-10.35798], "fy":[-13.02423,-13.12821,-13.02319,-13.02153]}, - {"t":1.9372, "x":3.44567, "y":2.87665, "heading":1.02054, "vx":0.36877, "vy":0.47468, "omega":0.05442, "ax":-0.62011, "ay":-0.78384, "alpha":-0.00292, "fx":[-10.31302,-10.39671,-10.3217,-10.3162], "fy":[-13.06502,-13.05974,-13.06788,-13.07251]}, - {"t":1.97061, "x":3.45765, "y":2.89207, "heading":1.02236, "vx":0.34805, "vy":0.44849, "omega":0.05432, "ax":-0.61896, "ay":-0.78478, "alpha":0.00255, "fx":[-10.3034,-10.32091,-10.33264,-10.31434], "fy":[-13.05533,-13.13958,-13.06132,-13.0711]}, - {"t":2.00402, "x":3.46893, "y":2.90661, "heading":1.02418, "vx":0.32737, "vy":0.42227, "omega":0.05441, "ax":-0.61791, "ay":-0.78563, "alpha":-0.00827, "fx":[-10.2599,-10.35972,-10.30476,-10.27663], "fy":[-13.09046,-13.0713,-13.10205,-13.1206]}, - {"t":2.03742, "x":3.47952, "y":2.92028, "heading":1.02599, "vx":0.30673, "vy":0.39603, "omega":0.05413, "ax":-0.61694, "ay":-0.78642, "alpha":-0.00455, "fx":[-10.24743,-10.29279,-10.32098,-10.27489], "fy":[-13.07924,-13.1397,-13.09583,-13.12222]}, - {"t":2.07083, "x":3.48942, "y":2.93307, "heading":1.0278, "vx":0.28612, "vy":0.36976, "omega":0.05398, "ax":-0.61603, "ay":-0.78715, "alpha":-0.0162, "fx":[-10.20357,-10.33046,-10.30179,-10.24012], "fy":[-13.10914,-13.06965,-13.13393,-13.17287]}, - {"t":2.10424, "x":3.49864, "y":2.94499, "heading":1.0296, "vx":0.26554, "vy":0.34346, "omega":0.05344, "ax":-0.6152, "ay":-0.78782, "alpha":-0.01473, "fx":[-10.1855,-10.27238,-10.32488,-10.23731], "fy":[-13.09593,-13.1266,-13.12861,-13.1795]}, - {"t":2.13765, "x":3.50717, "y":2.95602, "heading":1.03139, "vx":0.24499, "vy":0.31714, "omega":0.05295, "ax":-0.61441, "ay":-0.78845, "alpha":-0.02814, "fx":[-10.13786,-10.31066,-10.3157,-10.20378], "fy":[-13.12081,-13.05112,-13.16571,-13.23488]}, - {"t":2.17105, "x":3.51501, "y":2.96617, "heading":1.03316, "vx":0.22446, "vy":0.2908, "omega":0.05201, "ax":-0.61369, "ay":-0.78904, "alpha":-0.02984, "fx":[-10.11093,-10.26033,-10.34907,-10.19905], "fy":[-13.10523,-13.09394,-13.1625,-13.24988]}, - {"t":2.20446, "x":3.52216, "y":2.97545, "heading":1.0349, "vx":0.20396, "vy":0.26444, "omega":0.05101, "ax":-0.613, "ay":-0.78959, "alpha":-0.04617, "fx":[-10.05552,-10.29981,-10.35319,-10.16537], "fy":[-13.12426,-13.00934,-13.19999,-13.31442]}, - {"t":2.23787, "x":3.52864, "y":2.98384, "heading":1.0366, "vx":0.18348, "vy":0.23806, "omega":0.04947, "ax":-0.61236, "ay":-0.7901, "alpha":-0.05244, "fx":[-10.01422,-10.25841,-10.40166,-10.15692], "fy":[-13.10518,-13.03438,-13.20019,-13.34239]}, - {"t":2.27128, "x":3.53442, "y":2.99136, "heading":1.03825, "vx":0.16302, "vy":0.21167, "omega":0.04771, "ax":-0.61176, "ay":-0.79058, "alpha":-0.07353, "fx":[-9.9452,-10.30032,-10.42435,-10.12124], "fy":[-13.11709,-12.93395,-13.24024,-13.4229]}, - {"t":2.30468, "x":3.53953, "y":2.99799, "heading":1.03985, "vx":0.14259, "vy":0.18526, "omega":0.04526, "ax":-0.6112, "ay":-0.79103, "alpha":-0.08661, "fx":[-9.88136,-10.2699,-10.49553,-10.10657], "fy":[-13.09284,-12.9346,-13.24592,-13.47093]}, - {"t":2.33809, "x":3.54395, "y":3.00373, "heading":1.04136, "vx":0.12217, "vy":0.15883, "omega":0.04236, "ax":-0.61066, "ay":-0.79145, "alpha":-0.11523, "fx":[-9.78964,-10.31752,-10.54468,-10.06591], "fy":[-13.09522,-12.80868,-13.29137,-13.57738]}, - {"t":2.3715, "x":3.54769, "y":3.0086, "heading":1.04277, "vx":0.10177, "vy":0.13239, "omega":0.03851, "ax":-0.61016, "ay":-0.79186, "alpha":-0.13841, "fx":[-9.6913,-10.30019,-10.65084,-10.04177], "fy":[-13.06284,-12.77551,-13.30523,-13.65582]}, - {"t":2.40491, "x":3.55075, "y":3.01258, "heading":1.04406, "vx":0.08138, "vy":0.10594, "omega":0.03389, "ax":-0.60968, "ay":-0.79223, "alpha":-0.17885, "fx":[-9.56384,-10.35688,-10.73918,-9.99237], "fy":[-13.05211,-12.60842,-13.36054,-13.8036]}, - {"t":2.43831, "x":3.55313, "y":3.01567, "heading":1.04519, "vx":0.06102, "vy":0.07947, "omega":0.02792, "ax":-0.60923, "ay":-0.79259, "alpha":-0.21731, "fx":[-9.41223,-10.35779,-10.89866,-9.95343], "fy":[-13.00749,-12.52564,-13.38696,-13.92851]}, - {"t":2.47172, "x":3.55483, "y":3.01789, "heading":1.04613, "vx":0.04066, "vy":0.05299, "omega":0.02066, "ax":-0.6088, "ay":-0.79293, "alpha":-0.27595, "fx":[-9.22927,-10.42881,-11.0456,-9.88979], "fy":[-12.9778,-12.29454,-13.45831,-14.14064]}, - {"t":2.50513, "x":3.55585, "y":3.01922, "heading":1.04682, "vx":0.02032, "vy":0.0265, "omega":0.01144, "ax":-0.60839, "ay":-0.79326, "alpha":-0.34235, "fx":[-9.00355,-10.48205,-11.25793,-9.82274], "fy":[-12.92447,-12.07727,-13.52255,-14.36853]}, - {"t":2.53854, "x":3.55619, "y":3.01966, "heading":1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.64213, "y":0.85001, "heading":0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.24328, "ay":4.23745, "alpha":0.7434, "fx":[68.264,71.84601,73.11073,69.71283], "fy":[70.87449,67.98932,70.439,73.24236]}, + {"t":0.03844, "x":1.64527, "y":0.85314, "heading":0.94248, "vx":0.16313, "vy":0.16291, "omega":0.02858, "ax":4.24432, "ay":4.23851, "alpha":0.56487, "fx":[68.8815,71.59491,72.55645,69.97027], "fy":[70.83561,68.63895,70.50301,72.638]}, + {"t":0.07689, "x":1.65467, "y":0.86254, "heading":0.94358, "vx":0.3263, "vy":0.32586, "omega":0.0503, "ax":4.24422, "ay":4.2384, "alpha":0.40117, "fx":[69.41827,71.28327,72.09464,70.20028], "fy":[70.73824,69.28818,70.51032,72.07184]}, + {"t":0.11533, "x":1.67036, "y":0.8782, "heading":0.94551, "vx":0.48947, "vy":0.4888, "omega":0.06572, "ax":4.24409, "ay":4.23829, "alpha":0.2843, "fx":[69.79844,71.23329,71.62343,70.33286], "fy":[70.74477,69.62383,70.58595,71.64631]}, + {"t":0.15378, "x":1.69231, "y":0.90012, "heading":0.94804, "vx":0.65263, "vy":0.65174, "omega":0.07665, "ax":4.24395, "ay":4.23814, "alpha":0.151, "fx":[70.22909,70.92367,71.29122,70.53431], "fy":[70.65053,70.18996,70.56467,71.18603]}, + {"t":0.19222, "x":1.72054, "y":0.92831, "heading":0.95098, "vx":0.81579, "vy":0.81467, "omega":0.08245, "ax":4.24376, "ay":4.23798, "alpha":0.07279, "fx":[70.47683,71.01448,70.88172,70.59258], "fy":[70.69006,70.34374,70.65558,70.89077]}, + {"t":0.23067, "x":1.75503, "y":0.96276, "heading":0.95415, "vx":0.97894, "vy":0.9776, "omega":0.08525, "ax":4.24353, "ay":4.23776, "alpha":-0.04725, "fx":[70.85397,70.61184,70.68157,70.80314], "fy":[70.56993,70.93754,70.57703,70.48107]}, + {"t":0.26911, "x":1.79581, "y":1.00348, "heading":0.95743, "vx":1.14208, "vy":1.14052, "omega":0.08344, "ax":4.24323, "ay":4.23749, "alpha":-0.08742, "fx":[70.98006,70.86059,70.30515,70.78443], "fy":[70.65502,70.88059,70.70003,70.31205]}, + {"t":0.30756, "x":1.84285, "y":1.05046, "heading":0.96064, "vx":1.30521, "vy":1.30343, "omega":0.08008, "ax":4.24283, "ay":4.23713, "alpha":-0.18344, "fx":[71.28654,70.4341,70.20297,70.97995], "fy":[70.54861,71.38234,70.60652,69.98593]}, + {"t":0.346, "x":1.89616, "y":1.1037, "heading":0.96372, "vx":1.46833, "vy":1.46632, "omega":0.07302, "ax":4.24224, "ay":4.23664, "alpha":-0.20494, "fx":[71.34136,70.67716,69.91355,70.93254], "fy":[70.61714,71.29216,70.69915,69.88248]}, + {"t":0.38445, "x":1.95575, "y":1.1632, "heading":0.96652, "vx":1.63142, "vy":1.6292, "omega":0.06514, "ax":4.24137, "ay":4.2359, "alpha":-0.26087, "fx":[71.52215,70.35874,69.86991,71.05579], "fy":[70.55313,71.57544,70.62429,69.68879]}, + {"t":0.42289, "x":2.0216, "y":1.22897, "heading":0.96903, "vx":1.79448, "vy":1.79205, "omega":0.05511, "ax":4.23987, "ay":4.23471, "alpha":-0.28432, "fx":[71.58299,70.23101,69.81592,71.07664], "fy":[70.51935,71.66211,70.59192,69.58904]}, + {"t":0.46134, "x":2.09372, "y":1.30099, "heading":0.97115, "vx":1.95748, "vy":1.95485, "omega":0.04418, "ax":4.23687, "ay":4.23232, "alpha":-0.30042, "fx":[71.51548,70.21919,69.75851,71.01318], "fy":[70.42675,71.80846,70.47254,69.49496]}, + {"t":0.49978, "x":2.17211, "y":1.37927, "heading":0.97285, "vx":2.12036, "vy":2.11756, "omega":0.03263, "ax":4.22699, "ay":4.22588, "alpha":-0.24963, "fx":[71.21144,70.25635,69.64045,70.73922], "fy":[70.39033,71.40515,70.41515,69.56287]}, + {"t":0.53823, "x":2.25675, "y":1.4638, "heading":0.9741, "vx":2.28287, "vy":2.28002, "omega":0.02304, "ax":1.32897, "ay":1.34447, "alpha":-0.14496, "fx":[22.80878,22.00823,21.49721,22.29893], "fy":[22.68806,22.55355,22.47721,21.92788]}, + {"t":0.57667, "x":2.34549, "y":1.55245, "heading":0.97499, "vx":2.33396, "vy":2.33171, "omega":0.01746, "ax":0.03064, "ay":-0.03055, "alpha":-0.08216, "fx":[0.80667,0.4102,0.24617,0.58022], "fy":[-0.45758,-0.23945,-0.56064,-0.77904]}, + {"t":0.61512, "x":2.43525, "y":1.64207, "heading":0.97566, "vx":2.33514, "vy":2.33054, "omega":0.01431, "ax":0.04029, "ay":-0.04041, "alpha":0.01802, "fx":[0.8116,0.64452,0.53212,0.69843], "fy":[-0.43216,-1.16314,-0.48944,-0.60946]}, + {"t":0.65356, "x":2.52505, "y":1.73164, "heading":0.97621, "vx":2.33669, "vy":2.32898, "omega":0.015, "ax":-0.01201, "ay":0.01213, "alpha":0.0039, "fx":[-0.38388,0.42982,-0.44013,-0.40694], "fy":[0.20684,0.22682,0.19748,0.17793]}, + {"t":0.692, "x":2.61487, "y":1.82118, "heading":0.97678, "vx":2.33622, "vy":2.32945, "omega":0.01515, "ax":-0.3977, "ay":0.39613, "alpha":0.07749, "fx":[-6.65405,-6.6265,-6.60337,-6.63405], "fy":[6.83939,5.84482,6.85159,6.87766]}, + {"t":0.73045, "x":2.70439, "y":1.91103, "heading":0.97737, "vx":2.32093, "vy":2.34468, "omega":0.01813, "ax":-1.53174, "ay":1.47854, "alpha":0.07266, "fx":[-25.95067,-24.90816,-25.50433,-25.77032], "fy":[24.59457,24.46094,24.68106,24.84986]}, + {"t":0.76889, "x":2.79249, "y":2.00226, "heading":0.97806, "vx":2.26205, "vy":2.40152, "omega":0.02092, "ax":-4.9909, "ay":1.33487, "alpha":-0.01413, "fx":[-83.74514,-82.99242,-82.7048,-83.34126], "fy":[21.51188,23.56213,21.71704,22.21543]}, + {"t":0.80734, "x":2.87577, "y":2.09558, "heading":0.97887, "vx":2.07017, "vy":2.45284, "omega":0.02038, "ax":-4.37532, "ay":-4.07269, "alpha":0.26319, "fx":[-73.78499,-72.49612,-72.15302,-73.30411], "fy":[-67.89195,-68.84292,-67.8843,-66.94009]}, + {"t":0.84578, "x":2.95212, "y":2.18687, "heading":0.97965, "vx":1.90197, "vy":2.29627, "omega":0.0305, "ax":-4.31277, "ay":-4.15508, "alpha":0.28871, "fx":[-72.75241,-71.7207,-70.85858,-72.23567], "fy":[-69.29102,-70.25694,-69.31026,-68.19486]}, + {"t":0.88423, "x":3.02205, "y":2.27207, "heading":0.98082, "vx":1.73616, "vy":2.13653, "omega":0.0416, "ax":-4.29149, "ay":-4.18245, "alpha":0.29729, "fx":[-72.48925,-71.09902,-70.59418,-71.96609], "fy":[-69.70442,-70.76925,-69.75257,-68.65156]}, + {"t":0.92267, "x":3.08563, "y":2.35112, "heading":0.98242, "vx":1.57118, "vy":1.97573, "omega":0.05302, "ax":-4.2805, "ay":-4.19639, "alpha":0.30822, "fx":[-72.23588,-70.91076,-70.50121,-71.7677], "fy":[-69.82224,-71.28029,-69.85508,-68.84964]}, + {"t":0.96112, "x":3.14287, "y":2.42398, "heading":0.98446, "vx":1.40661, "vy":1.8144, "omega":0.06487, "ax":-4.27386, "ay":-4.20477, "alpha":0.23322, "fx":[-71.98929,-70.9416,-70.46682,-71.57488], "fy":[-70.08954,-70.89017,-70.13175,-69.25428]}, + {"t":0.99956, "x":3.19379, "y":2.49062, "heading":0.98696, "vx":1.24231, "vy":1.65275, "omega":0.07384, "ax":-4.26937, "ay":-4.2104, "alpha":0.21804, "fx":[-71.78872,-70.82818,-70.58954,-71.46682], "fy":[-70.07915,-71.15162,-70.0989,-69.41148]}, + {"t":1.03801, "x":3.23839, "y":2.55105, "heading":0.98979, "vx":1.07817, "vy":1.49089, "omega":0.08222, "ax":-2.08054, "ay":-2.15021, "alpha":0.19105, "fx":[-35.32484,-34.53318,-34.02495,-34.84325], "fy":[-35.95434,-36.47664,-35.74449,-35.19643]}, + {"t":1.06874, "x":3.27055, "y":2.59586, "heading":0.99232, "vx":1.01423, "vy":1.4248, "omega":0.08809, "ax":-2.00318, "ay":-2.22857, "alpha":0.17368, "fx":[-33.95556,-33.23611,-32.81033,-33.56613], "fy":[-37.31176,-37.71096,-37.03497,-36.53879]}, + {"t":1.09948, "x":3.30077, "y":2.6386, "heading":0.99503, "vx":0.95266, "vy":1.3563, "omega":0.09343, "ax":-1.94028, "ay":-2.28408, "alpha":0.16709, "fx":[-32.867,-32.19663,-31.82567,-32.48507], "fy":[-38.11119,-38.72872,-37.94448,-37.51349]}, + {"t":1.13021, "x":3.32914, "y":2.6792, "heading":0.9979, "vx":0.89303, "vy":1.2861, "omega":0.09857, "ax":-1.88969, "ay":-2.32653, "alpha":0.13764, "fx":[-31.89058,-31.35191,-31.02484,-31.73366], "fy":[-39.03035,-39.17647,-38.65603,-38.26556]}, + {"t":1.16095, "x":3.35569, "y":2.71763, "heading":1.00093, "vx":0.83495, "vy":1.2146, "omega":0.1028, "ax":-1.84824, "ay":-2.35993, "alpha":0.12843, "fx":[-31.20033,-30.69201,-30.42673,-30.91805], "fy":[-39.3489,-39.87194,-39.22312,-38.91148]}, + {"t":1.19168, "x":3.38048, "y":2.75385, "heading":1.00409, "vx":0.77814, "vy":1.14207, "omega":0.10675, "ax":-1.81367, "ay":-2.38687, "alpha":0.09419, "fx":[-30.48007,-30.12302,-29.90315,-30.42591], "fy":[-39.99954,-40.03986,-39.68849,-39.42408]}, + {"t":1.22242, "x":3.40354, "y":2.78782, "heading":1.00737, "vx":0.7224, "vy":1.06871, "omega":0.10964, "ax":-1.78444, "ay":-2.40903, "alpha":0.08097, "fx":[-29.98259,-29.66999,-29.51776,-29.81287], "fy":[-40.146,-40.52139,-40.06993,-39.89209]}, + {"t":1.25315, "x":3.4249, "y":2.81953, "heading":1.01074, "vx":0.66755, "vy":0.99466, "omega":0.11213, "ax":-1.75941, "ay":-2.42756, "alpha":0.04203, "fx":[-29.40279,-29.27069,-29.17719,-29.4634], "fy":[-40.6553,-40.54089,-40.39219,-40.27689]}, + {"t":1.28389, "x":3.44458, "y":2.84896, "heading":1.01419, "vx":0.61348, "vy":0.92005, "omega":0.11342, "ax":-1.73774, "ay":-2.44329, "alpha":0.02262, "fx":[-29.01274,-28.94641,-28.93066,-28.97951], "fy":[-40.68661,-40.88897,-40.67683,-40.66114]}, + {"t":1.31462, "x":3.46262, "y":2.87608, "heading":1.01767, "vx":0.56007, "vy":0.84496, "omega":0.11412, "ax":-1.71881, "ay":-2.45678, "alpha":-0.02493, "fx":[-28.50996,-28.66413,-28.72833,-28.7045], "fy":[-41.08309,-40.81255,-40.92357,-40.99436]}, + {"t":1.34535, "x":3.47902, "y":2.90089, "heading":1.02118, "vx":0.50724, "vy":0.76945, "omega":0.11335, "ax":-1.70213, "ay":-2.46849, "alpha":-0.05354, "fx":[-28.16996,-28.42864,-28.58545,-28.31051], "fy":[-41.06457,-41.04386,-41.14745,-41.33858]}, + {"t":1.37609, "x":3.49381, "y":2.92337, "heading":1.02466, "vx":0.45493, "vy":0.69358, "omega":0.1117, "ax":-1.68732, "ay":-2.47875, "alpha":-0.1134, "fx":[-27.69157,-28.23178,-28.50088,-28.08307], "fy":[-41.37577,-40.89181,-41.34998,-41.66058]}, + {"t":1.40682, "x":3.50699, "y":2.94352, "heading":1.0281, "vx":0.40307, "vy":0.6174, "omega":0.10822, "ax":-1.67409, "ay":-2.4878, "alpha":-0.12067, "fx":[-27.35501,-28.05276,-28.43255,-27.7848], "fy":[-41.44935,-41.12687,-41.66275,-41.64271]}, + {"t":1.43756, "x":3.51859, "y":2.96132, "heading":1.03142, "vx":0.35162, "vy":0.54094, "omega":0.10451, "ax":-1.6622, "ay":-2.49585, "alpha":-0.18063, "fx":[-26.91163,-27.86682,-28.49652,-27.5574], "fy":[-41.44292,-41.26959,-41.56327,-42.14239]}, + {"t":1.46829, "x":3.52861, "y":2.97677, "heading":1.03464, "vx":0.30053, "vy":0.46423, "omega":0.09896, "ax":-1.65146, "ay":-2.50305, "alpha":-0.27546, "fx":[-26.56572,-27.82172,-28.49671,-27.23179], "fy":[-41.50887,-40.88653,-41.87499,-42.62788]}, + {"t":1.49903, "x":3.53707, "y":2.98985, "heading":1.03768, "vx":0.24977, "vy":0.3873, "omega":0.09049, "ax":-1.6417, "ay":-2.50952, "alpha":-0.36706, "fx":[-26.05858,-27.7538,-28.6263,-27.02693], "fy":[-41.70195,-40.60656,-42.03391,-42.9878]}, + {"t":1.52976, "x":3.54397, "y":3.00057, "heading":1.04046, "vx":0.19931, "vy":0.31017, "omega":0.07921, "ax":-1.63281, "ay":-2.51538, "alpha":-0.44772, "fx":[-25.67589,-27.7023,-28.76044,-26.7338], "fy":[-41.60126,-40.51713,-42.20645,-43.39617]}, + {"t":1.5605, "x":3.54932, "y":3.00891, "heading":1.04289, "vx":0.14913, "vy":0.23286, "omega":0.06545, "ax":-1.62466, "ay":-2.52071, "alpha":-0.57071, "fx":[-25.08123,-27.69573,-29.02822,-26.52424], "fy":[-41.72694,-40.13678,-42.37045,-43.84196]}, + {"t":1.59123, "x":3.55314, "y":3.01488, "heading":1.0449, "vx":0.0992, "vy":0.15538, "omega":0.04791, "ax":-1.61718, "ay":-2.52557, "alpha":-0.68998, "fx":[-24.59662,-27.71519,-29.3108,-26.20771], "fy":[-41.60555,-39.88676,-42.55264,-44.35536]}, + {"t":1.62197, "x":3.55542, "y":3.01846, "heading":1.04638, "vx":0.04949, "vy":0.07776, "omega":0.0267, "ax":-1.61028, "ay":-2.53003, "alpha":-0.86883, "fx":[-23.8664,-27.86279,-29.76186,-25.87915], "fy":[-41.59987,-39.3417,-42.76086,-44.9949]}, + {"t":1.6527, "x":3.55619, "y":3.01966, "heading":1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/fetchPToCSlow.traj b/src/main/deploy/choreo/fetchPToCSlow.traj index 3d4c83b4..f13f447b 100644 --- a/src/main/deploy/choreo/fetchPToCSlow.traj +++ b/src/main/deploy/choreo/fetchPToCSlow.traj @@ -4,8 +4,8 @@ "snapshot":{ "waypoints":[ {"x":1.69577419757843, "y":0.8236, "heading":0.942477796076938, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.2043471336364746, "y":2.569765567779541, "heading":0.0, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":3.6132737518457456, "y":3.1185381922643174, "heading":1.0471975511965976, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.2043471336364746, "y":2.569765567779541, "heading":0.0, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.5561852518457457, "y":3.0196580097364216, "heading":1.0471975511965976, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -17,8 +17,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"fetchP.x", "val":1.69577419757843}, "y":{"exp":"fetchP.y", "val":0.8236}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.2043471336364746 m", "val":3.2043471336364746}, "y":{"exp":"2.569765567779541 m", "val":2.569765567779541}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"fetchP.x", "val":1.6421304941177368}, "y":{"exp":"fetchP.y", "val":0.8500130772590637}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.2043471336364746 m", "val":3.2043471336364746}, "y":{"exp":"2.569765567779541 m", "val":2.569765567779541}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"C.x", "val":3.5561852518457457}, "y":{"exp":"C.y", "val":3.0196580097364216}, "heading":{"exp":"C.heading", "val":1.0471975511965976}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -34,74 +34,71 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.18208,2.35233], + "waypoints":[0.0,1.19441,2.26345], "samples":[ - {"t":0.0, "x":1.69577, "y":0.8236, "heading":0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.32201, "ay":3.73338, "alpha":0.37922, "fx":[54.04717,55.9821,56.57131,54.90499], "fy":[62.2688,60.91576,62.2344,63.51566]}, - {"t":0.0394, "x":1.69835, "y":0.8265, "heading":0.94248, "vx":0.1309, "vy":0.1471, "omega":0.01494, "ax":3.32282, "ay":3.73428, "alpha":0.28621, "fx":[54.39614,55.81647,56.3063,55.04037], "fy":[62.273,61.25673,62.24519,63.22001]}, - {"t":0.07881, "x":1.70609, "y":0.83519, "heading":0.94307, "vx":0.26182, "vy":0.29425, "omega":0.02622, "ax":3.32275, "ay":3.7342, "alpha":0.19646, "fx":[54.69453,55.59052,56.10469,55.16481], "fy":[62.21025,61.65866,62.19728,62.9233]}, - {"t":0.11821, "x":1.71899, "y":0.84969, "heading":0.9441, "vx":0.39275, "vy":0.44138, "omega":0.03396, "ax":3.32267, "ay":3.73411, "alpha":0.1548, "fx":[54.82861,55.72518,55.82991,55.16532], "fy":[62.26789,61.69471,62.25511,62.76547]}, - {"t":0.15761, "x":1.73704, "y":0.86998, "heading":0.94544, "vx":0.52367, "vy":0.58852, "omega":0.04006, "ax":3.32257, "ay":3.734, "alpha":0.07837, "fx":[55.07915,55.44426,55.72674,55.29241], "fy":[62.18601,62.09366,62.17985,62.51613]}, - {"t":0.19701, "x":1.76025, "y":0.89606, "heading":0.94702, "vx":0.65459, "vy":0.73564, "omega":0.04315, "ax":3.32245, "ay":3.73386, "alpha":0.06221, "fx":[55.12491,55.68704,55.48067,55.24199], "fy":[62.26673,61.99963,62.25965,62.44075]}, - {"t":0.23642, "x":1.78863, "y":0.92795, "heading":0.94872, "vx":0.7855, "vy":0.88277, "omega":0.0456, "ax":3.32231, "ay":3.7337, "alpha":-0.0079, "fx":[55.35197,55.33463,55.45552,55.38298], "fy":[62.16256,62.42078,62.15651,62.21587]}, - {"t":0.27582, "x":1.82216, "y":0.96563, "heading":0.95051, "vx":0.91641, "vy":1.02989, "omega":0.04529, "ax":3.32213, "ay":3.73349, "alpha":-0.00377, "fx":[55.3336,55.65209,55.23154,55.29582], "fy":[62.26352,62.21517,62.25703,62.20632]}, - {"t":0.31522, "x":1.86084, "y":1.00911, "heading":0.9523, "vx":1.04731, "vy":1.17699, "omega":0.04514, "ax":3.3219, "ay":3.73323, "alpha":-0.08007, "fx":[55.54705,55.21869,55.27814,55.45381], "fy":[62.10187,62.77024,62.09004,61.96217]}, - {"t":0.35462, "x":1.90469, "y":1.05838, "heading":0.95408, "vx":1.1782, "vy":1.32409, "omega":0.04198, "ax":3.32159, "ay":3.73287, "alpha":-0.04605, "fx":[55.44992,55.67743,55.04169,55.30832], "fy":[62.26101,62.34284,62.25014,62.04652]}, - {"t":0.39403, "x":1.95369, "y":1.11345, "heading":0.95573, "vx":1.30908, "vy":1.47118, "omega":0.04017, "ax":3.32116, "ay":3.73237, "alpha":-0.1302, "fx":[55.66263,55.13199,55.16209,55.49199], "fy":[62.04023,63.03095,62.01902,61.77708]}, - {"t":0.43343, "x":2.00785, "y":1.17432, "heading":0.95731, "vx":1.43994, "vy":1.61824, "omega":0.03504, "ax":3.32051, "ay":3.73163, "alpha":-0.05986, "fx":[55.41354,55.95328,54.82776,55.21058], "fy":[62.26874,62.3376,62.25084,61.96058]}, - {"t":0.47283, "x":2.06716, "y":1.24098, "heading":0.95869, "vx":1.57078, "vy":1.76528, "omega":0.03268, "ax":3.31944, "ay":3.73037, "alpha":-0.13192, "fx":[55.66073,55.14226,55.07145,55.45917], "fy":[62.04818,62.93116,62.0185,61.73616]}, - {"t":0.51223, "x":2.13163, "y":1.31343, "heading":0.95998, "vx":1.70157, "vy":1.91226, "omega":0.02748, "ax":3.31727, "ay":3.72786, "alpha":-0.0939, "fx":[55.63045,55.06158,55.06425,55.43306], "fy":[62.13025,62.50407,62.10526,61.82668]}, - {"t":0.55164, "x":2.20126, "y":1.39167, "heading":0.96106, "vx":1.83228, "vy":2.05915, "omega":0.02378, "ax":3.311, "ay":3.72, "alpha":-0.14117, "fx":[55.4483,55.04361,54.99057,55.2886], "fy":[61.77339,62.99699,61.74339,61.5284]}, - {"t":0.59104, "x":2.27602, "y":1.4757, "heading":0.962, "vx":1.96274, "vy":2.20573, "omega":0.01822, "ax":0.80962, "ay":0.86496, "alpha":-0.05379, "fx":[13.75768,13.17618,13.42654,13.62369], "fy":[14.44713,14.58338,14.39193,14.25154]}, - {"t":0.63044, "x":2.35399, "y":1.56328, "heading":0.96272, "vx":1.99464, "vy":2.23981, "omega":0.0161, "ax":0.01767, "ay":-0.01566, "alpha":-0.07308, "fx":[0.33092,0.28773,0.25856,0.30123], "fy":[-0.46959,0.42749,-0.48415,-0.51791]}, - {"t":0.66984, "x":2.4326, "y":1.65152, "heading":0.96335, "vx":1.99534, "vy":2.23919, "omega":0.01322, "ax":-0.0159, "ay":0.01417, "alpha":-0.00428, "fx":[-0.40432,0.26876,-0.48645,-0.43809], "fy":[0.24521,0.28701,0.22704,0.18539]}, - {"t":0.70925, "x":2.51121, "y":1.73976, "heading":0.96387, "vx":1.99471, "vy":2.23975, "omega":0.01305, "ax":-0.14431, "ay":0.12819, "alpha":0.09229, "fx":[-2.33642,-2.41835,-2.47451,-2.39314], "fy":[2.53922,1.0266,2.51661,2.46521]}, - {"t":0.74865, "x":2.58969, "y":1.82811, "heading":0.96439, "vx":1.98903, "vy":2.2448, "omega":0.01669, "ax":-0.41134, "ay":0.3617, "alpha":0.02078, "fx":[-6.89522,-6.96087,-6.73924,-6.83173], "fy":[6.01565,5.95291,6.04298,6.10624]}, - {"t":0.78805, "x":2.66774, "y":1.91685, "heading":0.96505, "vx":1.97282, "vy":2.25905, "omega":0.01751, "ax":-2.1019, "ay":-0.2532, "alpha":-0.00958, "fx":[-35.2682,-34.97558,-34.8182,-35.08887], "fy":[-4.52445,-3.64453,-4.45123,-4.26257]}, - {"t":0.82745, "x":2.74385, "y":2.00566, "heading":0.96574, "vx":1.89, "vy":2.24908, "omega":0.01713, "ax":-3.35047, "ay":-3.68464, "alpha":0.08209, "fx":[-56.00133,-56.2692,-55.35778,-55.77479], "fy":[-61.46648,-61.69274,-61.42047,-61.10497]}, - {"t":0.86686, "x":2.81572, "y":2.09142, "heading":0.96641, "vx":1.75798, "vy":2.10389, "omega":0.02037, "ax":-3.337, "ay":-3.71024, "alpha":0.14246, "fx":[-56.0134,-55.47683,-55.25926,-55.75557], "fy":[-61.76775,-62.54267,-61.72,-61.36141]}, - {"t":0.90626, "x":2.88239, "y":2.17144, "heading":0.96721, "vx":1.6265, "vy":1.9577, "omega":0.02598, "ax":-3.33261, "ay":-3.71862, "alpha":0.10543, "fx":[-55.83363,-55.78001,-55.03667,-55.56173], "fy":[-62.03594,-62.31536,-61.99676,-61.60265]}, - {"t":0.94566, "x":2.9439, "y":2.24569, "heading":0.96824, "vx":1.49518, "vy":1.81118, "omega":0.03013, "ax":-3.33049, "ay":-3.72273, "alpha":0.16042, "fx":[-55.94149,-55.28706,-55.16192,-55.68048], "fy":[-61.92862,-62.8983,-61.88644,-61.51103]}, - {"t":0.98506, "x":3.00022, "y":2.31417, "heading":0.96943, "vx":1.36395, "vy":1.66449, "omega":0.03645, "ax":-3.32919, "ay":-3.72522, "alpha":0.09303, "fx":[-55.72329,-55.81138,-54.97678,-55.47257], "fy":[-62.15349,-62.36498,-62.12094,-61.7509]}, - {"t":1.02447, "x":3.05138, "y":2.37686, "heading":0.97086, "vx":1.23277, "vy":1.51771, "omega":0.04012, "ax":-3.32833, "ay":-3.72687, "alpha":0.13537, "fx":[-55.84843,-55.28039,-55.173,-55.62472], "fy":[-62.02617,-62.81659,-61.99192,-61.66571]}, - {"t":1.06387, "x":3.09737, "y":2.43377, "heading":0.97244, "vx":1.10163, "vy":1.37086, "omega":0.04545, "ax":-3.3277, "ay":-3.72805, "alpha":0.07869, "fx":[-55.69414,-55.57996,-55.11071,-55.50005], "fy":[-62.17751,-62.39077,-62.15017,-61.86102]}, - {"t":1.10327, "x":3.1382, "y":2.48489, "heading":0.97423, "vx":0.97051, "vy":1.22397, "omega":0.04855, "ax":-3.32724, "ay":-3.72894, "alpha":0.09802, "fx":[-55.7249,-55.31108,-55.25018,-55.56764], "fy":[-62.08107,-62.67428,-62.05505,-61.82819]}, - {"t":1.14267, "x":3.17385, "y":2.53022, "heading":0.97615, "vx":0.83941, "vy":1.07704, "omega":0.05242, "ax":-3.32687, "ay":-3.72963, "alpha":0.04349, "fx":[-55.56984,-55.57239,-55.23085,-55.45641], "fy":[-62.19784,-62.29973,-62.17699,-62.01011]}, - {"t":1.18208, "x":3.20435, "y":2.56977, "heading":0.97821, "vx":0.70832, "vy":0.93008, "omega":0.05413, "ax":-0.65513, "ay":-0.74967, "alpha":0.04585, "fx":[-11.04402,-10.9923,-10.72883,-10.91793], "fy":[-12.52858,-12.66183,-12.46505,-12.33138]}, - {"t":1.21551, "x":3.22766, "y":2.60044, "heading":0.98002, "vx":0.68642, "vy":0.90501, "omega":0.05566, "ax":-0.64507, "ay":-0.76169, "alpha":0.05373, "fx":[-10.89592,-10.72323,-10.61057,-10.78209], "fy":[-12.68023,-12.96375,-12.62718,-12.51679]}, - {"t":1.24895, "x":3.25025, "y":2.63028, "heading":0.98188, "vx":0.66485, "vy":0.87955, "omega":0.05746, "ax":-0.63655, "ay":-0.76915, "alpha":0.03447, "fx":[-10.69891,-10.68435,-10.45769,-10.60294], "fy":[-12.84599,-12.94578,-12.7971,-12.69633]}, - {"t":1.28238, "x":3.27213, "y":2.65926, "heading":0.9838, "vx":0.64356, "vy":0.85383, "omega":0.05861, "ax":-0.63, "ay":-0.77477, "alpha":0.04321, "fx":[-10.61168,-10.47826,-10.39244,-10.52467], "fy":[-12.89661,-13.13948,-12.85462,-12.76936]}, - {"t":1.31582, "x":3.29329, "y":2.68737, "heading":0.98576, "vx":0.6225, "vy":0.82792, "omega":0.06006, "ax":-0.6248, "ay":-0.77916, "alpha":0.02676, "fx":[-10.48101,-10.48249,-10.29125,-10.40591], "fy":[-13.0078,-13.08496,-12.96901,-12.89092]}, - {"t":1.34926, "x":3.31376, "y":2.71462, "heading":0.98777, "vx":0.60161, "vy":0.80187, "omega":0.06095, "ax":-0.62058, "ay":-0.78268, "alpha":0.03559, "fx":[-10.43144,-10.32579,-10.25881,-10.36329], "fy":[-13.02783,-13.23948,-12.99377,-12.92632]}, - {"t":1.38269, "x":3.33353, "y":2.74099, "heading":0.98981, "vx":0.58086, "vy":0.7757, "omega":0.06214, "ax":-0.61709, "ay":-0.78556, "alpha":0.0212, "fx":[-10.33672,-10.34855,-10.18434,-10.27673], "fy":[-13.11091,-13.17184,-13.07944,-13.01763]}, - {"t":1.41613, "x":3.3526, "y":2.76649, "heading":0.99189, "vx":0.56023, "vy":0.74944, "omega":0.06285, "ax":-0.61415, "ay":-0.78797, "alpha":0.0294, "fx":[-10.30694,-10.22191,-10.16871,-10.25267], "fy":[-13.11746,-13.29842,-13.08937,-13.03517]}, - {"t":1.44956, "x":3.37099, "y":2.79111, "heading":0.99399, "vx":0.53969, "vy":0.72309, "omega":0.06383, "ax":-0.61164, "ay":-0.79001, "alpha":0.017, "fx":[-10.23506,-10.24944,-10.11164,-10.18674], "fy":[-13.18221,-13.23081,-13.15642,-13.10704]}, - {"t":1.483, "x":3.38869, "y":2.81484, "heading":0.99612, "vx":0.51924, "vy":0.69668, "omega":0.0644, "ax":-0.60947, "ay":-0.79176, "alpha":0.02457, "fx":[-10.2152,-10.14672,-10.10451,-10.17199], "fy":[-13.18113,-13.33962,-13.15799,-13.11447]}, - {"t":1.51643, "x":3.40572, "y":2.83769, "heading":0.99828, "vx":0.49886, "vy":0.6702, "omega":0.06522, "ax":-0.60758, "ay":-0.79328, "alpha":0.01338, "fx":[-10.1572,-10.17803,-10.05844,-10.11876], "fy":[-13.23424,-13.27251,-13.21335,-13.17435]}, - {"t":1.54987, "x":3.42206, "y":2.85966, "heading":1.00046, "vx":0.47855, "vy":0.64368, "omega":0.06567, "ax":-0.60592, "ay":-0.79461, "alpha":0.02026, "fx":[-10.14374,-10.09007,-10.05754,-10.11028], "fy":[-13.22893,-13.36739,-13.21038,-13.1764]}, - {"t":1.58331, "x":3.43772, "y":2.88074, "heading":1.00265, "vx":0.45829, "vy":0.61711, "omega":0.06635, "ax":-0.60445, "ay":-0.79578, "alpha":0.00992, "fx":[-10.09486,-10.12309,-10.01968,-10.06576], "fy":[-13.27355,-13.30215,-13.25747,-13.22821]}, - {"t":1.61674, "x":3.4527, "y":2.90093, "heading":1.00487, "vx":0.43808, "vy":0.5905, "omega":0.06668, "ax":-0.60313, "ay":-0.79683, "alpha":0.01598, "fx":[-10.08488,-10.04627,-10.0234,-10.06115], "fy":[-13.26575,-13.38541,-13.25209,-13.22773]}, - {"t":1.65018, "x":3.46701, "y":2.92022, "heading":1.0071, "vx":0.41791, "vy":0.56386, "omega":0.06721, "ax":-0.60195, "ay":-0.79776, "alpha":0.0062, "fx":[-10.04246,-10.07829,-9.99285,-10.02335], "fy":[-13.30381,-13.32212,-13.29313,-13.27422]}, - {"t":1.68361, "x":3.48065, "y":2.93863, "heading":1.00935, "vx":0.39779, "vy":0.53719, "omega":0.06742, "ax":-0.60088, "ay":-0.7986, "alpha":0.01115, "fx":[-10.03327,-10.01203,-10.00005,-10.0205], "fy":[-13.29474,-13.39411,-13.28698,-13.27354]}, - {"t":1.71705, "x":3.49361, "y":2.95615, "heading":1.0116, "vx":0.37769, "vy":0.51048, "omega":0.06779, "ax":-0.59992, "ay":-0.79937, "alpha":0.00161, "fx":[-9.99427,-10.0443,-9.97566,-9.9871], "fy":[-13.32708,-13.3331,-13.32327,-13.31668]}, - {"t":1.75048, "x":3.50591, "y":2.97277, "heading":1.01387, "vx":0.35764, "vy":0.48376, "omega":0.06785, "ax":-0.59904, "ay":-0.80006, "alpha":0.00538, "fx":[-9.98448,-9.98545,-9.98714,-9.98544], "fy":[-13.31667,-13.39582,-13.31678,-13.31704]}, - {"t":1.78392, "x":3.51753, "y":2.9885, "heading":1.01614, "vx":0.33761, "vy":0.45701, "omega":0.06803, "ax":-0.59823, "ay":-0.80069, "alpha":-0.00447, "fx":[-9.94697,-10.01706,-9.96929,-9.95536], "fy":[-13.34441,-13.33442,-13.35008,-13.35955]}, - {"t":1.81736, "x":3.52848, "y":3.00333, "heading":1.01841, "vx":0.3176, "vy":0.43024, "omega":0.06788, "ax":-0.59749, "ay":-0.80127, "alpha":-0.00225, "fx":[-9.93421,-9.96563,-9.98574,-9.95364], "fy":[-13.33269,-13.38779,-13.34396,-13.36267]}, - {"t":1.85079, "x":3.53877, "y":3.01727, "heading":1.02068, "vx":0.29763, "vy":0.40344, "omega":0.0678, "ax":-0.5968, "ay":-0.8018, "alpha":-0.01304, "fx":[-9.89547,-9.99787,-9.97495,-9.92534], "fy":[-13.35611,-13.32396,-13.37548,-13.40714]}, - {"t":1.88423, "x":3.54839, "y":3.03031, "heading":1.02295, "vx":0.27767, "vy":0.37664, "omega":0.06737, "ax":-0.59617, "ay":-0.8023, "alpha":-0.01298, "fx":[-9.8774,-9.95252,-9.99864,-9.92289], "fy":[-13.34296,-13.3665,-13.37066,-13.4154]}, - {"t":1.91766, "x":3.55734, "y":3.04245, "heading":1.0252, "vx":0.25774, "vy":0.34981, "omega":0.06693, "ax":-0.59558, "ay":-0.80275, "alpha":-0.02548, "fx":[-9.83473,-9.9852,-9.99701,-9.8954], "fy":[-13.36167,-13.2977,-13.40153,-13.46504]}, - {"t":1.9511, "x":3.56562, "y":3.0537, "heading":1.02744, "vx":0.23782, "vy":0.32297, "omega":0.06608, "ax":-0.59504, "ay":-0.80318, "alpha":-0.02852, "fx":[-9.80735,-9.94695,-10.03092,-9.89074], "fy":[-13.34645,-13.32728,-13.39893,-13.48153]}, - {"t":1.98453, "x":3.57324, "y":3.06405, "heading":1.02965, "vx":0.21793, "vy":0.29611, "omega":0.06513, "ax":-0.59453, "ay":-0.80357, "alpha":-0.04398, "fx":[-9.7566,-9.98135,-10.04157,-9.86253], "fy":[-13.35973,-13.24895,-13.43073,-13.54107]}, - {"t":2.01797, "x":3.5802, "y":3.0735, "heading":1.03183, "vx":0.19805, "vy":0.26925, "omega":0.06366, "ax":-0.59405, "ay":-0.80394, "alpha":-0.05152, "fx":[-9.71448,-9.95084,-10.09094,-9.85408], "fy":[-13.34123,-13.26192,-13.43157,-13.57031]}, - {"t":2.05141, "x":3.58649, "y":3.08205, "heading":1.03395, "vx":0.17819, "vy":0.24237, "omega":0.06193, "ax":-0.59361, "ay":-0.80428, "alpha":-0.07185, "fx":[-9.64967,-9.98875,-10.11905,-9.82317], "fy":[-13.34773,-13.16705,-13.46649,-13.64672]}, - {"t":2.08484, "x":3.59211, "y":3.08971, "heading":1.03603, "vx":0.15834, "vy":0.21547, "omega":0.05953, "ax":-0.59319, "ay":-0.80461, "alpha":-0.08614, "fx":[-9.58461,-9.96764,-10.19197,-9.80855], "fy":[-13.32417,-13.1569,-13.47275,-13.6957]}, - {"t":2.11828, "x":3.59708, "y":3.09646, "heading":1.03802, "vx":0.13851, "vy":0.18857, "omega":0.05665, "ax":-0.5928, "ay":-0.80491, "alpha":-0.11417, "fx":[-9.49703,-10.01109,-10.24606,-9.77238], "fy":[-13.32136,-13.03532,-13.51376,-13.79931]}, - {"t":2.15171, "x":3.60137, "y":3.10232, "heading":1.03991, "vx":0.11869, "vy":0.16166, "omega":0.05283, "ax":-0.59243, "ay":-0.80519, "alpha":-0.13868, "fx":[-9.39627,-10.00306,-10.35473,-9.7478], "fy":[-13.28983,-12.9917,-13.52841,-13.87885]}, - {"t":2.18515, "x":3.60501, "y":3.10727, "heading":1.04168, "vx":0.09888, "vy":0.13474, "omega":0.0482, "ax":-0.59208, "ay":-0.80546, "alpha":-0.17878, "fx":[-9.27225,-10.05608,-10.44791,-9.70231], "fy":[-13.27377,-12.82781,-13.57991,-14.02526]}, - {"t":2.21859, "x":3.60799, "y":3.11133, "heading":1.04329, "vx":0.07908, "vy":0.10781, "omega":0.04222, "ax":-0.59175, "ay":-0.80572, "alpha":-0.21889, "fx":[-9.11658,-10.06608,-10.61148,-9.66237], "fy":[-13.22976,-12.73453,-13.6074,-14.15203]}, - {"t":2.25202, "x":3.6103, "y":3.11448, "heading":1.0447, "vx":0.0593, "vy":0.08087, "omega":0.0349, "ax":-0.59143, "ay":-0.80596, "alpha":-0.27781, "fx":[-8.93503,-10.13477,-10.7643,-9.60154], "fy":[-13.19436,-12.50421,-13.67599,-14.36519]}, - {"t":2.28546, "x":3.61195, "y":3.11674, "heading":1.04587, "vx":0.03952, "vy":0.05392, "omega":0.02561, "ax":-0.59114, "ay":-0.80619, "alpha":-0.34185, "fx":[-8.69487,-10.17055,-11.01226,-9.53818], "fy":[-13.1311,-12.33536,-13.72321,-14.56529]}, - {"t":2.31889, "x":3.61294, "y":3.11809, "heading":1.04672, "vx":0.01976, "vy":0.02696, "omega":0.01418, "ax":-0.59086, "ay":-0.8064, "alpha":-0.42416, "fx":[-8.42333,-10.22979,-11.25602,-9.48794], "fy":[-13.10726,-12.01458,-13.80105,-14.84648]}, - {"t":2.35233, "x":3.61327, "y":3.11854, "heading":1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.69577, "y":0.8236, "heading":0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.30159, "ay":3.75153, "alpha":0.362, "fx":[53.83024,55.37702,56.23537,54.70107], "fy":[62.48279,61.27748,62.58662,63.79797]}, + {"t":0.03981, "x":1.69839, "y":0.82657, "heading":0.94248, "vx":0.13145, "vy":0.14936, "omega":0.01441, "ax":3.30239, "ay":3.7524, "alpha":0.27794, "fx":[54.15258,55.17379,56.03869,54.83213], "fy":[62.50274,61.59998,62.57544,63.52482]}, + {"t":0.07963, "x":1.70624, "y":0.83549, "heading":0.94305, "vx":0.26293, "vy":0.29876, "omega":0.02548, "ax":3.30235, "ay":3.7523, "alpha":0.23277, "fx":[54.28981,55.31073,55.77224,54.82138], "fy":[62.58219,61.65145,62.63246,63.33017]}, + {"t":0.11944, "x":1.71933, "y":0.85036, "heading":0.94407, "vx":0.39441, "vy":0.44815, "omega":0.03475, "ax":3.30229, "ay":3.75219, "alpha":0.14965, "fx":[54.57406,55.05175,55.61794,54.94674], "fy":[62.51808,62.04921,62.55005,63.07117]}, + {"t":0.15925, "x":1.73765, "y":0.87118, "heading":0.94545, "vx":0.52588, "vy":0.59754, "omega":0.0407, "ax":3.30223, "ay":3.75205, "alpha":0.10801, "fx":[54.68689,55.17208,55.39291,54.93448], "fy":[62.54688,62.15231,62.56833,62.91164]}, + {"t":0.19907, "x":1.7612, "y":0.89794, "heading":0.94707, "vx":0.65736, "vy":0.74692, "omega":0.045, "ax":3.30215, "ay":3.75188, "alpha":0.06092, "fx":[54.81837,55.17777,55.22438,54.96061], "fy":[62.54356,62.33491,62.54639,62.74332]}, + {"t":0.23888, "x":1.78999, "y":0.93065, "heading":0.94886, "vx":0.78883, "vy":0.8963, "omega":0.04743, "ax":3.30206, "ay":3.75167, "alpha":0.00653, "fx":[54.98903,55.04626,55.11341,55.02638], "fy":[62.492,62.60377,62.49123,62.56744]}, + {"t":0.2787, "x":1.82401, "y":0.96931, "heading":0.95075, "vx":0.92029, "vy":1.04567, "omega":0.04769, "ax":3.30195, "ay":3.75142, "alpha":-0.00457, "fx":[54.99333,55.28623,54.92259,54.96533], "fy":[62.56013,62.53995,62.54053,62.49663]}, + {"t":0.31851, "x":1.86327, "y":1.01392, "heading":0.95265, "vx":1.05176, "vy":1.19502, "omega":0.04751, "ax":3.30179, "ay":3.75109, "alpha":-0.08152, "fx":[55.21674,54.9173,54.91937,55.1039], "fy":[62.40009,63.0703,62.38163,62.2634]}, + {"t":0.35832, "x":1.90776, "y":1.06447, "heading":0.95454, "vx":1.18321, "vy":1.34437, "omega":0.04426, "ax":3.3016, "ay":3.75064, "alpha":-0.04606, "fx":[55.06622,55.50951,54.65157,54.91722], "fy":[62.57592,62.64719,62.53923,62.32322]}, + {"t":0.39814, "x":1.95749, "y":1.12097, "heading":0.9563, "vx":1.31466, "vy":1.49369, "omega":0.04243, "ax":3.30133, "ay":3.75002, "alpha":-0.15278, "fx":[55.37443,54.79996,54.79036,55.16142], "fy":[62.28962,63.4902,62.25777,62.00639]}, + {"t":0.43795, "x":2.01244, "y":1.18341, "heading":0.95799, "vx":1.4461, "vy":1.643, "omega":0.03635, "ax":3.30091, "ay":3.74908, "alpha":-0.08143, "fx":[55.16813,55.47173,54.51989,54.93836], "fy":[62.55382,62.74877,62.50529,62.1738]}, + {"t":0.47776, "x":2.07264, "y":1.25179, "heading":0.95944, "vx":1.57752, "vy":1.79226, "omega":0.0331, "ax":3.30023, "ay":3.74749, "alpha":-0.12344, "fx":[55.39606,54.90459,54.63049,55.12191], "fy":[62.43871,62.97675,62.39904,62.06086]}, + {"t":0.51758, "x":2.13806, "y":1.32612, "heading":0.96076, "vx":1.70891, "vy":1.94146, "omega":0.02819, "ax":3.29901, "ay":3.74413, "alpha":-0.10944, "fx":[55.32143,54.96229,54.61569,55.07199], "fy":[62.43229,62.81825,62.37955,62.02159]}, + {"t":0.55739, "x":2.20871, "y":1.40638, "heading":0.96188, "vx":1.84026, "vy":2.09053, "omega":0.02383, "ax":3.2949, "ay":3.73404, "alpha":-0.12848, "fx":[55.26356,54.82507,54.58981,55.01891], "fy":[62.15318,62.89875,62.10947,61.81702]}, + {"t":0.5972, "x":2.28459, "y":1.49257, "heading":0.96283, "vx":1.97144, "vy":2.23919, "omega":0.01872, "ax":0.29439, "ay":0.27142, "alpha":-0.07285, "fx":[5.13197,4.88825,4.66786,4.94115], "fy":[4.57069,4.78888,4.47892,4.25935]}, + {"t":0.63702, "x":2.36331, "y":1.58194, "heading":0.96357, "vx":1.98316, "vy":2.25, "omega":0.01582, "ax":-0.05854, "ay":0.05156, "alpha":-0.01066, "fx":[-0.83092,-1.00107,-1.12055,-0.95065], "fy":[0.98003,0.68135,0.93852,0.83829]}, + {"t":0.67683, "x":2.44222, "y":1.67156, "heading":0.9642, "vx":1.98083, "vy":2.25205, "omega":0.01539, "ax":-0.0392, "ay":0.03446, "alpha":-0.00043, "fx":[-0.684,-0.56386,-0.68201,-0.68411], "fy":[0.57628,0.58897,0.57263,0.55995]}, + {"t":0.71664, "x":2.52106, "y":1.76125, "heading":0.96482, "vx":1.97927, "vy":2.25342, "omega":0.01537, "ax":-0.08845, "ay":0.07757, "alpha":0.02259, "fx":[-1.58831,-1.45315,-1.36056,-1.49571], "fy":[1.22733,1.29955,1.27307,1.37205]}, + {"t":0.75646, "x":2.59979, "y":1.85103, "heading":0.96543, "vx":1.97575, "vy":2.25651, "omega":0.01627, "ax":-0.33958, "ay":0.29512, "alpha":0.07034, "fx":[-5.85801,-5.76197,-5.36527,-5.65705], "fy":[4.87373,4.67235,4.96526,5.16695]}, + {"t":0.79627, "x":2.67818, "y":1.9411, "heading":0.96608, "vx":1.96223, "vy":2.26826, "omega":0.01907, "ax":-2.80515, "ay":-2.31419, "alpha":0.00352, "fx":[-47.14021,-46.69609,-46.35645,-46.84906], "fy":[-39.01615,-37.79364,-38.92271,-38.57308]}, + {"t":0.83609, "x":2.75408, "y":2.02958, "heading":0.96683, "vx":1.85055, "vy":2.17613, "omega":0.01921, "ax":-3.33152, "ay":-3.70291, "alpha":0.12272, "fx":[-55.86983,-55.687,-55.01241,-55.56989], "fy":[-61.75944,-62.15716,-61.70124,-61.28504]}, + {"t":0.8759, "x":2.82512, "y":2.11328, "heading":0.9676, "vx":1.71791, "vy":2.0287, "omega":0.0241, "ax":-3.31635, "ay":-3.72916, "alpha":0.13212, "fx":[-55.72295,-55.19713,-54.80047,-55.40727], "fy":[-62.20339,-62.59144,-62.14578,-61.71254]}, + {"t":0.91571, "x":2.89088, "y":2.19109, "heading":0.96856, "vx":1.58587, "vy":1.88023, "omega":0.02936, "ax":-3.31189, "ay":-3.73735, "alpha":0.12288, "fx":[-55.55205,-55.36197,-54.66806,-55.24865], "fy":[-62.34258,-62.70774,-62.29047,-61.8588]}, + {"t":0.95553, "x":2.9514, "y":2.26299, "heading":0.96973, "vx":1.45401, "vy":1.73143, "omega":0.03425, "ax":-3.30966, "ay":-3.74145, "alpha":0.1875, "fx":[-55.6286,-54.908,-54.79859,-55.3469], "fy":[-62.17607,-63.43931,-62.12392,-61.73333]}, + {"t":0.99534, "x":3.00667, "y":2.32896, "heading":0.97109, "vx":1.32224, "vy":1.58247, "omega":0.04172, "ax":-3.30833, "ay":-3.7439, "alpha":0.09943, "fx":[-55.39215,-55.4471,-54.62321,-55.1307], "fy":[-62.46047,-62.71766,-62.4163,-62.04172]}, + {"t":1.03515, "x":3.05669, "y":2.389, "heading":0.97275, "vx":1.19053, "vy":1.43341, "omega":0.04568, "ax":-3.30746, "ay":-3.74552, "alpha":0.14258, "fx":[-55.50507,-54.94188,-54.81444,-55.27355], "fy":[-62.32224,-63.19006,-62.27928,-61.95258]}, + {"t":1.07497, "x":3.10146, "y":2.4431, "heading":0.97457, "vx":1.05884, "vy":1.28429, "omega":0.05135, "ax":-3.30683, "ay":-3.74668, "alpha":0.08343, "fx":[-55.36791,-55.18505,-54.77223,-55.16811], "fy":[-62.48191,-62.73098,-62.44898,-62.15947]}, + {"t":1.11478, "x":3.141, "y":2.49126, "heading":0.97662, "vx":0.92719, "vy":1.13512, "omega":0.05468, "ax":-3.30637, "ay":-3.74754, "alpha":0.08491, "fx":[-55.36279,-55.01782,-54.87818,-55.20373], "fy":[-62.44167,-62.84487,-62.41259,-62.17969]}, + {"t":1.15459, "x":3.17529, "y":2.53348, "heading":0.97879, "vx":0.79555, "vy":0.98592, "omega":0.05806, "ax":-3.30601, "ay":-3.74822, "alpha":0.0547, "fx":[-55.2954,-55.02986,-54.93526,-55.17759], "fy":[-62.48592,-62.67184,-62.47,-62.29617]}, + {"t":1.19441, "x":3.20435, "y":2.56977, "heading":0.9811, "vx":0.66392, "vy":0.83669, "omega":0.06023, "ax":-0.65313, "ay":-0.75155, "alpha":0.04413, "fx":[-11.00681,-10.9542,-10.70319,-10.88537], "fy":[-12.5586,-12.68673,-12.49742,-12.36904]}, + {"t":1.22782, "x":3.22616, "y":2.5973, "heading":0.98312, "vx":0.64211, "vy":0.81158, "omega":0.06171, "ax":-0.64648, "ay":-0.76057, "alpha":0.04671, "fx":[-10.91695,-10.74791,-10.63609,-10.80491], "fy":[-12.6826,-12.87674,-12.63099,-12.52308]}, + {"t":1.26122, "x":3.24725, "y":2.62399, "heading":0.98518, "vx":0.62051, "vy":0.78617, "omega":0.06327, "ax":-0.64064, "ay":-0.76583, "alpha":0.03543, "fx":[-10.78372,-10.69888,-10.54527,-10.68868], "fy":[-12.79058,-12.89177,-12.74146,-12.64007]}, + {"t":1.29463, "x":3.26763, "y":2.64982, "heading":0.98729, "vx":0.59911, "vy":0.76059, "omega":0.06445, "ax":-0.63622, "ay":-0.76975, "alpha":0.03748, "fx":[-10.71854,-10.58212,-10.49244,-10.62865], "fy":[-12.83442,-12.99051,-12.79304,-12.70764]}, + {"t":1.32804, "x":3.28728, "y":2.6748, "heading":0.98944, "vx":0.57785, "vy":0.73487, "omega":0.0657, "ax":-0.63275, "ay":-0.77279, "alpha":0.02879, "fx":[-10.63405,-10.55499,-10.44349,-10.55829], "fy":[-12.90246,-12.98478,-12.86182,-12.77942]}, + {"t":1.36145, "x":3.30624, "y":2.69892, "heading":0.99164, "vx":0.55671, "vy":0.70906, "omega":0.06667, "ax":-0.62997, "ay":-0.77522, "alpha":0.02963, "fx":[-10.5946,-10.48185,-10.40795,-10.52061], "fy":[-12.92813,-13.04139,-12.89463,-12.82618]}, + {"t":1.39485, "x":3.32448, "y":2.72218, "heading":0.99387, "vx":0.53567, "vy":0.68316, "omega":0.06766, "ax":-0.62768, "ay":-0.7772, "alpha":0.02366, "fx":[-10.53433,-10.46283,-10.3815,-10.47367], "fy":[-12.97273,-13.04097,-12.93849,-12.87025]}, + {"t":1.42826, "x":3.34203, "y":2.74457, "heading":0.99613, "vx":0.5147, "vy":0.65719, "omega":0.06845, "ax":-0.62576, "ay":-0.77885, "alpha":0.02367, "fx":[-10.50896,-10.41488,-10.35336,-10.44742], "fy":[-12.98911,-13.07306,-12.96231,-12.90784]}, + {"t":1.46167, "x":3.35887, "y":2.76609, "heading":0.99841, "vx":0.49379, "vy":0.63117, "omega":0.06924, "ax":-0.62414, "ay":-0.78024, "alpha":0.01917, "fx":[-10.46069,-10.40138,-10.34099,-10.41317], "fy":[-13.02067,-13.07713,-12.99189,-12.93546]}, + {"t":1.49508, "x":3.37502, "y":2.78674, "heading":1.00073, "vx":0.47294, "vy":0.60511, "omega":0.06988, "ax":-0.62274, "ay":-0.78144, "alpha":0.01855, "fx":[-10.44447,-10.36741,-10.31706,-10.39414], "fy":[-13.03111,-13.09324,-13.01084,-12.96939]}, + {"t":1.52848, "x":3.39047, "y":2.80652, "heading":1.00306, "vx":0.45214, "vy":0.579, "omega":0.0705, "ax":-0.62153, "ay":-0.78247, "alpha":0.01462, "fx":[-10.4014,-10.35877,-10.31505,-10.36696], "fy":[-13.05492,-13.09984,-13.03174,-12.98686]}, + {"t":1.56189, "x":3.40523, "y":2.82542, "heading":1.00542, "vx":0.43137, "vy":0.55286, "omega":0.07099, "ax":-0.62046, "ay":-0.78337, "alpha":0.01331, "fx":[-10.39134,-10.33273,-10.29427,-10.35291], "fy":[-13.06098,-13.10392,-13.04806,-13.0205]}, + {"t":1.5953, "x":3.4193, "y":2.84345, "heading":1.00779, "vx":0.41065, "vy":0.52669, "omega":0.07143, "ax":-0.61952, "ay":-0.78416, "alpha":0.00933, "fx":[-10.3494,-10.32808,-10.3013,-10.32978], "fy":[-13.07974,-13.1114,-13.06345,-13.03184]}, + {"t":1.62871, "x":3.43267, "y":2.86061, "heading":1.01017, "vx":0.38995, "vy":0.50049, "omega":0.07174, "ax":-0.61869, "ay":-0.78487, "alpha":0.00702, "fx":[-10.34279,-10.3074,-10.28356,-10.319], "fy":[-13.08242,-13.10433,-13.07884,-13.0679]}, + {"t":1.66211, "x":3.44535, "y":2.8769, "heading":1.01257, "vx":0.36928, "vy":0.47427, "omega":0.07198, "ax":-0.61794, "ay":-0.7855, "alpha":0.00247, "fx":[-10.29914,-10.30605,-10.2995,-10.29807], "fy":[-13.09724,-13.11172,-13.09052,-13.07609]}, + {"t":1.69552, "x":3.45734, "y":2.8923, "heading":1.01498, "vx":0.34864, "vy":0.44803, "omega":0.07206, "ax":-0.61726, "ay":-0.78607, "alpha":-0.00123, "fx":[-10.29317,-10.28976,-10.28566,-10.28912], "fy":[-13.09681,-13.09397,-13.106,-13.11661]}, + {"t":1.72893, "x":3.46865, "y":2.90683, "heading":1.01738, "vx":0.32802, "vy":0.42177, "omega":0.07202, "ax":-0.61665, "ay":-0.78658, "alpha":-0.00697, "fx":[-10.24538,-10.29161,-10.31094,-10.26898], "fy":[-13.1082,-13.09892,-13.11557,-13.1249]}, + {"t":1.76234, "x":3.47926, "y":2.92048, "heading":1.01979, "vx":0.30742, "vy":0.39549, "omega":0.07178, "ax":-0.61609, "ay":-0.78705, "alpha":-0.01255, "fx":[-10.23681,-10.27932,-10.30304,-10.26059], "fy":[-13.1044,-13.07101,-13.13173,-13.17154]}, + {"t":1.79574, "x":3.48919, "y":2.93325, "heading":1.02219, "vx":0.28683, "vy":0.3692, "omega":0.07137, "ax":-0.61558, "ay":-0.78747, "alpha":-0.02035, "fx":[-10.18232,-10.28556,-10.33821,-10.23976], "fy":[-13.11253,-13.0695,-13.14098,-13.18404]}, + {"t":1.82915, "x":3.49843, "y":2.94515, "heading":1.02457, "vx":0.26627, "vy":0.34289, "omega":0.07069, "ax":-0.61512, "ay":-0.78786, "alpha":-0.02847, "fx":[-10.16732,-10.27651,-10.34,-10.23088], "fy":[-13.10478,-13.03159,-13.15825,-13.23841]}, + {"t":1.86256, "x":3.50698, "y":2.95616, "heading":1.02693, "vx":0.24572, "vy":0.31657, "omega":0.06973, "ax":-0.61469, "ay":-0.78822, "alpha":-0.03943, "fx":[-10.10357,-10.28842,-10.38604,-10.20802], "fy":[-13.10927,-13.018,-13.16919,-13.26046]}, + {"t":1.89597, "x":3.51484, "y":2.9663, "heading":1.02926, "vx":0.22518, "vy":0.29024, "omega":0.06842, "ax":-0.61429, "ay":-0.78855, "alpha":-0.05107, "fx":[-10.07679,-10.28263,-10.40297,-10.1972], "fy":[-13.09694,-12.9695,-13.18809,-13.32443]}, + {"t":1.92937, "x":3.52202, "y":2.97556, "heading":1.03155, "vx":0.20466, "vy":0.2639, "omega":0.06671, "ax":-0.61392, "ay":-0.78886, "alpha":-0.06658, "fx":[-10.00144,-10.3006,-10.46163,-10.17139], "fy":[-13.09663,-12.93666,-13.20307,-13.36301]}, + {"t":1.96278, "x":3.52852, "y":2.98393, "heading":1.03378, "vx":0.18415, "vy":0.23754, "omega":0.06449, "ax":-0.61358, "ay":-0.78914, "alpha":-0.08327, "fx":[-9.95499,-10.29993,-10.50108,-10.15626], "fy":[-13.07942,-12.87509,-13.22463,-13.43916]}, + {"t":1.99619, "x":3.53433, "y":2.99143, "heading":1.03593, "vx":0.16365, "vy":0.21118, "omega":0.0617, "ax":-0.61326, "ay":-0.7894, "alpha":-0.10498, "fx":[-9.86579,-10.32351,-10.57488,-10.12687], "fy":[-13.07184,-12.81459,-13.24616,-13.50335]}, + {"t":2.0296, "x":3.53945, "y":2.99804, "heading":1.03799, "vx":0.14317, "vy":0.18481, "omega":0.0582, "ax":-0.61296, "ay":-0.78965, "alpha":-0.12888, "fx":[-9.78843,-10.33182,-10.64706,-10.10392], "fy":[-13.04962,-12.73603,-13.27181,-13.59492]}, + {"t":2.063, "x":3.54389, "y":3.00378, "heading":1.03994, "vx":0.12269, "vy":0.15843, "omega":0.05389, "ax":-0.61269, "ay":-0.78988, "alpha":-0.15897, "fx":[-9.68272,-10.36079,-10.73894,-10.07023], "fy":[-13.03091,-12.63679,-13.30305,-13.69702]}, + {"t":2.09641, "x":3.54765, "y":3.00863, "heading":1.04174, "vx":0.10222, "vy":0.13204, "omega":0.04858, "ax":-0.61242, "ay":-0.7901, "alpha":-0.19287, "fx":[-9.55903,-10.38304,-10.85837,-10.03484], "fy":[-13.00307,-12.53795,-13.33387,-13.80731]}, + {"t":2.12982, "x":3.55072, "y":3.0126, "heading":1.04336, "vx":0.08176, "vy":0.10564, "omega":0.04214, "ax":-0.61218, "ay":-0.7903, "alpha":-0.23443, "fx":[-9.43342,-10.41831,-10.97141,-9.99579], "fy":[-12.9684,-12.38285,-13.37962,-13.96486]}, + {"t":2.16323, "x":3.55311, "y":3.01569, "heading":1.04477, "vx":0.06131, "vy":0.07924, "omega":0.03431, "ax":-0.61195, "ay":-0.79049, "alpha":-0.28194, "fx":[-9.24258,-10.46,-11.15872,-9.94224], "fy":[-12.93405,-12.26056,-13.41646,-14.09739]}, + {"t":2.19663, "x":3.55482, "y":3.01789, "heading":1.04591, "vx":0.04087, "vy":0.05283, "omega":0.02489, "ax":-0.61173, "ay":-0.79067, "alpha":-0.33921, "fx":[-9.09329,-10.50222,-11.29664,-9.89687], "fy":[-12.87713,-12.02511,-13.48341,-14.33482]}, + {"t":2.23004, "x":3.55584, "y":3.01922, "heading":1.04674, "vx":0.02043, "vy":0.02642, "omega":0.01356, "ax":-0.61152, "ay":-0.79084, "alpha":-0.40575, "fx":[-8.87914,-10.55685,-11.50373,-9.83558], "fy":[-12.81796,-11.79612,-13.54837,-14.56937]}, + {"t":2.26345, "x":3.55619, "y":3.01966, "heading":1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/fetchPToD.traj b/src/main/deploy/choreo/fetchPToD.traj index 1f675ccb..cea2d6e9 100644 --- a/src/main/deploy/choreo/fetchPToD.traj +++ b/src/main/deploy/choreo/fetchPToD.traj @@ -3,30 +3,30 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":1.69577419757843, "y":0.8236, "heading":0.942477796076938, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.427126884460449, "y":2.2866694927215576, "heading":0.0, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":1.6421304941177368, "y":0.8500130772590637, "heading":0.942477796076938, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.51680064201355, "y":2.4113705158233643, "heading":0.0, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":3.84197363509461, "y":2.854658009736421, "heading":1.0471975511965976, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":3.0}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":3.0}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":true}], + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":3.3}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":3.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"fetchP.x", "val":1.69577419757843}, "y":{"exp":"fetchP.y", "val":0.8236}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.427126884460449 m", "val":3.427126884460449}, "y":{"exp":"2.2866694927215576 m", "val":2.2866694927215576}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"fetchP.x", "val":1.6421304941177368}, "y":{"exp":"fetchP.y", "val":0.8500130772590637}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.51680064201355 m", "val":3.51680064201355}, "y":{"exp":"2.4113705158233643 m", "val":2.4113705158233643}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"D.x", "val":3.84197363509461}, "y":{"exp":"D.y", "val":2.854658009736421}, "heading":{"exp":"D.heading", "val":1.0471975511965976}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3 m / s", "val":3.0}}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"maxAccel", "val":3.0}}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":true}], + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.3 m / s", "val":3.3}}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"3 m / s ^ 2", "val":3.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -34,82 +34,56 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.43842,2.62551], + "waypoints":[0.0,1.07465,1.68059], "samples":[ - {"t":0.0, "x":1.69577, "y":0.8236, "heading":0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.41303, "ay":1.77985, "alpha":5.12422, "fx":[22.84274,44.32899,56.86126,36.86311], "fy":[28.06175,11.65227,31.60155,47.36142]}, - {"t":0.03888, "x":1.6976, "y":0.82494, "heading":0.94248, "vx":0.09381, "vy":0.06919, "omega":0.19921, "ax":2.41285, "ay":1.78136, "alpha":3.36995, "fx":[28.86825,42.85609,51.24648,37.91325], "fy":[28.61595,17.88424,30.91548,41.36182]}, - {"t":0.07775, "x":1.70307, "y":0.82898, "heading":0.95022, "vx":0.18761, "vy":0.13845, "omega":0.33022, "ax":2.41194, "ay":1.78252, "alpha":2.01761, "fx":[33.46787,41.7774,46.84812,38.73038], "fy":[28.98352,22.67373,30.48409,36.71337]}, - {"t":0.11663, "x":1.71218, "y":0.83571, "heading":0.96306, "vx":0.28138, "vy":0.20774, "omega":0.40866, "ax":2.41093, "ay":1.78381, "alpha":0.98221, "fx":[36.94696,40.96541,43.42271,39.4214], "fy":[29.31101,26.31326,30.16187,33.15469]}, - {"t":0.1555, "x":1.72494, "y":0.84513, "heading":0.97895, "vx":0.37511, "vy":0.27709, "omega":0.44684, "ax":2.4098, "ay":1.78525, "alpha":0.21553, "fx":[39.48401,40.31403,40.87917,40.00395], "fy":[29.63067,29.00289,29.87999,30.52365]}, - {"t":0.19438, "x":1.74135, "y":0.85726, "heading":0.99632, "vx":0.46879, "vy":0.34649, "omega":0.45522, "ax":2.40853, "ay":1.78688, "alpha":-0.33156, "fx":[41.26062,39.78107,39.07085,40.48372], "fy":[29.93739,30.92312,29.62635,28.65893]}, - {"t":0.23326, "x":1.76139, "y":0.87208, "heading":1.01402, "vx":0.56242, "vy":0.41596, "omega":0.44233, "ax":2.40709, "ay":1.78872, "alpha":-0.7016, "fx":[42.43393,39.34555,37.86289,40.85798], "fy":[30.21523,32.22857,29.40863,27.41586]}, - {"t":0.27213, "x":1.78508, "y":0.8896, "heading":1.03121, "vx":0.656, "vy":0.4855, "omega":0.41506, "ax":2.40546, "ay":1.7908, "alpha":-0.93229, "fx":[43.13965,39.00147,37.12531,41.12504], "fy":[30.45067,33.05366,29.2393,26.66356]}, - {"t":0.31101, "x":1.8124, "y":0.90983, "heading":1.04735, "vx":0.74952, "vy":0.55512, "omega":0.37881, "ax":2.40359, "ay":1.79319, "alpha":-1.05705, "fx":[43.4959,38.73879,36.73982,41.29213], "fy":[30.63701,33.5164,29.12712,26.28569]}, - {"t":0.34988, "x":1.84335, "y":0.93276, "heading":1.06207, "vx":0.84296, "vy":0.62483, "omega":0.33772, "ax":2.40143, "ay":1.79593, "alpha":-1.10129, "fx":[43.5923,38.53628,36.62348,41.37082], "fy":[30.77293,33.70829,29.07459,26.1932]}, - {"t":0.38876, "x":1.87794, "y":0.95841, "heading":1.0752, "vx":0.93632, "vy":0.69465, "omega":0.2949, "ax":2.39893, "ay":1.79911, "alpha":-1.08905, "fx":[43.50479,38.40106,36.67784,41.37217], "fy":[30.8655,33.71255,29.08061,26.3022]}, - {"t":0.42764, "x":1.91615, "y":0.98678, "heading":1.08667, "vx":1.02958, "vy":0.76459, "omega":0.25257, "ax":2.39598, "ay":1.80283, "alpha":-1.03432, "fx":[43.27106,38.35517,36.83815,41.29507], "fy":[30.92469,33.58147,29.14216,26.56102]}, - {"t":0.46651, "x":1.95799, "y":1.01786, "heading":1.09649, "vx":1.12272, "vy":0.83468, "omega":0.21236, "ax":2.39247, "ay":1.80726, "alpha":-0.95492, "fx":[42.95435,38.34194,37.05928,41.16986], "fy":[30.96063,33.37861,29.25065,26.91455]}, - {"t":0.50539, "x":2.00344, "y":1.05168, "heading":1.10474, "vx":1.21573, "vy":0.90494, "omega":0.17523, "ax":2.38823, "ay":1.81258, "alpha":-0.85955, "fx":[42.57832,38.35158,37.31035,41.00267], "fy":[30.98591,33.13778,29.40049,27.33502]}, - {"t":0.54427, "x":2.05251, "y":1.08823, "heading":1.11155, "vx":1.30858, "vy":0.9754, "omega":0.14182, "ax":2.38301, "ay":1.8191, "alpha":-0.75771, "fx":[42.1678,38.36957,37.55525,40.8021], "fy":[31.01539,32.89612,29.58734,27.79539]}, - {"t":0.58314, "x":2.10518, "y":1.12752, "heading":1.11707, "vx":1.40122, "vy":1.04612, "omega":0.11236, "ax":2.37644, "ay":1.82726, "alpha":-0.65503, "fx":[41.73804,38.35936,37.78285,40.57638], "fy":[31.06219,32.67981,29.81038,28.28579]}, - {"t":0.62202, "x":2.16145, "y":1.16957, "heading":1.12144, "vx":1.49361, "vy":1.11716, "omega":0.08689, "ax":2.36791, "ay":1.83777, "alpha":-0.55821, "fx":[41.30579,38.2664,37.98007,40.33542], "fy":[31.14312,32.52302,30.07315,28.79963]}, - {"t":0.66089, "x":2.22131, "y":1.21439, "heading":1.12481, "vx":1.58566, "vy":1.18861, "omega":0.06519, "ax":2.35643, "ay":1.85176, "alpha":-0.46393, "fx":[40.81489,38.21783,38.07131,40.01816], "fy":[31.28872,32.43514,30.39587,29.35195]}, - {"t":0.69977, "x":2.28473, "y":1.262, "heading":1.12735, "vx":1.67727, "vy":1.26059, "omega":0.04716, "ax":2.34012, "ay":1.87134, "alpha":-0.37471, "fx":[40.24076,38.18052,38.01643,39.597], "fy":[31.53829,32.46041,30.80979,29.96904]}, - {"t":0.73865, "x":2.35171, "y":1.31242, "heading":1.12918, "vx":1.76825, "vy":1.33335, "omega":0.03259, "ax":2.31525, "ay":1.90057, "alpha":-0.29448, "fx":[39.54756,38.01384,37.77821,39.03656], "fy":[31.95765,32.67868,31.37705,30.71316]}, - {"t":0.77752, "x":2.4222, "y":1.36569, "heading":1.13045, "vx":1.85826, "vy":1.40723, "omega":0.02114, "ax":2.27278, "ay":1.94878, "alpha":-0.22464, "fx":[38.59196,37.54902,37.21036,38.19333], "fy":[32.70267,33.25005,32.25071,31.73729]}, - {"t":0.8164, "x":2.49616, "y":1.42187, "heading":1.13127, "vx":1.94661, "vy":1.48299, "omega":0.01241, "ax":2.18376, "ay":2.04348, "alpha":-0.16983, "fx":[36.92334,36.20544,35.86317,36.61702], "fy":[34.2271,34.64969,33.88307,33.49537]}, - {"t":0.85527, "x":2.57349, "y":1.48107, "heading":1.13175, "vx":2.03151, "vy":1.56244, "omega":0.00581, "ax":1.88598, "ay":2.30908, "alpha":-0.12735, "fx":[31.80685,31.40437,30.97824,31.56426], "fy":[38.61409,38.94264,38.3488,38.05958]}, - {"t":0.89415, "x":2.65389, "y":1.54356, "heading":1.13198, "vx":2.10483, "vy":1.6522, "omega":0.00086, "ax":-2.19913, "ay":1.93486, "alpha":-0.13051, "fx":[-36.0687,-37.4986,-36.76661,-36.30002], "fy":[32.39824,32.58731,32.12045,31.90682]}, - {"t":0.93303, "x":2.73405, "y":1.60925, "heading":1.13201, "vx":2.01933, "vy":1.72742, "omega":-0.00422, "ax":-2.75961, "ay":-1.13353, "alpha":-0.10669, "fx":[-45.59586,-46.44742,-46.18927,-45.77283], "fy":[-18.78568,-18.57357,-19.00134,-19.22124]}, - {"t":0.9719, "x":2.81047, "y":1.67555, "heading":1.13185, "vx":1.91205, "vy":1.68336, "omega":-0.00837, "ax":-2.62194, "ay":-1.43983, "alpha":-0.10615, "fx":[-43.30014,-44.19012,-43.86938,-43.46615], "fy":[-23.90967,-23.66787,-24.10723,-24.3206]}, - {"t":1.01078, "x":2.88282, "y":1.7399, "heading":1.13152, "vx":1.81012, "vy":1.62738, "omega":-0.01249, "ax":-2.56548, "ay":-1.54369, "alpha":-0.09971, "fx":[-42.41867,-43.06127,-42.99587,-42.58534], "fy":[-25.63751,-25.40935,-25.83294,-26.05017]}, - {"t":1.04965, "x":2.95126, "y":1.802, "heading":1.13104, "vx":1.71039, "vy":1.56737, "omega":-0.01637, "ax":-2.53511, "ay":-1.59578, "alpha":-0.09556, "fx":[-41.95897,-42.39189,-42.55531,-42.13016], "fy":[-26.49893,-26.28524,-26.69728,-26.92241]}, - {"t":1.08853, "x":3.01583, "y":1.86173, "heading":1.1304, "vx":1.61183, "vy":1.50533, "omega":-0.02008, "ax":-2.51623, "ay":-1.627, "alpha":-0.09371, "fx":[-41.67336,-41.96331,-42.29106,-41.84992], "fy":[-27.0132,-26.80818,-27.21523,-27.44894]}, - {"t":1.12741, "x":3.07659, "y":1.91902, "heading":1.12962, "vx":1.51401, "vy":1.44208, "omega":-0.02373, "ax":-2.50336, "ay":-1.64782, "alpha":-0.09304, "fx":[-41.47437,-41.68199,-42.10765,-41.6549], "fy":[-27.35668,-27.15544,-27.56077,-27.8008]}, - {"t":1.16628, "x":3.13356, "y":1.97384, "heading":1.1287, "vx":1.41669, "vy":1.37802, "omega":-0.02734, "ax":-2.49401, "ay":-1.66269, "alpha":-0.08845, "fx":[-41.35999,-41.38909,-42.00379,-41.54307], "fy":[-27.59892,-27.41537,-27.80316,-28.04765]}, - {"t":1.20516, "x":3.18675, "y":2.02615, "heading":1.12763, "vx":1.31973, "vy":1.31338, "omega":-0.03078, "ax":-2.48694, "ay":-1.67382, "alpha":-0.08855, "fx":[-41.24008,-41.278,-41.8835,-41.42286], "fy":[-27.78656,-27.60068,-27.98763,-28.23219]}, - {"t":1.24403, "x":3.23618, "y":2.07595, "heading":1.12644, "vx":1.22305, "vy":1.24831, "omega":-0.03423, "ax":-2.48139, "ay":-1.68247, "alpha":-0.08724, "fx":[-41.1518,-41.18121,-41.78891,-41.33257], "fy":[-27.93378,-27.74932,-28.12937,-28.37171]}, - {"t":1.28291, "x":3.28185, "y":2.12321, "heading":1.12511, "vx":1.12658, "vy":1.1829, "omega":-0.03762, "ax":-2.47693, "ay":-1.68939, "alpha":-0.082, "fx":[-41.10595,-41.03986,-41.72884,-41.28248], "fy":[-28.05045,-27.88093,-28.23812,-28.47549]}, - {"t":1.32179, "x":3.32378, "y":2.16792, "heading":1.12364, "vx":1.03029, "vy":1.11722, "omega":-0.0408, "ax":-2.47326, "ay":-1.69505, "alpha":-0.07505, "fx":[-41.08152,-40.89459,-41.68391,-41.25192], "fy":[-28.1469,-27.9974,-28.32419,-28.5543]}, - {"t":1.36066, "x":3.36196, "y":2.21007, "heading":1.12206, "vx":0.93414, "vy":1.05133, "omega":-0.04372, "ax":-2.47019, "ay":-1.69975, "alpha":-0.06515, "fx":[-41.08083,-40.73546,-41.64992,-41.24143], "fy":[-28.22801,-28.10772,-28.39113,-28.60943]}, - {"t":1.39954, "x":3.39641, "y":2.24965, "heading":1.12036, "vx":0.83811, "vy":0.98525, "omega":-0.04625, "ax":-2.46758, "ay":-1.70374, "alpha":0.01148, "fx":[-41.57573,-39.16717,-42.07558,-41.71523], "fy":[-28.24193,-28.40423,-28.38029,-28.57566]}, - {"t":1.43842, "x":3.42713, "y":2.28667, "heading":1.11856, "vx":0.74218, "vy":0.91901, "omega":-0.04581, "ax":-0.80541, "ay":-0.58768, "alpha":-0.06972, "fx":[-13.18725,-13.53862,-13.64321,-13.3338], "fy":[-9.72158,-9.5775,-9.87087,-10.01537]}, - {"t":1.47139, "x":3.45116, "y":2.31665, "heading":1.11705, "vx":0.71562, "vy":0.89963, "omega":-0.04811, "ax":-0.78024, "ay":-0.62332, "alpha":-0.06014, "fx":[-12.81928,-13.02493,-13.22928,-12.95133], "fy":[-10.32306,-10.19337,-10.45707,-10.58808]}, - {"t":1.50436, "x":3.47434, "y":2.34598, "heading":1.11546, "vx":0.68989, "vy":0.87908, "omega":-0.05009, "ax":-0.75727, "ay":-0.65121, "alpha":-0.05489, "fx":[-12.45625,-12.62616,-12.83293,-12.57793], "fy":[-10.79348,-10.67373,-10.91648,-11.03771]}, - {"t":1.53734, "x":3.49667, "y":2.37461, "heading":1.11381, "vx":0.66492, "vy":0.85761, "omega":-0.0519, "ax":-0.73721, "ay":-0.67398, "alpha":-0.05105, "fx":[-12.13598,-12.2823,-12.48777,-12.24997], "fy":[-11.17714,-11.06473,-11.29183,-11.40581]}, - {"t":1.57031, "x":3.5182, "y":2.40253, "heading":1.1121, "vx":0.64061, "vy":0.83538, "omega":-0.05358, "ax":-0.71963, "ay":-0.69285, "alpha":-0.04812, "fx":[-11.85337,-11.98252,-12.18601,-11.96152], "fy":[-11.49491,-11.38805,-11.60312,-11.71159]}, - {"t":1.60329, "x":3.53893, "y":2.4297, "heading":1.11033, "vx":0.61688, "vy":0.81253, "omega":-0.05517, "ax":-0.70413, "ay":-0.7087, "alpha":-0.04581, "fx":[-11.60309,-11.71981,-11.92043,-11.70662], "fy":[-11.76177,-11.6593,-11.8647,-11.96879]}, - {"t":1.63626, "x":3.55889, "y":2.4561, "heading":1.10851, "vx":0.59366, "vy":0.78917, "omega":-0.05668, "ax":-0.69039, "ay":-0.72217, "alpha":-0.04397, "fx":[-11.37997,-11.48993,-11.6846,-11.47969], "fy":[-11.98865,-11.88976,-12.0871,-12.18757]}, - {"t":1.66924, "x":3.57809, "y":2.48173, "heading":1.10665, "vx":0.5709, "vy":0.76535, "omega":-0.05813, "ax":-0.67816, "ay":-0.73375, "alpha":-0.04235, "fx":[-11.18118,-11.28498,-11.47466,-11.27759], "fy":[-12.18367,-12.08794,-12.2781,-12.3754]}, - {"t":1.70221, "x":3.59655, "y":2.50657, "heading":1.10473, "vx":0.54853, "vy":0.74116, "omega":-0.05953, "ax":-0.6672, "ay":-0.74379, "alpha":-0.04089, "fx":[-11.00295,-11.10236,-11.28627,-11.09635], "fy":[-12.35296,-12.2601,-12.44367,-12.53805]}, - {"t":1.73519, "x":3.61427, "y":2.53061, "heading":1.10277, "vx":0.52653, "vy":0.71663, "omega":-0.06088, "ax":-0.65735, "ay":-0.75258, "alpha":-0.03949, "fx":[-10.84263,-10.93869,-11.11618,-10.93313], "fy":[-12.50124,-12.41118,-12.58833,-12.67986]}, - {"t":1.76816, "x":3.63128, "y":2.55383, "heading":1.10076, "vx":0.50486, "vy":0.69181, "omega":-0.06218, "ax":-0.64844, "ay":-0.76032, "alpha":-0.03808, "fx":[-10.69829,-10.79016,-10.96212,-10.78589], "fy":[-12.63215,-12.54492,-12.71564,-12.80429]}, - {"t":1.80114, "x":3.64757, "y":2.57623, "heading":1.09871, "vx":0.48347, "vy":0.66674, "omega":-0.06343, "ax":-0.64035, "ay":-0.7672, "alpha":-0.03663, "fx":[-10.56763,-10.65577,-10.82142,-10.65221], "fy":[-12.74859,-12.66435,-12.82837,-12.91398]}, - {"t":1.83411, "x":3.66316, "y":2.5978, "heading":1.09662, "vx":0.46236, "vy":0.64144, "omega":-0.06464, "ax":-0.63297, "ay":-0.77334, "alpha":-0.03508, "fx":[-10.44917,-10.53337,-10.6923,-10.53049], "fy":[-12.85286,-12.77185,-12.92875,-13.01108]}, - {"t":1.86709, "x":3.67807, "y":2.61853, "heading":1.09448, "vx":0.44149, "vy":0.61594, "omega":-0.0658, "ax":-0.62622, "ay":-0.77885, "alpha":-0.03338, "fx":[-10.34183,-10.42065,-10.5734,-10.41957], "fy":[-12.94683,-12.86942,-13.01856,-13.09725]}, - {"t":1.90006, "x":3.69228, "y":2.63842, "heading":1.09232, "vx":0.42084, "vy":0.59026, "omega":-0.0669, "ax":-0.62003, "ay":-0.78382, "alpha":-0.03149, "fx":[-10.24422,-10.31726,-10.46298,-10.31793], "fy":[-13.03204,-12.95869,-13.09926,-13.17383]}, - {"t":1.93304, "x":3.70582, "y":2.65745, "heading":1.09011, "vx":0.40039, "vy":0.56442, "omega":-0.06794, "ax":-0.61432, "ay":-0.78834, "alpha":-0.02933, "fx":[-10.15577,-10.22111,-10.36013,-10.22489], "fy":[-13.10974,-13.04107,-13.17199,-13.24186]}, - {"t":1.96601, "x":3.71869, "y":2.67564, "heading":1.08787, "vx":0.38014, "vy":0.53842, "omega":-0.0689, "ax":-0.60905, "ay":-0.79244, "alpha":-0.02686, "fx":[-10.07551,-10.13214,-10.2634,-10.13929], "fy":[-13.18103,-13.11779,-13.23773,-13.30212]}, - {"t":1.99899, "x":3.7309, "y":2.69296, "heading":1.0856, "vx":0.36005, "vy":0.51229, "omega":-0.06979, "ax":-0.60417, "ay":-0.7962, "alpha":-0.02401, "fx":[-10.00304,-10.04904,-10.17193,-10.06059], "fy":[-13.24681,-13.18996,-13.29724,-13.35522]}, - {"t":2.03196, "x":3.74244, "y":2.70942, "heading":1.0833, "vx":0.34013, "vy":0.48603, "omega":-0.07058, "ax":-0.59963, "ay":-0.79965, "alpha":-0.02065, "fx":[-9.93802,-9.97112,-10.08465,-9.98817], "fy":[-13.30789,-13.25861,-13.35114,-13.40151]}, - {"t":2.06494, "x":3.75333, "y":2.72501, "heading":1.08097, "vx":0.32036, "vy":0.45967, "omega":-0.07126, "ax":-0.5954, "ay":-0.80283, "alpha":-0.01669, "fx":[-9.88022,-9.89768,-10.00064,-9.92155], "fy":[-13.36496,-13.32473,-13.39995,-13.44124]}, - {"t":2.09791, "x":3.76357, "y":2.73973, "heading":1.07862, "vx":0.30072, "vy":0.43319, "omega":-0.07181, "ax":-0.59145, "ay":-0.80576, "alpha":-0.01196, "fx":[-9.82968,-9.82805,-9.91884,-9.86039], "fy":[-13.41867,-13.38935,-13.44403,-13.47439]}, - {"t":2.13089, "x":3.77317, "y":2.75358, "heading":1.07625, "vx":0.28122, "vy":0.40662, "omega":-0.07221, "ax":-0.58776, "ay":-0.80848, "alpha":-0.00629, "fx":[-9.78648,-9.76171,-9.83822,-9.80434], "fy":[-13.46961,-13.45349,-13.48368,-13.50082]}, - {"t":2.16386, "x":3.78212, "y":2.76655, "heading":1.07387, "vx":0.26184, "vy":0.37996, "omega":-0.07241, "ax":-0.5843, "ay":-0.811, "alpha":0.00055, "fx":[-9.75105,-9.69801,-9.7576,-9.75327], "fy":[-13.51837,-13.51831,-13.51907,-13.52013]}, - {"t":2.19683, "x":3.79044, "y":2.77864, "heading":1.07148, "vx":0.24257, "vy":0.35322, "omega":-0.0724, "ax":-0.58105, "ay":-0.81335, "alpha":0.00884, "fx":[-9.72397,-9.63632,-9.67573,-9.70707], "fy":[-13.56551,-13.58506,-13.55029,-13.53172]}, - {"t":2.22981, "x":3.79812, "y":2.78984, "heading":1.06909, "vx":0.22341, "vy":0.3264, "omega":-0.07211, "ax":-0.57799, "ay":-0.81554, "alpha":0.01894, "fx":[-9.70612,-9.57611,-9.59106,-9.66574], "fy":[-13.61161,-13.65523,-13.57733,-13.53467]}, - {"t":2.26278, "x":3.80517, "y":2.80016, "heading":1.06672, "vx":0.20435, "vy":0.29951, "omega":-0.07148, "ax":-0.5751, "ay":-0.8176, "alpha":0.03128, "fx":[-9.69867,-9.51674,-9.50186,-9.62937], "fy":[-13.65729,-13.73052,-13.60008,-13.5278]}, - {"t":2.29576, "x":3.8116, "y":2.80959, "heading":1.06436, "vx":0.18539, "vy":0.27255, "omega":-0.07045, "ax":-0.57238, "ay":-0.81952, "alpha":0.04644, "fx":[-9.70329,-9.45736,-9.40604,-9.59828], "fy":[-13.70323,-13.81299,-13.61829,-13.50946]}, - {"t":2.32873, "x":3.8174, "y":2.81813, "heading":1.06204, "vx":0.16652, "vy":0.24553, "omega":-0.06892, "ax":-0.5698, "ay":-0.82133, "alpha":0.06508, "fx":[-9.72201,-9.39729,-9.30106,-9.57278], "fy":[-13.75019,-13.9051,-13.63159,-13.47758]}, - {"t":2.36171, "x":3.82258, "y":2.82578, "heading":1.05976, "vx":0.14773, "vy":0.21844, "omega":-0.06677, "ax":-0.56736, "ay":-0.82303, "alpha":0.08808, "fx":[-9.75758,-9.33551,-9.18381,-9.55349], "fy":[-13.79905,-14.00991,-13.63944,-13.42944]}, - {"t":2.39468, "x":3.82714, "y":2.83254, "heading":1.05756, "vx":0.12902, "vy":0.1913, "omega":-0.06387, "ax":-0.56504, "ay":-0.82463, "alpha":0.11652, "fx":[-9.81351,-9.27071,-9.05057,-9.54124], "fy":[-13.85085,-14.13114,-13.6411,-13.36163]}, - {"t":2.42766, "x":3.83109, "y":2.8384, "heading":1.05546, "vx":0.11039, "vy":0.16411, "omega":-0.06003, "ax":-0.56284, "ay":-0.82614, "alpha":0.15175, "fx":[-9.89406,-9.20195,-8.89645,-9.53693], "fy":[-13.90685,-14.27341,-13.63561,-13.26979]}, - {"t":2.46063, "x":3.83442, "y":2.84336, "heading":1.05348, "vx":0.09183, "vy":0.13687, "omega":-0.05502, "ax":-0.56075, "ay":-0.82757, "alpha":0.19546, "fx":[-10.00487,-9.12744,-8.71564,-9.54201], "fy":[-13.96858,-14.44244,-13.62167,-13.14841]}, - {"t":2.49361, "x":3.83715, "y":2.84742, "heading":1.05166, "vx":0.07334, "vy":0.10958, "omega":-0.04858, "ax":-0.55876, "ay":-0.82893, "alpha":0.24981, "fx":[-10.15302,-9.04492,-8.50091,-9.55834], "fy":[-14.03793,-14.64539,-13.59763,-12.99056]}, - {"t":2.52658, "x":3.83926, "y":2.85059, "heading":1.05006, "vx":0.05491, "vy":0.08225, "omega":-0.04034, "ax":-0.55686, "ay":-0.83022, "alpha":0.31746, "fx":[-10.34734,-8.95204,-8.24303,-9.58822], "fy":[-14.11727,-14.89115,-13.56132,-12.7875]}, - {"t":2.55956, "x":3.84077, "y":2.85285, "heading":1.04873, "vx":0.03655, "vy":0.05487, "omega":-0.02987, "ax":-0.55505, "ay":-0.83144, "alpha":0.40177, "fx":[-10.59894,-8.84568,-7.93055,-9.63469], "fy":[-14.20963,-15.19086,-13.50993,-12.52823]}, - {"t":2.59253, "x":3.84167, "y":2.85421, "heading":1.04775, "vx":0.01825, "vy":0.02745, "omega":-0.01662, "ax":-0.55332, "ay":-0.8326, "alpha":0.50409, "fx":[-10.92242,-8.77357,-7.5495,-9.64899], "fy":[-14.26752,-15.57845,-13.45616,-12.21396]}, - {"t":2.62551, "x":3.84197, "y":2.85466, "heading":1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.64213, "y":0.85001, "heading":0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.78312, "ay":3.61708, "alpha":2.0744, "fx":[73.15798,82.50037,85.98191,77.28898], "fy":[60.96219,52.44538,59.8702,67.90196]}, + {"t":0.03838, "x":1.64565, "y":0.85268, "heading":0.94248, "vx":0.18358, "vy":0.13882, "omega":0.07962, "ax":4.78452, "ay":3.61769, "alpha":1.35865, "fx":[75.53143,81.66487,83.82667,77.99935], "fy":[60.69898,55.1619,60.01517,65.34434]}, + {"t":0.07676, "x":1.65622, "y":0.86067, "heading":0.94553, "vx":0.36721, "vy":0.27767, "omega":0.13176, "ax":4.7846, "ay":3.61734, "alpha":0.79155, "fx":[77.30564,80.89635,82.11513,78.71051], "fy":[60.50752,57.30194,60.14408,63.2438]}, + {"t":0.11514, "x":1.67384, "y":0.87399, "heading":0.95059, "vx":0.55084, "vy":0.41651, "omega":0.16214, "ax":4.78463, "ay":3.61701, "alpha":0.35635, "fx":[78.65192,80.31988,80.78895,79.26939], "fy":[60.37867,58.9318,60.24613,61.61849]}, + {"t":0.15352, "x":1.69851, "y":0.89264, "heading":0.95681, "vx":0.73448, "vy":0.55533, "omega":0.17582, "ax":4.78463, "ay":3.61667, "alpha":0.02931, "fx":[79.65264,79.88184,79.79216,79.70327], "fy":[60.29592,60.1534,60.31377,60.3896]}, + {"t":0.1919, "x":1.73022, "y":0.91662, "heading":0.96356, "vx":0.91812, "vy":0.69414, "omega":0.17694, "ax":4.78457, "ay":3.61633, "alpha":-0.2075, "fx":[80.36773,79.56952,79.06327,80.02536], "fy":[60.25143,61.03033,60.35468,59.49379]}, + {"t":0.23028, "x":1.76898, "y":0.94592, "heading":0.97035, "vx":1.10175, "vy":0.83294, "omega":0.16898, "ax":4.78444, "ay":3.61599, "alpha":-0.37209, "fx":[80.85762,79.34929,78.5542,80.25585], "fy":[60.23134,61.6355,60.37319,58.86723]}, + {"t":0.26866, "x":1.81479, "y":0.98056, "heading":0.97684, "vx":1.28538, "vy":0.97172, "omega":0.1547, "ax":4.7842, "ay":3.61563, "alpha":-0.47735, "fx":[81.16174,79.23105,78.20872,80.39997], "fy":[60.23101,62.00888,60.38237,58.46076]}, + {"t":0.30704, "x":1.86765, "y":1.02051, "heading":0.98278, "vx":1.469, "vy":1.11049, "omega":0.13638, "ax":4.78383, "ay":3.61522, "alpha":-0.53829, "fx":[81.32792,79.16518,78.00139,80.48213], "fy":[60.23734,62.22059,60.37825,58.22007]}, + {"t":0.34542, "x":1.92755, "y":1.0658, "heading":0.98801, "vx":1.6526, "vy":1.24924, "omega":0.11572, "ax":4.78324, "ay":3.61476, "alpha":-0.56181, "fx":[81.37259,79.19106,77.8764,80.49692], "fy":[60.25558,62.27644,60.3784,58.11463]}, + {"t":0.3838, "x":1.9945, "y":1.11641, "heading":0.99245, "vx":1.83619, "vy":1.38798, "omega":0.09416, "ax":4.78227, "ay":3.61415, "alpha":-0.46961, "fx":[80.96769,78.81396,78.98747,80.10354], "fy":[60.50119,62.50983,59.64263,58.33085]}, + {"t":0.42218, "x":2.0685, "y":1.17234, "heading":0.99606, "vx":2.01973, "vy":1.52669, "omega":0.07613, "ax":4.78056, "ay":3.61327, "alpha":-0.51952, "fx":[81.20953,79.25493,77.92426,80.36958], "fy":[60.23689,62.08581,60.33301,58.27023]}, + {"t":0.46056, "x":2.14954, "y":1.2336, "heading":0.99899, "vx":2.20321, "vy":1.66537, "omega":0.05619, "ax":4.77705, "ay":3.6116, "alpha":-0.44596, "fx":[80.91213,79.28853,78.11847,80.20507], "fy":[60.24036,61.80945,60.25159,58.51313]}, + {"t":0.49895, "x":2.23762, "y":1.30017, "heading":1.00114, "vx":2.38656, "vy":1.80398, "omega":0.03908, "ax":4.76562, "ay":3.60764, "alpha":-0.33258, "fx":[80.25174,79.86748,78.00532,79.63794], "fy":[60.21322,61.32535,60.17134,58.84029]}, + {"t":0.53733, "x":2.33272, "y":1.37207, "heading":1.00264, "vx":2.56946, "vy":1.94245, "omega":0.02631, "ax":1.66252, "ay":1.17369, "alpha":-0.28175, "fx":[28.71402,27.27878,26.84171,28.01901], "fy":[19.74571,20.48931,19.39727,18.6271]}, + {"t":0.57571, "x":2.43257, "y":1.44748, "heading":1.00365, "vx":2.63327, "vy":1.98749, "omega":0.0155, "ax":0.03923, "ay":-0.05182, "alpha":-0.115, "fx":[0.93711,0.99472,0.07972,0.60444], "fy":[-0.77344,-0.45796,-0.95425,-1.26982]}, + {"t":0.61409, "x":2.53366, "y":1.52373, "heading":1.00425, "vx":2.63478, "vy":1.9855, "omega":0.01108, "ax":0.032, "ay":-0.04251, "alpha":-0.03992, "fx":[0.51583,1.09459,0.14979,0.37368], "fy":[-0.67041,-0.53809,-0.74694,-0.87873]}, + {"t":0.65247, "x":2.63481, "y":1.5999, "heading":1.00467, "vx":2.63601, "vy":1.98387, "omega":0.00955, "ax":-0.02451, "ay":0.03255, "alpha":-0.00762, "fx":[-0.53062,0.14118,-0.66273,-0.58206], "fy":[0.55717,0.60759,0.5278,0.47758]}, + {"t":0.69085, "x":2.73596, "y":1.67607, "heading":1.00504, "vx":2.63506, "vy":1.98512, "omega":0.00926, "ax":-0.25081, "ay":0.33128, "alpha":0.01184, "fx":[-4.34627,-3.71665,-4.32295,-4.33741], "fy":[5.52208,5.51931,5.52326,5.52421]}, + {"t":0.72923, "x":2.83691, "y":1.7525, "heading":1.0054, "vx":2.62544, "vy":1.99784, "omega":0.00971, "ax":-0.88131, "ay":1.13828, "alpha":0.02916, "fx":[-14.78709,-14.70133,-14.5727,-14.70311], "fy":[18.95661,18.8774,18.99961,19.06464]}, + {"t":0.76761, "x":2.93703, "y":1.83002, "heading":1.00577, "vx":2.59161, "vy":2.04152, "omega":0.01083, "ax":-2.71985, "ay":3.27881, "alpha":0.04157, "fx":[-45.04368,-46.97184,-44.5108,-44.8278], "fy":[54.59685,54.39689,54.72926,54.90151]}, + {"t":0.80599, "x":3.03449, "y":1.91078, "heading":1.00618, "vx":2.48722, "vy":2.16737, "omega":0.01243, "ax":-5.73956, "ay":0.83381, "alpha":0.10257, "fx":[-95.49385,-97.34729,-94.66332,-95.19808], "fy":[13.79212,13.38309,13.98108,14.44066]}, + {"t":0.84437, "x":3.12572, "y":1.99458, "heading":1.00666, "vx":2.26694, "vy":2.19937, "omega":0.01637, "ax":-5.04921, "ay":-3.20216, "alpha":0.16097, "fx":[-84.58124,-84.18808,-83.59266,-84.30967], "fy":[-53.43696,-53.97502,-53.36813,-52.73345]}, + {"t":0.88275, "x":3.20901, "y":2.07664, "heading":1.00729, "vx":2.07315, "vy":2.07647, "omega":0.02254, "ax":-4.9296, "ay":-3.40125, "alpha":0.17165, "fx":[-82.6787,-82.01955,-81.60065,-82.39716], "fy":[-56.7585,-57.28071,-56.72118,-56.02828]}, + {"t":0.92113, "x":3.28495, "y":2.15383, "heading":1.00815, "vx":1.88395, "vy":1.94593, "omega":0.02913, "ax":-4.8856, "ay":-3.47028, "alpha":0.16282, "fx":[-81.91649,-81.30518,-80.88457,-81.65628], "fy":[-57.91984,-58.38618,-57.87796,-57.20732]}, + {"t":0.95951, "x":3.35366, "y":2.22596, "heading":1.00927, "vx":1.69644, "vy":1.81274, "omega":0.03538, "ax":-4.86284, "ay":-3.50519, "alpha":0.14697, "fx":[-81.49263,-80.94158,-80.55184,-81.25867], "fy":[-58.495,-58.90879,-58.46299,-57.85263]}, + {"t":0.99789, "x":3.41519, "y":2.29295, "heading":1.01063, "vx":1.5098, "vy":1.67821, "omega":0.04102, "ax":-4.84884, "ay":-3.52639, "alpha":0.1261, "fx":[-81.20628,-80.67939,-80.41513,-81.01062], "fy":[-58.83066,-59.20312,-58.80511,-58.29391]}, + {"t":1.03627, "x":3.46956, "y":2.35476, "heading":1.0122, "vx":1.3237, "vy":1.54286, "omega":0.04586, "ax":-4.83938, "ay":-3.5406, "alpha":0.09091, "fx":[-80.94638,-80.58573,-80.35014,-80.7983], "fy":[-59.06822,-59.30023,-59.04708,-58.66512]}, + {"t":1.07465, "x":3.5168, "y":2.41137, "heading":1.01397, "vx":1.13796, "vy":1.40697, "omega":0.04935, "ax":-2.34638, "ay":-1.8571, "alpha":0.14614, "fx":[-39.44898,-38.99062,-38.79685,-39.21545], "fy":[-30.85853,-31.78641,-30.72939,-30.45385]}, + {"t":1.10654, "x":3.5519, "y":2.4553, "heading":1.01554, "vx":1.06313, "vy":1.34775, "omega":0.05401, "ax":-2.23632, "ay":-1.99477, "alpha":0.11365, "fx":[-37.55439,-37.17492,-37.0207,-37.36362], "fy":[-33.18621,-33.87141,-33.08459,-32.86506]}, + {"t":1.13843, "x":3.58467, "y":2.49726, "heading":1.01726, "vx":0.99181, "vy":1.28413, "omega":0.05764, "ax":-2.14514, "ay":-2.09308, "alpha":0.09101, "fx":[-35.98394,-35.6739,-35.54834,-35.82762], "fy":[-34.83883,-35.38177,-34.75759,-34.58459]}, + {"t":1.17033, "x":3.61521, "y":2.53715, "heading":1.0191, "vx":0.9234, "vy":1.21738, "omega":0.06054, "ax":-2.07071, "ay":-2.16718, "alpha":0.07398, "fx":[-34.69806,-34.44706,-34.35272,-34.57334], "fy":[-36.07488,-36.53708,-36.0115,-35.8799]}, + {"t":1.20222, "x":3.6436, "y":2.57487, "heading":1.02103, "vx":0.85736, "vy":1.14826, "omega":0.0629, "ax":-2.00913, "ay":-2.22473, "alpha":0.05722, "fx":[-33.62666,-33.43583,-33.36976,-33.53288], "fy":[-37.03635,-37.417,-36.99003,-36.89759]}, + {"t":1.23411, "x":3.66992, "y":2.61036, "heading":1.02304, "vx":0.79329, "vy":1.07731, "omega":0.06472, "ax":-1.9575, "ay":-2.27057, "alpha":0.04105, "fx":[-32.72005,-32.59028,-32.55419,-32.65808], "fy":[-37.79953,-38.10988,-37.77064,-37.71735]}, + {"t":1.266, "x":3.69423, "y":2.64357, "heading":1.0251, "vx":0.73086, "vy":1.0049, "omega":0.06603, "ax":-1.91369, "ay":-2.30785, "alpha":0.02482, "fx":[-31.94072,-31.87537,-31.87218,-31.91266], "fy":[-38.41671,-38.66592,-38.40651,-38.39407]}, + {"t":1.29789, "x":3.71656, "y":2.67444, "heading":1.02721, "vx":0.66983, "vy":0.9313, "omega":0.06682, "ax":-1.87609, "ay":-2.33871, "alpha":0.00692, "fx":[-31.25999,-31.26653,-31.29824,-31.26901], "fy":[-38.92697,-39.1075,-38.93749,-38.96901]}, + {"t":1.32978, "x":3.73697, "y":2.70295, "heading":1.02934, "vx":0.61, "vy":0.85671, "omega":0.06704, "ax":-1.8435, "ay":-2.36465, "alpha":-0.01238, "fx":[-30.65666,-30.74285,-30.81442,-30.707], "fy":[-39.35219,-39.46564,-39.38622,-39.46622]}, + {"t":1.36168, "x":3.75549, "y":2.72907, "heading":1.03148, "vx":0.55121, "vy":0.7813, "omega":0.06665, "ax":-1.81501, "ay":-2.38673, "alpha":-0.03419, "fx":[-30.11334,-30.29059,-30.40678,-30.21037], "fy":[-39.71175,-39.7506,-39.77286,-39.90732]}, + {"t":1.39357, "x":3.77214, "y":2.75277, "heading":1.0336, "vx":0.49332, "vy":0.70519, "omega":0.06556, "ax":-1.78989, "ay":-2.40574, "alpha":-0.05857, "fx":[-29.61651,-29.89764,-30.0657,-29.76688], "fy":[-40.01642,-39.97905,-40.10909,-40.30559]}, + {"t":1.42546, "x":3.78696, "y":2.77404, "heading":1.03569, "vx":0.43624, "vy":0.62846, "omega":0.06369, "ax":-1.76761, "ay":-2.42227, "alpha":-0.08628, "fx":[-29.15457,-29.55557,-29.78401,-29.36639], "fy":[-40.27585,-40.15752,-40.40559,-40.67343]}, + {"t":1.45735, "x":3.79998, "y":2.79285, "heading":1.03772, "vx":0.37987, "vy":0.55121, "omega":0.06094, "ax":-1.74769, "ay":-2.43677, "alpha":-0.11855, "fx":[-28.71736,-29.25848,-29.55664,-29.00043], "fy":[-40.49847,-40.28643,-40.67189,-41.02239]}, + {"t":1.48924, "x":3.8112, "y":2.80919, "heading":1.03967, "vx":0.32413, "vy":0.4735, "omega":0.05716, "ax":-1.7298, "ay":-2.44959, "alpha":-0.15603, "fx":[-28.29595,-29.00097,-29.38094,-28.66217], "fy":[-40.68846,-40.37117,-40.91359,-41.36052]}, + {"t":1.52113, "x":3.82066, "y":2.82305, "heading":1.04149, "vx":0.26897, "vy":0.39538, "omega":0.05218, "ax":-1.71365, "ay":-2.46099, "alpha":-0.20005, "fx":[-27.88136,-28.78002,-29.25578,-28.34541], "fy":[-40.85169,-40.41022,-41.13615,-41.69624]}, + {"t":1.55302, "x":3.82837, "y":2.8344, "heading":1.04315, "vx":0.21431, "vy":0.3169, "omega":0.0458, "ax":-1.69898, "ay":-2.47121, "alpha":-0.25179, "fx":[-27.46541,-28.59261,-29.18195,-28.04474], "fy":[-40.98674,-40.40623,-41.34438,-42.03806]}, + {"t":1.58492, "x":3.83434, "y":2.84325, "heading":1.04461, "vx":0.16013, "vy":0.23808, "omega":0.03777, "ax":-1.68561, "ay":-2.48041, "alpha":-0.31323, "fx":[-27.03874,-28.43779,-29.16185,-27.75496], "fy":[-41.09777,-40.35306,-41.54288,-42.39508]}, + {"t":1.61681, "x":3.83859, "y":2.84958, "heading":1.04582, "vx":0.10637, "vy":0.15898, "omega":0.02778, "ax":-1.67338, "ay":-2.48873, "alpha":-0.38661, "fx":[-26.59131,-28.31533,-29.19983,-27.4711], "fy":[-41.18625,-40.24513,-41.73561,-42.77696]}, + {"t":1.6487, "x":3.84113, "y":2.85339, "heading":1.0467, "vx":0.05301, "vy":0.07961, "omega":0.01545, "ax":-1.66214, "ay":-2.4963, "alpha":-0.48458, "fx":[-26.04345,-28.23127,-29.32232,-27.23116], "fy":[-41.36,-39.9769,-41.92131,-43.19054]}, + {"t":1.68059, "x":3.84197, "y":2.85466, "heading":1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/fetchPToDSlow.traj b/src/main/deploy/choreo/fetchPToDSlow.traj index a41c8fd0..181ddbb1 100644 --- a/src/main/deploy/choreo/fetchPToDSlow.traj +++ b/src/main/deploy/choreo/fetchPToDSlow.traj @@ -4,8 +4,8 @@ "snapshot":{ "waypoints":[ {"x":1.69577419757843, "y":0.8236, "heading":0.942477796076938, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.427126884460449, "y":2.2866694927215576, "heading":0.0, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":3.89906213509461, "y":2.953538192264317, "heading":1.0471975511965976, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.427126884460449, "y":2.2866694927215576, "heading":0.0, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.84197363509461, "y":2.854658009736421, "heading":1.0471975511965976, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -17,8 +17,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"fetchP.x", "val":1.69577419757843}, "y":{"exp":"fetchP.y", "val":0.8236}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.427126884460449 m", "val":3.427126884460449}, "y":{"exp":"2.2866694927215576 m", "val":2.2866694927215576}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"fetchP.x", "val":1.6421304941177368}, "y":{"exp":"fetchP.y", "val":0.8500130772590637}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.427126884460449 m", "val":3.427126884460449}, "y":{"exp":"2.2866694927215576 m", "val":2.2866694927215576}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"D.x", "val":3.84197363509461}, "y":{"exp":"D.y", "val":2.854658009736421}, "heading":{"exp":"D.heading", "val":1.0471975511965976}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -34,76 +34,74 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.15973,2.43869], + "waypoints":[0.0,1.17,2.3567], "samples":[ - {"t":0.0, "x":1.69577, "y":0.8236, "heading":0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.98235, "ay":3.01917, "alpha":2.73523, "fx":[57.57247,69.48772,74.88572,63.58968], "fy":[50.44484,40.26666,50.42929,60.17175]}, - {"t":0.03999, "x":1.69896, "y":0.82601, "heading":0.94248, "vx":0.15926, "vy":0.12074, "omega":0.10938, "ax":3.98346, "ay":3.01966, "alpha":1.70501, "fx":[60.95056,68.30471,71.73221,64.62225], "fy":[50.39855,44.07702,50.37234,56.49721]}, - {"t":0.07998, "x":1.70851, "y":0.83326, "heading":0.94685, "vx":0.31856, "vy":0.2415, "omega":0.17757, "ax":3.98353, "ay":3.01939, "alpha":0.92599, "fx":[63.45913,67.43089,69.31451,65.40991], "fy":[50.33299,46.95236,50.35267,53.68904]}, - {"t":0.11997, "x":1.72444, "y":0.84533, "heading":0.95395, "vx":0.47786, "vy":0.36224, "omega":0.2146, "ax":3.98358, "ay":3.01913, "alpha":0.34959, "fx":[65.29273,66.79068,67.51627,66.01747], "fy":[50.30886,49.07084,50.33323,51.59672]}, - {"t":0.15996, "x":1.74673, "y":0.86223, "heading":0.96254, "vx":0.63717, "vy":0.48298, "omega":0.22858, "ax":3.98358, "ay":3.01888, "alpha":-0.0616, "fx":[66.58344,66.32447,66.23543,66.47396], "fy":[50.31539,50.58148,50.29898,50.09715]}, - {"t":0.19995, "x":1.7754, "y":0.88396, "heading":0.97168, "vx":0.79647, "vy":0.60371, "omega":0.22612, "ax":3.98354, "ay":3.01863, "alpha":-0.34039, "fx":[67.44561,65.99379,65.37388,66.80157], "fy":[50.33858,51.60893,50.25136,49.07761]}, - {"t":0.23994, "x":1.81044, "y":0.91052, "heading":0.98072, "vx":0.95578, "vy":0.72443, "omega":0.2125, "ax":3.98345, "ay":3.01839, "alpha":-0.51561, "fx":[67.97579,65.77031,64.84294,67.01961], "fy":[50.36447,52.26351,50.19619,48.43619]}, - {"t":0.27993, "x":1.85184, "y":0.9419, "heading":0.98922, "vx":1.11508, "vy":0.84513, "omega":0.19188, "ax":3.98329, "ay":3.01814, "alpha":-0.61029, "fx":[68.253,65.6368,64.56343,67.14468], "fy":[50.38719,52.62305,50.14434,48.08942]}, - {"t":0.31993, "x":1.89962, "y":0.97811, "heading":0.99689, "vx":1.27437, "vy":0.96583, "omega":0.16748, "ax":3.98303, "ay":3.01789, "alpha":-0.64896, "fx":[68.34805,65.55805,64.47452,67.20023], "fy":[50.38896,52.80192,50.08703,47.94904]}, - {"t":0.35992, "x":1.95377, "y":1.01915, "heading":1.00359, "vx":1.43366, "vy":1.08652, "omega":0.14153, "ax":3.98264, "ay":3.01761, "alpha":-0.63311, "fx":[68.30025,65.57257,64.50432,67.17738], "fy":[50.39372,52.72512,50.07568,48.01394]}, - {"t":0.39991, "x":2.01429, "y":1.06501, "heading":1.00925, "vx":1.59293, "vy":1.20719, "omega":0.11621, "ax":3.98202, "ay":3.01729, "alpha":-0.59752, "fx":[68.16184,65.58788,64.63785,67.12599], "fy":[50.37103,52.63478,50.03849,48.14269]}, - {"t":0.4399, "x":2.08117, "y":1.1157, "heading":1.0139, "vx":1.75217, "vy":1.32786, "omega":0.09231, "ax":3.98102, "ay":3.01688, "alpha":-0.53388, "fx":[67.94736,65.65396,64.81859,67.0269], "fy":[50.35605,52.3994,50.03245,48.37172]}, - {"t":0.47989, "x":2.15443, "y":1.17121, "heading":1.01759, "vx":1.91137, "vy":1.4485, "omega":0.07096, "ax":3.97923, "ay":3.01627, "alpha":-0.4609, "fx":[67.6828,65.71551,65.03143,66.8974], "fy":[50.31365,52.15655,50.01543,48.63343]}, - {"t":0.51988, "x":2.23405, "y":1.23155, "heading":1.02042, "vx":2.07051, "vy":1.56913, "omega":0.05253, "ax":3.97532, "ay":3.01511, "alpha":-0.38306, "fx":[67.35606,65.75604,65.23614,66.71866], "fy":[50.23918,51.91771,49.9808,48.90382]}, - {"t":0.55987, "x":2.32003, "y":1.29671, "heading":1.02253, "vx":2.22948, "vy":1.6897, "omega":0.03721, "ax":3.96151, "ay":3.01099, "alpha":-0.34569, "fx":[66.83747,65.67996,65.27712,66.35139], "fy":[49.9469,52.10558,49.73375,48.98047]}, - {"t":0.59986, "x":2.41235, "y":1.36669, "heading":1.02401, "vx":2.38791, "vy":1.81012, "omega":0.02339, "ax":0.2116, "ay":-0.1635, "alpha":-0.10786, "fx":[4.10009,3.38443,2.95573,3.66855], "fy":[-2.38971,-2.80564,-2.64804,-3.05846]}, - {"t":0.63985, "x":2.50802, "y":1.43895, "heading":1.02495, "vx":2.39637, "vy":1.80358, "omega":0.01907, "ax":-0.05323, "ay":0.07064, "alpha":-0.08543, "fx":[-0.59261,-0.95814,-1.18251,-0.81601], "fy":[1.23845,1.45919,1.10817,0.90414]}, - {"t":0.67984, "x":2.60381, "y":1.51113, "heading":1.02571, "vx":2.39424, "vy":1.8064, "omega":0.01566, "ax":-0.20524, "ay":0.27074, "alpha":-0.02787, "fx":[-3.26057,-3.45865,-3.58275,-3.38294], "fy":[4.60829,4.47103,4.54004,4.4332]}, - {"t":0.71983, "x":2.69939, "y":1.58359, "heading":1.02634, "vx":2.38603, "vy":1.81723, "omega":0.01454, "ax":-0.77704, "ay":1.00251, "alpha":-0.02079, "fx":[-12.85361,-12.97539,-13.05298,-12.92977], "fy":[16.75658,16.72433,16.71436,16.65042]}, - {"t":0.75982, "x":2.79419, "y":1.65706, "heading":1.02692, "vx":2.35496, "vy":1.85732, "omega":0.01371, "ax":-2.24143, "ay":2.70466, "alpha":-0.01122, "fx":[-37.28695,-37.4403,-37.3967,-37.33063], "fy":[45.11973,45.07257,45.09194,45.05706]}, - {"t":0.79981, "x":2.88657, "y":1.7335, "heading":1.02747, "vx":2.26532, "vy":1.96548, "omega":0.01326, "ax":-4.84418, "ay":-0.53931, "alpha":-0.19762, "fx":[-80.86056,-80.88206,-80.51519,-80.74287], "fy":[-9.90728,-6.54199,-9.83567,-9.67491]}, - {"t":0.8398, "x":2.97329, "y":1.81167, "heading":1.028, "vx":2.0716, "vy":1.94391, "omega":0.00536, "ax":-4.1763, "ay":-2.71866, "alpha":0.0803, "fx":[-69.76309,-69.53343,-69.49735,-69.67388], "fy":[-45.20084,-45.87217,-45.15809,-45.04363]}, - {"t":0.8798, "x":3.05279, "y":1.88724, "heading":1.02821, "vx":1.90459, "vy":1.83519, "omega":0.00857, "ax":-4.09095, "ay":-2.85924, "alpha":0.08321, "fx":[-68.32417,-68.08395,-68.11785,-68.25056], "fy":[-47.49293,-48.31018,-47.46105,-47.38479]}, - {"t":0.91979, "x":3.12569, "y":1.95834, "heading":1.02856, "vx":1.74099, "vy":1.72085, "omega":0.0119, "ax":-4.05879, "ay":-2.9096, "alpha":0.06322, "fx":[-67.76452,-67.56413,-67.60202,-67.70167], "fy":[-48.37071,-48.99237,-48.34551,-48.29796]}, - {"t":0.95978, "x":3.19207, "y":2.02483, "heading":1.02903, "vx":1.57867, "vy":1.60449, "omega":0.01443, "ax":-4.04199, "ay":-2.93539, "alpha":0.05362, "fx":[-67.46406,-67.29317,-67.34311,-67.4119], "fy":[-48.80548,-49.37012,-48.78628,-48.76423]}, - {"t":0.99977, "x":3.25197, "y":2.08665, "heading":1.02961, "vx":1.41703, "vy":1.4871, "omega":0.01657, "ax":-4.03163, "ay":-2.95112, "alpha":0.04284, "fx":[-67.26718,-67.13947,-67.18797,-67.22681], "fy":[-49.07743,-49.56869,-49.06398,-49.06476]}, - {"t":1.03976, "x":3.30541, "y":2.14376, "heading":1.03027, "vx":1.2558, "vy":1.36909, "omega":0.01828, "ax":-4.02463, "ay":-2.96167, "alpha":0.03589, "fx":[-67.13049,-67.0254,-67.09586,-67.10301], "fy":[-49.24968,-49.72019,-49.2423,-49.26643]}, - {"t":1.07975, "x":3.35241, "y":2.19614, "heading":1.031, "vx":1.09485, "vy":1.25065, "omega":0.01972, "ax":-4.01957, "ay":-2.96927, "alpha":0.02594, "fx":[-67.02113,-66.95614,-67.03205,-67.00776], "fy":[-49.38289,-49.79338,-49.38167,-49.42733]}, - {"t":1.11974, "x":3.39298, "y":2.24378, "heading":1.03179, "vx":0.93411, "vy":1.1319, "omega":0.02076, "ax":-4.01574, "ay":-2.97499, "alpha":0.02137, "fx":[-66.93743,-66.89093,-66.99477,-66.93888], "fy":[-49.46712,-49.88874,-49.47229,-49.53854]}, - {"t":1.15973, "x":3.42713, "y":2.28667, "heading":1.03262, "vx":0.77352, "vy":1.01293, "omega":0.02161, "ax":-0.77785, "ay":-0.62089, "alpha":-0.02179, "fx":[-12.85146,-13.12127,-12.9903,-12.90238], "fy":[-10.33686,-10.29527,-10.36349,-10.40405]}, - {"t":1.19339, "x":3.45272, "y":2.32041, "heading":1.03335, "vx":0.74734, "vy":0.99204, "omega":0.02088, "ax":-0.74459, "ay":-0.66451, "alpha":-0.01843, "fx":[-12.3201,-12.52563,-12.4385,-12.36346], "fy":[-11.06564,-11.02842,-11.089,-11.12492]}, - {"t":1.22704, "x":3.47745, "y":2.35342, "heading":1.03405, "vx":0.72228, "vy":0.96967, "omega":0.02026, "ax":-0.7173, "ay":-0.69426, "alpha":-0.0166, "fx":[-11.87593,-12.05684,-11.98088,-11.91441], "fy":[-11.56246,-11.52803,-11.5841,-11.61731]}, - {"t":1.2607, "x":3.50136, "y":2.38567, "heading":1.03473, "vx":0.69813, "vy":0.9463, "omega":0.0197, "ax":-0.69564, "ay":-0.71625, "alpha":-0.0156, "fx":[-11.52181,-11.68504,-11.61932,-11.5576], "fy":[-11.92931,-11.89606,-11.95033,-11.98247]}, - {"t":1.29436, "x":3.52446, "y":2.41711, "heading":1.0354, "vx":0.67472, "vy":0.9222, "omega":0.01917, "ax":-0.6781, "ay":-0.7331, "alpha":-0.01498, "fx":[-11.23468,-11.38319,-11.32761,-11.26883], "fy":[-12.21021,-12.1774,-12.23112,-12.26292]}, - {"t":1.32801, "x":3.54678, "y":2.44773, "heading":1.03604, "vx":0.6519, "vy":0.89752, "omega":0.01867, "ax":-0.66364, "ay":-0.74639, "alpha":-0.0148, "fx":[-10.99648,-11.13613,-11.08776,-11.03004], "fy":[-12.43155,-12.39843,-12.4528,-12.48497]}, - {"t":1.36167, "x":3.56835, "y":2.47752, "heading":1.03667, "vx":0.62956, "vy":0.8724, "omega":0.01817, "ax":-0.65154, "ay":-0.75712, "alpha":-0.01471, "fx":[-10.79714,-10.92824,-10.88768,-10.83044], "fy":[-12.61018,-12.57663,-12.6319,-12.6646]}, - {"t":1.39533, "x":3.58917, "y":2.50645, "heading":1.03728, "vx":0.60763, "vy":0.84692, "omega":0.01768, "ax":-0.64127, "ay":-0.76596, "alpha":-0.01483, "fx":[-10.62699,-10.75304,-10.71823,-10.66054], "fy":[-12.75716,-12.72287,-12.77951,-12.81301]}, - {"t":1.42899, "x":3.60926, "y":2.53452, "heading":1.03788, "vx":0.58605, "vy":0.82114, "omega":0.01718, "ax":-0.63246, "ay":-0.77335, "alpha":-0.01494, "fx":[-10.48092,-10.60245,-10.57285,-10.51472], "fy":[-12.88015,-12.84521,-12.9031,-12.93731]}, - {"t":1.46264, "x":3.62862, "y":2.56172, "heading":1.03845, "vx":0.56476, "vy":0.79511, "omega":0.01668, "ax":-0.62481, "ay":-0.77963, "alpha":-0.01514, "fx":[-10.35377,-10.47197,-10.44714,-10.38809], "fy":[-12.98448,-12.94877,-13.00807,-13.04309]}, - {"t":1.4963, "x":3.64728, "y":2.58804, "heading":1.03902, "vx":0.54373, "vy":0.76887, "omega":0.01617, "ax":-0.61811, "ay":-0.78503, "alpha":-0.0153, "fx":[-10.24253,-10.35778,-10.337,-10.27724], "fy":[-13.07411,-13.03779,-13.09823,-13.13391]}, - {"t":1.52996, "x":3.66523, "y":2.61347, "heading":1.03956, "vx":0.52293, "vy":0.74245, "omega":0.01565, "ax":-0.6122, "ay":-0.78971, "alpha":-0.0155, "fx":[-10.14409,-10.25709,-10.24008,-10.17933], "fy":[-13.15186,-13.1149,-13.17654,-13.21289]}, - {"t":1.56361, "x":3.68248, "y":2.63801, "heading":1.04009, "vx":0.50233, "vy":0.71587, "omega":0.01513, "ax":-0.60695, "ay":-0.79381, "alpha":-0.01564, "fx":[-10.05667,-10.16779,-10.15371,-10.09227], "fy":[-13.21999,-13.18254,-13.2451,-13.28197]}, - {"t":1.59727, "x":3.69904, "y":2.66166, "heading":1.0406, "vx":0.4819, "vy":0.68915, "omega":0.0146, "ax":-0.60226, "ay":-0.79743, "alpha":-0.01582, "fx":[-9.97827,-10.08793,-10.07668,-10.01435], "fy":[-13.28011,-13.24216,-13.30568,-13.34308]}, - {"t":1.63093, "x":3.71492, "y":2.6844, "heading":1.04109, "vx":0.46163, "vy":0.66231, "omega":0.01407, "ax":-0.59803, "ay":-0.80065, "alpha":-0.01593, "fx":[-9.90789,-10.01605,-10.00722,-9.94428], "fy":[-13.3336,-13.29528,-13.35951,-13.39731]}, - {"t":1.66458, "x":3.73012, "y":2.70624, "heading":1.04156, "vx":0.4415, "vy":0.63537, "omega":0.01353, "ax":-0.59421, "ay":-0.80353, "alpha":-0.01607, "fx":[-9.84412,-9.95091,-9.94462,-9.88091], "fy":[-13.38145,-13.34275,-13.40772,-13.44593]}, - {"t":1.69824, "x":3.74464, "y":2.72717, "heading":1.04202, "vx":0.4215, "vy":0.60832, "omega":0.01299, "ax":-0.59073, "ay":-0.80612, "alpha":-0.01618, "fx":[-9.78617,-9.89211,-9.88747,-9.82323], "fy":[-13.42454,-13.38556,-13.45109,-13.4896]}, - {"t":1.7319, "x":3.75849, "y":2.74719, "heading":1.04245, "vx":0.40162, "vy":0.58119, "omega":0.01245, "ax":-0.58756, "ay":-0.80847, "alpha":-0.0163, "fx":[-9.7332,-9.83831,-9.83552,-9.77061], "fy":[-13.46351,-13.42421,-13.49035,-13.52919]}, - {"t":1.76555, "x":3.77168, "y":2.76629, "heading":1.04287, "vx":0.38184, "vy":0.55398, "omega":0.0119, "ax":-0.58466, "ay":-0.8106, "alpha":-0.01639, "fx":[-9.68475,-9.78904,-9.78779,-9.7224], "fy":[-13.49894,-13.45941,-13.52603,-13.56512]}, - {"t":1.79921, "x":3.7842, "y":2.78448, "heading":1.04327, "vx":0.36217, "vy":0.5267, "omega":0.01135, "ax":-0.58199, "ay":-0.81255, "alpha":-0.0165, "fx":[-9.64002,-9.74393,-9.74399,-9.67798], "fy":[-13.53127,-13.49147,-13.55862,-13.59799]}, - {"t":1.83287, "x":3.79606, "y":2.80174, "heading":1.04365, "vx":0.34258, "vy":0.49935, "omega":0.01079, "ax":-0.57953, "ay":-0.81434, "alpha":-0.01659, "fx":[-9.59884,-9.7022,-9.70353,-9.63704], "fy":[-13.56091,-13.52089,-13.58848,-13.62808]}, - {"t":1.86652, "x":3.80726, "y":2.81809, "heading":1.04402, "vx":0.32307, "vy":0.47194, "omega":0.01023, "ax":-0.57724, "ay":-0.81598, "alpha":-0.0167, "fx":[-9.5606,-9.66364,-9.66621,-9.59912], "fy":[-13.58814,-13.54787,-13.61597,-13.65584]}, - {"t":1.90018, "x":3.81781, "y":2.83351, "heading":1.04436, "vx":0.30364, "vy":0.44448, "omega":0.00967, "ax":-0.57513, "ay":-0.81749, "alpha":-0.0168, "fx":[-9.52513,-9.62791,-9.6315,-9.5639], "fy":[-13.61328,-13.57279,-13.64133,-13.68144]}, - {"t":1.93384, "x":3.8277, "y":2.84801, "heading":1.04469, "vx":0.28429, "vy":0.41697, "omega":0.00911, "ax":-0.57316, "ay":-0.8189, "alpha":-0.01692, "fx":[-9.49202,-9.59459,-9.59935,-9.53112], "fy":[-13.63653,-13.59577,-13.66483,-13.70522]}, - {"t":1.9675, "x":3.83695, "y":2.86158, "heading":1.04499, "vx":0.265, "vy":0.3894, "omega":0.00854, "ax":-0.57132, "ay":-0.8202, "alpha":-0.01703, "fx":[-9.46113,-9.56355,-9.56935,-9.50053], "fy":[-13.65812,-13.6171,-13.68666,-13.72731]}, - {"t":2.00115, "x":3.84554, "y":2.87422, "heading":1.04528, "vx":0.24577, "vy":0.3618, "omega":0.00796, "ax":-0.5696, "ay":-0.82141, "alpha":-0.01716, "fx":[-9.4321,-9.53464,-9.54136,-9.47187], "fy":[-13.67818,-13.63687,-13.70699,-13.74794]}, - {"t":2.03481, "x":3.85349, "y":2.88593, "heading":1.04555, "vx":0.2266, "vy":0.33415, "omega":0.00739, "ax":-0.56799, "ay":-0.82254, "alpha":-0.0173, "fx":[-9.40485,-9.50761,-9.51515,-9.44498], "fy":[-13.6969,-13.65531,-13.72597,-13.76721]}, - {"t":2.06847, "x":3.86079, "y":2.89671, "heading":1.0458, "vx":0.20748, "vy":0.30647, "omega":0.0068, "ax":-0.56648, "ay":-0.8236, "alpha":-0.01745, "fx":[-9.37917,-9.48223,-9.49065,-9.41971], "fy":[-13.71439,-13.67248,-13.74373,-13.7853]}, - {"t":2.10212, "x":3.86746, "y":2.90656, "heading":1.04603, "vx":0.18841, "vy":0.27875, "omega":0.00622, "ax":-0.56506, "ay":-0.82459, "alpha":-0.0176, "fx":[-9.35496,-9.45832,-9.46767,-9.39594], "fy":[-13.73078,-13.68856,-13.76039,-13.80227]}, - {"t":2.13578, "x":3.87348, "y":2.91547, "heading":1.04624, "vx":0.1694, "vy":0.251, "omega":0.00563, "ax":-0.56372, "ay":-0.82552, "alpha":-0.01777, "fx":[-9.33199,-9.43592,-9.44608,-9.37346], "fy":[-13.74616,-13.70361,-13.77605,-13.81826]}, - {"t":2.16944, "x":3.87886, "y":2.92345, "heading":1.04643, "vx":0.15042, "vy":0.22321, "omega":0.00503, "ax":-0.56245, "ay":-0.8264, "alpha":-0.01794, "fx":[-9.31025,-9.41469,-9.42583,-9.35226], "fy":[-13.76062,-13.71777,-13.79078,-13.83332]}, - {"t":2.20309, "x":3.8836, "y":2.9305, "heading":1.0466, "vx":0.13149, "vy":0.1954, "omega":0.00442, "ax":-0.56125, "ay":-0.82722, "alpha":-0.01813, "fx":[-9.28951,-9.39482,-9.40675,-9.33211], "fy":[-13.77426,-13.7311,-13.80468,-13.84752]}, - {"t":2.23675, "x":3.88771, "y":2.93661, "heading":1.04674, "vx":0.1126, "vy":0.16756, "omega":0.00381, "ax":-0.56012, "ay":-0.828, "alpha":-0.01833, "fx":[-9.26971,-9.37603,-9.38882,-9.313], "fy":[-13.78714,-13.7437,-13.81779,-13.86091]}, - {"t":2.27041, "x":3.89118, "y":2.94178, "heading":1.04687, "vx":0.09375, "vy":0.13969, "omega":0.0032, "ax":-0.55904, "ay":-0.82874, "alpha":-0.01854, "fx":[-9.25076,-9.35823,-9.37202,-9.29484], "fy":[-13.79933,-13.75565,-13.83018,-13.87355]}, - {"t":2.30406, "x":3.89402, "y":2.94601, "heading":1.04698, "vx":0.07493, "vy":0.11179, "omega":0.00257, "ax":-0.55802, "ay":-0.82944, "alpha":-0.01876, "fx":[-9.23256,-9.34126,-9.35634,-9.27757], "fy":[-13.8109,-13.76703,-13.8419,-13.88546]}, - {"t":2.33772, "x":3.89623, "y":2.9493, "heading":1.04707, "vx":0.05615, "vy":0.08388, "omega":0.00194, "ax":-0.55705, "ay":-0.8301, "alpha":-0.01902, "fx":[-9.21487,-9.32535,-9.34172,-9.26101], "fy":[-13.82189,-13.7779,-13.85299,-13.89668]}, - {"t":2.37138, "x":3.8978, "y":2.95166, "heading":1.04713, "vx":0.03741, "vy":0.05594, "omega":0.0013, "ax":-0.55612, "ay":-0.83073, "alpha":-0.01931, "fx":[-9.19753,-9.31039,-9.32823,-9.24511], "fy":[-13.83237,-13.78831,-13.8635,-13.90725]}, - {"t":2.40503, "x":3.89875, "y":2.95307, "heading":1.04718, "vx":0.01869, "vy":0.02798, "omega":0.00065, "ax":-0.55524, "ay":-0.83133, "alpha":-0.01932, "fx":[-9.18004,-9.26698,-9.31618,-9.25925], "fy":[-13.86248,-13.78239,-13.86829,-13.91817]}, - {"t":2.43869, "x":3.89906, "y":2.95354, "heading":1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.69577, "y":0.8236, "heading":0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.97445, "ay":3.02946, "alpha":2.73761, "fx":[57.42189,69.3768,74.76144,63.44878], "fy":[50.64341,40.42869,50.58362,60.34292]}, + {"t":0.04034, "x":1.69901, "y":0.82607, "heading":0.94248, "vx":0.16035, "vy":0.12222, "omega":0.11045, "ax":3.9756, "ay":3.02995, "alpha":1.69056, "fx":[60.86119,68.15998,71.55933,64.50465], "fy":[50.57584,44.30206,50.54374,56.60941]}, + {"t":0.08069, "x":1.70871, "y":0.83346, "heading":0.94693, "vx":0.32074, "vy":0.24446, "omega":0.17865, "ax":3.97568, "ay":3.02965, "alpha":0.89867, "fx":[63.40484,67.25404,69.11451,65.31712], "fy":[50.48371,47.24185,50.53389,53.7517]}, + {"t":0.12103, "x":1.72489, "y":0.84579, "heading":0.95414, "vx":0.48114, "vy":0.3667, "omega":0.21491, "ax":3.97573, "ay":3.02936, "alpha":0.3106, "fx":[65.25899,66.58249,67.30013,65.9521], "fy":[50.44348,49.42566,50.51758,51.60497]}, + {"t":0.16138, "x":1.74754, "y":0.86305, "heading":0.96281, "vx":0.64154, "vy":0.48891, "omega":0.22744, "ax":3.97574, "ay":3.02907, "alpha":-0.08182, "fx":[66.52853,66.19632,65.97191,66.3978], "fy":[50.53845,50.70993,50.57247,50.15148]}, + {"t":0.20172, "x":1.77665, "y":0.88524, "heading":0.97199, "vx":0.80194, "vy":0.61112, "omega":0.22414, "ax":3.97571, "ay":3.02877, "alpha":-0.3369, "fx":[67.35984,65.95946,65.06556,66.7076], "fy":[50.66729,51.47331,50.61691,49.19525]}, + {"t":0.24207, "x":1.81224, "y":0.91236, "heading":0.98103, "vx":0.96234, "vy":0.73332, "omega":0.21055, "ax":3.97562, "ay":3.02847, "alpha":-0.51538, "fx":[67.8932,65.72157,64.54307,66.92893], "fy":[50.68644,52.1648,50.53466,48.54643]}, + {"t":0.28241, "x":1.85431, "y":0.94441, "heading":0.98953, "vx":1.12273, "vy":0.8555, "omega":0.18976, "ax":3.97546, "ay":3.02814, "alpha":-0.60701, "fx":[68.1661,65.589,64.27626,67.04464], "fy":[50.70424,52.52034,50.46432,48.22176]}, + {"t":0.32276, "x":1.90284, "y":0.98139, "heading":0.99718, "vx":1.28312, "vy":0.97767, "omega":0.16527, "ax":3.97519, "ay":3.02778, "alpha":-0.63462, "fx":[68.24587,65.53799,64.19451,67.07957], "fy":[50.71541,52.62533,50.41549,48.1304]}, + {"t":0.3631, "x":1.95784, "y":1.0233, "heading":1.00385, "vx":1.4435, "vy":1.09982, "omega":0.13966, "ax":3.97477, "ay":3.02733, "alpha":-0.62991, "fx":[68.20145,65.49996,64.26011,67.06829], "fy":[50.67529,52.68254,50.34182,48.15718]}, + {"t":0.40345, "x":2.01931, "y":1.07013, "heading":1.00948, "vx":1.60386, "vy":1.22196, "omega":0.11425, "ax":3.97405, "ay":3.02678, "alpha":-0.59231, "fx":[68.0577,65.51039,64.40477,67.00915], "fy":[50.63453,52.59369,50.2946,48.29687]}, + {"t":0.44379, "x":2.08725, "y":1.1219, "heading":1.01409, "vx":1.76419, "vy":1.34407, "omega":0.09035, "ax":3.9728, "ay":3.02594, "alpha":-0.52009, "fx":[67.8292,65.59673,64.57518,66.89729], "fy":[50.63282,52.27242,50.31038,48.54792]}, + {"t":0.48414, "x":2.16166, "y":1.17859, "heading":1.01774, "vx":1.92447, "vy":1.46615, "omega":0.06937, "ax":3.9703, "ay":3.02426, "alpha":-0.43829, "fx":[67.54407,65.66982,64.76783,66.75039], "fy":[50.60122,51.92178,50.31157,48.8176]}, + {"t":0.52448, "x":2.24254, "y":1.2402, "heading":1.02054, "vx":2.08465, "vy":1.58817, "omega":0.05169, "ax":3.96313, "ay":3.01911, "alpha":-0.38778, "fx":[67.18848,65.56317,64.95543,66.54715], "fy":[50.37068,51.89151,50.1189,48.92735]}, + {"t":0.56483, "x":2.32987, "y":1.30673, "heading":1.02262, "vx":2.24455, "vy":1.70997, "omega":0.03604, "ax":3.55406, "ay":2.59613, "alpha":-0.23686, "fx":[60.09041,59.0218,58.312,59.55394], "fy":[43.5648,43.80671,43.31671,42.41672]}, + {"t":0.60517, "x":2.42331, "y":1.37783, "heading":1.02408, "vx":2.38793, "vy":1.81471, "omega":0.02648, "ax":0.04482, "ay":-0.05798, "alpha":-0.18503, "fx":[1.42582,0.57823,0.06752,0.91724], "fy":[-0.71963,-0.49036,-1.06796,-1.58817]}, + {"t":0.64552, "x":2.51969, "y":1.451, "heading":1.02514, "vx":2.38974, "vy":1.81237, "omega":0.01902, "ax":-0.05301, "ay":0.0698, "alpha":-0.05683, "fx":[-0.45495,-0.99171,-1.31231,-0.77531], "fy":[1.54422,0.81782,1.314,0.97839]}, + {"t":0.68586, "x":2.61606, "y":1.52417, "heading":1.02591, "vx":2.3876, "vy":1.81519, "omega":0.01673, "ax":-0.35782, "ay":0.46681, "alpha":-0.04924, "fx":[-5.70523,-6.03033,-6.22403,-5.89933], "fy":[7.9676,7.7247,7.82047,7.61329]}, + {"t":0.7262, "x":2.7121, "y":1.59779, "heading":1.02659, "vx":2.37317, "vy":1.83402, "omega":0.01474, "ax":-1.1898, "ay":1.49905, "alpha":-0.07753, "fx":[-19.65151,-19.88161,-20.0114,-19.78873], "fy":[24.96725,25.4086,24.85968,24.71791]}, + {"t":0.76655, "x":2.80687, "y":1.673, "heading":1.02718, "vx":2.32516, "vy":1.8945, "omega":0.01161, "ax":-3.3847, "ay":2.82711, "alpha":-0.1422, "fx":[-56.30373,-56.49298,-56.50344,-56.38485], "fy":[46.78545,48.3925,46.70805,46.62028]}, + {"t":0.80689, "x":2.89793, "y":1.75173, "heading":1.02765, "vx":2.18861, "vy":2.00856, "omega":0.00588, "ax":-4.08614, "ay":-2.84891, "alpha":0.08576, "fx":[-68.19752,-68.14342,-67.98456,-68.13076], "fy":[-47.31901,-48.17728,-47.28542,-47.17842]}, + {"t":0.84724, "x":2.9829, "y":1.83045, "heading":1.02789, "vx":2.02376, "vy":1.89362, "omega":0.00934, "ax":-4.03373, "ay":-2.93875, "alpha":0.0578, "fx":[-67.38326,-67.25459,-67.03535,-67.28794], "fy":[-49.00765,-49.21568,-48.96446,-48.76228]}, + {"t":0.88758, "x":3.06126, "y":1.90446, "heading":1.02826, "vx":1.86102, "vy":1.77506, "omega":0.01167, "ax":-4.0161, "ay":-2.96804, "alpha":0.04912, "fx":[-67.09506,-66.97096,-66.71971,-67.00005], "fy":[-49.54741,-49.57736,-49.50552,-49.27317]}, + {"t":0.92793, "x":3.13308, "y":1.97366, "heading":1.02873, "vx":1.69899, "vy":1.65531, "omega":0.01365, "ax":-4.00721, "ay":-2.98264, "alpha":0.05737, "fx":[-66.94722,-66.77954,-66.60021,-66.86598], "fy":[-49.75155,-49.92363,-49.71423,-49.4871]}, + {"t":0.96827, "x":3.19836, "y":2.03801, "heading":1.02928, "vx":1.53732, "vy":1.53498, "omega":0.01596, "ax":-4.00181, "ay":-2.99142, "alpha":0.05259, "fx":[-66.83868,-66.68466,-66.53489,-66.77502], "fy":[-49.89282,-50.06028,-49.86133,-49.64786]}, + {"t":1.00862, "x":3.25713, "y":2.0975, "heading":1.02993, "vx":1.37587, "vy":1.41429, "omega":0.01809, "ax":-3.99822, "ay":-2.99726, "alpha":0.04689, "fx":[-66.75747,-66.62292,-66.50064,-66.71234], "fy":[-49.986,-50.14266,-49.96045,-49.76236]}, + {"t":1.04896, "x":3.30938, "y":2.15212, "heading":1.03066, "vx":1.21456, "vy":1.29337, "omega":0.01998, "ax":-3.99564, "ay":-3.00142, "alpha":0.02414, "fx":[-66.67453,-66.63245,-66.46687,-66.64796], "fy":[-50.10477,-50.03233,-50.08627,-49.90569]}, + {"t":1.08931, "x":3.35513, "y":2.20186, "heading":1.03146, "vx":1.05336, "vy":1.17228, "omega":0.02095, "ax":-3.99371, "ay":-3.00455, "alpha":0.00659, "fx":[-66.60345,-66.63379,-66.45885,-66.5967], "fy":[-50.17857,-49.97316,-50.16803,-50.01755]}, + {"t":1.12965, "x":3.39438, "y":2.24671, "heading":1.03231, "vx":0.89223, "vy":1.05106, "omega":0.02122, "ax":-3.9922, "ay":-3.00697, "alpha":-0.02485, "fx":[-66.51804,-66.68103,-66.45975,-66.53371], "fy":[-50.26801,-49.79427,-50.26832,-50.16835]}, + {"t":1.17, "x":3.42713, "y":2.28667, "heading":1.03317, "vx":0.73117, "vy":0.92974, "omega":0.02021, "ax":-0.77586, "ay":-0.624, "alpha":0.01235, "fx":[-12.97322,-12.96382,-12.86018,-12.93564], "fy":[-10.4118,-10.4341,-10.39181,-10.36958]}, + {"t":1.20296, "x":3.45081, "y":2.31698, "heading":1.03383, "vx":0.70559, "vy":0.90917, "omega":0.02062, "ax":-0.74578, "ay":-0.66339, "alpha":0.00287, "fx":[-12.46294,-12.37314,-12.43561,-12.45577], "fy":[-11.05827,-11.0549,-11.0579,-11.06246]}, + {"t":1.23592, "x":3.47366, "y":2.34659, "heading":1.03451, "vx":0.68101, "vy":0.88731, "omega":0.02072, "ax":-0.72079, "ay":-0.69081, "alpha":-0.00512, "fx":[-12.01891,-11.95867,-12.05056,-12.03253], "fy":[-11.50871,-11.4874,-11.52154,-11.54411]}, + {"t":1.26889, "x":3.49572, "y":2.37546, "heading":1.03519, "vx":0.65725, "vy":0.86453, "omega":0.02055, "ax":-0.70073, "ay":-0.7114, "alpha":-0.01049, "fx":[-11.66592,-11.62605,-11.73766,-11.69358], "fy":[-11.84763,-11.81453,-11.86926,-11.90367]}, + {"t":1.30185, "x":3.517, "y":2.40357, "heading":1.03587, "vx":0.63415, "vy":0.84108, "omega":0.0202, "ax":-0.68434, "ay":-0.72739, "alpha":-0.01405, "fx":[-11.37974,-11.35546,-11.47825,-11.41672], "fy":[-12.11123,-12.07058,-12.13859,-12.18055]}, + {"t":1.33482, "x":3.53754, "y":2.4309, "heading":1.03654, "vx":0.61159, "vy":0.81711, "omega":0.01974, "ax":-0.67072, "ay":-0.74013, "alpha":-0.01632, "fx":[-11.14431,-11.13019,-11.2603,-11.18731], "fy":[-12.32177,-12.2765,-12.35275,-12.39933]}, + {"t":1.36778, "x":3.55733, "y":2.45744, "heading":1.03719, "vx":0.58948, "vy":0.79271, "omega":0.0192, "ax":-0.65924, "ay":-0.7505, "alpha":-0.01773, "fx":[-10.94737,-10.94096,-11.07441,-10.99412], "fy":[-12.49362,-12.4457,-12.52678,-12.57599]}, + {"t":1.40074, "x":3.57641, "y":2.48316, "heading":1.03782, "vx":0.56775, "vy":0.76797, "omega":0.01862, "ax":-0.64945, "ay":-0.75911, "alpha":-0.01855, "fx":[-10.78039,-10.77994,-10.91408,-10.82936], "fy":[-12.63643,-12.58717,-12.6708,-12.72132]}, + {"t":1.43371, "x":3.59477, "y":2.50806, "heading":1.03844, "vx":0.54634, "vy":0.74295, "omega":0.018, "ax":-0.64099, "ay":-0.76635, "alpha":-0.01899, "fx":[-10.63716,-10.64123,-10.77454,-10.68731], "fy":[-12.75691,-12.70716,-12.79184,-12.84283]}, + {"t":1.46667, "x":3.61243, "y":2.53214, "heading":1.03903, "vx":0.52521, "vy":0.71768, "omega":0.01738, "ax":-0.63363, "ay":-0.77253, "alpha":-0.01917, "fx":[-10.51288,-10.52086,-10.65201,-10.56355], "fy":[-12.85987,-12.81015,-12.89494,-12.94585]}, + {"t":1.49964, "x":3.6294, "y":2.55538, "heading":1.0396, "vx":0.50433, "vy":0.69222, "omega":0.01675, "ax":-0.62716, "ay":-0.77786, "alpha":-0.01919, "fx":[-10.40423,-10.41479,-10.54388,-10.45499], "fy":[-12.94881,-12.89946,-12.98377,-13.03429]}, + {"t":1.5326, "x":3.64568, "y":2.57777, "heading":1.04015, "vx":0.48365, "vy":0.66658, "omega":0.01611, "ax":-0.62143, "ay":-0.78251, "alpha":-0.01911, "fx":[-10.3083,-10.32101,-10.44771,-10.3589], "fy":[-13.0264,-12.97757,-13.06109,-13.11105]}, + {"t":1.56556, "x":3.66129, "y":2.59932, "heading":1.04068, "vx":0.46317, "vy":0.64078, "omega":0.01548, "ax":-0.61633, "ay":-0.78659, "alpha":-0.019, "fx":[-10.22279,-10.23792,-10.36158,-10.27311], "fy":[-13.09463,-13.04641,-13.12898,-13.17831]}, + {"t":1.59853, "x":3.67622, "y":2.62001, "heading":1.04119, "vx":0.44285, "vy":0.61485, "omega":0.01486, "ax":-0.61175, "ay":-0.79021, "alpha":-0.01885, "fx":[-10.14634,-10.16302,-10.28432,-10.19632], "fy":[-13.1551,-13.10749,-13.18907,-13.23775]}, + {"t":1.63149, "x":3.69049, "y":2.63985, "heading":1.04168, "vx":0.42268, "vy":0.58881, "omega":0.01424, "ax":-0.60761, "ay":-0.79343, "alpha":-0.01871, "fx":[-10.07732,-10.09583,-10.21447,-10.12697], "fy":[-13.20903,-13.16201,-13.24264,-13.29069]}, + {"t":1.66446, "x":3.70409, "y":2.65883, "heading":1.04215, "vx":0.40266, "vy":0.56265, "omega":0.01362, "ax":-0.60387, "ay":-0.79632, "alpha":-0.01859, "fx":[-10.01474,-10.03497,-10.15115,-10.0641], "fy":[-13.25741,-13.21093,-13.29068,-13.33818]}, + {"t":1.69742, "x":3.71703, "y":2.67695, "heading":1.0426, "vx":0.38275, "vy":0.5364, "omega":0.01301, "ax":-0.60046, "ay":-0.79893, "alpha":-0.01849, "fx":[-9.95771,-9.97955,-10.09356,-10.00685], "fy":[-13.30105,-13.25503,-13.33402,-13.38103]}, + {"t":1.73038, "x":3.72932, "y":2.69419, "heading":1.04303, "vx":0.36296, "vy":0.51006, "omega":0.0124, "ax":-0.59735, "ay":-0.80129, "alpha":-0.01842, "fx":[-9.90545,-9.92902,-10.04096,-9.95446], "fy":[-13.3406,-13.29494,-13.37332,-13.41994]}, + {"t":1.76335, "x":3.74096, "y":2.71057, "heading":1.04344, "vx":0.34326, "vy":0.48365, "omega":0.01179, "ax":-0.59449, "ay":-0.80345, "alpha":-0.01839, "fx":[-9.85742,-9.88254,-9.99285,-9.90639], "fy":[-13.37659,-13.3312,-13.40912,-13.45546]}, + {"t":1.79631, "x":3.75196, "y":2.72608, "heading":1.04383, "vx":0.32367, "vy":0.45717, "omega":0.01118, "ax":-0.59185, "ay":-0.80542, "alpha":-0.0184, "fx":[-9.81306,-9.83968,-9.94874,-9.86212], "fy":[-13.40946,-13.36422,-13.44189,-13.48807]}, + {"t":1.82928, "x":3.7623, "y":2.74071, "heading":1.0442, "vx":0.30416, "vy":0.43062, "omega":0.01058, "ax":-0.58942, "ay":-0.80722, "alpha":-0.01846, "fx":[-9.77188,-9.80015,-9.90815,-9.82116], "fy":[-13.4396,-13.39437,-13.47201,-13.51815]}, + {"t":1.86224, "x":3.77201, "y":2.75447, "heading":1.04455, "vx":0.28473, "vy":0.40401, "omega":0.00997, "ax":-0.58716, "ay":-0.80889, "alpha":-0.01858, "fx":[-9.73348,-9.76365,-9.87073,-9.78312], "fy":[-13.46731,-13.42194,-13.4998,-13.54607]}, + {"t":1.8952, "x":3.78108, "y":2.76735, "heading":1.04487, "vx":0.26537, "vy":0.37734, "omega":0.00936, "ax":-0.58507, "ay":-0.81043, "alpha":-0.01877, "fx":[-9.69759,-9.72972,-9.83622,-9.74774], "fy":[-13.49285,-13.44716,-13.52554,-13.57211]}, + {"t":1.92817, "x":3.78951, "y":2.77934, "heading":1.04518, "vx":0.24609, "vy":0.35063, "omega":0.00874, "ax":-0.58312, "ay":-0.81185, "alpha":-0.01902, "fx":[-9.66394,-9.69804,-9.80437,-9.71476], "fy":[-13.51644,-13.47025,-13.54947,-13.59653]}, + {"t":1.96113, "x":3.7973, "y":2.79046, "heading":1.04547, "vx":0.22687, "vy":0.32387, "omega":0.00811, "ax":-0.58129, "ay":-0.81318, "alpha":-0.01934, "fx":[-9.63222,-9.66857,-9.77489,-9.68387], "fy":[-13.53828,-13.49138,-13.57179,-13.61955]}, + {"t":1.9941, "x":3.80447, "y":2.80069, "heading":1.04574, "vx":0.2077, "vy":0.29706, "omega":0.00747, "ax":-0.57959, "ay":-0.81441, "alpha":-0.01974, "fx":[-9.60226,-9.64105,-9.74756,-9.65489], "fy":[-13.55853,-13.5107,-13.59269,-13.64137]}, + {"t":2.02706, "x":3.811, "y":2.81004, "heading":1.04599, "vx":0.1886, "vy":0.27021, "omega":0.00682, "ax":-0.57799, "ay":-0.81556, "alpha":-0.0202, "fx":[-9.574,-9.61511,-9.72222,-9.62771], "fy":[-13.57734,-13.52836,-13.61231,-13.66214]}, + {"t":2.06002, "x":3.8169, "y":2.81851, "heading":1.04621, "vx":0.16955, "vy":0.24333, "omega":0.00616, "ax":-0.57648, "ay":-0.81664, "alpha":-0.02071, "fx":[-9.54725,-9.59086,-9.69852,-9.6021], "fy":[-13.59484,-13.5445,-13.63078,-13.68196]}, + {"t":2.09299, "x":3.82218, "y":2.82609, "heading":1.04641, "vx":0.15054, "vy":0.21641, "omega":0.00547, "ax":-0.57507, "ay":-0.81765, "alpha":-0.02125, "fx":[-9.52207,-9.56795,-9.67626,-9.57801], "fy":[-13.61115,-13.55931,-13.64821,-13.70089]}, + {"t":2.12595, "x":3.82683, "y":2.83278, "heading":1.04659, "vx":0.13159, "vy":0.18946, "omega":0.00477, "ax":-0.57373, "ay":-0.8186, "alpha":-0.02175, "fx":[-9.49864,-9.54591,-9.65517,-9.55548], "fy":[-13.62641,-13.57301,-13.66466,-13.7189]}, + {"t":2.15892, "x":3.83085, "y":2.83858, "heading":1.04675, "vx":0.11267, "vy":0.16247, "omega":0.00406, "ax":-0.57247, "ay":-0.8195, "alpha":-0.02214, "fx":[-9.477,-9.52516,-9.6346,-9.53428], "fy":[-13.64075,-13.58589,-13.68018,-13.73588]}, + {"t":2.19188, "x":3.83425, "y":2.84349, "heading":1.04688, "vx":0.0938, "vy":0.13546, "omega":0.00333, "ax":-0.57127, "ay":-0.82035, "alpha":-0.02229, "fx":[-9.45771,-9.50489,-9.61416,-9.51464], "fy":[-13.65433,-13.59837,-13.69476,-13.75156]}, + {"t":2.22484, "x":3.83704, "y":2.84751, "heading":1.04699, "vx":0.07497, "vy":0.10842, "omega":0.00259, "ax":-0.57014, "ay":-0.82114, "alpha":-0.02202, "fx":[-9.4413,-9.48511,-9.59294,-9.49657], "fy":[-13.66735,-13.61103,-13.70835,-13.76551]}, + {"t":2.25781, "x":3.8392, "y":2.85063, "heading":1.04708, "vx":0.05618, "vy":0.08135, "omega":0.00187, "ax":-0.56907, "ay":-0.8219, "alpha":-0.02108, "fx":[-9.4286,-9.46567,-9.56982,-9.48019], "fy":[-13.68006,-13.62468,-13.72083,-13.77704]}, + {"t":2.29077, "x":3.84074, "y":2.85287, "heading":1.04714, "vx":0.03742, "vy":0.05426, "omega":0.00117, "ax":-0.56805, "ay":-0.82261, "alpha":-0.01909, "fx":[-9.42103,-9.44572,-9.54349,-9.46597], "fy":[-13.69278,-13.64045,-13.73197,-13.78514]}, + {"t":2.32374, "x":3.84167, "y":2.85421, "heading":1.04718, "vx":0.01869, "vy":0.02714, "omega":0.00054, "ax":-0.56707, "ay":-0.82329, "alpha":-0.01645, "fx":[-9.42103,-9.47779,-9.5121,-9.40052], "fy":[-13.64826,-13.69815,-13.75749,-13.79174]}, + {"t":2.3567, "x":3.84197, "y":2.85466, "heading":1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/fetchToK.traj b/src/main/deploy/choreo/fetchToK.traj index c744ac96..94fbf74a 100644 --- a/src/main/deploy/choreo/fetchToK.traj +++ b/src/main/deploy/choreo/fetchToK.traj @@ -4,28 +4,28 @@ "snapshot":{ "waypoints":[ {"x":1.69577419757843, "y":7.228199481964111, "heading":-0.942477796076938, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.7710132598876953, "y":5.750344753265381, "heading":0.0, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.86409592628479, "y":5.6099772453308105, "heading":0.0, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":4.084460748154255, "y":5.3371419902635795, "heading":5.235987755982989, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":3.3}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":5.5}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":3.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ {"x":{"exp":"fetch.x", "val":1.69577419757843}, "y":{"exp":"fetch.y", "val":7.228199481964111}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.7710132598876953 m", "val":3.7710132598876953}, "y":{"exp":"5.750344753265381 m", "val":5.750344753265381}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"3.86409592628479 m", "val":3.86409592628479}, "y":{"exp":"5.6099772453308105 m", "val":5.6099772453308105}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"K.x", "val":4.084460748154255}, "y":{"exp":"K.y", "val":5.3371419902635795}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.3 m / s", "val":3.3}}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"maxAccel + 2.5 m / s ^ 2", "val":5.5}}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"3 m / s ^ 2", "val":3.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", @@ -34,58 +34,55 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.14436,1.73324], + "waypoints":[0.0,1.18557,1.66962], "samples":[ - {"t":0.0, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.669, "ay":-2.9013, "alpha":-0.74051, "fx":[76.99915,80.12854,78.60604,75.58611], "fy":[-51.14568,-48.29719,-45.59573,-48.41444]}, - {"t":0.03815, "x":1.69917, "y":7.22609, "heading":-0.94248, "vx":0.1781, "vy":-0.11067, "omega":-0.02825, "ax":4.67016, "ay":-2.90206, "alpha":-0.56106, "fx":[77.22128,79.57384,78.44619,76.1559], "fy":[-50.49411,-48.33124,-46.26806,-48.41064]}, - {"t":0.07629, "x":1.70936, "y":7.21976, "heading":-0.94356, "vx":0.35625, "vy":-0.22137, "omega":-0.04965, "ax":4.67003, "ay":-2.90204, "alpha":-0.40706, "fx":[77.40314,79.05332,78.30672,76.62552], "fy":[-49.89911,-48.43636,-46.8044,-48.36227]}, - {"t":0.11444, "x":1.72635, "y":7.2092, "heading":-0.94545, "vx":0.53439, "vy":-0.33207, "omega":-0.06518, "ax":4.66988, "ay":-2.902, "alpha":-0.29069, "fx":[77.50352,78.78699,78.12444,76.96374], "fy":[-49.46065,-48.32673,-47.31663,-48.39601]}, - {"t":0.15258, "x":1.75013, "y":7.19442, "heading":-0.94794, "vx":0.71252, "vy":-0.44277, "omega":-0.07626, "ax":4.6697, "ay":-2.90196, "alpha":-0.17086, "fx":[77.65847,78.33839,78.03864,77.33143], "fy":[-48.99553,-48.45917,-47.69853,-48.34421]}, - {"t":0.19073, "x":1.78071, "y":7.17542, "heading":-0.95084, "vx":0.89065, "vy":-0.55347, "omega":-0.08278, "ax":4.66949, "ay":-2.90192, "alpha":-0.08652, "fx":[77.71789,78.18639,77.8793,77.56918], "fy":[-48.67808,-48.32543,-48.10353,-48.38737]}, - {"t":0.22887, "x":1.81808, "y":7.1522, "heading":-0.954, "vx":1.06877, "vy":-0.66416, "omega":-0.08608, "ax":4.66923, "ay":-2.90186, "alpha":0.00892, "fx":[77.85982,77.77303,77.83633,77.86622], "fy":[-48.29007,-48.52996,-48.35372,-48.31701]}, - {"t":0.26702, "x":1.86224, "y":7.12475, "heading":-0.95729, "vx":1.24688, "vy":-0.77485, "omega":-0.08574, "ax":4.66891, "ay":-2.90179, "alpha":0.06501, "fx":[77.86941,77.76094,77.67437,78.00908], "fy":[-48.09,-48.3025,-48.70138,-48.39217]}, - {"t":0.30516, "x":1.9132, "y":7.09308, "heading":-0.96056, "vx":1.42498, "vy":-0.88554, "omega":-0.08326, "ax":4.66849, "ay":-2.90171, "alpha":0.1421, "fx":[78.00379,77.35876,77.67133,78.252], "fy":[-47.77499,-48.53961,-48.8506,-48.31504]}, - {"t":0.34331, "x":1.97096, "y":7.05719, "heading":-0.96373, "vx":1.60306, "vy":-0.99623, "omega":-0.07784, "ax":4.66793, "ay":-2.90158, "alpha":0.1669, "fx":[77.94324,77.53443,77.48414,78.2871], "fy":[-47.67631,-48.23567,-49.14479,-48.41529]}, - {"t":0.38145, "x":2.0355, "y":7.01708, "heading":-0.9667, "vx":1.78112, "vy":-1.10691, "omega":-0.07148, "ax":4.66716, "ay":-2.90142, "alpha":0.23177, "fx":[78.09417,77.04554,77.553,78.50435], "fy":[-47.40874,-48.60275,-49.15291,-48.29638]}, - {"t":0.4196, "x":2.10684, "y":6.97275, "heading":-0.96943, "vx":1.95915, "vy":-1.21759, "omega":-0.06263, "ax":4.66599, "ay":-2.90116, "alpha":0.22353, "fx":[77.95369,77.40229,77.34658,78.41698], "fy":[-47.43056,-48.19111,-49.39038,-48.43145]}, - {"t":0.45774, "x":2.18497, "y":6.92419, "heading":-0.97182, "vx":2.13713, "vy":-1.32825, "omega":-0.05411, "ax":4.66404, "ay":-2.90076, "alpha":0.2712, "fx":[78.08337,76.91788,77.43956,78.54814], "fy":[-47.25515,-48.49264,-49.33521,-48.33408]}, - {"t":0.49589, "x":2.26988, "y":6.87141, "heading":-0.97388, "vx":2.31505, "vy":-1.4389, "omega":-0.04376, "ax":4.66024, "ay":-2.89978, "alpha":0.26116, "fx":[77.9904,76.86525,77.39548,78.48492], "fy":[-47.34101,-48.39277,-49.28111,-48.33694]}, - {"t":0.53404, "x":2.36158, "y":6.81442, "heading":-0.97555, "vx":2.49281, "vy":-1.54952, "omega":-0.0338, "ax":4.64894, "ay":-2.89686, "alpha":0.23453, "fx":[77.77651,76.75632,77.23622,78.21341], "fy":[-47.30143,-48.52758,-49.08541,-48.24241]}, - {"t":0.57218, "x":2.46005, "y":6.7532, "heading":-0.97684, "vx":2.67015, "vy":-1.66002, "omega":-0.02485, "ax":3.48952, "ay":-2.08437, "alpha":0.2106, "fx":[58.4089,57.37542,58.03378,58.8562], "fy":[-34.04722,-34.70399,-35.43021,-34.80031]}, - {"t":0.61033, "x":2.56444, "y":6.68836, "heading":-0.97779, "vx":2.80326, "vy":-1.73953, "omega":-0.01682, "ax":0.06844, "ay":0.10963, "alpha":0.1041, "fx":[1.21534,0.76143,1.06655,1.52004], "fy":[2.43074,1.29682,1.62953,1.95269]}, - {"t":0.64847, "x":2.67143, "y":6.62209, "heading":-0.97843, "vx":2.80587, "vy":-1.73535, "omega":-0.01285, "ax":-0.03023, "ay":-0.04885, "alpha":-0.00015, "fx":[-0.68937,-0.04331,-0.77839,-0.50451], "fy":[-0.60104,-0.77368,-1.02664,-0.85595]}, - {"t":0.68662, "x":2.77843, "y":6.55586, "heading":-0.97892, "vx":2.80471, "vy":-1.73721, "omega":-0.01286, "ax":-0.04615, "ay":-0.07439, "alpha":0.02126, "fx":[-0.76223,-0.8073,-0.7774,-0.73018], "fy":[-1.36035,-0.74067,-1.447,-1.41236]}, - {"t":0.72476, "x":2.88539, "y":6.48954, "heading":-0.97941, "vx":2.80295, "vy":-1.74005, "omega":-0.01205, "ax":-0.47978, "ay":-0.76397, "alpha":-0.03181, "fx":[-8.01979,-7.88217,-7.9776,-8.11145], "fy":[-12.83811,-12.75024,-12.63593,-12.71581]}, - {"t":0.76291, "x":2.99196, "y":6.4226, "heading":-0.97987, "vx":2.78465, "vy":-1.76919, "omega":-0.01326, "ax":-1.36319, "ay":-2.0789, "alpha":-0.03456, "fx":[-22.73756,-22.60098,-22.68352,-22.8728], "fy":[-35.04932,-34.04508,-34.69175,-34.83124]}, - {"t":0.80105, "x":3.09719, "y":6.35361, "heading":-0.98038, "vx":2.73265, "vy":-1.84849, "omega":-0.01458, "ax":-2.82444, "ay":-3.92818, "alpha":-0.01193, "fx":[-46.86539,-47.56044,-46.77117,-47.13108], "fy":[-65.74372,-65.5792,-65.20095,-65.39969]}, - {"t":0.8392, "x":3.19937, "y":6.28024, "heading":-0.98093, "vx":2.62491, "vy":-1.99833, "omega":-0.01503, "ax":-5.24313, "ay":-1.17749, "alpha":-0.21225, "fx":[-87.52224,-86.84037,-87.29987,-87.93938], "fy":[-19.91948,-21.05425,-18.47634,-19.06252]}, - {"t":0.87734, "x":3.29569, "y":6.20315, "heading":-0.98151, "vx":2.42491, "vy":-2.04325, "omega":-0.02313, "ax":-4.95915, "ay":2.33176, "alpha":-0.22657, "fx":[-82.86083,-82.09957,-82.38807,-83.31814], "fy":[37.94375,38.805,39.7862,38.94263]}, - {"t":0.91549, "x":3.38458, "y":6.12691, "heading":-0.98239, "vx":2.23574, "vy":-1.9543, "omega":-0.03177, "ax":-4.83391, "ay":2.60127, "alpha":-0.25959, "fx":[-80.83046,-79.95529,-80.23875,-81.29136], "fy":[42.3775,43.07457,44.4825,43.51288]}, - {"t":0.95364, "x":3.46634, "y":6.05425, "heading":-0.9836, "vx":2.05135, "vy":-1.85508, "omega":-0.04167, "ax":-4.78479, "ay":2.69749, "alpha":-0.25924, "fx":[-80.01008,-79.14973,-79.39031,-80.49025], "fy":[43.93886,44.79658,46.0624,45.06556]}, - {"t":0.99178, "x":3.54111, "y":5.98545, "heading":-0.98519, "vx":1.86883, "vy":-1.75218, "omega":-0.05156, "ax":-4.75861, "ay":2.74691, "alpha":-0.2649, "fx":[-79.65367,-78.5316,-79.02035,-80.08963], "fy":[44.60732,46.09945,46.71852,45.73307]}, - {"t":1.02993, "x":3.60894, "y":5.92061, "heading":-0.98716, "vx":1.68732, "vy":-1.6474, "omega":-0.06167, "ax":-4.74237, "ay":2.77697, "alpha":-0.23304, "fx":[-79.30656,-78.44893,-78.73878,-79.71779], "fy":[45.38784,46.1711,47.24014,46.36419]}, - {"t":1.06807, "x":3.66985, "y":5.85979, "heading":-0.98951, "vx":1.50642, "vy":-1.54147, "omega":-0.07056, "ax":-4.73129, "ay":2.79723, "alpha":-0.20168, "fx":[-79.1264,-78.23063,-78.64036,-79.47582], "fy":[45.73479,46.91615,47.29921,46.56344]}, - {"t":1.10622, "x":3.72387, "y":5.80303, "heading":-0.9922, "vx":1.32594, "vy":-1.43477, "omega":-0.07825, "ax":-4.72326, "ay":2.81177, "alpha":-0.15767, "fx":[-78.91367,-78.31409,-78.52413,-79.18597], "fy":[46.26807,46.78806,47.50576,46.92172]}, - {"t":1.14436, "x":3.77101, "y":5.75034, "heading":-0.99518, "vx":1.14577, "vy":-1.32751, "omega":-0.08426, "ax":-2.50588, "ay":1.63694, "alpha":-0.15553, "fx":[-41.9141,-41.24427,-41.63986,-42.28896], "fy":[26.62255,27.48749,27.75319,27.28506]}, - {"t":1.17536, "x":3.80532, "y":5.70999, "heading":-0.9978, "vx":1.0681, "vy":-1.27678, "omega":-0.08908, "ax":-2.3922, "ay":1.8053, "alpha":-0.13624, "fx":[-39.9254,-39.34975,-39.69363,-40.53873], "fy":[29.86211,29.93276,30.51476,30.06451]}, - {"t":1.20635, "x":3.83728, "y":5.67128, "heading":-1.00056, "vx":0.99396, "vy":-1.22082, "omega":-0.09331, "ax":-2.29111, "ay":1.93247, "alpha":-0.12256, "fx":[-38.31235,-37.76734,-38.0834,-38.60353], "fy":[31.69979,32.3667,32.57577,32.21116]}, - {"t":1.23734, "x":3.86698, "y":5.63437, "heading":-1.00345, "vx":0.92295, "vy":-1.16093, "omega":-0.0971, "ax":-2.20392, "ay":2.03173, "alpha":-0.10653, "fx":[-36.76431,-36.32504,-36.58445,-37.27915], "fy":[33.69695,33.74888,34.18595,33.83985]}, - {"t":1.26834, "x":3.89453, "y":5.59937, "heading":-1.00646, "vx":0.85464, "vy":-1.09796, "omega":-0.10041, "ax":-2.12865, "ay":2.11077, "alpha":-0.08716, "fx":[-35.57428,-35.17343,-35.40495,-35.78135], "fy":[34.81148,35.32617,35.43043,35.17372]}, - {"t":1.29933, "x":3.91999, "y":5.56635, "heading":-1.00957, "vx":0.78867, "vy":-1.03254, "omega":-0.10311, "ax":-2.06339, "ay":2.17485, "alpha":-0.06199, "fx":[-34.40208,-34.13165,-34.29624,-34.75303], "fy":[36.21545,36.15438,36.4326,36.21254]}, - {"t":1.33032, "x":3.94345, "y":5.53539, "heading":-1.01277, "vx":0.72472, "vy":-0.96513, "omega":-0.10503, "ax":-2.00651, "ay":2.22765, "alpha":-0.04399, "fx":[-33.49706,-33.28333,-33.40784,-33.60177], "fy":[36.927,37.25365,37.2426,37.1125]}, - {"t":1.36132, "x":3.96495, "y":5.50655, "heading":-1.01602, "vx":0.66253, "vy":-0.89609, "omega":-0.10639, "ax":-1.95661, "ay":2.27178, "alpha":-0.01472, "fx":[-32.58049,-32.51963,-32.56577,-32.79736], "fy":[37.96352,37.81071,37.88988,37.81398]}, - {"t":1.39231, "x":3.98454, "y":5.47987, "heading":-1.01932, "vx":0.60189, "vy":-0.82568, "omega":-0.10685, "ax":-1.91259, "ay":2.30913, "alpha":0.01249, "fx":[-31.87597,-31.90501,-31.89746,-31.84914], "fy":[38.48193,38.63346,38.41196,38.44068]}, - {"t":1.4233, "x":4.00228, "y":5.45539, "heading":-1.02263, "vx":0.54261, "vy":-0.75411, "omega":-0.10646, "ax":-1.8735, "ay":2.34108, "alpha":0.0505, "fx":[-31.13697,-31.35837,-31.25191,-31.17438], "fy":[39.31732,39.01119,38.83068,38.93953]}, - {"t":1.4543, "x":4.01819, "y":5.43314, "heading":-1.02593, "vx":0.48454, "vy":-0.68156, "omega":-0.1049, "ax":-1.83862, "ay":2.3687, "alpha":0.08737, "fx":[-30.5655,-30.92283,-30.74095,-30.36609], "fy":[39.73415,39.65435,39.15887,39.39269]}, - {"t":1.48529, "x":4.03233, "y":5.41315, "heading":-1.02918, "vx":0.42756, "vy":-0.60814, "omega":-0.10219, "ax":-1.80731, "ay":2.39277, "alpha":0.13627, "fx":[-29.94621,-30.54894,-30.24139,-29.77126], "fy":[40.44607,39.93678,39.40796,39.75466]}, - {"t":1.51628, "x":4.04471, "y":5.39545, "heading":-1.03235, "vx":0.37154, "vy":-0.53398, "omega":-0.09796, "ax":-1.77907, "ay":2.41393, "alpha":0.18864, "fx":[-29.46487,-30.2696,-29.85597,-29.03476], "fy":[40.83291,40.45769,39.58021,40.08543]}, - {"t":1.54728, "x":4.05537, "y":5.38006, "heading":-1.03538, "vx":0.3164, "vy":-0.45917, "omega":-0.09212, "ax":-1.7535, "ay":2.43266, "alpha":0.25466, "fx":[-28.92548,-30.05319,-29.47605,-28.46496], "fy":[41.48788,40.68712,39.68271,40.34712]}, - {"t":1.57827, "x":4.06434, "y":5.367, "heading":-1.03824, "vx":0.26206, "vy":-0.38377, "omega":-0.08423, "ax":-1.73022, "ay":2.44934, "alpha":0.32785, "fx":[-28.49919,-29.91982,-29.19423,-27.7548], "fy":[41.88894,41.13382,39.71101,40.58323]}, - {"t":1.60926, "x":4.07163, "y":5.35628, "heading":-1.04085, "vx":0.20843, "vy":-0.30786, "omega":-0.07406, "ax":-1.70897, "ay":2.46428, "alpha":0.41931, "fx":[-28.00593,-29.86524,-28.91968,-27.15994], "fy":[42.54642,41.33274,39.66816,40.7661]}, - {"t":1.64026, "x":4.07727, "y":5.34793, "heading":-1.04315, "vx":0.15547, "vy":-0.23148, "omega":-0.06107, "ax":-1.68948, "ay":2.47774, "alpha":0.52226, "fx":[-27.6063,-29.8885,-28.73239,-26.42439], "fy":[43.00802,41.73677,39.54305,40.92303]}, - {"t":1.67125, "x":4.08127, "y":5.34194, "heading":-1.04504, "vx":0.1031, "vy":-0.15469, "omega":-0.04488, "ax":-1.67156, "ay":2.48992, "alpha":0.64778, "fx":[-27.16964,-30.00717,-28.57516,-25.70455], "fy":[43.62545,42.02827,39.33124,41.03815]}, - {"t":1.70224, "x":4.08367, "y":5.33834, "heading":-1.04643, "vx":0.05129, "vy":-0.07751, "omega":-0.0248, "ax":-1.65502, "ay":2.50099, "alpha":0.80031, "fx":[-26.70915,-30.31519,-28.44939,-24.88006], "fy":[44.30025,42.22982,39.07544,41.15599]}, - {"t":1.73324, "x":4.08446, "y":5.33714, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.94828, "ay":-3.38766, "alpha":-0.78978, "fx":[81.48722,84.879,83.46213,80.11359], "fy":[-59.46319,-56.3044,-53.47221,-56.64276]}, + {"t":0.03952, "x":1.69964, "y":7.22555, "heading":-0.94248, "vx":0.19555, "vy":-0.13388, "omega":-0.03121, "ax":4.94948, "ay":-3.3885, "alpha":-0.59231, "fx":[81.75465,84.29312,83.24191,80.73213], "fy":[-58.73777,-56.35596,-54.23238,-56.61238]}, + {"t":0.07904, "x":1.71123, "y":7.21762, "heading":-0.94371, "vx":0.39115, "vy":-0.26779, "omega":-0.05462, "ax":4.94935, "ay":-3.38841, "alpha":-0.42277, "fx":[81.97086,83.75813,83.04268,81.2414], "fy":[-58.08274,-56.44356,-54.85562,-56.55083]}, + {"t":0.11856, "x":1.73055, "y":7.20439, "heading":-0.94587, "vx":0.58674, "vy":-0.40169, "omega":-0.07133, "ax":4.94917, "ay":-3.38833, "alpha":-0.28495, "fx":[82.12557,83.39666,82.83269,81.64653], "fy":[-57.56032,-56.39787,-55.42467,-56.54466]}, + {"t":0.15808, "x":1.75761, "y":7.18587, "heading":-0.94869, "vx":0.78233, "vy":-0.5356, "omega":-0.08259, "ax":4.94898, "ay":-3.38822, "alpha":-0.15271, "fx":[82.30447,82.94605,82.69403,82.04363], "fy":[-57.04355,-56.50826,-55.88169,-56.4863]}, + {"t":0.1976, "x":1.79239, "y":7.16206, "heading":-0.95195, "vx":0.97791, "vy":-0.6695, "omega":-0.08862, "ax":4.94872, "ay":-3.38809, "alpha":-0.05002, "fx":[82.41079,82.70542,82.51626,82.33855], "fy":[-56.65536,-56.42857,-56.32968,-56.4974]}, + {"t":0.23711, "x":1.8349, "y":7.13295, "heading":-0.95545, "vx":1.17348, "vy":-0.80339, "omega":-0.0906, "ax":4.9484, "ay":-3.3879, "alpha":0.05449, "fx":[82.56762,82.30077,82.42737,82.65384], "fy":[-56.23008,-56.59665,-56.64335,-56.42873]}, + {"t":0.27663, "x":1.88514, "y":7.09856, "heading":-0.95903, "vx":1.36904, "vy":-0.93728, "omega":-0.08845, "ax":4.94796, "ay":-3.38767, "alpha":0.12493, "fx":[82.6169,82.20366,82.25839,82.84149], "fy":[-55.97245,-56.42665,-57.01084,-56.47356]}, + {"t":0.31615, "x":1.9431, "y":7.05887, "heading":-0.96253, "vx":1.56457, "vy":-1.07116, "omega":-0.08351, "ax":4.94735, "ay":-3.38735, "alpha":0.20501, "fx":[82.75183,81.82734,82.21659,83.08424], "fy":[-55.63369,-56.63906,-57.1937,-56.39525]}, + {"t":0.35567, "x":2.0088, "y":7.0139, "heading":-0.96583, "vx":1.76009, "vy":-1.20502, "omega":-0.07541, "ax":4.94644, "ay":-3.38686, "alpha":0.24147, "fx":[82.73072,81.88437,82.04983,83.15447], "fy":[-55.50169,-56.38646,-57.47409,-56.46707]}, + {"t":0.39519, "x":2.08222, "y":6.96363, "heading":-0.96881, "vx":1.95557, "vy":-1.33887, "omega":-0.06586, "ax":4.94494, "ay":-3.38605, "alpha":0.29035, "fx":[82.81122,81.56506,82.04739,83.2954], "fy":[-55.30383,-56.54288,-57.52581,-56.40286]}, + {"t":0.43471, "x":2.16336, "y":6.90807, "heading":-0.97141, "vx":2.15099, "vy":-1.47268, "omega":-0.05439, "ax":4.94193, "ay":-3.38449, "alpha":0.29412, "fx":[82.72885,81.61178,81.93501,83.24308], "fy":[-55.24296,-56.42692,-57.58772,-56.4136]}, + {"t":0.47423, "x":2.25223, "y":6.84723, "heading":-0.97356, "vx":2.34629, "vy":-1.60643, "omega":-0.04277, "ax":4.93323, "ay":-3.37985, "alpha":0.29064, "fx":[82.61186,81.35351,81.85768,83.11521], "fy":[-55.13614,-56.58987,-57.36804,-56.26759]}, + {"t":0.51375, "x":2.3488, "y":6.78111, "heading":-0.97525, "vx":2.54125, "vy":-1.74, "omega":-0.03128, "ax":4.55936, "ay":-3.162, "alpha":0.23406, "fx":[76.26568,75.26297,75.74239,76.73857], "fy":[-51.73703,-52.94101,-53.49838,-52.65962]}, + {"t":0.55327, "x":2.45279, "y":6.70988, "heading":-0.97649, "vx":2.72143, "vy":-1.86496, "omega":-0.02203, "ax":0.0535, "ay":0.07605, "alpha":0.14452, "fx":[0.99811,0.35202,0.78837,1.42878], "fy":[2.0839,0.5471,1.00088,1.43884]}, + {"t":0.59279, "x":2.56038, "y":6.63623, "heading":-0.97736, "vx":2.72354, "vy":-1.86195, "omega":-0.01632, "ax":-0.03412, "ay":-0.04983, "alpha":0.0668, "fx":[-0.49309,-0.87592,-0.56968,-0.33631], "fy":[-0.64609,-0.79472,-1.01517,-0.86638]}, + {"t":0.63231, "x":2.66798, "y":6.56261, "heading":-0.978, "vx":2.72219, "vy":-1.86392, "omega":-0.01368, "ax":0.01051, "ay":0.01532, "alpha":0.02755, "fx":[0.18689,0.11524,0.16362,0.23473], "fy":[0.16602,0.73941,0.03099,0.08527]}, + {"t":0.67182, "x":2.77557, "y":6.48896, "heading":-0.97855, "vx":2.72261, "vy":-1.86332, "omega":-0.01259, "ax":0.03228, "ay":0.04719, "alpha":-0.01357, "fx":[0.45647,0.76506,0.43968,0.49086], "fy":[0.82544,0.79421,0.74796,0.77908]}, + {"t":0.71134, "x":2.88319, "y":6.41536, "heading":-0.97904, "vx":2.72388, "vy":-1.86145, "omega":-0.01313, "ax":0.03821, "ay":0.05603, "alpha":0.01519, "fx":[0.63639,0.64014,0.63813,0.63284], "fy":[0.63289,1.82028,0.64356,0.63929]}, + {"t":0.75086, "x":2.99087, "y":6.34184, "heading":-0.97956, "vx":2.72539, "vy":-1.85924, "omega":-0.01253, "ax":-0.23554, "ay":-0.34357, "alpha":-0.06839, "fx":[-4.16579,-3.206,-4.16273,-4.17053], "fy":[-5.72327,-5.72989,-5.72893,-5.72658]}, + {"t":0.79038, "x":3.09839, "y":6.2681, "heading":-0.98006, "vx":2.71609, "vy":-1.87282, "omega":-0.01523, "ax":-0.95404, "ay":-1.35455, "alpha":-0.05476, "fx":[-15.9377,-15.70928,-15.86213,-16.10438], "fy":[-22.9241,-22.25112,-22.48485,-22.65895]}, + {"t":0.8299, "x":3.20498, "y":6.19303, "heading":-0.98066, "vx":2.67838, "vy":-1.92635, "omega":-0.01739, "ax":-2.67669, "ay":-3.50476, "alpha":-0.10293, "fx":[-44.5004,-44.6877,-44.32046,-44.96779], "fy":[-58.96295,-58.55057,-57.88423,-58.29268]}, + {"t":0.86942, "x":3.30874, "y":6.11417, "heading":-0.98135, "vx":2.5726, "vy":-2.06485, "omega":-0.02146, "ax":-5.53025, "ay":2.1583, "alpha":-0.27269, "fx":[-92.43907,-91.56394,-91.85441,-92.88918], "fy":[35.19946,35.04007,37.32952,36.34244]}, + {"t":0.90894, "x":3.40609, "y":6.03425, "heading":-0.98219, "vx":2.35405, "vy":-1.97956, "omega":-0.03224, "ax":-5.12524, "ay":3.08909, "alpha":-0.30165, "fx":[-85.76941,-84.72991,-84.96387,-86.27774], "fy":[50.24516,51.43899,52.75485,51.53535]}, + {"t":0.94846, "x":3.49511, "y":5.95843, "heading":-0.98347, "vx":2.15151, "vy":-1.85748, "omega":-0.04416, "ax":-5.05317, "ay":3.21832, "alpha":-0.31979, "fx":[-84.64158,-83.20744,-83.841,-85.24545], "fy":[52.32699,54.06466,54.74366,53.45625]}, + {"t":0.98798, "x":3.57619, "y":5.88754, "heading":-0.98521, "vx":1.95181, "vy":-1.73029, "omega":-0.0568, "ax":-5.02342, "ay":3.26954, "alpha":-0.32251, "fx":[-84.15708,-82.85996,-83.27764,-84.65728], "fy":[53.2425,54.45203,55.78495,54.52695]}, + {"t":1.0275, "x":3.6494, "y":5.82171, "heading":-0.98746, "vx":1.75329, "vy":-1.60109, "omega":-0.06954, "ax":-5.00715, "ay":3.29704, "alpha":-0.29768, "fx":[-83.89,-82.55323,-83.08177,-84.34235], "fy":[53.70171,55.2553,56.01144,54.87213]}, + {"t":1.06702, "x":3.71478, "y":5.76102, "heading":-0.99021, "vx":1.55541, "vy":-1.47079, "omega":-0.0813, "ax":-4.99693, "ay":3.31417, "alpha":-0.25596, "fx":[-83.6429,-82.57613,-82.93599,-84.0304], "fy":[54.25807,55.21341,56.24794,55.26301]}, + {"t":1.10653, "x":3.77235, "y":5.70548, "heading":-0.99342, "vx":1.35794, "vy":-1.33982, "omega":-0.09142, "ax":-4.98987, "ay":3.3259, "alpha":-0.20298, "fx":[-83.47693,-82.54012,-82.91635,-83.78174], "fy":[54.5693,55.69333,56.13646,55.36545]}, + {"t":1.14605, "x":3.82212, "y":5.65513, "heading":-0.99703, "vx":1.16074, "vy":-1.20838, "omega":-0.09944, "ax":-4.98473, "ay":3.33442, "alpha":-0.1392, "fx":[-83.28509,-82.69651,-82.89747,-83.49289], "fy":[55.05214,55.55774,56.12529,55.59753]}, + {"t":1.18557, "x":3.8641, "y":5.60998, "heading":-1.00096, "vx":0.96375, "vy":-1.07661, "omega":-0.10494, "ax":-2.42585, "ay":1.75396, "alpha":-0.12998, "fx":[-40.55421,-40.01843,-40.31763,-40.8605], "fy":[28.75516,29.22134,29.67801,29.29586]}, + {"t":1.21583, "x":3.89214, "y":5.57821, "heading":-1.00414, "vx":0.89036, "vy":-1.02354, "omega":-0.10887, "ax":-2.32159, "ay":1.89555, "alpha":-0.09952, "fx":[-38.76011,-38.3266,-38.57881,-39.13352], "fy":[31.38667,31.48868,31.91988,31.59659]}, + {"t":1.24608, "x":3.91802, "y":5.54811, "heading":-1.00743, "vx":0.82013, "vy":-0.9662, "omega":-0.11189, "ax":-2.23239, "ay":2.0003, "alpha":-0.0783, "fx":[-37.29182,-36.94191,-37.14078,-37.47687], "fy":[33.02714,33.4192,33.57939,33.35072]}, + {"t":1.27633, "x":3.94181, "y":5.5198, "heading":-1.01081, "vx":0.75259, "vy":-0.90568, "omega":-0.11425, "ax":-2.15759, "ay":2.08113, "alpha":-0.0475, "fx":[-35.97745,-35.7741,-35.89525,-36.2171], "fy":[34.63412,34.62623,34.83291,34.6727]}, + {"t":1.30658, "x":3.96359, "y":5.49335, "heading":-1.01427, "vx":0.68732, "vy":-0.84272, "omega":-0.11569, "ax":-2.09435, "ay":2.14505, "alpha":-0.01854, "fx":[-34.93441,-34.83533,-34.89619,-34.98145], "fy":[35.65417,35.85132,35.78877,35.73385]}, + {"t":1.33684, "x":3.98342, "y":5.46884, "heading":-1.01777, "vx":0.62396, "vy":-0.77783, "omega":-0.11625, "ax":-2.0404, "ay":2.19668, "alpha":0.01904, "fx":[-33.9596,-34.04599,-34.00973,-34.03439], "fy":[36.77287,36.59692,36.53386,36.56685]}, + {"t":1.36709, "x":4.00136, "y":5.44631, "heading":-1.02129, "vx":0.56223, "vy":-0.71137, "omega":-0.11568, "ax":-1.99394, "ay":2.23913, "alpha":0.05696, "fx":[-33.18415,-33.41161,-33.29794,-33.0585], "fy":[37.48813,37.44489,37.10526,37.26288]}, + {"t":1.39734, "x":4.01746, "y":5.42581, "heading":-1.02479, "vx":0.5019, "vy":-0.64363, "omega":-0.11395, "ax":-1.95359, "ay":2.27459, "alpha":0.10676, "fx":[-32.42628,-32.89391,-32.65766,-32.28405], "fy":[38.35548,37.95163,37.54067,37.81759]}, + {"t":1.4276, "x":4.03175, "y":5.40738, "heading":-1.02824, "vx":0.4428, "vy":-0.57482, "omega":-0.11072, "ax":-1.91827, "ay":2.3046, "alpha":0.1568, "fx":[-31.81774,-32.48018,-32.14162,-31.46706], "fy":[38.92091,38.5897,37.86229,38.29366]}, + {"t":1.45785, "x":4.04427, "y":5.39105, "heading":-1.03158, "vx":0.38477, "vy":-0.5051, "omega":-0.10598, "ax":-1.88712, "ay":2.33031, "alpha":0.22187, "fx":[-31.19415,-32.17496,-31.67528,-30.78488], "fy":[39.65396,38.96311,38.08498,38.67842]}, + {"t":1.4881, "x":4.05505, "y":5.37683, "heading":-1.03479, "vx":0.32768, "vy":-0.4346, "omega":-0.09927, "ax":-1.85945, "ay":2.35255, "alpha":0.29226, "fx":[-30.69037,-31.94756,-31.30893,-30.03794], "fy":[40.17927,39.46866,38.2096,39.00594]}, + {"t":1.51836, "x":4.06411, "y":5.36476, "heading":-1.03779, "vx":0.27142, "vy":-0.36343, "omega":-0.09043, "ax":-1.83474, "ay":2.37197, "alpha":0.36331, "fx":[-30.18733,-31.82497,-30.9721,-29.35255], "fy":[40.67975,39.8302,38.31358,39.33484]}, + {"t":1.54861, "x":4.07148, "y":5.35485, "heading":-1.04053, "vx":0.21592, "vy":-0.29167, "omega":-0.07943, "ax":-1.81254, "ay":2.38906, "alpha":0.45376, "fx":[-29.72916,-31.79413,-30.71153,-28.62169], "fy":[41.25322,40.07922,38.34015,39.6256]}, + {"t":1.57886, "x":4.07718, "y":5.34712, "heading":-1.04293, "vx":0.16108, "vy":-0.21939, "omega":-0.06571, "ax":-1.79249, "ay":2.40422, "alpha":0.57591, "fx":[-29.23657,-31.87852,-30.50001,-27.90455], "fy":[41.96301,40.3396,38.20811,39.798]}, + {"t":1.60912, "x":4.08124, "y":5.34158, "heading":-1.04492, "vx":0.10685, "vy":-0.14666, "omega":-0.04828, "ax":-1.7743, "ay":2.41774, "alpha":0.70438, "fx":[-28.80727,-31.97994,-30.34785,-27.17168], "fy":[42.6109,40.78412,38.06215,39.75331]}, + {"t":1.63937, "x":4.08366, "y":5.33825, "heading":-1.04638, "vx":0.05318, "vy":-0.07351, "omega":-0.02697, "ax":-1.75772, "ay":2.42988, "alpha":0.89161, "fx":[-28.31668,-32.31943,-30.27736,-26.2881], "fy":[43.42345,41.09707,37.57964,39.91966]}, + {"t":1.66962, "x":4.08446, "y":5.33714, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/fetchToKSlow.traj b/src/main/deploy/choreo/fetchToKSlow.traj index 4f0cdeb9..ab6ab966 100644 --- a/src/main/deploy/choreo/fetchToKSlow.traj +++ b/src/main/deploy/choreo/fetchToKSlow.traj @@ -4,8 +4,8 @@ "snapshot":{ "waypoints":[ {"x":1.69577419757843, "y":7.228199481964111, "heading":-0.942477796076938, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.6935606002807617, "y":5.774015426635742, "heading":0.0, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":4.141549248154255, "y":5.238261807735684, "heading":5.235987755982989, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.6935606002807617, "y":5.774015426635742, "heading":0.0, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":4.084460748154255, "y":5.3371419902635795, "heading":5.235987755982989, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -18,7 +18,7 @@ "params":{ "waypoints":[ {"x":{"exp":"fetch.x", "val":1.69577419757843}, "y":{"exp":"fetch.y", "val":7.228199481964111}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.6935606002807617 m", "val":3.6935606002807617}, "y":{"exp":"5.774015426635742 m", "val":5.774015426635742}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"3.6935606002807617 m", "val":3.6935606002807617}, "y":{"exp":"5.774015426635742 m", "val":5.774015426635742}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"K.x", "val":4.084460748154255}, "y":{"exp":"K.y", "val":5.3371419902635795}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -34,76 +34,73 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.23874,2.42123], + "waypoints":[0.0,1.25,2.33331], "samples":[ - {"t":0.0, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.17298, "ay":-2.74966, "alpha":-0.37498, "fx":[69.15784,70.77483,69.91761,68.39585], "fy":[-47.20436,-45.83397,-44.47444,-45.82899]}, - {"t":0.03996, "x":1.69911, "y":7.226, "heading":-0.94248, "vx":0.16675, "vy":-0.10987, "omega":-0.01498, "ax":4.17396, "ay":-2.75033, "alpha":-0.28057, "fx":[69.27888,70.47213,69.85227,68.70817], "fy":[-46.87728,-45.84915,-44.82018,-45.84024]}, - {"t":0.07992, "x":1.7091, "y":7.21942, "heading":-0.94308, "vx":0.33354, "vy":-0.21978, "omega":-0.0262, "ax":4.17385, "ay":-2.7503, "alpha":-0.2012, "fx":[69.37206,70.18469,69.79062,68.95701], "fy":[-46.57437,-45.91508,-45.07757,-45.81757]}, - {"t":0.11988, "x":1.72576, "y":7.20844, "heading":-0.94412, "vx":0.50032, "vy":-0.32968, "omega":-0.03423, "ax":4.17373, "ay":-2.75026, "alpha":-0.14307, "fx":[69.41402,70.05217,69.70047,69.12948], "fy":[-46.3632,-45.8404,-45.33535,-45.84297]}, - {"t":0.15984, "x":1.74909, "y":7.19307, "heading":-0.94549, "vx":0.6671, "vy":-0.43957, "omega":-0.03995, "ax":4.17358, "ay":-2.75021, "alpha":-0.08384, "fx":[69.49,69.81546,69.66495,69.31595], "fy":[-46.13253,-45.92356,-45.50791,-45.81479]}, - {"t":0.1998, "x":1.77908, "y":7.17331, "heading":-0.94709, "vx":0.83387, "vy":-0.54947, "omega":-0.0433, "ax":4.17341, "ay":-2.75015, "alpha":-0.04467, "fx":[69.50701,69.75578,69.58467,69.42719], "fy":[-45.9922,-45.82938,-45.70721,-45.84616]}, - {"t":0.23976, "x":1.81573, "y":7.14916, "heading":-0.94882, "vx":1.00064, "vy":-0.65937, "omega":-0.04509, "ax":4.17319, "ay":-2.75009, "alpha":0.0014, "fx":[69.5744,69.54288,69.57003,69.57287], "fy":[-45.80813,-45.93394,-45.8172,-45.81116]}, - {"t":0.27972, "x":1.85905, "y":7.12061, "heading":-0.95062, "vx":1.1674, "vy":-0.76926, "omega":-0.04503, "ax":4.17292, "ay":-2.75, "alpha":0.02576, "fx":[69.56843,69.54706,69.4918,69.63492], "fy":[-45.72246,-45.81651,-45.97611,-45.84938]}, - {"t":0.31967, "x":1.90903, "y":7.08768, "heading":-0.95242, "vx":1.33415, "vy":-0.87914, "omega":-0.044, "ax":4.17257, "ay":-2.74989, "alpha":0.06247, "fx":[69.63215,69.33814,69.49671,69.75177], "fy":[-45.56676,-45.95934,-46.02966,-45.80133]}, - {"t":0.35963, "x":1.96567, "y":7.05035, "heading":-0.95418, "vx":1.50088, "vy":-0.98903, "omega":-0.04151, "ax":4.1721, "ay":-2.74973, "alpha":0.0717, "fx":[69.59292,69.4287,69.40639,69.75956], "fy":[-45.53633,-45.79096,-46.16481,-45.85474]}, - {"t":0.39959, "x":2.02897, "y":7.00864, "heading":-0.95584, "vx":1.66759, "vy":-1.09891, "omega":-0.03864, "ax":4.17143, "ay":-2.74952, "alpha":0.102, "fx":[69.65961,69.18863,69.43807,69.85689], "fy":[-45.39811,-45.99653,-46.15413,-45.7841]}, - {"t":0.43955, "x":2.09894, "y":6.96253, "heading":-0.95738, "vx":1.83428, "vy":-1.20877, "omega":-0.03456, "ax":4.17043, "ay":-2.74919, "alpha":0.09572, "fx":[69.57994,69.3621,69.33108,69.80332], "fy":[-45.42219,-45.7746,-46.26278,-45.85123]}, - {"t":0.47951, "x":2.17557, "y":6.91203, "heading":-0.95876, "vx":2.00093, "vy":-1.31863, "omega":-0.03074, "ax":4.16872, "ay":-2.74864, "alpha":0.11939, "fx":[69.63736,69.09145,69.37178,69.86192], "fy":[-45.29156,-46.0334,-46.19285,-45.75633]}, - {"t":0.51947, "x":2.25885, "y":6.85715, "heading":-0.95999, "vx":2.16751, "vy":-1.42846, "omega":-0.02597, "ax":4.16521, "ay":-2.74746, "alpha":0.08221, "fx":[69.43307,69.44658,69.17641,69.67218], "fy":[-45.38656,-45.73348,-46.24941,-45.82601]}, - {"t":0.55943, "x":2.34879, "y":6.79787, "heading":-0.96103, "vx":2.33395, "vy":-1.53825, "omega":-0.02268, "ax":4.15345, "ay":-2.74382, "alpha":0.08994, "fx":[69.27625,69.13232,69.05453,69.48114], "fy":[-45.48458,-45.32583,-46.25528,-45.88668]}, - {"t":0.59939, "x":2.44537, "y":6.73422, "heading":-0.96193, "vx":2.49992, "vy":-1.64789, "omega":-0.01909, "ax":0.11941, "ay":-0.04859, "alpha":0.0918, "fx":[2.02938,1.71928,1.90474,2.30852], "fy":[-0.48538,-0.74921,-1.13729,-0.86809]}, - {"t":0.63935, "x":2.54536, "y":6.66833, "heading":-0.9627, "vx":2.50469, "vy":-1.64983, "omega":-0.01542, "ax":0.05028, "ay":0.07636, "alpha":0.0437, "fx":[0.86458,0.69044,0.81161,0.98589], "fy":[1.46333,1.20835,1.14464,1.27554]}, - {"t":0.67931, "x":2.64548, "y":6.60246, "heading":-0.96331, "vx":2.5067, "vy":-1.64678, "omega":-0.01368, "ax":0.00443, "ay":0.00673, "alpha":-0.00005, "fx":[0.03794,0.16083,0.0216,0.07533], "fy":[0.15171,0.11938,0.07251,0.10496]}, - {"t":0.71927, "x":2.74565, "y":6.53667, "heading":-0.96386, "vx":2.50687, "vy":-1.64651, "omega":-0.01368, "ax":-0.02317, "ay":-0.03513, "alpha":0.00001, "fx":[-0.39002,-0.36543,-0.3827,-0.40679], "fy":[-0.72679,-0.22965,-0.68445,-0.70177]}, - {"t":0.75923, "x":2.84581, "y":6.47084, "heading":-0.96441, "vx":2.50595, "vy":-1.64792, "omega":-0.01368, "ax":-0.36388, "ay":-0.54822, "alpha":-0.04266, "fx":[-6.13535,-5.81056,-6.10085,-6.21607], "fy":[-9.22839,-9.1553,-9.04894,-9.12184]}, - {"t":0.79919, "x":2.94565, "y":6.40456, "heading":-0.96495, "vx":2.49141, "vy":-1.66982, "omega":-0.01538, "ax":-1.19826, "ay":-1.73422, "alpha":-0.05252, "fx":[-19.99598,-19.80317,-19.93008,-20.16821], "fy":[-29.24126,-28.59638,-28.8126,-28.98427]}, - {"t":0.83915, "x":3.04425, "y":6.33645, "heading":-0.96557, "vx":2.44353, "vy":-1.73912, "omega":-0.01748, "ax":-3.57368, "ay":-2.62638, "alpha":-0.03445, "fx":[-59.38996,-59.9386,-59.28608,-59.67163], "fy":[-44.13915,-43.83931,-43.43191,-43.71167]}, - {"t":0.87911, "x":3.13904, "y":6.26486, "heading":-0.96627, "vx":2.30072, "vy":-1.84407, "omega":-0.01886, "ax":-4.27365, "ay":2.55811, "alpha":-0.08858, "fx":[-71.26663,-71.1682,-71.03873,-71.48512], "fy":[42.21587,42.59324,43.08206,42.67882]}, - {"t":0.91906, "x":3.22756, "y":6.19321, "heading":-0.96702, "vx":2.12995, "vy":-1.74185, "omega":-0.0224, "ax":-4.22626, "ay":2.65384, "alpha":-0.10684, "fx":[-70.51016,-70.28923,-70.24105,-70.75854], "fy":[43.78021,44.15623,44.73498,44.28138]}, - {"t":0.95902, "x":3.3093, "y":6.12573, "heading":-0.96791, "vx":1.96107, "vy":-1.6358, "omega":-0.02667, "ax":-4.2098, "ay":2.68584, "alpha":-0.12865, "fx":[-70.33124,-69.7372,-70.05124,-70.5819], "fy":[44.17577,45.07084,45.15228,44.68759]}, - {"t":0.99898, "x":3.3843, "y":6.0625, "heading":-0.96898, "vx":1.79285, "vy":-1.52848, "omega":-0.03181, "ax":-4.20147, "ay":2.70182, "alpha":-0.11767, "fx":[-70.13882,-69.76376,-69.8575,-70.38577], "fy":[44.57333,44.97667,45.53007,45.07199]}, - {"t":1.03894, "x":3.45259, "y":6.00358, "heading":-0.97025, "vx":1.62497, "vy":-1.42052, "omega":-0.03651, "ax":-4.19643, "ay":2.71141, "alpha":-0.11889, "fx":[-70.0958,-69.55683,-69.83256,-70.32442], "fy":[44.67883,45.39186,45.57404,45.14708]}, - {"t":1.0789, "x":3.51417, "y":5.94899, "heading":-0.97171, "vx":1.45728, "vy":-1.31217, "omega":-0.04126, "ax":-4.19304, "ay":2.71782, "alpha":-0.10182, "fx":[-69.99414,-69.6344,-69.75423,-70.20098], "fy":[44.91171,45.2626,45.71446,45.33053]}, - {"t":1.11886, "x":3.56905, "y":5.89872, "heading":-0.97336, "vx":1.28973, "vy":-1.20357, "omega":-0.04533, "ax":-4.19061, "ay":2.72241, "alpha":-0.09134, "fx":[-69.96765,-69.54783,-69.76376,-70.14259], "fy":[44.98251,45.53449,45.66666,45.34108]}, - {"t":1.15882, "x":3.61725, "y":5.8528, "heading":-0.97517, "vx":1.12227, "vy":-1.09478, "omega":-0.04898, "ax":-4.18878, "ay":2.72585, "alpha":-0.06917, "fx":[-69.89399,-69.64148,-69.73129,-70.03292], "fy":[45.174,45.4111,45.71297,45.45637]}, - {"t":1.19878, "x":3.65875, "y":5.81123, "heading":-0.97713, "vx":0.95489, "vy":-0.98586, "omega":-0.05174, "ax":-4.18735, "ay":2.72853, "alpha":-0.04928, "fx":[-69.86435,-69.62649,-69.75387,-69.95981], "fy":[45.25269,45.61193,45.62147,45.447]}, - {"t":1.23874, "x":3.69356, "y":5.77402, "heading":-0.97919, "vx":0.78757, "vy":-0.87683, "omega":-0.05371, "ax":-0.8158, "ay":0.57057, "alpha":-0.03839, "fx":[-13.59252,-13.55026,-13.52192,-13.73143], "fy":[9.34563,9.47906,9.67697,9.54288]}, - {"t":1.27159, "x":3.71899, "y":5.74552, "heading":-0.98096, "vx":0.76077, "vy":-0.85809, "omega":-0.05497, "ax":-0.78774, "ay":0.6129, "alpha":-0.04238, "fx":[-13.16102,-12.98746,-13.10203,-13.27481], "fy":[10.03923,10.26311,10.34366,10.22088]}, - {"t":1.30443, "x":3.74355, "y":5.71767, "heading":-0.98276, "vx":0.7349, "vy":-0.83796, "omega":-0.05636, "ax":-0.76421, "ay":0.64239, "alpha":-0.033, "fx":[-12.74644,-12.66577,-12.69088,-12.85269], "fy":[10.58077,10.68313,10.83634,10.73327]}, - {"t":1.33728, "x":3.76728, "y":5.69049, "heading":-0.98461, "vx":0.7098, "vy":-0.81686, "omega":-0.05745, "ax":-0.74532, "ay":0.6645, "alpha":-0.03344, "fx":[-12.44823,-12.30984,-12.40048,-12.53813], "fy":[10.93476,11.11906,11.175,11.07852]}, - {"t":1.37013, "x":3.79019, "y":5.66402, "heading":-0.9865, "vx":0.68531, "vy":-0.79503, "omega":-0.05855, "ax":-0.7299, "ay":0.68162, "alpha":-0.02679, "fx":[-12.17387,-12.10719,-12.12818,-12.25917], "fy":[11.25905,11.34146,11.46595,11.38289]}, - {"t":1.40297, "x":3.81231, "y":5.63827, "heading":-0.98843, "vx":0.66134, "vy":-0.77264, "omega":-0.05943, "ax":-0.7171, "ay":0.69526, "alpha":-0.02746, "fx":[-11.97397,-11.85933,-11.93382,-12.04777], "fy":[11.47194,11.62713,11.66906,11.59026]}, - {"t":1.43582, "x":3.83365, "y":5.61327, "heading":-0.99038, "vx":0.63778, "vy":-0.7498, "omega":-0.06033, "ax":-0.70632, "ay":0.70635, "alpha":-0.02239, "fx":[-11.78007,-11.72406,-11.74127,-11.85074], "fy":[11.68821,11.75672,11.86114,11.79206]}, - {"t":1.46867, "x":3.85421, "y":5.58902, "heading":-0.99236, "vx":0.61458, "vy":-0.7266, "omega":-0.06106, "ax":-0.69713, "ay":0.71555, "alpha":-0.02312, "fx":[-11.63826,-11.54102,-11.60368,-11.7003], "fy":[11.82829,11.9607,11.99409,11.92814]}, - {"t":1.50151, "x":3.87403, "y":5.56554, "heading":-0.99436, "vx":0.59169, "vy":-0.7031, "omega":-0.06182, "ax":-0.6892, "ay":0.72329, "alpha":-0.01899, "fx":[-11.49404,-11.44664,-11.46055,-11.55359], "fy":[11.98349,12.04136,12.13039,12.07202]}, - {"t":1.53436, "x":3.89309, "y":5.54283, "heading":-0.9964, "vx":0.56905, "vy":-0.67934, "omega":-0.06245, "ax":-0.6823, "ay":0.72989, "alpha":-0.01962, "fx":[-11.38883,-11.3057,-11.35882,-11.44137], "fy":[12.08175,12.19639,12.22246,12.16677]}, - {"t":1.56721, "x":3.91141, "y":5.52091, "heading":-0.99845, "vx":0.54664, "vy":-0.65537, "omega":-0.06309, "ax":-0.67624, "ay":0.73558, "alpha":-0.01599, "fx":[-11.27702,-11.23841,-11.2481,-11.32717], "fy":[12.19955,12.24836,12.32418,12.27492]}, - {"t":1.60006, "x":3.929, "y":5.49978, "heading":-1.00052, "vx":0.52442, "vy":-0.6312, "omega":-0.06362, "ax":-0.67088, "ay":0.74054, "alpha":-0.01645, "fx":[-11.19629,-11.12609,-11.17056,-11.24023], "fy":[12.27231,12.37107,12.39043,12.34393]}, - {"t":1.6329, "x":3.94587, "y":5.47945, "heading":-1.00261, "vx":0.50239, "vy":-0.60688, "omega":-0.06416, "ax":-0.66611, "ay":0.7449, "alpha":-0.01305, "fx":[-11.1068,-11.07736,-11.08241,-11.14803], "fy":[12.36564,12.40577,12.46874,12.42821]}, - {"t":1.66575, "x":3.96201, "y":5.45992, "heading":-1.00472, "vx":0.48051, "vy":-0.58241, "omega":-0.06459, "ax":-0.66182, "ay":0.74876, "alpha":-0.01319, "fx":[-11.04295,-10.98611,-11.0218,-11.07817], "fy":[12.42215,12.50608,12.51739,12.4801]}, - {"t":1.6986, "x":3.97743, "y":5.44119, "heading":-1.00684, "vx":0.45877, "vy":-0.55782, "omega":-0.06502, "ax":-0.65796, "ay":0.7522, "alpha":-0.00979, "fx":[-10.96936,-10.95104,-10.95006,-11.00115], "fy":[12.49899,12.52982,12.57878,12.54758]}, - {"t":1.73144, "x":3.99215, "y":5.42327, "heading":-1.00897, "vx":0.43716, "vy":-0.53311, "omega":-0.06534, "ax":-0.65446, "ay":0.75529, "alpha":-0.00947, "fx":[-10.91754,-10.87603,-10.90187,-10.94295], "fy":[12.545,12.61417,12.61442,12.58739]}, - {"t":1.76429, "x":4.00616, "y":5.40617, "heading":-1.01112, "vx":0.41566, "vy":-0.5083, "omega":-0.06565, "ax":-0.65128, "ay":0.75807, "alpha":-0.00579, "fx":[-10.85542,-10.85204,-10.84246,-10.87621], "fy":[12.61075,12.63063,12.66274,12.64252]}, - {"t":1.79714, "x":4.01946, "y":5.38988, "heading":-1.01328, "vx":0.39427, "vy":-0.4834, "omega":-0.06584, "ax":-0.64837, "ay":0.76059, "alpha":-0.00483, "fx":[-10.81239,-10.79011,-10.80389,-10.82577], "fy":[12.65038,12.70316,12.68797,12.67342]}, - {"t":1.82998, "x":4.03206, "y":5.37441, "heading":-1.01544, "vx":0.37297, "vy":-0.45842, "omega":-0.066, "ax":-0.6457, "ay":0.76289, "alpha":-0.00058, "fx":[-10.75873,-10.77555,-10.75428,-10.76563], "fy":[12.70901,12.71498,12.72523,12.71894]}, - {"t":1.86283, "x":4.04396, "y":5.35977, "heading":-1.01761, "vx":0.35176, "vy":-0.43336, "omega":-0.06602, "ax":-0.64325, "ay":0.76499, "alpha":0.00135, "fx":[-10.72196,-10.72541,-10.7234,-10.71958], "fy":[12.74561,12.77865,12.74115,12.74287]}, - {"t":1.89568, "x":4.05517, "y":5.34595, "heading":-1.01977, "vx":0.33063, "vy":-0.40823, "omega":-0.06598, "ax":-0.64098, "ay":0.76692, "alpha":0.00661, "fx":[-10.67434,-10.72017,-10.6819,-10.66262], "fy":[12.80055,12.78779,12.76805,12.78051]}, - {"t":1.92852, "x":4.06568, "y":5.33295, "heading":-1.02194, "vx":0.30958, "vy":-0.38304, "omega":-0.06576, "ax":-0.63887, "ay":0.7687, "alpha":0.00998, "fx":[-10.6419,-10.68141,-10.6577,-10.61783], "fy":[12.8373,12.84503,12.77449,12.79853]}, - {"t":1.96137, "x":4.07551, "y":5.32078, "heading":-1.0241, "vx":0.28859, "vy":-0.35779, "omega":-0.06543, "ax":-0.63692, "ay":0.77034, "alpha":0.01687, "fx":[-10.59833,-10.68623,-10.62347,-10.56058], "fy":[12.89205,12.85302,12.79048,12.82924]}, - {"t":1.99422, "x":4.08464, "y":5.30945, "heading":-1.02625, "vx":0.26767, "vy":-0.33249, "omega":-0.06488, "ax":-0.6351, "ay":0.77186, "alpha":0.02243, "fx":[-10.56836,-10.66009,-10.60546,-10.5134], "fy":[12.93257,12.90594,12.78596,12.84175]}, - {"t":2.02706, "x":4.09309, "y":5.29894, "heading":-1.02838, "vx":0.24681, "vy":-0.30714, "omega":-0.06414, "ax":-0.6334, "ay":0.77328, "alpha":0.03191, "fx":[-10.52684,-10.67741,-10.57826,-10.45156], "fy":[12.9913,12.91431,12.78907,12.86581]}, - {"t":2.05991, "x":4.10086, "y":5.28927, "heading":-1.03049, "vx":0.22601, "vy":-0.28174, "omega":-0.06309, "ax":-0.63181, "ay":0.77459, "alpha":0.0408, "fx":[-10.49748,-10.66648,-10.56673,-10.3974], "fy":[13.04029,12.96505,12.77044,12.87254]}, - {"t":2.09276, "x":4.10794, "y":5.28044, "heading":-1.03256, "vx":0.20525, "vy":-0.25629, "omega":-0.06175, "ax":-0.63032, "ay":0.77582, "alpha":0.05431, "fx":[-10.45584,-10.70042,-10.54714,-10.32532], "fy":[13.1085,12.97557,12.75678,12.88949]}, - {"t":2.12561, "x":4.11434, "y":5.27244, "heading":-1.03459, "vx":0.18455, "vy":-0.23081, "omega":-0.05997, "ax":-0.62892, "ay":0.77697, "alpha":0.06832, "fx":[-10.42477,-10.70979,-10.54308,-10.25769], "fy":[13.17255,13.02667,12.71855,12.88935]}, - {"t":2.15845, "x":4.12006, "y":5.26527, "heading":-1.03656, "vx":0.16389, "vy":-0.20529, "omega":-0.05772, "ax":-0.6276, "ay":0.77805, "alpha":0.08807, "fx":[-10.38025,-10.76733,-10.5326,-10.16725], "fy":[13.25812,13.04157,12.68154,12.89791]}, - {"t":2.1913, "x":4.12511, "y":5.25895, "heading":-1.03846, "vx":0.14328, "vy":-0.17973, "omega":-0.05483, "ax":-0.62636, "ay":0.77907, "alpha":0.10995, "fx":[-10.34441,-10.80536,-10.5381,-10.07667], "fy":[13.34696,13.09615,12.61488,12.88883]}, - {"t":2.22415, "x":4.12948, "y":5.25347, "heading":-1.04026, "vx":0.1227, "vy":-0.15414, "omega":-0.05122, "ax":-0.62519, "ay":0.78002, "alpha":0.13938, "fx":[-10.29323,-10.89767,-10.53952,-9.95582], "fy":[13.46151,13.1188,12.54381,12.88643]}, - {"t":2.25699, "x":4.13317, "y":5.24882, "heading":-1.04194, "vx":0.10217, "vy":-0.12852, "omega":-0.04664, "ax":-0.62408, "ay":0.78093, "alpha":0.17336, "fx":[-10.24831,-10.97767,-10.55816,-9.82801], "fy":[13.58981,13.18127,12.43471,12.86488]}, - {"t":2.28984, "x":4.13619, "y":5.24502, "heading":-1.04347, "vx":0.08167, "vy":-0.10287, "omega":-0.04095, "ax":-0.62302, "ay":0.78178, "alpha":0.21776, "fx":[-10.1851,-11.12213,-10.57617,-9.65856], "fy":[13.75099,13.21649,12.31268,12.8473]}, - {"t":2.32269, "x":4.13854, "y":5.24207, "heading":-1.04482, "vx":0.0612, "vy":-0.07719, "omega":-0.03379, "ax":-0.62202, "ay":0.78258, "alpha":0.27041, "fx":[-10.12476,-11.26528,-10.6137,-9.47163], "fy":[13.94113,13.29319,12.13924,12.80764]}, - {"t":2.35553, "x":4.14021, "y":5.23995, "heading":-1.04593, "vx":0.04077, "vy":-0.05148, "omega":-0.02491, "ax":-0.62108, "ay":0.78335, "alpha":0.33796, "fx":[-10.04162,-11.48926,-10.6556,-9.22562], "fy":[14.17606,13.34816,11.93972,12.7682]}, - {"t":2.38838, "x":4.14121, "y":5.23868, "heading":-1.04674, "vx":0.02037, "vy":-0.02575, "omega":-0.01381, "ax":-0.62017, "ay":0.78407, "alpha":0.42046, "fx":[-9.95051,-11.74846,-10.71732,-8.93562], "fy":[14.46195,13.43262,11.67778,12.70813]}, - {"t":2.42123, "x":4.14155, "y":5.23826, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.1458, "ay":-2.79041, "alpha":-0.38904, "fx":[68.71227,70.34463,69.48054,67.89661], "fy":[-47.93678,-46.54462,-45.08908,-46.48899]}, + {"t":0.04032, "x":1.69914, "y":7.22593, "heading":-0.94248, "vx":0.16717, "vy":-0.11252, "omega":-0.01569, "ax":4.14678, "ay":-2.7911, "alpha":-0.29388, "fx":[68.82713,70.04131,69.41996,68.21122], "fy":[-47.60708,-46.55042,-45.43877,-46.50868]}, + {"t":0.08064, "x":1.70926, "y":7.21913, "heading":-0.94311, "vx":0.33438, "vy":-0.22506, "omega":-0.02754, "ax":4.14668, "ay":-2.79105, "alpha":-0.21711, "fx":[68.89199,69.81705,69.3392,68.44432], "fy":[-47.34296,-46.47514,-45.74882,-46.53463]}, + {"t":0.12097, "x":1.72611, "y":7.20778, "heading":-0.94422, "vx":0.50158, "vy":-0.3376, "omega":-0.03629, "ax":4.14655, "ay":-2.79099, "alpha":-0.13947, "fx":[69.00664,69.45776,69.3238,68.69587], "fy":[-47.05949,-46.5979,-45.94236,-46.498]}, + {"t":0.16129, "x":1.74971, "y":7.1919, "heading":-0.94568, "vx":0.66878, "vy":-0.45014, "omega":-0.04192, "ax":4.1464, "ay":-2.79091, "alpha":-0.09766, "fx":[68.99803,69.45751,69.20557,68.81313], "fy":[-46.92679,-46.38582,-46.21252,-46.56756]}, + {"t":0.20161, "x":1.78004, "y":7.17148, "heading":-0.94737, "vx":0.83597, "vy":-0.56268, "omega":-0.04585, "ax":4.14621, "ay":-2.79083, "alpha":-0.0317, "fx":[69.11746,69.08938,69.22736,69.02741], "fy":[-46.66456,-46.60596,-46.31979,-46.49711]}, + {"t":0.24193, "x":1.81712, "y":7.14652, "heading":-0.94922, "vx":1.00316, "vy":-0.67521, "omega":-0.04713, "ax":4.14599, "ay":-2.79072, "alpha":-0.00659, "fx":[69.09123,69.14449,69.11428,69.09631], "fy":[-46.56501,-46.46056,-46.51131,-46.54323]}, + {"t":0.28226, "x":1.86094, "y":7.11703, "heading":-0.95112, "vx":1.17034, "vy":-0.78774, "omega":-0.0474, "ax":4.14568, "ay":-2.79059, "alpha":0.03782, "fx":[69.15718,68.93531,69.10515,69.22866], "fy":[-46.38395,-46.55778,-46.61787,-46.51166]}, + {"t":0.32258, "x":1.9115, "y":7.083, "heading":-0.95303, "vx":1.3375, "vy":-0.90026, "omega":-0.04587, "ax":4.1453, "ay":-2.79039, "alpha":0.0612, "fx":[69.1592,68.90221,69.0397,69.29942], "fy":[-46.28304,-46.53603,-46.7242,-46.5149]}, + {"t":0.3629, "x":1.96881, "y":7.04443, "heading":-0.95488, "vx":1.50465, "vy":-1.01278, "omega":-0.0434, "ax":4.14473, "ay":-2.79016, "alpha":0.07525, "fx":[69.13223,68.95513,68.95867,69.31689], "fy":[-46.19696,-46.46439,-46.84493,-46.53592]}, + {"t":0.40322, "x":2.03285, "y":7.00132, "heading":-0.95663, "vx":1.67177, "vy":-1.12528, "omega":-0.04037, "ax":4.14392, "ay":-2.78975, "alpha":0.10466, "fx":[69.18939,68.73145,68.9749,69.41299], "fy":[-46.08953,-46.58733,-46.85141,-46.48692]}, + {"t":0.44355, "x":2.10362, "y":6.95368, "heading":-0.95826, "vx":1.83887, "vy":-1.23777, "omega":-0.03615, "ax":4.14251, "ay":-2.78917, "alpha":0.11265, "fx":[69.15912,68.73267,68.9206,69.40229], "fy":[-46.05581,-46.49672,-46.91841,-46.50544]}, + {"t":0.48387, "x":2.18114, "y":6.9015, "heading":-0.95972, "vx":2.0059, "vy":-1.35024, "omega":-0.03161, "ax":4.13976, "ay":-2.78793, "alpha":0.12205, "fx":[69.15191,68.57529,68.89878,69.40559], "fy":[-45.9388,-46.72427,-46.82765,-46.40306]}, + {"t":0.52419, "x":2.26539, "y":6.84479, "heading":-0.96099, "vx":2.17283, "vy":-1.46266, "omega":-0.02669, "ax":4.13156, "ay":-2.78446, "alpha":0.09988, "fx":[68.92502,68.70522,68.68066,69.1738], "fy":[-45.98506,-46.3771,-46.85907,-46.4415]}, + {"t":0.56451, "x":2.35636, "y":6.78355, "heading":-0.96207, "vx":2.33942, "vy":-1.57493, "omega":-0.02266, "ax":3.68743, "ay":-2.47512, "alpha":0.09668, "fx":[61.50384,61.33057,61.30311,61.73331], "fy":[-41.10817,-40.56635,-41.85459,-41.5071]}, + {"t":0.60484, "x":2.45369, "y":6.71803, "heading":-0.96298, "vx":2.48811, "vy":-1.67474, "omega":-0.01876, "ax":0.00076, "ay":-0.00005, "alpha":0.05329, "fx":[-0.04058,0.04193,-0.14018,0.1893], "fy":[0.28044,0.04951,-0.28217,-0.05091]}, + {"t":0.64516, "x":2.55402, "y":6.6505, "heading":-0.96374, "vx":2.48814, "vy":-1.67474, "omega":-0.01661, "ax":0.0376, "ay":0.05589, "alpha":0.03495, "fx":[0.64705,0.51337,0.60663,0.74035], "fy":[1.02883,1.01101,0.79541,0.8914]}, + {"t":0.68548, "x":2.65438, "y":6.58302, "heading":-0.96441, "vx":2.48966, "vy":-1.67248, "omega":-0.0152, "ax":0.0065, "ay":0.00966, "alpha":0.01337, "fx":[0.15272,-0.02806,0.15117,0.15741], "fy":[0.16156,0.1613,0.16027,0.16092]}, + {"t":0.7258, "x":2.75477, "y":6.51559, "heading":-0.96502, "vx":2.48992, "vy":-1.67209, "omega":-0.01466, "ax":-0.00006, "ay":-0.00001, "alpha":-0.00426, "fx":[-0.006,0.02514,0.00351,-0.02673], "fy":[-0.0702,0.14495,-0.02951,-0.04598]}, + {"t":0.76613, "x":2.85517, "y":6.44816, "heading":-0.96561, "vx":2.48992, "vy":-1.6721, "omega":-0.01484, "ax":-0.28409, "ay":-0.42003, "alpha":-0.07052, "fx":[-4.93307,-4.11875,-4.91305,-4.9775], "fy":[-7.06131,-7.01472,-6.94082,-6.98993]}, + {"t":0.80645, "x":2.95534, "y":6.3804, "heading":-0.96621, "vx":2.47846, "vy":-1.68903, "omega":-0.01768, "ax":-1.01598, "ay":-1.45238, "alpha":-0.0348, "fx":[-16.95889,-16.77938,-16.89525,-17.1101], "fy":[-24.68215,-23.35382,-24.33245,-24.47381]}, + {"t":0.84677, "x":3.05445, "y":6.31111, "heading":-0.96693, "vx":2.43749, "vy":-1.7476, "omega":-0.01908, "ax":-4.42748, "ay":-0.77253, "alpha":-0.05316, "fx":[-73.77132,-73.8124,-73.66803,-73.96381], "fy":[-13.18938,-12.85072,-12.61406,-12.85649]}, + {"t":0.88709, "x":3.14914, "y":6.24002, "heading":-0.9677, "vx":2.25897, "vy":-1.77875, "omega":-0.02123, "ax":-4.222, "ay":2.64501, "alpha":-0.10366, "fx":[-70.49174,-70.01392,-70.2763,-70.73273], "fy":[43.499,44.6091,44.31579,43.94025]}, + {"t":0.92742, "x":3.23679, "y":6.17044, "heading":-0.96855, "vx":2.08873, "vy":-1.67209, "omega":-0.02541, "ax":-4.18644, "ay":2.71725, "alpha":-0.12339, "fx":[-69.89259,-69.45794,-69.63241,-70.16104], "fy":[44.819,45.2492,45.78111,45.33154]}, + {"t":0.96774, "x":3.31761, "y":6.10523, "heading":-0.96958, "vx":1.91992, "vy":-1.56253, "omega":-0.03038, "ax":-4.17411, "ay":2.74162, "alpha":-0.12892, "fx":[-69.70134,-69.22003,-69.42274,-69.97734], "fy":[45.21855,45.6487,46.19891,45.73971]}, + {"t":1.00806, "x":3.39163, "y":6.04445, "heading":-0.9708, "vx":1.75161, "vy":-1.45198, "omega":-0.03558, "ax":-4.16783, "ay":2.75388, "alpha":-0.1272, "fx":[-69.59795,-69.11521,-69.32544,-69.86447], "fy":[45.42469,45.86674,46.39375,45.93811]}, + {"t":1.04838, "x":3.45887, "y":5.98814, "heading":-0.97224, "vx":1.58355, "vy":-1.34093, "omega":-0.04071, "ax":-4.16403, "ay":2.76126, "alpha":-0.1186, "fx":[-69.53675,-69.04973,-69.27919,-69.78374], "fy":[45.56153,46.07213,46.449,46.03313]}, + {"t":1.08871, "x":3.51934, "y":5.93632, "heading":-0.97388, "vx":1.41565, "vy":-1.22959, "omega":-0.04549, "ax":-4.16147, "ay":2.76621, "alpha":-0.11124, "fx":[-69.49736,-68.99601,-69.27056,-69.71507], "fy":[45.70673,46.10935,46.50091,46.12826]}, + {"t":1.12903, "x":3.57304, "y":5.88899, "heading":-0.97571, "vx":1.24785, "vy":-1.11805, "omega":-0.04998, "ax":-4.15964, "ay":2.76974, "alpha":-0.08652, "fx":[-69.41768,-69.11478,-69.22395,-69.60023], "fy":[45.872,46.05524,46.53071,46.22321]}, + {"t":1.16935, "x":3.61998, "y":5.84616, "heading":-0.97773, "vx":1.08012, "vy":-1.00637, "omega":-0.05346, "ax":-4.15825, "ay":2.7724, "alpha":-0.08343, "fx":[-69.44256,-68.94395,-69.29417,-69.58371], "fy":[45.93533,46.26575,46.44862,46.20886]}, + {"t":1.20967, "x":3.66015, "y":5.80783, "heading":-0.97988, "vx":0.91245, "vy":-0.89458, "omega":-0.05683, "ax":-4.15718, "ay":2.77447, "alpha":-0.04364, "fx":[-69.32745,-69.21923,-69.2243,-69.42151], "fy":[46.12317,46.09338,46.47069,46.3094]}, + {"t":1.25, "x":3.69356, "y":5.77402, "heading":-0.98217, "vx":0.74482, "vy":-0.78271, "omega":-0.05859, "ax":-0.81168, "ay":0.57675, "alpha":-0.03543, "fx":[-13.53109,-13.47121,-13.47189,-13.64738], "fy":[9.46524,9.58504,9.76333,9.64325]}, + {"t":1.28282, "x":3.71757, "y":5.74863, "heading":-0.9841, "vx":0.71818, "vy":-0.76377, "omega":-0.05975, "ax":-0.78691, "ay":0.61413, "alpha":-0.03858, "fx":[-13.14449,-12.98576,-13.09026,-13.24917], "fy":[10.10246,10.22447,10.36344,10.25846]}, + {"t":1.31565, "x":3.74073, "y":5.72389, "heading":-0.98606, "vx":0.69234, "vy":-0.74361, "omega":-0.06102, "ax":-0.76642, "ay":0.63989, "alpha":-0.03185, "fx":[-12.78927,-12.69135,-12.74081,-12.88194], "fy":[10.54788,10.64318,10.78561,10.69022]}, + {"t":1.34848, "x":3.76304, "y":5.69982, "heading":-0.98806, "vx":0.66718, "vy":-0.72261, "omega":-0.06206, "ax":-0.7502, "ay":0.65912, "alpha":-0.03132, "fx":[-12.52775,-12.39897,-12.48301,-12.61197], "fy":[10.87943,10.9725,11.09075,11.00612]}, + {"t":1.38131, "x":3.78454, "y":5.67646, "heading":-0.9901, "vx":0.64256, "vy":-0.70097, "omega":-0.06309, "ax":-0.73708, "ay":0.67398, "alpha":-0.0264, "fx":[-12.29942,-12.21358,-12.25908,-12.37471], "fy":[11.13795,11.21538,11.33184,11.2544]}, + {"t":1.41413, "x":3.80524, "y":5.65381, "heading":-0.99217, "vx":0.61836, "vy":-0.67884, "omega":-0.06396, "ax":-0.72626, "ay":0.68579, "alpha":-0.02578, "fx":[-12.12519,-12.0189,-12.08765,-12.19412], "fy":[11.34379,11.41789,11.51728,11.44812]}, + {"t":1.44696, "x":3.82514, "y":5.63189, "heading":-0.99427, "vx":0.59452, "vy":-0.65633, "omega":-0.0648, "ax":-0.71721, "ay":0.69539, "alpha":-0.02182, "fx":[-11.96636,-11.89526,-11.9325,-12.02809], "fy":[11.51171,11.57541,11.67201,11.60831]}, + {"t":1.47979, "x":3.84427, "y":5.61072, "heading":-0.9964, "vx":0.57097, "vy":-0.6335, "omega":-0.06552, "ax":-0.70953, "ay":0.70335, "alpha":-0.0213, "fx":[-11.84326,-11.75485,-11.81157,-11.90011], "fy":[11.65183,11.7136,11.79461,11.73795]}, + {"t":1.51262, "x":3.86264, "y":5.59031, "heading":-0.99855, "vx":0.54768, "vy":-0.61041, "omega":-0.06622, "ax":-0.70292, "ay":0.71005, "alpha":-0.01784, "fx":[-11.72602,-11.66947,-11.69782,-11.77629], "fy":[11.76988,11.82225,11.90238,11.85]}, + {"t":1.54544, "x":3.88024, "y":5.57065, "heading":-1.00072, "vx":0.52461, "vy":-0.5871, "omega":-0.06681, "ax":-0.69719, "ay":0.71576, "alpha":-0.0173, "fx":[-11.63508,-11.56235,-11.60865,-11.68145], "fy":[11.87195,11.92452,11.98721,11.94168]}, + {"t":1.57827, "x":3.89708, "y":5.55176, "heading":-1.00291, "vx":0.50172, "vy":-0.56361, "omega":-0.06737, "ax":-0.69217, "ay":0.72069, "alpha":-0.01401, "fx":[-11.54435,-11.50292,-11.52175,-11.58383], "fy":[11.96015,12.00206,12.06684,12.02489]}, + {"t":1.6111, "x":3.91318, "y":5.53365, "heading":-1.00513, "vx":0.479, "vy":-0.53995, "omega":-0.06783, "ax":-0.68774, "ay":0.72498, "alpha":-0.01326, "fx":[-11.47481,-11.41781,-11.45385,-11.51089], "fy":[12.03899,12.08288,12.12639,12.09201]}, + {"t":1.64393, "x":3.92853, "y":5.51631, "heading":-1.00735, "vx":0.45642, "vy":-0.51615, "omega":-0.06827, "ax":-0.6838, "ay":0.72875, "alpha":-0.00987, "fx":[-11.40188,-11.37719,-11.38565,-11.42986], "fy":[12.1084,12.13932,12.1876,12.15662]}, + {"t":1.67676, "x":3.94315, "y":5.49976, "heading":-1.00959, "vx":0.43397, "vy":-0.49223, "omega":-0.06859, "ax":-0.68027, "ay":0.7321, "alpha":-0.00866, "fx":[-11.34702,-11.30815,-11.33266,-11.37154], "fy":[12.17326,12.20602,12.22868,12.20692]}, + {"t":1.70958, "x":3.95703, "y":5.484, "heading":-1.01185, "vx":0.41164, "vy":-0.46819, "omega":-0.06888, "ax":-0.6771, "ay":0.73508, "alpha":-0.00491, "fx":[-11.28664,-11.28157,-11.27855,-11.3009], "fy":[12.23049,12.24831,12.2764,12.25853]}, + {"t":1.74241, "x":3.97018, "y":5.46902, "heading":-1.01411, "vx":0.38941, "vy":-0.44406, "omega":-0.06904, "ax":-0.67423, "ay":0.73776, "alpha":-0.0028, "fx":[-11.24178,-11.22627,-11.23627,-11.25177], "fy":[12.28767,12.30534,12.30256,12.29658]}, + {"t":1.77524, "x":3.9826, "y":5.45484, "heading":-1.01637, "vx":0.36728, "vy":-0.41984, "omega":-0.06913, "ax":-0.67161, "ay":0.74017, "alpha":0.00169, "fx":[-11.19028,-11.21156,-11.1934,-11.18671], "fy":[12.33748,12.33812,12.33911,12.33843]}, + {"t":1.80807, "x":3.99429, "y":5.44146, "heading":-1.01864, "vx":0.34523, "vy":-0.39555, "omega":-0.06907, "ax":-0.66923, "ay":0.74236, "alpha":0.00521, "fx":[-11.15213,-11.16909,-11.15936,-11.14238], "fy":[12.39188,12.38859,12.35173,12.36692]}, + {"t":1.84089, "x":4.00526, "y":5.42888, "heading":-1.02091, "vx":0.32326, "vy":-0.37118, "omega":-0.0689, "ax":-0.66704, "ay":0.74435, "alpha":0.01104, "fx":[-11.10665,-11.16605,-11.12595,-11.07859], "fy":[12.4385,12.41516,12.37757,12.40088]}, + {"t":1.87372, "x":4.01552, "y":5.41709, "heading":-1.02317, "vx":0.30137, "vy":-0.34674, "omega":-0.06854, "ax":-0.66503, "ay":0.74618, "alpha":0.01666, "fx":[-11.07271,-11.13675,-11.0989,-11.03483], "fy":[12.49452,12.46149,12.37645,12.42128]}, + {"t":1.90655, "x":4.02505, "y":5.40611, "heading":-1.02542, "vx":0.27953, "vy":-0.32225, "omega":-0.06799, "ax":-0.66318, "ay":0.74785, "alpha":0.02472, "fx":[-11.03077,-11.14675,-11.07401,-10.96792], "fy":[12.54249,12.48452,12.39022,12.44817]}, + {"t":1.93938, "x":4.03387, "y":5.39594, "heading":-1.02765, "vx":0.25776, "vy":-0.2977, "omega":-0.06718, "ax":-0.66146, "ay":0.7494, "alpha":0.03342, "fx":[-10.99878,-11.13263,-11.05371,-10.91979], "fy":[12.60463,12.52906,12.3736,12.461]}, + {"t":1.9722, "x":4.04197, "y":5.38657, "heading":-1.02986, "vx":0.23605, "vy":-0.27309, "omega":-0.06609, "ax":-0.65987, "ay":0.75082, "alpha":0.04497, "fx":[-10.95811,-11.15843,-11.03714,-10.84489], "fy":[12.65972,12.55086,12.37197,12.48084]}, + {"t":2.00503, "x":4.04937, "y":5.37801, "heading":-1.03203, "vx":0.21439, "vy":-0.24845, "omega":-0.06461, "ax":-0.65838, "ay":0.75214, "alpha":0.05823, "fx":[-10.92558,-11.16388,-11.02425,-10.78586], "fy":[12.73329,12.59566,12.33662,12.48596]}, + {"t":2.03786, "x":4.05605, "y":5.37026, "heading":-1.03415, "vx":0.19277, "vy":-0.22376, "omega":-0.0627, "ax":-0.657, "ay":0.75337, "alpha":0.07506, "fx":[-10.88388,-11.20961,-11.01659,-10.69712], "fy":[12.80326,12.61908,12.31345,12.49767]}, + {"t":2.07069, "x":4.06203, "y":5.36332, "heading":-1.03621, "vx":0.17121, "vy":-0.19902, "omega":-0.06023, "ax":-0.6557, "ay":0.75452, "alpha":0.09504, "fx":[-10.8477,-11.24239,-11.0128,-10.61791], "fy":[12.89483,12.66654,12.25427,12.49418]}, + {"t":2.10352, "x":4.06729, "y":5.35719, "heading":-1.03819, "vx":0.14968, "vy":-0.17425, "omega":-0.05711, "ax":-0.65449, "ay":0.75559, "alpha":0.11977, "fx":[-10.80228,-11.31439,-11.01532,-10.50782], "fy":[12.99078,12.69507,12.19975,12.49557]}, + {"t":2.13634, "x":4.07185, "y":5.35188, "heading":-1.04006, "vx":0.1282, "vy":-0.14945, "omega":-0.05318, "ax":-0.65334, "ay":0.75659, "alpha":0.14958, "fx":[-10.75836,-11.38653,-11.02373,-10.39513], "fy":[13.10889,12.74791,12.10933,12.48183]}, + {"t":2.16917, "x":4.07571, "y":5.34738, "heading":-1.04181, "vx":0.10675, "vy":-0.12461, "omega":-0.04827, "ax":-0.65227, "ay":0.75753, "alpha":0.18599, "fx":[-10.706,-11.49377,-11.03857,-10.25383], "fy":[13.24687,12.78632,12.00832,12.4691]}, + {"t":2.202, "x":4.07886, "y":5.3437, "heading":-1.04339, "vx":0.08534, "vy":-0.09975, "omega":-0.04217, "ax":-0.65126, "ay":0.75841, "alpha":0.23012, "fx":[-10.64869,-11.62362,-11.06412,-10.08828], "fy":[13.40287,12.84791,11.87616,12.44259]}, + {"t":2.23483, "x":4.08131, "y":5.34083, "heading":-1.04478, "vx":0.06396, "vy":-0.07485, "omega":-0.03461, "ax":-0.6503, "ay":0.75925, "alpha":0.28359, "fx":[-10.58546,-11.77852,-11.09441,-9.90262], "fy":[13.60624,12.90281,11.706,12.40995]}, + {"t":2.26765, "x":4.08306, "y":5.33878, "heading":-1.04591, "vx":0.04261, "vy":-0.04992, "omega":-0.0253, "ax":-0.6494, "ay":0.76003, "alpha":0.34847, "fx":[-10.50688,-11.99351,-11.14446,-9.6559], "fy":[13.81543,12.97724,11.51752,12.36716]}, + {"t":2.30048, "x":4.08411, "y":5.33755, "heading":-1.04674, "vx":0.02129, "vy":-0.02497, "omega":-0.01386, "ax":-0.64854, "ay":0.76077, "alpha":0.42229, "fx":[-10.42832,-12.2119,-11.19425,-9.40919], "fy":[14.09315,13.04658,11.27535,12.31172]}, + {"t":2.33331, "x":4.08446, "y":5.33714, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/fetchToLSlow.traj b/src/main/deploy/choreo/fetchToLSlow.traj index 9d7119b4..f9ef23f4 100644 --- a/src/main/deploy/choreo/fetchToLSlow.traj +++ b/src/main/deploy/choreo/fetchToLSlow.traj @@ -4,8 +4,8 @@ "snapshot":{ "waypoints":[ {"x":1.69577419757843, "y":7.228199481964111, "heading":-0.942477796076938, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.410693407058716, "y":5.76054573059082, "heading":0.0, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":3.85576086490539, "y":5.073261807735684, "heading":5.235987755982989, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.410693407058716, "y":5.76054573059082, "heading":0.0, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.798672364905389, "y":5.1721419902635795, "heading":5.235987755982989, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -18,7 +18,7 @@ "params":{ "waypoints":[ {"x":{"exp":"fetch.x", "val":1.69577419757843}, "y":{"exp":"fetch.y", "val":7.228199481964111}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.410693407058716 m", "val":3.410693407058716}, "y":{"exp":"5.76054573059082 m", "val":5.76054573059082}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"3.410693407058716 m", "val":3.410693407058716}, "y":{"exp":"5.76054573059082 m", "val":5.76054573059082}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"L.x", "val":3.798672364905389}, "y":{"exp":"L.y", "val":5.1721419902635795}, "heading":{"exp":"L.heading", "val":5.235987755982989}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -34,76 +34,74 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.1578,2.43832], + "waypoints":[0.0,1.16825,2.35632], "samples":[ - {"t":0.0, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.99064, "ay":-3.0082, "alpha":-0.35022, "fx":[66.20085,67.5714,66.89919,65.41709], "fy":[-51.29088,-50.54373,-48.75922,-49.98739]}, - {"t":0.03992, "x":1.69895, "y":7.2258, "heading":-0.94248, "vx":0.15932, "vy":-0.1201, "omega":-0.01398, "ax":3.9916, "ay":-3.0089, "alpha":-0.26219, "fx":[66.30053,67.31017,66.8283,65.71331], "fy":[-50.9997,-50.50476,-49.09946,-50.02359]}, - {"t":0.07985, "x":1.7085, "y":7.21861, "heading":-0.94304, "vx":0.31868, "vy":-0.24023, "omega":-0.02445, "ax":3.99152, "ay":-3.00883, "alpha":-0.19315, "fx":[66.12951,67.05465,66.61832,66.34454], "fy":[-51.25062,-50.0941,-49.37451,-49.90355]}, - {"t":0.11977, "x":1.7244, "y":7.20662, "heading":-0.94401, "vx":0.47804, "vy":-0.36035, "omega":-0.03216, "ax":3.99145, "ay":-3.00872, "alpha":-0.13307, "fx":[66.43318,66.88147,66.69935,66.12796], "fy":[-50.50378,-50.57479,-49.53496,-50.00224]}, - {"t":0.1597, "x":1.74667, "y":7.18984, "heading":-0.9453, "vx":0.63739, "vy":-0.48047, "omega":-0.03747, "ax":3.99133, "ay":-3.00863, "alpha":-0.08533, "fx":[66.20663,66.68651,66.49306,66.74822], "fy":[-50.92146,-50.06128,-49.74411,-49.8827]}, - {"t":0.19962, "x":1.7753, "y":7.16826, "heading":-0.94679, "vx":0.79674, "vy":-0.60059, "omega":-0.04088, "ax":3.99124, "ay":-3.00846, "alpha":-0.04041, "fx":[66.52397,66.57273,66.60555,66.42609], "fy":[-50.14973,-50.61089,-49.84655,-49.99093]}, - {"t":0.23954, "x":1.81029, "y":7.14188, "heading":-0.94842, "vx":0.95609, "vy":-0.7207, "omega":-0.04249, "ax":3.99107, "ay":-3.00832, "alpha":-0.00529, "fx":[66.31173,66.43865,66.42248,66.9442], "fy":[-50.57184,-50.05884,-50.0325,-49.926]}, - {"t":0.27947, "x":1.85164, "y":7.11071, "heading":-0.95012, "vx":1.11543, "vy":-0.8408, "omega":-0.0427, "ax":3.99093, "ay":-3.00807, "alpha":0.02547, "fx":[66.57095,66.38901,66.5208,66.62646], "fy":[-49.95905,-50.43179,-50.13263,-50.04879]}, - {"t":0.31939, "x":1.89935, "y":7.07474, "heading":-0.95183, "vx":1.27476, "vy":-0.9609, "omega":-0.04169, "ax":3.99069, "ay":-3.0078, "alpha":0.04219, "fx":[66.73973,66.46328,66.57394,66.31478], "fy":[-49.68188,-50.18502,-50.36962,-50.31764]}, - {"t":0.35932, "x":1.95342, "y":7.03398, "heading":-0.95349, "vx":1.43409, "vy":-1.08098, "omega":-0.04, "ax":3.99041, "ay":-3.0074, "alpha":0.06748, "fx":[66.54101,66.42884,66.40079,66.70216], "fy":[-50.10249,-49.46755,-50.59667,-50.36086]}, - {"t":0.39924, "x":2.01386, "y":6.98843, "heading":-0.95509, "vx":1.5934, "vy":-1.20105, "omega":-0.03731, "ax":3.98998, "ay":-3.00688, "alpha":0.08601, "fx":[66.80759,66.34758,66.53637,66.35253], "fy":[-49.39618,-50.20287,-50.51715,-50.37662]}, - {"t":0.43916, "x":2.08065, "y":6.93808, "heading":-0.95658, "vx":1.7527, "vy":-1.32109, "omega":-0.03388, "ax":3.98935, "ay":-3.00606, "alpha":0.09357, "fx":[66.58362,66.22111,66.39539,66.80188], "fy":[-49.81364,-49.98444,-50.47765,-50.16272]}, - {"t":0.47909, "x":2.15381, "y":6.88295, "heading":-0.95793, "vx":1.91197, "vy":-1.44111, "omega":-0.03014, "ax":3.9883, "ay":-3.00466, "alpha":0.11165, "fx":[66.41673,66.0578,66.27577,67.18179], "fy":[-49.96157,-50.01788,-50.37984,-49.98584]}, - {"t":0.51901, "x":2.23332, "y":6.82302, "heading":-0.95913, "vx":2.0712, "vy":-1.56106, "omega":-0.02568, "ax":3.98627, "ay":-3.00164, "alpha":0.09601, "fx":[66.57083,66.05154,66.38274,66.79158], "fy":[-49.56059,-50.44738,-50.22453,-49.91106]}, - {"t":0.55894, "x":2.31919, "y":6.7583, "heading":-0.96016, "vx":2.23034, "vy":-1.6809, "omega":-0.02185, "ax":3.97926, "ay":-2.99266, "alpha":0.14234, "fx":[65.81553,65.5987,65.78513,68.13016], "fy":[-50.45912,-49.63657,-49.93513,-49.51426]}, - {"t":0.59886, "x":2.4114, "y":6.68881, "heading":-0.96103, "vx":2.38921, "vy":-1.80038, "omega":-0.01617, "ax":0.15403, "ay":-0.11336, "alpha":0.08425, "fx":[2.60953,2.32893,2.52535,2.80647], "fy":[-2.02194,-0.81457,-2.44949,-2.27241]}, - {"t":0.63878, "x":2.50691, "y":6.61684, "heading":-0.96168, "vx":2.39536, "vy":-1.80491, "omega":-0.0128, "ax":0.09715, "ay":0.1291, "alpha":0.14195, "fx":[1.68232,1.51212,1.63291,1.65023], "fy":[3.45184,1.79222,1.62119,1.74321]}, - {"t":0.67871, "x":2.60262, "y":6.54488, "heading":-0.96219, "vx":2.39924, "vy":-1.79975, "omega":-0.00714, "ax":-0.3863, "ay":-0.51031, "alpha":0.02965, "fx":[-6.42145,-6.54645,-6.45864,-6.33097], "fy":[-8.41513,-8.48598,-8.60096,-8.52459]}, - {"t":0.71863, "x":2.6981, "y":6.47262, "heading":-0.96247, "vx":2.38382, "vy":-1.82013, "omega":-0.00595, "ax":-1.16898, "ay":-1.49151, "alpha":-0.17027, "fx":[-19.34982,-19.44065,-19.38689,-19.76827], "fy":[-26.44445,-24.3024,-24.37828,-24.32595]}, - {"t":0.75856, "x":2.79234, "y":6.39877, "heading":-0.96271, "vx":2.33715, "vy":-1.87967, "omega":-0.01275, "ax":-2.65287, "ay":-3.11267, "alpha":-0.07645, "fx":[-44.24761,-44.10298,-44.22433,-44.31342], "fy":[-51.00571,-54.78664,-50.84525,-50.90903]}, - {"t":0.79848, "x":2.88353, "y":6.32124, "heading":-0.96322, "vx":2.23123, "vy":-2.00394, "omega":-0.0158, "ax":-4.79832, "ay":1.09512, "alpha":-0.24708, "fx":[-79.47771,-79.28407,-79.42372,-81.75743], "fy":[17.65562,18.39174,18.64168,18.33153]}, - {"t":0.8384, "x":2.96879, "y":6.24211, "heading":-0.96385, "vx":2.03967, "vy":-1.96022, "omega":-0.02567, "ax":-4.21655, "ay":2.65805, "alpha":-0.09135, "fx":[-70.40816,-69.89514,-70.22482,-70.62314], "fy":[43.69307,45.0766,44.39204,44.07179]}, - {"t":0.87833, "x":3.04686, "y":6.16597, "heading":-0.96487, "vx":1.87132, "vy":-1.8541, "omega":-0.02931, "ax":-4.12028, "ay":2.81737, "alpha":-0.06992, "fx":[-68.69572,-68.30514,-68.55324,-69.17848], "fy":[47.12187,46.75896,47.19812,46.77819]}, - {"t":0.91825, "x":3.11829, "y":6.09419, "heading":-0.96604, "vx":1.70683, "vy":-1.74162, "omega":-0.03211, "ax":-4.08197, "ay":2.87723, "alpha":-0.1151, "fx":[-68.16796,-67.65189,-67.92919,-68.42907], "fy":[47.43918,48.22345,48.29025,47.89503]}, - {"t":0.95818, "x":3.18318, "y":6.02695, "heading":-0.96733, "vx":1.54386, "vy":-1.62675, "omega":-0.0367, "ax":-4.06142, "ay":2.90857, "alpha":-0.05513, "fx":[-68.07461,-67.54497,-67.79914,-67.38912], "fy":[48.02454,48.40782,48.85779,48.64779]}, - {"t":0.9981, "x":3.24158, "y":5.96432, "heading":-0.96879, "vx":1.38171, "vy":-1.51063, "omega":-0.0389, "ax":-4.04858, "ay":2.9279, "alpha":-0.10512, "fx":[-67.57717,-67.2065,-67.35443,-67.8132], "fy":[48.48137,48.60083,49.252,48.89265]}, - {"t":1.03802, "x":3.29351, "y":5.90635, "heading":-0.97034, "vx":1.22007, "vy":-1.39373, "omega":-0.0431, "ax":-4.03983, "ay":2.94095, "alpha":-0.05749, "fx":[-67.45406,-67.09409,-67.29751,-67.52252], "fy":[48.93645,48.91242,49.27256,48.97566]}, - {"t":1.07795, "x":3.33901, "y":5.85305, "heading":-0.97206, "vx":1.05879, "vy":-1.27632, "omega":-0.04539, "ax":-4.03345, "ay":2.95042, "alpha":-0.07483, "fx":[-67.34162,-66.90631,-67.18443,-67.51031], "fy":[48.73999,49.68146,49.27885,49.02787]}, - {"t":1.11787, "x":3.37806, "y":5.80444, "heading":-0.97388, "vx":0.89776, "vy":-1.15853, "omega":-0.04838, "ax":-4.02862, "ay":2.95755, "alpha":-0.02226, "fx":[-67.00819,-66.91899,-67.03785,-67.65551], "fy":[49.72577,49.12053,49.32364,49.034]}, - {"t":1.1578, "x":3.41069, "y":5.76055, "heading":-0.97581, "vx":0.73692, "vy":-1.04045, "omega":-0.04927, "ax":-0.77757, "ay":0.62128, "alpha":-0.04694, "fx":[-12.96417,-12.87774,-12.89211,-13.11261], "fy":[10.15879,10.31914,10.55426,10.39361]}, - {"t":1.19149, "x":3.43508, "y":5.72584, "heading":-0.97747, "vx":0.71072, "vy":-1.01951, "omega":-0.05085, "ax":-0.73921, "ay":0.67049, "alpha":-0.04813, "fx":[-12.35692,-12.14723,-12.2879,-12.49686], "fy":[10.97146,11.26487,11.30259,11.16829]}, - {"t":1.22519, "x":3.45861, "y":5.69186, "heading":-0.97918, "vx":0.68581, "vy":-0.99692, "omega":-0.05247, "ax":-0.70762, "ay":0.70413, "alpha":-0.03903, "fx":[-11.80348,-11.71239,-11.74341,-11.92317], "fy":[11.58083,11.7072,11.89434,11.76751]}, - {"t":1.25889, "x":3.48132, "y":5.65867, "heading":-0.98095, "vx":0.66196, "vy":-0.97319, "omega":-0.05379, "ax":-0.68237, "ay":0.7289, "alpha":-0.03894, "fx":[-11.4036,-11.23348,-11.34649,-11.51584], "fy":[11.97629,12.23725,12.24893,12.13896]}, - {"t":1.29259, "x":3.50324, "y":5.62629, "heading":-0.98276, "vx":0.63897, "vy":-0.94863, "omega":-0.0551, "ax":-0.66185, "ay":0.7478, "alpha":-0.031, "fx":[-11.03557,-10.97643,-10.9841,-11.13491], "fy":[12.3373,12.44001,12.59386,12.49061]}, - {"t":1.32629, "x":3.5244, "y":5.59474, "heading":-0.98462, "vx":0.61666, "vy":-0.92343, "omega":-0.05615, "ax":-0.64489, "ay":0.76265, "alpha":-0.03218, "fx":[-10.77459,-10.63224,-10.72591,-10.86735], "fy":[12.55771,12.8102,12.78819,12.69577]}, - {"t":1.35998, "x":3.54481, "y":5.56406, "heading":-0.98651, "vx":0.59493, "vy":-0.89773, "omega":-0.05723, "ax":-0.63067, "ay":0.77459, "alpha":-0.02445, "fx":[-10.50915,-10.48484,-10.46392,-10.59377], "fy":[12.80394,12.88996,13.02055,12.93386]}, - {"t":1.39368, "x":3.5645, "y":5.53425, "heading":-0.98844, "vx":0.57368, "vy":-0.87163, "omega":-0.05805, "ax":-0.61858, "ay":0.78439, "alpha":-0.02719, "fx":[-10.33311,-10.21038,-10.2904,-10.41207], "fy":[12.93202,13.18614,13.13162,13.05207]}, - {"t":1.42738, "x":3.58348, "y":5.50532, "heading":-0.9904, "vx":0.55283, "vy":-0.8452, "omega":-0.05897, "ax":-0.6082, "ay":0.79257, "alpha":-0.01941, "fx":[-10.12889,-10.13388,-10.08833,-10.20251], "fy":[13.11799,13.19199,13.30595,13.23116]}, - {"t":1.46108, "x":3.60177, "y":5.47729, "heading":-0.99238, "vx":0.53234, "vy":-0.81849, "omega":-0.05962, "ax":-0.59919, "ay":0.79949, "alpha":-0.0234, "fx":[-10.00763,-9.89929,-9.9693,-10.07644], "fy":[13.19247,13.44905,13.36831,13.29866]}, - {"t":1.49477, "x":3.61937, "y":5.45016, "heading":-0.99439, "vx":0.51215, "vy":-0.79155, "omega":-0.06041, "ax":-0.59129, "ay":0.80542, "alpha":-0.0157, "fx":[-9.84327,-9.86852,-9.80651,-9.9081], "fy":[13.34306,13.40793,13.50923,13.44351]}, - {"t":1.52847, "x":3.63629, "y":5.42395, "heading":-0.99643, "vx":0.49222, "vy":-0.76441, "omega":-0.06094, "ax":-0.58433, "ay":0.81055, "alpha":-0.02033, "fx":[-9.75819,-9.66133,-9.72339,-9.81896], "fy":[13.38477,13.64109,13.54074,13.47933]}, - {"t":1.56217, "x":3.65254, "y":5.39865, "heading":-0.99848, "vx":0.47253, "vy":-0.73709, "omega":-0.06163, "ax":-0.57814, "ay":0.81503, "alpha":-0.0127, "fx":[-9.62121,-9.6615,-9.58792,-9.67833], "fy":[13.51252,13.56977,13.66041,13.60227]}, - {"t":1.59587, "x":3.66814, "y":5.37427, "heading":-1.00056, "vx":0.45305, "vy":-0.70963, "omega":-0.06205, "ax":-0.5726, "ay":0.81899, "alpha":-0.01754, "fx":[-9.56099,-9.4745,-9.52946,-9.6146], "fy":[13.53365,13.78647,13.67104,13.61727]}, - {"t":1.62957, "x":3.68308, "y":5.35082, "heading":-1.00265, "vx":0.43375, "vy":-0.68203, "omega":-0.06265, "ax":-0.56761, "ay":0.82249, "alpha":-0.00997, "fx":[-9.44381,-9.49589,-9.41417,-9.49332], "fy":[13.64568,13.69565,13.7759,13.72502]}, - {"t":1.66326, "x":3.69737, "y":5.32831, "heading":-1.00476, "vx":0.41463, "vy":-0.65431, "omega":-0.06298, "ax":-0.5631, "ay":0.82563, "alpha":-0.01469, "fx":[-9.40095,-9.32542,-9.37304,-9.4472], "fy":[13.65442,13.89827,13.77211,13.72632]}, - {"t":1.69696, "x":3.71103, "y":5.30673, "heading":-1.00688, "vx":0.39565, "vy":-0.62649, "omega":-0.06348, "ax":-0.55901, "ay":0.82844, "alpha":-0.00705, "fx":[-9.29855,-9.36201,-9.2733,-9.33963], "fy":[13.75469,13.79668,13.86517,13.82226]}, - {"t":1.73066, "x":3.72404, "y":5.28609, "heading":-1.00902, "vx":0.37681, "vy":-0.59857, "omega":-0.06371, "ax":-0.55527, "ay":0.83098, "alpha":-0.01132, "fx":[-9.26805,-9.20575,-9.24476,-9.30569], "fy":[13.75593,13.98837,13.85026,13.81378]}, - {"t":1.76436, "x":3.73643, "y":5.26639, "heading":-1.01117, "vx":0.3581, "vy":-0.57057, "omega":-0.0641, "ax":-0.55184, "ay":0.83329, "alpha":-0.00348, "fx":[-9.17704,-9.25345,-9.15758,-9.20785], "fy":[13.84795,13.8801,13.93361,13.90056]}, - {"t":1.79806, "x":3.74818, "y":5.24763, "heading":-1.01333, "vx":0.33951, "vy":-0.54249, "omega":-0.06421, "ax":-0.5487, "ay":0.83539, "alpha":-0.00695, "fx":[-9.15524,-9.11055,-9.13843,-9.18175], "fy":[13.84549,14.06217,13.90977,13.88507]}, - {"t":1.83175, "x":3.75931, "y":5.22983, "heading":-1.01549, "vx":0.32102, "vy":-0.51434, "omega":-0.06445, "ax":-0.54579, "ay":0.83732, "alpha":0.00134, "fx":[-9.07305,-9.16709,-9.0617,-9.09043], "fy":[13.93197,13.95099,13.98393,13.96402]}, - {"t":1.86545, "x":3.76982, "y":5.21297, "heading":-1.01767, "vx":0.30262, "vy":-0.48612, "omega":-0.0644, "ax":-0.5431, "ay":0.83909, "alpha":-0.00096, "fx":[-9.05727,-9.03733,-9.04992,-9.06851], "fy":[13.92903,14.12389,13.95243,13.94352]}, - {"t":1.89915, "x":3.77971, "y":5.19706, "heading":-1.01984, "vx":0.28432, "vy":-0.45785, "omega":-0.06443, "ax":-0.54061, "ay":0.84072, "alpha":0.0082, "fx":[-8.98188,-9.10165,-8.98227,-8.98091], "fy":[14.0125,14.01318,14.01673,14.01518]}, - {"t":1.93285, "x":3.78898, "y":5.18211, "heading":-1.02201, "vx":0.26611, "vy":-0.42952, "omega":-0.06416, "ax":-0.53829, "ay":0.84223, "alpha":0.00758, "fx":[-8.96993,-8.98578,-8.97671,-8.95952], "fy":[14.01153,14.17873,13.97742,13.99045]}, - {"t":1.96655, "x":3.79764, "y":5.16812, "heading":-1.02417, "vx":0.24797, "vy":-0.40114, "omega":-0.0639, "ax":-0.53612, "ay":0.84363, "alpha":0.01822, "fx":[-8.89946,-9.05828,-8.91714,-8.87266], "fy":[14.09551,14.06993,14.03059,14.05533]}, - {"t":2.00024, "x":3.80569, "y":5.15508, "heading":-1.02632, "vx":0.2299, "vy":-0.37271, "omega":-0.06329, "ax":-0.5341, "ay":0.84493, "alpha":0.01996, "fx":[-8.88944,-8.95788,-8.9175,-8.84775], "fy":[14.09972,14.2282,13.98295,14.02719]}, - {"t":2.03394, "x":3.81314, "y":5.143, "heading":-1.02846, "vx":0.2119, "vy":-0.34424, "omega":-0.06262, "ax":-0.5322, "ay":0.84614, "alpha":0.03295, "fx":[-8.82244,-9.03922,-8.86585,-8.75857], "fy":[14.1881,14.12437,14.02175,14.08466]}, - {"t":2.06764, "x":3.81998, "y":5.13188, "heading":-1.03057, "vx":0.19397, "vy":-0.31572, "omega":-0.06151, "ax":-0.53042, "ay":0.84727, "alpha":0.03817, "fx":[-8.81206,-8.9583,-8.87225,-8.72471], "fy":[14.2007,14.27742,13.96347,14.05281]}, - {"t":2.10134, "x":3.82621, "y":5.12172, "heading":-1.03264, "vx":0.17609, "vy":-0.28717, "omega":-0.06022, "ax":-0.52874, "ay":0.84833, "alpha":0.05482, "fx":[-8.74667,-9.05141,-8.82859,-8.62894], "fy":[14.29952,14.17993,13.98342,14.10225]}, - {"t":2.13504, "x":3.83184, "y":5.11253, "heading":-1.03467, "vx":0.15828, "vy":-0.25858, "omega":-0.05837, "ax":-0.52716, "ay":0.84933, "alpha":0.06509, "fx":[-8.73354,-8.99543,-8.84228,-8.57909], "fy":[14.32544,14.3288,13.91101,14.06623]}, - {"t":2.16873, "x":3.83688, "y":5.10429, "heading":-1.03663, "vx":0.14051, "vy":-0.22996, "omega":-0.05618, "ax":-0.52567, "ay":0.85026, "alpha":0.08735, "fx":[-8.66776,-9.10461,-8.80758,-8.47101], "fy":[14.44276,14.24084,13.90454,14.10574]}, - {"t":2.20243, "x":3.84132, "y":5.09703, "heading":-1.03853, "vx":0.1228, "vy":-0.20131, "omega":-0.05324, "ax":-0.52427, "ay":0.85114, "alpha":0.10511, "fx":[-8.64854,-9.08285,-8.83063,-8.39497], "fy":[14.48872,14.38806,13.8118,14.06405]}, - {"t":2.23613, "x":3.84516, "y":5.09073, "heading":-1.04032, "vx":0.10513, "vy":-0.17263, "omega":-0.04969, "ax":-0.52293, "ay":0.85198, "alpha":0.136, "fx":[-8.57946,-9.21611,-8.80652,-8.26591], "fy":[14.6366,14.3128,13.76776,14.09094]}, - {"t":2.26983, "x":3.8484, "y":5.08539, "heading":-1.042, "vx":0.08751, "vy":-0.14392, "omega":-0.04511, "ax":-0.52167, "ay":0.85276, "alpha":0.16485, "fx":[-8.54988,-9.24192,-8.8427,-8.14913], "fy":[14.71296,14.46138,13.64498,14.04117]}, - {"t":2.30353, "x":3.85105, "y":5.08103, "heading":-1.04352, "vx":0.06993, "vy":-0.11518, "omega":-0.03956, "ax":-0.52046, "ay":0.85351, "alpha":0.20883, "fx":[-8.47368,-9.41123,-8.83232,-7.98626], "fy":[14.90897,14.4038,13.5463,14.05103]}, - {"t":2.33722, "x":3.85312, "y":5.07763, "heading":-1.04485, "vx":0.05239, "vy":-0.08642, "omega":-0.03252, "ax":-0.51932, "ay":0.85421, "alpha":0.25435, "fx":[-8.42744,-9.50565,-8.88721,-7.80701], "fy":[15.03151,14.55836,13.37805,13.9892]}, - {"t":2.37092, "x":3.85459, "y":5.0752, "heading":-1.04595, "vx":0.03489, "vy":-0.05764, "omega":-0.02395, "ax":-0.51823, "ay":0.85488, "alpha":0.31828, "fx":[-8.33846,-9.73041,-8.89541,-7.59049], "fy":[15.30185,14.52536,13.19904,13.97551]}, - {"t":2.40462, "x":3.85547, "y":5.07375, "heading":-1.04675, "vx":0.01743, "vy":-0.02883, "omega":-0.01322, "ax":-0.5172, "ay":0.85552, "alpha":0.39236, "fx":[-8.25476,-9.95475,-8.94583,-7.33029], "fy":[15.56251,14.60259,12.9594,13.91968]}, - {"t":2.43832, "x":3.85576, "y":5.07326, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.98508, "ay":-3.01547, "alpha":-0.38339, "fx":[66.0342,67.58413,66.86044,65.23843], "fy":[-51.6145,-50.47031,-48.78752,-50.19325]}, + {"t":0.04028, "x":1.69901, "y":7.22575, "heading":-0.94248, "vx":0.16054, "vy":-0.12148, "omega":-0.01544, "ax":3.98606, "ay":-3.01617, "alpha":-0.28717, "fx":[66.14634,67.31478,66.76907,65.55249], "fy":[-51.29457,-50.40637,-49.17738,-50.23402]}, + {"t":0.08057, "x":1.70871, "y":7.21841, "heading":-0.9431, "vx":0.32111, "vy":-0.24298, "omega":-0.02701, "ax":3.98598, "ay":-3.01608, "alpha":-0.2119, "fx":[66.11321,67.07105,66.62577,65.9672], "fy":[-51.2259,-50.24176,-49.4665,-50.17242]}, + {"t":0.12085, "x":1.72488, "y":7.20618, "heading":-0.94419, "vx":0.48169, "vy":-0.36448, "omega":-0.03555, "ax":3.98589, "ay":-3.01596, "alpha":-0.14438, "fx":[66.2988,66.85593,66.61561,66.00146], "fy":[-50.75543,-50.44387,-49.68278,-50.21641]}, + {"t":0.16114, "x":1.74752, "y":7.18905, "heading":-0.94562, "vx":0.64226, "vy":-0.48598, "omega":-0.04137, "ax":3.9858, "ay":-3.01581, "alpha":-0.08263, "fx":[66.19942,66.65135,66.45791,66.45666], "fy":[-50.80516,-50.24247,-49.90592,-50.13513]}, + {"t":0.20142, "x":1.77663, "y":7.16702, "heading":-0.94729, "vx":0.80282, "vy":-0.60747, "omega":-0.04469, "ax":3.98567, "ay":-3.01564, "alpha":-0.04341, "fx":[66.40605,66.52966,66.5051,66.31629], "fy":[-50.36863,-50.47081,-50.03626,-50.20146]}, + {"t":0.24171, "x":1.8122, "y":7.1401, "heading":-0.94909, "vx":0.96339, "vy":-0.72896, "omega":-0.04644, "ax":3.98552, "ay":-3.01543, "alpha":0.00654, "fx":[66.22818,66.33927,66.32688,66.85225], "fy":[-50.57649,-50.21708,-50.19459,-50.07492]}, + {"t":0.28199, "x":1.85425, "y":7.10829, "heading":-0.95096, "vx":1.12394, "vy":-0.85043, "omega":-0.04618, "ax":3.98534, "ay":-3.01512, "alpha":0.02953, "fx":[66.49372,66.2559,66.43579,66.54941], "fy":[-50.03382,-50.64591,-50.22862,-50.13434]}, + {"t":0.32228, "x":1.90276, "y":7.07158, "heading":-0.95282, "vx":1.28449, "vy":-0.97189, "omega":-0.04499, "ax":3.98507, "ay":-3.01476, "alpha":0.06581, "fx":[66.18718,66.0922,66.20736,67.23017], "fy":[-50.53875,-50.15547,-50.35507,-49.96919]}, + {"t":0.36256, "x":1.95774, "y":7.02999, "heading":-0.95463, "vx":1.44503, "vy":-1.09334, "omega":-0.04234, "ax":3.98471, "ay":-3.01424, "alpha":0.07817, "fx":[66.54944,66.0556,66.3859,66.70173], "fy":[-49.78044,-50.81334,-50.32701,-50.06286]}, + {"t":0.40285, "x":2.01918, "y":6.98349, "heading":-0.95634, "vx":1.60555, "vy":-1.21477, "omega":-0.03919, "ax":3.98417, "ay":-3.01344, "alpha":0.11239, "fx":[66.213,65.94208,66.13708,67.36497], "fy":[-50.32048,-50.14971,-50.48598,-49.97435]}, + {"t":0.44313, "x":2.08709, "y":6.93211, "heading":-0.95792, "vx":1.76605, "vy":-1.33617, "omega":-0.03466, "ax":3.98349, "ay":-3.01185, "alpha":0.10277, "fx":[66.54984,65.9715,66.33372,66.75665], "fy":[-49.67203,-50.70134,-50.39955,-50.05151]}, + {"t":0.48342, "x":2.16147, "y":6.87584, "heading":-0.95931, "vx":1.92652, "vy":-1.4575, "omega":-0.03052, "ax":3.98201, "ay":-3.00887, "alpha":0.12294, "fx":[66.27957,65.9188,66.14307,67.17135], "fy":[-50.02377,-50.09467,-50.47829,-50.02896]}, + {"t":0.5237, "x":2.24231, "y":6.81469, "heading":-0.96054, "vx":2.08694, "vy":-1.57871, "omega":-0.02557, "ax":3.97961, "ay":-2.99755, "alpha":0.09844, "fx":[66.43948,66.03762,66.23614,66.63938], "fy":[-49.61221,-49.92253,-50.33835,-49.99768]}, + {"t":0.56398, "x":2.32961, "y":6.74866, "heading":-0.96157, "vx":2.24725, "vy":-1.69946, "omega":-0.0216, "ax":3.69992, "ay":-2.58589, "alpha":0.14274, "fx":[61.53692,61.2011,61.3919,62.57334], "fy":[-42.81054,-43.10433,-43.41137,-43.09568]}, + {"t":0.60427, "x":2.42314, "y":6.67809, "heading":-0.96244, "vx":2.3963, "vy":-1.80364, "omega":-0.01585, "ax":-0.03626, "ay":-0.04941, "alpha":0.07027, "fx":[-0.57205,-0.7827,-0.63574,-0.42745], "fy":[-0.9084,0.06619,-1.30877,-1.14348]}, + {"t":0.64455, "x":2.51965, "y":6.6054, "heading":-0.96308, "vx":2.39484, "vy":-1.80563, "omega":-0.01302, "ax":-0.15121, "ay":-0.19992, "alpha":0.0339, "fx":[-2.35322,-2.49023,-2.39315,-2.8459], "fy":[-2.77971,-3.45761,-3.59556,-3.49745]}, + {"t":0.68484, "x":2.616, "y":6.53249, "heading":-0.96361, "vx":2.38875, "vy":-1.81368, "omega":-0.01166, "ax":-0.57996, "ay":-0.75374, "alpha":0.00225, "fx":[-9.66527,-9.68699,-9.6712,-9.64708], "fy":[-12.43571,-12.85578,-12.49569,-12.47043]}, + {"t":0.72512, "x":2.71176, "y":6.45882, "heading":-0.96407, "vx":2.36539, "vy":-1.84404, "omega":-0.01157, "ax":-1.47291, "ay":-1.82867, "alpha":-0.17037, "fx":[-24.42874,-24.38728,-24.42144,-24.97355], "fy":[-31.8073,-30.06466,-30.01313,-30.04716]}, + {"t":0.76541, "x":2.80585, "y":6.38305, "heading":-0.96454, "vx":2.30605, "vy":-1.91771, "omega":-0.01843, "ax":-3.72945, "ay":-2.66162, "alpha":-0.11484, "fx":[-62.21083,-61.94683,-62.1406,-62.37447], "fy":[-43.58148,-47.41017,-43.15359,-43.32641]}, + {"t":0.80569, "x":2.89573, "y":6.30364, "heading":-0.96528, "vx":2.15581, "vy":-2.02493, "omega":-0.02306, "ax":-4.14174, "ay":2.76805, "alpha":-0.06152, "fx":[-68.9724,-68.62137,-68.84039,-69.72879], "fy":[46.53624,45.87545,46.26295,45.89338]}, + {"t":0.84598, "x":2.97921, "y":6.22431, "heading":-0.96621, "vx":1.98896, "vy":-1.91342, "omega":-0.02553, "ax":-4.067, "ay":2.89265, "alpha":-0.1132, "fx":[-67.92209,-67.39968,-67.68817,-68.16996], "fy":[47.62393,48.65567,48.49793,48.09886]}, + {"t":0.88626, "x":3.05604, "y":6.14957, "heading":-0.96724, "vx":1.82513, "vy":-1.79689, "omega":-0.03009, "ax":-4.04089, "ay":2.93426, "alpha":-0.07901, "fx":[-67.34394,-66.93073,-67.20361,-67.96052], "fy":[49.12103,48.68573,49.17522,48.66863]}, + {"t":0.92655, "x":3.12628, "y":6.07957, "heading":-0.96845, "vx":1.66234, "vy":-1.67869, "omega":-0.03328, "ax":-4.02781, "ay":2.95479, "alpha":-0.12327, "fx":[-67.3163,-66.61921,-67.05218,-67.57882], "fy":[48.53344,50.02854,49.43879,49.01889]}, + {"t":0.96683, "x":3.18998, "y":6.01434, "heading":-0.96979, "vx":1.50008, "vy":-1.55966, "omega":-0.03824, "ax":-4.01986, "ay":2.96715, "alpha":-0.08723, "fx":[-66.94674,-66.57033,-66.82157,-67.69791], "fy":[49.64432,49.2684,49.72807,49.2028]}, + {"t":1.00712, "x":3.24715, "y":5.95392, "heading":-0.97133, "vx":1.33814, "vy":-1.44013, "omega":-0.04176, "ax":-4.01455, "ay":2.97536, "alpha":-0.10504, "fx":[-67.06035,-66.5113,-66.82992,-67.28089], "fy":[49.05847,50.04049,49.82594,49.46633]}, + {"t":1.0474, "x":3.2978, "y":5.89832, "heading":-0.97302, "vx":1.17642, "vy":-1.32026, "omega":-0.04599, "ax":-4.01073, "ay":2.98124, "alpha":-0.07608, "fx":[-66.78876,-66.4958,-66.6856,-67.45754], "fy":[49.81557,49.56661,49.91651,49.48483]}, + {"t":1.08769, "x":3.34193, "y":5.84755, "heading":-0.97487, "vx":1.01485, "vy":-1.20017, "omega":-0.04905, "ax":-4.00787, "ay":2.98564, "alpha":-0.07096, "fx":[-66.91019,-66.5162,-66.75228,-67.05818], "fy":[49.37942,50.14537,49.89638,49.65556]}, + {"t":1.12797, "x":3.37957, "y":5.80162, "heading":-0.97684, "vx":0.85339, "vy":-1.07989, "omega":-0.05191, "ax":-4.00562, "ay":2.98908, "alpha":-0.01882, "fx":[-66.69979,-66.59649,-66.69976,-67.09124], "fy":[50.09899,49.68727,49.87704,49.64255]}, + {"t":1.16825, "x":3.41069, "y":5.76055, "heading":-0.97894, "vx":0.69203, "vy":-0.95948, "omega":-0.05267, "ax":-0.77528, "ay":0.62474, "alpha":-0.07207, "fx":[-13.04127,-12.50523,-12.97009,-13.17747], "fy":[10.2568,10.38442,10.57049,10.44476]}, + {"t":1.20126, "x":3.43311, "y":5.72922, "heading":-0.98067, "vx":0.66644, "vy":-0.93886, "omega":-0.05505, "ax":-0.7394, "ay":0.6705, "alpha":-0.04234, "fx":[-12.3549,-12.1854,-12.29617,-12.46503], "fy":[10.99913,11.21519,11.30846,11.18494]}, + {"t":1.23426, "x":3.4547, "y":5.6986, "heading":-0.98249, "vx":0.64204, "vy":-0.91673, "omega":-0.05645, "ax":-0.7095, "ay":0.7024, "alpha":-0.03189, "fx":[-11.82974,-11.76763,-11.77017,-11.9403], "fy":[11.58225,11.68254,11.83537,11.73421]}, + {"t":1.26726, "x":3.4755, "y":5.66873, "heading":-0.98435, "vx":0.61863, "vy":-0.89355, "omega":-0.0575, "ax":-0.68534, "ay":0.72624, "alpha":-0.03411, "fx":[-11.44922,-11.30877,-11.3999,-11.53932], "fy":[11.94361,12.18292,12.19963,12.09782]}, + {"t":1.30026, "x":3.49555, "y":5.63964, "heading":-0.98625, "vx":0.59601, "vy":-0.86958, "omega":-0.05862, "ax":-0.66552, "ay":0.74464, "alpha":-0.02522, "fx":[-11.09141,-11.05971,-11.04046,-11.18374], "fy":[12.3075,12.39061,12.51863,12.4346]}, + {"t":1.33326, "x":3.51485, "y":5.61135, "heading":-0.98819, "vx":0.57405, "vy":-0.84501, "omega":-0.05946, "ax":-0.649, "ay":0.75924, "alpha":-0.02853, "fx":[-10.84004,-10.72081,-10.79761,-10.91574], "fy":[12.51303,12.73757,12.72989,12.64401]}, + {"t":1.36627, "x":3.53344, "y":5.58387, "heading":-0.99015, "vx":0.55263, "vy":-0.81995, "omega":-0.0604, "ax":-0.63506, "ay":0.77107, "alpha":-0.02082, "fx":[-10.58199,-10.56442,-10.53771,-10.66047], "fy":[12.76366,12.83403,12.94342,12.87218]}, + {"t":1.39927, "x":3.55134, "y":5.55723, "heading":-0.99214, "vx":0.53167, "vy":-0.79451, "omega":-0.06108, "ax":-0.62315, "ay":0.78083, "alpha":-0.02418, "fx":[-10.40636,-10.30368,-10.36937,-10.47092], "fy":[12.88863,13.0999,13.07469,13.00131]}, + {"t":1.43227, "x":3.56854, "y":5.53144, "heading":-0.99416, "vx":0.5111, "vy":-0.76874, "omega":-0.06188, "ax":-0.61286, "ay":0.78903, "alpha":-0.01769, "fx":[-10.21207,-10.19978,-10.17336,-10.27926], "fy":[13.07569,13.13583,13.23005,13.16913]}, + {"t":1.46527, "x":3.58508, "y":5.5065, "heading":-0.9962, "vx":0.49088, "vy":-0.7427, "omega":-0.06247, "ax":-0.6039, "ay":0.79599, "alpha":-0.0206, "fx":[-10.08308,-9.99454,-10.05079,-10.13826], "fy":[13.15656,13.34837,13.31639,13.2536]}, + {"t":1.49827, "x":3.60095, "y":5.48242, "heading":-0.99826, "vx":0.47095, "vy":-0.71643, "omega":-0.06315, "ax":-0.59602, "ay":0.80197, "alpha":-0.01484, "fx":[-9.93102,-9.92455,-9.89738,-9.98825], "fy":[13.30274,13.35387,13.43465,13.38281]}, + {"t":1.53127, "x":3.61616, "y":5.45921, "heading":-1.00035, "vx":0.45128, "vy":-0.68996, "omega":-0.06364, "ax":-0.58904, "ay":0.80717, "alpha":-0.01731, "fx":[-9.83311,-9.75782,-9.80533,-9.87962], "fy":[13.35787,13.52913,13.49339,13.44037]}, + {"t":1.56428, "x":3.63074, "y":5.43688, "heading":-1.00245, "vx":0.43184, "vy":-0.66332, "omega":-0.06421, "ax":-0.58282, "ay":0.81173, "alpha":-0.01196, "fx":[-9.71036,-9.71091,-9.68179,-9.75799], "fy":[13.47634,13.51872,13.58628,13.54325]}, + {"t":1.59728, "x":3.64467, "y":5.41543, "heading":-1.00456, "vx":0.41261, "vy":-0.63654, "omega":-0.0646, "ax":-0.57724, "ay":0.81575, "alpha":-0.01388, "fx":[-9.63396,-9.5724,-9.611,-9.67162], "fy":[13.51518,13.66899,13.62596,13.5828]}, + {"t":1.63028, "x":3.65797, "y":5.39487, "heading":-1.0067, "vx":0.39356, "vy":-0.60961, "omega":-0.06506, "ax":-0.5722, "ay":0.81933, "alpha":-0.00881, "fx":[-9.53253,-9.54132,-9.5096,-9.57001], "fy":[13.61498,13.64796,13.70111,13.66754]}, + {"t":1.66328, "x":3.67065, "y":5.3752, "heading":-1.00884, "vx":0.37467, "vy":-0.58258, "omega":-0.06535, "ax":-0.56764, "ay":0.82254, "alpha":-0.01001, "fx":[-9.47119,-9.42538,-9.45395,-9.49888], "fy":[13.64413,13.77887,13.72723,13.69498]}, + {"t":1.69628, "x":3.68271, "y":5.35642, "heading":-1.011, "vx":0.35594, "vy":-0.55543, "omega":-0.06568, "ax":-0.56349, "ay":0.82542, "alpha":-0.00488, "fx":[-9.38536,-9.40681,-9.36928,-9.41113], "fy":[13.73058,13.75251,13.78844,13.76596]}, + {"t":1.72929, "x":3.69414, "y":5.33854, "heading":-1.01317, "vx":0.33734, "vy":-0.52819, "omega":-0.06584, "ax":-0.5597, "ay":0.82803, "alpha":-0.00525, "fx":[-9.33503,-9.30883,-9.32515,-9.35053], "fy":[13.75458,13.86795,13.80403,13.78491]}, + {"t":1.76229, "x":3.70497, "y":5.32156, "heading":-1.01534, "vx":0.31887, "vy":-0.50086, "omega":-0.06602, "ax":-0.55622, "ay":0.8304, "alpha":0.00034, "fx":[-9.26068,-9.30126,-9.25357,-9.27186], "fy":[13.83169,13.83961,13.85335,13.84492]}, + {"t":1.79529, "x":3.71519, "y":5.30548, "heading":-1.01752, "vx":0.30052, "vy":-0.47346, "omega":-0.066, "ax":-0.55301, "ay":0.83257, "alpha":0.00106, "fx":[-9.21847,-9.21834,-9.21869,-9.21807], "fy":[13.85372,13.94362,13.8594,13.85716]}, + {"t":1.82829, "x":3.72481, "y":5.29031, "heading":-1.0197, "vx":0.28227, "vy":-0.44598, "omega":-0.06597, "ax":-0.55005, "ay":0.83455, "alpha":0.00747, "fx":[-9.1528,-9.22047,-9.15814,-9.14468], "fy":[13.92533,13.91449,13.89799,13.90837]}, + {"t":1.86129, "x":3.73383, "y":5.27605, "heading":-1.02187, "vx":0.26411, "vy":-0.41844, "omega":-0.06572, "ax":-0.5473, "ay":0.83637, "alpha":0.00982, "fx":[-9.11613,-9.15227,-9.13079,-9.09393], "fy":[13.94847,14.01028,13.89421,13.91485]}, + {"t":1.89429, "x":3.74224, "y":5.26269, "heading":-1.02404, "vx":0.24605, "vy":-0.39084, "omega":-0.0654, "ax":-0.54475, "ay":0.83806, "alpha":0.01771, "fx":[-9.05649,-9.16519,-9.07977,-9.02168], "fy":[14.01831,13.98126,13.92192,13.95854]}, + {"t":1.9273, "x":3.75007, "y":5.25025, "heading":-1.0262, "vx":0.22807, "vy":-0.36318, "omega":-0.06481, "ax":-0.54238, "ay":0.83961, "alpha":0.02234, "fx":[-9.02341,-9.11174,-9.05932,-8.9703], "fy":[14.0464,14.07075,13.90695,13.95982]}, + {"t":1.9603, "x":3.7573, "y":5.23872, "heading":-1.02834, "vx":0.21017, "vy":-0.33547, "omega":-0.06408, "ax":-0.54016, "ay":0.84106, "alpha":0.03271, "fx":[-8.96732,-9.13806,-9.01716,-8.89438], "fy":[14.11842,14.04368,13.92194,13.99628]}, + {"t":1.9933, "x":3.76394, "y":5.22811, "heading":-1.03046, "vx":0.19235, "vy":-0.30772, "omega":-0.063, "ax":-0.53809, "ay":0.84241, "alpha":0.04071, "fx":[-8.93591,-9.10108,-9.0037,-8.83787], "fy":[14.1563,14.12909,13.89257,13.99208]}, + {"t":2.0263, "x":3.77, "y":5.21841, "heading":-1.03253, "vx":0.17459, "vy":-0.27991, "omega":-0.06165, "ax":-0.53614, "ay":0.84366, "alpha":0.05488, "fx":[-8.88128,-9.1438,-8.97112,-8.7526], "fy":[14.23574,14.10571,13.8913,14.02098]}, + {"t":2.0593, "x":3.77547, "y":5.20964, "heading":-1.03457, "vx":0.1569, "vy":-0.25207, "omega":-0.05984, "ax":-0.53431, "ay":0.84484, "alpha":0.0681, "fx":[-8.84877,-9.12882,-8.96501,-8.68429], "fy":[14.2902,14.18964,13.84193,14.01021]}, + {"t":2.09231, "x":3.78035, "y":5.20178, "heading":-1.03654, "vx":0.13926, "vy":-0.22419, "omega":-0.05759, "ax":-0.53259, "ay":0.84593, "alpha":0.08835, "fx":[-8.79221,-9.1962,-8.94297,-8.58073], "fy":[14.38446,14.17208,13.81836,14.03042]}, + {"t":2.12531, "x":3.78466, "y":5.19484, "heading":-1.03844, "vx":0.12169, "vy":-0.19627, "omega":-0.05468, "ax":-0.53097, "ay":0.84697, "alpha":0.1094, "fx":[-8.75595,-9.20958,-8.94636,-8.49197], "fy":[14.46535,14.25846,13.73964,14.01072]}, + {"t":2.15831, "x":3.78838, "y":5.18882, "heading":-1.04025, "vx":0.10416, "vy":-0.16832, "omega":-0.05107, "ax":-0.52943, "ay":0.84794, "alpha":0.13908, "fx":[-8.69352,-9.31276,-8.9377,-8.35763], "fy":[14.58551,14.24923,13.68407,14.02013]}, + {"t":2.19131, "x":3.79153, "y":5.18373, "heading":-1.04193, "vx":0.08669, "vy":-0.14034, "omega":-0.04648, "ax":-0.52798, "ay":0.84885, "alpha":0.17214, "fx":[-8.64923,-9.36715,-8.95371,-8.23479], "fy":[14.70819,14.3421,13.5617,13.988]}, + {"t":2.22431, "x":3.79411, "y":5.17956, "heading":-1.04347, "vx":0.06927, "vy":-0.11233, "omega":-0.0408, "ax":-0.52661, "ay":0.84972, "alpha":0.2165, "fx":[-8.57513,-9.5245,-8.96299,-8.05059], "fy":[14.8706,14.34627,13.4582,13.98254]}, + {"t":2.25731, "x":3.79611, "y":5.17632, "heading":-1.04481, "vx":0.05189, "vy":-0.08428, "omega":-0.03365, "ax":-0.5253, "ay":0.85054, "alpha":0.26806, "fx":[-8.51689,-9.63922,-8.99705,-7.87308], "fy":[15.05798,14.45294,13.2693,13.93187]}, + {"t":2.29032, "x":3.79753, "y":5.174, "heading":-1.04593, "vx":0.03455, "vy":-0.05621, "omega":-0.02481, "ax":-0.52406, "ay":0.85131, "alpha":0.33511, "fx":[-8.42309,-9.87835,-9.03192,-7.61026], "fy":[15.28834,14.47654,13.09323,13.90556]}, + {"t":2.32332, "x":3.79839, "y":5.17261, "heading":-1.04674, "vx":0.01726, "vy":-0.02812, "omega":-0.01375, "ax":-0.52289, "ay":0.85204, "alpha":0.41656, "fx":[-8.32816,-10.131,-9.08845,-7.31739], "fy":[15.56914,14.55997,12.83665,13.84685]}, + {"t":2.35632, "x":3.79867, "y":5.17214, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/startPToE.traj b/src/main/deploy/choreo/startPToE.traj index bc586e64..a35d5f38 100644 --- a/src/main/deploy/choreo/startPToE.traj +++ b/src/main/deploy/choreo/startPToE.traj @@ -3,32 +3,32 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1, "y":2.97, "heading":3.141592653589793, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.436488628387451, "y":2.3468501567840576, "heading":2.2861969362613612, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":7.1, "y":1.8818, "heading":3.141592653589793, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.279079437255859, "y":2.2349166870117188, "heading":2.191882300800504, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":4.894185251845, "y":2.7146580097364, "heading":2.0943951023931953, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":3.7}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":3.0}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":3.3}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":3.0}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}], + {"from":2, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"startP.x", "val":7.1}, "y":{"exp":"startP.y", "val":2.97}, "heading":{"exp":"startP.heading", "val":3.141592653589793}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.436488628387451 m", "val":5.436488628387451}, "y":{"exp":"2.3468501567840576 m", "val":2.3468501567840576}, "heading":{"exp":"2.2861969362613612 rad", "val":2.2861969362613612}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"midStartP.x", "val":7.1}, "y":{"exp":"midStartP.y", "val":1.8818}, "heading":{"exp":"midStartP.heading", "val":3.141592653589793}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.279079437255859 m", "val":5.279079437255859}, "y":{"exp":"2.2349166870117188 m", "val":2.2349166870117188}, "heading":{"exp":"2.191882300800504 rad", "val":2.191882300800504}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"E.x", "val":4.894185251845}, "y":{"exp":"E.y", "val":2.7146580097364}, "heading":{"exp":"E.heading", "val":2.0943951023931953}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.7 m / s", "val":3.7}}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"maxAccel", "val":3.0}}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"maxAccel", "val":3.0}}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}], + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3.3 m / s", "val":3.3}}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"3 m / s ^ 2", "val":3.0}}}, "enabled":true}, + {"from":2, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -36,67 +36,60 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.25227,1.95131], + "waypoints":[0.0,0.924,1.57158], "samples":[ - {"t":0.0, "x":7.1, "y":2.97, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.33232, "ay":-1.88384, "alpha":-8.8329, "fx":[-61.31845,-58.50227,-17.2133,-18.4804], "fy":[-9.04283,-51.50966,-55.27404,-9.78444]}, - {"t":0.03211, "x":7.0988, "y":2.96903, "heading":3.14159, "vx":-0.07489, "vy":-0.06049, "omega":-0.28362, "ax":-2.34284, "ay":-1.8722, "alpha":-7.174, "fx":[-57.32867,-54.98771,-21.28224,-22.61729], "fy":[-12.91785,-47.59388,-50.49748,-13.82567]}, - {"t":0.06422, "x":7.09519, "y":2.96612, "heading":3.13249, "vx":-0.15012, "vy":-0.1206, "omega":-0.51397, "ax":-2.35343, "ay":-1.85879, "alpha":-5.7161, "fx":[-54.02573,-52.32362,-24.89556,-25.67752], "fy":[-16.47494,-43.92644,-45.83593,-17.70339]}, - {"t":0.09633, "x":7.08915, "y":2.96129, "heading":3.11598, "vx":-0.22568, "vy":-0.18029, "omega":-0.69751, "ax":-2.36496, "ay":-1.84403, "alpha":-4.54868, "fx":[-50.52793,-49.84878,-28.4533,-28.86078], "fy":[-18.66252,-41.03946,-43.19921,-20.05505]}, - {"t":0.12844, "x":7.08069, "y":2.95455, "heading":3.09359, "vx":-0.30162, "vy":-0.2395, "omega":-0.84357, "ax":-2.37752, "ay":-1.82771, "alpha":-3.63192, "fx":[-48.43467,-48.17757,-30.95071,-30.96579], "fy":[-20.67451,-37.7567,-41.13523,-22.30157]}, - {"t":0.16055, "x":7.06978, "y":2.94592, "heading":3.0665, "vx":-0.37796, "vy":-0.29819, "omega":-0.96019, "ax":-2.39129, "ay":-1.80956, "alpha":-2.66459, "fx":[-45.84205,-46.273,-33.86157,-33.46975], "fy":[-22.64489,-36.00422,-37.80493,-24.20422]}, - {"t":0.19266, "x":7.05641, "y":2.93541, "heading":3.03567, "vx":-0.45474, "vy":-0.35629, "omega":-1.04574, "ax":-2.40641, "ay":-1.78929, "alpha":-2.09422, "fx":[-44.76442,-45.12073,-35.54686,-35.02298], "fy":[-23.71264,-34.5182,-35.79413,-25.28124]}, - {"t":0.22477, "x":7.04056, "y":2.92305, "heading":3.00209, "vx":-0.53201, "vy":-0.41374, "omega":-1.11299, "ax":-2.42311, "ay":-1.76648, "alpha":-1.3147, "fx":[-42.85437,-43.70875,-37.8273,-37.17817], "fy":[-25.3981,-32.2411,-33.52337,-26.62276]}, - {"t":0.25688, "x":7.02223, "y":2.90885, "heading":2.96635, "vx":-0.60982, "vy":-0.47046, "omega":-1.1552, "ax":-2.44163, "ay":-1.74065, "alpha":-0.9423, "fx":[-42.59269,-43.0481,-38.83841,-38.32422], "fy":[-26.07382,-31.14773,-31.73715,-27.10421]}, - {"t":0.28898, "x":7.00139, "y":2.89285, "heading":2.92926, "vx":-0.68822, "vy":-0.52635, "omega":-1.18546, "ax":-2.46228, "ay":-1.71115, "alpha":-0.34459, "fx":[-41.19129,-41.9144,-40.72977,-40.34437], "fy":[-27.18505,-29.36619,-29.8734,-27.67188]}, - {"t":0.32109, "x":6.97803, "y":2.87507, "heading":2.8912, "vx":-0.76728, "vy":-0.5813, "omega":-1.19652, "ax":-2.4854, "ay":-1.67719, "alpha":-0.10962, "fx":[-41.59745,-41.64687,-41.27752,-41.20003], "fy":[-27.52184,-28.51217,-28.13666,-27.66144]}, - {"t":0.3532, "x":6.95211, "y":2.85554, "heading":2.85278, "vx":-0.84708, "vy":-0.63515, "omega":-1.20004, "ax":-2.51146, "ay":-1.63769, "alpha":0.2042, "fx":[-41.32805,-41.54816,-42.16059,-42.42259], "fy":[-28.04507,-26.92447,-26.56928,-27.65926]}, - {"t":0.38531, "x":6.92361, "y":2.8343, "heading":2.81424, "vx":-0.92773, "vy":-0.68774, "omega":-1.19349, "ax":-2.54099, "ay":-1.59121, "alpha":0.48862, "fx":[-41.54846,-40.8818,-43.1845,-43.81374], "fy":[-28.0446,-26.22112,-24.71882,-27.11458]}, - {"t":0.41742, "x":6.89252, "y":2.8114, "heading":2.77592, "vx":-1.00931, "vy":-0.73883, "omega":-1.1778, "ax":-2.57465, "ay":-1.53582, "alpha":0.69607, "fx":[-41.62059,-41.11125,-43.91537,-45.02582], "fy":[-27.97304,-24.65021,-23.25084,-26.53162]}, - {"t":0.44953, "x":6.85878, "y":2.78688, "heading":2.7381, "vx":-1.09199, "vy":-0.78814, "omega":-1.15545, "ax":-2.61325, "ay":-1.46878, "alpha":0.90081, "fx":[-42.31703,-40.81258,-44.83555,-46.28135], "fy":[-27.44855,-23.84138,-21.20014,-25.44563]}, - {"t":0.48164, "x":6.82237, "y":2.76082, "heading":2.701, "vx":-1.1759, "vy":-0.83531, "omega":-1.12652, "ax":-2.65768, "ay":-1.38626, "alpha":1.01844, "fx":[-42.7042,-41.47678,-45.51281,-47.51512], "fy":[-26.64737,-21.96967,-19.59551,-24.2205]}, - {"t":0.51375, "x":6.78324, "y":2.73328, "heading":2.66483, "vx":-1.26123, "vy":-0.87982, "omega":-1.09382, "ax":-2.70895, "ay":-1.28249, "alpha":1.16028, "fx":[-43.85163,-41.54175,-46.50034,-48.7341], "fy":[-25.30613,-20.74758,-17.08527,-22.37519]}, - {"t":0.54586, "x":6.74135, "y":2.70437, "heading":2.62971, "vx":-1.34821, "vy":-0.921, "omega":-1.05657, "ax":-2.7678, "ay":-1.14905, "alpha":1.18733, "fx":[-44.54529,-42.81982,-47.2,-49.98677], "fy":[-23.40773,-18.07464,-14.92899,-20.20478]}, - {"t":0.57797, "x":6.69663, "y":2.6742, "heading":2.59578, "vx":-1.43709, "vy":-0.95789, "omega":-1.01844, "ax":-2.83427, "ay":-0.97221, "alpha":1.27394, "fx":[-46.14026,-43.20664,-48.39062,-51.24648], "fy":[-20.58626,-15.7878,-11.41501,-17.03596]}, - {"t":0.61008, "x":6.64903, "y":2.64294, "heading":2.56308, "vx":-1.52809, "vy":-0.98911, "omega":-0.97754, "ax":-2.90507, "ay":-0.73168, "alpha":1.19362, "fx":[-47.0291,-45.24086,-49.06889,-52.36568], "fy":[-16.65277,-11.3141,-7.7688,-13.05141]}, - {"t":0.64219, "x":6.59846, "y":2.61081, "heading":2.53169, "vx":-1.62137, "vy":-1.0126, "omega":-0.93921, "ax":-2.96913, "ay":-0.39312, "alpha":1.22583, "fx":[-48.76014,-45.55722,-50.25533,-53.40309], "fy":[-10.81388,-6.31583,-1.92401,-7.1588]}, - {"t":0.6743, "x":6.54487, "y":2.57809, "heading":2.50154, "vx":-1.71671, "vy":-1.02523, "omega":-0.89985, "ax":-2.9927, "ay":0.09107, "alpha":0.99938, "fx":[-48.76504,-47.63423,-49.9135,-53.235], "fy":[-2.46687,2.13996,5.49714,0.90223]}, - {"t":0.70641, "x":6.48821, "y":2.54522, "heading":2.47264, "vx":-1.8128, "vy":-1.0223, "omega":-0.86776, "ax":-2.89268, "ay":0.76847, "alpha":0.98986, "fx":[-47.92173,-44.98073,-48.53309,-51.44273], "fy":[9.2776,13.28962,16.35914,12.31377]}, - {"t":0.73852, "x":6.42851, "y":2.51279, "heading":2.44478, "vx":-1.90569, "vy":-0.99763, "omega":-0.83598, "ax":-2.52506, "ay":1.60533, "alpha":0.62652, "fx":[-41.29489,-41.1387,-41.66228,-44.27023], "fy":[24.05509,27.06079,29.49928,26.42494]}, - {"t":0.77063, "x":6.36601, "y":2.48158, "heading":2.41794, "vx":-1.98676, "vy":-0.94608, "omega":-0.81586, "ax":-1.82173, "ay":2.37356, "alpha":0.59507, "fx":[-30.3326,-28.3223,-30.43649,-32.3779], "fy":[37.17981,40.86652,41.21584,39.00229]}, - {"t":0.80273, "x":6.30128, "y":2.45243, "heading":2.39174, "vx":-2.04526, "vy":-0.86987, "omega":-0.79675, "ax":-0.98989, "ay":2.82438, "alpha":0.30298, "fx":[-16.22549,-15.88476,-16.2916,-17.60182], "fy":[45.85853,47.14625,48.31072,47.009]}, - {"t":0.83484, "x":6.2351, "y":2.42595, "heading":2.36616, "vx":-2.07704, "vy":-0.77918, "omega":-0.78702, "ax":-0.28106, "ay":2.98068, "alpha":0.2164, "fx":[-4.68167,-3.9259,-4.69484,-5.43832], "fy":[48.51369,51.04547,49.96428,49.22241]}, - {"t":0.86695, "x":6.16826, "y":2.40247, "heading":2.34089, "vx":-2.08607, "vy":-0.68347, "omega":-0.78007, "ax":0.23578, "ay":2.98559, "alpha":0.11864, "fx":[3.91808,4.38718,3.91938,3.49645], "fy":[49.39652,49.76177,50.14013,49.77484]}, - {"t":0.89906, "x":6.1014, "y":2.38207, "heading":2.31584, "vx":-2.0785, "vy":-0.58761, "omega":-0.77626, "ax":0.59758, "ay":2.93546, "alpha":0.091, "fx":[9.96618,10.27556,9.973,9.6308], "fy":[48.26475,49.92462,48.92721,48.61457]}, - {"t":0.93117, "x":6.03497, "y":2.36471, "heading":2.29091, "vx":-2.05931, "vy":-0.49335, "omega":-0.77334, "ax":0.85558, "ay":2.87154, "alpha":0.19202, "fx":[14.12937,15.19165,14.17055,13.55675], "fy":[47.36662,47.81071,48.37288,47.91829]}, - {"t":0.96328, "x":5.96929, "y":2.35035, "heading":2.26608, "vx":-2.03184, "vy":-0.40115, "omega":-0.76718, "ax":1.04353, "ay":2.80921, "alpha":0.31026, "fx":[17.35391,18.47125,17.4577,16.29767], "fy":[45.4993,47.41918,47.67068,46.72335]}, - {"t":0.99539, "x":5.90458, "y":2.33892, "heading":2.24145, "vx":-1.99833, "vy":-0.31094, "omega":-0.75722, "ax":1.18554, "ay":2.7527, "alpha":0.61449, "fx":[19.45702,22.28987,19.78425,17.51838], "fy":[44.06614,45.58576,47.70927,46.18355]}, - {"t":1.0275, "x":5.84103, "y":2.33035, "heading":2.21713, "vx":-1.96026, "vy":-0.22256, "omega":-0.73748, "ax":1.29534, "ay":2.70311, "alpha":0.81449, "fx":[21.27643,24.81818,21.99596,18.27992], "fy":[42.10924,45.45821,46.73284,45.93764]}, - {"t":1.05961, "x":5.77875, "y":2.3246, "heading":2.19345, "vx":-1.91867, "vy":-0.13576, "omega":-0.71133, "ax":1.38256, "ay":2.65983, "alpha":1.48598, "fx":[22.26387,28.70954,23.53854,17.67448], "fy":[39.73545,43.35685,48.90846,45.35155]}, - {"t":1.09172, "x":5.71786, "y":2.32161, "heading":2.17061, "vx":-1.87428, "vy":-0.05036, "omega":-0.66362, "ax":1.45315, "ay":2.62216, "alpha":1.91805, "fx":[23.15748,31.45254,25.2852,16.99817], "fy":[38.19662,42.16078,49.74306,44.74043]}, - {"t":1.12383, "x":5.65843, "y":2.32135, "heading":2.1493, "vx":-1.82762, "vy":0.03384, "omega":-0.60203, "ax":1.5114, "ay":2.58923, "alpha":2.97354, "fx":[23.3419,36.08952,26.78139,14.56466], "fy":[33.78777,40.77294,52.35707,45.72688]}, - {"t":1.15594, "x":5.60052, "y":2.32377, "heading":2.12997, "vx":-1.77909, "vy":0.11698, "omega":-0.50655, "ax":1.56017, "ay":2.56031, "alpha":3.82738, "fx":[23.56526,39.63844,28.5187,12.30691], "fy":[30.61998,39.35768,54.71925,46.01955]}, - {"t":1.18805, "x":5.5442, "y":2.32884, "heading":2.11371, "vx":-1.72899, "vy":0.19919, "omega":-0.38366, "ax":1.60157, "ay":2.53477, "alpha":5.1641, "fx":[23.92209,44.45648,29.62234,8.78873], "fy":[25.06438,37.5162,58.75812,47.67481]}, - {"t":1.22016, "x":5.48951, "y":2.33655, "heading":2.10139, "vx":-1.67757, "vy":0.28058, "omega":-0.21784, "ax":1.63711, "ay":2.51209, "alpha":6.78433, "fx":[22.48496,50.85332,32.2965,3.5245], "fy":[19.80092,35.63148,62.99348,49.07572]}, - {"t":1.25227, "x":5.43649, "y":2.34685, "heading":2.0944, "vx":-1.625, "vy":0.36124, "omega":0.0, "ax":1.74918, "ay":2.43058, "alpha":0.0, "fx":[29.15593,29.19507,29.18989,29.09103], "fy":[40.12785,41.37278,40.31544,40.25042]}, - {"t":1.28906, "x":5.37789, "y":2.36179, "heading":2.0944, "vx":-1.56065, "vy":0.45066, "omega":0.0, "ax":1.96507, "ay":2.26288, "alpha":0.0, "fx":[32.43522,32.89462,32.6165,33.0808], "fy":[37.83234,37.47784,37.92356,37.65094]}, - {"t":1.32585, "x":5.3218, "y":2.3799, "heading":2.0944, "vx":-1.48835, "vy":0.53392, "omega":0.0, "ax":2.19917, "ay":2.03573, "alpha":0.0, "fx":[36.66025,36.67183,36.67451,36.62969], "fy":[33.74879,34.33527,33.8433,33.81103]}, - {"t":1.36264, "x":5.26853, "y":2.40092, "heading":2.0944, "vx":-1.40744, "vy":0.60882, "omega":0.0, "ax":2.44205, "ay":1.73646, "alpha":0.0, "fx":[40.33068,40.81825,40.51823,41.16433], "fy":[28.9636,28.71263,29.20989,28.89747]}, - {"t":1.39943, "x":5.2184, "y":2.42449, "heading":2.0944, "vx":-1.31759, "vy":0.6727, "omega":0.0, "ax":2.67256, "ay":1.35458, "alpha":0.0, "fx":[44.54958,44.5622,44.56007,44.52938], "fy":[22.44641,22.87166,22.51307,22.48991]}, - {"t":1.43622, "x":5.17173, "y":2.45016, "heading":2.0944, "vx":-1.21926, "vy":0.72254, "omega":0.0, "ax":2.85984, "ay":0.8932, "alpha":0.0, "fx":[47.26672,47.71009,47.43829,48.27343], "fy":[14.73095,14.72381,15.20086,14.90091]}, - {"t":1.47302, "x":5.12881, "y":2.47735, "heading":2.0944, "vx":-1.11404, "vy":0.7554, "omega":0.0, "ax":2.97221, "ay":0.37693, "alpha":0.0, "fx":[49.54462,49.54843,49.5465,49.54175], "fy":[6.25995,6.33515,6.27102,6.26703]}, - {"t":1.50981, "x":5.08983, "y":2.5054, "heading":2.0944, "vx":-1.00469, "vy":0.76927, "omega":0.0, "ax":2.9923, "ay":-0.15048, "alpha":0.0, "fx":[49.49595,49.82054,49.63581,50.56848], "fy":[-2.87668,-2.54993,-2.20624,-2.40053]}, - {"t":1.5466, "x":5.05489, "y":2.5336, "heading":2.0944, "vx":-0.8946, "vy":0.76374, "omega":0.0, "ax":2.92655, "ay":-0.64253, "alpha":0.0, "fx":[48.79023,48.76453,48.78147,48.80036], "fy":[-10.61345,-10.93528,-10.65462,-10.63915]}, - {"t":1.58339, "x":5.02396, "y":2.56126, "heading":2.0944, "vx":-0.78693, "vy":0.7401, "omega":0.0, "ax":2.79971, "ay":-1.06799, "alpha":0.0, "fx":[46.35384,46.54492,46.45397,47.32642], "fy":[-18.2869,-17.73538,-17.56094,-17.62825]}, - {"t":1.62018, "x":4.9969, "y":2.58777, "heading":2.0944, "vx":-0.68392, "vy":0.7008, "omega":0.0, "ax":2.64071, "ay":-1.41677, "alpha":0.0, "fx":[44.03333,43.97793,44.01763,44.04866], "fy":[-23.44945,-24.01335,-23.51486,-23.48958]}, - {"t":1.65697, "x":4.97353, "y":2.61259, "heading":2.0944, "vx":-0.58676, "vy":0.64868, "omega":0.0, "ax":2.47244, "ay":-1.69388, "alpha":0.0, "fx":[40.98379,41.06621,41.04271,41.76489], "fy":[-28.72907,-28.10739,-28.06275,-28.04555]}, - {"t":1.69377, "x":4.95361, "y":2.63531, "heading":2.0944, "vx":-0.4958, "vy":0.58636, "omega":0.0, "ax":2.30902, "ay":-1.91105, "alpha":0.0, "fx":[38.50805,38.43893,38.48999,38.52431], "fy":[-31.66615,-32.31223,-31.73744,-31.70949]}, - {"t":1.73056, "x":4.93693, "y":2.65559, "heading":2.0944, "vx":-0.41085, "vy":0.51605, "omega":0.0, "ax":2.15752, "ay":-2.08091, "alpha":0.0, "fx":[35.81151,35.81583,35.83484,36.39712], "fy":[-35.13825,-34.53361,-34.56962,-34.50971]}, - {"t":1.76735, "x":4.92328, "y":2.67317, "heading":2.0944, "vx":-0.33147, "vy":0.43949, "omega":0.0, "ax":2.02053, "ay":-2.21445, "alpha":0.0, "fx":[33.6996,33.62827,33.6814,33.71564], "fy":[-36.7236,-37.37176,-36.79366,-36.76605]}, - {"t":1.80414, "x":4.91245, "y":2.68784, "heading":2.0944, "vx":-0.25713, "vy":0.35801, "omega":0.0, "ax":1.8982, "ay":-2.32039, "alpha":0.0, "fx":[31.54929,31.5011,31.54591,31.97228], "fy":[-39.07552,-38.5187,-38.60244,-38.52243]}, - {"t":1.84093, "x":4.90428, "y":2.69944, "heading":2.0944, "vx":-0.18729, "vy":0.27264, "omega":0.0, "ax":1.78959, "ay":-2.40535, "alpha":0.0, "fx":[29.84929,29.78073,29.83177,29.86479], "fy":[-39.91393,-40.53483,-39.98071,-39.95437]}, - {"t":1.87772, "x":4.8986, "y":2.70784, "heading":2.0944, "vx":-0.12145, "vy":0.18415, "omega":0.0, "ax":1.69327, "ay":-2.47427, "alpha":0.0, "fx":[28.24307,28.17663,28.22609,28.25809], "fy":[-41.06834,-41.67041,-41.13314,-41.10758]}, - {"t":1.91452, "x":4.89527, "y":2.71295, "heading":2.0944, "vx":-0.05915, "vy":0.09311, "omega":0.0, "ax":1.60774, "ay":-2.53082, "alpha":0.0, "fx":[26.77933,26.8623,26.75715,26.80246], "fy":[-42.16478,-42.17741,-42.21426,-42.19388]}, - {"t":1.95131, "x":4.89419, "y":2.71466, "heading":2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.1, "y":1.8818, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.99439, "ay":-0.1386, "alpha":-14.9933, "fx":[-128.09942,-126.40153,-72.58794,-72.60561], "fy":[36.35426,-41.60975,-54.40778,50.42181]}, + {"t":0.0308, "x":7.09716, "y":1.88173, "heading":3.14159, "vx":-0.18463, "vy":-0.00427, "omega":-0.46179, "ax":-5.9967, "ay":-0.11897, "alpha":-12.41623, "fx":[-123.31088,-122.01812,-77.26715,-77.25215], "fy":[31.05307,-35.40541,-44.10312,40.52287]}, + {"t":0.0616, "x":7.08863, "y":1.88155, "heading":3.12737, "vx":-0.36932, "vy":-0.00793, "omega":-0.84421, "ax":-5.99696, "ay":-0.09686, "alpha":-9.94232, "fx":[-118.76493,-119.01686,-81.36174,-80.72242], "fy":[25.55714,-28.08797,-34.1345,30.20701]}, + {"t":0.0924, "x":7.07441, "y":1.88126, "heading":3.10137, "vx":-0.55403, "vy":-0.01092, "omega":-1.15043, "ax":-5.99717, "ay":-0.07171, "alpha":-7.60693, "fx":[-114.05647,-112.95618,-87.10988,-85.75693], "fy":[21.43954,-22.50602,-26.91284,23.19786]}, + {"t":0.1232, "x":7.0545, "y":1.88089, "heading":3.06593, "vx":-0.73874, "vy":-0.01312, "omega":-1.38472, "ax":-5.99727, "ay":-0.043, "alpha":-5.5908, "fx":[-109.94015,-111.39448,-90.17234,-88.37926], "fy":[16.0333,-15.45372,-18.92927,15.48234]}, + {"t":0.154, "x":7.0289, "y":1.88046, "heading":3.02329, "vx":-0.92346, "vy":-0.01445, "omega":-1.55692, "ax":-5.99721, "ay":-0.00984, "alpha":-3.77642, "fx":[-106.30427,-108.13425,-93.62728,-91.8169], "fy":[11.60111,-9.55962,-12.8186,10.12068]}, + {"t":0.1848, "x":6.99761, "y":1.88001, "heading":2.97533, "vx":-1.10817, "vy":-0.01475, "omega":-1.67323, "ax":-5.99692, "ay":0.02883, "alpha":-1.86711, "fx":[-102.89014,-104.04983,-97.07768,-95.84545], "fy":[6.68321,-4.26227,-5.76427,5.26597]}, + {"t":0.2156, "x":6.96064, "y":1.87957, "heading":2.9238, "vx":-1.29287, "vy":-0.01386, "omega":-1.73074, "ax":-5.99625, "ay":0.07455, "alpha":-0.30968, "fx":[-100.24469,-100.93373,-99.43888,-99.20121], "fy":[2.21524,0.55704,0.27384,1.92449]}, + {"t":0.2464, "x":6.91797, "y":1.87918, "heading":2.87049, "vx":-1.47756, "vy":-0.01157, "omega":-1.74028, "ax":-5.99499, "ay":0.1294, "alpha":1.1508, "fx":[-98.36031,-97.08311,-101.52554,-102.76537], "fy":[-1.86682,4.44384,6.24887,-0.19758]}, + {"t":0.2772, "x":6.86962, "y":1.87888, "heading":2.81689, "vx":-1.6622, "vy":-0.00758, "omega":-1.70483, "ax":-5.99277, "ay":0.19633, "alpha":2.38355, "fx":[-96.7549,-94.40631,-102.70542,-105.71959], "fy":[-5.69587,8.06882,12.03616,-1.31796]}, + {"t":0.308, "x":6.81558, "y":1.87874, "heading":2.76438, "vx":-1.84678, "vy":-0.00154, "omega":-1.63142, "ax":-5.98893, "ay":0.28029, "alpha":3.48273, "fx":[-96.28519,-90.86716,-103.60282,-108.57484], "fy":[-8.52417,11.09012,17.48899,-1.36573]}, + {"t":0.3388, "x":6.75586, "y":1.87883, "heading":2.71413, "vx":-2.03124, "vy":0.0071, "omega":-1.52415, "ax":-5.98234, "ay":0.38683, "alpha":4.26335, "fx":[-95.43852,-89.12243,-103.64984,-110.68035], "fy":[-10.31688,13.6788,22.58533,-0.15429]}, + {"t":0.3696, "x":6.69046, "y":1.87923, "heading":2.66719, "vx":-2.21549, "vy":0.01901, "omega":-1.39284, "ax":-5.97045, "ay":0.53058, "alpha":4.956, "fx":[-95.99468,-85.85588,-103.63822,-112.60949], "fy":[-10.44713,16.56844,27.34506,1.91196]}, + {"t":0.4004, "x":6.61939, "y":1.88007, "heading":2.62429, "vx":-2.39938, "vy":0.03535, "omega":-1.2402, "ax":-5.94881, "ay":0.72475, "alpha":5.35315, "fx":[-95.22222,-84.76144,-102.83021,-113.84139], "fy":[-9.08269,19.49465,32.38666,5.52613]}, + {"t":0.4312, "x":6.54267, "y":1.8815, "heading":2.58609, "vx":-2.5826, "vy":0.05768, "omega":-1.07532, "ax":-5.90407, "ay":1.01718, "alpha":5.4628, "fx":[-96.14617,-82.28069,-101.53434,-113.71071], "fy":[-4.24998,24.22073,37.17754,10.67533]}, + {"t":0.462, "x":6.46033, "y":1.88376, "heading":2.55297, "vx":-2.76445, "vy":0.089, "omega":-0.90707, "ax":-5.8032, "ay":1.47716, "alpha":5.21188, "fx":[-94.21156,-82.25395,-98.48082,-111.99998], "fy":[4.21256,30.73058,44.49858,19.05244]}, + {"t":0.4928, "x":6.37243, "y":1.8872, "heading":2.52504, "vx":-2.94318, "vy":0.1345, "omega":-0.74654, "ax":-5.50615, "ay":2.34121, "alpha":4.60673, "fx":[-91.16705,-76.86978,-93.05493,-106.04798], "fy":[21.83577,45.22557,54.83559,34.21072]}, + {"t":0.5236, "x":6.27917, "y":1.89246, "heading":2.50204, "vx":-3.11277, "vy":0.20661, "omega":-0.60466, "ax":-4.32808, "ay":4.11684, "alpha":3.55979, "fx":[-73.65722,-56.47816,-74.1023,-84.35053], "fy":[57.92725,72.52433,78.52692,65.52419]}, + {"t":0.5544, "x":6.18124, "y":1.90077, "heading":2.48342, "vx":-3.24608, "vy":0.33341, "omega":-0.49501, "ax":-0.16235, "ay":5.96169, "alpha":1.02086, "fx":[-2.13192,1.36172,-3.25159,-6.80349], "fy":[96.54403,99.0798,102.4585,99.43201]}, + {"t":0.5852, "x":6.08119, "y":1.91387, "heading":2.46817, "vx":-3.25108, "vy":0.51703, "omega":-0.46357, "ax":3.56055, "ay":4.79665, "alpha":-1.72068, "fx":[60.95616,48.5043,62.8108,65.13935], "fy":[83.19888,80.97356,76.03719,79.62192]}, + {"t":0.616, "x":5.98274, "y":1.93207, "heading":2.4539, "vx":-3.14141, "vy":0.66476, "omega":-0.51657, "ax":4.9625, "ay":3.34258, "alpha":-2.6282, "fx":[80.66536,73.95701,85.12035,91.14728], "fy":[64.92937,56.61467,46.04663,55.28603]}, + {"t":0.6468, "x":5.88834, "y":1.95413, "heading":2.43799, "vx":-2.98857, "vy":0.76771, "omega":-0.59752, "ax":5.44414, "ay":2.49407, "alpha":-3.23986, "fx":[87.93434,81.75425,92.72286,100.59334], "fy":[54.44994,41.72059,28.86252,41.26659]}, + {"t":0.6776, "x":5.79888, "y":1.97896, "heading":2.41958, "vx":-2.82089, "vy":0.84453, "omega":-0.6973, "ax":5.64006, "ay":2.02044, "alpha":-3.48969, "fx":[92.04898,83.31523,96.41571,104.28847], "fy":[47.1078,34.21407,20.12777,33.26928]}, + {"t":0.7084, "x":5.71467, "y":2.00593, "heading":2.39811, "vx":-2.64718, "vy":0.90676, "omega":-0.80479, "ax":5.73985, "ay":1.72264, "alpha":-3.11406, "fx":[93.86515,87.37072,96.92394,104.56257], "fy":[41.58419,29.09507,15.93221,28.25083]}, + {"t":0.7392, "x":5.63586, "y":2.03467, "heading":2.37332, "vx":-2.47039, "vy":0.95982, "omega":-0.9007, "ax":5.79764, "ay":1.52152, "alpha":-2.63431, "fx":[95.68516,88.86263,97.80925,104.21838], "fy":[35.89304,25.80939,14.97212,24.77767]}, + {"t":0.77, "x":5.56252, "y":2.06495, "heading":2.34558, "vx":-2.29182, "vy":1.00668, "omega":-0.98184, "ax":5.83514, "ay":1.37441, "alpha":-1.61755, "fx":[96.64588,93.14243,97.46301,101.82492], "fy":[29.78951,23.18021,16.18624,22.48716]}, + {"t":0.8008, "x":5.4947, "y":2.09661, "heading":2.31534, "vx":-2.1121, "vy":1.04901, "omega":-1.03166, "ax":5.86064, "ay":1.26434, "alpha":-0.41373, "fx":[97.74818,96.20287,97.6355,99.18966], "fy":[22.58195,20.71213,19.85082,21.15917]}, + {"t":0.8316, "x":5.43243, "y":2.12952, "heading":2.28356, "vx":-1.9316, "vy":1.08795, "omega":-1.0444, "ax":5.8792, "ay":1.1776, "alpha":1.12639, "fx":[98.11507,101.46439,97.70201,94.7321], "fy":[15.30758,18.56854,24.01344,20.63077]}, + {"t":0.8624, "x":5.37573, "y":2.16359, "heading":2.25139, "vx":-1.75052, "vy":1.12422, "omega":-1.00971, "ax":5.89299, "ay":1.10873, "alpha":2.83105, "fx":[98.49123,105.93311,98.19535,90.31323], "fy":[6.99224,15.73869,30.05012,21.14675]}, + {"t":0.8932, "x":5.3246, "y":2.19874, "heading":2.2203, "vx":-1.56901, "vy":1.15837, "omega":-0.92251, "ax":5.90382, "ay":1.05138, "alpha":5.01841, "fx":[98.36577,112.20771,99.18618,83.8959], "fy":[-2.91331,13.20448,37.14528,22.66722]}, + {"t":0.924, "x":5.27908, "y":2.23492, "heading":2.19188, "vx":-1.38718, "vy":1.19075, "omega":-0.76794, "ax":2.97901, "ay":0.29647, "alpha":5.49032, "fx":[47.21209,67.15398,52.62888,31.63995], "fy":[-14.71451,1.53214,24.37169,8.57902]}, + {"t":0.95483, "x":5.23772, "y":2.27178, "heading":2.1682, "vx":-1.29531, "vy":1.1999, "omega":-0.59864, "ax":2.99406, "ay":-0.13114, "alpha":4.26394, "fx":[47.05359,63.56489,52.40462,36.61501], "fy":[-17.72423,-4.90313,13.00028,0.88316]}, + {"t":0.98567, "x":5.1992, "y":2.30872, "heading":2.14974, "vx":-1.20298, "vy":1.19585, "omega":-0.46715, "ax":2.94948, "ay":-0.53184, "alpha":3.33512, "fx":[46.90521,59.63972,51.6393,38.48156], "fy":[-20.63068,-11.52812,3.0841,-6.38704]}, + {"t":1.01651, "x":5.1635, "y":2.34534, "heading":2.13534, "vx":-1.11203, "vy":1.17945, "omega":-0.3643, "ax":2.8624, "ay":-0.88876, "alpha":2.641, "fx":[45.31382,56.08331,49.67901,39.78307], "fy":[-24.55257,-16.51608,-5.37554,-12.81653]}, + {"t":1.04734, "x":5.13057, "y":2.38129, "heading":2.1241, "vx":-1.02376, "vy":1.15204, "omega":-0.28286, "ax":2.74882, "ay":-1.19509, "alpha":2.09777, "fx":[44.05187,52.41633,47.68923,39.12844], "fy":[-27.14914,-21.80279,-12.42806,-18.30662]}, + {"t":1.07818, "x":5.10031, "y":2.41625, "heading":2.11538, "vx":-0.939, "vy":1.11519, "omega":-0.21817, "ax":2.62259, "ay":-1.45166, "alpha":1.67297, "fx":[41.91712,48.99178,45.11422,38.84625], "fy":[-30.45237,-25.24194,-18.23556,-22.8641]}, + {"t":1.10902, "x":5.0726, "y":2.44995, "heading":2.10865, "vx":-0.85812, "vy":1.07043, "omega":-0.16658, "ax":2.49373, "ay":-1.66362, "alpha":1.34364, "fx":[40.30457,45.8165,42.88796,37.26805], "fy":[-32.23925,-29.0742,-22.96053,-26.6533]}, + {"t":1.13986, "x":5.04732, "y":2.48216, "heading":2.10351, "vx":-0.78122, "vy":1.01912, "omega":-0.12515, "ax":2.36855, "ay":-1.83773, "alpha":1.07535, "fx":[38.20149,42.86169,40.42148,36.44591], "fy":[-34.72542,-31.27497,-26.8161,-29.71984]}, + {"t":1.17069, "x":5.02436, "y":2.51272, "heading":2.09965, "vx":-0.70818, "vy":0.96245, "omega":-0.09199, "ax":2.25053, "ay":-1.98074, "alpha":0.86773, "fx":[36.65464,40.27095,38.41001,34.72515], "fy":[-35.84397,-34.00394,-29.94439,-32.27936]}, + {"t":1.20153, "x":5.00359, "y":2.54145, "heading":2.09682, "vx":-0.63878, "vy":0.90137, "omega":-0.06523, "ax":2.14122, "ay":-2.09861, "alpha":0.69299, "fx":[34.80927,37.85419,36.29867,33.81016], "fy":[-37.69309,-35.36538,-32.52549,-34.34746]}, + {"t":1.23237, "x":4.98491, "y":2.56825, "heading":2.09481, "vx":-0.57275, "vy":0.83666, "omega":-0.04386, "ax":2.04104, "ay":-2.19633, "alpha":0.5536, "fx":[33.46169,35.78353,34.60895,32.23872], "fy":[-38.35234,-37.34832,-34.6425,-36.10368]}, + {"t":1.2632, "x":4.96822, "y":2.59301, "heading":2.09345, "vx":-0.50981, "vy":0.76893, "omega":-0.02679, "ax":1.94976, "ay":-2.27789, "alpha":0.43538, "fx":[31.91064,33.8366,32.87042,31.38865], "fy":[-39.753,-38.17729,-36.42111,-37.53415]}, + {"t":1.29404, "x":4.95343, "y":2.61564, "heading":2.09263, "vx":-0.44969, "vy":0.69869, "omega":-0.01336, "ax":1.8668, "ay":-2.34649, "alpha":0.33445, "fx":[30.77721,32.17639,31.47875,30.0424], "fy":[-40.11243,-39.66863,-37.9052,-38.77303]}, + {"t":1.32488, "x":4.94045, "y":2.63607, "heading":2.09222, "vx":-0.39212, "vy":0.62633, "omega":-0.00305, "ax":1.79145, "ay":-2.40462, "alpha":0.25131, "fx":[29.49715,30.6021,30.05893,29.29241], "fy":[-41.20483,-40.15866,-39.17897,-39.79276]}, + {"t":1.35572, "x":4.92921, "y":2.65424, "heading":2.09212, "vx":-0.33688, "vy":0.55218, "omega":0.0047, "ax":1.72298, "ay":-2.45424, "alpha":0.17022, "fx":[28.54787,29.2471,28.91009,28.17973], "fy":[-41.36015,-41.32108,-40.26469,-40.69831]}, + {"t":1.38655, "x":4.91964, "y":2.6701, "heading":2.09227, "vx":-0.28375, "vy":0.47649, "omega":0.00995, "ax":1.66065, "ay":-2.49692, "alpha":0.10731, "fx":[27.49494,27.95318,27.74389,27.53706], "fy":[-42.22793,-41.59314,-41.21846,-41.45055]}, + {"t":1.41739, "x":4.91168, "y":2.6836, "heading":2.09257, "vx":-0.23254, "vy":0.39949, "omega":0.01326, "ax":1.60382, "ay":-2.53388, "alpha":0.0355, "fx":[26.69892,26.82273,26.78412,26.63365], "fy":[-42.24017,-42.5276,-42.05169,-42.13491]}, + {"t":1.44823, "x":4.90527, "y":2.69472, "heading":2.09298, "vx":-0.18308, "vy":0.32136, "omega":0.01435, "ax":1.55187, "ay":-2.5661, "alpha":-0.01747, "fx":[25.83522,25.7315,25.81457,26.0943], "fy":[-42.93683,-42.65402,-42.8031,-42.70837]}, + {"t":1.47907, "x":4.90036, "y":2.70341, "heading":2.09342, "vx":-0.13522, "vy":0.24223, "omega":0.01382, "ax":1.50427, "ay":-2.59435, "alpha":-0.08739, "fx":[25.16289,24.76447,24.99976,25.3748], "fy":[-42.83775,-43.42512,-43.47881,-43.24437]}, + {"t":1.5099, "x":4.89691, "y":2.70964, "heading":2.09385, "vx":-0.08884, "vy":0.16222, "omega":0.01112, "ax":1.46055, "ay":-2.61927, "alpha":-0.14896, "fx":[24.49505,23.83697,24.2095,24.84526], "fy":[-43.0461,-43.78442,-44.10546,-43.71163]}, + {"t":1.54074, "x":4.89486, "y":2.7134, "heading":2.09419, "vx":-0.0438, "vy":0.08145, "omega":0.00653, "ax":1.42029, "ay":-2.64136, "alpha":-0.21166, "fx":[23.87205,22.97915,23.44909,24.4022], "fy":[-43.33673,-43.85644,-44.7267,-44.20119]}, + {"t":1.57158, "x":4.89419, "y":2.71466, "heading":2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/startPToESlow.traj b/src/main/deploy/choreo/startPToESlow.traj index 2cbf88a8..05ada68a 100644 --- a/src/main/deploy/choreo/startPToESlow.traj +++ b/src/main/deploy/choreo/startPToESlow.traj @@ -4,8 +4,8 @@ "snapshot":{ "waypoints":[ {"x":7.1, "y":2.97, "heading":3.141592653589793, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.436488628387451, "y":2.3468501567840576, "heading":2.0943951023931953, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":4.837096751845746, "y":2.813538192264317, "heading":2.0943951023931953, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":5.436488628387451, "y":2.3468501567840576, "heading":2.0943951023931953, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.894185251845, "y":2.7146580097364, "heading":2.0943951023931953, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -18,7 +18,7 @@ "params":{ "waypoints":[ {"x":{"exp":"startP.x", "val":7.1}, "y":{"exp":"startP.y", "val":2.97}, "heading":{"exp":"startP.heading", "val":3.141592653589793}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.436488628387451 m", "val":5.436488628387451}, "y":{"exp":"2.3468501567840576 m", "val":2.3468501567840576}, "heading":{"exp":"E.heading", "val":2.0943951023931953}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.436488628387451 m", "val":5.436488628387451}, "y":{"exp":"2.3468501567840576 m", "val":2.3468501567840576}, "heading":{"exp":"E.heading", "val":2.0943951023931953}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"E.x", "val":4.894185251845}, "y":{"exp":"E.y", "val":2.7146580097364}, "heading":{"exp":"E.heading", "val":2.0943951023931953}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -34,76 +34,74 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.0817,2.32501], + "waypoints":[0.0,1.07758,2.23079], "samples":[ - {"t":0.0, "x":7.1, "y":2.97, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.08719, "ay":-2.8748, "alpha":-10.75427, "fx":[-97.12634,-88.42882,-39.96645,-47.00405], "fy":[-18.24246,-70.03524,-81.27341,-22.13497]}, - {"t":0.0338, "x":7.09766, "y":2.96836, "heading":3.14159, "vx":-0.13816, "vy":-0.09718, "omega":-0.36353, "ax":-4.09756, "ay":-2.86248, "alpha":-8.63699, "fx":[-91.56831,-84.60826,-45.76351,-51.27734], "fy":[-23.4384,-65.70478,-74.29325,-27.42856]}, - {"t":0.06761, "x":7.09065, "y":2.96344, "heading":3.1293, "vx":-0.27667, "vy":-0.19394, "omega":-0.65549, "ax":-4.10789, "ay":-2.84745, "alpha":-6.73097, "fx":[-86.57124,-81.43961,-50.97211,-54.9232], "fy":[-27.87254,-61.54274,-67.89052,-32.55679]}, - {"t":0.10141, "x":7.07895, "y":2.95525, "heading":3.10715, "vx":-0.41553, "vy":-0.29019, "omega":-0.88302, "ax":-4.11955, "ay":-2.83034, "alpha":-5.12479, "fx":[-82.01261,-78.93484,-55.56859,-58.16763], "fy":[-32.06511,-57.56877,-63.07168,-36.01581]}, - {"t":0.13521, "x":7.06255, "y":2.94383, "heading":3.0773, "vx":-0.55479, "vy":-0.38587, "omega":-1.05625, "ax":-4.13302, "ay":-2.81038, "alpha":-3.6381, "fx":[-78.17632,-76.34788,-59.82433,-61.23315], "fy":[-35.8156,-54.22066,-58.12225,-39.23214]}, - {"t":0.16902, "x":7.04144, "y":2.92918, "heading":3.04159, "vx":-0.69449, "vy":-0.48087, "omega":-1.17923, "ax":-4.14851, "ay":-2.78716, "alpha":-2.37094, "fx":[-74.87828,-74.4608,-63.37532,-63.90033], "fy":[-39.09319,-50.83473,-54.01311,-41.90138]}, - {"t":0.20282, "x":7.01559, "y":2.91133, "heading":3.00173, "vx":-0.83473, "vy":-0.57508, "omega":-1.25938, "ax":-4.16678, "ay":-2.75942, "alpha":-1.23544, "fx":[-72.3638,-72.16186,-66.61275,-66.69439], "fy":[-41.98462,-48.46419,-49.87806,-43.66572]}, - {"t":0.23662, "x":6.985, "y":2.89032, "heading":2.95916, "vx":-0.97558, "vy":-0.66836, "omega":-1.30114, "ax":-4.18834, "ay":-2.72615, "alpha":-0.27463, "fx":[-70.29475,-70.74215,-69.13905,-69.09439], "fy":[-44.64184,-45.76254,-46.3088,-45.06113]}, - {"t":0.27043, "x":6.94963, "y":2.86617, "heading":2.91518, "vx":-1.11716, "vy":-0.76051, "omega":-1.31042, "ax":-4.21448, "ay":-2.68501, "alpha":0.56074, "fx":[-69.04593,-68.77289,-71.49986,-71.69492], "fy":[-46.56664,-44.1648,-42.76765,-45.53254]}, - {"t":0.30423, "x":6.90945, "y":2.83892, "heading":2.87088, "vx":-1.25962, "vy":-0.85127, "omega":-1.29147, "ax":-4.24646, "ay":-2.63346, "alpha":1.24846, "fx":[-68.12356,-67.97277,-73.19823,-73.85151], "fy":[-48.38517,-41.96461,-39.5081,-45.73602]}, - {"t":0.33803, "x":6.86445, "y":2.80864, "heading":2.82723, "vx":-1.40317, "vy":-0.94029, "omega":-1.24926, "ax":-4.2868, "ay":-2.56637, "alpha":1.87025, "fx":[-68.00704,-66.54764,-75.03955,-76.24142], "fy":[-49.35215,-40.74238,-35.93097,-45.09519]}, - {"t":0.37184, "x":6.81457, "y":2.77539, "heading":2.785, "vx":-1.54807, "vy":-1.02704, "omega":-1.18604, "ax":-4.33877, "ay":-2.47626, "alpha":2.23809, "fx":[-68.24221,-66.78101,-76.10567,-78.17179], "fy":[-49.55006,-38.4681,-33.1419,-43.95211]}, - {"t":0.40564, "x":6.75976, "y":2.73926, "heading":2.7449, "vx":-1.69474, "vy":-1.11075, "omega":-1.11039, "ax":-4.40839, "ay":-2.34825, "alpha":2.6112, "fx":[-69.42001,-66.25769,-77.77763,-80.48751], "fy":[-48.57557,-36.82713,-29.38784,-41.78652]}, - {"t":0.43944, "x":6.69995, "y":2.70037, "heading":2.70737, "vx":-1.84376, "vy":-1.19013, "omega":-1.02212, "ax":-4.50506, "ay":-2.15403, "alpha":2.57847, "fx":[-71.32436,-68.56595,-79.42936,-81.0694], "fy":[-45.92194,-33.03317,-25.72984,-38.94171]}, - {"t":0.47325, "x":6.63505, "y":2.65891, "heading":2.67282, "vx":-1.99604, "vy":-1.26294, "omega":-0.93496, "ax":-4.64575, "ay":-1.82532, "alpha":2.65122, "fx":[-74.1679,-69.75871,-80.96225,-84.88111], "fy":[-40.07731,-28.79971,-20.23613,-32.59562]}, - {"t":0.50705, "x":6.56493, "y":2.61518, "heading":2.64121, "vx":-2.15308, "vy":-1.32464, "omega":-0.84534, "ax":-4.84801, "ay":-1.17193, "alpha":2.21538, "fx":[-77.89008,-75.50932,-82.82939,-87.02697], "fy":[-28.38629,-17.25232,-10.84131,-21.66234]}, - {"t":0.54085, "x":6.48938, "y":2.56973, "heading":2.61264, "vx":-2.31696, "vy":-1.36426, "omega":-0.77045, "ax":-4.95432, "ay":0.49838, "alpha":1.42781, "fx":[-81.50723,-78.04841,-83.56062,-87.22794], "fy":[4.16857,5.43806,15.49166,8.13297]}, - {"t":0.57466, "x":6.40822, "y":2.5239, "heading":2.58659, "vx":-2.48443, "vy":-1.34741, "omega":-0.72219, "ax":-2.68651, "ay":4.18126, "alpha":0.66027, "fx":[-45.93814,-39.93027,-46.21696,-47.04596], "fy":[68.56673,70.6982,70.36862,69.1648]}, - {"t":0.60846, "x":6.32271, "y":2.48074, "heading":2.56218, "vx":-2.57525, "vy":-1.20607, "omega":-0.69987, "ax":1.17073, "ay":4.84187, "alpha":-0.92602, "fx":[18.65733,15.90692,20.47982,23.01803], "fy":[83.06283,81.89986,77.25656,80.62754]}, - {"t":0.64226, "x":6.23633, "y":2.44274, "heading":2.53852, "vx":-2.53567, "vy":-1.0424, "omega":-0.73117, "ax":2.4494, "ay":4.34582, "alpha":-1.68506, "fx":[39.10658,34.81887,42.57883,46.81687], "fy":[77.83305,72.1648,66.98219,72.79107]}, - {"t":0.67606, "x":6.15201, "y":2.40998, "heading":2.51381, "vx":-2.45287, "vy":-0.8955, "omega":-0.78813, "ax":2.9509, "ay":4.02637, "alpha":-2.13084, "fx":[47.13867,41.59612,51.53717,56.48852], "fy":[73.84754,68.08007,59.54436,66.99865]}, - {"t":0.70987, "x":6.07078, "y":2.38201, "heading":2.48717, "vx":-2.35312, "vy":-0.75939, "omega":-0.86016, "ax":3.20534, "ay":3.82936, "alpha":-2.20636, "fx":[51.20572,46.03191,55.41268,61.07536], "fy":[71.35118,64.00166,56.25534,63.72644]}, - {"t":0.74367, "x":5.99307, "y":2.35853, "heading":2.45809, "vx":-2.24477, "vy":-0.62995, "omega":-0.93475, "ax":3.35712, "ay":3.69861, "alpha":-2.17636, "fx":[54.23619,48.20118,57.96409,63.4451], "fy":[68.7281,62.67294,54.06033,61.1547]}, - {"t":0.77747, "x":5.91911, "y":2.33935, "heading":2.42649, "vx":-2.13129, "vy":-0.50492, "omega":-1.00831, "ax":3.45745, "ay":3.60612, "alpha":-1.87013, "fx":[55.4326,51.8427,59.82405,63.43668], "fy":[67.02911,60.5119,53.05907,59.84883]}, - {"t":0.81128, "x":5.84904, "y":2.32434, "heading":2.39241, "vx":-2.01442, "vy":-0.38303, "omega":-1.07153, "ax":3.52845, "ay":3.53749, "alpha":-1.60359, "fx":[57.87466,53.12172,59.92604,64.34813], "fy":[64.23867,60.0454,53.36785,58.22144]}, - {"t":0.84508, "x":5.78296, "y":2.31342, "heading":2.35619, "vx":-1.89514, "vy":-0.26345, "omega":-1.12574, "ax":3.58125, "ay":3.48467, "alpha":-1.06184, "fx":[59.01576,56.35454,60.12888,63.29154], "fy":[61.94529,58.59546,54.25453,57.55598]}, - {"t":0.87888, "x":5.72094, "y":2.3065, "heading":2.31813, "vx":-1.77409, "vy":-0.14565, "omega":-1.16163, "ax":3.62198, "ay":3.44282, "alpha":-0.48649, "fx":[60.24144,58.6079,60.56703,62.09012], "fy":[58.91143,57.92919,55.70959,57.01051]}, - {"t":0.91269, "x":5.66304, "y":2.30354, "heading":2.27887, "vx":-1.65165, "vy":-0.02927, "omega":-1.17807, "ax":3.65438, "ay":3.40881, "alpha":0.31526, "fx":[60.8908,62.28159,60.71347,59.78135], "fy":[55.94055,56.46944,57.75934,57.1238]}, - {"t":0.94649, "x":5.6093, "y":2.3045, "heading":2.23904, "vx":-1.52812, "vy":0.08595, "omega":-1.16742, "ax":3.68075, "ay":3.38067, "alpha":1.17542, "fx":[61.61789,65.34119,61.17508,57.29088], "fy":[52.25117,55.35607,60.30436,57.50489]}, - {"t":0.98029, "x":5.55975, "y":2.30934, "heading":2.19958, "vx":-1.4037, "vy":0.20023, "omega":-1.12768, "ax":3.70261, "ay":3.35699, "alpha":2.27666, "fx":[61.73257,69.70832,61.68898,53.75336], "fy":[48.38353,53.45742,63.37102,58.62549]}, - {"t":1.0141, "x":5.51441, "y":2.31803, "heading":2.16146, "vx":-1.27854, "vy":0.31371, "omega":-1.05073, "ax":3.72103, "ay":3.3368, "alpha":3.47301, "fx":[61.89383,73.77802,62.54147,49.89801], "fy":[43.79237,51.68001,66.88229,60.13694]}, - {"t":1.0479, "x":5.47332, "y":2.33054, "heading":2.12594, "vx":-1.15276, "vy":0.4265, "omega":-0.93333, "ax":3.73677, "ay":3.31938, "alpha":4.95106, "fx":[61.37755,79.1231,63.67359,44.9862], "fy":[38.7307,49.05786,70.93353,62.60799]}, - {"t":1.0817, "x":5.43649, "y":2.34685, "heading":2.0944, "vx":-1.02644, "vy":0.53871, "omega":-0.76597, "ax":0.80029, "ay":0.59397, "alpha":5.56738, "fx":[8.39226,32.15831,18.12705,-5.31577], "fy":[-8.52629,5.00298,28.1087,15.01933]}, - {"t":1.11723, "x":5.40053, "y":2.36636, "heading":2.06719, "vx":-0.99801, "vy":0.55981, "omega":-0.5682, "ax":0.89144, "ay":0.44944, "alpha":4.28441, "fx":[10.80302,28.88345,18.99099,0.76187], "fy":[-6.85083,3.34978,21.65032,11.81871]}, - {"t":1.15275, "x":5.36564, "y":2.38653, "heading":2.047, "vx":-0.96635, "vy":0.57577, "omega":-0.416, "ax":0.95278, "ay":0.2983, "alpha":3.35436, "fx":[12.37535,27.07135,19.26088,4.82227], "fy":[-5.9739,1.49934,15.84462,8.52003]}, - {"t":1.18827, "x":5.33192, "y":2.40717, "heading":2.03222, "vx":-0.9325, "vy":0.58637, "omega":-0.29684, "ax":0.9865, "ay":0.15419, "alpha":2.58512, "fx":[13.68982,24.7891,19.23685,8.06247], "fy":[-5.97237,-0.2801,11.07946,5.45421]}, - {"t":1.2238, "x":5.29941, "y":2.4281, "heading":2.02168, "vx":-0.89746, "vy":0.59185, "omega":-0.20501, "ax":0.99827, "ay":0.02529, "alpha":2.03513, "fx":[14.31247,23.42373,18.83229,9.99426], "fy":[-6.14305,-1.83101,6.96387,2.69641]}, - {"t":1.25932, "x":5.26816, "y":2.44914, "heading":2.0144, "vx":-0.862, "vy":0.59275, "omega":-0.13272, "ax":0.99506, "ay":-0.08527, "alpha":1.56991, "fx":[14.81282,21.61257,18.37798,11.54502], "fy":[-6.55571,-3.29111,3.74378,0.41751]}, - {"t":1.29484, "x":5.23817, "y":2.47014, "heading":2.00968, "vx":-0.82665, "vy":0.58972, "omega":-0.07695, "ax":0.98283, "ay":-0.17792, "alpha":1.2437, "fx":[14.89047,20.55088,17.75475,12.33741], "fy":[-6.94327,-4.37507,1.00465,-1.54976]}, - {"t":1.33036, "x":5.20942, "y":2.49098, "heading":2.00695, "vx":-0.79174, "vy":0.5834, "omega":-0.03277, "ax":0.96587, "ay":-0.25476, "alpha":0.96171, "fx":[14.98598,19.1648,17.22235,13.02946], "fy":[-7.3656,-5.45348,-1.07565,-3.09224]}, - {"t":1.36589, "x":5.18191, "y":2.51154, "heading":2.00578, "vx":-0.75743, "vy":0.57435, "omega":0.00139, "ax":0.9469, "ay":-0.3184, "alpha":0.76754, "fx":[14.83918,18.38377,16.62708,13.28725], "fy":[-7.74416,-6.17747,-2.87335,-4.43509]}, - {"t":1.40141, "x":5.1556, "y":2.53174, "heading":2.00583, "vx":-0.72379, "vy":0.56304, "omega":0.02866, "ax":0.92753, "ay":-0.37127, "alpha":0.595, "fx":[14.7685,17.35139,16.15741,13.56836], "fy":[-8.10319,-6.97049,-4.2173,-5.46462]}, - {"t":1.43693, "x":5.13047, "y":2.55151, "heading":2.00685, "vx":-0.69084, "vy":0.54985, "omega":0.04979, "ax":0.90866, "ay":-0.41548, "alpha":0.47895, "fx":[14.54867,16.79494,15.65935,13.58453], "fy":[-8.43488,-7.46118,-5.41814,-6.3895]}, - {"t":1.47246, "x":5.10651, "y":2.57078, "heading":2.00862, "vx":-0.65856, "vy":0.53509, "omega":0.06681, "ax":0.89075, "ay":-0.45274, "alpha":0.37169, "fx":[14.41908,16.02585,15.27914,13.66938], "fy":[-8.73204,-8.06036,-6.30574,-7.08958]}, - {"t":1.50798, "x":5.08367, "y":2.5895, "heading":2.01099, "vx":-0.62692, "vy":0.51901, "omega":0.08001, "ax":0.874, "ay":-0.48439, "alpha":0.30193, "fx":[14.18834,15.63078,14.87766,13.58005], "fy":[-9.01711,-8.40379,-7.1327,-7.74453]}, - {"t":1.5435, "x":5.06196, "y":2.60763, "heading":2.01384, "vx":-0.59587, "vy":0.5018, "omega":0.09073, "ax":0.85847, "ay":-0.5115, "alpha":0.23363, "fx":[14.0451,15.04822,14.57647,13.57165], "fy":[-9.26252,-8.87009,-7.7379,-8.23521]}, - {"t":1.57902, "x":5.04133, "y":2.62514, "heading":2.01706, "vx":-0.56538, "vy":0.48363, "omega":0.09903, "ax":0.84414, "ay":-0.5349, "alpha":0.19119, "fx":[13.8275,14.76324,14.25316,13.4417], "fy":[-9.50624,-9.11822,-8.32753,-8.71439]}, - {"t":1.61455, "x":5.02178, "y":2.64198, "heading":2.02058, "vx":-0.53539, "vy":0.46463, "omega":0.10583, "ax":0.83093, "ay":-0.55527, "alpha":0.14604, "fx":[13.68935,14.31053,14.01377,13.39143], "fy":[-9.70755,-9.49068,-8.75588,-9.0699]}, - {"t":1.65007, "x":5.00328, "y":2.65813, "heading":2.02434, "vx":-0.50587, "vy":0.4449, "omega":0.11101, "ax":0.81877, "ay":-0.5731, "alpha":0.11938, "fx":[13.4933,14.09822,13.74968,13.2531], "fy":[-9.91497,-9.67385,-9.19222,-9.43235]}, - {"t":1.68559, "x":4.98583, "y":2.67358, "heading":2.02828, "vx":-0.47679, "vy":0.42455, "omega":0.11525, "ax":0.80757, "ay":-0.58883, "alpha":0.08762, "fx":[13.36759,13.73554,13.5566,13.18773], "fy":[-10.07725,-9.97718,-9.50899,-9.69886]}, - {"t":1.72112, "x":4.9694, "y":2.68828, "heading":2.03237, "vx":-0.4481, "vy":0.40363, "omega":0.11837, "ax":0.79724, "ay":-0.60279, "alpha":0.06948, "fx":[13.19527,13.56904,13.33635,13.05807], "fy":[-10.25193,-10.11409,-9.84497,-9.98194]}, - {"t":1.75664, "x":4.95399, "y":2.70224, "heading":2.03658, "vx":-0.41978, "vy":0.38222, "omega":0.12083, "ax":0.78771, "ay":-0.61525, "alpha":0.04483, "fx":[13.08444,13.26838,13.17729,12.99256], "fy":[-10.378,-10.36432,-10.09215,-10.18889]}, - {"t":1.79216, "x":4.93957, "y":2.71543, "heading":2.04087, "vx":-0.3918, "vy":0.36036, "omega":0.12243, "ax":0.77888, "ay":-0.62642, "alpha":0.0303, "fx":[12.93558,13.1281,12.98893,12.88163], "fy":[-10.52172,-10.46678,-10.3628,-10.41696]}, - {"t":1.82769, "x":4.92615, "y":2.72784, "heading":2.04522, "vx":-0.36413, "vy":0.33811, "omega":0.1235, "ax":0.7707, "ay":-0.63649, "alpha":0.00821, "fx":[12.84036,12.86824,12.85442,12.82583], "fy":[-10.61203,-10.67447,-10.5693,-10.58386]}, - {"t":1.86321, "x":4.9137, "y":2.73945, "heading":2.04961, "vx":-0.33675, "vy":0.3155, "omega":0.12379, "ax":0.76311, "ay":-0.6456, "alpha":-0.00654, "fx":[12.7142,12.73827,12.68857,12.74136], "fy":[-10.72422,-10.7498,-10.79991,-10.77362]}, - {"t":1.89873, "x":4.90222, "y":2.75025, "heading":2.054, "vx":-0.30965, "vy":0.29256, "omega":0.12356, "ax":0.75604, "ay":-0.65389, "alpha":-0.02992, "fx":[12.635,12.50142,12.57094,12.70383], "fy":[-10.77629,-10.92174,-10.98802,-10.9142]}, - {"t":1.93425, "x":4.8917, "y":2.76023, "heading":2.05839, "vx":-0.28279, "vy":0.26934, "omega":0.1225, "ax":0.74945, "ay":-0.66146, "alpha":-0.04858, "fx":[12.53127,12.36666,12.41973,12.65424], "fy":[-10.85315,-10.97409,-11.19952,-11.07794]}, - {"t":1.96978, "x":4.88212, "y":2.76938, "heading":2.06275, "vx":-0.25617, "vy":0.24584, "omega":0.12077, "ax":0.7433, "ay":-0.66839, "alpha":-0.07736, "fx":[12.46907,12.13531,12.31211,12.64514], "fy":[-10.86031,-11.11417,-11.38954,-11.20274]}, - {"t":2.0053, "x":4.87349, "y":2.77769, "heading":2.06704, "vx":-0.22976, "vy":0.2221, "omega":0.11803, "ax":0.73754, "ay":-0.67476, "alpha":-0.10437, "fx":[12.38874,11.97854,12.16816,12.6423], "fy":[-10.89327,-11.14485,-11.60272,-11.35058]}, - {"t":2.04082, "x":4.8658, "y":2.78515, "heading":2.07123, "vx":-0.20356, "vy":0.19813, "omega":0.11432, "ax":0.73214, "ay":-0.68063, "alpha":-0.14379, "fx":[12.34587,11.73229,12.06353,12.67618], "fy":[-10.84278,-11.25384,-11.81724,-11.46905]}, - {"t":2.07635, "x":4.85903, "y":2.79176, "heading":2.07529, "vx":-0.17756, "vy":0.17395, "omega":0.10921, "ax":0.72707, "ay":-0.68606, "alpha":-0.18531, "fx":[12.29194,11.53064,11.91878,12.7386], "fy":[-10.81594,-11.26176,-12.05672,-11.61049]}, - {"t":2.11187, "x":4.85318, "y":2.79751, "heading":2.07917, "vx":-0.15173, "vy":0.14958, "omega":0.10263, "ax":0.72231, "ay":-0.69109, "alpha":-0.24296, "fx":[12.27303,11.24223,11.80879,12.83807], "fy":[-10.68556,-11.33745,-12.32502,-11.7325]}, - {"t":2.14739, "x":4.84824, "y":2.80238, "heading":2.08281, "vx":-0.12607, "vy":0.12503, "omega":0.094, "ax":0.71782, "ay":-0.69577, "alpha":-0.30825, "fx":[12.25164,10.96263,11.65307,12.99536], "fy":[-10.57188,-11.31786,-12.62443,-11.87832]}, - {"t":2.18291, "x":4.84422, "y":2.80639, "heading":2.08615, "vx":-0.10057, "vy":0.10031, "omega":0.08305, "ax":0.71358, "ay":-0.70013, "alpha":-0.39572, "fx":[12.26518,10.59213,11.52645,13.19642], "fy":[-10.32465,-11.35343,-12.98875,-12.01632]}, - {"t":2.21844, "x":4.8411, "y":2.80951, "heading":2.0891, "vx":-0.07522, "vy":0.07544, "omega":0.06899, "ax":0.70958, "ay":-0.7042, "alpha":-0.49923, "fx":[12.28779,10.18399,11.34521,13.49623], "fy":[-10.07947,-11.29678,-13.39754,-12.1808]}, - {"t":2.25396, "x":4.83887, "y":2.81174, "heading":2.09155, "vx":-0.05002, "vy":0.05043, "omega":0.05126, "ax":0.70579, "ay":-0.70801, "alpha":-0.63471, "fx":[12.34888,9.67047,11.18499,13.85626], "fy":[-9.65517,-11.27864,-13.922,-12.35281]}, - {"t":2.28948, "x":4.83754, "y":2.81309, "heading":2.09338, "vx":-0.02494, "vy":0.02528, "omega":0.02871, "ax":0.7022, "ay":-0.71158, "alpha":-0.8082, "fx":[12.43668,9.00696,10.95503,14.42258], "fy":[-9.20641,-11.14371,-14.54215,-12.55455]}, - {"t":2.32501, "x":4.8371, "y":2.81354, "heading":2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.1, "y":2.97, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.16718, "ay":-2.75755, "alpha":-10.84504, "fx":[-98.27566,-89.93808,-41.28885,-48.35685], "fy":[-16.07898,-68.54797,-79.74404,-19.49694]}, + {"t":0.03367, "x":7.09764, "y":2.96844, "heading":3.14159, "vx":-0.14033, "vy":-0.09286, "omega":-0.3652, "ax":-4.17587, "ay":-2.74697, "alpha":-8.70215, "fx":[-92.69701,-86.07397,-47.19378,-52.47454], "fy":[-21.35146,-64.16546,-72.6326,-25.01307]}, + {"t":0.06735, "x":7.09054, "y":2.96375, "heading":3.12929, "vx":-0.28095, "vy":-0.18536, "omega":-0.65824, "ax":-4.18424, "ay":-2.73401, "alpha":-6.7495, "fx":[-87.71279,-82.84243,-52.34198,-56.10015], "fy":[-26.00416,-59.76748,-66.06422,-30.46244]}, + {"t":0.10102, "x":7.07871, "y":2.95596, "heading":3.10713, "vx":-0.42185, "vy":-0.27743, "omega":-0.88553, "ax":-4.19375, "ay":-2.71917, "alpha":-5.17834, "fx":[-83.1654,-80.31068,-56.86971,-59.28558], "fy":[-30.07254,-55.98518,-61.41387,-33.83762]}, + {"t":0.1347, "x":7.06213, "y":2.94508, "heading":3.07731, "vx":-0.56307, "vy":-0.36899, "omega":-1.0599, "ax":-4.20465, "ay":-2.70203, "alpha":-3.61768, "fx":[-79.43186,-77.73224,-61.11256,-62.08131], "fy":[-33.46545,-52.30342,-55.91562,-38.48167]}, + {"t":0.16837, "x":7.04078, "y":2.93112, "heading":3.04162, "vx":-0.70466, "vy":-0.45998, "omega":-1.18173, "ax":-4.21726, "ay":-2.682, "alpha":-2.41026, "fx":[-76.02007,-75.69324,-64.52593,-64.95935], "fy":[-37.21909,-49.23516,-52.40357,-39.97297]}, + {"t":0.20205, "x":7.01466, "y":2.91411, "heading":3.00182, "vx":-0.84668, "vy":-0.5503, "omega":-1.26289, "ax":-4.23202, "ay":-2.65829, "alpha":-1.27145, "fx":[-73.49471,-73.30193,-67.67377,-67.7123], "fy":[-40.18247,-46.9989,-48.24098,-41.82709]}, + {"t":0.23572, "x":6.98375, "y":2.89407, "heading":2.9593, "vx":-0.98919, "vy":-0.63982, "omega":-1.30571, "ax":-4.24952, "ay":-2.62978, "alpha":-0.29527, "fx":[-71.35096,-71.81355,-70.08346,-70.10154], "fy":[-42.96206,-44.17595,-44.79351,-43.41716]}, + {"t":0.2694, "x":6.94803, "y":2.87103, "heading":2.91533, "vx":-1.13229, "vy":-0.72837, "omega":-1.31565, "ax":-4.2706, "ay":-2.59485, "alpha":0.53618, "fx":[-70.04042,-69.69603,-72.39237,-72.62669], "fy":[-44.91083,-42.80169,-41.33329,-43.97379]}, + {"t":0.30307, "x":6.90748, "y":2.84503, "heading":2.87102, "vx":-1.2761, "vy":-0.81575, "omega":-1.29759, "ax":-4.29647, "ay":-2.5511, "alpha":1.21368, "fx":[-68.97266,-69.14142,-73.81613,-74.55002], "fy":[-46.9971,-40.49047,-38.17208,-44.44282]}, + {"t":0.33674, "x":6.86207, "y":2.81612, "heading":2.82733, "vx":-1.42078, "vy":-0.90166, "omega":-1.25672, "ax":-4.32894, "ay":-2.49469, "alpha":1.89729, "fx":[-68.74078,-67.17622,-75.71323,-77.01544], "fy":[-48.22487,-39.51299,-34.58051,-44.02272]}, + {"t":0.37042, "x":6.81178, "y":2.78434, "heading":2.78501, "vx":-1.56655, "vy":-0.98567, "omega":-1.19283, "ax":-4.37084, "ay":-2.41929, "alpha":2.20844, "fx":[-68.7676,-67.72627,-76.34711,-78.59852], "fy":[-48.63373,-37.32683,-32.1882,-43.16496]}, + {"t":0.40409, "x":6.75654, "y":2.74978, "heading":2.74484, "vx":-1.71374, "vy":-1.06713, "omega":-1.11847, "ax":-4.42681, "ay":-2.3135, "alpha":2.66941, "fx":[-69.73846,-66.38913,-78.06155,-80.9823], "fy":[-48.16457,-35.98593,-28.66423,-41.44529]}, + {"t":0.43777, "x":6.69632, "y":2.71253, "heading":2.70718, "vx":-1.86281, "vy":-1.14504, "omega":-1.02858, "ax":-4.50488, "ay":-2.15474, "alpha":2.4707, "fx":[-71.43628,-69.31884,-79.73902,-79.88245], "fy":[-45.93846,-32.81263,-25.65834,-39.26422]}, + {"t":0.47144, "x":6.63104, "y":2.67275, "heading":2.67254, "vx":-2.01451, "vy":-1.2176, "omega":-0.94538, "ax":-4.6194, "ay":-1.89167, "alpha":2.70882, "fx":[-73.69537,-68.98458,-80.55948,-84.77298], "fy":[-41.1764,-29.93547,-21.2142,-33.80716]}, + {"t":0.50512, "x":6.56059, "y":2.63068, "heading":2.6407, "vx":-2.17006, "vy":-1.2813, "omega":-0.85416, "ax":-4.79402, "ay":-1.3779, "alpha":2.1671, "fx":[-76.4109,-75.48929,-81.60752,-86.14813], "fy":[-31.85054,-20.60849,-14.25605,-25.16053]}, + {"t":0.53879, "x":6.48479, "y":2.58675, "heading":2.61194, "vx":-2.3315, "vy":-1.3277, "omega":-0.78118, "ax":-4.97918, "ay":-0.08783, "alpha":1.51042, "fx":[-81.74187,-77.89611,-84.30442,-88.05963], "fy":[-5.43038,-5.74004,6.44753,-1.13363]}, + {"t":0.57247, "x":6.40346, "y":2.54199, "heading":2.58564, "vx":-2.49917, "vy":-1.33066, "omega":-0.73032, "ax":-3.33051, "ay":3.68232, "alpha":0.92346, "fx":[-56.97033,-48.98663,-57.45046,-58.6644], "fy":[59.7636,62.33928,62.66851,60.75888]}, + {"t":0.60614, "x":6.31741, "y":2.49927, "heading":2.56104, "vx":-2.61132, "vy":-1.20666, "omega":-0.69922, "ax":1.29192, "ay":4.80736, "alpha":-0.81805, "fx":[20.78753,18.38103,22.34539,24.6291], "fy":[82.69867,79.43362,77.97011,80.44336]}, + {"t":0.63981, "x":6.23021, "y":2.46136, "heading":2.5375, "vx":-2.56782, "vy":-1.04477, "omega":-0.72677, "ax":2.69037, "ay":4.19941, "alpha":-1.6405, "fx":[43.0827,39.40607,46.29073,50.6091], "fy":[75.50085,69.54154,64.48772,70.47818]}, + {"t":0.67349, "x":6.14526, "y":2.42856, "heading":2.51302, "vx":-2.47722, "vy":-0.90336, "omega":-0.78201, "ax":3.17723, "ay":3.84948, "alpha":-2.18014, "fx":[50.9972,45.28004,55.21521,60.3589], "fy":[70.93436,65.68498,56.01392,64.04243]}, + {"t":0.70716, "x":6.06364, "y":2.40032, "heading":2.48669, "vx":-2.37023, "vy":-0.77373, "omega":-0.85543, "ax":3.41192, "ay":3.64599, "alpha":-2.30381, "fx":[54.61392,49.32359,58.70961,64.85306], "fy":[68.74217,60.78776,52.79874,60.7792]}, + {"t":0.74084, "x":5.98576, "y":2.37633, "heading":2.45788, "vx":-2.25534, "vy":-0.65095, "omega":-0.93301, "ax":3.54812, "ay":3.51543, "alpha":-2.17471, "fx":[57.71875,50.71629,60.93782,67.20912], "fy":[64.88586,61.35192,51.15038,57.01422]}, + {"t":0.77451, "x":5.91183, "y":2.3564, "heading":2.42646, "vx":-2.13586, "vy":-0.53257, "omega":-1.00624, "ax":3.63654, "ay":3.42516, "alpha":-1.91508, "fx":[57.9093,55.1592,63.30997,66.09923], "fy":[64.60568,57.32764,49.45509,56.99479]}, + {"t":0.80819, "x":5.84197, "y":2.34041, "heading":2.39258, "vx":-2.0134, "vy":-0.41723, "omega":-1.07073, "ax":3.69843, "ay":3.35917, "alpha":-1.69828, "fx":[60.75717,55.61139,62.71965,67.51599], "fy":[61.5045,57.27972,49.99411,55.20495]}, + {"t":0.84186, "x":5.77626, "y":2.32827, "heading":2.35652, "vx":-1.88885, "vy":-0.30412, "omega":-1.12792, "ax":3.74408, "ay":3.30893, "alpha":-1.12132, "fx":[61.68359,59.12905,62.71331,66.12189], "fy":[59.41604,55.59423,50.96173,54.6612]}, + {"t":0.87554, "x":5.71478, "y":2.3199, "heading":2.31854, "vx":-1.76277, "vy":-0.19269, "omega":-1.16568, "ax":3.77912, "ay":3.26941, "alpha":-0.54806, "fx":[62.89471,60.98192,63.159,64.94871], "fy":[56.14732,55.18814,52.59076,54.072]}, + {"t":0.90921, "x":5.65756, "y":2.31527, "heading":2.27929, "vx":-1.63551, "vy":-0.08259, "omega":-1.18413, "ax":3.80683, "ay":3.23756, "alpha":0.2986, "fx":[63.36731,64.92672,63.18574,62.35239], "fy":[53.25406,53.5755,54.76513,54.27932]}, + {"t":0.94288, "x":5.60465, "y":2.31432, "heading":2.23941, "vx":-1.50732, "vy":0.02643, "omega":-1.17408, "ax":3.82932, "ay":3.2113, "alpha":1.16232, "fx":[64.10367,67.6953,63.64939,59.88323], "fy":[49.34979,52.65925,57.48133,54.63293]}, + {"t":0.97656, "x":5.55606, "y":2.31703, "heading":2.19988, "vx":-1.37837, "vy":0.13457, "omega":-1.13494, "ax":3.84791, "ay":3.18932, "alpha":2.28851, "fx":[64.14546,72.01338,64.2126,56.19956], "fy":[45.46605,50.5978,60.72666,55.86698]}, + {"t":1.01023, "x":5.51182, "y":2.32337, "heading":2.16166, "vx":-1.2488, "vy":0.24197, "omega":-1.05787, "ax":3.86354, "ay":3.17062, "alpha":3.51276, "fx":[64.23244,76.15074,64.95698,52.27346], "fy":[40.72745,48.94649,64.37017,57.3669]}, + {"t":1.04391, "x":5.47196, "y":2.33332, "heading":2.12604, "vx":-1.11869, "vy":0.34874, "omega":-0.93958, "ax":3.87685, "ay":3.15456, "alpha":5.02433, "fx":[63.65567,81.45493,66.11201,47.27847], "fy":[35.53869,46.18552,68.64623,59.96982]}, + {"t":1.07758, "x":5.43649, "y":2.34685, "heading":2.0944, "vx":-0.98814, "vy":0.45496, "omega":-0.77039, "ax":0.82782, "ay":0.55479, "alpha":5.62074, "fx":[8.72531,32.97602,18.59048,-5.09406], "fy":[-9.232,4.33584,27.53191,14.35627]}, + {"t":1.11253, "x":5.40246, "y":2.36309, "heading":2.06747, "vx":-0.95921, "vy":0.47435, "omega":-0.57397, "ax":0.91363, "ay":0.40239, "alpha":4.34598, "fx":[11.13076,29.40711,19.40493,0.97647], "fy":[-7.89741,2.52209,21.12798,11.07818]}, + {"t":1.14747, "x":5.3695, "y":2.37991, "heading":2.04742, "vx":-0.92729, "vy":0.48841, "omega":-0.4221, "ax":0.9662, "ay":0.25159, "alpha":3.44287, "fx":[12.47295,27.70338,19.5365,4.71142], "fy":[-6.97502,0.67014,15.29286,7.78744]}, + {"t":1.18242, "x":5.33769, "y":2.39713, "heading":2.03266, "vx":-0.89352, "vy":0.49721, "omega":-0.30178, "ax":0.99194, "ay":0.11456, "alpha":2.66948, "fx":[13.70819,25.13118,19.40046,7.90082], "fy":[-6.93442,-1.03631,10.72984,4.8794]}, + {"t":1.21737, "x":5.30707, "y":2.41458, "heading":2.02212, "vx":-0.85886, "vy":0.50121, "omega":-0.2085, "ax":0.99865, "ay":-0.00364, "alpha":2.12842, "fx":[14.19524,23.82044,18.90522,9.66704], "fy":[-6.88505,-2.3858,6.74206,2.28633]}, + {"t":1.25231, "x":5.27766, "y":2.43209, "heading":2.01483, "vx":-0.82396, "vy":0.50108, "omega":-0.13412, "ax":0.99348, "ay":-0.10264, "alpha":1.65231, "fx":[14.70867,21.84098,18.43016,11.26372], "fy":[-7.12247,-3.68841,3.74471,0.22217]}, + {"t":1.28726, "x":5.24948, "y":2.44954, "heading":2.01015, "vx":-0.78924, "vy":0.49749, "omega":-0.07638, "ax":0.98169, "ay":-0.1845, "alpha":1.32651, "fx":[14.76401,20.86329,17.79871,12.03124], "fy":[-7.29258,-4.55877,1.1345,-1.5851]}, + {"t":1.3222, "x":5.22249, "y":2.46681, "heading":2.00748, "vx":-0.75493, "vy":0.49105, "omega":-0.03002, "ax":0.96667, "ay":-0.25199, "alpha":1.03208, "fx":[14.92936,19.39802,17.30574,12.82239], "fy":[-7.5484,-5.5073,-0.7853,-2.9614]}, + {"t":1.35715, "x":5.1967, "y":2.48382, "heading":2.00643, "vx":-0.72115, "vy":0.48224, "omega":0.00605, "ax":0.95045, "ay":-0.30783, "alpha":0.83493, "fx":[14.81133,18.70858,16.73713,13.11702], "fy":[-7.76629,-6.06501,-2.49921,-4.19514]}, + {"t":1.39209, "x":5.17208, "y":2.50048, "heading":2.00664, "vx":-0.68794, "vy":0.47148, "omega":0.03522, "ax":0.93419, "ay":-0.35434, "alpha":0.65037, "fx":[14.82342,17.63675,16.32471,13.50489], "fy":[-7.99755,-6.77026,-3.74402,-5.11465]}, + {"t":1.42704, "x":5.14861, "y":2.51674, "heading":2.00787, "vx":-0.65529, "vy":0.4591, "omega":0.05795, "ax":0.9185, "ay":-0.39338, "alpha":0.53041, "fx":[14.64596,17.16301,15.85956,13.5751], "fy":[-8.21836,-7.14174,-4.89798,-5.972]}, + {"t":1.46199, "x":5.12627, "y":2.53254, "heading":2.0099, "vx":-0.6232, "vy":0.44535, "omega":0.07649, "ax":0.90368, "ay":-0.42646, "alpha":0.41225, "fx":[14.59358,16.36969,15.53577,13.75652], "fy":[-8.42086,-7.6854,-5.72755,-6.6015]}, + {"t":1.49693, "x":5.10505, "y":2.54785, "heading":2.01257, "vx":-0.59162, "vy":0.43045, "omega":0.09089, "ax":0.88985, "ay":-0.45471, "alpha":0.33854, "fx":[14.40432,16.04439,15.16355,13.72135], "fy":[-8.62943,-7.943,-6.5311,-7.21588]}, + {"t":1.53188, "x":5.08492, "y":2.56261, "heading":2.01574, "vx":-0.56052, "vy":0.41456, "omega":0.10272, "ax":0.87704, "ay":-0.47906, "alpha":0.26045, "fx":[14.32806,15.44265,14.91234,13.79601], "fy":[-8.80303,-8.37558,-7.10343,-7.66066]}, + {"t":1.56682, "x":5.06586, "y":2.57681, "heading":2.01933, "vx":-0.52987, "vy":0.39782, "omega":0.11183, "ax":0.8652, "ay":-0.5002, "alpha":0.21414, "fx":[14.14689,15.21486,14.61237,13.7156], "fy":[-8.99251,-8.55937,-7.68433,-8.11617]}, + {"t":1.60177, "x":5.04788, "y":2.5904, "heading":2.02324, "vx":-0.49964, "vy":0.38034, "omega":0.11931, "ax":0.85428, "ay":-0.5187, "alpha":0.15993, "fx":[14.06584,14.74362,14.41557,13.73664], "fy":[-9.13665,-8.91165,-8.09595,-8.44139]}, + {"t":1.63671, "x":5.03094, "y":2.60338, "heading":2.02741, "vx":-0.46978, "vy":0.36221, "omega":0.1249, "ax":0.84421, "ay":-0.53499, "alpha":0.12924, "fx":[13.90098,14.57672,14.16898,13.64354], "fy":[-9.30323,-9.04465,-8.53331,-8.79079]}, + {"t":1.67166, "x":5.01504, "y":2.61571, "heading":2.03178, "vx":-0.44028, "vy":0.34352, "omega":0.12941, "ax":0.83492, "ay":-0.54943, "alpha":0.08858, "fx":[13.82423,14.19381,14.01149,13.64103], "fy":[-9.41661,-9.33566,-8.84526,-9.03744]}, + {"t":1.70661, "x":5.00016, "y":2.62738, "heading":2.0363, "vx":-0.4111, "vy":0.32432, "omega":0.13251, "ax":0.82633, "ay":-0.56231, "alpha":0.06574, "fx":[13.67892,14.06119,13.80328,13.55462], "fy":[-9.55732,-9.43183,-9.18998,-9.31449]}, + {"t":1.74155, "x":4.9863, "y":2.63837, "heading":2.04093, "vx":-0.38223, "vy":0.30467, "omega":0.13481, "ax":0.81838, "ay":-0.57385, "alpha":0.03156, "fx":[13.61103,13.73633,13.67344,13.54738], "fy":[-9.63715,-9.67408,-9.443,-9.50913]}, + {"t":1.7765, "x":4.97344, "y":2.64866, "heading":2.04564, "vx":-0.35363, "vy":0.28461, "omega":0.13591, "ax":0.81102, "ay":-0.58425, "alpha":0.01087, "fx":[13.48659,13.61728,13.49183,13.48132], "fy":[-9.74847,-9.7415,-9.7304,-9.73648]}, + {"t":1.81144, "x":4.96158, "y":2.65825, "heading":2.05039, "vx":-0.32529, "vy":0.2642, "omega":0.13629, "ax":0.80418, "ay":-0.59367, "alpha":-0.0224, "fx":[13.43035,13.32651,13.38045,13.48358], "fy":[-9.78981,-9.94249,-9.95476,-9.89746]}, + {"t":1.84639, "x":4.9507, "y":2.66712, "heading":2.05515, "vx":-0.29719, "vy":0.24345, "omega":0.13551, "ax":0.79781, "ay":-0.60222, "alpha":-0.0459, "fx":[13.32789,13.20226,13.21584,13.45052], "fy":[-9.86574,-9.98604,-10.21218,-10.09107]}, + {"t":1.88133, "x":4.9408, "y":2.67526, "heading":2.05989, "vx":-0.26931, "vy":0.22241, "omega":0.1339, "ax":0.79188, "ay":-0.61003, "alpha":-0.08356, "fx":[13.28581,12.92386,13.11503,13.47621], "fy":[-9.8596,-10.15031,-10.43381,-10.2319]}, + {"t":1.91628, "x":4.93188, "y":2.68266, "heading":2.06457, "vx":-0.24163, "vy":0.20109, "omega":0.13098, "ax":0.78634, "ay":-0.61718, "alpha":-0.11529, "fx":[13.20724,12.7742,12.95864,13.49138], "fy":[-9.89,-10.17144,-10.68656,-10.4044]}, + {"t":1.95122, "x":4.92391, "y":2.68931, "heading":2.06914, "vx":-0.21415, "vy":0.17952, "omega":0.12695, "ax":0.78115, "ay":-0.62375, "alpha":-0.16358, "fx":[13.18273,12.48452,12.86068,13.55787], "fy":[-9.82124,-10.30005,-10.93262,-10.53665]}, + {"t":1.98617, "x":4.9169, "y":2.69521, "heading":2.07358, "vx":-0.18686, "vy":0.15772, "omega":0.12124, "ax":0.77629, "ay":-0.62981, "alpha":-0.21056, "fx":[13.13188,12.28359,12.70329,13.64307], "fy":[-9.78882,-10.29766,-11.20873,-10.69929]}, + {"t":2.02112, "x":4.91085, "y":2.70033, "heading":2.07782, "vx":-0.15973, "vy":0.13571, "omega":0.11388, "ax":0.77173, "ay":-0.63541, "alpha":-0.27795, "fx":[13.13037,11.95275,12.59932,13.77521], "fy":[-9.63228,-10.38829,-11.51285,-10.83454]}, + {"t":2.05606, "x":4.90574, "y":2.70469, "heading":2.0818, "vx":-0.13276, "vy":0.11351, "omega":0.10417, "ax":0.76744, "ay":-0.6406, "alpha":-0.35031, "fx":[13.11429,11.66391,12.42956,13.96381], "fy":[-9.50857,-10.35757,-11.84872,-10.99937]}, + {"t":2.09101, "x":4.90157, "y":2.70826, "heading":2.08544, "vx":-0.10594, "vy":0.09112, "omega":0.09192, "ax":0.7634, "ay":-0.64543, "alpha":-0.44939, "fx":[13.14499,11.24966,12.30787,14.19959], "fy":[-9.22339,-10.40298,-12.25806,-11.15168]}, + {"t":2.12595, "x":4.89833, "y":2.71105, "heading":2.08865, "vx":-0.07926, "vy":0.06857, "omega":0.07622, "ax":0.75959, "ay":-0.64993, "alpha":-0.56263, "fx":[13.17642,10.81751,12.10979,14.54419], "fy":[-8.96221,-10.33406,-12.70568,-11.33407]}, + {"t":2.1609, "x":4.89603, "y":2.71305, "heading":2.09131, "vx":-0.05272, "vy":0.04586, "omega":0.05656, "ax":0.75599, "ay":-0.65413, "alpha":-0.71277, "fx":[13.25519,10.25683,11.95286,14.94286], "fy":[-8.48266,-10.31948,-13.29063,-11.52335]}, + {"t":2.19584, "x":4.89464, "y":2.71426, "heading":2.09329, "vx":-0.0263, "vy":0.023, "omega":0.03165, "ax":0.75258, "ay":-0.65806, "alpha":-0.90568, "fx":[13.35727,9.52115,11.70096,15.60112], "fy":[-8.00186,-10.16152,-13.9707,-11.74419]}, + {"t":2.23079, "x":4.89419, "y":2.71466, "heading":2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/startToG.traj b/src/main/deploy/choreo/startToG.traj index 1fbeea49..dfe06232 100644 --- a/src/main/deploy/choreo/startToG.traj +++ b/src/main/deploy/choreo/startToG.traj @@ -3,7 +3,7 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.1, "y":5.07, "heading":3.14159, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":5.827323, "y":3.7209, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -30,53 +30,53 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,2.19479], + "waypoints":[0.0,2.19], "samples":[ - {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.05388, "ay":-2.18486, "alpha":0.00266, "fx":[-34.11392,-34.42183,-34.20851,-34.20442], "fy":[-36.46662,-36.37065,-36.38324,-36.46197]}, - {"t":0.04988, "x":7.09833, "y":5.07296, "heading":3.14159, "vx":-0.10245, "vy":-0.10898, "omega":0.00013, "ax":-2.05424, "ay":-2.18525, "alpha":0.00798, "fx":[-34.15116,-34.33939,-34.24313,-34.23897], "fy":[-36.4704,-36.38285,-36.38947,-36.46577]}, - {"t":0.09976, "x":7.09067, "y":5.06481, "heading":-3.14159, "vx":-0.20492, "vy":-0.21799, "omega":0.00053, "ax":-2.05409, "ay":-2.18512, "alpha":0.00201, "fx":[-34.20368,-34.19212,-34.28587,-34.28138], "fy":[-36.39378,-36.61402,-36.30178,-36.38973]}, - {"t":0.14964, "x":7.07789, "y":5.05121, "heading":-3.14156, "vx":-0.30738, "vy":-0.32699, "omega":0.00063, "ax":-2.05388, "ay":-2.18487, "alpha":0.00181, "fx":[-34.11136,-34.43271,-34.20419,-34.2003], "fy":[-36.46657,-36.37,-36.38388,-36.46216]}, - {"t":0.19953, "x":7.06, "y":5.03218, "heading":-3.14153, "vx":-0.40983, "vy":-0.43597, "omega":0.00072, "ax":-2.05345, "ay":-2.18435, "alpha":-0.0093, "fx":[-34.19493,-34.17162,-34.27901,-34.27483], "fy":[-36.32182,-36.77924,-36.22922,-36.31821]}, - {"t":0.24941, "x":7.037, "y":5.00772, "heading":-3.14149, "vx":-0.51226, "vy":-0.54493, "omega":0.00026, "ax":-2.05183, "ay":-2.18316, "alpha":0.00048, "fx":[-34.07825,-34.41002,-34.16344,-34.16053], "fy":[-36.43744,-36.33752,-36.35996,-36.43374]}, - {"t":0.29929, "x":7.0089, "y":4.97782, "heading":-3.14148, "vx":-0.61461, "vy":-0.65383, "omega":0.00028, "ax":-1.40624, "ay":-1.49693, "alpha":-0.09064, "fx":[-23.4136,-23.38161,-23.48602,-23.4844], "fy":[-24.46855,-26.48352,-24.39319,-24.46734]}, - {"t":0.34917, "x":6.97649, "y":4.94335, "heading":-3.14147, "vx":-0.68476, "vy":-0.7285, "omega":-0.00424, "ax":-0.00371, "ay":0.00346, "alpha":0.03684, "fx":[-0.19766,0.40537,-0.22747,-0.22743], "fy":[0.04398,0.07219,0.07095,0.04388]}, - {"t":0.39905, "x":6.94233, "y":4.90701, "heading":3.14151, "vx":-0.68494, "vy":-0.72833, "omega":-0.0024, "ax":0.00453, "ay":-0.00426, "alpha":0.03735, "fx":[0.07748,0.07745,0.07345,0.07345], "fy":[-0.25086,0.46282,-0.24483,-0.25086]}, - {"t":0.44893, "x":6.90817, "y":4.87068, "heading":3.14139, "vx":-0.68472, "vy":-0.72854, "omega":-0.00054, "ax":-0.00048, "ay":0.00045, "alpha":0.01871, "fx":[-0.1022,0.2715,-0.10074,-0.10074], "fy":[0.00915,0.00597,0.00596,0.00915]}, - {"t":0.49882, "x":6.87401, "y":4.83434, "heading":3.14136, "vx":-0.68474, "vy":-0.72852, "omega":0.00039, "ax":-0.00161, "ay":0.00151, "alpha":-0.00271, "fx":[-0.02863,-0.02863,-0.02495,-0.02495], "fy":[0.03655,-0.00642,0.03405,0.03654]}, - {"t":0.5487, "x":6.83986, "y":4.798, "heading":3.14138, "vx":-0.68482, "vy":-0.72844, "omega":0.00026, "ax":0.00034, "ay":-0.00032, "alpha":0.00348, "fx":[-0.01818,0.06894,-0.01389,-0.01388], "fy":[-0.00269,-0.00811,-0.00811,-0.00269]}, - {"t":0.59858, "x":6.8057, "y":4.76166, "heading":3.14139, "vx":-0.6848, "vy":-0.72846, "omega":0.00043, "ax":0.00008, "ay":-0.00008, "alpha":-0.00281, "fx":[-0.00103,-0.00103,0.00382,0.00382], "fy":[0.00995,-0.03099,0.00584,0.00995]}, - {"t":0.64846, "x":6.77154, "y":4.72533, "heading":3.14142, "vx":-0.6848, "vy":-0.72846, "omega":0.00029, "ax":0.00004, "ay":-0.00004, "alpha":0.00061, "fx":[-0.00916,0.02165,-0.00477,-0.00477], "fy":[0.00184,-0.00322,-0.00322,0.00184]}, - {"t":0.69834, "x":6.73738, "y":4.68899, "heading":3.14143, "vx":-0.6848, "vy":-0.72846, "omega":0.00032, "ax":0.00005, "ay":-0.00005, "alpha":-0.00161, "fx":[-0.00129,-0.0013,0.00306,0.00306], "fy":[0.00481,-0.01386,0.00091,0.00481]}, - {"t":0.74822, "x":6.70322, "y":4.65265, "heading":3.14145, "vx":-0.68479, "vy":-0.72846, "omega":0.00024, "ax":-0.00002, "ay":0.00002, "alpha":-0.00013, "fx":[-0.00532,0.00738,-0.00172,-0.00172], "fy":[0.00232,-0.00166,-0.00167,0.00231]}, - {"t":0.79811, "x":6.66906, "y":4.61631, "heading":3.14146, "vx":-0.68479, "vy":-0.72846, "omega":0.00024, "ax":0.0, "ay":0.0, "alpha":-0.00095, "fx":[-0.00161,-0.00162,0.00165,0.00166], "fy":[0.00299,-0.00604,-0.00002,0.00299]}, - {"t":0.84799, "x":6.6349, "y":4.57998, "heading":3.14147, "vx":-0.68479, "vy":-0.72846, "omega":0.00019, "ax":0.0, "ay":0.0, "alpha":-0.00025, "fx":[-0.00274,0.0029,-0.00021,-0.00021], "fy":[0.00142,-0.0013,-0.0013,0.00142]}, - {"t":0.89787, "x":6.60074, "y":4.54364, "heading":3.14148, "vx":-0.68479, "vy":-0.72846, "omega":0.00018, "ax":0.0, "ay":0.0, "alpha":-0.00054, "fx":[-0.0011,-0.00111,0.00097,0.00097], "fy":[0.00168,-0.00284,-0.00026,0.00168]}, - {"t":0.94775, "x":6.56659, "y":4.5073, "heading":3.14149, "vx":-0.68479, "vy":-0.72846, "omega":0.00015, "ax":0.0, "ay":0.0, "alpha":-0.00017, "fx":[-0.00127,0.00122,0.00016,0.00016], "fy":[0.00069,-0.00082,-0.00082,0.00069]}, - {"t":0.99763, "x":6.53243, "y":4.47097, "heading":3.1415, "vx":-0.68479, "vy":-0.72846, "omega":0.00014, "ax":0.0, "ay":0.0, "alpha":-0.00026, "fx":[-0.00049,-0.0005,0.00043,0.00043], "fy":[0.00082,-0.00145,-0.00008,0.00082]}, - {"t":1.04751, "x":6.49827, "y":4.43463, "heading":3.1415, "vx":-0.68479, "vy":-0.72846, "omega":0.00013, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[-0.00045,0.00079,-0.00004,-0.00004], "fy":[0.00014,-0.00026,-0.00026,0.00014]}, - {"t":1.0974, "x":6.46411, "y":4.39829, "heading":3.14151, "vx":-0.68479, "vy":-0.72846, "omega":0.00013, "ax":0.0, "ay":0.0, "alpha":-0.00005, "fx":[0.00007,0.00007,-0.00007,-0.00006], "fy":[0.00031,-0.00102,0.00039,0.00031]}, - {"t":1.14728, "x":6.42995, "y":4.36196, "heading":3.14152, "vx":-0.68479, "vy":-0.72846, "omega":0.00012, "ax":0.0, "ay":0.0, "alpha":0.00018, "fx":[-0.00003,0.00123,-0.00055,-0.00055], "fy":[-0.00033,0.00029,0.00029,-0.00033]}, - {"t":1.19716, "x":6.39579, "y":4.32562, "heading":3.14152, "vx":-0.68479, "vy":-0.72846, "omega":0.00013, "ax":0.0, "ay":0.0, "alpha":0.00009, "fx":[0.00059,0.00059,-0.00054,-0.00054], "fy":[0.00012,-0.00145,0.00111,0.00012]}, - {"t":1.24704, "x":6.36163, "y":4.28928, "heading":3.14153, "vx":-0.68479, "vy":-0.72846, "omega":0.00014, "ax":0.0, "ay":0.0, "alpha":0.00044, "fx":[-0.00003,0.00255,-0.00141,-0.00141], "fy":[-0.00072,0.00085,0.00085,-0.00072]}, - {"t":1.29692, "x":6.32747, "y":4.25294, "heading":3.14154, "vx":-0.68479, "vy":-0.72846, "omega":0.00016, "ax":0.0, "ay":0.0, "alpha":0.00012, "fx":[0.0011,0.0011,-0.00097,-0.00097], "fy":[0.00036,-0.00314,0.00216,0.00036]}, - {"t":1.3468, "x":6.29332, "y":4.21661, "heading":3.14154, "vx":-0.68479, "vy":-0.72846, "omega":0.00017, "ax":-0.00002, "ay":0.00002, "alpha":0.00077, "fx":[-0.00081,0.00528,-0.00288,-0.00288], "fy":[-0.00091,0.00152,0.00152,-0.00091]}, - {"t":1.39669, "x":6.25916, "y":4.18027, "heading":3.14155, "vx":-0.6848, "vy":-0.72846, "omega":0.0002, "ax":0.00002, "ay":-0.00002, "alpha":-0.00003, "fx":[0.00186,0.00186,-0.00103,-0.00103], "fy":[0.00121,-0.00759,0.00362,0.00121]}, - {"t":1.44657, "x":6.225, "y":4.14393, "heading":3.14156, "vx":-0.68479, "vy":-0.72846, "omega":0.0002, "ax":-0.00003, "ay":0.00003, "alpha":0.0013, "fx":[-0.00282,0.01156,-0.00526,-0.00526], "fy":[-0.00108,0.00192,0.00192,-0.00109]}, - {"t":1.49645, "x":6.19084, "y":4.1076, "heading":3.14157, "vx":-0.6848, "vy":-0.72846, "omega":0.00027, "ax":0.00011, "ay":-0.00011, "alpha":-0.00069, "fx":[0.00355,0.00354,0.00018,0.00018], "fy":[0.00327,-0.0194,0.00584,0.00327]}, - {"t":1.54633, "x":6.15668, "y":4.07126, "heading":3.14159, "vx":-0.68479, "vy":-0.72847, "omega":0.00023, "ax":0.00002, "ay":-0.00002, "alpha":0.00228, "fx":[-0.00738,0.02723,-0.00938,-0.00938], "fy":[-0.0017,0.00119,0.00118,-0.0017]}, - {"t":1.59621, "x":6.12252, "y":4.03492, "heading":-3.14159, "vx":-0.68479, "vy":-0.72847, "omega":0.00035, "ax":0.00015, "ay":-0.00014, "alpha":-0.00293, "fx":[0.00395,0.00395,0.00097,0.00097], "fy":[0.01347,-0.05137,0.01518,0.01347]}, - {"t":1.64609, "x":6.08836, "y":3.99859, "heading":-3.14157, "vx":-0.68478, "vy":-0.72848, "omega":0.0002, "ax":-0.00126, "ay":0.00119, "alpha":0.00195, "fx":[-0.03021,0.00603,-0.02994,-0.02994], "fy":[0.0192,0.02032,0.02032,0.0192]}, - {"t":1.69598, "x":6.0542, "y":3.96225, "heading":-3.14156, "vx":-0.68484, "vy":-0.72842, "omega":0.0003, "ax":0.00069, "ay":-0.00065, "alpha":-0.01597, "fx":[0.01161,0.01161,0.01155,0.01155], "fy":[0.06691,-0.24238,0.06502,0.06691]}, - {"t":1.74586, "x":6.02004, "y":3.92591, "heading":-3.14155, "vx":-0.68481, "vy":-0.72845, "omega":-0.0005, "ax":0.0032, "ay":-0.00301, "alpha":-0.03823, "fx":[0.22746,-0.48673,0.23631,0.23631], "fy":[-0.04689,-0.05336,-0.05339,-0.0469]}, - {"t":1.79574, "x":5.98589, "y":3.88957, "heading":-3.14157, "vx":-0.68465, "vy":-0.7286, "omega":-0.00241, "ax":-0.00509, "ay":0.00481, "alpha":-0.03584, "fx":[-0.09909,-0.10012,-0.07024,-0.07017], "fy":[0.24001,-0.3674,0.20806,0.23998]}, - {"t":1.84562, "x":5.95173, "y":3.85324, "heading":3.14149, "vx":-0.6849, "vy":-0.72836, "omega":-0.00419, "ax":1.40839, "ay":1.494, "alpha":0.08791, "fx":[22.92976,24.96844,23.00586,23.00473], "fy":[24.94886,24.84473,24.87637,24.94736]}, - {"t":1.8955, "x":5.91932, "y":3.81876, "heading":3.14129, "vx":-0.61465, "vy":-0.65384, "omega":0.00019, "ax":2.05222, "ay":2.18279, "alpha":0.00997, "fx":[34.17944,34.14957,34.25627,34.25276], "fy":[36.29388,36.74919,36.21022,36.29114]}, - {"t":1.94538, "x":5.89121, "y":3.78886, "heading":3.1413, "vx":-0.51228, "vy":-0.54495, "omega":0.00069, "ax":2.05343, "ay":2.18438, "alpha":0.00877, "fx":[34.06153,34.56834,34.14598,34.14283], "fy":[36.45695,36.35897,36.38085,36.45331]}, - {"t":1.99527, "x":5.86821, "y":3.7644, "heading":3.14133, "vx":-0.40986, "vy":-0.43599, "omega":0.00113, "ax":2.05388, "ay":2.18486, "alpha":0.00364, "fx":[34.20756,34.19018,34.27731,34.2737], "fy":[36.3661,36.66508,36.28832,36.36293]}, - {"t":2.04515, "x":5.85032, "y":3.74537, "heading":3.14139, "vx":-0.3074, "vy":-0.32701, "omega":0.00131, "ax":2.0541, "ay":2.18511, "alpha":0.00221, "fx":[34.13135,34.43607,34.19946,34.19653], "fy":[36.4585,36.38661,36.39861,36.45525]}, - {"t":2.09503, "x":5.83755, "y":3.73177, "heading":3.14145, "vx":-0.20494, "vy":-0.21801, "omega":0.00142, "ax":2.05424, "ay":2.18525, "alpha":0.00008, "fx":[34.22161,34.21184,34.27108,34.26815], "fy":[36.40158,36.56309,36.34482,36.39897]}, - {"t":2.14491, "x":5.82988, "y":3.72362, "heading":3.14152, "vx":-0.10247, "vy":-0.10901, "omega":0.00142, "ax":2.05433, "ay":2.18535, "alpha":-0.02851, "fx":[34.16214,34.17568,34.21814,34.42285], "fy":[36.58802,36.36693,36.3644,36.39549]}, - {"t":2.19479, "x":5.82732, "y":3.7209, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.1, "y":5.07, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.05772, "ay":-2.18124, "alpha":0.00033, "fx":[-34.19182,-34.47552,-34.27061,-34.26701], "fy":[-36.3963,-36.31992,-36.33238,-36.3921]}, + {"t":0.04977, "x":7.09745, "y":5.0673, "heading":3.14159, "vx":-0.10242, "vy":-0.10857, "omega":0.00002, "ax":-2.05809, "ay":-2.18162, "alpha":0.00566, "fx":[-34.22869,-34.39424,-34.30518,-34.3015], "fy":[-36.40028,-36.33169,-36.33818,-36.39609]}, + {"t":0.09955, "x":7.0898, "y":5.05919, "heading":3.14159, "vx":-0.20486, "vy":-0.21715, "omega":0.0003, "ax":-2.05796, "ay":-2.18147, "alpha":-0.00205, "fx":[-34.27754,-34.26551,-34.34102,-34.33697], "fy":[-36.32205,-36.56991,-36.2456,-36.31847]}, + {"t":0.14932, "x":7.07706, "y":5.04568, "heading":-3.14158, "vx":-0.30729, "vy":-0.32573, "omega":0.0002, "ax":-2.05774, "ay":-2.18122, "alpha":-0.00008, "fx":[-34.19274,-34.47923,-34.26893,-34.26551], "fy":[-36.39582,-36.31908,-36.33259,-36.39185]}, + {"t":0.19909, "x":7.05922, "y":5.02677, "heading":-3.14157, "vx":-0.40971, "vy":-0.43429, "omega":0.00019, "ax":-2.05733, "ay":-2.18069, "alpha":-0.01144, "fx":[-34.26933,-34.24774,-34.33275,-34.32908], "fy":[-36.26077,-36.69989,-36.1861,-36.25761]}, + {"t":0.24886, "x":7.03628, "y":5.00245, "heading":-3.14156, "vx":-0.5121, "vy":-0.54283, "omega":-0.00038, "ax":-2.05577, "ay":-2.17943, "alpha":0.00244, "fx":[-34.18395,-34.39351,-34.24989,-34.24741], "fy":[-36.36362,-36.28812,-36.30815,-36.36033]}, + {"t":0.29864, "x":7.00824, "y":4.97273, "heading":-3.14158, "vx":-0.61443, "vy":-0.65131, "omega":-0.00026, "ax":-1.43669, "ay":-1.52854, "alpha":-0.07248, "fx":[-23.93248,-23.9037,-23.98065,-23.97917], "fy":[-25.09657,-26.68562,-25.04222,-25.09553]}, + {"t":0.34841, "x":6.97588, "y":4.93842, "heading":-3.14159, "vx":-0.68593, "vy":-0.72739, "omega":-0.00386, "ax":-0.00653, "ay":0.00614, "alpha":0.04341, "fx":[-0.30494,0.50319,-0.31682,-0.31679], "fy":[0.09888,0.10648,0.10509,0.09878]}, + {"t":0.39818, "x":6.94173, "y":4.90223, "heading":3.1414, "vx":-0.68626, "vy":-0.72708, "omega":-0.0017, "ax":0.00548, "ay":-0.00517, "alpha":0.03012, "fx":[0.08559,0.08556,0.09708,0.09709], "fy":[-0.23866,0.37877,-0.24611,-0.23866]}, + {"t":0.44795, "x":6.90758, "y":4.86603, "heading":3.14132, "vx":-0.68599, "vy":-0.72734, "omega":-0.0002, "ax":-0.00046, "ay":0.00043, "alpha":0.01957, "fx":[-0.12429,0.31596,-0.11122,-0.11121], "fy":[0.01552,-0.00101,-0.00102,0.01551]}, + {"t":0.49773, "x":6.87344, "y":4.82983, "heading":3.14131, "vx":-0.68601, "vy":-0.72732, "omega":0.00077, "ax":-0.00158, "ay":0.00149, "alpha":-0.00605, "fx":[-0.03326,-0.03327,-0.01946,-0.01946], "fy":[0.04746,-0.03158,0.03613,0.04745]}, + {"t":0.5475, "x":6.83929, "y":4.79363, "heading":3.14134, "vx":-0.68609, "vy":-0.72725, "omega":0.00047, "ax":0.00028, "ay":-0.00026, "alpha":0.00287, "fx":[-0.0276,0.07824,-0.01615,-0.01615], "fy":[0.00246,-0.0111,-0.0111,0.00245]}, + {"t":0.59727, "x":6.80514, "y":4.75743, "heading":3.14137, "vx":-0.68607, "vy":-0.72726, "omega":0.00061, "ax":0.00005, "ay":-0.00005, "alpha":-0.00451, "fx":[-0.00456,-0.00456,0.00637,0.00638], "fy":[0.01565,-0.04085,0.00613,0.01564]}, + {"t":0.64705, "x":6.77099, "y":4.72123, "heading":3.1414, "vx":-0.68607, "vy":-0.72726, "omega":0.00039, "ax":0.00005, "ay":-0.00005, "alpha":-0.00003, "fx":[-0.01238,0.02358,-0.00381,-0.00381], "fy":[0.004,-0.00568,-0.00568,0.00399]}, + {"t":0.69682, "x":6.73685, "y":4.68504, "heading":3.14142, "vx":-0.68607, "vy":-0.72726, "omega":0.00039, "ax":0.00004, "ay":-0.00004, "alpha":-0.00243, "fx":[-0.00318,-0.00318,0.00448,0.00448], "fy":[0.00738,-0.01761,0.00039,0.00737]}, + {"t":0.74659, "x":6.7027, "y":4.64884, "heading":3.14144, "vx":-0.68607, "vy":-0.72726, "omega":0.00026, "ax":0.0, "ay":0.0, "alpha":-0.00056, "fx":[-0.00636,0.00702,-0.0005,-0.00049], "fy":[0.00321,-0.00306,-0.00306,0.00321]}, + {"t":0.79636, "x":6.66855, "y":4.61264, "heading":3.14145, "vx":-0.68607, "vy":-0.72726, "omega":0.00024, "ax":-0.00001, "ay":0.00001, "alpha":-0.00131, "fx":[-0.00249,-0.00249,0.00223,0.00223], "fy":[0.00417,-0.00741,-0.00042,0.00417]}, + {"t":0.84614, "x":6.6344, "y":4.57644, "heading":3.14146, "vx":-0.68607, "vy":-0.72726, "omega":0.00017, "ax":0.00001, "ay":-0.00001, "alpha":-0.00047, "fx":[-0.00294,0.00217,0.00064,0.00064], "fy":[0.00164,-0.00187,-0.00188,0.00163]}, + {"t":0.89591, "x":6.60026, "y":4.54025, "heading":3.14147, "vx":-0.68607, "vy":-0.72726, "omega":0.00015, "ax":-0.00001, "ay":0.00001, "alpha":-0.00066, "fx":[-0.00137,-0.00137,0.00096,0.00097], "fy":[0.00223,-0.00332,-0.00037,0.00222]}, + {"t":0.94568, "x":6.56611, "y":4.50405, "heading":3.14148, "vx":-0.68607, "vy":-0.72726, "omega":0.00012, "ax":0.00001, "ay":-0.00001, "alpha":-0.00023, "fx":[-0.00113,0.00071,0.00064,0.00064], "fy":[0.00048,-0.00088,-0.00088,0.00048]}, + {"t":0.99546, "x":6.53196, "y":4.46785, "heading":3.14148, "vx":-0.68607, "vy":-0.72726, "omega":0.0001, "ax":-0.00001, "ay":0.00001, "alpha":-0.00024, "fx":[-0.00038,-0.00038,0.00008,0.00009], "fy":[0.00105,-0.0016,0.00005,0.00105]}, + {"t":1.04523, "x":6.49781, "y":4.43165, "heading":3.14149, "vx":-0.68607, "vy":-0.72726, "omega":0.00009, "ax":0.00001, "ay":-0.00001, "alpha":0.00003, "fx":[-0.00017,0.00056,0.00016,0.00016], "fy":[-0.00031,-0.00002,-0.00002,-0.00032]}, + {"t":1.095, "x":6.46367, "y":4.39545, "heading":3.14149, "vx":-0.68607, "vy":-0.72726, "omega":0.00009, "ax":-0.00001, "ay":0.00001, "alpha":0.00004, "fx":[0.00041,0.00041,-0.0006,-0.00059], "fy":[0.00038,-0.00106,0.00065,0.00038]}, + {"t":1.14477, "x":6.42952, "y":4.35926, "heading":3.1415, "vx":-0.68607, "vy":-0.72726, "omega":0.0001, "ax":0.00001, "ay":-0.00001, "alpha":0.0003, "fx":[0.00028,0.00121,-0.00053,-0.00053], "fy":[-0.0009,0.0007,0.0007,-0.0009]}, + {"t":1.19455, "x":6.39537, "y":4.32306, "heading":3.1415, "vx":-0.68607, "vy":-0.72726, "omega":0.00011, "ax":0.0, "ay":0.0, "alpha":0.00021, "fx":[0.00105,0.00104,-0.00115,-0.00114], "fy":[0.00011,-0.00146,0.00143,0.00011]}, + {"t":1.24432, "x":6.36122, "y":4.28686, "heading":3.14151, "vx":-0.68607, "vy":-0.72726, "omega":0.00012, "ax":0.0, "ay":0.0, "alpha":0.00058, "fx":[0.00023,0.00268,-0.00151,-0.0015], "fy":[-0.00131,0.00136,0.00136,-0.00131]}, + {"t":1.29409, "x":6.32708, "y":4.25066, "heading":3.14151, "vx":-0.68607, "vy":-0.72726, "omega":0.00015, "ax":0.0, "ay":0.0, "alpha":0.00026, "fx":[0.0016,0.0016,-0.00158,-0.00158], "fy":[0.00032,-0.00317,0.0025,0.00032]}, + {"t":1.34386, "x":6.29293, "y":4.21446, "heading":3.14152, "vx":-0.68607, "vy":-0.72726, "omega":0.00016, "ax":-0.00002, "ay":0.00002, "alpha":0.00093, "fx":[-0.0006,0.0055,-0.00307,-0.00307], "fy":[-0.00147,0.00205,0.00205,-0.00147]}, + {"t":1.39364, "x":6.25878, "y":4.17827, "heading":3.14153, "vx":-0.68607, "vy":-0.72726, "omega":0.00021, "ax":0.00002, "ay":-0.00002, "alpha":0.0001, "fx":[0.00235,0.00235,-0.00161,-0.00161], "fy":[0.00113,-0.00761,0.00394,0.00113]}, + {"t":1.44341, "x":6.22463, "y":4.14207, "heading":3.14154, "vx":-0.68607, "vy":-0.72726, "omega":0.00021, "ax":-0.00003, "ay":0.00002, "alpha":0.00144, "fx":[-0.00259,0.01177,-0.00543,-0.00543], "fy":[-0.00162,0.00241,0.00241,-0.00162]}, + {"t":1.49318, "x":6.19049, "y":4.10587, "heading":3.14155, "vx":-0.68607, "vy":-0.72726, "omega":0.00029, "ax":0.00011, "ay":-0.00011, "alpha":-0.00055, "fx":[0.00406,0.00406,-0.00028,-0.00027], "fy":[0.00307,-0.01931,0.00603,0.00307]}, + {"t":1.54296, "x":6.15634, "y":4.06967, "heading":3.14157, "vx":-0.68606, "vy":-0.72727, "omega":0.00026, "ax":0.00002, "ay":-0.00002, "alpha":0.00237, "fx":[-0.00693,0.0269,-0.00933,-0.00933], "fy":[-0.00221,0.00159,0.00159,-0.00221]}, + {"t":1.59273, "x":6.12219, "y":4.03347, "heading":3.14158, "vx":-0.68606, "vy":-0.72727, "omega":0.00038, "ax":0.00015, "ay":-0.00014, "alpha":-0.00279, "fx":[0.00439,0.00439,0.00058,0.00058], "fy":[0.01314,-0.05089,0.01523,0.01314]}, + {"t":1.6425, "x":6.08804, "y":3.99727, "heading":-3.14159, "vx":-0.68605, "vy":-0.72728, "omega":0.00024, "ax":-0.00131, "ay":0.00123, "alpha":0.00171, "fx":[-0.02899,0.00024,-0.02914,-0.02914], "fy":[0.01959,0.02148,0.02147,0.01958]}, + {"t":1.69227, "x":6.0539, "y":3.96108, "heading":-3.14158, "vx":-0.68612, "vy":-0.72722, "omega":0.00032, "ax":0.00062, "ay":-0.00058, "alpha":-0.01607, "fx":[0.01062,0.01062,0.00998,0.00999], "fy":[0.06887,-0.24391,0.0673,0.06887]}, + {"t":1.74205, "x":6.01975, "y":3.92488, "heading":-3.14156, "vx":-0.68609, "vy":-0.72724, "omega":-0.00048, "ax":0.00347, "ay":-0.00327, "alpha":-0.04004, "fx":[0.24126,-0.50963,0.24984,0.24984], "fy":[-0.05152,-0.05756,-0.05758,-0.05152]}, + {"t":1.79182, "x":5.9856, "y":3.88868, "heading":-3.14158, "vx":-0.68592, "vy":-0.72741, "omega":-0.00247, "ax":-0.00533, "ay":0.00505, "alpha":-0.03155, "fx":[-0.10339,-0.1045,-0.07384,-0.07375], "fy":[0.22283,-0.29898,0.19018,0.2228]}, + {"t":1.84159, "x":5.95146, "y":3.85248, "heading":3.14148, "vx":-0.68618, "vy":-0.72716, "omega":-0.00404, "ax":1.44128, "ay":1.5233, "alpha":0.08942, "fx":[23.46991,25.54053,23.54614,23.54496], "fy":[25.43788,25.33135,25.36522,25.43627]}, + {"t":1.89137, "x":5.91909, "y":3.81818, "heading":3.14128, "vx":-0.61445, "vy":-0.65134, "omega":0.00041, "ax":2.05605, "ay":2.17917, "alpha":0.00395, "fx":[34.24311,34.21569,34.31893,34.31541], "fy":[36.26393,36.59656,36.18123,36.26118]}, + {"t":1.94114, "x":5.89105, "y":3.78846, "heading":3.1413, "vx":-0.51211, "vy":-0.54287, "omega":0.00061, "ax":2.05717, "ay":2.18085, "alpha":0.0109, "fx":[34.11343,34.66244,34.19764,34.19447], "fy":[36.3984,36.29933,36.32219,36.39474]}, + {"t":1.99091, "x":5.86811, "y":3.76414, "heading":3.14133, "vx":-0.40972, "vy":-0.43433, "omega":0.00115, "ax":2.05768, "ay":2.18127, "alpha":0.00386, "fx":[34.27088,34.2535,34.34077,34.33716], "fy":[36.30519,36.60824,36.22778,36.302]}, + {"t":2.04068, "x":5.85027, "y":3.74522, "heading":3.14138, "vx":-0.3073, "vy":-0.32576, "omega":0.00134, "ax":2.05791, "ay":2.18151, "alpha":0.00156, "fx":[34.19847,34.48997,34.26611,34.26316], "fy":[36.39864,36.32671,36.33844,36.39538]}, + {"t":2.09046, "x":5.83752, "y":3.73171, "heading":3.14145, "vx":-0.20487, "vy":-0.21718, "omega":0.00142, "ax":2.05805, "ay":2.18166, "alpha":-0.00007, "fx":[34.28481,34.27523,34.33497,34.33204], "fy":[36.34209,36.50147,36.28565,36.33944]}, + {"t":2.14023, "x":5.82987, "y":3.7236, "heading":3.14152, "vx":-0.10244, "vy":-0.10859, "omega":0.00142, "ax":2.05814, "ay":2.18175, "alpha":-0.02844, "fx":[34.22704,34.24077,34.28305,34.48219], "fy":[36.53138,36.30592,36.3034,36.33451]}, + {"t":2.19, "x":5.82732, "y":3.7209, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/startToH.traj b/src/main/deploy/choreo/startToH.traj index 0ece48b3..d7bdd1c1 100644 --- a/src/main/deploy/choreo/startToH.traj +++ b/src/main/deploy/choreo/startToH.traj @@ -3,7 +3,7 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.1, "y":5.07, "heading":3.14159, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":5.827323, "y":4.0509, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -30,49 +30,49 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.96969], + "waypoints":[0.0,1.96537], "samples":[ - {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.33628, "ay":-1.8798, "alpha":-0.00191, "fx":[-38.78316,-39.22798,-38.88655,-38.88121], "fy":[-31.38984,-31.27378,-31.29392,-31.38411]}, - {"t":0.04924, "x":7.09806, "y":5.0734, "heading":3.14159, "vx":-0.11504, "vy":-0.09257, "omega":-0.00009, "ax":-2.33671, "ay":-1.88013, "alpha":0.00528, "fx":[-38.83435,-39.11242,-38.93308,-38.92775], "fy":[-31.39092,-31.28782,-31.2996,-31.38523]}, - {"t":0.09848, "x":7.08956, "y":5.06656, "heading":3.14159, "vx":-0.23011, "vy":-0.18515, "omega":0.00017, "ax":-2.33657, "ay":-1.87999, "alpha":0.00399, "fx":[-38.91022,-38.89826,-38.99743,-38.99208], "fy":[-31.31519,-31.51116,-31.21744,-31.31015]}, - {"t":0.14773, "x":7.07539, "y":5.05517, "heading":-3.14159, "vx":-0.34517, "vy":-0.27772, "omega":0.00036, "ax":-2.33633, "ay":-1.87975, "alpha":-0.00274, "fx":[-38.79023,-39.22438,-38.88607,-38.88123], "fy":[-31.38556,-31.27601,-31.29606,-31.38037]}, - {"t":0.19697, "x":7.05556, "y":5.03921, "heading":-3.14157, "vx":-0.46021, "vy":-0.37028, "omega":0.00023, "ax":-2.33591, "ay":-1.87919, "alpha":-0.00763, "fx":[-38.90275,-38.87922,-38.98837,-38.98363], "fy":[-31.24277,-31.6716,-31.14817,-31.23835]}, - {"t":0.24621, "x":7.03007, "y":5.0187, "heading":-3.14156, "vx":-0.57524, "vy":-0.46282, "omega":-0.00015, "ax":-2.33428, "ay":-1.878, "alpha":-0.02823, "fx":[-38.63978,-39.55709,-38.72594,-38.72226], "fy":[-31.35709,-31.23549,-31.27594,-31.35301]}, - {"t":0.29545, "x":6.99891, "y":4.99363, "heading":-3.14157, "vx":-0.69018, "vy":-0.5553, "omega":-0.00154, "ax":-1.80006, "ay":-1.45462, "alpha":-0.0167, "fx":[-29.98204,-29.96494,-30.03968,-30.03787], "fy":[-24.13462,-24.65207,-24.07148,-24.13301]}, - {"t":0.3447, "x":6.96275, "y":4.96452, "heading":3.14154, "vx":-0.77882, "vy":-0.62693, "omega":-0.00236, "ax":-0.00171, "ay":0.00208, "alpha":0.03923, "fx":[-0.18174,0.48417,-0.20809,-0.20804], "fy":[0.02326,0.046,0.04624,0.02329]}, - {"t":0.39394, "x":6.92439, "y":4.93366, "heading":3.14143, "vx":-0.7789, "vy":-0.62682, "omega":-0.00043, "ax":-0.00215, "ay":0.00267, "alpha":0.00398, "fx":[-0.03123,-0.03124,-0.04028,-0.04028], "fy":[0.02955,0.0772,0.04144,0.02955]}, - {"t":0.44318, "x":6.88604, "y":4.90279, "heading":3.1414, "vx":-0.77901, "vy":-0.62669, "omega":-0.00023, "ax":0.00087, "ay":-0.00109, "alpha":0.00785, "fx":[-0.02045,0.12542,-0.02336,-0.02336], "fy":[-0.01841,-0.01779,-0.01779,-0.01841]}, - {"t":0.49242, "x":6.84768, "y":4.87193, "heading":3.14139, "vx":-0.77897, "vy":-0.62675, "omega":0.00015, "ax":0.00098, "ay":-0.00121, "alpha":0.00039, "fx":[0.01537,0.01537,0.01716,0.01717], "fy":[-0.02301,-0.0118,-0.02302,-0.02302]}, - {"t":0.54166, "x":6.80932, "y":4.84107, "heading":3.1414, "vx":-0.77892, "vy":-0.6268, "omega":0.00017, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00303,0.00596,-0.00143,-0.00143], "fy":[0.00148,-0.00152,-0.00153,0.00148]}, - {"t":0.59091, "x":6.77096, "y":4.8102, "heading":3.14141, "vx":-0.77892, "vy":-0.62681, "omega":0.00017, "ax":-0.00063, "ay":0.00078, "alpha":0.00055, "fx":[-0.01216,-0.01216,-0.00878,-0.00878], "fy":[0.00865,0.02841,0.00635,0.00864]}, - {"t":0.64015, "x":6.73261, "y":4.77934, "heading":3.14142, "vx":-0.77895, "vy":-0.62677, "omega":0.0002, "ax":0.00009, "ay":-0.00012, "alpha":-0.0006, "fx":[0.00071,-0.00036,0.00295,0.00295], "fy":[-0.00041,-0.00347,-0.00347,-0.00041]}, - {"t":0.68939, "x":6.69425, "y":4.74848, "heading":3.14143, "vx":-0.77895, "vy":-0.62677, "omega":0.00017, "ax":0.0001, "ay":-0.00013, "alpha":-0.00006, "fx":[0.00036,0.00036,0.00305,0.00305], "fy":[-0.00316,0.0031,-0.00525,-0.00316]}, - {"t":0.73863, "x":6.65589, "y":4.71761, "heading":3.14144, "vx":-0.77894, "vy":-0.62678, "omega":0.00017, "ax":0.00001, "ay":-0.00001, "alpha":-0.0004, "fx":[-0.00069,-0.00077,0.00102,0.00102], "fy":[0.00088,-0.00124,-0.00124,0.00088]}, - {"t":0.78787, "x":6.61754, "y":4.68675, "heading":3.14144, "vx":-0.77894, "vy":-0.62678, "omega":0.00015, "ax":-0.00003, "ay":0.00004, "alpha":-0.00018, "fx":[-0.00134,-0.00135,0.00025,0.00025], "fy":[0.00076,0.00179,-0.00059,0.00076]}, - {"t":0.83712, "x":6.57918, "y":4.65588, "heading":3.14145, "vx":-0.77894, "vy":-0.62678, "omega":0.00014, "ax":0.00001, "ay":-0.00001, "alpha":-0.00018, "fx":[-0.00045,0.00021,0.00051,0.00051], "fy":[0.0003,-0.00078,-0.00078,0.0003]}, - {"t":0.88636, "x":6.54082, "y":4.62502, "heading":3.14146, "vx":-0.77894, "vy":-0.62678, "omega":0.00013, "ax":0.0, "ay":0.0, "alpha":-0.00011, "fx":[-0.00035,-0.00035,0.00026,0.00026], "fy":[0.0003,-0.00004,-0.00031,0.0003]}, - {"t":0.9356, "x":6.50247, "y":4.59416, "heading":3.14146, "vx":-0.77894, "vy":-0.62678, "omega":0.00012, "ax":0.00001, "ay":-0.00001, "alpha":-0.00001, "fx":[-0.00019,0.00061,0.00012,0.00012], "fy":[-0.0001,-0.0003,-0.0003,-0.0001]}, - {"t":0.98484, "x":6.46411, "y":4.56329, "heading":3.14147, "vx":-0.77894, "vy":-0.62678, "omega":0.00012, "ax":-0.00001, "ay":0.00001, "alpha":0.00002, "fx":[-0.00006,-0.00006,-0.00024,-0.00024], "fy":[0.00019,0.00021,0.00016,0.00019]}, - {"t":1.03409, "x":6.42575, "y":4.53243, "heading":3.14148, "vx":-0.77894, "vy":-0.62678, "omega":0.00012, "ax":0.00001, "ay":-0.00002, "alpha":0.00011, "fx":[0.00013,0.00082,-0.00007,-0.00007], "fy":[-0.0005,0.0,0.0,-0.0005]}, - {"t":1.08333, "x":6.3874, "y":4.50156, "heading":3.14148, "vx":-0.77894, "vy":-0.62678, "omega":0.00013, "ax":-0.00002, "ay":0.00003, "alpha":0.00017, "fx":[0.00003,0.00003,-0.00075,-0.00075], "fy":[0.00003,0.0013,0.00041,0.00003]}, - {"t":1.13257, "x":6.34904, "y":4.4707, "heading":3.14149, "vx":-0.77894, "vy":-0.62678, "omega":0.00014, "ax":0.00001, "ay":-0.00001, "alpha":0.00014, "fx":[0.00048,0.00021,-0.00004,-0.00004], "fy":[-0.0007,0.00032,0.00032,-0.0007]}, - {"t":1.18181, "x":6.31068, "y":4.43984, "heading":3.1415, "vx":-0.77894, "vy":-0.62678, "omega":0.00015, "ax":0.00004, "ay":-0.00005, "alpha":0.00035, "fx":[0.00131,0.00131,0.00013,0.00013], "fy":[-0.002,0.00188,-0.00145,-0.002]}, - {"t":1.23105, "x":6.27233, "y":4.40897, "heading":3.1415, "vx":-0.77894, "vy":-0.62678, "omega":0.00016, "ax":0.00004, "ay":-0.00005, "alpha":-0.00002, "fx":[0.00188,-0.00208,0.00136,0.00136], "fy":[-0.00142,-0.00014,-0.00014,-0.00142]}, - {"t":1.2803, "x":6.23397, "y":4.37811, "heading":3.14151, "vx":-0.77894, "vy":-0.62678, "omega":0.00016, "ax":-0.00022, "ay":0.00027, "alpha":0.00059, "fx":[-0.00305,-0.00305,-0.00413,-0.00413], "fy":[0.00212,0.01133,0.00227,0.00212]}, - {"t":1.32954, "x":6.19561, "y":4.34724, "heading":3.14152, "vx":-0.77895, "vy":-0.62677, "omega":0.00019, "ax":-0.00002, "ay":0.00003, "alpha":-0.00058, "fx":[0.00247,-0.00959,0.00285,0.00285], "fy":[0.00008,0.0008,0.0008,0.00008]}, - {"t":1.37878, "x":6.15726, "y":4.31638, "heading":3.14153, "vx":-0.77895, "vy":-0.62677, "omega":0.00016, "ax":0.00082, "ay":-0.00102, "alpha":-0.00055, "fx":[0.01345,0.01345,0.01383,0.01383], "fy":[-0.01446,-0.02269,-0.01618,-0.01446]}, - {"t":1.42802, "x":6.1189, "y":4.28552, "heading":3.14154, "vx":-0.77891, "vy":-0.62682, "omega":0.00013, "ax":-0.00043, "ay":0.00053, "alpha":-0.00149, "fx":[-0.00518,-0.02122,-0.00111,-0.00111], "fy":[0.01012,0.00766,0.00766,0.01012]}, - {"t":1.47726, "x":6.08054, "y":4.25465, "heading":3.14154, "vx":-0.77893, "vy":-0.62679, "omega":0.00006, "ax":-0.00118, "ay":0.00146, "alpha":-0.01107, "fx":[-0.02254,-0.02254,-0.01669,-0.01669], "fy":[0.07537,-0.12075,0.06754,0.07537]}, - {"t":1.52651, "x":6.04219, "y":4.22379, "heading":3.14155, "vx":-0.77899, "vy":-0.62672, "omega":-0.00048, "ax":0.0008, "ay":-0.00099, "alpha":-0.00875, "fx":[0.0301,-0.07315,0.04804,0.04804], "fy":[-0.00863,-0.02432,-0.02434,-0.00863]}, - {"t":1.57575, "x":6.00383, "y":4.19293, "heading":3.14152, "vx":-0.77895, "vy":-0.62677, "omega":-0.00091, "ax":-0.00001, "ay":0.00005, "alpha":-0.02606, "fx":[-0.01687,-0.01669,0.01639,0.01636], "fy":[0.11125,-0.29452,0.07556,0.1112]}, - {"t":1.62499, "x":5.96547, "y":4.16206, "heading":3.14148, "vx":-0.77895, "vy":-0.62677, "omega":-0.0022, "ax":1.80283, "ay":1.4501, "alpha":0.02962, "fx":[29.80118,30.66919,29.8703,29.86868], "fy":[24.21191,24.12164,24.14594,24.21007]}, - {"t":1.67423, "x":5.9293, "y":4.13296, "heading":3.14137, "vx":-0.69017, "vy":-0.55536, "omega":-0.00074, "ax":2.33421, "ay":1.87809, "alpha":0.02063, "fx":[38.88037,38.84795,38.95788,38.95419], "fy":[31.16205,31.82856,31.07805,31.15868]}, - {"t":1.72348, "x":5.89814, "y":4.10789, "heading":3.14133, "vx":-0.57523, "vy":-0.46288, "omega":0.00028, "ax":2.33566, "ay":1.8795, "alpha":0.01496, "fx":[38.73217,39.37572,38.81661,38.81276], "fy":[31.37929,31.26796,31.29928,31.37511]}, - {"t":1.77272, "x":5.87265, "y":4.08737, "heading":3.14135, "vx":-0.46022, "vy":-0.37033, "omega":0.00101, "ax":2.33619, "ay":1.87992, "alpha":0.0028, "fx":[38.9134,38.89683,38.98327,38.97929], "fy":[31.28745,31.56821,31.20994,31.28371]}, - {"t":1.82196, "x":5.85282, "y":4.07142, "heading":3.1414, "vx":-0.34518, "vy":-0.27776, "omega":0.00115, "ax":2.33648, "ay":1.8801, "alpha":0.00478, "fx":[38.82614,39.18459,38.89228,38.88883], "fy":[31.37641,31.29708,31.31538,31.37271]}, - {"t":1.8712, "x":5.83866, "y":4.06002, "heading":3.14145, "vx":-0.23013, "vy":-0.18518, "omega":0.00139, "ax":2.33664, "ay":1.88023, "alpha":0.00125, "fx":[38.93116,38.9201,38.97721,38.97412], "fy":[31.31281,31.48769,31.2594,31.3099]}, - {"t":1.92044, "x":5.83016, "y":4.05318, "heading":3.14152, "vx":-0.11507, "vy":-0.09259, "omega":0.00145, "ax":2.33674, "ay":1.88031, "alpha":-0.0294, "fx":[38.86337,38.87811,38.91419,39.15397], "fy":[31.50866,31.28357,31.28109,31.3021]}, - {"t":1.96969, "x":5.82732, "y":4.0509, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.1, "y":5.07, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.34073, "ay":-1.87425, "alpha":-0.00182, "fx":[-38.85668,-39.3027,-38.96068,-38.95536], "fy":[-31.29773,-31.18085,-31.2011,-31.29202]}, + {"t":0.04913, "x":7.09717, "y":5.06774, "heading":3.14159, "vx":-0.11501, "vy":-0.09209, "omega":-0.00009, "ax":-2.34117, "ay":-1.87458, "alpha":0.00538, "fx":[-38.90822,-39.18672,-39.00746,-39.00215], "fy":[-31.29869,-31.19484,-31.20675,-31.29302]}, + {"t":0.09827, "x":7.0887, "y":5.06095, "heading":3.14159, "vx":-0.23004, "vy":-0.1842, "omega":0.00017, "ax":-2.34102, "ay":-1.87443, "alpha":0.004, "fx":[-38.98418,-38.9724,-39.07181,-39.06649], "fy":[-31.22248,-31.41933,-31.12437,-31.21747]}, + {"t":0.1474, "x":7.07457, "y":5.04964, "heading":-3.14159, "vx":-0.34507, "vy":-0.2763, "omega":0.00037, "ax":-2.34078, "ay":-1.8742, "alpha":-0.0027, "fx":[-38.86429,-39.29864,-38.96023,-38.95543], "fy":[-31.2932,-31.18315,-31.20344,-31.28804]}, + {"t":0.19654, "x":7.05479, "y":5.0338, "heading":-3.14157, "vx":-0.46008, "vy":-0.36838, "omega":0.00024, "ax":-2.34033, "ay":-1.87367, "alpha":-0.00761, "fx":[-38.97645,-38.95312,-39.06199,-39.0573], "fy":[-31.15089,-31.57878,-31.05651,-31.14652]}, + {"t":0.24567, "x":7.02936, "y":5.01344, "heading":-3.14156, "vx":-0.57507, "vy":-0.46044, "omega":-0.00014, "ax":-2.33885, "ay":-1.87228, "alpha":-0.0283, "fx":[-38.71645,-39.6326,-38.80221,-38.79858], "fy":[-31.26132,-31.14134,-31.18038,-31.25731]}, + {"t":0.29481, "x":6.99828, "y":4.98855, "heading":-3.14157, "vx":-0.68999, "vy":-0.55244, "omega":-0.00153, "ax":-1.838, "ay":-1.47859, "alpha":-0.0178, "fx":[-30.61456,-30.59639,-30.67249,-30.67064], "fy":[-24.5285,-25.06912,-24.46509,-24.52685]}, + {"t":0.34394, "x":6.96216, "y":4.95963, "heading":3.14154, "vx":-0.7803, "vy":-0.62509, "omega":-0.0024, "ax":-0.00162, "ay":0.00198, "alpha":0.03968, "fx":[-0.18178,0.49086,-0.20865,-0.20859], "fy":[0.02138,0.04466,0.04489,0.02141]}, + {"t":0.39307, "x":6.92382, "y":4.92891, "heading":3.14142, "vx":-0.78038, "vy":-0.62499, "omega":-0.00045, "ax":-0.00279, "ay":0.00348, "alpha":0.00392, "fx":[-0.04171,-0.04173,-0.05123,-0.05123], "fy":[0.04369,0.08872,0.05607,0.04369]}, + {"t":0.44221, "x":6.88547, "y":4.89821, "heading":3.1414, "vx":-0.78051, "vy":-0.62482, "omega":-0.00026, "ax":0.00101, "ay":-0.00126, "alpha":0.00819, "fx":[-0.01931,0.13187,-0.0226,-0.0226], "fy":[-0.0215,-0.02057,-0.02057,-0.0215]}, + {"t":0.49134, "x":6.84712, "y":4.86751, "heading":3.14139, "vx":-0.78046, "vy":-0.62488, "omega":0.00015, "ax":0.00136, "ay":-0.0017, "alpha":0.00027, "fx":[0.0219,0.02189,0.02342,0.02342], "fy":[-0.03039,-0.02235,-0.03005,-0.03039]}, + {"t":0.54048, "x":6.80878, "y":4.8368, "heading":3.1414, "vx":-0.7804, "vy":-0.62496, "omega":0.00016, "ax":0.00001, "ay":-0.00001, "alpha":0.0001, "fx":[-0.00304,0.00726,-0.00172,-0.00172], "fy":[0.00117,-0.00165,-0.00165,0.00117]}, + {"t":0.58961, "x":6.77043, "y":4.8061, "heading":3.1414, "vx":-0.7804, "vy":-0.62497, "omega":0.00016, "ax":-0.00086, "ay":0.00108, "alpha":0.00064, "fx":[-0.01604,-0.01604,-0.01278,-0.01278], "fy":[0.01325,0.03432,0.01117,0.01325]}, + {"t":0.63875, "x":6.73209, "y":4.77539, "heading":3.14141, "vx":-0.78044, "vy":-0.62491, "omega":0.0002, "ax":0.00012, "ay":-0.00016, "alpha":-0.00056, "fx":[0.00126,0.0004,0.00331,0.00332], "fy":[-0.0011,-0.00408,-0.00408,-0.0011]}, + {"t":0.68788, "x":6.69374, "y":4.74469, "heading":3.14142, "vx":-0.78043, "vy":-0.62492, "omega":0.00017, "ax":0.00017, "ay":-0.00021, "alpha":-0.00002, "fx":[0.00152,0.00151,0.00417,0.00417], "fy":[-0.00479,0.00212,-0.00673,-0.00479]}, + {"t":0.73702, "x":6.65539, "y":4.71398, "heading":3.14143, "vx":-0.78042, "vy":-0.62493, "omega":0.00017, "ax":0.00001, "ay":-0.00001, "alpha":-0.00038, "fx":[-0.00066,-0.00076,0.00092,0.00092], "fy":[0.00093,-0.00119,-0.00119,0.00092]}, + {"t":0.78615, "x":6.61705, "y":4.68327, "heading":3.14144, "vx":-0.78042, "vy":-0.62493, "omega":0.00015, "ax":-0.00006, "ay":0.00007, "alpha":-0.00014, "fx":[-0.00176,-0.00176,-0.00015,-0.00015], "fy":[0.00108,0.00277,-0.00017,0.00108]}, + {"t":0.83528, "x":6.5787, "y":4.65257, "heading":3.14145, "vx":-0.78043, "vy":-0.62493, "omega":0.00014, "ax":0.00001, "ay":-0.00001, "alpha":-0.00017, "fx":[-0.00046,0.00024,0.00041,0.00042], "fy":[0.00036,-0.00075,-0.00075,0.00036]}, + {"t":0.88442, "x":6.54036, "y":4.62186, "heading":3.14145, "vx":-0.78043, "vy":-0.62493, "omega":0.00013, "ax":0.00001, "ay":-0.00001, "alpha":-0.0001, "fx":[-0.00017,-0.00017,0.00048,0.00048], "fy":[-0.00005,-0.00009,-0.00058,-0.00005]}, + {"t":0.93355, "x":6.50201, "y":4.59116, "heading":3.14146, "vx":-0.78043, "vy":-0.62493, "omega":0.00013, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.0003,0.00056,-0.00007,-0.00007], "fy":[0.00008,-0.00016,-0.00016,0.00007]}, + {"t":0.98269, "x":6.46367, "y":4.56045, "heading":3.14147, "vx":-0.78043, "vy":-0.62493, "omega":0.00013, "ax":0.0, "ay":0.0, "alpha":0.00003, "fx":[0.00004,0.00004,-0.00011,-0.00011], "fy":[-0.00003,0.00023,0.00001,-0.00003]}, + {"t":1.03182, "x":6.42532, "y":4.52975, "heading":3.14147, "vx":-0.78043, "vy":-0.62493, "omega":0.00013, "ax":0.0, "ay":-0.00001, "alpha":0.00012, "fx":[0.00003,0.00077,-0.00025,-0.00025], "fy":[-0.00034,0.00016,0.00016,-0.00034]}, + {"t":1.08096, "x":6.38697, "y":4.49904, "heading":3.14148, "vx":-0.78043, "vy":-0.62493, "omega":0.00014, "ax":-0.00002, "ay":0.00003, "alpha":0.00019, "fx":[0.00005,0.00005,-0.00075,-0.00075], "fy":[-0.00009,0.00153,0.0004,-0.00009]}, + {"t":1.13009, "x":6.34863, "y":4.46834, "heading":3.14149, "vx":-0.78043, "vy":-0.62493, "omega":0.00014, "ax":0.0, "ay":0.0, "alpha":0.00016, "fx":[0.00042,0.00014,-0.00022,-0.00022], "fy":[-0.00058,0.00051,0.00051,-0.00058]}, + {"t":1.17922, "x":6.31028, "y":4.43763, "heading":3.14149, "vx":-0.78043, "vy":-0.62493, "omega":0.00015, "ax":0.00007, "ay":-0.00008, "alpha":0.00039, "fx":[0.00174,0.00174,0.00047,0.00047], "fy":[-0.00266,0.00175,-0.00195,-0.00266]}, + {"t":1.22836, "x":6.27194, "y":4.40693, "heading":3.1415, "vx":-0.78042, "vy":-0.62493, "omega":0.00017, "ax":0.00005, "ay":-0.00006, "alpha":0.00001, "fx":[0.00212,-0.00189,0.00141,0.00141], "fy":[-0.00167,-0.00024,-0.00023,-0.00167]}, + {"t":1.27749, "x":6.23359, "y":4.37622, "heading":3.14151, "vx":-0.78042, "vy":-0.62493, "omega":0.00017, "ax":-0.00027, "ay":0.00034, "alpha":0.00067, "fx":[-0.00388,-0.00388,-0.00515,-0.00515], "fy":[0.003,0.01318,0.00338,0.003]}, + {"t":1.32663, "x":6.19525, "y":4.34551, "heading":3.14152, "vx":-0.78043, "vy":-0.62492, "omega":0.00021, "ax":0.00001, "ay":-0.00001, "alpha":-0.00051, "fx":[0.00303,-0.00878,0.00314,0.00314], "fy":[-0.00067,0.00034,0.00034,-0.00067]}, + {"t":1.37576, "x":6.1569, "y":4.31481, "heading":3.14153, "vx":-0.78043, "vy":-0.62492, "omega":0.00018, "ax":0.00081, "ay":-0.00101, "alpha":-0.00054, "fx":[0.01345,0.01345,0.01348,0.01348], "fy":[-0.01422,-0.02321,-0.01561,-0.01422]}, + {"t":1.4249, "x":6.11855, "y":4.2841, "heading":3.14154, "vx":-0.78039, "vy":-0.62497, "omega":0.00015, "ax":-0.0004, "ay":0.0005, "alpha":-0.00132, "fx":[-0.00499,-0.01912,-0.00128,-0.00128], "fy":[0.00936,0.0073,0.0073,0.00936]}, + {"t":1.47403, "x":6.08021, "y":4.2534, "heading":3.14154, "vx":-0.78041, "vy":-0.62494, "omega":0.00009, "ax":-0.00109, "ay":0.00137, "alpha":-0.01156, "fx":[-0.0209,-0.0209,-0.01556,-0.01556], "fy":[0.07641,-0.13077,0.06902,0.07641]}, + {"t":1.52316, "x":6.04186, "y":4.22269, "heading":3.14155, "vx":-0.78047, "vy":-0.62488, "omega":-0.00048, "ax":0.00102, "ay":-0.00128, "alpha":-0.00849, "fx":[0.0331,-0.06643,0.05075,0.05075], "fy":[-0.01362,-0.02894,-0.02895,-0.01362]}, + {"t":1.5723, "x":6.00352, "y":4.19199, "heading":3.14152, "vx":-0.78042, "vy":-0.62494, "omega":-0.0009, "ax":-0.00057, "ay":0.00075, "alpha":-0.02671, "fx":[-0.02595,-0.02577,0.00699,0.00696], "fy":[0.12615,-0.29304,0.09066,0.1261]}, + {"t":1.62143, "x":5.96517, "y":4.16128, "heading":3.14148, "vx":-0.78045, "vy":-0.6249, "omega":-0.00221, "ax":1.84124, "ay":1.47346, "alpha":0.02996, "fx":[30.43963,31.31486,30.50867,30.507], "fy":[24.6016,24.51021,24.53582,24.59969]}, + {"t":1.67057, "x":5.92905, "y":4.13236, "heading":3.14137, "vx":-0.68998, "vy":-0.5525, "omega":-0.00074, "ax":2.33866, "ay":1.87251, "alpha":0.0203, "fx":[38.95482,38.92257,39.03197,39.02827], "fy":[31.07096,31.72991,30.98715,31.06758]}, + {"t":1.7197, "x":5.89797, "y":4.10747, "heading":3.14134, "vx":-0.57507, "vy":-0.4605, "omega":0.00026, "ax":2.34011, "ay":1.87395, "alpha":0.01518, "fx":[38.80563,39.45235,38.88983,38.88597], "fy":[31.28664,31.17547,31.20696,31.28245]}, + {"t":1.76884, "x":5.87254, "y":4.08711, "heading":3.14135, "vx":-0.46009, "vy":-0.36842, "omega":0.00101, "ax":2.34066, "ay":1.87434, "alpha":0.0027, "fx":[38.98808,38.9717,39.05751,39.05353], "fy":[31.19525,31.47272,31.11804,31.19151]}, + {"t":1.81797, "x":5.85276, "y":4.07127, "heading":3.1414, "vx":-0.34508, "vy":-0.27633, "omega":0.00114, "ax":2.34093, "ay":1.87455, "alpha":0.00499, "fx":[38.89984,39.26115,38.96569,38.96224], "fy":[31.28363,31.20458,31.22296,31.27993]}, + {"t":1.86711, "x":5.83863, "y":4.05995, "heading":3.14145, "vx":-0.23006, "vy":-0.18423, "omega":0.00138, "ax":2.3411, "ay":1.87467, "alpha":0.00118, "fx":[39.0056,38.99476,39.05126,39.04817], "fy":[31.22074,31.39307,31.16761,31.21783]}, + {"t":1.91624, "x":5.83015, "y":4.05316, "heading":3.14152, "vx":-0.11503, "vy":-0.09211, "omega":0.00144, "ax":2.3412, "ay":1.87475, "alpha":-0.02936, "fx":[38.93747,38.952,38.9879,39.22961], "fy":[31.41476,31.19143,31.18895,31.20961]}, + {"t":1.96537, "x":5.82732, "y":4.0509, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/startToI.traj b/src/main/deploy/choreo/startToI.traj index 7365b35d..1011360e 100644 --- a/src/main/deploy/choreo/startToI.traj +++ b/src/main/deploy/choreo/startToI.traj @@ -3,7 +3,7 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.1, "y":5.07, "heading":3.14159, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":6.0711771011352536, "y":5.008563137054443, "heading":4.1887902047863905, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":5.422460748154253, "y":5.032141990263579, "heading":4.1887902047863905, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ @@ -34,65 +34,65 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.19898,2.01545], + "waypoints":[0.0,1.19775,2.01422], "samples":[ - {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.99157, "ay":-0.19852, "alpha":6.75068, "fx":[-34.40815,-34.65608,-65.36215,-65.04579], "fy":[-21.66772,14.7943,13.67404,-20.03773]}, - {"t":0.03331, "x":7.09923, "y":5.07557, "heading":3.14159, "vx":-0.09963, "vy":-0.00661, "omega":0.22483, "ax":-2.99239, "ay":-0.19858, "alpha":5.4416, "fx":[-37.39345,-37.64107,-62.3714,-62.1208], "fy":[-18.00402,11.17173,10.48597,-16.89444]}, - {"t":0.06661, "x":7.09425, "y":5.07524, "heading":-3.1341, "vx":-0.1993, "vy":-0.01323, "omega":0.40606, "ax":-2.99226, "ay":-0.19857, "alpha":4.3981, "fx":[-39.71238,-40.02492,-60.0624,-59.71827], "fy":[-15.01735,8.38905,7.81009,-14.4219]}, - {"t":0.09992, "x":7.08595, "y":5.07469, "heading":-3.12058, "vx":-0.29895, "vy":-0.01984, "omega":0.55254, "ax":-2.99209, "ay":-0.19856, "alpha":3.4355, "fx":[-41.85818,-42.40008,-57.86142,-57.38697], "fy":[-12.34216,5.96755,5.37456,-12.2394]}, - {"t":0.13322, "x":7.07434, "y":5.07392, "heading":-3.10218, "vx":-0.3986, "vy":-0.02645, "omega":0.66696, "ax":-2.99186, "ay":-0.19854, "alpha":2.74263, "fx":[-43.33003,-43.93862,-56.41076,-55.81205], "fy":[-10.30712,4.13592,3.46311,-10.53044]}, - {"t":0.16653, "x":7.0594, "y":5.07293, "heading":-3.07996, "vx":-0.49825, "vy":-0.03306, "omega":0.75831, "ax":-2.99154, "ay":-0.19852, "alpha":2.13069, "fx":[-44.58112,-45.34958,-55.11045,-54.42886], "fy":[-8.54176,2.50805,1.78291,-8.98626]}, - {"t":0.19983, "x":7.04115, "y":5.07171, "heading":-3.05471, "vx":-0.59788, "vy":-0.03968, "omega":0.82927, "ax":-2.99105, "ay":-0.19849, "alpha":1.62881, "fx":[-45.80702,-46.52305,-53.90434,-53.20293], "fy":[-7.22267,1.27943,0.52055,-7.81209]}, - {"t":0.23314, "x":7.01958, "y":5.07028, "heading":-3.02709, "vx":-0.6975, "vy":-0.04629, "omega":0.88352, "ax":-2.99021, "ay":-0.19843, "alpha":1.2439, "fx":[-46.70954,-47.19495,-53.08483,-52.39232], "fy":[-6.12795,0.18315,-0.53322,-6.75322]}, - {"t":0.26644, "x":6.99469, "y":5.06863, "heading":-2.99767, "vx":-0.79709, "vy":-0.0529, "omega":0.92494, "ax":-2.98847, "ay":-0.19832, "alpha":0.92644, "fx":[-47.4104,-48.05469,-52.21778,-51.58245], "fy":[-5.37241,-0.5531,-1.29979,-5.99848]}, - {"t":0.29975, "x":6.96648, "y":5.06676, "heading":-2.96686, "vx":-0.89662, "vy":-0.0595, "omega":0.9558, "ax":-2.98257, "ay":-0.19792, "alpha":0.74917, "fx":[-48.10937,-47.52091,-51.91452,-51.32747], "fy":[-4.75905,-1.24933,-1.84866,-5.34022]}, - {"t":0.33305, "x":6.93497, "y":5.06467, "heading":-2.93503, "vx":-0.99595, "vy":-0.06609, "omega":0.98075, "ax":-0.04652, "ay":-0.003, "alpha":0.58455, "fx":[0.94717,0.34897,-2.49758,-1.90043], "fy":[-1.12274,1.49341,1.15321,-1.7242]}, - {"t":0.36636, "x":6.90177, "y":5.06247, "heading":-2.90236, "vx":-0.9975, "vy":-0.06619, "omega":1.00022, "ax":0.00001, "ay":-0.00022, "alpha":0.38404, "fx":[1.99461,-0.93414,-0.80442,-0.25517], "fy":[-0.84758,1.38581,0.84044,-1.39304]}, - {"t":0.39966, "x":6.86855, "y":5.06026, "heading":-2.86905, "vx":-0.9975, "vy":-0.0662, "omega":1.01301, "ax":-0.00001, "ay":0.00012, "alpha":0.36487, "fx":[1.09849,0.61791,-1.09875,-0.61818], "fy":[-0.61078,1.0779,0.63615,-1.09528]}, - {"t":0.43297, "x":6.83533, "y":5.05806, "heading":-2.83531, "vx":-0.9975, "vy":-0.0662, "omega":1.02516, "ax":0.0, "ay":0.00001, "alpha":0.28583, "fx":[0.82791,0.55256,-0.89775,-0.48276], "fy":[-0.44413,0.85618,0.44434,-0.85596]}, - {"t":0.46627, "x":6.80211, "y":5.05585, "heading":-2.80117, "vx":-0.9975, "vy":-0.0662, "omega":1.03468, "ax":0.0, "ay":-0.00002, "alpha":0.21506, "fx":[0.65499,0.312,-0.65496,-0.31196], "fy":[-0.32842,0.70098,0.30033,-0.67396]}, - {"t":0.49958, "x":6.76888, "y":5.05365, "heading":-2.76671, "vx":-0.9975, "vy":-0.0662, "omega":1.04184, "ax":0.0, "ay":0.00004, "alpha":0.15768, "fx":[0.45799,0.30373,-0.51901,-0.24287], "fy":[-0.21033,0.48585,0.21155,-0.48463]}, - {"t":0.53288, "x":6.73566, "y":5.05144, "heading":-2.73201, "vx":-0.9975, "vy":-0.06619, "omega":1.0471, "ax":0.00001, "ay":-0.00009, "alpha":0.10837, "fx":[0.3387,0.13352,-0.33851,-0.13333], "fy":[-0.14455,0.36603,0.12341,-0.35062]}, - {"t":0.56619, "x":6.70244, "y":5.04924, "heading":-2.69714, "vx":-0.9975, "vy":-0.0662, "omega":1.0507, "ax":-0.00001, "ay":0.00011, "alpha":0.06473, "fx":[0.19659,0.10334,-0.217,-0.08343], "fy":[-0.07148,0.20905,0.07528,-0.20525]}, - {"t":0.59949, "x":6.66922, "y":5.04703, "heading":-2.66214, "vx":-0.9975, "vy":-0.06619, "omega":1.05286, "ax":0.00001, "ay":-0.00012, "alpha":0.02576, "fx":[0.08386,0.02651,-0.0836,-0.02625], "fy":[-0.02981,0.086,0.02152,-0.08575]}, - {"t":0.6328, "x":6.636, "y":5.04483, "heading":-2.62708, "vx":-0.9975, "vy":-0.0662, "omega":1.05372, "ax":-0.00001, "ay":0.00008, "alpha":-0.01196, "fx":[-0.04486,-0.00442,0.03969,0.00925], "fy":[0.01197,-0.03707,-0.0094,0.03964]}, - {"t":0.6661, "x":6.60278, "y":5.04262, "heading":-2.59198, "vx":-0.9975, "vy":-0.0662, "omega":1.05332, "ax":0.0, "ay":-0.00003, "alpha":-0.04986, "fx":[-0.16585,-0.03984,0.16592,0.03991], "fy":[0.03889,-0.16642,-0.04374,0.16917]}, - {"t":0.69941, "x":6.56955, "y":5.04042, "heading":-2.5569, "vx":-0.9975, "vy":-0.0662, "omega":1.05166, "ax":0.00001, "ay":-0.0001, "alpha":-0.09175, "fx":[-0.31381,-0.06255,0.31321,0.06358], "fy":[0.06065,-0.30754,-0.06387,0.30432]}, - {"t":0.73271, "x":6.53633, "y":5.03821, "heading":-2.52188, "vx":-0.9975, "vy":-0.0662, "omega":1.0486, "ax":-0.00002, "ay":0.00028, "alpha":-0.13697, "fx":[-0.46684,-0.07836,0.46622,0.07774], "fy":[0.07825,-0.45279,-0.08122,0.47453]}, - {"t":0.76602, "x":6.50311, "y":5.03601, "heading":-2.48695, "vx":-0.9975, "vy":-0.06619, "omega":1.04404, "ax":0.00004, "ay":-0.00058, "alpha":-0.19397, "fx":[-0.66714,-0.08821,0.66895,0.08895], "fy":[0.07679,-0.66613,-0.09603,0.6469]}, - {"t":0.79932, "x":6.46989, "y":5.0338, "heading":-2.45218, "vx":-0.9975, "vy":-0.06621, "omega":1.03758, "ax":-0.00007, "ay":0.00103, "alpha":-0.25587, "fx":[-0.88815,-0.08668,0.88587,0.0844], "fy":[0.08731,-0.83289,-0.0867,0.901]}, - {"t":0.83263, "x":6.43667, "y":5.0316, "heading":-2.41762, "vx":-0.9975, "vy":-0.06618, "omega":1.02906, "ax":0.00011, "ay":-0.00162, "alpha":-0.34279, "fx":[-1.18364,-0.07417,1.18894,0.07602], "fy":[0.04526,-1.19721,-0.09917,1.1433]}, - {"t":0.86593, "x":6.40344, "y":5.0294, "heading":-2.38335, "vx":-0.9975, "vy":-0.06623, "omega":1.01764, "ax":-0.00016, "ay":0.00247, "alpha":-0.43447, "fx":[-1.52575,-0.04447,1.52028,0.03901], "fy":[0.04707,-1.3907,-0.03782,1.5462]}, - {"t":0.89924, "x":6.37022, "y":5.02719, "heading":-2.34946, "vx":-0.99751, "vy":-0.06615, "omega":1.00317, "ax":0.00023, "ay":-0.00349, "alpha":-0.57315, "fx":[-1.97872,0.01038,1.99047,-0.0067], "fy":[-0.07077,-2.02132,-0.04543,1.90508]}, - {"t":0.93254, "x":6.337, "y":5.02499, "heading":-2.31605, "vx":-0.9975, "vy":-0.06626, "omega":0.98408, "ax":-0.0003, "ay":0.00454, "alpha":-0.71476, "fx":[-2.52193,0.09529,2.51189,-0.10534], "fy":[-0.09491,-2.25702,0.10716,2.54747]}, - {"t":0.96585, "x":6.30378, "y":5.02278, "heading":-2.28327, "vx":-0.99751, "vy":-0.06611, "omega":0.96028, "ax":0.0004, "ay":-0.006, "alpha":-0.93884, "fx":[-3.2282,0.22891,3.25011,-0.22426], "fy":[-0.33346,-3.31129,0.13354,3.1111]}, - {"t":0.99915, "x":6.27056, "y":5.02058, "heading":-2.25129, "vx":-0.9975, "vy":-0.06631, "omega":0.92901, "ax":-0.00026, "ay":0.00397, "alpha":-1.17055, "fx":[-4.09108,0.42453,4.08227,-0.43327], "fy":[-0.45994,-3.76968,0.40385,4.09019]}, - {"t":1.03246, "x":6.23733, "y":5.01837, "heading":-2.22035, "vx":-0.9975, "vy":-0.06618, "omega":0.89003, "ax":0.00027, "ay":-0.00411, "alpha":-1.52489, "fx":[-5.21297,0.70161,5.2316,-0.70203], "fy":[-0.7751,-5.25523,0.63909,5.1171]}, - {"t":1.06576, "x":6.20411, "y":5.01616, "heading":-2.19071, "vx":-0.9975, "vy":-0.06632, "omega":0.83924, "ax":0.0014, "ay":-0.02089, "alpha":-1.9274, "fx":[-6.54926,1.11932,6.59496,-1.07193], "fy":[-1.49439,-6.82458,0.70938,6.2167]}, - {"t":1.09907, "x":6.17089, "y":5.01394, "heading":-2.16276, "vx":-0.99745, "vy":-0.06701, "omega":0.77505, "ax":-0.00403, "ay":0.06098, "alpha":-2.46829, "fx":[-8.46914,1.68892,8.25099,-1.73976], "fy":[-0.60375,-7.29946,2.64782,9.3211]}, - {"t":1.13237, "x":6.13767, "y":5.01175, "heading":-2.13694, "vx":-0.99758, "vy":-0.06498, "omega":0.69284, "ax":-0.01203, "ay":0.1944, "alpha":-3.24607, "fx":[-10.7095,2.14565,10.2895,-2.5276], "fy":[1.34609,-8.5713,6.02768,14.15954]}, - {"t":1.16568, "x":6.10444, "y":5.00969, "heading":-2.11387, "vx":-0.99798, "vy":-0.05851, "omega":0.58473, "ax":-0.04288, "ay":1.48359, "alpha":-3.92989, "fx":[-13.99111,2.53226,12.5389,-3.9393], "fy":[21.59152,11.64112,27.99391,37.69619]}, - {"t":1.19898, "x":6.07118, "y":5.00856, "heading":-2.0944, "vx":-0.99941, "vy":-0.0091, "omega":0.45384, "ax":0.00435, "ay":1.01036, "alpha":-3.50992, "fx":[-12.06686,3.32148,12.19719,-3.16201], "fy":[13.10496,6.16096,19.63521,28.46796]}, - {"t":1.2398, "x":6.03038, "y":5.00903, "heading":-2.07587, "vx":-0.99923, "vy":0.03215, "omega":0.31056, "ax":0.00489, "ay":0.13963, "alpha":-2.80642, "fx":[-8.82251,2.87645,9.3196,-3.04747], "fy":[-2.31726,-6.0487,5.6289,12.04736]}, - {"t":1.28063, "x":5.98959, "y":5.01046, "heading":-2.06319, "vx":-0.99903, "vy":0.03785, "omega":0.19599, "ax":0.00033, "ay":0.00875, "alpha":-2.03143, "fx":[-6.73441,2.03964,6.74233,-2.02535], "fy":[-1.96432,-6.43594,2.12603,6.85765]}, - {"t":1.32145, "x":5.94881, "y":5.01202, "heading":-2.05519, "vx":-0.99902, "vy":0.03821, "omega":0.11306, "ax":0.00001, "ay":0.00032, "alpha":-1.53323, "fx":[-5.07435,1.56019,5.05236,-1.53738], "fy":[-1.38703,-5.11733,1.51698,5.0088]}, - {"t":1.36227, "x":5.90803, "y":5.01358, "heading":-2.05057, "vx":-0.99902, "vy":0.03822, "omega":0.05047, "ax":-0.00001, "ay":-0.00017, "alpha":-1.13461, "fx":[-3.74882,1.1812,3.74834,-1.18117], "fy":[-1.23428,-3.6603,1.14754,3.73548]}, - {"t":1.4031, "x":5.86724, "y":5.01514, "heading":-2.04851, "vx":-0.99902, "vy":0.03821, "omega":0.00415, "ax":-0.00001, "ay":-0.00016, "alpha":-0.8494, "fx":[-2.80895,0.89902,2.82058,-0.91106], "fy":[-0.6032,-2.91486,0.79299,2.71453]}, - {"t":1.44392, "x":5.82646, "y":5.0167, "heading":-2.04834, "vx":-0.99902, "vy":0.03821, "omega":-0.03052, "ax":0.0, "ay":-0.0001, "alpha":-0.63278, "fx":[-2.06462,0.65524,2.06448,-0.65535], "fy":[-0.66246,-2.09328,0.66249,2.08686]}, - {"t":1.48474, "x":5.78568, "y":5.01825, "heading":-2.04959, "vx":-0.99902, "vy":0.0382, "omega":-0.05636, "ax":0.0, "ay":-0.00008, "alpha":-0.46617, "fx":[-1.4933,0.52826,1.57885,-0.61403], "fy":[-0.30366,-1.59848,0.42307,1.47369]}, - {"t":1.52557, "x":5.74489, "y":5.01981, "heading":-2.05189, "vx":-0.99902, "vy":0.0382, "omega":-0.07539, "ax":0.0, "ay":-0.00006, "alpha":-0.31438, "fx":[-1.09644,0.34367,1.09637,-0.34374], "fy":[-0.42932,-0.87748,0.26919,1.03392]}, - {"t":1.56639, "x":5.70411, "y":5.02137, "heading":-2.05497, "vx":-0.99902, "vy":0.0382, "omega":-0.08822, "ax":0.0, "ay":-0.00001, "alpha":-0.2398, "fx":[-0.87293,0.14206,0.67648,0.05436], "fy":[-0.6829,-0.62764,0.38768,0.92233]}, - {"t":1.60721, "x":5.66333, "y":5.02293, "heading":-2.05857, "vx":-0.99902, "vy":0.0382, "omega":-0.09801, "ax":0.00001, "ay":-0.00003, "alpha":-0.1152, "fx":[-0.49,0.15025,0.49029,-0.14996], "fy":[-0.28667,-0.11598,0.02417,0.37619]}, - {"t":1.64804, "x":5.62254, "y":5.02449, "heading":-2.06257, "vx":-0.99902, "vy":0.0382, "omega":-0.10271, "ax":0.52266, "ay":-0.01999, "alpha":-0.10022, "fx":[8.59384,8.92721,9.10584,8.22311], "fy":[-0.79259,-0.46422,-0.12801,0.05223]}, - {"t":1.68886, "x":5.5822, "y":5.02603, "heading":-2.06676, "vx":-0.97768, "vy":0.03738, "omega":-0.1068, "ax":2.98553, "ay":-0.11416, "alpha":-0.00133, "fx":[49.7471,49.7737,49.78728,49.76137], "fy":[-1.93624,-1.8642,-1.91702,-1.8947]}, - {"t":1.72968, "x":5.54477, "y":5.02747, "heading":-2.07112, "vx":-0.85581, "vy":0.03272, "omega":-0.10686, "ax":2.99169, "ay":-0.11439, "alpha":0.04968, "fx":[50.16987,49.93336,49.80249,49.57459], "fy":[-1.83341,-1.70668,-1.96951,-2.11768]}, - {"t":1.77051, "x":5.51233, "y":5.02871, "heading":-2.07549, "vx":-0.73367, "vy":0.02805, "omega":-0.10483, "ax":2.99374, "ay":-0.11446, "alpha":0.13444, "fx":[50.32561,49.78506,49.4824,50.02362], "fy":[-1.7735,-1.4377,-2.04596,-2.3747]}, - {"t":1.81133, "x":5.48487, "y":5.02976, "heading":-2.07976, "vx":-0.61146, "vy":0.02338, "omega":-0.09934, "ax":2.99476, "ay":-0.1145, "alpha":0.221, "fx":[50.62397,49.73501,49.23153,50.09422], "fy":[-1.69274,-1.1328,-2.12954,-2.67923]}, - {"t":1.85215, "x":5.46241, "y":5.03061, "heading":-2.08382, "vx":-0.48921, "vy":0.0187, "omega":-0.09032, "ax":2.99537, "ay":-0.11452, "alpha":0.32576, "fx":[50.95187,49.65269,48.90888,50.21207], "fy":[-1.58235,-0.77136,-2.23239,-3.0499]}, - {"t":1.89298, "x":5.44493, "y":5.03128, "heading":-2.08751, "vx":-0.36692, "vy":0.01403, "omega":-0.07702, "ax":2.99578, "ay":-0.11454, "alpha":0.45667, "fx":[51.36597,49.54854,48.49711,50.34106], "fy":[-1.47722,-0.3044,-2.35445,-3.50094]}, - {"t":1.9338, "x":5.43245, "y":5.03176, "heading":-2.09065, "vx":-0.24463, "vy":0.00935, "omega":-0.05838, "ax":2.99607, "ay":-0.11455, "alpha":0.61381, "fx":[51.86037,49.435,48.01833,50.45837], "fy":[-1.31076,0.24938,-2.50886,-4.06741]}, - {"t":1.97462, "x":5.42496, "y":5.03205, "heading":-2.09303, "vx":-0.12232, "vy":0.00468, "omega":-0.03332, "ax":2.99629, "ay":-0.11455, "alpha":0.8162, "fx":[52.50363,49.29364,47.3642,50.62515], "fy":[-1.12937,0.95764,-2.70064,-4.76577]}, - {"t":2.01545, "x":5.42246, "y":5.03214, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.1, "y":5.07, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.99262, "ay":-0.18207, "alpha":6.75951, "fx":[-34.41578,-34.64635,-65.38543,-65.09469], "fy":[-21.40976,15.10543,13.96035,-19.7959]}, + {"t":0.03327, "x":7.09834, "y":5.0699, "heading":3.14159, "vx":-0.09957, "vy":-0.00606, "omega":0.2249, "ax":-2.99344, "ay":-0.18212, "alpha":5.44995, "fx":[-37.40068,-37.63337,-62.39652,-62.16616], "fy":[-17.74574,11.47912,10.77323,-16.64981]}, + {"t":0.06654, "x":7.09337, "y":5.0696, "heading":-3.13411, "vx":-0.19916, "vy":-0.01212, "omega":0.40622, "ax":-2.99331, "ay":-0.18211, "alpha":4.406, "fx":[-39.71902,-40.01918,-60.08898,-59.76089], "fy":[-14.75853,8.69322,8.09753,-14.17488]}, + {"t":0.09981, "x":7.08509, "y":5.06909, "heading":-3.1206, "vx":-0.29875, "vy":-0.01818, "omega":0.55281, "ax":-2.99314, "ay":-0.1821, "alpha":3.44246, "fx":[-41.86528,-42.3984,-57.88798,-57.42523], "fy":[-12.08212,6.26714,5.66178,-11.98883]}, + {"t":0.13308, "x":7.07349, "y":5.06839, "heading":-3.1022, "vx":-0.39834, "vy":-0.02423, "omega":0.66735, "ax":-2.99292, "ay":-0.18209, "alpha":2.74905, "fx":[-43.33699,-43.93829,-56.43826,-55.84837], "fy":[-10.04544,4.43226,3.74937,-10.27735]}, + {"t":0.16635, "x":7.05859, "y":5.06748, "heading":-3.08, "vx":-0.49791, "vy":-0.03029, "omega":0.75881, "ax":-2.9926, "ay":-0.18207, "alpha":2.13636, "fx":[-44.58775,-45.35268,-55.13804,-54.46235], "fy":[-8.27837,2.80161,2.06743,-8.73055]}, + {"t":0.19963, "x":7.04036, "y":5.06637, "heading":-3.05476, "vx":-0.59748, "vy":-0.03635, "omega":0.82989, "ax":-2.99212, "ay":-0.18204, "alpha":1.63369, "fx":[-45.81545,-46.52822,-53.93139,-53.23384], "fy":[-6.95789,1.56961,0.80407,-7.55375]}, + {"t":0.2329, "x":7.01883, "y":5.06506, "heading":-3.02714, "vx":-0.69703, "vy":-0.04241, "omega":0.88424, "ax":-2.99131, "ay":-0.18199, "alpha":1.248, "fx":[-46.71836,-47.20449,-53.11128,-52.42076], "fy":[-5.86158,0.47125,-0.25175,-6.49262]}, + {"t":0.26617, "x":6.99398, "y":5.06355, "heading":-2.99772, "vx":-0.79655, "vy":-0.04846, "omega":0.92576, "ax":-2.98965, "ay":-0.18189, "alpha":0.92955, "fx":[-47.42285,-48.06633,-52.24453,-51.61015], "fy":[-5.1038,-0.27108,-1.01892,-5.73459]}, + {"t":0.29944, "x":6.96582, "y":5.06184, "heading":-2.96692, "vx":-0.89602, "vy":-0.05451, "omega":0.95669, "ax":-2.98427, "ay":-0.18156, "alpha":0.75099, "fx":[-48.12843,-47.5559,-51.94401,-51.35715], "fy":[-4.49095,-0.96715,-1.57215,-5.07585]}, + {"t":0.33271, "x":6.93436, "y":5.05992, "heading":-2.93509, "vx":-0.99531, "vy":-0.06055, "omega":0.98168, "ax":-0.07632, "ay":-0.00446, "alpha":0.58824, "fx":[0.45543,-0.14436,-2.99938,-2.40069], "fy":[-1.15836,1.49742,1.12513,-1.7615]}, + {"t":0.36598, "x":6.9012, "y":5.05791, "heading":-2.90243, "vx":-0.99785, "vy":-0.0607, "omega":1.00125, "ax":0.00002, "ay":-0.00038, "alpha":0.38431, "fx":[2.00901,-0.95886,-0.79988,-0.24883], "fy":[-0.85346,1.38787,0.84073,-1.4007]}, + {"t":0.39925, "x":6.868, "y":5.05589, "heading":-2.86912, "vx":-0.99785, "vy":-0.06072, "omega":1.01404, "ax":-0.00001, "ay":0.00023, "alpha":0.36645, "fx":[1.10259,0.62027,-1.10305,-0.62073], "fy":[-0.61207,1.08576,0.63985,-1.09833]}, + {"t":0.43252, "x":6.83481, "y":5.05387, "heading":-2.83538, "vx":-0.99785, "vy":-0.06071, "omega":1.02623, "ax":0.0, "ay":-0.00005, "alpha":0.28699, "fx":[0.83202,0.5536,-0.90106,-0.48436], "fy":[-0.44695,0.85888,0.44539,-0.86043]}, + {"t":0.46579, "x":6.80161, "y":5.05185, "heading":-2.80124, "vx":-0.99785, "vy":-0.06071, "omega":1.03578, "ax":0.0, "ay":0.00001, "alpha":0.21574, "fx":[0.65797,0.31345,-0.658,-0.31348], "fy":[-0.32823,0.70094,0.30363,-0.67538]}, + {"t":0.49906, "x":6.76841, "y":5.04983, "heading":-2.76678, "vx":-0.99785, "vy":-0.06071, "omega":1.04295, "ax":0.0, "ay":0.00003, "alpha":0.15851, "fx":[0.46028,0.30579,-0.52187,-0.24432], "fy":[-0.21154,0.48815,0.21252,-0.48717]}, + {"t":0.53233, "x":6.73521, "y":5.04781, "heading":-2.73208, "vx":-0.99785, "vy":-0.06071, "omega":1.04823, "ax":0.00001, "ay":-0.00009, "alpha":0.10878, "fx":[0.34057,0.13428,-0.34038,-0.13409], "fy":[-0.14477,0.36576,0.12485,-0.35206]}, + {"t":0.56561, "x":6.70201, "y":5.04579, "heading":-2.6972, "vx":-0.99785, "vy":-0.06071, "omega":1.05185, "ax":-0.00001, "ay":0.00013, "alpha":0.06519, "fx":[0.19795,0.10434,-0.21868,-0.08415], "fy":[-0.07167,0.21067,0.07606,-0.20628]}, + {"t":0.59888, "x":6.66881, "y":5.04377, "heading":-2.66221, "vx":-0.99785, "vy":-0.06071, "omega":1.05402, "ax":0.00001, "ay":-0.00014, "alpha":0.02596, "fx":[0.08469,0.02679,-0.0844,-0.0265], "fy":[-0.03027,0.08584,0.02172,-0.08691]}, + {"t":0.63215, "x":6.63561, "y":5.04175, "heading":-2.62714, "vx":-0.99785, "vy":-0.06071, "omega":1.05488, "ax":-0.00001, "ay":0.0001, "alpha":-0.01182, "fx":[-0.04438,-0.00411,0.03907,0.00902], "fy":[0.01225,-0.03637,-0.00894,0.03968]}, + {"t":0.66542, "x":6.60241, "y":5.03973, "heading":-2.59204, "vx":-0.99785, "vy":-0.06071, "omega":1.05449, "ax":0.0, "ay":-0.00006, "alpha":-0.04992, "fx":[-0.16596,-0.03985,0.16608,0.03997], "fy":[0.03864,-0.16733,-0.04395,0.16883]}, + {"t":0.69869, "x":6.56921, "y":5.03771, "heading":-2.55696, "vx":-0.99785, "vy":-0.06071, "omega":1.05283, "ax":0.0, "ay":-0.00008, "alpha":-0.09189, "fx":[-0.3142,-0.06256,0.31348,0.0636], "fy":[0.06108,-0.30784,-0.0637,0.30521]}, + {"t":0.73196, "x":6.53601, "y":5.03569, "heading":-2.52193, "vx":-0.99785, "vy":-0.06071, "omega":1.04977, "ax":-0.00002, "ay":0.00026, "alpha":-0.13733, "fx":[-0.46784,-0.0785,0.4673,0.07797], "fy":[0.0784,-0.45482,-0.08134,0.4753]}, + {"t":0.76523, "x":6.50281, "y":5.03367, "heading":-2.487, "vx":-0.99785, "vy":-0.0607, "omega":1.0452, "ax":0.00003, "ay":-0.00057, "alpha":-0.19436, "fx":[-0.66844,-0.08831,0.67004,0.08903], "fy":[0.07707,-0.66757,-0.09619,0.64845]}, + {"t":0.7985, "x":6.46961, "y":5.03165, "heading":-2.45223, "vx":-0.99785, "vy":-0.06072, "omega":1.03873, "ax":-0.00006, "ay":0.00103, "alpha":-0.25663, "fx":[-0.89001,-0.0868,0.88791,0.08471], "fy":[0.08837,-0.83693,-0.08599,0.90352]}, + {"t":0.83177, "x":6.43641, "y":5.02963, "heading":-2.41767, "vx":-0.99785, "vy":-0.06069, "omega":1.03019, "ax":0.0001, "ay":-0.00165, "alpha":-0.34346, "fx":[-1.18596,-0.07417,1.19085,0.07597], "fy":[0.04492,-1.20018,-0.09987,1.14523]}, + {"t":0.86504, "x":6.40321, "y":5.02761, "heading":-2.38339, "vx":-0.99785, "vy":-0.06074, "omega":1.01877, "ax":-0.00015, "ay":0.00252, "alpha":-0.43583, "fx":[-1.52851,-0.04441,1.52341,0.0393], "fy":[0.04984,-1.39826,-0.03525,1.55148]}, + {"t":0.89832, "x":6.37001, "y":5.02559, "heading":-2.3495, "vx":-0.99785, "vy":-0.06066, "omega":1.00427, "ax":0.00022, "ay":-0.00358, "alpha":-0.57413, "fx":[-1.98232,0.01064,1.99328,-0.00709], "fy":[-0.07226,-2.02638,-0.04696,1.90712]}, + {"t":0.93159, "x":6.33681, "y":5.02357, "heading":-2.31608, "vx":-0.99785, "vy":-0.06078, "omega":0.98516, "ax":-0.00028, "ay":0.00461, "alpha":-0.71692, "fx":[-2.52576,0.09576,2.5164,-0.10512], "fy":[-0.09035,-2.26987,0.11194,2.55597]}, + {"t":0.96486, "x":6.30361, "y":5.02155, "heading":-2.28331, "vx":-0.99786, "vy":-0.06062, "omega":0.96131, "ax":0.00037, "ay":-0.00614, "alpha":-0.94024, "fx":[-3.23348,0.22953,3.25406,-0.22521], "fy":[-0.33605,-3.31853,0.13157,3.11379]}, + {"t":0.99813, "x":6.27041, "y":5.01953, "heading":-2.25132, "vx":-0.99784, "vy":-0.06083, "omega":0.93003, "ax":-0.00023, "ay":0.00371, "alpha":-1.17383, "fx":[-4.09625,0.42569,4.08867,-0.4332], "fy":[-0.45895,-3.7964,0.40587,4.09715]}, + {"t":1.0314, "x":6.23722, "y":5.01751, "heading":-2.22038, "vx":-0.99785, "vy":-0.06071, "omega":0.89097, "ax":0.00023, "ay":-0.00372, "alpha":-1.52686, "fx":[-5.22039,0.70198,5.23726,-0.70375], "fy":[-0.76947,-5.25558,0.64645,5.13055]}, + {"t":1.06467, "x":6.20402, "y":5.01548, "heading":-2.19074, "vx":-0.99784, "vy":-0.06083, "omega":0.84017, "ax":0.00141, "ay":-0.02292, "alpha":-1.93096, "fx":[-6.55705,1.1207,6.60309,-1.07302], "fy":[-1.52504,-6.87909,0.68111,6.19509]}, + {"t":1.09794, "x":6.17082, "y":5.01345, "heading":-2.16278, "vx":-0.9978, "vy":-0.06159, "omega":0.77593, "ax":-0.00392, "ay":0.06465, "alpha":-2.47101, "fx":[-8.47499,1.68882,8.26319,-1.73845], "fy":[-0.54451,-7.24768,2.71058,9.39254]}, + {"t":1.13121, "x":6.13762, "y":5.01143, "heading":-2.13697, "vx":-0.99793, "vy":-0.05944, "omega":0.69372, "ax":-0.01071, "ay":0.19004, "alpha":-3.2425, "fx":[-10.69885,2.16984,10.32345,-2.50877], "fy":[1.24267,-8.57808,5.93021,14.07649]}, + {"t":1.16448, "x":6.10441, "y":5.00956, "heading":-2.11389, "vx":-0.99828, "vy":-0.05312, "omega":0.58584, "ax":-0.0344, "ay":1.38871, "alpha":-3.93799, "fx":[-13.86711,2.68072,12.69746,-3.80481], "fy":[19.9979,10.02644,26.41782,36.15407]}, + {"t":1.19775, "x":6.07118, "y":5.00856, "heading":-2.0944, "vx":-0.99943, "vy":-0.00691, "omega":0.45482, "ax":0.00497, "ay":0.96319, "alpha":-3.52427, "fx":[-12.07641,3.33702,12.22893,-3.15847], "fy":[12.33781,5.27088,18.8806,27.73463]}, + {"t":1.23858, "x":6.03038, "y":5.00908, "heading":-2.07583, "vx":-0.99923, "vy":0.03241, "omega":0.31094, "ax":0.00462, "ay":0.13166, "alpha":-2.80789, "fx":[-8.84261,2.87465,9.32894,-3.05269], "fy":[-2.33291,-6.23937,5.45977,11.8914]}, + {"t":1.2794, "x":5.98959, "y":5.01052, "heading":-2.06313, "vx":-0.99904, "vy":0.03778, "omega":0.19632, "ax":0.00031, "ay":0.00822, "alpha":-2.03562, "fx":[-6.74421,2.0425,6.75159,-2.02906], "fy":[-1.97193,-6.46783,2.12496,6.86292]}, + {"t":1.32022, "x":5.94881, "y":5.01207, "heading":-2.05512, "vx":-0.99902, "vy":0.03812, "omega":0.11322, "ax":0.00001, "ay":0.00027, "alpha":-1.53543, "fx":[-5.08072,1.56235,5.05841,-1.53934], "fy":[-1.40029,-5.12096,1.5219,5.01764]}, + {"t":1.36105, "x":5.90803, "y":5.01362, "heading":-2.0505, "vx":-0.99902, "vy":0.03813, "omega":0.05054, "ax":-0.00001, "ay":-0.00019, "alpha":-1.13671, "fx":[-3.75322,1.18292,3.75273,-1.18292], "fy":[-1.23349,-3.67316,1.15166,3.74211]}, + {"t":1.40187, "x":5.86724, "y":5.01518, "heading":-2.04844, "vx":-0.99902, "vy":0.03812, "omega":0.00413, "ax":-0.00001, "ay":-0.00017, "alpha":-0.85069, "fx":[-2.81158,0.90036,2.82356,-0.91278], "fy":[-0.61823,-2.91338,0.79845,2.72162]}, + {"t":1.44269, "x":5.82646, "y":5.01673, "heading":-2.04827, "vx":-0.99902, "vy":0.03811, "omega":-0.0306, "ax":0.0, "ay":-0.00011, "alpha":-0.63357, "fx":[-2.06681,0.65613,2.06666,-0.65625], "fy":[-0.663,-2.09696,0.6636,2.08908]}, + {"t":1.48352, "x":5.78568, "y":5.01829, "heading":-2.04952, "vx":-0.99902, "vy":0.03811, "omega":-0.05646, "ax":0.0, "ay":-0.00009, "alpha":-0.46682, "fx":[-1.49449,0.52891,1.58025,-0.6149], "fy":[-0.31267,-1.59719,0.42621,1.47757]}, + {"t":1.52434, "x":5.74489, "y":5.01985, "heading":-2.05182, "vx":-0.99903, "vy":0.0381, "omega":-0.07552, "ax":0.0, "ay":-0.00006, "alpha":-0.31499, "fx":[-1.09754,0.34411,1.09747,-0.34419], "fy":[-0.42875,-0.88171,0.2705,1.03574]}, + {"t":1.56516, "x":5.70411, "y":5.0214, "heading":-2.0549, "vx":-0.99903, "vy":0.0381, "omega":-0.08838, "ax":0.0, "ay":-0.00001, "alpha":-0.24014, "fx":[-0.87304,0.1429,0.67771,0.05239], "fy":[-0.68608,-0.62747,0.3888,0.92384]}, + {"t":1.60598, "x":5.66333, "y":5.02296, "heading":-2.05851, "vx":-0.99903, "vy":0.0381, "omega":-0.09818, "ax":0.00001, "ay":-0.00004, "alpha":-0.11516, "fx":[-0.49072,0.1505,0.491,-0.15021], "fy":[-0.28776,-0.11415,0.0235,0.37587]}, + {"t":1.64681, "x":5.62254, "y":5.02451, "heading":-2.06252, "vx":-0.99902, "vy":0.0381, "omega":-0.10288, "ax":0.52272, "ay":-0.01994, "alpha":-0.10024, "fx":[8.59396,8.92786,9.10675,8.22569], "fy":[-0.79015,-0.46453,-0.12778,0.05274]}, + {"t":1.68763, "x":5.5822, "y":5.02605, "heading":-2.06672, "vx":-0.97769, "vy":0.03729, "omega":-0.10697, "ax":2.98554, "ay":-0.11388, "alpha":-0.00131, "fx":[49.74702,49.77397,49.78772,49.76147], "fy":[-1.93184,-1.85875,-1.91249,-1.89003]}, + {"t":1.72845, "x":5.54477, "y":5.02748, "heading":-2.07109, "vx":-0.85581, "vy":0.03264, "omega":-0.10703, "ax":2.9917, "ay":-0.1141, "alpha":0.04978, "fx":[50.16944,49.9328,49.80187,49.57693], "fy":[-1.82783,-1.70208,-1.96503,-2.11319]}, + {"t":1.76928, "x":5.51233, "y":5.02871, "heading":-2.07546, "vx":-0.73368, "vy":0.02798, "omega":-0.105, "ax":2.99375, "ay":-0.11417, "alpha":0.13463, "fx":[50.32623,49.7851,49.48214,50.02395], "fy":[-1.76871,-1.43194,-2.04145,-2.37053]}, + {"t":1.8101, "x":5.48487, "y":5.02976, "heading":-2.07974, "vx":-0.61146, "vy":0.02332, "omega":-0.0995, "ax":2.99477, "ay":-0.11421, "alpha":0.22134, "fx":[50.62468,49.73451,49.23041,50.09587], "fy":[-1.68698,-1.12725,-2.12523,-2.67559]}, + {"t":1.85092, "x":5.46241, "y":5.03062, "heading":-2.0838, "vx":-0.48921, "vy":0.01866, "omega":-0.09046, "ax":2.99538, "ay":-0.11423, "alpha":0.32628, "fx":[50.95352,49.65243,48.90759,50.21271], "fy":[-1.57733,-0.76427,-2.22823,-3.04688]}, + {"t":1.89175, "x":5.44493, "y":5.03129, "heading":-2.0875, "vx":-0.36692, "vy":0.01399, "omega":-0.07714, "ax":2.99579, "ay":-0.11425, "alpha":0.45739, "fx":[51.36806,49.5478,48.49485,50.34271], "fy":[-1.47095,-0.29751,-2.35053,-3.49872]}, + {"t":1.93257, "x":5.43245, "y":5.03176, "heading":-2.09065, "vx":-0.24463, "vy":0.00933, "omega":-0.05847, "ax":2.99608, "ay":-0.11426, "alpha":0.61482, "fx":[51.86357,49.43431,48.01548,50.45945], "fy":[-1.30529,0.25814,-2.5051,-4.06608]}, + {"t":1.97339, "x":5.42496, "y":5.03205, "heading":-2.09303, "vx":-0.12232, "vy":0.00466, "omega":-0.03337, "ax":2.9963, "ay":-0.11426, "alpha":0.81749, "fx":[52.5077,49.29328,47.36005,50.62632], "fy":[-1.12338,0.96705,-2.69699,-4.7655]}, + {"t":2.01422, "x":5.42246, "y":5.03214, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/startToJSlow.traj b/src/main/deploy/choreo/startToJSlow.traj index af5f88bf..7390bf3a 100644 --- a/src/main/deploy/choreo/startToJSlow.traj +++ b/src/main/deploy/choreo/startToJSlow.traj @@ -18,7 +18,7 @@ "params":{ "waypoints":[ {"x":{"exp":"start.x", "val":7.1}, "y":{"exp":"start.y", "val":5.07}, "heading":{"exp":"start.heading", "val":3.14159}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.7971601486206055 m", "val":5.7971601486206055}, "y":{"exp":"5.54304313659668 m", "val":5.54304313659668}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.7971601486206055 m", "val":5.7971601486206055}, "y":{"exp":"5.54304313659668 m", "val":5.54304313659668}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"J.x", "val":5.1366723649053885}, "y":{"exp":"J.y", "val":5.197141990263579}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, diff --git a/src/main/deploy/choreo/startToK.traj b/src/main/deploy/choreo/startToK.traj index c1f5352c..c538b6d3 100644 --- a/src/main/deploy/choreo/startToK.traj +++ b/src/main/deploy/choreo/startToK.traj @@ -3,7 +3,7 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":73, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.1, "y":5.07, "heading":3.14159, "intervals":73, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":4.455554008483887, "y":5.959114074707031, "heading":-1.325818250323842, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":4.084460748154255, "y":5.3371419902635795, "heading":5.235987755982989, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ @@ -34,121 +34,121 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,3.02888,4.26334], + "waypoints":[0.0,3.03019,4.26469], "samples":[ - {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.76978, "ay":1.1487, "alpha":3.94164, "fx":[-37.22119,-36.76244,-54.89325,-55.8065], "fy":[9.32993,29.77208,28.56225,8.92915]}, - {"t":0.04149, "x":7.0985, "y":5.07667, "heading":3.14159, "vx":-0.11492, "vy":0.04766, "omega":0.16354, "ax":-2.77036, "ay":1.14895, "alpha":2.99581, "fx":[-39.41964,-39.00479,-52.80667,-53.49099], "fy":[11.72388,27.20185,26.34869,11.33514]}, - {"t":0.08298, "x":7.09135, "y":5.07963, "heading":-3.13481, "vx":-0.22987, "vy":0.09533, "omega":0.28785, "ax":-2.77023, "ay":1.14889, "alpha":2.26447, "fx":[-41.07465,-40.71329,-51.2448,-51.6806], "fy":[13.61128,25.27006,24.52188,13.20282]}, - {"t":0.12447, "x":7.07943, "y":5.08458, "heading":-3.12286, "vx":-0.34481, "vy":0.143, "omega":0.3818, "ax":-2.77004, "ay":1.14882, "alpha":1.67927, "fx":[-42.26899,-42.28395,-49.96545,-50.18265], "fy":[15.09195,23.71365,23.14248,14.65308]}, - {"t":0.16597, "x":7.06274, "y":5.0915, "heading":-3.10702, "vx":-0.45974, "vy":0.19067, "omega":0.45148, "ax":-2.76976, "ay":1.14871, "alpha":1.24194, "fx":[-43.30205,-43.25564,-49.02964,-49.09522], "fy":[16.20879,22.61738,21.98455,15.78301]}, - {"t":0.20746, "x":7.04128, "y":5.1004, "heading":-3.08829, "vx":-0.57466, "vy":0.23833, "omega":0.50301, "ax":-2.7693, "ay":1.14853, "alpha":0.8778, "fx":[-43.97153,-44.32058,-48.20025,-48.15906], "fy":[17.09897,21.60527,21.1823,16.69494]}, - {"t":0.24895, "x":7.01505, "y":5.11128, "heading":-3.06742, "vx":-0.68957, "vy":0.28598, "omega":0.53943, "ax":-2.76836, "ay":1.14814, "alpha":0.64062, "fx":[-44.62092,-44.71291,-47.67369,-47.58125], "fy":[17.68339,21.03485,20.50046,17.33712]}, - {"t":0.29044, "x":6.98406, "y":5.12413, "heading":-3.04504, "vx":-0.80443, "vy":0.33362, "omega":0.56601, "ax":-2.76542, "ay":1.14701, "alpha":0.4197, "fx":[-44.80132,-45.60994,-47.05438,-46.92744], "fy":[18.14427,20.38377,20.1053,17.84683]}, - {"t":0.33193, "x":6.9483, "y":5.13896, "heading":-3.02155, "vx":-0.91917, "vy":0.38121, "omega":0.58342, "ax":-0.10485, "ay":0.04094, "alpha":0.35469, "fx":[-0.75418,-0.96729,-2.74104,-2.5284], "fy":[-0.04308,1.53235,1.49801,-0.25742]}, - {"t":0.37342, "x":6.91007, "y":5.15481, "heading":-2.99735, "vx":-0.92352, "vy":0.38291, "omega":0.59814, "ax":0.00131, "ay":0.00316, "alpha":0.28366, "fx":[0.69539,0.78898,-0.79314,-0.60396], "fy":[-0.50973,0.80694,0.61499,-0.70158]}, - {"t":0.41492, "x":6.87175, "y":5.1707, "heading":-2.97253, "vx":-0.92347, "vy":0.38304, "omega":0.60991, "ax":-0.00045, "ay":-0.00108, "alpha":0.20052, "fx":[0.56592,0.39827,-0.58086,-0.41321], "fy":[-0.40961,0.52113,0.39151,-0.57505]}, - {"t":0.45641, "x":6.83344, "y":5.1866, "heading":-2.94722, "vx":-0.92349, "vy":0.383, "omega":0.61823, "ax":0.0, "ay":0.0, "alpha":0.15398, "fx":[0.41371,0.33767,-0.44654,-0.30474], "fy":[-0.29193,0.43571,0.29204,-0.4356]}, - {"t":0.4979, "x":6.79512, "y":5.20249, "heading":-2.92157, "vx":-0.92349, "vy":0.383, "omega":0.62462, "ax":-0.00007, "ay":-0.00017, "alpha":0.11364, "fx":[0.33051,0.20896,-0.3328,-0.21124], "fy":[-0.21168,0.32889,0.20349,-0.33172]}, - {"t":0.53939, "x":6.7568, "y":5.21838, "heading":-2.89565, "vx":-0.92349, "vy":0.38299, "omega":0.62933, "ax":-0.00002, "ay":-0.00004, "alpha":0.08526, "fx":[0.24843,0.15317,-0.25163,-0.15099], "fy":[-0.15173,0.25231,0.15049,-0.25354]}, - {"t":0.58088, "x":6.71849, "y":5.23427, "heading":-2.86954, "vx":-0.92349, "vy":0.38299, "omega":0.63287, "ax":0.00005, "ay":0.00012, "alpha":0.06426, "fx":[0.19418,0.10949,-0.19251,-0.10781], "fy":[-0.10628,0.19582,0.10865,-0.19011]}, - {"t":0.62237, "x":6.68017, "y":5.25016, "heading":-2.84328, "vx":-0.92349, "vy":0.383, "omega":0.63554, "ax":-0.00004, "ay":-0.0001, "alpha":0.04843, "fx":[0.14634,0.07624,-0.14753,-0.07793], "fy":[-0.0799,0.14662,0.07643,-0.15008]}, - {"t":0.66386, "x":6.64185, "y":5.26605, "heading":-2.81691, "vx":-0.92349, "vy":0.38299, "omega":0.63755, "ax":0.00005, "ay":0.00012, "alpha":0.0368, "fx":[0.11519,0.0571,-0.1135,-0.05542], "fy":[-0.05348,0.11504,0.05779,-0.11122]}, - {"t":0.70536, "x":6.60354, "y":5.28194, "heading":-2.79046, "vx":-0.92349, "vy":0.383, "omega":0.63907, "ax":-0.00005, "ay":-0.00011, "alpha":0.02817, "fx":[0.08752,0.03846,-0.08857,-0.04055], "fy":[-0.04242,0.08681,0.03864,-0.09059]}, - {"t":0.74685, "x":6.56522, "y":5.29783, "heading":-2.76395, "vx":-0.92349, "vy":0.38299, "omega":0.64024, "ax":0.00004, "ay":0.0001, "alpha":0.02162, "fx":[0.06999,0.02987,-0.06866,-0.02855], "fy":[-0.02664,0.06896,0.03085,-0.06679]}, - {"t":0.78834, "x":6.5269, "y":5.31373, "heading":-2.73738, "vx":-0.92349, "vy":0.383, "omega":0.64114, "ax":-0.00004, "ay":-0.00009, "alpha":0.01693, "fx":[0.05406,0.01931,-0.0548,-0.02096], "fy":[-0.02249,0.05327,0.01961,-0.05614]}, - {"t":0.82983, "x":6.48858, "y":5.32962, "heading":-2.71078, "vx":-0.92349, "vy":0.38299, "omega":0.64184, "ax":0.00003, "ay":0.00007, "alpha":0.01321, "fx":[0.04413,0.01561,-0.04317,-0.01465], "fy":[-0.01293,0.04298,0.01634,-0.04176]}, - {"t":0.87132, "x":6.45027, "y":5.34551, "heading":-2.68415, "vx":-0.92349, "vy":0.38299, "omega":0.64239, "ax":-0.00002, "ay":-0.00006, "alpha":0.01065, "fx":[0.03493,0.00942,-0.03541,-0.01051], "fy":[-0.01176,0.03434,0.00987,-0.03623]}, - {"t":0.91281, "x":6.41195, "y":5.3614, "heading":-2.65749, "vx":-0.92349, "vy":0.38299, "omega":0.64283, "ax":0.00002, "ay":0.00005, "alpha":0.00851, "fx":[0.02924,0.00795,-0.02859,-0.00731], "fy":[-0.0058,0.02827,0.00824,-0.0276]}, - {"t":0.95431, "x":6.37363, "y":5.37729, "heading":-2.63082, "vx":-0.92349, "vy":0.38299, "omega":0.64319, "ax":-0.00001, "ay":-0.00004, "alpha":0.0071, "fx":[0.02384,0.00409,-0.02417,-0.00475], "fy":[-0.00586,0.02347,0.00466,-0.02466]}, - {"t":0.9958, "x":6.33532, "y":5.39318, "heading":-2.60413, "vx":-0.92349, "vy":0.38299, "omega":0.64348, "ax":0.00001, "ay":0.00003, "alpha":0.00584, "fx":[0.02053,0.00366,-0.02011,-0.00325], "fy":[-0.00189,0.0198,0.00352,-0.01943]}, - {"t":1.03729, "x":6.297, "y":5.40907, "heading":-2.57744, "vx":-0.92349, "vy":0.38299, "omega":0.64372, "ax":-0.00001, "ay":-0.00002, "alpha":0.00503, "fx":[0.01722,0.00103,-0.01751,-0.00139], "fy":[-0.00246,0.01704,0.00168,-0.01782]}, - {"t":1.07878, "x":6.25868, "y":5.42496, "heading":-2.55073, "vx":-0.92349, "vy":0.38299, "omega":0.64393, "ax":0.00001, "ay":0.00002, "alpha":0.00427, "fx":[0.01528,0.00112,-0.01501,-0.00085], "fy":[0.00045,0.01475,0.00063,-0.01452]}, - {"t":1.12027, "x":6.22037, "y":5.44085, "heading":-2.52401, "vx":-0.92349, "vy":0.38299, "omega":0.64411, "ax":-0.00001, "ay":-0.00002, "alpha":0.00379, "fx":[0.0131,-0.00087,-0.01339,0.0007], "fy":[-0.00039,0.01308,-0.00016,-0.01362]}, - {"t":1.16176, "x":6.18205, "y":5.45674, "heading":-2.49728, "vx":-0.92349, "vy":0.38299, "omega":0.64427, "ax":0.00001, "ay":0.00001, "alpha":0.00331, "fx":[0.01197,-0.00051,-0.01178,0.00069], "fy":[0.00199,0.01155,-0.00126,-0.01137]}, - {"t":1.20325, "x":6.14373, "y":5.47264, "heading":-2.47055, "vx":-0.92349, "vy":0.38299, "omega":0.6444, "ax":-0.00001, "ay":-0.00001, "alpha":0.003, "fx":[0.01036,-0.00213,-0.01068,0.0021], "fy":[0.00098,0.01047,-0.00139,-0.01088]}, - {"t":1.24475, "x":6.10541, "y":5.48853, "heading":-2.44382, "vx":-0.92349, "vy":0.38299, "omega":0.64453, "ax":0.0, "ay":0.00001, "alpha":0.00267, "fx":[0.00972,-0.00161,-0.00958,0.00174], "fy":[0.00308,0.00928,-0.00252,-0.00918]}, - {"t":1.28624, "x":6.0671, "y":5.50442, "heading":-2.41707, "vx":-0.92349, "vy":0.38299, "omega":0.64464, "ax":0.0, "ay":-0.00001, "alpha":0.00247, "fx":[0.00839,-0.00299,-0.00875,0.00307], "fy":[0.00192,0.00863,-0.00225,-0.00895]}, - {"t":1.32773, "x":6.02878, "y":5.52031, "heading":-2.39033, "vx":-0.92349, "vy":0.38299, "omega":0.64474, "ax":0.0, "ay":0.00001, "alpha":0.00222, "fx":[0.00807,-0.00238,-0.00798,0.00248], "fy":[0.00389,0.00743,-0.00336,-0.0075]}, - {"t":1.36922, "x":5.99046, "y":5.5362, "heading":-2.36357, "vx":-0.92349, "vy":0.38299, "omega":0.64483, "ax":0.0, "ay":-0.00001, "alpha":0.00209, "fx":[0.00687,-0.00357,-0.00726,0.00374], "fy":[0.00258,0.00722,-0.00284,-0.00749]}, - {"t":1.41071, "x":5.95215, "y":5.55209, "heading":-2.33682, "vx":-0.92349, "vy":0.38299, "omega":0.64492, "ax":0.0, "ay":0.00001, "alpha":0.00185, "fx":[0.00678,-0.00291,-0.00671,0.00298], "fy":[0.00453,0.0057,-0.00383,-0.00606]}, - {"t":1.4522, "x":5.91383, "y":5.56798, "heading":-2.31006, "vx":-0.92349, "vy":0.38299, "omega":0.645, "ax":0.0, "ay":-0.00001, "alpha":0.00179, "fx":[0.0056,-0.00393,-0.00602,0.00415], "fy":[0.00301,0.00606,-0.00325,-0.0063]}, - {"t":1.4937, "x":5.87551, "y":5.58387, "heading":-2.2833, "vx":-0.92349, "vy":0.38299, "omega":0.64507, "ax":0.0, "ay":0.0, "alpha":0.00152, "fx":[0.0057,-0.00323,-0.00563,0.0033], "fy":[0.00506,0.00387,-0.00393,-0.00468]}, - {"t":1.53519, "x":5.8372, "y":5.59976, "heading":-2.25653, "vx":-0.92349, "vy":0.38299, "omega":0.64513, "ax":0.0, "ay":-0.00001, "alpha":0.00153, "fx":[0.00445,-0.00413,-0.00492,0.00433], "fy":[0.00319,0.00498,-0.00352,-0.00531]}, - {"t":1.57668, "x":5.79888, "y":5.61565, "heading":-2.22977, "vx":-0.92349, "vy":0.38299, "omega":0.6452, "ax":0.0, "ay":0.00001, "alpha":0.00118, "fx":[0.00473,-0.00334,-0.0046,0.00347], "fy":[0.00556,0.00188,-0.00361,-0.00318]}, - {"t":1.61817, "x":5.76056, "y":5.63154, "heading":-2.203, "vx":-0.92349, "vy":0.38299, "omega":0.64525, "ax":-0.00001, "ay":-0.00002, "alpha":0.00127, "fx":[0.00327,-0.00421,-0.00387,0.00424], "fy":[0.00306,0.00378,-0.00375,-0.00446]}, - {"t":1.65966, "x":5.72224, "y":5.64744, "heading":-2.17622, "vx":-0.92349, "vy":0.38299, "omega":0.6453, "ax":0.00001, "ay":0.00003, "alpha":0.0008, "fx":[0.00378,-0.00317,-0.00341,0.00354], "fy":[0.00609,-0.00015,-0.0028,-0.00135]}, - {"t":1.70115, "x":5.68393, "y":5.66333, "heading":-2.14945, "vx":-0.92349, "vy":0.38299, "omega":0.64533, "ax":-0.00002, "ay":-0.00005, "alpha":0.00096, "fx":[0.00182,-0.00418,-0.00276,0.00379], "fy":[0.00243,0.00212,-0.00404,-0.00374]}, - {"t":1.74264, "x":5.64561, "y":5.67922, "heading":-2.12267, "vx":-0.92349, "vy":0.38299, "omega":0.64537, "ax":0.00003, "ay":0.00007, "alpha":0.00036, "fx":[0.00272,-0.00257,-0.00176,0.00354], "fy":[0.00664,-0.00169,-0.00137,0.00107]}, - {"t":1.78414, "x":5.60729, "y":5.69511, "heading":-2.0959, "vx":-0.92349, "vy":0.38299, "omega":0.64539, "ax":-0.00004, "ay":-0.00011, "alpha":0.0005, "fx":[-0.00028,-0.00399,-0.00147,0.00275], "fy":[0.00096,-0.00048,-0.00456,-0.00312]}, - {"t":1.82563, "x":5.56898, "y":5.711, "heading":-2.06912, "vx":-0.92349, "vy":0.38299, "omega":0.64541, "ax":0.00007, "ay":0.00016, "alpha":-0.00013, "fx":[0.00138,-0.00126,0.00082,0.00346], "fy":[0.00699,-0.00139,0.0007,0.00431]}, - {"t":1.86712, "x":5.53066, "y":5.72689, "heading":-2.04234, "vx":-0.92349, "vy":0.383, "omega":0.6454, "ax":-0.00009, "ay":-0.00022, "alpha":-0.00023, "fx":[-0.00358,-0.00342,0.00013,0.00075], "fy":[-0.00191,-0.00478,-0.00547,-0.0026]}, - {"t":1.90861, "x":5.49234, "y":5.74278, "heading":-2.01556, "vx":-0.92349, "vy":0.38299, "omega":0.64539, "ax":0.00013, "ay":0.00032, "alpha":-0.00059, "fx":[-0.00054,0.00117,0.0049,0.00319], "fy":[0.00637,0.00327,0.00314,0.00824]}, - {"t":1.9501, "x":5.45403, "y":5.75867, "heading":-1.98878, "vx":-0.92349, "vy":0.383, "omega":0.64537, "ax":-0.00017, "ay":-0.00042, "alpha":-0.00141, "fx":[-0.00886,-0.00198,0.00217,-0.00285], "fy":[-0.00695,-0.01169,-0.00693,-0.00219]}, - {"t":1.99159, "x":5.41571, "y":5.77456, "heading":-1.962, "vx":-0.92349, "vy":0.38298, "omega":0.64531, "ax":0.00022, "ay":0.00053, "alpha":-0.00082, "fx":[-0.00361,0.00507,0.01096,0.00229], "fy":[0.00281,0.01612,0.00474,0.01179]}, - {"t":2.03309, "x":5.37739, "y":5.79045, "heading":-1.93523, "vx":-0.92348, "vy":0.38301, "omega":0.64528, "ax":-0.00029, "ay":-0.0007, "alpha":-0.00326, "fx":[-0.01685,0.0013,0.00483,-0.00875], "fy":[-0.01482,-0.02188,-0.00865,-0.00159]}, - {"t":2.07458, "x":5.33908, "y":5.80635, "heading":-1.90846, "vx":-0.9235, "vy":0.38298, "omega":0.64514, "ax":0.00031, "ay":0.00075, "alpha":-0.00058, "fx":[-0.00871,0.01055,0.01904,-0.00023], "fy":[-0.00718,0.04148,0.00306,0.01243]}, - {"t":2.11607, "x":5.30076, "y":5.82224, "heading":-1.88169, "vx":-0.92348, "vy":0.38301, "omega":0.64512, "ax":-0.00041, "ay":-0.00099, "alpha":-0.006, "fx":[-0.02753,0.00832,0.00888,-0.0171], "fy":[-0.02451,-0.03418,-0.00856,0.00111]}, - {"t":2.15756, "x":5.26244, "y":5.83813, "heading":-1.85492, "vx":-0.9235, "vy":0.38297, "omega":0.64487, "ax":0.00037, "ay":0.00089, "alpha":0.00018, "fx":[-0.01669,0.01772,0.02893,-0.00548], "fy":[-0.0267,0.08164,-0.00386,0.00794]}, - {"t":2.19905, "x":5.22412, "y":5.85402, "heading":-1.82816, "vx":-0.92348, "vy":0.383, "omega":0.64488, "ax":-0.00037, "ay":-0.0009, "alpha":-0.00982, "fx":[-0.0385,0.02237,0.01718,-0.02585], "fy":[-0.03027,-0.04244,0.00038,0.01255]}, - {"t":2.24054, "x":5.18581, "y":5.86991, "heading":-1.80141, "vx":-0.9235, "vy":0.38297, "omega":0.64447, "ax":0.0004, "ay":0.00096, "alpha":0.00031, "fx":[-0.02765,0.02732,0.04089,-0.01408], "fy":[-0.05295,0.12664,-0.01187,0.002]}, - {"t":2.28203, "x":5.14749, "y":5.8858, "heading":-1.77467, "vx":-0.92348, "vy":0.38301, "omega":0.64448, "ax":0.00008, "ay":0.0002, "alpha":-0.01485, "fx":[-0.04483,0.0452,0.03568,-0.0305], "fy":[-0.02269,-0.03681,0.02937,0.04349]}, - {"t":2.32353, "x":5.10917, "y":5.90169, "heading":-1.74793, "vx":-0.92348, "vy":0.38301, "omega":0.64386, "ax":0.00005, "ay":0.00012, "alpha":-0.00501, "fx":[-0.04824,0.03481,0.04991,-0.03314], "fy":[-0.08334,0.10777,-0.01577,-0.0006]}, - {"t":2.36502, "x":5.07086, "y":5.91758, "heading":-1.72121, "vx":-0.92348, "vy":0.38302, "omega":0.64366, "ax":0.00068, "ay":0.00165, "alpha":-0.0213, "fx":[-0.04873,0.06039,0.06727,-0.03338], "fy":[-0.01534,-0.03069,0.07022,0.08557]}, - {"t":2.40651, "x":5.03254, "y":5.93348, "heading":-1.69451, "vx":-0.92345, "vy":0.38309, "omega":0.64277, "ax":-0.00252, "ay":-0.00608, "alpha":-0.02857, "fx":[-0.11364,0.01382,0.02957,-0.0979], "fy":[-0.14703,-0.20234,-0.03587,-0.02022]}, - {"t":2.448, "x":4.99422, "y":5.94937, "heading":-1.66784, "vx":-0.92355, "vy":0.38283, "omega":0.64159, "ax":0.00066, "ay":0.0016, "alpha":-0.031, "fx":[-0.0624,0.0348,0.11839,-0.04666], "fy":[-0.04838,-0.0643,0.1016,0.11753]}, - {"t":2.48949, "x":4.9559, "y":5.96525, "heading":-1.64122, "vx":-0.92353, "vy":0.3829, "omega":0.6403, "ax":-0.00079, "ay":-0.0019, "alpha":-0.08883, "fx":[-0.13044,0.08906,0.10418,-0.11533], "fy":[0.0668,-0.74781,0.26964,0.28458]}, - {"t":2.53098, "x":4.91758, "y":5.98114, "heading":-1.61465, "vx":-0.92356, "vy":0.38282, "omega":0.63662, "ax":0.001, "ay":0.0024, "alpha":-0.05833, "fx":[-0.1147,0.07228,0.21047,-0.10169], "fy":[-0.11083,-0.12427,0.19093,0.20443]}, - {"t":2.57248, "x":4.87927, "y":5.99702, "heading":-1.58824, "vx":-0.92352, "vy":0.38292, "omega":0.6342, "ax":0.00671, "ay":0.01616, "alpha":-0.13385, "fx":[-0.11439,0.33219,0.33702,-0.10757], "fy":[0.26023,-0.57247,0.69115,0.69859]}, - {"t":2.61397, "x":4.84095, "y":6.01292, "heading":-1.56192, "vx":-0.92324, "vy":0.38359, "omega":0.62864, "ax":-0.33272, "ay":-0.84548, "alpha":-0.13387, "fx":[-5.87693,-5.16725,-5.2539,-5.88675], "fy":[-14.4116,-14.40836,-13.77634,-13.77881]}, - {"t":2.65546, "x":4.80236, "y":6.02811, "heading":-1.53584, "vx":-0.93705, "vy":0.34851, "omega":0.62309, "ax":-0.83159, "ay":-2.71686, "alpha":-0.12538, "fx":[-14.26247,-13.44485,-13.43433,-14.30748], "fy":[-45.93428,-45.00288,-45.10096,-45.11709]}, - {"t":2.69695, "x":4.76276, "y":6.04023, "heading":-1.50999, "vx":-0.97155, "vy":0.23578, "omega":0.61788, "ax":-0.52085, "ay":-2.92348, "alpha":-0.207, "fx":[-9.12973,-8.29599,-8.09098,-9.2124], "fy":[-49.27979,-49.22938,-48.18685,-48.2359]}, - {"t":2.73844, "x":4.72201, "y":6.0475, "heading":-1.48435, "vx":-0.99316, "vy":0.11449, "omega":0.6093, "ax":-0.15684, "ay":-2.98337, "alpha":-0.31353, "fx":[-3.2837,-1.8123,-1.9427,-3.41895], "fy":[-50.32055,-50.73567,-48.87945,-48.98995]}, - {"t":2.77993, "x":4.68066, "y":6.04968, "heading":-1.45907, "vx":-0.99967, "vy":-0.0093, "omega":0.59629, "ax":0.21478, "ay":-2.98549, "alpha":-0.37038, "fx":[2.75278,4.53871,4.47526,2.55407], "fy":[-50.74054,-50.53135,-48.79097,-49.00384]}, - {"t":2.82142, "x":4.63937, "y":6.04673, "heading":-1.43433, "vx":-0.99076, "vy":-0.13317, "omega":0.58092, "ax":0.58477, "ay":-2.93809, "alpha":-0.51322, "fx":[8.62654,11.15244,10.88175,8.3306], "fy":[-50.22834,-50.16351,-47.57136,-47.94283]}, - {"t":2.86292, "x":4.59877, "y":6.03867, "heading":-1.41022, "vx":-0.96649, "vy":-0.25508, "omega":0.55963, "ax":0.94744, "ay":-2.84334, "alpha":-0.65888, "fx":[14.34975,17.64409,17.25704,13.92257], "fy":[-49.21709,-48.59899,-45.58002,-46.19242]}, - {"t":2.90441, "x":4.55948, "y":6.02564, "heading":-1.387, "vx":-0.92718, "vy":-0.37305, "omega":0.53229, "ax":1.30183, "ay":-2.7004, "alpha":-0.88914, "fx":[19.82332,24.17209,23.60683,19.20107], "fy":[-47.44257,-46.75525,-42.4519,-43.40758]}, - {"t":2.9459, "x":4.52213, "y":6.00784, "heading":-1.36492, "vx":-0.87317, "vy":-0.4851, "omega":0.4954, "ax":1.64342, "ay":-2.5078, "alpha":-1.1672, "fx":[24.97356,30.72082,29.82788,24.05796], "fy":[-45.21332,-43.75981,-38.40146,-39.84047]}, - {"t":2.98739, "x":4.48732, "y":5.98555, "heading":-1.34436, "vx":-0.80498, "vy":-0.58915, "omega":0.44697, "ax":1.9037, "ay":-2.31685, "alpha":-1.54791, "fx":[28.63878,36.10636,34.89942,27.29057], "fy":[-43.22684,-41.17977,-33.99674,-36.07999]}, - {"t":3.02888, "x":4.45555, "y":5.95911, "heading":-1.32582, "vx":-0.72599, "vy":-0.68528, "omega":0.38274, "ax":0.67232, "ay":-0.73573, "alpha":-1.06602, "fx":[9.27334,14.48096,13.02562,8.04887], "fy":[-15.37958,-14.10845,-9.15128,-10.41807]}, - {"t":3.06053, "x":4.43291, "y":5.93705, "heading":-1.3137, "vx":-0.70471, "vy":-0.70857, "omega":0.349, "ax":0.71509, "ay":-0.69641, "alpha":-0.83573, "fx":[10.45843,14.39278,13.38896,9.44098], "fy":[-14.07555,-13.08445,-9.11233,-10.16323]}, - {"t":3.09219, "x":4.41096, "y":5.91428, "heading":-1.30266, "vx":-0.68208, "vy":-0.73061, "omega":0.32255, "ax":0.73996, "ay":-0.66969, "alpha":-0.66974, "fx":[11.16604,14.4128,13.43029,10.32983], "fy":[-13.15196,-12.27711,-9.17641,-10.04802]}, - {"t":3.12384, "x":4.38974, "y":5.89082, "heading":-1.29245, "vx":-0.65866, "vy":-0.75181, "omega":0.30135, "ax":0.76637, "ay":-0.639, "alpha":-0.52775, "fx":[11.8955,14.33815,13.65723,11.20911], "fy":[-12.17419,-11.66075,-9.03182,-9.7407]}, - {"t":3.15549, "x":4.36928, "y":5.8667, "heading":-1.28291, "vx":-0.6344, "vy":-0.77203, "omega":0.28464, "ax":0.8292, "ay":-0.55468, "alpha":-0.41733, "fx":[13.11556,15.14822,14.46537,12.56048], "fy":[-10.49455,-9.91416,-7.99919,-8.57711]}, - {"t":3.18715, "x":4.34962, "y":5.84198, "heading":-1.2739, "vx":-0.60815, "vy":-0.78959, "omega":0.27143, "ax":0.92473, "ay":-0.37394, "alpha":-0.32175, "fx":[14.8969,16.37998,15.93366,14.44859], "fy":[-7.18338,-6.81315,-5.23864,-5.69865]}, - {"t":3.2188, "x":4.33083, "y":5.8168, "heading":-1.26531, "vx":-0.57888, "vy":-0.80143, "omega":0.26125, "ax":0.98653, "ay":-0.14724, "alpha":-0.26099, "fx":[15.99698,17.36485,16.77692,15.6413], "fy":[-3.21262,-2.84601,-1.69685,-2.0621]}, - {"t":3.25045, "x":4.313, "y":5.79136, "heading":-1.25704, "vx":-0.54765, "vy":-0.80609, "omega":0.25299, "ax":0.9943, "ay":0.08091, "alpha":-0.18529, "fx":[16.28181,17.14815,16.8672,16.00051], "fy":[0.75352,1.10742,1.90837,1.62571]}, - {"t":3.2821, "x":4.29616, "y":5.76589, "heading":-1.24903, "vx":-0.51618, "vy":-0.80353, "omega":0.24712, "ax":0.9587, "ay":0.27655, "alpha":-0.15558, "fx":[15.7097,16.58248,16.13928,15.49294], "fy":[4.17396,4.39032,5.04625,4.82958]}, - {"t":3.31376, "x":4.28031, "y":5.74059, "heading":-1.24121, "vx":-0.48584, "vy":-0.79477, "omega":0.2422, "ax":0.90155, "ay":0.42809, "alpha":-0.09946, "fx":[14.8725,15.34954,15.18451,14.7074], "fy":[6.77897,7.08263,7.42268,7.26024]}, - {"t":3.34541, "x":4.26538, "y":5.71565, "heading":-1.23354, "vx":-0.4573, "vy":-0.78122, "omega":0.23905, "ax":0.83956, "ay":0.54005, "alpha":-0.08882, "fx":[13.83128,14.38713,14.05225,13.70973], "fy":[8.77053,8.8879,9.23491,9.11593]}, - {"t":3.37706, "x":4.25132, "y":5.6912, "heading":-1.22598, "vx":-0.43073, "vy":-0.76413, "omega":0.23624, "ax":0.78115, "ay":0.62186, "alpha":-0.04667, "fx":[12.94555,13.18411,13.09775,12.85827], "fy":[10.15578,10.42776,10.48309,10.39785]}, - {"t":3.40871, "x":4.23808, "y":5.66732, "heading":-1.2185, "vx":-0.406, "vy":-0.74444, "omega":0.23476, "ax":0.72931, "ay":0.68217, "alpha":-0.0461, "fx":[12.06551,12.39067,12.16704,12.0055], "fy":[11.26229,11.31865,11.48137,11.42342]}, - {"t":3.44037, "x":4.2256, "y":5.6441, "heading":-1.21107, "vx":-0.38292, "vy":-0.72285, "omega":0.2333, "ax":0.68441, "ay":0.7274, "alpha":-0.01674, "fx":[11.37739,11.47845,11.44089,11.33841], "fy":[12.00517,12.23856,12.14799,12.10966]}, - {"t":3.47202, "x":4.21382, "y":5.62158, "heading":-1.20368, "vx":-0.36125, "vy":-0.69983, "omega":0.23277, "ax":0.64585, "ay":0.762, "alpha":-0.02142, "fx":[10.71699,10.89855,10.75518,10.69313], "fy":[12.66085,12.68231,12.74398,12.72134]}, - {"t":3.50367, "x":4.20271, "y":5.59981, "heading":-1.19631, "vx":-0.34081, "vy":-0.67571, "omega":0.23209, "ax":0.61273, "ay":0.789, "alpha":-0.00219, "fx":[10.2042,10.23579,10.2243,10.19125], "fy":[13.08353,13.2755,13.13158,13.11837]}, - {"t":3.53532, "x":4.19223, "y":5.57882, "heading":-1.18897, "vx":-0.32142, "vy":-0.65073, "omega":0.23203, "ax":0.58418, "ay":0.81047, "alpha":-0.00976, "fx":[9.7104,9.81642,9.72175,9.7032], "fy":[13.49871,13.50437,13.52202,13.51554]}, - {"t":3.56698, "x":4.18235, "y":5.55863, "heading":-1.18162, "vx":-0.30292, "vy":-0.62508, "omega":0.23172, "ax":0.55943, "ay":0.82784, "alpha":0.00171, "fx":[9.32212,9.33259,9.32924,9.31746], "fy":[13.75115,13.91324,13.76967,13.76449]}, - {"t":3.59863, "x":4.17304, "y":5.53926, "heading":-1.17429, "vx":-0.28522, "vy":-0.59888, "omega":0.23177, "ax":0.53783, "ay":0.8421, "alpha":-0.00814, "fx":[8.94363,9.02768,8.95375,8.9367], "fy":[14.02654,14.03229,14.04851,14.04218]}, - {"t":3.63028, "x":4.16428, "y":5.52072, "heading":-1.16695, "vx":-0.26819, "vy":-0.57222, "omega":0.23151, "ax":0.51888, "ay":0.85397, "alpha":-0.00286, "fx":[8.64137,8.66944,8.65812,8.6289], "fy":[14.17966,14.32905,14.22258,14.20997]}, - {"t":3.66194, "x":4.15605, "y":5.50304, "heading":-1.15963, "vx":-0.25177, "vy":-0.54519, "omega":0.23142, "ax":0.50213, "ay":0.86398, "alpha":-0.01521, "fx":[8.34225,8.44798,8.37161,8.31942], "fy":[14.3656,14.38715,14.43891,14.41693]}, - {"t":3.69359, "x":4.14833, "y":5.48621, "heading":-1.1523, "vx":-0.23588, "vy":-0.51785, "omega":0.23094, "ax":0.48725, "ay":0.87251, "alpha":-0.01564, "fx":[8.09945,8.18209,8.14552,8.0619], "fy":[14.45428,14.60999,14.57481,14.53814]}, - {"t":3.72524, "x":4.14111, "y":5.47026, "heading":-1.14499, "vx":-0.22045, "vy":-0.49023, "omega":0.23045, "ax":0.47395, "ay":0.87985, "alpha":-0.03129, "fx":[7.85554,8.02504,7.92362,7.79797], "fy":[14.57587,14.63167,14.75754,14.70143]}, - {"t":3.75689, "x":4.13437, "y":5.45518, "heading":-1.1377, "vx":-0.20545, "vy":-0.46238, "omega":0.22946, "ax":0.462, "ay":0.88622, "alpha":-0.03794, "fx":[7.65364,7.83299,7.74954,7.56935], "fy":[14.61633,14.80255,14.87699,14.79538]}, - {"t":3.78855, "x":4.1281, "y":5.44099, "heading":-1.13043, "vx":-0.19083, "vy":-0.43433, "omega":0.22825, "ax":0.45122, "ay":0.89179, "alpha":-0.05843, "fx":[7.44816,7.73043,7.57695,7.33083], "fy":[14.68482,14.79946,15.04672,14.9319]}, - {"t":3.8202, "x":4.12228, "y":5.42769, "heading":-1.12321, "vx":-0.17654, "vy":-0.4061, "omega":0.2264, "ax":0.44144, "ay":0.8967, "alpha":-0.07287, "fx":[7.27349,7.60437,7.44401,7.11239], "fy":[14.68251,14.93063,15.16641,15.01087]}, - {"t":3.85185, "x":4.11692, "y":5.41529, "heading":-1.11604, "vx":-0.16257, "vy":-0.37772, "omega":0.2241, "ax":0.43253, "ay":0.90106, "alpha":-0.10072, "fx":[7.09378,7.55448,7.31135,6.88085], "fy":[14.69939,14.9082,15.34108,15.13226]}, - {"t":3.8835, "x":4.11199, "y":5.40378, "heading":-1.10895, "vx":-0.14888, "vy":-0.3492, "omega":0.22091, "ax":0.42439, "ay":0.90495, "alpha":-0.12574, "fx":[6.93555,7.49496,7.21358,6.65356], "fy":[14.65138,15.00658,15.47711,15.20508]}, - {"t":3.91516, "x":4.10749, "y":5.39318, "heading":-1.10196, "vx":-0.13545, "vy":-0.32055, "omega":0.21693, "ax":0.41692, "ay":0.90844, "alpha":-0.16491, "fx":[6.77103,7.5039,7.11576,6.40901], "fy":[14.60989,14.96561,15.67631,15.32091]}, - {"t":3.94681, "x":4.10341, "y":5.38349, "heading":-1.09509, "vx":-0.12225, "vy":-0.2918, "omega":0.21171, "ax":0.41005, "ay":0.91158, "alpha":-0.20509, "fx":[6.61969,7.52001,7.05121,6.15042], "fy":[14.50535,15.03336,15.84816,15.39551]}, - {"t":3.97846, "x":4.09975, "y":5.37471, "heading":-1.08839, "vx":-0.10927, "vy":-0.26294, "omega":0.20522, "ax":0.4037, "ay":0.91443, "alpha":-0.26153, "fx":[6.4607,7.60177,6.98718,5.86845], "fy":[14.38922,14.97191,16.09637,15.51479]}, - {"t":4.01012, "x":4.09649, "y":5.36685, "heading":-1.08189, "vx":-0.09649, "vy":-0.234, "omega":0.19694, "ax":0.39782, "ay":0.91702, "alpha":-0.32412, "fx":[6.30604,7.71442,6.95717,5.54854], "fy":[14.20737,15.00762,16.33043,15.59956]}, - {"t":4.04177, "x":4.09363, "y":5.3599, "heading":-1.07566, "vx":-0.0839, "vy":-0.20497, "omega":0.18668, "ax":0.39237, "ay":0.91938, "alpha":-0.40696, "fx":[6.14178,7.89544,6.92952,5.19552], "fy":[13.98836,14.92019,16.66152,15.7326]}, - {"t":4.07342, "x":4.09118, "y":5.35387, "heading":-1.06975, "vx":-0.07148, "vy":-0.17587, "omega":0.1738, "ax":0.38729, "ay":0.92155, "alpha":-0.50307, "fx":[5.9719,8.13944,6.93976,4.77239], "fy":[13.69453,14.91849,16.996,15.83814]}, - {"t":4.10507, "x":4.08911, "y":5.34877, "heading":-1.06425, "vx":-0.05922, "vy":-0.1467, "omega":0.15788, "ax":0.38255, "ay":0.92354, "alpha":-0.6262, "fx":[5.78875,8.46273,6.9561,4.29978], "fy":[13.32756,14.79488,17.45888,15.99867]}, - {"t":4.13673, "x":4.08742, "y":5.34459, "heading":-1.05925, "vx":-0.04712, "vy":-0.11747, "omega":0.13806, "ax":0.37811, "ay":0.92538, "alpha":-0.77292, "fx":[5.58728,8.894,7.01805,3.71241], "fy":[12.86681,14.74431,17.95096,16.14042]}, - {"t":4.16838, "x":4.08612, "y":5.34133, "heading":-1.05488, "vx":-0.03515, "vy":-0.08818, "omega":0.11359, "ax":0.37396, "ay":0.92708, "alpha":-0.95752, "fx":[5.36544,9.42788,7.09349,3.04788], "fy":[12.28207,14.5676,18.61738,16.34874]}, - {"t":4.20003, "x":4.0852, "y":5.33901, "heading":-1.05129, "vx":-0.02331, "vy":-0.05884, "omega":0.08328, "ax":0.37005, "ay":0.92865, "alpha":-1.18097, "fx":[5.10746,10.13239,7.22826,2.20643], "fy":[11.56874,14.44705,19.35353,16.55155]}, - {"t":4.23168, "x":4.08464, "y":5.33761, "heading":-1.04865, "vx":-0.0116, "vy":-0.02944, "omega":0.0459, "ax":0.36638, "ay":0.93012, "alpha":-1.45018, "fx":[4.81443,10.97089,7.39131,1.25315], "fy":[10.72491,14.18181,20.29221,16.81963]}, - {"t":4.26334, "x":4.08446, "y":5.33714, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.1, "y":5.07, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.76721, "ay":1.15488, "alpha":3.93966, "fx":[-37.18261,-36.72107,-54.84573,-55.7628], "fy":[9.44173,29.8692,28.65731,9.03672]}, + {"t":0.04151, "x":7.09762, "y":5.07099, "heading":3.14159, "vx":-0.11487, "vy":0.04794, "omega":0.16353, "ax":-2.76779, "ay":1.15512, "alpha":2.994, "fx":[-39.38089,-38.96353,-52.75966,-53.44677], "fy":[11.83432,27.2996,26.44473,11.44254]}, + {"t":0.08302, "x":7.09046, "y":5.07398, "heading":-3.13481, "vx":-0.22975, "vy":0.09589, "omega":0.28781, "ax":-2.76766, "ay":1.15507, "alpha":2.26295, "fx":[-41.03538,-40.67227,-51.19832,-51.63615], "fy":[13.71995,25.36922,24.61896,13.30951]}, + {"t":0.12453, "x":7.07854, "y":5.07896, "heading":-3.12286, "vx":-0.34464, "vy":0.14383, "omega":0.38175, "ax":-2.76747, "ay":1.15499, "alpha":1.67791, "fx":[-42.22938,-42.24259,-49.9196,-50.13824], "fy":[15.19982,23.81274,23.24065,14.75955]}, + {"t":0.16604, "x":7.06185, "y":5.08592, "heading":-3.10701, "vx":-0.45952, "vy":0.19178, "omega":0.4514, "ax":-2.7672, "ay":1.15488, "alpha":1.2392, "fx":[-43.26126,-43.2162,-48.98374,-49.0501], "fy":[16.32259,22.69636,22.09046,15.89585]}, + {"t":0.20755, "x":7.04039, "y":5.09488, "heading":-3.08828, "vx":-0.57438, "vy":0.23971, "omega":0.50283, "ax":-2.76673, "ay":1.1547, "alpha":0.87687, "fx":[-43.93139,-44.27827,-48.15545,-48.11503], "fy":[17.205,21.70534,21.28205,16.80055]}, + {"t":0.24906, "x":7.01417, "y":5.10582, "heading":-3.0674, "vx":-0.68923, "vy":0.28765, "omega":0.53923, "ax":-2.76578, "ay":1.15432, "alpha":0.63983, "fx":[-44.58011,-44.67139,-47.62873,-47.53689], "fy":[17.78849,21.13633,20.6006,17.44217]}, + {"t":0.29057, "x":6.98318, "y":5.11876, "heading":-3.04502, "vx":-0.80403, "vy":0.33556, "omega":0.56579, "ax":-2.76284, "ay":1.15316, "alpha":0.41966, "fx":[-44.7634,-45.55772,-47.01296,-46.88645], "fy":[18.24856,20.48496,20.20567,17.95131]}, + {"t":0.33208, "x":6.94742, "y":5.13368, "heading":-3.02154, "vx":-0.91872, "vy":0.38343, "omega":0.58321, "ax":-0.09517, "ay":0.03715, "alpha":0.35428, "fx":[-0.59451,-0.80731,-2.57822,-2.36587], "fy":[-0.10576,1.47004,1.43281,-0.31974]}, + {"t":0.37359, "x":6.9092, "y":5.14963, "heading":-2.99733, "vx":-0.92267, "vy":0.38497, "omega":0.59792, "ax":0.00131, "ay":0.00314, "alpha":0.28319, "fx":[0.69406,0.78819,-0.79186,-0.603], "fy":[-0.50899,0.80535,0.61373,-0.70051]}, + {"t":0.41509, "x":6.8709, "y":5.16561, "heading":-2.97251, "vx":-0.92261, "vy":0.3851, "omega":0.60967, "ax":-0.00044, "ay":-0.00106, "alpha":0.20009, "fx":[0.56501,0.39765,-0.57972,-0.41236], "fy":[-0.40823,0.51965,0.39144,-0.57338]}, + {"t":0.4566, "x":6.83261, "y":5.18159, "heading":-2.9472, "vx":-0.92263, "vy":0.38506, "omega":0.61798, "ax":0.0, "ay":0.00001, "alpha":0.15369, "fx":[0.41311,0.3368,-0.44561,-0.30406], "fy":[-0.29131,0.43502,0.2916,-0.43473]}, + {"t":0.49811, "x":6.79431, "y":5.19758, "heading":-2.92155, "vx":-0.92263, "vy":0.38506, "omega":0.62436, "ax":-0.00008, "ay":-0.00018, "alpha":0.1133, "fx":[0.32983,0.20848,-0.33234,-0.21099], "fy":[-0.21101,0.32639,0.20344,-0.33085]}, + {"t":0.53962, "x":6.75601, "y":5.21356, "heading":-2.89563, "vx":-0.92263, "vy":0.38505, "omega":0.62906, "ax":-0.00001, "ay":-0.00002, "alpha":0.08511, "fx":[0.24815,0.15286,-0.25108,-0.1506], "fy":[-0.15126,0.2521,0.15045,-0.25291]}, + {"t":0.58113, "x":6.71771, "y":5.22954, "heading":-2.86952, "vx":-0.92263, "vy":0.38505, "omega":0.63259, "ax":0.00004, "ay":0.00011, "alpha":0.06405, "fx":[0.19379,0.10923,-0.19231,-0.10774], "fy":[-0.1059,0.19391,0.1087,-0.1896]}, + {"t":0.62264, "x":6.67942, "y":5.24553, "heading":-2.84326, "vx":-0.92263, "vy":0.38505, "omega":0.63525, "ax":-0.00004, "ay":-0.00009, "alpha":0.04836, "fx":[0.14624,0.07613,-0.1472,-0.0777], "fy":[-0.07957,0.14662,0.07653,-0.14966]}, + {"t":0.66415, "x":6.64112, "y":5.26151, "heading":-2.81689, "vx":-0.92263, "vy":0.38505, "omega":0.63726, "ax":0.00005, "ay":0.00011, "alpha":0.03668, "fx":[0.11498,0.05696,-0.11343,-0.05541], "fy":[-0.05329,0.11387,0.05783,-0.11096]}, + {"t":0.70566, "x":6.60282, "y":5.27749, "heading":-2.79044, "vx":-0.92263, "vy":0.38505, "omega":0.63878, "ax":-0.00004, "ay":-0.0001, "alpha":0.02813, "fx":[0.08749,0.03841,-0.0884,-0.04042], "fy":[-0.04222,0.08685,0.03873,-0.09034]}, + {"t":0.74717, "x":6.56452, "y":5.29348, "heading":-2.76392, "vx":-0.92263, "vy":0.38505, "omega":0.63995, "ax":0.00004, "ay":0.00009, "alpha":0.02156, "fx":[0.06988,0.02979,-0.06864,-0.02855], "fy":[-0.02656,0.06833,0.03085,-0.06668]}, + {"t":0.78868, "x":6.52622, "y":5.30946, "heading":-2.73736, "vx":-0.92263, "vy":0.38505, "omega":0.64085, "ax":-0.00003, "ay":-0.00008, "alpha":0.01691, "fx":[0.05405,0.01929,-0.05471,-0.02089], "fy":[-0.02237,0.0533,0.01966,-0.05601]}, + {"t":0.83019, "x":6.48793, "y":5.32544, "heading":-2.71076, "vx":-0.92263, "vy":0.38505, "omega":0.64155, "ax":0.00003, "ay":0.00007, "alpha":0.01318, "fx":[0.04407,0.01556,-0.04316,-0.01465], "fy":[-0.01289,0.04269,0.01631,-0.04172]}, + {"t":0.8717, "x":6.44963, "y":5.34143, "heading":-2.68413, "vx":-0.92263, "vy":0.38505, "omega":0.64209, "ax":-0.00002, "ay":-0.00005, "alpha":0.01064, "fx":[0.03492,0.00939,-0.03536,-0.01046], "fy":[-0.01169,0.03435,0.00989,-0.03616]}, + {"t":0.91321, "x":6.41133, "y":5.35741, "heading":-2.65748, "vx":-0.92263, "vy":0.38505, "omega":0.64254, "ax":0.00002, "ay":0.00004, "alpha":0.0085, "fx":[0.0292,0.00792,-0.02858,-0.00729], "fy":[-0.00577,0.02815,0.0082,-0.02759]}, + {"t":0.95472, "x":6.37303, "y":5.37339, "heading":-2.6308, "vx":-0.92263, "vy":0.38505, "omega":0.64289, "ax":-0.00001, "ay":-0.00003, "alpha":0.00709, "fx":[0.02382,0.00406,-0.02414,-0.00471], "fy":[-0.00581,0.02347,0.00466,-0.02463]}, + {"t":0.99623, "x":6.33473, "y":5.38938, "heading":-2.60412, "vx":-0.92263, "vy":0.38505, "omega":0.64318, "ax":0.00001, "ay":0.00003, "alpha":0.00583, "fx":[0.02051,0.00363,-0.0201,-0.00323], "fy":[-0.00186,0.01977,0.00347,-0.01943]}, + {"t":1.03774, "x":6.29644, "y":5.40536, "heading":-2.57742, "vx":-0.92263, "vy":0.38505, "omega":0.64343, "ax":-0.00001, "ay":-0.00002, "alpha":0.00503, "fx":[0.01721,0.00099,-0.01749,-0.00135], "fy":[-0.00243,0.01704,0.00167,-0.0178]}, + {"t":1.07925, "x":6.25814, "y":5.42134, "heading":-2.55071, "vx":-0.92263, "vy":0.38505, "omega":0.64363, "ax":0.00001, "ay":0.00002, "alpha":0.00427, "fx":[0.01527,0.00109,-0.015,-0.00082], "fy":[0.00048,0.01476,0.00057,-0.01452]}, + {"t":1.12076, "x":6.21984, "y":5.43733, "heading":-2.52399, "vx":-0.92263, "vy":0.38505, "omega":0.64381, "ax":-0.00001, "ay":-0.00002, "alpha":0.00379, "fx":[0.01309,-0.00091,-0.01338,0.00075], "fy":[-0.00035,0.01308,-0.00018,-0.0136]}, + {"t":1.16227, "x":6.18154, "y":5.45331, "heading":-2.49727, "vx":-0.92263, "vy":0.38505, "omega":0.64397, "ax":0.00001, "ay":0.00001, "alpha":0.00331, "fx":[0.01195,-0.00054,-0.01177,0.00073], "fy":[0.00202,0.01156,-0.00132,-0.01137]}, + {"t":1.20378, "x":6.14324, "y":5.46929, "heading":-2.47054, "vx":-0.92263, "vy":0.38505, "omega":0.64411, "ax":0.0, "ay":-0.00001, "alpha":0.003, "fx":[0.01035,-0.00217,-0.01066,0.00216], "fy":[0.00102,0.01047,-0.00142,-0.01086]}, + {"t":1.24528, "x":6.10495, "y":5.48528, "heading":-2.4438, "vx":-0.92263, "vy":0.38505, "omega":0.64423, "ax":0.0, "ay":0.00001, "alpha":0.00267, "fx":[0.00971,-0.00165,-0.00958,0.00178], "fy":[0.00312,0.00929,-0.0026,-0.00918]}, + {"t":1.28679, "x":6.06665, "y":5.50126, "heading":-2.41706, "vx":-0.92263, "vy":0.38505, "omega":0.64434, "ax":0.0, "ay":-0.00001, "alpha":0.00247, "fx":[0.00838,-0.00304,-0.00873,0.00314], "fy":[0.00197,0.00863,-0.00227,-0.00893]}, + {"t":1.3283, "x":6.02835, "y":5.51724, "heading":-2.39031, "vx":-0.92263, "vy":0.38505, "omega":0.64444, "ax":0.0, "ay":0.00001, "alpha":0.00222, "fx":[0.00806,-0.00242,-0.00797,0.00251], "fy":[0.00394,0.00744,-0.00344,-0.0075]}, + {"t":1.36981, "x":5.99005, "y":5.53323, "heading":-2.36356, "vx":-0.92263, "vy":0.38505, "omega":0.64454, "ax":0.0, "ay":-0.00001, "alpha":0.00208, "fx":[0.00686,-0.00362,-0.00724,0.00381], "fy":[0.00264,0.00723,-0.00287,-0.00746]}, + {"t":1.41132, "x":5.95175, "y":5.54921, "heading":-2.33681, "vx":-0.92263, "vy":0.38505, "omega":0.64462, "ax":0.0, "ay":0.0, "alpha":0.00185, "fx":[0.00677,-0.00296,-0.0067,0.00302], "fy":[0.00457,0.0057,-0.00391,-0.00606]}, + {"t":1.45283, "x":5.91345, "y":5.56519, "heading":-2.31005, "vx":-0.92263, "vy":0.38505, "omega":0.6447, "ax":0.0, "ay":-0.00001, "alpha":0.00178, "fx":[0.00558,-0.00399,-0.00599,0.00422], "fy":[0.00307,0.00607,-0.00327,-0.00627]}, + {"t":1.49434, "x":5.87516, "y":5.58118, "heading":-2.28329, "vx":-0.92263, "vy":0.38505, "omega":0.64477, "ax":0.0, "ay":0.0, "alpha":0.00152, "fx":[0.00568,-0.00328,-0.00562,0.00334], "fy":[0.0051,0.00388,-0.00402,-0.00468]}, + {"t":1.53585, "x":5.83686, "y":5.59716, "heading":-2.25653, "vx":-0.92263, "vy":0.38505, "omega":0.64484, "ax":0.0, "ay":-0.00001, "alpha":0.00153, "fx":[0.00443,-0.00419,-0.00489,0.0044], "fy":[0.00326,0.00499,-0.00354,-0.00528]}, + {"t":1.57736, "x":5.79856, "y":5.61314, "heading":-2.22976, "vx":-0.92263, "vy":0.38505, "omega":0.6449, "ax":0.0, "ay":0.00001, "alpha":0.00118, "fx":[0.00471,-0.00339,-0.00459,0.00351], "fy":[0.00559,0.0019,-0.0037,-0.00318]}, + {"t":1.61887, "x":5.76026, "y":5.62913, "heading":-2.20299, "vx":-0.92263, "vy":0.38505, "omega":0.64495, "ax":-0.00001, "ay":-0.00002, "alpha":0.00127, "fx":[0.00324,-0.00426,-0.00383,0.00431], "fy":[0.00312,0.00378,-0.00377,-0.00443]}, + {"t":1.66038, "x":5.72196, "y":5.64511, "heading":-2.17622, "vx":-0.92263, "vy":0.38505, "omega":0.645, "ax":0.00001, "ay":0.00003, "alpha":0.0008, "fx":[0.00376,-0.00322,-0.00339,0.00358], "fy":[0.00612,-0.00012,-0.00289,-0.00135]}, + {"t":1.70189, "x":5.68367, "y":5.66109, "heading":-2.14945, "vx":-0.92263, "vy":0.38505, "omega":0.64503, "ax":-0.00002, "ay":-0.00005, "alpha":0.00095, "fx":[0.00179,-0.00423,-0.00273,0.00385], "fy":[0.00249,0.00212,-0.00406,-0.0037]}, + {"t":1.7434, "x":5.64537, "y":5.67708, "heading":-2.12267, "vx":-0.92263, "vy":0.38505, "omega":0.64507, "ax":0.00003, "ay":0.00007, "alpha":0.00036, "fx":[0.00271,-0.00261,-0.00174,0.00358], "fy":[0.00666,-0.00164,-0.00146,0.00106]}, + {"t":1.78491, "x":5.60707, "y":5.69306, "heading":-2.09589, "vx":-0.92263, "vy":0.38505, "omega":0.64509, "ax":-0.00004, "ay":-0.00011, "alpha":0.00049, "fx":[-0.0003,-0.00403,-0.00145,0.00281], "fy":[0.00101,-0.00048,-0.00457,-0.00308]}, + {"t":1.82642, "x":5.56877, "y":5.70904, "heading":-2.06912, "vx":-0.92263, "vy":0.38505, "omega":0.64511, "ax":0.00007, "ay":0.00016, "alpha":-0.00012, "fx":[0.00137,-0.00129,0.00083,0.0035], "fy":[0.007,-0.00132,0.00061,0.00429]}, + {"t":1.86793, "x":5.53047, "y":5.72503, "heading":-2.04234, "vx":-0.92263, "vy":0.38505, "omega":0.6451, "ax":-0.00009, "ay":-0.00022, "alpha":-0.00023, "fx":[-0.0036,-0.00344,0.00015,0.00079], "fy":[-0.00184,-0.00475,-0.00547,-0.00256]}, + {"t":1.90944, "x":5.49218, "y":5.74101, "heading":-2.01556, "vx":-0.92264, "vy":0.38505, "omega":0.6451, "ax":0.00013, "ay":0.00031, "alpha":-0.00057, "fx":[-0.00054,0.00113,0.0049,0.00323], "fy":[0.00634,0.00333,0.00302,0.00819]}, + {"t":1.95095, "x":5.45388, "y":5.75699, "heading":-1.98878, "vx":-0.92263, "vy":0.38506, "omega":0.64507, "ax":-0.00017, "ay":-0.00041, "alpha":-0.00141, "fx":[-0.00886,-0.00197,0.00218,-0.0028], "fy":[-0.00683,-0.01159,-0.00688,-0.00212]}, + {"t":1.99246, "x":5.41558, "y":5.77297, "heading":-1.96201, "vx":-0.92264, "vy":0.38504, "omega":0.64501, "ax":0.00022, "ay":0.00053, "alpha":-0.0008, "fx":[-0.00361,0.005,0.01091,0.0023], "fy":[0.00272,0.01608,0.00456,0.01165]}, + {"t":2.03397, "x":5.37728, "y":5.78896, "heading":-1.93523, "vx":-0.92263, "vy":0.38506, "omega":0.64498, "ax":-0.00029, "ay":-0.00069, "alpha":-0.00325, "fx":[-0.0168,0.00139,0.00484,-0.00868], "fy":[-0.01457,-0.02164,-0.0085,-0.00143]}, + {"t":2.07547, "x":5.33898, "y":5.80494, "heading":-1.90846, "vx":-0.92264, "vy":0.38503, "omega":0.64484, "ax":0.0003, "ay":0.00073, "alpha":-0.00056, "fx":[-0.00875,0.01041,0.01889,-0.00027], "fy":[-0.00741,0.04119,0.00272,0.01211]}, + {"t":2.11698, "x":5.30069, "y":5.82092, "heading":-1.88169, "vx":-0.92263, "vy":0.38506, "omega":0.64482, "ax":-0.0004, "ay":-0.00097, "alpha":-0.00598, "fx":[-0.02738,0.00855,0.00892,-0.01695], "fy":[-0.02401,-0.03366,-0.00817,0.00149]}, + {"t":2.15849, "x":5.26239, "y":5.83691, "heading":-1.85493, "vx":-0.92264, "vy":0.38502, "omega":0.64457, "ax":0.00035, "ay":0.00085, "alpha":0.00019, "fx":[-0.01683,0.01741,0.0286,-0.00564], "fy":[-0.0272,0.08081,-0.0045,0.00729]}, + {"t":2.2, "x":5.22409, "y":5.85289, "heading":-1.82817, "vx":-0.92263, "vy":0.38506, "omega":0.64458, "ax":-0.00035, "ay":-0.00084, "alpha":-0.0098, "fx":[-0.03814,0.02286,0.01734,-0.02552], "fy":[-0.02929,-0.04143,0.00118,0.01332]}, + {"t":2.24151, "x":5.18579, "y":5.86887, "heading":-1.80141, "vx":-0.92264, "vy":0.38502, "omega":0.64417, "ax":0.00037, "ay":0.00088, "alpha":0.0003, "fx":[-0.028,0.02672,0.04024,-0.01448], "fy":[-0.05393,0.12481,-0.01303,0.00081]}, + {"t":2.28302, "x":5.14749, "y":5.88486, "heading":-1.77467, "vx":-0.92263, "vy":0.38506, "omega":0.64419, "ax":0.00012, "ay":0.00029, "alpha":-0.01481, "fx":[-0.04414,0.04603,0.03609,-0.02984], "fy":[-0.02103,-0.03511,0.03079,0.04486]}, + {"t":2.32453, "x":5.1092, "y":5.90084, "heading":-1.74793, "vx":-0.92262, "vy":0.38507, "omega":0.64357, "ax":-0.00001, "ay":-0.00002, "alpha":-0.00506, "fx":[-0.04901,0.0337,0.04875,-0.03396], "fy":[-0.08509,0.10425,-0.01776,-0.00264]}, + {"t":2.36604, "x":5.0709, "y":5.91682, "heading":-1.72122, "vx":-0.92262, "vy":0.38507, "omega":0.64336, "ax":0.00074, "ay":0.00178, "alpha":-0.02124, "fx":[-0.0476,0.06138,0.06807,-0.03228], "fy":[-0.01295,-0.02826,0.07232,0.08762]}, + {"t":2.40755, "x":5.0326, "y":5.93281, "heading":-1.69451, "vx":-0.92259, "vy":0.38515, "omega":0.64248, "ax":-0.0026, "ay":-0.00623, "alpha":-0.02869, "fx":[-0.11476,0.01234,0.02804,-0.09906], "fy":[-0.14864,-0.20708,-0.03774,-0.02213]}, + {"t":2.44906, "x":4.9943, "y":5.94879, "heading":-1.66784, "vx":-0.9227, "vy":0.38489, "omega":0.64129, "ax":0.00077, "ay":0.00183, "alpha":-0.03091, "fx":[-0.06034,0.03585,0.12016,-0.04463], "fy":[-0.04426,-0.06016,0.10543,0.12133]}, + {"t":2.49057, "x":4.956, "y":5.96477, "heading":-1.64123, "vx":-0.92267, "vy":0.38496, "omega":0.64001, "ax":-0.00069, "ay":-0.00164, "alpha":-0.08897, "fx":[-0.12858,0.09063,0.10573,-0.11349], "fy":[0.07207,-0.74605,0.2747,0.28962]}, + {"t":2.53208, "x":4.9177, "y":5.98075, "heading":-1.61466, "vx":-0.9227, "vy":0.3849, "omega":0.63631, "ax":0.00095, "ay":0.00227, "alpha":-0.05827, "fx":[-0.11529,0.07086,0.20978,-0.10228], "fy":[-0.11299,-0.12643,0.18862,0.20212]}, + {"t":2.57359, "x":4.8794, "y":5.99673, "heading":-1.58825, "vx":-0.92266, "vy":0.38499, "omega":0.6339, "ax":0.00444, "ay":0.01063, "alpha":-0.13374, "fx":[-0.15217,0.29439,0.29915,-0.14536], "fy":[0.16766,-0.66326,0.59851,0.60595]}, + {"t":2.6151, "x":4.84111, "y":6.01272, "heading":-1.56193, "vx":-0.92247, "vy":0.38543, "omega":0.62834, "ax":-0.3427, "ay":-0.86705, "alpha":-0.13351, "fx":[-6.04171,-5.33828,-5.4189,-6.0516], "fy":[-14.77096,-14.76781,-14.13612,-14.1385]}, + {"t":2.65661, "x":4.80252, "y":6.02797, "heading":-1.53585, "vx":-0.9367, "vy":0.34944, "omega":0.6228, "ax":-0.83567, "ay":-2.72205, "alpha":-0.12407, "fx":[-14.32993,-13.51383,-13.5023,-14.37506], "fy":[-46.02616,-45.07163,-45.19359,-45.20954]}, + {"t":2.69812, "x":4.76292, "y":6.04013, "heading":-1.51, "vx":-0.97139, "vy":0.23645, "omega":0.61765, "ax":-0.52282, "ay":-2.92363, "alpha":-0.20668, "fx":[-9.16164,-8.33107,-8.12379,-9.24434], "fy":[-49.28172,-49.23137,-48.18992,-48.23879]}, + {"t":2.73963, "x":4.72215, "y":6.04743, "heading":-1.48436, "vx":-0.99309, "vy":0.11509, "omega":0.60907, "ax":-0.15857, "ay":-2.98341, "alpha":-0.31322, "fx":[-3.31184,-1.84203,-1.9724,-3.44703], "fy":[-50.32026,-50.73591,-48.88092,-48.99117]}, + {"t":2.78114, "x":4.68079, "y":6.04963, "heading":-1.45908, "vx":-0.99967, "vy":-0.00875, "omega":0.59607, "ax":0.21323, "ay":-2.98565, "alpha":-0.36998, "fx":[2.72774,4.51251,4.44807,2.52919], "fy":[-50.74201,-50.53318,-48.79501,-49.00749]}, + {"t":2.82265, "x":4.63948, "y":6.0467, "heading":-1.43434, "vx":-0.99082, "vy":-0.13268, "omega":0.58071, "ax":0.58342, "ay":-2.93838, "alpha":-0.51287, "fx":[8.60548,11.12803,10.85793,8.30983], "fy":[-50.23005,-50.17197,-47.57645,-47.94735]}, + {"t":2.86416, "x":4.59885, "y":6.03866, "heading":-1.41023, "vx":-0.9666, "vy":-0.25465, "omega":0.55942, "ax":0.94633, "ay":-2.84373, "alpha":-0.65819, "fx":[14.33258,17.62412,17.23673,13.90584], "fy":[-49.2215,-48.60404,-45.58857,-46.20023]}, + {"t":2.90566, "x":4.55954, "y":6.02564, "heading":-1.38701, "vx":-0.92732, "vy":-0.37269, "omega":0.5321, "ax":1.30116, "ay":-2.70073, "alpha":-0.88833, "fx":[19.81409,24.15862,23.59406,19.1924], "fy":[-47.44536,-46.76003,-42.45963,-43.41431]}, + {"t":2.94717, "x":4.52217, "y":6.00784, "heading":-1.36492, "vx":-0.87331, "vy":-0.4848, "omega":0.49523, "ax":1.64311, "ay":-2.50801, "alpha":-1.16659, "fx":[24.96878,30.71724,29.81936,24.05378], "fy":[-45.21408,-43.76146,-38.40809,-39.84582]}, + {"t":2.98868, "x":4.48733, "y":5.98556, "heading":-1.34436, "vx":-0.80511, "vy":-0.58891, "omega":0.4468, "ax":1.90261, "ay":-2.31776, "alpha":-1.547, "fx":[28.62254,36.08547,34.87942,27.27496], "fy":[-43.23775,-41.19522,-34.01444,-36.09611]}, + {"t":3.03019, "x":4.45555, "y":5.95911, "heading":-1.32582, "vx":-0.72613, "vy":-0.68512, "omega":0.38259, "ax":0.672, "ay":-0.73604, "alpha":-1.06493, "fx":[9.26992,14.47261,13.01834,8.04673], "fy":[-15.38138,-14.11156,-9.15965,-10.42512]}, + {"t":3.06185, "x":4.43291, "y":5.93706, "heading":-1.31371, "vx":-0.70486, "vy":-0.70841, "omega":0.34888, "ax":0.7149, "ay":-0.69662, "alpha":-0.83486, "fx":[10.45673,14.38688,13.38417,9.44039], "fy":[-14.07624,-13.08684,-9.1183,-10.16807]}, + {"t":3.0935, "x":4.41095, "y":5.91429, "heading":-1.30266, "vx":-0.68223, "vy":-0.73046, "omega":0.32245, "ax":0.73984, "ay":-0.66983, "alpha":-0.66886, "fx":[11.16583,14.40673,13.42768,10.33055], "fy":[-13.1522,-12.27834,-9.18102,-10.05166]}, + {"t":3.12515, "x":4.38973, "y":5.89083, "heading":-1.29246, "vx":-0.65881, "vy":-0.75167, "omega":0.30128, "ax":0.76646, "ay":-0.63891, "alpha":-0.52717, "fx":[11.89793,14.33781,13.65771,11.21236], "fy":[-12.17084,-11.65837,-9.03201,-9.74007]}, + {"t":3.15681, "x":4.36926, "y":5.86672, "heading":-1.28292, "vx":-0.63455, "vy":-0.77189, "omega":0.2846, "ax":0.82993, "ay":-0.55361, "alpha":-0.41706, "fx":[13.12758,15.1613,14.47583,12.57318], "fy":[-10.47527,-9.89558,-7.9829,-8.56011]}, + {"t":3.18846, "x":4.34959, "y":5.842, "heading":-1.27391, "vx":-0.60828, "vy":-0.78942, "omega":0.27139, "ax":0.92507, "ay":-0.37312, "alpha":-0.32132, "fx":[14.90327,16.38452,15.93879,14.45554], "fy":[-7.16867,-6.79805,-5.22632,-5.68575]}, + {"t":3.22012, "x":4.3308, "y":5.81683, "heading":-1.26532, "vx":-0.579, "vy":-0.80123, "omega":0.26122, "ax":0.98654, "ay":-0.14725, "alpha":-0.26075, "fx":[15.9973,17.36494,16.77628,15.64211], "fy":[-3.21183,-2.8457,-1.69803,-2.0628]}, + {"t":3.25177, "x":4.31296, "y":5.79139, "heading":-1.25705, "vx":-0.54777, "vy":-0.80589, "omega":0.25297, "ax":0.99437, "ay":0.08012, "alpha":-0.18505, "fx":[16.2834,17.14858,16.86804,16.00251], "fy":[0.7412,1.09427,1.8945,1.61222]}, + {"t":3.28342, "x":4.29612, "y":5.76592, "heading":-1.24905, "vx":-0.51629, "vy":-0.80335, "omega":0.24711, "ax":0.95905, "ay":0.27536, "alpha":-0.15547, "fx":[15.71549,16.58872,16.14449,15.49906], "fy":[4.15465,4.37069,5.0257,4.80934]}, + {"t":3.31508, "x":4.28026, "y":5.74063, "heading":-1.24122, "vx":-0.48594, "vy":-0.79463, "omega":0.24219, "ax":0.90216, "ay":0.42684, "alpha":-0.09932, "fx":[14.88273,15.35907,15.1943,14.71789], "fy":[6.75865,7.06141,7.40142,7.23922]}, + {"t":3.34673, "x":4.26533, "y":5.71569, "heading":-1.23356, "vx":-0.45738, "vy":-0.78112, "omega":0.23905, "ax":0.8403, "ay":0.53891, "alpha":-0.08862, "fx":[13.84403,14.39806,14.06467,13.72269], "fy":[8.75188,8.86908,9.21553,9.09675]}, + {"t":3.37838, "x":4.25127, "y":5.69124, "heading":-1.22599, "vx":-0.43078, "vy":-0.76407, "omega":0.23624, "ax":0.78192, "ay":0.6209, "alpha":-0.04661, "fx":[12.95852,13.19666,13.11046,12.8714], "fy":[10.14027,10.41092,10.467,10.38191]}, + {"t":3.41004, "x":4.23803, "y":5.66736, "heading":-1.21851, "vx":-0.40603, "vy":-0.74441, "omega":0.23477, "ax":0.73005, "ay":0.68138, "alpha":-0.04605, "fx":[12.07786,12.40306,12.17919,12.01799], "fy":[11.24934,11.30557,11.46796,11.41014]}, + {"t":3.44169, "x":4.22554, "y":5.64414, "heading":-1.21108, "vx":-0.38292, "vy":-0.72284, "omega":0.23331, "ax":0.68509, "ay":0.72676, "alpha":-0.01671, "fx":[11.38875,11.48955,11.45209,11.34988], "fy":[11.99498,12.22737,12.13742,12.09919]}, + {"t":3.47335, "x":4.21377, "y":5.62163, "heading":-1.2037, "vx":-0.36124, "vy":-0.69984, "omega":0.23278, "ax":0.64645, "ay":0.76149, "alpha":-0.0214, "fx":[10.72693,10.90868,10.76499,10.70315], "fy":[12.65255,12.67393,12.73538,12.71283]}, + {"t":3.505, "x":4.20266, "y":5.59985, "heading":-1.19633, "vx":-0.34077, "vy":-0.67573, "omega":0.2321, "ax":0.61324, "ay":0.7886, "alpha":-0.00213, "fx":[10.21284,10.24427,10.23285,10.19996], "fy":[13.07678,13.26947,13.1246,13.11146]}, + {"t":3.53665, "x":4.19218, "y":5.57886, "heading":-1.18898, "vx":-0.32136, "vy":-0.65077, "omega":0.23203, "ax":0.58461, "ay":0.81016, "alpha":-0.00974, "fx":[9.71763,9.82372,9.7289,9.71049], "fy":[13.49359,13.49919,13.51672,13.51029]}, + {"t":3.56831, "x":4.1823, "y":5.55867, "heading":-1.18164, "vx":-0.30286, "vy":-0.62513, "omega":0.23173, "ax":0.55978, "ay":0.8276, "alpha":0.00174, "fx":[9.3281,9.33847,9.33517,9.32348], "fy":[13.74716,13.90936,13.76554,13.7604]}, + {"t":3.59996, "x":4.17299, "y":5.53929, "heading":-1.1743, "vx":-0.28514, "vy":-0.59893, "omega":0.23178, "ax":0.53812, "ay":0.84192, "alpha":-0.00813, "fx":[8.94838,9.03248,8.95845,8.94148], "fy":[14.02356,14.02927,14.04542,14.03912]}, + {"t":3.63161, "x":4.16423, "y":5.52076, "heading":-1.16696, "vx":-0.26811, "vy":-0.57228, "omega":0.23152, "ax":0.5191, "ay":0.85384, "alpha":-0.00284, "fx":[8.64503,8.67305,8.66176,8.6326], "fy":[14.17749,14.32686,14.22032,14.20773]}, + {"t":3.66327, "x":4.15601, "y":5.50307, "heading":-1.15964, "vx":-0.25167, "vy":-0.54525, "omega":0.23143, "ax":0.50229, "ay":0.86389, "alpha":-0.0152, "fx":[8.34489,8.45063,8.37422,8.32208], "fy":[14.3641,14.38563,14.43734,14.41539]}, + {"t":3.69492, "x":4.14829, "y":5.48624, "heading":-1.15231, "vx":-0.23577, "vy":-0.51791, "omega":0.23095, "ax":0.48735, "ay":0.87245, "alpha":-0.01563, "fx":[8.10117,8.18377,8.14723,8.06364], "fy":[14.45334,14.60907,14.57382,14.53716]}, + {"t":3.72658, "x":4.14107, "y":5.47029, "heading":-1.145, "vx":-0.22035, "vy":-0.49029, "omega":0.23046, "ax":0.474, "ay":0.87982, "alpha":-0.03129, "fx":[7.85639,8.02594,7.92446,7.79884], "fy":[14.57543,14.63121,14.75705,14.70096]}, + {"t":3.75823, "x":4.13434, "y":5.45521, "heading":-1.1377, "vx":-0.20534, "vy":-0.46244, "omega":0.22947, "ax":0.46201, "ay":0.88621, "alpha":-0.03794, "fx":[7.65373,7.83305,7.74963,7.56945], "fy":[14.61629,14.80256,14.87691,14.79531]}, + {"t":3.78988, "x":4.12807, "y":5.44101, "heading":-1.13044, "vx":-0.19072, "vy":-0.43439, "omega":0.22827, "ax":0.45118, "ay":0.89181, "alpha":-0.05842, "fx":[7.44756,7.72972,7.57635,7.33025], "fy":[14.68515,14.79978,15.04702,14.93221]}, + {"t":3.82154, "x":4.12226, "y":5.42771, "heading":-1.12322, "vx":-0.17644, "vy":-0.40616, "omega":0.22642, "ax":0.44136, "ay":0.89674, "alpha":-0.07286, "fx":[7.27221,7.60308,7.44272,7.11113], "fy":[14.68316,14.93126,15.16703,15.0115]}, + {"t":3.85319, "x":4.11689, "y":5.4153, "heading":-1.11605, "vx":-0.16247, "vy":-0.37778, "omega":0.22411, "ax":0.43242, "ay":0.90111, "alpha":-0.10071, "fx":[7.09191,7.55256,7.30948,6.87898], "fy":[14.70031,14.90911,15.34197,15.13316]}, + {"t":3.88484, "x":4.11197, "y":5.4038, "heading":-1.10896, "vx":-0.14878, "vy":-0.34925, "omega":0.22092, "ax":0.42425, "ay":0.90502, "alpha":-0.12573, "fx":[6.93312,7.49251,7.21115,6.65113], "fy":[14.65253,15.00776,15.47823,15.20622]}, + {"t":3.9165, "x":4.10747, "y":5.39319, "heading":-1.10196, "vx":-0.13535, "vy":-0.32061, "omega":0.21694, "ax":0.41675, "ay":0.90852, "alpha":-0.1649, "fx":[6.76811,7.50083,7.11285,6.4061], "fy":[14.61125,14.96696,15.67765,15.32227]}, + {"t":3.94815, "x":4.10339, "y":5.3835, "heading":-1.09509, "vx":-0.12216, "vy":-0.29185, "omega":0.21172, "ax":0.40984, "ay":0.91167, "alpha":-0.20508, "fx":[6.61626,7.51657,7.04779,6.147], "fy":[14.5069,15.03493,15.84968,15.39704]}, + {"t":3.97981, "x":4.09973, "y":5.37472, "heading":-1.08839, "vx":-0.10919, "vy":-0.26299, "omega":0.20523, "ax":0.40347, "ay":0.91453, "alpha":-0.26153, "fx":[6.45683,7.59786,6.98333,5.86459], "fy":[14.39093,14.97362,16.09807,15.5165]}, + {"t":4.01146, "x":4.09648, "y":5.36685, "heading":-1.0819, "vx":-0.09641, "vy":-0.23404, "omega":0.19695, "ax":0.39757, "ay":0.91713, "alpha":-0.32411, "fx":[6.30175,7.71014,6.95289,5.54426], "fy":[14.20922,15.0095,16.33228,15.60141]}, + {"t":4.04311, "x":4.09363, "y":5.3599, "heading":-1.07566, "vx":-0.08383, "vy":-0.20501, "omega":0.18669, "ax":0.39209, "ay":0.9195, "alpha":-0.40695, "fx":[6.13714,7.89064,6.92491,5.19089], "fy":[13.99034,14.92218,16.66352,15.7346]}, + {"t":4.07477, "x":4.09117, "y":5.35388, "heading":-1.06975, "vx":-0.07142, "vy":-0.17591, "omega":0.17381, "ax":0.38698, "ay":0.92168, "alpha":-0.50308, "fx":[5.96685,8.13444,6.93475,4.76733], "fy":[13.69661,14.92065,16.99811,15.84024]}, + {"t":4.10642, "x":4.0891, "y":5.34877, "heading":-1.06425, "vx":-0.05917, "vy":-0.14673, "omega":0.15789, "ax":0.38222, "ay":0.92368, "alpha":-0.62621, "fx":[5.78337,8.4574,6.95077,4.29438], "fy":[13.32975,14.79709,17.46113,16.0009]}, + {"t":4.13807, "x":4.08742, "y":5.34459, "heading":-1.05925, "vx":-0.04707, "vy":-0.11749, "omega":0.13807, "ax":0.37777, "ay":0.92552, "alpha":-0.77294, "fx":[5.58157,8.8884,7.01241,3.70666], "fy":[12.86905,14.74666,17.95332,16.14274]}, + {"t":4.16973, "x":4.08612, "y":5.34133, "heading":-1.05488, "vx":-0.03511, "vy":-0.0882, "omega":0.1136, "ax":0.3736, "ay":0.92722, "alpha":-0.95754, "fx":[5.35945,9.42192,7.08759,3.04183], "fy":[12.28438,14.56999,18.61989,16.35117]}, + {"t":4.20138, "x":4.0852, "y":5.33901, "heading":-1.05129, "vx":-0.02329, "vy":-0.05885, "omega":0.08329, "ax":0.36968, "ay":0.9288, "alpha":-1.18102, "fx":[5.10114,10.12631,7.22206,2.20001], "fy":[11.5711,14.44948,19.35617,16.55408]}, + {"t":4.23304, "x":4.08464, "y":5.33761, "heading":-1.04865, "vx":-0.01159, "vy":-0.02945, "omega":0.04591, "ax":0.36599, "ay":0.93027, "alpha":-1.45026, "fx":[4.8079,10.96468,7.38494,1.2462], "fy":[10.72733,14.18431,20.29497,16.82221]}, + {"t":4.26469, "x":4.08446, "y":5.33714, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/startToL.traj b/src/main/deploy/choreo/startToL.traj index 8b48bf32..f4deb3f6 100644 --- a/src/main/deploy/choreo/startToL.traj +++ b/src/main/deploy/choreo/startToL.traj @@ -3,7 +3,7 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1008875, "y":6.1663548, "heading":3.141592653589793, "intervals":76, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.1, "y":5.07, "heading":3.14159, "intervals":78, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":4.180931568145752, "y":5.788817405700684, "heading":-1.2827410322432835, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":3.798672364905389, "y":5.1721419902635795, "heading":5.235987755982989, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ @@ -17,7 +17,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"start.x", "val":7.1}, "y":{"exp":"start.y", "val":5.07}, "heading":{"exp":"start.heading", "val":3.14159}, "intervals":76, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"start.x", "val":7.1}, "y":{"exp":"start.y", "val":5.07}, "heading":{"exp":"start.heading", "val":3.14159}, "intervals":78, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"4.180931568145752 m", "val":4.180931568145752}, "y":{"exp":"5.788817405700684 m", "val":5.788817405700684}, "heading":{"exp":"-1.2827410322432835 rad", "val":-1.2827410322432835}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"L.x", "val":3.798672364905389}, "y":{"exp":"L.y", "val":5.1721419902635795}, "heading":{"exp":"L.heading", "val":5.235987755982989}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ @@ -34,123 +34,125 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,3.12876,4.35798], + "waypoints":[0.0,3.22992,4.46371], "samples":[ - {"t":0.0, "x":7.10089, "y":6.16635, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.98273, "ay":-0.30728, "alpha":3.8714, "fx":[-40.82092,-41.33439,-58.48722,-58.24], "fy":[-15.66273,5.20633,4.9747,-15.0074]}, - {"t":0.04117, "x":7.09836, "y":6.16609, "heading":3.14159, "vx":-0.12279, "vy":-0.01265, "omega":0.15938, "ax":-2.98336, "ay":-0.30735, "alpha":2.96383, "fx":[-42.91854,-43.26583,-56.46384,-56.27654], "fy":[-13.13377,2.72199,2.6241,-12.70578]}, - {"t":0.08234, "x":7.09078, "y":6.16531, "heading":-3.13503, "vx":-0.24561, "vy":-0.0253, "omega":0.28139, "ax":-2.98322, "ay":-0.30734, "alpha":2.30248, "fx":[-44.4086,-44.60572,-55.05709,-54.84391], "fy":[-11.22288,0.91475,0.82382,-11.0082]}, - {"t":0.1235, "x":7.07814, "y":6.16401, "heading":-3.12345, "vx":-0.36842, "vy":-0.03796, "omega":0.37618, "ax":-2.98302, "ay":-0.30731, "alpha":1.62387, "fx":[-45.97706,-46.48147,-53.34175,-53.10183], "fy":[-9.53153,-0.63661,-0.7888,-9.53422]}, - {"t":0.16467, "x":7.06044, "y":6.16219, "heading":-3.10796, "vx":-0.49123, "vy":-0.05061, "omega":0.44303, "ax":-2.98272, "ay":-0.30728, "alpha":1.2647, "fx":[-46.72322,-46.98968,-52.7184,-52.45095], "fy":[-8.36392,-1.74076,-1.91931,-8.46518]}, - {"t":0.20584, "x":7.03769, "y":6.15984, "heading":-3.08972, "vx":-0.61402, "vy":-0.06326, "omega":0.4951, "ax":-2.98223, "ay":-0.30723, "alpha":0.89603, "fx":[-47.32566,-48.03977,-51.87724,-51.60642], "fy":[-7.36459,-2.69071,-2.90652,-7.52391]}, - {"t":0.24701, "x":7.00989, "y":6.15698, "heading":-3.06934, "vx":-0.73679, "vy":-0.07591, "omega":0.53198, "ax":-2.98123, "ay":-0.30713, "alpha":0.64906, "fx":[-48.11291,-48.36144,-51.27731,-51.03079], "fy":[-6.71589,-3.27996,-3.57292,-6.91024]}, - {"t":0.28818, "x":6.97703, "y":6.15359, "heading":-3.04744, "vx":-0.85952, "vy":-0.08855, "omega":0.5587, "ax":-2.9782, "ay":-0.30682, "alpha":0.47167, "fx":[-48.51126,-48.50082,-50.89624,-50.67208], "fy":[-6.1755,-3.86308,-4.05715,-6.36225]}, - {"t":0.32934, "x":6.93912, "y":6.14969, "heading":-3.02444, "vx":-0.98213, "vy":-0.10118, "omega":0.57812, "ax":-0.30033, "ay":-0.03069, "alpha":0.32695, "fx":[-4.02003,-4.2278,-5.99268,-5.7852], "fy":[-1.12477,-0.06259,0.47599,-1.33514]}, - {"t":0.37051, "x":6.89843, "y":6.1455, "heading":-3.00064, "vx":-0.99449, "vy":-0.10244, "omega":0.59158, "ax":0.00005, "ay":-0.00048, "alpha":0.29144, "fx":[0.65706,0.8826,-0.86216,-0.67437], "fy":[-0.56879,0.73707,0.55258,-0.75319]}, - {"t":0.41168, "x":6.85749, "y":6.14128, "heading":-2.97628, "vx":-0.99449, "vy":-0.10246, "omega":0.60358, "ax":-0.00003, "ay":0.00025, "alpha":0.21684, "fx":[0.56117,0.40128,-0.56204,-0.40214], "fy":[-0.46636,0.7582,0.35428,-0.62944]}, - {"t":0.45285, "x":6.81655, "y":6.13706, "heading":-2.95144, "vx":-0.99449, "vy":-0.10245, "omega":0.61251, "ax":0.0, "ay":0.0, "alpha":0.15289, "fx":[0.42419,0.32819,-0.44603,-0.30636], "fy":[-0.28917,0.4261,0.28922,-0.42605]}, - {"t":0.49401, "x":6.77561, "y":6.13284, "heading":-2.92622, "vx":-0.99449, "vy":-0.10245, "omega":0.6188, "ax":0.00001, "ay":-0.00006, "alpha":0.11428, "fx":[0.32433,0.20855,-0.32412,-0.20834], "fy":[-0.21872,0.34444,0.2071,-0.33689]}, - {"t":0.53518, "x":6.73467, "y":6.12863, "heading":-2.90075, "vx":-0.99449, "vy":-0.10246, "omega":0.62351, "ax":-0.00001, "ay":0.00013, "alpha":0.08502, "fx":[0.25321,0.15224,-0.25279,-0.15358], "fy":[-0.14838,0.25003,0.15285,-0.24557]}, - {"t":0.57635, "x":6.69373, "y":6.12441, "heading":-2.87508, "vx":-0.99449, "vy":-0.10245, "omega":0.62701, "ax":0.00001, "ay":-0.00014, "alpha":0.06395, "fx":[0.19001,0.10903,-0.18952,-0.10854], "fy":[-0.11254,0.18865,0.10966,-0.19523]}, - {"t":0.61752, "x":6.65279, "y":6.12019, "heading":-2.84927, "vx":-0.99449, "vy":-0.10246, "omega":0.62964, "ax":-0.00002, "ay":0.00015, "alpha":0.04875, "fx":[0.1495,0.07969,-0.14956,-0.08065], "fy":[-0.07638,0.14873,0.08133,-0.14378]}, - {"t":0.65869, "x":6.61185, "y":6.11597, "heading":-2.82334, "vx":-0.99449, "vy":-0.10245, "omega":0.63165, "ax":0.00001, "ay":-0.00012, "alpha":0.03654, "fx":[0.11302,0.05718,-0.11261,-0.05676], "fy":[-0.05807,0.10655,0.05871,-0.11531]}, - {"t":0.69985, "x":6.5709, "y":6.11176, "heading":-2.79734, "vx":-0.99449, "vy":-0.10246, "omega":0.63315, "ax":-0.00001, "ay":0.00009, "alpha":0.02866, "fx":[0.09057,0.04233,-0.09077,-0.04276], "fy":[-0.03988,0.08956,0.04294,-0.0865]}, - {"t":0.74102, "x":6.52996, "y":6.10754, "heading":-2.77128, "vx":-0.99449, "vy":-0.10245, "omega":0.63433, "ax":0.00001, "ay":-0.00007, "alpha":0.0217, "fx":[0.06861,0.02987,-0.06836,-0.02962], "fy":[-0.03063,0.06544,0.03117,-0.0708]}, - {"t":0.78219, "x":6.48902, "y":6.10332, "heading":-2.74516, "vx":-0.99449, "vy":-0.10245, "omega":0.63522, "ax":-0.00001, "ay":0.00005, "alpha":0.01743, "fx":[0.05711,0.02254,-0.05721,-0.02278], "fy":[-0.02075,0.05527,0.02244,-0.05358]}, - {"t":0.82336, "x":6.44808, "y":6.0991, "heading":-2.71901, "vx":-0.99449, "vy":-0.10245, "omega":0.63594, "ax":0.0, "ay":-0.00004, "alpha":0.01333, "fx":[0.04277,0.01522,-0.04264,-0.01509], "fy":[-0.01589,0.0419,0.01668,-0.04519]}, - {"t":0.86453, "x":6.40714, "y":6.09488, "heading":-2.69283, "vx":-0.99449, "vy":-0.10245, "omega":0.63649, "ax":0.0, "ay":0.00002, "alpha":0.01105, "fx":[0.03797,0.0118,-0.03797,-0.01196], "fy":[-0.01039,0.03516,0.01117,-0.03437]}, - {"t":0.90569, "x":6.3662, "y":6.09067, "heading":-2.66663, "vx":-0.99449, "vy":-0.10245, "omega":0.63694, "ax":0.0, "ay":-0.00002, "alpha":0.00852, "fx":[0.02746,0.00705,-0.02741,-0.00699], "fy":[-0.00767,0.02824,0.00874,-0.03035]}, - {"t":0.94686, "x":6.32526, "y":6.08645, "heading":-2.64041, "vx":-0.99449, "vy":-0.10245, "omega":0.6373, "ax":0.0, "ay":0.00001, "alpha":0.00736, "fx":[0.02686,0.00566,-0.02681,-0.00577], "fy":[-0.00445,0.02311,0.0047,-0.02286]}, - {"t":0.98803, "x":6.28432, "y":6.08223, "heading":-2.61417, "vx":-0.99449, "vy":-0.10245, "omega":0.6376, "ax":0.0, "ay":0.0, "alpha":0.00569, "fx":[0.01812,0.00221,-0.01811,-0.0022], "fy":[-0.00281,0.02019,0.00404,-0.02162]}, - {"t":1.0292, "x":6.24338, "y":6.07801, "heading":-2.58792, "vx":-0.99449, "vy":-0.10245, "omega":0.63783, "ax":0.0, "ay":0.0, "alpha":0.00515, "fx":[0.02024,0.00183,-0.02014,-0.00192], "fy":[-0.00077,0.01563,0.00073,-0.01566]}, - {"t":1.07036, "x":6.20243, "y":6.0738, "heading":-2.56166, "vx":-0.99449, "vy":-0.10245, "omega":0.63804, "ax":0.0, "ay":0.0, "alpha":0.00396, "fx":[0.01216,-0.00086,-0.01217,0.00085], "fy":[0.00031,0.01528,0.00097,-0.01633]}, - {"t":1.11153, "x":6.16149, "y":6.06958, "heading":-2.5354, "vx":-0.99449, "vy":-0.10245, "omega":0.63821, "ax":0.0, "ay":-0.00001, "alpha":0.00376, "fx":[0.01607,-0.00081,-0.01594,0.00072], "fy":[0.00172,0.0107,-0.00189,-0.01088]}, - {"t":1.1527, "x":6.12055, "y":6.06536, "heading":-2.50912, "vx":-0.99449, "vy":-0.10245, "omega":0.63836, "ax":0.0, "ay":0.00001, "alpha":0.00286, "fx":[0.00812,-0.00296,-0.00814,0.00293], "fy":[0.00251,0.0121,-0.00124,-0.01294]}, - {"t":1.19387, "x":6.07961, "y":6.06114, "heading":-2.48284, "vx":-0.99449, "vy":-0.10245, "omega":0.63848, "ax":0.0, "ay":-0.00001, "alpha":0.00283, "fx":[0.01321,-0.00278,-0.01307,0.00269], "fy":[0.0035,0.00721,-0.00373,-0.00745]}, - {"t":1.23504, "x":6.03867, "y":6.05692, "heading":-2.45656, "vx":-0.99449, "vy":-0.10245, "omega":0.6386, "ax":0.0, "ay":0.00001, "alpha":0.00211, "fx":[0.00518,-0.00445,-0.0052,0.00442], "fy":[0.00415,0.00988,-0.00294,-0.01059]}, - {"t":1.2762, "x":5.99773, "y":6.05271, "heading":-2.43027, "vx":-0.99449, "vy":-0.10245, "omega":0.63868, "ax":0.0, "ay":-0.00001, "alpha":0.00216, "fx":[0.01104,-0.00433,-0.01088,0.00423], "fy":[0.0048,0.00453,-0.00505,-0.00478]}, - {"t":1.31737, "x":5.95679, "y":6.04849, "heading":-2.40397, "vx":-0.99449, "vy":-0.10245, "omega":0.63877, "ax":0.0, "ay":0.00001, "alpha":0.00158, "fx":[0.00288,-0.00551,-0.00291,0.00548], "fy":[0.00542,0.00818,-0.00428,-0.00878]}, - {"t":1.35854, "x":5.91585, "y":6.04427, "heading":-2.37768, "vx":-0.99449, "vy":-0.10245, "omega":0.63884, "ax":0.0, "ay":-0.00001, "alpha":0.00163, "fx":[0.00917,-0.00554,-0.00901,0.00542], "fy":[0.00572,0.00228,-0.00596,-0.00253]}, - {"t":1.39971, "x":5.87491, "y":6.04005, "heading":-2.35138, "vx":-0.99449, "vy":-0.10245, "omega":0.6389, "ax":0.0, "ay":0.00001, "alpha":0.00117, "fx":[0.00096,-0.0062,-0.00098,0.00618], "fy":[0.00635,0.00675,-0.00534,-0.00725]}, - {"t":1.44088, "x":5.83396, "y":6.03584, "heading":-2.32508, "vx":-0.99449, "vy":-0.10245, "omega":0.63895, "ax":0.0, "ay":-0.00001, "alpha":0.00119, "fx":[0.00743,-0.00642,-0.00727,0.0063], "fy":[0.00629,0.00031,-0.0065,-0.00053]}, - {"t":1.48204, "x":5.79302, "y":6.03162, "heading":-2.29877, "vx":-0.99449, "vy":-0.10245, "omega":0.639, "ax":0.0, "ay":0.00001, "alpha":0.00082, "fx":[-0.00073,-0.00655,0.00071,0.00653], "fy":[0.00694,0.00547,-0.00613,-0.00585]}, - {"t":1.52321, "x":5.75208, "y":6.0274, "heading":-2.27247, "vx":-0.99449, "vy":-0.10245, "omega":0.63904, "ax":0.0, "ay":0.0, "alpha":0.00077, "fx":[0.0057,-0.00698,-0.00554,0.00684], "fy":[0.00652,-0.00149,-0.00667,0.00134]}, - {"t":1.56438, "x":5.71114, "y":6.02318, "heading":-2.24616, "vx":-0.99449, "vy":-0.10245, "omega":0.63907, "ax":0.0, "ay":0.0, "alpha":0.00052, "fx":[-0.00229,-0.00657,0.00227,0.00655], "fy":[0.00715,0.00427,-0.00666,-0.0045]}, - {"t":1.60555, "x":5.6702, "y":6.01896, "heading":-2.21985, "vx":-0.99449, "vy":-0.10245, "omega":0.63909, "ax":0.0, "ay":0.0, "alpha":0.00037, "fx":[0.00392,-0.00718,-0.00376,0.00703], "fy":[0.00641,-0.00318,-0.00645,0.00313]}, - {"t":1.64671, "x":5.62926, "y":6.01475, "heading":-2.19354, "vx":-0.99449, "vy":-0.10245, "omega":0.6391, "ax":0.0, "ay":0.0, "alpha":0.00022, "fx":[-0.00379,-0.00623,0.00378,0.00623], "fy":[0.00693,0.00316,-0.00692,-0.00314]}, - {"t":1.68788, "x":5.58832, "y":6.01053, "heading":-2.16723, "vx":-0.99449, "vy":-0.10245, "omega":0.63911, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[0.00202,-0.007,-0.00186,0.00683], "fy":[0.00593,-0.00484,-0.00585,0.00491]}, - {"t":1.72905, "x":5.54738, "y":6.00631, "heading":-2.14092, "vx":-0.99449, "vy":-0.10245, "omega":0.63911, "ax":0.0, "ay":0.0, "alpha":-0.0001, "fx":[-0.00534,-0.00553,0.00535,0.00554], "fy":[0.00621,0.00225,-0.00692,-0.00171]}, - {"t":1.77022, "x":5.50644, "y":6.00209, "heading":-2.11461, "vx":-0.99449, "vy":-0.10245, "omega":0.63911, "ax":0.0, "ay":0.0, "alpha":-0.00058, "fx":[-0.0001,-0.00639,0.00026,0.0062], "fy":[0.00502,-0.00661,-0.00488,0.00676]}, - {"t":1.81139, "x":5.46549, "y":5.99788, "heading":-2.0883, "vx":-0.99449, "vy":-0.10245, "omega":0.63908, "ax":0.0, "ay":0.0, "alpha":-0.00047, "fx":[-0.00709,-0.00443,0.0071,0.00443], "fy":[0.00485,0.00176,-0.00664,-0.00012]}, - {"t":1.85255, "x":5.42455, "y":5.99366, "heading":-2.06199, "vx":-0.99449, "vy":-0.10245, "omega":0.63906, "ax":0.0, "ay":0.0, "alpha":-0.00121, "fx":[-0.00256,-0.00526,0.00275,0.00507], "fy":[0.00355,-0.00872,-0.00355,0.00871]}, - {"t":1.89372, "x":5.38361, "y":5.98944, "heading":-2.03568, "vx":-0.99449, "vy":-0.10245, "omega":0.63901, "ax":0.0, "ay":0.00001, "alpha":-0.00091, "fx":[-0.00925,-0.00283,0.00922,0.0028], "fy":[0.0027,0.00222,-0.00602,0.00174]}, - {"t":1.93489, "x":5.34267, "y":5.98522, "heading":-2.00937, "vx":-0.99449, "vy":-0.10245, "omega":0.63897, "ax":0.0, "ay":-0.00002, "alpha":-0.00206, "fx":[-0.00555,-0.00348,0.00584,0.00334], "fy":[0.0012,-0.01159,-0.00195,0.01083]}, - {"t":1.97606, "x":5.30173, "y":5.981, "heading":-1.98307, "vx":-0.99449, "vy":-0.10245, "omega":0.63889, "ax":0.0, "ay":0.00005, "alpha":-0.00145, "fx":[-0.01213,-0.00061,0.01196,0.00045], "fy":[-0.00051,0.0046,-0.00496,0.00407]}, - {"t":2.01723, "x":5.26079, "y":5.97679, "heading":-1.95676, "vx":-0.99449, "vy":-0.10245, "omega":0.63883, "ax":0.00001, "ay":-0.00008, "alpha":-0.00324, "fx":[-0.00934,-0.00079,0.00988,0.00081], "fy":[-0.00255,-0.01586,-0.00018,0.01312]}, - {"t":2.05839, "x":5.21985, "y":5.97257, "heading":-1.93046, "vx":-0.99449, "vy":-0.10246, "omega":0.6387, "ax":-0.00001, "ay":0.00014, "alpha":-0.0021, "fx":[-0.01616,0.00249,0.01569,-0.00296], "fy":[-0.00522,0.01058,-0.00329,0.0071]}, - {"t":2.09956, "x":5.17891, "y":5.96835, "heading":-1.90417, "vx":-0.99449, "vy":-0.10245, "omega":0.63861, "ax":0.00002, "ay":-0.00021, "alpha":-0.00493, "fx":[-0.0143,0.00321,0.01534,-0.00282], "fy":[-0.00856,-0.02248,0.00161,0.01553]}, - {"t":2.14073, "x":5.13797, "y":5.96413, "heading":-1.87788, "vx":-0.99449, "vy":-0.10246, "omega":0.63841, "ax":-0.00003, "ay":0.00031, "alpha":-0.00288, "fx":[-0.02193,0.00688,0.02086,-0.00795], "fy":[-0.0122,0.02272,-0.00081,0.01107]}, - {"t":2.1819, "x":5.09702, "y":5.95992, "heading":-1.8516, "vx":-0.99449, "vy":-0.10245, "omega":0.63829, "ax":0.00005, "ay":-0.00044, "alpha":-0.00736, "fx":[-0.02094,0.00916,0.02289,-0.00808], "fy":[-0.01806,-0.03269,0.00337,0.018]}, - {"t":2.22306, "x":5.05608, "y":5.9557, "heading":-1.82532, "vx":-0.99449, "vy":-0.10246, "omega":0.63799, "ax":-0.00006, "ay":0.00061, "alpha":-0.00387, "fx":[-0.03018,0.01325,0.0281,-0.01533], "fy":[-0.02273,0.04429,0.00271,0.01611]}, - {"t":2.26423, "x":5.01514, "y":5.95148, "heading":-1.79906, "vx":-0.99449, "vy":-0.10244, "omega":0.63783, "ax":0.00008, "ay":-0.00081, "alpha":-0.01089, "fx":[-0.03001,0.01802,0.03338,-0.01581], "fy":[-0.03251,-0.04786,0.00542,0.02078]}, - {"t":2.3054, "x":4.9742, "y":5.94726, "heading":-1.7728, "vx":-0.99449, "vy":-0.10247, "omega":0.63738, "ax":-0.00011, "ay":0.00102, "alpha":-0.00531, "fx":[-0.04186,0.02276,0.03834,-0.02628], "fy":[-0.03903,0.07774,0.00745,0.02217]}, - {"t":2.34657, "x":4.93326, "y":5.94304, "heading":-1.74656, "vx":-0.99449, "vy":-0.10243, "omega":0.63716, "ax":0.00013, "ay":-0.00131, "alpha":-0.01605, "fx":[-0.04268,0.03123,0.04801,-0.02756], "fy":[-0.05313,-0.0689,0.00949,0.02525]}, - {"t":2.38774, "x":4.89232, "y":5.93883, "heading":-1.72033, "vx":-0.99449, "vy":-0.10248, "omega":0.6365, "ax":-0.00015, "ay":0.00147, "alpha":-0.00806, "fx":[-0.05822,0.03746,0.05318,-0.04251], "fy":[-0.06464,0.11955,0.01383,0.02917]}, - {"t":2.4289, "x":4.85138, "y":5.93461, "heading":-1.69413, "vx":-0.9945, "vy":-0.10242, "omega":0.63617, "ax":0.00018, "ay":-0.00177, "alpha":-0.02375, "fx":[-0.06117,0.05094,0.06853,-0.04616], "fy":[-0.07975,-0.09497,0.02083,0.03605]}, - {"t":2.47007, "x":4.81044, "y":5.93039, "heading":-1.66794, "vx":-0.99449, "vy":-0.1025, "omega":0.63519, "ax":-0.00016, "ay":0.00157, "alpha":-0.01457, "fx":[-0.08115,0.06143,0.07575,-0.06683], "fy":[-0.10438,0.14663,0.02413,0.03844]}, - {"t":2.51124, "x":4.7695, "y":5.92617, "heading":-1.64179, "vx":-0.99449, "vy":-0.10243, "omega":0.63459, "ax":0.00017, "ay":-0.00167, "alpha":-0.03573, "fx":[-0.08983,0.08048,0.09814,-0.07729], "fy":[-0.1087,-0.12115,0.05291,0.06536]}, - {"t":2.55241, "x":4.72855, "y":5.92195, "heading":-1.61566, "vx":-0.99449, "vy":-0.1025, "omega":0.63312, "ax":-0.00005, "ay":0.00044, "alpha":-0.03119, "fx":[-0.11411,0.1031,0.11259,-0.10462], "fy":[-0.16241,0.08536,0.04843,0.05809]}, - {"t":2.59358, "x":4.68761, "y":5.91773, "heading":-1.5896, "vx":-0.99449, "vy":-0.10248, "omega":0.63183, "ax":-0.00002, "ay":0.00015, "alpha":-0.05556, "fx":[-0.13739,0.12526,0.1438,-0.13267], "fy":[-0.13098,-0.13556,0.13585,0.14044]}, - {"t":2.63474, "x":4.64667, "y":5.91352, "heading":-1.56359, "vx":-0.99449, "vy":-0.10248, "omega":0.62955, "ax":0.00035, "ay":-0.00335, "alpha":-0.07235, "fx":[-0.16557,0.18031,0.17707,-0.16881], "fy":[-0.23126,-0.2392,0.12528,0.12204]}, - {"t":2.67591, "x":4.60573, "y":5.90929, "heading":-1.53767, "vx":-0.99448, "vy":-0.10261, "omega":0.62657, "ax":-0.00054, "ay":0.00526, "alpha":-0.09108, "fx":[-0.2203,0.19785,0.22262,-0.23634], "fy":[-0.14455,-0.12876,0.31998,0.30419]}, - {"t":2.71708, "x":4.56479, "y":5.90507, "heading":-1.51188, "vx":-0.9945, "vy":-0.1024, "omega":0.62282, "ax":0.0007, "ay":-0.00676, "alpha":-0.16516, "fx":[-0.26473,0.32368,0.28799,-0.30046], "fy":[-0.20853,-1.06202,0.4281,0.3917]}, - {"t":2.75825, "x":4.52385, "y":5.90085, "heading":-1.48623, "vx":-0.99447, "vy":-0.10268, "omega":0.61602, "ax":-0.00068, "ay":0.00658, "alpha":-0.16079, "fx":[-0.35987,0.36011,0.38418,-0.42969], "fy":[-0.32323,-0.25442,0.54276,0.47392]}, - {"t":2.79941, "x":4.48291, "y":5.89663, "heading":-1.46087, "vx":-0.9945, "vy":-0.1024, "omega":0.6094, "ax":-0.00549, "ay":0.0539, "alpha":-0.30295, "fx":[-0.57102,0.50743,0.38855,-0.691], "fy":[0.72514,-0.88702,1.93906,1.81701]}, - {"t":2.84058, "x":4.44196, "y":5.89246, "heading":-1.43579, "vx":-0.99472, "vy":-0.10019, "omega":0.59693, "ax":0.02106, "ay":-0.20072, "alpha":-0.30254, "fx":[-0.27123,1.1124,1.03794,-0.47491], "fy":[-4.18785,-3.9831,-2.50384,-2.70883]}, - {"t":2.88175, "x":4.40103, "y":5.88817, "heading":-1.41121, "vx":-0.99386, "vy":-0.10845, "omega":0.58447, "ax":0.22937, "ay":-1.60355, "alpha":-0.38776, "fx":[2.97206,4.98464,4.67682,2.66028], "fy":[-28.03534,-27.08365,-25.73519,-26.06778]}, - {"t":2.92292, "x":4.36031, "y":5.88234, "heading":-1.38715, "vx":-0.98441, "vy":-0.17446, "omega":0.56851, "ax":0.68252, "ay":-2.84204, "alpha":-0.53829, "fx":[10.25605,12.94943,12.49402,9.80949], "fy":[-48.86527,-48.35011,-45.88475,-46.40139]}, - {"t":2.96409, "x":4.32036, "y":5.87275, "heading":-1.36375, "vx":-0.95632, "vy":-0.29146, "omega":0.54635, "ax":1.04162, "ay":-2.78988, "alpha":-0.72392, "fx":[15.92458,19.41118,18.81377,15.30385], "fy":[-48.46701,-47.98918,-44.38001,-45.18736]}, - {"t":3.00525, "x":4.28187, "y":5.85839, "heading":-1.34125, "vx":-0.91343, "vy":-0.40632, "omega":0.51655, "ax":1.38165, "ay":-2.65139, "alpha":-0.93032, "fx":[21.18556,25.69008,24.95036,20.29975], "fy":[-46.929,-45.72192,-41.46231,-42.67652]}, - {"t":3.04642, "x":4.24544, "y":5.83942, "heading":-1.31999, "vx":-0.85656, "vy":-0.51547, "omega":0.47825, "ax":1.70014, "ay":-2.46459, "alpha":-1.25612, "fx":[25.96142,31.90065,30.76924,24.73091], "fy":[-44.76446,-43.32874,-37.22343,-39.01765]}, - {"t":3.08759, "x":4.21162, "y":5.81611, "heading":-1.3003, "vx":-0.78656, "vy":-0.61693, "omega":0.42654, "ax":1.99806, "ay":-2.23264, "alpha":-1.64062, "fx":[30.26126,38.03872,36.43883,28.48799], "fy":[-42.28515,-39.6846,-32.16372,-34.73486]}, - {"t":3.12876, "x":4.18093, "y":5.78882, "heading":-1.28274, "vx":-0.70431, "vy":-0.70884, "omega":0.359, "ax":0.70613, "ay":-0.69573, "alpha":-1.15894, "fx":[9.89512,15.19247,13.65855,8.33711], "fy":[-14.92184,-13.88995,-7.98864,-9.58925]}, - {"t":3.16111, "x":4.15852, "y":5.76552, "heading":-1.27113, "vx":-0.68147, "vy":-0.73135, "omega":0.32151, "ax":0.7384, "ay":-0.66687, "alpha":-0.91943, "fx":[10.80304,15.01351,13.74055,9.67815], "fy":[-14.17165,-12.46028,-8.25738,-9.57664]}, - {"t":3.19345, "x":4.13686, "y":5.74152, "heading":-1.26073, "vx":-0.65758, "vy":-0.75292, "omega":0.29177, "ax":0.75908, "ay":-0.64199, "alpha":-0.7272, "fx":[11.52639,14.81732,13.78612,10.48442], "fy":[-12.8031,-12.12928,-8.3971,-9.47729]}, - {"t":3.2258, "x":4.11599, "y":5.71683, "heading":-1.25129, "vx":-0.63303, "vy":-0.77369, "omega":0.26824, "ax":0.7786, "ay":-0.61663, "alpha":-0.57837, "fx":[12.03549,14.66696,13.81169,11.40159], "fy":[-12.31163,-11.0608,-8.43127,-9.3121]}, - {"t":3.25815, "x":4.09592, "y":5.69148, "heading":-1.24261, "vx":-0.60784, "vy":-0.79364, "omega":0.24953, "ax":0.79713, "ay":-0.59059, "alpha":-0.45159, "fx":[12.60541,14.65952,13.97145,11.91502], "fy":[-11.22266,-10.59505,-8.4223,-9.13921]}, - {"t":3.2905, "x":4.07667, "y":5.66549, "heading":-1.23454, "vx":-0.58205, "vy":-0.81274, "omega":0.23493, "ax":0.8163, "ay":-0.56152, "alpha":-0.32593, "fx":[13.00439,14.64728,14.07958,12.69834], "fy":[-10.3043,-9.95136,-8.30215,-8.88295]}, - {"t":3.32285, "x":4.05827, "y":5.63891, "heading":-1.22694, "vx":-0.55565, "vy":-0.8309, "omega":0.22438, "ax":0.93469, "ay":-0.32539, "alpha":-0.28514, "fx":[15.17465,16.43648,15.98694,14.72493], "fy":[-6.25601,-5.98861,-4.49371,-4.95835]}, - {"t":3.35519, "x":4.04078, "y":5.61186, "heading":-1.21968, "vx":-0.52541, "vy":-0.84143, "omega":0.21516, "ax":0.91332, "ay":0.39008, "alpha":-0.18696, "fx":[14.83866,15.84754,15.47563,14.73681], "fy":[5.99964,6.11504,7.13198,6.76318]}, - {"t":3.38754, "x":4.02427, "y":5.58485, "heading":-1.21272, "vx":-0.49587, "vy":-0.82881, "omega":0.20911, "ax":0.7635, "ay":0.63911, "alpha":-0.16259, "fx":[12.49171,13.25032,12.9629,12.20399], "fy":[10.05263,10.58386,11.13456,10.84357]}, - {"t":3.41989, "x":4.00863, "y":5.55837, "heading":-1.20596, "vx":-0.47117, "vy":-0.80814, "omega":0.20385, "ax":0.67561, "ay":0.73312, "alpha":-0.09433, "fx":[11.00237,11.619,11.38099,11.04588], "fy":[12.05166,11.94309,12.56096,12.32708]}, - {"t":3.45224, "x":3.99374, "y":5.53261, "heading":-1.19936, "vx":-0.44932, "vy":-0.78442, "omega":0.2008, "ax":0.62294, "ay":0.77927, "alpha":-0.09232, "fx":[10.24819,10.69725,10.52093,10.07002], "fy":[12.5741,13.09746,13.23582,13.0529]}, - {"t":3.48459, "x":3.97953, "y":5.50765, "heading":-1.19287, "vx":-0.42917, "vy":-0.75921, "omega":0.19781, "ax":0.58858, "ay":0.80609, "alpha":-0.04649, "fx":[9.64957,10.02396,9.87425,9.69741], "fy":[13.43487,13.23813,13.61127,13.46435]}, - {"t":3.51693, "x":3.96595, "y":5.48351, "heading":-1.18647, "vx":-0.41013, "vy":-0.73314, "omega":0.19631, "ax":0.56457, "ay":0.82346, "alpha":-0.05145, "fx":[9.33423,9.59429,9.48895,9.22672], "fy":[13.44376,13.88542,13.8457,13.73163]}, - {"t":3.54928, "x":3.95298, "y":5.46022, "heading":-1.18012, "vx":-0.39186, "vy":-0.7065, "omega":0.19465, "ax":0.5469, "ay":0.83556, "alpha":-0.0219, "fx":[9.01998,9.24648,9.15297,9.04681], "fy":[13.99513,13.78692,14.01154,13.91988]}, - {"t":3.58163, "x":3.94059, "y":5.43781, "heading":-1.17382, "vx":-0.37417, "vy":-0.67947, "omega":0.19394, "ax":0.53338, "ay":0.84445, "alpha":-0.02857, "fx":[8.84816,8.9974,8.93517,8.78396], "fy":[13.88439,14.23166,14.13111,14.05933]}, - {"t":3.61398, "x":3.92877, "y":5.41627, "heading":-1.16755, "vx":-0.35692, "vy":-0.65216, "omega":0.19301, "ax":0.52271, "ay":0.85125, "alpha":-0.00909, "fx":[8.6509,8.7936,8.73288,8.67577], "fy":[14.27646,14.08711,14.22797,14.16842]}, - {"t":3.64632, "x":3.9175, "y":5.39562, "heading":-1.16131, "vx":-0.34001, "vy":-0.62462, "omega":0.19272, "ax":0.51408, "ay":0.85662, "alpha":-0.01753, "fx":[8.54329,8.63629,8.59636,8.5017], "fy":[14.14034,14.41389,14.30643,14.25693]}, - {"t":3.67867, "x":3.90677, "y":5.37586, "heading":-1.15507, "vx":-0.32338, "vy":-0.59691, "omega":0.19215, "ax":0.50695, "ay":0.86095, "alpha":-0.00501, "fx":[8.40357,8.50962,8.46304,8.42648], "fy":[14.43043,14.27103,14.37544,14.32975]}, - {"t":3.71102, "x":3.89657, "y":5.357, "heading":-1.14886, "vx":-0.30698, "vy":-0.56906, "omega":0.19199, "ax":0.50098, "ay":0.86453, "alpha":-0.01539, "fx":[8.32958,8.40823,8.37324,8.29316], "fy":[14.29261,14.52246,14.43703,14.39292]}, - {"t":3.74337, "x":3.8869, "y":5.33905, "heading":-1.14265, "vx":-0.29078, "vy":-0.5411, "omega":0.19149, "ax":0.49589, "ay":0.86753, "alpha":-0.00849, "fx":[8.22169,8.32996,8.28064,8.23285], "fy":[14.51212,14.38914,14.49597,14.44768]}, - {"t":3.77572, "x":3.87776, "y":5.322, "heading":-1.13645, "vx":-0.27474, "vy":-0.51303, "omega":0.19122, "ax":0.49151, "ay":0.87007, "alpha":-0.02132, "fx":[8.16645,8.26775,8.22075,8.11822], "fy":[14.37626,14.58827,14.55281,14.49755]}, - {"t":3.80806, "x":3.86913, "y":5.30586, "heading":-1.13027, "vx":-0.25884, "vy":-0.48489, "omega":0.19053, "ax":0.4877, "ay":0.87227, "alpha":-0.01967, "fx":[8.07705,8.22495,8.15494,8.06218], "fy":[14.54225,14.46468,14.61129,14.54294]}, - {"t":3.84041, "x":3.86101, "y":5.29063, "heading":-1.1241, "vx":-0.24306, "vy":-0.45667, "omega":0.18989, "ax":0.48436, "ay":0.87418, "alpha":-0.03594, "fx":[8.03221,8.19481,8.11638,7.9527], "fy":[14.40511,14.62496,14.67188,14.58642]}, - {"t":3.87276, "x":3.8534, "y":5.27631, "heading":-1.11796, "vx":-0.22739, "vy":-0.42839, "omega":0.18873, "ax":0.4814, "ay":0.87585, "alpha":-0.04015, "fx":[7.95302,8.18303,8.0702,7.89248], "fy":[14.52485,14.50916,14.73797,14.628]}, - {"t":3.90511, "x":3.8463, "y":5.26292, "heading":-1.11186, "vx":-0.21182, "vy":-0.40006, "omega":0.18743, "ax":0.47876, "ay":0.87733, "alpha":-0.06145, "fx":[7.91308,8.18415,8.04883,7.77682], "fy":[14.38072,14.63792,14.81036,14.66969]}, - {"t":3.93746, "x":3.83969, "y":5.25043, "heading":-1.10579, "vx":-0.19633, "vy":-0.37168, "omega":0.18544, "ax":0.4764, "ay":0.87865, "alpha":-0.0732, "fx":[7.83829,8.20548,8.01935,7.70205], "fy":[14.45526,14.52688,14.89286,14.71162]}, - {"t":3.9698, "x":3.83359, "y":5.23887, "heading":-1.09979, "vx":-0.18092, "vy":-0.34326, "omega":0.18307, "ax":0.47426, "ay":0.87983, "alpha":-0.10199, "fx":[7.7984,8.24206,8.0135,7.56901], "fy":[14.29431,14.6282,14.98724,14.75569]}, - {"t":4.00215, "x":3.82799, "y":5.22823, "heading":-1.09387, "vx":-0.16558, "vy":-0.3148, "omega":0.17978, "ax":0.47233, "ay":0.8809, "alpha":-0.12426, "fx":[7.72254,8.30436,8.00029,7.46689], "fy":[14.31823,14.51778,15.0982,14.80226]}, - {"t":4.0345, "x":3.82288, "y":5.2185, "heading":-1.08806, "vx":-0.1503, "vy":-0.2863, "omega":0.17576, "ax":0.47057, "ay":0.88186, "alpha":-0.16451, "fx":[7.67794,8.38689,8.01078,7.30112], "fy":[14.12608,14.59234,15.22904,14.85336]}, - {"t":4.06685, "x":3.81826, "y":5.2097, "heading":-1.08237, "vx":-0.13508, "vy":-0.25778, "omega":0.17043, "ax":0.46896, "ay":0.88274, "alpha":-0.20219, "fx":[7.59508,8.50535,8.01588,7.15313], "fy":[14.08629,14.4775,15.38592,14.90969]}, - {"t":4.0992, "x":3.81414, "y":5.20183, "heading":-1.07686, "vx":-0.11991, "vy":-0.22922, "omega":0.16389, "ax":0.46748, "ay":0.88354, "alpha":-0.26002, "fx":[7.53989,8.65237,8.04588,6.93282], "fy":[13.84126,14.52342,15.57456,14.9737]}, - {"t":4.13154, "x":3.81051, "y":5.19487, "heading":-1.07156, "vx":-0.10479, "vy":-0.20064, "omega":0.15548, "ax":0.46612, "ay":0.88428, "alpha":-0.32082, "fx":[7.44258,8.8523,8.07414,6.71124], "fy":[13.71486,14.39676,15.80363,15.04682]}, - {"t":4.16389, "x":3.80736, "y":5.18885, "heading":-1.06653, "vx":-0.08971, "vy":-0.17204, "omega":0.1451, "ax":0.46487, "ay":0.88496, "alpha":-0.4057, "fx":[7.36858,9.094,8.12983,6.40402], "fy":[13.38388,14.40882,16.0824,15.13219]}, - {"t":4.19624, "x":3.8047, "y":5.18374, "heading":-1.06183, "vx":-0.07467, "vy":-0.14341, "omega":0.13198, "ax":0.4637, "ay":0.88558, "alpha":-0.50154, "fx":[7.24622,9.41516,8.18986,6.06751], "fy":[13.13322,14.25964,16.4236,15.23261]}, - {"t":4.22859, "x":3.80253, "y":5.17957, "heading":-1.05756, "vx":-0.05967, "vy":-0.11476, "omega":0.11576, "ax":0.46262, "ay":0.88617, "alpha":-0.62812, "fx":[7.14111,9.79951,8.28223,5.62371], "fy":[12.66641,14.22725,16.84173,15.3524]}, - {"t":4.26094, "x":3.80084, "y":5.17632, "heading":-1.05382, "vx":-0.04471, "vy":-0.0861, "omega":0.09544, "ax":0.46161, "ay":0.8867, "alpha":-0.77733, "fx":[6.97766,10.30338,8.38878,5.10946], "fy":[12.23222,14.03949,17.35559,15.49645]}, - {"t":4.29328, "x":3.79964, "y":5.174, "heading":-1.05073, "vx":-0.02977, "vy":-0.05741, "omega":0.07029, "ax":0.46067, "ay":0.88721, "alpha":-0.96823, "fx":[6.82129,10.90512,8.53672,4.45331], "fy":[11.55328,13.94438,17.9878,15.67181]}, - {"t":4.32563, "x":3.79891, "y":5.17261, "heading":-1.04846, "vx":-0.01487, "vy":-0.02871, "omega":0.03897, "ax":0.45979, "ay":0.88768, "alpha":-1.20483, "fx":[6.5904,11.72881,8.71533,3.62309], "fy":[10.82873,13.71299,18.75232,15.89452]}, - {"t":4.35798, "x":3.79867, "y":5.17214, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.1, "y":5.07, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.86716, "ay":0.87781, "alpha":3.77195, "fx":[-39.29275,-38.87305,-56.15552,-56.85557], "fy":[5.08263,24.80602,23.77939,4.86258]}, + {"t":0.04141, "x":7.09754, "y":5.07075, "heading":3.14159, "vx":-0.11873, "vy":0.03635, "omega":0.15619, "ax":-2.86776, "ay":0.87801, "alpha":2.87059, "fx":[-41.35676,-40.9988,-54.16847,-54.69276], "fy":[7.40454,22.34736,21.63424,7.15766]}, + {"t":0.08282, "x":7.09017, "y":5.07301, "heading":-3.13513, "vx":-0.23748, "vy":0.07271, "omega":0.27506, "ax":-2.86762, "ay":0.87797, "alpha":2.16841, "fx":[-42.92358,-42.65144,-52.6539,-52.97879], "fy":[9.23868,20.47546,19.88843,8.93873]}, + {"t":0.12423, "x":7.07787, "y":5.07677, "heading":-3.12374, "vx":-0.35622, "vy":0.10906, "omega":0.36485, "ax":-2.86742, "ay":0.87795, "alpha":1.6121, "fx":[-44.08699,-44.07297,-51.44213,-51.59211], "fy":[10.67698,19.00996,18.52317,10.3302]}, + {"t":0.16564, "x":7.06066, "y":5.08204, "heading":-3.10863, "vx":-0.47496, "vy":0.14542, "omega":0.43161, "ax":-2.86714, "ay":0.87787, "alpha":1.18684, "fx":[-45.05372,-45.04629,-50.52496,-50.55013], "fy":[11.77912,17.90223,17.43192,11.42153]}, + {"t":0.20705, "x":7.03854, "y":5.08882, "heading":-3.09076, "vx":-0.59369, "vy":0.18177, "omega":0.48076, "ax":-2.86662, "ay":0.87784, "alpha":0.84682, "fx":[-45.72345,-45.96471,-49.75523,-49.69746], "fy":[12.63035,16.99645,16.62139,12.28478]}, + {"t":0.24846, "x":7.0115, "y":5.0971, "heading":-3.07085, "vx":-0.71239, "vy":0.21812, "omega":0.51582, "ax":-2.86562, "ay":0.87767, "alpha":0.61123, "fx":[-46.31417,-46.41427,-49.22189,-49.12391], "fy":[13.21993,16.40239,15.98579,12.91309]}, + {"t":0.28986, "x":6.97954, "y":5.10688, "heading":-3.04949, "vx":-0.83106, "vy":0.25446, "omega":0.54113, "ax":-2.86242, "ay":0.87758, "alpha":0.40076, "fx":[-46.47919,-47.27054,-48.61117,-48.4998], "fy":[13.6745,15.83721,15.59504,13.40847]}, + {"t":0.33127, "x":6.94267, "y":5.11817, "heading":-3.02708, "vx":-0.94959, "vy":0.2908, "omega":0.55773, "ax":-0.15671, "ay":0.03589, "alpha":0.33801, "fx":[-1.65023,-1.84911,-3.57415,-3.37576], "fy":[-0.08249,1.3179,1.4404,-0.28256]}, + {"t":0.37268, "x":6.90322, "y":5.13024, "heading":-3.00399, "vx":-0.95608, "vy":0.29229, "omega":0.57172, "ax":0.00252, "ay":0.00825, "alpha":0.27075, "fx":[0.73829,0.68643,-0.71717,-0.53937], "fy":[-0.41398,0.86737,0.68903,-0.5923]}, + {"t":0.41409, "x":6.86363, "y":5.14235, "heading":-2.98031, "vx":-0.95597, "vy":0.29263, "omega":0.58294, "ax":0.00111, "ay":0.00363, "alpha":0.19794, "fx":[0.57254,0.41688,-0.53546,-0.3798], "fy":[-0.33709,0.61386,0.45773,-0.49242]}, + {"t":0.4555, "x":6.82404, "y":5.15448, "heading":-2.95617, "vx":-0.95593, "vy":0.29278, "omega":0.59113, "ax":-0.00076, "ay":-0.00247, "alpha":0.14871, "fx":[0.40267,0.28931,-0.43782,-0.30457], "fy":[-0.32864,0.37982,0.24635,-0.46211]}, + {"t":0.49691, "x":6.78446, "y":5.1666, "heading":-2.93169, "vx":-0.95596, "vy":0.29268, "omega":0.59729, "ax":-0.00005, "ay":-0.00017, "alpha":0.11046, "fx":[0.31963,0.20669,-0.32135,-0.2084], "fy":[-0.20872,0.31313,0.20593,-0.32153]}, + {"t":0.53832, "x":6.74487, "y":5.17872, "heading":-2.90696, "vx":-0.95596, "vy":0.29267, "omega":0.60186, "ax":-0.0004, "ay":-0.0013, "alpha":0.08334, "fx":[0.23705,0.14516,-0.25164,-0.1571], "fy":[-0.17153,0.22278,0.12817,-0.26614]}, + {"t":0.57973, "x":6.70529, "y":5.19083, "heading":-2.88204, "vx":-0.95598, "vy":0.29262, "omega":0.60532, "ax":0.00021, "ay":0.00067, "alpha":0.06252, "fx":[0.19046,0.11177,-0.1836,-0.1049], "fy":[-0.09617,0.19533,0.12059,-0.17486]}, + {"t":0.62114, "x":6.6657, "y":5.20295, "heading":-2.85697, "vx":-0.95597, "vy":0.29265, "omega":0.6079, "ax":-0.00014, "ay":-0.00044, "alpha":0.04753, "fx":[0.14154,0.07606,-0.14592,-0.08073], "fy":[-0.08585,0.1362,0.07105,-0.15101]}, + {"t":0.66255, "x":6.62612, "y":5.21507, "heading":-2.8318, "vx":-0.95597, "vy":0.29263, "omega":0.60987, "ax":0.00014, "ay":0.00045, "alpha":0.0361, "fx":[0.11311,0.05927,-0.10852,-0.05468], "fy":[-0.04915,0.11719,0.06502,-0.10306]}, + {"t":0.70396, "x":6.58653, "y":5.22719, "heading":-2.80655, "vx":-0.95597, "vy":0.29265, "omega":0.61137, "ax":-0.00005, "ay":-0.00015, "alpha":0.02776, "fx":[0.08555,0.04058,-0.08691,-0.04233], "fy":[-0.04407,0.0835,0.03899,-0.08858]}, + {"t":0.74537, "x":6.54694, "y":5.23931, "heading":-2.78123, "vx":-0.95597, "vy":0.29264, "omega":0.61252, "ax":0.00005, "ay":0.00016, "alpha":0.02147, "fx":[0.06816,0.03126,-0.06649,-0.0296], "fy":[-0.02777,0.07007,0.03332,-0.06474]}, + {"t":0.78677, "x":6.50736, "y":5.25142, "heading":-2.75587, "vx":-0.95597, "vy":0.29265, "omega":0.61341, "ax":-0.00002, "ay":-0.00005, "alpha":0.01678, "fx":[0.05313,0.02213,-0.05359,-0.02277], "fy":[-0.02334,0.05227,0.02154,-0.05407]}, + {"t":0.82818, "x":6.46777, "y":5.26354, "heading":-2.73046, "vx":-0.95597, "vy":0.29265, "omega":0.6141, "ax":0.00001, "ay":0.00004, "alpha":0.01331, "fx":[0.04264,0.01685,-0.04227,-0.01648], "fy":[-0.01629,0.04363,0.01723,-0.04216]}, + {"t":0.86959, "x":6.42819, "y":5.27566, "heading":-2.70503, "vx":-0.95597, "vy":0.29265, "omega":0.61465, "ax":0.0, "ay":-0.00001, "alpha":0.01063, "fx":[0.03442,0.01247,-0.03455,-0.01263], "fy":[-0.01272,0.03407,0.01224,-0.03455]}, + {"t":0.911, "x":6.3886, "y":5.28778, "heading":-2.67958, "vx":-0.95597, "vy":0.29265, "omega":0.61509, "ax":0.0, "ay":0.0, "alpha":0.00868, "fx":[0.02811,0.00943,-0.0281,-0.00942], "fy":[-0.00968,0.02884,0.00933,-0.02844]}, + {"t":0.95241, "x":6.34902, "y":5.2999, "heading":-2.65411, "vx":-0.95597, "vy":0.29265, "omega":0.61545, "ax":0.0, "ay":0.0, "alpha":0.00712, "fx":[0.0235,0.00729,-0.02351,-0.00725], "fy":[-0.00716,0.02337,0.0072,-0.02332]}, + {"t":0.99382, "x":6.30943, "y":5.31202, "heading":-2.62863, "vx":-0.95597, "vy":0.29265, "omega":0.61575, "ax":0.0, "ay":0.0, "alpha":0.006, "fx":[0.01964,0.00549,-0.01968,-0.00553], "fy":[-0.0058,0.02025,0.00533,-0.02005]}, + {"t":1.03523, "x":6.26984, "y":5.32413, "heading":-2.60313, "vx":-0.95597, "vy":0.29265, "omega":0.616, "ax":0.0, "ay":0.00001, "alpha":0.00505, "fx":[0.01695,0.00437,-0.01693,-0.00429], "fy":[-0.00416,0.01687,0.00433,-0.0167]}, + {"t":1.07664, "x":6.23026, "y":5.33625, "heading":-2.57762, "vx":-0.95597, "vy":0.29265, "omega":0.6162, "ax":0.0, "ay":0.0, "alpha":0.00438, "fx":[0.0145,0.00327,-0.01452,-0.0033], "fy":[-0.00349,0.01497,0.00317,-0.01482]}, + {"t":1.11805, "x":6.19067, "y":5.34837, "heading":-2.5521, "vx":-0.95597, "vy":0.29265, "omega":0.61639, "ax":0.0, "ay":0.0, "alpha":0.00378, "fx":[0.01285,0.00264,-0.01283,-0.00257], "fy":[-0.00246,0.01276,0.00261,-0.01261]}, + {"t":1.15946, "x":6.15109, "y":5.36049, "heading":-2.52658, "vx":-0.95597, "vy":0.29265, "omega":0.61654, "ax":0.0, "ay":0.0, "alpha":0.00334, "fx":[0.01118,0.00193,-0.0112,-0.00195], "fy":[-0.00206,0.01148,0.0019,-0.01142]}, + {"t":1.20087, "x":6.1115, "y":5.37261, "heading":-2.50105, "vx":-0.95597, "vy":0.29265, "omega":0.61668, "ax":0.0, "ay":0.0, "alpha":0.00294, "fx":[0.01011,0.00154,-0.01009,-0.00149], "fy":[-0.00141,0.01,0.00152,-0.00989]}, + {"t":1.24228, "x":6.07191, "y":5.38473, "heading":-2.47551, "vx":-0.95597, "vy":0.29265, "omega":0.6168, "ax":0.0, "ay":0.0, "alpha":0.00262, "fx":[0.00888,0.00107,-0.0089,-0.00109], "fy":[-0.00112,0.00894,0.0011,-0.00906]}, + {"t":1.28369, "x":6.03233, "y":5.39684, "heading":-2.44997, "vx":-0.95597, "vy":0.29265, "omega":0.61691, "ax":0.0, "ay":0.0, "alpha":0.00235, "fx":[0.00814,0.00081,-0.00811,-0.00077], "fy":[-0.0007,0.00803,0.00082,-0.00792]}, + {"t":1.32509, "x":5.99274, "y":5.40896, "heading":-2.42443, "vx":-0.95597, "vy":0.29265, "omega":0.61701, "ax":0.0, "ay":0.0, "alpha":0.00208, "fx":[0.00714,0.00048,-0.00719,-0.00052], "fy":[-0.00049,0.00691,0.00055,-0.00729]}, + {"t":1.3665, "x":5.95316, "y":5.42108, "heading":-2.39888, "vx":-0.95597, "vy":0.29265, "omega":0.61709, "ax":0.0, "ay":0.00001, "alpha":0.00189, "fx":[0.00663,0.00031,-0.00656,-0.00026], "fy":[-0.00019,0.00653,0.00038,-0.00633]}, + {"t":1.40791, "x":5.91357, "y":5.4332, "heading":-2.37332, "vx":-0.95597, "vy":0.29265, "omega":0.61717, "ax":0.0, "ay":-0.00001, "alpha":0.00163, "fx":[0.00572,0.00006,-0.00582,-0.00016], "fy":[-0.00006,0.00509,0.00017,-0.00587]}, + {"t":1.44932, "x":5.87399, "y":5.44532, "heading":-2.34777, "vx":-0.95597, "vy":0.29265, "omega":0.61724, "ax":0.0, "ay":0.00001, "alpha":0.00152, "fx":[0.00538,-0.00002,-0.00524,0.0001], "fy":[0.00022,0.00534,0.00015,-0.00496]}, + {"t":1.49073, "x":5.8344, "y":5.45744, "heading":-2.32221, "vx":-0.95597, "vy":0.29265, "omega":0.6173, "ax":-0.00001, "ay":-0.00002, "alpha":0.00125, "fx":[0.00449,-0.00024,-0.00468,0.00006], "fy":[0.00022,0.00332,-0.00009,-0.00468]}, + {"t":1.53214, "x":5.79481, "y":5.46955, "heading":-2.29665, "vx":-0.95597, "vy":0.29265, "omega":0.61735, "ax":0.00001, "ay":0.00002, "alpha":0.0012, "fx":[0.00432,-0.00021,-0.00406,0.00037], "fy":[0.00058,0.00437,0.00011,-0.00369]}, + {"t":1.57355, "x":5.75523, "y":5.48167, "heading":-2.27108, "vx":-0.95597, "vy":0.29265, "omega":0.6174, "ax":-0.00001, "ay":-0.00003, "alpha":0.00089, "fx":[0.00337,-0.00046,-0.00369,0.00013], "fy":[0.00035,0.00144,-0.00027,-0.00365]}, + {"t":1.61496, "x":5.71564, "y":5.49379, "heading":-2.24552, "vx":-0.95597, "vy":0.29265, "omega":0.61744, "ax":0.00001, "ay":0.00004, "alpha":0.00091, "fx":[0.0034,-0.00029,-0.00295,0.00057], "fy":[0.00092,0.00362,0.00026,-0.00243]}, + {"t":1.65637, "x":5.67606, "y":5.50591, "heading":-2.21995, "vx":-0.95597, "vy":0.29265, "omega":0.61748, "ax":-0.00002, "ay":-0.00005, "alpha":0.00053, "fx":[0.00229,-0.00062,-0.00284,0.00008], "fy":[0.00032,-0.00069,-0.00042,-0.00276]}, + {"t":1.69778, "x":5.63647, "y":5.51803, "heading":-2.19438, "vx":-0.95597, "vy":0.29265, "omega":0.6175, "ax":0.00002, "ay":0.00006, "alpha":0.00063, "fx":[0.00258,-0.00024,-0.00185,0.00073], "fy":[0.00134,0.00308,0.00066,-0.00108]}, + {"t":1.73919, "x":5.59688, "y":5.53015, "heading":-2.16881, "vx":-0.95597, "vy":0.29265, "omega":0.61753, "ax":-0.00003, "ay":-0.00009, "alpha":0.00017, "fx":[0.0012,-0.00075,-0.00209,-0.00013], "fy":[0.00006,-0.0032,-0.0006,-0.00204]}, + {"t":1.7806, "x":5.5573, "y":5.54226, "heading":-2.14324, "vx":-0.95597, "vy":0.29264, "omega":0.61753, "ax":0.00003, "ay":0.0001, "alpha":0.00036, "fx":[0.00184,-0.00001,-0.00066,0.00086], "fy":[0.00191,0.00281,0.00141,0.00052]}, + {"t":1.822, "x":5.51771, "y":5.55438, "heading":-2.11767, "vx":-0.95597, "vy":0.29265, "omega":0.61755, "ax":-0.00004, "ay":-0.00014, "alpha":-0.00022, "fx":[-0.00002,-0.00088,-0.0014,-0.00054], "fy":[-0.00057,-0.00624,-0.00095,-0.00154]}, + {"t":1.86341, "x":5.47813, "y":5.5665, "heading":-2.09209, "vx":-0.95597, "vy":0.29264, "omega":0.61754, "ax":0.00005, "ay":0.00016, "alpha":0.00005, "fx":[0.00111,0.00048,0.00076,0.00098], "fy":[0.00275,0.00284,0.00269,0.0026]}, + {"t":1.90482, "x":5.43854, "y":5.57862, "heading":-2.06652, "vx":-0.95597, "vy":0.29265, "omega":0.61754, "ax":-0.00007, "ay":-0.00022, "alpha":-0.00067, "fx":[-0.00156,-0.001,-0.00069,-0.00126], "fy":[-0.00187,-0.00991,-0.00163,-0.00135]}, + {"t":1.94623, "x":5.39896, "y":5.59074, "heading":-2.04095, "vx":-0.95597, "vy":0.29264, "omega":0.61751, "ax":0.00008, "ay":0.00026, "alpha":-0.00035, "fx":[0.00025,0.0014,0.00264,0.00106], "fy":[0.00399,0.0032,0.00475,0.00554]}, + {"t":1.98764, "x":5.35937, "y":5.60285, "heading":-2.01538, "vx":-0.95597, "vy":0.29265, "omega":0.6175, "ax":-0.00011, "ay":-0.00035, "alpha":-0.00119, "fx":[-0.00372,-0.00108,0.00019,-0.00246], "fy":[-0.00438,-0.01416,-0.00295,-0.00164]}, + {"t":2.02905, "x":5.31978, "y":5.61497, "heading":-1.98981, "vx":-0.95597, "vy":0.29264, "omega":0.61745, "ax":0.00013, "ay":0.00041, "alpha":-0.00095, "fx":[-0.00099,0.00305,0.00532,0.00096], "fy":[0.00566,0.0038,0.00797,0.00983]}, + {"t":2.07046, "x":5.2802, "y":5.62709, "heading":-1.96424, "vx":-0.95596, "vy":0.29265, "omega":0.61741, "ax":-0.00016, "ay":-0.00053, "alpha":-0.00183, "fx":[-0.00695,-0.00097,0.00151,-0.00447], "fy":[-0.00896,-0.01859,-0.0053,-0.0027]}, + {"t":2.11187, "x":5.24061, "y":5.63921, "heading":-1.93867, "vx":-0.95597, "vy":0.29263, "omega":0.61733, "ax":0.00019, "ay":0.00061, "alpha":-0.0019, "fx":[-0.00303,0.00588,0.00921,0.00038], "fy":[0.00758,0.00434,0.01275,0.016]}, + {"t":2.15328, "x":5.20103, "y":5.65133, "heading":-1.91311, "vx":-0.95596, "vy":0.29266, "omega":0.61726, "ax":-0.00024, "ay":-0.00079, "alpha":-0.0026, "fx":[-0.0118,-0.00035,0.00373,-0.00772], "fy":[-0.01684,-0.02189,-0.00913,-0.00486]}, + {"t":2.19469, "x":5.16144, "y":5.66345, "heading":-1.88755, "vx":-0.95597, "vy":0.29262, "omega":0.61715, "ax":0.00026, "ay":0.00086, "alpha":-0.00342, "fx":[-0.00649,0.01053,0.01475,-0.00124], "fy":[0.00919,0.00418,0.01949,0.02451]}, + {"t":2.2361, "x":5.12185, "y":5.67556, "heading":-1.862, "vx":-0.95596, "vy":0.29266, "omega":0.61701, "ax":-0.00033, "ay":-0.00109, "alpha":-0.0035, "fx":[-0.01871,0.00156,0.00763,-0.01263], "fy":[-0.02911,-0.0209,-0.01434,-0.00802]}, + {"t":2.27751, "x":5.08227, "y":5.68768, "heading":-1.83645, "vx":-0.95598, "vy":0.29261, "omega":0.61686, "ax":0.00034, "ay":0.00113, "alpha":-0.00578, "fx":[-0.01223,0.01779,0.02227,-0.00485], "fy":[0.00924,0.00212,0.0283,0.03542]}, + {"t":2.31892, "x":5.04268, "y":5.6998, "heading":-1.8109, "vx":-0.95596, "vy":0.29266, "omega":0.61662, "ax":-0.00038, "ay":-0.00124, "alpha":-0.00451, "fx":[-0.02743,0.00646,0.01475,-0.01914], "fy":[-0.04506,-0.00907,-0.01865,-0.0101]}, + {"t":2.36032, "x":5.0031, "y":5.71192, "heading":-1.78537, "vx":-0.95598, "vy":0.29261, "omega":0.61644, "ax":0.0004, "ay":0.0013, "alpha":-0.00929, "fx":[-0.02146,0.02831,0.03159,-0.01197], "fy":[0.00501,-0.00424,0.03826,0.04752]}, + {"t":2.40173, "x":4.96351, "y":5.72403, "heading":-1.75984, "vx":-0.95596, "vy":0.29266, "omega":0.61605, "ax":-0.00025, "ay":-0.00083, "alpha":-0.00578, "fx":[-0.03631,0.01755,0.02784,-0.02602], "fy":[-0.05937,0.02325,-0.01489,-0.00437]}, + {"t":2.44314, "x":4.92392, "y":5.73615, "heading":-1.73433, "vx":-0.95597, "vy":0.29263, "omega":0.61581, "ax":0.00031, "ay":0.00101, "alpha":-0.01427, "fx":[-0.0365,0.0414,0.04123,-0.02549], "fy":[-0.01064,-0.0215,0.04435,0.05521]}, + {"t":2.48455, "x":4.88434, "y":5.74827, "heading":-1.70883, "vx":-0.95596, "vy":0.29267, "omega":0.61522, "ax":0.00018, "ay":0.0006, "alpha":-0.00831, "fx":[-0.04361,0.03836,0.04974,-0.03223], "fy":[-0.06344,0.07633,0.00783,0.01932]}, + {"t":2.52596, "x":4.84475, "y":5.76039, "heading":-1.68336, "vx":-0.95595, "vy":0.2927, "omega":0.61488, "ax":-0.00026, "ay":-0.00085, "alpha":-0.02117, "fx":[-0.06348,0.0518,0.04664,-0.0523], "fy":[-0.05799,-0.06914,0.02964,0.04079]}, + {"t":2.56737, "x":4.80517, "y":5.77251, "heading":-1.65789, "vx":-0.95596, "vy":0.29266, "omega":0.614, "ax":0.0007, "ay":0.00228, "alpha":-0.01562, "fx":[-0.05474,0.06734,0.07798,-0.04409], "fy":[-0.06042,0.10006,0.05081,0.06144]}, + {"t":2.60878, "x":4.76558, "y":5.78463, "heading":-1.63247, "vx":-0.95593, "vy":0.29276, "omega":0.61335, "ax":-0.00192, "ay":-0.00629, "alpha":-0.03121, "fx":[-0.11423,0.0486,0.04242,-0.10513], "fy":[-0.17462,-0.18379,-0.03504,-0.02587]}, + {"t":2.65019, "x":4.726, "y":5.79675, "heading":-1.60707, "vx":-0.95601, "vy":0.29249, "omega":0.61206, "ax":0.00154, "ay":0.00504, "alpha":-0.03621, "fx":[-0.07112,0.11583,0.12259,-0.06436], "fy":[-0.01435,0.01633,0.16381,0.17051]}, + {"t":2.6916, "x":4.68641, "y":5.80887, "heading":-1.58173, "vx":-0.95595, "vy":0.2927, "omega":0.61056, "ax":-0.00426, "ay":-0.01392, "alpha":-0.04881, "fx":[-0.19021,0.04499,0.04886,-0.18765], "fy":[-0.35057,-0.35316,-0.11362,-0.11104]}, + {"t":2.73301, "x":4.64682, "y":5.82097, "heading":-1.55644, "vx":-0.95613, "vy":0.29213, "omega":0.60854, "ax":0.01479, "ay":0.04822, "alpha":-0.08224, "fx":[0.0907,0.40696,0.40234,0.08608], "fy":[0.73368,0.38657,1.04972,1.04523]}, + {"t":2.77442, "x":4.60724, "y":5.83311, "heading":-1.53124, "vx":-0.95551, "vy":0.29412, "omega":0.60513, "ax":-0.01953, "ay":-0.06376, "alpha":-0.08961, "fx":[-0.53472,-0.09574,-0.11982,-0.552], "fy":[-1.28981,-1.27249,-0.83597,-0.85327]}, + {"t":2.81583, "x":4.56766, "y":5.84524, "heading":-1.50619, "vx":-0.95632, "vy":0.29148, "omega":0.60142, "ax":-0.00101, "ay":-0.00332, "alpha":-0.14188, "fx":[-0.30316,0.31156,0.26765,-0.34348], "fy":[-0.28836,-0.60038,0.35305,0.31447]}, + {"t":2.85723, "x":4.52806, "y":5.8573, "heading":-1.48128, "vx":-0.95636, "vy":0.29135, "omega":0.59555, "ax":-0.36132, "ay":-1.31909, "alpha":-0.17387, "fx":[-6.40356,-5.57628,-5.62542,-6.48678], "fy":[-22.44554,-22.37574,-21.53143,-21.60194]}, + {"t":2.89864, "x":4.48814, "y":5.86824, "heading":-1.45662, "vx":-0.97133, "vy":0.23672, "omega":0.58835, "ax":-0.51621, "ay":-2.85516, "alpha":-0.20058, "fx":[-9.10168,-7.98469,-8.08335,-9.25013], "fy":[-48.34948,-47.59998,-47.1579,-47.26903]}, + {"t":2.94005, "x":4.44748, "y":5.87559, "heading":-1.43226, "vx":-0.9927, "vy":0.11849, "omega":0.58004, "ax":-0.16954, "ay":-2.97046, "alpha":-0.28844, "fx":[-3.42549,-2.11157,-2.12331,-3.6442], "fy":[-50.29763,-50.11221,-48.73421,-48.92065]}, + {"t":2.98146, "x":4.40623, "y":5.87795, "heading":-1.40824, "vx":-0.99972, "vy":-0.00451, "omega":0.5681, "ax":0.19919, "ay":-2.9824, "alpha":-0.41255, "fx":[2.48795,4.45689,4.16053,2.17613], "fy":[-50.62277,-50.8465,-48.54173,-48.84964]}, + {"t":3.02287, "x":4.365, "y":5.87521, "heading":-1.38471, "vx":-0.99147, "vy":-0.12801, "omega":0.55102, "ax":0.56789, "ay":-2.93947, "alpha":-0.50904, "fx":[8.4031,10.93099,10.56404,7.96747], "fy":[-50.40216,-49.91215,-47.59459,-48.08915]}, + {"t":3.06428, "x":4.32443, "y":5.86739, "heading":-1.3619, "vx":-0.96796, "vy":-0.24973, "omega":0.52994, "ax":0.92909, "ay":-2.84832, "alpha":-0.68685, "fx":[14.09585,17.46951,16.89913,13.48529], "fy":[-49.32322,-48.82269,-45.50203,-46.27251]}, + {"t":3.10569, "x":4.28515, "y":5.8546, "heading":-1.33995, "vx":-0.92949, "vy":-0.36768, "omega":0.50149, "ax":1.27767, "ay":-2.71124, "alpha":-0.88824, "fx":[19.53102,23.8405,23.14217,18.67892], "fy":[-47.80024,-46.63517,-42.5883,-43.75628]}, + {"t":3.1471, "x":4.24775, "y":5.83705, "heading":-1.31919, "vx":-0.87658, "vy":-0.47995, "omega":0.46471, "ax":1.61074, "ay":-2.52844, "alpha":-1.19134, "fx":[24.55022,30.30321,29.19505,23.35287], "fy":[-45.64731,-44.12011,-38.55487,-40.26935]}, + {"t":3.18851, "x":4.21283, "y":5.81501, "heading":-1.29994, "vx":-0.80988, "vy":-0.58465, "omega":0.41538, "ax":1.90492, "ay":-2.3155, "alpha":-1.57606, "fx":[28.78732,36.41689,34.72835,27.08392], "fy":[-43.4025,-40.93605,-33.79897,-36.25574]}, + {"t":3.22992, "x":4.18093, "y":5.78882, "heading":-1.28274, "vx":-0.731, "vy":-0.68053, "omega":0.35012, "ax":0.67372, "ay":-0.73374, "alpha":-1.09306, "fx":[9.42154,14.52179,13.05078,7.92829], "fy":[-15.52169,-14.04976,-8.90941,-10.44345]}, + {"t":3.26239, "x":4.15755, "y":5.76634, "heading":-1.27137, "vx":-0.70912, "vy":-0.70435, "omega":0.31463, "ax":0.71276, "ay":-0.69837, "alpha":-0.85164, "fx":[10.43303,14.45152,13.24226,9.39862], "fy":[-14.28667,-13.0091,-9.0041,-10.26612]}, + {"t":3.29485, "x":4.1349, "y":5.7431, "heading":-1.26116, "vx":-0.68598, "vy":-0.72703, "omega":0.28698, "ax":0.73628, "ay":-0.67321, "alpha":-0.68482, "fx":[11.18613,14.35376,13.3653,10.18853], "fy":[-13.29938,-12.34039,-9.10866,-10.13973]}, + {"t":3.32732, "x":4.11302, "y":5.71914, "heading":-1.25184, "vx":-0.66208, "vy":-0.74889, "omega":0.26474, "ax":0.75849, "ay":-0.64771, "alpha":-0.54254, "fx":[11.78132,14.26965,13.46568,11.05795], "fy":[-12.57452,-11.58303,-9.09705,-9.93314]}, + {"t":3.35979, "x":4.09192, "y":5.69448, "heading":-1.24324, "vx":-0.63745, "vy":-0.76992, "omega":0.24713, "ax":0.79418, "ay":-0.60295, "alpha":-0.43119, "fx":[12.58935,14.53879,13.8898,11.93676], "fy":[-11.30309,-10.87509,-8.67538,-9.35013]}, + {"t":3.39226, "x":4.07165, "y":5.66917, "heading":-1.23522, "vx":-0.61166, "vy":-0.78949, "omega":0.23313, "ax":0.90568, "ay":-0.41661, "alpha":-0.33204, "fx":[14.56474,16.08422,15.56194,14.17813], "fy":[-8.08921,-7.4018,-5.87449,-6.41346]}, + {"t":3.42473, "x":4.05226, "y":5.64331, "heading":-1.22765, "vx":-0.58226, "vy":-0.80302, "omega":0.22235, "ax":0.98844, "ay":-0.12975, "alpha":-0.25647, "fx":[16.09778,17.27381,16.85649,15.67953], "fy":[-2.98271,-2.50948,-1.36624,-1.79329]}, + {"t":3.45719, "x":4.03388, "y":5.61717, "heading":-1.22043, "vx":-0.55016, "vy":-0.80723, "omega":0.21402, "ax":0.98631, "ay":0.14692, "alpha":-0.17841, "fx":[16.1121,17.02022,16.68737,15.94576], "fy":[1.93984,2.11785,3.03685,2.70191]}, + {"t":3.48966, "x":4.01654, "y":5.59104, "heading":-1.21348, "vx":-0.51814, "vy":-0.80246, "omega":0.20823, "ax":0.93067, "ay":0.35916, "alpha":-0.14688, "fx":[15.29884,15.98947,15.72897,15.03791], "fy":[5.45779,5.90999,6.41982,6.1604]}, + {"t":3.52213, "x":4.0002, "y":5.56518, "heading":-1.20672, "vx":-0.48792, "vy":-0.7908, "omega":0.20346, "ax":0.8602, "ay":0.50586, "alpha":-0.09914, "fx":[14.15713,14.68477,14.48084,14.03404], "fy":[8.20168,8.22106,8.75375,8.55298]}, + {"t":3.5546, "x":3.98482, "y":5.53977, "heading":-1.20012, "vx":-0.45999, "vy":-0.77438, "omega":0.20024, "ax":0.79385, "ay":0.60517, "alpha":-0.08236, "fx":[13.11491,13.50599,13.35165,12.95987], "fy":[9.77184,10.08529,10.32344,10.171]}, + {"t":3.58707, "x":3.9703, "y":5.51494, "heading":-1.19362, "vx":-0.43422, "vy":-0.75473, "omega":0.19756, "ax":0.73699, "ay":0.67361, "alpha":-0.05215, "fx":[12.18304,12.47812,12.35936,12.12038], "fy":[11.12872,11.10325,11.39956,11.28348]}, + {"t":3.61953, "x":3.95659, "y":5.49079, "heading":-1.1872, "vx":-0.41029, "vy":-0.73286, "omega":0.19587, "ax":0.68968, "ay":0.72223, "alpha":-0.04384, "fx":[11.43452,11.64553,11.55918,11.34735], "fy":[11.85598,12.06877,12.15911,12.0731]}, + {"t":3.652, "x":3.94363, "y":5.46738, "heading":-1.18084, "vx":-0.3879, "vy":-0.70941, "omega":0.19445, "ax":0.65052, "ay":0.7579, "alpha":-0.0242, "fx":[10.78601,10.94429,10.87835,10.76667], "fy":[12.60982,12.55786,12.71588,12.65145]}, + {"t":3.68447, "x":3.93138, "y":5.44475, "heading":-1.17453, "vx":-0.36678, "vy":-0.6848, "omega":0.19366, "ax":0.61794, "ay":0.78484, "alpha":-0.02206, "fx":[10.26935,10.37871,10.33253,10.22233], "fy":[12.97599,13.12783,13.13753,13.09049]}, + {"t":3.71694, "x":3.9198, "y":5.42293, "heading":-1.16824, "vx":-0.34671, "vy":-0.65932, "omega":0.19295, "ax":0.5906, "ay":0.80574, "alpha":-0.00983, "fx":[9.81051,9.89593,9.85921,9.81428], "fy":[13.44269,13.38308,13.46784,13.43187]}, + {"t":3.74941, "x":3.90885, "y":5.40194, "heading":-1.16198, "vx":-0.32754, "vy":-0.63316, "omega":0.19263, "ax":0.56743, "ay":0.82233, "alpha":-0.01198, "fx":[9.44155,9.50318,9.47638,9.414], "fy":[13.63885,13.75454,13.733,13.70488]}, + {"t":3.78188, "x":3.89852, "y":5.38182, "heading":-1.15572, "vx":-0.30912, "vy":-0.60646, "omega":0.19224, "ax":0.5476, "ay":0.83575, "alpha":-0.00517, "fx":[9.10309,9.16152,9.13562,9.11296], "fy":[13.95027,13.89525,13.95297,13.92755]}, + {"t":3.81434, "x":3.88877, "y":5.36257, "heading":-1.14948, "vx":-0.29134, "vy":-0.57932, "omega":0.19207, "ax":0.53048, "ay":0.84679, "alpha":-0.01082, "fx":[8.82803,8.88256,8.85804,8.80285], "fy":[14.05527,14.15452,14.1392,14.11351]}, + {"t":3.84681, "x":3.87959, "y":5.34421, "heading":-1.14324, "vx":-0.27411, "vy":-0.55183, "omega":0.19172, "ax":0.51557, "ay":0.85602, "alpha":-0.00859, "fx":[8.56778,8.63631,8.60486,8.56799], "fy":[14.2714,14.23376,14.30163,14.27086]}, + {"t":3.87928, "x":3.87096, "y":5.32674, "heading":-1.13702, "vx":-0.25737, "vy":-0.52404, "omega":0.19144, "ax":0.50247, "ay":0.86382, "alpha":-0.01758, "fx":[8.35394,8.43697,8.3982,8.31461], "fy":[14.32123,14.42326,14.44645,14.40721]}, + {"t":3.91175, "x":3.86287, "y":5.31018, "heading":-1.1308, "vx":-0.24106, "vy":-0.49599, "omega":0.19087, "ax":0.49089, "ay":0.8705, "alpha":-0.01991, "fx":[8.14578,8.26008,8.20574,8.11994], "fy":[14.47157,14.46591,14.57953,14.52657]}, + {"t":3.94422, "x":3.8553, "y":5.29454, "heading":-1.12461, "vx":-0.22512, "vy":-0.46773, "omega":0.19022, "ax":0.48058, "ay":0.87628, "alpha":-0.03286, "fx":[7.9727,8.1217,8.04966,7.90017], "fy":[14.48244,14.60786,14.70477,14.63353]}, + {"t":3.97668, "x":3.84824, "y":5.27981, "heading":-1.11843, "vx":-0.20952, "vy":-0.43927, "omega":0.18915, "ax":0.47135, "ay":0.88131, "alpha":-0.04052, "fx":[7.79961,8.00066,7.90185,7.72669], "fy":[14.58119,14.62629,14.82651,14.73038]}, + {"t":4.00915, "x":3.84169, "y":5.26601, "heading":-1.11229, "vx":-0.19421, "vy":-0.41066, "omega":0.18784, "ax":0.46304, "ay":0.88574, "alpha":-0.05887, "fx":[7.65338,7.91476,7.78423,7.52242], "fy":[14.5579,14.73329,14.94802,14.82032]}, + {"t":4.04162, "x":3.83563, "y":5.25315, "heading":-1.10619, "vx":-0.17918, "vy":-0.3819, "omega":0.18593, "ax":0.45553, "ay":0.88966, "alpha":-0.0736, "fx":[7.50328,7.84491,7.67168,7.35366], "fy":[14.60939,14.73301,15.07346,14.90499]}, + {"t":4.07409, "x":3.83005, "y":5.24122, "heading":-1.10015, "vx":-0.16439, "vy":-0.35302, "omega":0.18354, "ax":0.44869, "ay":0.89315, "alpha":-0.09983, "fx":[7.37344,7.81105,7.58573,7.14776], "fy":[14.54949,14.81122,15.20617,14.98679]}, + {"t":4.10656, "x":3.82495, "y":5.23023, "heading":-1.09419, "vx":-0.14982, "vy":-0.32402, "omega":0.1803, "ax":0.44246, "ay":0.89628, "alpha":-0.12462, "fx":[7.23669,7.79543,7.50355,6.96655], "fy":[14.55006,14.79413,15.35098,15.06717]}, + {"t":4.13902, "x":3.82032, "y":5.22018, "heading":-1.08834, "vx":-0.13545, "vy":-0.29492, "omega":0.17625, "ax":0.43675, "ay":0.8991, "alpha":-0.16272, "fx":[7.11432,7.82108,7.44649,6.73944], "fy":[14.44371,14.84594,15.51231,15.14838]}, + {"t":4.17149, "x":3.81615, "y":5.21108, "heading":-1.08262, "vx":-0.12127, "vy":-0.26572, "omega":0.17097, "ax":0.43149, "ay":0.90165, "alpha":-0.20243, "fx":[6.98207,7.87104,7.39342,6.52461], "fy":[14.38144,14.8106,15.69645,15.23204]}, + {"t":4.20396, "x":3.81244, "y":5.20292, "heading":-1.07707, "vx":-0.10726, "vy":-0.23645, "omega":0.1644, "ax":0.42665, "ay":0.90397, "alpha":-0.25864, "fx":[6.85819,7.97309,7.36595,6.25089], "fy":[14.21017,14.83471,15.9098,15.32059]}, + {"t":4.23643, "x":3.80918, "y":5.19572, "heading":-1.07173, "vx":-0.09341, "vy":-0.2071, "omega":0.156, "ax":0.42217, "ay":0.90609, "alpha":-0.32086, "fx":[6.72093,8.11068,7.34434,5.9733], "fy":[14.06257,14.77673,16.16115,15.41612]}, + {"t":4.2689, "x":3.80637, "y":5.18948, "heading":-1.06666, "vx":-0.0797, "vy":-0.17768, "omega":0.14558, "ax":0.41801, "ay":0.90804, "alpha":-0.40481, "fx":[6.58497,8.31861,7.35098,5.6174], "fy":[13.79572,14.76825,16.46025,15.52182]}, + {"t":4.30136, "x":3.80401, "y":5.18419, "heading":-1.06194, "vx":-0.06613, "vy":-0.1482, "omega":0.13244, "ax":0.41414, "ay":0.90982, "alpha":-0.50135, "fx":[6.43064,8.58086,7.36751,5.23497], "fy":[13.52537,14.67904,16.81991,15.64079]}, + {"t":4.33383, "x":3.80208, "y":5.17985, "heading":-1.05764, "vx":-0.05269, "vy":-0.11866, "omega":0.11616, "ax":0.41053, "ay":0.91147, "alpha":-0.62789, "fx":[6.26837,8.94251,7.41807,4.74446], "fy":[13.11458,14.62772,17.25503,15.77761]}, + {"t":4.3663, "x":3.80058, "y":5.17648, "heading":-1.05387, "vx":-0.03936, "vy":-0.08906, "omega":0.09577, "ax":0.40716, "ay":0.91299, "alpha":-0.777, "fx":[6.0797,9.38868,7.48596,4.1942], "fy":[12.6619,14.49267,17.78471,15.93728]}, + {"t":4.39877, "x":3.79952, "y":5.17407, "heading":-1.05076, "vx":-0.02614, "vy":-0.05942, "omega":0.07054, "ax":0.404, "ay":0.91441, "alpha":-0.96897, "fx":[5.86954,9.97892,7.5986,3.49082], "fy":[12.03199,14.38002,18.43173,16.12712]}, + {"t":4.43124, "x":3.79888, "y":5.17262, "heading":-1.04847, "vx":-0.01302, "vy":-0.02973, "omega":0.03908, "ax":0.40103, "ay":0.91572, "alpha":-1.20377, "fx":[5.61912,10.7164,7.74207,2.66255], "fy":[11.27803,14.17841,19.23753,16.3646]}, + {"t":4.46371, "x":3.79867, "y":5.17214, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1e52943a..68079c8b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -82,6 +82,7 @@ public void autonomousInit() { if (m_robotContainer.getAutoSwitch().getAutoCommand() != null) { m_robotContainer.getAutoSwitch().getAutoCommand().schedule(); } + m_robotContainer.setHeadlights(true); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8fc52841..463f54c8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -725,11 +725,15 @@ public void configTestDash() { .withSize(1, 1); Shuffleboard.getTab("Test") - .add("Headlights On", new InstantCommand(() -> pathHandler.setHeadlights(true))) + .add( + "Headlights On", + new InstantCommand(() -> pathHandler.setHeadlights(true)).runsWhenDisabled()) .withPosition(2, 2) .withSize(1, 1); Shuffleboard.getTab("Test") - .add("Headlights Off", new InstantCommand(() -> pathHandler.setHeadlights(false))) + .add( + "Headlights Off", + new InstantCommand(() -> pathHandler.setHeadlights(false)).runsWhenDisabled()) .withPosition(3, 2) .withSize(1, 1); } @@ -755,7 +759,7 @@ public void setScoringSide(ScoreSide side) { } public void stow() { - robotStateSubsystem.toStow(); + robotStateSubsystem.toStowSafe(); } public AutoSwitch getAutoSwitch() { @@ -777,4 +781,8 @@ public void finishAuto() { public boolean wasScoringCoral() { return robotStateSubsystem.getState() == RobotStates.PLACE_CORAL; } + + public void setHeadlights(boolean on) { + pathHandler.setHeadlights(on); + } } diff --git a/src/main/java/frc/robot/constants/AutonConstants.java b/src/main/java/frc/robot/constants/AutonConstants.java index 7cd1cdb8..88e426e5 100644 --- a/src/main/java/frc/robot/constants/AutonConstants.java +++ b/src/main/java/frc/robot/constants/AutonConstants.java @@ -11,7 +11,7 @@ public class AutonConstants { public static final double kMaxOmegaErrorRadians = Units.degreesToRadians(kMaxOmegaErrorDegrees); public static final int kSwitchStableCounts = 3; public static final double kElevatorStageRadius = 2.3; - public static final double kElevatorStageRadiusPathOne = 1.8; + public static final double kElevatorStageRadiusPathOne = 2.1; // 1.8 public static final double kInitPathPrestageTime = 0.6; // Start Poses diff --git a/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java index 6ba74aba..274177fe 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java @@ -11,6 +11,7 @@ import frc.robot.commands.auton.NonProcessorDeepAutonCommand; import frc.robot.commands.auton.NonProcessorShallowAutonCommand; import frc.robot.commands.auton.NonProcessorShallowSlowAutonCommand; +import frc.robot.commands.auton.ProcessorShallowAutonCommand; import frc.robot.commands.auton.ProcessorShallowSlowAutonCommand; import frc.robot.constants.AutonConstants; import frc.robot.constants.RobotConstants; @@ -267,6 +268,28 @@ private AutoCommandInterface getAutoCommand(int switchPos) { true, AutonConstants.kNonProcessorShallow); } + + case 0x21 -> { + return new ProcessorShallowAutonCommand( + driveSubsystem, + pathHandler, + robotStateSubsystem, + algaeSubsystem, + biscuitSubsystem, + coralSubsystem, + elevatorSubsystem, + tagAlignSubsystem, + visionSubsystem, + "startPToE", + new ArrayList(Arrays.asList('d', 'c', 'c')), + new ArrayList( + Arrays.asList(ScoringLevel.L4, ScoringLevel.L4, ScoringLevel.L3)), + 'e', + true, + true, + AutonConstants.kNonProcessorShallow); + } + default -> { String msg = String.format("no auto command assigned for switch pos: %02X", switchPos); DriverStation.reportWarning(msg, false); diff --git a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java index c2d52bbd..be2ea52a 100644 --- a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java +++ b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java @@ -93,6 +93,8 @@ public PathHandler( this.nodeLevels = nodeLevels; this.startNode = startNode; this.mirrorToProcessor = mirrorToProcessor; + + setHeadlights(true); } public void setPathNames(String[][] pathNames) { @@ -190,7 +192,7 @@ private void startPath(Trajectory path) { } else if (curState == PathStates.FETCH) { robotStateSubsystem.setLEDLoadCoral(false); hasLEDsOn = false; - setHeadlights(true); + // setHeadlights(true); curState = PathStates.DRIVE_PLACE; } } @@ -232,7 +234,7 @@ private void drivePath() { advanceNodes(); robotStateSubsystem.setLEDLoadCoral(false); hasLEDsOn = false; - setHeadlights(true); + // setHeadlights(true); curState = PathStates.DRIVE_PLACE; } else if (shouldTransitionToServoing()) { @@ -439,7 +441,7 @@ public void periodic() { && !hasLEDsOn) { robotStateSubsystem.setLEDLoadCoral(true); hasLEDsOn = true; - setHeadlights(false); + // setHeadlights(false); } drivePath(); } @@ -451,7 +453,7 @@ public void periodic() { robotStateSubsystem.setLEDLoadCoral(false); hasLEDsOn = false; - setHeadlights(true); + // setHeadlights(true); curState = PathStates.DRIVE_PLACE; } } diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index 66d598c8..d3f8a870 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -432,7 +432,10 @@ public void periodic() { if (finalDrive && driveSubsystem.getAvgDriveCurrent() > TagServoingConstants.kEndDriveCurrentThreshold - && driveSubsystem.getAvgRearDriveVel() < TagServoingConstants.kEndVelThreshold) { + && (!isAuto + && driveSubsystem.getAvgRearDriveVel() + < TagServoingConstants.kEndVelThreshold + || isAuto && driveSubsystem.getAvgRearDriveVel() < 8)) { currentThresCount++; if (currentThresCount >= TagServoingConstants.kEndCountThreshold) { @@ -447,7 +450,11 @@ public void periodic() { } if (finalDrive) { - vX = 0.25; + if (isAuto) { + vX = 0.35; + } else { + vX = 0.25; + } } if (curState == TagAlignStates.WAITING) { break; From f42a539f7c7604ab6634e40a90a764cc66bf923b Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Sat, 29 Mar 2025 17:15:32 -0400 Subject: [PATCH 08/12] added autoAlign to barge the isFinished on the command might be screwy, needs testing --- src/main/java/frc/robot/RobotContainer.java | 5 +- .../robotState/ScoreAlgaeCommand.java | 11 +- .../robot/constants/BargeAlignConstants.java | 13 +- .../robotState/RobotStateSubsystem.java | 31 ++-- .../tagAlign/BargeAlignSubsystem.java | 138 ++++++++++++++++++ .../robot/subsystems/vision/BargeAlign.java | 115 --------------- 6 files changed, 179 insertions(+), 134 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java delete mode 100644 src/main/java/frc/robot/subsystems/vision/BargeAlign.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 23ce1668..b2646e26 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -94,6 +94,7 @@ import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates; import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide; import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; +import frc.robot.subsystems.tagAlign.BargeAlignSubsystem; import frc.robot.subsystems.tagAlign.TagAlignSubsystem; import frc.robot.subsystems.vision.VisionSubsystem; import java.util.Map; @@ -133,6 +134,7 @@ public class RobotContainer { private final LEDSubsystem ledSubsystem; private final TagAlignSubsystem tagAlignSubsystem; + private final BargeAlignSubsystem bargeAlignSubsystem; private final VisionSubsystem visionSubsystem; @@ -191,6 +193,7 @@ public RobotContainer() { visionSubsystem = new VisionSubsystem(driveSubsystem); tagAlignSubsystem = new TagAlignSubsystem(driveSubsystem, visionSubsystem); + bargeAlignSubsystem = new BargeAlignSubsystem(flysky, driveSubsystem); robotStateSubsystem = new RobotStateSubsystem( @@ -205,7 +208,7 @@ public RobotContainer() { funnelSubsystem, ledSubsystem, tagAlignSubsystem, - visionSubsystem); + visionSubsystem, bargeAlignSubsystem); driveSubsystem.setRobotStateSubsystem(robotStateSubsystem); diff --git a/src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java index 3ca60657..b84a4ea6 100644 --- a/src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java @@ -12,6 +12,7 @@ public class ScoreAlgaeCommand extends Command { private ElevatorSubsystem elevatorSubsystem; private RobotStates startState; private boolean startingElevatorFinished; + private boolean hasEjectedToBarge; public ScoreAlgaeCommand( RobotStateSubsystem robotStateSubsystem, @@ -27,7 +28,7 @@ public ScoreAlgaeCommand( public void initialize() { startState = robotStateSubsystem.getState(); startingElevatorFinished = elevatorSubsystem.isFinished(); - if (startState == RobotStates.BARGE_ALGAE || startState == RobotStates.PROCESSOR_ALGAE) { + if (startState == RobotStates.PROCESSOR_ALGAE) { robotStateSubsystem.releaseAlgae(); } else { robotStateSubsystem.toScoreAlgae(); @@ -36,14 +37,16 @@ public void initialize() { @Override public boolean isFinished() { - if (startState == RobotStates.BARGE_ALGAE || startState == RobotStates.PROCESSOR_ALGAE) { + if (startState == RobotStates.PROCESSOR_ALGAE || hasEjectedToBarge) { return robotStateSubsystem.getState() == RobotStates.FUNNEL_LOAD || robotStateSubsystem.getState() == RobotStates.STOW || !startingElevatorFinished; + } else if (robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE) { + hasEjectedToBarge = true; } else { - return robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE - || robotStateSubsystem.getState() == RobotStates.PROCESSOR_ALGAE + return robotStateSubsystem.getState() == RobotStates.PROCESSOR_ALGAE || !robotStateSubsystem.getIsBargeSafe(); } + return false; } } diff --git a/src/main/java/frc/robot/constants/BargeAlignConstants.java b/src/main/java/frc/robot/constants/BargeAlignConstants.java index dfd021f6..83eceab9 100644 --- a/src/main/java/frc/robot/constants/BargeAlignConstants.java +++ b/src/main/java/frc/robot/constants/BargeAlignConstants.java @@ -1,10 +1,17 @@ package frc.robot.constants; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; public class BargeAlignConstants { + public static final double kXSpeed = 1.0; - public static final Translation2d redBargePos = new Translation2d(2, 3); //TODO random values now - public static final Translation2d blueBargePos = new Translation2d(3, 2); - + public static final double kBlueEjectAlgaeX = 6.0; + public static final double kRedEjectAlgaeX = 6.0; + public static final double kBlueRaiseElevatorX = 5.0; + public static final double kRedRaiseElevatorX = 5.0; + + public static final Rotation2d kBlueDesiredYaw = Rotation2d.fromDegrees(0.0); + public static final Rotation2d kRedDesiredYaw = Rotation2d.fromDegrees(180.0); } diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index b31ad202..cb60c825 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -27,9 +27,13 @@ import frc.robot.subsystems.led.LEDSubsystem; import frc.robot.subsystems.led.LEDSubsystem.LEDStates; import frc.robot.subsystems.led.LEDSubsystem.PlaceStates; +import frc.robot.subsystems.tagAlign.BargeAlignSubsystem; import frc.robot.subsystems.tagAlign.TagAlignSubsystem; +import frc.robot.subsystems.tagAlign.BargeAlignSubsystem.BargeAlignStates; import frc.robot.subsystems.tagAlign.TagAlignSubsystem.TagAlignStates; import frc.robot.subsystems.vision.VisionSubsystem; +import kotlinx.html.BaseTarget; + import java.util.Set; import net.jafama.FastMath; import org.littletonrobotics.junction.Logger; @@ -54,6 +58,7 @@ public class RobotStateSubsystem extends MeasurableSubsystem { private FunnelSubsystem funnelSubsystem; private LEDSubsystem ledSubsystem; private TagAlignSubsystem tagAlignSubsystem; + private BargeAlignSubsystem bargeAlignSubsystem; private VisionSubsystem visionSubsystem; private RobotStates curState = RobotStates.IDLE; @@ -93,7 +98,8 @@ public RobotStateSubsystem( FunnelSubsystem funnelSubsystem, LEDSubsystem ledSubsystem, TagAlignSubsystem tagAlignSubsystem, - VisionSubsystem visionSubsystem) { + VisionSubsystem visionSubsystem, + BargeAlignSubsystem bargeAlignSubsystem) { this.algaeSubsystem = algaeSubsystem; this.battMonSubsystem = battMonSubsystem; this.biscuitSubsystem = biscuitSubsystem; @@ -106,6 +112,7 @@ public RobotStateSubsystem( this.ledSubsystem = ledSubsystem; this.tagAlignSubsystem = tagAlignSubsystem; this.visionSubsystem = visionSubsystem; + this.bargeAlignSubsystem = bargeAlignSubsystem; ledSubsystem.setState(LEDStates.NORMAL); } @@ -556,7 +563,7 @@ public void toAlgaeFloorPickup() { public void toScoreAlgae() { currentAlgaeHeight = algaeHeight; - if (curState == RobotStates.BARGE_ALGAE && algaeHeight == AlgaeHeight.LOW) { + if (curState == RobotStates.BARGE_ALGAE || curState == RobotStates.BARGE_ALIGN || curState == RobotStates.TO_BARGE_ALGAE && algaeHeight == AlgaeHeight.LOW) { futureState = RobotStates.PROCESSOR_ALGAE; toStowSafe(); } else if (curState == RobotStates.PROCESSOR_ALGAE && algaeHeight == AlgaeHeight.HIGH) { @@ -577,18 +584,12 @@ public void toScoreAlgae() { if (needSafeAlgaeTransfer(RobotStates.BARGE_ALGAE)) { return; } - - driveSubsystem.setDriveMultiplier(DriveConstants.kBargeScoreStickMultiplier); - double poseX = driveSubsystem.getPoseMeters().getX(); - isBargeSafe = poseX > RobotStateConstants.kRedBargeSafeX || poseX < RobotStateConstants.kBlueBargeSafeX; - if (isBargeSafe) { - elevatorSubsystem.setPosition(ElevatorConstants.kBargeSetpoint); - + bargeAlignSubsystem.startBargeAlign(); setState(RobotStates.TO_BARGE_ALGAE); } } @@ -617,7 +618,6 @@ public void releaseAlgae() { } case HIGH -> { algaeSubsystem.scoreBarge(); - setState(RobotStates.BARGE_ALGAE); } } } @@ -908,6 +908,12 @@ public void periodic() { toScoreAlgae(); } } + case BARGE_ALIGN -> { + if (bargeAlignSubsystem.getState() == BargeAlignStates.RAISE_ELEV) { + elevatorSubsystem.setPosition(ElevatorConstants.kBargeSetpoint); + curState = RobotStates.TO_BARGE_ALGAE; + } + } case TO_BARGE_ALGAE -> { if (elevatorSubsystem.isHigherThan(ElevatorConstants.kBargeHigherThan)) { biscuitSubsystem.setPosition( @@ -917,7 +923,9 @@ public void periodic() { } } case BARGE_ALGAE -> { - if (!algaeSubsystem.hasAlgae() + if (!isEjectingAlgae && elevatorSubsystem.isFinished() && biscuitSubsystem.isFinished() && bargeAlignSubsystem.getState() == BargeAlignStates.FINISHED) { + releaseAlgae(); + } else if (!algaeSubsystem.hasAlgae() && scoringTimer.hasElapsed(RobotStateConstants.kAlgaeEjectTimer) && isEjectingAlgae) { isEjectingAlgae = false; @@ -992,6 +1000,7 @@ public enum RobotStates { PRESTAGE, HP_ALGAE, PROCESSOR_ALGAE, + BARGE_ALIGN, TO_BARGE_ALGAE, BARGE_ALGAE, MIC_ALGAE, diff --git a/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java new file mode 100644 index 00000000..549d941a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java @@ -0,0 +1,138 @@ +package frc.robot.subsystems.tagAlign; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.XboxController; +import frc.robot.constants.BargeAlignConstants; +import frc.robot.constants.DriveConstants; +import frc.robot.constants.RobotStateConstants; +import frc.robot.controllers.FlyskyJoystick; +import frc.robot.subsystems.drive.DriveSubsystem; +import java.util.Set; +import java.util.function.DoubleSupplier; +import org.strykeforce.telemetry.measurable.MeasurableSubsystem; +import org.strykeforce.telemetry.measurable.Measure; + +public class BargeAlignSubsystem extends MeasurableSubsystem { + + private DriveSubsystem driveSubsystem; + + private FlyskyJoystick flysky; + private PIDController driveOmega; + + private BargeAlignStates curState = BargeAlignStates.FINISHED; + private boolean isOnBlueSide = true; + + private double vX; + private Rotation2d targetYaw; + + public BargeAlignSubsystem(FlyskyJoystick flysky, DriveSubsystem driveSubsystem) { + this.flysky = flysky; + this.driveSubsystem = driveSubsystem; + this.driveOmega = new PIDController(6.0, 0, 0); + this.driveOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180)); + } + + public void startBargeAlign() { + if (isSafe()) { + setState(BargeAlignStates.DRIVE); + isOnBlueSide(); + driveOmega.reset(); + } + } + + public void killBargeAlign() { + setState(BargeAlignStates.FINISHED); + } + + private void setState(BargeAlignStates desiredState) { + this.curState = desiredState; + } + + public BargeAlignStates getState() { + return curState; + } + + private boolean isOnBlueSide() { + isOnBlueSide = driveSubsystem + .getPoseMeters() + .getX() < DriveConstants.kCenterLineX; + vX = isOnBlueSide ? BargeAlignConstants.kXSpeed : -BargeAlignConstants.kXSpeed; + targetYaw = isOnBlueSide ? BargeAlignConstants.kBlueDesiredYaw : BargeAlignConstants.kRedDesiredYaw; + return isOnBlueSide; + } + + private boolean isSafe() { + double poseX = driveSubsystem.getPoseMeters().getX(); + + return poseX > RobotStateConstants.kRedBargeSafeX + || poseX < RobotStateConstants.kBlueBargeSafeX; + } + + private boolean shouldRaiseElevator() { + double poseX = driveSubsystem.getPoseMeters().getX(); + return isOnBlueSide ? poseX > BargeAlignConstants.kBlueRaiseElevatorX : poseX < BargeAlignConstants.kRedRaiseElevatorX; + } + + private boolean shouldEjectAlgae() { + double poseX = driveSubsystem.getPoseMeters().getX(); + return isOnBlueSide ? poseX > BargeAlignConstants.kBlueEjectAlgaeX : poseX < BargeAlignConstants.kRedEjectAlgaeX; + } + + private double getYStickReading() { + return flysky.getStr(); // just a placeholder, to remind me + } // same joystick reading as the drive + + /* + private void configureDriverBindings() { + driveSubsystem.setDefaultCommand( + new DriveTeleopCommand( + () -> flysky.getStr() + driveSubsystem, + robotStateSubsystem)); + */ + + private Rotation2d getTargetYaw() { + return isOnBlueSide() ? BargeAlignConstants.kBlueDesiredYaw : BargeAlignConstants.kRedDesiredYaw; + } + + @Override + public void periodic() { + switch (curState) { + case DRIVE -> { + double vOmega = + driveOmega.calculate( + driveSubsystem.getPoseMeters().getRotation().getRadians(), targetYaw.getRadians()); + driveSubsystem.move(vX, getYStickReading(), vOmega, true); + if (shouldRaiseElevator()) { + setState(BargeAlignStates.RAISE_ELEV); + } + } + case RAISE_ELEV -> { + double vOmega = + driveOmega.calculate( + driveSubsystem.getPoseMeters().getRotation().getRadians(), targetYaw.getRadians()); + driveSubsystem.move(vX, getYStickReading(), vOmega, true); + if (shouldEjectAlgae()) { + setState(BargeAlignStates.FINISHED); + } + } + case FINISHED -> {} + } + } + + @Override + public Set getMeasures() { + return Set.of(new Measure("BargeAlign/curState", () -> curState.ordinal())); + } + + public enum BargeAlignStates { + // INIT, + DRIVE, + RAISE_ELEV, + FINISHED + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/BargeAlign.java b/src/main/java/frc/robot/subsystems/vision/BargeAlign.java deleted file mode 100644 index 3b40f378..00000000 --- a/src/main/java/frc/robot/subsystems/vision/BargeAlign.java +++ /dev/null @@ -1,115 +0,0 @@ -package frc.robot.subsystems.vision; - -import java.time.OffsetDateTime; -import java.util.Set; - -import org.strykeforce.telemetry.measurable.MeasurableSubsystem; -import org.strykeforce.telemetry.measurable.Measure; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import frc.robot.constants.BargeAlignConstants; -import java.util.function.DoubleSupplier; -import frc.robot.subsystems.drive.DriveSubsystem; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; - -public class BargeAlign extends MeasurableSubsystem{ -private DoubleSupplier strStick; //note to Huck: what's strStick? - -private DriveSubsystem driveSubsystem; - -//These are here to determine offsets for target positions. - -private boolean blueAlliance; -private boolean redBarge; -private final XboxController xboxController = new XboxController(1); -private Alliance alliance; -private double driveRadius = 1.5; //TODO This is a magic number that needs creating -private BargeAlignStates curState; - -public BargeAlign( - DoubleSupplier strStick, - DriveSubsystem driveSubsystem){ - - if(alliance == Alliance.Blue){ - blueAlliance = true; - } - else{ - blueAlliance = false; - } - curState = BargeAlignStates.DRIVE; -} - -public void setState(BargeAlignStates curState){ - this.curState = curState; -} - -private boolean getOnBlueSide() { - return - driveSubsystem.getPoseMeters().getMeasureX() - .compareTo(BargeAlignConstants.blueBargePos.getMeasureX()) <= 0; -} - -private boolean getOnRedSide() { - return - driveSubsystem.getPoseMeters().getMeasureX() - .compareTo(BargeAlignConstants.redBargePos.getMeasureX()) <= 0; -} - -private double getYStickReading() { - return XboxController.Button.kLeftStick.kY.value; //just a placeholder, to remind me -} //same joystick reading as the drive - -/* - private void configureDriverBindings() { - driveSubsystem.setDefaultCommand( - new DriveTeleopCommand( - () -> flysky.getStr() - driveSubsystem, - robotStateSubsystem)); -*/ - -private Translation2d getTargetTranslation(){ - - Translation2d blueOffset = new Translation2d( - getOnBlueSide() ? driveRadius : driveRadius * -1 , new Rotation2d( - getOnBlueSide() ? 0 : Math.PI)); - - /* - Translation2d redOffset = new Translation2d( - getOnRedSide() ? driveRadius : driveRadius * -1 , new Rotation2d( - getOnRedSide() ? 0 : Math.PI)); - */ - - Translation2d targetBarge = - blueAlliance ? BargeAlignConstants.blueBargePos : BargeAlignConstants.redBargePos; - - Translation2d targetPos = targetBarge.minus(offset); - return targetPos; -} - -@Override -public void periodic(){ - switch(curState){ - case DRIVE: - - break; - case FINISHED: - break; - } -} - -@Override -public Set getMeasures(){ - return Set.of(new Measure("BargeAlign/curState", () -> curState.ordinal())); -} - -public enum BargeAlignStates { - //INIT, - DRIVE, - FINISHED -} - -} From 8c7272e88b7cb6ad7df978c13e6a6e73790e959c Mon Sep 17 00:00:00 2001 From: David Shen Date: Sat, 29 Mar 2025 17:29:54 -0400 Subject: [PATCH 09/12] camera positions --- .../java/frc/robot/constants/VisionConstants.java | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 2d5945c5..8b02c0aa 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -76,9 +76,9 @@ public final class VisionConstants { // Names public static final String kCam1Name = "Left Servo"; - public static final String kCam2Name = "Front"; + public static final String kCam2Name = "Rear Right"; // when looking out the back public static final String kCam3Name = "Right Servo"; - public static final String kCam4Name = "Back"; + public static final String kCam4Name = "Rear Left"; public static final String kCam5Name = "Rear"; // public static final String kPi1Name = "Left"; @@ -99,18 +99,16 @@ public final class VisionConstants { new Pose3d(new Translation3d(0.305, 0.025, 0.311), new Rotation3d()); public static final Pose3d kCam2Pose = new Pose3d( - new Translation3d(-0.2225, -0.3025, 0.54), + new Translation3d(-0.275, -0.21, 0.49), new Rotation3d( - Units.degreesToRadians(10), Units.degreesToRadians(0), Units.degreesToRadians(67.5))); + Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(145))); public static final Pose3d kCam3Pose = new Pose3d(new Translation3d(0.133, -0.305, 0.311), new Rotation3d()); public static final Pose3d kCam4Pose = new Pose3d( - new Translation3d(-0.2675, -0.3025, 0.54), + new Translation3d(-0.275, -0.145, 0.49), new Rotation3d( - Units.degreesToRadians(10), - Units.degreesToRadians(0), - Units.degreesToRadians(112.5))); + Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(-145))); public static final Pose3d kCam5Pose = new Pose3d( new Translation3d(-0.229, 0.073, 0.934), From ea0ca16d6e086ac4d16db11f72df8830e5e4b604 Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Sat, 29 Mar 2025 18:33:40 -0400 Subject: [PATCH 10/12] bring back auto vs manual choosing --- src/main/java/frc/robot/RobotContainer.java | 3 +- .../robotState/InterruptAutoCommand.java | 18 +++++-- .../robot/constants/BargeAlignConstants.java | 12 ++--- .../robotState/RobotStateSubsystem.java | 40 +++++++++++--- .../tagAlign/BargeAlignSubsystem.java | 54 +++++++++++-------- 5 files changed, 87 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b2646e26..8372e294 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -208,7 +208,8 @@ public RobotContainer() { funnelSubsystem, ledSubsystem, tagAlignSubsystem, - visionSubsystem, bargeAlignSubsystem); + visionSubsystem, + bargeAlignSubsystem); driveSubsystem.setRobotStateSubsystem(robotStateSubsystem); diff --git a/src/main/java/frc/robot/commands/robotState/InterruptAutoCommand.java b/src/main/java/frc/robot/commands/robotState/InterruptAutoCommand.java index 5c308da4..be6de296 100644 --- a/src/main/java/frc/robot/commands/robotState/InterruptAutoCommand.java +++ b/src/main/java/frc/robot/commands/robotState/InterruptAutoCommand.java @@ -8,6 +8,8 @@ public class InterruptAutoCommand extends Command { private RobotStateSubsystem robotState; private boolean ejectCoral = false; + private boolean ejectAlgae = false; + private RobotStates interruptedState; public InterruptAutoCommand(RobotStateSubsystem robotState, CoralSubsystem coralSubsystem) { this.robotState = robotState; @@ -17,9 +19,17 @@ public InterruptAutoCommand(RobotStateSubsystem robotState, CoralSubsystem coral @Override public void initialize() { + interruptedState = robotState.getInterruptedState(); if (robotState.getState() == RobotStates.INTERRUPTED) { - ejectCoral = true; - robotState.toPlaceCoral(); + if (interruptedState == RobotStates.BARGE_ALIGN + || interruptedState == RobotStates.TO_BARGE_ALGAE + || interruptedState == RobotStates.BARGE_ALGAE) { + ejectAlgae = true; + robotState.releaseAlgae(); + } else { + ejectCoral = true; + robotState.toPlaceCoral(); + } } else { ejectCoral = false; robotState.toInterrupted(); @@ -28,6 +38,8 @@ public void initialize() { @Override public boolean isFinished() { - return !ejectCoral || !robotState.hasCoral(); + return !ejectCoral + || (ejectCoral && !robotState.hasCoral()) + || (ejectAlgae && !robotState.hasAlgae()); } } diff --git a/src/main/java/frc/robot/constants/BargeAlignConstants.java b/src/main/java/frc/robot/constants/BargeAlignConstants.java index 83eceab9..c7bd8203 100644 --- a/src/main/java/frc/robot/constants/BargeAlignConstants.java +++ b/src/main/java/frc/robot/constants/BargeAlignConstants.java @@ -1,16 +1,14 @@ package frc.robot.constants; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; public class BargeAlignConstants { - public static final double kXSpeed = 1.0; + public static final double kXSpeed = 0.35; - public static final double kBlueEjectAlgaeX = 6.0; - public static final double kRedEjectAlgaeX = 6.0; - public static final double kBlueRaiseElevatorX = 5.0; - public static final double kRedRaiseElevatorX = 5.0; + public static final double kBlueEjectAlgaeX = 7.72; + public static final double kRedEjectAlgaeX = 9.81; + public static final double kBlueRaiseElevatorX = 6.72; + public static final double kRedRaiseElevatorX = 10.81; public static final Rotation2d kBlueDesiredYaw = Rotation2d.fromDegrees(0.0); public static final Rotation2d kRedDesiredYaw = Rotation2d.fromDegrees(180.0); diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index cb60c825..9d718b88 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -7,7 +7,6 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; -import frc.robot.constants.DriveConstants; import frc.robot.constants.ElevatorConstants; import frc.robot.constants.RobotConstants; import frc.robot.constants.RobotStateConstants; @@ -28,12 +27,10 @@ import frc.robot.subsystems.led.LEDSubsystem.LEDStates; import frc.robot.subsystems.led.LEDSubsystem.PlaceStates; import frc.robot.subsystems.tagAlign.BargeAlignSubsystem; -import frc.robot.subsystems.tagAlign.TagAlignSubsystem; import frc.robot.subsystems.tagAlign.BargeAlignSubsystem.BargeAlignStates; +import frc.robot.subsystems.tagAlign.TagAlignSubsystem; import frc.robot.subsystems.tagAlign.TagAlignSubsystem.TagAlignStates; import frc.robot.subsystems.vision.VisionSubsystem; -import kotlinx.html.BaseTarget; - import java.util.Set; import net.jafama.FastMath; import org.littletonrobotics.junction.Logger; @@ -83,6 +80,8 @@ public class RobotStateSubsystem extends MeasurableSubsystem { private boolean prestagingForAlgae = false; private boolean hasElevatorAutoCoralPrestaged = false; + private RobotStates interruptedState = RobotStates.IDLE; + private Timer scoringTimer = new Timer(); private Angle nextBiscuitSetpoint; @@ -125,6 +124,10 @@ public RobotStates getNextState() { return nextState; } + public RobotStates getInterruptedState() { + return interruptedState; + } + public CoralLoc getCoralLoc() { return coralLoc; } @@ -563,8 +566,12 @@ public void toAlgaeFloorPickup() { public void toScoreAlgae() { currentAlgaeHeight = algaeHeight; - if (curState == RobotStates.BARGE_ALGAE || curState == RobotStates.BARGE_ALIGN || curState == RobotStates.TO_BARGE_ALGAE && algaeHeight == AlgaeHeight.LOW) { + if ((curState == RobotStates.BARGE_ALGAE + || curState == RobotStates.BARGE_ALIGN + || curState == RobotStates.TO_BARGE_ALGAE) + && algaeHeight == AlgaeHeight.LOW) { futureState = RobotStates.PROCESSOR_ALGAE; + bargeAlignSubsystem.terminate(); toStowSafe(); } else if (curState == RobotStates.PROCESSOR_ALGAE && algaeHeight == AlgaeHeight.HIGH) { futureState = RobotStates.BARGE_ALGAE; @@ -589,8 +596,14 @@ public void toScoreAlgae() { poseX > RobotStateConstants.kRedBargeSafeX || poseX < RobotStateConstants.kBlueBargeSafeX; if (isBargeSafe) { - bargeAlignSubsystem.startBargeAlign(); - setState(RobotStates.TO_BARGE_ALGAE); + if (isAutoPlacing) { + driveSubsystem.setIgnoreSticks(true); + bargeAlignSubsystem.startBargeAlign(); + setState(RobotStates.BARGE_ALIGN); + } else { + elevatorSubsystem.setPosition(ElevatorConstants.kBargeSetpoint); + setState(RobotStates.TO_BARGE_ALGAE); + } } } } @@ -658,6 +671,7 @@ private void toProcessor() { } public void toInterrupted() { + interruptedState = curState; if (tagAlignSubsystem.getState() != TagAlignSubsystem.TagAlignStates.DONE) { tagAlignSubsystem.terminate(); visionSubsystem.setYawUpdateCamera(-1); @@ -670,6 +684,11 @@ public void toInterrupted() { driveSubsystem.setIgnoreSticks(false); } + if (bargeAlignSubsystem.getState() != BargeAlignStates.FINISHED) { + bargeAlignSubsystem.terminate(); + driveSubsystem.setIgnoreSticks(false); + } + biscuitSubsystem.setIsRemovingAlgae(false); biscuitSubsystem.setPosition(biscuitSubsystem.getPosition(), hasAlgae()); @@ -923,11 +942,16 @@ public void periodic() { } } case BARGE_ALGAE -> { - if (!isEjectingAlgae && elevatorSubsystem.isFinished() && biscuitSubsystem.isFinished() && bargeAlignSubsystem.getState() == BargeAlignStates.FINISHED) { + if (!isEjectingAlgae + && elevatorSubsystem.isFinished() + && biscuitSubsystem.isFinished() + && bargeAlignSubsystem.getState() == BargeAlignStates.FINISHED + && isAutoPlacing) { releaseAlgae(); } else if (!algaeSubsystem.hasAlgae() && scoringTimer.hasElapsed(RobotStateConstants.kAlgaeEjectTimer) && isEjectingAlgae) { + driveSubsystem.setIgnoreSticks(false); isEjectingAlgae = false; toStowSafe(); } else if (algaeHeight != currentAlgaeHeight) { diff --git a/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java index 549d941a..78bcb5c3 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java @@ -1,18 +1,13 @@ package frc.robot.subsystems.tagAlign; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.XboxController; import frc.robot.constants.BargeAlignConstants; import frc.robot.constants.DriveConstants; import frc.robot.constants.RobotStateConstants; import frc.robot.controllers.FlyskyJoystick; import frc.robot.subsystems.drive.DriveSubsystem; import java.util.Set; -import java.util.function.DoubleSupplier; import org.strykeforce.telemetry.measurable.MeasurableSubsystem; import org.strykeforce.telemetry.measurable.Measure; @@ -22,24 +17,28 @@ public class BargeAlignSubsystem extends MeasurableSubsystem { private FlyskyJoystick flysky; private PIDController driveOmega; + private PIDController driveX; private BargeAlignStates curState = BargeAlignStates.FINISHED; private boolean isOnBlueSide = true; private double vX; private Rotation2d targetYaw; + private double targetX; public BargeAlignSubsystem(FlyskyJoystick flysky, DriveSubsystem driveSubsystem) { this.flysky = flysky; this.driveSubsystem = driveSubsystem; this.driveOmega = new PIDController(6.0, 0, 0); + this.driveX = new PIDController(4, 0, 0); this.driveOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180)); } public void startBargeAlign() { if (isSafe()) { + this.isOnBlueSide = isOnBlueSide(); setState(BargeAlignStates.DRIVE); - isOnBlueSide(); + setupBargeAlign(); driveOmega.reset(); } } @@ -57,33 +56,43 @@ public BargeAlignStates getState() { } private boolean isOnBlueSide() { - isOnBlueSide = driveSubsystem - .getPoseMeters() - .getX() < DriveConstants.kCenterLineX; + return driveSubsystem.getPoseMeters().getX() < DriveConstants.kCenterLineX; + } + + private void setupBargeAlign() { vX = isOnBlueSide ? BargeAlignConstants.kXSpeed : -BargeAlignConstants.kXSpeed; - targetYaw = isOnBlueSide ? BargeAlignConstants.kBlueDesiredYaw : BargeAlignConstants.kRedDesiredYaw; - return isOnBlueSide; + targetYaw = + isOnBlueSide ? BargeAlignConstants.kBlueDesiredYaw : BargeAlignConstants.kRedDesiredYaw; + targetX = + isOnBlueSide + ? BargeAlignConstants.kBlueRaiseElevatorX + 0.5 + : BargeAlignConstants.kRedRaiseElevatorX - 0.5; } private boolean isSafe() { double poseX = driveSubsystem.getPoseMeters().getX(); return poseX > RobotStateConstants.kRedBargeSafeX - || poseX < RobotStateConstants.kBlueBargeSafeX; + || poseX < RobotStateConstants.kBlueBargeSafeX; } - + private boolean shouldRaiseElevator() { double poseX = driveSubsystem.getPoseMeters().getX(); - return isOnBlueSide ? poseX > BargeAlignConstants.kBlueRaiseElevatorX : poseX < BargeAlignConstants.kRedRaiseElevatorX; + return isOnBlueSide + ? poseX > BargeAlignConstants.kBlueRaiseElevatorX + : poseX < BargeAlignConstants.kRedRaiseElevatorX; } - + private boolean shouldEjectAlgae() { double poseX = driveSubsystem.getPoseMeters().getX(); - return isOnBlueSide ? poseX > BargeAlignConstants.kBlueEjectAlgaeX : poseX < BargeAlignConstants.kRedEjectAlgaeX; + return isOnBlueSide + ? poseX > BargeAlignConstants.kBlueEjectAlgaeX + : poseX < BargeAlignConstants.kRedEjectAlgaeX; } private double getYStickReading() { - return flysky.getStr(); // just a placeholder, to remind me + return flysky.getStr() + * DriveConstants.kMaxSpeedMetersPerSecond; // just a placeholder, to remind me } // same joystick reading as the drive /* @@ -95,18 +104,21 @@ private void configureDriverBindings() { robotStateSubsystem)); */ - private Rotation2d getTargetYaw() { - return isOnBlueSide() ? BargeAlignConstants.kBlueDesiredYaw : BargeAlignConstants.kRedDesiredYaw; + public void terminate() { + driveSubsystem.move(0, 0, 0, true); + driveSubsystem.drive(0, 0, 0); + setState(BargeAlignStates.FINISHED); } @Override public void periodic() { switch (curState) { case DRIVE -> { + double driveXVel = driveX.calculate(driveSubsystem.getPoseMeters().getX(), targetX); double vOmega = driveOmega.calculate( driveSubsystem.getPoseMeters().getRotation().getRadians(), targetYaw.getRadians()); - driveSubsystem.move(vX, getYStickReading(), vOmega, true); + driveSubsystem.move(driveXVel, getYStickReading(), vOmega, true); if (shouldRaiseElevator()) { setState(BargeAlignStates.RAISE_ELEV); } @@ -117,7 +129,7 @@ public void periodic() { driveSubsystem.getPoseMeters().getRotation().getRadians(), targetYaw.getRadians()); driveSubsystem.move(vX, getYStickReading(), vOmega, true); if (shouldEjectAlgae()) { - setState(BargeAlignStates.FINISHED); + terminate(); } } case FINISHED -> {} From c2ea525313cf3e94a937b70a2509d960d23820b3 Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Sat, 29 Mar 2025 20:01:36 -0400 Subject: [PATCH 11/12] tweak constants through testing, fix interrupt behavior --- .../robot/constants/BargeAlignConstants.java | 10 +++++----- .../robotState/RobotStateSubsystem.java | 5 ++++- .../tagAlign/BargeAlignSubsystem.java | 18 ++++++++++++++++-- 3 files changed, 25 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/constants/BargeAlignConstants.java b/src/main/java/frc/robot/constants/BargeAlignConstants.java index c7bd8203..07bc5f35 100644 --- a/src/main/java/frc/robot/constants/BargeAlignConstants.java +++ b/src/main/java/frc/robot/constants/BargeAlignConstants.java @@ -3,12 +3,12 @@ import edu.wpi.first.math.geometry.Rotation2d; public class BargeAlignConstants { - public static final double kXSpeed = 0.35; + public static final double kXSpeed = 0.45; - public static final double kBlueEjectAlgaeX = 7.72; - public static final double kRedEjectAlgaeX = 9.81; - public static final double kBlueRaiseElevatorX = 6.72; - public static final double kRedRaiseElevatorX = 10.81; + public static final double kBlueEjectAlgaeX = 7.67; // 7.72 + public static final double kRedEjectAlgaeX = 9.86; // 9.81 + public static final double kBlueRaiseElevatorX = 7.02; // 7.22 + public static final double kRedRaiseElevatorX = 10.51; // 10.31 public static final Rotation2d kBlueDesiredYaw = Rotation2d.fromDegrees(0.0); public static final Rotation2d kRedDesiredYaw = Rotation2d.fromDegrees(180.0); diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index ba5e2368..7ff87c89 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -7,6 +7,7 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; +import frc.robot.constants.DriveConstants; import frc.robot.constants.ElevatorConstants; import frc.robot.constants.RobotConstants; import frc.robot.constants.RobotStateConstants; @@ -596,9 +597,10 @@ public void toScoreAlgae() { poseX > RobotStateConstants.kRedBargeSafeX || poseX < RobotStateConstants.kBlueBargeSafeX; if (isBargeSafe) { + driveSubsystem.setDriveMultiplier(DriveConstants.kBargeScoreStickMultiplier); if (isAutoPlacing) { driveSubsystem.setIgnoreSticks(true); - bargeAlignSubsystem.startBargeAlign(); + bargeAlignSubsystem.startBargeAlign(allianceColor); setState(RobotStates.BARGE_ALIGN); } else { elevatorSubsystem.setPosition(ElevatorConstants.kBargeSetpoint); @@ -623,6 +625,7 @@ public void releaseAlgae() { processorReleasePose = driveSubsystem.getPoseMeters(); Logger.recordOutput("RobotState/Processor Release Pose", processorReleasePose); driveSubsystem.removeDriveMultiplier(); + if (curState == RobotStates.INTERRUPTED) setState(RobotStates.BARGE_ALGAE); switch (algaeHeight) { case LOW -> { diff --git a/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java index 78bcb5c3..31a2760c 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java @@ -2,12 +2,14 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.robot.constants.BargeAlignConstants; import frc.robot.constants.DriveConstants; import frc.robot.constants.RobotStateConstants; import frc.robot.controllers.FlyskyJoystick; import frc.robot.subsystems.drive.DriveSubsystem; import java.util.Set; +import org.littletonrobotics.junction.Logger; import org.strykeforce.telemetry.measurable.MeasurableSubsystem; import org.strykeforce.telemetry.measurable.Measure; @@ -18,6 +20,7 @@ public class BargeAlignSubsystem extends MeasurableSubsystem { private FlyskyJoystick flysky; private PIDController driveOmega; private PIDController driveX; + private Alliance alliance; private BargeAlignStates curState = BargeAlignStates.FINISHED; private boolean isOnBlueSide = true; @@ -34,7 +37,8 @@ public BargeAlignSubsystem(FlyskyJoystick flysky, DriveSubsystem driveSubsystem) this.driveOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180)); } - public void startBargeAlign() { + public void startBargeAlign(Alliance alliance) { + this.alliance = alliance; if (isSafe()) { this.isOnBlueSide = isOnBlueSide(); setState(BargeAlignStates.DRIVE); @@ -92,7 +96,9 @@ private boolean shouldEjectAlgae() { private double getYStickReading() { return flysky.getStr() - * DriveConstants.kMaxSpeedMetersPerSecond; // just a placeholder, to remind me + * DriveConstants.kMaxSpeedMetersPerSecond + * DriveConstants.kBargeScoreStickMultiplier + * (alliance == Alliance.Blue ? -1 : 1); // just a placeholder, to remind me } // same joystick reading as the drive /* @@ -112,6 +118,7 @@ public void terminate() { @Override public void periodic() { + Logger.recordOutput("BargeAlign/State", curState); switch (curState) { case DRIVE -> { double driveXVel = driveX.calculate(driveSubsystem.getPoseMeters().getX(), targetX); @@ -119,6 +126,10 @@ public void periodic() { driveOmega.calculate( driveSubsystem.getPoseMeters().getRotation().getRadians(), targetYaw.getRadians()); driveSubsystem.move(driveXVel, getYStickReading(), vOmega, true); + Logger.recordOutput("BargeAlign/XErr", driveX.getError()); + Logger.recordOutput("BargeAlign/Vx", driveXVel); + Logger.recordOutput("BargeAlign/OmegaErr", driveOmega.getError()); + Logger.recordOutput("BargeAlign/Vomega", vOmega); if (shouldRaiseElevator()) { setState(BargeAlignStates.RAISE_ELEV); } @@ -128,6 +139,9 @@ public void periodic() { driveOmega.calculate( driveSubsystem.getPoseMeters().getRotation().getRadians(), targetYaw.getRadians()); driveSubsystem.move(vX, getYStickReading(), vOmega, true); + Logger.recordOutput("BargeAlign/Vx", vX); + Logger.recordOutput("BargeAlign/OmegaErr", driveOmega.getError()); + Logger.recordOutput("BargeAlign/Vomega", vOmega); if (shouldEjectAlgae()) { terminate(); } From 1a07ea20f87e3032762e206fde3d2b14726a745e Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Sun, 30 Mar 2025 13:26:18 -0400 Subject: [PATCH 12/12] tweak elevator safety, add autoPlace LEDs --- src/main/java/frc/robot/constants/RobotStateConstants.java | 2 +- .../frc/robot/subsystems/robotState/RobotStateSubsystem.java | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/constants/RobotStateConstants.java b/src/main/java/frc/robot/constants/RobotStateConstants.java index 6d4d9bdd..c8fd2cd5 100644 --- a/src/main/java/frc/robot/constants/RobotStateConstants.java +++ b/src/main/java/frc/robot/constants/RobotStateConstants.java @@ -4,7 +4,7 @@ public class RobotStateConstants { public static final double[] kNodeAngles = {0.0, 60.0, 120.0, 180.0, -120.0, -60.0}; public static final double kAlgaeRetreatDistance = 0; - public static final double kBlueBargeSafeX = 7.6; + public static final double kBlueBargeSafeX = 7; // 7.6 public static final double kRedBargeSafeX = DriveConstants.kFieldMaxX - kBlueBargeSafeX; public static final double kCoralEjectTimer = 0.25; diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index 7ff87c89..fc5c5931 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -599,6 +599,7 @@ public void toScoreAlgae() { if (isBargeSafe) { driveSubsystem.setDriveMultiplier(DriveConstants.kBargeScoreStickMultiplier); if (isAutoPlacing) { + setAutoPlacingLed(true); driveSubsystem.setIgnoreSticks(true); bargeAlignSubsystem.startBargeAlign(allianceColor); setState(RobotStates.BARGE_ALIGN); @@ -958,10 +959,12 @@ public void periodic() { } else if (!algaeSubsystem.hasAlgae() && scoringTimer.hasElapsed(RobotStateConstants.kAlgaeEjectTimer) && isEjectingAlgae) { + setAutoPlacingLed(false); driveSubsystem.setIgnoreSticks(false); isEjectingAlgae = false; toStowSafe(); } else if (algaeHeight != currentAlgaeHeight) { + setAutoPlacingLed(false); toScoreAlgae(); } }