From d72c20925e1166a2ebe1f0fd999cfaef792be740 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 22 Jan 2024 18:55:48 +0000 Subject: [PATCH 1/2] Bump edu.wpi.first.GradleRIO from 2024.1.1 to 2024.2.1 Bumps edu.wpi.first.GradleRIO from 2024.1.1 to 2024.2.1. --- updated-dependencies: - dependency-name: edu.wpi.first.GradleRIO dependency-type: direct:production update-type: version-update:semver-minor ... Signed-off-by: dependabot[bot] --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index d34b717..4ebc7dd 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.2.1" id 'com.diffplug.spotless' version '6.24.0' } From 5f9ffbd906ea265ac265918b5231259d14f756cd Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Tue, 23 Jan 2024 19:50:57 -0600 Subject: [PATCH 2/2] Update YAGSL libraries, merge old swerve config Updated the YAGSL libraries and removed the offline install of the YAGSL library. Updated the base swerve code to include most of the changes made for 2024. Updated PathPlannerLib. --- src/main/deploy/pathplanner/SamplePath.path | 76 + .../generatedJSON/New Path.wpilib.json | 9002 +++++++++++++++++ .../generatedJSON/SamplePath.wpilib.json | 9002 +++++++++++++++++ src/main/deploy/pathplanner/navgrid.json | 1 + .../deploy/pathplanner/paths/New Path.path | 65 + .../deploy/swerve/controllerproperties.json | 8 + src/main/deploy/swerve/modules/backleft.json | 30 + src/main/deploy/swerve/modules/backright.json | 30 + src/main/deploy/swerve/modules/frontleft.json | 30 + .../deploy/swerve/modules/frontright.json | 30 + .../swerve/modules/physicalproperties.json | 16 + .../deploy/swerve/modules/pidfproperties.json | 16 + src/main/deploy/swerve/swervedrive.json | 14 + src/main/java/frc/robot/Constants.java | 6 +- src/main/java/frc/robot/Robot.java | 18 +- src/main/java/frc/robot/RobotContainer.java | 105 +- src/main/java/frc/robot/commands/Autos.java | 20 - .../frc/robot/commands/ExampleCommand.java | 43 - .../swervedrive/auto/AutoBalanceCommand.java | 76 + .../swervedrive/drivebase/AbsoluteDrive.java | 19 +- .../drivebase/AbsoluteDriveAdv.java | 152 + .../drivebase/AbsoluteFieldDrive.java | 89 + .../swervedrive/drivebase/SwerveCommand.java | 68 - .../swervedrive/drivebase/TeleopDrive.java | 76 - .../robot/subsystems/ExampleSubsystem.java | 47 - .../swervedrive/SwerveSubsystem.java | 309 +- src/main/java/swervelib/SwerveController.java | 228 - src/main/java/swervelib/SwerveDrive.java | 1044 -- src/main/java/swervelib/SwerveModule.java | 431 - .../encoders/AnalogAbsoluteEncoderSwerve.java | 123 - .../swervelib/encoders/CANCoderSwerve.java | 190 - .../swervelib/encoders/CanAndCoderSwerve.java | 86 - .../encoders/PWMDutyCycleEncoderSwerve.java | 104 - .../encoders/SparkMaxAnalogEncoderSwerve.java | 127 - .../encoders/SparkMaxEncoderSwerve.java | 133 - .../encoders/SwerveAbsoluteEncoder.java | 59 - .../java/swervelib/encoders/package-info.java | 5 - .../java/swervelib/imu/ADIS16448Swerve.java | 92 - .../java/swervelib/imu/ADIS16470Swerve.java | 91 - .../java/swervelib/imu/ADXRS450Swerve.java | 89 - .../java/swervelib/imu/AnalogGyroSwerve.java | 94 - src/main/java/swervelib/imu/NavXSwerve.java | 148 - .../java/swervelib/imu/Pigeon2Swerve.java | 125 - src/main/java/swervelib/imu/PigeonSwerve.java | 95 - src/main/java/swervelib/imu/SwerveIMU.java | 51 - src/main/java/swervelib/imu/package-info.java | 2 - src/main/java/swervelib/math/Matter.java | 33 - src/main/java/swervelib/math/SwerveMath.java | 381 - .../java/swervelib/math/package-info.java | 6 - .../swervelib/motors/SparkFlexSwerve.java | 399 - .../motors/SparkMaxBrushedMotorSwerve.java | 414 - .../java/swervelib/motors/SparkMaxSwerve.java | 391 - .../java/swervelib/motors/SwerveMotor.java | 154 - .../java/swervelib/motors/TalonFXSwerve.java | 400 - .../java/swervelib/motors/TalonSRXSwerve.java | 379 - .../java/swervelib/motors/package-info.java | 2 - src/main/java/swervelib/package-info.java | 6 - .../java/swervelib/parser/PIDFConfig.java | 88 - .../parser/SwerveControllerConfiguration.java | 59 - .../parser/SwerveDriveConfiguration.java | 89 - .../parser/SwerveModuleConfiguration.java | 144 - .../SwerveModulePhysicalCharacteristics.java | 85 - .../java/swervelib/parser/SwerveParser.java | 213 - .../parser/deserializer/PIDFRange.java | 11 - .../parser/deserializer/package-info.java | 2 - .../parser/json/ControllerPropertiesJson.java | 31 - .../swervelib/parser/json/DeviceJson.java | 187 - .../swervelib/parser/json/ModuleJson.java | 124 - .../parser/json/MotorConfigDouble.java | 25 - .../swervelib/parser/json/MotorConfigInt.java | 25 - .../parser/json/PIDFPropertiesJson.java | 13 - .../parser/json/PhysicalPropertiesJson.java | 49 - .../parser/json/SwerveDriveJson.java | 17 - .../parser/json/modules/BoolMotorJson.java | 11 - .../parser/json/modules/LocationJson.java | 15 - .../parser/json/modules/package-info.java | 2 - .../swervelib/parser/json/package-info.java | 2 - .../java/swervelib/parser/package-info.java | 2 - .../simulation/SwerveIMUSimulation.java | 106 - .../simulation/SwerveModuleSimulation.java | 75 - .../swervelib/simulation/package-info.java | 2 - src/main/java/swervelib/telemetry/Alert.java | 217 - .../telemetry/SwerveDriveTelemetry.java | 97 - .../swervelib/telemetry/package-info.java | 2 - vendordeps/PathplannerLib.json | 6 +- vendordeps/yagsl.json | 4 +- 86 files changed, 18914 insertions(+), 8019 deletions(-) create mode 100644 src/main/deploy/pathplanner/SamplePath.path create mode 100644 src/main/deploy/pathplanner/generatedJSON/New Path.wpilib.json create mode 100644 src/main/deploy/pathplanner/generatedJSON/SamplePath.wpilib.json create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/New Path.path create mode 100644 src/main/deploy/swerve/controllerproperties.json create mode 100644 src/main/deploy/swerve/modules/backleft.json create mode 100644 src/main/deploy/swerve/modules/backright.json create mode 100644 src/main/deploy/swerve/modules/frontleft.json create mode 100644 src/main/deploy/swerve/modules/frontright.json create mode 100644 src/main/deploy/swerve/modules/physicalproperties.json create mode 100644 src/main/deploy/swerve/modules/pidfproperties.json create mode 100644 src/main/deploy/swerve/swervedrive.json delete mode 100644 src/main/java/frc/robot/commands/Autos.java delete mode 100644 src/main/java/frc/robot/commands/ExampleCommand.java create mode 100644 src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java create mode 100644 src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java create mode 100644 src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java delete mode 100644 src/main/java/frc/robot/commands/swervedrive/drivebase/SwerveCommand.java delete mode 100644 src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java delete mode 100644 src/main/java/frc/robot/subsystems/ExampleSubsystem.java delete mode 100644 src/main/java/swervelib/SwerveController.java delete mode 100644 src/main/java/swervelib/SwerveDrive.java delete mode 100644 src/main/java/swervelib/SwerveModule.java delete mode 100644 src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java delete mode 100644 src/main/java/swervelib/encoders/CANCoderSwerve.java delete mode 100644 src/main/java/swervelib/encoders/CanAndCoderSwerve.java delete mode 100644 src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java delete mode 100644 src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java delete mode 100644 src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java delete mode 100644 src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java delete mode 100644 src/main/java/swervelib/encoders/package-info.java delete mode 100644 src/main/java/swervelib/imu/ADIS16448Swerve.java delete mode 100644 src/main/java/swervelib/imu/ADIS16470Swerve.java delete mode 100644 src/main/java/swervelib/imu/ADXRS450Swerve.java delete mode 100644 src/main/java/swervelib/imu/AnalogGyroSwerve.java delete mode 100644 src/main/java/swervelib/imu/NavXSwerve.java delete mode 100644 src/main/java/swervelib/imu/Pigeon2Swerve.java delete mode 100644 src/main/java/swervelib/imu/PigeonSwerve.java delete mode 100644 src/main/java/swervelib/imu/SwerveIMU.java delete mode 100644 src/main/java/swervelib/imu/package-info.java delete mode 100644 src/main/java/swervelib/math/Matter.java delete mode 100644 src/main/java/swervelib/math/SwerveMath.java delete mode 100644 src/main/java/swervelib/math/package-info.java delete mode 100644 src/main/java/swervelib/motors/SparkFlexSwerve.java delete mode 100644 src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java delete mode 100644 src/main/java/swervelib/motors/SparkMaxSwerve.java delete mode 100644 src/main/java/swervelib/motors/SwerveMotor.java delete mode 100644 src/main/java/swervelib/motors/TalonFXSwerve.java delete mode 100644 src/main/java/swervelib/motors/TalonSRXSwerve.java delete mode 100644 src/main/java/swervelib/motors/package-info.java delete mode 100644 src/main/java/swervelib/package-info.java delete mode 100644 src/main/java/swervelib/parser/PIDFConfig.java delete mode 100644 src/main/java/swervelib/parser/SwerveControllerConfiguration.java delete mode 100644 src/main/java/swervelib/parser/SwerveDriveConfiguration.java delete mode 100644 src/main/java/swervelib/parser/SwerveModuleConfiguration.java delete mode 100644 src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java delete mode 100644 src/main/java/swervelib/parser/SwerveParser.java delete mode 100644 src/main/java/swervelib/parser/deserializer/PIDFRange.java delete mode 100644 src/main/java/swervelib/parser/deserializer/package-info.java delete mode 100644 src/main/java/swervelib/parser/json/ControllerPropertiesJson.java delete mode 100644 src/main/java/swervelib/parser/json/DeviceJson.java delete mode 100644 src/main/java/swervelib/parser/json/ModuleJson.java delete mode 100644 src/main/java/swervelib/parser/json/MotorConfigDouble.java delete mode 100644 src/main/java/swervelib/parser/json/MotorConfigInt.java delete mode 100644 src/main/java/swervelib/parser/json/PIDFPropertiesJson.java delete mode 100644 src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java delete mode 100644 src/main/java/swervelib/parser/json/SwerveDriveJson.java delete mode 100644 src/main/java/swervelib/parser/json/modules/BoolMotorJson.java delete mode 100644 src/main/java/swervelib/parser/json/modules/LocationJson.java delete mode 100644 src/main/java/swervelib/parser/json/modules/package-info.java delete mode 100644 src/main/java/swervelib/parser/json/package-info.java delete mode 100644 src/main/java/swervelib/parser/package-info.java delete mode 100644 src/main/java/swervelib/simulation/SwerveIMUSimulation.java delete mode 100644 src/main/java/swervelib/simulation/SwerveModuleSimulation.java delete mode 100644 src/main/java/swervelib/simulation/package-info.java delete mode 100644 src/main/java/swervelib/telemetry/Alert.java delete mode 100644 src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java delete mode 100644 src/main/java/swervelib/telemetry/package-info.java diff --git a/src/main/deploy/pathplanner/SamplePath.path b/src/main/deploy/pathplanner/SamplePath.path new file mode 100644 index 0000000..d9df9e5 --- /dev/null +++ b/src/main/deploy/pathplanner/SamplePath.path @@ -0,0 +1,76 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 0.8409049675242211, + "y": 3.593000471715229 + }, + "prevControl": null, + "nextControl": { + "x": 1.8409049675242182, + "y": 3.593000471715229 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.0, + "y": 5.0 + }, + "prevControl": { + "x": 3.0, + "y": 3.8506044103316963 + }, + "nextControl": { + "x": 3.0, + "y": 3.8506044103316963 + }, + "holonomicAngle": 90.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "marker1" + ], + "executionBehavior": "parallel", + "waitBehavior": "after", + "waitTime": 2.0 + } + }, + { + "anchorPoint": { + "x": 5.0, + "y": 3.0 + }, + "prevControl": { + "x": 4.0, + "y": 3.0 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/generatedJSON/New Path.wpilib.json b/src/main/deploy/pathplanner/generatedJSON/New Path.wpilib.json new file mode 100644 index 0000000..71f0fcd --- /dev/null +++ b/src/main/deploy/pathplanner/generatedJSON/New Path.wpilib.json @@ -0,0 +1,9002 @@ +[ + { + "time": 0.0, + "pose": { + "rotation": { + "radians": 0.003994666723425411 + }, + "translation": { + "x": 1.0, + "y": 3.0 + } + }, + "velocity": 0, + "acceleration": 3.0, + "curvature": 0.6639756640813854, + "holonomicRotation": 0.0, + "angularVelocity": 0.0, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.08944283740393223, + "pose": { + "rotation": { + "radians": 0.003994666723425411 + }, + "translation": { + "x": 1.011999936, + "y": 3.000047936 + } + }, + "velocity": 0.2683285122117967, + "acceleration": 3.0, + "curvature": 0.6639756640813854, + "holonomicRotation": 0.0, + "angularVelocity": 0.21506066046275568, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.12649227216974931, + "pose": { + "rotation": { + "radians": 0.011962542633933815 + }, + "translation": { + "x": 1.023999488, + "y": 3.000191488 + } + }, + "velocity": 0.37947681650924797, + "acceleration": 3.0000000000000004, + "curvature": 0.6612478412834536, + "holonomicRotation": 0.0, + "angularVelocity": 0.2791196654351322, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.15492278103446244, + "pose": { + "rotation": { + "radians": 0.019898056756403104 + }, + "translation": { + "x": 1.035998272, + "y": 3.000430272 + } + }, + "velocity": 0.4647683431033873, + "acceleration": 2.999999999999999, + "curvature": 0.6584819160891061, + "holonomicRotation": 0.0, + "angularVelocity": 0.3297027488703112, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.1788926532379475, + "pose": { + "rotation": { + "radians": 0.027800989511962193 + }, + "translation": { + "x": 1.047995904, + "y": 3.000763904 + } + }, + "velocity": 0.5366779597138426, + "acceleration": 3.000000000000002, + "curvature": 0.6556801390904735, + "holonomicRotation": 0.0, + "angularVelocity": 0.3726393102517324, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.20001267329763361, + "pose": { + "rotation": { + "radians": 0.035671139219506376 + }, + "translation": { + "x": 1.059992, + "y": 3.001192 + } + }, + "velocity": 0.600038019892901, + "acceleration": 3.0000000000000027, + "curvature": 0.6528447360474213, + "holonomicRotation": 0.0, + "angularVelocity": 0.41040227730614015, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.21910901626437457, + "pose": { + "rotation": { + "radians": 0.043508321861275956 + }, + "translation": { + "x": 1.071986176, + "y": 3.001714176 + } + }, + "velocity": 0.6573270487931238, + "acceleration": 2.999999999999997, + "curvature": 0.6499779059037429, + "holonomicRotation": 0.0, + "angularVelocity": 0.44433343266184905, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.23667251341123324, + "pose": { + "rotation": { + "radians": 0.05131237083808626 + }, + "translation": { + "x": 1.083978048, + "y": 3.0023300479999997 + } + }, + "velocity": 0.7100175402336998, + "acceleration": 3.0, + "curvature": 0.6470818190817624, + "holonomicRotation": 0.0, + "angularVelocity": 0.4752620325524382, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.2530230008132736, + "pose": { + "rotation": { + "radians": 0.059083136714003004 + }, + "translation": { + "x": 1.095967232, + "y": 3.0030392320000003 + } + }, + "velocity": 0.7590690024398208, + "acceleration": 2.9999999999999933, + "curvature": 0.6441586157879952, + "holonomicRotation": 0.0, + "angularVelocity": 0.503744881604611, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.2683826610691287, + "pose": { + "rotation": { + "radians": 0.06682048695107579 + }, + "translation": { + "x": 1.107953344, + "y": 3.003841344 + } + }, + "velocity": 0.8051479832073861, + "acceleration": 3.0, + "curvature": 0.6412104045031461, + "holonomicRotation": 0.0, + "angularVelocity": 0.5301782294749301, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.28291328163710894, + "pose": { + "rotation": { + "radians": 0.07452430563697954 + }, + "translation": { + "x": 1.119936, + "y": 3.004736 + } + }, + "velocity": 0.8487398449113267, + "acceleration": 2.999999999999996, + "curvature": 0.6382392605344197, + "holonomicRotation": 0.0, + "angularVelocity": 0.5548565565084475, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.29673701224983334, + "pose": { + "rotation": { + "radians": 0.08219449320285621 + }, + "translation": { + "x": 1.131914816, + "y": 3.005722816 + } + }, + "velocity": 0.8902110367494999, + "acceleration": 2.999999999999996, + "curvature": 0.6352472246423004, + "holonomicRotation": 0.0, + "angularVelocity": 0.578006214771147, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.3099487610809687, + "pose": { + "rotation": { + "radians": 0.08983096613524788 + }, + "translation": { + "x": 1.1438894080000002, + "y": 3.0068014080000003 + } + }, + "velocity": 0.9298462832429059, + "acceleration": 2.999999999999996, + "curvature": 0.6322363019942066, + "holonomicRotation": 0.0, + "angularVelocity": 0.5998059641247867, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.32262401107954614, + "pose": { + "rotation": { + "radians": 0.09743365668116732 + }, + "translation": { + "x": 1.1558593920000002, + "y": 3.007971392 + } + }, + "velocity": 0.9678720332386382, + "acceleration": 3.0000000000000044, + "curvature": 0.6292084609699508, + "holonomicRotation": 0.0, + "angularVelocity": 0.6204001606264152, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.33482396902093875, + "pose": { + "rotation": { + "radians": 0.10500251254764281 + }, + "translation": { + "x": 1.1678243840000002, + "y": 3.009232384 + } + }, + "velocity": 1.004471907062816, + "acceleration": 3.0, + "curvature": 0.6261656322069515, + "holonomicRotation": 0.0, + "angularVelocity": 0.6399075802749538, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.34659908199060496, + "pose": { + "rotation": { + "radians": 0.11253749659552614 + }, + "translation": { + "x": 1.1797840000000002, + "y": 3.010584 + } + }, + "velocity": 1.0397972459718146, + "acceleration": 2.999999999999986, + "curvature": 0.6231097077741713, + "holonomicRotation": 0.0, + "angularVelocity": 0.6584275258643302, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.35799151265261864, + "pose": { + "rotation": { + "radians": 0.12003858652989674 + }, + "translation": { + "x": 1.191737856, + "y": 3.012025856 + } + }, + "velocity": 1.0739745379578558, + "acceleration": 3.0000000000000195, + "curvature": 0.6200425403858878, + "holonomicRotation": 0.0, + "angularVelocity": 0.6760441751840484, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.3690369270353713, + "pose": { + "rotation": { + "radians": 0.12750577458585077 + }, + "translation": { + "x": 1.203685568, + "y": 3.013557568 + } + }, + "velocity": 1.1071107811061136, + "acceleration": 2.99999999999999, + "curvature": 0.6169659427566434, + "holonomicRotation": 0.0, + "angularVelocity": 0.6928297531996462, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.3797658147476807, + "pose": { + "rotation": { + "radians": 0.1349390672116768 + }, + "translation": { + "x": 1.2156267520000001, + "y": 3.0151787519999997 + } + }, + "velocity": 1.1392974442430417, + "acceleration": 2.99999999999999, + "curvature": 0.6138816870208452, + "holonomicRotation": 0.0, + "angularVelocity": 0.7088468950979862, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.39020448294066074, + "pose": { + "rotation": { + "radians": 0.14233848474922883 + }, + "translation": { + "x": 1.2275610240000001, + "y": 3.016889024 + } + }, + "velocity": 1.170613448821982, + "acceleration": 3.0000000000000053, + "curvature": 0.6107915042289077, + "holonomicRotation": 0.0, + "angularVelocity": 0.7241504390542802, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.40037581746696743, + "pose": { + "rotation": { + "radians": 0.1497040611122218 + }, + "translation": { + "x": 1.2394880000000001, + "y": 3.018688 + } + }, + "velocity": 1.2011274524009021, + "acceleration": 3.000000000000011, + "curvature": 0.6076970841012796, + "holonomicRotation": 0.0, + "angularVelocity": 0.7387888084367961, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.4102998746100373, + "pose": { + "rotation": { + "radians": 0.15703584346380906 + }, + "translation": { + "x": 1.2514072960000002, + "y": 3.020575296 + } + }, + "velocity": 1.2308996238301118, + "acceleration": 3.0, + "curvature": 0.6046000745612534, + "holonomicRotation": 0.0, + "angularVelocity": 0.7528050925445897, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.4199943473238247, + "pose": { + "rotation": { + "radians": 0.16433389189228276 + }, + "translation": { + "x": 1.263318528, + "y": 3.022550528 + } + }, + "velocity": 1.2599830419714737, + "acceleration": 2.9999999999999827, + "curvature": 0.6015020816536292, + "holonomicRotation": 0.0, + "angularVelocity": 0.7662379027190784, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.4294749370586641, + "pose": { + "rotation": { + "radians": 0.17159827908724612 + }, + "translation": { + "x": 1.2752213120000002, + "y": 3.024613312 + } + }, + "velocity": 1.288424811175992, + "acceleration": 3.0000000000000058, + "curvature": 0.598404669370064, + "holonomicRotation": 0.0, + "angularVelocity": 0.7791220580680622, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.4387556535437481, + "pose": { + "rotation": { + "radians": 0.178829090015451 + }, + "translation": { + "x": 1.2871152640000003, + "y": 3.026763264 + } + }, + "velocity": 1.316266960631244, + "acceleration": 2.9999999999999942, + "curvature": 0.595309359586865, + "holonomicRotation": 0.0, + "angularVelocity": 0.7914891405693073, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.4478490588896583, + "pose": { + "rotation": { + "radians": 0.1860264215975338 + }, + "translation": { + "x": 1.2990000000000002, + "y": 3.0290000000000004 + } + }, + "velocity": 1.3435471766689746, + "acceleration": 3.0, + "curvature": 0.5922176320623258, + "holonomicRotation": 0.0, + "angularVelocity": 0.8033679485304233, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.4567664681550803, + "pose": { + "rotation": { + "radians": 0.19319038238530206 + }, + "translation": { + "x": 1.3108751360000002, + "y": 3.031323136 + } + }, + "velocity": 1.3702994044652408, + "acceleration": 3.0000000000000187, + "curvature": 0.5891309246519478, + "holonomicRotation": 0.0, + "angularVelocity": 0.8147848704001008, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.4655181155102056, + "pose": { + "rotation": { + "radians": 0.2003210922413352 + }, + "translation": { + "x": 1.3227402880000003, + "y": 3.033732288 + } + }, + "velocity": 1.3965543465306167, + "acceleration": 3.000000000000006, + "curvature": 0.586050633250819, + "holonomicRotation": 0.0, + "angularVelocity": 0.8257641950388135, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.4741132929479354, + "pose": { + "rotation": { + "radians": 0.20741868201941793 + }, + "translation": { + "x": 1.3345950720000002, + "y": 3.036227072 + } + }, + "velocity": 1.4223398788438062, + "acceleration": 3.0000000000000067, + "curvature": 0.5829781121894732, + "holonomicRotation": 0.0, + "angularVelocity": 0.8363283714373475, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.48256046689262294, + "pose": { + "rotation": { + "radians": 0.21448329324782645 + }, + "translation": { + "x": 1.3464391040000003, + "y": 3.038807104 + } + }, + "velocity": 1.4476814006778689, + "acceleration": 3.0000000000000133, + "curvature": 0.5799146744073622, + "holonomicRotation": 0.0, + "angularVelocity": 0.8464982273774265, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.4908673768658824, + "pose": { + "rotation": { + "radians": 0.22151507781517443 + }, + "translation": { + "x": 1.3582720000000004, + "y": 3.041472 + } + }, + "velocity": 1.472602130597647, + "acceleration": 2.9999999999999867, + "curvature": 0.576861591813293, + "holonomicRotation": 0.0, + "angularVelocity": 0.8562931546703403, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.49904111947427243, + "pose": { + "rotation": { + "radians": 0.22851419765877612 + }, + "translation": { + "x": 1.3700933760000003, + "y": 3.044221376 + } + }, + "velocity": 1.4971233584228174, + "acceleration": 3.0000000000000204, + "curvature": 0.5738200956320813, + "holonomicRotation": 0.0, + "angularVelocity": 0.865731267137874, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.5070882203039146, + "pose": { + "rotation": { + "radians": 0.2354808244568085 + }, + "translation": { + "x": 1.3819028480000004, + "y": 3.047054848 + } + }, + "velocity": 1.521264660911744, + "acceleration": 2.9999999999999862, + "curvature": 0.5707913768705666, + "holonomicRotation": 0.0, + "angularVelocity": 0.8748295358184321, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.5150146957856219, + "pose": { + "rotation": { + "radians": 0.24241513932314662 + }, + "translation": { + "x": 1.3937000320000004, + "y": 3.0499720320000003 + } + }, + "velocity": 1.5450440873568656, + "acceleration": 3.0, + "curvature": 0.5677765867125653, + "holonomicRotation": 0.0, + "angularVelocity": 0.8836039055955357, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.5228261066903811, + "pose": { + "rotation": { + "radians": 0.24931733250680344 + }, + "translation": { + "x": 1.4054845440000003, + "y": 3.0529725439999997 + } + }, + "velocity": 1.5684783200711434, + "acceleration": 3.000000000000014, + "curvature": 0.5647768371119535, + "holonomicRotation": 0.0, + "angularVelocity": 0.8920693959970966, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.5305276045997832, + "pose": { + "rotation": { + "radians": 0.2561876030951167 + }, + "translation": { + "x": 1.4172560000000003, + "y": 3.0560560000000003 + } + }, + "velocity": 1.5915828137993497, + "acceleration": 3.0, + "curvature": 0.5617932012303706, + "holonomicRotation": 0.0, + "angularVelocity": 0.9002401887205707, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.5381239724478855, + "pose": { + "rotation": { + "radians": 0.2630261587202831 + }, + "translation": { + "x": 1.4290140160000004, + "y": 3.059222016 + } + }, + "velocity": 1.6143719173436566, + "acceleration": 3.0000000000000147, + "curvature": 0.5588267141256478, + "holonomicRotation": 0.0, + "angularVelocity": 0.9081297043799728, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.545619660034259, + "pose": { + "rotation": { + "radians": 0.26983321527222115 + }, + "translation": { + "x": 1.4407582080000003, + "y": 3.062470208 + } + }, + "velocity": 1.636858980102777, + "acceleration": 2.9999999999999853, + "curvature": 0.5558783732950194, + "holonomicRotation": 0.0, + "angularVelocity": 0.9157506692832025, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.5530188152508757, + "pose": { + "rotation": { + "radians": 0.2766089966139682 + }, + "translation": { + "x": 1.4524881920000003, + "y": 3.065800192 + } + }, + "velocity": 1.6590564457526273, + "acceleration": 3.000000000000015, + "curvature": 0.5529491392614916, + "holonomicRotation": 0.0, + "angularVelocity": 0.9231151746218296, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.5603253116392223, + "pose": { + "rotation": { + "radians": 0.2833537343033705 + }, + "translation": { + "x": 1.4642035840000003, + "y": 3.069211584 + } + }, + "velocity": 1.680975934917667, + "acceleration": 2.999999999999985, + "curvature": 0.550039936396451, + "holonomicRotation": 0.0, + "angularVelocity": 0.930234728482705, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.5675427727919045, + "pose": { + "rotation": { + "radians": 0.2900676673190703 + }, + "translation": { + "x": 1.4759040000000003, + "y": 3.072704 + } + }, + "velocity": 1.7026283183757136, + "acceleration": 3.0000000000000155, + "curvature": 0.5471516534114057, + "holonomicRotation": 0.0, + "angularVelocity": 0.9371203020376715, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.5746745940299195, + "pose": { + "rotation": { + "radians": 0.29675104179171763 + }, + "translation": { + "x": 1.4875890560000005, + "y": 3.0762770560000003 + } + }, + "velocity": 1.7240237820897588, + "acceleration": 3.0000000000000155, + "curvature": 0.5442851441519642, + "holonomicRotation": 0.0, + "angularVelocity": 0.9437823705764029, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.5817239617188056, + "pose": { + "rotation": { + "radians": 0.30340411074019924 + }, + "translation": { + "x": 1.4992583680000005, + "y": 3.0799303680000003 + } + }, + "velocity": 1.745171885156417, + "acceleration": 3.0, + "curvature": 0.5414412284047404, + "holonomicRotation": 0.0, + "angularVelocity": 0.9502309502250106, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.5886938705309835, + "pose": { + "rotation": { + "radians": 0.3100271338137768 + }, + "translation": { + "x": 1.5109115520000005, + "y": 3.083663552 + } + }, + "velocity": 1.766081611592951, + "acceleration": 3.0, + "curvature": 0.538620692477182, + "holonomicRotation": 0.0, + "angularVelocity": 0.9564756306438044, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.595587138915401, + "pose": { + "rotation": { + "radians": 0.3166203770389595 + }, + "translation": { + "x": 1.5225482240000003, + "y": 3.087476224 + } + }, + "velocity": 1.7867614167462034, + "acceleration": 2.999999999999984, + "curvature": 0.5358242899805497, + "holonomicRotation": 0.0, + "angularVelocity": 0.9625256044965456, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.6024064229972305, + "pose": { + "rotation": { + "radians": 0.32318411257205604 + }, + "translation": { + "x": 1.5341680000000004, + "y": 3.091368 + } + }, + "velocity": 1.8072192689916917, + "acceleration": 3.0000000000000164, + "curvature": 0.5330527427341295, + "holonomicRotation": 0.0, + "angularVelocity": 0.9683896940146024, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.609154229098364, + "pose": { + "rotation": { + "radians": 0.32971861845760264 + }, + "translation": { + "x": 1.5457704960000005, + "y": 3.095338496 + } + }, + "velocity": 1.8274626872950925, + "acceleration": 3.0000000000000164, + "curvature": 0.530306741366546, + "holonomicRotation": 0.0, + "angularVelocity": 0.9740763749015857, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.6158329250426408, + "pose": { + "rotation": { + "radians": 0.33622417839207364 + }, + "translation": { + "x": 1.5573553280000003, + "y": 3.099387328 + } + }, + "velocity": 1.8474987751279226, + "acceleration": 2.9999999999999836, + "curvature": 0.5275869461466284, + "holonomicRotation": 0.0, + "angularVelocity": 0.9795937980259228, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.6224447503871835, + "pose": { + "rotation": { + "radians": 0.3427010814932183 + }, + "translation": { + "x": 1.5689221120000005, + "y": 3.103514112 + } + }, + "velocity": 1.867334251161551, + "acceleration": 3.000000000000017, + "curvature": 0.5248939877909048, + "holonomicRotation": 0.0, + "angularVelocity": 0.9849498091847217, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.6289918257021776, + "pose": { + "rotation": { + "radians": 0.34914962207543976 + }, + "translation": { + "x": 1.5804704640000005, + "y": 3.1077184640000004 + } + }, + "velocity": 1.8869754771065332, + "acceleration": 3.0, + "curvature": 0.5222284682600525, + "holonomicRotation": 0.0, + "angularVelocity": 0.9901519669517042, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.6354761610052784, + "pose": { + "rotation": { + "radians": 0.35557009943017936 + }, + "translation": { + "x": 1.5920000000000005, + "y": 3.112 + } + }, + "velocity": 1.9064284830158356, + "acceleration": 3.0, + "curvature": 0.5195909614569448, + "holonomicRotation": 0.0, + "angularVelocity": 0.995207559235847, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.6418996634430969, + "pose": { + "rotation": { + "radians": 0.36196281761306626 + }, + "translation": { + "x": 1.6035103360000005, + "y": 3.1163583360000002 + } + }, + "velocity": 1.925698990329291, + "acceleration": 2.9999999999999654, + "curvature": 0.5169820141397625, + "holonomicRotation": 0.0, + "angularVelocity": 1.0001236180941289, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.6482641443004898, + "pose": { + "rotation": { + "radians": 0.3683280852354529 + }, + "translation": { + "x": 1.6150010880000005, + "y": 3.120793088 + } + }, + "velocity": 1.9447924329014699, + "acceleration": 3.0000000000000173, + "curvature": 0.5144021466205864, + "holonomicRotation": 0.0, + "angularVelocity": 1.0049069337160952, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.654571325408339, + "pose": { + "rotation": { + "radians": 0.37466621526293364 + }, + "translation": { + "x": 1.6264718720000004, + "y": 3.1253038720000004 + } + }, + "velocity": 1.963713976225017, + "acceleration": 2.9999999999999822, + "curvature": 0.5118518535480218, + "holonomicRotation": 0.0, + "angularVelocity": 1.0095640669411767, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.6608228450118765, + "pose": { + "rotation": { + "radians": 0.3809775248184435 + }, + "translation": { + "x": 1.6379223040000006, + "y": 3.1298903040000003 + } + }, + "velocity": 1.9824685350356297, + "acceleration": 3.0, + "curvature": 0.5093316047891678, + "holonomicRotation": 0.0, + "angularVelocity": 1.0141013611142926, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.6670202631541803, + "pose": { + "rotation": { + "radians": 0.3872623349919482 + }, + "translation": { + "x": 1.6493520000000006, + "y": 3.1345520000000002 + } + }, + "velocity": 2.001060789462541, + "acceleration": 2.9999999999999822, + "curvature": 0.5068418460483123, + "holonomicRotation": 0.0, + "angularVelocity": 1.0185249527914209, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.6731650666230385, + "pose": { + "rotation": { + "radians": 0.3935209706549796 + }, + "translation": { + "x": 1.6607605760000006, + "y": 3.139288576 + } + }, + "velocity": 2.019495199869116, + "acceleration": 3.000000000000036, + "curvature": 0.5043829997706955, + "holonomicRotation": 0.0, + "angularVelocity": 1.0228407818457754, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.6792586735038131, + "pose": { + "rotation": { + "radians": 0.3997537602811718 + }, + "translation": { + "x": 1.6721476480000006, + "y": 3.144099648 + } + }, + "velocity": 2.0377760205114397, + "acceleration": 3.000000000000018, + "curvature": 0.5019554658535227, + "holonomicRotation": 0.0, + "angularVelocity": 1.0270546007967487, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.6853024373760951, + "pose": { + "rotation": { + "radians": 0.4059610357723282 + }, + "translation": { + "x": 1.6835128320000006, + "y": 3.1489848320000005 + } + }, + "velocity": 2.055907312128286, + "acceleration": 3.000000000000018, + "curvature": 0.4995596223797258, + "holonomicRotation": 0.0, + "angularVelocity": 1.0311719834309776, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.6912976511877287, + "pose": { + "rotation": { + "radians": 0.41214313228956323 + }, + "translation": { + "x": 1.6948557440000005, + "y": 3.153943744 + } + }, + "velocity": 2.073892953563187, + "acceleration": 3.0000000000000555, + "curvature": 0.497195826433042, + "holonomicRotation": 0.0, + "angularVelocity": 1.0351983330346652, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.6972455508361, + "pose": { + "rotation": { + "radians": 0.41830038809061465 + }, + "translation": { + "x": 1.7061760000000006, + "y": 3.158976 + } + }, + "velocity": 2.0917366525083008, + "acceleration": 3.0, + "curvature": 0.49486441475959786, + "holonomicRotation": 0.0, + "angularVelocity": 1.0391388898355578, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.7031473184833589, + "pose": { + "rotation": { + "radians": 0.4244331443716547 + }, + "translation": { + "x": 1.7174732160000006, + "y": 3.164081216 + } + }, + "velocity": 2.1094419554500776, + "acceleration": 3.0000000000000187, + "curvature": 0.49256570452373255, + "holonomicRotation": 0.0, + "angularVelocity": 1.0429987382017707, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.7090040856294209, + "pose": { + "rotation": { + "radians": 0.4305417451149389 + }, + "translation": { + "x": 1.7287470080000005, + "y": 3.1692590080000005 + } + }, + "velocity": 2.1270122568882632, + "acceleration": 2.999999999999943, + "curvature": 0.4902999940555179, + "holonomicRotation": 0.0, + "angularVelocity": 1.0467828132720296, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.7148169359641001, + "pose": { + "rotation": { + "radians": 0.43662653694140374 + }, + "translation": { + "x": 1.7399969920000007, + "y": 3.1745089920000003 + } + }, + "velocity": 2.144450807892301, + "acceleration": 3.0, + "curvature": 0.48806756353004915, + "holonomicRotation": 0.0, + "angularVelocity": 1.0504959073278146, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.7205869080175377, + "pose": { + "rotation": { + "radians": 0.44268786896893575 + }, + "translation": { + "x": 1.7512227840000005, + "y": 3.1798307840000004 + } + }, + "velocity": 2.1617607240526135, + "acceleration": 2.999999999999981, + "curvature": 0.48586867565117137, + "holonomicRotation": 0.0, + "angularVelocity": 1.0541426755241503, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.7263149976261485, + "pose": { + "rotation": { + "radians": 0.44872609267459884 + }, + "translation": { + "x": 1.7624240000000007, + "y": 3.1852240000000003 + } + }, + "velocity": 2.178944992878446, + "acceleration": 3.0000000000000195, + "curvature": 0.4837035763245057, + "holonomicRotation": 0.0, + "angularVelocity": 1.057727641711773, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.7320021602295974, + "pose": { + "rotation": { + "radians": 0.45474156176317626 + }, + "translation": { + "x": 1.7736002560000006, + "y": 3.190688256 + } + }, + "velocity": 2.196006480688793, + "acceleration": 3.0, + "curvature": 0.48157249540881814, + "holonomicRotation": 0.0, + "angularVelocity": 1.0612552035501235, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.7376493130127955, + "pose": { + "rotation": { + "radians": 0.46073463203958775 + }, + "translation": { + "x": 1.7847511680000006, + "y": 3.1962231680000004 + } + }, + "velocity": 2.212947939038387, + "acceleration": 2.9999999999999605, + "curvature": 0.47947564724702235, + "holonomicRotation": 0.0, + "angularVelocity": 1.064729637583078, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.7432573369055535, + "pose": { + "rotation": { + "radians": 0.4667056612864813 + }, + "translation": { + "x": 1.7958763520000005, + "y": 3.2018283520000006 + } + }, + "velocity": 2.229772010716661, + "acceleration": 3.0, + "curvature": 0.4774132314234743, + "holonomicRotation": 0.0, + "angularVelocity": 1.0681551040334438, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.7488270784513326, + "pose": { + "rotation": { + "radians": 0.4726550091467523 + }, + "translation": { + "x": 1.8069754240000007, + "y": 3.2075034240000004 + } + }, + "velocity": 2.246481235353998, + "acceleration": 2.99999999999994, + "curvature": 0.47538543331431704, + "holonomicRotation": 0.0, + "angularVelocity": 1.0715356512801077, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.7543593515554528, + "pose": { + "rotation": { + "radians": 0.47858303701043514 + }, + "translation": { + "x": 1.8180480000000006, + "y": 3.2132480000000005 + } + }, + "velocity": 2.2630780546663583, + "acceleration": 2.99999999999998, + "curvature": 0.47339242474423837, + "holonomicRotation": 0.0, + "angularVelocity": 1.0748752201197043, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.7598549391221702, + "pose": { + "rotation": { + "radians": 0.48449010790589764 + }, + "translation": { + "x": 1.8290936960000006, + "y": 3.2190616960000002 + } + }, + "velocity": 2.2795648173665106, + "acceleration": 3.00000000000002, + "curvature": 0.4714343645997835, + "holonomicRotation": 0.0, + "angularVelocity": 1.078177648009616, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.7653145945891694, + "pose": { + "rotation": { + "radians": 0.4903765863962497 + }, + "translation": { + "x": 1.8401121280000006, + "y": 3.2249441280000006 + } + }, + "velocity": 2.2959437837675085, + "acceleration": 3.000000000000041, + "curvature": 0.46951139933779384, + "holonomicRotation": 0.0, + "angularVelocity": 1.081446672778329, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.7707390433672545, + "pose": { + "rotation": { + "radians": 0.49624283847896633 + }, + "translation": { + "x": 1.8511029120000007, + "y": 3.230894912 + } + }, + "velocity": 2.312217130101764, + "acceleration": 3.0000000000000204, + "curvature": 0.4676236637211531, + "holonomicRotation": 0.0, + "angularVelocity": 1.084685936629946, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.7761289841923315, + "pose": { + "rotation": { + "radians": 0.5020892314911949 + }, + "translation": { + "x": 1.8620656640000006, + "y": 3.2369136640000002 + } + }, + "velocity": 2.328386952576995, + "acceleration": 3.0000000000000617, + "curvature": 0.465771281161275, + "holonomicRotation": 0.0, + "angularVelocity": 1.087898989421331, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.7814850903961527, + "pose": { + "rotation": { + "radians": 0.5079161340175653 + }, + "translation": { + "x": 1.8730000000000007, + "y": 3.2430000000000003 + } + }, + "velocity": 2.3444552711884588, + "acceleration": 3.0, + "curvature": 0.4639543644847911, + "holonomicRotation": 0.0, + "angularVelocity": 1.091089292363366, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.78680801110174, + "pose": { + "rotation": { + "radians": 0.5137239158035309 + }, + "translation": { + "x": 1.8839055360000008, + "y": 3.249153536 + } + }, + "velocity": 2.360424033305221, + "acceleration": 3.0, + "curvature": 0.462173016289964, + "holonomicRotation": 0.0, + "angularVelocity": 1.0942602211599142, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.7920983723488966, + "pose": { + "rotation": { + "radians": 0.5195129476718603 + }, + "translation": { + "x": 1.8947818880000007, + "y": 3.2553738880000003 + } + }, + "velocity": 2.376295117046691, + "acceleration": 3.000000000000063, + "curvature": 0.4604273295606459, + "holonomicRotation": 0.0, + "angularVelocity": 1.0974150691626414, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.7973567781547642, + "pose": { + "rotation": { + "radians": 0.5252836014429918 + }, + "translation": { + "x": 1.9056286720000006, + "y": 3.2616606720000005 + } + }, + "velocity": 2.392070334464294, + "acceleration": 3.0000000000000213, + "curvature": 0.4587173881423953, + "holonomicRotation": 0.0, + "angularVelocity": 1.1005570504763318, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.8025838115139726, + "pose": { + "rotation": { + "radians": 0.5310362498595436 + }, + "translation": { + "x": 1.9164455040000006, + "y": 3.2680135040000002 + } + }, + "velocity": 2.407751434541919, + "acceleration": 3.0, + "curvature": 0.45704326727847255, + "holonomicRotation": 0.0, + "angularVelocity": 1.103689302847837, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.8077800353425552, + "pose": { + "rotation": { + "radians": 0.5367712665143531 + }, + "translation": { + "x": 1.9272320000000007, + "y": 3.2744320000000005 + } + }, + "velocity": 2.4233401060276667, + "acceleration": 3.0, + "curvature": 0.4554050339448593, + "holonomicRotation": 0.0, + "angularVelocity": 1.106814890388444, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.8129459933694659, + "pose": { + "rotation": { + "radians": 0.5424890257816597 + }, + "translation": { + "x": 1.9379877760000008, + "y": 3.2809157760000005 + } + }, + "velocity": 2.4388379801083993, + "acceleration": 3.000000000000043, + "curvature": 0.45380274753675837, + "holonomicRotation": 0.0, + "angularVelocity": 1.1099368064146276, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.8180822109792315, + "pose": { + "rotation": { + "radians": 0.5481899027524935 + }, + "translation": { + "x": 1.9487124480000007, + "y": 3.2874644480000006 + } + }, + "velocity": 2.454246632937696, + "acceleration": 3.0, + "curvature": 0.45223646009204105, + "holonomicRotation": 0.0, + "angularVelocity": 1.1130579759190018, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.8231891960089878, + "pose": { + "rotation": { + "radians": 0.5538742731727626 + }, + "translation": { + "x": 1.9594056320000008, + "y": 3.2940776320000005 + } + }, + "velocity": 2.469567588026965, + "acceleration": 3.0000000000000435, + "curvature": 0.45070621683540724, + "holonomicRotation": 0.0, + "angularVelocity": 1.1161812581904715, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.8282674395029013, + "pose": { + "rotation": { + "radians": 0.5595425133851966 + }, + "translation": { + "x": 1.9700669440000007, + "y": 3.3007549440000004 + } + }, + "velocity": 2.484802318508706, + "acceleration": 3.0000000000000657, + "curvature": 0.44921205667481234, + "holonomicRotation": 0.0, + "angularVelocity": 1.1193094491211109, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.833317416426745, + "pose": { + "rotation": { + "radians": 0.5651950002738984 + }, + "translation": { + "x": 1.980696000000001, + "y": 3.3074960000000004 + } + }, + "velocity": 2.499952249280237, + "acceleration": 3.0, + "curvature": 0.4477540124322439, + "holonomicRotation": 0.0, + "angularVelocity": 1.1224452836441048, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.8383395863451837, + "pose": { + "rotation": { + "radians": 0.5708321112125092 + }, + "translation": { + "x": 1.9912924160000007, + "y": 3.3143004160000005 + } + }, + "velocity": 2.515018759035553, + "acceleration": 3.0, + "curvature": 0.44633211133455136, + "holonomicRotation": 0.0, + "angularVelocity": 1.1255914379185776, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.8433343940641395, + "pose": { + "rotation": { + "radians": 0.5764542240150154 + }, + "translation": { + "x": 2.0018558080000006, + "y": 3.3211678080000007 + } + }, + "velocity": 2.5300031821924205, + "acceleration": 3.000000000000022, + "curvature": 0.4449463754713489, + "holonomicRotation": 0.0, + "angularVelocity": 1.1287505315367143, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.8483022702404226, + "pose": { + "rotation": { + "radians": 0.5820617168896036 + }, + "translation": { + "x": 2.012385792000001, + "y": 3.3280977920000003 + } + }, + "velocity": 2.54490681072127, + "acceleration": 2.999999999999978, + "curvature": 0.44359682198548334, + "holonomicRotation": 0.0, + "angularVelocity": 1.1319251296711867, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.8532436319606614, + "pose": { + "rotation": { + "radians": 0.5876549683955372 + }, + "translation": { + "x": 2.022881984000001, + "y": 3.3350899840000006 + } + }, + "velocity": 2.559730895881986, + "acceleration": 2.9999999999999325, + "curvature": 0.44228346357140413, + "holonomicRotation": 0.0, + "angularVelocity": 1.1351177449809835, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.8581588832914108, + "pose": { + "rotation": { + "radians": 0.5932343574021122 + }, + "translation": { + "x": 2.0333440000000005, + "y": 3.3421440000000007 + } + }, + "velocity": 2.574476649874234, + "acceleration": 3.0000000000000453, + "curvature": 0.44100630878383207, + "holonomicRotation": 0.0, + "angularVelocity": 1.138330839718644, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.8630484158021899, + "pose": { + "rotation": { + "radians": 0.598800263050939 + }, + "translation": { + "x": 2.043771456000001, + "y": 3.3492594560000004 + } + }, + "velocity": 2.589145247406572, + "acceleration": 3.0000000000000453, + "curvature": 0.43976536240568537, + "holonomicRotation": 0.0, + "angularVelocity": 1.1415668276354243, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.8679126090630681, + "pose": { + "rotation": { + "radians": 0.6043530647207653 + }, + "translation": { + "x": 2.0541639680000006, + "y": 3.356435968000001 + } + }, + "velocity": 2.603737827189206, + "acceleration": 2.9999999999999543, + "curvature": 0.43856062556178693, + "holonomicRotation": 0.0, + "angularVelocity": 1.1448280756028915, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.8727518311183128, + "pose": { + "rotation": { + "radians": 0.6098931419936862 + }, + "translation": { + "x": 2.0645211520000006, + "y": 3.3636731520000005 + } + }, + "velocity": 2.6182554933549405, + "acceleration": 3.0000000000000457, + "curvature": 0.43739209639321713, + "holonomicRotation": 0.0, + "angularVelocity": 1.1458505886525359, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.8775759614950274, + "pose": { + "rotation": { + "radians": 0.6154208746255811 + }, + "translation": { + "x": 2.074842624000001, + "y": 3.3709706240000004 + } + }, + "velocity": 2.622334232261645, + "acceleration": 0.8454868729070896, + "curvature": 0.4362597700073285, + "holonomicRotation": 0.0, + "angularVelocity": 1.1445003863931786, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.8823953289299136, + "pose": { + "rotation": { + "radians": 0.6209366425169791 + }, + "translation": { + "x": 2.085128000000001, + "y": 3.3783280000000007 + } + }, + "velocity": 2.625634845989314, + "acceleration": 0.6848645122545891, + "curvature": 0.43516363895714666, + "holonomicRotation": 0.0, + "angularVelocity": 1.1430465952169684, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.8872106909573768, + "pose": { + "rotation": { + "radians": 0.626440825687208 + }, + "translation": { + "x": 2.0953768960000008, + "y": 3.3857448960000003 + } + }, + "velocity": 2.6288383813504206, + "acceleration": 0.6652740422913254, + "curvature": 0.4341036934661007, + "holonomicRotation": 0.0, + "angularVelocity": 1.1416385324626965, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.8920221776120565, + "pose": { + "rotation": { + "radians": 0.6319338042506204 + }, + "translation": { + "x": 2.105588928000001, + "y": 3.3932209280000003 + } + }, + "velocity": 2.6319437458571184, + "acceleration": 0.6454064470235114, + "curvature": 0.4330799216330578, + "holonomicRotation": 0.0, + "angularVelocity": 1.1402763514325127, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.8968299192939927, + "pose": { + "rotation": { + "radians": 0.6374159583943286 + }, + "translation": { + "x": 2.1157637120000006, + "y": 3.4007557120000005 + } + }, + "velocity": 2.6349498803219897, + "acceleration": 0.6252695472733159, + "curvature": 0.4320923098070697, + "holonomicRotation": 0.0, + "angularVelocity": 1.1389601981623954, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.9016340467540943, + "pose": { + "rotation": { + "radians": 0.6428876683582834 + }, + "translation": { + "x": 2.125900864000001, + "y": 3.4083488640000006 + } + }, + "velocity": 2.6378557595292262, + "acceleration": 0.6048713801558966, + "curvature": 0.4311408427216499, + "holonomicRotation": 0.0, + "angularVelocity": 1.1376902117011674, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.906434691079727, + "pose": { + "rotation": { + "radians": 0.6483493144174144 + }, + "translation": { + "x": 2.136000000000001, + "y": 3.4160000000000004 + } + }, + "velocity": 2.640660392350118, + "acceleration": 0.584220081857935, + "curvature": 0.4302255037988389, + "holonomicRotation": 0.0, + "angularVelocity": 1.1364665242198737, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.9112319836804512, + "pose": { + "rotation": { + "radians": 0.6538012768650252 + }, + "translation": { + "x": 2.146060736000001, + "y": 3.4237087360000005 + } + }, + "velocity": 2.6433628223610883, + "acceleration": 0.5633239904028693, + "curvature": 0.4293462752752821, + "holonomicRotation": 0.0, + "angularVelocity": 1.135289261297001, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.9160260562739342, + "pose": { + "rotation": { + "radians": 0.6592439359982847 + }, + "translation": { + "x": 2.1560826880000006, + "y": 3.4314746880000007 + } + }, + "velocity": 2.6459621279004737, + "acceleration": 0.5421915268698363, + "curvature": 0.4285031384986032, + "holonomicRotation": 0.0, + "angularVelocity": 1.134158542054684, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.9208170408723271, + "pose": { + "rotation": { + "radians": 0.6646776721054044 + }, + "translation": { + "x": 2.1660654720000005, + "y": 3.4392974720000007 + } + }, + "velocity": 2.648457422337088, + "acceleration": 0.5208312373726626, + "curvature": 0.42769607414071226, + "holonomicRotation": 0.0, + "angularVelocity": 1.133074479392834, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.9256050697685123, + "pose": { + "rotation": { + "radians": 0.6701028654542673 + }, + "translation": { + "x": 2.176008704000001, + "y": 3.4471767040000008 + } + }, + "velocity": 2.6508478547544363, + "acceleration": 0.49925187779313374, + "curvature": 0.42692506226405785, + "holonomicRotation": 0.0, + "angularVelocity": 1.132037180125681, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.9303902755226879, + "pose": { + "rotation": { + "radians": 0.6755198962825455 + }, + "translation": { + "x": 2.185912000000001, + "y": 3.4551120000000006 + } + }, + "velocity": 2.653132609673996, + "acceleration": 0.47746221101694586, + "curvature": 0.4261900826856946, + "holonomicRotation": 0.0, + "angularVelocity": 1.1310467451481108, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.9351727909491765, + "pose": { + "rotation": { + "radians": 0.680929144789296 + }, + "translation": { + "x": 2.195774976000001, + "y": 3.463102976000001 + } + }, + "velocity": 2.655310907833249, + "acceleration": 0.4554712248680483, + "curvature": 0.42549111498766573, + "holonomicRotation": 0.0, + "angularVelocity": 1.1301032696261668, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.9399527491031683, + "pose": { + "rotation": { + "radians": 0.6863309911277984 + }, + "translation": { + "x": 2.205597248000001, + "y": 3.4711492480000006 + } + }, + "velocity": 2.657382006198557, + "acceleration": 0.4332879700166909, + "curvature": 0.42482813876175346, + "holonomicRotation": 0.0, + "angularVelocity": 1.1292068432179385, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.9447302832677758, + "pose": { + "rotation": { + "radians": 0.6917258154001806 + }, + "translation": { + "x": 2.215378432000001, + "y": 3.4792504320000006 + } + }, + "velocity": 2.6593451982965712, + "acceleration": 0.41092162407919103, + "curvature": 0.424201133738781, + "holonomicRotation": 0.0, + "angularVelocity": 1.1283575500272023, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.9495055269412395, + "pose": { + "rotation": { + "radians": 0.697113997652353 + }, + "translation": { + "x": 2.225118144000001, + "y": 3.487406144000001 + } + }, + "velocity": 2.6611998143698017, + "acceleration": 0.38838145235114474, + "curvature": 0.42361007996169614, + "holonomicRotation": 0.0, + "angularVelocity": 1.1275554689255944, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.954278613824305, + "pose": { + "rotation": { + "radians": 0.7024959178710106 + }, + "translation": { + "x": 2.234816000000001, + "y": 3.495616000000001 + } + }, + "velocity": 2.6629452216557663, + "acceleration": 0.3656768311000486, + "curvature": 0.42305495790658737, + "holonomicRotation": 0.0, + "angularVelocity": 1.1268006736112952, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.9590496778075177, + "pose": { + "rotation": { + "radians": 0.7078719559811373 + }, + "translation": { + "x": 2.244471616000001, + "y": 3.5038796160000008 + } + }, + "velocity": 2.6645808247919645, + "acceleration": 0.34281727135773626, + "curvature": 0.42253574855162407, + "holonomicRotation": 0.0, + "angularVelocity": 1.1260932327968134, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.9638188529587666, + "pose": { + "rotation": { + "radians": 0.7132424918449813 + }, + "translation": { + "x": 2.2540846080000008, + "y": 3.512196608000001 + } + }, + "velocity": 2.6661060656923015, + "acceleration": 0.3198123054754257, + "curvature": 0.4220524336016502, + "holonomicRotation": 0.0, + "angularVelocity": 1.1254332101237627, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.9685862735110955, + "pose": { + "rotation": { + "radians": 0.7186079052611989 + }, + "translation": { + "x": 2.263654592000001, + "y": 3.5205665920000007 + } + }, + "velocity": 2.6675204238998145, + "acceleration": 0.29667158413832084, + "curvature": 0.4216049955488846, + "holonomicRotation": 0.0, + "angularVelocity": 1.124820664571741, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.9733520738504342, + "pose": { + "rotation": { + "radians": 0.7239685759661101 + }, + "translation": { + "x": 2.2731811840000007, + "y": 3.5289891840000007 + } + }, + "velocity": 2.6688234168146447, + "acceleration": 0.27340484746599136, + "curvature": 0.4211934177613658, + "holonomicRotation": 0.0, + "angularVelocity": 1.1242556502653611, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.9781163885034462, + "pose": { + "rotation": { + "radians": 0.7293248836344008 + }, + "translation": { + "x": 2.282664000000001, + "y": 3.5374640000000004 + } + }, + "velocity": 2.6700145997957936, + "acceleration": 0.2500218956772268, + "curvature": 0.4208176845991983, + "holonomicRotation": 0.0, + "angularVelocity": 1.123738216844328, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.9828793521254612, + "pose": { + "rotation": { + "radians": 0.7346772078818984 + }, + "translation": { + "x": 2.292102656000001, + "y": 3.5459906560000007 + } + }, + "velocity": 2.671093566389595, + "acceleration": 0.22653261276539288, + "curvature": 0.4204777814786742, + "holonomicRotation": 0.0, + "angularVelocity": 1.1232684092819534, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.9876410994884608, + "pose": { + "rotation": { + "radians": 0.7400259282677375 + }, + "translation": { + "x": 2.3014967680000007, + "y": 3.5545687680000007 + } + }, + "velocity": 2.6720599484322793, + "acceleration": 0.20294693712507567, + "curvature": 0.42017369496416096, + "holonomicRotation": 0.0, + "angularVelocity": 1.1228462681894709, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.9924017654693136, + "pose": { + "rotation": { + "radians": 0.7453714242984346 + }, + "translation": { + "x": 2.310845952000001, + "y": 3.563197952000001 + } + }, + "velocity": 2.672913416024821, + "acceleration": 0.17927483170934566, + "curvature": 0.4199054128880666, + "holonomicRotation": 0.0, + "angularVelocity": 1.1224718297170306, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 0.997161485037997, + "pose": { + "rotation": { + "radians": 0.7507140754316346 + }, + "translation": { + "x": 2.320149824000001, + "y": 3.571877824000001 + } + }, + "velocity": 2.6736536778913673, + "acceleration": 0.15552636155641372, + "curvature": 0.4196729243382383, + "holonomicRotation": 0.0, + "angularVelocity": 1.1221451257033959, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.0019203932456864, + "pose": { + "rotation": { + "radians": 0.7560542610805632 + }, + "translation": { + "x": 2.329408000000001, + "y": 3.5806080000000007 + } + }, + "velocity": 2.6742804814591006, + "acceleration": 0.1317116322438141, + "curvature": 0.41947621972129306, + "holonomicRotation": 0.0, + "angularVelocity": 1.1218661837350683, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.0066786252132192, + "pose": { + "rotation": { + "radians": 0.7613923606193054 + }, + "translation": { + "x": 2.338620096000001, + "y": 3.5893880960000004 + } + }, + "velocity": 2.674793612605643, + "acceleration": 0.10784071689727757, + "curvature": 0.419315290918276, + "holonomicRotation": 0.0, + "angularVelocity": 1.1216350272072997, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.0114363161192206, + "pose": { + "rotation": { + "radians": 0.7667287533881022 + }, + "translation": { + "x": 2.3477857280000007, + "y": 3.598217728000001 + } + }, + "velocity": 2.6751928965083205, + "acceleration": 0.08392388462520017, + "curvature": 0.4191901310828717, + "holonomicRotation": 0.0, + "angularVelocity": 1.1214516752369148, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.0161936011885375, + "pose": { + "rotation": { + "radians": 0.7720638186986672 + }, + "translation": { + "x": 2.3569045120000007, + "y": 3.607096512000001 + } + }, + "velocity": 2.6754781966265213, + "acceleration": 0.05997120501374331, + "curvature": 0.4191007350133541, + "holonomicRotation": 0.0, + "angularVelocity": 1.1213161429216323, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.020950615680859, + "pose": { + "rotation": { + "radians": 0.7773979358410195 + }, + "translation": { + "x": 2.3659760640000007, + "y": 3.616024064000001 + } + }, + "velocity": 2.6756494156563497, + "acceleration": 0.03599295947170036, + "curvature": 0.4190470988943057, + "holonomicRotation": 0.0, + "angularVelocity": 1.1212284410937938, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.0257074948789369, + "pose": { + "rotation": { + "radians": 0.7827314840887518 + }, + "translation": { + "x": 2.375000000000001, + "y": 3.625000000000001 + } + }, + "velocity": 2.675706495129876, + "acceleration": 0.011999353178753079, + "curvature": 0.41902922045158014, + "holonomicRotation": 0.0, + "angularVelocity": 1.1211885766455594, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.030464374077015, + "pose": { + "rotation": { + "radians": 0.7880648427061199 + }, + "translation": { + "x": 2.383975936000001, + "y": 3.634023936000001 + } + }, + "velocity": 2.67564941565635, + "acceleration": -0.011999353178659161, + "curvature": 0.4190470988943055, + "holonomicRotation": 0.0, + "angularVelocity": 1.1211965522473806, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.0352213885693364, + "pose": { + "rotation": { + "radians": 0.7933983909539015 + }, + "translation": { + "x": 2.392903488000001, + "y": 3.643095488000001 + } + }, + "velocity": 2.675478196626521, + "acceleration": -0.03599295947188707, + "curvature": 0.41910073501335415, + "holonomicRotation": 0.0, + "angularVelocity": 1.1212523665452967, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.0399786736386536, + "pose": { + "rotation": { + "radians": 0.798732508096204 + }, + "translation": { + "x": 2.401782272000001, + "y": 3.652214272000001 + } + }, + "velocity": 2.675192896508296, + "acceleration": -0.05997120501878138, + "curvature": 0.4191901310828794, + "holonomicRotation": 0.0, + "angularVelocity": 1.1213560141038383, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.0447363645446093, + "pose": { + "rotation": { + "radians": 0.8040675734068445 + }, + "translation": { + "x": 2.410611904000001, + "y": 3.661379904000001 + } + }, + "velocity": 2.674793612656831, + "acceleration": -0.08392387386188453, + "curvature": 0.4193152909022269, + "holonomicRotation": 0.0, + "angularVelocity": 1.1215074853712201, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.0494945965121423, + "pose": { + "rotation": { + "radians": 0.8094039661755654 + }, + "translation": { + "x": 2.419392000000001, + "y": 3.670592000000001 + } + }, + "velocity": 2.6742804814079513, + "acceleration": -0.10784073840468338, + "curvature": 0.41947621973733923, + "holonomicRotation": 0.0, + "angularVelocity": 1.1217067667100076, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.054253504719877, + "pose": { + "rotation": { + "radians": 0.8147420657143334 + }, + "translation": { + "x": 2.428122176000001, + "y": 3.679850176000001 + } + }, + "velocity": 2.673653677891392, + "acceleration": -0.13171162148921356, + "curvature": 0.4196729243382304, + "holonomicRotation": 0.0, + "angularVelocity": 1.1219538403138642, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.0590132242886061, + "pose": { + "rotation": { + "radians": 0.8200822513632859 + }, + "translation": { + "x": 2.436802048000001, + "y": 3.689154048000001 + } + }, + "velocity": 2.6729134159737282, + "acceleration": -0.15552637229458416, + "curvature": 0.4199054129041197, + "holonomicRotation": 0.0, + "angularVelocity": 1.1222486842418435, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.0637738902695042, + "pose": { + "rotation": { + "radians": 0.8254249024964637 + }, + "translation": { + "x": 2.4454312320000007, + "y": 3.698503232000001 + } + }, + "velocity": 2.672059948432254, + "acceleration": -0.17927482098065445, + "curvature": 0.42017369496416906, + "holonomicRotation": 0.0, + "angularVelocity": 1.1225912723143032, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.068535637632504, + "pose": { + "rotation": { + "radians": 0.8307703985271329 + }, + "translation": { + "x": 2.454009344000001, + "y": 3.707897344000001 + } + }, + "velocity": 2.6710935663895956, + "acceleration": -0.20294693711966175, + "curvature": 0.42047778147867415, + "holonomicRotation": 0.0, + "angularVelocity": 1.1229815741545088, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.073298601254519, + "pose": { + "rotation": { + "radians": 0.8361191189130239 + }, + "translation": { + "x": 2.462536000000001, + "y": 3.717336000000001 + } + }, + "velocity": 2.6700145997957936, + "acceleration": -0.2265326127654861, + "curvature": 0.4208176845991983, + "holonomicRotation": 0.0, + "angularVelocity": 1.1234195550165338, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.078062915907531, + "pose": { + "rotation": { + "radians": 0.8414714431604695 + }, + "translation": { + "x": 2.471010816000001, + "y": 3.726818816000001 + } + }, + "velocity": 2.6688234168146683, + "acceleration": -0.2500218956722808, + "curvature": 0.4211934177613584, + "holonomicRotation": 0.0, + "angularVelocity": 1.1239051758218945, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.0828287162468697, + "pose": { + "rotation": { + "radians": 0.846827750828786 + }, + "translation": { + "x": 2.479433408000001, + "y": 3.736345408000001 + } + }, + "velocity": 2.6675204238998145, + "acceleration": -0.27340484747093, + "curvature": 0.42160499554888453, + "holonomicRotation": 0.0, + "angularVelocity": 1.124438393072715, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.0875961367991533, + "pose": { + "rotation": { + "radians": 0.8521884215336977 + }, + "translation": { + "x": 2.487803392000001, + "y": 3.745915392000001 + } + }, + "velocity": 2.6661060657428792, + "acceleration": -0.2966715735320978, + "curvature": 0.42205243358563704, + "holonomicRotation": 0.0, + "angularVelocity": 1.125019158687412, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.0923653119503571, + "pose": { + "rotation": { + "radians": 0.8575538349499379 + }, + "translation": { + "x": 2.496120384000001, + "y": 3.755528384000001 + } + }, + "velocity": 2.6645808247919636, + "acceleration": -0.31981231608377336, + "curvature": 0.4225357485516242, + "holonomicRotation": 0.0, + "angularVelocity": 1.1256474200922537, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.0971363759335697, + "pose": { + "rotation": { + "radians": 0.8629243708137362 + }, + "translation": { + "x": 2.504384000000001, + "y": 3.765184000000001 + } + }, + "velocity": 2.6629452216557663, + "acceleration": -0.3428172713575581, + "curvature": 0.4230549579065874, + "holonomicRotation": 0.0, + "angularVelocity": 1.1263231199908168, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.1019094628166803, + "pose": { + "rotation": { + "radians": 0.8683004089239086 + }, + "translation": { + "x": 2.512593856000001, + "y": 3.7748818560000013 + } + }, + "velocity": 2.6611998143196263, + "acceleration": -0.36567684160874825, + "curvature": 0.42361007997767003, + "holonomicRotation": 0.0, + "angularVelocity": 1.1270461963058591, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.106684706490189, + "pose": { + "rotation": { + "radians": 0.8736823291425702 + }, + "translation": { + "x": 2.5207495680000007, + "y": 3.7846215680000013 + } + }, + "velocity": 2.6593451982965486, + "acceleration": -0.38838144184482565, + "curvature": 0.42420113373878826, + "holonomicRotation": 0.0, + "angularVelocity": 1.1278165820374502, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.1114622406548413, + "pose": { + "rotation": { + "radians": 0.8790705113947155 + }, + "translation": { + "x": 2.5288507520000008, + "y": 3.794402752000001 + } + }, + "velocity": 2.6573820061486866, + "acceleration": -0.41092163450910085, + "curvature": 0.42482813877769865, + "holonomicRotation": 0.0, + "angularVelocity": 1.1286342052595717, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.116242198808878, + "pose": { + "rotation": { + "radians": 0.8844653356670706 + }, + "translation": { + "x": 2.536897024000001, + "y": 3.8042250240000013 + } + }, + "velocity": 2.6553109078332495, + "acceleration": -0.4332879595793174, + "curvature": 0.4254911149876656, + "holonomicRotation": 0.0, + "angularVelocity": 1.1294989888873823, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.1210247142353664, + "pose": { + "rotation": { + "radians": 0.8898671820056276 + }, + "translation": { + "x": 2.5448880000000007, + "y": 3.8140880000000013 + } + }, + "velocity": 2.6531326096739956, + "acceleration": -0.4554712248682446, + "curvature": 0.42619008268569475, + "holonomicRotation": 0.0, + "angularVelocity": 1.1304108505628179, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.1258099199895422, + "pose": { + "rotation": { + "radians": 0.8952764305123235 + }, + "translation": { + "x": 2.552823296000001, + "y": 3.8239912960000013 + } + }, + "velocity": 2.6508478547544305, + "acceleration": -0.4774622110180485, + "curvature": 0.4269250622640598, + "holonomicRotation": 0.0, + "angularVelocity": 1.1313697025929383, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.1305979488857274, + "pose": { + "rotation": { + "radians": 0.900693461340607 + }, + "translation": { + "x": 2.560702528000001, + "y": 3.8339345280000012 + } + }, + "velocity": 2.648457422337061, + "acceleration": -0.49925187779758573, + "curvature": 0.427696074140721, + "holonomicRotation": 0.0, + "angularVelocity": 1.1323754517400342, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.1353889334841203, + "pose": { + "rotation": { + "radians": 0.9061186546894917 + }, + "translation": { + "x": 2.568525312000001, + "y": 3.843917312000001 + } + }, + "velocity": 2.645962127900453, + "acceleration": -0.5208312373713648, + "curvature": 0.42850313849861, + "holonomicRotation": 0.0, + "angularVelocity": 1.133427999095026, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.1401830060776033, + "pose": { + "rotation": { + "radians": 0.9115523907966394 + }, + "translation": { + "x": 2.576291264000001, + "y": 3.853939264000001 + } + }, + "velocity": 2.643362822361095, + "acceleration": -0.542191526864093, + "curvature": 0.42934627527527985, + "holonomicRotation": 0.0, + "angularVelocity": 1.1345272398916992, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.1449802986783277, + "pose": { + "rotation": { + "radians": 0.9169950499298922 + }, + "translation": { + "x": 2.584000000000001, + "y": 3.864000000000001 + } + }, + "velocity": 2.6406603923501453, + "acceleration": -0.5633239903985849, + "curvature": 0.43022550379883, + "holonomicRotation": 0.0, + "angularVelocity": 1.1356730633905898, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.1497809430040045, + "pose": { + "rotation": { + "radians": 0.9224470123774822 + }, + "translation": { + "x": 2.5916511360000007, + "y": 3.874099136000001 + } + }, + "velocity": 2.6378557594807597, + "acceleration": -0.5842200919540596, + "curvature": 0.43114084273749304, + "holonomicRotation": 0.0, + "angularVelocity": 1.1368653526407893, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.15458507046415, + "pose": { + "rotation": { + "radians": 0.9279086584365919 + }, + "translation": { + "x": 2.599244288000001, + "y": 3.884236288000001 + } + }, + "velocity": 2.6349498803219897, + "acceleration": -0.6048713700618353, + "curvature": 0.43209230980706964, + "holonomicRotation": 0.0, + "angularVelocity": 1.1381039843726668, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.1593928121460426, + "pose": { + "rotation": { + "radians": 0.9333803684005884 + }, + "translation": { + "x": 2.606779072000001, + "y": 3.8944110720000014 + } + }, + "velocity": 2.6319437459051516, + "acceleration": -0.6252695372882013, + "curvature": 0.4330799216172504, + "holonomicRotation": 0.0, + "angularVelocity": 1.139388828711607, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.164204298800591, + "pose": { + "rotation": { + "radians": 0.9388625225442757 + }, + "translation": { + "x": 2.614255104000001, + "y": 3.9046231040000015 + } + }, + "velocity": 2.628838381445997, + "acceleration": -0.6454064371599206, + "curvature": 0.4341036934345355, + "holonomicRotation": 0.0, + "angularVelocity": 1.1407197490356886, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.1690196608279666, + "pose": { + "rotation": { + "radians": 0.9443555011076596 + }, + "translation": { + "x": 2.621672000000001, + "y": 3.9148720000000012 + } + }, + "velocity": 2.625634845989314, + "acceleration": -0.6652740621516621, + "curvature": 0.43516363895714666, + "holonomicRotation": 0.0, + "angularVelocity": 1.141368679059864, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.173842101877269, + "pose": { + "rotation": { + "radians": 0.9498596842779459 + }, + "translation": { + "x": 2.629029376000001, + "y": 3.9251573760000014 + } + }, + "velocity": 2.618989404766512, + "acceleration": -1.3780243563088321, + "curvature": 0.43625977000734745, + "holonomicRotation": 0.0, + "angularVelocity": 1.1396328812838714, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.1786820533931346, + "pose": { + "rotation": { + "radians": 0.9553754521692461 + }, + "translation": { + "x": 2.6363268480000013, + "y": 3.9354788480000007 + } + }, + "velocity": 2.604469550218915, + "acceleration": -3.0, + "curvature": 0.437392096408939, + "holonomicRotation": 0.0, + "angularVelocity": 1.1362290451451276, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.1835470343189463, + "pose": { + "rotation": { + "radians": 0.9609031848012304 + }, + "translation": { + "x": 2.643564032000001, + "y": 3.945836032000001 + } + }, + "velocity": 2.5898746074414802, + "acceleration": -2.9999999999999085, + "curvature": 0.4385606255617841, + "holonomicRotation": 0.0, + "angularVelocity": 1.1328524486335545, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.1884374128317103, + "pose": { + "rotation": { + "radians": 0.9664432620741601 + }, + "translation": { + "x": 2.650740544000001, + "y": 3.956228544000001 + } + }, + "velocity": 2.5752034719031878, + "acceleration": -3.0000000000000453, + "curvature": 0.4397653623900097, + "holonomicRotation": 0.0, + "angularVelocity": 1.1295007542703375, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.1933535686189238, + "pose": { + "rotation": { + "radians": 0.9719960637439282 + }, + "translation": { + "x": 2.657856000000001, + "y": 3.9666560000000013 + } + }, + "velocity": 2.5604550045415473, + "acceleration": -3.0, + "curvature": 0.4410063087994853, + "holonomicRotation": 0.0, + "angularVelocity": 1.1261715790988696, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.1982958933518377, + "pose": { + "rotation": { + "radians": 0.9775619693928133 + }, + "translation": { + "x": 2.6649100160000008, + "y": 3.977118016000001 + } + }, + "velocity": 2.5456280303428054, + "acceleration": -3.000000000000045, + "curvature": 0.4422834635714009, + "holonomicRotation": 0.0, + "angularVelocity": 1.1228624931602014, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.2032647911826513, + "pose": { + "rotation": { + "radians": 0.9831413583993789 + }, + "translation": { + "x": 2.671902208000001, + "y": 3.9876142080000014 + } + }, + "velocity": 2.5307213368503647, + "acceleration": -3.0, + "curvature": 0.4435968219542736, + "holonomicRotation": 0.0, + "angularVelocity": 1.1195710174570126, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.208260679267134, + "pose": { + "rotation": { + "radians": 0.9887346099052245 + }, + "translation": { + "x": 2.678832192000001, + "y": 3.9981441920000007 + } + }, + "velocity": 2.5157336725969164, + "acceleration": -3.0000000000000444, + "curvature": 0.4449463754713487, + "holonomicRotation": 0.0, + "angularVelocity": 1.116294622142275, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.213283988314313, + "pose": { + "rotation": { + "radians": 0.994342102779949 + }, + "translation": { + "x": 2.685699584000001, + "y": 4.0087075840000015 + } + }, + "velocity": 2.500663745455379, + "acceleration": -3.0000000000000884, + "curvature": 0.44633211131899053, + "holonomicRotation": 0.0, + "angularVelocity": 1.1130307242686392, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.2183351631649564, + "pose": { + "rotation": { + "radians": 0.9999642155823683 + }, + "translation": { + "x": 2.692504000000001, + "y": 4.019304000000001 + } + }, + "velocity": 2.4855102209034485, + "acceleration": -3.000000000000044, + "curvature": 0.4477540124322387, + "holonomicRotation": 0.0, + "angularVelocity": 1.1097766860870264, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.2234146634007506, + "pose": { + "rotation": { + "radians": 1.0056013265210262 + }, + "translation": { + "x": 2.699245056000001, + "y": 4.029933056000002 + } + }, + "velocity": 2.470271720196066, + "acceleration": -3.0000000000000435, + "curvature": 0.44921205667481756, + "holonomicRotation": 0.0, + "angularVelocity": 1.1065298124299, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.2285229639861848, + "pose": { + "rotation": { + "radians": 1.0112538134096622 + }, + "translation": { + "x": 2.705922368000001, + "y": 4.040594368000001 + } + }, + "velocity": 2.4549468184397636, + "acceleration": -2.999999999999913, + "curvature": 0.4507062168508919, + "holonomicRotation": 0.0, + "angularVelocity": 1.1032873489277657, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.2336605559453493, + "pose": { + "rotation": { + "radians": 1.0169220536221615 + }, + "translation": { + "x": 2.7125355520000007, + "y": 4.051287552000002 + } + }, + "velocity": 2.43953404256227, + "acceleration": -3.000000000000043, + "curvature": 0.45223646007658325, + "holonomicRotation": 0.0, + "angularVelocity": 1.1000464792593154, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.2388279470759953, + "pose": { + "rotation": { + "radians": 1.0226064240423844 + }, + "translation": { + "x": 2.719084224000001, + "y": 4.062012224000001 + } + }, + "velocity": 2.4240318691703315, + "acceleration": -3.000000000000043, + "curvature": 0.4538027475367534, + "holonomicRotation": 0.0, + "angularVelocity": 1.0968043231892513, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.2440256627034307, + "pose": { + "rotation": { + "radians": 1.028307301013264 + }, + "translation": { + "x": 2.725568000000001, + "y": 4.072768000000002 + } + }, + "velocity": 2.4084387222880252, + "acceleration": -3.0, + "curvature": 0.4554050339448751, + "holonomicRotation": 0.0, + "angularVelocity": 1.093557933627122, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.2492542464770038, + "pose": { + "rotation": { + "radians": 1.0340250602804888 + }, + "translation": { + "x": 2.731986496000001, + "y": 4.0835544960000005 + } + }, + "velocity": 2.392752970967306, + "acceleration": -3.0000000000000426, + "curvature": 0.45704326729385686, + "holonomicRotation": 0.0, + "angularVelocity": 1.0903042944971009, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.2545142612121893, + "pose": { + "rotation": { + "radians": 1.0397600769353796 + }, + "translation": { + "x": 2.738339328000001, + "y": 4.094371328000001 + } + }, + "velocity": 2.3769729267617494, + "acceleration": -2.999999999999958, + "curvature": 0.45871738815772506, + "holonomicRotation": 0.0, + "angularVelocity": 1.087040317564224, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.2598062897815119, + "pose": { + "rotation": { + "radians": 1.045512725351935 + }, + "translation": { + "x": 2.7446261120000006, + "y": 4.105218112000001 + } + }, + "velocity": 2.3610968410537816, + "acceleration": -3.0000000000000417, + "curvature": 0.4604273295453317, + "holonomicRotation": 0.0, + "angularVelocity": 1.0837628401270711, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.2651309360578522, + "pose": { + "rotation": { + "radians": 1.0512833791230536 + }, + "translation": { + "x": 2.7508464640000008, + "y": 4.1160944640000015 + } + }, + "velocity": 2.345122902224761, + "acceleration": -2.9999999999999165, + "curvature": 0.4621730162746951, + "holonomicRotation": 0.0, + "angularVelocity": 1.0804686217426966, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.270488825913959, + "pose": { + "rotation": { + "radians": 1.0570724109913305 + }, + "translation": { + "x": 2.7570000000000006, + "y": 4.127000000000001 + } + }, + "velocity": 2.3290492326564407, + "acceleration": -2.9999999999999587, + "curvature": 0.4639543644847909, + "holonomicRotation": 0.0, + "angularVelocity": 1.0771543414066895, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.2758806082823524, + "pose": { + "rotation": { + "radians": 1.0628801927773655 + }, + "translation": { + "x": 2.7630863360000006, + "y": 4.137934336000002 + } + }, + "velocity": 2.312873885551261, + "acceleration": -2.9999999999999587, + "curvature": 0.4657712811612695, + "holonomicRotation": 0.0, + "angularVelocity": 1.0738165942689333, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.2813069562801476, + "pose": { + "rotation": { + "radians": 1.068707095303676 + }, + "translation": { + "x": 2.7691050880000008, + "y": 4.148897088000001 + } + }, + "velocity": 2.2965948415578756, + "acceleration": -2.999999999999959, + "curvature": 0.46762366370596037, + "holonomicRotation": 0.0, + "angularVelocity": 1.0704518885397352, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.286768568403775, + "pose": { + "rotation": { + "radians": 1.0745534883158845 + }, + "translation": { + "x": 2.775055872000001, + "y": 4.159887872000001 + } + }, + "velocity": 2.280210005186993, + "acceleration": -3.000000000000041, + "curvature": 0.46951139935295344, + "holonomicRotation": 0.0, + "angularVelocity": 1.0670566418127445, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.2922661697989868, + "pose": { + "rotation": { + "radians": 1.0804197403986842 + }, + "translation": { + "x": 2.7809383040000006, + "y": 4.170906304000001 + } + }, + "velocity": 2.2637172010013575, + "acceleration": -3.0000000000000404, + "curvature": 0.4714343645997839, + "holonomicRotation": 0.0, + "angularVelocity": 1.0636271776953912, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.2978005136120798, + "pose": { + "rotation": { + "radians": 1.0863062188890003 + }, + "translation": { + "x": 2.786752000000001, + "y": 4.181952000000002 + } + }, + "velocity": 2.2471141695620784, + "acceleration": -3.0, + "curvature": 0.4733924247442271, + "holonomicRotation": 0.0, + "angularVelocity": 1.0601597219446663, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.303372382428791, + "pose": { + "rotation": { + "radians": 1.092213289784437 + }, + "translation": { + "x": 2.792496576000001, + "y": 4.193024576000001 + } + }, + "velocity": 2.2303985631119447, + "acceleration": -3.0, + "curvature": 0.47538543332938343, + "holonomicRotation": 0.0, + "angularVelocity": 1.05665039864859, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.3089825898079743, + "pose": { + "rotation": { + "radians": 1.0981413176481523 + }, + "translation": { + "x": 2.7981716480000007, + "y": 4.204123648000001 + } + }, + "velocity": 2.213567940974395, + "acceleration": -2.9999999999999605, + "curvature": 0.4774132314234743, + "holonomicRotation": 0.0, + "angularVelocity": 1.0530952259318735, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.314631981917819, + "pose": { + "rotation": { + "radians": 1.1040906655084468 + }, + "translation": { + "x": 2.8037768320000005, + "y": 4.215248832000001 + } + }, + "velocity": 2.1966197646448617, + "acceleration": -2.9999999999999214, + "curvature": 0.4794756472470108, + "holonomicRotation": 0.0, + "angularVelocity": 1.0494901118766216, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.3203214392831757, + "pose": { + "rotation": { + "radians": 1.1100616947553323 + }, + "translation": { + "x": 2.809311744000001, + "y": 4.226399744000002 + } + }, + "velocity": 2.1795513925487917, + "acceleration": -2.999999999999961, + "curvature": 0.48157249539387187, + "holonomicRotation": 0.0, + "angularVelocity": 1.0458308498080762, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.3260518786533824, + "pose": { + "rotation": { + "radians": 1.1160547650316492 + }, + "translation": { + "x": 2.814776000000001, + "y": 4.237576000000001 + } + }, + "velocity": 2.1623600744381717, + "acceleration": -3.0, + "curvature": 0.4837035763394283, + "holonomicRotation": 0.0, + "angularVelocity": 1.0421131136383825, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.3318242550009647, + "pose": { + "rotation": { + "radians": 1.1220702341203208 + }, + "translation": { + "x": 2.820169216000001, + "y": 4.2487772160000015 + } + }, + "velocity": 2.1450429453954247, + "acceleration": -3.0, + "curvature": 0.4858686756511655, + "holonomicRotation": 0.0, + "angularVelocity": 1.0383324526570552, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.3376395636626297, + "pose": { + "rotation": { + "radians": 1.1281084578259453 + }, + "translation": { + "x": 2.825491008000001, + "y": 4.260003008000001 + } + }, + "velocity": 2.1275970194104294, + "acceleration": -3.000000000000038, + "curvature": 0.48806756354488817, + "holonomicRotation": 0.0, + "angularVelocity": 1.0344842865366874, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.343498842635211, + "pose": { + "rotation": { + "radians": 1.1341697898535155 + }, + "translation": { + "x": 2.830740992000001, + "y": 4.271252992000002 + } + }, + "velocity": 2.1100191824926853, + "acceleration": -3.0000000000000377, + "curvature": 0.49029999405552943, + "holonomicRotation": 0.0, + "angularVelocity": 1.0305638993094042, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.3494031750405304, + "pose": { + "rotation": { + "radians": 1.1402545816799603 + }, + "translation": { + "x": 2.835918784000001, + "y": 4.282526784000001 + } + }, + "velocity": 2.0923061852767266, + "acceleration": -3.000000000000075, + "curvature": 0.49256570453850973, + "holonomicRotation": 0.0, + "angularVelocity": 1.026566433840469, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.3553536917747062, + "pose": { + "rotation": { + "radians": 1.1463631824232712 + }, + "translation": { + "x": 2.841024000000001, + "y": 4.293824000000002 + } + }, + "velocity": 2.074454635074199, + "acceleration": -3.0000000000000746, + "curvature": 0.4948644147448579, + "holonomicRotation": 0.0, + "angularVelocity": 1.0224868851113715, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.3613515743591078, + "pose": { + "rotation": { + "radians": 1.1524959387042597 + }, + "translation": { + "x": 2.8460562560000007, + "y": 4.305144256000001 + } + }, + "velocity": 2.0564609873209942, + "acceleration": -2.999999999999963, + "curvature": 0.49719582643303056, + "holonomicRotation": 0.0, + "angularVelocity": 1.0183200938587464, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.3673980580121383, + "pose": { + "rotation": { + "radians": 1.158653194505329 + }, + "translation": { + "x": 2.851015168000001, + "y": 4.316487168000002 + } + }, + "velocity": 2.0383215363619027, + "acceleration": -3.0, + "curvature": 0.49955962237973184, + "holonomicRotation": 0.0, + "angularVelocity": 1.014060739171566, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.3734944349631748, + "pose": { + "rotation": { + "radians": 1.1648352910225657 + }, + "translation": { + "x": 2.855900352000001, + "y": 4.327852352000001 + } + }, + "velocity": 2.0200324055087933, + "acceleration": -3.0, + "curvature": 0.5019554658535155, + "holonomicRotation": 0.0, + "angularVelocity": 1.009703331056945, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.3796420580325273, + "pose": { + "rotation": { + "radians": 1.1710425665137734 + }, + "translation": { + "x": 2.8607114240000007, + "y": 4.339239424000002 + } + }, + "velocity": 2.001589536300736, + "acceleration": -3.0, + "curvature": 0.5043829997560954, + "holonomicRotation": 0.0, + "angularVelocity": 1.005242202071386, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.385842344504059, + "pose": { + "rotation": { + "radians": 1.1772753561398894 + }, + "translation": { + "x": 2.8654480000000007, + "y": 4.350648000000001 + } + }, + "velocity": 1.9829886768861407, + "acceleration": -2.999999999999964, + "curvature": 0.5068418460483195, + "holonomicRotation": 0.0, + "angularVelocity": 1.000671498890062, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.3920967803203892, + "pose": { + "rotation": { + "radians": 1.1835339918029282 + }, + "translation": { + "x": 2.870109696000001, + "y": 4.362077696000002 + } + }, + "velocity": 1.9642253694371505, + "acceleration": -2.9999999999999645, + "curvature": 0.5093316047891755, + "holonomicRotation": 0.0, + "angularVelocity": 0.9959851725933427, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.3984069246342183, + "pose": { + "rotation": { + "radians": 1.1898188019764262 + }, + "translation": { + "x": 2.874696128000001, + "y": 4.373528128000001 + } + }, + "velocity": 1.9452949364956633, + "acceleration": -2.999999999999965, + "curvature": 0.5118518535624833, + "holonomicRotation": 0.0, + "angularVelocity": 0.9911769688340709, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.4047744147535932, + "pose": { + "rotation": { + "radians": 1.196130111532029 + }, + "translation": { + "x": 2.879206912000001, + "y": 4.384998912000002 + } + }, + "velocity": 1.9261924661375385, + "acceleration": -3.000000000000035, + "curvature": 0.5144021466205787, + "holonomicRotation": 0.0, + "angularVelocity": 0.9862404167758717, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.4112009715237113, + "pose": { + "rotation": { + "radians": 1.2024682415594241 + }, + "translation": { + "x": 2.8836416640000007, + "y": 4.3964896640000015 + } + }, + "velocity": 1.9069127958271843, + "acceleration": -3.0, + "curvature": 0.5169820141541447, + "holonomicRotation": 0.0, + "angularVelocity": 0.9811688175075026, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.41768840519348, + "pose": { + "rotation": { + "radians": 1.2088335091818494 + }, + "translation": { + "x": 2.888000000000001, + "y": 4.408000000000002 + } + }, + "velocity": 1.8874504948178785, + "acceleration": -2.999999999999966, + "curvature": 0.5195909614569449, + "holonomicRotation": 0.0, + "angularVelocity": 0.9759552310949556, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.4242386218214258, + "pose": { + "rotation": { + "radians": 1.2152262273646983 + }, + "translation": { + "x": 2.8922815360000005, + "y": 4.4195295360000015 + } + }, + "velocity": 1.867799844934041, + "acceleration": -3.0, + "curvature": 0.5222284682600655, + "holonomicRotation": 0.0, + "angularVelocity": 0.9705924628872844, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.4308536302830257, + "pose": { + "rotation": { + "radians": 1.2216467047194628 + }, + "translation": { + "x": 2.896485888000001, + "y": 4.431077888000002 + } + }, + "velocity": 1.847954819549241, + "acceleration": -3.0000000000000338, + "curvature": 0.5248939878051837, + "holonomicRotation": 0.0, + "angularVelocity": 0.9650730483864461, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.4375355499501146, + "pose": { + "rotation": { + "radians": 1.2280952453016536 + }, + "translation": { + "x": 2.9006126720000007, + "y": 4.442644672000001 + } + }, + "velocity": 1.8279090605479742, + "acceleration": -3.0000000000000333, + "curvature": 0.527586946160825, + "holonomicRotation": 0.0, + "angularVelocity": 0.9593892367630158, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.4442866191231065, + "pose": { + "rotation": { + "radians": 1.2345721484028647 + }, + "translation": { + "x": 2.904661504000001, + "y": 4.454229504000002 + } + }, + "velocity": 1.8076558530289983, + "acceleration": -3.0, + "curvature": 0.5303067413665304, + "holonomicRotation": 0.0, + "angularVelocity": 0.9535329728703047, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.4511092043084484, + "pose": { + "rotation": { + "radians": 1.2410777083373046 + }, + "translation": { + "x": 2.9086320000000003, + "y": 4.465832000000002 + } + }, + "velocity": 1.787188097472973, + "acceleration": -2.9999999999999676, + "curvature": 0.533052742734141, + "holonomicRotation": 0.0, + "angularVelocity": 0.9474958775078474, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.4580058104475067, + "pose": { + "rotation": { + "radians": 1.2476122142228576 + }, + "translation": { + "x": 2.9125237760000005, + "y": 4.477451776000002 + } + }, + "velocity": 1.766498279055798, + "acceleration": -3.000000000000032, + "curvature": 0.5358242899805569, + "holonomicRotation": 0.0, + "angularVelocity": 0.9412692255924726, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.4649790922191754, + "pose": { + "rotation": { + "radians": 1.2541759497559144 + }, + "translation": { + "x": 2.9163364480000005, + "y": 4.489088448000001 + } + }, + "velocity": 1.7455784337407918, + "acceleration": -2.999999999999968, + "curvature": 0.53862069246313, + "holonomicRotation": 0.0, + "angularVelocity": 0.9348439222379099, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.472031866557601, + "pose": { + "rotation": { + "radians": 1.2607691929811071 + }, + "translation": { + "x": 2.9200696320000006, + "y": 4.500741632000002 + } + }, + "velocity": 1.724420110725515, + "acceleration": -2.9999999999999685, + "curvature": 0.5414412283907459, + "holonomicRotation": 0.0, + "angularVelocity": 0.9282104760898124, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.4791671265489101, + "pose": { + "rotation": { + "radians": 1.2673922160546647 + }, + "translation": { + "x": 2.9237229440000005, + "y": 4.512410944000001 + } + }, + "velocity": 1.7030143307515877, + "acceleration": -3.000000000000031, + "curvature": 0.5442851441519826, + "holonomicRotation": 0.0, + "angularVelocity": 0.9213589699954693, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.4863880568977, + "pose": { + "rotation": { + "radians": 1.2740452850032347 + }, + "translation": { + "x": 2.9272960000000006, + "y": 4.524096000000002 + } + }, + "velocity": 1.6813515397052183, + "acceleration": -3.0, + "curvature": 0.5471516533835759, + "holonomicRotation": 0.0, + "angularVelocity": 0.9142790279992806, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.4936980511859816, + "pose": { + "rotation": { + "radians": 1.2807286594758052 + }, + "translation": { + "x": 2.9307884160000004, + "y": 4.535796416000001 + } + }, + "velocity": 1.6594215568403732, + "acceleration": -3.0, + "curvature": 0.5500399363964437, + "holonomicRotation": 0.0, + "angularVelocity": 0.9069597788904717, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.5011007311857052, + "pose": { + "rotation": { + "radians": 1.2874425924915514 + }, + "translation": { + "x": 2.9341998080000002, + "y": 4.547511808000002 + } + }, + "velocity": 1.6372135168412023, + "acceleration": -3.00000000000003, + "curvature": 0.5529491392614659, + "holonomicRotation": 0.0, + "angularVelocity": 0.8993898149606812, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.508599968532113, + "pose": { + "rotation": { + "radians": 1.2941873301808833 + }, + "translation": { + "x": 2.9375297920000008, + "y": 4.559241792000002 + } + }, + "velocity": 1.6147158048019792, + "acceleration": -2.9999999999999702, + "curvature": 0.5558783732812413, + "holonomicRotation": 0.0, + "angularVelocity": 0.8915571460678879, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.516199909121133, + "pose": { + "rotation": { + "radians": 1.3009631115227154 + }, + "translation": { + "x": 2.9407779840000003, + "y": 4.570985984000002 + } + }, + "velocity": 1.5919159830349192, + "acceleration": -3.0000000000000293, + "curvature": 0.5588267141256481, + "holonomicRotation": 0.0, + "angularVelocity": 0.8834491473354206, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.5239050006619295, + "pose": { + "rotation": { + "radians": 1.3077701680745735 + }, + "translation": { + "x": 2.9439440000000006, + "y": 4.582744000000002 + } + }, + "velocity": 1.5688007084125297, + "acceleration": -2.999999999999971, + "curvature": 0.5617932012303778, + "holonomicRotation": 0.0, + "angularVelocity": 0.875052500540059, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.5317200238988449, + "pose": { + "rotation": { + "radians": 1.314608723699815 + }, + "translation": { + "x": 2.9470274560000007, + "y": 4.594515456000002 + } + }, + "velocity": 1.545355638701783, + "acceleration": -3.000000000000057, + "curvature": 0.5647768371119463, + "holonomicRotation": 0.0, + "angularVelocity": 0.866353126952262, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.539650128119067, + "pose": { + "rotation": { + "radians": 1.3214789942880618 + }, + "translation": { + "x": 2.9500279680000006, + "y": 4.606299968000002 + } + }, + "velocity": 1.5215653260411162, + "acceleration": -3.000000000000028, + "curvature": 0.5677765867261405, + "holonomicRotation": 0.0, + "angularVelocity": 0.8573361111436658, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.5477008716886214, + "pose": { + "rotation": { + "radians": 1.3283811874717983 + }, + "translation": { + "x": 2.9529451520000003, + "y": 4.6180971520000025 + } + }, + "velocity": 1.4974130953324531, + "acceleration": -3.0000000000000275, + "curvature": 0.5707913768706009, + "holonomicRotation": 0.0, + "angularVelocity": 0.8479856135548375, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.55587826851637, + "pose": { + "rotation": { + "radians": 1.335315502338058 + }, + "translation": { + "x": 2.9557786240000006, + "y": 4.629906624000001 + } + }, + "velocity": 1.472880904849207, + "acceleration": -3.000000000000054, + "curvature": 0.5738200956456002, + "holonomicRotation": 0.0, + "angularVelocity": 0.8382847700393716, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.5641888415424363, + "pose": { + "rotation": { + "radians": 1.3422821291361096 + }, + "translation": { + "x": 2.9585280000000007, + "y": 4.641728000000002 + } + }, + "velocity": 1.4479491857710078, + "acceleration": -3.0, + "curvature": 0.576861591813262, + "holonomicRotation": 0.0, + "angularVelocity": 0.8282155756134849, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.5726396845955335, + "pose": { + "rotation": { + "radians": 1.3492812489797497 + }, + "translation": { + "x": 2.9611928960000005, + "y": 4.653560896000002 + } + }, + "velocity": 1.4225966566117165, + "acceleration": -2.999999999999974, + "curvature": 0.5799146744073892, + "holonomicRotation": 0.0, + "angularVelocity": 0.8177587498554331, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.5812385342789634, + "pose": { + "rotation": { + "radians": 1.3563130335470661 + }, + "translation": { + "x": 2.9637729280000005, + "y": 4.665404928000002 + } + }, + "velocity": 1.3968001075614265, + "acceleration": -3.0000000000000258, + "curvature": 0.5829781121894888, + "holonomicRotation": 0.0, + "angularVelocity": 0.8068935795489924, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.5899938539486989, + "pose": { + "rotation": { + "radians": 1.3633776447754746 + }, + "translation": { + "x": 2.9662677120000005, + "y": 4.6772597120000015 + } + }, + "velocity": 1.37053414855222, + "acceleration": -3.0000000000000253, + "curvature": 0.5860506332508155, + "holonomicRotation": 0.0, + "angularVelocity": 0.7955977342737229, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.5989149323684775, + "pose": { + "rotation": { + "radians": 1.3704752345535287 + }, + "translation": { + "x": 2.9686768640000007, + "y": 4.689124864000002 + } + }, + "velocity": 1.3437709132928843, + "acceleration": -2.999999999999975, + "curvature": 0.5891309246386679, + "holonomicRotation": 0.0, + "angularVelocity": 0.7838470488111148, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.608012000306507, + "pose": { + "rotation": { + "radians": 1.3776059444095874 + }, + "translation": { + "x": 2.9710000000000005, + "y": 4.701000000000001 + } + }, + "velocity": 1.316479709478796, + "acceleration": -2.9999999999999756, + "curvature": 0.5922176320623127, + "holonomicRotation": 0.0, + "angularVelocity": 0.7716152648033403, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.6172963692331, + "pose": { + "rotation": { + "radians": 1.3847699051974123 + }, + "translation": { + "x": 2.9732367360000005, + "y": 4.712884736000002 + } + }, + "velocity": 1.288626602699017, + "acceleration": -3.000000000000024, + "curvature": 0.5953093595736683, + "holonomicRotation": 0.0, + "angularVelocity": 0.7588737220632151, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.6267805974686687, + "pose": { + "rotation": { + "radians": 1.3919672367794353 + }, + "translation": { + "x": 2.9753866880000004, + "y": 4.724778688000002 + } + }, + "velocity": 1.2601739179923108, + "acceleration": -3.0, + "curvature": 0.5984046693700557, + "holonomicRotation": 0.0, + "angularVelocity": 0.7455909869329629, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.636478690732875, + "pose": { + "rotation": { + "radians": 1.3991980477076629 + }, + "translation": { + "x": 2.9774494720000004, + "y": 4.736681472000003 + } + }, + "velocity": 1.2310796381996918, + "acceleration": -3.0, + "curvature": 0.6015020816536214, + "holonomicRotation": 0.0, + "angularVelocity": 0.7317324013974611, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.6464063462269714, + "pose": { + "rotation": { + "radians": 1.4064624349026047 + }, + "translation": { + "x": 2.9794247040000004, + "y": 4.748592704000002 + } + }, + "velocity": 1.201296671717403, + "acceleration": -2.999999999999978, + "curvature": 0.60460007456127, + "holonomicRotation": 0.0, + "angularVelocity": 0.717259531383458, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.6565812523929566, + "pose": { + "rotation": { + "radians": 1.41376048333109 + }, + "translation": { + "x": 2.9813120000000004, + "y": 4.760512000000002 + } + }, + "velocity": 1.1707719532194472, + "acceleration": -3.0, + "curvature": 0.6076970841012796, + "holonomicRotation": 0.0, + "angularVelocity": 0.7021294852003354, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.6670234607122523, + "pose": { + "rotation": { + "radians": 1.4210922656826717 + }, + "translation": { + "x": 2.9831109760000003, + "y": 4.772438976000002 + } + }, + "velocity": 1.1394453282615602, + "acceleration": -3.0, + "curvature": 0.6107915042418585, + "holonomicRotation": 0.0, + "angularVelocity": 0.6862940629818453, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.6777558519135327, + "pose": { + "rotation": { + "radians": 1.428457842045709 + }, + "translation": { + "x": 2.9848212480000003, + "y": 4.784373248000002 + } + }, + "velocity": 1.107248154657719, + "acceleration": -2.999999999999979, + "curvature": 0.6138816870079442, + "holonomicRotation": 0.0, + "angularVelocity": 0.6696986828773126, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.6888047276641542, + "pose": { + "rotation": { + "radians": 1.4358572595831753 + }, + "translation": { + "x": 2.9864424320000005, + "y": 4.796314432000002 + } + }, + "velocity": 1.0741015274058545, + "acceleration": -3.0, + "curvature": 0.6169659427695113, + "holonomicRotation": 0.0, + "angularVelocity": 0.652281008129766, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.7002005716841848, + "pose": { + "rotation": { + "radians": 1.4432905522090504 + }, + "translation": { + "x": 2.9879741440000003, + "y": 4.808262144000002 + } + }, + "velocity": 1.039913995345763, + "acceleration": -2.9999999999999805, + "curvature": 0.6200425403987194, + "holonomicRotation": 0.0, + "angularVelocity": 0.6339691664027446, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.7119790436535878, + "pose": { + "rotation": { + "radians": 1.450757740264991 + }, + "translation": { + "x": 2.9894160000000003, + "y": 4.820216000000001 + } + }, + "velocity": 1.004578579437554, + "acceleration": -3.0000000000000187, + "curvature": 0.623109707774154, + "holonomicRotation": 0.0, + "angularVelocity": 0.6146794031013213, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.724182299361434, + "pose": { + "rotation": { + "radians": 1.4582588301993826 + }, + "translation": { + "x": 2.9907676160000003, + "y": 4.8321756160000024 + } + }, + "velocity": 0.9679688123140158, + "acceleration": -2.9999999999999636, + "curvature": 0.6261656322069277, + "holonomicRotation": 0.0, + "angularVelocity": 0.5943129313748706, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.7368607784086099, + "pose": { + "rotation": { + "radians": 1.4657938142472846 + }, + "translation": { + "x": 2.992028608, + "y": 4.844140608000002 + } + }, + "velocity": 0.9299333751724882, + "acceleration": -2.999999999999991, + "curvature": 0.6292084609699685, + "holonomicRotation": 0.0, + "angularVelocity": 0.5727516148206816, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.7500756793718817, + "pose": { + "rotation": { + "radians": 1.473362670113694 + }, + "translation": { + "x": 2.993198592, + "y": 4.856110592000002 + } + }, + "velocity": 0.8902886722826729, + "acceleration": -2.999999999999983, + "curvature": 0.6322363020068372, + "holonomicRotation": 0.0, + "angularVelocity": 0.549851904665754, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.763902476153777, + "pose": { + "rotation": { + "radians": 1.4809653606596453 + }, + "translation": { + "x": 2.9942771840000004, + "y": 4.868085184000002 + } + }, + "velocity": 0.8488082819369874, + "acceleration": -2.999999999999984, + "curvature": 0.6352472246674356, + "holonomicRotation": 0.0, + "angularVelocity": 0.5254360802848803, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.7784360668586583, + "pose": { + "rotation": { + "radians": 1.488601833592083 + }, + "translation": { + "x": 2.995264, + "y": 4.880064000000003 + } + }, + "velocity": 0.8052075098223431, + "acceleration": -3.000000000000015, + "curvature": 0.6382392605344301, + "holonomicRotation": 0.0, + "angularVelocity": 0.49927915802747, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.7937985898931266, + "pose": { + "rotation": { + "radians": 1.49627202115791 + }, + "translation": { + "x": 2.996158656, + "y": 4.8920466560000015 + } + }, + "velocity": 0.759119940718938, + "acceleration": -3.0000000000000218, + "curvature": 0.6412104045156426, + "holonomicRotation": 0.0, + "angularVelocity": 0.4710885081646315, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.81015181981141, + "pose": { + "rotation": { + "radians": 1.5039758398437872 + }, + "translation": { + "x": 2.996960768, + "y": 4.904032768000002 + } + }, + "velocity": 0.7100602509640875, + "acceleration": -3.0000000000000204, + "curvature": 0.6441586157880042, + "holonomicRotation": 0.0, + "angularVelocity": 0.44047046706200677, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.8277179242740946, + "pose": { + "rotation": { + "radians": 1.511713190080926 + }, + "translation": { + "x": 2.9976699520000003, + "y": 4.916021952000001 + } + }, + "velocity": 0.6573619375760337, + "acceleration": -2.9999999999999876, + "curvature": 0.6470818190817547, + "holonomicRotation": 0.0, + "angularVelocity": 0.40687199814347647, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.8468167217124687, + "pose": { + "rotation": { + "radians": 1.5194839559568147 + }, + "translation": { + "x": 2.998285824, + "y": 4.928013824000002 + } + }, + "velocity": 0.6000655452609113, + "acceleration": -3.0000000000000115, + "curvature": 0.6499779058913984, + "holonomicRotation": 0.0, + "angularVelocity": 0.3694696536402002, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.8679390220148424, + "pose": { + "rotation": { + "radians": 1.527288004933617 + }, + "translation": { + "x": 2.998808, + "y": 4.9400080000000015 + } + }, + "velocity": 0.5366986443537898, + "acceleration": -3.000000000000016, + "curvature": 0.6528447360474303, + "holonomicRotation": 0.0, + "angularVelocity": 0.3269313584685308, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.891910973431456, + "pose": { + "rotation": { + "radians": 1.5351251875753922 + }, + "translation": { + "x": 2.999236096, + "y": 4.952004096000002 + } + }, + "velocity": 0.4647827901039487, + "acceleration": -3.000000000000002, + "curvature": 0.6556801391027219, + "holonomicRotation": 0.0, + "angularVelocity": 0.2768026259312415, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.9203433254767548, + "pose": { + "rotation": { + "radians": 1.5429953372829324 + }, + "translation": { + "x": 2.999569728, + "y": 4.964001728000001 + } + }, + "velocity": 0.3794857339680527, + "acceleration": -2.999999999999996, + "curvature": 0.6584819160890972, + "holonomicRotation": 0.0, + "angularVelocity": 0.21329879823738362, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 1.9573943207044333, + "pose": { + "rotation": { + "radians": 1.5508982700384952 + }, + "translation": { + "x": 2.999808512, + "y": 4.976000512000002 + } + }, + "velocity": 0.26833274828501735, + "acceleration": -2.9999999999999942, + "curvature": 0.6612478412834537, + "holonomicRotation": 0.0, + "angularVelocity": 0.0887202271044126, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.0468385701327723, + "pose": { + "rotation": { + "radians": 1.5588337841609612 + }, + "translation": { + "x": 2.999952064, + "y": 4.988000064000001 + } + }, + "velocity": 0, + "acceleration": -3.0000000000000058, + "curvature": 0.6612478412834537, + "holonomicRotation": 0.0, + "angularVelocity": 0.0, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.0468385701327723, + "pose": { + "rotation": { + "radians": 1.5747909935183229 + }, + "translation": { + "x": 3.0, + "y": 5.0 + } + }, + "velocity": 0, + "acceleration": -3.0, + "curvature": -0.663975664093471, + "holonomicRotation": 0.0, + "angularVelocity": 0.0, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.1362814075366994, + "pose": { + "rotation": { + "radians": 1.5747909935183229 + }, + "translation": { + "x": 3.000047936, + "y": 4.988000064000001 + } + }, + "velocity": -0.26832851221178183, + "acceleration": -3.0, + "curvature": -0.663975664093471, + "holonomicRotation": 0.0, + "angularVelocity": 0.21506066046267544, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.1733308423025224, + "pose": { + "rotation": { + "radians": 1.5827588694288295 + }, + "translation": { + "x": 3.000191488, + "y": 4.976000512 + } + }, + "velocity": -0.37947681650924975, + "acceleration": -2.999999999999999, + "curvature": -0.6612478412834536, + "holonomicRotation": 0.0, + "angularVelocity": 0.2791196654352754, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.201761351167232, + "pose": { + "rotation": { + "radians": 1.5906943835513019 + }, + "translation": { + "x": 3.000430272, + "y": 4.964001728000001 + } + }, + "velocity": -0.4647683431033788, + "acceleration": -2.999999999999999, + "curvature": -0.6584819160890969, + "holonomicRotation": 0.0, + "angularVelocity": 0.32970274886998213, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.2257312233707207, + "pose": { + "rotation": { + "radians": 1.5985973163068543 + }, + "translation": { + "x": 3.000763904, + "y": 4.952004096 + } + }, + "velocity": -0.536677959713845, + "acceleration": -2.9999999999999956, + "curvature": -0.6556801391026855, + "holonomicRotation": 0.0, + "angularVelocity": 0.37263931025212366, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.2468512434304047, + "pose": { + "rotation": { + "radians": 1.606467466014406 + }, + "translation": { + "x": 3.001192, + "y": 4.940008000000001 + } + }, + "velocity": -0.6000380198928976, + "acceleration": -3.0000000000000013, + "curvature": -0.6528447360474302, + "holonomicRotation": 0.0, + "angularVelocity": 0.41040227730578355, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.2659475863971474, + "pose": { + "rotation": { + "radians": 1.6143046486561694 + }, + "translation": { + "x": 3.001714176, + "y": 4.928013824 + } + }, + "velocity": -0.6573270487931249, + "acceleration": -3.000000000000003, + "curvature": -0.6499779059037698, + "holonomicRotation": 0.0, + "angularVelocity": 0.4443334326624615, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.2835110835440036, + "pose": { + "rotation": { + "radians": 1.6221086976329895 + }, + "translation": { + "x": 3.0023300479999997, + "y": 4.916021952000001 + } + }, + "velocity": -0.7100175402336942, + "acceleration": -3.0, + "curvature": -0.6470818190817534, + "holonomicRotation": 0.0, + "angularVelocity": 0.47526203255141736, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.2998615709460464, + "pose": { + "rotation": { + "radians": 1.6298794635088907 + }, + "translation": { + "x": 3.0030392320000003, + "y": 4.9040327679999995 + } + }, + "velocity": -0.7590690024398227, + "acceleration": -3.000000000000005, + "curvature": -0.6441586157755366, + "holonomicRotation": 0.0, + "angularVelocity": 0.5037448816056413, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.3152212312019, + "pose": { + "rotation": { + "radians": 1.6376168137459786 + }, + "translation": { + "x": 3.003841344, + "y": 4.892046656000001 + } + }, + "velocity": -0.8051479832073837, + "acceleration": -3.0, + "curvature": -0.6412104045031639, + "holonomicRotation": 0.0, + "angularVelocity": 0.5301782294741988, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.329751851769881, + "pose": { + "rotation": { + "radians": 1.645320632431872 + }, + "translation": { + "x": 3.004736, + "y": 4.880064 + } + }, + "velocity": -0.8487398449113267, + "acceleration": -3.000000000000004, + "curvature": -0.6382392605344286, + "holonomicRotation": 0.0, + "angularVelocity": 0.5548565565092586, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.3435755823826048, + "pose": { + "rotation": { + "radians": 1.6529908199977594 + }, + "translation": { + "x": 3.005722816, + "y": 4.868085184000001 + } + }, + "velocity": -0.8902110367494969, + "acceleration": -2.999999999999996, + "curvature": -0.6352472246548937, + "holonomicRotation": 0.0, + "angularVelocity": 0.5780062147703856, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.3567873312137406, + "pose": { + "rotation": { + "radians": 1.6606272929301413 + }, + "translation": { + "x": 3.0068014080000003, + "y": 4.856110592 + } + }, + "velocity": -0.9298462832429044, + "acceleration": -3.0, + "curvature": -0.6322363020068388, + "holonomicRotation": 0.0, + "angularVelocity": 0.5998059641255189, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.3694625812123173, + "pose": { + "rotation": { + "radians": 1.6682299834760697 + }, + "translation": { + "x": 3.007971392, + "y": 4.844140608000001 + } + }, + "velocity": -0.9678720332386348, + "acceleration": -3.0000000000000044, + "curvature": -0.629208460982605, + "holonomicRotation": 0.0, + "angularVelocity": 0.6204001606251212, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.381662539153711, + "pose": { + "rotation": { + "radians": 1.67579883934253 + }, + "translation": { + "x": 3.009232384, + "y": 4.832175616 + } + }, + "velocity": -1.004471907062816, + "acceleration": -3.000000000000009, + "curvature": -0.6261656322196447, + "holonomicRotation": 0.0, + "angularVelocity": 0.639907580276147, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.3934376521233767, + "pose": { + "rotation": { + "radians": 1.6833338233904271 + }, + "translation": { + "x": 3.010584, + "y": 4.820216 + } + }, + "velocity": -1.0397972459718132, + "acceleration": -2.999999999999986, + "curvature": -0.623109707774154, + "holonomicRotation": 0.0, + "angularVelocity": 0.6584275258631126, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.404830082785391, + "pose": { + "rotation": { + "radians": 1.6908349133247844 + }, + "translation": { + "x": 3.012025856, + "y": 4.8082621439999995 + } + }, + "velocity": -1.0739745379578567, + "acceleration": -2.999999999999995, + "curvature": -0.6200425403987191, + "holonomicRotation": 0.0, + "angularVelocity": 0.6760441751862138, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.4158754971681424, + "pose": { + "rotation": { + "radians": 1.6983021013807615 + }, + "translation": { + "x": 3.013557568, + "y": 4.796314432000001 + } + }, + "velocity": -1.107110781106111, + "acceleration": -3.0, + "curvature": -0.6169659427566521, + "holonomicRotation": 0.0, + "angularVelocity": 0.6928297531971396, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.426604384880453, + "pose": { + "rotation": { + "radians": 1.7057353940065614 + }, + "translation": { + "x": 3.0151787519999997, + "y": 4.784373248 + } + }, + "velocity": -1.1392974442430421, + "acceleration": -3.0, + "curvature": -0.6138816870079441, + "holonomicRotation": 0.0, + "angularVelocity": 0.7088468951007417, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.437043053073432, + "pose": { + "rotation": { + "radians": 1.7131348115441414 + }, + "translation": { + "x": 3.016889024, + "y": 4.772438976000001 + } + }, + "velocity": -1.170613448821979, + "acceleration": -3.0000000000000053, + "curvature": -0.6107915042418562, + "holonomicRotation": 0.0, + "angularVelocity": 0.7241504390507798, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.44721438759974, + "pose": { + "rotation": { + "radians": 1.7205003879070997 + }, + "translation": { + "x": 3.018688, + "y": 4.760511999999999 + } + }, + "velocity": -1.201127452400903, + "acceleration": -2.999999999999989, + "curvature": -0.6076970841012463, + "holonomicRotation": 0.0, + "angularVelocity": 0.7387888084396566, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.4571384447428093, + "pose": { + "rotation": { + "radians": 1.727832170258715 + }, + "translation": { + "x": 3.020575296, + "y": 4.748592704 + } + }, + "velocity": -1.230899623830111, + "acceleration": -3.000000000000011, + "curvature": -0.60460007456127, + "holonomicRotation": 0.0, + "angularVelocity": 0.7528050925433399, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.466832917456597, + "pose": { + "rotation": { + "radians": 1.7351302186871767 + }, + "translation": { + "x": 3.022550528, + "y": 4.736681472 + } + }, + "velocity": -1.2599830419714737, + "acceleration": -3.0, + "curvature": -0.6015020816667189, + "holonomicRotation": 0.0, + "angularVelocity": 0.7662379027193594, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.476313507191436, + "pose": { + "rotation": { + "radians": 1.7423946058821427 + }, + "translation": { + "x": 3.024613312, + "y": 4.724778688 + } + }, + "velocity": -1.288424811175992, + "acceleration": -3.0000000000000058, + "curvature": -0.5984046693700557, + "holonomicRotation": 0.0, + "angularVelocity": 0.7791220580677133, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.4855942236765203, + "pose": { + "rotation": { + "radians": 1.7496254168103444 + }, + "translation": { + "x": 3.026763264, + "y": 4.7128847359999995 + } + }, + "velocity": -1.3162669606312447, + "acceleration": -3.000000000000012, + "curvature": -0.5953093595736652, + "holonomicRotation": 0.0, + "angularVelocity": 0.791489140569654, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.4946876290224305, + "pose": { + "rotation": { + "radians": 1.7568227483924304 + }, + "translation": { + "x": 3.0290000000000004, + "y": 4.701 + } + }, + "velocity": -1.3435471766689753, + "acceleration": -3.000000000000018, + "curvature": -0.5922176320623417, + "holonomicRotation": 0.0, + "angularVelocity": 0.8033679485311952, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.5036050382878523, + "pose": { + "rotation": { + "radians": 1.7639867091802053 + }, + "translation": { + "x": 3.031323136, + "y": 4.689124864 + } + }, + "velocity": -1.3702994044652403, + "acceleration": -2.9999999999999876, + "curvature": -0.589130924651956, + "holonomicRotation": 0.0, + "angularVelocity": 0.8147848703989182, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.5123566856429775, + "pose": { + "rotation": { + "radians": 1.7711174190362282 + }, + "translation": { + "x": 3.033732288, + "y": 4.677259712 + } + }, + "velocity": -1.3965543465306167, + "acceleration": -3.0, + "curvature": -0.586050633250819, + "holonomicRotation": 0.0, + "angularVelocity": 0.8257641950397024, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.5209518630807075, + "pose": { + "rotation": { + "radians": 1.7782150088143185 + }, + "translation": { + "x": 3.036227072, + "y": 4.665404928 + } + }, + "velocity": -1.4223398788438055, + "acceleration": -2.9999999999999676, + "curvature": -0.5829781121894888, + "holonomicRotation": 0.0, + "angularVelocity": 0.8363283714374167, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.529399037025395, + "pose": { + "rotation": { + "radians": 1.7852796200427274 + }, + "translation": { + "x": 3.038807104, + "y": 4.653560896 + } + }, + "velocity": -1.4476814006778675, + "acceleration": -2.9999999999999933, + "curvature": -0.5799146744073547, + "holonomicRotation": 0.0, + "angularVelocity": 0.8464982273758479, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.5377059469986545, + "pose": { + "rotation": { + "radians": 1.7923114046100626 + }, + "translation": { + "x": 3.041472, + "y": 4.641728 + } + }, + "velocity": -1.4726021305976469, + "acceleration": -3.00000000000002, + "curvature": -0.5768615917997932, + "holonomicRotation": 0.0, + "angularVelocity": 0.8562931546703597, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.545879689607045, + "pose": { + "rotation": { + "radians": 1.7993105244536647 + }, + "translation": { + "x": 3.044221376, + "y": 4.629906623999999 + } + }, + "velocity": -1.497123358422818, + "acceleration": -3.0000000000000067, + "curvature": -0.5738200956456038, + "holonomicRotation": 0.0, + "angularVelocity": 0.8657312671405707, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.5539267904366865, + "pose": { + "rotation": { + "radians": 1.8062771512517184 + }, + "translation": { + "x": 3.047054848, + "y": 4.618097152 + } + }, + "velocity": -1.5212646609117435, + "acceleration": -3.0000000000000138, + "curvature": -0.5707913768570366, + "holonomicRotation": 0.0, + "angularVelocity": 0.87482953581738, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.5618532659183937, + "pose": { + "rotation": { + "radians": 1.813211466118048 + }, + "translation": { + "x": 3.0499720320000003, + "y": 4.606299968 + } + }, + "velocity": -1.5450440873568647, + "acceleration": -2.999999999999986, + "curvature": -0.5677765867125653, + "holonomicRotation": 0.0, + "angularVelocity": 0.8836039055943861, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.569664676823153, + "pose": { + "rotation": { + "radians": 1.820113659301696 + }, + "translation": { + "x": 3.0529725439999997, + "y": 4.594515456 + } + }, + "velocity": -1.5684783200711427, + "acceleration": -3.0, + "curvature": -0.5647768371119459, + "holonomicRotation": 0.0, + "angularVelocity": 0.8920693959976156, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.577366174732555, + "pose": { + "rotation": { + "radians": 1.8269839298900132 + }, + "translation": { + "x": 3.0560560000000003, + "y": 4.582744 + } + }, + "velocity": -1.591582813799349, + "acceleration": -3.0, + "curvature": -0.5617932012303779, + "holonomicRotation": 0.0, + "angularVelocity": 0.9002401887212853, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.5849625425806573, + "pose": { + "rotation": { + "radians": 1.833822485515185 + }, + "translation": { + "x": 3.059222016, + "y": 4.570985984 + } + }, + "velocity": -1.6143719173436557, + "acceleration": -3.0000000000000293, + "curvature": -0.5588267141256478, + "holonomicRotation": 0.0, + "angularVelocity": 0.9081297043785966, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.592458230167031, + "pose": { + "rotation": { + "radians": 1.8406295420671128 + }, + "translation": { + "x": 3.062470208, + "y": 4.559241792 + } + }, + "velocity": -1.6368589801027766, + "acceleration": -3.0, + "curvature": -0.5558783732812124, + "holonomicRotation": 0.0, + "angularVelocity": 0.9157506692825748, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.5998573853836477, + "pose": { + "rotation": { + "radians": 1.8474053234088554 + }, + "translation": { + "x": 3.065800192, + "y": 4.547511807999999 + } + }, + "velocity": -1.6590564457526276, + "acceleration": -3.000000000000015, + "curvature": -0.5529491392614698, + "holonomicRotation": 0.0, + "angularVelocity": 0.9231151746224233, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.607163881771995, + "pose": { + "rotation": { + "radians": 1.8541500610982622 + }, + "translation": { + "x": 3.069211584, + "y": 4.535796415999999 + } + }, + "velocity": -1.6809759349176676, + "acceleration": -3.0, + "curvature": -0.5500399363825702, + "holonomicRotation": 0.0, + "angularVelocity": 0.9302347284834435, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.614381342924677, + "pose": { + "rotation": { + "radians": 1.8608639941139673 + }, + "translation": { + "x": 3.072704, + "y": 4.524095999999999 + } + }, + "velocity": -1.7026283183757143, + "acceleration": -3.0000000000000155, + "curvature": -0.5471516533975169, + "holonomicRotation": 0.0, + "angularVelocity": 0.9371203020405943, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.6215131641626916, + "pose": { + "rotation": { + "radians": 1.867547368586635 + }, + "translation": { + "x": 3.0762770560000003, + "y": 4.512410944 + } + }, + "velocity": -1.7240237820897581, + "acceleration": -3.0000000000000155, + "curvature": -0.5442851441519785, + "holonomicRotation": 0.0, + "angularVelocity": 0.9437823705718374, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.6285625318515775, + "pose": { + "rotation": { + "radians": 1.8742004375350847 + }, + "translation": { + "x": 3.0799303680000003, + "y": 4.5007416319999995 + } + }, + "velocity": -1.7451718851564173, + "acceleration": -3.0000000000000315, + "curvature": -0.5414412284047262, + "holonomicRotation": 0.0, + "angularVelocity": 0.9502309502266035, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.6355324406637557, + "pose": { + "rotation": { + "radians": 1.8808234606086733 + }, + "translation": { + "x": 3.083663552, + "y": 4.4890884479999995 + } + }, + "velocity": -1.7660816115929512, + "acceleration": -3.0, + "curvature": -0.538620692477182, + "holonomicRotation": 0.0, + "angularVelocity": 0.9564756306438044, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.6424257090481733, + "pose": { + "rotation": { + "radians": 1.887416703833856 + }, + "translation": { + "x": 3.087476224, + "y": 4.477451776 + } + }, + "velocity": -1.7867614167462036, + "acceleration": -2.999999999999984, + "curvature": -0.5358242899805565, + "holonomicRotation": 0.0, + "angularVelocity": 0.9625256044974729, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.6492449931300026, + "pose": { + "rotation": { + "radians": 1.8939804393669588 + }, + "translation": { + "x": 3.091368, + "y": 4.465832 + } + }, + "velocity": -1.8072192689916917, + "acceleration": -3.0000000000000324, + "curvature": -0.5330527427341364, + "holonomicRotation": 0.0, + "angularVelocity": 0.9683896940137469, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.655992799231136, + "pose": { + "rotation": { + "radians": 1.9005149452524996 + }, + "translation": { + "x": 3.095338496, + "y": 4.454229504 + } + }, + "velocity": -1.8274626872950925, + "acceleration": -3.0000000000000164, + "curvature": -0.5303067413665257, + "holonomicRotation": 0.0, + "angularVelocity": 0.9740763748988773, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.662671495175413, + "pose": { + "rotation": { + "radians": 1.9070205051869529 + }, + "translation": { + "x": 3.099387328, + "y": 4.442644671999999 + } + }, + "velocity": -1.8474987751279237, + "acceleration": -3.0, + "curvature": -0.527586946146635, + "holonomicRotation": 0.0, + "angularVelocity": 0.9795937980323695, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.6692833205199555, + "pose": { + "rotation": { + "radians": 1.9134974082881397 + }, + "translation": { + "x": 3.103514112, + "y": 4.431077888 + } + }, + "velocity": -1.8673342511615505, + "acceleration": -2.999999999999983, + "curvature": -0.5248939878051837, + "holonomicRotation": 0.0, + "angularVelocity": 0.9849498091799569, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.6758303958349496, + "pose": { + "rotation": { + "radians": 1.91994594887033 + }, + "translation": { + "x": 3.1077184640000004, + "y": 4.419529536 + } + }, + "velocity": -1.886975477106533, + "acceleration": -2.999999999999983, + "curvature": -0.5222284682600391, + "holonomicRotation": 0.0, + "angularVelocity": 0.9901519669516187, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.6823147311380504, + "pose": { + "rotation": { + "radians": 1.9263664262250693 + }, + "translation": { + "x": 3.112, + "y": 4.4079999999999995 + } + }, + "velocity": -1.9064284830158356, + "acceleration": -2.9999999999999827, + "curvature": -0.5195909614569447, + "holonomicRotation": 0.0, + "angularVelocity": 0.9952075592378692, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.6887382335758687, + "pose": { + "rotation": { + "radians": 1.932759144407969 + }, + "translation": { + "x": 3.1163583360000002, + "y": 4.396489664 + } + }, + "velocity": -1.9256989903292907, + "acceleration": -2.9999999999999827, + "curvature": -0.5169820141397626, + "holonomicRotation": 0.0, + "angularVelocity": 1.0001236180920878, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.695102714433262, + "pose": { + "rotation": { + "radians": 1.9391244120303428 + }, + "translation": { + "x": 3.120793088, + "y": 4.3849989119999995 + } + }, + "velocity": -1.9447924329014699, + "acceleration": -3.0, + "curvature": -0.5144021466205735, + "holonomicRotation": 0.0, + "angularVelocity": 1.0049069337160776, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.7014098955411114, + "pose": { + "rotation": { + "radians": 1.9454625420578235 + }, + "translation": { + "x": 3.1253038720000004, + "y": 4.373528127999999 + } + }, + "velocity": -1.9637139762250175, + "acceleration": -3.0, + "curvature": -0.5118518535624782, + "holonomicRotation": 0.0, + "angularVelocity": 1.009564066942242, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.7076614151446488, + "pose": { + "rotation": { + "radians": 1.95177385161334 + }, + "translation": { + "x": 3.1298903040000003, + "y": 4.362077695999999 + } + }, + "velocity": -1.9824685350356301, + "acceleration": -3.0, + "curvature": -0.5093316047891807, + "holonomicRotation": 0.0, + "angularVelocity": 1.0141013611165504, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.7138588332869524, + "pose": { + "rotation": { + "radians": 1.9580586617868585 + }, + "translation": { + "x": 3.1345520000000002, + "y": 4.350648 + } + }, + "velocity": -2.001060789462541, + "acceleration": -3.0000000000000178, + "curvature": -0.506841846048325, + "holonomicRotation": 0.0, + "angularVelocity": 1.018524952789108, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.7200036367558105, + "pose": { + "rotation": { + "radians": 1.9643172974498757 + }, + "translation": { + "x": 3.139288576, + "y": 4.339239424 + } + }, + "velocity": -2.019495199869116, + "acceleration": -3.000000000000036, + "curvature": -0.5043829997561204, + "holonomicRotation": 0.0, + "angularVelocity": 1.0228407818470144, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.726097243636585, + "pose": { + "rotation": { + "radians": 1.9705500870760755 + }, + "translation": { + "x": 3.144099648, + "y": 4.327852352 + } + }, + "velocity": -2.0377760205114392, + "acceleration": -2.9999999999999454, + "curvature": -0.5019554658388903, + "holonomicRotation": 0.0, + "angularVelocity": 1.027054600792063, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.7321410075088677, + "pose": { + "rotation": { + "radians": 1.976757362567204 + }, + "translation": { + "x": 3.1489848320000005, + "y": 4.316487167999999 + } + }, + "velocity": -2.0559073121282863, + "acceleration": -3.0, + "curvature": -0.49955962237970714, + "holonomicRotation": 0.0, + "angularVelocity": 1.0311719834343849, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.738136221320501, + "pose": { + "rotation": { + "radians": 1.9829394590844593 + }, + "translation": { + "x": 3.153943744, + "y": 4.305144255999999 + } + }, + "velocity": -2.0738929535631874, + "acceleration": -3.0000000000000555, + "curvature": -0.4971958264330603, + "holonomicRotation": 0.0, + "angularVelocity": 1.0351983330386056, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.744084120968872, + "pose": { + "rotation": { + "radians": 1.9890967148855339 + }, + "translation": { + "x": 3.158976, + "y": 4.293824 + } + }, + "velocity": -2.0917366525083003, + "acceleration": -3.0000000000000187, + "curvature": -0.4948644147448579, + "holonomicRotation": 0.0, + "angularVelocity": 1.0391388898266953, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.7499858886161315, + "pose": { + "rotation": { + "radians": 1.995229471166522 + }, + "translation": { + "x": 3.164081216, + "y": 4.282526783999999 + } + }, + "velocity": -2.109441955450078, + "acceleration": -3.0, + "curvature": -0.49256570452370857, + "holonomicRotation": 0.0, + "angularVelocity": 1.0429987382066992, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.7558426557621933, + "pose": { + "rotation": { + "radians": 2.001338071909835 + }, + "translation": { + "x": 3.1692590080000005, + "y": 4.271252991999999 + } + }, + "velocity": -2.1270122568882637, + "acceleration": -2.999999999999943, + "curvature": -0.49029999405552377, + "holonomicRotation": 0.0, + "angularVelocity": 1.0467828132733483, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.761655506096872, + "pose": { + "rotation": { + "radians": 2.0074228637363074 + }, + "translation": { + "x": 3.1745089920000003, + "y": 4.260003007999999 + } + }, + "velocity": -2.144450807892301, + "acceleration": -2.999999999999981, + "curvature": -0.4880675635449118, + "holonomicRotation": 0.0, + "angularVelocity": 1.0504959073292972, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.76742547815031, + "pose": { + "rotation": { + "radians": 2.013484195763848 + }, + "translation": { + "x": 3.1798307840000004, + "y": 4.248777216 + } + }, + "velocity": -2.161760724052613, + "acceleration": -2.9999999999999614, + "curvature": -0.4858686756363079, + "holonomicRotation": 0.0, + "angularVelocity": 1.0541426755227752, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.7731535677589205, + "pose": { + "rotation": { + "radians": 2.019522419469503 + }, + "translation": { + "x": 3.1852240000000003, + "y": 4.237576 + } + }, + "velocity": -2.1789449928784457, + "acceleration": -3.0000000000000777, + "curvature": -0.4837035763095772, + "holonomicRotation": 0.0, + "angularVelocity": 1.0577276417062655, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.7788407303623694, + "pose": { + "rotation": { + "radians": 2.0255378885580493 + }, + "translation": { + "x": 3.190688256, + "y": 4.226399743999999 + } + }, + "velocity": -2.1960064806887933, + "acceleration": -3.000000000000039, + "curvature": -0.4815724954088063, + "holonomicRotation": 0.0, + "angularVelocity": 1.0612552035558065, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.7844878831455677, + "pose": { + "rotation": { + "radians": 2.0315309588344928 + }, + "translation": { + "x": 3.1962231680000004, + "y": 4.2152488319999994 + } + }, + "velocity": -2.2129479390383873, + "acceleration": -3.0000000000000195, + "curvature": -0.47947564724702796, + "holonomicRotation": 0.0, + "angularVelocity": 1.0647296375816526, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.7900959070383258, + "pose": { + "rotation": { + "radians": 2.0375019880813783 + }, + "translation": { + "x": 3.2018283520000006, + "y": 4.2041236479999995 + } + }, + "velocity": -2.2297720107166614, + "acceleration": -3.0, + "curvature": -0.47741323142346864, + "holonomicRotation": 0.0, + "angularVelocity": 1.0681551040319075, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.7956656485841047, + "pose": { + "rotation": { + "radians": 2.043451335941641 + }, + "translation": { + "x": 3.2075034240000004, + "y": 4.193024575999999 + } + }, + "velocity": -2.2464812353539987, + "acceleration": -2.99999999999996, + "curvature": -0.4753854333143056, + "holonomicRotation": 0.0, + "angularVelocity": 1.0715356512800862, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.801197921688225, + "pose": { + "rotation": { + "radians": 2.0493793638053237 + }, + "translation": { + "x": 3.2132480000000005, + "y": 4.181951999999999 + } + }, + "velocity": -2.2630780546663596, + "acceleration": -3.0, + "curvature": -0.4733924247442328, + "holonomicRotation": 0.0, + "angularVelocity": 1.0748752201213205, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.8066935092549423, + "pose": { + "rotation": { + "radians": 2.055286434700795 + }, + "translation": { + "x": 3.2190616960000002, + "y": 4.170906303999999 + } + }, + "velocity": -2.279564817366512, + "acceleration": -3.00000000000002, + "curvature": -0.47143436459980015, + "holonomicRotation": 0.0, + "angularVelocity": 1.0781776480141334, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.8121531647219413, + "pose": { + "rotation": { + "radians": 2.0611729131911716 + }, + "translation": { + "x": 3.2249441280000006, + "y": 4.159887872 + } + }, + "velocity": -2.295943783767509, + "acceleration": -3.0, + "curvature": -0.46951139935295344, + "holonomicRotation": 0.0, + "angularVelocity": 1.0814466727704475, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.817577613500027, + "pose": { + "rotation": { + "radians": 2.0670391652738456 + }, + "translation": { + "x": 3.230894912, + "y": 4.148897087999999 + } + }, + "velocity": -2.312217130101765, + "acceleration": -3.000000000000041, + "curvature": -0.4676236637363229, + "holonomicRotation": 0.0, + "angularVelocity": 1.084685936631654, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.822967554325104, + "pose": { + "rotation": { + "radians": 2.0728855582860835 + }, + "translation": { + "x": 3.2369136640000002, + "y": 4.137934335999999 + } + }, + "velocity": -2.328386952576996, + "acceleration": -3.0, + "curvature": -0.46577128117650907, + "holonomicRotation": 0.0, + "angularVelocity": 1.087898989429441, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.8283236605289246, + "pose": { + "rotation": { + "radians": 2.078712460812497 + }, + "translation": { + "x": 3.2430000000000003, + "y": 4.127 + } + }, + "velocity": -2.3444552711884588, + "acceleration": -3.000000000000021, + "curvature": -0.46395436448481275, + "holonomicRotation": 0.0, + "angularVelocity": 1.0910892923568585, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.833646581234512, + "pose": { + "rotation": { + "radians": 2.084520242598428 + }, + "translation": { + "x": 3.249153536, + "y": 4.116094464 + } + }, + "velocity": -2.360424033305221, + "acceleration": -3.0, + "curvature": -0.4621730162899535, + "holonomicRotation": 0.0, + "angularVelocity": 1.0942602211565104, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.838936942481669, + "pose": { + "rotation": { + "radians": 2.0903092744667395 + }, + "translation": { + "x": 3.2553738880000003, + "y": 4.105218111999999 + } + }, + "velocity": -2.376295117046691, + "acceleration": -2.999999999999937, + "curvature": -0.46042732956063537, + "holonomicRotation": 0.0, + "angularVelocity": 1.0974150691660196, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.8441953482875366, + "pose": { + "rotation": { + "radians": 2.096079928237889 + }, + "translation": { + "x": 3.2616606720000005, + "y": 4.094371327999999 + } + }, + "velocity": -2.392070334464294, + "acceleration": -3.0000000000000213, + "curvature": -0.45871738814240054, + "holonomicRotation": 0.0, + "angularVelocity": 1.1005570504780544, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.849422381646745, + "pose": { + "rotation": { + "radians": 2.1018325766544494 + }, + "translation": { + "x": 3.2680135040000002, + "y": 4.083554496 + } + }, + "velocity": -2.4077514345419186, + "acceleration": -2.9999999999999787, + "curvature": -0.45704326726309885, + "holonomicRotation": 0.0, + "angularVelocity": 1.1036893028426855, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.8546186054753275, + "pose": { + "rotation": { + "radians": 2.1075675933092324 + }, + "translation": { + "x": 3.2744320000000005, + "y": 4.072767999999999 + } + }, + "velocity": -2.4233401060276667, + "acceleration": -3.0000000000000213, + "curvature": -0.4554050339602513, + "holonomicRotation": 0.0, + "angularVelocity": 1.1068148903936257, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.859784563502238, + "pose": { + "rotation": { + "radians": 2.1132853525765656 + }, + "translation": { + "x": 3.2809157760000005, + "y": 4.062012223999999 + } + }, + "velocity": -2.438837980108399, + "acceleration": -3.0000000000000213, + "curvature": -0.4538027475367536, + "holonomicRotation": 0.0, + "angularVelocity": 1.1099368064092427, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.8649207811120037, + "pose": { + "rotation": { + "radians": 2.118986229547372 + }, + "translation": { + "x": 3.2874644480000006, + "y": 4.051287551999999 + } + }, + "velocity": -2.454246632937696, + "acceleration": -3.0000000000000218, + "curvature": -0.45223646007658297, + "holonomicRotation": 0.0, + "angularVelocity": 1.113057975924393, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.87002776614176, + "pose": { + "rotation": { + "radians": 2.1246705999676685 + }, + "translation": { + "x": 3.2940776320000005, + "y": 4.040594367999999 + } + }, + "velocity": -2.469567588026965, + "acceleration": -3.0000000000000435, + "curvature": -0.4507062168354121, + "holonomicRotation": 0.0, + "angularVelocity": 1.1161812581885475, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.8751060096356733, + "pose": { + "rotation": { + "radians": 2.1303388401800927 + }, + "translation": { + "x": 3.3007549440000004, + "y": 4.029933055999999 + } + }, + "velocity": -2.484802318508706, + "acceleration": -3.0000000000000657, + "curvature": -0.4492120566748173, + "holonomicRotation": 0.0, + "angularVelocity": 1.119309449123158, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.880155986559517, + "pose": { + "rotation": { + "radians": 2.1359913270688047 + }, + "translation": { + "x": 3.3074960000000004, + "y": 4.019303999999999 + } + }, + "velocity": -2.499952249280237, + "acceleration": -3.000000000000066, + "curvature": -0.44775401243225865, + "holonomicRotation": 0.0, + "angularVelocity": 1.1224452836459866, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.8851781564779557, + "pose": { + "rotation": { + "radians": 2.141628438007425 + }, + "translation": { + "x": 3.3143004160000005, + "y": 4.008707584 + } + }, + "velocity": -2.5150187590355526, + "acceleration": -2.999999999999978, + "curvature": -0.4463321113345515, + "holonomicRotation": 0.0, + "angularVelocity": 1.1255914379109953, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.8901729641969114, + "pose": { + "rotation": { + "radians": 2.1472505508098934 + }, + "translation": { + "x": 3.3211678080000007, + "y": 3.9981441919999994 + } + }, + "velocity": -2.5300031821924205, + "acceleration": -3.0000000000000444, + "curvature": -0.4449463754557567, + "holonomicRotation": 0.0, + "angularVelocity": 1.1287505315403796, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.8951408403731946, + "pose": { + "rotation": { + "radians": 2.1528580436844997 + }, + "translation": { + "x": 3.3280977920000003, + "y": 3.987614207999999 + } + }, + "velocity": -2.54490681072127, + "acceleration": -2.999999999999978, + "curvature": -0.44359682198548334, + "holonomicRotation": 0.0, + "angularVelocity": 1.1319251296711867, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.900082202093434, + "pose": { + "rotation": { + "radians": 2.1584512951904333 + }, + "translation": { + "x": 3.3350899840000006, + "y": 3.977118015999999 + } + }, + "velocity": -2.559730895881986, + "acceleration": -2.9999999999999325, + "curvature": -0.44228346357140413, + "holonomicRotation": 0.0, + "angularVelocity": 1.1351177449810737, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.904997453424183, + "pose": { + "rotation": { + "radians": 2.1640306841970087 + }, + "translation": { + "x": 3.3421440000000007, + "y": 3.9666559999999995 + } + }, + "velocity": -2.574476649874234, + "acceleration": -3.0000000000000453, + "curvature": -0.44100630878383207, + "holonomicRotation": 0.0, + "angularVelocity": 1.1383308397188256, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.9098869859349623, + "pose": { + "rotation": { + "radians": 2.1695965898458365 + }, + "translation": { + "x": 3.3492594560000004, + "y": 3.956228543999999 + } + }, + "velocity": -2.589145247406572, + "acceleration": -3.0000000000000453, + "curvature": -0.43976536240568537, + "holonomicRotation": 0.0, + "angularVelocity": 1.1415668276352418, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.9147511791958403, + "pose": { + "rotation": { + "radians": 2.175149391515662 + }, + "translation": { + "x": 3.356435968000001, + "y": 3.9458360319999994 + } + }, + "velocity": -2.603737827189206, + "acceleration": -2.9999999999999543, + "curvature": -0.43856062556178693, + "holonomicRotation": 0.0, + "angularVelocity": 1.1448280756029832, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.919590401251085, + "pose": { + "rotation": { + "radians": 2.180689468788583 + }, + "translation": { + "x": 3.3636731520000005, + "y": 3.9354788479999994 + } + }, + "velocity": -2.6182554933549405, + "acceleration": -3.0000000000000457, + "curvature": -0.43739209639321713, + "holonomicRotation": 0.0, + "angularVelocity": 1.145850588652444, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.9244145316277996, + "pose": { + "rotation": { + "radians": 2.1862172014204777 + }, + "translation": { + "x": 3.3709706240000004, + "y": 3.925157375999999 + } + }, + "velocity": -2.622334232261645, + "acceleration": -0.8454868729070896, + "curvature": -0.4362597700073285, + "holonomicRotation": 0.0, + "angularVelocity": 1.1445003863930865, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.9292338990626856, + "pose": { + "rotation": { + "radians": 2.1917329693118752 + }, + "translation": { + "x": 3.3783280000000007, + "y": 3.914871999999999 + } + }, + "velocity": -2.625634845989314, + "acceleration": -0.6848645122545891, + "curvature": -0.43516363895714666, + "holonomicRotation": 0.0, + "angularVelocity": 1.1430465952170605, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.934049261090149, + "pose": { + "rotation": { + "radians": 2.1972371524821046 + }, + "translation": { + "x": 3.3857448960000003, + "y": 3.9046231039999992 + } + }, + "velocity": -2.6288383813504206, + "acceleration": -0.6652740422913254, + "curvature": -0.4341036934661007, + "holonomicRotation": 0.0, + "angularVelocity": 1.141638532462789, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.9388607477448287, + "pose": { + "rotation": { + "radians": 2.2027301310455174 + }, + "translation": { + "x": 3.3932209280000003, + "y": 3.894411071999999 + } + }, + "velocity": -2.6319437458571184, + "acceleration": -0.6454064470235114, + "curvature": -0.4330799216330578, + "holonomicRotation": 0.0, + "angularVelocity": 1.1402763514325127, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.9436684894267646, + "pose": { + "rotation": { + "radians": 2.2082122851892256 + }, + "translation": { + "x": 3.4007557120000005, + "y": 3.8842362879999994 + } + }, + "velocity": -2.6349498803219897, + "acceleration": -0.6252695472733159, + "curvature": -0.4320923098070697, + "holonomicRotation": 0.0, + "angularVelocity": 1.138960198162488, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.9484726168868667, + "pose": { + "rotation": { + "radians": 2.213683995153181 + }, + "translation": { + "x": 3.4083488640000006, + "y": 3.874099135999999 + } + }, + "velocity": -2.6378557595292262, + "acceleration": -0.6048713801558966, + "curvature": -0.4311408427216499, + "holonomicRotation": 0.0, + "angularVelocity": 1.1376902117009824, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.953273261212499, + "pose": { + "rotation": { + "radians": 2.219145641212311 + }, + "translation": { + "x": 3.4160000000000004, + "y": 3.863999999999999 + } + }, + "velocity": -2.640660392350118, + "acceleration": -0.584220081857935, + "curvature": -0.4302255037988389, + "holonomicRotation": 0.0, + "angularVelocity": 1.1364665242199663, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.9580705538132235, + "pose": { + "rotation": { + "radians": 2.224597603659922 + }, + "translation": { + "x": 3.4237087360000005, + "y": 3.853939263999999 + } + }, + "velocity": -2.6433628223610883, + "acceleration": -0.5633239904028693, + "curvature": -0.4293462752752821, + "holonomicRotation": 0.0, + "angularVelocity": 1.135289261297001, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.9628646264067062, + "pose": { + "rotation": { + "radians": 2.2300402627931817 + }, + "translation": { + "x": 3.4314746880000007, + "y": 3.8439173119999994 + } + }, + "velocity": -2.6459621279004737, + "acceleration": -0.5421915268698363, + "curvature": -0.4285031384986032, + "holonomicRotation": 0.0, + "angularVelocity": 1.1341585420547766, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.967655611005099, + "pose": { + "rotation": { + "radians": 2.235473998900302 + }, + "translation": { + "x": 3.4392974720000007, + "y": 3.8339345279999995 + } + }, + "velocity": -2.648457422337088, + "acceleration": -0.5208312373726626, + "curvature": -0.42769607414071226, + "holonomicRotation": 0.0, + "angularVelocity": 1.133074479392834, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.9724436399012846, + "pose": { + "rotation": { + "radians": 2.240899192249165 + }, + "translation": { + "x": 3.4471767040000008, + "y": 3.823991295999999 + } + }, + "velocity": -2.6508478547544363, + "acceleration": -0.49925187779313374, + "curvature": -0.42692506226405785, + "holonomicRotation": 0.0, + "angularVelocity": 1.1320371801254954, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.97722884565546, + "pose": { + "rotation": { + "radians": 2.246316223077442 + }, + "translation": { + "x": 3.4551120000000006, + "y": 3.814087999999999 + } + }, + "velocity": -2.653132609673996, + "acceleration": -0.47746221101694586, + "curvature": -0.4261900826856946, + "holonomicRotation": 0.0, + "angularVelocity": 1.1310467451481108, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.9820113610819488, + "pose": { + "rotation": { + "radians": 2.2517254715841926 + }, + "translation": { + "x": 3.463102976000001, + "y": 3.804225023999999 + } + }, + "velocity": -2.655310907833249, + "acceleration": -0.4554712248680483, + "curvature": -0.42549111498766573, + "holonomicRotation": 0.0, + "angularVelocity": 1.1301032696261668, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.9867913192359405, + "pose": { + "rotation": { + "radians": 2.257127317922695 + }, + "translation": { + "x": 3.4711492480000006, + "y": 3.794402751999999 + } + }, + "velocity": -2.657382006198557, + "acceleration": -0.4332879700166909, + "curvature": -0.42482813876175346, + "holonomicRotation": 0.0, + "angularVelocity": 1.1292068432180316, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.991568853400548, + "pose": { + "rotation": { + "radians": 2.2625221421950776 + }, + "translation": { + "x": 3.4792504320000006, + "y": 3.784621567999999 + } + }, + "velocity": -2.6593451982965712, + "acceleration": -0.41092162407919103, + "curvature": -0.424201133738781, + "holonomicRotation": 0.0, + "angularVelocity": 1.1283575500272023, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.9963440970740116, + "pose": { + "rotation": { + "radians": 2.26791032444725 + }, + "translation": { + "x": 3.487406144000001, + "y": 3.774881855999999 + } + }, + "velocity": -2.6611998143698017, + "acceleration": -0.38838145235114474, + "curvature": -0.42361007996169614, + "holonomicRotation": 0.0, + "angularVelocity": 1.1275554689254084, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.001117183957077, + "pose": { + "rotation": { + "radians": 2.2732922446659067 + }, + "translation": { + "x": 3.495616000000001, + "y": 3.765183999999999 + } + }, + "velocity": -2.6629452216557663, + "acceleration": -0.3656768311000486, + "curvature": -0.42305495790658737, + "holonomicRotation": 0.0, + "angularVelocity": 1.1268006736114815, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.0058882479402897, + "pose": { + "rotation": { + "radians": 2.2786682827760343 + }, + "translation": { + "x": 3.5038796160000008, + "y": 3.755528383999999 + } + }, + "velocity": -2.6645808247919645, + "acceleration": -0.34281727135773626, + "curvature": -0.42253574855162407, + "holonomicRotation": 0.0, + "angularVelocity": 1.1260932327968134, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.010657423091539, + "pose": { + "rotation": { + "radians": 2.2840388186398783 + }, + "translation": { + "x": 3.512196608000001, + "y": 3.7459153919999992 + } + }, + "velocity": -2.6661060656923015, + "acceleration": -0.3198123054754257, + "curvature": -0.4220524336016502, + "holonomicRotation": 0.0, + "angularVelocity": 1.1254332101236695, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.0154248436438675, + "pose": { + "rotation": { + "radians": 2.2894042320560954 + }, + "translation": { + "x": 3.5205665920000007, + "y": 3.736345407999999 + } + }, + "velocity": -2.6675204238998145, + "acceleration": -0.29667158413832084, + "curvature": -0.4216049955488846, + "holonomicRotation": 0.0, + "angularVelocity": 1.1248206645718342, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.0201906439832062, + "pose": { + "rotation": { + "radians": 2.294764902761007 + }, + "translation": { + "x": 3.5289891840000007, + "y": 3.7268188159999993 + } + }, + "velocity": -2.6688234168146447, + "acceleration": -0.27340484746599136, + "curvature": -0.4211934177613658, + "holonomicRotation": 0.0, + "angularVelocity": 1.1242556502653611, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.0249549586362185, + "pose": { + "rotation": { + "radians": 2.300121210429298 + }, + "translation": { + "x": 3.5374640000000004, + "y": 3.717335999999999 + } + }, + "velocity": -2.6700145997957936, + "acceleration": -0.2500218956772268, + "curvature": -0.4208176845991983, + "holonomicRotation": 0.0, + "angularVelocity": 1.123738216844328, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.029717922258233, + "pose": { + "rotation": { + "radians": 2.3054735346767954 + }, + "translation": { + "x": 3.5459906560000007, + "y": 3.707897343999999 + } + }, + "velocity": -2.671093566389595, + "acceleration": -0.22653261276539288, + "curvature": -0.4204777814786742, + "holonomicRotation": 0.0, + "angularVelocity": 1.1232684092818601, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.034479669621233, + "pose": { + "rotation": { + "radians": 2.310822255062634 + }, + "translation": { + "x": 3.5545687680000007, + "y": 3.6985032319999993 + } + }, + "velocity": -2.6720599484322793, + "acceleration": -0.20294693712507567, + "curvature": -0.42017369496416096, + "holonomicRotation": 0.0, + "angularVelocity": 1.1228462681895641, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.0392403356020856, + "pose": { + "rotation": { + "radians": 2.3161677510933316 + }, + "translation": { + "x": 3.563197952000001, + "y": 3.689154047999999 + } + }, + "velocity": -2.672913416024821, + "acceleration": -0.17927483170934566, + "curvature": -0.4199054128880666, + "holonomicRotation": 0.0, + "angularVelocity": 1.1224718297169374, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.044000055170769, + "pose": { + "rotation": { + "radians": 2.321510402226531 + }, + "translation": { + "x": 3.571877824000001, + "y": 3.679850175999999 + } + }, + "velocity": -2.6736536778913673, + "acceleration": -0.15552636155641372, + "curvature": -0.4196729243382383, + "holonomicRotation": 0.0, + "angularVelocity": 1.122145125703422, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.048758963378458, + "pose": { + "rotation": { + "radians": 2.3268505878754597 + }, + "translation": { + "x": 3.5806080000000007, + "y": 3.670591999999999 + } + }, + "velocity": -2.6742804814591006, + "acceleration": -0.13171163224381718, + "curvature": -0.41947621972129306, + "holonomicRotation": 0.0, + "angularVelocity": 1.1218661837351616, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.053517195345991, + "pose": { + "rotation": { + "radians": 2.3321886874142024 + }, + "translation": { + "x": 3.5893880960000004, + "y": 3.661379903999999 + } + }, + "velocity": -2.674793612605643, + "acceleration": -0.10784071689727757, + "curvature": -0.419315290918276, + "holonomicRotation": 0.0, + "angularVelocity": 1.1216350272072997, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.0582748862519926, + "pose": { + "rotation": { + "radians": 2.337525080182999 + }, + "translation": { + "x": 3.598217728000001, + "y": 3.6522142719999993 + } + }, + "velocity": -2.6751928965083205, + "acceleration": -0.08392388462520017, + "curvature": -0.4191901310828717, + "holonomicRotation": 0.0, + "angularVelocity": 1.1214516752368215, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.06303217132131, + "pose": { + "rotation": { + "radians": 2.342860145493564 + }, + "translation": { + "x": 3.607096512000001, + "y": 3.6430954879999993 + } + }, + "velocity": -2.6754781966265213, + "acceleration": -0.05997120501374331, + "curvature": -0.4191007350133541, + "holonomicRotation": 0.0, + "angularVelocity": 1.1213161429216323, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.067789185813631, + "pose": { + "rotation": { + "radians": 2.348194262635916 + }, + "translation": { + "x": 3.616024064000001, + "y": 3.6340239359999993 + } + }, + "velocity": -2.6756494156563497, + "acceleration": -0.03599295947170036, + "curvature": -0.4190470988943057, + "holonomicRotation": 0.0, + "angularVelocity": 1.121228441093887, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.072546065011709, + "pose": { + "rotation": { + "radians": 2.353527810883649 + }, + "translation": { + "x": 3.625000000000001, + "y": 3.624999999999999 + } + }, + "velocity": -2.675706495129876, + "acceleration": -0.011999353178753079, + "curvature": -0.41902922045158014, + "holonomicRotation": 0.0, + "angularVelocity": 1.1211885766455594, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.0773029442097872, + "pose": { + "rotation": { + "radians": 2.358861169501017 + }, + "translation": { + "x": 3.634023936000001, + "y": 3.616024063999999 + } + }, + "velocity": -2.67564941565635, + "acceleration": 0.011999353178659161, + "curvature": -0.4190470988943055, + "holonomicRotation": 0.0, + "angularVelocity": 1.1211965522473806, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.0820599587021085, + "pose": { + "rotation": { + "radians": 2.3641947177487985 + }, + "translation": { + "x": 3.643095488000001, + "y": 3.607096511999999 + } + }, + "velocity": -2.675478196626521, + "acceleration": 0.03599295947188707, + "curvature": -0.41910073501335415, + "holonomicRotation": 0.0, + "angularVelocity": 1.1212523665345682, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.0868172437714714, + "pose": { + "rotation": { + "radians": 2.369528834891101 + }, + "translation": { + "x": 3.652214272000001, + "y": 3.598217727999999 + } + }, + "velocity": -2.675192896457028, + "acceleration": 0.059971215794916974, + "curvature": -0.4191901310989462, + "holonomicRotation": 0.0, + "angularVelocity": 1.1213560140984302, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.0915749346774724, + "pose": { + "rotation": { + "radians": 2.374863900201767 + }, + "translation": { + "x": 3.661379904000001, + "y": 3.5893880959999995 + } + }, + "velocity": -2.674793612656831, + "acceleration": 0.08392386308529144, + "curvature": -0.4193152909022269, + "holonomicRotation": 0.0, + "angularVelocity": 1.121507485349945, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.096333166645051, + "pose": { + "rotation": { + "radians": 2.3802002929704376 + }, + "translation": { + "x": 3.670592000000001, + "y": 3.580607999999999 + } + }, + "velocity": -2.674280481356777, + "acceleration": 0.10784074915852378, + "curvature": -0.4194762197533931, + "holonomicRotation": 0.0, + "angularVelocity": 1.1217067667098233, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.101092074852831, + "pose": { + "rotation": { + "radians": 2.3855383925092557 + }, + "translation": { + "x": 3.679850176000001, + "y": 3.5718778239999995 + } + }, + "velocity": -2.6736536778913917, + "acceleration": 0.13171161073470328, + "curvature": -0.4196729243382305, + "holonomicRotation": 0.0, + "angularVelocity": 1.121953840313771, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.1058517944215147, + "pose": { + "rotation": { + "radians": 2.3908785781581567 + }, + "translation": { + "x": 3.689154048000001, + "y": 3.563197951999999 + } + }, + "velocity": -2.672913416024821, + "acceleration": 0.15552636156154168, + "curvature": -0.4199054128880666, + "holonomicRotation": 0.0, + "angularVelocity": 1.1222486842580777, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.1106124604023675, + "pose": { + "rotation": { + "radians": 2.3962212292913607 + }, + "translation": { + "x": 3.698503232000001, + "y": 3.5545687679999993 + } + }, + "velocity": -2.672059948432254, + "acceleration": 0.17927483171467115, + "curvature": -0.42017369496416906, + "holonomicRotation": 0.0, + "angularVelocity": 1.1225912723143032, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.1153742077653668, + "pose": { + "rotation": { + "radians": 2.40156672532203 + }, + "translation": { + "x": 3.707897344000001, + "y": 3.545990655999999 + } + }, + "velocity": -2.671093566389572, + "acceleration": 0.20294693712460463, + "curvature": -0.42047778147868153, + "holonomicRotation": 0.0, + "angularVelocity": 1.1229815741706488, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.1201371713873365, + "pose": { + "rotation": { + "radians": 2.4069154457079467 + }, + "translation": { + "x": 3.717336000000001, + "y": 3.5374639999999995 + } + }, + "velocity": -2.670014599846641, + "acceleration": 0.22653260208714474, + "curvature": -0.4208176845831703, + "holonomicRotation": 0.0, + "angularVelocity": 1.123419555016309, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.1249014860403035, + "pose": { + "rotation": { + "radians": 2.4122677699553403 + }, + "translation": { + "x": 3.726818816000001, + "y": 3.528989183999999 + } + }, + "velocity": -2.6688234168146687, + "acceleration": 0.2500219063471021, + "curvature": -0.4211934177613582, + "holonomicRotation": 0.0, + "angularVelocity": 1.1239051758329424, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.1296672863796418, + "pose": { + "rotation": { + "radians": 2.4176240776237092 + }, + "translation": { + "x": 3.736345408000001, + "y": 3.5205665919999993 + } + }, + "velocity": -2.6675204238998145, + "acceleration": 0.27340484747103594, + "curvature": -0.42160499554888453, + "holonomicRotation": 0.0, + "angularVelocity": 1.1244383930403559, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.1344347069320158, + "pose": { + "rotation": { + "radians": 2.4229847483285685 + }, + "translation": { + "x": 3.745915392000001, + "y": 3.512196607999999 + } + }, + "velocity": -2.666106065641778, + "acceleration": 0.2966715947331795, + "curvature": -0.42205243361764627, + "holonomicRotation": 0.0, + "angularVelocity": 1.1250191586770815, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.13920388208331, + "pose": { + "rotation": { + "radians": 2.428350161744861 + }, + "translation": { + "x": 3.755528384000001, + "y": 3.5038796159999994 + } + }, + "velocity": -2.6645808247919636, + "acceleration": 0.31981229487879587, + "curvature": -0.4225357485516243, + "holonomicRotation": 0.0, + "angularVelocity": 1.1256474200811772, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.143974946066523, + "pose": { + "rotation": { + "radians": 2.4337206976086065 + }, + "translation": { + "x": 3.765184000000001, + "y": 3.495615999999999 + } + }, + "velocity": -2.6629452216557663, + "acceleration": 0.3428172713575581, + "curvature": -0.4230549579065874, + "holonomicRotation": 0.0, + "angularVelocity": 1.1263231200018886, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.1487480329496336, + "pose": { + "rotation": { + "radians": 2.439096735718832 + }, + "translation": { + "x": 3.7748818560000013, + "y": 3.4874061439999995 + } + }, + "velocity": -2.661199814319626, + "acceleration": 0.3656768416088413, + "curvature": -0.42361007997767014, + "holonomicRotation": 0.0, + "angularVelocity": 1.1270461962946063, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.153523276623142, + "pose": { + "rotation": { + "radians": 2.4444786559374396 + }, + "translation": { + "x": 3.7846215680000013, + "y": 3.4792504319999993 + } + }, + "velocity": -2.659345198296571, + "acceleration": 0.3883814418400827, + "curvature": -0.42420113373878104, + "holonomicRotation": 0.0, + "angularVelocity": 1.1278165820431203, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.1583008107877943, + "pose": { + "rotation": { + "radians": 2.449866838189612 + }, + "translation": { + "x": 3.794402752000001, + "y": 3.4711492479999992 + } + }, + "velocity": -2.6573820061486866, + "acceleration": 0.4109216345137485, + "curvature": -0.42482813877769865, + "holonomicRotation": 0.0, + "angularVelocity": 1.1286342052597576, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.163080768941831, + "pose": { + "rotation": { + "radians": 2.455261662461968 + }, + "translation": { + "x": 3.8042250240000013, + "y": 3.463102975999999 + } + }, + "velocity": -2.6553109078332495, + "acceleration": 0.4332879595793174, + "curvature": -0.4254911149876656, + "holonomicRotation": 0.0, + "angularVelocity": 1.1294989888871036, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.1678632843683197, + "pose": { + "rotation": { + "radians": 2.4606635088005238 + }, + "translation": { + "x": 3.8140880000000013, + "y": 3.4551119999999993 + } + }, + "velocity": -2.6531326096739956, + "acceleration": 0.4554712248682446, + "curvature": -0.42619008268569475, + "holonomicRotation": 0.0, + "angularVelocity": 1.1304108505629107, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.172648490122495, + "pose": { + "rotation": { + "radians": 2.46607275730722 + }, + "translation": { + "x": 3.8239912960000013, + "y": 3.447176703999999 + } + }, + "velocity": -2.650847854754409, + "acceleration": 0.4774622110225031, + "curvature": -0.4269250622640667, + "holonomicRotation": 0.0, + "angularVelocity": 1.1313697026092349, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.1774365190186358, + "pose": { + "rotation": { + "radians": 2.471489788135531 + }, + "translation": { + "x": 3.8339345280000012, + "y": 3.4392974719999994 + } + }, + "velocity": -2.6484574223862847, + "acceleration": 0.49925186751720285, + "curvature": -0.4276960741248228, + "holonomicRotation": 0.0, + "angularVelocity": 1.1323754517286329, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.1822275036170287, + "pose": { + "rotation": { + "radians": 2.476914981484361 + }, + "translation": { + "x": 3.843917312000001, + "y": 3.431474687999999 + } + }, + "velocity": -2.6459621278514356, + "acceleration": 0.5208312578767472, + "curvature": -0.4285031385144863, + "holonomicRotation": 0.0, + "angularVelocity": 1.13342799909027, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.1870215762105563, + "pose": { + "rotation": { + "radians": 2.4823487175915364 + }, + "translation": { + "x": 3.853939264000001, + "y": 3.423708735999999 + } + }, + "velocity": -2.643362822361095, + "acceleration": 0.5421915166345206, + "curvature": -0.42934627527527985, + "holonomicRotation": 0.0, + "angularVelocity": 1.1345272398917918, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.1918188688112803, + "pose": { + "rotation": { + "radians": 2.4877913767247897 + }, + "translation": { + "x": 3.864000000000001, + "y": 3.415999999999999 + } + }, + "velocity": -2.6406603923501453, + "acceleration": 0.5633239903985849, + "curvature": -0.43022550379883, + "holonomicRotation": 0.0, + "angularVelocity": 1.1356730633904046, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.196619513136957, + "pose": { + "rotation": { + "radians": 2.4932433391723787 + }, + "translation": { + "x": 3.874099136000001, + "y": 3.4083488639999993 + } + }, + "velocity": -2.6378557594807597, + "acceleration": 0.5842200919540596, + "curvature": -0.43114084273749304, + "holonomicRotation": 0.0, + "angularVelocity": 1.1368653526407893, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.2014236405971026, + "pose": { + "rotation": { + "radians": 2.4987049852314884 + }, + "translation": { + "x": 3.884236288000001, + "y": 3.400755711999999 + } + }, + "velocity": -2.6349498803219897, + "acceleration": 0.6048713700618353, + "curvature": -0.43209230980706964, + "holonomicRotation": 0.0, + "angularVelocity": 1.1381039843727592, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.2062313822789954, + "pose": { + "rotation": { + "radians": 2.5041766951954854 + }, + "translation": { + "x": 3.8944110720000014, + "y": 3.393220927999999 + } + }, + "velocity": -2.6319437459051516, + "acceleration": 0.6252695372882013, + "curvature": -0.4330799216172504, + "holonomicRotation": 0.0, + "angularVelocity": 1.1393888286909821, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.2110428689336312, + "pose": { + "rotation": { + "radians": 2.509658849339173 + }, + "translation": { + "x": 3.9046231040000015, + "y": 3.385744895999999 + } + }, + "velocity": -2.6288383813504206, + "acceleration": 0.6454064570124066, + "curvature": -0.4341036934661007, + "holonomicRotation": 0.0, + "angularVelocity": 1.1407197490207739, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.2158582309610946, + "pose": { + "rotation": { + "radians": 2.515151827902585 + }, + "translation": { + "x": 3.9148720000000012, + "y": 3.3783279999999993 + } + }, + "velocity": -2.625634845989314, + "acceleration": 0.6652740422913407, + "curvature": -0.43516363895714666, + "holonomicRotation": 0.0, + "angularVelocity": 1.1413686790480242, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.220680672010397, + "pose": { + "rotation": { + "radians": 2.5206560110728145 + }, + "translation": { + "x": 3.9251573760000014, + "y": 3.370970623999999 + } + }, + "velocity": -2.618989404766512, + "acceleration": 1.3780243563087686, + "curvature": -0.4362597700073408, + "holonomicRotation": 0.0, + "angularVelocity": 1.1396328812896521, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.2255206235262626, + "pose": { + "rotation": { + "radians": 2.5261717789641427 + }, + "translation": { + "x": 3.9354788480000007, + "y": 3.3636731519999987 + } + }, + "velocity": -2.604469550218915, + "acceleration": 3.0, + "curvature": -0.437392096408939, + "holonomicRotation": 0.0, + "angularVelocity": 1.1362290451451276, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.2303856044520742, + "pose": { + "rotation": { + "radians": 2.531699511596127 + }, + "translation": { + "x": 3.945836032000001, + "y": 3.356435967999999 + } + }, + "velocity": -2.5898746074414802, + "acceleration": 2.9999999999999085, + "curvature": -0.4385606255617841, + "holonomicRotation": 0.0, + "angularVelocity": 1.1328524486334637, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.235275982964838, + "pose": { + "rotation": { + "radians": 2.537239588869056 + }, + "translation": { + "x": 3.956228544000001, + "y": 3.349259455999999 + } + }, + "velocity": -2.5752034719031878, + "acceleration": 3.0000000000000453, + "curvature": -0.4397653623900097, + "holonomicRotation": 0.0, + "angularVelocity": 1.1295007542704276, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.240192138752052, + "pose": { + "rotation": { + "radians": 2.5427923905388248 + }, + "translation": { + "x": 3.9666560000000013, + "y": 3.342143999999999 + } + }, + "velocity": -2.5604550045415473, + "acceleration": 3.0, + "curvature": -0.4410063087994853, + "holonomicRotation": 0.0, + "angularVelocity": 1.1261715790987796, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.2451344634849657, + "pose": { + "rotation": { + "radians": 2.5483582961877094 + }, + "translation": { + "x": 3.977118016000001, + "y": 3.3350899839999992 + } + }, + "velocity": -2.5456280303428054, + "acceleration": 3.000000000000045, + "curvature": -0.4422834635714009, + "holonomicRotation": 0.0, + "angularVelocity": 1.1228624931603801, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.2501033613157793, + "pose": { + "rotation": { + "radians": 2.553937685194276 + }, + "translation": { + "x": 3.9876142080000014, + "y": 3.328097791999999 + } + }, + "velocity": -2.5307213368503647, + "acceleration": 3.0, + "curvature": -0.4435968219542736, + "holonomicRotation": 0.0, + "angularVelocity": 1.1195710174568347, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.2550992494002617, + "pose": { + "rotation": { + "radians": 2.5595309367001207 + }, + "translation": { + "x": 3.9981441920000007, + "y": 3.321167807999999 + } + }, + "velocity": -2.5157336725969164, + "acceleration": 3.0000000000000444, + "curvature": -0.4449463754713487, + "holonomicRotation": 0.0, + "angularVelocity": 1.1162946221424517, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.260122558447441, + "pose": { + "rotation": { + "radians": 2.565138429574846 + }, + "translation": { + "x": 4.0087075840000015, + "y": 3.314300415999999 + } + }, + "velocity": -2.500663745455379, + "acceleration": 3.0000000000000884, + "curvature": -0.44633211131899053, + "holonomicRotation": 0.0, + "angularVelocity": 1.1130307242685513, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.2651737332980844, + "pose": { + "rotation": { + "radians": 2.570760542377265 + }, + "translation": { + "x": 4.019304000000001, + "y": 3.307495999999999 + } + }, + "velocity": -2.4855102209034485, + "acceleration": 3.000000000000044, + "curvature": -0.4477540124322387, + "holonomicRotation": 0.0, + "angularVelocity": 1.109776686086939, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.2702532335338788, + "pose": { + "rotation": { + "radians": 2.5763976533159223 + }, + "translation": { + "x": 4.029933056000002, + "y": 3.300754943999999 + } + }, + "velocity": -2.470271720196066, + "acceleration": 3.0000000000000435, + "curvature": -0.44921205667481756, + "holonomicRotation": 0.0, + "angularVelocity": 1.1065298124301608, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.2753615341193125, + "pose": { + "rotation": { + "radians": 2.5820501402045597 + }, + "translation": { + "x": 4.040594368000001, + "y": 3.294077631999999 + } + }, + "velocity": -2.4549468184397636, + "acceleration": 2.999999999999913, + "curvature": -0.4507062168508919, + "holonomicRotation": 0.0, + "angularVelocity": 1.103287348927679, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.2804991260784773, + "pose": { + "rotation": { + "radians": 2.5877183804170585 + }, + "translation": { + "x": 4.051287552000002, + "y": 3.2874644479999993 + } + }, + "velocity": -2.43953404256227, + "acceleration": 3.000000000000043, + "curvature": -0.45223646007658325, + "holonomicRotation": 0.0, + "angularVelocity": 1.1000464792593154, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.2856665172091235, + "pose": { + "rotation": { + "radians": 2.5934027508372814 + }, + "translation": { + "x": 4.062012224000001, + "y": 3.280915775999999 + } + }, + "velocity": -2.4240318691703315, + "acceleration": 3.000000000000043, + "curvature": -0.4538027475367534, + "holonomicRotation": 0.0, + "angularVelocity": 1.0968043231891658, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.290864232836559, + "pose": { + "rotation": { + "radians": 2.5991036278081605 + }, + "translation": { + "x": 4.072768000000002, + "y": 3.274431999999999 + } + }, + "velocity": -2.4084387222880252, + "acceleration": 3.0, + "curvature": -0.4554050339448751, + "holonomicRotation": 0.0, + "angularVelocity": 1.0935579336272918, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.2960928166101318, + "pose": { + "rotation": { + "radians": 2.6048213870753862 + }, + "translation": { + "x": 4.0835544960000005, + "y": 3.268013503999999 + } + }, + "velocity": -2.392752970967306, + "acceleration": 3.0000000000000426, + "curvature": -0.45704326729385686, + "holonomicRotation": 0.0, + "angularVelocity": 1.0903042944970165, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.3013528313453175, + "pose": { + "rotation": { + "radians": 2.6105564037302766 + }, + "translation": { + "x": 4.094371328000001, + "y": 3.261660671999999 + } + }, + "velocity": -2.3769729267617494, + "acceleration": 2.999999999999958, + "curvature": -0.45871738815772506, + "holonomicRotation": 0.0, + "angularVelocity": 1.087040317564224, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.30664485991464, + "pose": { + "rotation": { + "radians": 2.616309052146832 + }, + "translation": { + "x": 4.105218112000001, + "y": 3.2553738879999994 + } + }, + "velocity": -2.3610968410537816, + "acceleration": 3.0000000000000417, + "curvature": -0.4604273295453317, + "holonomicRotation": 0.0, + "angularVelocity": 1.0837628401271546, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.31196950619098, + "pose": { + "rotation": { + "radians": 2.622079705917951 + }, + "translation": { + "x": 4.1160944640000015, + "y": 3.2491535359999992 + } + }, + "velocity": -2.345122902224761, + "acceleration": 2.9999999999999165, + "curvature": -0.4621730162746951, + "holonomicRotation": 0.0, + "angularVelocity": 1.0804686217425308, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.3173273960470873, + "pose": { + "rotation": { + "radians": 2.627868737786227 + }, + "translation": { + "x": 4.127000000000001, + "y": 3.2429999999999994 + } + }, + "velocity": -2.3290492326564407, + "acceleration": 2.9999999999999587, + "curvature": -0.4639543644847909, + "holonomicRotation": 0.0, + "angularVelocity": 1.0771543414066895, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.3227191784154804, + "pose": { + "rotation": { + "radians": 2.633676519572262 + }, + "translation": { + "x": 4.137934336000002, + "y": 3.2369136639999994 + } + }, + "velocity": -2.312873885551261, + "acceleration": 2.9999999999999587, + "curvature": -0.4657712811612695, + "holonomicRotation": 0.0, + "angularVelocity": 1.0738165942689333, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.3281455264132758, + "pose": { + "rotation": { + "radians": 2.6395034220985725 + }, + "translation": { + "x": 4.148897088000001, + "y": 3.2308949119999992 + } + }, + "velocity": -2.2965948415578756, + "acceleration": 2.999999999999959, + "curvature": -0.46762366370596037, + "holonomicRotation": 0.0, + "angularVelocity": 1.0704518885397352, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.333607138536903, + "pose": { + "rotation": { + "radians": 2.645349815110781 + }, + "translation": { + "x": 4.159887872000001, + "y": 3.224944127999999 + } + }, + "velocity": -2.280210005186993, + "acceleration": 3.000000000000041, + "curvature": -0.46951139935295344, + "holonomicRotation": 0.0, + "angularVelocity": 1.0670566418128253, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.3391047399321145, + "pose": { + "rotation": { + "radians": 2.651216067193581 + }, + "translation": { + "x": 4.170906304000001, + "y": 3.2190616959999994 + } + }, + "velocity": -2.2637172010013575, + "acceleration": 3.0000000000000404, + "curvature": -0.4714343645997839, + "holonomicRotation": 0.0, + "angularVelocity": 1.0636271776952306, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.344639083745208, + "pose": { + "rotation": { + "radians": 2.6571025456838964 + }, + "translation": { + "x": 4.181952000000002, + "y": 3.213247999999999 + } + }, + "velocity": -2.2471141695620784, + "acceleration": 3.0, + "curvature": -0.4733924247442271, + "holonomicRotation": 0.0, + "angularVelocity": 1.0601597219448258, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.350210952561919, + "pose": { + "rotation": { + "radians": 2.663009616579334 + }, + "translation": { + "x": 4.193024576000001, + "y": 3.207503423999999 + } + }, + "velocity": -2.2303985631119447, + "acceleration": 3.0, + "curvature": -0.47538543332938343, + "holonomicRotation": 0.0, + "angularVelocity": 1.056650398648511, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.3558211599411023, + "pose": { + "rotation": { + "radians": 2.668937644443049 + }, + "translation": { + "x": 4.204123648000001, + "y": 3.2018283519999993 + } + }, + "velocity": -2.213567940974395, + "acceleration": 2.9999999999999605, + "curvature": -0.4774132314234743, + "holonomicRotation": 0.0, + "angularVelocity": 1.0530952259318735, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.361470552050947, + "pose": { + "rotation": { + "radians": 2.6748869923033434 + }, + "translation": { + "x": 4.215248832000001, + "y": 3.1962231679999995 + } + }, + "velocity": -2.1966197646448617, + "acceleration": 2.9999999999999214, + "curvature": -0.4794756472470108, + "holonomicRotation": 0.0, + "angularVelocity": 1.0494901118766216, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.3671600094163034, + "pose": { + "rotation": { + "radians": 2.680858021550229 + }, + "translation": { + "x": 4.226399744000002, + "y": 3.190688255999999 + } + }, + "velocity": -2.1795513925487917, + "acceleration": 2.999999999999961, + "curvature": -0.48157249539387187, + "holonomicRotation": 0.0, + "angularVelocity": 1.0458308498079987, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.37289044878651, + "pose": { + "rotation": { + "radians": 2.6868510918265454 + }, + "translation": { + "x": 4.237576000000001, + "y": 3.185223999999999 + } + }, + "velocity": -2.1623600744381717, + "acceleration": 3.0, + "curvature": -0.4837035763394283, + "holonomicRotation": 0.0, + "angularVelocity": 1.0421131136385364, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.378662825134093, + "pose": { + "rotation": { + "radians": 2.692866560915218 + }, + "translation": { + "x": 4.2487772160000015, + "y": 3.179830783999999 + } + }, + "velocity": -2.1450429453954247, + "acceleration": 3.0, + "curvature": -0.4858686756511655, + "holonomicRotation": 0.0, + "angularVelocity": 1.0383324526569788, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.3844781337957577, + "pose": { + "rotation": { + "radians": 2.698904784620842 + }, + "translation": { + "x": 4.260003008000001, + "y": 3.174508991999999 + } + }, + "velocity": -2.1275970194104294, + "acceleration": 3.000000000000038, + "curvature": -0.48806756354488817, + "holonomicRotation": 0.0, + "angularVelocity": 1.0344842865367634, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.390337412768339, + "pose": { + "rotation": { + "radians": 2.7049661166484125 + }, + "translation": { + "x": 4.271252992000002, + "y": 3.169259007999999 + } + }, + "velocity": -2.1100191824926853, + "acceleration": 3.0000000000000377, + "curvature": -0.49029999405552943, + "holonomicRotation": 0.0, + "angularVelocity": 1.0305638993092536, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.3962417451736586, + "pose": { + "rotation": { + "radians": 2.7110509084748564 + }, + "translation": { + "x": 4.282526784000001, + "y": 3.164081215999999 + } + }, + "velocity": -2.0923061852767266, + "acceleration": 3.000000000000075, + "curvature": -0.49256570453850973, + "holonomicRotation": 0.0, + "angularVelocity": 1.0265664338405436, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.4021922619078344, + "pose": { + "rotation": { + "radians": 2.7171595092181677 + }, + "translation": { + "x": 4.293824000000002, + "y": 3.158975999999999 + } + }, + "velocity": -2.074454635074199, + "acceleration": 3.0000000000000746, + "curvature": -0.4948644147448579, + "holonomicRotation": 0.0, + "angularVelocity": 1.0224868851114455, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.408190144492236, + "pose": { + "rotation": { + "radians": 2.7232922654991567 + }, + "translation": { + "x": 4.305144256000001, + "y": 3.1539437439999993 + } + }, + "velocity": -2.0564609873209942, + "acceleration": 2.999999999999963, + "curvature": -0.49719582643303056, + "holonomicRotation": 0.0, + "angularVelocity": 1.0183200938587098, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.4142366281452663, + "pose": { + "rotation": { + "radians": 2.729449521300226 + }, + "translation": { + "x": 4.316487168000002, + "y": 3.148984831999999 + } + }, + "velocity": -2.0383215363619027, + "acceleration": 3.0, + "curvature": -0.49955962237973184, + "holonomicRotation": 0.0, + "angularVelocity": 1.0140607391716023, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.420333005096303, + "pose": { + "rotation": { + "radians": 2.7356316178174627 + }, + "translation": { + "x": 4.327852352000001, + "y": 3.144099647999999 + } + }, + "velocity": -2.0200324055087933, + "acceleration": 3.0, + "curvature": -0.5019554658535155, + "holonomicRotation": 0.0, + "angularVelocity": 1.0097033310569088, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.4264806281656552, + "pose": { + "rotation": { + "radians": 2.74183889330867 + }, + "translation": { + "x": 4.339239424000002, + "y": 3.1392885759999993 + } + }, + "velocity": -2.001589536300736, + "acceleration": 3.0, + "curvature": -0.5043829997560954, + "holonomicRotation": 0.0, + "angularVelocity": 1.0052422020712786, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.432680914637187, + "pose": { + "rotation": { + "radians": 2.7480716829347855 + }, + "translation": { + "x": 4.350648000000001, + "y": 3.1345519999999993 + } + }, + "velocity": -1.9829886768861407, + "acceleration": 2.999999999999964, + "curvature": -0.5068418460483195, + "holonomicRotation": 0.0, + "angularVelocity": 1.0006714988901686, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.438935350453517, + "pose": { + "rotation": { + "radians": 2.754330318597825 + }, + "translation": { + "x": 4.362077696000002, + "y": 3.129890303999999 + } + }, + "velocity": -1.9642253694371505, + "acceleration": 2.9999999999999645, + "curvature": -0.5093316047891755, + "holonomicRotation": 0.0, + "angularVelocity": 0.9959851725933427, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.445245494767346, + "pose": { + "rotation": { + "radians": 2.760615128771323 + }, + "translation": { + "x": 4.373528128000001, + "y": 3.125303871999999 + } + }, + "velocity": -1.9452949364956633, + "acceleration": 2.999999999999965, + "curvature": -0.5118518535624833, + "holonomicRotation": 0.0, + "angularVelocity": 0.9911769688340359, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.4516129848867214, + "pose": { + "rotation": { + "radians": 2.7669264383269256 + }, + "translation": { + "x": 4.384998912000002, + "y": 3.120793087999999 + } + }, + "velocity": -1.9261924661375385, + "acceleration": 3.000000000000035, + "curvature": -0.5144021466205787, + "holonomicRotation": 0.0, + "angularVelocity": 0.9862404167759408, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.4580395416568392, + "pose": { + "rotation": { + "radians": 2.773264568354321 + }, + "translation": { + "x": 4.3964896640000015, + "y": 3.1163583359999993 + } + }, + "velocity": -1.9069127958271843, + "acceleration": 3.0, + "curvature": -0.5169820141541447, + "holonomicRotation": 0.0, + "angularVelocity": 0.9811688175075026, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.464526975326608, + "pose": { + "rotation": { + "radians": 2.7796298359767464 + }, + "translation": { + "x": 4.408000000000002, + "y": 3.111999999999999 + } + }, + "velocity": -1.8874504948178785, + "acceleration": 2.999999999999966, + "curvature": -0.5195909614569449, + "holonomicRotation": 0.0, + "angularVelocity": 0.9759552310949895, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.4710771919545538, + "pose": { + "rotation": { + "radians": 2.7860225541595955 + }, + "translation": { + "x": 4.4195295360000015, + "y": 3.1077184639999995 + } + }, + "velocity": -1.867799844934041, + "acceleration": 3.0, + "curvature": -0.5222284682600655, + "holonomicRotation": 0.0, + "angularVelocity": 0.9705924628872172, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.4776922004161537, + "pose": { + "rotation": { + "radians": 2.7924430315143596 + }, + "translation": { + "x": 4.431077888000002, + "y": 3.103514111999999 + } + }, + "velocity": -1.847954819549241, + "acceleration": 3.0000000000000338, + "curvature": -0.5248939878051837, + "holonomicRotation": 0.0, + "angularVelocity": 0.9650730483865126, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.4843741200832428, + "pose": { + "rotation": { + "radians": 2.798891572096551 + }, + "translation": { + "x": 4.442644672000001, + "y": 3.0993873279999993 + } + }, + "velocity": -1.8279090605479742, + "acceleration": 3.0000000000000333, + "curvature": -0.527586946160825, + "holonomicRotation": 0.0, + "angularVelocity": 0.9593892367629828, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.4911251892562345, + "pose": { + "rotation": { + "radians": 2.8053684751977617 + }, + "translation": { + "x": 4.454229504000002, + "y": 3.095338495999999 + } + }, + "velocity": -1.8076558530289983, + "acceleration": 3.0, + "curvature": -0.5303067413665304, + "holonomicRotation": 0.0, + "angularVelocity": 0.9535329728703047, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.4979477744415766, + "pose": { + "rotation": { + "radians": 2.8118740351322016 + }, + "translation": { + "x": 4.465832000000002, + "y": 3.0913679999999997 + } + }, + "velocity": -1.787188097472973, + "acceleration": 2.9999999999999676, + "curvature": -0.533052742734141, + "holonomicRotation": 0.0, + "angularVelocity": 0.9474958775078796, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.504844380580635, + "pose": { + "rotation": { + "radians": 2.818408541017755 + }, + "translation": { + "x": 4.477451776000002, + "y": 3.0874762239999995 + } + }, + "velocity": -1.766498279055798, + "acceleration": 3.000000000000032, + "curvature": -0.5358242899805569, + "holonomicRotation": 0.0, + "angularVelocity": 0.9412692255924409, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.5118176623523034, + "pose": { + "rotation": { + "radians": 2.8249722765508114 + }, + "translation": { + "x": 4.489088448000001, + "y": 3.0836635519999995 + } + }, + "velocity": -1.7455784337407918, + "acceleration": 2.999999999999968, + "curvature": -0.53862069246313, + "holonomicRotation": 0.0, + "angularVelocity": 0.9348439222378784, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.518870436690729, + "pose": { + "rotation": { + "radians": 2.831565519776004 + }, + "translation": { + "x": 4.500741632000002, + "y": 3.0799303679999994 + } + }, + "velocity": -1.724420110725515, + "acceleration": 2.9999999999999685, + "curvature": -0.5414412283907459, + "holonomicRotation": 0.0, + "angularVelocity": 0.9282104760898124, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.526005696682038, + "pose": { + "rotation": { + "radians": 2.8381885428495615 + }, + "translation": { + "x": 4.512410944000001, + "y": 3.0762770559999995 + } + }, + "velocity": -1.7030143307515877, + "acceleration": 3.000000000000031, + "curvature": -0.5442851441519826, + "holonomicRotation": 0.0, + "angularVelocity": 0.9213589699954078, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.533226627030828, + "pose": { + "rotation": { + "radians": 2.844841611798131 + }, + "translation": { + "x": 4.524096000000002, + "y": 3.0727039999999994 + } + }, + "velocity": -1.6813515397052183, + "acceleration": 3.0, + "curvature": -0.5471516533835759, + "holonomicRotation": 0.0, + "angularVelocity": 0.9142790279993414, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.5405366213191094, + "pose": { + "rotation": { + "radians": 2.851524986270702 + }, + "translation": { + "x": 4.535796416000001, + "y": 3.0692115839999996 + } + }, + "velocity": -1.6594215568403732, + "acceleration": 3.0, + "curvature": -0.5500399363964437, + "holonomicRotation": 0.0, + "angularVelocity": 0.9069597788904417, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.5479393013188334, + "pose": { + "rotation": { + "radians": 2.858238919286448 + }, + "translation": { + "x": 4.547511808000002, + "y": 3.0658001919999998 + } + }, + "velocity": -1.6372135168412023, + "acceleration": 3.00000000000003, + "curvature": -0.5529491392614659, + "holonomicRotation": 0.0, + "angularVelocity": 0.8993898149607107, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.555438538665241, + "pose": { + "rotation": { + "radians": 2.86498365697578 + }, + "translation": { + "x": 4.559241792000002, + "y": 3.0624702079999992 + } + }, + "velocity": -1.6147158048019792, + "acceleration": 2.9999999999999702, + "curvature": -0.5558783732812413, + "holonomicRotation": 0.0, + "angularVelocity": 0.8915571460679171, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.563038479254261, + "pose": { + "rotation": { + "radians": 2.8717594383176124 + }, + "translation": { + "x": 4.570985984000002, + "y": 3.0592220159999997 + } + }, + "velocity": -1.5919159830349192, + "acceleration": 3.0000000000000293, + "curvature": -0.5588267141256481, + "holonomicRotation": 0.0, + "angularVelocity": 0.883449147335363, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.5707435707950577, + "pose": { + "rotation": { + "radians": 2.87856649486947 + }, + "translation": { + "x": 4.582744000000002, + "y": 3.0560559999999994 + } + }, + "velocity": -1.5688007084125297, + "acceleration": 2.999999999999971, + "curvature": -0.5617932012303778, + "holonomicRotation": 0.0, + "angularVelocity": 0.8750525005400022, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.578558594031973, + "pose": { + "rotation": { + "radians": 2.885405050494711 + }, + "translation": { + "x": 4.594515456000002, + "y": 3.0529725439999993 + } + }, + "velocity": -1.545355638701783, + "acceleration": 3.000000000000057, + "curvature": -0.5647768371119463, + "holonomicRotation": 0.0, + "angularVelocity": 0.866353126952402, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.586488698252195, + "pose": { + "rotation": { + "radians": 2.892275321082959 + }, + "translation": { + "x": 4.606299968000002, + "y": 3.0499720319999994 + } + }, + "velocity": -1.5215653260411162, + "acceleration": 3.000000000000028, + "curvature": -0.5677765867261405, + "holonomicRotation": 0.0, + "angularVelocity": 0.857336111143583, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.594539441821749, + "pose": { + "rotation": { + "radians": 2.899177514266695 + }, + "translation": { + "x": 4.6180971520000025, + "y": 3.0470548479999997 + } + }, + "velocity": -1.4974130953324531, + "acceleration": 3.0000000000000275, + "curvature": -0.5707913768706009, + "holonomicRotation": 0.0, + "angularVelocity": 0.847985613554919, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.602716838649498, + "pose": { + "rotation": { + "radians": 2.9061118291329553 + }, + "translation": { + "x": 4.629906624000001, + "y": 3.0442213759999994 + } + }, + "velocity": -1.472880904849207, + "acceleration": 3.000000000000054, + "curvature": -0.5738200956456002, + "holonomicRotation": 0.0, + "angularVelocity": 0.8382847700393182, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.6110274116755643, + "pose": { + "rotation": { + "radians": 2.9130784559310063 + }, + "translation": { + "x": 4.641728000000002, + "y": 3.0414719999999993 + } + }, + "velocity": -1.4479491857710078, + "acceleration": 3.0, + "curvature": -0.576861591813262, + "holonomicRotation": 0.0, + "angularVelocity": 0.8282155756135112, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.6194782547286612, + "pose": { + "rotation": { + "radians": 2.9200775757746467 + }, + "translation": { + "x": 4.653560896000002, + "y": 3.0388071039999995 + } + }, + "velocity": -1.4225966566117165, + "acceleration": 2.999999999999974, + "curvature": -0.5799146744073892, + "holonomicRotation": 0.0, + "angularVelocity": 0.8177587498553298, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.6280771044120916, + "pose": { + "rotation": { + "radians": 2.9271093603419622 + }, + "translation": { + "x": 4.665404928000002, + "y": 3.0362270719999995 + } + }, + "velocity": -1.3968001075614265, + "acceleration": 3.0000000000000258, + "curvature": -0.5829781121894888, + "holonomicRotation": 0.0, + "angularVelocity": 0.8068935795491445, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.6368324240818266, + "pose": { + "rotation": { + "radians": 2.934173971570372 + }, + "translation": { + "x": 4.6772597120000015, + "y": 3.0337322879999995 + } + }, + "velocity": -1.37053414855222, + "acceleration": 3.0000000000000253, + "curvature": -0.5860506332508155, + "holonomicRotation": 0.0, + "angularVelocity": 0.7955977342736981, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.6457535025016057, + "pose": { + "rotation": { + "radians": 2.941271561348426 + }, + "translation": { + "x": 4.689124864000002, + "y": 3.0313231359999993 + } + }, + "velocity": -1.3437709132928843, + "acceleration": 2.999999999999975, + "curvature": -0.5891309246386679, + "holonomicRotation": 0.0, + "angularVelocity": 0.7838470488110904, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.654850570439635, + "pose": { + "rotation": { + "radians": 2.9484022712044844 + }, + "translation": { + "x": 4.701000000000001, + "y": 3.0289999999999995 + } + }, + "velocity": -1.316479709478796, + "acceleration": 2.9999999999999756, + "curvature": -0.5922176320623127, + "holonomicRotation": 0.0, + "angularVelocity": 0.7716152648033164, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.664134939366228, + "pose": { + "rotation": { + "radians": 2.955566231992309 + }, + "translation": { + "x": 4.712884736000002, + "y": 3.0267632639999995 + } + }, + "velocity": -1.288626602699017, + "acceleration": 3.000000000000024, + "curvature": -0.5953093595736683, + "holonomicRotation": 0.0, + "angularVelocity": 0.7588737220632854, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.673619167601797, + "pose": { + "rotation": { + "radians": 2.962763563574333 + }, + "translation": { + "x": 4.724778688000002, + "y": 3.0246133119999996 + } + }, + "velocity": -1.2601739179923108, + "acceleration": 3.0, + "curvature": -0.5984046693700557, + "holonomicRotation": 0.0, + "angularVelocity": 0.7455909869329629, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.683317260866003, + "pose": { + "rotation": { + "radians": 2.9699943745025603 + }, + "translation": { + "x": 4.736681472000003, + "y": 3.0225505279999996 + } + }, + "velocity": -1.2310796381996918, + "acceleration": 3.0, + "curvature": -0.6015020816536214, + "holonomicRotation": 0.0, + "angularVelocity": 0.731732401397394, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.6932449163600993, + "pose": { + "rotation": { + "radians": 2.9772587616975015 + }, + "translation": { + "x": 4.748592704000002, + "y": 3.0205752959999996 + } + }, + "velocity": -1.201296671717403, + "acceleration": 2.999999999999978, + "curvature": -0.60460007456127, + "holonomicRotation": 0.0, + "angularVelocity": 0.7172595313835017, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.703419822526085, + "pose": { + "rotation": { + "radians": 2.984556810125987 + }, + "translation": { + "x": 4.760512000000002, + "y": 3.0186879999999996 + } + }, + "velocity": -1.1707719532194472, + "acceleration": 3.0, + "curvature": -0.6076970841012796, + "holonomicRotation": 0.0, + "angularVelocity": 0.7021294852003142, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.7138620308453802, + "pose": { + "rotation": { + "radians": 2.9918885924775687 + }, + "translation": { + "x": 4.772438976000002, + "y": 3.0168890239999997 + } + }, + "velocity": -1.1394453282615602, + "acceleration": 3.0, + "curvature": -0.6107915042418585, + "holonomicRotation": 0.0, + "angularVelocity": 0.6862940629818453, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.7245944220466605, + "pose": { + "rotation": { + "radians": 2.999254168840606 + }, + "translation": { + "x": 4.784373248000002, + "y": 3.0151787519999997 + } + }, + "velocity": -1.107248154657719, + "acceleration": 2.999999999999979, + "curvature": -0.6138816870079442, + "holonomicRotation": 0.0, + "angularVelocity": 0.6696986828773126, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.7356432977972824, + "pose": { + "rotation": { + "radians": 3.0066535863780723 + }, + "translation": { + "x": 4.796314432000002, + "y": 3.0135575679999995 + } + }, + "velocity": -1.0741015274058545, + "acceleration": 3.0, + "curvature": -0.6169659427695113, + "holonomicRotation": 0.0, + "angularVelocity": 0.6522810081297855, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.7470391418173126, + "pose": { + "rotation": { + "radians": 3.0140868790039477 + }, + "translation": { + "x": 4.808262144000002, + "y": 3.0120258559999997 + } + }, + "velocity": -1.039913995345763, + "acceleration": 2.9999999999999805, + "curvature": -0.6200425403987194, + "holonomicRotation": 0.0, + "angularVelocity": 0.6339691664026881, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.7588176137867158, + "pose": { + "rotation": { + "radians": 3.0215540670598875 + }, + "translation": { + "x": 4.820216000000001, + "y": 3.0105839999999997 + } + }, + "velocity": -1.004578579437554, + "acceleration": 3.0000000000000187, + "curvature": -0.623109707774154, + "holonomicRotation": 0.0, + "angularVelocity": 0.6146794031013395, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.7710208694945617, + "pose": { + "rotation": { + "radians": 3.0290551569942794 + }, + "translation": { + "x": 4.8321756160000024, + "y": 3.0092323839999997 + } + }, + "velocity": -0.9679688123140158, + "acceleration": 2.9999999999999636, + "curvature": -0.6261656322069277, + "holonomicRotation": 0.0, + "angularVelocity": 0.5943129313748355, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.783699348541738, + "pose": { + "rotation": { + "radians": 3.036590141042181 + }, + "translation": { + "x": 4.844140608000002, + "y": 3.007971392 + } + }, + "velocity": -0.9299333751724882, + "acceleration": 2.999999999999991, + "curvature": -0.6292084609699685, + "holonomicRotation": 0.0, + "angularVelocity": 0.5727516148207152, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.7969142495050097, + "pose": { + "rotation": { + "radians": 3.0441589969085907 + }, + "translation": { + "x": 4.856110592000002, + "y": 3.006801408 + } + }, + "velocity": -0.8902886722826729, + "acceleration": 2.999999999999983, + "curvature": -0.6322363020068372, + "holonomicRotation": 0.0, + "angularVelocity": 0.549851904665754, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.810741046286905, + "pose": { + "rotation": { + "radians": 3.051761687454542 + }, + "translation": { + "x": 4.868085184000002, + "y": 3.0057228159999996 + } + }, + "velocity": -0.8488082819369874, + "acceleration": 2.999999999999984, + "curvature": -0.6352472246674356, + "holonomicRotation": 0.0, + "angularVelocity": 0.5254360802848955, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.8252746369917863, + "pose": { + "rotation": { + "radians": 3.05939816038698 + }, + "translation": { + "x": 4.880064000000003, + "y": 3.004736 + } + }, + "velocity": -0.8052075098223431, + "acceleration": 3.000000000000015, + "curvature": -0.6382392605344301, + "holonomicRotation": 0.0, + "angularVelocity": 0.49927915802747, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.8406371600262545, + "pose": { + "rotation": { + "radians": 3.067068347952807 + }, + "translation": { + "x": 4.8920466560000015, + "y": 3.003841344 + } + }, + "velocity": -0.759119940718938, + "acceleration": 3.0000000000000218, + "curvature": -0.6412104045156426, + "holonomicRotation": 0.0, + "angularVelocity": 0.4710885081646451, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.8569903899445377, + "pose": { + "rotation": { + "radians": 3.0747721666386845 + }, + "translation": { + "x": 4.904032768000002, + "y": 3.003039232 + } + }, + "velocity": -0.7100602509640875, + "acceleration": 3.0000000000000204, + "curvature": -0.6441586157880042, + "holonomicRotation": 0.0, + "angularVelocity": 0.4404704670619436, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.874556494407223, + "pose": { + "rotation": { + "radians": 3.082509516875822 + }, + "translation": { + "x": 4.916021952000001, + "y": 3.0023300479999997 + } + }, + "velocity": -0.6573619375760337, + "acceleration": 2.9999999999999876, + "curvature": -0.6470818190817547, + "holonomicRotation": 0.0, + "angularVelocity": 0.40687199814352293, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.8936552918455964, + "pose": { + "rotation": { + "radians": 3.0902802827517117 + }, + "translation": { + "x": 4.928013824000002, + "y": 3.001714176 + } + }, + "velocity": -0.6000655452609113, + "acceleration": 3.0000000000000115, + "curvature": -0.6499779058913984, + "holonomicRotation": 0.0, + "angularVelocity": 0.3694696536401581, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.9147775921479706, + "pose": { + "rotation": { + "radians": 3.098084331728513 + }, + "translation": { + "x": 4.9400080000000015, + "y": 3.001192 + } + }, + "velocity": -0.5366986443537898, + "acceleration": 3.000000000000016, + "curvature": -0.6528447360474303, + "holonomicRotation": 0.0, + "angularVelocity": 0.3269313584685586, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.938749543564584, + "pose": { + "rotation": { + "radians": 3.105921514370289 + }, + "translation": { + "x": 4.952004096000002, + "y": 3.000763904 + } + }, + "velocity": -0.4647827901039487, + "acceleration": 3.000000000000002, + "curvature": -0.6556801391027219, + "holonomicRotation": 0.0, + "angularVelocity": 0.2768026259312415, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 3.9671818956098828, + "pose": { + "rotation": { + "radians": 3.113791664077829 + }, + "translation": { + "x": 4.964001728000001, + "y": 3.000430272 + } + }, + "velocity": -0.3794857339680527, + "acceleration": 2.999999999999996, + "curvature": -0.6584819160890972, + "holonomicRotation": 0.0, + "angularVelocity": 0.21329879823738962, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 4.004232890837561, + "pose": { + "rotation": { + "radians": 3.1216945968333922 + }, + "translation": { + "x": 4.976000512000002, + "y": 3.000191488 + } + }, + "velocity": -0.26833274828501735, + "acceleration": 2.9999999999999942, + "curvature": -0.6612478412834537, + "holonomicRotation": 0.0, + "angularVelocity": 0.08872022710441509, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 4.0936771402659, + "pose": { + "rotation": { + "radians": 3.1296301109558584 + }, + "translation": { + "x": 4.988000064000001, + "y": 3.000047936 + } + }, + "velocity": 0, + "acceleration": 3.0000000000000058, + "curvature": -0.6612478412834537, + "holonomicRotation": 0.0, + "angularVelocity": 0.0, + "holonomicAngularVelocity": 0.0 + } +] \ No newline at end of file diff --git a/src/main/deploy/pathplanner/generatedJSON/SamplePath.wpilib.json b/src/main/deploy/pathplanner/generatedJSON/SamplePath.wpilib.json new file mode 100644 index 0000000..c667a9f --- /dev/null +++ b/src/main/deploy/pathplanner/generatedJSON/SamplePath.wpilib.json @@ -0,0 +1,9002 @@ +[ + { + "time": 0.0, + "pose": { + "rotation": { + "radians": 0.001033147509964305 + }, + "translation": { + "x": 0.8409049675242211, + "y": 3.593000471715229 + } + }, + "velocity": 0, + "acceleration": 3.0, + "curvature": 0.17278306017897527, + "holonomicRotation": 0.0, + "angularVelocity": 0.0, + "holonomicAngularVelocity": 0.03971136388020405 + }, + { + "time": 0.08947088392615327, + "pose": { + "rotation": { + "radians": 0.001033147509964305 + }, + "translation": { + "x": 0.8529125197216157, + "y": 3.5930128772922965 + } + }, + "velocity": 0.2684126517784598, + "acceleration": 3.0, + "curvature": 0.17278306017897527, + "holonomicRotation": 0.0035530108282749717, + "angularVelocity": 0.05595780204105875, + "holonomicAngularVelocity": 0.2872974144043468 + }, + { + "time": 0.12656996971265755, + "pose": { + "rotation": { + "radians": 0.003109130808309768 + }, + "translation": { + "x": 0.8649348388571431, + "y": 3.593050256375552 + } + }, + "velocity": 0.3797099091379727, + "acceleration": 3.0000000000000013, + "curvature": 0.17384777875567378, + "holonomicRotation": 0.014211482251502705, + "angularVelocity": 0.07339801897523364, + "holonomicAngularVelocity": 0.6233950140939789 + }, + { + "time": 0.15506273677111426, + "pose": { + "rotation": { + "radians": 0.005200443465523286 + }, + "translation": { + "x": 0.8769714187458183, + "y": 3.5931128524930784 + } + }, + "velocity": 0.4651882103133429, + "acceleration": 3.0000000000000018, + "curvature": 0.17493225912360005, + "holonomicRotation": 0.031973731173485787, + "angularVelocity": 0.08763419519836242, + "holonomicAngularVelocity": 1.0341918462716129 + }, + { + "time": 0.17910394502987506, + "pose": { + "rotation": { + "radians": 0.007307275402876012 + }, + "translation": { + "x": 0.8890217532026562, + "y": 3.5932009091729564 + } + }, + "velocity": 0.5373118350896253, + "acceleration": 3.0000000000000013, + "curvature": 0.17603670832924992, + "holonomicRotation": 0.05683695272921396, + "angularVelocity": 0.10012823022338202, + "holonomicAngularVelocity": 1.507684277812072 + }, + { + "time": 0.20030219486832673, + "pose": { + "rotation": { + "radians": 0.009429818643033272 + }, + "translation": { + "x": 0.9010853360426722, + "y": 3.5933146699432683 + } + }, + "velocity": 0.6009065846049804, + "acceleration": 3.0000000000000013, + "curvature": 0.1771613391262139, + "holonomicRotation": 0.08879722072777985, + "angularVelocity": 0.11149423879801537, + "holonomicAngularVelocity": 2.0361034769837696 + }, + { + "time": 0.21948209804419289, + "pose": { + "rotation": { + "radians": 0.011568267347846106 + }, + "translation": { + "x": 0.9131616610808811, + "y": 3.593454378332095 + } + }, + "velocity": 0.6584462941325788, + "acceleration": 3.0, + "curvature": 0.178306370195383, + "holonomicRotation": 0.12784948827237297, + "angularVelocity": 0.1220626987297335, + "holonomicAngularVelocity": 2.6138821118678917 + }, + { + "time": 0.23713327675473306, + "pose": { + "rotation": { + "radians": 0.013722817857015457 + }, + "translation": { + "x": 0.925250222132298, + "y": 3.5936202778675184 + } + }, + "velocity": 0.7113998302641993, + "acceleration": 2.999999999999997, + "curvature": 0.1794720262645367, + "holonomicRotation": 0.17398758855723728, + "angularVelocity": 0.13203637709398355, + "holonomicAngularVelocity": 3.2367646293136128 + }, + { + "time": 0.25357458570945307, + "pose": { + "rotation": { + "radians": 0.015893668726079557 + }, + "translation": { + "x": 0.9373505130119382, + "y": 3.59381261207762 + } + }, + "velocity": 0.7607237571283594, + "acceleration": 3.0000000000000036, + "curvature": 0.18065853783088526, + "holonomicRotation": 0.22720423584149219, + "angularVelocity": 0.14155054256928343, + "holonomicAngularVelocity": 3.901350943412706 + }, + { + "time": 0.2690273844021526, + "pose": { + "rotation": { + "radians": 0.018081020765245093 + }, + "translation": { + "x": 0.9494620275348163, + "y": 3.5940316244904813 + } + }, + "velocity": 0.807082153206458, + "acceleration": 3.0, + "curvature": 0.18186614145581717, + "holonomicRotation": 0.2874910265996222, + "angularVelocity": 0.1507009024564452, + "holonomicAngularVelocity": 4.604835206383188 + }, + { + "time": 0.28365275343935875, + "pose": { + "rotation": { + "radians": 0.020285077077910607 + }, + "translation": { + "x": 0.9615842595159476, + "y": 3.594277558634184 + } + }, + "velocity": 0.8509582603180764, + "acceleration": 3.0, + "curvature": 0.18309507957524826, + "holonomicRotation": 0.3548384408484956, + "angularVelocity": 0.1595582172890896, + "holonomicAngularVelocity": 5.344844021463974 + }, + { + "time": 0.297572224709883, + "pose": { + "rotation": { + "radians": 0.022506043099442152 + }, + "translation": { + "x": 0.9737167027703473, + "y": 3.5945506580368094 + } + }, + "velocity": 0.8927166741296492, + "acceleration": 3.000000000000004, + "curvature": 0.1843456007826043, + "holonomicRotation": 0.42923584365069656, + "angularVelocity": 0.16817663067487754, + "holonomicAngularVelocity": 6.11933015643093 + }, + { + "time": 0.3108801589218642, + "pose": { + "rotation": { + "radians": 0.024744126636456087 + }, + "translation": { + "x": 0.9858588511130302, + "y": 3.59485116622644 + } + }, + "velocity": 0.9326404767655929, + "acceleration": 3.000000000000004, + "curvature": 0.18561795951179086, + "holonomicRotation": 0.5106714867938722, + "angularVelocity": 0.17659873477805743, + "holonomicAngularVelocity": 6.926499439766466 + }, + { + "time": 0.32365154865795415, + "pose": { + "rotation": { + "radians": 0.02699953790520704 + }, + "translation": { + "x": 0.9980101983590115, + "y": 3.595179326731156 + } + }, + "velocity": 0.9709546459738628, + "acceleration": 3.000000000000009, + "curvature": 0.1869124166040281, + "holonomicRotation": 0.5991325106459383, + "angularVelocity": 0.1848588118153447, + "holonomicAngularVelocity": 7.764758597682022 + }, + { + "time": 0.33594715763777777, + "pose": { + "rotation": { + "radians": 0.029272489571763316 + }, + "translation": { + "x": 1.0101702383233062, + "y": 3.59553538307904 + } + }, + "velocity": 1.0078414729133338, + "acceleration": 3.000000000000009, + "curvature": 0.1882292385335524, + "holonomicRotation": 0.6946049461857601, + "angularVelocity": 0.1929849951641493, + "holonomicAngularVelocity": 8.632676895489821 + }, + { + "time": 0.3478170297833208, + "pose": { + "rotation": { + "radians": 0.03156319679037001 + }, + "translation": { + "x": 1.0223384648209295, + "y": 3.595919578798173 + } + }, + "velocity": 1.043451089349963, + "acceleration": 3.0, + "curvature": 0.1895686982557005, + "holonomicRotation": 0.7970737172090075, + "angularVelocity": 0.20100076046015578, + "holonomicAngularVelocity": 9.528957212034914 + }, + { + "time": 0.35930295873278145, + "pose": { + "rotation": { + "radians": 0.03387187724380292 + }, + "translation": { + "x": 1.0345143716668963, + "y": 3.596332157416637 + } + }, + "velocity": 1.0779088761983449, + "acceleration": 3.0, + "curvature": 0.1909310748431833, + "holonomicRotation": 0.9065226427088913, + "angularVelocity": 0.208925983886048, + "holonomicAngularVelocity": 10.452413757552776 + }, + { + "time": 0.3704402710725541, + "pose": { + "rotation": { + "radians": 0.036198751182236144 + }, + "translation": { + "x": 1.0466974526762218, + "y": 3.596773362462513 + } + }, + "velocity": 1.1113208132176626, + "acceleration": 2.99999999999998, + "curvature": 0.1923166534196862, + "holonomicRotation": 1.0229344394312934, + "angularVelocity": 0.21677771226045245, + "holonomicAngularVelocity": 11.401954592092753 + }, + { + "time": 0.38125914288759216, + "pose": { + "rotation": { + "radians": 0.03854404146353918 + }, + "translation": { + "x": 1.058887201663921, + "y": 3.5972434374638835 + } + }, + "velocity": 1.1437774286627767, + "acceleration": 2.99999999999999, + "curvature": 0.19372572545101252, + "holonomicRotation": 1.1462907246040293, + "angularVelocity": 0.22457073506166303, + "holonomicAngularVelocity": 12.376567690501977 + }, + { + "time": 0.39178559072786273, + "pose": { + "rotation": { + "radians": 0.040907973592617 + }, + "translation": { + "x": 1.0710831124450086, + "y": 3.597742625948829 + } + }, + "velocity": 1.1753567721835882, + "acceleration": 2.9999999999999893, + "curvature": 0.1951585886678108, + "holonomicRotation": 1.2765720188396763, + "angularVelocity": 0.23231801754627757, + "holonomicAngularVelocity": 13.375309678614832 + }, + { + "time": 0.4020422302874884, + "pose": { + "rotation": { + "radians": 0.04329077576179596 + }, + "translation": { + "x": 1.0832846788345003, + "y": 3.598271171445432 + } + }, + "velocity": 1.2061266908624655, + "acceleration": 3.0000000000000164, + "curvature": 0.19661554710751872, + "holonomicRotation": 1.4137577492116016, + "angularVelocity": 0.2400310338015318, + "holonomicAngularVelocity": 14.397296615747562 + }, + { + "time": 0.41204886605642116, + "pose": { + "rotation": { + "radians": 0.045692678890288274 + }, + "translation": { + "x": 1.0954913946474107, + "y": 3.5988293174817736 + } + }, + "velocity": 1.2361465981692639, + "acceleration": 3.0000000000000164, + "curvature": 0.19809691103775634, + "holonomicRotation": 1.5578262525026754, + "angularVelocity": 0.2477200268917496, + "holonomicAngularVelocity": 15.441696368515231 + }, + { + "time": 0.4218229558015866, + "pose": { + "rotation": { + "radians": 0.048113916664803025 + }, + "translation": { + "x": 1.1077027536987554, + "y": 3.5994173075859357 + } + }, + "velocity": 1.2654688674047603, + "acceleration": 3.000000000000017, + "curvature": 0.19960299718679397, + "holonomicRotation": 1.7087547786261381, + "angularVelocity": 0.25539421439292753, + "holonomicAngularVelocity": 16.50772223888046 + }, + { + "time": 0.4313799808901459, + "pose": { + "rotation": { + "radians": 0.05055472557922913 + }, + "translation": { + "x": 1.1199182498035485, + "y": 3.600035385285999 + } + }, + "velocity": 1.2941399426704383, + "acceleration": 3.0000000000000058, + "curvature": 0.20113412869661867, + "holonomicRotation": 1.8665194942180874, + "angularVelocity": 0.26306195299790774, + "holonomicAngularVelocity": 17.594627592697993 + }, + { + "time": 0.4407337447772134, + "pose": { + "rotation": { + "radians": 0.05301534497524241 + }, + "translation": { + "x": 1.132137376776806, + "y": 3.6006837941100462 + } + }, + "velocity": 1.3222012343316407, + "acceleration": 2.9999999999999942, + "curvature": 0.20269063507705265, + "holonomicRotation": 2.031095486401067, + "angularVelocity": 0.27073087146300023, + "holonomicAngularVelocity": 18.701701295088498 + }, + { + "time": 0.4498966159841248, + "pose": { + "rotation": { + "radians": 0.05549601708219276 + }, + "translation": { + "x": 1.1443596284335424, + "y": 3.6013627775861585 + } + }, + "velocity": 1.349689847952375, + "acceleration": 3.0, + "curvature": 0.2042728523829029, + "holonomicRotation": 2.202456766718091, + "angularVelocity": 0.2784079792538743, + "holonomicAngularVelocity": 19.828263802885605 + }, + { + "time": 0.458879727682109, + "pose": { + "rotation": { + "radians": 0.057996987057440386 + }, + "translation": { + "x": 1.156584498588773, + "y": 3.602072579242417 + } + }, + "velocity": 1.3766391830463274, + "acceleration": 2.9999999999999813, + "curvature": 0.20588112312325477, + "holonomicRotation": 2.3805762752365096, + "angularVelocity": 0.28609975601445164, + "holonomicAngularVelocity": 20.97366379705548 + }, + { + "time": 0.46769314299062287, + "pose": { + "rotation": { + "radians": 0.06051850302686024 + }, + "translation": { + "x": 1.1688114810575128, + "y": 3.602813442606904 + } + }, + "velocity": 1.403079428971869, + "acceleration": 3.0000000000000124, + "curvature": 0.20751579628291553, + "holonomicRotation": 2.5654258848211016, + "angularVelocity": 0.29381222575858285, + "holonomicAngularVelocity": 22.137275262484405 + }, + { + "time": 0.47634599292284474, + "pose": { + "rotation": { + "radians": 0.06306081612460135 + }, + "translation": { + "x": 1.1810400696547767, + "y": 3.6035856112077007 + } + }, + "velocity": 1.4290379787685348, + "acceleration": 3.000000000000006, + "curvature": 0.2091772275932128, + "holonomicRotation": 2.7569764055756667, + "angularVelocity": 0.30155101908780385, + "holonomicAngularVelocity": 23.318494941239045 + }, + { + "time": 0.48484659231299837, + "pose": { + "rotation": { + "radians": 0.06562418053355934 + }, + "translation": { + "x": 1.1932697581955802, + "y": 3.604389328572888 + } + }, + "velocity": 1.4545397769389956, + "acceleration": 2.9999999999999933, + "curvature": 0.21086577917913393, + "holonomicRotation": 2.955197589452464, + "angularVelocity": 0.30932142554000075, + "holonomicAngularVelocity": 24.516740099768963 + }, + { + "time": 0.49320253787274904, + "pose": { + "rotation": { + "radians": 0.06820885352583606 + }, + "translation": { + "x": 1.2055000404949379, + "y": 3.605224838230549 + } + }, + "velocity": 1.4796076136182474, + "acceleration": 2.99999999999998, + "curvature": 0.2125818197893795, + "holonomicRotation": 3.1600581350286894, + "angularVelocity": 0.3171284377955142, + "holonomicAngularVelocity": 25.731446561727278 + }, + { + "time": 0.5014207916313713, + "pose": { + "rotation": { + "radians": 0.07081509550171505 + }, + "translation": { + "x": 1.217730410367865, + "y": 3.6060923837087637 + } + }, + "velocity": 1.5042623748941144, + "acceleration": 3.0000000000000133, + "curvature": 0.21432572497715796, + "holonomicRotation": 3.371525692449393, + "angularVelocity": 0.32497678982298894, + "holonomicAngularVelocity": 26.962066966824505 + }, + { + "time": 0.5095077523363771, + "pose": { + "rotation": { + "radians": 0.0734431700310525 + }, + "translation": { + "x": 1.2299603616293766, + "y": 3.606992208535615 + } + }, + "velocity": 1.5285232570091318, + "acceleration": 3.0, + "curvature": 0.21609787681652287, + "holonomicRotation": 3.589566868535838, + "angularVelocity": 0.3328709889911717, + "holonomicAngularVelocity": 28.20806922315502 + }, + { + "time": 0.5174693168711992, + "pose": { + "rotation": { + "radians": 0.07609334389167577 + }, + "translation": { + "x": 1.2421893880944879, + "y": 3.6079245562391833 + } + }, + "velocity": 1.5524079506135984, + "acceleration": 3.000000000000028, + "curvature": 0.21789866414830383, + "holonomicRotation": 3.814147232058716, + "angularVelocity": 0.3408153443469722, + "holonomicAngularVelocity": 29.468935125899826 + }, + { + "time": 0.5253109333439094, + "pose": { + "rotation": { + "radians": 0.07876588711005938 + }, + "translation": { + "x": 1.2544169835782135, + "y": 3.6088896703475513 + } + }, + "velocity": 1.575932800031729, + "acceleration": 3.0, + "curvature": 0.21972848239994464, + "holonomicRotation": 4.0452313191752, + "angularVelocity": 0.34881399063156393, + "holonomicAngularVelocity": 30.744159119871735 + }, + { + "time": 0.5330376471866174, + "pose": { + "rotation": { + "radians": 0.08146107300000249 + }, + "translation": { + "x": 1.2666426418955692, + "y": 3.6098877943888 + } + }, + "velocity": 1.5991129415598528, + "acceleration": 3.0, + "curvature": 0.2215877338186406, + "holonomicRotation": 4.282782639029129, + "angularVelocity": 0.35687090951375877, + "holonomicAngularVelocity": 32.03324718691037 + }, + { + "time": 0.5406541413578321, + "pose": { + "rotation": { + "radians": 0.08417917820219012 + }, + "translation": { + "x": 1.2788658568615694, + "y": 3.6109191718910107 + } + }, + "velocity": 1.6219624240734969, + "acceleration": 3.0, + "curvature": 0.22347682738654667, + "holonomicRotation": 4.526763679513311, + "angularVelocity": 0.36498994798864565, + "holonomicAngularVelocity": 33.33571584213352 + }, + { + "time": 0.5481647715439514, + "pose": { + "rotation": { + "radians": 0.08692048272318376 + }, + "translation": { + "x": 1.2910861222912293, + "y": 3.6119840463822657 + } + }, + "velocity": 1.6444943146318547, + "acceleration": 2.9999999999999853, + "curvature": 0.225396178703331, + "holonomicRotation": 4.777135913193135, + "angularVelocity": 0.37317483448183175, + "holonomicAngularVelocity": 34.65109122547071 + }, + { + "time": 0.5555735970994554, + "pose": { + "rotation": { + "radians": 0.08968526997356374 + }, + "translation": { + "x": 1.3033029319995642, + "y": 3.6130826613906457 + } + }, + "velocity": 1.6667207912983668, + "acceleration": 3.0, + "curvature": 0.22734621016862638, + "holonomicRotation": 5.033859803390504, + "angularVelocity": 0.3814291932276495, + "holonomicAngularVelocity": 35.97890827687574 + }, + { + "time": 0.5628844083395413, + "pose": { + "rotation": { + "radians": 0.09247382680670935 + }, + "translation": { + "x": 1.315515779801589, + "y": 3.614215260444233 + } + }, + "velocity": 1.6886532250186246, + "acceleration": 3.0, + "curvature": 0.22932735098729715, + "holonomicRotation": 5.296894810427108, + "angularVelocity": 0.3897565567330589, + "holonomicAngularVelocity": 37.318709985335744 + }, + { + "time": 0.5701007506971646, + "pose": { + "rotation": { + "radians": 0.09528644355622351 + }, + "translation": { + "x": 1.3277241595123186, + "y": 3.615382087071109 + } + }, + "velocity": 1.7103022520914943, + "acceleration": 3.0, + "curvature": 0.2313400369423704, + "holonomicRotation": 5.566199398026144, + "angularVelocity": 0.3981603770417206, + "holonomicAngularVelocity": 38.670046703091636 + }, + { + "time": 0.5772259461736723, + "pose": { + "rotation": { + "radians": 0.09812341407364578 + }, + "translation": { + "x": 1.3399275649467683, + "y": 3.6165833847993554 + } + }, + "velocity": 1.7316778385210174, + "acceleration": 3.0, + "curvature": 0.23338471062629995, + "holonomicRotation": 5.841731039871354, + "angularVelocity": 0.40664403557729134, + "holonomicAngularVelocity": 40.032475517762556 + }, + { + "time": 0.5842631124444932, + "pose": { + "rotation": { + "radians": 0.10098503576504081 + }, + "translation": { + "x": 1.352125489919953, + "y": 3.617819397157053 + } + }, + "velocity": 1.7527893373334802, + "acceleration": 2.999999999999984, + "curvature": 0.23546182136796504, + "holonomicRotation": 6.123446226322418, + "angularVelocity": 0.41521085211936093, + "holonomicAngularVelocity": 41.40555967593165 + }, + { + "time": 0.5912151799256765, + "pose": { + "rotation": { + "radians": 0.10387160962789421 + }, + "translation": { + "x": 1.3643174282468882, + "y": 3.619090367672284 + } + }, + "velocity": 1.7736455397770299, + "acceleration": 2.999999999999984, + "curvature": 0.2375718250628036, + "holonomicRotation": 6.411300471285656, + "angularVelocity": 0.4238640925660133, + "holonomicAngularVelocity": 42.7888680526676 + }, + { + "time": 0.5980849070610476, + "pose": { + "rotation": { + "radians": 0.10678344028630438 + }, + "translation": { + "x": 1.3765028737425884, + "y": 3.62039653987313 + } + }, + "velocity": 1.794254721183143, + "acceleration": 3.0, + "curvature": 0.23971518428892508, + "holonomicRotation": 6.705248319238877, + "angularVelocity": 0.43260697617490873, + "holonomicAngularVelocity": 44.18197466210464 + }, + { + "time": 0.6048748940515513, + "pose": { + "rotation": { + "radians": 0.10972083602653315 + }, + "translation": { + "x": 1.3886813202220687, + "y": 3.621738157287673 + } + }, + "velocity": 1.8146246821546541, + "acceleration": 3.0, + "curvature": 0.24189236838038028, + "holonomicRotation": 7.005243352409332, + "angularVelocity": 0.44144268185248914, + "holonomicAngularVelocity": 45.58445820485198 + }, + { + "time": 0.6115875952164789, + "pose": { + "rotation": { + "radians": 0.11268410883125313 + }, + "translation": { + "x": 1.400852261500344, + "y": 3.6231154634439937 + } + }, + "velocity": 1.834762785649437, + "acceleration": 3.0000000000000164, + "curvature": 0.24410385296432457, + "holonomicRotation": 7.311238198103636, + "angularVelocity": 0.4503743539059442, + "holonomicAngularVelocity": 46.9959016484623 + }, + { + "time": 0.6182253301495848, + "pose": { + "rotation": { + "radians": 0.11567357441314963 + }, + "translation": { + "x": 1.4130151913924303, + "y": 3.6245287018701737 + } + }, + "velocity": 1.8546759904487549, + "acceleration": 3.0, + "curvature": 0.24635012043498902, + "holonomicRotation": 7.623184536188445, + "angularVelocity": 0.45940510725794936, + "holonomicAngularVelocity": 48.415891837714405 + }, + { + "time": 0.6247902938106432, + "pose": { + "rotation": { + "radians": 0.1186895522480027 + }, + "translation": { + "x": 1.4251696037133417, + "y": 3.625978116094295 + } + }, + "velocity": 1.87437088143193, + "acceleration": 2.999999999999983, + "curvature": 0.24863165957534217, + "holonomicRotation": 7.9410331067207744, + "angularVelocity": 0.46853803201309485, + "holonomicAngularVelocity": 49.84401913173306 + }, + { + "time": 0.6312845656740427, + "pose": { + "rotation": { + "radians": 0.1217323656062379 + }, + "translation": { + "x": 1.437314992278094, + "y": 3.6274639496444396 + } + }, + "velocity": 1.8938536970221285, + "acceleration": 3.000000000000017, + "curvature": 0.25094896537060735, + "holonomicRotation": 8.264733717726733, + "angularVelocity": 0.4777761977161289, + "holonomicAngularVelocity": 51.27987706542535 + }, + { + "time": 0.6377101180399395, + "pose": { + "rotation": { + "radians": 0.12480234158384196 + }, + "translation": { + "x": 1.4494508509017014, + "y": 3.628986446048688 + } + }, + "velocity": 1.9131303541198188, + "acceleration": 2.9999999999999827, + "curvature": 0.2533025394819179, + "holonomicRotation": 8.594235253127374, + "angularVelocity": 0.4871226572145231, + "holonomicAngularVelocity": 52.72306203291004 + }, + { + "time": 0.6440688235998314, + "pose": { + "rotation": { + "radians": 0.12789981113262128 + }, + "translation": { + "x": 1.4615766733991795, + "y": 3.6305458488351228 + } + }, + "velocity": 1.9322064707994946, + "acceleration": 3.0, + "curvature": 0.25569288941062784, + "holonomicRotation": 8.929485680810567, + "angularVelocity": 0.49658044992512607, + "holonomicAngularVelocity": 54.1731729909019 + }, + { + "time": 0.6503624623367412, + "pose": { + "rotation": { + "radians": 0.13102510908826215 + }, + "translation": { + "x": 1.4736919535855433, + "y": 3.6321424015318247 + } + }, + "velocity": 1.951087387010224, + "acceleration": 3.0000000000000178, + "curvature": 0.2581205291382108, + "holonomicRotation": 9.270432060847423, + "angularVelocity": 0.5061526051228734, + "holonomicAngularVelocity": 55.62981118023849 + }, + { + "time": 0.6565927278302157, + "pose": { + "rotation": { + "radians": 0.1341785741983914 + }, + "translation": { + "x": 1.4857961852758077, + "y": 3.6337763476668763 + } + }, + "velocity": 1.9697781834906476, + "acceleration": 3.0, + "curvature": 0.2605859783513634, + "holonomicRotation": 9.617020553852164, + "angularVelocity": 0.515842144530513, + "holonomicAngularVelocity": 57.0925798639063 + }, + { + "time": 0.6627612330277614, + "pose": { + "rotation": { + "radians": 0.13736054914804097 + }, + "translation": { + "x": 1.497888862284988, + "y": 3.6354479307683576 + } + }, + "velocity": 1.9882836990832846, + "acceleration": 3.000000000000018, + "curvature": 0.2630897628253225, + "holonomicRotation": 9.96919642948396, + "angularVelocity": 0.5256520850758009, + "holonomicAngularVelocity": 58.56108408011048 + }, + { + "time": 0.6688695155369478, + "pose": { + "rotation": { + "radians": 0.1405713805852269 + }, + "translation": { + "x": 1.5099694784280993, + "y": 3.637157394364352 + } + }, + "velocity": 2.006608546610844, + "acceleration": 3.0, + "curvature": 0.2656324140586707, + "holonomicRotation": 10.326904075089498, + "angularVelocity": 0.535585440834627, + "holonomicAngularVelocity": 60.034930409087174 + }, + { + "time": 0.6749190424860186, + "pose": { + "rotation": { + "radians": 0.14381141914308593 + }, + "translation": { + "x": 1.5220375275201565, + "y": 3.63890498198294 + } + }, + "velocity": 2.0247571274580562, + "acceleration": 2.999999999999963, + "curvature": 0.26821446914236624, + "holonomicRotation": 10.69008700448486, + "angularVelocity": 0.5456452253632049, + "holonomicAngularVelocity": 61.51372675245395 + }, + { + "time": 0.6809112149953159, + "pose": { + "rotation": { + "radians": 0.14708101946233665 + }, + "translation": { + "x": 1.5340925033761743, + "y": 3.640690937152203 + } + }, + "velocity": 2.0427336449859483, + "acceleration": 3.000000000000037, + "curvature": 0.2708364706228389, + "holonomicRotation": 11.05868786687534, + "angularVelocity": 0.5558344532631501, + "holonomicAngularVelocity": 62.99708212405171 + }, + { + "time": 0.6868473722970159, + "pose": { + "rotation": { + "radians": 0.1503805402106111 + }, + "translation": { + "x": 1.5461338998111684, + "y": 3.6425155034002232 + } + }, + "velocity": 2.0605421168910483, + "acceleration": 3.0000000000000187, + "curvature": 0.27349896630261744, + "holonomicRotation": 11.43264845591182, + "angularVelocity": 0.5661561418797494, + "holonomicAngularVelocity": 64.48460645132965 + }, + { + "time": 0.6927287955364773, + "pose": { + "rotation": { + "radians": 0.15371034410062645 + }, + "translation": { + "x": 1.5581612106401537, + "y": 3.6443789242550824 + } + }, + "velocity": 2.078186386609432, + "acceleration": 2.999999999999943, + "curvature": 0.2762025090925794, + "holonomicRotation": 11.811909718882193, + "angularVelocity": 0.5766133126207268, + "holonomicAngularVelocity": 65.9759103863855 + }, + { + "time": 0.698556711284854, + "pose": { + "rotation": { + "radians": 0.15707079790597245 + }, + "translation": { + "x": 1.5701739296781445, + "y": 3.6462814432448614 + } + }, + "velocity": 2.0956701338545622, + "acceleration": 3.0, + "curvature": 0.2789476567183776, + "holonomicRotation": 12.1964117660365, + "angularVelocity": 0.5872089922494581, + "holonomicAngularVelocity": 67.4706051258612 + }, + { + "time": 0.7043322947894155, + "pose": { + "rotation": { + "radians": 0.16046227247533862 + }, + "translation": { + "x": 1.582171550740157, + "y": 3.648223303897642 + } + }, + "velocity": 2.112996884368247, + "acceleration": 3.000000000000019, + "curvature": 0.2817349716179003, + "holonomicRotation": 12.586093880044208, + "angularVelocity": 0.5979462138251876, + "holonomicAngularVelocity": 68.96830223906366 + }, + { + "time": 0.7100566729852058, + "pose": { + "rotation": { + "radians": 0.1638851427440149 + }, + "translation": { + "x": 1.5941535676412057, + "y": 3.6502047497415058 + } + }, + "velocity": 2.1301700189556176, + "acceleration": 2.9999999999999614, + "curvature": 0.28456502063684747, + "holonomicRotation": 12.98089452558218, + "angularVelocity": 0.6088280175906541, + "holonomicAngularVelocity": 70.46861350355422 + }, + { + "time": 0.7157309272892082, + "pose": { + "rotation": { + "radians": 0.1673397877432259 + }, + "translation": { + "x": 1.6061194741963056, + "y": 3.6522260243045346 + } + }, + "velocity": 2.147192781867625, + "acceleration": 3.0000000000000195, + "curvature": 0.2874383746404486, + "holonomicRotation": 13.380751359051802, + "angularVelocity": 0.6198574516656178, + "holonomicAngularVelocity": 71.97115074772233 + }, + { + "time": 0.7213560961959983, + "pose": { + "rotation": { + "radians": 0.17082659060697747 + }, + "translation": { + "x": 1.6180687642204723, + "y": 3.65428737111481 + } + }, + "velocity": 2.164068288587995, + "acceleration": 3.0, + "curvature": 0.2903556083940122, + "holonomicRotation": 13.785601238423792, + "angularVelocity": 0.6310375725674175, + "holonomicAngularVelocity": 73.47552569976867 + }, + { + "time": 0.7269331776919463, + "pose": { + "rotation": { + "radians": 0.17434593857619118 + }, + "translation": { + "x": 1.6300009315287198, + "y": 3.656389033700413 + } + }, + "velocity": 2.1807995330758394, + "acceleration": 3.00000000000002, + "curvature": 0.29331730027060743, + "holonomicRotation": 14.195380233209027, + "angularVelocity": 0.6423714456238293, + "holonomicAngularVelocity": 74.98134984260406 + }, + { + "time": 0.7324631315033312, + "pose": { + "rotation": { + "radians": 0.1778982230002435 + }, + "translation": { + "x": 1.6419154699360639, + "y": 3.6585312555894256 + } + }, + "velocity": 2.197389394509994, + "acceleration": 2.99999999999996, + "curvature": 0.2963240316619205, + "holonomicRotation": 14.610023634553919, + "angularVelocity": 0.6538621451108233, + "holonomicAngularVelocity": 76.48823427430644 + }, + { + "time": 0.7379468811922121, + "pose": { + "rotation": { + "radians": 0.18148383933506596 + }, + "translation": { + "x": 1.6538118732575198, + "y": 3.660714280309929 + } + }, + "velocity": 2.2138406435766362, + "acceleration": 2.9999999999999596, + "curvature": 0.29937638705136754, + "holonomicRotation": 15.029465965458694, + "angularVelocity": 0.665512754437001, + "holonomicAngularVelocity": 77.99578957364865 + }, + { + "time": 0.7433853161125709, + "pose": { + "rotation": { + "radians": 0.1851031871387403 + }, + "translation": { + "x": 1.665689635308102, + "y": 3.662938351390006 + } + }, + "velocity": 2.2301559483377127, + "acceleration": 3.0000000000000204, + "curvature": 0.3024749530991072, + "holonomicRotation": 15.45364099111698, + "angularVelocity": 0.6773263659089981, + "holonomicAngularVelocity": 79.50362567038812 + }, + { + "time": 0.7487792932380414, + "pose": { + "rotation": { + "radians": 0.1887566700629315 + }, + "translation": { + "x": 1.6775482499028262, + "y": 3.665203712357737 + } + }, + "velocity": 2.2463378797141242, + "acceleration": 3.0, + "curvature": 0.30562031875355833, + "holonomicRotation": 15.882481729375025, + "angularVelocity": 0.6893060807149793, + "holonomicAngularVelocity": 81.01135172003737 + }, + { + "time": 0.7541296388714762, + "pose": { + "rotation": { + "radians": 0.19244469584198498 + }, + "translation": { + "x": 1.689387210856707, + "y": 3.6675106067412035 + } + }, + "velocity": 2.262388916614429, + "acceleration": 3.000000000000021, + "curvature": 0.3088130744624381, + "holonomicRotation": 16.31592046130898, + "angularVelocity": 0.7014550083142539, + "holonomicAngularVelocity": 82.51857598273511 + }, + { + "time": 0.7594371502456564, + "pose": { + "rotation": { + "radians": 0.19616767627708853 + }, + "translation": { + "x": 1.7012060119847596, + "y": 3.6698592780684876 + } + }, + "velocity": 2.2783114507369695, + "acceleration": 3.000000000000021, + "curvature": 0.31205381190339315, + "holonomicRotation": 16.753888741918498, + "angularVelocity": 0.7137762660898251, + "holonomicAngularVelocity": 84.02490570606173 + }, + { + "time": 0.7647025970235938, + "pose": { + "rotation": { + "radians": 0.19992602721753938 + }, + "translation": { + "x": 1.7130041471019988, + "y": 3.672249969867671 + } + }, + "velocity": 2.2941077910707817, + "acceleration": 3.0, + "curvature": 0.3153431232712081, + "holonomicRotation": 17.196317410934974, + "angularVelocity": 0.7262729785212538, + "holonomicAngularVelocity": 85.52994701147115 + }, + { + "time": 0.7699267227061208, + "pose": { + "rotation": { + "radians": 0.2037201685371577 + }, + "translation": { + "x": 1.7247811100234403, + "y": 3.674682925666835 + } + }, + "velocity": 2.3097801681183627, + "acceleration": 2.9999999999999787, + "curvature": 0.31868160109621285, + "holonomicRotation": 17.64313660374278, + "angularVelocity": 0.7389482764615447, + "holonomicAngularVelocity": 87.03330478423526 + }, + { + "time": 0.7751102459537709, + "pose": { + "rotation": { + "radians": 0.20755052410700703 + }, + "translation": { + "x": 1.7365363945640986, + "y": 3.6771583889940604 + } + }, + "velocity": 2.3253307378613126, + "acceleration": 2.9999999999999574, + "curvature": 0.32206983737018163, + "holonomicRotation": 18.094275762411673, + "angularVelocity": 0.7518052961123932, + "holonomicAngularVelocity": 88.53458256661038 + }, + { + "time": 0.7802538618293463, + "pose": { + "rotation": { + "radians": 0.21141752176343243 + }, + "translation": { + "x": 1.7482694945389885, + "y": 3.6796766033774304 + } + }, + "velocity": 2.3407615854880386, + "acceleration": 2.999999999999935, + "curvature": 0.32550842300216626, + "holonomicRotation": 18.549663646838734, + "angularVelocity": 0.7648471777350144, + "holonomicAngularVelocity": 90.03338245416519 + }, + { + "time": 0.7853582429670116, + "pose": { + "rotation": { + "radians": 0.21532159327065958 + }, + "translation": { + "x": 1.759979903763126, + "y": 3.6822378123450257 + } + }, + "velocity": 2.3560747289010346, + "acceleration": 3.0000000000000218, + "curvature": 0.3289979473654669, + "holonomicRotation": 19.009228345997983, + "angularVelocity": 0.7780770645594818, + "holonomicAngularVelocity": 91.52930499509503 + }, + { + "time": 0.7904240406732528, + "pose": { + "rotation": { + "radians": 0.21926317427958386 + }, + "translation": { + "x": 1.7716671160515254, + "y": 3.684842259424927 + } + }, + "velocity": 2.371272122019758, + "acceleration": 3.0, + "curvature": 0.332538997417837, + "holonomicRotation": 19.472897289295982, + "angularVelocity": 0.7914981011534582, + "holonomicAngularVelocity": 93.0219490923428 + }, + { + "time": 0.7954518859645968, + "pose": { + "rotation": { + "radians": 0.22324270428057602 + }, + "translation": { + "x": 1.783330625219202, + "y": 3.6874901881452176 + } + }, + "velocity": 2.38635565789379, + "acceleration": 2.999999999999978, + "curvature": 0.33613215718878975, + "holonomicRotation": 19.940597258031563, + "angularVelocity": 0.805113431703494, + "holonomicAngularVelocity": 94.51091190859103 + }, + { + "time": 0.8004423905465707, + "pose": { + "rotation": { + "radians": 0.22726062655050105 + }, + "translation": { + "x": 1.794969925081171, + "y": 3.6901818420339785 + } + }, + "velocity": 2.401327171639712, + "acceleration": 3.000000000000022, + "curvature": 0.33977800684591697, + "holonomicRotation": 20.41225439695792, + "angularVelocity": 0.8189261983273711, + "holonomicAngularVelocity": 95.99578877384728 + }, + { + "time": 0.8053961477380197, + "pose": { + "rotation": { + "radians": 0.23131738809473124 + }, + "translation": { + "x": 1.8065845094524469, + "y": 3.69291746461929 + } + }, + "velocity": 2.4161884432140592, + "acceleration": 3.000000000000067, + "curvature": 0.34347712200275476, + "holonomicRotation": 20.887794225945182, + "angularVelocity": 0.832939539021313, + "holonomicAngularVelocity": 97.47617309569827 + }, + { + "time": 0.8103137333445651, + "pose": { + "rotation": { + "radians": 0.23541343958294503 + }, + "translation": { + "x": 1.8181738721480456, + "y": 3.6956972994292356 + } + }, + "velocity": 2.4309412000336956, + "acceleration": 3.0000000000000226, + "curvature": 0.34723007290001834, + "holonomicRotation": 21.36714165174172, + "angularVelocity": 0.8471565852901716, + "holonomicAngularVelocity": 98.9516562722208 + }, + { + "time": 0.8151957064846765, + "pose": { + "rotation": { + "radians": 0.23954923527780014 + }, + "translation": { + "x": 1.8297375069829815, + "y": 3.6985215899918953 + } + }, + "velocity": 2.44558711945403, + "acceleration": 3.0000000000000226, + "curvature": 0.351037423417464, + "holonomicRotation": 21.850220979832237, + "angularVelocity": 0.8615804600176101, + "holonomicAngularVelocity": 100.42182760730242 + }, + { + "time": 0.8200426103715678, + "pose": { + "rotation": { + "radians": 0.24372523295832904 + }, + "translation": { + "x": 1.8412749077722699, + "y": 3.7013905798353517 + } + }, + "velocity": 2.4601278311147037, + "acceleration": 3.0, + "curvature": 0.3548997302495343, + "holonomicRotation": 22.336955926390793, + "angularVelocity": 0.8762142745937123, + "holonomicAngularVelocity": 101.88627422867015 + }, + { + "time": 0.8248549730538627, + "pose": { + "rotation": { + "radians": 0.24794189383507792 + }, + "translation": { + "x": 1.852785568330926, + "y": 3.704304512487686 + } + }, + "velocity": 2.4745649191615886, + "acceleration": 3.000000000000046, + "curvature": 0.35881754198039423, + "holonomicRotation": 22.82726963032691, + "angularVelocity": 0.8910611263198046, + "holonomicAngularVelocity": 103.34458100839828 + }, + { + "time": 0.8296333081177574, + "pose": { + "rotation": { + "radians": 0.25219968245904534 + }, + "translation": { + "x": 1.8642689824739647, + "y": 3.70726363147698 + } + }, + "velocity": 2.4888999243532726, + "acceleration": 2.999999999999977, + "curvature": 0.3627913978312969, + "holonomicRotation": 23.321084665422845, + "angularVelocity": 0.9061240952935538, + "holonomicAngularVelocity": 104.7963304860019 + }, + { + "time": 0.8343781153531943, + "pose": { + "rotation": { + "radians": 0.2564990666225979 + }, + "translation": { + "x": 1.8757246440164008, + "y": 3.710268180331315 + } + }, + "velocity": 2.5031343460595834, + "acceleration": 3.0000000000000235, + "curvature": 0.36682182668811725, + "holonomicRotation": 23.818323052560064, + "angularVelocity": 0.9214062411363679, + "holonomicAngularVelocity": 106.24110279417981 + }, + { + "time": 0.8390898813863725, + "pose": { + "rotation": { + "radians": 0.2608405172523427 + }, + "translation": { + "x": 1.8871520467732499, + "y": 3.713318402578772 + } + }, + "velocity": 2.5172696441591182, + "acceleration": 3.0000000000000235, + "curvature": 0.37090934612145404, + "holonomicRotation": 24.31890627203308, + "angularVelocity": 0.9369105996727586, + "holonomicAngularVelocity": 107.67847558721752 + }, + { + "time": 0.8437690802807457, + "pose": { + "rotation": { + "radians": 0.265224508294458 + }, + "translation": { + "x": 1.8985506845595264, + "y": 3.716414541747434 + } + }, + "velocity": 2.5313072408422377, + "acceleration": 2.9999999999999765, + "curvature": 0.37505446099249756, + "holonomicRotation": 24.822755275948577, + "angularVelocity": 0.9526401790487136, + "holonomicAngularVelocity": 109.1080239721245 + }, + { + "time": 0.8484161741085013, + "pose": { + "rotation": { + "radians": 0.2696515165905873 + }, + "translation": { + "x": 1.909920051190246, + "y": 3.7195568413653817 + } + }, + "velocity": 2.5452485223255046, + "acceleration": 3.0, + "curvature": 0.37925766213227546, + "holonomicRotation": 25.32979050070805, + "angularVelocity": 0.9685979559362367, + "holonomicAngularVelocity": 110.52932044265238 + }, + { + "time": 0.8530316134943646, + "pose": { + "rotation": { + "radians": 0.27412202174548206 + }, + "translation": { + "x": 1.9212596404804234, + "y": 3.7227455449606963 + } + }, + "velocity": 2.5590948404830947, + "acceleration": 3.000000000000048, + "curvature": 0.3835194253104217, + "holonomicRotation": 25.839931879571772, + "angularVelocity": 0.9847868713264119, + "holonomicAngularVelocity": 111.94193481613654 + }, + { + "time": 0.8576158381334451, + "pose": { + "rotation": { + "radians": 0.2786365059852596 + }, + "translation": { + "x": 1.932568946245074, + "y": 3.7259808960614604 + } + }, + "velocity": 2.5728475144003364, + "acceleration": 3.000000000000073, + "curvature": 0.3878402095521523, + "holonomicRotation": 26.353098855302246, + "angularVelocity": 1.001209825967563, + "holonomicAngularVelocity": 113.34543417355982 + }, + { + "time": 0.8621692772847097, + "pose": { + "rotation": { + "radians": 0.2831954540054511 + }, + "translation": { + "x": 1.9438474622992123, + "y": 3.7292631381957544 + } + }, + "velocity": 2.5865078318541306, + "acceleration": 3.0000000000000733, + "curvature": 0.39222045599243144, + "holonomicRotation": 26.86921039288522, + "angularVelocity": 1.0178696758847163, + "holonomicAngularVelocity": 114.73938280258619 + }, + { + "time": 0.8666923502415655, + "pose": { + "rotation": { + "radians": 0.28779935281004887 + }, + "translation": { + "x": 1.955094682457854, + "y": 3.7325925148916608 + } + }, + "velocity": 2.6000770507246984, + "acceleration": 3.0000000000000737, + "curvature": 0.3966605860226018, + "holonomicRotation": 27.388184992325925, + "angularVelocity": 1.0347692271435858, + "holonomicAngularVelocity": 116.12334214411754 + }, + { + "time": 0.8711854667809218, + "pose": { + "rotation": { + "radians": 0.2924486915389446 + }, + "translation": { + "x": 1.9663101005360133, + "y": 3.7359692696772613 + } + }, + "velocity": 2.613556400342767, + "acceleration": 2.9999999999999507, + "curvature": 0.40116099996983984, + "holonomicRotation": 27.909940701518984, + "angularVelocity": 1.051911230753245, + "holonomicAngularVelocity": 117.49687074220618 + }, + { + "time": 0.8756490275920111, + "pose": { + "rotation": { + "radians": 0.29714396128527953 + }, + "translation": { + "x": 1.977493210348706, + "y": 3.7393936460806363 + } + }, + "velocity": 2.626947082776035, + "acceleration": 2.999999999999975, + "curvature": 0.4057220754674419, + "holonomicRotation": 28.434395129189525, + "angularVelocity": 1.0692983771740423, + "holonomicAngularVelocity": 118.85952419769865 + }, + { + "time": 0.8800834246861603, + "pose": { + "rotation": { + "radians": 0.30188565490179853 + }, + "translation": { + "x": 1.988643505710947, + "y": 3.742865887629869 + } + }, + "velocity": 2.640250274058482, + "acceleration": 2.99999999999995, + "curvature": 0.4103441654649296, + "holonomicRotation": 28.961465457903753, + "angularVelocity": 1.086933290206177, + "holonomicAngularVelocity": 120.2108551257639 + }, + { + "time": 0.8844890417886171, + "pose": { + "rotation": { + "radians": 0.3066742667943605 + }, + "translation": { + "x": 1.9997604804377516, + "y": 3.7463862378530393 + } + }, + "velocity": 2.6534671253658524, + "acceleration": 2.9999999999999747, + "curvature": 0.415027596780641, + "holonomicRotation": 29.491068457146774, + "angularVelocity": 1.1025750395539304, + "holonomicAngularVelocity": 121.30358876060048 + }, + { + "time": 0.8888751613152941, + "pose": { + "rotation": { + "radians": 0.31151029270497466 + }, + "translation": { + "x": 2.010843628344134, + "y": 3.7499549402782297 + } + }, + "velocity": 2.655795659422093, + "acceleration": 0.5308870499488159, + "curvature": 0.4197726679935436, + "holonomicRotation": 30.02312049646564, + "angularVelocity": 1.1129123544597121, + "holonomicAngularVelocity": 121.77867532899292 + }, + { + "time": 0.893263590248079, + "pose": { + "rotation": { + "radians": 0.3163942294809394 + }, + "translation": { + "x": 2.02189244324511, + "y": 3.753572238433522 + } + }, + "velocity": 2.642630372623738, + "acceleration": -3.0, + "curvature": 0.4245796475376652, + "holonomicRotation": 30.557537558675612, + "angularVelocity": 1.1200604476801486, + "holonomicAngularVelocity": 121.875865767175 + }, + { + "time": 0.8976672323561652, + "pose": { + "rotation": { + "radians": 0.3213265748319456 + }, + "translation": { + "x": 2.032906418955695, + "y": 3.757238375846997 + } + }, + "velocity": 2.6294194462994795, + "acceleration": -3.0, + "curvature": 0.42944877184255026, + "holonomicRotation": 31.094235253127405, + "angularVelocity": 1.1272248224221166, + "holonomicAngularVelocity": 121.94809376065527 + }, + { + "time": 0.9020862729437429, + "pose": { + "rotation": { + "radians": 0.326307827073554 + }, + "translation": { + "x": 2.043885049290903, + "y": 3.760953596046737 + } + }, + "velocity": 2.6161623245367465, + "acceleration": -3.0, + "curvature": 0.4343802430219193, + "holonomicRotation": 31.63312882903347, + "angularVelocity": 1.1344026062952288, + "holonomicAngularVelocity": 121.99532988592229 + }, + { + "time": 0.9065209046654943, + "pose": { + "rotation": { + "radians": 0.3313384848566683 + }, + "translation": { + "x": 2.0548278280657497, + "y": 3.7647181425608234 + } + }, + "velocity": 2.602858429371492, + "acceleration": -3.00000000000005, + "curvature": 0.43937422680447413, + "holonomicRotation": 32.17413318885111, + "angularVelocity": 1.1415907828044285, + "holonomicAngularVelocity": 122.01754683195679 + }, + { + "time": 0.91097132779699, + "pose": { + "rotation": { + "radians": 0.3364190468831634 + }, + "translation": { + "x": 2.06573424909525, + "y": 3.7685322589173373 + } + }, + "velocity": 2.589507159977005, + "acceleration": -2.9999999999999503, + "curvature": 0.4444308503282336, + "holonomicRotation": 32.71716290172041, + "angularVelocity": 1.1487861865184061, + "holonomicAngularVelocity": 122.01471948451228 + }, + { + "time": 0.9154377505166148, + "pose": { + "rotation": { + "radians": 0.34155001160662035 + }, + "translation": { + "x": 2.076603806194419, + "y": 3.772396188644361 + } + }, + "velocity": 2.576107891818131, + "acceleration": -2.999999999999975, + "curvature": 0.4495501996821507, + "holonomicRotation": 33.26213221695468, + "angularVelocity": 1.1559854981829414, + "holonomicAngularVelocity": 121.98682501486593 + }, + { + "time": 0.9199203891996058, + "pose": { + "rotation": { + "radians": 0.34673187691775187 + }, + "translation": { + "x": 2.0874359931782718, + "y": 3.7763101752699755 + } + }, + "velocity": 2.562659975769158, + "acceleration": -2.9999999999999503, + "curvature": 0.4547323176058648, + "holonomicRotation": 33.80895507758158, + "angularVelocity": 1.1631852399490095, + "holonomicAngularVelocity": 121.93384297290179 + }, + { + "time": 0.9244194687248265, + "pose": { + "rotation": { + "radians": 0.35196513981484534 + }, + "translation": { + "x": 2.0982303038618237, + "y": 3.7802744623222626 + } + }, + "velocity": 2.549162737193496, + "acceleration": -3.0, + "curvature": 0.45997720094103783, + "holonomicRotation": 34.35754513393243, + "angularVelocity": 1.1703817702780115, + "holonomicAngularVelocity": 121.8557553849208 + }, + { + "time": 0.9289352227949338, + "pose": { + "rotation": { + "radians": 0.35725029605755765 + }, + "translation": { + "x": 2.108986232060089, + "y": 3.7842892933293037 + } + }, + "velocity": 2.535615474983174, + "acceleration": -3.0, + "curvature": 0.46528479796284883, + "holonomicRotation": 34.90781575727789, + "angularVelocity": 1.1775712790493746, + "holonomicAngularVelocity": 121.75254685600395 + }, + { + "time": 0.9334678942706475, + "pose": { + "rotation": { + "radians": 0.3625878398047244 + }, + "translation": { + "x": 2.1197032715880835, + "y": 3.788354911819181 + } + }, + "velocity": 2.5220174605560333, + "acceleration": -2.999999999999951, + "curvature": 0.4706550059298928, + "holonomicRotation": 35.45968005350759, + "angularVelocity": 1.1847497824518636, + "holonomicAngularVelocity": 121.6242046774087 + }, + { + "time": 0.9380177355198616, + "pose": { + "rotation": { + "radians": 0.36797826323492133 + }, + "translation": { + "x": 2.130380916260822, + "y": 3.792471561319975 + } + }, + "velocity": 2.508367936808391, + "acceleration": -2.9999999999999756, + "curvature": 0.47608766817867815, + "holonomicRotation": 36.01305087685172, + "angularVelocity": 1.191913118109341, + "holonomicAngularVelocity": 121.47071893885429 + }, + { + "time": 0.9425850087823997, + "pose": { + "rotation": { + "radians": 0.3734220561505306 + }, + "translation": { + "x": 2.14101865989332, + "y": 3.7966394853597683 + } + }, + "velocity": 2.4946661170207767, + "acceleration": -3.0, + "curvature": 0.4815825712862567, + "holonomicRotation": 36.567840843642436, + "angularVelocity": 1.1990569397193662, + "holonomicAngularVelocity": 121.29208264592796 + }, + { + "time": 0.9471699865512587, + "pose": { + "rotation": { + "radians": 0.37891970556274 + }, + "translation": { + "x": 2.1516159963005914, + "y": 3.8008589274666416 + } + }, + "velocity": 2.4809111837141997, + "acceleration": -2.9999999999999756, + "curvature": 0.4871394423167028, + "holonomicRotation": 37.12396234611263, + "angularVelocity": 1.2061767123340414, + "holonomicAngularVelocity": 121.08829184271038 + }, + { + "time": 0.9517729519712488, + "pose": { + "rotation": { + "radians": 0.3844716952600109 + }, + "translation": { + "x": 2.162172419297652, + "y": 3.805130131168677 + } + }, + "velocity": 2.4671022874542294, + "acceleration": -3.000000000000024, + "curvature": 0.4927579457268436, + "holonomicRotation": 37.68132756623029, + "angularVelocity": 1.21326770704613, + "holonomicAngularVelocity": 120.85934573977968 + }, + { + "time": 0.9563941992559911, + "pose": { + "rotation": { + "radians": 0.3900785053568634 + }, + "translation": { + "x": 2.1726874226995174, + "y": 3.8094533399939565 + } + }, + "velocity": 2.4532385456000023, + "acceleration": -3.000000000000024, + "curvature": 0.4984376803646599, + "holonomicRotation": 38.23984848956598, + "angularVelocity": 1.220324996056866, + "holonomicAngularVelocity": 120.60524684760273 + }, + { + "time": 0.961034034124307, + "pose": { + "rotation": { + "radians": 0.39574061182424547 + }, + "translation": { + "x": 2.183160500321202, + "y": 3.8138287974705607 + } + }, + "velocity": 2.4393190409950547, + "acceleration": -3.0, + "curvature": 0.5041781763296166, + "holonomicRotation": 38.79943691919133, + "angularVelocity": 1.2273594995345545, + "holonomicAngularVelocity": 120.32757479526596 + }, + { + "time": 0.9656927133287107, + "pose": { + "rotation": { + "radians": 0.4014584860010544 + }, + "translation": { + "x": 2.1935911459777206, + "y": 3.818256747126572 + } + }, + "velocity": 2.425406442924914, + "acceleration": -2.986382504506831, + "curvature": 0.5099788918334808, + "holonomicRotation": 39.36000448960636, + "angularVelocity": 1.2344056650303263, + "holonomicAngularVelocity": 120.03016944871452 + }, + { + "time": 0.9703703555720377, + "pose": { + "rotation": { + "radians": 0.4072325940852024 + }, + "translation": { + "x": 2.2039788534840885, + "y": 3.8227374324900714 + } + }, + "velocity": 2.411589876401709, + "acceleration": -2.9537458840326973, + "curvature": 0.515839209876031, + "holonomicRotation": 39.92146268069337, + "angularVelocity": 1.2414839156218722, + "holonomicAngularVelocity": 119.71541334139856 + }, + { + "time": 0.9750669951718074, + "pose": { + "rotation": { + "radians": 0.4130633966057893 + }, + "translation": { + "x": 2.214323116655321, + "y": 3.827271097089141 + } + }, + "velocity": 2.3978714010210127, + "acceleration": -2.9209129398323563, + "curvature": 0.5217584349674527, + "holonomicRotation": 40.48372283169538, + "angularVelocity": 1.248592889108532, + "holonomicAngularVelocity": 119.38354385696823 + }, + { + "time": 0.979782664562521, + "pose": { + "rotation": { + "radians": 0.4189513478744211 + }, + "translation": { + "x": 2.2246234293064333, + "y": 3.8318579844518617 + } + }, + "velocity": 2.38425308569492, + "acceleration": -2.887885938931768, + "curvature": 0.5277357897683702, + "holonomicRotation": 41.04669615521661, + "angularVelocity": 1.2557311548634056, + "holonomicAngularVelocity": 119.03479959258911 + }, + { + "time": 0.9845173942210365, + "pose": { + "rotation": { + "radians": 0.4248968954164747 + }, + "translation": { + "x": 2.23487928525244, + "y": 3.836498338106315 + } + }, + "velocity": 2.370737008561894, + "acceleration": -2.854667131568365, + "curvature": 0.5337704117079999, + "holonomicRotation": 41.61029375124308, + "angularVelocity": 1.2628972121620219, + "holonomicAngularVelocity": 118.6694202733467 + }, + { + "time": 0.989271212590069, + "pose": { + "rotation": { + "radians": 0.4309004793818505 + }, + "translation": { + "x": 2.2450901783083568, + "y": 3.8411924015805843 + } + }, + "velocity": 2.3573252573892143, + "acceleration": -2.821258645481045, + "curvature": 0.5398613493301961, + "holonomicRotation": 42.174426621180956, + "angularVelocity": 1.2700894881479043, + "holonomicAngularVelocity": 118.2876466477837 + }, + { + "time": 0.9940441460005254, + "pose": { + "rotation": { + "radians": 0.43696253193410106 + }, + "translation": { + "x": 2.2552556022891976, + "y": 3.8459404184027486 + } + }, + "velocity": 2.3440199287650296, + "acceleration": -2.787662739026658, + "curvature": 0.5460075591396993, + "holonomicRotation": 42.73900568191042, + "angularVelocity": 1.2773063364563315, + "holonomicAngularVelocity": 117.88972039477449 + }, + { + "time": 0.9988362185921881, + "pose": { + "rotation": { + "radians": 0.44308347662019054 + }, + "translation": { + "x": 2.2653750510099786, + "y": 3.8507426321008915 + } + }, + "velocity": 2.3308231289954664, + "acceleration": -2.7538814400522993, + "curvature": 0.5522079016400215, + "holonomicRotation": 43.303941779853, + "angularVelocity": 1.2845460351181257, + "holonomicAngularVelocity": 117.47588402985137 + }, + { + "time": 1.0036474522327303, + "pose": { + "rotation": { + "radians": 0.44926372771717604 + }, + "translation": { + "x": 2.275448018285714, + "y": 3.855599286203093 + } + }, + "velocity": 2.3177369733019524, + "acceleration": -2.71991690098823, + "curvature": 0.5584611381164556, + "holonomicRotation": 43.86914570504986, + "angularVelocity": 1.2918067851196746, + "holonomicAngularVelocity": 117.04638079070317 + }, + { + "time": 1.0084778664358856, + "pose": { + "rotation": { + "radians": 0.4555036895597504 + }, + "translation": { + "x": 2.28547399793142, + "y": 3.8605106242374356 + } + }, + "velocity": 2.3047635859336775, + "acceleration": -2.6857712035958974, + "curvature": 0.5647659269857161, + "holonomicRotation": 44.43452820524919, + "angularVelocity": 1.2990867085650026, + "holonomicAngularVelocity": 116.6014545495639 + }, + { + "time": 1.013327478277653, + "pose": { + "rotation": { + "radians": 0.4618037558450898 + }, + "translation": { + "x": 2.2954524837621104, + "y": 3.865476889732001 + } + }, + "velocity": 2.291905100460139, + "acceleration": -2.6514463204651553, + "curvature": 0.5711208200358487, + "holonomicRotation": 45.00000000000005, + "angularVelocity": 1.3063838471897808, + "holonomicAngularVelocity": 116.14134970547647 + }, + { + "time": 1.0181963023113398, + "pose": { + "rotation": { + "radians": 0.4681643089175078 + }, + "translation": { + "x": 2.305382969592801, + "y": 3.87049832621487 + } + }, + "velocity": 2.279163659116622, + "acceleration": -2.6169443083915382, + "curvature": 0.577524259126085, + "holonomicRotation": 45.56547179475091, + "angularVelocity": 1.3136961607432687, + "holonomicAngularVelocity": 115.66631107512583 + }, + { + "time": 1.0230843504814946, + "pose": { + "rotation": { + "radians": 0.4745857190321683 + }, + "translation": { + "x": 2.3152649492385065, + "y": 3.8755751772141256 + } + }, + "velocity": 2.2665414130747012, + "acceleration": -2.582267114098703, + "curvature": 0.5839745724478352, + "holonomicRotation": 46.13085429495023, + "angularVelocity": 1.3210215253598987, + "holonomicAngularVelocity": 115.17658379068665 + }, + { + "time": 1.027991632036433, + "pose": { + "rotation": { + "radians": 0.4810683435972436 + }, + "translation": { + "x": 2.3250979165142422, + "y": 3.8807076862578476 + } + }, + "velocity": 2.254040522088207, + "acceleration": -2.5474167003753343, + "curvature": 0.5904699711152717, + "holonomicRotation": 46.696058220147094, + "angularVelocity": 1.328357732343505, + "holonomicAngularVelocity": 114.67241318996534 + }, + { + "time": 1.032918153439682, + "pose": { + "rotation": { + "radians": 0.4876125263968052 + }, + "translation": { + "x": 2.334881365235023, + "y": 3.8858960968741183 + } + }, + "velocity": 2.2416631544886365, + "acceleration": -2.512394971309341, + "curvature": 0.5970085456160249, + "holonomicRotation": 47.26099431808967, + "angularVelocity": 1.3357024865477265, + "holonomicAngularVelocity": 114.15404470903117 + }, + { + "time": 1.0378639182802407, + "pose": { + "rotation": { + "radians": 0.4942185967922197 + }, + "translation": { + "x": 2.344614789215864, + "y": 3.8911406525910204 + } + }, + "velocity": 2.2294114869513537, + "acceleration": -2.477203816245118, + "curvature": 0.6035882624271405, + "holonomicRotation": 47.82557337881916, + "angularVelocity": 1.3430534051522829, + "holonomicAngularVelocity": 113.62172376696698 + }, + { + "time": 1.0428289271820144, + "pose": { + "rotation": { + "radians": 0.5008868689043582 + }, + "translation": { + "x": 2.3542976822717803, + "y": 3.8964415969366333 + } + }, + "velocity": 2.2172877042031023, + "acceleration": -2.441845118126668, + "curvature": 0.6102069607302846, + "holonomicRotation": 48.389706248757015, + "angularVelocity": 1.3504080166046994, + "holonomicAngularVelocity": 113.07569565838182 + }, + { + "time": 1.0478131777121122, + "pose": { + "rotation": { + "radians": 0.5076176407769686 + }, + "translation": { + "x": 2.3639295382177874, + "y": 3.9017991734390405 + } + }, + "velocity": 2.2052939991133176, + "acceleration": -2.4063206729596667, + "curvature": 0.6168623489972463, + "holonomicRotation": 48.95330384478349, + "angularVelocity": 1.3577637592114598, + "holonomicAngularVelocity": 112.51620543704176 + }, + { + "time": 1.0528166642884191, + "pose": { + "rotation": { + "radians": 0.5144111935199791 + }, + "translation": { + "x": 2.3735098508688997, + "y": 3.9072136256263224 + } + }, + "velocity": 2.1934325720653676, + "acceleration": -2.37063233148614, + "curvature": 0.6235520020647222, + "holonomicRotation": 49.51627716830472, + "angularVelocity": 1.3651179803976707, + "holonomicAngularVelocity": 111.94349779995073 + }, + { + "time": 1.057839378086471, + "pose": { + "rotation": { + "radians": 0.5212677904360912 + }, + "translation": { + "x": 2.383038114040132, + "y": 3.912685197026561 + } + }, + "velocity": 2.1817056310849785, + "acceleration": -2.3347818434204934, + "curvature": 0.6302733579072911, + "holonomicRotation": 50.07853731930672, + "angularVelocity": 1.37246793566263, + "holonomicAngularVelocity": 111.3578169758298 + }, + { + "time": 1.062881306945483, + "pose": { + "rotation": { + "radians": 0.5281876761289772 + }, + "translation": { + "x": 2.3925138215465003, + "y": 3.9182141311678382 + } + }, + "velocity": 2.1701153913885043, + "acceleration": -2.298770970510162, + "curvature": 0.6370237148626468, + "holonomicRotation": 50.63999551039373, + "angularVelocity": 1.3798107878877686, + "holonomicAngularVelocity": 110.75940660188803 + }, + { + "time": 1.0679424352741127, + "pose": { + "rotation": { + "radians": 0.5351710755957049 + }, + "translation": { + "x": 2.401936467203019, + "y": 3.923800671578235 + } + }, + "velocity": 2.1586640750615573, + "acceleration": -2.262601456313476, + "curvature": 0.6438002289410851, + "holonomicRotation": 51.20056308080876, + "angularVelocity": 1.3871436066633136, + "holonomicAngularVelocity": 110.14850960775404 + }, + { + "time": 1.0730227439557107, + "pose": { + "rotation": { + "radians": 0.5422181933032597 + }, + "translation": { + "x": 2.411305544824703, + "y": 3.9294450617858336 + } + }, + "velocity": 2.1473539108942203, + "acceleration": -2.2262749915777413, + "curvature": 0.6505999112142703, + "holonomicRotation": 51.76015151043412, + "angularVelocity": 1.394463367787446, + "holonomicAngularVelocity": 109.5253680946616 + }, + { + "time": 1.0781222102533539, + "pose": { + "rotation": { + "radians": 0.5493292122505897 + }, + "translation": { + "x": 2.4206205482265686, + "y": 3.935147545318714 + } + }, + "velocity": 2.1361871338676965, + "acceleration": -2.189793279285885, + "curvature": 0.6574196256038366, + "holonomicRotation": 52.318672433769805, + "angularVelocity": 1.4017669531365187, + "holonomicAngularVelocity": 108.89022321711077 + }, + { + "time": 1.0832408077146372, + "pose": { + "rotation": { + "radians": 0.5565042930182251 + }, + "translation": { + "x": 2.4298809712236293, + "y": 3.94090836570496 + } + }, + "velocity": 2.125165985087164, + "acceleration": -2.153157942951349, + "curvature": 0.6642560866170671, + "holonomicRotation": 52.87603765388748, + "angularVelocity": 1.4090511501061669, + "holonomicAngularVelocity": 108.24331505807763 + }, + { + "time": 1.0883785060766265, + "pose": { + "rotation": { + "radians": 0.5637435728040847 + }, + "translation": { + "x": 2.439086307630901, + "y": 3.946727766472652 + } + }, + "velocity": 2.1142927110054988, + "acceleration": -2.116370661639031, + "curvature": 0.671105857754649, + "holonomicRotation": 53.432159156357656, + "angularVelocity": 1.4163126520875524, + "holonomicAngularVelocity": 107.58488250693321 + }, + { + "time": 1.0935352711709692, + "pose": { + "rotation": { + "radians": 0.5710471644510458 + }, + "translation": { + "x": 2.448236051263399, + "y": 3.952605991149871 + } + }, + "velocity": 2.1035695634481897, + "acceleration": -2.079433009091889, + "curvature": 0.6779653496872295, + "holonomicRotation": 53.98694912314837, + "angularVelocity": 1.4235480584824276, + "holonomicAngularVelocity": 106.9151631380171 + }, + { + "time": 1.098711064829253, + "pose": { + "rotation": { + "radians": 0.5784151554644015 + }, + "translation": { + "x": 2.4573296959361373, + "y": 3.9585432832647 + } + }, + "velocity": 2.092998798846448, + "acceleration": -2.0423466041431677, + "curvature": 0.684830819211508, + "holonomicRotation": 54.54031994649251, + "angularVelocity": 1.4307538750320115, + "holonomicAngularVelocity": 106.23439308674577 + }, + { + "time": 1.1039058447888757, + "pose": { + "rotation": { + "radians": 0.5858476070215701 + }, + "translation": { + "x": 2.466366735464131, + "y": 3.96453988634522 + } + }, + "velocity": 2.0825826781433334, + "acceleration": -2.005112975732495, + "curvature": 0.6916983680903239, + "holonomicRotation": 55.092184242722205, + "angularVelocity": 1.4379265145967277, + "holonomicAngularVelocity": 105.54280692289525 + }, + { + "time": 1.1091195645997147, + "pose": { + "rotation": { + "radians": 0.5933445529772539 + }, + "translation": { + "x": 2.475346663662397, + "y": 3.970596043919512 + } + }, + "velocity": 2.0723234658855545, + "acceleration": -1.9677337160409976, + "curvature": 0.6985639427568586, + "holonomicRotation": 55.642454866067666, + "angularVelocity": 1.4450622981042924, + "holonomicAngularVelocity": 104.84063752676244 + }, + { + "time": 1.114352173531636, + "pose": { + "rotation": { + "radians": 0.600905998865497 + }, + "translation": { + "x": 2.484268974345949, + "y": 3.9767119995156586 + } + }, + "velocity": 2.062223430211611, + "acceleration": -1.9302103033782358, + "curvature": 0.7054233337930159, + "holonomicRotation": 56.19104492241852, + "angularVelocity": 1.4521574553278018, + "holonomicAngularVelocity": 104.128115966359 + }, + { + "time": 1.1196036164829668, + "pose": { + "rotation": { + "radians": 0.6085319208985007 + }, + "translation": { + "x": 2.4931331613298013, + "y": 3.9828879966617396 + } + }, + "velocity": 2.052284841943108, + "acceleration": -1.8925442703294657, + "curvature": 0.7122721763885163, + "holonomicRotation": 56.73786778304542, + "angularVelocity": 1.4592081266024142, + "holonomicAngularVelocity": 103.40547136831155 + }, + { + "time": 1.1248738338904498, + "pose": { + "rotation": { + "radians": 0.6162222649684614 + }, + "translation": { + "x": 2.501938718428971, + "y": 3.9891242788858388 + } + }, + "velocity": 2.042509974283066, + "acceleration": -1.8547370827933478, + "curvature": 0.7191059508084997, + "holonomicRotation": 57.28283709827968, + "angularVelocity": 1.4662103641391377, + "holonomicAngularVelocity": 102.6729307941759 + }, + { + "time": 1.1301627616406267, + "pose": { + "rotation": { + "radians": 0.6239769456509539 + }, + "translation": { + "x": 2.510685139458471, + "y": 3.9954210897160363 + } + }, + "velocity": 2.0329011020603422, + "acceleration": -1.8167902222530004, + "curvature": 0.7259199836089303, + "holonomicRotation": 57.825866811148984, + "angularVelocity": 1.4731601340928637, + "holonomicAngularVelocity": 101.93071911786525 + }, + { + "time": 1.1354703309828194, + "pose": { + "rotation": { + "radians": 0.6317958452148056 + }, + "translation": { + "x": 2.519371918233318, + "y": 4.001778672680414 + } + }, + "velocity": 2.023460501427057, + "acceleration": -1.7787050954260963, + "curvature": 0.7327094489985542, + "holonomicRotation": 58.36687117096662, + "angularVelocity": 1.4800533186565177, + "holonomicAngularVelocity": 101.17905889902231 + }, + { + "time": 1.1407964684441678, + "pose": { + "rotation": { + "radians": 0.6396788126400952 + }, + "translation": { + "x": 2.527998548568526, + "y": 4.008197271307054 + } + }, + "velocity": 2.0141904488627254, + "acceleration": -1.7404831609405098, + "curvature": 0.7394693711805369, + "holonomicRotation": 58.905764746872684, + "angularVelocity": 1.4868857184770778, + "holonomicAngularVelocity": 100.41817025860232 + }, + { + "time": 1.1461410957468727, + "pose": { + "rotation": { + "radians": 0.6476256626470698 + }, + "translation": { + "x": 2.53656452427911, + "y": 4.0146771291240375 + } + }, + "velocity": 2.005093220898503, + "acceleration": -1.7021257889428896, + "curvature": 0.7461946266930923, + "holonomicRotation": 59.44246244132448, + "angularVelocity": 1.4936530554391196, + "holonomicAngularVelocity": 99.64827075825995 + }, + { + "time": 1.1515041297277275, + "pose": { + "rotation": { + "radians": 0.6556361747389974 + }, + "translation": { + "x": 2.5450693391800865, + "y": 4.021218489659446 + } + }, + "velocity": 1.9961710931849446, + "acceleration": -1.6636343803543105, + "curvature": 0.752879947758616, + "holonomicRotation": 59.97687950353445, + "angularVelocity": 1.500350975697925, + "holonomicAngularVelocity": 98.86957527277421 + }, + { + "time": 1.1568854822606072, + "pose": { + "rotation": { + "radians": 0.6637100922622778 + }, + "translation": { + "x": 2.5535124870864694, + "y": 4.02782159644136 + } + }, + "velocity": 1.9874263398522192, + "acceleration": -1.6250103072221436, + "curvature": 0.7595199259827242, + "holonomicRotation": 60.508931542853325, + "angularVelocity": 1.5069750531000181, + "holonomicAngularVelocity": 98.08229587625448 + }, + { + "time": 1.162285060181491, + "pose": { + "rotation": { + "radians": 0.6718471214863193 + }, + "translation": { + "x": 2.5618934618132734, + "y": 4.034486692997864 + } + }, + "velocity": 1.9788612330364417, + "acceleration": -1.5862548779323185, + "curvature": 0.766109016499769, + "holonomicRotation": 61.038534542096336, + "angularVelocity": 1.5135207926411571, + "holonomicAngularVelocity": 97.2866417166188 + }, + { + "time": 1.167702765216983, + "pose": { + "rotation": { + "radians": 0.6800469307059331 + }, + "translation": { + "x": 2.5702117571755148, + "y": 4.041214022857036 + } + }, + "velocity": 1.970478041658806, + "acceleration": -1.5473694715228203, + "curvature": 0.7726415432872881, + "holonomicRotation": 61.56560487081057, + "angularVelocity": 1.5199836349511098, + "holonomicAngularVelocity": 96.48281889831824 + }, + { + "time": 1.173138493916237, + "pose": { + "rotation": { + "radians": 0.6883091493728335 + }, + "translation": { + "x": 2.578466866988207, + "y": 4.0480038295469605 + } + }, + "velocity": 1.9622790311765919, + "acceleration": -1.508355353227819, + "curvature": 0.7791117043521054, + "holonomicRotation": 62.090059298481094, + "angularVelocity": 1.526358960058976, + "holonomicAngularVelocity": 95.67103036918452 + }, + { + "time": 1.178592137586423, + "pose": { + "rotation": { + "radians": 0.6966333672537908 + }, + "translation": { + "x": 2.586658285066367, + "y": 4.054856356595717 + } + }, + "velocity": 1.9542664624020496, + "acceleration": -1.4692138429111983, + "curvature": 0.7855135782652968, + "holonomicRotation": 62.61181500767417, + "angularVelocity": 1.5326420925587887, + "holonomicAngularVelocity": 94.85147580198162 + }, + { + "time": 1.18406358223241, + "pose": { + "rotation": { + "radians": 0.7050191336253357 + }, + "translation": { + "x": 2.5947855052250084, + "y": 4.061771847531389 + } + }, + "velocity": 1.946442590910683, + "acceleration": -1.4299462020702385, + "curvature": 0.791841130883845, + "holonomicRotation": 63.13078960711488, + "angularVelocity": 1.538828306323637, + "holonomicAngularVelocity": 94.02435148577685 + }, + { + "time": 1.189552708500524, + "pose": { + "rotation": { + "radians": 0.7134659565036943 + }, + "translation": { + "x": 2.6028480212791467, + "y": 4.068750545882057 + } + }, + "velocity": 1.9388096661170229, + "acceleration": -1.3905536912129923, + "curvature": 0.7980882229865759, + "holonomicRotation": 63.64690114469784, + "angularVelocity": 1.5449128300065775, + "holonomicAngularVelocity": 93.18985021411004 + }, + { + "time": 1.1950593916269745, + "pose": { + "rotation": { + "radians": 0.7219733019165284 + }, + "translation": { + "x": 2.6108453270437972, + "y": 4.075792695175802 + } + }, + "velocity": 1.9313699304026128, + "acceleration": -1.3510375562876273, + "curvature": 0.8042486185327693, + "holonomicRotation": 64.16006812042833, + "angularVelocity": 1.5508908527558884, + "holonomicAngularVelocity": 92.34816117906108 + }, + { + "time": 1.2005835013910504, + "pose": { + "rotation": { + "radians": 0.7305405932192532 + }, + "translation": { + "x": 2.618776916333975, + "y": 4.082898538940706 + } + }, + "velocity": 1.924125618274244, + "acceleration": -1.31139901952701, + "curvature": 0.8103159935614521, + "holonomicRotation": 64.67020949929204, + "angularVelocity": 1.5567575301253238, + "holonomicAngularVelocity": 91.49946986965955 + }, + { + "time": 1.206124902073281, + "pose": { + "rotation": { + "radians": 0.7391672104587572 + }, + "translation": { + "x": 2.6266422829646943, + "y": 4.0900683207048525 + } + }, + "velocity": 1.917078955499023, + "acceleration": -1.271639280266702, + "curvature": 0.8162839457746397, + "holonomicRotation": 65.1772447240515, + "angularVelocity": 1.5625079904987484, + "holonomicAngularVelocity": 90.6439579687776 + }, + { + "time": 1.2116834524191191, + "pose": { + "rotation": { + "radians": 0.747852489789719 + }, + "translation": { + "x": 2.634440920750971, + "y": 4.09730228399632 + } + }, + "velocity": 1.9102321580214021, + "acceleration": -1.2317595508957315, + "curvature": 0.8221460049700963, + "holonomicRotation": 65.681093727967, + "angularVelocity": 1.5681373419540958, + "holonomicAngularVelocity": 89.78180325772642 + }, + { + "time": 1.2172590056081773, + "pose": { + "rotation": { + "radians": 0.7565957229475324 + }, + "translation": { + "x": 2.6421723235078196, + "y": 4.104600672343191 + } + }, + "velocity": 1.9035874312272603, + "acceleration": -1.1917609910316764, + "curvature": 0.8278956438384681, + "holonomicRotation": 66.18167694744001, + "angularVelocity": 1.573640678936801, + "holonomicAngularVelocity": 88.91317952568637 + }, + { + "time": 1.2228514092292129, + "pose": { + "rotation": { + "radians": 0.7653961567786274 + }, + "translation": { + "x": 2.649835985050256, + "y": 4.1119637292735485 + } + }, + "velocity": 1.8971469688632474, + "acceleration": -1.1516447668024896, + "curvature": 0.8335262896990334, + "holonomicRotation": 66.67891533457724, + "angularVelocity": 1.579013089939411, + "holonomicAngularVelocity": 88.03825647813974 + }, + { + "time": 1.2284605052614423, + "pose": { + "rotation": { + "radians": 0.7742529928362449 + }, + "translation": { + "x": 2.6574313991932947, + "y": 4.119391698315472 + } + }, + "velocity": 1.890912952045104, + "acceleration": -1.111412031871654, + "curvature": 0.8390313367951864, + "holonomicRotation": 67.17273036967318, + "angularVelocity": 1.5842496649074382, + "holonomicAngularVelocity": 87.15719965492562 + }, + { + "time": 1.234086130062117, + "pose": { + "rotation": { + "radians": 0.7831653870416089 + }, + "translation": { + "x": 2.6649580597519504, + "y": 4.126884822997045 + } + }, + "velocity": 1.884887548364822, + "acceleration": -1.0710639073475614, + "curvature": 0.8444041591150093, + "holonomicRotation": 67.66304407360929, + "angularVelocity": 1.589345503179362, + "holonomicAngularVelocity": 86.27017034976315 + }, + { + "time": 1.2397281143608059, + "pose": { + "rotation": { + "radians": 0.7921324494157385 + }, + "translation": { + "x": 2.6724154605412394, + "y": 4.134443346846348 + } + }, + "velocity": 1.8790729107255957, + "acceleration": -1.0306015280080867, + "curvature": 0.8496381240420783, + "holonomicRotation": 68.14977902016786, + "angularVelocity": 1.5942957218618874, + "holonomicAngularVelocity": 85.37732553857849 + }, + { + "time": 1.245386283260372, + "pose": { + "rotation": { + "radians": 0.8011532438858886 + }, + "translation": { + "x": 2.679803095376175, + "y": 4.142067513391463 + } + }, + "velocity": 1.8734711765865795, + "acceleration": -0.9900259674902657, + "curvature": 0.8547266062009597, + "holonomicRotation": 68.63285834825837, + "angularVelocity": 1.5990954640501065, + "holonomicAngularVelocity": 84.47881780955483 + }, + { + "time": 1.2510604562450975, + "pose": { + "rotation": { + "radians": 0.810226788167999 + }, + "translation": { + "x": 2.6871204580717736, + "y": 4.1497575661604715 + } + }, + "velocity": 1.86808446661834, + "acceleration": -0.949338341065735, + "curvature": 0.8596630023597044, + "holonomicRotation": 69.1122057740549, + "angularVelocity": 1.6037399075864955, + "holonomicAngularVelocity": 83.57479529775051 + }, + { + "time": 1.2567504471962336, + "pose": { + "rotation": { + "radians": 0.819352053730142 + }, + "translation": { + "x": 2.6943670424430497, + "y": 4.157513748681454 + } + }, + "velocity": 1.862914883876036, + "acceleration": -0.9085397124000278, + "curvature": 0.8644407463563594, + "holonomicRotation": 69.58774560304215, + "angularVelocity": 1.6082242740958255, + "holonomicAngularVelocity": 82.66540163083593 + }, + { + "time": 1.2624560644148213, + "pose": { + "rotation": { + "radians": 0.8285279658397737 + }, + "translation": { + "x": 2.701542342305019, + "y": 4.165336304482494 + } + }, + "velocity": 1.8579645126741293, + "acceleration": -0.867631145282505, + "curvature": 0.8690533247412898, + "holonomicRotation": 70.05940274196851, + "angularVelocity": 1.6125438378778367, + "holonomicAngularVelocity": 81.75077587745383 + }, + { + "time": 1.2681771106521889, + "pose": { + "rotation": { + "radians": 0.837753403696055 + }, + "translation": { + "x": 2.708645851472695, + "y": 4.173225477091672 + } + }, + "velocity": 1.8532354175856258, + "acceleration": -0.826613680836024, + "curvature": 0.8734942927047408, + "holonomicRotation": 70.5271027107041, + "angularVelocity": 1.6166939352853913, + "holonomicAngularVelocity": 80.83105250208973 + }, + { + "time": 1.273913383148282, + "pose": { + "rotation": { + "radians": 0.8470272006516333 + }, + "translation": { + "x": 2.715677063761095, + "y": 4.18118151003707 + } + }, + "velocity": 1.8487296423162756, + "acceleration": -0.7854883589332607, + "curvature": 0.8777572904677117, + "holonomicRotation": 70.9907716540021, + "angularVelocity": 1.6206699741819475, + "holonomicAngularVelocity": 79.90636132658405 + }, + { + "time": 1.279664673677968, + "pose": { + "rotation": { + "radians": 0.8563481445258923 + }, + "translation": { + "x": 2.7226354729852322, + "y": 4.189204646846768 + } + }, + "velocity": 1.8444492086921047, + "acceleration": -0.7442561981657716, + "curvature": 0.881836059857402, + "holonomicRotation": 71.45033635316135, + "angularVelocity": 1.624467443317504, + "holonomicAngularVelocity": 78.97682749979187 + }, + { + "time": 1.2854307686053472, + "pose": { + "rotation": { + "radians": 0.865714978010498 + }, + "translation": { + "x": 2.729520572960122, + "y": 4.1972951310488495 + } + }, + "velocity": 1.8403961155928805, + "acceleration": -0.7029182055222216, + "curvature": 0.8857244611617751, + "holonomicRotation": 71.9057242375884, + "angularVelocity": 1.6280819220161982, + "holonomicAngularVelocity": 78.04257147270275 + }, + { + "time": 1.2912114489462903, + "pose": { + "rotation": { + "radians": 0.8751263991705418 + }, + "translation": { + "x": 2.7363318575007805, + "y": 4.205453206171397 + } + }, + "velocity": 1.8365723378633072, + "acceleration": -0.661475380759339, + "curvature": 0.8894164901698529, + "holonomicRotation": 72.3568633962573, + "angularVelocity": 1.631509089802121, + "holonomicAngularVelocity": 77.10370898187273 + }, + { + "time": 1.297006490439208, + "pose": { + "rotation": { + "radians": 0.8845810620420176 + }, + "translation": { + "x": 2.7430688204222218, + "y": 4.213679115742488 + } + }, + "velocity": 1.832979825296675, + "acceleration": -0.619928704742282, + "curvature": 0.8929062952374428, + "holonomicRotation": 72.8036825890651, + "angularVelocity": 1.6347447361535672, + "holonomicAngularVelocity": 76.16035104140812 + }, + { + "time": 1.3028156636240313, + "pose": { + "rotation": { + "radians": 0.894077577327312 + }, + "translation": { + "x": 2.7497309555394613, + "y": 4.221973103290208 + } + }, + "velocity": 1.8296205016132843, + "acceleration": -0.5782791417145218, + "curvature": 0.8961881943792127, + "holonomicRotation": 73.24611125808158, + "angularVelocity": 1.6377847699871186, + "holonomicAngularVelocity": 75.21260394021503 + }, + { + "time": 1.30863873392961, + "pose": { + "rotation": { + "radians": 0.9036145131883528 + }, + "translation": { + "x": 2.7563177566675137, + "y": 4.230335412342637 + } + }, + "velocity": 1.826496263335565, + "acceleration": -0.5365276587380652, + "curvature": 0.8992566924089129, + "holonomicRotation": 73.6840795386911, + "angularVelocity": 1.6406252293242067, + "holonomicAngularVelocity": 74.26056924841903 + }, + { + "time": 1.314475461769441, + "pose": { + "rotation": { + "radians": 0.9131903961390786 + }, + "translation": { + "x": 2.7628287176213946, + "y": 4.238766286427857 + } + }, + "velocity": 1.8236089788536598, + "acceleration": -0.49467519492716594, + "curvature": 0.9021064977629244, + "holonomicRotation": 74.11751827062504, + "angularVelocity": 1.643262290855695, + "holonomicAngularVelocity": 73.30434383206303 + }, + { + "time": 1.320325602645683, + "pose": { + "rotation": { + "radians": 0.9228037120372008 + }, + "translation": { + "x": 2.7692633322161186, + "y": 4.247265969073948 + } + }, + "velocity": 1.8209604873361642, + "acceleration": -0.45272269053405195, + "curvature": 0.9047325392640337, + "holonomicRotation": 74.5463590088831, + "angularVelocity": 1.6456922792088542, + "holonomicAngularVelocity": 72.34401987525794 + }, + { + "time": 1.3261889072614577, + "pose": { + "rotation": { + "radians": 0.9324529071740306 + }, + "translation": { + "x": 2.7756210942667012, + "y": 4.255834703808993 + } + }, + "velocity": 1.8185525977962105, + "acceleration": -0.4106710631195233, + "curvature": 0.9071299824396285, + "holonomicRotation": 74.97053403454139, + "angularVelocity": 1.6479116761303385, + "holonomicAngularVelocity": 71.3796849104472 + }, + { + "time": 1.3320651216413755, + "pose": { + "rotation": { + "radians": 0.9421363894621422 + }, + "translation": { + "x": 2.781901497588157, + "y": 4.264472734161075 + } + }, + "velocity": 1.8163870880234456, + "acceleration": -0.3685212336986165, + "curvature": 0.9092942455973303, + "holonomicRotation": 75.38997636544616, + "angularVelocity": 1.6499171295340616, + "holonomicAngularVelocity": 70.4114218583858 + }, + { + "time": 1.3379539872600648, + "pose": { + "rotation": { + "radians": 0.9518525297199418 + }, + "translation": { + "x": 2.788104035995501, + "y": 4.273180303658273 + } + }, + "velocity": 1.8144657037678693, + "acceleration": -0.32627408740292496, + "curvature": 0.9112210151912336, + "holonomicRotation": 75.80461976679103, + "angularVelocity": 1.6517054622237413, + "holonomicAngularVelocity": 69.439309074703 + }, + { + "time": 1.343855241178712, + "pose": { + "rotation": { + "radians": 0.9615996630513406 + }, + "translation": { + "x": 2.7942282033037484, + "y": 4.281957655828669 + } + }, + "velocity": 1.8127901576232959, + "acceleration": -0.28393052860832513, + "curvature": 0.9129062609469006, + "holonomicRotation": 76.21439876157628, + "angularVelocity": 1.6532736802559593, + "holonomicAngularVelocity": 68.46342040422189 + }, + { + "time": 1.3497686161895086, + "pose": { + "rotation": { + "radians": 0.9713760903181741 + }, + "translation": { + "x": 2.800273493327915, + "y": 4.290805034200345 + } + }, + "velocity": 1.8113621282279633, + "acceleration": -0.24149143132733641, + "curvature": 0.914346250046392, + "holonomicRotation": 76.61924864094827, + "angularVelocity": 1.6546189811787644, + "holonomicAngularVelocity": 67.48382524759128 + }, + { + "time": 1.3556938409674844, + "pose": { + "rotation": { + "radians": 0.9811800797035635 + }, + "translation": { + "x": 2.8062393998830153, + "y": 4.299722682301384 + } + }, + "velocity": 1.8101832594033358, + "acceleration": -0.19895765457023182, + "curvature": 0.9155375606708208, + "holonomicRotation": 77.01910547441788, + "angularVelocity": 1.655738761630261, + "holonomicAngularVelocity": 66.50058863161212 + }, + { + "time": 1.3616306402298943, + "pose": { + "rotation": { + "radians": 0.9910098683623536 + }, + "translation": { + "x": 2.8121254167840637, + "y": 4.308710843659867 + } + }, + "velocity": 1.8092551591757633, + "acceleration": -0.15633006718771086, + "curvature": 0.9164770948804419, + "holonomicRotation": 77.41390611995587, + "angularVelocity": 1.6566306247801388, + "holonomicAngularVelocity": 65.51377128920768 + }, + { + "time": 1.3675787349028061, + "pose": { + "rotation": { + "radians": 1.000863664156591 + }, + "translation": { + "x": 2.8179310378460762, + "y": 4.3177697618038735 + } + }, + "velocity": 1.8085793990451318, + "acceleration": -0.11360951158175212, + "curvature": 0.9171620903893286, + "holonomicRotation": 77.80358823396357, + "angularVelocity": 1.6572923872679133, + "holonomicAngularVelocity": 64.52342974920651 + }, + { + "time": 1.3735378422944748, + "pose": { + "rotation": { + "radians": 1.0107396474717154 + }, + "translation": { + "x": 2.8236557568840674, + "y": 4.326899680261486 + } + }, + "velocity": 1.8081575131573642, + "acceleration": -0.0707968257725009, + "curvature": 0.917590131509392, + "holonomicRotation": 78.18809028111788, + "angularVelocity": 1.657722085504442, + "holonomicAngularVelocity": 63.52961643318937 + }, + { + "time": 1.37950767627528, + "pose": { + "rotation": { + "radians": 1.020635973108491 + }, + "translation": { + "x": 2.8292990677130523, + "y": 4.336100842560788 + } + }, + "velocity": 1.807990997598541, + "acceleration": -0.027892829073383706, + "curvature": 0.9177591589767008, + "holonomicRotation": 78.56735154408825, + "angularVelocity": 1.6579179816540077, + "holonomicAngularVelocity": 62.53237975938081 + }, + { + "time": 1.3854879474639896, + "pose": { + "rotation": { + "radians": 1.03055077224742 + }, + "translation": { + "x": 2.834860464148046, + "y": 4.345373492229859 + } + }, + "velocity": 1.8080813095853832, + "acceleration": 0.015101654087621652, + "curvature": 0.9176674788264284, + "holonomicRotation": 78.94131213312473, + "angularVelocity": 1.6578785690903888, + "holonomicAngularVelocity": 61.53176425402279 + }, + { + "time": 1.391478363420028, + "pose": { + "rotation": { + "radians": 1.0404821544808733 + }, + "translation": { + "x": 2.8403394400040645, + "y": 4.354717872796781 + } + }, + "velocity": 1.8084298668271106, + "acceleration": 0.05818581619129324, + "curvature": 0.9173137699807672, + "holonomicRotation": 79.30991299551522, + "angularVelocity": 1.6576025771005691, + "holonomicAngularVelocity": 60.527810672655114 + }, + { + "time": 1.39747862884115, + "pose": { + "rotation": { + "radians": 1.0504282099062126 + }, + "translation": { + "x": 2.8457354890961217, + "y": 4.364134227789636 + } + }, + "velocity": 1.8090380469286687, + "acceleration": 0.10135886646233248, + "curvature": 0.9166970906312715, + "holonomicRotation": 79.67309592491057, + "angularVelocity": 1.6570889751192701, + "holonomicAngularVelocity": 59.52055612693776 + }, + { + "time": 1.403488445766292, + "pose": { + "rotation": { + "radians": 1.0603870112753504 + }, + "translation": { + "x": 2.8510481052392325, + "y": 4.373622800736505 + } + }, + "velocity": 1.8099071867381205, + "acceleration": 0.14462001426628804, + "curvature": 0.9158168834705848, + "holonomicRotation": 80.0308035705161, + "angularVelocity": 1.6563369764275115, + "holonomicAngularVelocity": 58.510034217747226 + }, + { + "time": 1.409507513783261, + "pose": { + "rotation": { + "radians": 1.0703566161954883 + }, + "translation": { + "x": 2.856276782248413, + "y": 4.38318383516547 + } + }, + "velocity": 1.8110385817969141, + "acceleration": 0.18796847877512687, + "curvature": 0.9146729795756269, + "holonomicRotation": 80.3829794461479, + "angularVelocity": 1.655346041048964, + "holonomicAngularVelocity": 57.49627517669591 + }, + { + "time": 1.4155355302406682, + "pose": { + "rotation": { + "radians": 1.0803350693736355 + }, + "translation": { + "x": 2.8614210139386773, + "y": 4.392817574604613 + } + }, + "velocity": 1.8124334858675517, + "acceleration": 0.2314034940836123, + "curvature": 0.913265600939143, + "holonomicRotation": 80.72956793915264, + "angularVelocity": 1.6541158780443799, + "holonomicAngularVelocity": 56.47930601365878 + }, + { + "time": 1.4215721904636955, + "pose": { + "rotation": { + "radians": 1.090320404898904 + }, + "translation": { + "x": 2.866480294125041, + "y": 4.4025242625820145 + } + }, + "velocity": 1.814093110459821, + "acceleration": 0.27492430101309223, + "curvature": 0.9115953617080983, + "holonomicRotation": 81.07051431918948, + "angularVelocity": 1.6526464472421345, + "holonomicAngularVelocity": 55.45915066950544 + }, + { + "time": 1.4276171879733226, + "pose": { + "rotation": { + "radians": 1.1003106485567766 + }, + "translation": { + "x": 2.8714541166225196, + "y": 4.412304142625757 + } + }, + "velocity": 1.8160186243942888, + "acceleration": 0.3185301451987892, + "curvature": 0.9096632680760596, + "holonomicRotation": 81.40576474687268, + "angularVelocity": 1.6509379601583025, + "holonomicAngularVelocity": 54.43583017529773 + }, + { + "time": 1.4336702147084632, + "pose": { + "rotation": { + "radians": 1.1103038201676734 + }, + "translation": { + "x": 2.876341975246127, + "y": 4.422157458263921 + } + }, + "velocity": 1.818211153470297, + "acceleration": 0.36222028613874074, + "curvature": 0.9074707167569798, + "holonomicRotation": 81.73526628227333, + "angularVelocity": 1.6489908802443665, + "holonomicAngularVelocity": 53.409362817062245 + }, + { + "time": 1.439730961250501, + "pose": { + "rotation": { + "radians": 1.1202979359429661 + }, + "translation": { + "x": 2.881143363810879, + "y": 4.43208445302459 + } + }, + "velocity": 1.8206717801454617, + "acceleration": 0.4059939906903755, + "curvature": 0.9050194921374667, + "holonomicRotation": 82.05896689327928, + "angularVelocity": 1.646805922558907, + "holonomicAngularVelocity": 52.379764305061734 + }, + { + "time": 1.4457991170497746, + "pose": { + "rotation": { + "radians": 1.1302910108522202 + }, + "translation": { + "x": 2.8858577761317905, + "y": 4.442085370435843 + } + }, + "velocity": 1.8234015432666075, + "acceleration": 0.4498505330849536, + "curvature": 0.9023117620787452, + "holonomicRotation": 82.37681546381162, + "angularVelocity": 1.6393738158270388, + "holonomicAngularVelocity": 51.19059978089827 + }, + { + "time": 1.4518929377789702, + "pose": { + "rotation": { + "radians": 1.1402810609940075 + }, + "translation": { + "x": 2.8904847060238765, + "y": 4.452160454025765 + } + }, + "velocity": 1.8152809351623431, + "acceleration": -1.332597144736865, + "curvature": 0.8993500723685289, + "holonomicRotation": 82.68876180189642, + "angularVelocity": 1.6219775507255298, + "holonomicAngularVelocity": 49.70601253218088 + }, + { + "time": 1.4580490309778806, + "pose": { + "rotation": { + "radians": 1.1502661059628143 + }, + "translation": { + "x": 2.8950236473021524, + "y": 4.462309947322433 + } + }, + "velocity": 1.7968126555656119, + "acceleration": -3.0, + "curvature": 0.8961373399400934, + "holonomicRotation": 82.99475664759073, + "angularVelocity": 1.5994721218793286, + "holonomicAngularVelocity": 48.088850948024216 + }, + { + "time": 1.4642873799352032, + "pose": { + "rotation": { + "radians": 1.1602441712066067 + }, + "translation": { + "x": 2.8994740937816323, + "y": 4.472534093853932 + } + }, + "velocity": 1.7780976086936442, + "acceleration": -3.0, + "curvature": 0.8926768446185156, + "holonomicRotation": 83.29475168076118, + "angularVelocity": 1.5764262934325932, + "holonomicAngularVelocity": 46.482252747741015 + }, + { + "time": 1.470611252608683, + "pose": { + "rotation": { + "radians": 1.1702132903654001 + }, + "translation": { + "x": 2.9038355392773325, + "y": 4.482833137148344 + } + }, + "velocity": 1.7591259906732046, + "acceleration": -3.000000000000035, + "curvature": 0.8889722200024526, + "holonomicRotation": 83.5886995287144, + "angularVelocity": 1.5528541730227239, + "holonomicAngularVelocity": 44.88711739884929 + }, + { + "time": 1.4770241002505906, + "pose": { + "rotation": { + "radians": 1.1801715075870953 + }, + "translation": { + "x": 2.9081074776042675, + "y": 4.493207320733747 + } + }, + "velocity": 1.7398874477474815, + "acceleration": -3.0000000000000346, + "curvature": 0.8850274427968513, + "holonomicRotation": 83.87655377367764, + "angularVelocity": 1.5287702004538215, + "holonomicAngularVelocity": 43.30434018507438 + }, + { + "time": 1.4835295725715414, + "pose": { + "rotation": { + "radians": 1.190116879811242 + }, + "translation": { + "x": 2.9122894025774526, + "y": 4.503656888138226 + } + }, + "velocity": 1.7203710307846292, + "acceleration": -3.0, + "curvature": 0.8808468212891777, + "holonomicRotation": 84.15826896012868, + "angularVelocity": 1.504189078344182, + "holonomicAngularVelocity": 41.734811554628145 + }, + { + "time": 1.4901315346166932, + "pose": { + "rotation": { + "radians": 1.2000474790152023 + }, + "translation": { + "x": 2.9163808080119025, + "y": 4.514182082889862 + } + }, + "velocity": 1.7005651446491736, + "acceleration": -3.0, + "curvature": 0.8764349826621125, + "holonomicRotation": 84.4338006019739, + "angularVelocity": 1.4791256986724945, + "holonomicAngularVelocity": 40.179416523208545 + }, + { + "time": 1.496834085596832, + "pose": { + "rotation": { + "radians": 1.2099613944165881 + }, + "translation": { + "x": 2.920381187722632, + "y": 4.524783148516736 + } + }, + "velocity": 1.6804574917087571, + "acceleration": -3.0, + "curvature": 0.8717968593211561, + "holonomicRotation": 84.70310518957294, + "angularVelocity": 1.4535950649122817, + "holonomicAngularVelocity": 38.639034138616516 + }, + { + "time": 1.50364157995844, + "pose": { + "rotation": { + "radians": 1.2198567346250395 + }, + "translation": { + "x": 2.9242900355246566, + "y": 4.535460328546927 + } + }, + "velocity": 1.6600350086239335, + "acceleration": -3.0, + "curvature": 0.8669376744563995, + "holonomicRotation": 84.96614019660954, + "angularVelocity": 1.4276122098245985, + "holonomicAngularVelocity": 37.11453701340954 + }, + { + "time": 1.5105586510269702, + "pose": { + "rotation": { + "radians": 1.2297316297386978 + }, + "translation": { + "x": 2.9281068452329917, + "y": 4.546213866508522 + } + }, + "velocity": 1.639283795418343, + "acceleration": -2.999999999999968, + "curvature": 0.8618629265041059, + "holonomicRotation": 85.2228640868069, + "angularVelocity": 1.4011921081103897, + "holonomicAngularVelocity": 35.60679093330834 + }, + { + "time": 1.5175902376181007, + "pose": { + "rotation": { + "radians": 1.2395842333776848 + }, + "translation": { + "x": 2.9318311106626513, + "y": 4.5570440059295985 + } + }, + "velocity": 1.618189035644951, + "acceleration": -3.0000000000000315, + "curvature": 0.856578373185448, + "holonomicRotation": 85.47323632048673, + "angularVelocity": 1.3743495841726672, + "holonomicAngularVelocity": 34.116654548705206 + }, + { + "time": 1.5247416140854468, + "pose": { + "rotation": { + "radians": 1.249412724651844 + }, + "translation": { + "x": 2.935462325628652, + "y": 4.56795099033824 + } + }, + "velocity": 1.5967349062429128, + "acceleration": -3.0, + "curvature": 0.8510900147003576, + "holonomicRotation": 85.71721736097092, + "angularVelocity": 1.347099213871016, + "holonomicAngularVelocity": 32.644979158266835 + }, + { + "time": 1.532018424363139, + "pose": { + "rotation": { + "radians": 1.2592153100564116 + }, + "translation": { + "x": 2.9389999839460073, + "y": 4.578935063262526 + } + }, + "velocity": 1.5749044754098365, + "acceleration": -3.0, + "curvature": 0.8454040763550249, + "holonomicRotation": 85.95476868082484, + "angularVelocity": 1.3194552197086669, + "holonomicAngularVelocity": 31.19260859353292 + }, + { + "time": 1.5394267206722971, + "pose": { + "rotation": { + "radians": 1.2689902252906788 + }, + "translation": { + "x": 2.9424435794297326, + "y": 4.58999646823054 + } + }, + "velocity": 1.552679586482362, + "acceleration": -3.0, + "curvature": 0.8395269907945926, + "holonomicRotation": 86.18585276794133, + "angularVelocity": 1.2914313587824056, + "holonomicAngularVelocity": 29.760379215041628 + }, + { + "time": 1.546973007697073, + "pose": { + "rotation": { + "radians": 1.278735736996847 + }, + "translation": { + "x": 2.9457926058948445, + "y": 4.601135448770364 + } + }, + "velocity": 1.5300407254080348, + "acceleration": -2.9999999999999707, + "curvature": 0.833465379886221, + "holonomicRotation": 86.4104331314642, + "angularVelocity": 1.263040802049618, + "holonomicAngularVelocity": 28.349120031543038 + }, + { + "time": 1.5546642932058943, + "pose": { + "rotation": { + "radians": 1.2884501444147014 + }, + "translation": { + "x": 2.9490465571563558, + "y": 4.612352248410078 + } + }, + "velocity": 1.5069668688815712, + "acceleration": -2.9999999999999423, + "curvature": 0.8272260361053153, + "holonomicRotation": 86.62847430755065, + "angularVelocity": 1.2342960033859218, + "holonomicAngularVelocity": 26.959652955290828 + }, + { + "time": 1.562508146306355, + "pose": { + "rotation": { + "radians": 1.2981317809477464 + }, + "translation": { + "x": 2.952204927029283, + "y": 4.623647110677764 + } + }, + "velocity": 1.4834353095801889, + "acceleration": -3.0, + "curvature": 0.8208159040072, + "holonomicRotation": 86.83994186497135, + "angularVelocity": 1.2052085570168602, + "holonomicAngularVelocity": 25.592793208687993 + }, + { + "time": 1.5705127647903852, + "pose": { + "rotation": { + "radians": 1.3077790156403548 + }, + "translation": { + "x": 2.9552672093286407, + "y": 4.635020279101503 + } + }, + "velocity": 1.4594214541280985, + "acceleration": -3.0, + "curvature": 0.8142420613900655, + "holonomicRotation": 87.04480241054758, + "angularVelocity": 1.175789040240505, + "holonomicAngularVelocity": 24.24934989960506 + }, + { + "time": 1.5786870533670874, + "pose": { + "rotation": { + "radians": 1.3173902545606044 + }, + "translation": { + "x": 2.9582328978694443, + "y": 4.646471997209379 + } + }, + "velocity": 1.4348985883979921, + "acceleration": -2.999999999999973, + "curvature": 0.8075117004620126, + "holonomicRotation": 87.24302359442437, + "angularVelocity": 1.1460468390736178, + "holonomicAngularVelocity": 22.930126786042262 + }, + { + "time": 1.5870407150167543, + "pose": { + "rotation": { + "radians": 1.3269639420888957 + }, + "translation": { + "x": 2.9611014864667085, + "y": 4.65800250852947 + } + }, + "velocity": 1.4098376034489917, + "acceleration": -2.9999999999999734, + "curvature": 0.8006321092309213, + "holonomicRotation": 87.43457411517893, + "angularVelocity": 1.115989952587679, + "holonomicAngularVelocity": 21.635923254361483 + }, + { + "time": 1.595584358262823, + "pose": { + "rotation": { + "radians": 1.336498562110002 + }, + "translation": { + "x": 2.963872468935448, + "y": 4.669612056589861 + } + }, + "velocity": 1.3842066737107854, + "acceleration": -2.999999999999974, + "curvature": 0.7936106527983469, + "holonomicRotation": 87.61942372476352, + "angularVelocity": 1.0856247710293516, + "holonomicAngularVelocity": 20.367535540449826 + }, + { + "time": 1.6043296238922686, + "pose": { + "rotation": { + "radians": 1.3459926391065598 + }, + "translation": { + "x": 2.9665453390906786, + "y": 4.68130088491863 + } + }, + "velocity": 1.3579708768224485, + "acceleration": -3.0000000000000253, + "curvature": 0.7864547551434554, + "holonomicRotation": 87.79754323328193, + "angularVelocity": 1.0549558217086663, + "holonomicAngularVelocity": 19.125758228972256 + }, + { + "time": 1.613289335620583, + "pose": { + "rotation": { + "radians": 1.3554447391551765 + }, + "translation": { + "x": 2.969119590747415, + "y": 4.693069237043862 + } + }, + "velocity": 1.3310917416375052, + "acceleration": -3.0, + "curvature": 0.7791718809381487, + "holonomicRotation": 87.96890451359896, + "angularVelocity": 1.0239854739651646, + "holonomicAngularVelocity": 17.91138607403565 + }, + { + "time": 1.622477680481373, + "pose": { + "rotation": { + "radians": 1.3648534708224078 + }, + "translation": { + "x": 2.9715947177206727, + "y": 4.704917356493637 + } + }, + "velocity": 1.3035267070551353, + "acceleration": -3.000000000000024, + "curvature": 0.7717695180203282, + "holonomicRotation": 88.13348050578195, + "angularVelocity": 0.9927135930553639, + "holonomicAngularVelocity": 16.725216194464355 + }, + { + "time": 1.63191042644756, + "pose": { + "rotation": { + "radians": 1.3742174859628797 + }, + "translation": { + "x": 2.973970213825466, + "y": 4.716845486796037 + } + }, + "velocity": 1.2752284691565743, + "acceleration": -3.0000000000000235, + "curvature": 0.7642551600626761, + "holonomicRotation": 88.29124522137388, + "angularVelocity": 0.9611371284041571, + "holonomicAngularVelocity": 15.56805071013536 + }, + { + "time": 1.6416051871426292, + "pose": { + "rotation": { + "radians": 1.383535480417904 + }, + "translation": { + "x": 2.97624557287681, + "y": 4.728853871479143 + } + }, + "velocity": 1.2461441870713668, + "acceleration": -2.999999999999977, + "curvature": 0.7566362899300489, + "holonomicRotation": 88.44217374749735, + "angularVelocity": 0.9292496179521041, + "holonomicAngularVelocity": 14.440699902870017 + }, + { + "time": 1.6515817467457017, + "pose": { + "rotation": { + "radians": 1.3928061946175356 + }, + "translation": { + "x": 2.978420288689721, + "y": 4.740942754071038 + } + }, + "velocity": 1.2162145082621492, + "acceleration": -3.0, + "curvature": 0.7489203635027577, + "holonomicRotation": 88.58624225078842, + "angularVelocity": 0.8970405829253777, + "holonomicAngularVelocity": 13.343986008255815 + }, + { + "time": 1.6618624627391885, + "pose": { + "rotation": { + "radians": 1.4020284140852233 + }, + "translation": { + "x": 2.980493855079212, + "y": 4.753112378099801 + } + }, + "velocity": 1.1853723602816888, + "acceleration": -3.0, + "curvature": 0.7411147940451478, + "holonomicRotation": 88.72342798116036, + "angularVelocity": 0.8644947789010164, + "holonomicAngularVelocity": 12.278747774850764 + }, + { + "time": 1.672472770616225, + "pose": { + "rotation": { + "radians": 1.4112009698474537 + }, + "translation": { + "x": 2.9824657658603, + "y": 4.765362987093515 + } + }, + "velocity": 1.153541436650579, + "acceleration": -3.000000000000021, + "curvature": 0.7332269374608648, + "holonomicRotation": 88.853709275396, + "angularVelocity": 0.8315912554192696, + "holonomicAngularVelocity": 11.245845968659761 + }, + { + "time": 1.6834418240352835, + "pose": { + "rotation": { + "radians": 1.4203227387509696 + }, + "translation": { + "x": 2.984335514847999, + "y": 4.777694824580263 + } + }, + "velocity": 1.1206342763934034, + "acceleration": -3.0, + "curvature": 0.725264077949128, + "holonomicRotation": 88.97706556056873, + "angularVelocity": 0.7983021566049804, + "holonomicAngularVelocity": 10.246170056824981 + }, + { + "time": 1.6948033177521344, + "pose": { + "rotation": { + "radians": 1.4293926436873856 + }, + "translation": { + "x": 2.9861025958573246, + "y": 4.7901081340881255 + } + }, + "velocity": 1.0865497952428504, + "acceleration": -3.0000000000000195, + "curvature": 0.7172334145932396, + "holonomicRotation": 89.09347735729114, + "angularVelocity": 0.7645911667003292, + "holonomicAngularVelocity": 9.280646383290382 + }, + { + "time": 1.70659656156077, + "pose": { + "rotation": { + "radians": 1.4384096537302118 + }, + "translation": { + "x": 2.987766502703291, + "y": 4.802603159145181 + } + }, + "velocity": 1.0511700638169434, + "acceleration": -3.0, + "curvature": 0.7091420486074421, + "holonomicRotation": 89.20292628279101, + "angularVelocity": 0.730411458422808, + "holonomicAngularVelocity": 8.350248260396057 + }, + { + "time": 1.7188679058163951, + "pose": { + "rotation": { + "radians": 1.4473727841847712 + }, + "translation": { + "x": 2.9893267292009145, + "y": 4.815180143279516 + } + }, + "velocity": 1.0143560310500683, + "acceleration": -3.0, + "curvature": 0.7009969714592601, + "holonomicRotation": 89.30539505381427, + "angularVelocity": 0.6957029316515805, + "holonomicAngularVelocity": 7.456008561063265 + }, + { + "time": 1.7316726705585361, + "pose": { + "rotation": { + "radians": 1.4562810965549875 + }, + "translation": { + "x": 2.9907827691652096, + "y": 4.82783933001921 + } + }, + "velocity": 0.9759417368236457, + "acceleration": -2.999999999999974, + "curvature": 0.692805053609917, + "holonomicRotation": 89.40086748935407, + "angularVelocity": 0.660388413677914, + "holonomicAngularVelocity": 6.599035632827374 + }, + { + "time": 1.7450778146999044, + "pose": { + "rotation": { + "radians": 1.4651336984296295 + }, + "translation": { + "x": 2.992134116411191, + "y": 4.840580962892344 + } + }, + "velocity": 0.9357263043995407, + "acceleration": -3.0000000000000084, + "curvature": 0.6845730342358501, + "holonomicRotation": 89.48932851320615, + "angularVelocity": 0.6243682973153083, + "holonomicAngularVelocity": 5.780533711561292 + }, + { + "time": 1.7591657254719308, + "pose": { + "rotation": { + "radians": 1.4739297432910896 + }, + "translation": { + "x": 2.9933802647538736, + "y": 4.853405285426999 + } + }, + "velocity": 0.8934625720834616, + "acceleration": -2.999999999999992, + "curvature": 0.6763075116811156, + "holonomicRotation": 89.57076415634931, + "angularVelocity": 0.587512750883515, + "holonomicAngularVelocity": 5.001829563513309 + }, + { + "time": 1.7740397634329392, + "pose": { + "rotation": { + "radians": 1.4826684302503075 + }, + "translation": { + "x": 2.9945207080082734, + "y": 4.866312541151259 + } + }, + "velocity": 0.8488404582004362, + "acceleration": -3.000000000000015, + "curvature": 0.6680149347162724, + "holonomicRotation": 89.6451615591515, + "angularVelocity": 0.5496500045273275, + "holonomicAngularVelocity": 4.264407957958817 + }, + { + "time": 1.78983267417526, + "pose": { + "rotation": { + "radians": 1.4913490037113237 + }, + "translation": { + "x": 2.995554939989405, + "y": 4.879302973593203 + } + }, + "velocity": 0.801461725973474, + "acceleration": -3.000000000000007, + "curvature": 0.6597015946752146, + "holonomicRotation": 89.71250897340039, + "angularVelocity": 0.5105479920421858, + "holonomicAngularVelocity": 3.569959998904688 + }, + { + "time": 1.8067199196280448, + "pose": { + "rotation": { + "radians": 1.4999707529683666 + }, + "translation": { + "x": 2.9964824545122832, + "y": 4.892376826280915 + } + }, + "velocity": 0.7507999896151193, + "acceleration": -3.0000000000000067, + "curvature": 0.6513736182652573, + "holonomicRotation": 89.77279576415852, + "angularVelocity": 0.4698840718376818, + "holonomicAngularVelocity": 2.9204507340812516 + }, + { + "time": 1.8249419860588283, + "pose": { + "rotation": { + "radians": 1.5085330117401599 + }, + "translation": { + "x": 2.9973027453919228, + "y": 4.905534342742476 + } + }, + "velocity": 0.6961337903227689, + "acceleration": -2.999999999999994, + "curvature": 0.6430369613174203, + "holonomicRotation": 89.82601241144278, + "angularVelocity": 0.4271917365972208, + "holonomicAngularVelocity": 2.318216531133641 + }, + { + "time": 1.8448443979626472, + "pose": { + "rotation": { + "radians": 1.5170351576438255 + }, + "translation": { + "x": 2.99801530644334, + "y": 4.918775766505965 + } + }, + "velocity": 0.6364265546113125, + "acceleration": -2.999999999999983, + "curvature": 0.6346974032758499, + "holonomicRotation": 89.87215051172764, + "angularVelocity": 0.38175841711322217, + "holonomicAngularVelocity": 1.7661094753355508 + }, + { + "time": 1.8669564290060066, + "pose": { + "rotation": { + "radians": 1.5254766116140968 + }, + "translation": { + "x": 2.9986196314815485, + "y": 4.932101341099467 + } + }, + "velocity": 0.5700904614812343, + "acceleration": -3.000000000000005, + "curvature": 0.6263605423064249, + "holonomicRotation": 89.91120277927223, + "angularVelocity": 0.3324044885642192, + "holonomicAngularVelocity": 1.2677148535800413 + }, + { + "time": 1.8921673570852195, + "pose": { + "rotation": { + "radians": 1.5338568372684969 + }, + "translation": { + "x": 2.9991152143215647, + "y": 4.945511310051062 + } + }, + "velocity": 0.4944576772435955, + "acceleration": -2.9999999999999933, + "curvature": 0.6180317914296721, + "holonomicRotation": 89.94316304727079, + "angularVelocity": 0.2769089779627116, + "holonomicAngularVelocity": 0.8276548442472411 + }, + { + "time": 1.9222079249965291, + "pose": { + "rotation": { + "radians": 1.542175340226237 + }, + "translation": { + "x": 2.9995015487784027, + "y": 4.959005916888831 + } + }, + "velocity": 0.4043359735096668, + "acceleration": -2.9999999999999964, + "curvature": 0.6097163750781969, + "holonomicRotation": 89.96802626882652, + "angularVelocity": 0.20992359095393126, + "holonomicAngularVelocity": 0.45161910465029553 + }, + { + "time": 1.9615380789671784, + "pose": { + "rotation": { + "radians": 1.5504316673805267 + }, + "translation": { + "x": 2.999778128667078, + "y": 4.9725854051408565 + } + }, + "velocity": 0.2863455115977193, + "acceleration": -2.9999999999999973, + "curvature": 0.6014193265576269, + "holonomicRotation": 89.9857885177485, + "angularVelocity": 0.08584460121184416, + "holonomicAngularVelocity": 0.11166724455106 + }, + { + "time": 2.0569865828330847, + "pose": { + "rotation": { + "radians": 1.5586254061311626 + }, + "translation": { + "x": 2.9999444478026054, + "y": 4.9862500183352205 + } + }, + "velocity": 0, + "acceleration": -3.000000000000004, + "curvature": 0.6014193265576269, + "holonomicRotation": 89.99644698917173, + "angularVelocity": 0.0, + "holonomicAngularVelocity": 0.0 + }, + { + "time": 2.0569865828330847, + "pose": { + "rotation": { + "radians": 1.5742753890751118 + }, + "translation": { + "x": 3.0, + "y": 5.0 + } + }, + "velocity": 0, + "acceleration": -3.0, + "curvature": -0.5057336661187407, + "holonomicRotation": 90.0, + "angularVelocity": 0.0, + "holonomicAngularVelocity": -0.03707165103584605 + }, + { + "time": 2.1528283036357116, + "pose": { + "rotation": { + "radians": 1.5742753890751118 + }, + "translation": { + "x": 3.000047936, + "y": 4.986221630216636 + } + }, + "velocity": -0.287525162407881, + "acceleration": -2.9999999999999996, + "curvature": -0.5057336661187407, + "holonomicRotation": 89.99644698917173, + "angularVelocity": 0.17565459412480444, + "holonomicAngularVelocity": -0.26895297967861 + }, + { + "time": 2.192457801088997, + "pose": { + "rotation": { + "radians": 1.5812364923656386 + }, + "translation": { + "x": 3.000191488, + "y": 4.972472156282768 + } + }, + "velocity": -0.4064136547677368, + "acceleration": -3.0, + "curvature": -0.5068077788280827, + "holonomicRotation": 89.9857885177485, + "angularVelocity": 0.22928766634703351, + "holonomicAngularVelocity": -0.5850105091967069 + }, + { + "time": 2.2228200743178532, + "pose": { + "rotation": { + "radians": 1.588198187139274 + }, + "translation": { + "x": 3.000430272, + "y": 4.958751790094678 + } + }, + "velocity": -0.4975004744543062, + "acceleration": -3.000000000000001, + "curvature": -0.5078447188468552, + "holonomicRotation": 89.96802626882652, + "angularVelocity": 0.27239456891071173, + "holonomicAngularVelocity": -0.9728038942611524 + }, + { + "time": 2.248378382320534, + "pose": { + "rotation": { + "radians": 1.5951601314297514 + }, + "translation": { + "x": 3.000763904, + "y": 4.9450607435486456 + } + }, + "velocity": -0.5741753984623478, + "acceleration": -2.999999999999998, + "curvature": -0.508844493249413, + "holonomicRotation": 89.94316304727079, + "angularVelocity": 0.3096323802410236, + "holonomicAngularVelocity": -1.4214508491667075 + }, + { + "time": 2.270862640581948, + "pose": { + "rotation": { + "radians": 1.602121985833187 + }, + "translation": { + "x": 3.001192, + "y": 4.931399228540954 + } + }, + "velocity": -0.6416281732465902, + "acceleration": -3.0000000000000013, + "curvature": -0.5098071295921854, + "holonomicRotation": 89.91120277927222, + "angularVelocity": 0.3429615754353077, + "holonomicAngularVelocity": -1.9239482966682593 + }, + { + "time": 2.291160622399972, + "pose": { + "rotation": { + "radians": 1.6090834136556538 + }, + "translation": { + "x": 3.001714176, + "y": 4.917767456967881 + } + }, + "velocity": -0.7025221187006625, + "acceleration": -3.0, + "curvature": -0.5107326758902916, + "holonomicRotation": 89.87215051172763, + "angularVelocity": 0.37343784439063415, + "holonomicAngularVelocity": -2.475296076666404 + }, + { + "time": 2.3098000493031976, + "pose": { + "rotation": { + "radians": 1.616044081059071 + }, + "translation": { + "x": 3.0023300479999997, + "y": 4.904165640725709 + } + }, + "velocity": -0.7584403994103386, + "acceleration": -2.9999999999999987, + "curvature": -0.5116212004719533, + "holonomicRotation": 89.82601241144276, + "angularVelocity": 0.4017085053948185, + "holonomicAngularVelocity": -3.0716784178770995 + }, + { + "time": 2.327124990278309, + "pose": { + "rotation": { + "radians": 1.6230036572042366 + }, + "translation": { + "x": 3.0030392320000003, + "y": 4.890593991710718 + } + }, + "velocity": -0.8104152223356732, + "acceleration": -2.999999999999997, + "curvature": -0.5124727917725321, + "holonomicRotation": 89.77279576415852, + "angularVelocity": 0.4282045821605271, + "holonomicAngularVelocity": -3.71004554133979 + }, + { + "time": 2.3433745996392688, + "pose": { + "rotation": { + "radians": 1.629961814390918 + }, + "translation": { + "x": 3.003841344, + "y": 4.87705272181919 + } + }, + "velocity": -0.8591640504185518, + "acceleration": -3.0, + "curvature": -0.5132875582522389, + "holonomicRotation": 89.71250897340038, + "angularVelocity": 0.45323008286571875, + "holonomicAngularVelocity": -4.387874987552203 + }, + { + "time": 2.358723125848408, + "pose": { + "rotation": { + "radians": 1.6369182281965529 + }, + "translation": { + "x": 3.004736, + "y": 4.863542042947403 + } + }, + "velocity": -0.9052096290459696, + "acceleration": -3.0000000000000036, + "curvature": -0.5140656281361321, + "holonomicRotation": 89.6451615591515, + "angularVelocity": 0.477008781354567, + "holonomicAngularVelocity": -5.103024356948047 + }, + { + "time": 2.373302206330536, + "pose": { + "rotation": { + "radians": 1.643872577610603 + }, + "translation": { + "x": 3.005722816, + "y": 4.85006216699164 + } + }, + "velocity": -0.9489468704923536, + "acceleration": -2.999999999999996, + "curvature": -0.5148071492013819, + "holonomicRotation": 89.5707641563493, + "angularVelocity": 0.49971092182910903, + "holonomicAngularVelocity": -5.853634956609349 + }, + { + "time": 2.3872141847416968, + "pose": { + "rotation": { + "radians": 1.6508245451669108 + }, + "translation": { + "x": 3.0068014080000003, + "y": 4.83661330584818 + } + }, + "velocity": -0.9906828057258363, + "acceleration": -2.999999999999996, + "curvature": -0.5155122885814692, + "holonomicRotation": 89.48932851320613, + "angularVelocity": 0.5214694818560188, + "holonomicAngularVelocity": -6.638065814143911 + }, + { + "time": 2.400540509958839, + "pose": { + "rotation": { + "radians": 1.6577738170729388 + }, + "translation": { + "x": 3.007971392, + "y": 4.823195671413306 + } + }, + "velocity": -1.0306617813772634, + "acceleration": -3.000000000000004, + "curvature": -0.5161812324778612, + "holonomicRotation": 89.40086748935406, + "angularVelocity": 0.542390592745728, + "holonomicAngularVelocity": -7.45484681173555 + }, + { + "time": 2.4133472701714718, + "pose": { + "rotation": { + "radians": 1.6647200833358209 + }, + "translation": { + "x": 3.009232384, + "y": 4.809809475583298 + } + }, + "velocity": -1.0690820620151604, + "acceleration": -2.999999999999991, + "curvature": -0.516814185876635, + "holonomicRotation": 89.30539505381425, + "angularVelocity": 0.5625604962136552, + "holonomicAngularVelocity": -8.30264439528027 + }, + { + "time": 2.425688973145226, + "pose": { + "rotation": { + "radians": 1.6716630378848576 + }, + "translation": { + "x": 3.010584, + "y": 4.796454930254436 + } + }, + "velocity": -1.106107170936423, + "acceleration": -2.9999999999999956, + "curvature": -0.5174113722714463, + "holonomicRotation": 89.202926282791, + "angularVelocity": 0.5820503490649594, + "holonomicAngularVelocity": -9.180235856393876 + }, + { + "time": 2.437611207738323, + "pose": { + "rotation": { + "radians": 1.6786023786914042 + }, + "translation": { + "x": 3.012025856, + "y": 4.7831322473230005 + } + }, + "velocity": -1.1418738747157147, + "acceleration": -3.0000000000000044, + "curvature": -0.5179730333484562, + "holonomicRotation": 89.0934773572911, + "angularVelocity": 0.6009196375531689, + "holonomicAngularVelocity": -10.086489637904608 + }, + { + "time": 2.4491525666155414, + "pose": { + "rotation": { + "radians": 1.6855378078847734 + }, + "translation": { + "x": 3.013557568, + "y": 4.769841638685271 + } + }, + "velocity": -1.1764979513473701, + "acceleration": -2.9999999999999902, + "curvature": -0.5184994287118657, + "holonomicRotation": 88.9770655605687, + "angularVelocity": 0.6192186636953354, + "holonomicAngularVelocity": -11.020349981504285 + }, + { + "time": 2.460346066402732, + "pose": { + "rotation": { + "radians": 1.6924690318650715 + }, + "translation": { + "x": 3.0151787519999997, + "y": 4.756583316237531 + } + }, + "velocity": -1.2100784507089415, + "acceleration": -2.999999999999995, + "curvature": -0.5189908354035975, + "holonomicRotation": 88.85370927539597, + "angularVelocity": 0.636990394203234, + "holonomicAngularVelocity": -11.980824775627683 + }, + { + "time": 2.4712202171123465, + "pose": { + "rotation": { + "radians": 1.6993957614122142 + }, + "translation": { + "x": 3.016889024, + "y": 4.74335749187606 + } + }, + "velocity": -1.242700902837785, + "acceleration": -3.000000000000005, + "curvature": -0.5194475476872736, + "holonomicRotation": 88.72342798116031, + "angularVelocity": 0.6542718609777953, + "holonomicAngularVelocity": -12.966975807849181 + }, + { + "time": 2.4817998412594715, + "pose": { + "rotation": { + "radians": 1.7063177117913995 + }, + "translation": { + "x": 3.018688, + "y": 4.73016437749714 + } + }, + "velocity": -1.2744397752791607, + "acceleration": -2.9999999999999893, + "curvature": -0.5198698765939959, + "holonomicRotation": 88.5862422507884, + "angularVelocity": 0.6710952392501868, + "holonomicAngularVelocity": -13.977910854785828 + }, + { + "time": 2.4921067107757953, + "pose": { + "rotation": { + "radians": 1.713234602855377 + }, + "translation": { + "x": 3.020575296, + "y": 4.717004184997049 + } + }, + "velocity": -1.3053603838281316, + "acceleration": -3.0000000000000053, + "curvature": -0.5202581495425616, + "holonomicRotation": 88.44217374749732, + "angularVelocity": 0.6874886894244737, + "holonomicAngularVelocity": -15.012777197368143 + }, + { + "time": 2.502160048951621, + "pose": { + "rotation": { + "radians": 1.7201461591422165 + }, + "translation": { + "x": 3.022550528, + "y": 4.703877126272069 + } + }, + "velocity": -1.3355203983556083, + "acceleration": -2.9999999999999947, + "curvature": -0.5206127099914765, + "holonomicRotation": 88.29124522137386, + "angularVelocity": 0.7034770231023145, + "holonomicAngularVelocity": -16.07075625679041 + }, + { + "time": 2.5119769308127515, + "pose": { + "rotation": { + "radians": 1.7270521099700318 + }, + "translation": { + "x": 3.024613312, + "y": 4.69078341321848 + } + }, + "velocity": -1.3649710439390004, + "acceleration": -3.0000000000000058, + "curvature": -0.520933916894771, + "holonomicRotation": 88.13348050578192, + "angularVelocity": 0.7190822359132523, + "holonomicAngularVelocity": -17.151059122085808 + }, + { + "time": 2.5215726059842662, + "pose": { + "rotation": { + "radians": 1.7339521895274617 + }, + "translation": { + "x": 3.026763264, + "y": 4.6777232577325645 + } + }, + "velocity": -1.3937580694535436, + "acceleration": -2.9999999999999942, + "curvature": -0.5212221444517889, + "holonomicRotation": 87.96890451359894, + "angularVelocity": 0.7343239383776761, + "holonomicAngularVelocity": -18.25292279485364 + }, + { + "time": 2.530960761637088, + "pose": { + "rotation": { + "radians": 1.7408461369605446 + }, + "translation": { + "x": 3.0290000000000004, + "y": 4.664696871710602 + } + }, + "velocity": -1.4219225364120098, + "acceleration": -3.000000000000012, + "curvature": -0.521477781524844, + "holonomicRotation": 87.79754323328191, + "angularVelocity": 0.7492197075330436, + "holonomicAngularVelocity": -19.375607016536954 + }, + { + "time": 2.5401537385779607, + "pose": { + "rotation": { + "radians": 1.7477336964555432 + }, + "translation": { + "x": 3.031323136, + "y": 4.651704467048874 + } + }, + "velocity": -1.449501467234628, + "acceleration": -3.000000000000018, + "curvature": -0.5217012312954975, + "holonomicRotation": 87.61942372476348, + "angularVelocity": 0.763785376451716, + "holonomicAngularVelocity": -20.518391573003203 + }, + { + "time": 2.54916271030593, + "pose": { + "rotation": { + "radians": 1.754614617318233 + }, + "translation": { + "x": 3.033732288, + "y": 4.638746255643659 + } + }, + "velocity": -1.476528382418536, + "acceleration": -3.0000000000000187, + "curvature": -0.5218929107130195, + "holonomicRotation": 87.43457411517889, + "angularVelocity": 0.7780352743756296, + "holonomicAngularVelocity": -21.6805739935594 + }, + { + "time": 2.5579978325130934, + "pose": { + "rotation": { + "radians": 1.7614886540488257 + }, + "translation": { + "x": 3.036227072, + "y": 4.62582244939124 + } + }, + "velocity": -1.5030337490400265, + "acceleration": -3.0000000000000253, + "curvature": -0.5220532500924302, + "holonomicRotation": 87.24302359442434, + "angularVelocity": 0.7919824274550814, + "holonomicAngularVelocity": -22.86146757815466 + }, + { + "time": 2.566668368785499, + "pose": { + "rotation": { + "radians": 1.7683555664131827 + }, + "translation": { + "x": 3.038807104, + "y": 4.612933260187897 + } + }, + "velocity": -1.529045357857243, + "acceleration": -3.000000000000013, + "curvature": -0.5221826926786177, + "holonomicRotation": 87.04480241054753, + "angularVelocity": 0.8056387276148198, + "holonomicAngularVelocity": -24.06039969965495 + }, + { + "time": 2.5751827969801044, + "pose": { + "rotation": { + "radians": 1.7752151195102526 + }, + "translation": { + "x": 3.041472, + "y": 4.600078899929912 + } + }, + "velocity": -1.5545886424410602, + "acceleration": -3.000000000000026, + "curvature": -0.5222816940473599, + "holonomicRotation": 86.83994186497131, + "angularVelocity": 0.819015075408048, + "holonomicAngularVelocity": -25.27671033811035 + }, + { + "time": 2.5835488997914293, + "pose": { + "rotation": { + "radians": 1.7820670838351411 + }, + "translation": { + "x": 3.044221376, + "y": 4.587259580513562 + } + }, + "velocity": -1.5796869508750342, + "acceleration": -3.0, + "curvature": -0.5223507217767429, + "holonomicRotation": 86.6284743075506, + "angularVelocity": 0.8321215017430816, + "holonomicAngularVelocity": -26.509750811682704 + }, + { + "time": 2.591773842290336, + "pose": { + "rotation": { + "radians": 1.7889112353390821 + }, + "translation": { + "x": 3.047054848, + "y": 4.574475513835131 + } + }, + "velocity": -1.6043617783717554, + "acceleration": -3.0000000000000133, + "curvature": -0.5223902548388374, + "holonomicRotation": 86.41043313146416, + "angularVelocity": 0.8449672717787239, + "holonomicAngularVelocity": -27.758882675285424 + }, + { + "time": 2.5998642386564272, + "pose": { + "rotation": { + "radians": 1.7957473554841465 + }, + "translation": { + "x": 3.0499720320000003, + "y": 4.5617269117909 + } + }, + "velocity": -1.6286329674700286, + "acceleration": -3.0000000000000275, + "curvature": -0.5224007831335928, + "holonomicRotation": 86.18585276794128, + "angularVelocity": 0.8575609745438143, + "holonomicAngularVelocity": -29.023476762925224 + }, + { + "time": 2.607826209891096, + "pose": { + "rotation": { + "radians": 1.8025752312954388 + }, + "translation": { + "x": 3.0529725439999997, + "y": 4.5490139862771475 + } + }, + "velocity": -1.6525188811740348, + "acceleration": -3.0000000000000138, + "curvature": -0.5223828070623401, + "holonomicRotation": 85.9547686808248, + "angularVelocity": 0.8699106001067596, + "holonomicAngularVelocity": -30.302912353676327 + }, + { + "time": 2.615665433959112, + "pose": { + "rotation": { + "radians": 1.809394655408818 + }, + "translation": { + "x": 3.0560560000000003, + "y": 4.536336949190155 + } + }, + "velocity": -1.6760365533780828, + "acceleration": -2.9999999999999716, + "curvature": -0.5223368369618795, + "holonomicRotation": 85.71721736097088, + "angularVelocity": 0.8820236064459887, + "holonomicAngularVelocity": -31.596576444504613 + }, + { + "time": 2.6233871895395846, + "pose": { + "rotation": { + "radians": 1.816205426114001 + }, + "translation": { + "x": 3.059222016, + "y": 4.523696012426204 + } + }, + "velocity": -1.699201820119501, + "acceleration": -3.0, + "curvature": -0.5222633926490773, + "holonomicRotation": 85.47323632048669, + "angularVelocity": 0.8939069779563974, + "holonomicAngularVelocity": -32.90386311573669 + }, + { + "time": 2.630996394355419, + "pose": { + "rotation": { + "radians": 1.823007347395575 + }, + "translation": { + "x": 3.062470208, + "y": 4.511091387881574 + } + }, + "velocity": -1.7220294345670046, + "acceleration": -2.9999999999999853, + "curvature": -0.5221630028621961, + "holonomicRotation": 85.22286408680687, + "angularVelocity": 0.9055672761180343, + "holonomicAngularVelocity": -34.22417297723497 + }, + { + "time": 2.6384976388812733, + "pose": { + "rotation": { + "radians": 1.829800228968348 + }, + "translation": { + "x": 3.065800192, + "y": 4.498523287452546 + } + }, + "velocity": -1.7445331681445666, + "acceleration": -3.0, + "curvature": -0.5220362048391741, + "holonomicRotation": 84.9661401966095, + "angularVelocity": 0.9170106842737414, + "holonomicAngularVelocity": -35.556912684970456 + }, + { + "time": 2.6458952160940945, + "pose": { + "rotation": { + "radians": 1.8365838863102448 + }, + "translation": { + "x": 3.069211584, + "y": 4.485991923035402 + } + }, + "velocity": -1.76672589978303, + "acceleration": -3.000000000000015, + "curvature": -0.5218835437565383, + "holonomicRotation": 84.7031051895729, + "angularVelocity": 0.9282430467271324, + "holonomicAngularVelocity": -36.9014945192324 + }, + { + "time": 2.653193147820374, + "pose": { + "rotation": { + "radians": 1.8433581406906532 + }, + "translation": { + "x": 3.072704, + "y": 4.473497506526421 + } + }, + "velocity": -1.7886196949618687, + "acceleration": -3.0, + "curvature": -0.521705572307682, + "holonomicRotation": 84.43380060197386, + "angularVelocity": 0.9392699033985431, + "holonomicAngularVelocity": -38.2573360170749 + }, + { + "time": 2.660395208144758, + "pose": { + "rotation": { + "radians": 1.850122819195808 + }, + "translation": { + "x": 3.0762770560000003, + "y": 4.461040249821885 + } + }, + "velocity": -1.810225875935021, + "acceleration": -3.0, + "curvature": -0.5215028501053297, + "holonomicRotation": 84.15826896012865, + "angularVelocity": 0.9500965202307644, + "holonomicAngularVelocity": -39.623859652300446 + }, + { + "time": 2.6675049442715038, + "pose": { + "rotation": { + "radians": 1.856877754749588 + }, + "translation": { + "x": 3.0799303680000003, + "y": 4.4486203648180735 + } + }, + "velocity": -1.8315550843152586, + "acceleration": -3.000000000000031, + "curvature": -0.5212759433218822, + "holonomicRotation": 83.87655377367759, + "angularVelocity": 0.9607279164011417, + "holonomicAngularVelocity": -41.00049255752908 + }, + { + "time": 2.674525695170029, + "pose": { + "rotation": { + "radians": 1.8636227861318995 + }, + "translation": { + "x": 3.083663552, + "y": 4.436238063411268 + } + }, + "velocity": -1.8526173370108336, + "acceleration": -2.999999999999984, + "curvature": -0.5210254240939713, + "holonomicRotation": 83.58869952871434, + "angularVelocity": 0.9711688882073912, + "holonomicAngularVelocity": -42.38666628337523 + }, + { + "time": 2.6814606082860557, + "pose": { + "rotation": { + "radians": 1.8703577579926058 + }, + "translation": { + "x": 3.087476224, + "y": 4.423893557497749 + } + }, + "velocity": -1.8734220763589131, + "acceleration": -2.999999999999968, + "curvature": -0.5207518700684239, + "holonomicRotation": 83.29475168076112, + "angularVelocity": 0.981424030448601, + "holonomicAngularVelocity": -43.78181659045681 + }, + { + "time": 2.6883126545585223, + "pose": { + "rotation": { + "radians": 1.8770825208621504 + }, + "translation": { + "x": 3.091368, + "y": 4.411587058973797 + } + }, + "velocity": -1.8939782151763134, + "acceleration": -3.000000000000016, + "curvature": -0.5204558639450751, + "holonomicRotation": 82.99475664759066, + "angularVelocity": 0.9914977555239444, + "holonomicAngularVelocity": -45.18538327053407 + }, + { + "time": 2.695084641947945, + "pose": { + "rotation": { + "radians": 1.8837969311591998 + }, + "translation": { + "x": 3.095338496, + "y": 4.399318779735693 + } + }, + "velocity": -1.914294177344582, + "acceleration": -2.999999999999967, + "curvature": -0.5201379930167327, + "holonomicRotation": 82.68876180189636, + "angularVelocity": 1.0013943103829934, + "holonomicAngularVelocity": -46.596809993490176 + }, + { + "time": 2.7017792276530264, + "pose": { + "rotation": { + "radians": 1.8905008511946395 + }, + "translation": { + "x": 3.099387328, + "y": 4.387088931679718 + } + }, + "velocity": -1.934377934459826, + "acceleration": -3.0, + "curvature": -0.5197988486561339, + "holonomicRotation": 82.37681546381155, + "angularVelocity": 1.0111177916912393, + "holonomicAngularVelocity": -48.01554417719809 + }, + { + "time": 2.708398929168005, + "pose": { + "rotation": { + "radians": 1.8971941491721198 + }, + "translation": { + "x": 3.103514112, + "y": 4.3748977267021525 + } + }, + "velocity": -1.9542370390047616, + "acceleration": -2.999999999999983, + "curvature": -0.5194390259123196, + "holonomicRotation": 82.05896689327923, + "angularVelocity": 1.0206721594876074, + "holonomicAngularVelocity": -49.4410368778254 + }, + { + "time": 2.714946134312709, + "pose": { + "rotation": { + "radians": 1.9038766991857727 + }, + "translation": { + "x": 3.1077184640000004, + "y": 4.362745376699276 + } + }, + "velocity": -1.9738786544388722, + "acceleration": -3.0, + "curvature": -0.5190591229932082, + "holonomicRotation": 81.73526628227326, + "angularVelocity": 1.030061249349988, + "holonomicAngularVelocity": -50.8727426982381 + }, + { + "time": 2.7214231103498596, + "pose": { + "rotation": { + "radians": 1.9105483812146105 + }, + "translation": { + "x": 3.112, + "y": 4.350632093567372 + } + }, + "velocity": -1.9933095825503253, + "acceleration": -3.0, + "curvature": -0.5186597408707463, + "holonomicRotation": 81.40576474687262, + "angularVelocity": 1.0392887833198277, + "holonomicAngularVelocity": -52.31011971248371 + }, + { + "time": 2.7278320122894053, + "pose": { + "rotation": { + "radians": 1.9172090811137767 + }, + "translation": { + "x": 3.1163583360000002, + "y": 4.338558089202717 + } + }, + "velocity": -2.012536288368962, + "acceleration": -3.0000000000000346, + "curvature": -0.518241482848376, + "holonomicRotation": 81.07051431918943, + "angularVelocity": 1.0483583797739298, + "holonomicAngularVelocity": -53.75262940469507 + }, + { + "time": 2.7341748904669525, + "pose": { + "rotation": { + "radians": 1.923858690603094 + }, + "translation": { + "x": 3.120793088, + "y": 4.326523575501596 + } + }, + "velocity": -2.0315649229016044, + "acceleration": -2.9999999999999827, + "curvature": -0.5178049541021739, + "holonomicRotation": 80.72956793915259, + "angularVelocity": 1.057273562202763, + "holonomicAngularVelocity": -55.19973662061289 + }, + { + "time": 2.7404536974726246, + "pose": { + "rotation": { + "radians": 1.9304971072523642 + }, + "translation": { + "x": 3.1253038720000004, + "y": 4.314528764360286 + } + }, + "velocity": -2.0504013439186197, + "acceleration": -2.9999999999999645, + "curvature": -0.5173507612460925, + "holonomicRotation": 80.38297944614784, + "angularVelocity": 1.0660377671971724, + "holonomicAngularVelocity": -56.65090953051544 + }, + { + "time": 2.7466702944972865, + "pose": { + "rotation": { + "radians": 1.9371242344640995 + }, + "translation": { + "x": 3.1298903040000003, + "y": 4.302573867675071 + } + }, + "velocity": -2.069051134992606, + "acceleration": -3.0000000000000178, + "curvature": -0.5168795119868999, + "holonomicRotation": 80.03080357051604, + "angularVelocity": 1.0746543515953761, + "holonomicAngularVelocity": -58.10561960214269 + }, + { + "time": 2.7528264571551295, + "pose": { + "rotation": { + "radians": 1.9437399814534793 + }, + "translation": { + "x": 3.1345520000000002, + "y": 4.29065909734223 + } + }, + "velocity": -2.0875196229661346, + "acceleration": -3.000000000000036, + "curvature": -0.5163918146518766, + "holonomicRotation": 79.67309592491051, + "angularVelocity": 1.0831265989126386, + "holonomicAngularVelocity": -59.563341582570494 + }, + { + "time": 2.7589238808346237, + "pose": { + "rotation": { + "radians": 1.9503442632255794 + }, + "translation": { + "x": 3.139288576, + "y": 4.278784665258044 + } + }, + "velocity": -2.105811894004618, + "acceleration": -3.0000000000000364, + "curvature": -0.5158882778577482, + "holonomicRotation": 79.30991299551513, + "angularVelocity": 1.0914577251328634, + "holonomicAngularVelocity": -61.02355348789271 + }, + { + "time": 2.764964185623882, + "pose": { + "rotation": { + "radians": 1.9569370005499724 + }, + "translation": { + "x": 3.144099648, + "y": 4.266950783318793 + } + }, + "velocity": -2.123932808372393, + "acceleration": -3.0, + "curvature": -0.5153695100652415, + "holonomicRotation": 78.94131213312465, + "angularVelocity": 1.0996508839876262, + "holonomicAngularVelocity": -62.485736599971624 + }, + { + "time": 2.7709489208512275, + "pose": { + "rotation": { + "radians": 1.9635181199331546 + }, + "translation": { + "x": 3.1489848320000005, + "y": 4.255157663420759 + } + }, + "velocity": -2.1418870140544293, + "acceleration": -3.0000000000000555, + "curvature": -0.51483611928948, + "holonomicRotation": 78.56735154408818, + "angularVelocity": 1.1077091715727203, + "holonomicAngularVelocity": -63.94937546922451 + }, + { + "time": 2.7768795692772295, + "pose": { + "rotation": { + "radians": 1.9700875535880105 + }, + "translation": { + "x": 3.153943744, + "y": 4.243405517460222 + } + }, + "velocity": -2.1596789593324357, + "acceleration": -3.0000000000000187, + "curvature": -0.5142887126180925, + "holonomicRotation": 78.18809028111781, + "angularVelocity": 1.1156356307027255, + "holonomicAngularVelocity": -65.41395792283565 + }, + { + "time": 2.7827575509704925, + "pose": { + "rotation": { + "radians": 1.9766452394016332 + }, + "translation": { + "x": 3.158976, + "y": 4.231694557333463 + } + }, + "velocity": -2.1773129044122252, + "acceleration": -3.000000000000019, + "curvature": -0.5137278959903189, + "holonomicRotation": 77.8035882339635, + "angularVelocity": 1.1234332546485775, + "holonomicAngularVelocity": -66.87897507773808 + }, + { + "time": 2.7885842268959946, + "pose": { + "rotation": { + "radians": 1.9831911209004023 + }, + "translation": { + "x": 3.164081216, + "y": 4.220024994936762 + } + }, + "velocity": -2.194792932188731, + "acceleration": -3.000000000000038, + "curvature": -0.5131542737842859, + "holonomicRotation": 77.4139061199558, + "angularVelocity": 1.1311049906097959, + "holonomicAngularVelocity": -68.34392135764897 + }, + { + "time": 2.7943609022417353, + "pose": { + "rotation": { + "radians": 1.9897251472131021 + }, + "translation": { + "x": 3.1692590080000005, + "y": 4.2083970421664 + } + }, + "velocity": -2.212122958225953, + "acceleration": -3.0, + "curvature": -0.512568448497083, + "holonomicRotation": 77.01910547441783, + "angularVelocity": 1.1386537427992665, + "holonomicAngularVelocity": -69.80829451368561 + }, + { + "time": 2.800088829506757, + "pose": { + "rotation": { + "radians": 1.9962472730319014 + }, + "translation": { + "x": 3.1745089920000003, + "y": 4.196810910918658 + } + }, + "velocity": -2.2293067400210194, + "acceleration": -3.0000000000000195, + "curvature": -0.5119710204146639, + "holonomicRotation": 76.6192486409482, + "angularVelocity": 1.146082375300637, + "holonomicAngularVelocity": -71.27159564806851 + }, + { + "time": 2.8057692113712394, + "pose": { + "rotation": { + "radians": 2.002757458571762 + }, + "translation": { + "x": 3.1798307840000004, + "y": 4.185266813089816 + } + }, + "velocity": -2.246347885614466, + "acceleration": -3.0, + "curvature": -0.5113625873507105, + "holonomicRotation": 76.2143987615762, + "angularVelocity": 1.1533937144450512, + "holonomicAngularVelocity": -72.73332924047713 + }, + { + "time": 2.811403203367266, + "pose": { + "rotation": { + "radians": 2.009255669527213 + }, + "translation": { + "x": 3.1852240000000003, + "y": 4.173764960576157 + } + }, + "velocity": -2.263249861602546, + "acceleration": -2.999999999999941, + "curvature": -0.5107437442964814, + "holonomicRotation": 75.80461976679098, + "angularVelocity": 1.1605905511796997, + "holonomicAngularVelocity": -74.1930031765793 + }, + { + "time": 2.816991916367036, + "pose": { + "rotation": { + "radians": 2.0157418770280007 + }, + "translation": { + "x": 3.190688256, + "y": 4.162305565273958 + } + }, + "velocity": -2.280016000601855, + "acceleration": -2.99999999999998, + "curvature": -0.5101150831929077, + "holonomicRotation": 75.38997636544607, + "angularVelocity": 1.167675643067086, + "holonomicAngularVelocity": -75.65012877847903 + }, + { + "time": 2.8225364189036144, + "pose": { + "rotation": { + "radians": 2.022216057592887 + }, + "translation": { + "x": 3.1962231680000004, + "y": 4.150888839079503 + } + }, + "velocity": -2.2966495082115905, + "acceleration": -3.0, + "curvature": -0.5094771925471605, + "holonomicRotation": 74.9705340345413, + "angularVelocity": 1.1746517159963967, + "holonomicAngularVelocity": -77.10422083667376 + }, + { + "time": 2.8280377393378977, + "pose": { + "rotation": { + "radians": 2.028678193081264 + }, + "translation": { + "x": 3.2018283520000006, + "y": 4.13951499388907 + } + }, + "velocity": -2.3131534695144405, + "acceleration": -3.0, + "curvature": -0.5088306573722707, + "holonomicRotation": 74.54635900888302, + "angularVelocity": 1.1815214660057185, + "holonomicAngularVelocity": -78.55479764327413 + }, + { + "time": 2.833496867884132, + "pose": { + "rotation": { + "radians": 2.035128270644324 + }, + "translation": { + "x": 3.2075034240000004, + "y": 4.128184241598942 + } + }, + "velocity": -2.3295308551531426, + "acceleration": -3.000000000000041, + "curvature": -0.5081760586777138, + "holonomicRotation": 74.11751827062498, + "angularVelocity": 1.1882875605163865, + "holonomicAngularVelocity": -80.00138102615517 + }, + { + "time": 2.838914758505198, + "pose": { + "rotation": { + "radians": 2.0415662826735756 + }, + "translation": { + "x": 3.2132480000000005, + "y": 4.116896794105398 + } + }, + "velocity": -2.3457845270163418, + "acceleration": -3.0000000000000204, + "curvature": -0.5075139734310048, + "holonomicRotation": 73.68407953869102, + "angularVelocity": 1.1949526397867256, + "holonomicAngularVelocity": -81.4434963839311 + }, + { + "time": 2.844292330687818, + "pose": { + "rotation": { + "radians": 2.047992226748841 + }, + "translation": { + "x": 3.2190616960000002, + "y": 4.105652863304721 + } + }, + "velocity": -2.3619172435642013, + "acceleration": -3.0000000000000204, + "curvature": -0.5068449742568512, + "holonomicRotation": 73.2461112580815, + "angularVelocity": 1.2015193179741286, + "holonomicAngularVelocity": -82.8806727213178 + }, + { + "time": 2.8496304711069254, + "pose": { + "rotation": { + "radians": 2.054406105584457 + }, + "translation": { + "x": 3.2249441280000006, + "y": 4.094452661093189 + } + }, + "velocity": -2.3779316648215234, + "acceleration": -2.999999999999979, + "curvature": -0.5061696291938161, + "holonomicRotation": 72.80368258906502, + "angularVelocity": 1.2079901842417025, + "holonomicAngularVelocity": -84.31244268503457 + }, + { + "time": 2.8549300351876026, + "pose": { + "rotation": { + "radians": 2.0608079269746753 + }, + "translation": { + "x": 3.230894912, + "y": 4.083296399367084 + } + }, + "velocity": -2.393830357063556, + "acceleration": -2.999999999999979, + "curvature": -0.5054885015363642, + "holonomicRotation": 72.35686339625722, + "angularVelocity": 1.2143678036086012, + "holonomicAngularVelocity": -85.73834259958811 + }, + { + "time": 2.860191848572261, + "pose": { + "rotation": { + "radians": 2.067197703737601 + }, + "translation": { + "x": 3.2369136640000002, + "y": 4.072184290022687 + } + }, + "velocity": -2.40961579721753, + "acceleration": -2.999999999999958, + "curvature": -0.5048021496278324, + "holonomicRotation": 71.90572423758832, + "angularVelocity": 1.2206547177528895, + "holonomicAngularVelocity": -87.15791250329711 + }, + { + "time": 2.8654167085000366, + "pose": { + "rotation": { + "radians": 2.073575453658038 + }, + "translation": { + "x": 3.2430000000000003, + "y": 4.061116544956278 + } + }, + "velocity": -2.425290377000857, + "acceleration": -3.000000000000064, + "curvature": -0.5041111265899505, + "holonomicRotation": 71.45033635316126, + "angularVelocity": 1.2268534457939924, + "holonomicAngularVelocity": -88.57069618405971 + }, + { + "time": 2.870605385104816, + "pose": { + "rotation": { + "radians": 2.0799411994297223 + }, + "translation": { + "x": 3.249153536, + "y": 4.050093376064137 + } + }, + "velocity": -2.440856406815195, + "acceleration": -3.0, + "curvature": -0.5034159803157897, + "holonomicRotation": 70.99077165400202, + "angularVelocity": 1.229578840879155, + "holonomicAngularVelocity": -89.72902648596558 + }, + { + "time": 2.87577282045385, + "pose": { + "rotation": { + "radians": 2.0862949685965058 + }, + "translation": { + "x": 3.2553738880000003, + "y": 4.039114995242547 + } + }, + "velocity": -2.442860865424066, + "acceleration": -0.3879020197603165, + "curvature": -0.5027172531268748, + "holonomicRotation": 70.52710271070401, + "angularVelocity": 1.2287989005424262, + "holonomicAngularVelocity": -90.62205545373247 + }, + { + "time": 2.880933815383769, + "pose": { + "rotation": { + "radians": 2.0926367934920957 + }, + "translation": { + "x": 3.2616606720000005, + "y": 4.028181614387787 + } + }, + "velocity": -2.444567716338234, + "acceleration": -0.330721292569582, + "curvature": -0.5020154817355811, + "holonomicRotation": 70.05940274196843, + "angularVelocity": 1.2279368134428386, + "holonomicAngularVelocity": -91.49647637683879 + }, + { + "time": 2.8860887367272294, + "pose": { + "rotation": { + "radians": 2.0989667111801324 + }, + "translation": { + "x": 3.2680135040000002, + "y": 4.017293445396137 + } + }, + "velocity": -2.446284282103891, + "acceleration": -0.3329955301518617, + "curvature": -0.50131119707638, + "holonomicRotation": 69.58774560304208, + "angularVelocity": 1.2270709076372577, + "holonomicAngularVelocity": -92.35775044436066 + }, + { + "time": 2.8912376259834023, + "pose": { + "rotation": { + "radians": 2.105284763393028 + }, + "translation": { + "x": 3.2744320000000005, + "y": 4.006450700163881 + } + }, + "velocity": -2.4480093305759216, + "acceleration": -0.33503312776881694, + "curvature": -0.5006049240997247, + "holonomicRotation": 69.11220577405483, + "angularVelocity": 1.2262018193366309, + "holonomicAngularVelocity": -93.20567103737582 + }, + { + "time": 2.8963805257711663, + "pose": { + "rotation": { + "radians": 2.1115909964694506 + }, + "translation": { + "x": 3.2809157760000005, + "y": 3.9956535905872963 + } + }, + "velocity": -2.449741633776713, + "acceleration": -0.33683394043815795, + "curvature": -0.49989718179259984, + "holonomicRotation": 68.63285834825828, + "angularVelocity": 1.2253301792953024, + "holonomicAngularVelocity": -94.04003299918955 + }, + { + "time": 2.9015174798181165, + "pose": { + "rotation": { + "radians": 2.1178854612928317 + }, + "translation": { + "x": 3.2874644480000006, + "y": 3.9849023285626646 + } + }, + "velocity": -2.4514799686116775, + "acceleration": -0.3383979726267414, + "curvature": -0.4991884828858999, + "holonomicRotation": 68.14977902016777, + "angularVelocity": 1.224456612402177, + "holonomicAngularVelocity": -94.86063272099182 + }, + { + "time": 2.9066485329494, + "pose": { + "rotation": { + "radians": 2.1241682132280184 + }, + "translation": { + "x": 3.2940776320000005, + "y": 3.9741971259862665 + } + }, + "velocity": -2.453223116749019, + "acceleration": -0.3397252167812531, + "curvature": -0.4984793339133297, + "holonomicRotation": 67.66304407360921, + "angularVelocity": 1.2235817376586304, + "holonomicAngularVelocity": -95.66726822646376 + }, + { + "time": 2.9117737310764693, + "pose": { + "rotation": { + "radians": 2.130439312058183 + }, + "translation": { + "x": 3.3007549440000004, + "y": 3.9635381947543835 + } + }, + "velocity": -2.4549698652585956, + "acceleration": -0.3408158019006307, + "curvature": -0.497770234969751, + "holonomicRotation": 67.17273036967309, + "angularVelocity": 1.2227061680084468, + "holonomicAngularVelocity": -96.4597392604555 + }, + { + "time": 2.916893121185565, + "pose": { + "rotation": { + "radians": 2.1366988219210157 + }, + "translation": { + "x": 3.3074960000000004, + "y": 3.9529257467632957 + } + }, + "velocity": -2.456719006680205, + "acceleration": -0.341669883391296, + "curvature": -0.497061679711388, + "holonomicRotation": 66.67891533457716, + "angularVelocity": 1.2218305102229903, + "holonomicAngularVelocity": -97.23784737105578 + }, + { + "time": 2.922006751326304, + "pose": { + "rotation": { + "radians": 2.1429468112449666 + }, + "translation": { + "x": 3.3143004160000005, + "y": 3.9423599939092844 + } + }, + "velocity": -2.458469339304636, + "acceleration": -0.3422876853150851, + "curvature": -0.4963541552778526, + "holonomicRotation": 66.18167694743994, + "angularVelocity": 1.2209553647377336, + "holonomicAngularVelocity": -98.00139599816646 + }, + { + "time": 2.9271146706000497, + "pose": { + "rotation": { + "radians": 2.1491833526848936 + }, + "translation": { + "x": 3.3211678080000007, + "y": 3.9318411480886293 + } + }, + "velocity": -2.4602196675683676, + "acceleration": -0.34266952352364594, + "curvature": -0.49564814217695435, + "holonomicRotation": 65.68109372796692, + "angularVelocity": 1.2200813256067014, + "holonomicAngularVelocity": -98.75019056144184 + }, + { + "time": 2.932216929148118, + "pose": { + "rotation": { + "radians": 2.155408523057809 + }, + "translation": { + "x": 3.3280977920000003, + "y": 3.921369421197612 + } + }, + "velocity": -2.461968802283485, + "acceleration": -0.3428157743553925, + "curvature": -0.49494411424464496, + "holonomicRotation": 65.17724472405142, + "angularVelocity": 1.2192089803800714, + "holonomicAngularVelocity": -99.48403854872984 + }, + { + "time": 2.9373135781398165, + "pose": { + "rotation": { + "radians": 2.161622403278333 + }, + "translation": { + "x": 3.3350899840000006, + "y": 3.910945025132512 + } + }, + "velocity": -2.463715561050444, + "acceleration": -0.342726911310549, + "curvature": -0.4942425385393566, + "holonomicRotation": 64.67020949929194, + "angularVelocity": 1.2183389099592048, + "holonomicAngularVelocity": -100.20274960216537 + }, + { + "time": 2.942404669760447, + "pose": { + "rotation": { + "radians": 2.167825078293914 + }, + "translation": { + "x": 3.3421440000000007, + "y": 3.9005681717896112 + } + }, + "velocity": -2.4654587683611564, + "acceleration": -0.3424034451960193, + "curvature": -0.49354387536872596, + "holonomicRotation": 64.16006812042824, + "angularVelocity": 1.217471688504721, + "holonomicAngularVelocity": -100.9061356007967 + }, + { + "time": 2.9474902571994575, + "pose": { + "rotation": { + "radians": 2.1740166370203244 + }, + "translation": { + "x": 3.3492594560000004, + "y": 3.8902390730651897 + } + }, + "velocity": -2.4671972558464152, + "acceleration": -0.34184595311906746, + "curvature": -0.49284857826519035, + "holonomicRotation": 63.64690114469775, + "angularVelocity": 1.2166078834262275, + "holonomicAngularVelocity": -101.59401075428647 + }, + { + "time": 2.9525703946381814, + "pose": { + "rotation": { + "radians": 2.1801971722771647 + }, + "translation": { + "x": 3.356435968000001, + "y": 3.8799579408555287 + } + }, + "velocity": -2.4689298629225123, + "acceleration": -0.3410551578565608, + "curvature": -0.4921570938094199, + "holonomicRotation": 63.13078960711478, + "angularVelocity": 1.2157480551464432, + "holonomicAngularVelocity": -102.26619168893342 + }, + { + "time": 2.9576451372375665, + "pose": { + "rotation": { + "radians": 2.186366780722736 + }, + "translation": { + "x": 3.3636731520000005, + "y": 3.869724987056909 + } + }, + "velocity": -2.470655436660188, + "acceleration": -0.3400317757761122, + "curvature": -0.4914698617718516, + "holonomicRotation": 62.611815007674075, + "angularVelocity": 1.2148927572524348, + "holonomicAngularVelocity": -102.9224975329877 + }, + { + "time": 2.9627145411259224, + "pose": { + "rotation": { + "radians": 2.192525562790287 + }, + "translation": { + "x": 3.3709706240000004, + "y": 3.85954042356561 + } + }, + "velocity": -2.4723728324069336, + "acceleration": -0.3387766657713812, + "curvature": -0.49078731495962646, + "holonomicRotation": 62.090059298481016, + "angularVelocity": 1.2140425361947913, + "holonomicAngularVelocity": -103.56275000564223 + }, + { + "time": 2.967778663386503, + "pose": { + "rotation": { + "radians": 2.198673622623123 + }, + "translation": { + "x": 3.3783280000000007, + "y": 3.8494044622779136 + } + }, + "velocity": -2.4740809138402975, + "acceleration": -0.33729071801042204, + "curvature": -0.4901098792969417, + "holonomicRotation": 61.565604870810475, + "angularVelocity": 1.2131979313363541, + "holonomicAngularVelocity": -104.18677350226001 + }, + { + "time": 2.9728375620451013, + "pose": { + "rotation": { + "radians": 2.2048110680105744 + }, + "translation": { + "x": 3.3857448960000003, + "y": 3.8393173150901005 + } + }, + "velocity": -2.4757785534211156, + "acceleration": -0.33557493347547346, + "curvature": -0.4894379737521331, + "holonomicRotation": 61.03853454209625, + "angularVelocity": 1.2123594749319873, + "holonomicAngularVelocity": -104.79439518315132 + }, + { + "time": 2.977891296057473, + "pose": { + "rotation": { + "radians": 2.2109380103242593 + }, + "translation": { + "x": 3.3932209280000003, + "y": 3.8292791938984516 + } + }, + "velocity": -2.4774646326332874, + "acceleration": -0.3336303826129742, + "curvature": -0.48877201035551443, + "holonomicRotation": 60.50893154285323, + "angularVelocity": 1.2115276919168534, + "holonomicAngularVelocity": -105.3854450568814 + }, + { + "time": 2.9829399252968494, + "pose": { + "rotation": { + "radians": 2.217054564453985 + }, + "translation": { + "x": 3.4007557120000005, + "y": 3.819290310599246 + } + }, + "velocity": -2.479138042187502, + "acceleration": -0.33145819882409144, + "curvature": -0.4881123942366022, + "holonomicRotation": 59.97687950353436, + "angularVelocity": 1.2107031000622246, + "holonomicAngularVelocity": -105.95975606700145 + }, + { + "time": 2.987983510541346, + "pose": { + "rotation": { + "radians": 2.223160848744925 + }, + "translation": { + "x": 3.4083488640000006, + "y": 3.8093508770887663 + } + }, + "velocity": -2.4807976824479416, + "acceleration": -0.32905962326121424, + "curvature": -0.4874595235784447, + "holonomicRotation": 59.44246244132438, + "angularVelocity": 1.2098862097183536, + "holonomicAngularVelocity": -106.51716417997883 + }, + { + "time": 2.9930221134612, + "pose": { + "rotation": { + "radians": 2.229256984933903 + }, + "translation": { + "x": 3.4160000000000004, + "y": 3.7994611052632923 + } + }, + "velocity": -2.4824424637147877, + "acceleration": -0.3264359769977284, + "curvature": -0.48681378963401156, + "holonomicRotation": 58.90576474687259, + "angularVelocity": 1.2090775239829443, + "holonomicAngularVelocity": -107.05750846728509 + }, + { + "time": 2.998055796606093, + "pose": { + "rotation": { + "radians": 2.2353430980872453 + }, + "translation": { + "x": 3.4237087360000005, + "y": 3.7896212070191044 + } + }, + "velocity": -2.4840713063948767, + "acceleration": -0.32358863941235766, + "curvature": -0.4861755767908796, + "holonomicRotation": 58.366871170966526, + "angularVelocity": 1.2082775384561817, + "holonomicAngularVelocity": -107.58063118750654 + }, + { + "time": 3.0030846233925312, + "pose": { + "rotation": { + "radians": 2.241419316538085 + }, + "translation": { + "x": 3.4314746880000007, + "y": 3.779831394252484 + } + }, + "velocity": -2.4856831413207248, + "acceleration": -0.32051907816648595, + "curvature": -0.4855452625816768, + "holonomicRotation": 57.825866811148884, + "angularVelocity": 1.2074867413515442, + "holonomicAngularVelocity": -108.08637787511516 + }, + { + "time": 3.008108658090934, + "pose": { + "rotation": { + "radians": 2.247485771824497 + }, + "translation": { + "x": 3.4392974720000007, + "y": 3.7700918788597115 + } + }, + "velocity": -2.4872769102582915, + "acceleration": -0.3172288873867211, + "curvature": -0.48492321762505763, + "holonomicRotation": 57.2828370982796, + "angularVelocity": 1.2067056134625382, + "holonomicAngularVelocity": -108.57459742055106 + }, + { + "time": 3.013127965812826, + "pose": { + "rotation": { + "radians": 2.2535425986282 + }, + "translation": { + "x": 3.4471767040000008, + "y": 3.7604028727370675 + } + }, + "velocity": -2.4888515658522445, + "acceleration": -0.3137196763380634, + "curvature": -0.4843098057902691, + "holonomicRotation": 56.73786778304532, + "angularVelocity": 1.2059346280378531, + "holonomicAngularVelocity": -109.04514215102253 + }, + { + "time": 3.0181426124980457, + "pose": { + "rotation": { + "radians": 2.259589934713281 + }, + "translation": { + "x": 3.4551120000000006, + "y": 3.750764587780833 + } + }, + "velocity": -2.490406072207704, + "acceleration": -0.30999319653791363, + "curvature": -0.4837053841163542, + "holonomicRotation": 56.19104492241842, + "angularVelocity": 1.2051742508556123, + "holonomicAngularVelocity": -109.49786791291324 + }, + { + "time": 3.023152664901871, + "pose": { + "rotation": { + "radians": 2.2656279208658088 + }, + "translation": { + "x": 3.463102976000001, + "y": 3.741177235887289 + } + }, + "velocity": -2.4919394049436834, + "acceleration": -0.30605123707062537, + "curvature": -0.4831103029406247, + "holonomicRotation": 55.64245486606757, + "angularVelocity": 1.2044249401110856, + "holonomicAngularVelocity": -109.93263414992425 + }, + { + "time": 3.0281581905822166, + "pose": { + "rotation": { + "radians": 2.2716567008335833 + }, + "translation": { + "x": 3.4711492480000006, + "y": 3.731641028952715 + } + }, + "velocity": -2.4934505516220318, + "acceleration": -0.30189569984268283, + "curvature": -0.48252490588364283, + "holonomicRotation": 55.092184242722105, + "angularVelocity": 1.2036871465900916, + "holonomicAngularVelocity": -110.3493039885737 + }, + { + "time": 3.033159257886518, + "pose": { + "rotation": { + "radians": 2.277676421267003 + }, + "translation": { + "x": 3.4792504320000006, + "y": 3.722156178873393 + } + }, + "velocity": -2.49493851217749, + "acceleration": -0.29752860038065254, + "curvature": -0.48194952983696343, + "holonomicRotation": 54.54031994649241, + "angularVelocity": 1.2029613133807362, + "holonomicAngularVelocity": -110.7477443083269 + }, + { + "time": 3.0381559359389714, + "pose": { + "rotation": { + "radians": 2.283687231659523 + }, + "translation": { + "x": 3.487406144000001, + "y": 3.7127228975456026 + } + }, + "velocity": -2.496402298692382, + "acceleration": -0.29295193717221096, + "curvature": -0.4813845052065131, + "holonomicRotation": 53.98694912314827, + "angularVelocity": 1.2022478761007076, + "holonomicAngularVelocity": -111.12782582146583 + }, + { + "time": 3.0431482946276227, + "pose": { + "rotation": { + "radians": 2.289689284289687 + }, + "translation": { + "x": 3.495616000000001, + "y": 3.7033413968656252 + } + }, + "velocity": -2.497840936318273, + "acceleration": -0.28816792133974267, + "curvature": -0.48083015571435544, + "holonomicRotation": 53.43215915635756, + "angularVelocity": 1.2015472627763575, + "holonomicAngularVelocity": -111.48942315254924 + }, + { + "time": 3.048136404591285, + "pose": { + "rotation": { + "radians": 2.295682734162953 + }, + "translation": { + "x": 3.5038796160000008, + "y": 3.694011888729742 + } + }, + "velocity": -2.4992534630817476, + "acceleration": -0.2831787538294935, + "curvature": -0.48028679863367535, + "holonomicRotation": 52.876037653887366, + "angularVelocity": 1.2008598938776565, + "holonomicAngularVelocity": -111.83241490896566 + }, + { + "time": 3.0531203372066305, + "pose": { + "rotation": { + "radians": 2.30166773895451 + }, + "translation": { + "x": 3.512196608000001, + "y": 3.684734585034233 + } + }, + "velocity": -2.5006389304550187, + "acceleration": -0.2779867787548527, + "curvature": -0.4797547447299139, + "holonomicRotation": 52.31867243376971, + "angularVelocity": 1.2001861822685285, + "holonomicAngularVelocity": -112.15668375425062 + }, + { + "time": 3.058100164575274, + "pose": { + "rotation": { + "radians": 2.3076444589524385 + }, + "translation": { + "x": 3.5205665920000007, + "y": 3.675509697675378 + } + }, + "velocity": -2.501996403341811, + "acceleration": -0.27259436649151636, + "curvature": -0.4792342984282738, + "holonomicRotation": 51.76015151043402, + "angularVelocity": 1.1995265333241345, + "holonomicAngularVelocity": -112.46211648169826 + }, + { + "time": 3.063075959510775, + "pose": { + "rotation": { + "radians": 2.3136130570019517 + }, + "translation": { + "x": 3.5289891840000007, + "y": 3.6663374385494594 + } + }, + "velocity": -2.5033249607201844, + "acceleration": -0.2670040457042024, + "curvature": -0.478725757729861, + "holonomicRotation": 51.20056308080867, + "angularVelocity": 1.1988813446963977, + "holonomicAngularVelocity": -112.74860408376729 + }, + { + "time": 3.0680477955256897, + "pose": { + "rotation": { + "radians": 2.3195736984491226 + }, + "translation": { + "x": 3.5374640000000004, + "y": 3.657218019552756 + } + }, + "velocity": -2.504623695499301, + "acceleration": -0.26121834574198977, + "curvature": -0.4782294144300909, + "holonomicRotation": 50.63999551039364, + "angularVelocity": 1.1982510067002008, + "holonomicAngularVelocity": -113.01604182264045 + }, + { + "time": 3.073015746818552, + "pose": { + "rotation": { + "radians": 2.325526551087033 + }, + "translation": { + "x": 3.5459906560000007, + "y": 3.648151652581551 + } + }, + "velocity": -2.505891715271616, + "acceleration": -0.25523997671568976, + "curvature": -0.47774555399467156, + "holonomicRotation": 50.07853731930663, + "angularVelocity": 1.1976359018500262, + "holonomicAngularVelocity": -113.26432929716299 + }, + { + "time": 3.0779798882608844, + "pose": { + "rotation": { + "radians": 2.3314717851002316 + }, + "translation": { + "x": 3.5545687680000007, + "y": 3.639138549532123 + } + }, + "velocity": -2.5071281420774696, + "acceleration": -0.24907163105990904, + "curvature": -0.4772744558135262, + "holonomicRotation": 49.51627716830462, + "angularVelocity": 1.1970364053482834, + "holonomicAngularVelocity": -113.4933705072513 + }, + { + "time": 3.08294029538427, + "pose": { + "rotation": { + "radians": 2.3374095730122733 + }, + "translation": { + "x": 3.563197952000001, + "y": 3.630178922300754 + } + }, + "velocity": -2.508332113108115, + "acceleration": -0.24271617242250404, + "curvature": -0.4768163930962272, + "holonomicRotation": 48.95330384478339, + "angularVelocity": 1.1964528846801932, + "holonomicAngularVelocity": -113.70307392083812 + }, + { + "time": 3.0878970443673177, + "pose": { + "rotation": { + "radians": 2.343340089631676 + }, + "translation": { + "x": 3.571877824000001, + "y": 3.621272982783724 + } + }, + "velocity": -2.5095027806524683, + "acceleration": -0.23617648348888906, + "curvature": -0.4763716330557826, + "holonomicRotation": 48.38970624875692, + "angularVelocity": 1.195885699914519, + "holonomicAngularVelocity": -113.89335253366002 + }, + { + "time": 3.0928502120227446, + "pose": { + "rotation": { + "radians": 2.34926351200008 + }, + "translation": { + "x": 3.5806080000000007, + "y": 3.612420942877314 + } + }, + "velocity": -2.510639312551345, + "acceleration": -0.22945556822238247, + "curvature": -0.4759404368985347, + "holonomicRotation": 47.825573378819044, + "angularVelocity": 1.1953352035476041, + "holonomicAngularVelocity": -114.06412393141328 + }, + { + "time": 3.0977998757844105, + "pose": { + "rotation": { + "radians": 2.355180019340123 + }, + "translation": { + "x": 3.5893880960000004, + "y": 3.603623014477804 + } + }, + "velocity": -2.5117408923267215, + "acceleration": -0.2225564863431604, + "curvature": -0.4755230599370365, + "holonomicRotation": 47.26099431808958, + "angularVelocity": 1.1948017406665197, + "holonomicAngularVelocity": -114.21531035103021 + }, + { + "time": 3.102746113694252, + "pose": { + "rotation": { + "radians": 2.3610897930045525 + }, + "translation": { + "x": 3.598217728000001, + "y": 3.5948794094814756 + } + }, + "velocity": -2.512806719663429, + "acceleration": -0.21548242444768187, + "curvature": -0.4751197515683634, + "holonomicRotation": 46.696058220147, + "angularVelocity": 1.1942856487011924, + "holonomicAngularVelocity": -114.34683873146564 + }, + { + "time": 3.1076890043895005, + "pose": { + "rotation": { + "radians": 2.3669930164249866 + }, + "translation": { + "x": 3.607096512000001, + "y": 3.586190339784609 + } + }, + "velocity": -2.5138360101605874, + "acceleration": -0.2082365483314603, + "curvature": -0.47473075552777316, + "holonomicRotation": 46.13085429495014, + "angularVelocity": 1.1937872578194393, + "holonomicAngularVelocity": -114.4586407752318 + }, + { + "time": 3.11262862708963, + "pose": { + "rotation": { + "radians": 2.372889875062837 + }, + "translation": { + "x": 3.616024064000001, + "y": 3.5775560172834857 + } + }, + "velocity": -2.5148279963615496, + "acceleration": -0.2008222613715301, + "curvature": -0.47435630965733216, + "holonomicRotation": 45.56547179475081, + "angularVelocity": 1.1933068905729998, + "holonomicAngularVelocity": -114.55065300129512 + }, + { + "time": 3.117565061583325, + "pose": { + "rotation": { + "radians": 2.3787805563590254 + }, + "translation": { + "x": 3.625000000000001, + "y": 3.5689766538743855 + } + }, + "velocity": -2.51578192720497, + "acceleration": -0.19324288504972006, + "curvature": -0.47399664627005966, + "holonomicRotation": 44.99999999999995, + "angularVelocity": 1.1928448621924224, + "holonomicAngularVelocity": -114.622816790235 + }, + { + "time": 3.1224983882157153, + "pose": { + "rotation": { + "radians": 2.3846652496859893 + }, + "translation": { + "x": 3.634023936000001, + "y": 3.560452461453589 + } + }, + "velocity": -2.5166970687976487, + "acceleration": -0.1855019261587106, + "curvature": -0.47365199201272495, + "holonomicRotation": 44.43452820524909, + "angularVelocity": 1.1924014804181657, + "holonomicAngularVelocity": -114.67507844083346 + }, + { + "time": 3.127428687875297, + "pose": { + "rotation": { + "radians": 2.3905441462989794 + }, + "translation": { + "x": 3.643095488000001, + "y": 3.5519836519173777 + } + }, + "velocity": -2.5175727044523204, + "acceleration": -0.17760292784030876, + "curvature": -0.4733225680044457, + "holonomicRotation": 43.86914570504977, + "angularVelocity": 1.191977045585318, + "holonomicAngularVelocity": -114.70738921137864 + }, + { + "time": 3.132356041981102, + "pose": { + "rotation": { + "radians": 2.3964174392885695 + }, + "translation": { + "x": 3.652214272000001, + "y": 3.543570437162032 + } + }, + "velocity": -2.518408134876489, + "acceleration": -0.16954949983895642, + "curvature": -0.47300858991591366, + "holonomicRotation": 43.303941779852906, + "angularVelocity": 1.1915718506935267, + "holonomicAngularVelocity": -114.71970536786596 + }, + { + "time": 3.137280532469733, + "pose": { + "rotation": { + "radians": 2.40228532353383 + }, + "translation": { + "x": 3.661379904000001, + "y": 3.535213029083832 + } + }, + "velocity": -2.5192026785969412, + "acceleration": -0.16134536604072128, + "curvature": -0.47271026795776055, + "holonomicRotation": 42.73900568191033, + "angularVelocity": 1.191186181363483, + "holonomicAngularVelocity": -114.71198822779128 + }, + { + "time": 3.142202241782349, + "pose": { + "rotation": { + "radians": 2.408147995655707 + }, + "translation": { + "x": 3.670592000000001, + "y": 3.5269116395790587 + } + }, + "velocity": -2.5199556720485856, + "acceleration": -0.15299429604956574, + "curvature": -0.47242780699268483, + "holonomicRotation": 42.17442662118084, + "angularVelocity": 1.1908203158165607, + "holonomicAngularVelocity": -114.68420419402902 + }, + { + "time": 3.147121252851942, + "pose": { + "rotation": { + "radians": 2.4140056539711043 + }, + "translation": { + "x": 3.679850176000001, + "y": 3.5186664805439936 + } + }, + "velocity": -2.5206664696512644, + "acceleration": -0.1445001022812573, + "curvature": -0.4721614066490972, + "holonomicRotation": 41.610293751242985, + "angularVelocity": 1.190474524961364, + "holonomicAngularVelocity": -114.63632479819923 + }, + { + "time": 3.1520376490903823, + "pose": { + "rotation": { + "radians": 2.4198584984475833 + }, + "translation": { + "x": 3.689154048000001, + "y": 3.5104777638749165 + } + }, + "velocity": -2.521334444410858, + "acceleration": -0.13586674612813227, + "curvature": -0.47191126123538196, + "holonomicRotation": 41.04669615521651, + "angularVelocity": 1.1901490724165622, + "holonomicAngularVelocity": -114.56832673867883 + }, + { + "time": 3.1569515143753777, + "pose": { + "rotation": { + "radians": 2.4257067306585007 + }, + "translation": { + "x": 3.698503232000001, + "y": 3.5023457014681085 + } + }, + "velocity": -2.5219589878529036, + "acceleration": -0.12709820188860255, + "curvature": -0.47167755990147925, + "holonomicRotation": 40.48372283169528, + "angularVelocity": 1.1898442144572068, + "holonomicAngularVelocity": -114.48019190561627 + }, + { + "time": 3.161862933037792, + "pose": { + "rotation": { + "radians": 2.4315505537387514 + }, + "translation": { + "x": 3.707897344000001, + "y": 3.49427050521985 + } + }, + "velocity": -2.5225395101493246, + "acceleration": -0.11819849544971883, + "curvature": -0.47146048672457774, + "holonomicRotation": 39.92146268069328, + "angularVelocity": 1.1895602000975292, + "holonomicAngularVelocity": -114.37190742014383 + }, + { + "time": 3.166771989848609, + "pose": { + "rotation": { + "radians": 2.4373901723409173 + }, + "translation": { + "x": 3.717336000000001, + "y": 3.4862523870264224 + } + }, + "velocity": -2.5230754407727187, + "acceleration": -0.10917181121496528, + "curvature": -0.471260220594255, + "holonomicRotation": 39.36000448960627, + "angularVelocity": 1.189297271060311, + "holonomicAngularVelocity": -114.24346565953405 + }, + { + "time": 3.171678770006046, + "pose": { + "rotation": { + "radians": 2.44322579259185 + }, + "translation": { + "x": 3.726818816000001, + "y": 3.4782915587841052 + } + }, + "velocity": -2.5235662281046483, + "acceleration": -0.10002227859865927, + "curvature": -0.4710769354852334, + "holonomicRotation": 38.79943691919124, + "angularVelocity": 1.189055661833772, + "holonomicAngularVelocity": -114.09486428153625 + }, + { + "time": 3.176583359122735, + "pose": { + "rotation": { + "radians": 2.4490576220500175 + }, + "translation": { + "x": 3.736345408000001, + "y": 3.4703882323891806 + } + }, + "velocity": -2.524011340151306, + "acceleration": -0.09075419695059489, + "curvature": -0.47091080031243926, + "holonomicRotation": 38.23984848956588, + "angularVelocity": 1.1888355996123872, + "holonomicAngularVelocity": -113.92610625049942 + }, + { + "time": 3.1814858432127964, + "pose": { + "rotation": { + "radians": 2.4548858696628155 + }, + "translation": { + "x": 3.745915392000001, + "y": 3.4625426197379277 + } + }, + "velocity": -2.5244102643344153, + "acceleration": -0.08137184655387633, + "curvature": -0.47076197912776857, + "holonomicRotation": 37.681327566230195, + "angularVelocity": 1.1886373044021066, + "holonomicAngularVelocity": -113.73719985418194 + }, + { + "time": 3.186386308679084, + "pose": { + "rotation": { + "radians": 2.4607107457249793 + }, + "translation": { + "x": 3.755528384000001, + "y": 3.4547549327266283 + } + }, + "velocity": -2.5247625078932936, + "acceleration": -0.07187961251875728, + "curvature": -0.4706306310845671, + "holonomicRotation": 37.12396234611252, + "angularVelocity": 1.1884609889574362, + "holonomicAngularVelocity": -113.52815873068948 + }, + { + "time": 3.1912848423000635, + "pose": { + "rotation": { + "radians": 2.46653246183661 + }, + "translation": { + "x": 3.765184000000001, + "y": 3.447025383251563 + } + }, + "velocity": -2.5250675982355864, + "acceleration": -0.062281973729059366, + "curvature": -0.4705169104173357, + "holonomicRotation": 36.567840843642344, + "angularVelocity": 1.1883068588401509, + "holonomicAngularVelocity": -113.29900187478664 + }, + { + "time": 3.1961815312170927, + "pose": { + "rotation": { + "radians": 2.472351230862322 + }, + "translation": { + "x": 3.7748818560000013, + "y": 3.4393541832090118 + } + }, + "velocity": -2.5253250825219324, + "acceleration": -0.052583345748317134, + "curvature": -0.47042096670266986, + "holonomicRotation": 36.01305087685163, + "angularVelocity": 1.1881751123985647, + "holonomicAngularVelocity": -113.04975365531308 + }, + { + "time": 3.2010764629215114, + "pose": { + "rotation": { + "radians": 2.478167266890403 + }, + "translation": { + "x": 3.7846215680000013, + "y": 3.4317415444952566 + } + }, + "velocity": -2.525534528618787, + "acceleration": -0.0427883593688127, + "curvature": -0.4703429446057929, + "holonomicRotation": 35.45968005350749, + "angularVelocity": 1.1880659407550722, + "holonomicAngularVelocity": -112.78044382583438 + }, + { + "time": 3.205969725241723, + "pose": { + "rotation": { + "radians": 2.4839807851922266 + }, + "translation": { + "x": 3.794402752000001, + "y": 3.424187679006577 + } + }, + "velocity": -2.525695524490209, + "acceleration": -0.03290154111643255, + "curvature": -0.47028298420432846, + "holonomicRotation": 34.907815757277795, + "angularVelocity": 1.1879795278928396, + "holonomicAngularVelocity": -112.4911075305061 + }, + { + "time": 3.210861406330348, + "pose": { + "rotation": { + "radians": 2.4897920021824937 + }, + "translation": { + "x": 3.8042250240000013, + "y": 3.416692798639254 + } + }, + "velocity": -2.525807679042665, + "acceleration": -0.022927609225605317, + "curvature": -0.47024122076600894, + "holonomicRotation": 34.357545133932334, + "angularVelocity": 1.1879160505732695, + "holonomicAngularVelocity": -112.18178531250864 + }, + { + "time": 3.2157515946512296, + "pose": { + "rotation": { + "radians": 2.4956011353791947 + }, + "translation": { + "x": 3.8140880000000013, + "y": 3.4092571152895688 + } + }, + "velocity": -2.5258706217414932, + "acceleration": -0.0128712218626321, + "curvature": -0.47021778497915745, + "holonomicRotation": 33.80895507758148, + "angularVelocity": 1.1878756783720692, + "holonomicAngularVelocity": -111.85252311199142 + }, + { + "time": 3.220640378966621, + "pose": { + "rotation": { + "radians": 2.5014084033642554 + }, + "translation": { + "x": 3.8239912960000013, + "y": 3.4018808408538015 + } + }, + "velocity": -2.5258840030950935, + "acceleration": -0.002737153602412999, + "curvature": -0.4702128028550573, + "holonomicRotation": 33.26213221695458, + "angularVelocity": 1.187858573687972, + "holonomicAngularVelocity": -111.50337226844077 + }, + { + "time": 3.2255278483242495, + "pose": { + "rotation": { + "radians": 2.507214025744352 + }, + "translation": { + "x": 3.8339345280000012, + "y": 3.3945641872282333 + } + }, + "velocity": -2.525847494579902, + "acceleration": 0.007469819761519579, + "curvature": -0.4702263958335384, + "holonomicRotation": 32.71716290172032, + "angularVelocity": 1.1878648917443635, + "holonomicAngularVelocity": -111.13438951908545 + }, + { + "time": 3.2304140920443007, + "pose": { + "rotation": { + "radians": 2.5130182231119065 + }, + "translation": { + "x": 3.843917312000001, + "y": 3.387307366309144 + } + }, + "velocity": -2.525760789056921, + "acceleration": 0.017744821574322937, + "curvature": -0.47025868070038246, + "holonomicRotation": 32.174133188851016, + "angularVelocity": 1.1878947806022002, + "holonomicAngularVelocity": -110.74563699043296 + }, + { + "time": 3.2352991997065046, + "pose": { + "rotation": { + "radians": 2.5188212170065185 + }, + "translation": { + "x": 3.853939264000001, + "y": 3.3801105899928157 + } + }, + "velocity": -2.5256236005006802, + "acceleration": 0.028083015918425037, + "curvature": -0.4703097697554681, + "holonomicRotation": 31.63312882903338, + "angularVelocity": 1.1879483810710896, + "holonomicAngularVelocity": -110.33718218871411 + }, + { + "time": 3.2401832611372594, + "pose": { + "rotation": { + "radians": 2.5246232298762354 + }, + "translation": { + "x": 3.864000000000001, + "y": 3.3729740701755277 + } + }, + "velocity": -2.525435664472835, + "acceleration": 0.03847945618823787, + "curvature": -0.47037977069829995, + "holonomicRotation": 31.094235253127316, + "angularVelocity": 1.1880258268210526, + "holonomicAngularVelocity": -109.90909799181578 + }, + { + "time": 3.2450663663965873, + "pose": { + "rotation": { + "radians": 2.5304244850394024 + }, + "translation": { + "x": 3.874099136000001, + "y": 3.365898018753562 + } + }, + "velocity": -2.5251967380340625, + "acceleration": 0.04892920100707005, + "curvature": -0.4704687867172178, + "holonomicRotation": 30.55753755867552, + "angularVelocity": 1.1881272442978712, + "holonomicAngularVelocity": -109.46146263436894 + }, + { + "time": 3.2499486057651135, + "pose": { + "rotation": { + "radians": 2.536225206646332 + }, + "translation": { + "x": 3.884236288000001, + "y": 3.358882647623198 + } + }, + "velocity": -2.524906600021658, + "acceleration": 0.059427240351045144, + "curvature": -0.47057691643683625, + "holonomicRotation": 30.02312049646555, + "angularVelocity": 1.1882527527596127, + "holonomicAngularVelocity": -108.99435968894919 + }, + { + "time": 3.2548300697311134, + "pose": { + "rotation": { + "radians": 2.5420256196414286 + }, + "translation": { + "x": 3.8944110720000014, + "y": 3.3519281686807174 + } + }, + "velocity": -2.5245650508903887, + "acceleration": 0.06996858599140866, + "curvature": -0.47070425402255034, + "holonomicRotation": 29.491068457146678, + "angularVelocity": 1.1884024641917774, + "holonomicAngularVelocity": -108.50787804778435 + }, + { + "time": 3.2597108489774813, + "pose": { + "rotation": { + "radians": 2.547825949724988 + }, + "translation": { + "x": 3.9046231040000015, + "y": 3.3450347938224 + } + }, + "velocity": -2.5241719131306364, + "acceleration": 0.08054815428190878, + "curvature": -0.4708508890641576, + "holonomicRotation": 28.96146545790366, + "angularVelocity": 1.1885764834315444, + "holonomicAngularVelocity": -108.0021119014011 + }, + { + "time": 3.264591034368628, + "pose": { + "rotation": { + "radians": 2.5536264233156913 + }, + "translation": { + "x": 3.9148720000000012, + "y": 3.338202734944528 + } + }, + "velocity": -2.523727031080917, + "acceleration": 0.09116089124942546, + "curvature": -0.47101690667937196, + "holonomicRotation": 28.434395129189433, + "angularVelocity": 1.188774907954385, + "holonomicAngularVelocity": -107.47716071321132 + }, + { + "time": 3.2694707169373736, + "pose": { + "rotation": { + "radians": 2.5594272675121985 + }, + "translation": { + "x": 3.9251573760000014, + "y": 3.33143220394338 + } + }, + "velocity": -2.523230271274464, + "acceleration": 0.10180166423829849, + "curvature": -0.47120238741225473, + "holonomicRotation": 27.909940701518906, + "angularVelocity": 1.1889978281035867, + "holonomicAngularVelocity": -106.93312919028769 + }, + { + "time": 3.2743499878718714, + "pose": { + "rotation": { + "radians": 2.5652287100560454 + }, + "translation": { + "x": 3.9354788480000007, + "y": 3.3247234127152385 + } + }, + "velocity": -2.5226815221802, + "acceleration": 0.11246538706927674, + "curvature": -0.4714074073516342, + "holonomicRotation": 27.388184992325836, + "angularVelocity": 1.1892453268004288, + "holonomicAngularVelocity": -106.37012725172265 + }, + { + "time": 3.2792289385025555, + "pose": { + "rotation": { + "radians": 2.571030979293276 + }, + "translation": { + "x": 3.945836032000001, + "y": 3.3180765731563833 + } + }, + "velocity": -2.5220806945623155, + "acceleration": 0.1231468943558796, + "curvature": -0.4716320380125646, + "holonomicRotation": 26.86921039288512, + "angularVelocity": 1.1895174797487822, + "holonomicAngularVelocity": -105.78827000146062 + }, + { + "time": 3.284107660288778, + "pose": { + "rotation": { + "radians": 2.5768343041368196 + }, + "translation": { + "x": 3.956228544000001, + "y": 3.311491897163096 + } + }, + "velocity": -2.5214277215293275, + "acceleration": 0.13384100623076972, + "curvature": -0.47187634632741926, + "holonomicRotation": 26.353098855302157, + "angularVelocity": 1.1898143552818996, + "holonomicAngularVelocity": -105.18767768515991 + }, + { + "time": 3.288986244805745, + "pose": { + "rotation": { + "radians": 2.5826389140285624 + }, + "translation": { + "x": 3.9666560000000013, + "y": 3.304969596631656 + } + }, + "velocity": -2.520722558272556, + "acceleration": 0.14454259310650955, + "curvature": -0.47214039474709035, + "holonomicRotation": 25.83993187957167, + "angularVelocity": 1.1901360143775113, + "holonomicAngularVelocity": -104.56847565338994 + }, + { + "time": 3.2938647837313253, + "pose": { + "rotation": { + "radians": 2.5884450389014386 + }, + "translation": { + "x": 3.977118016000001, + "y": 3.298509883458345 + } + }, + "velocity": -2.5199651825072515, + "acceleration": 0.15524643276555644, + "curvature": -0.47242424107305725, + "holonomicRotation": 25.32979050070796, + "angularVelocity": 1.189645268913223, + "holonomicAngularVelocity": -103.85770194645232 + }, + { + "time": 3.2987468022552946, + "pose": { + "rotation": { + "radians": 2.5942529091412254 + }, + "translation": { + "x": 3.9876142080000014, + "y": 3.292112969539443 + } + }, + "velocity": -2.515611684910395, + "acceleration": 0.891741310583367, + "curvature": -0.4727279384940904, + "holonomicRotation": 24.8227552759485, + "angularVelocity": 1.1859076290663977, + "holonomicAngularVelocity": -102.8458130887217 + }, + { + "time": 3.3036458738731236, + "pose": { + "rotation": { + "radians": 2.6000627555481515 + }, + "translation": { + "x": 3.9981441920000007, + "y": 3.285779066771231 + } + }, + "velocity": -2.5009144700569084, + "acceleration": 2.9999999999999094, + "curvature": -0.47305153562240315, + "holonomicRotation": 24.318906272032997, + "angularVelocity": 1.179747509794824, + "holonomicAngularVelocity": -101.60983224477155 + }, + { + "time": 3.3085723973050998, + "pose": { + "rotation": { + "radians": 2.605874809298971 + }, + "translation": { + "x": 4.0087075840000015, + "y": 3.27950838704999 + } + }, + "velocity": -2.4861348997609802, + "acceleration": 3.000000000000045, + "curvature": -0.4733950762761256, + "holonomicRotation": 23.818323052559983, + "angularVelocity": 1.1735879413475419, + "holonomicAngularVelocity": -100.36180530480989 + }, + { + "time": 3.3135268556832354, + "pose": { + "rotation": { + "radians": 2.611689301907459 + }, + "translation": { + "x": 4.019304000000001, + "y": 3.2733011422720004 + } + }, + "velocity": -2.471271524626574, + "acceleration": 2.9999999999999103, + "curvature": -0.47375859973557605, + "holonomicRotation": 23.321084665422756, + "angularVelocity": 1.1674273485551854, + "holonomicAngularVelocity": -99.10211376668313 + }, + { + "time": 3.318509746726897, + "pose": { + "rotation": { + "radians": 2.6175064651867004 + }, + "translation": { + "x": 4.029933056000002, + "y": 3.267157544333543 + } + }, + "velocity": -2.4563228514955893, + "acceleration": 3.0, + "curvature": -0.47414214031863405, + "holonomicRotation": 22.82726963032682, + "angularVelocity": 1.161264108636459, + "holonomicAngularVelocity": -97.83114215653715 + }, + { + "time": 3.3235215833785796, + "pose": { + "rotation": { + "radians": 2.6233265312086482 + }, + "translation": { + "x": 4.040594368000001, + "y": 3.2610778051308986 + } + }, + "velocity": -2.441287341540541, + "acceleration": 3.0, + "curvature": -0.4745457277939175, + "holonomicRotation": 22.336955926390708, + "angularVelocity": 1.155096550640754, + "holonomicAngularVelocity": -96.54927801758882 + }, + { + "time": 3.3285628944743486, + "pose": { + "rotation": { + "radians": 2.629149732266078 + }, + "translation": { + "x": 4.051287552000002, + "y": 3.2550621365603476 + } + }, + "velocity": -2.4261634082532337, + "acceleration": 3.000000000000088, + "curvature": -0.47496938687565216, + "holonomicRotation": 21.85022097983215, + "angularVelocity": 1.1489229537358603, + "holonomicAngularVelocity": -95.25691190089452 + }, + { + "time": 3.333634225451287, + "pose": { + "rotation": { + "radians": 2.6349763008314744 + }, + "translation": { + "x": 4.062012224000001, + "y": 3.2491107505181707 + } + }, + "velocity": -2.410949415322418, + "acceleration": 3.000000000000044, + "curvature": -0.4754131375406458, + "holonomicRotation": 21.367141651741637, + "angularVelocity": 1.1427415464961193, + "holonomicAngularVelocity": -93.95443735758198 + }, + { + "time": 3.3387361390945163, + "pose": { + "rotation": { + "radians": 2.6408064695182274 + }, + "translation": { + "x": 4.072768000000002, + "y": 3.2432238589006492 + } + }, + "velocity": -2.395643674392731, + "acceleration": 3.0, + "curvature": -0.47587699474317613, + "holonomicRotation": 20.887794225945097, + "angularVelocity": 1.1365505050778797, + "holonomicAngularVelocity": -92.64225093318588 + }, + { + "time": 3.3438692163265253, + "pose": { + "rotation": { + "radians": 2.6466404710388716 + }, + "translation": { + "x": 4.0835544960000005, + "y": 3.2374016736040634 + } + }, + "velocity": -2.3802444426967027, + "acceleration": 3.0, + "curvature": -0.4763609684774519, + "holonomicRotation": 20.41225439695785, + "angularVelocity": 1.1303479523230882, + "holonomicAngularVelocity": -91.32075216362355 + }, + { + "time": 3.349034057041824, + "pose": { + "rotation": { + "radians": 2.652478538165484 + }, + "translation": { + "x": 4.094371328000001, + "y": 3.2316444065246936 + } + }, + "velocity": -2.3647499205508073, + "acceleration": 3.0, + "curvature": -0.4768650635862502, + "holonomicRotation": 19.940597258031488, + "angularVelocity": 1.1241319557916827, + "holonomicAngularVelocity": -89.99034357339347 + }, + { + "time": 3.3542312809901262, + "pose": { + "rotation": { + "radians": 2.6583209036871764 + }, + "translation": { + "x": 4.105218112000001, + "y": 3.225952269558821 + } + }, + "velocity": -2.3491582487059004, + "acceleration": 3.0000000000000426, + "curvature": -0.4773892799171057, + "holonomicRotation": 19.4728972892959, + "angularVelocity": 1.117900526573738, + "holonomicAngularVelocity": -88.65143067557575 + }, + { + "time": 3.359461528711619, + "pose": { + "rotation": { + "radians": 2.6641678003691442 + }, + "translation": { + "x": 4.1160944640000015, + "y": 3.2203254746027263 + } + }, + "velocity": -2.3334675055414222, + "acceleration": 3.0, + "curvature": -0.47793361195538225, + "holonomicRotation": 19.0092283459979, + "angularVelocity": 1.111651617315528, + "holonomicAngularVelocity": -87.30442197418871 + }, + { + "time": 3.3647254625281113, + "pose": { + "rotation": { + "radians": 2.67001946090969 + }, + "translation": { + "x": 4.127000000000001, + "y": 3.21476423355269 + } + }, + "velocity": -2.317675704091945, + "acceleration": 3.0, + "curvature": -0.47849804904257176, + "holonomicRotation": 18.54966364683865, + "angularVelocity": 1.1053831206956037, + "holonomicAngularVelocity": -85.9497289685807 + }, + { + "time": 3.370023767594245, + "pose": { + "rotation": { + "radians": 2.67587611789809 + }, + "translation": { + "x": 4.137934336000002, + "y": 3.209268758304993 + } + }, + "velocity": -2.3017807888935438, + "acceleration": 3.0000000000000417, + "curvature": -0.4790825750718403, + "holonomicRotation": 18.0942757624116, + "angularVelocity": 1.099092867194516, + "holonomicAngularVelocity": -84.58776616038908 + }, + { + "time": 3.375357153013284, + "pose": { + "rotation": { + "radians": 2.681738003770155 + }, + "translation": { + "x": 4.148897088000001, + "y": 3.2038392607559154 + } + }, + "velocity": -2.2857806326364263, + "acceleration": 3.000000000000125, + "curvature": -0.4796871685309068, + "holonomicRotation": 17.643136603742704, + "angularVelocity": 1.0927786234901078, + "holonomicAngularVelocity": -83.2189510626718 + }, + { + "time": 3.3807263530224416, + "pose": { + "rotation": { + "radians": 2.6876053507654056 + }, + "translation": { + "x": 4.159887872000001, + "y": 3.1984759528017386 + } + }, + "velocity": -2.269673032608953, + "acceleration": 2.9999999999999587, + "curvature": -0.48031180230736537, + "holonomicRotation": 17.1963174109349, + "angularVelocity": 1.0864380898787445, + "holonomicAngularVelocity": -81.84370421196407 + }, + { + "time": 3.3861321282531263, + "pose": { + "rotation": { + "radians": 2.693478390881344 + }, + "translation": { + "x": 4.170906304000001, + "y": 3.1931790463387437 + } + }, + "velocity": -2.2534557069169003, + "acceleration": 3.0, + "curvature": -0.4809564438096599, + "holonomicRotation": 16.75388874191842, + "angularVelocity": 1.0800688983034952, + "holonomicAngularVelocity": -80.46244918269026 + }, + { + "time": 3.391575267072019, + "pose": { + "rotation": { + "radians": 2.699357355828779 + }, + "translation": { + "x": 4.181952000000002, + "y": 3.18794875326321 + } + }, + "velocity": -2.237126290460221, + "acceleration": 2.999999999999959, + "curvature": -0.4816210545661112, + "holonomicRotation": 16.315920461308902, + "angularVelocity": 1.0736686097512136, + "holonomicAngularVelocity": -79.07561260483114 + }, + { + "time": 3.3970565870094314, + "pose": { + "rotation": { + "radians": 2.705242476985582 + }, + "translation": { + "x": 4.193024576000001, + "y": 3.1827852854714194 + } + }, + "velocity": -2.220682330647984, + "acceleration": 3.000000000000081, + "curvature": -0.4823055904398, + "holonomicRotation": 15.882481729374959, + "angularVelocity": 1.0672347117267755, + "holonomicAngularVelocity": -77.6836241842693 + }, + { + "time": 3.402576936282025, + "pose": { + "rotation": { + "radians": 2.7111339853501493 + }, + "translation": { + "x": 4.204123648000001, + "y": 3.1776888548596522 + } + }, + "velocity": -2.2041212828302035, + "acceleration": 3.0000000000000404, + "curvature": -0.4830100011954003, + "holonomicRotation": 15.453640991116902, + "angularVelocity": 1.0607646153637207, + "holonomicAngularVelocity": -76.2869167266683 + }, + { + "time": 3.408137195417652, + "pose": { + "rotation": { + "radians": 2.7170321114934755 + }, + "translation": { + "x": 4.215248832000001, + "y": 3.1726596733241887 + } + }, + "velocity": -2.187440505423322, + "acceleration": 3.0, + "curvature": -0.48373423076088373, + "holonomicRotation": 15.029465965458614, + "angularVelocity": 1.0542556526712294, + "holonomicAngularVelocity": -74.88592616458293 + }, + { + "time": 3.4137382789908584, + "pose": { + "rotation": { + "radians": 2.722937085511612 + }, + "translation": { + "x": 4.226399744000002, + "y": 3.1676979527613107 + } + }, + "velocity": -2.1706372547037036, + "acceleration": 3.0, + "curvature": -0.4844782167624344, + "holonomicRotation": 14.610023634553844, + "angularVelocity": 1.047705073073782, + "holonomicAngularVelocity": -73.4810915883214 + }, + { + "time": 3.4193811374784255, + "pose": { + "rotation": { + "radians": 2.7288491369756738 + }, + "translation": { + "x": 4.237576000000001, + "y": 3.162803905067298 + } + }, + "velocity": -2.1537086792410016, + "acceleration": 2.9999999999999605, + "curvature": -0.48524189070637075, + "holonomicRotation": 14.195380233208967, + "angularVelocity": 1.0411100403867837, + "holonomicAngularVelocity": -72.07285528061695 + }, + { + "time": 3.4250667592453135, + "pose": { + "rotation": { + "radians": 2.7347684948830224 + }, + "translation": { + "x": 4.2487772160000015, + "y": 3.157977742138431 + } + }, + "velocity": -2.136651813940338, + "acceleration": 3.0, + "curvature": -0.4860251776092164, + "holonomicRotation": 13.785601238423729, + "angularVelocity": 1.0344676288016506, + "holonomicAngularVelocity": -70.66166275559415 + }, + { + "time": 3.4307961726723977, + "pose": { + "rotation": { + "radians": 2.7406953876053626 + }, + "translation": { + "x": 4.260003008000001, + "y": 3.153219675870991 + } + }, + "velocity": -2.119463573659085, + "acceleration": 3.0, + "curvature": -0.486827996068867, + "holonomicRotation": 13.38075135905173, + "angularVelocity": 1.0277748193668228, + "holonomicAngularVelocity": -69.24796280197698 + }, + { + "time": 3.4365704484386477, + "pose": { + "rotation": { + "radians": 2.746630042837994 + }, + "translation": { + "x": 4.271252992000002, + "y": 3.148529918161259 + } + }, + "velocity": -2.1021407463603357, + "acceleration": 3.0000000000000386, + "curvature": -0.4876502580522222, + "holonomicRotation": 12.980894525582109, + "angularVelocity": 1.0210284955354487, + "holonomicAngularVelocity": -67.83220753132842 + }, + { + "time": 3.4423907019716853, + "pose": { + "rotation": { + "radians": 2.7525726875464667 + }, + "translation": { + "x": 4.282526784000001, + "y": 3.1439086809055152 + } + }, + "velocity": -2.0846799857612224, + "acceleration": 2.999999999999962, + "curvature": -0.4884918687304187, + "holonomicRotation": 12.586093880044139, + "angularVelocity": 1.0142254389443273, + "holonomicAngularVelocity": -66.4148524311318 + }, + { + "time": 3.448258096082239, + "pose": { + "rotation": { + "radians": 2.7585235479137022 + }, + "translation": { + "x": 4.293824000000002, + "y": 3.1393561760000406 + } + }, + "velocity": -2.067077803429562, + "acceleration": 2.9999999999999623, + "curvature": -0.4893527263930312, + "holonomicRotation": 12.19641176603643, + "angularVelocity": 1.0073623243585152, + "holonomicAngularVelocity": -64.99635642368415 + }, + { + "time": 3.454173843799663, + "pose": { + "rotation": { + "radians": 2.7644828492846454 + }, + "translation": { + "x": 4.305144256000001, + "y": 3.134872615341115 + } + }, + "velocity": -2.0493305602772884, + "acceleration": 3.000000000000075, + "curvature": -0.49023272239377874, + "holonomicRotation": 11.811909718882124, + "angularVelocity": 1.0004357148136194, + "holonomicAngularVelocity": -63.577181930649736 + }, + { + "time": 3.4601392114276917, + "pose": { + "rotation": { + "radians": 2.770450816111718 + }, + "translation": { + "x": 4.316487168000002, + "y": 3.1304582108250205 + } + }, + "velocity": -2.031434457393203, + "acceleration": 3.0000000000000373, + "curvature": -0.4911317407982573, + "holonomicRotation": 11.43264845591175, + "angularVelocity": 0.9934420556728796, + "holonomicAngularVelocity": -62.15779494429541 + }, + { + "time": 3.466155521841721, + "pose": { + "rotation": { + "radians": 2.7764276718969976 + }, + "translation": { + "x": 4.327852352000001, + "y": 3.126113174348037 + } + }, + "velocity": -2.013385526151115, + "acceleration": 2.9999999999999263, + "curvature": -0.4920496584562309, + "holonomicRotation": 11.058687866875282, + "angularVelocity": 0.9863776690339577, + "holonomicAngularVelocity": -60.73866510544962 + }, + { + "time": 3.472224158051466, + "pose": { + "rotation": { + "radians": 2.7824136391357808 + }, + "translation": { + "x": 4.339239424000002, + "y": 3.121837717806445 + } + }, + "velocity": -1.9951796175218803, + "acceleration": 3.0000000000000364, + "curvature": -0.4929863447293872, + "holonomicRotation": 10.690087004484784, + "angularVelocity": 0.9792387468544239, + "holonomicAngularVelocity": -59.32026578919857 + }, + { + "time": 3.478346567055599, + "pose": { + "rotation": { + "radians": 2.788408939256718 + }, + "translation": { + "x": 4.350648000000001, + "y": 3.117632053096526 + } + }, + "velocity": -1.9768123905094812, + "acceleration": 3.0000000000000364, + "curvature": -0.4939416613608526, + "holonomicRotation": 10.326904075089434, + "angularVelocity": 0.9720213442853626, + "holonomicAngularVelocity": -57.90307419853841 + }, + { + "time": 3.484524264018268, + "pose": { + "rotation": { + "radians": 2.7944137925629597 + }, + "translation": { + "x": 4.362077696000002, + "y": 3.11349639211456 + } + }, + "velocity": -1.9582792996214728, + "acceleration": 3.0, + "curvature": -0.49491546234555484, + "holonomicRotation": 9.9691964294839, + "angularVelocity": 0.964721371740083, + "holonomicAngularVelocity": -56.487571467135446 + }, + { + "time": 3.490758836801003, + "pose": { + "rotation": { + "radians": 2.8004284181701333 + }, + "translation": { + "x": 4.373528128000001, + "y": 3.1094309467568277 + } + }, + "velocity": -1.9395755812732676, + "acceleration": 3.0, + "curvature": -0.4959075937861373, + "holonomicRotation": 9.617020553852093, + "angularVelocity": 0.9573345869699085, + "holonomicAngularVelocity": -55.074242771548406 + }, + { + "time": 3.4970519508877898, + "pose": { + "rotation": { + "radians": 2.8064530339451617 + }, + "translation": { + "x": 4.384998912000002, + "y": 3.1054359289196105 + } + }, + "velocity": -1.920696239012908, + "acceleration": 2.999999999999965, + "curvature": -0.4969178936404211, + "holonomicRotation": 9.270432060847359, + "angularVelocity": 0.949856585776932, + "holonomicAngularVelocity": -53.66357745423141 + }, + { + "time": 3.503405354745884, + "pose": { + "rotation": { + "radians": 2.812487856441873 + }, + "translation": { + "x": 4.3964896640000015, + "y": 3.1015115504991884 + } + }, + "velocity": -1.9016360274386255, + "acceleration": 3.000000000000035, + "curvature": -0.4979461917199805, + "holonomicRotation": 8.929485680810512, + "angularVelocity": 0.9422827925820624, + "holonomicAngularVelocity": -52.25606915784077 + }, + { + "time": 3.5098208856705417, + "pose": { + "rotation": { + "radians": 2.818533100837456 + }, + "translation": { + "x": 4.408000000000002, + "y": 3.0976580233918423 + } + }, + "velocity": -1.882389434664652, + "acceleration": 3.0, + "curvature": -0.4989923093779659, + "holonomicRotation": 8.594235253127325, + "angularVelocity": 0.9346084494762857, + "holonomicAngularVelocity": -50.85221597241104 + }, + { + "time": 3.516300476168219, + "pose": { + "rotation": { + "radians": 2.824588980865731 + }, + "translation": { + "x": 4.4195295360000015, + "y": 3.0938755594938527 + } + }, + "velocity": -1.8629506631716213, + "acceleration": 3.0, + "curvature": -0.5000560594170246, + "holonomicRotation": 8.264733717726678, + "angularVelocity": 0.92682860485997, + "holonomicAngularVelocity": -49.452520596195136 + }, + { + "time": 3.5228461609402677, + "pose": { + "rotation": { + "radians": 2.8306557087508626 + }, + "translation": { + "x": 4.431077888000002, + "y": 3.090164370701501 + } + }, + "velocity": -1.8433136088554738, + "acceleration": 3.0000000000000338, + "curvature": -0.5011372458585169, + "holonomicRotation": 7.941033106720719, + "angularVelocity": 0.9189381004802402, + "holonomicAngularVelocity": -48.05749051188394 + }, + { + "time": 3.5294600845377504, + "pose": { + "rotation": { + "radians": 2.8367334951382546 + }, + "translation": { + "x": 4.442644672000001, + "y": 3.0865246689110672 + } + }, + "velocity": -1.8234718380630266, + "acceleration": 2.9999999999999662, + "curvature": -0.5022356639002403, + "holonomicRotation": 7.62318453618838, + "angularVelocity": 0.910931557733977, + "holonomicAngularVelocity": -46.66763817947671 + }, + { + "time": 3.5361445097680377, + "pose": { + "rotation": { + "radians": 2.8428225490258363 + }, + "translation": { + "x": 4.454229504000002, + "y": 3.0829566660188323 + } + }, + "velocity": -1.803418562372165, + "acceleration": 3.0000000000000333, + "curvature": -0.5033510995965044, + "holonomicRotation": 7.311238198103576, + "angularVelocity": 0.9028033620501636, + "holonomicAngularVelocity": -45.28348124775536 + }, + { + "time": 3.5429018269455783, + "pose": { + "rotation": { + "radians": 2.8489230776921595 + }, + "translation": { + "x": 4.465832000000002, + "y": 3.079460573921077 + } + }, + "velocity": -1.7831466108395428, + "acceleration": 2.999999999999967, + "curvature": -0.504483329742147, + "holonomicRotation": 7.005243352409272, + "angularVelocity": 0.894547646292854, + "holonomicAngularVelocity": -43.905542786037614 + }, + { + "time": 3.549734564092968, + "pose": { + "rotation": { + "radians": 2.8550352866250943 + }, + "translation": { + "x": 4.477451776000002, + "y": 3.0760366045140817 + } + }, + "velocity": -1.7626483993973747, + "acceleration": 3.0000000000000324, + "curvature": -0.5056321217279383, + "holonomicRotation": 6.705248319238822, + "angularVelocity": 0.8861582718762545, + "holonomicAngularVelocity": -42.53435153875966 + }, + { + "time": 3.5566453982145445, + "pose": { + "rotation": { + "radians": 2.8611593794474945 + }, + "translation": { + "x": 4.489088448000001, + "y": 3.0726849696941274 + } + }, + "velocity": -1.7419158970326432, + "acceleration": 3.000000000000032, + "curvature": -0.506797233320977, + "holonomicRotation": 6.411300471285606, + "angularVelocity": 0.8776288083554918, + "holonomicAngularVelocity": -41.170442204949744 + }, + { + "time": 3.5636371677838463, + "pose": { + "rotation": { + "radians": 2.8672955578428967 + }, + "translation": { + "x": 4.500741632000002, + "y": 3.069405881357495 + } + }, + "velocity": -1.720940588324739, + "acceleration": 3.0, + "curvature": -0.507978412463492, + "holonomicRotation": 6.123446226322358, + "angularVelocity": 0.8689525103825774, + "holonomicAngularVelocity": -39.81435574581486 + }, + { + "time": 3.5707128866087237, + "pose": { + "rotation": { + "radians": 2.8734440214785355 + }, + "translation": { + "x": 4.512410944000001, + "y": 3.066199551400465 + } + }, + "velocity": -1.6997134318501057, + "acceleration": 3.0, + "curvature": -0.5091753972438993, + "holonomicRotation": 5.8417310398713145, + "angularVelocity": 0.8601222925409711, + "holonomicAngularVelocity": -38.46663972327497 + }, + { + "time": 3.5778757592648063, + "pose": { + "rotation": { + "radians": 2.879604967928664 + }, + "translation": { + "x": 4.524096000000002, + "y": 3.0630661917193183 + } + }, + "velocity": -1.6782248138818594, + "acceleration": 2.999999999999969, + "curvature": -0.5103879154242502, + "holonomicRotation": 5.566199398026104, + "angularVelocity": 0.8511307007328054, + "holonomicAngularVelocity": -37.127848673380825 + }, + { + "time": 3.585129198319898, + "pose": { + "rotation": { + "radians": 2.885778592594347 + }, + "translation": { + "x": 4.535796416000001, + "y": 3.0600060142103347 + } + }, + "velocity": -1.6564644967165831, + "acceleration": 3.0000000000000306, + "curvature": -0.5116156846177222, + "holonomicRotation": 5.296894810427057, + "angularVelocity": 0.8419698808848267, + "holonomicAngularVelocity": -35.79854451851486 + }, + { + "time": 3.5924768436103527, + "pose": { + "rotation": { + "radians": 2.8919650886243353 + }, + "translation": { + "x": 4.547511808000002, + "y": 3.0570192307697956 + } + }, + "velocity": -1.6344215608452184, + "acceleration": 3.00000000000003, + "curvature": -0.5128584118230285, + "holonomicRotation": 5.033859803390453, + "angularVelocity": 0.8326315431606574, + "holonomicAngularVelocity": -34.479297023430654 + }, + { + "time": 3.5999225838765447, + "pose": { + "rotation": { + "radians": 2.8981646468321483 + }, + "translation": { + "x": 4.559241792000002, + "y": 3.054106053293982 + } + }, + "velocity": -1.6120843400466425, + "acceleration": 2.9999999999999702, + "curvature": -0.5141157933934679, + "holonomicRotation": 4.7771359131931, + "angularVelocity": 0.8231069223561549, + "holonomicAngularVelocity": -33.170684300285295 + }, + { + "time": 3.607470581120536, + "pose": { + "rotation": { + "radians": 2.904377455613602 + }, + "translation": { + "x": 4.570985984000002, + "y": 3.051266693679174 + } + }, + "velocity": -1.5894403483146702, + "acceleration": 3.0000000000000293, + "curvature": -0.5153875148097261, + "holonomicRotation": 4.5267636795132615, + "angularVelocity": 0.8133867328283466, + "holonomicAngularVelocity": -31.873293369280947 + }, + { + "time": 3.6151252981169075, + "pose": { + "rotation": { + "radians": 2.9106037008620067 + }, + "translation": { + "x": 4.582744000000002, + "y": 3.0485013638216527 + } + }, + "velocity": -1.5664761973255548, + "acceleration": 2.999999999999971, + "curvature": -0.5166732504881834, + "holonomicRotation": 4.2827826390290795, + "angularVelocity": 0.8034611176815546, + "holonomicAngularVelocity": -30.5877207820835 + }, + { + "time": 3.622891529590848, + "pose": { + "rotation": { + "radians": 2.9168435658822327 + }, + "translation": { + "x": 4.594515456000002, + "y": 3.045810275617699 + } + }, + "velocity": -1.5431775029037327, + "acceleration": 3.0000000000000284, + "curvature": -0.5179726636388742, + "holonomicRotation": 4.045231319175155, + "angularVelocity": 0.7933195910243753, + "holonomicAngularVelocity": -29.31457331653535 + }, + { + "time": 3.630774437679643, + "pose": { + "rotation": { + "radians": 2.923097231303318 + }, + "translation": { + "x": 4.606299968000002, + "y": 3.043193640963593 + } + }, + "velocity": -1.5195287786373484, + "acceleration": 3.0, + "curvature": -0.5192854061280454, + "holonomicRotation": 3.814147232058671, + "angularVelocity": 0.7829509721905035, + "holonomicAngularVelocity": -28.054468752649058 + }, + { + "time": 3.6387795924199784, + "pose": { + "rotation": { + "radians": 2.9293648749897994 + }, + "translation": { + "x": 4.6180971520000025, + "y": 3.040651671755615 + } + }, + "velocity": -1.495513314416341, + "acceleration": 3.0000000000000275, + "curvature": -0.5206111181933657, + "holonomicRotation": 3.589566868535803, + "angularVelocity": 0.7723433104766161, + "holonomicAngularVelocity": -26.80803674146673 + }, + { + "time": 3.6469130181605207, + "pose": { + "rotation": { + "radians": 2.9356466719517655 + }, + "translation": { + "x": 4.629906624000001, + "y": 3.0381845798900473 + } + }, + "velocity": -1.4711130371947143, + "acceleration": 2.999999999999973, + "curvature": -0.5219494283315183, + "holonomicRotation": 3.371525692449353, + "angularVelocity": 0.7614837986078355, + "holonomicAngularVelocity": -25.575919780440216 + }, + { + "time": 3.6551812469959284, + "pose": { + "rotation": { + "radians": 2.9419427942531104 + }, + "translation": { + "x": 4.641728000000002, + "y": 3.035792577263169 + } + }, + "velocity": -1.4463083506884924, + "acceleration": 2.9999999999999463, + "curvature": -0.5232999531912867, + "holonomicRotation": 3.1600581350286494, + "angularVelocity": 0.7503586730135737, + "holonomicAngularVelocity": -24.35877431164802 + }, + { + "time": 3.663591380566494, + "pose": { + "rotation": { + "radians": 2.948253410918987 + }, + "translation": { + "x": 4.653560896000002, + "y": 3.0334758757712623 + } + }, + "velocity": -1.4210779499767952, + "acceleration": 3.0, + "curvature": -0.5246622973501797, + "holonomicRotation": 2.955197589452424, + "angularVelocity": 0.7389530980618538, + "holonomicAngularVelocity": -23.15727196183363 + }, + { + "time": 3.672151161882855, + "pose": { + "rotation": { + "radians": 2.954578687841444 + }, + "translation": { + "x": 4.665404928000002, + "y": 3.031234687310606 + } + }, + "velocity": -1.3953986060277122, + "acceleration": 3.0000000000000258, + "curvature": -0.5260360531124699, + "holonomicRotation": 2.7569764055756316, + "angularVelocity": 0.7272510313156108, + "holonomicAngularVelocity": -21.972100947492603 + }, + { + "time": 3.680869059238794, + "pose": { + "rotation": { + "radians": 2.960918787684454 + }, + "translation": { + "x": 4.6772597120000015, + "y": 3.029069223777483 + } + }, + "velocity": -1.3692449139598961, + "acceleration": 2.9999999999999747, + "curvature": -0.5274208004195751, + "holonomicRotation": 2.5654258848210665, + "angularVelocity": 0.7152350654277648, + "holonomicAngularVelocity": -20.8039676724005 + }, + { + "time": 3.689754364796628, + "pose": { + "rotation": { + "radians": 2.9672738697864576 + }, + "translation": { + "x": 4.689124864000002, + "y": 3.0269796970681724 + } + }, + "velocity": -1.3425889972863922, + "acceleration": 3.0, + "curvature": -0.5288161067204993, + "holonomicRotation": 2.3805762752364745, + "angularVelocity": 0.7028862421564607, + "holonomicAngularVelocity": -19.653598551045544 + }, + { + "time": 3.6988173111092735, + "pose": { + "rotation": { + "radians": 2.9736440900630186 + }, + "translation": { + "x": 4.701000000000001, + "y": 3.0249663190789553 + } + }, + "velocity": -1.3154001583484565, + "acceleration": 3.0, + "curvature": -0.530221526743801, + "holonomicRotation": 2.202456766718066, + "angularVelocity": 0.6901838315299829, + "holonomicAngularVelocity": -18.521742098502607 + }, + { + "time": 3.7080692097376904, + "pose": { + "rotation": { + "radians": 2.980029600907306 + }, + "translation": { + "x": 4.712884736000002, + "y": 3.0230293017061127 + } + }, + "velocity": -1.2876444624632064, + "acceleration": 3.0, + "curvature": -0.5316366023821089, + "holonomicRotation": 2.031095486401042, + "angularVelocity": 0.6771050683668322, + "holonomicAngularVelocity": -17.409171336522384 + }, + { + "time": 3.7175226173124507, + "pose": { + "rotation": { + "radians": 2.986430551089514 + }, + "translation": { + "x": 4.724778688000002, + "y": 3.021168856845925 + } + }, + "velocity": -1.2592842397389246, + "acceleration": 3.0000000000000235, + "curvature": -0.5330608626280348, + "holonomicRotation": 1.8665194942180574, + "angularVelocity": 0.6636248354538734, + "holonomicAngularVelocity": -16.3166865772123 + }, + { + "time": 3.727191535989376, + "pose": { + "rotation": { + "radians": 2.992847085655505 + }, + "translation": { + "x": 4.736681472000003, + "y": 3.0193851963946727 + } + }, + "velocity": -1.2302774837081494, + "acceleration": 3.000000000000023, + "curvature": -0.5344938233678657, + "holonomicRotation": 1.7087547786261181, + "angularVelocity": 0.6497152794409348, + "holonomicAngularVelocity": -15.245118660858344 + }, + { + "time": 3.7370916574302786, + "pose": { + "rotation": { + "radians": 2.9992793458239806 + }, + "translation": { + "x": 4.748592704000002, + "y": 3.0176785322486377 + } + }, + "velocity": -1.2005771193854406, + "acceleration": 3.0, + "curvature": -0.5359349872727417, + "holonomicRotation": 1.5578262525026454, + "angularVelocity": 0.6353453412347533, + "holonomicAngularVelocity": -14.195332743797312 + }, + { + "time": 3.7472406624512438, + "pose": { + "rotation": { + "radians": 3.0057274688822186 + }, + "translation": { + "x": 4.760512000000002, + "y": 3.016049076304099 + } + }, + "velocity": -1.1701301043225467, + "acceleration": 2.9999999999999782, + "curvature": -0.5373838437421952, + "holonomicRotation": 1.4137577492115816, + "angularVelocity": 0.62048017653661, + "holonomicAngularVelocity": -13.168232758378622 + }, + { + "time": 3.757658592699678, + "pose": { + "rotation": { + "radians": 3.0121915880819135 + }, + "translation": { + "x": 4.772438976000002, + "y": 3.0144970404573384 + } + }, + "velocity": -1.1388763135772426, + "acceleration": 3.0000000000000213, + "curvature": -0.5388398686776363, + "holonomicRotation": 1.2765720188396412, + "angularVelocity": 0.6050804328165802, + "holonomicAngularVelocity": -12.164766700902831 + }, + { + "time": 3.768368316728004, + "pose": { + "rotation": { + "radians": 3.018671832532319 + }, + "translation": { + "x": 4.784373248000002, + "y": 3.0130226366046355 + } + }, + "velocity": -1.106747141492265, + "acceleration": 3.000000000000021, + "curvature": -0.5403025245756908, + "holonomicRotation": 1.1462907246040044, + "angularVelocity": 0.589101337479254, + "holonomicAngularVelocity": -11.185932950216882 + }, + { + "time": 3.779396121536215, + "pose": { + "rotation": { + "radians": 3.0251683270942964 + }, + "translation": { + "x": 4.796314432000002, + "y": 3.011626076642273 + } + }, + "velocity": -1.073663727067631, + "acceleration": 3.0, + "curvature": -0.5417712602151914, + "holonomicRotation": 1.0229344394312734, + "angularVelocity": 0.57249153214415, + "holonomicAngularVelocity": -10.232787882509578 + }, + { + "time": 3.7907724735195667, + "pose": { + "rotation": { + "radians": 3.0316811922714564 + }, + "translation": { + "x": 4.808262144000002, + "y": 3.01030757246653 + } + }, + "velocity": -1.039534671117577, + "acceleration": 2.9999999999999805, + "curvature": -0.5432455108121239, + "holonomicRotation": 0.9065226427088713, + "angularVelocity": 0.5551915617904356, + "holonomicAngularVelocity": -9.306455136223637 + }, + { + "time": 3.8025330121884613, + "pose": { + "rotation": { + "radians": 3.0382105441025367 + }, + "translation": { + "x": 4.820216000000001, + "y": 3.009067335973687 + } + }, + "velocity": -1.0042530551108941, + "acceleration": 2.999999999999981, + "curvature": -0.5447246977401435, + "holonomicRotation": 0.7970737172089826, + "angularVelocity": 0.5371318836759915, + "holonomicAngularVelocity": -8.408137005072163 + }, + { + "time": 3.814719870104957, + "pose": { + "rotation": { + "radians": 3.044756494051316 + }, + "translation": { + "x": 4.8321756160000024, + "y": 3.0079055790600258 + } + }, + "velocity": -0.9676924813614058, + "acceleration": 3.0, + "curvature": -0.5462082286298287, + "holonomicRotation": 0.6946049461857351, + "angularVelocity": 0.5182301955923827, + "holonomicAngularVelocity": -7.539128616449812 + }, + { + "time": 3.827383460341566, + "pose": { + "rotation": { + "radians": 3.0513191488965354 + }, + "translation": { + "x": 4.844140608000002, + "y": 3.006822513621827 + } + }, + "velocity": -0.9297017106515791, + "acceleration": 3.000000000000026, + "curvature": -0.5476954972706566, + "holonomicRotation": 0.5991325106459233, + "angularVelocity": 0.4983877740925543, + "holonomicAngularVelocity": -6.700835815571002 + }, + { + "time": 3.8405849513625085, + "pose": { + "rotation": { + "radians": 3.057898610621166 + }, + "translation": { + "x": 4.856110592000002, + "y": 3.0058183515553702 + } + }, + "velocity": -0.8900972375887511, + "acceleration": 3.000000000000017, + "curvature": -0.549185883552854, + "holonomicRotation": 0.5106714867938522, + "angularVelocity": 0.4774843322945403, + "holonomicAngularVelocity": -5.8947980717647805 + }, + { + "time": 3.8543997830415164, + "pose": { + "rotation": { + "radians": 3.0644949763011784 + }, + "translation": { + "x": 4.868085184000002, + "y": 3.004893304756937 + } + }, + "velocity": -0.8486527425517276, + "acceleration": 2.999999999999992, + "curvature": -0.550678753507413, + "holonomicRotation": 0.42923584365068157, + "angularVelocity": 0.4553705897180785, + "holonomicAngularVelocity": -5.122718333648229 + }, + { + "time": 3.868922815142172, + "pose": { + "rotation": { + "radians": 3.0711083379933486 + }, + "translation": { + "x": 4.880064000000003, + "y": 3.0040475851228083 + } + }, + "velocity": -0.8050836462497605, + "acceleration": 2.9999999999999925, + "curvature": -0.5521734592537758, + "holonomicRotation": 0.3548384408484906, + "angularVelocity": 0.43185716592552986, + "holonomicAngularVelocity": -4.386502727126849 + }, + { + "time": 3.8842761429384574, + "pose": { + "rotation": { + "radians": 3.077738782622978 + }, + "translation": { + "x": 4.8920466560000015, + "y": 3.003281404549264 + } + }, + "velocity": -0.759023662860904, + "acceleration": 3.0000000000000218, + "curvature": -0.5536693390456592, + "holonomicRotation": 0.28749102659961223, + "angularVelocity": 0.4066972833724865, + "holonomicAngularVelocity": -3.688314566573384 + }, + { + "time": 3.900621492465988, + "pose": { + "rotation": { + "radians": 3.0843863918715986 + }, + "translation": { + "x": 4.904032768000002, + "y": 3.002594974932586 + } + }, + "velocity": -0.7099876142783116, + "acceleration": 3.0000000000000067, + "curvature": -0.555165717284197, + "holonomicRotation": 0.2272042358414872, + "angularVelocity": 0.3795584251366028, + "holonomicAngularVelocity": -3.030649789986334 + }, + { + "time": 3.9181809767257976, + "pose": { + "rotation": { + "radians": 3.091051242063463 + }, + "translation": { + "x": 4.916021952000001, + "y": 3.001988508169053 + } + }, + "velocity": -0.6573091614988833, + "acceleration": 2.999999999999994, + "curvature": -0.5566619045708889, + "holonomicRotation": 0.1739875885572273, + "angularVelocity": 0.34997279260682573, + "holonomicAngularVelocity": -2.416445429868007 + }, + { + "time": 3.9372743523570404, + "pose": { + "rotation": { + "radians": 3.09773340405342 + }, + "translation": { + "x": 4.928013824000002, + "y": 3.001462216154948 + } + }, + "velocity": -0.6000290346051556, + "acceleration": 3.0, + "curvature": -0.5581571976422951, + "holonomicRotation": 0.12784948827235798, + "angularVelocity": 0.3172429663973512, + "holonomicAngularVelocity": -1.8492402373960939 + }, + { + "time": 3.958392358960432, + "pose": { + "rotation": { + "radians": 3.1044329431126787 + }, + "translation": { + "x": 4.9400080000000015, + "y": 3.00101631078655 + } + }, + "velocity": -0.5366750147949809, + "acceleration": 2.9999999999999947, + "curvature": -0.5596508797624753, + "holonomicRotation": 0.08879722072776985, + "angularVelocity": 0.28023940774195016, + "holonomicAngularVelocity": -1.3334165508090225 + }, + { + "time": 3.982361063998006, + "pose": { + "rotation": { + "radians": 3.11114991881675 + }, + "translation": { + "x": 4.952004096000002, + "y": 3.00065100396014 + } + }, + "velocity": -0.4647688996822589, + "acceleration": 2.9999999999999907, + "curvature": -0.5611422204213573, + "holonomicRotation": 0.05683695272920897, + "angularVelocity": 0.2368783781248903, + "holonomicAngularVelocity": -0.8745399406582713 + }, + { + "time": 4.010791122363344, + "pose": { + "rotation": { + "radians": 3.117884384932327 + }, + "translation": { + "x": 4.964001728000001, + "y": 3.000366507572 + } + }, + "velocity": -0.379478724586247, + "acceleration": 3.0000000000000115, + "curvature": -0.5626304757071848, + "holonomicRotation": 0.031973731173475795, + "angularVelocity": 0.18224260470890688, + "holonomicAngularVelocity": -0.4794189000382439 + }, + { + "time": 4.047840660711865, + "pose": { + "rotation": { + "radians": 3.124636389304224 + }, + "translation": { + "x": 4.976000512000002, + "y": 3.0001630335184095 + } + }, + "velocity": -0.26833010954068137, + "acceleration": 3.0, + "curvature": -0.5641148883737164, + "holonomicRotation": 0.014211482251502705, + "angularVelocity": 0.07568570427505669, + "holonomicAngularVelocity": -0.1191644662033369 + }, + { + "time": 4.137284030558758, + "pose": { + "rotation": { + "radians": 3.1314059737438207 + }, + "translation": { + "x": 4.988000064000001, + "y": 3.000040793695649 + } + }, + "velocity": 0, + "acceleration": 3.0000000000000027, + "curvature": -0.5641148883737164, + "holonomicRotation": 0.0035530108282699757, + "angularVelocity": 0.0, + "holonomicAngularVelocity": 0.0 + } +] \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..bab0da9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..2012ffc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 6.5 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.0, + "y": 5.0 + }, + "prevControl": { + "x": 4.0, + "y": 6.0 + }, + "nextControl": { + "x": 6.0, + "y": 4.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.225280307703473, + "y": 6.012192024384049 + }, + "prevControl": { + "x": 6.975280307703473, + "y": 7.512192024384059 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/swerve/controllerproperties.json b/src/main/deploy/swerve/controllerproperties.json new file mode 100644 index 0000000..ab93140 --- /dev/null +++ b/src/main/deploy/swerve/controllerproperties.json @@ -0,0 +1,8 @@ +{ + "angleJoystickRadiusDeadband": 0.25, + "heading": { + "p": 0.4, + "i": 0, + "d": 0.01 + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json new file mode 100644 index 0000000..b0721ec --- /dev/null +++ b/src/main/deploy/swerve/modules/backleft.json @@ -0,0 +1,30 @@ +{ + "location": { + "front": -14.25, + "left": 14.25 + }, + "absoluteEncoderOffset": 55.020, + "drive": { + "type": "sparkmax", + "id": 7, + "canbus": null + }, + "angle": { + "type": "sparkmax", + "id": 8, + "canbus": null + }, + "conversionFactor": { + "angle": 0, + "drive": 0 + }, + "encoder": { + "type": "cancoder", + "id": 9, + "canbus": null + }, + "inverted": { + "drive": true, + "angle": true + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json new file mode 100644 index 0000000..2a1317d --- /dev/null +++ b/src/main/deploy/swerve/modules/backright.json @@ -0,0 +1,30 @@ +{ + "location": { + "front": -14.25, + "left": -14.25 + }, + "absoluteEncoderOffset": 298.740, + "drive": { + "type": "sparkmax", + "id": 10, + "canbus": null + }, + "angle": { + "type": "sparkmax", + "id": 11, + "canbus": null + }, + "conversionFactor": { + "angle": 0, + "drive": 0 + }, + "encoder": { + "type": "cancoder", + "id": 12, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": true + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json new file mode 100644 index 0000000..92521e3 --- /dev/null +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -0,0 +1,30 @@ +{ + "location": { + "front": 14.25, + "left": 14.25 + }, + "absoluteEncoderOffset": 185.977, + "drive": { + "type": "sparkmax", + "id": 1, + "canbus": null + }, + "angle": { + "type": "sparkmax", + "id": 2, + "canbus": null + }, + "conversionFactor": { + "angle": 0, + "drive": 0 + }, + "encoder": { + "type": "cancoder", + "id": 3, + "canbus": null + }, + "inverted": { + "drive": true, + "angle": true + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json new file mode 100644 index 0000000..ba03095 --- /dev/null +++ b/src/main/deploy/swerve/modules/frontright.json @@ -0,0 +1,30 @@ +{ + "location": { + "front": 14.25, + "left": -14.25 + }, + "absoluteEncoderOffset": 88.330, + "drive": { + "type": "sparkmax", + "id": 4, + "canbus": null + }, + "angle": { + "type": "sparkmax", + "id": 5, + "canbus": null + }, + "conversionFactor": { + "angle": 0, + "drive": 0 + }, + "encoder": { + "type": "cancoder", + "id": 6, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": true + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json new file mode 100644 index 0000000..57daa8b --- /dev/null +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -0,0 +1,16 @@ +{ + "optimalVoltage": 12, + "wheelGripCoefficientOfFriction": 1.19, + "currentLimit": { + "drive": 40, + "angle": 35 + }, + "conversionFactor": { + "angle": 16.8, + "drive": 0.047286787200699704 + }, + "rampRate": { + "drive": 0.25, + "angle": 0.25 + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/pidfproperties.json b/src/main/deploy/swerve/modules/pidfproperties.json new file mode 100644 index 0000000..99664bb --- /dev/null +++ b/src/main/deploy/swerve/modules/pidfproperties.json @@ -0,0 +1,16 @@ +{ + "drive": { + "p": 0.0020645, + "i": 0, + "d": 0, + "f": 0, + "iz": 0 + }, + "angle": { + "p": 0.01, + "i": 0, + "d": 0, + "f": 0, + "iz": 0 + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/swervedrive.json b/src/main/deploy/swerve/swervedrive.json new file mode 100644 index 0000000..d2b6dbd --- /dev/null +++ b/src/main/deploy/swerve/swervedrive.json @@ -0,0 +1,14 @@ +{ + "imu": { + "type": "navx_mxp", + "id": 0, + "canbus": null + }, + "invertedIMU": true, + "modules": [ + "frontleft.json", + "frontright.json", + "backleft.json", + "backright.json" + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8155deb..8061563 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -26,11 +26,9 @@ public final class Constants { public static final class Auton { - public static final PIDFConfig xAutoPID = new PIDFConfig(0.7, 0, 0); - public static final PIDFConfig yAutoPID = new PIDFConfig(0.7, 0, 0); + public static final PIDFConfig TranslationPID = new PIDFConfig(0.7, 0, 0); public static final PIDFConfig angleAutoPID = new PIDFConfig(0.4, 0, 0.01); - public static final double MAX_SPEED = 4; public static final double MAX_ACCELERATION = 2; } @@ -46,6 +44,6 @@ public static class OperatorConstants { public static final double LEFT_X_DEADBAND = 0.01; public static final double LEFT_Y_DEADBAND = 0.01; public static final double RIGHT_X_DEADBAND = 0.01; - public static final double TURN_CONSTANT = 0.75; + public static final double TURN_CONSTANT = 6; } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6fd9181..8a9977a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,7 +4,6 @@ package frc.robot; -import com.pathplanner.lib.server.PathPlannerServer; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.Timer; @@ -43,14 +42,11 @@ public static Robot getInstance() { */ @Override public void robotInit() { - PathPlannerServer.startServer(5811); - // Instantiate our RobotContainer. This will perform all our button bindings, - // and put our + // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - // Create a timer to disable motor brake a few seconds after disable. This will - // let the robot + // Create a timer to disable motor brake a few seconds after disable. This will let the robot // stop // immediately when disabled, but then also let it be pushed more disabledTimer = new Timer(); @@ -65,12 +61,9 @@ public void robotInit() { */ @Override public void robotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding - // newly-scheduled - // commands, running already-scheduled commands, removing finished or - // interrupted commands, - // and running subsystem periodic() methods. This must be called from the - // robot's periodic + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); } @@ -116,6 +109,7 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + m_robotContainer.setDriveMode(); m_robotContainer.setMotorBrake(true); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 47e1f2a..019232f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,12 +4,22 @@ package frc.robot; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.commands.swervedrive.drivebase.SwerveCommand; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants.OperatorConstants; +import frc.robot.commands.swervedrive.drivebase.AbsoluteDriveAdv; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import java.io.File; @@ -23,30 +33,89 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final SwerveSubsystem drivebase = - new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve")); - + new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve/neo")); + // CommandJoystick rotationController = new CommandJoystick(1); + // Replace with CommandPS4Controller or CommandJoystick if needed CommandJoystick driverController = new CommandJoystick(1); - CommandXboxController driverXbox = new CommandXboxController(0); + // CommandJoystick driverController = new + // CommandJoystick(3);//(OperatorConstants.DRIVER_CONTROLLER_PORT); + XboxController driverXbox = new XboxController(0); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - - // Configure Swerve (Command) - configureDrive(); - // Configure the trigger bindings configureBindings(); - } - private void configureDrive() { - SwerveCommand swerveCommand = new SwerveCommand(drivebase, driverXbox); - drivebase.setDefaultCommand(swerveCommand); + AbsoluteDriveAdv closedAbsoluteDriveAdv = + new AbsoluteDriveAdv( + drivebase, + () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), + () -> + MathUtil.applyDeadband(driverXbox.getRightX(), OperatorConstants.RIGHT_X_DEADBAND), + driverXbox::getYButtonPressed, + driverXbox::getAButtonPressed, + driverXbox::getXButtonPressed, + driverXbox::getBButtonPressed); + + // Applies deadbands and inverts controls because joysticks + // are back-right positive while robot + // controls are front-left positive + // left stick controls translation + // right stick controls the desired angle NOT angular rotation + Command driveFieldOrientedDirectAngle = + drivebase.driveCommand( + () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), + () -> driverXbox.getRightX(), + () -> driverXbox.getRightY()); + + // Applies deadbands and inverts controls because joysticks + // are back-right positive while robot + // controls are front-left positive + // left stick controls translation + // right stick controls the angular velocity of the robot + Command driveFieldOrientedAnglularVelocity = + drivebase.driveCommand( + () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), + () -> driverXbox.getRawAxis(2)); + + Command driveFieldOrientedDirectAngleSim = + drivebase.simDriveCommand( + () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), + () -> driverXbox.getRawAxis(2)); + + drivebase.setDefaultCommand( + !RobotBase.isSimulation() + ? driveFieldOrientedDirectAngle + : driveFieldOrientedDirectAngleSim); } + /** + * Use this method to define your trigger->command mappings. Triggers can be created via the + * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary + * predicate, or via the named factories in {@link + * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link + * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller + * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight + * joysticks}. + */ private void configureBindings() { // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - driverController.button(10).toggleOnTrue(new InstantCommand(() -> drivebase.zeroGyro())); + + new JoystickButton(driverXbox, 1).onTrue((new InstantCommand(drivebase::zeroGyro))); + new JoystickButton(driverXbox, 3).onTrue(new InstantCommand(drivebase::addFakeVisionReading)); + new JoystickButton(driverXbox, 2) + .whileTrue( + Commands.deferredProxy( + () -> + drivebase.driveToPose( + new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0))))); + // new JoystickButton(driverXbox, 3).whileTrue(new RepeatCommand(new + // InstantCommand(drivebase::lock, drivebase))); } /** @@ -56,10 +125,14 @@ private void configureBindings() { */ public Command getAutonomousCommand() { // An example command will be run in autonomous - return null; + return drivebase.getAutonomousCommand("New Path", true); + } + + public void setDriveMode() { + // drivebase.setDefaultCommand(); } - public void setMotorBrake(boolean shouldBrake) { - drivebase.setMotorBrake(shouldBrake); + public void setMotorBrake(boolean brake) { + drivebase.setMotorBrake(brake); } } diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java deleted file mode 100644 index 6c45474..0000000 --- a/src/main/java/frc/robot/commands/Autos.java +++ /dev/null @@ -1,20 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.subsystems.ExampleSubsystem; - -public final class Autos { - /** Example static factory for an autonomous command. */ - public static Command exampleAuto(ExampleSubsystem subsystem) { - return Commands.sequence(subsystem.exampleMethodCommand(), new ExampleCommand(subsystem)); - } - - private Autos() { - throw new UnsupportedOperationException("This is a utility class!"); - } -} diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/ExampleCommand.java deleted file mode 100644 index 318de13..0000000 --- a/src/main/java/frc/robot/commands/ExampleCommand.java +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.ExampleSubsystem; - -/** An example command that uses an example subsystem. */ -public class ExampleCommand extends Command { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final ExampleSubsystem m_subsystem; - - /** - * Creates a new ExampleCommand. - * - * @param subsystem The subsystem used by this command. - */ - public ExampleCommand(ExampleSubsystem subsystem) { - m_subsystem = subsystem; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(subsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java b/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java new file mode 100644 index 0000000..513bb63 --- /dev/null +++ b/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java @@ -0,0 +1,76 @@ +package frc.robot.commands.swervedrive.auto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; + +/** + * Auto Balance command using a simple PID controller. Created by Team 3512 ... + */ +public class AutoBalanceCommand extends Command { + + private final SwerveSubsystem swerveSubsystem; + private final PIDController controller; + + public AutoBalanceCommand(SwerveSubsystem swerveSubsystem) { + this.swerveSubsystem = swerveSubsystem; + controller = new PIDController(1.0, 0.0, 0.0); + controller.setTolerance(1); + controller.setSetpoint(0.0); + // each subsystem used by the command must be passed into the + // addRequirements() method (which takes a vararg of Subsystem) + addRequirements(this.swerveSubsystem); + } + + /** The initial subroutine of a command. Called once when the command is initially scheduled. */ + @Override + public void initialize() {} + + /** + * The main body of a command. Called repeatedly while the command is scheduled. (That is, it is + * called repeatedly until {@link #isFinished()}) returns true.) + */ + @Override + public void execute() { + SmartDashboard.putBoolean("At Tolerance", controller.atSetpoint()); + + double translationVal = + MathUtil.clamp( + controller.calculate(swerveSubsystem.getPitch().getDegrees(), 0.0), -0.5, 0.5); + swerveSubsystem.drive(new Translation2d(translationVal, 0.0), 0.0, true); + } + + /** + * Returns whether this command has finished. Once a command finishes -- indicated by this method + * returning true -- the scheduler will call its {@link #end(boolean)} method. + * + *

Returning false will result in the command never ending automatically. It may still be + * cancelled manually or interrupted by another command. Hard coding this command to always return + * true will result in the command executing once and finishing immediately. It is recommended to + * use * {@link edu.wpi.first.wpilibj2.command.InstantCommand InstantCommand} for such an + * operation. + * + * @return whether this command has finished. + */ + @Override + public boolean isFinished() { + return controller.atSetpoint(); + } + + /** + * The action to take when the command ends. Called when either the command finishes normally -- + * that is it is called when {@link #isFinished()} returns true -- or when it is + * interrupted/canceled. This is where you may want to wrap up loose ends, like shutting off a + * motor that was being used in the command. + * + * @param interrupted whether the command was interrupted/canceled + */ + @Override + public void end(boolean interrupted) { + swerveSubsystem.lock(); + } +} diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java index b5b16ed..2b8446e 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java @@ -7,9 +7,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import java.util.List; @@ -18,7 +17,7 @@ import swervelib.math.SwerveMath; /** An example command that uses an example subsystem. */ -public class AbsoluteDrive extends CommandBase { +public class AbsoluteDrive extends Command { private final SwerveSubsystem swerve; private final DoubleSupplier vX, vY; @@ -118,18 +117,4 @@ public void end(boolean interrupted) {} public boolean isFinished() { return false; } - - @Override - public void initSendable(SendableBuilder builder) { - builder.addDoubleProperty( - "Target Rotation Speed", - () -> - swerve.getTargetSpeeds( - vX.getAsDouble(), - vY.getAsDouble(), - headingHorizontal.getAsDouble(), - headingVertical.getAsDouble()) - .omegaRadiansPerSecond, - null); - } } diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java new file mode 100644 index 0000000..d1379a8 --- /dev/null +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java @@ -0,0 +1,152 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.swervedrive.drivebase; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import java.util.List; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import swervelib.SwerveController; +import swervelib.math.SwerveMath; + +/** A more advanced Swerve Control System that has 4 buttons for which direction to face */ +public class AbsoluteDriveAdv extends Command { + + private final SwerveSubsystem swerve; + private final DoubleSupplier vX, vY; + private final DoubleSupplier headingAdjust; + private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight; + private boolean resetHeading = false; + + /** + * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, + * where x is torwards/away from alliance wall and y is left/right. Heading Adjust changes the + * current heading after being multipied by a constant. The look booleans are shortcuts to get the + * robot to face a certian direction. Based off of ideas in + * https://www.chiefdelphi.com/t/experiments-with-a-swerve-steering-knob/446172 + * + * @param swerve The swerve drivebase subsystem. + * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range + * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. + * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range + * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when + * looking through the driver station glass. + * @param headingAdjust DoubleSupplier that supplies the component of the robot's heading angle + * that should be adjusted. Should range from -1 to 1 with deadband already accounted for. + * @param lookAway Face the robot towards the opposing alliance's wall in the same direction the + * driver is facing + * @param lookTowards Face the robot towards the driver + * @param lookLeft Face the robot left + * @param lookRight Face the robot right + */ + public AbsoluteDriveAdv( + SwerveSubsystem swerve, + DoubleSupplier vX, + DoubleSupplier vY, + DoubleSupplier headingAdjust, + BooleanSupplier lookAway, + BooleanSupplier lookTowards, + BooleanSupplier lookLeft, + BooleanSupplier lookRight) { + this.swerve = swerve; + this.vX = vX; + this.vY = vY; + this.headingAdjust = headingAdjust; + this.lookAway = lookAway; + this.lookTowards = lookTowards; + this.lookLeft = lookLeft; + this.lookRight = lookRight; + + addRequirements(swerve); + } + + @Override + public void initialize() { + resetHeading = true; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double headingX = 0; + double headingY = 0; + + // These are written to allow combinations for 45 angles + // Face Away from Drivers + if (lookAway.getAsBoolean()) { + headingY = -1; + } + // Face Right + if (lookRight.getAsBoolean()) { + headingX = 1; + } + // Face Left + if (lookLeft.getAsBoolean()) { + headingX = -1; + } + // Face Towards the Drivers + if (lookTowards.getAsBoolean()) { + headingY = 1; + } + + // Prevent Movement After Auto + if (resetHeading) { + if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) { + // Get the curret Heading + Rotation2d currentHeading = swerve.getHeading(); + + // Set the Current Heading to the desired Heading + headingX = currentHeading.getSin(); + headingY = currentHeading.getCos(); + } + // Dont reset Heading Again + resetHeading = false; + } + + ChassisSpeeds desiredSpeeds = + swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), headingX, headingY); + + // Limit velocity to prevent tippy + Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); + translation = + SwerveMath.limitVelocity( + translation, + swerve.getFieldVelocity(), + swerve.getPose(), + Constants.LOOP_TIME, + Constants.ROBOT_MASS, + List.of(Constants.CHASSIS), + swerve.getSwerveDriveConfiguration()); + SmartDashboard.putNumber("LimitedTranslation", translation.getX()); + SmartDashboard.putString("Translation", translation.toString()); + + // Make the robot move + if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) { + resetHeading = true; + swerve.drive( + translation, + (Constants.OperatorConstants.TURN_CONSTANT * -headingAdjust.getAsDouble()), + true); + } else { + swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java new file mode 100644 index 0000000..1e0945d --- /dev/null +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java @@ -0,0 +1,89 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.swervedrive.drivebase; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import java.util.List; +import java.util.function.DoubleSupplier; +import swervelib.SwerveController; +import swervelib.math.SwerveMath; + +/** An example command that uses an example subsystem. */ +public class AbsoluteFieldDrive extends Command { + + private final SwerveSubsystem swerve; + private final DoubleSupplier vX, vY, heading; + + /** + * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, + * where x is torwards/away from alliance wall and y is left/right. headingHorzontal and + * headingVertical are the Cartesian coordinates from which the robot's angle will be derived— + * they will be converted to a polar angle, which the robot will rotate to. + * + * @param swerve The swerve drivebase subsystem. + * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range + * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. + * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range + * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when + * looking through the driver station glass. + * @param heading DoubleSupplier that supplies the robot's heading angle. + */ + public AbsoluteFieldDrive( + SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier heading) { + this.swerve = swerve; + this.vX = vX; + this.vY = vY; + this.heading = heading; + + addRequirements(swerve); + } + + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + // Get the desired chassis speeds based on a 2 joystick module. + + ChassisSpeeds desiredSpeeds = + swerve.getTargetSpeeds( + vX.getAsDouble(), vY.getAsDouble(), new Rotation2d(heading.getAsDouble() * Math.PI)); + + // Limit velocity to prevent tippy + Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); + translation = + SwerveMath.limitVelocity( + translation, + swerve.getFieldVelocity(), + swerve.getPose(), + Constants.LOOP_TIME, + Constants.ROBOT_MASS, + List.of(Constants.CHASSIS), + swerve.getSwerveDriveConfiguration()); + SmartDashboard.putNumber("LimitedTranslation", translation.getX()); + SmartDashboard.putString("Translation", translation.toString()); + + // Make the robot move + swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/SwerveCommand.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/SwerveCommand.java deleted file mode 100644 index 3aaeec0..0000000 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/SwerveCommand.java +++ /dev/null @@ -1,68 +0,0 @@ -package frc.robot.commands.swervedrive.drivebase; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.subsystems.swervedrive.SwerveSubsystem; - -/** The only goal of this command is to move either robot or angle centric */ -public class SwerveCommand extends CommandBase { - - private SwerveSubsystem swerve; - private CommandXboxController xbox; - - private Rotation2d targetAngle = new Rotation2d(); - - private DriveMode driveMode = DriveMode.AngleCentric; - - private final double MAX_FWD_SENS = 4; - private final double MAX_STR_SENS = 4; - private final double MAX_ROT_SENS = 2; - private final double MIN_SLOW_SENS = 0.2; // For slow mode - - public SwerveCommand(SwerveSubsystem swerve, CommandXboxController xbox) { - - this.swerve = swerve; - this.xbox = xbox; - - targetAngle = swerve.getHeading(); - - addRequirements(swerve); - } - - @Override - public void initialize() {} - - @Override - public void execute() { - - double envelopeModifier = Math.max(1 - xbox.getLeftTriggerAxis(), MIN_SLOW_SENS); - double forwardVelocity = -xbox.getLeftX() * MAX_FWD_SENS * envelopeModifier; - double strafeVelocity = -xbox.getLeftY() * MAX_STR_SENS * envelopeModifier; - double rotationVelocity = -xbox.getRightX() * MAX_ROT_SENS * envelopeModifier; - - switch (driveMode) { - case RobotCentric: - swerve.driveRobotCentric( - new ChassisSpeeds(forwardVelocity, strafeVelocity, rotationVelocity)); - break; - - case AngleCentric: - if (Math.abs(xbox.getRightY()) > 0.5) { - targetAngle = Rotation2d.fromDegrees(90 + 90 * Math.signum(-xbox.getRightY())); - } - targetAngle = Rotation2d.fromDegrees(targetAngle.getDegrees() + rotationVelocity); - swerve.driveAngleCentric(forwardVelocity, strafeVelocity, targetAngle); - break; - - default: - break; - } - } - - enum DriveMode { - RobotCentric, - AngleCentric - } -} diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java deleted file mode 100644 index fcdcb39..0000000 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java +++ /dev/null @@ -1,76 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands.swervedrive.drivebase; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.swervedrive.SwerveSubsystem; -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; -import swervelib.SwerveController; - -/** An example command that uses an example subsystem. */ -public class TeleopDrive extends CommandBase { - - private final SwerveSubsystem swerve; - private final DoubleSupplier vX; - private final DoubleSupplier vY; - private final DoubleSupplier omega; - private final BooleanSupplier driveMode; - private final SwerveController controller; - - /** - * Creates a new ExampleCommand. - * - * @param swerve The subsystem used by this command. - */ - public TeleopDrive( - SwerveSubsystem swerve, - DoubleSupplier vX, - DoubleSupplier vY, - DoubleSupplier omega, - BooleanSupplier driveMode) { - this.swerve = swerve; - this.vX = vX; - this.vY = vY; - this.omega = omega; - this.driveMode = driveMode; - this.controller = swerve.getSwerveController(); - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(swerve); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - double xVelocity = Math.pow(vX.getAsDouble(), 3); - double yVelocity = Math.pow(vY.getAsDouble(), 3); - double angVelocity = Math.pow(omega.getAsDouble(), 3); - SmartDashboard.putNumber("vX", xVelocity); - SmartDashboard.putNumber("vY", yVelocity); - SmartDashboard.putNumber("omega", angVelocity); - - // Drive using raw values. - swerve.drive( - new Translation2d(xVelocity * swerve.maximumSpeed, yVelocity * swerve.maximumSpeed), - angVelocity * controller.config.maxAngularVelocity, - driveMode.getAsBoolean()); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java deleted file mode 100644 index 6b375da..0000000 --- a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class ExampleSubsystem extends SubsystemBase { - /** Creates a new ExampleSubsystem. */ - public ExampleSubsystem() {} - - /** - * Example command factory method. - * - * @return a command - */ - public Command exampleMethodCommand() { - // Inline construction of command goes here. - // Subsystem::RunOnce implicitly requires `this` subsystem. - return runOnce( - () -> { - /* one-time action goes here */ - }); - } - - /** - * An example method querying a boolean state of the subsystem (for example, a digital sensor). - * - * @return value of some boolean subsystem state, such as a digital sensor. - */ - public boolean exampleCondition() { - // Query some boolean state, such as a digital sensor. - return false; - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } - - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation - } -} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 487fb4f..8c24cf0 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -4,30 +4,27 @@ package frc.robot.subsystems.swervedrive; -import com.pathplanner.lib.PathConstraints; -import com.pathplanner.lib.PathPlanner; -import com.pathplanner.lib.PathPlannerTrajectory; -import com.pathplanner.lib.auto.PIDConstants; -import com.pathplanner.lib.auto.SwerveAutoBuilder; -import edu.wpi.first.math.controller.PIDController; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.io.File; -import java.util.List; -import java.util.Map; +import java.util.function.DoubleSupplier; import swervelib.SwerveController; import swervelib.SwerveDrive; -import swervelib.SwerveModule; import swervelib.math.SwerveMath; import swervelib.parser.SwerveControllerConfiguration; import swervelib.parser.SwerveDriveConfiguration; @@ -40,18 +37,9 @@ public class SwerveSubsystem extends SubsystemBase { /** Swerve drive object. */ private final SwerveDrive swerveDrive; - private final SwerveModule[] modules; - private PIDController anglePID; - /** Maximum speed of the robot in meters per second, used to limit acceleration. */ public double maximumSpeed = Units.feetToMeters(14.5); - /** - * The auto builder for PathPlanner, there can only ever be one created so we save it just incase - * we generate multiple paths with events. - */ - private SwerveAutoBuilder autoBuilder = null; - /** * Initialize {@link SwerveDrive} with the directory provided. * @@ -59,46 +47,37 @@ public class SwerveSubsystem extends SubsystemBase { */ public SwerveSubsystem(File directory) { // Angle conversion factor is 360 / (GEAR RATIO * ENCODER RESOLUTION) - // In this case the gear ratio is 12.8 motor revolutions per wheel rotation. - // The encoder resolution per motor revolution is 1 per motor revolution. - double angleConversionFactor = SwerveMath.calculateDegreesPerSteeringRotation(12.8, 1); - // Motor conversion factor is (PI * WHEEL DIAMETER IN METERS) / (GEAR RATIO * - // ENCODER + // In this case the gear ratio is 12.8 motor revolutions per wheel rotation. + // The encoder resolution per motor revolution is 1 per motor revolution. + double angleConversionFactor = SwerveMath.calculateDegreesPerSteeringRotation(12.8); + // Motor conversion factor is (PI * WHEEL DIAMETER IN METERS) / (GEAR RATIO * ENCODER // RESOLUTION). - // In this case the wheel diameter is 4 inches, which must be converted to - // meters to get + // In this case the wheel diameter is 4 inches, which must be converted to meters to get // meters/second. - // The gear ratio is 6.75 motor revolutions per wheel rotation. - // The encoder resolution per motor revolution is 1 per motor revolution. + // The gear ratio is 6.75 motor revolutions per wheel rotation. + // The encoder resolution per motor revolution is 1 per motor revolution. double driveConversionFactor = - SwerveMath.calculateMetersPerRotation(Units.inchesToMeters(4), 6.75, 1); + SwerveMath.calculateMetersPerRotation(Units.inchesToMeters(4), 6.75); System.out.println("\"conversionFactor\": {"); System.out.println("\t\"angle\": " + angleConversionFactor + ","); System.out.println("\t\"drive\": " + driveConversionFactor); System.out.println("}"); - // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary - // objects being + // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary objects being // created. SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; try { swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed); - // Alternative method if you don't want to supply the conversion factor via JSON - // files. + // Alternative method if you don't want to supply the conversion factor via JSON files. // swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed, // angleConversionFactor, driveConversionFactor); } catch (Exception e) { throw new RuntimeException(e); } - swerveDrive.setHeadingCorrection(false); - // Heading correction should only be used while controlling the robot via angle. + swerveDrive.setHeadingCorrection( + false); // Heading correction should only be used while controlling the robot via angle. - modules = swerveDrive.getModules(); - - anglePID = new PIDController(4, 0, 0); - anglePID.enableContinuousInput(-Math.PI, Math.PI); - anglePID.setTolerance(Math.PI / 32, Math.PI / 32); - anglePID.setSetpoint(0); + setupPathPlanner(); } /** @@ -110,60 +89,167 @@ public SwerveSubsystem(File directory) { public SwerveSubsystem( SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) { swerveDrive = new SwerveDrive(driveCfg, controllerCfg, maximumSpeed); - modules = swerveDrive.getModules(); } - /* START MAIN DRIVE METHODS */ + /** Setup AutoBuilder for PathPlanner. */ + public void setupPathPlanner() { + AutoBuilder.configureHolonomic( + this::getPose, // Robot pose supplier + this::resetOdometry, // Method to reset odometry (will be called if your auto has a starting + // pose) + this::getRobotVelocity, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + this::setChassisSpeeds, // Method that will drive the robot given ROBOT RELATIVE + // ChassisSpeeds + new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in + // your Constants class + new PIDConstants(5.0, 0.0, 0.0), + // Translation PID constants + new PIDConstants( + swerveDrive.swerveController.config.headingPIDF.p, + swerveDrive.swerveController.config.headingPIDF.i, + swerveDrive.swerveController.config.headingPIDF.d), + // Rotation PID constants + 4.5, + // Max module speed, in m/s + swerveDrive.swerveDriveConfiguration.getDriveBaseRadiusMeters(), + // Drive base radius in meters. Distance from robot center to furthest module. + new ReplanningConfig() + // Default path replanning config. See the API for the options here + ), + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + var alliance = DriverStation.getAlliance(); + return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; + }, + this // Reference to this subsystem to set requirements + ); + } - public void driveRobotCentric(ChassisSpeeds targetChassisSpeeds) { - SwerveModuleState[] states = - swerveDrive.kinematics.toSwerveModuleStates(discretize(targetChassisSpeeds)); - SwerveDriveKinematics.desaturateWheelSpeeds(states, maximumSpeed); - for (int i = 0; i < modules.length; i++) { - modules[i].setDesiredState(states[i], false, false); + /** + * Get the path follower with events. + * + * @param pathName PathPlanner path name. + * @param setOdomToStart Set the odometry position to the start of the path. + * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command. + */ + public Command getAutonomousCommand(String pathName, boolean setOdomToStart) { + // Load the path you want to follow using its name in the GUI + PathPlannerPath path = PathPlannerPath.fromPathFile(pathName); + + if (setOdomToStart) { + resetOdometry(new Pose2d(path.getPoint(0).position, getHeading())); } + + // Create a path following command using AutoBuilder. This will also trigger event markers. + return AutoBuilder.followPath(path); } - public void driveAngleCentric( - double forwardVelocity, double sidewaysVelocity, Rotation2d targetRotation) { - driveRobotCentric( - ChassisSpeeds.fromFieldRelativeSpeeds( - forwardVelocity, - sidewaysVelocity, - calculateRotationalVelocityToTarget(targetRotation), - getHeading())); + /** + * Use PathPlanner Path finding to go to a point on the field. + * + * @param pose Target {@link Pose2d} to go to. + * @return PathFinding command + */ + public Command driveToPose(Pose2d pose) { + // Create the constraints to use while pathfinding + PathConstraints constraints = + new PathConstraints( + swerveDrive.getMaximumVelocity(), + 4.0, + swerveDrive.getMaximumAngularVelocity(), + Units.degreesToRadians(720)); + + // Since AutoBuilder is configured, we can use it to build pathfinding commands + return AutoBuilder.pathfindToPose( + pose, + constraints, + 0.0, // Goal end velocity in meters/sec + 0.0 // Rotation delay distance in meters. This is how far the robot should travel before + // attempting to rotate. + ); } - private double calculateRotationalVelocityToTarget(Rotation2d targetRotation) { - double rotationalVelocity = - anglePID.calculate(getHeading().getRadians(), targetRotation.getRadians()); - if (anglePID.atSetpoint()) { - rotationalVelocity = 0; - } - return rotationalVelocity; + /** + * Command to drive the robot using translative values and heading as a setpoint. + * + * @param translationX Translation in the X direction. Cubed for smoother controls. + * @param translationY Translation in the Y direction. Cubed for smoother controls. + * @param headingX Heading X to calculate angle of the joystick. + * @param headingY Heading Y to calculate angle of the joystick. + * @return Drive command. + */ + public Command driveCommand( + DoubleSupplier translationX, + DoubleSupplier translationY, + DoubleSupplier headingX, + DoubleSupplier headingY) { + // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for + // this kind of control. + return run( + () -> { + double xInput = Math.pow(translationX.getAsDouble(), 3); // Smooth controll out + double yInput = Math.pow(translationY.getAsDouble(), 3); // Smooth controll out + // Make the robot move + driveFieldOriented( + swerveDrive.swerveController.getTargetSpeeds( + xInput, + yInput, + headingX.getAsDouble(), + headingY.getAsDouble(), + swerveDrive.getYaw().getRadians(), + swerveDrive.getMaximumVelocity())); + }); } /** - * Fixes situation where robot drifts in the direction it's rotating in if turning and translating - * at the same time + * Command to drive the robot using translative values and heading as a setpoint. * - * @see Chief - * Delphi + * @param translationX Translation in the X direction. + * @param translationY Translation in the Y direction. + * @param rotation Rotation as a value between [-1, 1] converted to radians. + * @return Drive command. */ - private ChassisSpeeds discretize(ChassisSpeeds originalChassisSpeeds) { - double vx = originalChassisSpeeds.vxMetersPerSecond; - double vy = originalChassisSpeeds.vyMetersPerSecond; - double omega = originalChassisSpeeds.omegaRadiansPerSecond; - double dt = - 0.02; // This should be the time these values will be used, so normally just the loop - // time - Pose2d desiredDeltaPose = new Pose2d(vx * dt, vy * dt, new Rotation2d(omega * dt)); - Twist2d twist = new Pose2d().log(desiredDeltaPose); - return new ChassisSpeeds(twist.dx / dt, twist.dy / dt, twist.dtheta / dt); + public Command simDriveCommand( + DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier rotation) { + // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for + // this kind of control. + return run( + () -> { + // Make the robot move + driveFieldOriented( + swerveDrive.swerveController.getTargetSpeeds( + translationX.getAsDouble(), + translationY.getAsDouble(), + rotation.getAsDouble() * Math.PI, + swerveDrive.getYaw().getRadians(), + swerveDrive.getMaximumVelocity())); + }); } - /* END MAIN DRIVE METHODS */ + /** + * Command to drive the robot using translative values and heading as angular velocity. + * + * @param translationX Translation in the X direction. Cubed for smoother controls. + * @param translationY Translation in the Y direction. Cubed for smoother controls. + * @param angularRotationX Angular velocity of the robot to set. Cubed for smoother controls. + * @return Drive command. + */ + public Command driveCommand( + DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX) { + return run( + () -> { + // Make the robot move + swerveDrive.drive( + new Translation2d( + Math.pow(translationX.getAsDouble(), 3) * swerveDrive.getMaximumVelocity(), + Math.pow(translationY.getAsDouble(), 3) * swerveDrive.getMaximumVelocity()), + Math.pow(angularRotationX.getAsDouble(), 3) * swerveDrive.getMaximumAngularVelocity(), + true, + false); + }); + } /** * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation @@ -276,12 +362,14 @@ public void setMotorBrake(boolean brake) { } /** - * Gets the current yaw angle of the robot, as reported by the imu. CCW positive, not wrapped. + * Gets the current yaw angle of the robot, as reported by the swerve pose estimator in the + * underlying drivebase. Note, this is not the raw gyro reading, this may be corrected from calls + * to resetOdometry(). * * @return The yaw angle */ public Rotation2d getHeading() { - return swerveDrive.getYaw(); + return getPose().getRotation(); } /** @@ -303,7 +391,8 @@ public ChassisSpeeds getTargetSpeeds( } /** - * Get the chassis speeds based on controller input of 1 joystick and one angle. + * Get the chassis speeds based on controller input of 1 joystick and one angle. Control the robot + * at an offset of 90deg. * * @param xInput X joystick input for the robot to move in the X direction. * @param yInput Y joystick input for the robot to move in the Y direction. @@ -372,56 +461,4 @@ public void addFakeVisionReading() { swerveDrive.addVisionMeasurement( new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp()); } - - /** - * Factory to fetch the PathPlanner command to follow the defined path. - * - * @param path Path planner path to specify. - * @param constraints {@link PathConstraints} for {@link - * com.pathplanner.lib.PathPlanner#loadPathGroup} function limiting velocity and acceleration. - * @param eventMap {@link java.util.HashMap} of commands corresponding to path planner events - * given as strings. - * @param translation The {@link PIDConstants} for the translation of the robot while following - * the path. - * @param rotation The {@link PIDConstants} for the rotation of the robot while following the - * path. - * @param useAllianceColor Automatically transform the path based on alliance color. - * @return PathPlanner command to follow the given path. - */ - public Command creatPathPlannerCommand( - String path, - PathConstraints constraints, - Map eventMap, - PIDConstants translation, - PIDConstants rotation, - boolean useAllianceColor) { - List pathGroup = PathPlanner.loadPathGroup(path, constraints); - // SwerveAutoBuilder autoBuilder = new SwerveAutoBuilder( - // Pose2d supplier, - // Pose2d consumer- used to reset odometry at the beginning of auto, - // PID constants to correct for translation error (used to create the X and Y - // PID - // controllers), - // PID constants to correct for rotation error (used to create the rotation - // controller), - // Module states consumer used to output to the drive subsystem, - // Should the path be automatically mirrored depending on alliance color. - // Optional- - // defaults to true - // ) - if (autoBuilder == null) { - autoBuilder = - new SwerveAutoBuilder( - swerveDrive::getPose, - swerveDrive::resetOdometry, - translation, - rotation, - swerveDrive::setChassisSpeeds, - eventMap, - useAllianceColor, - this); - } - - return autoBuilder.fullAuto(pathGroup); - } } diff --git a/src/main/java/swervelib/SwerveController.java b/src/main/java/swervelib/SwerveController.java deleted file mode 100644 index 8c0c124..0000000 --- a/src/main/java/swervelib/SwerveController.java +++ /dev/null @@ -1,228 +0,0 @@ -package swervelib; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import swervelib.parser.SwerveControllerConfiguration; - -/** Controller class used to convert raw inputs into robot speeds. */ -public class SwerveController { - - /** - * {@link SwerveControllerConfiguration} object storing data to generate the {@link PIDController} - * for controlling the robot heading, and deadband for heading joystick. - */ - public final SwerveControllerConfiguration config; - - /** PID Controller for the robot heading. */ - public final PIDController thetaController; // TODO: Switch to ProfilePIDController - - /** Last angle as a scalar [-1,1] the robot was set to. */ - public double lastAngleScalar; - - /** {@link SlewRateLimiter} for movement in the X direction in meters/second. */ - public SlewRateLimiter xLimiter = null; - - /** {@link SlewRateLimiter} for movement in the Y direction in meters/second. */ - public SlewRateLimiter yLimiter = null; - - /** {@link SlewRateLimiter} for angular movement in radians/second. */ - public SlewRateLimiter angleLimiter = null; - - /** - * Construct the SwerveController object which is used for determining the speeds of the robot - * based on controller input. - * - * @param cfg {@link SwerveControllerConfiguration} containing the PIDF variables for the heading - * PIDF. - */ - public SwerveController(SwerveControllerConfiguration cfg) { - config = cfg; - thetaController = config.headingPIDF.createPIDController(); - thetaController.enableContinuousInput(-Math.PI, Math.PI); - lastAngleScalar = 0; - } - - /** - * Helper function to get the {@link Translation2d} of the chassis speeds given the {@link - * ChassisSpeeds}. - * - * @param speeds Chassis speeds. - * @return {@link Translation2d} of the speed the robot is going in. - */ - public static Translation2d getTranslation2d(ChassisSpeeds speeds) { - return new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); - } - - /** - * Add slew rate limiters to all controls. This prevents the robot from ramping up too much. To - * disable a {@link SlewRateLimiter} set the desired one to null. - * - * @param x The {@link SlewRateLimiter} for the X velocity in meters/second. - * @param y The {@link SlewRateLimiter} for the Y velocity in meters/second. - * @param angle The {@link SlewRateLimiter} for the angular velocity in radians/second. - */ - public void addSlewRateLimiters(SlewRateLimiter x, SlewRateLimiter y, SlewRateLimiter angle) { - xLimiter = x; - yLimiter = y; - angleLimiter = angle; - } - - /** - * Calculate the hypot deadband and check if the joystick is within it. - * - * @param x The x value for the joystick in which the deadband should be applied. - * @param y The y value for the joystick in which the deadband should be applied. - * @return Whether the values are within the deadband from {@link - * SwerveControllerConfiguration#angleJoyStickRadiusDeadband}. - */ - public boolean withinHypotDeadband(double x, double y) { - return Math.hypot(x, y) < config.angleJoyStickRadiusDeadband; - } - - /** - * Get the chassis speeds based on controller input of 1 joystick [-1,1] and an angle. - * - * @param xInput X joystick input for the robot to move in the X direction. X = xInput * maxSpeed - * @param yInput Y joystick input for the robot to move in the Y direction. Y = yInput * maxSpeed; - * @param angle The desired angle of the robot in radians. - * @param currentHeadingAngleRadians The current robot heading in radians. - * @param maxSpeed Maximum speed in meters per second. - * @return {@link ChassisSpeeds} which can be sent to th Swerve Drive. - */ - public ChassisSpeeds getTargetSpeeds( - double xInput, - double yInput, - double angle, - double currentHeadingAngleRadians, - double maxSpeed) { - // Convert joystick inputs to m/s by scaling by max linear speed. Also uses a cubic function - // to allow for precise control and fast movement. - double x = xInput * maxSpeed; - double y = yInput * maxSpeed; - - return getRawTargetSpeeds(x, y, angle, currentHeadingAngleRadians); - } - - /** - * Get the angle in radians based off of the heading joysticks. - * - * @param headingX X joystick which controls the angle of the robot. - * @param headingY Y joystick which controls the angle of the robot. - * @return angle in radians from the joystick. - */ - public double getJoystickAngle(double headingX, double headingY) { - lastAngleScalar = - withinHypotDeadband(headingX, headingY) ? lastAngleScalar : Math.atan2(headingX, headingY); - return lastAngleScalar; - } - - /** - * Get the chassis speeds based on controller input of 2 joysticks. One for speeds in which - * direction. The other for the angle of the robot. - * - * @param xInput X joystick input for the robot to move in the X direction. - * @param yInput Y joystick input for the robot to move in the Y direction. - * @param headingX X joystick which controls the angle of the robot. - * @param headingY Y joystick which controls the angle of the robot. - * @param currentHeadingAngleRadians The current robot heading in radians. - * @param maxSpeed Maximum speed of the drive motors in meters per second, multiplier of the - * xInput and yInput. - * @return {@link ChassisSpeeds} which can be sent to th Swerve Drive. - */ - public ChassisSpeeds getTargetSpeeds( - double xInput, - double yInput, - double headingX, - double headingY, - double currentHeadingAngleRadians, - double maxSpeed) { - // Converts the horizontal and vertical components to the commanded angle, in radians, unless - // the joystick is near - // the center (i. e. has been released), in which case the angle is held at the last valid - // joystick input (hold - // position when stick released). - double angle = - withinHypotDeadband(headingX, headingY) ? lastAngleScalar : Math.atan2(headingX, headingY); - ChassisSpeeds speeds = - getTargetSpeeds(xInput, yInput, angle, currentHeadingAngleRadians, maxSpeed); - - // Used for the position hold feature - lastAngleScalar = angle; - - return speeds; - } - - /** - * Get the {@link ChassisSpeeds} based of raw speeds desired in meters/second and heading in - * radians. - * - * @param xSpeed X speed in meters per second. - * @param ySpeed Y speed in meters per second. - * @param omega Angular velocity in radians/second. - * @return {@link ChassisSpeeds} the robot should move to. - */ - public ChassisSpeeds getRawTargetSpeeds(double xSpeed, double ySpeed, double omega) { - if (xLimiter != null) { - xSpeed = xLimiter.calculate(xSpeed); - } - if (yLimiter != null) { - ySpeed = yLimiter.calculate(ySpeed); - } - if (angleLimiter != null) { - omega = angleLimiter.calculate(omega); - } - - return new ChassisSpeeds(xSpeed, ySpeed, omega); - } - - /** - * Get the {@link ChassisSpeeds} based of raw speeds desired in meters/second and heading in - * radians. - * - * @param xSpeed X speed in meters per second. - * @param ySpeed Y speed in meters per second. - * @param targetHeadingAngleRadians Target heading in radians. - * @param currentHeadingAngleRadians Current heading in radians. - * @return {@link ChassisSpeeds} the robot should move to. - */ - public ChassisSpeeds getRawTargetSpeeds( - double xSpeed, - double ySpeed, - double targetHeadingAngleRadians, - double currentHeadingAngleRadians) { - // Calculates an angular rate using a PIDController and the commanded angle. Returns a value - // between -1 and 1 - // which is then scaled to be between -maxAngularVelocity and +maxAngularVelocity. - return getRawTargetSpeeds( - xSpeed, - ySpeed, - thetaController.calculate(currentHeadingAngleRadians, targetHeadingAngleRadians) - * config.maxAngularVelocity); - } - - /** - * Calculate the angular velocity given the current and target heading angle in radians. - * - * @param currentHeadingAngleRadians The current heading of the robot in radians. - * @param targetHeadingAngleRadians The target heading of the robot in radians. - * @return Angular velocity in radians per second. - */ - public double headingCalculate( - double currentHeadingAngleRadians, double targetHeadingAngleRadians) { - return thetaController.calculate(currentHeadingAngleRadians, targetHeadingAngleRadians) - * config.maxAngularVelocity; - } - - /** - * Set a new maximum angular velocity that is different from the auto-generated one. Modified the - * {@link SwerveControllerConfiguration#maxAngularVelocity} field which is used in the {@link - * SwerveController} class for {@link ChassisSpeeds} generation. - * - * @param angularVelocity Angular velocity in radians per second. - */ - public void setMaximumAngularVelocity(double angularVelocity) { - config.maxAngularVelocity = angularVelocity; - } -} diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java deleted file mode 100644 index 24445e1..0000000 --- a/src/main/java/swervelib/SwerveDrive.java +++ /dev/null @@ -1,1044 +0,0 @@ -package swervelib; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; -import java.util.concurrent.locks.Lock; -import java.util.concurrent.locks.ReentrantLock; -import swervelib.encoders.CANCoderSwerve; -import swervelib.imu.Pigeon2Swerve; -import swervelib.imu.SwerveIMU; -import swervelib.math.SwerveMath; -import swervelib.motors.TalonFXSwerve; -import swervelib.parser.SwerveControllerConfiguration; -import swervelib.parser.SwerveDriveConfiguration; -import swervelib.simulation.SwerveIMUSimulation; -import swervelib.telemetry.Alert; -import swervelib.telemetry.Alert.AlertType; -import swervelib.telemetry.SwerveDriveTelemetry; -import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; - -/** Swerve Drive class representing and controlling the swerve drive. */ -public class SwerveDrive { - - /** Swerve Kinematics object. */ - public final SwerveDriveKinematics kinematics; - - /** Swerve drive configuration. */ - public final SwerveDriveConfiguration swerveDriveConfiguration; - - /** Swerve odometry. */ - public final SwerveDrivePoseEstimator swerveDrivePoseEstimator; - - /** Swerve modules. */ - private final SwerveModule[] swerveModules; - - /** WPILib {@link Notifier} to keep odometry up to date. */ - private final Notifier odometryThread; - - /** Odometry lock to ensure thread safety. */ - private final Lock odometryLock = new ReentrantLock(); - - /** Deadband for speeds in heading correction. */ - private double HEADING_CORRECTION_DEADBAND = 0.01; - - /** Field object. */ - public Field2d field = new Field2d(); - - /** Swerve controller for controlling heading of the robot. */ - public SwerveController swerveController; - - /** - * Standard deviation of encoders and gyroscopes, usually should not change. (meters of position - * and degrees of rotation) - */ - public Matrix stateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); - - /** - * The standard deviation of the vision measurement, for best accuracy calculate the standard - * deviation at 2 or more points and fit a line to it and modify this using {@link - * SwerveDrive#addVisionMeasurement(Pose2d, double, Matrix)} with the calculated optimal standard - * deviation. (Units should be meters per pixel). By optimizing this you can get vision accurate - * to inches instead of feet. - */ - public Matrix visionMeasurementStdDevs = VecBuilder.fill(0.9, 0.9, 0.9); - - /** Invert odometry readings of drive motor positions, used as a patch for debugging currently. */ - public boolean invertOdometry = false; - - /** - * Correct chassis velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} - * using 254's correction. - */ - public boolean chassisVelocityCorrection = true; - - /** Whether to correct heading when driving translationally. Set to true to enable. */ - public boolean headingCorrection = false; - - /** Whether heading correction PID is currently active. */ - private boolean correctionEnabled = false; - - /** Swerve IMU device for sensing the heading of the robot. */ - private SwerveIMU imu; - - /** Simulation of the swerve drive. */ - private SwerveIMUSimulation simIMU; - - /** Counter to synchronize the modules relative encoder with absolute encoder when not moving. */ - private int moduleSynchronizationCounter = 0; - - /** The last heading set in radians. */ - private double lastHeadingRadians = 0; - - /** The absolute max speed that your robot can reach while translating in meters per second. */ - private double attainableMaxTranslationalSpeedMetersPerSecond = 0; - - /** The absolute max speed the robot can reach while rotating radians per second. */ - private double attainableMaxRotationalVelocityRadiansPerSecond = 0; - - /** Maximum speed of the robot in meters per second. */ - private double maxSpeedMPS; - - /** Alert to recommend Tuner X if the configuration is compatible. */ - private final Alert tunerXRecommendation = - new Alert( - "Swerve Drive", - "Your Swerve Drive is compatible with Tuner X swerve generator, please consider using that instead of YAGSL. More information here!\n" - + "https://pro.docs.ctr-electronics.com/en/latest/docs/tuner/tuner-swerve/index.html", - AlertType.WARNING); - - /** - * Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} - * method, or via the {@link SwerveDrive#setRawModuleStates} method. The {@link SwerveDrive#drive} - * method incorporates kinematics-- it takes a translation and rotation, as well as parameters for - * field-centric and closed-loop velocity control. {@link SwerveDrive#setRawModuleStates} takes a - * list of SwerveModuleStates and directly passes them to the modules. This subsystem also handles - * odometry. - * - * @param config The {@link SwerveDriveConfiguration} configuration to base the swerve drive off - * of. - * @param controllerConfig The {@link SwerveControllerConfiguration} to use when creating the - * {@link SwerveController}. - * @param maxSpeedMPS Maximum speed in meters per second, remember to use {@link - * Units#feetToMeters(double)} if you have feet per second! - */ - public SwerveDrive( - SwerveDriveConfiguration config, - SwerveControllerConfiguration controllerConfig, - double maxSpeedMPS) { - this.maxSpeedMPS = maxSpeedMPS; - swerveDriveConfiguration = config; - swerveController = new SwerveController(controllerConfig); - // Create Kinematics from swerve module locations. - kinematics = new SwerveDriveKinematics(config.moduleLocationsMeters); - odometryThread = new Notifier(this::updateOdometry); - - // Create an integrator for angle if the robot is being simulated to emulate an IMU - // If the robot is real, instantiate the IMU instead. - if (SwerveDriveTelemetry.isSimulation) { - simIMU = new SwerveIMUSimulation(); - } else { - imu = config.imu; - imu.factoryDefault(); - } - - this.swerveModules = config.modules; - - // odometry = new SwerveDriveOdometry(kinematics, getYaw(), getModulePositions()); - swerveDrivePoseEstimator = - new SwerveDrivePoseEstimator( - kinematics, - getYaw(), - getModulePositions(), - new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(0)), - stateStdDevs, - visionMeasurementStdDevs); // x,y,heading in radians; Vision measurement std dev, - // higher=less weight - - zeroGyro(); - - // Initialize Telemetry - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) { - SmartDashboard.putData("Field", field); - } - - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) { - SwerveDriveTelemetry.maxSpeed = maxSpeedMPS; - SwerveDriveTelemetry.maxAngularVelocity = swerveController.config.maxAngularVelocity; - SwerveDriveTelemetry.moduleCount = swerveModules.length; - SwerveDriveTelemetry.sizeFrontBack = - Units.metersToInches( - SwerveMath.getSwerveModule(swerveModules, true, false).moduleLocation.getX() - + SwerveMath.getSwerveModule(swerveModules, false, false).moduleLocation.getX()); - SwerveDriveTelemetry.sizeLeftRight = - Units.metersToInches( - SwerveMath.getSwerveModule(swerveModules, false, true).moduleLocation.getY() - + SwerveMath.getSwerveModule(swerveModules, false, false).moduleLocation.getY()); - SwerveDriveTelemetry.wheelLocations = new double[SwerveDriveTelemetry.moduleCount * 2]; - for (SwerveModule module : swerveModules) { - SwerveDriveTelemetry.wheelLocations[module.moduleNumber * 2] = - Units.metersToInches(module.configuration.moduleLocation.getX()); - SwerveDriveTelemetry.wheelLocations[(module.moduleNumber * 2) + 1] = - Units.metersToInches(module.configuration.moduleLocation.getY()); - } - SwerveDriveTelemetry.measuredStates = new double[SwerveDriveTelemetry.moduleCount * 2]; - SwerveDriveTelemetry.desiredStates = new double[SwerveDriveTelemetry.moduleCount * 2]; - } - - odometryThread.startPeriodic(SwerveDriveTelemetry.isSimulation ? 0.01 : 0.02); - - checkIfTunerXCompatible(); - } - - /** Check all components to ensure that Tuner X Swerve Generator is recommended instead. */ - private void checkIfTunerXCompatible() { - boolean compatible = imu instanceof Pigeon2Swerve; - for (SwerveModule module : swerveModules) { - compatible = - compatible - && module.getDriveMotor() instanceof TalonFXSwerve - && module.getAngleMotor() instanceof TalonFXSwerve - && module.getAbsoluteEncoder() instanceof CANCoderSwerve; - if (!compatible) { - break; - } - } - if (compatible) { - tunerXRecommendation.set(true); - } - } - - /** - * Set the odometry update period in seconds. - * - * @param period period in seconds. - */ - public void setOdometryPeriod(double period) { - odometryThread.stop(); - odometryThread.startPeriodic(period); - } - - /** Stop the odometry thread in favor of manually updating odometry. */ - public void stopOdometryThread() { - odometryThread.stop(); - } - - /** - * Set the conversion factor for the angle/azimuth motor controller. - * - * @param conversionFactor Angle motor conversion factor for PID, should be generated from {@link - * SwerveMath#calculateDegreesPerSteeringRotation(double, double)} or calculated. - */ - public void setAngleMotorConversionFactor(double conversionFactor) { - for (SwerveModule module : swerveModules) { - module.setAngleMotorConversionFactor(conversionFactor); - } - } - - /** - * Set the conversion factor for the drive motor controller. - * - * @param conversionFactor Drive motor conversion factor for PID, should be generated from {@link - * SwerveMath#calculateMetersPerRotation(double, double, double)} or calculated. - */ - public void setDriveMotorConversionFactor(double conversionFactor) { - for (SwerveModule module : swerveModules) { - module.setDriveMotorConversionFactor(conversionFactor); - } - } - - /** - * Set the heading correction capabilities of YAGSL. - * - * @param state {@link SwerveDrive#headingCorrection} state. - */ - public void setHeadingCorrection(boolean state) { - setHeadingCorrection(state, HEADING_CORRECTION_DEADBAND); - } - - /** - * Set the heading correction capabilities of YAGSL. - * - * @param state {@link SwerveDrive#headingCorrection} state. - * @param deadband {@link SwerveDrive#HEADING_CORRECTION_DEADBAND} deadband. - */ - public void setHeadingCorrection(boolean state, double deadband) { - headingCorrection = state; - HEADING_CORRECTION_DEADBAND = deadband; - } - - /** - * Secondary method of controlling the drive base given velocity and adjusting it for field - * oriented use. - * - * @param velocity Velocity of the robot desired. - */ - public void driveFieldOriented(ChassisSpeeds velocity) { - ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getYaw()); - drive(fieldOrientedVelocity); - } - - /** - * Secondary method of controlling the drive base given velocity and adjusting it for field - * oriented use. - * - * @param velocity Velocity of the robot desired. - * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot. - */ - public void driveFieldOriented(ChassisSpeeds velocity, Translation2d centerOfRotationMeters) { - ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getYaw()); - drive(fieldOrientedVelocity, centerOfRotationMeters); - } - - /** - * Secondary method for controlling the drivebase. Given a simple {@link ChassisSpeeds} set the - * swerve module states, to achieve the goal. - * - * @param velocity The desired robot-oriented {@link ChassisSpeeds} for the robot to achieve. - */ - public void drive(ChassisSpeeds velocity) { - drive(velocity, false, new Translation2d()); - } - - /** - * Secondary method for controlling the drivebase. Given a simple {@link ChassisSpeeds} set the - * swerve module states, to achieve the goal. - * - * @param velocity The desired robot-oriented {@link ChassisSpeeds} for the robot to achieve. - * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot. - */ - public void drive(ChassisSpeeds velocity, Translation2d centerOfRotationMeters) { - drive(velocity, false, centerOfRotationMeters); - } - - /** - * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation - * rate, and calculates and commands module states accordingly. Can use either open-loop or - * closed-loop velocity control for the wheel velocities. Also has field- and robot-relative - * modes, which affect how the translation vector is used. - * - * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in - * meters per second. In robot-relative mode, positive x is torwards the bow (front) and - * positive y is torwards port (left). In field-relative mode, positive x is away from the - * alliance wall (field North) and positive y is torwards the left wall when looking through - * the driver station glass (field West). - * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by - * field/robot relativity. - * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. - * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable - * closed-loop. - * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot. - */ - public void drive( - Translation2d translation, - double rotation, - boolean fieldRelative, - boolean isOpenLoop, - Translation2d centerOfRotationMeters) { - // Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if - // necessary. - ChassisSpeeds velocity = - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds( - translation.getX(), translation.getY(), rotation, getYaw()) - : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); - - drive(velocity, isOpenLoop, centerOfRotationMeters); - } - - /** - * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation - * rate, and calculates and commands module states accordingly. Can use either open-loop or - * closed-loop velocity control for the wheel velocities. Also has field- and robot-relative - * modes, which affect how the translation vector is used. - * - * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in - * meters per second. In robot-relative mode, positive x is torwards the bow (front) and - * positive y is torwards port (left). In field-relative mode, positive x is away from the - * alliance wall (field North) and positive y is torwards the left wall when looking through - * the driver station glass (field West). - * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by - * field/robot relativity. - * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. - * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable - * closed-loop. - */ - public void drive( - Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) { - // Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if - // necessary. - ChassisSpeeds velocity = - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds( - translation.getX(), translation.getY(), rotation, getYaw()) - : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); - - drive(velocity, isOpenLoop, new Translation2d()); - } - - /** - * The primary method for controlling the drivebase. Takes a {@link ChassisSpeeds}, and calculates - * and commands module states accordingly. Can use either open-loop or closed-loop velocity - * control for the wheel velocities. Also has field- and robot-relative modes, which affect how - * the translation vector is used. - * - * @param velocity The chassis speeds to set the robot to achieve. - * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable - * closed-loop. - * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot. - */ - public void drive( - ChassisSpeeds velocity, boolean isOpenLoop, Translation2d centerOfRotationMeters) { - - // Thank you to Jared Russell FRC254 for Open Loop Compensation Code - // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5 - if (chassisVelocityCorrection) { - velocity = ChassisSpeeds.discretize(velocity, 0.02); - } - - // Heading Angular Velocity Deadband, might make a configuration option later. - // Originally made by Team 1466 Webb Robotics. - // Modified by Team 7525 Pioneers and BoiledBurntBagel of 6036 - if (headingCorrection) { - if (Math.abs(velocity.omegaRadiansPerSecond) < HEADING_CORRECTION_DEADBAND - && (Math.abs(velocity.vxMetersPerSecond) > HEADING_CORRECTION_DEADBAND - || Math.abs(velocity.vyMetersPerSecond) > HEADING_CORRECTION_DEADBAND)) { - if (!correctionEnabled) { - lastHeadingRadians = getYaw().getRadians(); - correctionEnabled = true; - } - velocity.omegaRadiansPerSecond = - swerveController.headingCalculate(lastHeadingRadians, getYaw().getRadians()); - } else { - correctionEnabled = false; - } - } - - // Display commanded speed for testing - if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) { - SmartDashboard.putString("RobotVelocity", velocity.toString()); - } - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) { - SwerveDriveTelemetry.desiredChassisSpeeds[1] = velocity.vyMetersPerSecond; - SwerveDriveTelemetry.desiredChassisSpeeds[0] = velocity.vxMetersPerSecond; - SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(velocity.omegaRadiansPerSecond); - } - - // Calculate required module states via kinematics - SwerveModuleState[] swerveModuleStates = - kinematics.toSwerveModuleStates(velocity, centerOfRotationMeters); - - setRawModuleStates(swerveModuleStates, isOpenLoop); - } - - /** - * Set the maximum speeds for desaturation. - * - * @param attainableMaxModuleSpeedMetersPerSecond The absolute max speed that a module can reach - * in meters per second. - * @param attainableMaxTranslationalSpeedMetersPerSecond The absolute max speed that your robot - * can reach while translating in meters per second. - * @param attainableMaxRotationalVelocityRadiansPerSecond The absolute max speed the robot can - * reach while rotating in radians per second. - */ - public void setMaximumSpeeds( - double attainableMaxModuleSpeedMetersPerSecond, - double attainableMaxTranslationalSpeedMetersPerSecond, - double attainableMaxRotationalVelocityRadiansPerSecond) { - setMaximumSpeed(attainableMaxModuleSpeedMetersPerSecond); - this.attainableMaxTranslationalSpeedMetersPerSecond = - attainableMaxTranslationalSpeedMetersPerSecond; - this.attainableMaxRotationalVelocityRadiansPerSecond = - attainableMaxRotationalVelocityRadiansPerSecond; - this.swerveController.config.maxAngularVelocity = - attainableMaxRotationalVelocityRadiansPerSecond; - } - - /** - * Get the maximum velocity from {@link - * SwerveDrive#attainableMaxTranslationalSpeedMetersPerSecond} or {@link SwerveDrive#maxSpeedMPS} - * whichever is higher. - * - * @return Maximum speed in meters/second. - */ - public double getMaximumVelocity() { - return Math.max(this.attainableMaxTranslationalSpeedMetersPerSecond, maxSpeedMPS); - } - - /** - * Get the maximum angular velocity, either {@link - * SwerveDrive#attainableMaxRotationalVelocityRadiansPerSecond} or {@link - * SwerveControllerConfiguration#maxAngularVelocity}. - * - * @return Maximum angular velocity in radians per second. - */ - public double getMaximumAngularVelocity() { - return Math.max( - this.attainableMaxRotationalVelocityRadiansPerSecond, - swerveController.config.maxAngularVelocity); - } - - /** - * Set the module states (azimuth and velocity) directly. Used primarily for auto pathing. - * - * @param desiredStates A list of SwerveModuleStates to send to the modules. - * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable - * closed-loop. - */ - private void setRawModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop) { - // Desaturates wheel speeds - if (attainableMaxTranslationalSpeedMetersPerSecond != 0 - || attainableMaxRotationalVelocityRadiansPerSecond != 0) { - SwerveDriveKinematics.desaturateWheelSpeeds( - desiredStates, - getRobotVelocity(), - maxSpeedMPS, - attainableMaxTranslationalSpeedMetersPerSecond, - attainableMaxRotationalVelocityRadiansPerSecond); - } - - // Sets states - for (SwerveModule module : swerveModules) { - module.setDesiredState(desiredStates[module.moduleNumber], isOpenLoop, false); - - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) { - SwerveDriveTelemetry.desiredStates[module.moduleNumber * 2] = - module.lastState.angle.getDegrees(); - SwerveDriveTelemetry.desiredStates[(module.moduleNumber * 2) + 1] = - module.lastState.speedMetersPerSecond; - } - } - } - - /** - * Set the module states (azimuth and velocity) directly. Used primarily for auto paths. - * - * @param desiredStates A list of SwerveModuleStates to send to the modules. - * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable - * closed-loop. - */ - public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop) { - setRawModuleStates( - kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates)), isOpenLoop); - } - - /** - * Set chassis speeds with closed-loop velocity control. - * - * @param chassisSpeeds Chassis speeds to set. - */ - public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { - SwerveDriveTelemetry.desiredChassisSpeeds[1] = chassisSpeeds.vyMetersPerSecond; - SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond; - SwerveDriveTelemetry.desiredChassisSpeeds[2] = - Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond); - - setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), false); - } - - /** - * Gets the current pose (position and rotation) of the robot, as reported by odometry. - * - * @return The robot's pose - */ - public Pose2d getPose() { - - odometryLock.lock(); - Pose2d poseEstimation = swerveDrivePoseEstimator.getEstimatedPosition(); - odometryLock.unlock(); - return poseEstimation; - } - - /** - * Gets the current field-relative velocity (x, y and omega) of the robot - * - * @return A ChassisSpeeds object of the current field-relative velocity - */ - public ChassisSpeeds getFieldVelocity() { - // ChassisSpeeds has a method to convert from field-relative to robot-relative speeds, - // but not the reverse. However, because this transform is a simple rotation, negating the - // angle - // given as the robot angle reverses the direction of rotation, and the conversion is reversed. - return ChassisSpeeds.fromFieldRelativeSpeeds( - kinematics.toChassisSpeeds(getStates()), getYaw().unaryMinus()); - } - - /** - * Gets the current robot-relative velocity (x, y and omega) of the robot - * - * @return A ChassisSpeeds object of the current robot-relative velocity - */ - public ChassisSpeeds getRobotVelocity() { - return kinematics.toChassisSpeeds(getStates()); - } - - /** - * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when - * calling this method. However, if either gyro angle or module position is reset, this must be - * called in order for odometry to keep working. - * - * @param pose The pose to set the odometry to - */ - public void resetOdometry(Pose2d pose) { - odometryLock.lock(); - swerveDrivePoseEstimator.resetPosition(pose.getRotation(), getModulePositions(), pose); - odometryLock.unlock(); - kinematics.toSwerveModuleStates( - ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, pose.getRotation())); - } - - /** - * Post the trajectory to the field - * - * @param trajectory the trajectory to post. - */ - public void postTrajectory(Trajectory trajectory) { - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) { - field.getObject("Trajectory").setTrajectory(trajectory); - } - } - - /** - * Gets the current module states (azimuth and velocity) - * - * @return A list of SwerveModuleStates containing the current module states - */ - public SwerveModuleState[] getStates() { - SwerveModuleState[] states = new SwerveModuleState[swerveDriveConfiguration.moduleCount]; - for (SwerveModule module : swerveModules) { - states[module.moduleNumber] = module.getState(); - } - return states; - } - - /** - * Gets the current module positions (azimuth and wheel position (meters)). Inverts the distance - * from each module if {@link #invertOdometry} is true. - * - * @return A list of SwerveModulePositions containg the current module positions - */ - public SwerveModulePosition[] getModulePositions() { - SwerveModulePosition[] positions = - new SwerveModulePosition[swerveDriveConfiguration.moduleCount]; - for (SwerveModule module : swerveModules) { - positions[module.moduleNumber] = module.getPosition(); - if (invertOdometry) { - positions[module.moduleNumber].distanceMeters *= -1; - } - } - return positions; - } - - /** - * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0. - */ - public void zeroGyro() { - // Resets the real gyro or the angle accumulator, depending on whether the robot is being - // simulated - if (!SwerveDriveTelemetry.isSimulation) { - imu.setOffset(imu.getRawRotation3d()); - } else { - simIMU.setAngle(0); - } - swerveController.lastAngleScalar = 0; - lastHeadingRadians = 0; - resetOdometry(new Pose2d(getPose().getTranslation(), new Rotation2d())); - } - - /** - * Gets the current yaw angle of the robot, as reported by the imu. CCW positive, not wrapped. - * - * @return The yaw as a {@link Rotation2d} angle - */ - public Rotation2d getYaw() { - // Read the imu if the robot is real or the accumulator if the robot is simulated. - if (!SwerveDriveTelemetry.isSimulation) { - return swerveDriveConfiguration.invertedIMU - ? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getZ()) - : Rotation2d.fromRadians(imu.getRotation3d().getZ()); - } else { - return simIMU.getYaw(); - } - } - - /** - * Gets the current pitch angle of the robot, as reported by the imu. - * - * @return The heading as a {@link Rotation2d} angle - */ - public Rotation2d getPitch() { - // Read the imu if the robot is real or the accumulator if the robot is simulated. - if (!SwerveDriveTelemetry.isSimulation) { - return swerveDriveConfiguration.invertedIMU - ? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getY()) - : Rotation2d.fromRadians(imu.getRotation3d().getY()); - } else { - return simIMU.getPitch(); - } - } - - /** - * Gets the current roll angle of the robot, as reported by the imu. - * - * @return The heading as a {@link Rotation2d} angle - */ - public Rotation2d getRoll() { - // Read the imu if the robot is real or the accumulator if the robot is simulated. - if (!SwerveDriveTelemetry.isSimulation) { - return swerveDriveConfiguration.invertedIMU - ? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getX()) - : Rotation2d.fromRadians(imu.getRotation3d().getX()); - } else { - return simIMU.getRoll(); - } - } - - /** - * Gets the current gyro {@link Rotation3d} of the robot, as reported by the imu. - * - * @return The heading as a {@link Rotation3d} angle - */ - public Rotation3d getGyroRotation3d() { - // Read the imu if the robot is real or the accumulator if the robot is simulated. - if (!SwerveDriveTelemetry.isSimulation) { - return swerveDriveConfiguration.invertedIMU - ? imu.getRotation3d().unaryMinus() - : imu.getRotation3d(); - } else { - return simIMU.getGyroRotation3d(); - } - } - - /** - * Gets current acceleration of the robot in m/s/s. If gyro unsupported returns empty. - * - * @return acceleration of the robot as a {@link Translation3d} - */ - public Optional getAccel() { - if (!SwerveDriveTelemetry.isSimulation) { - return imu.getAccel(); - } else { - return simIMU.getAccel(); - } - } - - /** - * Sets the drive motors to brake/coast mode. - * - * @param brake True to set motors to brake mode, false for coast. - */ - public void setMotorIdleMode(boolean brake) { - for (SwerveModule swerveModule : swerveModules) { - swerveModule.setMotorBrake(brake); - } - } - - /** - * Set the maximum speed of the drive motors, modified {@link SwerveDrive#maxSpeedMPS} which is - * used for the {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], boolean)} function and - * {@link SwerveController#getTargetSpeeds(double, double, double, double, double)} functions. - * This function overrides what was placed in the JSON and could damage your motor/robot if set - * too high or unachievable rates. - * - * @param maximumSpeed Maximum speed for the drive motors in meters / second. - * @param updateModuleFeedforward Update the swerve module feedforward to account for the new - * maximum speed. This should be true unless you have replaced the drive motor feedforward - * with {@link SwerveDrive#replaceSwerveModuleFeedforward(SimpleMotorFeedforward)} - * @param optimalVoltage Optimal voltage to use for the feedforward. - */ - public void setMaximumSpeed( - double maximumSpeed, boolean updateModuleFeedforward, double optimalVoltage) { - maxSpeedMPS = maximumSpeed; - swerveDriveConfiguration.physicalCharacteristics.optimalVoltage = optimalVoltage; - for (SwerveModule module : swerveModules) { - if (updateModuleFeedforward) { - module.feedforward = - SwerveMath.createDriveFeedforward( - optimalVoltage, - maximumSpeed, - swerveDriveConfiguration.physicalCharacteristics.wheelGripCoefficientOfFriction); - } - } - } - - /** - * Set the maximum speed of the drive motors, modified {@link SwerveDrive#maxSpeedMPS} which is - * used for the {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], boolean)} function and - * {@link SwerveController#getTargetSpeeds(double, double, double, double, double)} functions. - * This function overrides what was placed in the JSON and could damage your motor/robot if set - * too high or unachievable rates. Overwrites the {@link SwerveModule#feedforward}. - * - * @param maximumSpeed Maximum speed for the drive motors in meters / second. - */ - public void setMaximumSpeed(double maximumSpeed) { - setMaximumSpeed( - maximumSpeed, true, swerveDriveConfiguration.physicalCharacteristics.optimalVoltage); - } - - /** - * Point all modules toward the robot center, thus making the robot very difficult to move. - * Forcing the robot to keep the current pose. - */ - public void lockPose() { - // Sets states - for (SwerveModule swerveModule : swerveModules) { - SwerveModuleState desiredState = - new SwerveModuleState(0, swerveModule.configuration.moduleLocation.getAngle()); - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) { - SwerveDriveTelemetry.desiredStates[swerveModule.moduleNumber * 2] = - desiredState.angle.getDegrees(); - SwerveDriveTelemetry.desiredStates[(swerveModule.moduleNumber * 2) + 1] = - desiredState.speedMetersPerSecond; - } - swerveModule.setDesiredState(desiredState, false, true); - } - - // Update kinematics because we are not using setModuleStates - kinematics.toSwerveModuleStates(new ChassisSpeeds()); - } - - /** - * Get the swerve module poses and on the field relative to the robot. - * - * @param robotPose Robot pose. - * @return Swerve module poses. - */ - public Pose2d[] getSwerveModulePoses(Pose2d robotPose) { - Pose2d[] poseArr = new Pose2d[swerveDriveConfiguration.moduleCount]; - List poses = new ArrayList<>(); - for (SwerveModule module : swerveModules) { - poses.add( - robotPose.plus( - new Transform2d(module.configuration.moduleLocation, module.getState().angle))); - } - return poses.toArray(poseArr); - } - - /** - * Setup the swerve module feedforward. - * - * @param feedforward Feedforward for the drive motor on swerve modules. - */ - public void replaceSwerveModuleFeedforward(SimpleMotorFeedforward feedforward) { - for (SwerveModule swerveModule : swerveModules) { - swerveModule.feedforward = feedforward; - } - } - - /** - * Update odometry should be run every loop. Synchronizes module absolute encoders with relative - * encoders periodically. In simulation mode will also post the pose of each module. Updates - * SmartDashboard with module encoder readings and states. - */ - public void updateOdometry() { - odometryLock.lock(); - try { - // Update odometry - swerveDrivePoseEstimator.update(getYaw(), getModulePositions()); - - // Update angle accumulator if the robot is simulated - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) { - Pose2d[] modulePoses = - getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition()); - if (SwerveDriveTelemetry.isSimulation) { - simIMU.updateOdometry(kinematics, getStates(), modulePoses, field); - } - - ChassisSpeeds measuredChassisSpeeds = getRobotVelocity(); - SwerveDriveTelemetry.measuredChassisSpeeds[1] = measuredChassisSpeeds.vyMetersPerSecond; - SwerveDriveTelemetry.measuredChassisSpeeds[0] = measuredChassisSpeeds.vxMetersPerSecond; - SwerveDriveTelemetry.measuredChassisSpeeds[2] = - Math.toDegrees(measuredChassisSpeeds.omegaRadiansPerSecond); - SwerveDriveTelemetry.robotRotation = getYaw().getDegrees(); - } - - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) { - field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition()); - } - - double sumVelocity = 0; - for (SwerveModule module : swerveModules) { - SwerveModuleState moduleState = module.getState(); - sumVelocity += Math.abs(moduleState.speedMetersPerSecond); - if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) { - module.updateTelemetry(); - SmartDashboard.putNumber("Adjusted IMU Yaw", getYaw().getDegrees()); - } - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) { - SwerveDriveTelemetry.measuredStates[module.moduleNumber * 2] = - moduleState.angle.getDegrees(); - SwerveDriveTelemetry.measuredStates[(module.moduleNumber * 2) + 1] = - moduleState.speedMetersPerSecond; - } - } - - // If the robot isn't moving synchronize the encoders every 100ms (Inspired by democrat's SDS - // lib) - // To ensure that everytime we initialize it works. - if (sumVelocity <= .01 && ++moduleSynchronizationCounter > 5) { - synchronizeModuleEncoders(); - moduleSynchronizationCounter = 0; - } - - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) { - SwerveDriveTelemetry.updateData(); - } - } catch (Exception e) { - odometryLock.unlock(); - throw e; - } - odometryLock.unlock(); - } - - /** Synchronize angle motor integrated encoders with data from absolute encoders. */ - public void synchronizeModuleEncoders() { - for (SwerveModule module : swerveModules) { - module.queueSynchronizeEncoders(); - } - } - - /** - * Set the gyro scope offset to a desired known rotation. Unlike {@link - * SwerveDrive#setGyro(Rotation3d)} it DOES NOT take the current rotation into account. - * - * @param offset {@link Rotation3d} known offset of the robot for gyroscope to use. - */ - public void setGyroOffset(Rotation3d offset) { - if (SwerveDriveTelemetry.isSimulation) { - simIMU.setAngle(offset.getZ()); - } else { - imu.setOffset(offset); - } - } - - /** - * Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link - * SwerveIMU} gyro reading with the given timestamp of the vision measurement.
- * IT IS HIGHLY RECOMMENDED TO UPDATE YOUR GYROSCOPE OFFSET AFTER USING THIS FUNCTION!
- * To update your gyroscope readings directly use {@link SwerveDrive#setGyroOffset(Rotation3d)}. - * - * @param robotPose Robot {@link Pose2d} as measured by vision. - * @param timestamp Timestamp the measurement was taken as time since startup, should be taken - * from {@link Timer#getFPGATimestamp()} or similar sources. - */ - public void addVisionMeasurement(Pose2d robotPose, double timestamp) { - odometryLock.lock(); - swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp); - Pose2d newOdometry = - new Pose2d( - swerveDrivePoseEstimator.getEstimatedPosition().getTranslation(), - robotPose.getRotation()); - odometryLock.unlock(); - - setGyroOffset(new Rotation3d(0, 0, robotPose.getRotation().getRadians())); - resetOdometry(newOdometry); - } - - /** - * Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link - * SwerveIMU} gyro reading with the given timestamp of the vision measurement. - * - * @param robotPose Robot {@link Pose2d} as measured by vision. - * @param timestamp Timestamp the measurement was taken as time since startup, should be taken - * from {@link Timer#getFPGATimestamp()} or similar sources. - * @param visionMeasurementStdDevs Vision measurement standard deviation that will be sent to the - * {@link SwerveDrivePoseEstimator}.The standard deviation of the vision measurement, for best - * accuracy calculate the standard deviation at 2 or more points and fit a line to it with the - * calculated optimal standard deviation. (Units should be meters per pixel). By optimizing - * this you can get * vision accurate to inches instead of feet. - */ - public void addVisionMeasurement( - Pose2d robotPose, double timestamp, Matrix visionMeasurementStdDevs) { - this.visionMeasurementStdDevs = visionMeasurementStdDevs; - addVisionMeasurement(robotPose, timestamp); - } - - /** - * Set the expected gyroscope angle using a {@link Rotation3d} object. To reset gyro, set to a new - * {@link Rotation3d} subtracted from the current gyroscopic readings {@link - * SwerveIMU#getRotation3d()}. - * - * @param gyro expected gyroscope angle as {@link Rotation3d}. - */ - public void setGyro(Rotation3d gyro) { - if (SwerveDriveTelemetry.isSimulation) { - setGyroOffset(simIMU.getGyroRotation3d().minus(gyro)); - } else { - setGyroOffset(imu.getRawRotation3d().minus(gyro)); - } - } - - /** - * Helper function to get the {@link SwerveDrive#swerveController} for the {@link SwerveDrive} - * which can be used to generate {@link ChassisSpeeds} for the robot to orient it correctly given - * axis or angles, and apply {@link edu.wpi.first.math.filter.SlewRateLimiter} to given inputs. - * Important functions to look at are {@link SwerveController#getTargetSpeeds(double, double, - * double, double, double)}, {@link SwerveController#addSlewRateLimiters(SlewRateLimiter, - * SlewRateLimiter, SlewRateLimiter)}, {@link SwerveController#getRawTargetSpeeds(double, double, - * double)}. - * - * @return {@link SwerveController} for the {@link SwerveDrive}. - */ - public SwerveController getSwerveController() { - return swerveController; - } - - /** - * Get the {@link SwerveModule}s associated with the {@link SwerveDrive}. - * - * @return {@link SwerveModule} array specified by configurations. - */ - public SwerveModule[] getModules() { - return swerveDriveConfiguration.modules; - } - - /** - * Reset the drive encoders on the robot, useful when manually resetting the robot without a - * reboot, like in autonomous. - */ - public void resetDriveEncoders() { - for (SwerveModule module : swerveModules) { - module.configuration.driveMotor.setPosition(0); - } - } - - /** - * Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. Also - * removes the internal offsets to prevent double offsetting. - */ - public void pushOffsetsToControllers() { - for (SwerveModule module : swerveModules) { - module.pushOffsetsToControllers(); - } - } - - /** Restores Internal YAGSL Encoder offsets and sets the Encoder stored offset back to 0 */ - public void restoreInternalOffset() { - for (SwerveModule module : swerveModules) { - module.restoreInternalOffset(); - } - } -} diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java deleted file mode 100644 index fdd3a01..0000000 --- a/src/main/java/swervelib/SwerveModule.java +++ /dev/null @@ -1,431 +0,0 @@ -package swervelib; - -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import swervelib.encoders.SwerveAbsoluteEncoder; -import swervelib.math.SwerveMath; -import swervelib.motors.SwerveMotor; -import swervelib.parser.SwerveModuleConfiguration; -import swervelib.simulation.SwerveModuleSimulation; -import swervelib.telemetry.Alert; -import swervelib.telemetry.SwerveDriveTelemetry; -import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; - -/** The Swerve Module class which represents and controls Swerve Modules for the swerve drive. */ -public class SwerveModule { - - /** Swerve module configuration options. */ - public final SwerveModuleConfiguration configuration; - - /** Swerve Motors. */ - private final SwerveMotor angleMotor, driveMotor; - - /** Absolute encoder for swerve drive. */ - private final SwerveAbsoluteEncoder absoluteEncoder; - - /** An {@link Alert} for if pushing the Absolute Encoder offset to the encoder fails. */ - private final Alert encoderOffsetWarning; - - /** An {@link Alert} for if there is no Absolute Encoder on the module. */ - private final Alert noEncoderWarning; - - /** - * Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back - * right. - */ - public int moduleNumber; - - /** Feedforward for drive motor during closed loop control. */ - public SimpleMotorFeedforward feedforward; - - /** Maximum speed of the drive motors in meters per second. */ - public double maxSpeed; - - /** Last swerve module state applied. */ - public SwerveModuleState lastState; - - /** Angle offset from the absolute encoder. */ - private double angleOffset; - - /** Simulated swerve module. */ - private SwerveModuleSimulation simModule; - - /** Encoder synchronization queued. */ - private boolean synchronizeEncoderQueued = false; - - /** - * Construct the swerve module and initialize the swerve module motors and absolute encoder. - * - * @param moduleNumber Module number for kinematics. - * @param moduleConfiguration Module constants containing CAN ID's and offsets. - * @param driveFeedforward Drive motor feedforward created by {@link - * SwerveMath#createDriveFeedforward(double, double, double)}. - */ - public SwerveModule( - int moduleNumber, - SwerveModuleConfiguration moduleConfiguration, - SimpleMotorFeedforward driveFeedforward) { - // angle = 0; - // speed = 0; - // omega = 0; - // fakePos = 0; - this.moduleNumber = moduleNumber; - configuration = moduleConfiguration; - angleOffset = moduleConfiguration.angleOffset; - - // Initialize Feedforward for drive motor. - feedforward = driveFeedforward; - - // Create motors from configuration and reset them to defaults. - angleMotor = moduleConfiguration.angleMotor; - driveMotor = moduleConfiguration.driveMotor; - angleMotor.factoryDefaults(); - driveMotor.factoryDefaults(); - - // Configure voltage comp, current limit, and ramp rate. - angleMotor.setVoltageCompensation(configuration.physicalCharacteristics.optimalVoltage); - driveMotor.setVoltageCompensation(configuration.physicalCharacteristics.optimalVoltage); - angleMotor.setCurrentLimit(configuration.physicalCharacteristics.angleMotorCurrentLimit); - driveMotor.setCurrentLimit(configuration.physicalCharacteristics.driveMotorCurrentLimit); - angleMotor.setLoopRampRate(configuration.physicalCharacteristics.angleMotorRampRate); - driveMotor.setLoopRampRate(configuration.physicalCharacteristics.driveMotorRampRate); - - // Config angle encoders - absoluteEncoder = moduleConfiguration.absoluteEncoder; - if (absoluteEncoder != null) { - absoluteEncoder.factoryDefault(); - absoluteEncoder.configure(moduleConfiguration.absoluteEncoderInverted); - angleMotor.setPosition(getAbsolutePosition()); - } - - // Config angle motor/controller - angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle); - angleMotor.configurePIDF(moduleConfiguration.anglePIDF); - angleMotor.configurePIDWrapping(0, 180); - angleMotor.setInverted(moduleConfiguration.angleMotorInverted); - angleMotor.setMotorBrake(false); - - // Config drive motor/controller - driveMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.drive); - driveMotor.configurePIDF(moduleConfiguration.velocityPIDF); - driveMotor.setInverted(moduleConfiguration.driveMotorInverted); - driveMotor.setMotorBrake(true); - - driveMotor.burnFlash(); - angleMotor.burnFlash(); - - if (SwerveDriveTelemetry.isSimulation) { - simModule = new SwerveModuleSimulation(); - } - - lastState = getState(); - - noEncoderWarning = - new Alert( - "Motors", - "There is no Absolute Encoder on module #" + moduleNumber, - Alert.AlertType.WARNING); - encoderOffsetWarning = - new Alert( - "Motors", - "Pushing the Absolute Encoder offset to the encoder failed on module #" + moduleNumber, - Alert.AlertType.WARNING); - } - - /** - * Set the voltage compensation for the swerve module motor. - * - * @param optimalVoltage Nominal voltage for operation to output to. - */ - public void setAngleMotorVoltageCompensation(double optimalVoltage) { - angleMotor.setVoltageCompensation(optimalVoltage); - } - - /** - * Set the voltage compensation for the swerve module motor. - * - * @param optimalVoltage Nominal voltage for operation to output to. - */ - public void setDriveMotorVoltageCompensation(double optimalVoltage) { - driveMotor.setVoltageCompensation(optimalVoltage); - } - - /** Queue synchronization of the integrated angle encoder with the absolute encoder. */ - public void queueSynchronizeEncoders() { - if (absoluteEncoder != null) { - synchronizeEncoderQueued = true; - } - } - - /** - * Set the desired state of the swerve module.
- * WARNING: If you are not using one of the functions from {@link SwerveDrive} you may screw up - * {@link SwerveDrive#kinematics} - * - * @param desiredState Desired swerve module state. - * @param isOpenLoop Whether to use open loop (direct percent) or direct velocity control. - * @param force Disables optimizations that prevent movement in the angle motor and forces the - * desired state onto the swerve module. - */ - public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force) { - desiredState = - SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(getAbsolutePosition())); - - if (isOpenLoop) { - double percentOutput = desiredState.speedMetersPerSecond / maxSpeed; - driveMotor.set(percentOutput); - } else { - // Taken from the CTRE SwerveModule class. - // https://api.ctr-electronics.com/phoenix6/release/java/src-html/com/ctre/phoenix6/mechanisms/swerve/SwerveModule.html#line.46 - /* From FRC 900's whitepaper, we add a cosine compensator to the applied drive velocity */ - /* To reduce the "skew" that occurs when changing direction */ - double steerMotorError = desiredState.angle.getDegrees() - getAbsolutePosition(); - /* If error is close to 0 rotations, we're already there, so apply full power */ - /* If the error is close to 0.25 rotations, then we're 90 degrees, so movement doesn't help us at all */ - double cosineScalar = Math.cos(Units.degreesToRadians(steerMotorError)); - /* Make sure we don't invert our drive, even though we shouldn't ever target over 90 degrees anyway */ - if (cosineScalar < 0.0) { - cosineScalar = 0.0; - } - - double velocity = desiredState.speedMetersPerSecond * (cosineScalar); - driveMotor.setReference(velocity, feedforward.calculate(velocity)); - } - - /* // Not necessary anymore. - // If we are forcing the angle - if (!force) - { - // Prevents module rotation if speed is less than 1% - SwerveMath.antiJitter(desiredState, lastState, Math.min(maxSpeed, 4)); - } - */ - - // Prevent module rotation if angle is the same as the previous angle. - // Synchronize encoders if queued and send in the current position as the value from the - // absolute encoder. - if (absoluteEncoder != null && synchronizeEncoderQueued) { - double absoluteEncoderPosition = getAbsolutePosition(); - angleMotor.setPosition(absoluteEncoderPosition); - angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition); - synchronizeEncoderQueued = false; - } else { - angleMotor.setReference(desiredState.angle.getDegrees(), 0); - } - - lastState = desiredState; - - if (SwerveDriveTelemetry.isSimulation) { - simModule.updateStateAndPosition(desiredState); - } - - if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) { - SmartDashboard.putNumber( - "Module[" + configuration.name + "] Speed Setpoint", desiredState.speedMetersPerSecond); - SmartDashboard.putNumber( - "Module[" + configuration.name + "] Angle Setpoint", desiredState.angle.getDegrees()); - } - } - - /** - * Set the angle for the module. - * - * @param angle Angle in degrees. - */ - public void setAngle(double angle) { - angleMotor.setReference(angle, 0); - lastState.angle = Rotation2d.fromDegrees(angle); - } - - /** - * Get the Swerve Module state. - * - * @return Current SwerveModule state. - */ - public SwerveModuleState getState() { - double velocity; - Rotation2d azimuth; - if (!SwerveDriveTelemetry.isSimulation) { - velocity = driveMotor.getVelocity(); - azimuth = Rotation2d.fromDegrees(getAbsolutePosition()); - } else { - return simModule.getState(); - } - return new SwerveModuleState(velocity, azimuth); - } - - /** - * Get the position of the swerve module. - * - * @return {@link SwerveModulePosition} of the swerve module. - */ - public SwerveModulePosition getPosition() { - double position; - Rotation2d azimuth; - if (!SwerveDriveTelemetry.isSimulation) { - position = driveMotor.getPosition(); - azimuth = Rotation2d.fromDegrees(getAbsolutePosition()); - } else { - return simModule.getPosition(); - } - return new SwerveModulePosition(position, azimuth); - } - - /** - * Get the absolute position. Falls back to relative position on reading failure. - * - * @return Absolute encoder angle in degrees in the range [0, 360). - */ - public double getAbsolutePosition() { - double angle; - if (absoluteEncoder != null) { - angle = absoluteEncoder.getAbsolutePosition() - angleOffset; - if (absoluteEncoder.readingError) { - angle = getRelativePosition(); - } - } else { - angle = getRelativePosition(); - } - angle %= 360; - if (angle < 0.0) { - angle += 360; - } - - return angle; - } - - /** - * Get the relative angle in degrees. - * - * @return Angle in degrees. - */ - public double getRelativePosition() { - return angleMotor.getPosition(); - } - - /** - * Set the brake mode. - * - * @param brake Set the brake mode. - */ - public void setMotorBrake(boolean brake) { - driveMotor.setMotorBrake(brake); - } - - /** - * Set the conversion factor for the angle/azimuth motor controller. - * - * @param conversionFactor Angle motor conversion factor for PID, should be generated from {@link - * SwerveMath#calculateDegreesPerSteeringRotation(double, double)} or calculated. - */ - public void setAngleMotorConversionFactor(double conversionFactor) { - angleMotor.configureIntegratedEncoder(conversionFactor); - } - - /** - * Set the conversion factor for the drive motor controller. - * - * @param conversionFactor Drive motor conversion factor for PID, should be generated from {@link - * SwerveMath#calculateMetersPerRotation(double, double, double)} or calculated. - */ - public void setDriveMotorConversionFactor(double conversionFactor) { - driveMotor.configureIntegratedEncoder(conversionFactor); - } - - /** - * Get the angle {@link SwerveMotor} for the {@link SwerveModule}. - * - * @return {@link SwerveMotor} for the angle/steering motor of the module. - */ - public SwerveMotor getAngleMotor() { - return angleMotor; - } - - /** - * Get the drive {@link SwerveMotor} for the {@link SwerveModule}. - * - * @return {@link SwerveMotor} for the drive motor of the module. - */ - public SwerveMotor getDriveMotor() { - return driveMotor; - } - - /** - * Get the {@link SwerveAbsoluteEncoder} for the {@link SwerveModule}. - * - * @return {@link SwerveAbsoluteEncoder} for the swerve module. - */ - public SwerveAbsoluteEncoder getAbsoluteEncoder() { - return absoluteEncoder; - } - - /** - * Fetch the {@link SwerveModuleConfiguration} for the {@link SwerveModule} with the parsed - * configurations. - * - * @return {@link SwerveModuleConfiguration} for the {@link SwerveModule}. - */ - public SwerveModuleConfiguration getConfiguration() { - return configuration; - } - - /** - * Push absolute encoder offset in the memory of the encoder or controller. Also removes the - * internal angle offset. - */ - public void pushOffsetsToControllers() { - if (absoluteEncoder != null) { - if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset)) { - angleOffset = 0; - } else { - encoderOffsetWarning.set(true); - } - } else { - noEncoderWarning.set(true); - } - } - - /** - * Restore internal offset in YAGSL and either sets absolute encoder offset to 0 or restores old - * value. - */ - public void restoreInternalOffset() { - absoluteEncoder.setAbsoluteEncoderOffset(0); - angleOffset = configuration.angleOffset; - } - - /** - * Get if the last Absolute Encoder had a read issue, such as it does not exist. - * - * @return If the last Absolute Encoder had a read issue, or absolute encoder does not exist. - */ - public boolean getAbsoluteEncoderReadIssue() { - if (absoluteEncoder == null) { - return true; - } else { - return absoluteEncoder.readingError; - } - } - - /** Update data sent to {@link SmartDashboard}. */ - public void updateTelemetry() { - if (absoluteEncoder != null) { - SmartDashboard.putNumber( - "Module[" + configuration.name + "] Raw Absolute Encoder", - absoluteEncoder.getAbsolutePosition()); - } - SmartDashboard.putNumber( - "Module[" + configuration.name + "] Raw Angle Encoder", angleMotor.getPosition()); - SmartDashboard.putNumber( - "Module[" + configuration.name + "] Raw Drive Encoder", driveMotor.getPosition()); - SmartDashboard.putNumber( - "Module[" + configuration.name + "] Adjusted Absolute Encoder", getAbsolutePosition()); - SmartDashboard.putNumber( - "Module[" + configuration.name + "] Absolute Encoder Read Issue", - getAbsoluteEncoderReadIssue() ? 1 : 0); - } -} diff --git a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java deleted file mode 100644 index fef3f9c..0000000 --- a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java +++ /dev/null @@ -1,123 +0,0 @@ -package swervelib.encoders; - -import edu.wpi.first.wpilibj.AnalogInput; -import edu.wpi.first.wpilibj.RobotController; -import swervelib.telemetry.Alert; - -/** Swerve Absolute Encoder for Thrifty Encoders and other analog encoders. */ -public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder { - // Entire class inspired by 5010 - // Source: - // https://github.com/FRC5010/FRCLibrary/blob/main/FRC5010Example2023/src/main/java/frc/robot/FRC5010/sensors/AnalogInput5010.java - /** Encoder as Analog Input. */ - public AnalogInput encoder; - - /** Inversion state of the encoder. */ - private boolean inverted = false; - - /** An {@link Alert} for if the absolute encoder offset cannot be set. */ - private Alert cannotSetOffset; - - /** - * An {@link Alert} detailing how the analog absolute encoder may not report accurate velocities. - */ - private Alert inaccurateVelocities; - - /** - * Construct the Thrifty Encoder as a Swerve Absolute Encoder. - * - * @param encoder Encoder to construct. - */ - public AnalogAbsoluteEncoderSwerve(AnalogInput encoder) { - this.encoder = encoder; - cannotSetOffset = - new Alert( - "Encoders", - "Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" - + encoder.getChannel(), - Alert.AlertType.WARNING); - inaccurateVelocities = - new Alert( - "Encoders", - "The Analog Absolute encoder may not report accurate velocities!", - Alert.AlertType.WARNING_TRACE); - } - - /** - * Construct the Encoder given the analog input channel. - * - * @param channel Analog Input channel of which the encoder resides. - */ - public AnalogAbsoluteEncoderSwerve(int channel) { - this(new AnalogInput(channel)); - } - - /** Reset the encoder to factory defaults. */ - @Override - public void factoryDefault() { - // Do nothing - } - - /** Clear sticky faults on the encoder. */ - @Override - public void clearStickyFaults() { - // Do nothing - } - - /** - * Configure the absolute encoder to read from [0, 360) per second. - * - * @param inverted Whether the encoder is inverted. - */ - @Override - public void configure(boolean inverted) { - this.inverted = inverted; - } - - /** - * Get the absolute position of the encoder. - * - * @return Absolute position in degrees from [0, 360). - */ - @Override - public double getAbsolutePosition() { - return (inverted ? -1.0 : 1.0) - * (encoder.getAverageVoltage() / RobotController.getVoltage5V()) - * 360; - } - - /** - * Get the instantiated absolute encoder Object. - * - * @return Absolute encoder object. - */ - @Override - public Object getAbsoluteEncoder() { - return encoder; - } - - /** - * Cannot Set the offset of an Analog Absolute Encoder. - * - * @param offset the offset the Absolute Encoder uses as the zero point. - * @return Will always be false as setting the offset is unsupported of an Analog absolute - * encoder. - */ - @Override - public boolean setAbsoluteEncoderOffset(double offset) { - // Do Nothing - cannotSetOffset.set(true); - return false; - } - - /** - * Get the velocity in degrees/sec. - * - * @return velocity in degrees/sec. - */ - @Override - public double getVelocity() { - inaccurateVelocities.set(true); - return encoder.getValue(); - } -} diff --git a/src/main/java/swervelib/encoders/CANCoderSwerve.java b/src/main/java/swervelib/encoders/CANCoderSwerve.java deleted file mode 100644 index 7669947..0000000 --- a/src/main/java/swervelib/encoders/CANCoderSwerve.java +++ /dev/null @@ -1,190 +0,0 @@ -package swervelib.encoders; - -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.CANcoderConfigurator; -import com.ctre.phoenix6.configs.MagnetSensorConfigs; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; -import com.ctre.phoenix6.signals.MagnetHealthValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; -import swervelib.telemetry.Alert; - -/** Swerve Absolute Encoder for CTRE CANCoders. */ -public class CANCoderSwerve extends SwerveAbsoluteEncoder { - - /** CANCoder with WPILib sendable and support. */ - public CANcoder encoder; - - /** An {@link Alert} for if the CANCoder magnet field is less than ideal. */ - private Alert magnetFieldLessThanIdeal; - - /** An {@link Alert} for if the CANCoder reading is faulty. */ - private Alert readingFaulty; - - /** An {@link Alert} for if the CANCoder reading is faulty and the reading is ignored. */ - private Alert readingIgnored; - - /** An {@link Alert} for if the absolute encoder offset cannot be set. */ - private Alert cannotSetOffset; - - /** - * Initialize the CANCoder on the standard CANBus. - * - * @param id CAN ID. - */ - public CANCoderSwerve(int id) { - encoder = new CANcoder(id); - } - - /** - * Initialize the CANCoder on the CANivore. - * - * @param id CAN ID. - * @param canbus CAN bus to initialize it on. - */ - public CANCoderSwerve(int id, String canbus) { - encoder = new CANcoder(id, canbus); - magnetFieldLessThanIdeal = - new Alert( - "Encoders", - "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.", - Alert.AlertType.WARNING); - readingFaulty = - new Alert( - "Encoders", - "CANCoder " + encoder.getDeviceID() + " reading was faulty.", - Alert.AlertType.WARNING); - readingIgnored = - new Alert( - "Encoders", - "CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.", - Alert.AlertType.WARNING); - cannotSetOffset = - new Alert( - "Encoders", - "Failure to set CANCoder " + encoder.getDeviceID() + " Absolute Encoder Offset", - Alert.AlertType.WARNING); - } - - /** Reset the encoder to factory defaults. */ - @Override - public void factoryDefault() { - encoder.getConfigurator().apply(new CANcoderConfiguration()); - } - - /** Clear sticky faults on the encoder. */ - @Override - public void clearStickyFaults() { - encoder.clearStickyFaults(); - } - - /** - * Configure the absolute encoder to read from [0, 360) per second. - * - * @param inverted Whether the encoder is inverted. - */ - @Override - public void configure(boolean inverted) { - CANcoderConfigurator cfg = encoder.getConfigurator(); - MagnetSensorConfigs magnetSensorConfiguration = new MagnetSensorConfigs(); - cfg.refresh(magnetSensorConfiguration); - cfg.apply( - magnetSensorConfiguration - .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1) - .withSensorDirection( - inverted - ? SensorDirectionValue.Clockwise_Positive - : SensorDirectionValue.CounterClockwise_Positive)); - } - - /** - * Get the absolute position of the encoder. Sets {@link SwerveAbsoluteEncoder#readingError} on - * erroneous readings. - * - * @return Absolute position in degrees from [0, 360). - */ - @Override - public double getAbsolutePosition() { - readingError = false; - MagnetHealthValue strength = encoder.getMagnetHealth().getValue(); - - magnetFieldLessThanIdeal.set(strength != MagnetHealthValue.Magnet_Green); - if (strength == MagnetHealthValue.Magnet_Invalid || strength == MagnetHealthValue.Magnet_Red) { - readingError = true; - readingFaulty.set(true); - return 0; - } else { - readingFaulty.set(false); - } - - StatusSignal angle = encoder.getAbsolutePosition().refresh(); - - // Taken from democat's library. - // Source: - // https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/CanCoderFactoryBuilder.java#L51-L74 - for (int i = 0; i < maximumRetries; i++) { - if (angle.getStatus() == StatusCode.OK) { - break; - } - angle = angle.waitForUpdate(0.01); - } - if (angle.getStatus() != StatusCode.OK) { - readingError = true; - readingIgnored.set(true); - } else { - readingIgnored.set(false); - } - - return angle.getValue() * 360; - } - - /** - * Get the instantiated absolute encoder Object. - * - * @return Absolute encoder object. - */ - @Override - public Object getAbsoluteEncoder() { - return encoder; - } - - /** - * Sets the Absolute Encoder Offset within the CANcoder's Memory. - * - * @param offset the offset the Absolute Encoder uses as the zero point in degrees. - * @return if setting Absolute Encoder Offset was successful or not. - */ - @Override - public boolean setAbsoluteEncoderOffset(double offset) { - CANcoderConfigurator cfg = encoder.getConfigurator(); - MagnetSensorConfigs magCfg = new MagnetSensorConfigs(); - StatusCode error = cfg.refresh(magCfg); - if (error != StatusCode.OK) { - return false; - } - error = cfg.apply(magCfg.withMagnetOffset(offset / 360)); - cannotSetOffset.setText( - "Failure to set CANCoder " - + encoder.getDeviceID() - + " Absolute Encoder Offset Error: " - + error); - if (error == StatusCode.OK) { - cannotSetOffset.set(false); - return true; - } - cannotSetOffset.set(true); - return false; - } - - /** - * Get the velocity in degrees/sec. - * - * @return velocity in degrees/sec. - */ - @Override - public double getVelocity() { - return encoder.getVelocity().getValue() * 360; - } -} diff --git a/src/main/java/swervelib/encoders/CanAndCoderSwerve.java b/src/main/java/swervelib/encoders/CanAndCoderSwerve.java deleted file mode 100644 index bfb9fcc..0000000 --- a/src/main/java/swervelib/encoders/CanAndCoderSwerve.java +++ /dev/null @@ -1,86 +0,0 @@ -package swervelib.encoders; - -import com.reduxrobotics.sensors.canandcoder.Canandcoder; - -/** HELIUM {@link Canandcoder} from ReduxRobotics absolute encoder, attached through the CAN bus. */ -public class CanAndCoderSwerve extends SwerveAbsoluteEncoder { - - /** The {@link Canandcoder} representing the CANandCoder on the CAN bus. */ - public Canandcoder encoder; - - /** - * Create the {@link Canandcoder} - * - * @param canid The CAN ID whenever the CANandCoder is operating on the CANBus. - */ - public CanAndCoderSwerve(int canid) { - encoder = new Canandcoder(canid); - } - - /** - * Reset the encoder to factory defaults. - * - *

This will not clear the stored zero offset. - */ - @Override - public void factoryDefault() { - encoder.resetFactoryDefaults(false); - } - - /** Clear sticky faults on the encoder. */ - @Override - public void clearStickyFaults() { - encoder.clearStickyFaults(); - } - - /** - * Configure the Canandcoder to read from [0, 360) per second. - * - * @param inverted Whether the encoder is inverted. - */ - @Override - public void configure(boolean inverted) { - encoder.setSettings(new Canandcoder.Settings().setInvertDirection(inverted)); - } - - /** - * Get the absolute position of the encoder. - * - * @return Absolute position in degrees from [0, 360). - */ - @Override - public double getAbsolutePosition() { - return encoder.getAbsPosition() * 360; - } - - /** - * Get the instantiated absolute encoder Object. - * - * @return Absolute encoder object. - */ - @Override - public Object getAbsoluteEncoder() { - return encoder; - } - - /** - * Cannot set the offset of the Canandcoder. - * - * @param offset the offset the Absolute Encoder uses as the zero point. - * @return true if setting the zero point succeeded, false otherwise - */ - @Override - public boolean setAbsoluteEncoderOffset(double offset) { - return encoder.setSettings(new Canandcoder.Settings().setZeroOffset(offset)); - } - - /** - * Get the velocity in degrees/sec. - * - * @return velocity in degrees/sec. - */ - @Override - public double getVelocity() { - return encoder.getVelocity() * 360; - } -} diff --git a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java deleted file mode 100644 index 5b03800..0000000 --- a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java +++ /dev/null @@ -1,104 +0,0 @@ -package swervelib.encoders; - -import edu.wpi.first.wpilibj.DutyCycleEncoder; -import swervelib.telemetry.Alert; - -/** - * DutyCycle encoders such as "US Digital MA3 with PWM Output, the CTRE Mag Encoder, the Rev Hex - * Encoder, and the AM Mag Encoder." attached via a PWM lane. - * - *

Credits to - * p2reneker25 for building this. - */ -public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder { - - /** Duty Cycle Encoder. */ - private final DutyCycleEncoder encoder; - - /** Inversion state. */ - private boolean isInverted; - - /** An {@link Alert} for if the encoder cannot report accurate velocities. */ - private Alert inaccurateVelocities; - - /** - * Constructor for the PWM duty cycle encoder. - * - * @param pin PWM lane for the encoder. - */ - public PWMDutyCycleEncoderSwerve(int pin) { - encoder = new DutyCycleEncoder(pin); - inaccurateVelocities = - new Alert( - "Encoders", - "The PWM Duty Cycle encoder may not report accurate velocities!", - Alert.AlertType.WARNING_TRACE); - } - - /** - * Configure the inversion state of the encoder. - * - * @param inverted Whether the encoder is inverted. - */ - @Override - public void configure(boolean inverted) { - isInverted = inverted; - } - - /** - * Get the absolute position of the encoder. - * - * @return Absolute position in degrees from [0, 360). - */ - @Override - public double getAbsolutePosition() { - return (isInverted ? -1.0 : 1.0) * encoder.getAbsolutePosition() * 360; - } - - /** - * Get the encoder object. - * - * @return {@link DutyCycleEncoder} from the class. - */ - @Override - public Object getAbsoluteEncoder() { - return encoder; - } - - /** - * Get the velocity in degrees/sec. - * - * @return velocity in degrees/sec. - */ - @Override - public double getVelocity() { - inaccurateVelocities.set(true); - return encoder.get(); - } - - /** Reset the encoder to factory defaults. */ - @Override - public void factoryDefault() { - // Do nothing - } - - /** Clear sticky faults on the encoder. */ - @Override - public void clearStickyFaults() { - // Do nothing - } - - /** - * Sets the offset of the Encoder in the WPILib Encoder Library. - * - * @param offset the offset the Absolute Encoder uses as the zero point. - * @return Always true due to no external device commands. - */ - @Override - public boolean setAbsoluteEncoderOffset(double offset) { - encoder.setPositionOffset(offset); - - return true; - } -} diff --git a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java deleted file mode 100644 index 85dab57..0000000 --- a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java +++ /dev/null @@ -1,127 +0,0 @@ -package swervelib.encoders; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.REVLibError; -import com.revrobotics.SparkAnalogSensor; -import com.revrobotics.SparkAnalogSensor.Mode; -import java.util.function.Supplier; -import swervelib.motors.SwerveMotor; -import swervelib.telemetry.Alert; - -/** SparkMax absolute encoder, attached through the data port analog pin. */ -public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder { - - /** - * The {@link SparkAnalogSensor} representing the duty cycle encoder attached to the SparkMax - * analog port. - */ - public SparkAnalogSensor encoder; - - /** An {@link Alert} for if there is a failure configuring the encoder. */ - private Alert failureConfiguring; - - /** An {@link Alert} for if the absolute encoder does not support integrated offsets. */ - private Alert doesNotSupportIntegratedOffsets; - - /** - * Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link - * CANSparkMax} motor data port analog pin. - * - * @param motor Motor to create the encoder from. - */ - public SparkMaxAnalogEncoderSwerve(SwerveMotor motor) { - if (motor.getMotor() instanceof CANSparkMax) { - encoder = ((CANSparkMax) motor.getMotor()).getAnalog(Mode.kAbsolute); - } else { - throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); - } - failureConfiguring = - new Alert( - "Encoders", - "Failure configuring SparkMax Analog Encoder", - Alert.AlertType.WARNING_TRACE); - doesNotSupportIntegratedOffsets = - new Alert( - "Encoders", - "SparkMax Analog Sensors do not support integrated offsets", - Alert.AlertType.WARNING_TRACE); - } - - /** - * Run the configuration until it succeeds or times out. - * - * @param config Lambda supplier returning the error state. - */ - private void configureSparkMax(Supplier config) { - for (int i = 0; i < maximumRetries; i++) { - if (config.get() == REVLibError.kOk) { - return; - } - } - failureConfiguring.set(true); - } - - /** Reset the encoder to factory defaults. */ - @Override - public void factoryDefault() { - // Do nothing - } - - /** Clear sticky faults on the encoder. */ - @Override - public void clearStickyFaults() { - // Do nothing - } - - /** - * Configure the absolute encoder to read from [0, 360) per second. - * - * @param inverted Whether the encoder is inverted. - */ - @Override - public void configure(boolean inverted) { - encoder.setInverted(inverted); - } - - /** - * Get the absolute position of the encoder. - * - * @return Absolute position in degrees from [0, 360). - */ - @Override - public double getAbsolutePosition() { - return encoder.getPosition(); - } - - /** - * Get the instantiated absolute encoder Object. - * - * @return Absolute encoder object. - */ - @Override - public Object getAbsoluteEncoder() { - return encoder; - } - - /** - * Sets the Absolute Encoder offset at the Encoder Level. - * - * @param offset the offset the Absolute Encoder uses as the zero point. - * @return if setting Absolute Encoder Offset was successful or not. - */ - @Override - public boolean setAbsoluteEncoderOffset(double offset) { - doesNotSupportIntegratedOffsets.set(true); - return false; - } - - /** - * Get the velocity in degrees/sec. - * - * @return velocity in degrees/sec. - */ - @Override - public double getVelocity() { - return encoder.getVelocity(); - } -} diff --git a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java deleted file mode 100644 index b9d749f..0000000 --- a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java +++ /dev/null @@ -1,133 +0,0 @@ -package swervelib.encoders; - -import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkMax; -import com.revrobotics.REVLibError; -import com.revrobotics.SparkAbsoluteEncoder.Type; -import java.util.function.Supplier; -import swervelib.motors.SwerveMotor; -import swervelib.telemetry.Alert; - -/** SparkMax absolute encoder, attached through the data port. */ -public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder { - - /** The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax. */ - public AbsoluteEncoder encoder; - - /** An {@link Alert} for if there is a failure configuring the encoder. */ - private Alert failureConfiguring; - - /** An {@link Alert} for if there is a failure configuring the encoder offset. */ - private Alert offsetFailure; - - /** - * Create the {@link SparkMaxEncoderSwerve} object as a duty cycle from the {@link CANSparkMax} - * motor. - * - * @param motor Motor to create the encoder from. - * @param conversionFactor The conversion factor to set if the output is not from 0 to 360. - */ - public SparkMaxEncoderSwerve(SwerveMotor motor, int conversionFactor) { - if (motor.getMotor() instanceof CANSparkMax) { - encoder = ((CANSparkMax) motor.getMotor()).getAbsoluteEncoder(Type.kDutyCycle); - configureSparkMax(() -> encoder.setVelocityConversionFactor(conversionFactor)); - configureSparkMax(() -> encoder.setPositionConversionFactor(conversionFactor)); - } else { - throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); - } - failureConfiguring = - new Alert( - "Encoders", - "Failure configuring SparkMax Analog Encoder", - Alert.AlertType.WARNING_TRACE); - offsetFailure = - new Alert( - "Encoders", "Failure to set Absolute Encoder Offset", Alert.AlertType.WARNING_TRACE); - } - - /** - * Run the configuration until it succeeds or times out. - * - * @param config Lambda supplier returning the error state. - */ - private void configureSparkMax(Supplier config) { - for (int i = 0; i < maximumRetries; i++) { - if (config.get() == REVLibError.kOk) { - return; - } - } - failureConfiguring.set(true); - } - - /** Reset the encoder to factory defaults. */ - @Override - public void factoryDefault() { - // Do nothing - } - - /** Clear sticky faults on the encoder. */ - @Override - public void clearStickyFaults() { - // Do nothing - } - - /** - * Configure the absolute encoder to read from [0, 360) per second. - * - * @param inverted Whether the encoder is inverted. - */ - @Override - public void configure(boolean inverted) { - encoder.setInverted(inverted); - } - - /** - * Get the absolute position of the encoder. - * - * @return Absolute position in degrees from [0, 360). - */ - @Override - public double getAbsolutePosition() { - return encoder.getPosition(); - } - - /** - * Get the instantiated absolute encoder Object. - * - * @return Absolute encoder object. - */ - @Override - public Object getAbsoluteEncoder() { - return encoder; - } - - /** - * Sets the Absolute Encoder Offset inside of the SparkMax's Memory. - * - * @param offset the offset the Absolute Encoder uses as the zero point. - * @return if setting Absolute Encoder Offset was successful or not. - */ - @Override - public boolean setAbsoluteEncoderOffset(double offset) { - REVLibError error = null; - for (int i = 0; i < maximumRetries; i++) { - error = encoder.setZeroOffset(offset); - if (error == REVLibError.kOk) { - return true; - } - } - offsetFailure.setText("Failure to set Absolute Encoder Offset Error: " + error); - offsetFailure.set(true); - return false; - } - - /** - * Get the velocity in degrees/sec. - * - * @return velocity in degrees/sec. - */ - @Override - public double getVelocity() { - return encoder.getVelocity(); - } -} diff --git a/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java b/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java deleted file mode 100644 index 8261a85..0000000 --- a/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java +++ /dev/null @@ -1,59 +0,0 @@ -package swervelib.encoders; - -/** - * Swerve abstraction class to define a standard interface with absolute encoders for swerve - * modules.. - */ -public abstract class SwerveAbsoluteEncoder { - - /** - * The maximum amount of times the swerve encoder will attempt to configure itself if failures - * occur. - */ - public final int maximumRetries = 5; - - /** Last angle reading was faulty. */ - public boolean readingError = false; - - /** Reset the encoder to factory defaults. */ - public abstract void factoryDefault(); - - /** Clear sticky faults on the encoder. */ - public abstract void clearStickyFaults(); - - /** - * Configure the absolute encoder to read from [0, 360) per second. - * - * @param inverted Whether the encoder is inverted. - */ - public abstract void configure(boolean inverted); - - /** - * Get the absolute position of the encoder. - * - * @return Absolute position in degrees from [0, 360). - */ - public abstract double getAbsolutePosition(); - - /** - * Get the instantiated absolute encoder Object. - * - * @return Absolute encoder object. - */ - public abstract Object getAbsoluteEncoder(); - - /** - * Sets the Absolute Encoder offset at the Encoder Level. - * - * @param offset the offset the Absolute Encoder uses as the zero point. - * @return if setting Absolute Encoder Offset was successful or not. - */ - public abstract boolean setAbsoluteEncoderOffset(double offset); - - /** - * Get the velocity in degrees/sec. - * - * @return velocity in degrees/sec. - */ - public abstract double getVelocity(); -} diff --git a/src/main/java/swervelib/encoders/package-info.java b/src/main/java/swervelib/encoders/package-info.java deleted file mode 100644 index 6b93a2d..0000000 --- a/src/main/java/swervelib/encoders/package-info.java +++ /dev/null @@ -1,5 +0,0 @@ -/** - * Absolute encoders for the swerve drive, all implement {@link - * swervelib.encoders.SwerveAbsoluteEncoder}. - */ -package swervelib.encoders; diff --git a/src/main/java/swervelib/imu/ADIS16448Swerve.java b/src/main/java/swervelib/imu/ADIS16448Swerve.java deleted file mode 100644 index 5755326..0000000 --- a/src/main/java/swervelib/imu/ADIS16448Swerve.java +++ /dev/null @@ -1,92 +0,0 @@ -package swervelib.imu; - -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.ADIS16448_IMU; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import java.util.Optional; - -/** IMU Swerve class for the {@link ADIS16448_IMU} device. */ -public class ADIS16448Swerve extends SwerveIMU { - - /** {@link ADIS16448_IMU} device to read the current headings from. */ - private final ADIS16448_IMU imu; - - /** Offset for the ADIS16448. */ - private Rotation3d offset = new Rotation3d(); - - /** - * Construct the ADIS16448 imu and reset default configurations. Publish the gyro to the - * SmartDashboard. - */ - public ADIS16448Swerve() { - imu = new ADIS16448_IMU(); - factoryDefault(); - SmartDashboard.putData(imu); - } - - /** Reset IMU to factory default. */ - @Override - public void factoryDefault() { - offset = new Rotation3d(0, 0, 0); - imu.calibrate(); - } - - /** Clear sticky faults on IMU. */ - @Override - public void clearStickyFaults() { - // Do nothing. - } - - /** - * Set the gyro offset. - * - * @param offset gyro offset as a {@link Rotation3d}. - */ - public void setOffset(Rotation3d offset) { - this.offset = offset; - } - - /** - * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - public Rotation3d getRawRotation3d() { - return new Rotation3d( - Math.toRadians(-imu.getGyroAngleX()), - Math.toRadians(-imu.getGyroAngleY()), - Math.toRadians(-imu.getGyroAngleZ())); - } - - /** - * Fetch the {@link Rotation3d} from the IMU. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - @Override - public Rotation3d getRotation3d() { - return getRawRotation3d().minus(offset); - } - - /** - * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration - * isn't supported returns empty. - * - * @return {@link Translation3d} of the acceleration. - */ - @Override - public Optional getAccel() { - return Optional.of(new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ())); - } - - /** - * Get the instantiated IMU object. - * - * @return IMU object. - */ - @Override - public Object getIMU() { - return imu; - } -} diff --git a/src/main/java/swervelib/imu/ADIS16470Swerve.java b/src/main/java/swervelib/imu/ADIS16470Swerve.java deleted file mode 100644 index c2ae84e..0000000 --- a/src/main/java/swervelib/imu/ADIS16470Swerve.java +++ /dev/null @@ -1,91 +0,0 @@ -package swervelib.imu; - -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.ADIS16470_IMU; -import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import java.util.Optional; - -/** IMU Swerve class for the {@link ADIS16470_IMU} device. */ -public class ADIS16470Swerve extends SwerveIMU { - - /** {@link ADIS16470_IMU} device to read the current headings from. */ - private final ADIS16470_IMU imu; - - /** Offset for the ADIS16470. */ - private Rotation3d offset = new Rotation3d(); - - /** - * Construct the ADIS16470 imu and reset default configurations. Publish the gyro to the - * SmartDashboard. - */ - public ADIS16470Swerve() { - imu = new ADIS16470_IMU(); - offset = new Rotation3d(); - factoryDefault(); - SmartDashboard.putData(imu); - } - - /** Reset IMU to factory default. */ - @Override - public void factoryDefault() { - offset = new Rotation3d(0, 0, 0); - imu.calibrate(); - } - - /** Clear sticky faults on IMU. */ - @Override - public void clearStickyFaults() { - // Do nothing. - } - - /** - * Set the gyro offset. - * - * @param offset gyro offset as a {@link Rotation3d}. - */ - public void setOffset(Rotation3d offset) { - this.offset = offset; - } - - /** - * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - public Rotation3d getRawRotation3d() { - return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle(IMUAxis.kYaw))); - } - - /** - * Fetch the {@link Rotation3d} from the IMU. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - @Override - public Rotation3d getRotation3d() { - return getRawRotation3d().minus(offset); - } - - /** - * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration - * isn't supported returns empty. - * - * @return {@link Translation3d} of the acceleration as an {@link Optional}. - */ - @Override - public Optional getAccel() { - return Optional.of(new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ())); - } - - /** - * Get the instantiated IMU object. - * - * @return IMU object. - */ - @Override - public Object getIMU() { - return imu; - } -} diff --git a/src/main/java/swervelib/imu/ADXRS450Swerve.java b/src/main/java/swervelib/imu/ADXRS450Swerve.java deleted file mode 100644 index 88d1e69..0000000 --- a/src/main/java/swervelib/imu/ADXRS450Swerve.java +++ /dev/null @@ -1,89 +0,0 @@ -package swervelib.imu; - -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.ADXRS450_Gyro; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import java.util.Optional; - -/** IMU Swerve class for the {@link ADXRS450_Gyro} device. */ -public class ADXRS450Swerve extends SwerveIMU { - - /** {@link ADXRS450_Gyro} device to read the current headings from. */ - private final ADXRS450_Gyro imu; - - /** Offset for the ADXRS450. */ - private Rotation3d offset = new Rotation3d(); - - /** - * Construct the ADXRS450 imu and reset default configurations. Publish the gyro to the - * SmartDashboard. - */ - public ADXRS450Swerve() { - imu = new ADXRS450_Gyro(); - factoryDefault(); - SmartDashboard.putData(imu); - } - - /** Reset IMU to factory default. */ - @Override - public void factoryDefault() { - imu.calibrate(); - offset = new Rotation3d(0, 0, 0); // Math.toRadians(-imu.getAngle())); - } - - /** Clear sticky faults on IMU. */ - @Override - public void clearStickyFaults() { - // Do nothing. - } - - /** - * Set the gyro offset. - * - * @param offset gyro offset as a {@link Rotation3d}. - */ - public void setOffset(Rotation3d offset) { - this.offset = offset; - } - - /** - * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - public Rotation3d getRawRotation3d() { - return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle())); - } - - /** - * Fetch the {@link Rotation3d} from the IMU. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - @Override - public Rotation3d getRotation3d() { - return getRawRotation3d().minus(offset); - } - - /** - * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration - * isn't supported returns empty. - * - * @return {@link Translation3d} of the acceleration as an {@link Optional}. - */ - @Override - public Optional getAccel() { - return Optional.empty(); - } - - /** - * Get the instantiated IMU object. - * - * @return IMU object. - */ - @Override - public Object getIMU() { - return imu; - } -} diff --git a/src/main/java/swervelib/imu/AnalogGyroSwerve.java b/src/main/java/swervelib/imu/AnalogGyroSwerve.java deleted file mode 100644 index 83e0cb4..0000000 --- a/src/main/java/swervelib/imu/AnalogGyroSwerve.java +++ /dev/null @@ -1,94 +0,0 @@ -package swervelib.imu; - -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.AnalogGyro; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import java.util.Optional; - -/** Creates a IMU for {@link edu.wpi.first.wpilibj.AnalogGyro} devices, only uses yaw. */ -public class AnalogGyroSwerve extends SwerveIMU { - - /** Gyroscope object. */ - private final AnalogGyro gyro; - - /** Offset for the analog gyro. */ - private Rotation3d offset = new Rotation3d(); - - /** - * Analog port in which the gyroscope is connected. Can only be attached to analog ports 0 or 1. - * - * @param channel Analog port 0 or 1. - */ - public AnalogGyroSwerve(int channel) { - if (!(channel == 0 || channel == 1)) { - throw new RuntimeException( - "Analog Gyroscope must be attached to port 0 or 1 on the roboRIO.\n"); - } - gyro = new AnalogGyro(channel); - factoryDefault(); - SmartDashboard.putData(gyro); - } - - /** Reset IMU to factory default. */ - @Override - public void factoryDefault() { - gyro.calibrate(); - offset = new Rotation3d(0, 0, 0); - } - - /** Clear sticky faults on IMU. */ - @Override - public void clearStickyFaults() { - // Do nothing. - } - - /** - * Set the gyro offset. - * - * @param offset gyro offset as a {@link Rotation3d}. - */ - public void setOffset(Rotation3d offset) { - this.offset = offset; - } - - /** - * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - public Rotation3d getRawRotation3d() { - return new Rotation3d(0, 0, Math.toRadians(-gyro.getAngle())); - } - - /** - * Fetch the {@link Rotation3d} from the IMU. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - @Override - public Rotation3d getRotation3d() { - return getRawRotation3d().minus(offset); - } - - /** - * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration - * isn't supported returns empty. - * - * @return {@link Translation3d} of the acceleration as an {@link Optional}. - */ - @Override - public Optional getAccel() { - return Optional.empty(); - } - - /** - * Get the instantiated IMU object. - * - * @return IMU object. - */ - @Override - public Object getIMU() { - return gyro; - } -} diff --git a/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java deleted file mode 100644 index bf2cf04..0000000 --- a/src/main/java/swervelib/imu/NavXSwerve.java +++ /dev/null @@ -1,148 +0,0 @@ -package swervelib.imu; - -import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.I2C; -import edu.wpi.first.wpilibj.SPI; -import edu.wpi.first.wpilibj.SerialPort; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import java.util.Optional; -import swervelib.telemetry.Alert; - -/** Communicates with the NavX as the IMU. */ -public class NavXSwerve extends SwerveIMU { - - /** NavX IMU. */ - private AHRS gyro; - - /** Offset for the NavX. */ - private Rotation3d offset = new Rotation3d(); - - /** An {@link Alert} for if there is an error instantiating the NavX. */ - private Alert navXError; - - /** - * Constructor for the NavX swerve. - * - * @param port Serial Port to connect to. - */ - public NavXSwerve(SerialPort.Port port) { - navXError = new Alert("IMU", "Error instantiating NavX.", Alert.AlertType.ERROR_TRACE); - try { - /* Communicate w/navX-MXP via the MXP SPI Bus. */ - /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ - /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ - gyro = new AHRS(port); - factoryDefault(); - SmartDashboard.putData(gyro); - } catch (RuntimeException ex) { - navXError.setText("Error instantiating NavX: " + ex.getMessage()); - navXError.set(true); - } - } - - /** - * Constructor for the NavX swerve. - * - * @param port SPI Port to connect to. - */ - public NavXSwerve(SPI.Port port) { - try { - /* Communicate w/navX-MXP via the MXP SPI Bus. */ - /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ - /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ - gyro = new AHRS(port); - factoryDefault(); - SmartDashboard.putData(gyro); - } catch (RuntimeException ex) { - navXError.setText("Error instantiating NavX: " + ex.getMessage()); - navXError.set(true); - } - } - - /** - * Constructor for the NavX swerve. - * - * @param port I2C Port to connect to. - */ - public NavXSwerve(I2C.Port port) { - try { - /* Communicate w/navX-MXP via the MXP SPI Bus. */ - /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ - /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ - gyro = new AHRS(port); - factoryDefault(); - SmartDashboard.putData(gyro); - } catch (RuntimeException ex) { - navXError.setText("Error instantiating NavX: " + ex.getMessage()); - navXError.set(true); - } - } - - /** Reset IMU to factory default. */ - @Override - public void factoryDefault() { - // gyro.reset(); // Reported to be slow - offset = gyro.getRotation3d(); - } - - /** Clear sticky faults on IMU. */ - @Override - public void clearStickyFaults() {} - - /** - * Set the gyro offset. - * - * @param offset gyro offset as a {@link Rotation3d}. - */ - public void setOffset(Rotation3d offset) { - this.offset = offset; - } - - /** - * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - @Override - public Rotation3d getRawRotation3d() { - return gyro.getRotation3d(); - } - - /** - * Fetch the {@link Rotation3d} from the IMU. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - @Override - public Rotation3d getRotation3d() { - return gyro.getRotation3d().minus(offset); - } - - /** - * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration - * isn't supported returns empty. - * - * @return {@link Translation3d} of the acceleration as an {@link Optional}. - */ - @Override - public Optional getAccel() { - return Optional.of( - new Translation3d( - gyro.getWorldLinearAccelX(), - gyro.getWorldLinearAccelY(), - gyro.getWorldLinearAccelZ()) - .times(9.81)); - } - - /** - * Get the instantiated IMU object. - * - * @return IMU object. - */ - @Override - public Object getIMU() { - return gyro; - } -} diff --git a/src/main/java/swervelib/imu/Pigeon2Swerve.java b/src/main/java/swervelib/imu/Pigeon2Swerve.java deleted file mode 100644 index 4506214..0000000 --- a/src/main/java/swervelib/imu/Pigeon2Swerve.java +++ /dev/null @@ -1,125 +0,0 @@ -package swervelib.imu; - -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.Pigeon2Configuration; -import com.ctre.phoenix6.configs.Pigeon2Configurator; -import com.ctre.phoenix6.hardware.Pigeon2; -import edu.wpi.first.math.geometry.Quaternion; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import java.util.Optional; - -/** SwerveIMU interface for the Pigeon2 */ -public class Pigeon2Swerve extends SwerveIMU { - - /** Pigeon2 IMU device. */ - Pigeon2 imu; - - /** Offset for the Pigeon 2. */ - private Rotation3d offset = new Rotation3d(); - - /** - * Generate the SwerveIMU for pigeon. - * - * @param canid CAN ID for the pigeon - * @param canbus CAN Bus name the pigeon resides on. - */ - public Pigeon2Swerve(int canid, String canbus) { - imu = new Pigeon2(canid, canbus); - SmartDashboard.putData(imu); - } - - /** - * Generate the SwerveIMU for pigeon. - * - * @param canid CAN ID for the pigeon - */ - public Pigeon2Swerve(int canid) { - this(canid, ""); - } - - /** Reset IMU to factory default. */ - @Override - public void factoryDefault() { - Pigeon2Configurator cfg = imu.getConfigurator(); - Pigeon2Configuration config = new Pigeon2Configuration(); - - // Compass utilization causes readings to jump dramatically in some cases. - cfg.apply(config.Pigeon2Features.withEnableCompass(false)); - } - - /** Clear sticky faults on IMU. */ - @Override - public void clearStickyFaults() { - imu.clearStickyFaults(); - } - - /** - * Set the gyro offset. - * - * @param offset gyro offset as a {@link Rotation3d}. - */ - public void setOffset(Rotation3d offset) { - this.offset = offset; - } - - /** - * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - @Override - public Rotation3d getRawRotation3d() { - // TODO: Switch to suppliers. - StatusSignal w = imu.getQuatW(); - StatusSignal x = imu.getQuatX(); - StatusSignal y = imu.getQuatY(); - StatusSignal z = imu.getQuatZ(); - return new Rotation3d( - new Quaternion( - w.refresh().getValue(), - x.refresh().getValue(), - y.refresh().getValue(), - z.refresh().getValue())); - } - - /** - * Fetch the {@link Rotation3d} from the IMU. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - @Override - public Rotation3d getRotation3d() { - return getRawRotation3d().minus(offset); - } - - /** - * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration - * isn't supported returns empty. - * - * @return {@link Translation3d} of the acceleration as an {@link Optional}. - */ - @Override - public Optional getAccel() { - // TODO: Switch to suppliers. - StatusSignal xAcc = imu.getAccelerationX(); - StatusSignal yAcc = imu.getAccelerationX(); - StatusSignal zAcc = imu.getAccelerationX(); - - return Optional.of( - new Translation3d( - xAcc.refresh().getValue(), yAcc.refresh().getValue(), zAcc.refresh().getValue()) - .times(9.81 / 16384.0)); - } - - /** - * Get the instantiated IMU object. - * - * @return IMU object. - */ - @Override - public Object getIMU() { - return imu; - } -} diff --git a/src/main/java/swervelib/imu/PigeonSwerve.java b/src/main/java/swervelib/imu/PigeonSwerve.java deleted file mode 100644 index 2d5023b..0000000 --- a/src/main/java/swervelib/imu/PigeonSwerve.java +++ /dev/null @@ -1,95 +0,0 @@ -package swervelib.imu; - -import com.ctre.phoenix.sensors.WPI_PigeonIMU; -import edu.wpi.first.math.geometry.Quaternion; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import java.util.Optional; - -/** SwerveIMU interface for the Pigeon. */ -public class PigeonSwerve extends SwerveIMU { - - /** Pigeon v1 IMU device. */ - WPI_PigeonIMU imu; - - /** Offset for the Pigeon. */ - private Rotation3d offset = new Rotation3d(); - - /** - * Generate the SwerveIMU for pigeon. - * - * @param canid CAN ID for the pigeon, does not support CANBus. - */ - public PigeonSwerve(int canid) { - imu = new WPI_PigeonIMU(canid); - offset = new Rotation3d(); - SmartDashboard.putData(imu); - } - - /** Reset IMU to factory default. */ - @Override - public void factoryDefault() { - imu.configFactoryDefault(); - } - - /** Clear sticky faults on IMU. */ - @Override - public void clearStickyFaults() { - imu.clearStickyFaults(); - } - - /** - * Set the gyro offset. - * - * @param offset gyro offset as a {@link Rotation3d}. - */ - public void setOffset(Rotation3d offset) { - this.offset = offset; - } - - /** - * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - @Override - public Rotation3d getRawRotation3d() { - double[] wxyz = new double[4]; - imu.get6dQuaternion(wxyz); - return new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3])); - } - - /** - * Fetch the {@link Rotation3d} from the IMU. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - @Override - public Rotation3d getRotation3d() { - return getRawRotation3d().minus(offset); - } - - /** - * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration - * isn't supported returns empty. - * - * @return {@link Translation3d} of the acceleration as an {@link Optional}. - */ - @Override - public Optional getAccel() { - short[] initial = new short[3]; - imu.getBiasedAccelerometer(initial); - return Optional.of(new Translation3d(initial[0], initial[1], initial[2]).times(9.81 / 16384.0)); - } - - /** - * Get the instantiated IMU object. - * - * @return IMU object. - */ - @Override - public Object getIMU() { - return imu; - } -} diff --git a/src/main/java/swervelib/imu/SwerveIMU.java b/src/main/java/swervelib/imu/SwerveIMU.java deleted file mode 100644 index 9f23535..0000000 --- a/src/main/java/swervelib/imu/SwerveIMU.java +++ /dev/null @@ -1,51 +0,0 @@ -package swervelib.imu; - -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import java.util.Optional; - -/** Swerve IMU abstraction to define a standard interface with a swerve drive. */ -public abstract class SwerveIMU { - - /** Reset IMU to factory default. */ - public abstract void factoryDefault(); - - /** Clear sticky faults on IMU. */ - public abstract void clearStickyFaults(); - - /** - * Set the gyro offset. - * - * @param offset gyro offset as a {@link Rotation3d}. - */ - public abstract void setOffset(Rotation3d offset); - - /** - * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - public abstract Rotation3d getRawRotation3d(); - - /** - * Fetch the {@link Rotation3d} from the IMU. Robot relative. - * - * @return {@link Rotation3d} from the IMU. - */ - public abstract Rotation3d getRotation3d(); - - /** - * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration - * isn't supported returns empty. - * - * @return {@link Translation3d} of the acceleration as an {@link Optional}. - */ - public abstract Optional getAccel(); - - /** - * Get the instantiated IMU object. - * - * @return IMU object. - */ - public abstract Object getIMU(); -} diff --git a/src/main/java/swervelib/imu/package-info.java b/src/main/java/swervelib/imu/package-info.java deleted file mode 100644 index f32f846..0000000 --- a/src/main/java/swervelib/imu/package-info.java +++ /dev/null @@ -1,2 +0,0 @@ -/** IMUs used for controlling the robot heading. All implement {@link swervelib.imu.SwerveIMU}. */ -package swervelib.imu; diff --git a/src/main/java/swervelib/math/Matter.java b/src/main/java/swervelib/math/Matter.java deleted file mode 100644 index bcbfd6c..0000000 --- a/src/main/java/swervelib/math/Matter.java +++ /dev/null @@ -1,33 +0,0 @@ -package swervelib.math; - -import edu.wpi.first.math.geometry.Translation3d; - -/** Object with significant mass that needs to be taken into account. */ -public class Matter { - - /** Position in meters from robot center in 3d space. */ - public Translation3d position; - - /** Mass in kg of object. */ - public double mass; - - /** - * Construct an object representing some significant matter on the robot. - * - * @param position Position of the matter in meters. - * @param mass Mass in kg. - */ - public Matter(Translation3d position, double mass) { - this.mass = mass; - this.position = position; - } - - /** - * Get the center mass of the object. - * - * @return center mass = position * mass - */ - public Translation3d massMoment() { - return position.times(mass); - } -} diff --git a/src/main/java/swervelib/math/SwerveMath.java b/src/main/java/swervelib/math/SwerveMath.java deleted file mode 100644 index c59b188..0000000 --- a/src/main/java/swervelib/math/SwerveMath.java +++ /dev/null @@ -1,381 +0,0 @@ -package swervelib.math; - -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import java.util.List; -import swervelib.SwerveController; -import swervelib.SwerveModule; -import swervelib.parser.SwerveDriveConfiguration; -import swervelib.parser.SwerveModuleConfiguration; - -/** Mathematical functions which pertain to swerve drive. */ -public class SwerveMath { - - /** - * Calculate the meters per rotation for the integrated encoder. Calculation: (PI * WHEEL DIAMETER - * IN METERS) / (GEAR RATIO * ENCODER RESOLUTION) - * - * @param wheelDiameter Wheel diameter in meters. - * @param driveGearRatio The gear ratio of the drive motor. - * @param pulsePerRotation The number of encoder pulses per rotation. 1 if using an integrated - * encoder. - * @return Meters per rotation for the drive motor. - */ - public static double calculateMetersPerRotation( - double wheelDiameter, double driveGearRatio, double pulsePerRotation) { - return (Math.PI * wheelDiameter) / (driveGearRatio * pulsePerRotation); - } - - /** - * Calculate the meters per rotation for the integrated encoder. Calculation: (PI * WHEEL DIAMETER - * IN METERS) / (GEAR RATIO) - * - * @param wheelDiameter Wheel diameter in meters. - * @param driveGearRatio The gear ratio of the drive motor. - * @return Meters per rotation for the drive motor. - */ - public static double calculateMetersPerRotation(double wheelDiameter, double driveGearRatio) { - return calculateMetersPerRotation(wheelDiameter, driveGearRatio, 1); - } - - /** - * Normalize an angle to be within 0 to 360. - * - * @param angle Angle in degrees. - * @return Normalized angle in degrees. - */ - public static double normalizeAngle(double angle) { - Rotation2d angleRotation = Rotation2d.fromDegrees(angle); - return new Rotation2d(angleRotation.getCos(), angleRotation.getSin()).getDegrees(); - } - - /** - * Algebraically apply a deadband using a piece wise function. - * - * @param value value to apply deadband to. - * @param scaled Use algebra to determine deadband by starting the value at 0 past deadband. - * @param deadband The deadbnad to apply. - * @return Value with deadband applied. - */ - public static double applyDeadband(double value, boolean scaled, double deadband) { - value = Math.abs(value) > deadband ? value : 0; - return scaled - ? ((1 / (1 - deadband)) * (Math.abs(value) - deadband)) * Math.signum(value) - : value; - } - - /** - * Create the drive feedforward for swerve modules. - * - * @param optimalVoltage Optimal voltage to calculate kV (voltage/max Velocity) - * @param maxSpeed Maximum velocity in meters per second to use for the feed forward, should be as - * close to physical max as possible. - * @param wheelGripCoefficientOfFriction Wheel grip coefficient of friction for kA - * (voltage/(cof*9.81)) - * @return Drive feedforward for drive motor on a swerve module. - */ - public static SimpleMotorFeedforward createDriveFeedforward( - double optimalVoltage, double maxSpeed, double wheelGripCoefficientOfFriction) { - double kv = optimalVoltage / maxSpeed; - /// ^ Volt-seconds per meter (max voltage divided by max speed) - double ka = optimalVoltage / calculateMaxAcceleration(wheelGripCoefficientOfFriction); - /// ^ Volt-seconds^2 per meter (max voltage divided by max accel) - return new SimpleMotorFeedforward(0, kv, ka); - } - - /** - * Calculate the degrees per steering rotation for the integrated encoder. Encoder conversion - * values. Drive converts motor rotations to linear wheel distance and steering converts motor - * rotations to module azimuth. - * - * @param angleGearRatio The gear ratio of the steering motor. - * @param pulsePerRotation The number of pulses in a complete rotation for the encoder, 1 if - * integrated. - * @return Degrees per steering rotation for the angle motor. - */ - public static double calculateDegreesPerSteeringRotation( - double angleGearRatio, double pulsePerRotation) { - return 360 / (angleGearRatio * pulsePerRotation); - } - - /** - * Calculate the degrees per steering rotation for the integrated encoder. Encoder conversion - * values. Drive converts motor rotations to linear wheel distance and steering converts motor - * rotations to module azimuth. - * - * @param angleGearRatio The gear ratio of the steering motor. - * @return Degrees per steering rotation for the angle motor. - */ - public static double calculateDegreesPerSteeringRotation(double angleGearRatio) { - return calculateDegreesPerSteeringRotation(angleGearRatio, 1); - } - - /** - * Calculate the maximum angular velocity. - * - * @param maxSpeed Max speed of the robot in meters per second. - * @param furthestModuleX X of the furthest module in meters. - * @param furthestModuleY Y of the furthest module in meters. - * @return Maximum angular velocity in rad/s. - */ - public static double calculateMaxAngularVelocity( - double maxSpeed, double furthestModuleX, double furthestModuleY) { - return maxSpeed / new Rotation2d(furthestModuleX, furthestModuleY).getRadians(); - } - - /** - * Calculate the practical maximum acceleration of the robot using the wheel coefficient of - * friction. - * - * @param cof Coefficient of Friction of the wheel grip tape. - * @return Practical maximum acceleration in m/s/s. - */ - public static double calculateMaxAcceleration(double cof) { - return cof * 9.81; - } - - /** - * Calculate the maximum theoretical acceleration without friction. - * - * @param stallTorqueNm Stall torque of driving motor in nM. - * @param gearRatio Gear ratio for driving motor number of motor rotations until one wheel - * rotation. - * @param moduleCount Number of swerve modules. - * @param wheelDiameter Wheel diameter in meters. - * @param robotMass Mass of the robot in kg. - * @return Theoretical maximum acceleration in m/s/s. - */ - public static double calculateMaxAcceleration( - double stallTorqueNm, - double gearRatio, - double moduleCount, - double wheelDiameter, - double robotMass) { - return (stallTorqueNm * gearRatio * moduleCount) / ((wheelDiameter / 2) * robotMass); - } - - /** - * Calculates the maximum acceleration allowed in a direction without tipping the robot. Reads arm - * position from NetworkTables and is passed the direction in question. - * - * @param angle The direction in which to calculate max acceleration, as a Rotation2d. Note that - * this is robot-relative. - * @param matter Matter that the robot is composed of in kg. (Includes chassis) - * @param robotMass The weight of the robot in kg. (Including manipulators, etc). - * @param config The swerve drive configuration. - * @return Maximum acceleration allowed in the robot direction. - */ - private static double calcMaxAccel( - Rotation2d angle, List matter, double robotMass, SwerveDriveConfiguration config) { - // Calculate the vertical mass moment using the floor as the datum. This will be used later to - // calculate max acceleration - Translation3d centerMass = new Translation3d(); - for (Matter object : matter) { - centerMass = centerMass.plus(object.massMoment()); - } - Translation3d robotCG = centerMass.div(robotMass); - Translation2d horizontalCG = robotCG.toTranslation2d(); - - Translation2d projectedHorizontalCg = - new Translation2d( - (angle.getSin() * angle.getCos() * horizontalCG.getY()) - + (Math.pow(angle.getCos(), 2) * horizontalCG.getX()), - (angle.getSin() * angle.getCos() * horizontalCG.getX()) - + (Math.pow(angle.getSin(), 2) * horizontalCG.getY())); - - // Projects the edge of the wheelbase onto the direction line. Assumes the wheelbase is - // rectangular. - // Because a line is being projected, rather than a point, one of the coordinates of the - // projected point is - // already known. - Translation2d projectedWheelbaseEdge; - double angDeg = angle.getDegrees(); - if (angDeg <= 45 && angDeg >= -45) { - SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, true); - projectedWheelbaseEdge = - new Translation2d( - conf.moduleLocation.getX(), conf.moduleLocation.getX() * angle.getTan()); - } else if (135 >= angDeg && angDeg > 45) { - SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, true); - projectedWheelbaseEdge = - new Translation2d( - conf.moduleLocation.getY() / angle.getTan(), conf.moduleLocation.getY()); - } else if (-135 <= angDeg && angDeg < -45) { - SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, false); - projectedWheelbaseEdge = - new Translation2d( - conf.moduleLocation.getY() / angle.getTan(), conf.moduleLocation.getY()); - } else { - SwerveModuleConfiguration conf = getSwerveModule(config.modules, false, true); - projectedWheelbaseEdge = - new Translation2d( - conf.moduleLocation.getX(), conf.moduleLocation.getX() * angle.getTan()); - } - - double horizontalDistance = projectedHorizontalCg.plus(projectedWheelbaseEdge).getNorm(); - return 9.81 * horizontalDistance / robotCG.getZ(); - } - - /** - * Logical inverse of the Pose exponential from 254. Taken from team 3181. - * - * @param transform Pose to perform the log on. - * @return {@link Twist2d} of the transformed pose. - */ - public static Twist2d PoseLog(final Pose2d transform) { - - final double kEps = 1E-9; - final double dtheta = transform.getRotation().getRadians(); - final double half_dtheta = 0.5 * dtheta; - final double cos_minus_one = transform.getRotation().getCos() - 1.0; - double halftheta_by_tan_of_halfdtheta; - if (Math.abs(cos_minus_one) < kEps) { - halftheta_by_tan_of_halfdtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta; - } else { - halftheta_by_tan_of_halfdtheta = - -(half_dtheta * transform.getRotation().getSin()) / cos_minus_one; - } - final Translation2d translation_part = - transform - .getTranslation() - .rotateBy(new Rotation2d(halftheta_by_tan_of_halfdtheta, -half_dtheta)); - return new Twist2d(translation_part.getX(), translation_part.getY(), dtheta); - } - - /** - * Limits a commanded velocity to prevent exceeding the maximum acceleration given by {@link - * SwerveMath#calcMaxAccel}. Note that this takes and returns field-relative velocities. - * - * @param commandedVelocity The desired velocity - * @param fieldVelocity The velocity of the robot within a field relative state. - * @param robotPose The current pose of the robot. - * @param loopTime The time it takes to update the velocity in seconds. Note: this should - * include the 100ms that it takes for a SparkMax velocity to update. - * @param matter Matter that the robot is composed of with position in meters and mass in kg. - * @param robotMass The weight of the robot in kg. (Including manipulators, etc). - * @param config The swerve drive configuration. - * @return The limited velocity. This is either the commanded velocity, if attainable, or the - * closest attainable velocity. - */ - public static Translation2d limitVelocity( - Translation2d commandedVelocity, - ChassisSpeeds fieldVelocity, - Pose2d robotPose, - double loopTime, - double robotMass, - List matter, - SwerveDriveConfiguration config) { - // Get the robot's current field-relative velocity - Translation2d currentVelocity = SwerveController.getTranslation2d(fieldVelocity); - - // Calculate the commanded change in velocity by subtracting current velocity - // from commanded velocity - Translation2d deltaV = commandedVelocity.minus(currentVelocity); - - // Creates an acceleration vector with the direction of delta V and a magnitude - // of the maximum allowed acceleration in that direction - Translation2d maxAccel = - new Translation2d( - calcMaxAccel( - deltaV - // Rotates the velocity vector to convert from field-relative to robot-relative - .rotateBy(robotPose.getRotation().unaryMinus()) - .getAngle(), - matter, - robotMass, - config), - deltaV.getAngle()); - - // Calculate the maximum achievable velocity by the next loop cycle. - // delta V = Vf - Vi = at - Translation2d maxAchievableDeltaVelocity = maxAccel.times(loopTime); - - if (deltaV.getNorm() > maxAchievableDeltaVelocity.getNorm()) { - return maxAchievableDeltaVelocity.plus(currentVelocity); - } else { - // If the commanded velocity is attainable, use that. - return commandedVelocity; - } - } - - /** - * Get the fruthest module from center based on the module locations. - * - * @param modules Swerve module list. - * @param front True = furthest front, False = furthest back. - * @param left True = furthest left, False = furthest right. - * @return Module location which is the furthest from center and abides by parameters. - */ - public static SwerveModuleConfiguration getSwerveModule( - SwerveModule[] modules, boolean front, boolean left) { - Translation2d target = modules[0].configuration.moduleLocation, current, temp; - SwerveModuleConfiguration configuration = modules[0].configuration; - for (SwerveModule module : modules) { - current = module.configuration.moduleLocation; - temp = - front - ? (target.getY() >= current.getY() ? current : target) - : (target.getY() <= current.getY() ? current : target); - target = - left - ? (target.getX() >= temp.getX() ? temp : target) - : (target.getX() <= temp.getX() ? temp : target); - configuration = current.equals(target) ? module.configuration : configuration; - } - return configuration; - } - - /** - * Put an angle within the 360 deg scope of a reference. For example, given a scope reference of - * 756 degrees, assumes the full scope is (720-1080), and places an angle of 22 degrees into it, - * returning 742 deg. - * - * @param scopeReference Current Angle (deg) - * @param newAngle Target Angle (deg) - * @return Closest angle within scope (deg) - */ - public static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) { - double lowerBound; - double upperBound; - double lowerOffset = scopeReference % 360; - if (lowerOffset >= 0) { - lowerBound = scopeReference - lowerOffset; - upperBound = scopeReference + (360 - lowerOffset); - } else { - upperBound = scopeReference - lowerOffset; - lowerBound = scopeReference - (360 + lowerOffset); - } - while (newAngle < lowerBound) { - newAngle += 360; - } - while (newAngle > upperBound) { - newAngle -= 360; - } - if (newAngle - scopeReference > 180) { - newAngle -= 360; - } else if (newAngle - scopeReference < -180) { - newAngle += 360; - } - return newAngle; - } - - /** - * Perform anti-jitter within modules if the speed requested is too low. - * - * @param moduleState Current {@link SwerveModuleState} requested. - * @param lastModuleState Previous {@link SwerveModuleState} used. - * @param maxSpeed Maximum speed of the modules. - */ - public static void antiJitter( - SwerveModuleState moduleState, SwerveModuleState lastModuleState, double maxSpeed) { - if (Math.abs(moduleState.speedMetersPerSecond) <= (maxSpeed * 0.01)) { - moduleState.angle = lastModuleState.angle; - } - } -} diff --git a/src/main/java/swervelib/math/package-info.java b/src/main/java/swervelib/math/package-info.java deleted file mode 100644 index 633b280..0000000 --- a/src/main/java/swervelib/math/package-info.java +++ /dev/null @@ -1,6 +0,0 @@ -/** - * Mathematics for swerve drives. Original second order kinematics was developed by Team 3181 here. - * - */ -package swervelib.math; diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java deleted file mode 100644 index 9e24ed8..0000000 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ /dev/null @@ -1,399 +0,0 @@ -package swervelib.motors; - -import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkBase.ControlType; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkLowLevel.PeriodicFrame; -import com.revrobotics.CANSparkMax; -import com.revrobotics.MotorFeedbackSensor; -import com.revrobotics.REVLibError; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkAnalogSensor; -import com.revrobotics.SparkPIDController; -import java.util.function.Supplier; -import swervelib.encoders.SwerveAbsoluteEncoder; -import swervelib.parser.PIDFConfig; -import swervelib.telemetry.Alert; -import swervelib.telemetry.SwerveDriveTelemetry; - -/** An implementation of {@link CANSparkFlex} as a {@link SwerveMotor}. */ -public class SparkFlexSwerve extends SwerveMotor { - - /** SparkMAX Instance. */ - public CANSparkFlex motor; - - /** Integrated encoder. */ - public RelativeEncoder encoder; - - /** Absolute encoder attached to the SparkMax (if exists) */ - public SwerveAbsoluteEncoder absoluteEncoder; - - /** Closed-loop PID controller. */ - public SparkPIDController pid; - - /** Factory default already occurred. */ - private boolean factoryDefaultOccurred = false; - - /** An {@link Alert} for if there is an error configuring the motor. */ - private Alert failureConfiguring; - - /** - * An {@link Alert} for if the absolute encoder's offset is set in the json instead of the - * hardware client. - */ - private Alert absoluteEncoderOffsetWarning; - - /** - * Initialize the swerve motor. - * - * @param motor The SwerveMotor as a SparkFlex object. - * @param isDriveMotor Is the motor being initialized a drive motor? - */ - public SparkFlexSwerve(CANSparkFlex motor, boolean isDriveMotor) { - this.motor = motor; - this.isDriveMotor = isDriveMotor; - factoryDefaults(); - clearStickyFaults(); - - encoder = motor.getEncoder(); - pid = motor.getPIDController(); - pid.setFeedbackDevice( - encoder); // Configure feedback of the PID controller as the integrated encoder. - - // Spin off configurations in a different thread. - // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents - // feedback. - failureConfiguring = - new Alert( - "Motors", - "Failure configuring motor " + motor.getDeviceId(), - Alert.AlertType.WARNING_TRACE); - absoluteEncoderOffsetWarning = - new Alert( - "Motors", - "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the " - + "absoluteEncoderOffset in the Swerve Module JSON!", - Alert.AlertType.WARNING); - } - - /** - * Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor. - * - * @param id CAN ID of the SparkMax. - * @param isDriveMotor Is the motor being initialized a drive motor? - */ - public SparkFlexSwerve(int id, boolean isDriveMotor) { - this(new CANSparkFlex(id, MotorType.kBrushless), isDriveMotor); - } - - /** - * Run the configuration until it succeeds or times out. - * - * @param config Lambda supplier returning the error state. - */ - private void configureSparkFlex(Supplier config) { - for (int i = 0; i < maximumRetries; i++) { - if (config.get() == REVLibError.kOk) { - return; - } - } - failureConfiguring.set(true); - } - - /** - * Set the voltage compensation for the swerve module motor. - * - * @param nominalVoltage Nominal voltage for operation to output to. - */ - @Override - public void setVoltageCompensation(double nominalVoltage) { - configureSparkFlex(() -> motor.enableVoltageCompensation(nominalVoltage)); - } - - /** - * Set the current limit for the swerve drive motor, remember this may cause jumping if used in - * conjunction with voltage compensation. This is useful to protect the motor from current spikes. - * - * @param currentLimit Current limit in AMPS at free speed. - */ - @Override - public void setCurrentLimit(int currentLimit) { - configureSparkFlex(() -> motor.setSmartCurrentLimit(currentLimit)); - } - - /** - * Set the maximum rate the open/closed loop output can change by. - * - * @param rampRate Time in seconds to go from 0 to full throttle. - */ - @Override - public void setLoopRampRate(double rampRate) { - configureSparkFlex(() -> motor.setOpenLoopRampRate(rampRate)); - configureSparkFlex(() -> motor.setClosedLoopRampRate(rampRate)); - } - - /** - * Get the motor object from the module. - * - * @return Motor object. - */ - @Override - public Object getMotor() { - return motor; - } - - /** - * Queries whether the absolute encoder is directly attached to the motor controller. - * - * @return connected absolute encoder state. - */ - @Override - public boolean isAttachedAbsoluteEncoder() { - return absoluteEncoder != null; - } - - /** Configure the factory defaults. */ - @Override - public void factoryDefaults() { - if (!factoryDefaultOccurred) { - configureSparkFlex(motor::restoreFactoryDefaults); - factoryDefaultOccurred = true; - } - } - - /** Clear the sticky faults on the motor controller. */ - @Override - public void clearStickyFaults() { - configureSparkFlex(motor::clearFaults); - } - - /** - * Set the absolute encoder to be a compatible absolute encoder. - * - * @param encoder The encoder to use. - * @return The {@link SwerveMotor} for easy instantiation. - */ - @Override - public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { - if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) { - absoluteEncoderOffsetWarning.set(true); - absoluteEncoder = encoder; - configureSparkFlex( - () -> pid.setFeedbackDevice((MotorFeedbackSensor) absoluteEncoder.getAbsoluteEncoder())); - } - return this; - } - - /** - * Configure the integrated encoder for the swerve module. Sets the conversion factors for - * position and velocity. - * - * @param positionConversionFactor The conversion factor to apply. - */ - @Override - public void configureIntegratedEncoder(double positionConversionFactor) { - if (absoluteEncoder == null) { - configureSparkFlex(() -> encoder.setPositionConversionFactor(positionConversionFactor)); - configureSparkFlex(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60)); - - // Taken from - // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67 - configureCANStatusFrames(10, 20, 20, 500, 500); - } else { - configureSparkFlex( - () -> { - if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) { - return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()) - .setPositionConversionFactor(positionConversionFactor); - } else { - return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()) - .setPositionConversionFactor(positionConversionFactor); - } - }); - configureSparkFlex( - () -> { - if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) { - return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()) - .setVelocityConversionFactor(positionConversionFactor / 60); - } else { - return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()) - .setVelocityConversionFactor(positionConversionFactor / 60); - } - }); - } - } - - /** - * Configure the PIDF values for the closed loop controller. - * - * @param config Configuration class holding the PIDF values. - */ - @Override - public void configurePIDF(PIDFConfig config) { - // int pidSlot = - // isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : - // SparkMAX_slotIdx.Position.ordinal(); - int pidSlot = 0; - configureSparkFlex(() -> pid.setP(config.p, pidSlot)); - configureSparkFlex(() -> pid.setI(config.i, pidSlot)); - configureSparkFlex(() -> pid.setD(config.d, pidSlot)); - configureSparkFlex(() -> pid.setFF(config.f, pidSlot)); - configureSparkFlex(() -> pid.setIZone(config.iz, pidSlot)); - configureSparkFlex(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot)); - } - - /** - * Configure the PID wrapping for the position closed loop controller. - * - * @param minInput Minimum PID input. - * @param maxInput Maximum PID input. - */ - @Override - public void configurePIDWrapping(double minInput, double maxInput) { - configureSparkFlex(() -> pid.setPositionPIDWrappingEnabled(true)); - configureSparkFlex(() -> pid.setPositionPIDWrappingMinInput(minInput)); - configureSparkFlex(() -> pid.setPositionPIDWrappingMaxInput(maxInput)); - } - - /** - * Set the CAN status frames. - * - * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower - * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current - * @param CANStatus2 Motor Position - * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position - * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position - */ - public void configureCANStatusFrames( - int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4) { - configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0)); - configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1)); - configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2)); - configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3)); - configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4)); - // TODO: Configure Status Frame 5 and 6 if necessary - // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces - } - - /** - * Set the idle mode. - * - * @param isBrakeMode Set the brake mode. - */ - @Override - public void setMotorBrake(boolean isBrakeMode) { - configureSparkFlex(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast)); - } - - /** - * Set the motor to be inverted. - * - * @param inverted State of inversion. - */ - @Override - public void setInverted(boolean inverted) { - motor.setInverted(inverted); - } - - /** Save the configurations from flash to EEPROM. */ - @Override - public void burnFlash() { - try { - Thread.sleep(200); - } catch (Exception e) { - } - configureSparkFlex(() -> motor.burnFlash()); - } - - /** - * Set the percentage output. - * - * @param percentOutput percent out for the motor controller. - */ - @Override - public void set(double percentOutput) { - motor.set(percentOutput); - } - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in MPS or Angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - */ - @Override - public void setReference(double setpoint, double feedforward) { - boolean possibleBurnOutIssue = true; - // int pidSlot = - // isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : - // SparkMAX_slotIdx.Position.ordinal(); - int pidSlot = 0; - - if (isDriveMotor) { - configureSparkFlex( - () -> pid.setReference(setpoint, ControlType.kVelocity, pidSlot, feedforward)); - } else { - configureSparkFlex( - () -> pid.setReference(setpoint, ControlType.kPosition, pidSlot, feedforward)); - if (SwerveDriveTelemetry.isSimulation) { - encoder.setPosition(setpoint); - } - } - } - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in meters per second or angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - * @param position Only used on the angle motor, the position of the motor in degrees. - */ - @Override - public void setReference(double setpoint, double feedforward, double position) { - setReference(setpoint, feedforward); - } - - /** - * Get the velocity of the integrated encoder. - * - * @return velocity - */ - @Override - public double getVelocity() { - return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity(); - } - - /** - * Get the position of the integrated encoder. - * - * @return Position - */ - @Override - public double getPosition() { - return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getAbsolutePosition(); - } - - /** - * Set the integrated encoder position. - * - * @param position Integrated encoder position. - */ - @Override - public void setPosition(double position) { - if (absoluteEncoder == null) { - configureSparkFlex(() -> encoder.setPosition(position)); - } - } - - /** REV Slots for PID configuration. */ - enum SparkMAX_slotIdx { - /** Slot 1, used for position PID's. */ - Position, - /** Slot 2, used for velocity PID's. */ - Velocity, - /** Slot 3, used arbitrarily. (Documentation show simulations). */ - Simulation - } -} diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java deleted file mode 100644 index 46447d6..0000000 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ /dev/null @@ -1,414 +0,0 @@ -package swervelib.motors; - -import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkBase.ControlType; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkLowLevel.PeriodicFrame; -import com.revrobotics.CANSparkMax; -import com.revrobotics.REVLibError; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkMaxAlternateEncoder; -import com.revrobotics.SparkPIDController; -import com.revrobotics.SparkRelativeEncoder.Type; -import java.util.function.Supplier; -import swervelib.encoders.SwerveAbsoluteEncoder; -import swervelib.parser.PIDFConfig; -import swervelib.telemetry.Alert; - -/** Brushed motor control with SparkMax. */ -public class SparkMaxBrushedMotorSwerve extends SwerveMotor { - - /** SparkMAX Instance. */ - public CANSparkMax motor; - - /** Absolute encoder attached to the SparkMax (if exists) */ - public AbsoluteEncoder absoluteEncoder; - - /** Integrated encoder. */ - public RelativeEncoder encoder; - - /** Closed-loop PID controller. */ - public SparkPIDController pid; - - /** Factory default already occurred. */ - private boolean factoryDefaultOccurred = false; - - /** An {@link Alert} for if the motor has no encoder. */ - private Alert noEncoderAlert; - - /** An {@link Alert} for if there is an error configuring the motor. */ - private Alert failureConfiguringAlert; - - /** An {@link Alert} for if the motor has no encoder defined. */ - private Alert noEncoderDefinedAlert; - - /** - * Initialize the swerve motor. - * - * @param motor The SwerveMotor as a SparkMax object. - * @param isDriveMotor Is the motor being initialized a drive motor? - * @param encoderType {@link Type} of encoder to use for the {@link CANSparkMax} device. - * @param countsPerRevolution The number of encoder pulses for the {@link Type} encoder per - * revolution. - * @param useDataPortQuadEncoder Use the encoder attached to the data port of the spark max for a - * quadrature encoder. - */ - public SparkMaxBrushedMotorSwerve( - CANSparkMax motor, - boolean isDriveMotor, - Type encoderType, - int countsPerRevolution, - boolean useDataPortQuadEncoder) { - // Drive motors **MUST** have an encoder attached. - if (isDriveMotor && encoderType == Type.kNoSensor) { - noEncoderAlert.set(true); - throw new RuntimeException( - "Cannot use SparkMAX as a drive motor without an encoder attached."); - } - - // Hall encoders can be used as quadrature encoders. - if (encoderType == Type.kHallSensor) { - encoderType = Type.kQuadrature; - } - - this.motor = motor; - this.isDriveMotor = isDriveMotor; - - factoryDefaults(); - clearStickyFaults(); - - // Get the onboard PID controller. - pid = motor.getPIDController(); - - // If there is a sensor attached to the data port or encoder port set the relative encoder. - if (isDriveMotor || (encoderType != Type.kNoSensor || useDataPortQuadEncoder)) { - this.encoder = - useDataPortQuadEncoder - ? motor.getAlternateEncoder( - SparkMaxAlternateEncoder.Type.kQuadrature, countsPerRevolution) - : motor.getEncoder(encoderType, countsPerRevolution); - - // Configure feedback of the PID controller as the integrated encoder. - configureSparkMax(() -> pid.setFeedbackDevice(encoder)); - } - // Spin off configurations in a different thread. - // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented it out because it prevents - // feedback. - - noEncoderAlert = - new Alert("Motors", "Cannot use motor without encoder.", Alert.AlertType.ERROR_TRACE); - failureConfiguringAlert = - new Alert( - "Motors", - "Failure configuring motor " + motor.getDeviceId(), - Alert.AlertType.WARNING_TRACE); - noEncoderDefinedAlert = - new Alert( - "Motors", - "An encoder MUST be defined to work with a SparkMAX", - Alert.AlertType.ERROR_TRACE); - } - - /** - * Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor. - * - * @param id CAN ID of the SparkMax. - * @param isDriveMotor Is the motor being initialized a drive motor? - * @param encoderType {@link Type} of encoder to use for the {@link CANSparkMax} device. - * @param countsPerRevolution The number of encoder pulses for the {@link Type} encoder per - * revolution. - * @param useDataPortQuadEncoder Use the encoder attached to the data port of the spark max for a - * quadrature encoder. - */ - public SparkMaxBrushedMotorSwerve( - int id, - boolean isDriveMotor, - Type encoderType, - int countsPerRevolution, - boolean useDataPortQuadEncoder) { - this( - new CANSparkMax(id, MotorType.kBrushed), - isDriveMotor, - encoderType, - countsPerRevolution, - useDataPortQuadEncoder); - } - - /** - * Run the configuration until it succeeds or times out. - * - * @param config Lambda supplier returning the error state. - */ - private void configureSparkMax(Supplier config) { - for (int i = 0; i < maximumRetries; i++) { - if (config.get() == REVLibError.kOk) { - return; - } - } - failureConfiguringAlert.set(true); - } - - /** - * Set the voltage compensation for the swerve module motor. - * - * @param nominalVoltage Nominal voltage for operation to output to. - */ - @Override - public void setVoltageCompensation(double nominalVoltage) { - configureSparkMax(() -> motor.enableVoltageCompensation(nominalVoltage)); - } - - /** - * Set the current limit for the swerve drive motor, remember this may cause jumping if used in - * conjunction with voltage compensation. This is useful to protect the motor from current spikes. - * - * @param currentLimit Current limit in AMPS at free speed. - */ - @Override - public void setCurrentLimit(int currentLimit) { - configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit)); - } - - /** - * Set the maximum rate the open/closed loop output can change by. - * - * @param rampRate Time in seconds to go from 0 to full throttle. - */ - @Override - public void setLoopRampRate(double rampRate) { - configureSparkMax(() -> motor.setOpenLoopRampRate(rampRate)); - configureSparkMax(() -> motor.setClosedLoopRampRate(rampRate)); - } - - /** - * Get the motor object from the module. - * - * @return Motor object. - */ - @Override - public Object getMotor() { - return motor; - } - - /** - * Queries whether the absolute encoder is directly attached to the motor controller. - * - * @return connected absolute encoder state. - */ - @Override - public boolean isAttachedAbsoluteEncoder() { - return absoluteEncoder != null; - } - - /** Configure the factory defaults. */ - @Override - public void factoryDefaults() { - if (!factoryDefaultOccurred) { - configureSparkMax(motor::restoreFactoryDefaults); - factoryDefaultOccurred = true; - } - } - - /** Clear the sticky faults on the motor controller. */ - @Override - public void clearStickyFaults() { - configureSparkMax(motor::clearFaults); - } - - /** - * Set the absolute encoder to be a compatible absolute encoder. - * - * @param encoder The encoder to use. - * @return The {@link SwerveMotor} for easy instantiation. - */ - @Override - public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { - if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) { - absoluteEncoder = (AbsoluteEncoder) encoder.getAbsoluteEncoder(); - configureSparkMax(() -> pid.setFeedbackDevice(absoluteEncoder)); - } - if (absoluteEncoder == null && this.encoder == null) { - noEncoderDefinedAlert.set(true); - throw new RuntimeException("An encoder MUST be defined to work with a SparkMAX"); - } - return this; - } - - /** - * Configure the integrated encoder for the swerve module. Sets the conversion factors for - * position and velocity. - * - * @param positionConversionFactor The conversion factor to apply. - */ - @Override - public void configureIntegratedEncoder(double positionConversionFactor) { - if (absoluteEncoder == null) { - configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor)); - configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60)); - - // Taken from - // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67 - configureCANStatusFrames(10, 20, 20, 500, 500); - } else { - configureSparkMax( - () -> absoluteEncoder.setPositionConversionFactor(positionConversionFactor)); - configureSparkMax( - () -> absoluteEncoder.setVelocityConversionFactor(positionConversionFactor / 60)); - } - } - - /** - * Configure the PIDF values for the closed loop controller. - * - * @param config Configuration class holding the PIDF values. - */ - @Override - public void configurePIDF(PIDFConfig config) { - // int pidSlot = - // isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : - // SparkMAX_slotIdx.Position.ordinal(); - int pidSlot = 0; - - configureSparkMax(() -> pid.setP(config.p, pidSlot)); - configureSparkMax(() -> pid.setI(config.i, pidSlot)); - configureSparkMax(() -> pid.setD(config.d, pidSlot)); - configureSparkMax(() -> pid.setFF(config.f, pidSlot)); - configureSparkMax(() -> pid.setIZone(config.iz, pidSlot)); - configureSparkMax(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot)); - } - - /** - * Configure the PID wrapping for the position closed loop controller. - * - * @param minInput Minimum PID input. - * @param maxInput Maximum PID input. - */ - @Override - public void configurePIDWrapping(double minInput, double maxInput) { - configureSparkMax(() -> pid.setPositionPIDWrappingEnabled(true)); - configureSparkMax(() -> pid.setPositionPIDWrappingMinInput(minInput)); - configureSparkMax(() -> pid.setPositionPIDWrappingMaxInput(maxInput)); - } - - /** - * Set the CAN status frames. - * - * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower - * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current - * @param CANStatus2 Motor Position - * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position - * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position - */ - public void configureCANStatusFrames( - int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4) { - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4)); - // TODO: Configure Status Frame 5 and 6 if necessary - // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces - } - - /** - * Set the idle mode. - * - * @param isBrakeMode Set the brake mode. - */ - @Override - public void setMotorBrake(boolean isBrakeMode) { - configureSparkMax(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast)); - } - - /** - * Set the motor to be inverted. - * - * @param inverted State of inversion. - */ - @Override - public void setInverted(boolean inverted) { - motor.setInverted(inverted); - } - - /** Save the configurations from flash to EEPROM. */ - @Override - public void burnFlash() { - configureSparkMax(() -> motor.burnFlash()); - } - - /** - * Set the percentage output. - * - * @param percentOutput percent out for the motor controller. - */ - @Override - public void set(double percentOutput) { - motor.set(percentOutput); - } - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in MPS or Angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - */ - @Override - public void setReference(double setpoint, double feedforward) { - // int pidSlot = - // isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : - // SparkMAX_slotIdx.Position.ordinal(); - int pidSlot = 0; - configureSparkMax( - () -> - pid.setReference( - setpoint, - isDriveMotor ? ControlType.kVelocity : ControlType.kPosition, - pidSlot, - feedforward)); - } - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in meters per second or angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - * @param position Only used on the angle motor, the position of the motor in degrees. - */ - @Override - public void setReference(double setpoint, double feedforward, double position) { - setReference(setpoint, feedforward); - } - - /** - * Get the velocity of the integrated encoder. - * - * @return velocity - */ - @Override - public double getVelocity() { - return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity(); - } - - /** - * Get the position of the integrated encoder. - * - * @return Position - */ - @Override - public double getPosition() { - return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getPosition(); - } - - /** - * Set the integrated encoder position. - * - * @param position Integrated encoder position. - */ - @Override - public void setPosition(double position) { - if (absoluteEncoder == null) { - configureSparkMax(() -> encoder.setPosition(position)); - } - } -} diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java deleted file mode 100644 index ecb1db3..0000000 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ /dev/null @@ -1,391 +0,0 @@ -package swervelib.motors; - -import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkBase.ControlType; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkLowLevel.PeriodicFrame; -import com.revrobotics.CANSparkMax; -import com.revrobotics.MotorFeedbackSensor; -import com.revrobotics.REVLibError; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkAnalogSensor; -import com.revrobotics.SparkPIDController; -import edu.wpi.first.wpilibj.DriverStation; -import java.util.function.Supplier; -import swervelib.encoders.SwerveAbsoluteEncoder; -import swervelib.parser.PIDFConfig; -import swervelib.telemetry.SwerveDriveTelemetry; - -/** An implementation of {@link CANSparkMax} as a {@link SwerveMotor}. */ -public class SparkMaxSwerve extends SwerveMotor { - - /** SparkMAX Instance. */ - public CANSparkMax motor; - - /** Integrated encoder. */ - public RelativeEncoder encoder; - - /** Absolute encoder attached to the SparkMax (if exists) */ - public SwerveAbsoluteEncoder absoluteEncoder; - - /** Closed-loop PID controller. */ - public SparkPIDController pid; - - /** Factory default already occurred. */ - private boolean factoryDefaultOccurred = false; - - /** - * Initialize the swerve motor. - * - * @param motor The SwerveMotor as a SparkMax object. - * @param isDriveMotor Is the motor being initialized a drive motor? - */ - public SparkMaxSwerve(CANSparkMax motor, boolean isDriveMotor) { - this.motor = motor; - this.isDriveMotor = isDriveMotor; - factoryDefaults(); - clearStickyFaults(); - - encoder = motor.getEncoder(); - pid = motor.getPIDController(); - pid.setFeedbackDevice( - encoder); // Configure feedback of the PID controller as the integrated encoder. - - // Spin off configurations in a different thread. - // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents - // feedback. - } - - /** - * Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor. - * - * @param id CAN ID of the SparkMax. - * @param isDriveMotor Is the motor being initialized a drive motor? - */ - public SparkMaxSwerve(int id, boolean isDriveMotor) { - this(new CANSparkMax(id, MotorType.kBrushless), isDriveMotor); - } - - /** - * Run the configuration until it succeeds or times out. - * - * @param config Lambda supplier returning the error state. - */ - private void configureSparkMax(Supplier config) { - for (int i = 0; i < maximumRetries; i++) { - if (config.get() == REVLibError.kOk) { - return; - } - } - DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); - } - - /** - * Set the voltage compensation for the swerve module motor. - * - * @param nominalVoltage Nominal voltage for operation to output to. - */ - @Override - public void setVoltageCompensation(double nominalVoltage) { - configureSparkMax(() -> motor.enableVoltageCompensation(nominalVoltage)); - } - - /** - * Set the current limit for the swerve drive motor, remember this may cause jumping if used in - * conjunction with voltage compensation. This is useful to protect the motor from current spikes. - * - * @param currentLimit Current limit in AMPS at free speed. - */ - @Override - public void setCurrentLimit(int currentLimit) { - configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit)); - } - - /** - * Set the maximum rate the open/closed loop output can change by. - * - * @param rampRate Time in seconds to go from 0 to full throttle. - */ - @Override - public void setLoopRampRate(double rampRate) { - configureSparkMax(() -> motor.setOpenLoopRampRate(rampRate)); - configureSparkMax(() -> motor.setClosedLoopRampRate(rampRate)); - } - - /** - * Get the motor object from the module. - * - * @return Motor object. - */ - @Override - public Object getMotor() { - return motor; - } - - /** - * Queries whether the absolute encoder is directly attached to the motor controller. - * - * @return connected absolute encoder state. - */ - @Override - public boolean isAttachedAbsoluteEncoder() { - return absoluteEncoder != null; - } - - /** Configure the factory defaults. */ - @Override - public void factoryDefaults() { - if (!factoryDefaultOccurred) { - configureSparkMax(motor::restoreFactoryDefaults); - factoryDefaultOccurred = true; - } - } - - /** Clear the sticky faults on the motor controller. */ - @Override - public void clearStickyFaults() { - configureSparkMax(motor::clearFaults); - } - - /** - * Set the absolute encoder to be a compatible absolute encoder. - * - * @param encoder The encoder to use. - * @return The {@link SwerveMotor} for easy instantiation. - */ - @Override - public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { - if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) { - DriverStation.reportWarning( - "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the" - + " absoluteEncoderOffset in the Swerve Module JSON!", - false); - absoluteEncoder = encoder; - configureSparkMax( - () -> pid.setFeedbackDevice((MotorFeedbackSensor) absoluteEncoder.getAbsoluteEncoder())); - } - return this; - } - - /** - * Configure the integrated encoder for the swerve module. Sets the conversion factors for - * position and velocity. - * - * @param positionConversionFactor The conversion factor to apply. - */ - @Override - public void configureIntegratedEncoder(double positionConversionFactor) { - if (absoluteEncoder == null) { - configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor)); - configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60)); - - // Taken from - // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67 - configureCANStatusFrames(10, 20, 20, 500, 500, 200, 200); - } else { - configureSparkMax( - () -> { - if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) { - return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()) - .setPositionConversionFactor(positionConversionFactor); - } else { - return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()) - .setPositionConversionFactor(positionConversionFactor); - } - }); - configureSparkMax( - () -> { - if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) { - return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()) - .setVelocityConversionFactor(positionConversionFactor / 60); - } else { - return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()) - .setVelocityConversionFactor(positionConversionFactor / 60); - } - }); - } - } - - /** - * Configure the PIDF values for the closed loop controller. - * - * @param config Configuration class holding the PIDF values. - */ - @Override - public void configurePIDF(PIDFConfig config) { - // int pidSlot = - // isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : - // SparkMAX_slotIdx.Position.ordinal(); - int pidSlot = 0; - configureSparkMax(() -> pid.setP(config.p)); - configureSparkMax(() -> pid.setI(config.i)); - configureSparkMax(() -> pid.setD(config.d)); - configureSparkMax(() -> pid.setFF(config.f)); - configureSparkMax(() -> pid.setIZone(config.iz)); - configureSparkMax(() -> pid.setOutputRange(config.output.min, config.output.max)); - } - - /** - * Configure the PID wrapping for the position closed loop controller. - * - * @param minInput Minimum PID input. - * @param maxInput Maximum PID input. - */ - @Override - public void configurePIDWrapping(double minInput, double maxInput) { - configureSparkMax(() -> pid.setPositionPIDWrappingEnabled(true)); - configureSparkMax(() -> pid.setPositionPIDWrappingMinInput(minInput)); - configureSparkMax(() -> pid.setPositionPIDWrappingMaxInput(maxInput)); - } - - /** - * Set the CAN status frames. - * - * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower - * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current - * @param CANStatus2 Motor Position - * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position - * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position - * @param CANStatus5 Duty Cycle Absolute Encoder Position, Duty Cycle Absolute Encoder Absolute - * Angle - * @param CANStatus6 Duty Cycle Absolute Encoder Velocity, Duty Cycle Absolute Encoder Frequency - */ - public void configureCANStatusFrames( - int CANStatus0, - int CANStatus1, - int CANStatus2, - int CANStatus3, - int CANStatus4, - int CANStatus5, - int CANStatus6) { - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus5, CANStatus5)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus6, CANStatus6)); - // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces - } - - /** - * Set the idle mode. - * - * @param isBrakeMode Set the brake mode. - */ - @Override - public void setMotorBrake(boolean isBrakeMode) { - configureSparkMax(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast)); - } - - /** - * Set the motor to be inverted. - * - * @param inverted State of inversion. - */ - @Override - public void setInverted(boolean inverted) { - motor.setInverted(inverted); - } - - /** Save the configurations from flash to EEPROM. */ - @Override - public void burnFlash() { - try { - Thread.sleep(200); - } catch (Exception e) { - } - configureSparkMax(() -> motor.burnFlash()); - } - - /** - * Set the percentage output. - * - * @param percentOutput percent out for the motor controller. - */ - @Override - public void set(double percentOutput) { - motor.set(percentOutput); - } - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in MPS or Angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - */ - @Override - public void setReference(double setpoint, double feedforward) { - boolean possibleBurnOutIssue = true; - // int pidSlot = - // isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : - // SparkMAX_slotIdx.Position.ordinal(); - int pidSlot = 0; - - if (isDriveMotor) { - configureSparkMax( - () -> pid.setReference(setpoint, ControlType.kVelocity, pidSlot, feedforward)); - } else { - configureSparkMax( - () -> pid.setReference(setpoint, ControlType.kPosition, pidSlot, feedforward)); - if (SwerveDriveTelemetry.isSimulation) { - encoder.setPosition(setpoint); - } - } - } - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in meters per second or angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - * @param position Only used on the angle motor, the position of the motor in degrees. - */ - @Override - public void setReference(double setpoint, double feedforward, double position) { - setReference(setpoint, feedforward); - } - - /** - * Get the velocity of the integrated encoder. - * - * @return velocity - */ - @Override - public double getVelocity() { - return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity(); - } - - /** - * Get the position of the integrated encoder. - * - * @return Position - */ - @Override - public double getPosition() { - return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getAbsolutePosition(); - } - - /** - * Set the integrated encoder position. - * - * @param position Integrated encoder position. - */ - @Override - public void setPosition(double position) { - if (absoluteEncoder == null) { - configureSparkMax(() -> encoder.setPosition(position)); - } - } - - /** REV Slots for PID configuration. */ - enum SparkMAX_slotIdx { - /** Slot 1, used for position PID's. */ - Position, - /** Slot 2, used for velocity PID's. */ - Velocity, - /** Slot 3, used arbitrarily. (Documentation show simulations). */ - Simulation - } -} diff --git a/src/main/java/swervelib/motors/SwerveMotor.java b/src/main/java/swervelib/motors/SwerveMotor.java deleted file mode 100644 index f1c6253..0000000 --- a/src/main/java/swervelib/motors/SwerveMotor.java +++ /dev/null @@ -1,154 +0,0 @@ -package swervelib.motors; - -import swervelib.encoders.SwerveAbsoluteEncoder; -import swervelib.parser.PIDFConfig; - -/** - * Swerve motor abstraction which defines a standard interface for motors within a swerve module. - */ -public abstract class SwerveMotor { - - /** - * The maximum amount of times the swerve motor will attempt to configure a motor if failures - * occur. - */ - public final int maximumRetries = 5; - - /** Whether the swerve motor is a drive motor. */ - protected boolean isDriveMotor; - - /** Configure the factory defaults. */ - public abstract void factoryDefaults(); - - /** Clear the sticky faults on the motor controller. */ - public abstract void clearStickyFaults(); - - /** - * Set the absolute encoder to be a compatible absolute encoder. - * - * @param encoder The encoder to use. - * @return The {@link SwerveMotor} for single line configuration. - */ - public abstract SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder); - - /** - * Configure the integrated encoder for the swerve module. Sets the conversion factors for - * position and velocity. - * - * @param positionConversionFactor The conversion factor to apply for position. - */ - public abstract void configureIntegratedEncoder(double positionConversionFactor); - - /** - * Configure the PIDF values for the closed loop controller. 0 is disabled or off. - * - * @param config Configuration class holding the PIDF values. - */ - public abstract void configurePIDF(PIDFConfig config); - - /** - * Configure the PID wrapping for the position closed loop controller. - * - * @param minInput Minimum PID input. - * @param maxInput Maximum PID input. - */ - public abstract void configurePIDWrapping(double minInput, double maxInput); - - /** - * Set the idle mode. - * - * @param isBrakeMode Set the brake mode. - */ - public abstract void setMotorBrake(boolean isBrakeMode); - - /** - * Set the motor to be inverted. - * - * @param inverted State of inversion. - */ - public abstract void setInverted(boolean inverted); - - /** Save the configurations from flash to EEPROM. */ - public abstract void burnFlash(); - - /** - * Set the percentage output. - * - * @param percentOutput percent out for the motor controller. - */ - public abstract void set(double percentOutput); - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in meters per second or angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - */ - public abstract void setReference(double setpoint, double feedforward); - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in meters per second or angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - * @param position Only used on the angle motor, the position of the motor in degrees. - */ - public abstract void setReference(double setpoint, double feedforward, double position); - - /** - * Get the velocity of the integrated encoder. - * - * @return velocity in meters per second or degrees per second. - */ - public abstract double getVelocity(); - - /** - * Get the position of the integrated encoder. - * - * @return Position in meters or degrees. - */ - public abstract double getPosition(); - - /** - * Set the integrated encoder position. - * - * @param position Integrated encoder position. Should be angle in degrees or meters per second. - */ - public abstract void setPosition(double position); - - /** - * Set the voltage compensation for the swerve module motor. - * - * @param nominalVoltage Nominal voltage for operation to output to. - */ - public abstract void setVoltageCompensation(double nominalVoltage); - - /** - * Set the current limit for the swerve drive motor, remember this may cause jumping if used in - * conjunction with voltage compensation. This is useful to protect the motor from current spikes. - * - * @param currentLimit Current limit in AMPS at free speed. - */ - public abstract void setCurrentLimit(int currentLimit); - - /** - * Set the maximum rate the open/closed loop output can change by. - * - * @param rampRate Time in seconds to go from 0 to full throttle. - */ - public abstract void setLoopRampRate(double rampRate); - - /** - * Get the motor object from the module. - * - * @return Motor object. - */ - public abstract Object getMotor(); - - /** - * Queries whether the absolute encoder is directly attached to the motor controller. - * - * @return connected absolute encoder state. - */ - public abstract boolean isAttachedAbsoluteEncoder(); -} diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java deleted file mode 100644 index 2d7197a..0000000 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ /dev/null @@ -1,400 +0,0 @@ -package swervelib.motors; - -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.configs.TalonFXConfigurator; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import swervelib.encoders.SwerveAbsoluteEncoder; -import swervelib.parser.PIDFConfig; -import swervelib.telemetry.SwerveDriveTelemetry; - -/** {@link com.ctre.phoenix6.hardware.TalonFX} Swerve Motor. Made by Team 1466 WebbRobotics. */ -public class TalonFXSwerve extends SwerveMotor { - - /** Factory default already occurred. */ - private final boolean factoryDefaultOccurred = false; - - /** Whether the absolute encoder is integrated. */ - private final boolean absoluteEncoder = false; - - /** Motion magic angle voltage setter. */ - private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0); - - /** Velocity voltage setter for controlling drive motor. */ - private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); - - /** Conversion factor for the motor. */ - private double conversionFactor; - - /** TalonFX motor controller. */ - TalonFX motor; - - /** Current TalonFX configuration. */ - private TalonFXConfiguration configuration = new TalonFXConfiguration(); - - /** - * Constructor for TalonFX swerve motor. - * - * @param motor Motor to use. - * @param isDriveMotor Whether this motor is a drive motor. - */ - public TalonFXSwerve(TalonFX motor, boolean isDriveMotor) { - this.isDriveMotor = isDriveMotor; - this.motor = motor; - - factoryDefaults(); - clearStickyFaults(); - - // if (SwerveDriveTelemetry.isSimulation) - // { - //// PhysicsSim.getInstance().addTalonFX(motor, .25, 6800); - // } - } - - /** - * Construct the TalonFX swerve motor given the ID and CANBus. - * - * @param id ID of the TalonFX on the CANBus. - * @param canbus CANBus on which the TalonFX is on. - * @param isDriveMotor Whether the motor is a drive or steering motor. - */ - public TalonFXSwerve(int id, String canbus, boolean isDriveMotor) { - this(new TalonFX(id, canbus), isDriveMotor); - } - - /** - * Construct the TalonFX swerve motor given the ID. - * - * @param id ID of the TalonFX on the canbus. - * @param isDriveMotor Whether the motor is a drive or steering motor. - */ - public TalonFXSwerve(int id, boolean isDriveMotor) { - this(new TalonFX(id), isDriveMotor); - } - - /** Configure the factory defaults. */ - @Override - public void factoryDefaults() { - if (!factoryDefaultOccurred) { - TalonFXConfigurator cfg = motor.getConfigurator(); - configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; - configuration.ClosedLoopGeneral.ContinuousWrap = true; - cfg.apply(configuration); - - m_angleVoltageSetter.UpdateFreqHz = 0; - // m_angleVoltageExpoSetter.UpdateFreqHz = 0; - m_velocityVoltageSetter.UpdateFreqHz = 0; - // motor.configFactoryDefault(); - // motor.setSensorPhase(true); - // motor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30); - // motor.configNeutralDeadband(0.001); - } - } - - /** Clear the sticky faults on the motor controller. */ - @Override - public void clearStickyFaults() { - motor.clearStickyFaults(); - } - - /** - * Set the absolute encoder to be a compatible absolute encoder. - * - * @param encoder The encoder to use. - */ - @Override - public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { - // Do not support. - return this; - } - - /** - * Configure the integrated encoder for the swerve module. Sets the conversion factors for - * position and velocity. - * - * @param positionConversionFactor The conversion factor to apply for position. - *


- * Degrees:
- * - * 360 / (angleGearRatio * encoderTicksPerRotation) - *
- *


- * Meters:
- * - * (Math.PI * wheelDiameter) / (driveGearRatio * encoderTicksPerRotation) - * - */ - @Override - public void configureIntegratedEncoder(double positionConversionFactor) { - TalonFXConfigurator cfg = motor.getConfigurator(); - cfg.refresh(configuration); - - positionConversionFactor = 1 / positionConversionFactor; - if (!isDriveMotor) { - positionConversionFactor *= 360; - } - conversionFactor = positionConversionFactor; - - configuration.MotionMagic = - configuration.MotionMagic.withMotionMagicCruiseVelocity(100.0 / positionConversionFactor) - .withMotionMagicAcceleration((100.0 / positionConversionFactor) / 0.100) - .withMotionMagicExpo_kV(0.12 * positionConversionFactor) - .withMotionMagicExpo_kA(0.1); - - configuration.Feedback.withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor) - .withSensorToMechanismRatio(positionConversionFactor); - - cfg.apply(configuration); - // Taken from democat's library. - // https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500DriveControllerFactoryBuilder.java#L16 - configureCANStatusFrames(250); - } - - /** - * Set the CAN status frames. - * - * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information - */ - public void configureCANStatusFrames(int CANStatus1) { - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); - } - - /** - * Set the CAN status frames. - * - * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information - * @param CANStatus2 Selected Sensor Position (PID 0), Selected Sensor Velocity (PID 0), Brushed - * Supply Current Measurement, Sticky Fault Information - * @param CANStatus3 Quadrature Information - * @param CANStatus4 Analog Input, Supply Battery Voltage, Controller Temperature - * @param CANStatus8 Pulse Width Information - * @param CANStatus10 Motion Profiling/Motion Magic Information - * @param CANStatus12 Selected Sensor Position (Aux PID 1), Selected Sensor Velocity (Aux PID 1) - * @param CANStatus13 PID0 (Primary PID) Information - * @param CANStatus14 PID1 (Auxiliary PID) Information - * @param CANStatus21 Integrated Sensor Position (Talon FX), Integrated Sensor Velocity (Talon FX) - * @param CANStatusCurrent Brushless Supply Current Measurement, Brushless Stator Current - * Measurement - */ - public void configureCANStatusFrames( - int CANStatus1, - int CANStatus2, - int CANStatus3, - int CANStatus4, - int CANStatus8, - int CANStatus10, - int CANStatus12, - int CANStatus13, - int CANStatus14, - int CANStatus21, - int CANStatusCurrent) { - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, CANStatus2); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, CANStatus3); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, CANStatus4); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, CANStatus8); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, CANStatus10); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, CANStatus12); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, CANStatus13); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, CANStatus14); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, CANStatus21); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_Brushless_Current, - // CANStatusCurrent); - - // TODO: Configure Status Frame 2 thru 21 if necessary - // https://v5.docs.ctr-electronics.com/en/stable/ch18_CommonAPI.html#setting-status-frame-periods - } - - /** - * Configure the PIDF values for the closed loop controller. 0 is disabled or off. - * - * @param config Configuration class holding the PIDF values. - */ - @Override - public void configurePIDF(PIDFConfig config) { - - TalonFXConfigurator cfg = motor.getConfigurator(); - cfg.refresh(configuration.Slot0); - cfg.apply( - configuration.Slot0.withKP(config.p).withKI(config.i).withKD(config.d).withKS(config.f)); - // configuration.slot0.integralZone = config.iz; - // configuration.slot0.closedLoopPeakOutput = config.output.max; - } - - /** - * Configure the PID wrapping for the position closed loop controller. - * - * @param minInput Minimum PID input. - * @param maxInput Maximum PID input. - */ - @Override - public void configurePIDWrapping(double minInput, double maxInput) { - TalonFXConfigurator cfg = motor.getConfigurator(); - cfg.refresh(configuration.ClosedLoopGeneral); - configuration.ClosedLoopGeneral.ContinuousWrap = true; - cfg.apply(configuration.ClosedLoopGeneral); - } - - /** - * Set the idle mode. - * - * @param isBrakeMode Set the brake mode. - */ - @Override - public void setMotorBrake(boolean isBrakeMode) { - motor.setNeutralMode(isBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast); - } - - /** - * Set the motor to be inverted. - * - * @param inverted State of inversion. - */ - @Override - public void setInverted(boolean inverted) { - // Timer.delay(1); - motor.setInverted(inverted); - } - - /** Save the configurations from flash to EEPROM. */ - @Override - public void burnFlash() { - // Do nothing - } - - /** - * Set the percentage output. - * - * @param percentOutput percent out for the motor controller. - */ - @Override - public void set(double percentOutput) { - motor.set(percentOutput); - } - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in MPS or Angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - */ - @Override - public void setReference(double setpoint, double feedforward) { - setReference(setpoint, feedforward, getPosition()); - } - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in meters per second or angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - * @param position Only used on the angle motor, the position of the motor in degrees. - */ - @Override - public void setReference(double setpoint, double feedforward, double position) { - // if (SwerveDriveTelemetry.isSimulation) - // { - // PhysicsSim.getInstance().run(); - // } - - if (isDriveMotor) { - motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint).withFeedForward(feedforward)); - } else { - motor.setControl(m_angleVoltageSetter.withPosition(setpoint / 360.0)); - } - } - - /** - * Get the velocity of the integrated encoder. - * - * @return velocity in Meters Per Second, or Degrees per Second. - */ - @Override - public double getVelocity() { - return motor.getVelocity().getValue(); - } - - /** - * Get the position of the integrated encoder. - * - * @return Position in Meters or Degrees. - */ - @Override - public double getPosition() { - return motor.getPosition().getValue(); - } - - /** - * Set the integrated encoder position. - * - * @param position Integrated encoder position. Should be angle in degrees or meters. - */ - @Override - public void setPosition(double position) { - if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation) { - position = position < 0 ? (position % 360) + 360 : position; - TalonFXConfigurator cfg = motor.getConfigurator(); - cfg.setPosition(position / 360); - } - } - - /** - * Set the voltage compensation for the swerve module motor. - * - * @param nominalVoltage Nominal voltage for operation to output to. - */ - @Override - public void setVoltageCompensation(double nominalVoltage) { - // Do not implement - } - - /** - * Set the current limit for the swerve drive motor, remember this may cause jumping if used in - * conjunction with voltage compensation. This is useful to protect the motor from current spikes. - * - * @param currentLimit Current limit in AMPS at free speed. - */ - @Override - public void setCurrentLimit(int currentLimit) { - TalonFXConfigurator cfg = motor.getConfigurator(); - cfg.refresh(configuration.CurrentLimits); - cfg.apply( - configuration.CurrentLimits.withStatorCurrentLimit(currentLimit) - .withStatorCurrentLimitEnable(true)); - } - - /** - * Set the maximum rate the open/closed loop output can change by. - * - * @param rampRate Time in seconds to go from 0 to full throttle. - */ - @Override - public void setLoopRampRate(double rampRate) { - TalonFXConfigurator cfg = motor.getConfigurator(); - cfg.refresh(configuration.ClosedLoopRamps); - cfg.apply(configuration.ClosedLoopRamps.withVoltageClosedLoopRampPeriod(rampRate)); - } - - /** - * Get the motor object from the module. - * - * @return Motor object. - */ - @Override - public Object getMotor() { - return motor; - } - - /** - * Queries whether the absolute encoder is directly attached to the motor controller. - * - * @return connected absolute encoder state. - */ - @Override - public boolean isAttachedAbsoluteEncoder() { - return absoluteEncoder; - } -} diff --git a/src/main/java/swervelib/motors/TalonSRXSwerve.java b/src/main/java/swervelib/motors/TalonSRXSwerve.java deleted file mode 100644 index c2594f6..0000000 --- a/src/main/java/swervelib/motors/TalonSRXSwerve.java +++ /dev/null @@ -1,379 +0,0 @@ -package swervelib.motors; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.DemandType; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; -import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import edu.wpi.first.wpilibj.Timer; -import swervelib.encoders.SwerveAbsoluteEncoder; -import swervelib.math.SwerveMath; -import swervelib.parser.PIDFConfig; -import swervelib.telemetry.SwerveDriveTelemetry; - -/** {@link com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX} Swerve Motor. */ -public class TalonSRXSwerve extends SwerveMotor { - - /** Factory default already occurred. */ - private final boolean factoryDefaultOccurred = false; - - /** Current TalonFX configuration. */ - private final TalonSRXConfiguration configuration = new TalonSRXConfiguration(); - - /** Whether the absolute encoder is integrated. */ - private final boolean absoluteEncoder = false; - - /** TalonSRX motor controller. */ - WPI_TalonSRX motor; - - /** - * The position conversion factor to convert raw sensor units to Meters Per 100ms, or Ticks to - * Degrees. - */ - private double positionConversionFactor = 1; - - /** If the TalonFX configuration has changed. */ - private boolean configChanged = true; - - /** Nominal voltage default to use with feedforward. */ - private double nominalVoltage = 12.0; - - /** - * Constructor for TalonSRX swerve motor. - * - * @param motor Motor to use. - * @param isDriveMotor Whether this motor is a drive motor. - */ - public TalonSRXSwerve(WPI_TalonSRX motor, boolean isDriveMotor) { - this.isDriveMotor = isDriveMotor; - this.motor = motor; - motor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); - - factoryDefaults(); - clearStickyFaults(); - } - - /** - * Construct the TalonSRX swerve motor given the ID. - * - * @param id ID of the TalonSRX on the canbus. - * @param isDriveMotor Whether the motor is a drive or steering motor. - */ - public TalonSRXSwerve(int id, boolean isDriveMotor) { - this(new WPI_TalonSRX(id), isDriveMotor); - } - - /** Configure the factory defaults. */ - @Override - public void factoryDefaults() { - if (!factoryDefaultOccurred) { - motor.configFactoryDefault(); - motor.setSensorPhase(true); - } - } - - /** Clear the sticky faults on the motor controller. */ - @Override - public void clearStickyFaults() { - motor.clearStickyFaults(); - } - - /** - * Set the absolute encoder to be a compatible absolute encoder. - * - * @param encoder The encoder to use. - */ - @Override - public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { - // Do not support. - return this; - } - - /** - * Configure the integrated encoder for the swerve module. Sets the conversion factors for - * position and velocity. - * - * @param positionConversionFactor The conversion factor to apply for position. - *


- * Degrees:
- * - * 360 / (angleGearRatio * encoderTicksPerRotation) - *
- *


- * Meters:
- * - * (Math.PI * wheelDiameter) / (driveGearRatio * encoderTicksPerRotation) - * - */ - @Override - public void configureIntegratedEncoder(double positionConversionFactor) { - this.positionConversionFactor = positionConversionFactor; - // Taken from democat's library. - // https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500DriveControllerFactoryBuilder.java#L16 - configureCANStatusFrames(250); - } - - /** - * Set the CAN status frames. - * - * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information - */ - public void configureCANStatusFrames(int CANStatus1) { - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); - } - - /** - * Set the CAN status frames. - * - * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information - * @param CANStatus2 Selected Sensor Position (PID 0), Selected Sensor Velocity (PID 0), Brushed - * Supply Current Measurement, Sticky Fault Information - * @param CANStatus3 Quadrature Information - * @param CANStatus4 Analog Input, Supply Battery Voltage, Controller Temperature - * @param CANStatus8 Pulse Width Information - * @param CANStatus10 Motion Profiling/Motion Magic Information - * @param CANStatus12 Selected Sensor Position (Aux PID 1), Selected Sensor Velocity (Aux PID 1) - * @param CANStatus13 PID0 (Primary PID) Information - * @param CANStatus14 PID1 (Auxiliary PID) Information - * @param CANStatus21 Integrated Sensor Position (Talon FX), Integrated Sensor Velocity (Talon FX) - * @param CANStatusCurrent Brushless Supply Current Measurement, Brushless Stator Current - * Measurement - */ - public void configureCANStatusFrames( - int CANStatus1, - int CANStatus2, - int CANStatus3, - int CANStatus4, - int CANStatus8, - int CANStatus10, - int CANStatus12, - int CANStatus13, - int CANStatus14, - int CANStatus21, - int CANStatusCurrent) { - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, CANStatus2); - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, CANStatus3); - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, CANStatus4); - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, CANStatus8); - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, CANStatus10); - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, CANStatus12); - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, CANStatus13); - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, CANStatus14); - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, CANStatus21); - motor.setStatusFramePeriod(StatusFrameEnhanced.Status_Brushless_Current, CANStatusCurrent); - - // TODO: Configure Status Frame 2 thru 21 if necessary - // https://v5.docs.ctr-electronics.com/en/stable/ch18_CommonAPI.html#setting-status-frame-periods - } - - /** - * Configure the PIDF values for the closed loop controller. 0 is disabled or off. - * - * @param config Configuration class holding the PIDF values. - */ - @Override - public void configurePIDF(PIDFConfig config) { - configuration.slot0.kP = config.p; - configuration.slot0.kI = config.i; - configuration.slot0.kD = config.d; - configuration.slot0.kF = config.f; - configuration.slot0.integralZone = config.iz; - configuration.slot0.closedLoopPeakOutput = config.output.max; - configChanged = true; - } - - /** - * Configure the PID wrapping for the position closed loop controller. - * - * @param minInput Minimum PID input. - * @param maxInput Maximum PID input. - */ - @Override - public void configurePIDWrapping(double minInput, double maxInput) { - // Do nothing - } - - /** - * Set the idle mode. - * - * @param isBrakeMode Set the brake mode. - */ - @Override - public void setMotorBrake(boolean isBrakeMode) { - motor.setNeutralMode(isBrakeMode ? NeutralMode.Brake : NeutralMode.Coast); - } - - /** - * Set the motor to be inverted. - * - * @param inverted State of inversion. - */ - @Override - public void setInverted(boolean inverted) { - Timer.delay(1); - motor.setInverted(inverted); - } - - /** Save the configurations from flash to EEPROM. */ - @Override - public void burnFlash() { - if (configChanged) { - motor.configAllSettings(configuration, 250); - configChanged = false; - } - } - - /** - * Set the percentage output. - * - * @param percentOutput percent out for the motor controller. - */ - @Override - public void set(double percentOutput) { - motor.set(percentOutput); - } - - /** - * Convert the setpoint into native sensor units. - * - * @param setpoint Setpoint to mutate. In meters per second or degrees. - * @param position Position in degrees, only used on angle motors. - * @return Setpoint as native sensor units. Encoder ticks per 100ms, or Encoder tick. - */ - public double convertToNativeSensorUnits(double setpoint, double position) { - setpoint = - isDriveMotor ? setpoint * .1 : SwerveMath.placeInAppropriate0To360Scope(position, setpoint); - return setpoint / positionConversionFactor; - } - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in MPS or Angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - */ - @Override - public void setReference(double setpoint, double feedforward) { - setReference(setpoint, feedforward, getPosition()); - } - - /** - * Set the closed loop PID controller reference point. - * - * @param setpoint Setpoint in meters per second or angle in degrees. - * @param feedforward Feedforward in volt-meter-per-second or kV. - * @param position Only used on the angle motor, the position of the motor in degrees. - */ - @Override - public void setReference(double setpoint, double feedforward, double position) { - - burnFlash(); - - motor.set( - isDriveMotor ? ControlMode.Velocity : ControlMode.Position, - convertToNativeSensorUnits(setpoint, position), - DemandType.ArbitraryFeedForward, - feedforward / nominalVoltage); - } - - /** - * Get the velocity of the integrated encoder. - * - * @return velocity in Meters Per Second, or Degrees per Second. - */ - @Override - public double getVelocity() { - return (motor.getSelectedSensorVelocity() * 10) * positionConversionFactor; - } - - /** - * Get the position of the integrated encoder. - * - * @return Position in Meters or Degrees. - */ - @Override - public double getPosition() { - if (isDriveMotor) { - return motor.getSelectedSensorPosition() * positionConversionFactor; - } else { - var pos = motor.getSelectedSensorPosition() * positionConversionFactor; - pos %= 360; - if (pos < 360) { - pos += 360; - } - return pos; - } - } - - /** - * Set the integrated encoder position. - * - * @param position Integrated encoder position. Should be angle in degrees or meters. - */ - @Override - public void setPosition(double position) { - if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation) { - motor.setSelectedSensorPosition(position / positionConversionFactor, 0, 0); - } - } - - /** - * Set the voltage compensation for the swerve module motor. - * - * @param nominalVoltage Nominal voltage for operation to output to. - */ - @Override - public void setVoltageCompensation(double nominalVoltage) { - configuration.voltageCompSaturation = nominalVoltage; - configChanged = true; - this.nominalVoltage = nominalVoltage; - } - - /** - * Set the current limit for the swerve drive motor, remember this may cause jumping if used in - * conjunction with voltage compensation. This is useful to protect the motor from current spikes. - * - * @param currentLimit Current limit in AMPS at free speed. - */ - @Override - public void setCurrentLimit(int currentLimit) { - configuration.continuousCurrentLimit = currentLimit; - configuration.peakCurrentLimit = currentLimit; - configChanged = true; - } - - /** - * Set the maximum rate the open/closed loop output can change by. - * - * @param rampRate Time in seconds to go from 0 to full throttle. - */ - @Override - public void setLoopRampRate(double rampRate) { - configuration.closedloopRamp = rampRate; - configuration.openloopRamp = rampRate; - configChanged = true; - } - - /** - * Get the motor object from the module. - * - * @return Motor object. - */ - @Override - public Object getMotor() { - return motor; - } - - /** - * Queries whether the absolute encoder is directly attached to the motor controller. - * - * @return connected absolute encoder state. - */ - @Override - public boolean isAttachedAbsoluteEncoder() { - return absoluteEncoder; - } -} diff --git a/src/main/java/swervelib/motors/package-info.java b/src/main/java/swervelib/motors/package-info.java deleted file mode 100644 index de65fb0..0000000 --- a/src/main/java/swervelib/motors/package-info.java +++ /dev/null @@ -1,2 +0,0 @@ -/** Swerve motor controller wrappers which implement {@link swervelib.motors.SwerveMotor}. */ -package swervelib.motors; diff --git a/src/main/java/swervelib/package-info.java b/src/main/java/swervelib/package-info.java deleted file mode 100644 index a9884ca..0000000 --- a/src/main/java/swervelib/package-info.java +++ /dev/null @@ -1,6 +0,0 @@ -/** - * Yet-Another Generic Swerve Library (YAGSL) main package AKA swervelib. - * - * @version 1.0.0 - */ -package swervelib; diff --git a/src/main/java/swervelib/parser/PIDFConfig.java b/src/main/java/swervelib/parser/PIDFConfig.java deleted file mode 100644 index d96a881..0000000 --- a/src/main/java/swervelib/parser/PIDFConfig.java +++ /dev/null @@ -1,88 +0,0 @@ -package swervelib.parser; - -import edu.wpi.first.math.controller.PIDController; -import swervelib.parser.deserializer.PIDFRange; - -/** Hold the PIDF and Integral Zone values for a PID. */ -public class PIDFConfig { - - /** Proportional Gain for PID. */ - public double p; - - /** Integral Gain for PID. */ - public double i; - - /** Derivative Gain for PID. */ - public double d; - - /** Feedforward value for PID. */ - public double f; - - /** Integral zone of the PID. */ - public double iz; - - /** The PIDF output range. */ - public PIDFRange output = new PIDFRange(); - - /** Used when parsing PIDF values from JSON. */ - public PIDFConfig() {} - - /** - * PIDF Config constructor to contain the values. - * - * @param p P gain. - * @param i I gain. - * @param d D gain. - * @param f F gain. - * @param iz Intergral zone. - */ - public PIDFConfig(double p, double i, double d, double f, double iz) { - this.p = p; - this.i = i; - this.d = d; - this.f = f; - this.iz = iz; - } - - /** - * PIDF Config constructor to contain the values. - * - * @param p P gain. - * @param i I gain. - * @param d D gain. - * @param f F gain. - */ - public PIDFConfig(double p, double i, double d, double f) { - this(p, i, d, f, 0); - } - - /** - * PIDF Config constructor to contain the values. - * - * @param p P gain. - * @param i I gain. - * @param d D gain. - */ - public PIDFConfig(double p, double i, double d) { - this(p, i, d, 0, 0); - } - - /** - * PIDF Config constructor to contain the values. - * - * @param p P gain. - * @param d D gain. - */ - public PIDFConfig(double p, double d) { - this(p, 0, d, 0, 0); - } - - /** - * Create a PIDController from the PID values. - * - * @return PIDController. - */ - public PIDController createPIDController() { - return new PIDController(p, i, d); - } -} diff --git a/src/main/java/swervelib/parser/SwerveControllerConfiguration.java b/src/main/java/swervelib/parser/SwerveControllerConfiguration.java deleted file mode 100644 index af1ac34..0000000 --- a/src/main/java/swervelib/parser/SwerveControllerConfiguration.java +++ /dev/null @@ -1,59 +0,0 @@ -package swervelib.parser; - -import static swervelib.math.SwerveMath.calculateMaxAngularVelocity; - -/** - * Swerve Controller configuration class which is used to configure {@link - * swervelib.SwerveController}. - */ -public class SwerveControllerConfiguration { - - /** PIDF for the heading of the robot. */ - public final PIDFConfig headingPIDF; - - /** hypotenuse deadband for the robot angle control joystick. */ - public final double - angleJoyStickRadiusDeadband; // Deadband for the minimum hypot for the heading joystick. - - /** Maximum angular velocity in rad/s */ - public double maxAngularVelocity; - - /** - * Construct the swerve controller configuration. Assumes robot is square to fetch maximum angular - * velocity. - * - * @param driveCfg {@link SwerveDriveConfiguration} to fetch the first module X and Y used to - * calculate the maximum angular velocity. - * @param headingPIDF Heading PIDF configuration. - * @param angleJoyStickRadiusDeadband Deadband on radius of angle joystick. - * @param maxSpeedMPS Maximum speed in meters per second for angular velocity, remember if you - * have feet per second use {@link edu.wpi.first.math.util.Units#feetToMeters(double)}. - */ - public SwerveControllerConfiguration( - SwerveDriveConfiguration driveCfg, - PIDFConfig headingPIDF, - double angleJoyStickRadiusDeadband, - double maxSpeedMPS) { - this.maxAngularVelocity = - calculateMaxAngularVelocity( - maxSpeedMPS, - Math.abs(driveCfg.moduleLocationsMeters[0].getX()), - Math.abs(driveCfg.moduleLocationsMeters[0].getY())); - this.headingPIDF = headingPIDF; - this.angleJoyStickRadiusDeadband = angleJoyStickRadiusDeadband; - } - - /** - * Construct the swerve controller configuration. Assumes hypotenuse deadband of 0.5 (minimum - * radius for angle to be set on angle joystick is .5 of the controller). - * - * @param driveCfg Drive configuration. - * @param headingPIDF Heading PIDF configuration. - * @param maxSpeedMPS Maximum speed in meters per second for angular velocity, remember if you - * have feet per second use {@link edu.wpi.first.math.util.Units#feetToMeters(double)}. - */ - public SwerveControllerConfiguration( - SwerveDriveConfiguration driveCfg, PIDFConfig headingPIDF, double maxSpeedMPS) { - this(driveCfg, headingPIDF, 0.5, maxSpeedMPS); - } -} diff --git a/src/main/java/swervelib/parser/SwerveDriveConfiguration.java b/src/main/java/swervelib/parser/SwerveDriveConfiguration.java deleted file mode 100644 index 70aba2c..0000000 --- a/src/main/java/swervelib/parser/SwerveDriveConfiguration.java +++ /dev/null @@ -1,89 +0,0 @@ -package swervelib.parser; - -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Translation2d; -import swervelib.SwerveModule; -import swervelib.imu.SwerveIMU; - -/** Swerve drive configurations used during SwerveDrive construction. */ -public class SwerveDriveConfiguration { - - /** Swerve Module locations. */ - public Translation2d[] moduleLocationsMeters; - - /** Swerve IMU */ - public SwerveIMU imu; - - /** Invert the imu measurements. */ - public boolean invertedIMU = false; - - /** Number of modules on the robot. */ - public int moduleCount; - - /** Swerve Modules. */ - public SwerveModule[] modules; - - /** Physical characteristics of the swerve drive from physicalproperties.json. */ - public SwerveModulePhysicalCharacteristics physicalCharacteristics; - - /** - * Create swerve drive configuration. - * - * @param moduleConfigs Module configuration. - * @param swerveIMU Swerve IMU. - * @param invertedIMU Invert the IMU. - * @param driveFeedforward The drive motor feedforward to use for the {@link SwerveModule}. - * @param physicalCharacteristics {@link SwerveModulePhysicalCharacteristics} to store in - * association with self. - */ - public SwerveDriveConfiguration( - SwerveModuleConfiguration[] moduleConfigs, - SwerveIMU swerveIMU, - boolean invertedIMU, - SimpleMotorFeedforward driveFeedforward, - SwerveModulePhysicalCharacteristics physicalCharacteristics) { - this.moduleCount = moduleConfigs.length; - this.imu = swerveIMU; - this.invertedIMU = invertedIMU; - this.modules = createModules(moduleConfigs, driveFeedforward); - this.moduleLocationsMeters = new Translation2d[moduleConfigs.length]; - for (SwerveModule module : modules) { - this.moduleLocationsMeters[module.moduleNumber] = module.configuration.moduleLocation; - } - this.physicalCharacteristics = physicalCharacteristics; - } - - /** - * Create modules based off of the SwerveModuleConfiguration. - * - * @param swerves Swerve constants. - * @param driveFeedforward Drive feedforward created using {@link - * swervelib.math.SwerveMath#createDriveFeedforward(double, double, double)}. - * @return Swerve Modules. - */ - public SwerveModule[] createModules( - SwerveModuleConfiguration[] swerves, SimpleMotorFeedforward driveFeedforward) { - SwerveModule[] modArr = new SwerveModule[swerves.length]; - for (int i = 0; i < swerves.length; i++) { - modArr[i] = new SwerveModule(i, swerves[i], driveFeedforward); - } - return modArr; - } - - /** - * Calculate the Drive Base Radius - * - * @return Drive base radius from center of robot to the farthest wheel in meters. - */ - public double getDriveBaseRadiusMeters() { - Translation2d centerOfModules = moduleLocationsMeters[0]; - - // Calculate the Center by adding all module offsets together. - for (int i = 1; i < moduleLocationsMeters.length; i++) { - centerOfModules = centerOfModules.plus(moduleLocationsMeters[i]); - } - - // Return Largest Radius - return centerOfModules.getDistance(moduleLocationsMeters[0]); - } -} diff --git a/src/main/java/swervelib/parser/SwerveModuleConfiguration.java b/src/main/java/swervelib/parser/SwerveModuleConfiguration.java deleted file mode 100644 index 68b1231..0000000 --- a/src/main/java/swervelib/parser/SwerveModuleConfiguration.java +++ /dev/null @@ -1,144 +0,0 @@ -package swervelib.parser; - -import edu.wpi.first.math.geometry.Translation2d; -import swervelib.encoders.SwerveAbsoluteEncoder; -import swervelib.motors.SwerveMotor; -import swervelib.parser.json.MotorConfigDouble; - -/** Swerve Module configuration class which is used to configure {@link swervelib.SwerveModule}. */ -public class SwerveModuleConfiguration { - - /** - * Conversion factor for drive motor onboard PID's and angle PID's. Use {@link - * swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and {@link - * swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} respectively to - * calculate the conversion factors. - */ - public final MotorConfigDouble conversionFactors; - - /** Angle offset in degrees for the Swerve Module. */ - public final double angleOffset; - - /** Whether the absolute encoder is inverted. */ - public final boolean absoluteEncoderInverted; - - /** State of inversion of the drive motor. */ - public final boolean driveMotorInverted; - - /** State of inversion of the angle motor. */ - public final boolean angleMotorInverted; - - /** PIDF configuration options for the angle motor closed-loop PID controller. */ - public PIDFConfig anglePIDF; - - /** PIDF configuration options for the drive motor closed-loop PID controller. */ - public PIDFConfig velocityPIDF; - - /** Swerve module location relative to the robot. */ - public Translation2d moduleLocation; - - /** Physical characteristics of the swerve module. */ - public SwerveModulePhysicalCharacteristics physicalCharacteristics; - - /** The drive motor and angle motor of this swerve module. */ - public SwerveMotor driveMotor, angleMotor; - - /** The Absolute Encoder for the swerve module. */ - public SwerveAbsoluteEncoder absoluteEncoder; - - /** Name for the swerve module for telemetry. */ - public String name; - - /** - * Construct a configuration object for swerve modules. - * - * @param driveMotor Drive {@link SwerveMotor}. - * @param angleMotor Angle {@link SwerveMotor} - * @param absoluteEncoder Absolute encoder {@link SwerveAbsoluteEncoder}. - * @param angleOffset Absolute angle offset to 0. - * @param absoluteEncoderInverted Absolute encoder inverted. - * @param angleMotorInverted State of inversion of the angle motor. - * @param driveMotorInverted Drive motor inverted. - * @param xMeters Module location in meters from the center horizontally. - * @param yMeters Module location in meters from center vertically. - * @param anglePIDF Angle PIDF configuration. - * @param velocityPIDF Velocity PIDF configuration. - * @param physicalCharacteristics Physical characteristics of the swerve module. - * @param name The name for the swerve module. - * @param conversionFactors Conversion factors to be applied to the drive and angle motors. - */ - public SwerveModuleConfiguration( - SwerveMotor driveMotor, - SwerveMotor angleMotor, - MotorConfigDouble conversionFactors, - SwerveAbsoluteEncoder absoluteEncoder, - double angleOffset, - double xMeters, - double yMeters, - PIDFConfig anglePIDF, - PIDFConfig velocityPIDF, - SwerveModulePhysicalCharacteristics physicalCharacteristics, - boolean absoluteEncoderInverted, - boolean driveMotorInverted, - boolean angleMotorInverted, - String name) { - this.driveMotor = driveMotor; - this.angleMotor = angleMotor; - this.conversionFactors = conversionFactors; - this.absoluteEncoder = absoluteEncoder; - this.angleOffset = angleOffset; - this.absoluteEncoderInverted = absoluteEncoderInverted; - this.driveMotorInverted = driveMotorInverted; - this.angleMotorInverted = angleMotorInverted; - this.moduleLocation = new Translation2d(xMeters, yMeters); - this.anglePIDF = anglePIDF; - this.velocityPIDF = velocityPIDF; - this.physicalCharacteristics = physicalCharacteristics; - this.name = name; - } - - /** - * Construct a configuration object for swerve modules. Assumes the absolute encoder and drive - * motor are not inverted. - * - * @param driveMotor Drive {@link SwerveMotor}. - * @param angleMotor Angle {@link SwerveMotor} - * @param conversionFactors Conversion factors for angle/azimuth motors drive factors. - * @param absoluteEncoder Absolute encoder {@link SwerveAbsoluteEncoder}. - * @param angleOffset Absolute angle offset to 0. - * @param xMeters Module location in meters from the center horizontally. - * @param yMeters Module location in meters from center vertically. - * @param anglePIDF Angle PIDF configuration. - * @param velocityPIDF Velocity PIDF configuration. - * @param physicalCharacteristics Physical characteristics of the swerve module. - * @param name Name for the module. - */ - public SwerveModuleConfiguration( - SwerveMotor driveMotor, - SwerveMotor angleMotor, - MotorConfigDouble conversionFactors, - SwerveAbsoluteEncoder absoluteEncoder, - double angleOffset, - double xMeters, - double yMeters, - PIDFConfig anglePIDF, - PIDFConfig velocityPIDF, - SwerveModulePhysicalCharacteristics physicalCharacteristics, - String name) { - this( - driveMotor, - angleMotor, - conversionFactors, - absoluteEncoder, - angleOffset, - xMeters, - yMeters, - anglePIDF, - velocityPIDF, - physicalCharacteristics, - false, - false, - false, - name); - } -} diff --git a/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java b/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java deleted file mode 100644 index c90c53b..0000000 --- a/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java +++ /dev/null @@ -1,85 +0,0 @@ -package swervelib.parser; - -import swervelib.parser.json.MotorConfigDouble; - -/** Configuration class which stores physical characteristics shared between every swerve module. */ -public class SwerveModulePhysicalCharacteristics { - - /** Current limits for the Swerve Module. */ - public final int driveMotorCurrentLimit, angleMotorCurrentLimit; - - /** The time it takes for the motor to go from 0 to full throttle in seconds. */ - public final double driveMotorRampRate, angleMotorRampRate; - - /** Wheel grip tape coefficient of friction on carpet, as described by the vendor. */ - public final double wheelGripCoefficientOfFriction; - - /** The voltage to use for the smart motor voltage compensation. */ - public double optimalVoltage; - - /** - * The conversion factors for the drive and angle motors, created by {@link - * swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and {@link - * swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. - */ - public MotorConfigDouble conversionFactor; - - /** - * Construct the swerve module physical characteristics. - * - * @param conversionFactors The conversion factors for the drive and angle motors, created by - * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and - * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. - * @param wheelGripCoefficientOfFriction Wheel grip coefficient of friction on carpet given by - * manufacturer. - * @param optimalVoltage Optimal robot voltage. - * @param driveMotorCurrentLimit Current limit for the drive motor. - * @param angleMotorCurrentLimit Current limit for the angle motor. - * @param driveMotorRampRate The time in seconds to go from 0 to full throttle on the motor. - * (Prevents over drawing power from battery) - * @param angleMotorRampRate The time in seconds to go from 0 to full throttle on the motor. - * (Prevents overdrawing power and power loss). - */ - public SwerveModulePhysicalCharacteristics( - MotorConfigDouble conversionFactors, - double wheelGripCoefficientOfFriction, - double optimalVoltage, - int driveMotorCurrentLimit, - int angleMotorCurrentLimit, - double driveMotorRampRate, - double angleMotorRampRate) { - this.wheelGripCoefficientOfFriction = wheelGripCoefficientOfFriction; - this.optimalVoltage = optimalVoltage; - - this.conversionFactor = conversionFactors; - // Set the conversion factors to null if they are both 0. - if (conversionFactors != null) { - if (conversionFactors.angle == 0 && conversionFactors.drive == 0) { - this.conversionFactor = null; - } - } - - this.driveMotorCurrentLimit = driveMotorCurrentLimit; - this.angleMotorCurrentLimit = angleMotorCurrentLimit; - this.driveMotorRampRate = driveMotorRampRate; - this.angleMotorRampRate = angleMotorRampRate; - } - - /** - * Construct the swerve module physical characteristics. Assume coefficient of friction is 1.19 - * (taken from blue nitrile on carpet from Studica) and optimal voltage is 12v. Assumes the drive - * motor current limit is 40A, and the angle motor current limit is 20A. - * - * @param conversionFactors The conversion factors for the drive and angle motors, created by - * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and - * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. - * @param driveMotorRampRate The time in seconds to go from 0 to full throttle on the motor. - * (Prevents over drawing power from battery) - * @param angleMotorRampRate The time in seconds to go from 0 to full throttle on the motor. - * (Prevents overdrawing power and power loss). - */ - public SwerveModulePhysicalCharacteristics( - MotorConfigDouble conversionFactors, double driveMotorRampRate, double angleMotorRampRate) { - this(conversionFactors, 1.19, 12, 40, 20, driveMotorRampRate, angleMotorRampRate); - } -} diff --git a/src/main/java/swervelib/parser/SwerveParser.java b/src/main/java/swervelib/parser/SwerveParser.java deleted file mode 100644 index 9c05ee5..0000000 --- a/src/main/java/swervelib/parser/SwerveParser.java +++ /dev/null @@ -1,213 +0,0 @@ -package swervelib.parser; - -import com.fasterxml.jackson.databind.JsonNode; -import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import java.io.File; -import java.io.IOException; -import java.util.HashMap; -import swervelib.SwerveDrive; -import swervelib.SwerveModule; -import swervelib.math.SwerveMath; -import swervelib.parser.json.ControllerPropertiesJson; -import swervelib.parser.json.ModuleJson; -import swervelib.parser.json.PIDFPropertiesJson; -import swervelib.parser.json.PhysicalPropertiesJson; -import swervelib.parser.json.SwerveDriveJson; - -/** Helper class used to parse the JSON directory with specified configuration options. */ -public class SwerveParser { - - /** Module number mapped to the JSON name. */ - private static final HashMap moduleConfigs = new HashMap<>(); - - /** Parsed swervedrive.json */ - public static SwerveDriveJson swerveDriveJson; - - /** Parsed controllerproperties.json */ - public static ControllerPropertiesJson controllerPropertiesJson; - - /** Parsed modules/pidfproperties.json */ - public static PIDFPropertiesJson pidfPropertiesJson; - - /** Parsed modules/physicalproperties.json */ - public static PhysicalPropertiesJson physicalPropertiesJson; - - /** Array holding the module jsons given in {@link SwerveDriveJson}. */ - public static ModuleJson[] moduleJsons; - - /** - * Construct a swerve parser. Will throw an error if there is a missing file. - * - * @param directory Directory with swerve configurations. - * @throws IOException if a file doesn't exist. - */ - public SwerveParser(File directory) throws IOException { - checkDirectory(directory); - swerveDriveJson = - new ObjectMapper() - .readValue(new File(directory, "swervedrive.json"), SwerveDriveJson.class); - controllerPropertiesJson = - new ObjectMapper() - .readValue( - new File(directory, "controllerproperties.json"), ControllerPropertiesJson.class); - pidfPropertiesJson = - new ObjectMapper() - .readValue( - new File(directory, "modules/pidfproperties.json"), PIDFPropertiesJson.class); - physicalPropertiesJson = - new ObjectMapper() - .readValue( - new File(directory, "modules/physicalproperties.json"), - PhysicalPropertiesJson.class); - moduleJsons = new ModuleJson[swerveDriveJson.modules.length]; - for (int i = 0; i < moduleJsons.length; i++) { - moduleConfigs.put(swerveDriveJson.modules[i], i); - File moduleFile = new File(directory, "modules/" + swerveDriveJson.modules[i]); - assert moduleFile.exists(); - moduleJsons[i] = new ObjectMapper().readValue(moduleFile, ModuleJson.class); - } - } - - /** - * Get the swerve module by the json name. - * - * @param name JSON name. - * @param driveConfiguration {@link SwerveDriveConfiguration} to pull from. - * @return {@link SwerveModuleConfiguration} based on the file. - */ - public static SwerveModule getModuleConfigurationByName( - String name, SwerveDriveConfiguration driveConfiguration) { - return driveConfiguration.modules[moduleConfigs.get(name + ".json")]; - } - - /** - * Open JSON file. - * - * @param file JSON File to open. - * @return JsonNode of file. - */ - private JsonNode openJson(File file) { - try { - return new ObjectMapper().readTree(file); - } catch (IOException e) { - throw new RuntimeException(e); - } - } - - /** - * Check directory structure. - * - * @param directory JSON Configuration Directory - */ - private void checkDirectory(File directory) { - assert new File(directory, "swervedrive.json").exists(); - assert new File(directory, "controllerproperties.json").exists(); - assert new File(directory, "modules").exists() && new File(directory, "modules").isDirectory(); - assert new File(directory, "modules/pidfproperties.json").exists(); - assert new File(directory, "modules/physicalproperties.json").exists(); - } - - /** - * Create {@link SwerveDrive} from JSON configuration directory. - * - * @param maxSpeed Maximum speed of the robot in meters per second, used for both angular - * acceleration used in {@link swervelib.SwerveController} and drive feedforward in {@link - * SwerveMath#createDriveFeedforward(double, double, double)}. - * @return {@link SwerveDrive} instance. - */ - public SwerveDrive createSwerveDrive(double maxSpeed) { - return createSwerveDrive( - SwerveMath.createDriveFeedforward( - physicalPropertiesJson.optimalVoltage, - maxSpeed, - physicalPropertiesJson.wheelGripCoefficientOfFriction), - maxSpeed); - } - - /** - * Create {@link SwerveDrive} from JSON configuration directory. - * - * @param maxSpeed Maximum speed of the robot in meters per second, used for both angular - * acceleration used in {@link swervelib.SwerveController} and drive feedforward in {@link - * SwerveMath#createDriveFeedforward(double, double, double)}. - * @param angleMotorConversionFactor Angle (AKA azimuth) motor conversion factor to convert motor - * controller PID loop units to degrees, usually created using {@link - * SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. - * @param driveMotorConversion Drive motor conversion factor to convert motor controller PID loop - * units to meters per rotation, usually created using {@link - * SwerveMath#calculateMetersPerRotation(double, double, double)}. - * @return {@link SwerveDrive} instance. - */ - public SwerveDrive createSwerveDrive( - double maxSpeed, double angleMotorConversionFactor, double driveMotorConversion) { - physicalPropertiesJson.conversionFactor.angle = angleMotorConversionFactor; - physicalPropertiesJson.conversionFactor.drive = driveMotorConversion; - return createSwerveDrive( - SwerveMath.createDriveFeedforward( - physicalPropertiesJson.optimalVoltage, - maxSpeed, - physicalPropertiesJson.wheelGripCoefficientOfFriction), - maxSpeed); - } - - /** - * Create {@link SwerveDrive} from JSON configuration directory. - * - * @param driveFeedforward Drive feedforward to use for swerve modules, should be created using - * {@link swervelib.math.SwerveMath#createDriveFeedforward(double, double, double)}. - * @param maxSpeed Maximum speed of the robot in meters per second for normal+angular acceleration - * in {@link swervelib.SwerveController} of the robot. - * @param angleMotorConversionFactor Angle (AKA azimuth) motor conversion factor to convert motor - * controller PID loop units to degrees, usually created using {@link - * SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. - * @param driveMotorConversion Drive motor conversion factor to convert motor controller PID loop - * units to meters per rotation, usually created using {@link - * SwerveMath#calculateMetersPerRotation(double, double, double)}. - * @return {@link SwerveDrive} instance. - */ - public SwerveDrive createSwerveDrive( - SimpleMotorFeedforward driveFeedforward, - double maxSpeed, - double angleMotorConversionFactor, - double driveMotorConversion) { - physicalPropertiesJson.conversionFactor.angle = angleMotorConversionFactor; - physicalPropertiesJson.conversionFactor.drive = driveMotorConversion; - return createSwerveDrive(driveFeedforward, maxSpeed); - } - - /** - * Create {@link SwerveDrive} from JSON configuration directory. - * - * @param driveFeedforward Drive feedforward to use for swerve modules, should be created using - * {@link swervelib.math.SwerveMath#createDriveFeedforward(double, double, double)}. - * @param maxSpeed Maximum speed of the robot in meters per second for normal+angular acceleration - * in {@link swervelib.SwerveController} of the robot - * @return {@link SwerveDrive} instance. - */ - public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, double maxSpeed) { - SwerveModuleConfiguration[] moduleConfigurations = - new SwerveModuleConfiguration[moduleJsons.length]; - for (int i = 0; i < moduleConfigurations.length; i++) { - ModuleJson module = moduleJsons[i]; - moduleConfigurations[i] = - module.createModuleConfiguration( - pidfPropertiesJson.angle, - pidfPropertiesJson.drive, - physicalPropertiesJson.createPhysicalProperties(), - swerveDriveJson.modules[i]); - } - SwerveDriveConfiguration swerveDriveConfiguration = - new SwerveDriveConfiguration( - moduleConfigurations, - swerveDriveJson.imu.createIMU(), - swerveDriveJson.invertedIMU, - driveFeedforward, - physicalPropertiesJson.createPhysicalProperties()); - - return new SwerveDrive( - swerveDriveConfiguration, - controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration, maxSpeed), - maxSpeed); - } -} diff --git a/src/main/java/swervelib/parser/deserializer/PIDFRange.java b/src/main/java/swervelib/parser/deserializer/PIDFRange.java deleted file mode 100644 index 056cb3e..0000000 --- a/src/main/java/swervelib/parser/deserializer/PIDFRange.java +++ /dev/null @@ -1,11 +0,0 @@ -package swervelib.parser.deserializer; - -/** Class to hold the minimum and maximum input or output of the PIDF. */ -public class PIDFRange { - - /** Minimum value. */ - public double min = -1; - - /** Maximum value. */ - public double max = 1; -} diff --git a/src/main/java/swervelib/parser/deserializer/package-info.java b/src/main/java/swervelib/parser/deserializer/package-info.java deleted file mode 100644 index 8a17665..0000000 --- a/src/main/java/swervelib/parser/deserializer/package-info.java +++ /dev/null @@ -1,2 +0,0 @@ -/** Deserialize specific variables for outside the parser. */ -package swervelib.parser.deserializer; diff --git a/src/main/java/swervelib/parser/json/ControllerPropertiesJson.java b/src/main/java/swervelib/parser/json/ControllerPropertiesJson.java deleted file mode 100644 index 611a749..0000000 --- a/src/main/java/swervelib/parser/json/ControllerPropertiesJson.java +++ /dev/null @@ -1,31 +0,0 @@ -package swervelib.parser.json; - -import swervelib.parser.PIDFConfig; -import swervelib.parser.SwerveControllerConfiguration; -import swervelib.parser.SwerveDriveConfiguration; - -/** {@link swervelib.SwerveController} parsed class. Used to access the JSON data. */ -public class ControllerPropertiesJson { - - /** - * The minimum radius of the angle control joystick to allow for heading adjustment of the robot. - */ - public double angleJoystickRadiusDeadband; - - /** The PID used to control the robot heading. */ - public PIDFConfig heading; - - /** - * Create the {@link SwerveControllerConfiguration} based on parsed and given data. - * - * @param driveConfiguration {@link SwerveDriveConfiguration} parsed configuration. - * @param maxSpeedMPS Maximum speed in meters per second for the angular acceleration of the - * robot. - * @return {@link SwerveControllerConfiguration} object based on parsed data. - */ - public SwerveControllerConfiguration createControllerConfiguration( - SwerveDriveConfiguration driveConfiguration, double maxSpeedMPS) { - return new SwerveControllerConfiguration( - driveConfiguration, heading, angleJoystickRadiusDeadband, maxSpeedMPS); - } -} diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java deleted file mode 100644 index 7cb6d11..0000000 --- a/src/main/java/swervelib/parser/json/DeviceJson.java +++ /dev/null @@ -1,187 +0,0 @@ -package swervelib.parser.json; - -import com.revrobotics.SparkRelativeEncoder.Type; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.I2C; -import edu.wpi.first.wpilibj.SPI; -import edu.wpi.first.wpilibj.SerialPort.Port; -import swervelib.encoders.AnalogAbsoluteEncoderSwerve; -import swervelib.encoders.CANCoderSwerve; -import swervelib.encoders.CanAndCoderSwerve; -import swervelib.encoders.PWMDutyCycleEncoderSwerve; -import swervelib.encoders.SparkMaxAnalogEncoderSwerve; -import swervelib.encoders.SparkMaxEncoderSwerve; -import swervelib.encoders.SwerveAbsoluteEncoder; -import swervelib.imu.ADIS16448Swerve; -import swervelib.imu.ADIS16470Swerve; -import swervelib.imu.ADXRS450Swerve; -import swervelib.imu.AnalogGyroSwerve; -import swervelib.imu.NavXSwerve; -import swervelib.imu.Pigeon2Swerve; -import swervelib.imu.PigeonSwerve; -import swervelib.imu.SwerveIMU; -import swervelib.motors.SparkFlexSwerve; -import swervelib.motors.SparkMaxBrushedMotorSwerve; -import swervelib.motors.SparkMaxSwerve; -import swervelib.motors.SwerveMotor; -import swervelib.motors.TalonFXSwerve; -import swervelib.motors.TalonSRXSwerve; -import swervelib.telemetry.Alert; - -/** Device JSON parsed class. Used to access the JSON data. */ -public class DeviceJson { - - /** An {@link Alert} for if the CAN ID is greater than 40. */ - private final Alert canIdWarning = - new Alert( - "JSON", - "CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", - Alert.AlertType.WARNING); - - /** An {@link Alert} for if there is an I2C lockup issue on the roboRIO. */ - private final Alert i2cLockupWarning = - new Alert( - "IMU", - "I2C lockup issue detected on roboRIO. Check console for more information.", - Alert.AlertType.WARNING); - - /** The device type, e.g. pigeon/pigeon2/sparkmax/talonfx/navx */ - public String type; - - /** The CAN ID or pin ID of the device. */ - public int id; - - /** The CAN bus name which the device resides on if using CAN. */ - public String canbus = ""; - - /** - * Create a {@link SwerveAbsoluteEncoder} from the current configuration. - * - * @param motor {@link SwerveMotor} of which attached encoders will be created from, only used - * when the type is "attached" or "canandencoder". - * @return {@link SwerveAbsoluteEncoder} given. - */ - public SwerveAbsoluteEncoder createEncoder(SwerveMotor motor) { - if (id > 40) { - canIdWarning.set(true); - } - switch (type) { - case "none": - return null; - case "integrated": - case "attached": - return new SparkMaxEncoderSwerve(motor, 1); - case "sparkmax_analog": - return new SparkMaxAnalogEncoderSwerve(motor); - case "canandcoder": - return new SparkMaxEncoderSwerve(motor, 360); - case "canandcoder_can": - return new CanAndCoderSwerve(id); - case "ctre_mag": - case "rev_hex": - case "throughbore": - case "am_mag": - case "dutycycle": - return new PWMDutyCycleEncoderSwerve(id); - case "thrifty": - case "ma3": - case "analog": - return new AnalogAbsoluteEncoderSwerve(id); - case "cancoder": - return new CANCoderSwerve(id, canbus != null ? canbus : ""); - default: - throw new RuntimeException(type + " is not a recognized absolute encoder type."); - } - } - - /** - * Create a {@link SwerveIMU} from the given configuration. - * - * @return {@link SwerveIMU} given. - */ - public SwerveIMU createIMU() { - if (id > 40) { - canIdWarning.set(true); - } - switch (type) { - case "adis16448": - return new ADIS16448Swerve(); - case "adis16470": - return new ADIS16470Swerve(); - case "adxrs450": - return new ADXRS450Swerve(); - case "analog": - return new AnalogGyroSwerve(id); - case "navx": - case "navx_spi": - return new NavXSwerve(SPI.Port.kMXP); - case "navx_i2c": - DriverStation.reportWarning( - "WARNING: There exists an I2C lockup issue on the roboRIO that could occur, more information here: " - + "\nhttps://docs.wpilib.org/en/stable/docs/yearly-overview/known-issues" - + ".html#onboard-i2c-causing-system-lockups", - false); - i2cLockupWarning.set(true); - return new NavXSwerve(I2C.Port.kMXP); - case "navx_usb": - return new NavXSwerve(Port.kUSB); - case "navx_mxp": - return new NavXSwerve(Port.kMXP); - case "pigeon": - return new PigeonSwerve(id); - case "pigeon2": - return new Pigeon2Swerve(id, canbus != null ? canbus : ""); - default: - throw new RuntimeException(type + " is not a recognized imu/gyroscope type."); - } - } - - /** - * Create a {@link SwerveMotor} from the given configuration. - * - * @param isDriveMotor If the motor being generated is a drive motor. - * @return {@link SwerveMotor} given. - */ - public SwerveMotor createMotor(boolean isDriveMotor) { - if (id > 40) { - canIdWarning.set(true); - } - switch (type) { - case "sparkmax_brushed": - switch (canbus) { - case "greyhill_63r256": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, false); - case "srx_mag_encoder": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, false); - case "throughbore": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 8192, false); - case "throughbore_dataport": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 8192, true); - case "greyhill_63r256_dataport": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, true); - case "srx_mag_encoder_dataport": - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, true); - default: - if (isDriveMotor) { - throw new RuntimeException( - "Spark MAX " + id + " MUST have a encoder attached to the motor controller."); - } - // We are creating a motor for an angle motor which will use the absolute encoder - // attached to the data port. - return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 0, false); - } - case "neo": - case "sparkmax": - return new SparkMaxSwerve(id, isDriveMotor); - case "sparkflex": - return new SparkFlexSwerve(id, isDriveMotor); - case "falcon": - case "talonfx": - return new TalonFXSwerve(id, canbus != null ? canbus : "", isDriveMotor); - case "talonsrx": - return new TalonSRXSwerve(id, isDriveMotor); - default: - throw new RuntimeException(type + " is not a recognized motor type."); - } - } -} diff --git a/src/main/java/swervelib/parser/json/ModuleJson.java b/src/main/java/swervelib/parser/json/ModuleJson.java deleted file mode 100644 index cd12166..0000000 --- a/src/main/java/swervelib/parser/json/ModuleJson.java +++ /dev/null @@ -1,124 +0,0 @@ -package swervelib.parser.json; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.MotorFeedbackSensor; -import edu.wpi.first.math.util.Units; -import swervelib.encoders.SwerveAbsoluteEncoder; -import swervelib.motors.SwerveMotor; -import swervelib.parser.PIDFConfig; -import swervelib.parser.SwerveModuleConfiguration; -import swervelib.parser.SwerveModulePhysicalCharacteristics; -import swervelib.parser.json.modules.BoolMotorJson; -import swervelib.parser.json.modules.LocationJson; - -/** {@link swervelib.SwerveModule} JSON parsed class. Used to access the JSON data. */ -public class ModuleJson { - - /** Drive motor device configuration. */ - public DeviceJson drive; - - /** Angle motor device configuration. */ - public DeviceJson angle; - - /** - * Conversion factor for the module, if different from the one in swervedrive.json - * - *

Conversion factor applied to the motor controllers PID loops. Can be calculated with {@link - * swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} for angle motors - * or {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} for - * drive motors. - */ - public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0); - - /** Absolute encoder device configuration. */ - public DeviceJson encoder; - - /** Defines which motors are inverted. */ - public BoolMotorJson inverted; - - /** Absolute encoder offset from 0 in degrees. */ - public double absoluteEncoderOffset; - - /** Absolute encoder inversion state. */ - public boolean absoluteEncoderInverted = false; - - /** The location of the swerve module from the center of the robot in inches. */ - public LocationJson location; - - /** - * Create the swerve module configuration based off of parsed data. - * - * @param anglePIDF The PIDF values for the angle motor. - * @param velocityPIDF The velocity PIDF values for the drive motor. - * @param physicalCharacteristics Physical characteristics of the swerve module. - * @param name Module json filename. - * @return {@link SwerveModuleConfiguration} based on the provided data and parsed data. - */ - public SwerveModuleConfiguration createModuleConfiguration( - PIDFConfig anglePIDF, - PIDFConfig velocityPIDF, - SwerveModulePhysicalCharacteristics physicalCharacteristics, - String name) { - SwerveMotor angleMotor = angle.createMotor(false); - SwerveAbsoluteEncoder absEncoder = encoder.createEncoder(angleMotor); - - // If the absolute encoder is attached. - if (absEncoder != null && angleMotor.getMotor() instanceof CANSparkMax) { - if (absEncoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) { - angleMotor.setAbsoluteEncoder(absEncoder); - } - } - - // Set the conversion factors to null if they are both 0. - if (this.conversionFactor != null) { - if (this.conversionFactor.angle == 0 && this.conversionFactor.drive == 0) { - this.conversionFactor = null; - } - } - - if (this.conversionFactor == null && physicalCharacteristics.conversionFactor == null) { - throw new RuntimeException( - "No Conversion Factor configured! Please create SwerveDrive using \n" - + "SwerveParser.createSwerveDrive(driveFeedforward, maxSpeed, angleMotorConversionFactor, driveMotorConversion)\n" - + "OR\n" - + "SwerveParser.createSwerveDrive(maxSpeed, angleMotorConversionFactor, driveMotorConversion)\n" - + "OR\n" - + "Set the conversion factor in physicalproperties.json OR the module JSON file." - + "REMEMBER: You can calculate the conversion factors using SwerveMath.calculateMetersPerRotation AND SwerveMath.calculateDegreesPerSteeringRotation\n"); - } else if (physicalCharacteristics.conversionFactor != null && this.conversionFactor == null) { - this.conversionFactor = physicalCharacteristics.conversionFactor; - } else if (physicalCharacteristics.conversionFactor - != null) // If both are defined, override 0 with the physical characterstics input. - { - this.conversionFactor.angle = - this.conversionFactor.angle == 0 - ? physicalCharacteristics.conversionFactor.angle - : this.conversionFactor.angle; - this.conversionFactor.drive = - this.conversionFactor.drive == 0 - ? physicalCharacteristics.conversionFactor.drive - : this.conversionFactor.drive; - } - - if (this.conversionFactor.drive == 0 || this.conversionFactor.angle == 0) { - throw new RuntimeException( - "Conversion factors cannot be 0, please configure conversion factors in physicalproperties.json or the module JSON files."); - } - - return new SwerveModuleConfiguration( - drive.createMotor(true), - angleMotor, - conversionFactor, - absEncoder, - absoluteEncoderOffset, - Units.inchesToMeters(Math.round(location.x) == 0 ? location.front : location.x), - Units.inchesToMeters(Math.round(location.y) == 0 ? location.left : location.y), - anglePIDF, - velocityPIDF, - physicalCharacteristics, - absoluteEncoderInverted, - inverted.drive, - inverted.angle, - name.replaceAll("\\.json", "")); - } -} diff --git a/src/main/java/swervelib/parser/json/MotorConfigDouble.java b/src/main/java/swervelib/parser/json/MotorConfigDouble.java deleted file mode 100644 index 29cdc77..0000000 --- a/src/main/java/swervelib/parser/json/MotorConfigDouble.java +++ /dev/null @@ -1,25 +0,0 @@ -package swervelib.parser.json; - -/** Used to store doubles for motor configuration. */ -public class MotorConfigDouble { - - /** Drive motor. */ - public double drive; - - /** Angle motor. */ - public double angle; - - /** Default constructor. */ - public MotorConfigDouble() {} - - /** - * Default constructor. - * - * @param angle Angle data. - * @param drive Drive data. - */ - public MotorConfigDouble(double angle, double drive) { - this.angle = angle; - this.drive = drive; - } -} diff --git a/src/main/java/swervelib/parser/json/MotorConfigInt.java b/src/main/java/swervelib/parser/json/MotorConfigInt.java deleted file mode 100644 index 59b0ffb..0000000 --- a/src/main/java/swervelib/parser/json/MotorConfigInt.java +++ /dev/null @@ -1,25 +0,0 @@ -package swervelib.parser.json; - -/** Used to store ints for motor configuration. */ -public class MotorConfigInt { - - /** Drive motor. */ - public int drive; - - /** Angle motor. */ - public int angle; - - /** Default constructor. */ - public MotorConfigInt() {} - - /** - * Default constructor with values. - * - * @param drive Drive data. - * @param angle Angle data. - */ - public MotorConfigInt(int drive, int angle) { - this.angle = angle; - this.drive = drive; - } -} diff --git a/src/main/java/swervelib/parser/json/PIDFPropertiesJson.java b/src/main/java/swervelib/parser/json/PIDFPropertiesJson.java deleted file mode 100644 index 4e4c8b4..0000000 --- a/src/main/java/swervelib/parser/json/PIDFPropertiesJson.java +++ /dev/null @@ -1,13 +0,0 @@ -package swervelib.parser.json; - -import swervelib.parser.PIDFConfig; - -/** {@link swervelib.SwerveModule} PID with Feedforward for the drive motor and angle motor. */ -public class PIDFPropertiesJson { - - /** The PIDF with Integral Zone used for the drive motor. */ - public PIDFConfig drive; - - /** The PIDF with Integral Zone used for the angle motor. */ - public PIDFConfig angle; -} diff --git a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java deleted file mode 100644 index 0bfb5ef..0000000 --- a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java +++ /dev/null @@ -1,49 +0,0 @@ -package swervelib.parser.json; - -import swervelib.parser.SwerveModulePhysicalCharacteristics; - -/** - * {@link swervelib.parser.SwerveModulePhysicalCharacteristics} parsed data. Used to configure the - * SwerveModule. - */ -public class PhysicalPropertiesJson { - - /** - * Conversion factor applied to the motor controllers PID loops. Can be calculated with {@link - * swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} for angle motors - * or {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} for - * drive motors. - */ - public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0); - - /** The current limit in AMPs to apply to the motors. */ - public MotorConfigInt currentLimit = new MotorConfigInt(40, 20); - - /** The minimum number of seconds to take for the motor to go from 0 to full throttle. */ - public MotorConfigDouble rampRate = new MotorConfigDouble(0.25, 0.25); - - /** - * The grip tape coefficient of friction on carpet. Used to calculate the practical maximum - * acceleration. - */ - public double wheelGripCoefficientOfFriction = 1.19; - - /** The voltage to use for the smart motor voltage compensation, default is 12. */ - public double optimalVoltage = 12; - - /** - * Create the physical characteristics based off the parsed data. - * - * @return {@link SwerveModulePhysicalCharacteristics} based on parsed data. - */ - public SwerveModulePhysicalCharacteristics createPhysicalProperties() { - return new SwerveModulePhysicalCharacteristics( - conversionFactor, - wheelGripCoefficientOfFriction, - optimalVoltage, - currentLimit.drive, - currentLimit.angle, - rampRate.drive, - rampRate.angle); - } -} diff --git a/src/main/java/swervelib/parser/json/SwerveDriveJson.java b/src/main/java/swervelib/parser/json/SwerveDriveJson.java deleted file mode 100644 index 995050f..0000000 --- a/src/main/java/swervelib/parser/json/SwerveDriveJson.java +++ /dev/null @@ -1,17 +0,0 @@ -package swervelib.parser.json; - -/** - * {@link swervelib.SwerveDrive} JSON parsed class. Used to access parsed data from the - * swervedrive.json file. - */ -public class SwerveDriveJson { - - /** Robot IMU used to determine heading of the robot. */ - public DeviceJson imu; - - /** Invert the IMU of the robot. */ - public boolean invertedIMU; - - /** Module JSONs in order clockwise order starting from front left. */ - public String[] modules; -} diff --git a/src/main/java/swervelib/parser/json/modules/BoolMotorJson.java b/src/main/java/swervelib/parser/json/modules/BoolMotorJson.java deleted file mode 100644 index 47592a7..0000000 --- a/src/main/java/swervelib/parser/json/modules/BoolMotorJson.java +++ /dev/null @@ -1,11 +0,0 @@ -package swervelib.parser.json.modules; - -/** Inverted motor JSON parsed class. Used to access the JSON data. */ -public class BoolMotorJson { - - /** Drive motor inversion state. */ - public boolean drive; - - /** Angle motor inversion state. */ - public boolean angle; -} diff --git a/src/main/java/swervelib/parser/json/modules/LocationJson.java b/src/main/java/swervelib/parser/json/modules/LocationJson.java deleted file mode 100644 index d356604..0000000 --- a/src/main/java/swervelib/parser/json/modules/LocationJson.java +++ /dev/null @@ -1,15 +0,0 @@ -package swervelib.parser.json.modules; - -/** - * Location JSON parsed class. Used to access the JSON data. Module locations, in inches, as - * distances to the center of the robot. +x is towards the robot front, and +y is towards robot - * left. - */ -public class LocationJson { - - /** Location of the swerve module in inches from the center of the robot horizontally. */ - public double front = 0, x = 0; - - /** Location of the swerve module in inches from the center of the robot vertically. */ - public double left = 0, y = 0; -} diff --git a/src/main/java/swervelib/parser/json/modules/package-info.java b/src/main/java/swervelib/parser/json/modules/package-info.java deleted file mode 100644 index f6e0ab7..0000000 --- a/src/main/java/swervelib/parser/json/modules/package-info.java +++ /dev/null @@ -1,2 +0,0 @@ -/** JSON Mapped Configuration types for modules. */ -package swervelib.parser.json.modules; diff --git a/src/main/java/swervelib/parser/json/package-info.java b/src/main/java/swervelib/parser/json/package-info.java deleted file mode 100644 index 776e415..0000000 --- a/src/main/java/swervelib/parser/json/package-info.java +++ /dev/null @@ -1,2 +0,0 @@ -/** JSON Mapped classes for parsing configuration files. */ -package swervelib.parser.json; diff --git a/src/main/java/swervelib/parser/package-info.java b/src/main/java/swervelib/parser/package-info.java deleted file mode 100644 index 60372be..0000000 --- a/src/main/java/swervelib/parser/package-info.java +++ /dev/null @@ -1,2 +0,0 @@ -/** JSON Parser for YAGSL configurations. */ -package swervelib.parser; diff --git a/src/main/java/swervelib/simulation/SwerveIMUSimulation.java b/src/main/java/swervelib/simulation/SwerveIMUSimulation.java deleted file mode 100644 index 1f4e55b..0000000 --- a/src/main/java/swervelib/simulation/SwerveIMUSimulation.java +++ /dev/null @@ -1,106 +0,0 @@ -package swervelib.simulation; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import java.util.Optional; - -/** Simulation for {@link swervelib.SwerveDrive} IMU. */ -public class SwerveIMUSimulation { - - /** Main timer to control movement estimations. */ - private final Timer timer; - - /** The last time the timer was read, used to determine position changes. */ - private double lastTime; - - /** Heading of the robot. */ - private double angle; - - /** Create the swerve drive IMU simulation. */ - public SwerveIMUSimulation() { - timer = new Timer(); - timer.start(); - lastTime = timer.get(); - } - - /** - * Get the estimated angle of the robot. - * - * @return {@link Rotation2d} estimation of the robot. - */ - public Rotation2d getYaw() { - return new Rotation2d(angle); - } - - /** - * Pitch is not simulated currently, always returns 0. - * - * @return Pitch of the robot as {@link Rotation2d}. - */ - public Rotation2d getPitch() { - return new Rotation2d(); - } - - /** - * Roll is not simulated currently, always returns 0. - * - * @return Roll of the robot as {@link Rotation2d}. - */ - public Rotation2d getRoll() { - return new Rotation2d(); - } - - /** - * Gets the estimated gyro {@link Rotation3d} of the robot. - * - * @return The heading as a {@link Rotation3d} angle - */ - public Rotation3d getGyroRotation3d() { - return new Rotation3d(0, 0, angle); - } - - /** - * Fetch the acceleration [x, y, z] from the IMU in m/s/s. If acceleration isn't supported returns - * empty. - * - * @return {@link Translation3d} of the acceleration as an {@link Optional}. - */ - public Optional getAccel() { - return Optional.empty(); - } - - /** - * Update the odometry of the simulated {@link swervelib.SwerveDrive} and post the {@link - * swervelib.SwerveModule} states to the {@link Field2d}. - * - * @param kinematics {@link SwerveDriveKinematics} of the swerve drive. - * @param states {@link SwerveModuleState} array of the module states. - * @param modulePoses {@link Pose2d} representing the swerve modules. - * @param field {@link Field2d} to update. - */ - public void updateOdometry( - SwerveDriveKinematics kinematics, - SwerveModuleState[] states, - Pose2d[] modulePoses, - Field2d field) { - - angle += kinematics.toChassisSpeeds(states).omegaRadiansPerSecond * (timer.get() - lastTime); - lastTime = timer.get(); - field.getObject("XModules").setPoses(modulePoses); - } - - /** - * Set the heading of the robot. - * - * @param angle Angle of the robot in radians. - */ - public void setAngle(double angle) { - this.angle = angle; - } -} diff --git a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java deleted file mode 100644 index 2dbde61..0000000 --- a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java +++ /dev/null @@ -1,75 +0,0 @@ -package swervelib.simulation; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.Timer; - -/** Class to hold simulation data for {@link swervelib.SwerveModule} */ -public class SwerveModuleSimulation { - - /** Main timer to simulate the passage of time. */ - private final Timer timer; - - /** Time delta since last update */ - private double dt; - - /** Fake motor position. */ - private double fakePos; - - /** - * The fake speed of the previous state, used to calculate {@link SwerveModuleSimulation#fakePos}. - */ - private double fakeSpeed; - - /** Last time queried. */ - private double lastTime; - - /** Current simulated swerve module state. */ - private SwerveModuleState state; - - /** Create simulation class and initialize module at 0. */ - public SwerveModuleSimulation() { - timer = new Timer(); - timer.start(); - lastTime = timer.get(); - state = new SwerveModuleState(0, Rotation2d.fromDegrees(0)); - fakeSpeed = 0; - fakePos = 0; - dt = 0; - } - - /** - * Update the position and state of the module. Called from {@link - * swervelib.SwerveModule#setDesiredState} function when simulated. - * - * @param desiredState State the swerve module is set to. - */ - public void updateStateAndPosition(SwerveModuleState desiredState) { - dt = timer.get() - lastTime; - lastTime = timer.get(); - - state = desiredState; - fakeSpeed = desiredState.speedMetersPerSecond; - fakePos += (fakeSpeed * dt); - } - - /** - * Get the simulated swerve module position. - * - * @return {@link SwerveModulePosition} of the simulated module. - */ - public SwerveModulePosition getPosition() { - - return new SwerveModulePosition(fakePos, state.angle); - } - - /** - * Get the {@link SwerveModuleState} of the simulated module. - * - * @return {@link SwerveModuleState} of the simulated module. - */ - public SwerveModuleState getState() { - return state; - } -} diff --git a/src/main/java/swervelib/simulation/package-info.java b/src/main/java/swervelib/simulation/package-info.java deleted file mode 100644 index 908fc59..0000000 --- a/src/main/java/swervelib/simulation/package-info.java +++ /dev/null @@ -1,2 +0,0 @@ -/** Classes used to simulate the swerve drive. */ -package swervelib.simulation; diff --git a/src/main/java/swervelib/telemetry/Alert.java b/src/main/java/swervelib/telemetry/Alert.java deleted file mode 100644 index 21fcd2b..0000000 --- a/src/main/java/swervelib/telemetry/Alert.java +++ /dev/null @@ -1,217 +0,0 @@ -// Copyright (c) 2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by an MIT-style -// license that can be found below. - -// MIT License -// -// Copyright (c) 2023 FRC 6328 -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -package swervelib.telemetry; - -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import java.util.ArrayList; -import java.util.Comparator; -import java.util.HashMap; -import java.util.List; -import java.util.Map; -import java.util.function.Predicate; - -/** Class for managing persistent alerts to be sent over NetworkTables. */ -public class Alert { - - /** Group of the alert. */ - private static Map groups = new HashMap(); - - /** Type of the Alert to raise. */ - private final AlertType type; - - /** Activation state of alert. */ - private boolean active = false; - - /** When the alert was raised. */ - private double activeStartTime = 0.0; - - /** Text of the alert. */ - private String text; - - /** - * Creates a new Alert in the default group - "Alerts". If this is the first to be instantiated, - * the appropriate entries will be added to NetworkTables. - * - * @param text Text to be displayed when the alert is active. - * @param type Alert level specifying urgency. - */ - public Alert(String text, AlertType type) { - this("Alerts", text, type); - } - - /** - * Creates a new Alert. If this is the first to be instantiated in its group, the appropriate - * entries will be added to NetworkTables. - * - * @param group Group identifier, also used as NetworkTables title - * @param text Text to be displayed when the alert is active. - * @param type Alert level specifying urgency. - */ - public Alert(String group, String text, AlertType type) { - if (!groups.containsKey(group)) { - groups.put(group, new SendableAlerts()); - SmartDashboard.putData(group, groups.get(group)); - } - - this.text = text; - this.type = type; - groups.get(group).alerts.add(this); - } - - /** - * Sets whether the alert should currently be displayed. When activated, the alert text will also - * be sent to the console. - * - * @param active Set the alert as active and report it to the driver station. - */ - public void set(boolean active) { - if (active && !this.active) { - activeStartTime = Timer.getFPGATimestamp(); - switch (type) { - case ERROR: - DriverStation.reportError(text, false); - break; - case ERROR_TRACE: - DriverStation.reportError(text, true); - break; - case WARNING: - DriverStation.reportWarning(text, false); - break; - case WARNING_TRACE: - DriverStation.reportWarning(text, true); - break; - case INFO: - System.out.println(text); - break; - } - } - this.active = active; - } - - /** - * Updates current alert text. - * - * @param text The text for the alert. - */ - public void setText(String text) { - if (active && !text.equals(this.text)) { - switch (type) { - case ERROR: - DriverStation.reportError(text, false); - break; - case ERROR_TRACE: - DriverStation.reportError(text, true); - break; - case WARNING: - DriverStation.reportWarning(text, false); - break; - case WARNING_TRACE: - DriverStation.reportWarning(text, true); - break; - case INFO: - System.out.println(text); - break; - } - } - this.text = text; - } - - /** Represents an alert's level of urgency. */ - public static enum AlertType { - /** - * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type - * for problems which will seriously affect the robot's functionality and thus require immediate - * attention. - */ - ERROR, - /** - * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type - * for problems which will seriously affect the robot's functionality and thus require immediate - * attention. Trace printed to driver station console. - */ - ERROR_TRACE, - - /** - * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this - * type for problems which could affect the robot's functionality but do not necessarily require - * immediate attention. - */ - WARNING, - /** - * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this - * type for problems which could affect the robot's functionality but do not necessarily require - * immediate attention. Trace printed to driver station console. - */ - WARNING_TRACE, - /** - * Low priority alert - displayed last on the dashboard with a green "i" symbol. Use this type - * for problems which are unlikely to affect the robot's functionality, or any other alerts - * which do not fall under "ERROR" or "WARNING". - */ - INFO - } - - /** Sendable alert for advantage scope. */ - private static class SendableAlerts implements Sendable { - - /** Alert list for sendable. */ - public final List alerts = new ArrayList<>(); - - /** - * Get alerts based off of type. - * - * @param type Type of alert to fetch. - * @return Active alert strings. - */ - public String[] getStrings(AlertType type) { - Predicate activeFilter = (Alert x) -> x.type == type && x.active; - Comparator timeSorter = - (Alert a1, Alert a2) -> (int) (a2.activeStartTime - a1.activeStartTime); - return alerts.stream() - .filter(activeFilter) - .sorted(timeSorter) - .map((Alert a) -> a.text) - .toArray(String[]::new); - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType("Alerts"); - builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR), null); - builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR_TRACE), null); - builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING), null); - builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING_TRACE), null); - builder.addStringArrayProperty("infos", () -> getStrings(AlertType.INFO), null); - } - } -} diff --git a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java deleted file mode 100644 index 261fc89..0000000 --- a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java +++ /dev/null @@ -1,97 +0,0 @@ -package swervelib.telemetry; - -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -/** - * Telemetry to describe the {@link swervelib.SwerveDrive} following frc-web-components. (Which - * follows AdvantageKit) - */ -public class SwerveDriveTelemetry { - - /** The current telemetry verbosity level. */ - public static TelemetryVerbosity verbosity = TelemetryVerbosity.MACHINE; - - /** State of simulation of the Robot, used to optimize retrieval. */ - public static boolean isSimulation = RobotBase.isSimulation(); - - /** The number of swerve modules */ - public static int moduleCount; - - /** The number of swerve modules */ - public static double[] wheelLocations; - - /** - * An array of rotation and velocity values describing the measured state of each swerve module - */ - public static double[] measuredStates; - - /** An array of rotation and velocity values describing the desired state of each swerve module */ - public static double[] desiredStates; - - /** The robot's current rotation based on odometry or gyro readings */ - public static double robotRotation = 0; - - /** The maximum achievable speed of the modules, used to adjust the size of the vectors. */ - public static double maxSpeed; - - /** The units of the module rotations and robot rotation */ - public static String rotationUnit = "degrees"; - - /** The distance between the left and right modules. */ - public static double sizeLeftRight; - - /** The distance between the front and back modules. */ - public static double sizeFrontBack; - - /** - * The direction the robot should be facing when the "Robot Rotation" is zero or blank. This - * option is often useful to align with odometry data or match videos. 'up', 'right', 'down' or - * 'left' - */ - public static String forwardDirection = "up"; - - /** - * The maximum achievable angular velocity of the robot. This is used to visualize the angular - * velocity from the chassis speeds properties. - */ - public static double maxAngularVelocity; - - /** - * The maximum achievable angular velocity of the robot. This is used to visualize the angular - * velocity from the chassis speeds properties. - */ - public static double[] measuredChassisSpeeds = new double[3]; - - /** Describes the desired forward, sideways and angular velocity of the robot. */ - public static double[] desiredChassisSpeeds = new double[3]; - - /** Upload data to smartdashboard */ - public static void updateData() { - SmartDashboard.putNumber("swerve/moduleCount", moduleCount); - SmartDashboard.putNumberArray("swerve/wheelLocations", wheelLocations); - SmartDashboard.putNumberArray("swerve/measuredStates", measuredStates); - SmartDashboard.putNumberArray("swerve/desiredStates", desiredStates); - SmartDashboard.putNumber("swerve/robotRotation", robotRotation); - SmartDashboard.putNumber("swerve/maxSpeed", maxSpeed); - SmartDashboard.putString("swerve/rotationUnit", rotationUnit); - SmartDashboard.putNumber("swerve/sizeLeftRight", sizeLeftRight); - SmartDashboard.putNumber("swerve/sizeFrontBack", sizeFrontBack); - SmartDashboard.putString("swerve/forwardDirection", forwardDirection); - SmartDashboard.putNumber("swerve/maxAngularVelocity", maxAngularVelocity); - SmartDashboard.putNumberArray("swerve/measuredChassisSpeeds", measuredChassisSpeeds); - SmartDashboard.putNumberArray("swerve/desiredChassisSpeeds", desiredChassisSpeeds); - } - - /** Verbosity of telemetry data sent back. */ - public enum TelemetryVerbosity { - /** No telemetry data is sent back. */ - NONE, - /** Low telemetry data, only post the robot position on the field. */ - LOW, - /** Full swerve drive data is sent back in both human and machine readable forms. */ - HIGH, - /** Only send the machine readable data related to swerve drive. */ - MACHINE - } -} diff --git a/src/main/java/swervelib/telemetry/package-info.java b/src/main/java/swervelib/telemetry/package-info.java deleted file mode 100644 index 2f47093..0000000 --- a/src/main/java/swervelib/telemetry/package-info.java +++ /dev/null @@ -1,2 +0,0 @@ -/** Telemetry package for sending data to NT4 or SmartDashboard. */ -package swervelib.telemetry; diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index f86862c..0bf11fb 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.1.3", + "version": "2024.1.4", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.1.3" + "version": "2024.1.4" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.1.3", + "version": "2024.1.4", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/yagsl.json b/vendordeps/yagsl.json index c6bbccf..2019d20 100644 --- a/vendordeps/yagsl.json +++ b/vendordeps/yagsl.json @@ -1,7 +1,7 @@ { "fileName": "yagsl.json", "name": "YAGSL", - "version": "2024.4.6", + "version": "2024.4.6.2", "frcYear": "2024", "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "swervelib", "artifactId": "YAGSL-java", - "version": "2024.4.6" + "version": "2024.4.6.2" } ], "jniDependencies": [],