From 69b800cb50f95dda7945c3d5487de45cf5922d9e Mon Sep 17 00:00:00 2001 From: Dana Batali Date: Mon, 20 Jan 2020 10:57:46 -0800 Subject: [PATCH 1/2] Integrate SpartronicsLib with this year's project. Integrate Spartronics tests into this year's project. (A few tests still fail). Update Phoenix vendordep to latest and greatest. Update WPI GradleRIO to latest and greatest. Add support for build-tagging. --- build.gradle | 191 ++++--- .../lib/hardware/motors/SensorModel.java | 34 ++ .../hardware/motors/SpartronicsEncoder.java | 41 ++ .../lib/hardware/motors/SpartronicsMax.java | 235 ++++++++ .../lib/hardware/motors/SpartronicsMotor.java | 141 +++++ .../lib/hardware/motors/SpartronicsSRX.java | 228 ++++++++ .../lib/hardware/sensors/A51IRSensor.java | 22 + .../lib/hardware/sensors/IRSensor.java | 41 ++ .../lib/hardware/sensors/RPLidarA1.java | 520 ++++++++++++++++++ .../lib/hardware/sensors/SpartronicsIMU.java | 8 + .../lib/hardware/sensors/T265Camera.java | 249 +++++++++ .../lib/math/PolynomialRegression.java | 204 +++++++ .../com/spartronics4915/lib/math/Util.java | 60 ++ .../twodim/control/FeedForwardTracker.java | 31 ++ .../twodim/control/PurePursuitTracker.java | 108 ++++ .../math/twodim/control/RamseteTracker.java | 66 +++ .../twodim/control/TrajectoryTracker.java | 113 ++++ .../lib/math/twodim/geometry/Pose2d.java | 199 +++++++ .../twodim/geometry/Pose2dWithCurvature.java | 114 ++++ .../lib/math/twodim/geometry/Rectangle2d.java | 158 ++++++ .../lib/math/twodim/geometry/Rotation2d.java | 183 ++++++ .../math/twodim/geometry/Translation2d.java | 160 ++++++ .../lib/math/twodim/geometry/Twist2d.java | 118 ++++ .../lib/math/twodim/lidar/ObjectFinder.java | 229 ++++++++ .../lib/math/twodim/lidar/TargetTracker.java | 120 ++++ .../twodim/physics/DCMotorTransmission.java | 141 +++++ .../twodim/physics/DifferentialDrive.java | 469 ++++++++++++++++ .../twodim/physics/DriveCharacterization.java | 165 ++++++ .../twodim/spline/CubicHermiteSpline.java | 115 ++++ .../twodim/spline/QuinticHermiteSpline.java | 116 ++++ .../lib/math/twodim/spline/Spline.java | 117 ++++ .../lib/math/twodim/spline/SplineHelper.java | 276 ++++++++++ .../twodim/spline/SplineParameterizer.java | 156 ++++++ .../lib/math/twodim/trajectory/Line.java | 35 ++ .../math/twodim/trajectory/PathFinder.java | 267 +++++++++ .../trajectory/TrajectoryGenerator.java | 387 +++++++++++++ .../AngularAccelerationConstraint.java | 71 +++ .../CentripetalAccelerationConstraint.java | 23 + .../DifferentialDriveDynamicsConstraint.java | 33 ++ .../constraints/TimingConstraint.java | 25 + .../VelocityLimitRadiusConstraint.java | 30 + .../VelocityLimitRegionConstraint.java | 28 + .../trajectory/types/DistancedTrajectory.java | 78 +++ .../trajectory/types/IndexedTrajectory.java | 62 +++ .../math/twodim/trajectory/types/State.java | 7 + .../trajectory/types/TimedTrajectory.java | 111 ++++ .../twodim/trajectory/types/Trajectory.java | 66 +++ .../trajectory/types/TrajectoryIterator.java | 59 ++ .../lib/subsystems/SpartronicsSubsystem.java | 130 +++++ .../lib/subsystems/drive/AbstractDrive.java | 116 ++++ .../drive/CharacterizeDriveBaseCommand.java | 92 ++++ .../drive/DifferentialTrackerDriveBase.java | 56 ++ .../drive/TrajectoryTrackerCommand.java | 70 +++ .../drive/TrajectoryTrackerDriveBase.java | 21 + .../estimator/RobotStateEstimator.java | 184 +++++++ .../subsystems/estimator/RobotStateMap.java | 146 +++++ .../spartronics4915/lib/util/DeltaTime.java | 40 ++ .../lib/util/Interpolable.java | 27 + .../lib/util/InterpolatingDouble.java | 60 ++ .../lib/util/InterpolatingLong.java | 59 ++ .../lib/util/InterpolatingTreeMap.java | 100 ++++ .../lib/util/InverseInterpolable.java | 28 + .../spartronics4915/lib/util/Kinematics.java | 58 ++ .../com/spartronics4915/lib/util/Logger.java | 159 ++++++ .../com/spartronics4915/lib/util/Units.java | 55 ++ .../lib/hardware/sensors/TestT265Camera.java | 100 ++++ .../math/twodim/lidar/TestObjectFinder.java | 181 ++++++ .../twodim/spline/CubicHermiteSplineTest.java | 163 ++++++ .../spline/QuinticHermiteSplineTest.java | 108 ++++ .../twodim/trajectory/PathFinderTest.java | 125 +++++ .../trajectory/TrajectoryGeneratorTest.java | 75 +++ .../trajectory/TrajectoryIntegrationTest.java | 160 ++++++ .../lib/subsystems/TestSubsystem.java | 26 + vendordeps/Phoenix.json | 30 +- 74 files changed, 8690 insertions(+), 89 deletions(-) create mode 100644 src/main/java/com/spartronics4915/lib/hardware/motors/SensorModel.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsEncoder.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsMax.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsMotor.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsSRX.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/sensors/A51IRSensor.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/sensors/IRSensor.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/sensors/RPLidarA1.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/sensors/SpartronicsIMU.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/sensors/T265Camera.java create mode 100644 src/main/java/com/spartronics4915/lib/math/PolynomialRegression.java create mode 100644 src/main/java/com/spartronics4915/lib/math/Util.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/control/FeedForwardTracker.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/control/PurePursuitTracker.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/control/RamseteTracker.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/control/TrajectoryTracker.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/geometry/Pose2d.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/geometry/Pose2dWithCurvature.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/geometry/Rectangle2d.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/geometry/Rotation2d.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/geometry/Translation2d.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/geometry/Twist2d.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/lidar/ObjectFinder.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/lidar/TargetTracker.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/physics/DCMotorTransmission.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/physics/DifferentialDrive.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/physics/DriveCharacterization.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/spline/CubicHermiteSpline.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/spline/QuinticHermiteSpline.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/spline/Spline.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/spline/SplineHelper.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/spline/SplineParameterizer.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/trajectory/Line.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/trajectory/PathFinder.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/trajectory/TrajectoryGenerator.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/AngularAccelerationConstraint.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/CentripetalAccelerationConstraint.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/DifferentialDriveDynamicsConstraint.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/TimingConstraint.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/VelocityLimitRadiusConstraint.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/VelocityLimitRegionConstraint.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/DistancedTrajectory.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/IndexedTrajectory.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/State.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/TimedTrajectory.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/Trajectory.java create mode 100644 src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/TrajectoryIterator.java create mode 100644 src/main/java/com/spartronics4915/lib/subsystems/SpartronicsSubsystem.java create mode 100644 src/main/java/com/spartronics4915/lib/subsystems/drive/AbstractDrive.java create mode 100644 src/main/java/com/spartronics4915/lib/subsystems/drive/CharacterizeDriveBaseCommand.java create mode 100644 src/main/java/com/spartronics4915/lib/subsystems/drive/DifferentialTrackerDriveBase.java create mode 100644 src/main/java/com/spartronics4915/lib/subsystems/drive/TrajectoryTrackerCommand.java create mode 100644 src/main/java/com/spartronics4915/lib/subsystems/drive/TrajectoryTrackerDriveBase.java create mode 100644 src/main/java/com/spartronics4915/lib/subsystems/estimator/RobotStateEstimator.java create mode 100644 src/main/java/com/spartronics4915/lib/subsystems/estimator/RobotStateMap.java create mode 100644 src/main/java/com/spartronics4915/lib/util/DeltaTime.java create mode 100644 src/main/java/com/spartronics4915/lib/util/Interpolable.java create mode 100644 src/main/java/com/spartronics4915/lib/util/InterpolatingDouble.java create mode 100644 src/main/java/com/spartronics4915/lib/util/InterpolatingLong.java create mode 100644 src/main/java/com/spartronics4915/lib/util/InterpolatingTreeMap.java create mode 100644 src/main/java/com/spartronics4915/lib/util/InverseInterpolable.java create mode 100644 src/main/java/com/spartronics4915/lib/util/Kinematics.java create mode 100644 src/main/java/com/spartronics4915/lib/util/Logger.java create mode 100644 src/main/java/com/spartronics4915/lib/util/Units.java create mode 100644 src/test/java/com/spartronics4915/lib/hardware/sensors/TestT265Camera.java create mode 100644 src/test/java/com/spartronics4915/lib/math/twodim/lidar/TestObjectFinder.java create mode 100644 src/test/java/com/spartronics4915/lib/math/twodim/spline/CubicHermiteSplineTest.java create mode 100644 src/test/java/com/spartronics4915/lib/math/twodim/spline/QuinticHermiteSplineTest.java create mode 100644 src/test/java/com/spartronics4915/lib/math/twodim/trajectory/PathFinderTest.java create mode 100644 src/test/java/com/spartronics4915/lib/math/twodim/trajectory/TrajectoryGeneratorTest.java create mode 100644 src/test/java/com/spartronics4915/lib/math/twodim/trajectory/TrajectoryIntegrationTest.java create mode 100644 src/test/java/com/spartronics4915/lib/subsystems/TestSubsystem.java diff --git a/build.gradle b/build.gradle index 06056c1..df79ce9 100644 --- a/build.gradle +++ b/build.gradle @@ -1,74 +1,117 @@ -plugins { - id "java" - id "edu.wpi.first.GradleRIO" version "2020.1.2" -} - -sourceCompatibility = JavaVersion.VERSION_11 -targetCompatibility = JavaVersion.VERSION_11 - -def ROBOT_MAIN_CLASS = "com.spartronics4915.frc2020.Main" - -// Define my targets (RoboRIO) and artifacts (deployable files) -// This is added by GradleRIO's backing project EmbeddedTools. -deploy { - targets { - roboRIO("roborio") { - // Team number is loaded either from the .wpilib/wpilib_preferences.json - // or from command line. If not found an exception will be thrown. - // You can use getTeamOrDefault(team) instead of getTeamNumber if you - // want to store a team number in this file. - team = frc.getTeamNumber() - } - } - artifacts { - frcJavaArtifact('frcJava') { - targets << "roborio" - // Debug can be overridden by command line, for use with VSCode - debug = frc.getDebugOrDefault(false) - } - // Built in artifact to deploy arbitrary files to the roboRIO. - fileTreeArtifact('frcStaticFileDeploy') { - // The directory below is the local directory to deploy - files = fileTree(dir: 'src/main/deploy') - // Deploy to RoboRIO target, into /home/lvuser/deploy - targets << "roborio" - directory = '/home/lvuser/deploy' - } - } -} - -repositories { - mavenCentral() - maven { url 'https://jitpack.io' } -} - -// Set this to true to enable desktop support. -def includeDesktopSupport = true - -// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. -// Also defines JUnit 4. -dependencies { - implementation wpi.deps.wpilib() - nativeZip wpi.deps.wpilibJni(wpi.platforms.roborio) - nativeDesktopZip wpi.deps.wpilibJni(wpi.platforms.desktop) - - implementation wpi.deps.vendor.java() - nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio) - nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop) - - implementation "com.github.Spartronics4915:SpartronicsLib:master-SNAPSHOT" - - testImplementation 'junit:junit:4.12' - - // Enable simulation gui support. Must check the box in vscode to enable support - // upon debugging - simulation wpi.deps.sim.gui(wpi.platforms.desktop, false) -} - -// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') -// in order to make them all available at runtime. Also adding the manifest so WPILib -// knows where to look for our Robot Class. -jar { - from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } - manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) -} +import java.text.SimpleDateFormat; +import java.io.ByteArrayOutputStream; + +plugins { + id "java" + id "edu.wpi.first.GradleRIO" version "2020.2.2" + id "maven" + id "maven-publish" +} + +sourceCompatibility = JavaVersion.VERSION_11 +targetCompatibility = JavaVersion.VERSION_11 + +def ROBOT_MAIN_CLASS = "com.spartronics4915.frc2020.Main" +def includeDesktopSupport = true // Set this to true to enable desktop support. + +// Define my targets (RoboRIO) and artifacts (deployable files) +// This is added by GradleRIO's backing project EmbeddedTools. +deploy { + targets { + roboRIO("roborio") { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. + team = frc.getTeamNumber() + if(!project.hasProperty("discover")) + addresses = ["10.49.15.2"] + } + } + artifacts { + frcJavaArtifact('frcJava') { + targets << "roborio" + // Debug can be overridden by command line, for use with VSCode + debug = frc.getDebugOrDefault(false) + } + // Built in artifact to deploy arbitrary files to the roboRIO. + fileTreeArtifact('frcStaticFileDeploy') { + // The directory below is the local directory to deploy + files = fileTree(dir: 'src/main/deploy') + // Deploy to RoboRIO target, into /home/lvuser/deploy + targets << "roborio" + directory = '/home/lvuser/deploy' + } + } +} + +repositories { + mavenCentral() // needed for JUnit + maven { url 'https://jitpack.io' } +} + +// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. +// Also defines JUnit 4. +dependencies { + implementation wpi.deps.wpilib() + nativeZip wpi.deps.wpilibJni(wpi.platforms.roborio) + nativeDesktopZip wpi.deps.wpilibJni(wpi.platforms.desktop) + + implementation wpi.deps.vendor.java() + nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio) + nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop) + + // implementation "com.github.Spartronics4915:SpartronicsLib:master-SNAPSHOT" + + implementation("gov.nist.math:jama:1.0.3") + implementation("org.ejml:ejml-simple:0.38") + implementation("org.apache.commons:commons-math3:3.6.1") + implementation("com.fazecast:jSerialComm:2.4.1") + + testImplementation("org.knowm.xchart:xchart:3.2.2") + //testImplementation 'junit:junit:4.12' + testImplementation("org.junit.jupiter:junit-jupiter-api:5.3.2") + testRuntime("org.junit.jupiter:junit-jupiter-engine:5.3.2") + + // Enable simulation gui support. Must check the box in vscode to enable support + // upon debugging + simulation wpi.deps.sim.gui(wpi.platforms.desktop, false) +} + +// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') +// in order to make them all available at runtime. Also adding the manifest so WPILib +// knows where to look for our Robot Class. +jar { + from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + manifest { + String user = System.getProperty("user.name"); + SimpleDateFormat fmt = new SimpleDateFormat("MMMMM dd, hh:mm a"); + ByteArrayOutputStream gitinfo = new ByteArrayOutputStream(); + exec { + ignoreExitValue true + commandLine 'git', 'describe', "--tags", "--dirty" + standardOutput = gitinfo + } + attributes ( + "Built-By": user, + "Built-At": fmt.format(new Date()), + "Code-Version": gitinfo.toString().trim() + ) + } + manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) + // we could exclude source by subdir (see 2019-Deepspace/build.gradle) +} + +test { + useJUnitPlatform { + // Some tests only work if you have the right stuff plugged in + // Pass -PallTests if you want tests with the excluded tags + if (!project.hasProperty("allTests")) { + excludeTags "hardwareDependant" + } + } + testLogging { + events "PASSED", "SKIPPED", "FAILED" + exceptionFormat "full" + } +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/hardware/motors/SensorModel.java b/src/main/java/com/spartronics4915/lib/hardware/motors/SensorModel.java new file mode 100644 index 0000000..3a00876 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/motors/SensorModel.java @@ -0,0 +1,34 @@ +package com.spartronics4915.lib.hardware.motors; + +public class SensorModel { + private final double mToMetersMultiplier; + + /** + * This is a convenience constructor for sensors connected to a wheel. Use the + * more direct constructor if your sensor is not connected to a wheel. + * + * @param wheelDiameterMeters The diameter of your wheel in meters. + * @param nativeUnitsPerRevolution The number of meters per wheel revolution. + */ + public SensorModel(double wheelDiameterMeters, double nativeUnitsPerRevolution) { + mToMetersMultiplier = (1 / nativeUnitsPerRevolution) * (wheelDiameterMeters * Math.PI); + } + + /** + * @param nativeUnitsToMetersMultiplier A number that, when multiplied with some + * amount of meters, converts to meters. + * This factor will also be used to convert + * to related units, like meters/sec. + */ + public SensorModel(double nativeUnitsToMetersMultiplier) { + mToMetersMultiplier = nativeUnitsToMetersMultiplier; + } + + public double toMeters(double nativeUnits) { + return nativeUnits * mToMetersMultiplier; + } + + public double toNativeUnits(double meters) { + return meters / mToMetersMultiplier; + } +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsEncoder.java b/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsEncoder.java new file mode 100644 index 0000000..87249e8 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsEncoder.java @@ -0,0 +1,41 @@ +package com.spartronics4915.lib.hardware.motors; + +import com.spartronics4915.lib.util.Logger; + +public interface SpartronicsEncoder { + + public final SpartronicsEncoder kDisconnectedEncoder = new SpartronicsEncoder() { + + @Override + public void setPhase(boolean isReversed) { + Logger.error("Couldn't set the phase of a disconnected encoder"); + } + + @Override + public double getVelocity() { + return 0; + } + + @Override + public double getPosition() { + return 0; + } + }; + + /** + * @return Velocity in meters/second. + */ + double getVelocity(); + + /** + * @return Position in meters. + */ + double getPosition(); + + /** + * Sets the "direction" (phase) of this encoder. + * + * @param isReversed If true, the sensor's output is reversed. + */ + void setPhase(boolean isReversed); +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsMax.java b/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsMax.java new file mode 100644 index 0000000..4f2f8c0 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsMax.java @@ -0,0 +1,235 @@ +/************************************************\ +| Before we begin, here's a word from our | +| sponsor, RAID: Shadow Legends. | +| RAID: Shadow Legends™ is | +| an immersive online experience with everything | +| you'd expect from a brand new RPG title. It's | +| got an amazing storyline, awesome 3D graphics, | +| giant boss fights, PVP battles, and hundreds | +| of never before seen champions to collect and | +| customize. | +| I never expected to get this level of | +| performance out of a mobile game. Look how | +| crazy the level of detail is on these | +| champions! | +| RAID: Shadow Legends™ is getting big real | +| fast, so you should definitely get in early. | +| Starting now will give you a huge head start. | +| There's also an upcoming Special Launch | +| Tournament with crazy prizes! And not to | +| mention, this game is absolutely free! | +| So go ahead and check out the video | +| description to find out more about | +| RAID: Shadow Legends™. There, you will find a | +| link to the store page and a special code to | +| unlock all sorts of goodies. Using the special | +| code, you can get 50,000 Silver immediately, | +| and a FREE Epic Level Champion as part of the | +| new players program, courtesy of course of the | +| RAID: Shadow Legends™ devs. | +| DISCLAIMER: Not actually sponsored by RAID: | +| Shadow Legends | +\************************************************/ + + +package com.spartronics4915.lib.hardware.motors; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.ControlType; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +public class SpartronicsMax implements SpartronicsMotor { + + private static final int kVelocitySlotIdx = 0; + private static final int kPositionSlotIdx = 1; + + private final double kRPMtoRPS = 1/60; + + private CANSparkMax mSparkMax; + private SpartronicsEncoder mEncoder; + private SensorModel mSensorModel; + + private boolean mBrakeMode = false; + /** Volts */ + private double mVoltageCompSaturation = 12.0; + /** Native units/sec, converted to meters on get and set */ + private double mMotionProfileCruiseVelocity = 0.0; + /** Native units/sec^2, converted to meters on get and set */ + private double mMotionProfileAcceleration = 0.0; + private boolean mUseMotionProfileForPosition = false; + + public class SpartronicsMaxEncoder implements SpartronicsEncoder { + + @Override + public double getVelocity() { + return mSensorModel.toMeters(mSparkMax.getEncoder().getVelocity()); + } + + @Override + public double getPosition() { + return mSensorModel.toMeters(mSparkMax.getEncoder().getPosition()); + } + + @Override + public void setPhase(boolean isReversed) { + mSparkMax.setInverted(isReversed); + } + } + + public SpartronicsMax(int deviceNumber, SensorModel sensorModel) { + this(new CANSparkMax(deviceNumber, MotorType.kBrushless), sensorModel); + } + + public SpartronicsMax(CANSparkMax spark, SensorModel sensorModel) { + mSparkMax = spark; + mSensorModel = sensorModel; + mEncoder = new SpartronicsMaxEncoder(); + mSparkMax.getEncoder().setPosition(0); + mSparkMax.getEncoder().setVelocityConversionFactor(kRPMtoRPS); // Set conversion factor. + + // mSparkMax.configFactoryDefault(); + mSparkMax.enableVoltageCompensation(mVoltageCompSaturation); + } + + @Override + public SpartronicsEncoder getEncoder() { + return mEncoder; + } + + @Override + public double getVoltageOutput() { + return mSparkMax.getBusVoltage() * mSparkMax.getAppliedOutput(); + } + + @Override + public boolean getOutputInverted() { + return mSparkMax.getInverted(); + } + + @Override + public void setOutputInverted(boolean inverted) { + mSparkMax.setInverted(inverted); + } + + @Override + public boolean getBrakeMode() { + return mBrakeMode; + } + + @Override + public void setBrakeMode(boolean mode) { + mBrakeMode = mode; + mSparkMax.setIdleMode(mode ? IdleMode.kBrake : IdleMode.kCoast); + } + + @Override + public double getVoltageCompSaturation() { + return mVoltageCompSaturation; + } + + @Override + public void setVoltageCompSaturation(double voltage) { + mVoltageCompSaturation = voltage; + mSparkMax.enableVoltageCompensation(mVoltageCompSaturation); + } + + @Override + public double getMotionProfileCruiseVelocity() { + return mSensorModel.toMeters(mMotionProfileCruiseVelocity); + } + + @Override + public void setMotionProfileCruiseVelocity(double velocityMetersPerSecond) { // Set to slot + mMotionProfileCruiseVelocity = mSensorModel + .toNativeUnits(velocityMetersPerSecond); + mSparkMax.getPIDController().setSmartMotionMaxVelocity((int) mMotionProfileCruiseVelocity, kVelocitySlotIdx); + } + + @Override + public double getMotionProfileMaxAcceleration() { + return mSensorModel.toMeters(mMotionProfileAcceleration); + } + + @Override + public void setMotionProfileMaxAcceleration(double accelerationMetersPerSecondSq) { + mMotionProfileAcceleration = mSensorModel + .toNativeUnits(accelerationMetersPerSecondSq); + mSparkMax.getPIDController().setSmartMotionMaxAccel((int) mMotionProfileAcceleration, kVelocitySlotIdx); + } + + @Override + public void setUseMotionProfileForPosition(boolean useMotionProfile) { + mUseMotionProfileForPosition = useMotionProfile; + } + + @Override + public void setDutyCycle(double dutyCycle, double arbitraryFeedForwardVolts) { + mSparkMax.getPIDController().setReference(dutyCycle, ControlType.kDutyCycle, 0, + arbitraryFeedForwardVolts); + } + + @Override + public void setDutyCycle(double dutyCycle) { + setDutyCycle(dutyCycle, 0.0); + } + + @Override + public void setVelocity(double velocityMetersPerSecond, double arbitraryFeedForwardVolts) { + double velocityNative = mSensorModel.toNativeUnits(velocityMetersPerSecond); + mSparkMax.getPIDController().setReference(velocityNative, ControlType.kVelocity, kVelocitySlotIdx, + arbitraryFeedForwardVolts); + } + + @Override + public void setVelocityGains(double kP, double kD) { + setVelocityGains(kP, 0, kD, 0); + } + + @Override + public void setVelocityGains(double kP, double kI, double kD, double kF) { + mSparkMax.getPIDController().setP(kP, kVelocitySlotIdx); + mSparkMax.getPIDController().setI(kI, kVelocitySlotIdx); + mSparkMax.getPIDController().setD(kD, kVelocitySlotIdx); + mSparkMax.getPIDController().setFF(kF, kVelocitySlotIdx); + } + + @Override + public void setPosition(double positionMeters) { + positionMeters = mSensorModel.toNativeUnits(positionMeters); + mSparkMax.getPIDController().setReference(positionMeters, mUseMotionProfileForPosition ? ControlType.kSmartMotion : ControlType.kPosition, kPositionSlotIdx); + } + + @Override + public void setPositionGains(double kP, double kD) { + setPositionGains(kP, 0, kD, 0); + } + + @Override + public void setPositionGains(double kP, double kI, double kD, double kF) { + mSparkMax.getPIDController().setP(kP, kPositionSlotIdx); + mSparkMax.getPIDController().setI(kI, kPositionSlotIdx); + mSparkMax.getPIDController().setD(kD, kPositionSlotIdx); + mSparkMax.getPIDController().setFF(kF, kPositionSlotIdx); + } + + public void follow(SpartronicsMax other) { + mSparkMax.follow(other.mSparkMax); + } + + @Override + public SensorModel getSensorModel() { + return mSensorModel; + } + + @Override + public void setVelocity(double velocityMetersPerSecond) { + setVelocity(velocityMetersPerSecond, 0.0); + } + + @Override + public void setNeutral() { + mSparkMax.getPIDController().setReference(0.0, ControlType.kDutyCycle, 0); + } + +} diff --git a/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsMotor.java b/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsMotor.java new file mode 100644 index 0000000..d833579 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsMotor.java @@ -0,0 +1,141 @@ +package com.spartronics4915.lib.hardware.motors; + +public interface SpartronicsMotor { + + /** + * @return The encoder attached to the motor. + */ + SpartronicsEncoder getEncoder(); + + /** + * @return A {@link SensorModel} that converts between native units and meters. + */ + SensorModel getSensorModel(); + + /** + * @return Current output of the motor controller in Volts. + */ + double getVoltageOutput(); + + /** + * @return Is the motor output inverted. + */ + boolean getOutputInverted(); + + /** + * @param inverted If true, the motor output is inverted. + */ + void setOutputInverted(boolean inverted); + + /** + * @return Are the motor leads electrically commonized to reduce movement. + */ + boolean getBrakeMode(); + + /** + * @param mode If true, commonize the motor leads to reduce movement. + */ + void setBrakeMode(boolean mode); + + /** + * @return The max voltage output given to the motor. + */ + double getVoltageCompSaturation(); + + /** + * @param voltage Sets the max voltage output given to the motor, in Volts. + */ + void setVoltageCompSaturation(double voltage); + + /** + * @return The max velocity in meters/s that the on board motion profile + * generator will use. + */ + double getMotionProfileCruiseVelocity(); + + /** + * @param velocityMetersPerSecond The max velocity in meters/s that the on board + * motion profile generator will use. + */ + void setMotionProfileCruiseVelocity(double velocityMetersPerSecond); + + /** + * @return The max acceleration in meters/s^2 that the on board motion profile + * generator will use. + */ + double getMotionProfileMaxAcceleration(); + + /** + * @param accelerationMetersSecSq The max acceleration in meters/s^2 that the on + * board motion profile generator will use. + */ + void setMotionProfileMaxAcceleration(double accelerationMetersSecSq); + + /** + * @param useMotionProfile If true, we will use the motion profile to get to + * positions passed to + * {@link SpartronicsMotor#setPosition(double) + * setPosition}. + */ + void setUseMotionProfileForPosition(boolean useMotionProfile); + + /** + * Sets the output as a percentage (like setOpenLoop). + * + * @param dutyCycle Output in perecnt. + * @param arbitraryFeedforward Additional arbitrary feedforward in Volts. + */ + void setDutyCycle(double dutyCycle, double arbitraryFeedForwardVolts); + + /** + * Sets the output as a percentage (like setOpenLoop). + * + * @param dutyCycle Output in percent. + */ + void setDutyCycle(double dutyCycle); + + /** + * Sets the target output velocity. + * + * @param velocityMetersPerSecond Velocity in meters/s. + */ + void setVelocity(double velocityMetersPerSecond); + + /** + * Sets the target output velocity. + * + * @param velocityMetersPerSecond Velocity in meters/s. + * @param arbitraryFeedForwardVolts Additional arbitrary feedforward in Volts. + */ + void setVelocity(double velocityMetersPerSecond, double arbitraryFeedForwardVolts); + + /** + * Sets the PD gains for the built in velocity PID controller. + */ + void setVelocityGains(double kP, double kD); + /** + * Sets the PID gains for the built in velocity PID controller. + */ + void setVelocityGains(double kP, double kI, double kD, double kF); + + /** + * Sets the target position. + * + * @param positionMeters Target position in meters. + */ + void setPosition(double positionMeters); + + /** + * Sets the PID gains for the built in position PID controller. + */ + void setPositionGains(double kP, double kD); + /** + * Sets the PID gains for the built in position PID controller. + */ + void setPositionGains(double kP, double kI, double kD, double kF); + + /** + * Turns the motor off. + */ + void setNeutral(); +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsSRX.java b/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsSRX.java new file mode 100644 index 0000000..866b51b --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsSRX.java @@ -0,0 +1,228 @@ +package com.spartronics4915.lib.hardware.motors; + +import com.ctre.phoenix.ErrorCode; +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.can.TalonSRX; +import com.spartronics4915.lib.util.Logger; + +public class SpartronicsSRX implements SpartronicsMotor { + + private static final int kVelocitySlotIdx = 0; + private static final int kPositionSlotIdx = 1; + + // This conversion could go into the sensor model, but the per 100ms thing is + // Talon-only, so it's not worth it. + private static final double kMetersPer100msToMetersPerSecond = 10; + private static final double kMetersPerSecondToMetersPer100ms = 1 / kMetersPer100msToMetersPerSecond; + + private TalonSRX mTalonSRX; + private SpartronicsEncoder mEncoder; + private SensorModel mSensorModel; + + private boolean mBrakeMode = false; + /** Volts */ + private double mVoltageCompSaturation = 12.0; + /** Native units/sec, converted to meters on get and set */ + private double mMotionProfileCruiseVelocity = 0.0; + /** Native units/sec^2, converted to meters on get and set */ + private double mMotionProfileAcceleration = 0.0; + private boolean mUseMotionProfileForPosition = false; + + private ControlMode mLastControlMode = null; + + public class SpartronicsSRXEncoder implements SpartronicsEncoder { + + @Override + public double getVelocity() { + return mSensorModel.toMeters(mTalonSRX.getSelectedSensorVelocity()) * kMetersPer100msToMetersPerSecond; + } + + @Override + public double getPosition() { + return mSensorModel.toMeters(mTalonSRX.getSelectedSensorPosition()); + } + + @Override + public void setPhase(boolean isReversed) { + mTalonSRX.setSensorPhase(isReversed); + } + } + + public SpartronicsSRX(int deviceNumber, SensorModel sensorModel) { + this(new TalonSRX(deviceNumber), sensorModel); + } + + public SpartronicsSRX(TalonSRX talon, SensorModel sensorModel) { + mTalonSRX = talon; + mSensorModel = sensorModel; + + ErrorCode err = mTalonSRX.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 5); + if (err != ErrorCode.OK) { + Logger.error("TalonSRX on with ID " + mTalonSRX.getDeviceID() + + " returned a non-OK error code on sensor configuration... Is the encoder plugged in?"); + mEncoder = SpartronicsEncoder.kDisconnectedEncoder; + } else { + mEncoder = new SpartronicsSRXEncoder(); + } + + mTalonSRX.configFactoryDefault(); + mTalonSRX.configVoltageCompSaturation(mVoltageCompSaturation); + mTalonSRX.enableVoltageCompensation(true); + } + + @Override + public SpartronicsEncoder getEncoder() { + return mEncoder; + } + + @Override + public double getVoltageOutput() { + return mTalonSRX.getMotorOutputVoltage(); + } + + @Override + public boolean getOutputInverted() { + return mTalonSRX.getInverted(); + } + + @Override + public void setOutputInverted(boolean inverted) { + mTalonSRX.setInverted(inverted); + } + + @Override + public boolean getBrakeMode() { + return mBrakeMode; + } + + @Override + public void setBrakeMode(boolean mode) { + mBrakeMode = mode; + mTalonSRX.setNeutralMode(mode ? NeutralMode.Brake : NeutralMode.Coast); + } + + @Override + public double getVoltageCompSaturation() { + return mVoltageCompSaturation; + } + + @Override + public void setVoltageCompSaturation(double voltage) { + mVoltageCompSaturation = voltage; + mTalonSRX.configVoltageCompSaturation(mVoltageCompSaturation); + mTalonSRX.enableVoltageCompensation(true); + } + + @Override + public double getMotionProfileCruiseVelocity() { + return mSensorModel.toMeters(mMotionProfileCruiseVelocity) * kMetersPer100msToMetersPerSecond; + } + + @Override + public void setMotionProfileCruiseVelocity(double velocityMetersPerSecond) { + mMotionProfileCruiseVelocity = mSensorModel + .toNativeUnits(velocityMetersPerSecond * kMetersPerSecondToMetersPer100ms); + mTalonSRX.configMotionCruiseVelocity((int) mMotionProfileCruiseVelocity); + } + + @Override + public double getMotionProfileMaxAcceleration() { + return mSensorModel.toMeters(mMotionProfileAcceleration) * kMetersPer100msToMetersPerSecond; + } + + @Override + public void setMotionProfileMaxAcceleration(double accelerationMetersPerSecondSq) { + mMotionProfileAcceleration = mSensorModel + .toNativeUnits(accelerationMetersPerSecondSq * kMetersPerSecondToMetersPer100ms); + mTalonSRX.configMotionAcceleration((int) mMotionProfileAcceleration); + } + + @Override + public void setUseMotionProfileForPosition(boolean useMotionProfile) { + mUseMotionProfileForPosition = useMotionProfile; + } + + @Override + public void setDutyCycle(double dutyCycle, double arbitraryFeedForwardVolts) { + mLastControlMode = ControlMode.PercentOutput; + mTalonSRX.set(ControlMode.PercentOutput, dutyCycle, DemandType.ArbitraryFeedForward, + arbitraryFeedForwardVolts / mVoltageCompSaturation); + } + + @Override + public void setDutyCycle(double dutyCycle) { + setDutyCycle(dutyCycle, 0.0); + } + + @Override + public void setVelocity(double velocityMetersPerSecond, double arbitraryFeedForwardVolts) { + if (mLastControlMode != ControlMode.Velocity) { + mTalonSRX.selectProfileSlot(kVelocitySlotIdx, 0); + mLastControlMode = ControlMode.Velocity; + } + + double velocityNative = mSensorModel.toNativeUnits(velocityMetersPerSecond * kMetersPerSecondToMetersPer100ms); + mTalonSRX.set(ControlMode.Velocity, velocityNative, DemandType.ArbitraryFeedForward, + arbitraryFeedForwardVolts / mVoltageCompSaturation); + } + + @Override + public void setVelocityGains(double kP, double kD) { + setVelocityGains(kP, 0, kD, 0); + } + + @Override + public void setVelocityGains(double kP, double kI, double kD, double kF) { + mTalonSRX.config_kP(kVelocitySlotIdx, kP); + mTalonSRX.config_kI(kVelocitySlotIdx, kI); + mTalonSRX.config_kD(kVelocitySlotIdx, kD); + mTalonSRX.config_kF(kVelocitySlotIdx, kF); + } + + @Override + public void setPosition(double positionMeters) { + if (mLastControlMode != ControlMode.Position) { + mTalonSRX.selectProfileSlot(kPositionSlotIdx, 0); + mLastControlMode = ControlMode.Position; + } + + positionMeters = mSensorModel.toNativeUnits(positionMeters); + mTalonSRX.set(mUseMotionProfileForPosition ? ControlMode.MotionMagic : ControlMode.Position, positionMeters); + } + + @Override + public void setPositionGains(double kP, double kD) { + setPositionGains(kP, 0, kD, 0); + } + + @Override + public void setPositionGains(double kP, double kI, double kD, double kF) { + mTalonSRX.config_kP(kPositionSlotIdx, kP); + mTalonSRX.config_kI(kPositionSlotIdx, kI); + mTalonSRX.config_kD(kPositionSlotIdx, kD); + mTalonSRX.config_kF(kPositionSlotIdx, kF); + } + + public void follow(SpartronicsSRX other) { + mTalonSRX.follow(other.mTalonSRX); + } + + @Override + public SensorModel getSensorModel() { + return mSensorModel; + } + + @Override + public void setVelocity(double velocityMetersPerSecond) { + setVelocity(velocityMetersPerSecond, 0.0); + } + + @Override + public void setNeutral() { + mTalonSRX.set(ControlMode.Disabled, 0.0, DemandType.Neutral, 0.0); + } + +} diff --git a/src/main/java/com/spartronics4915/lib/hardware/sensors/A51IRSensor.java b/src/main/java/com/spartronics4915/lib/hardware/sensors/A51IRSensor.java new file mode 100644 index 0000000..f72602f --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/sensors/A51IRSensor.java @@ -0,0 +1,22 @@ +package com.spartronics4915.lib.hardware.sensors; + +public class A51IRSensor extends IRSensor +{ + + public A51IRSensor(int port) + { + super(port); + } + + //How did we get here? We measured 9 seperate centimeter values and got the voltage. + //We then used a cubic polynomial regression to find the equation below. + //Range: 10 cm - 80 cm + + @Override + public double getDistance() + { + double volt = getVoltage(); + double cm = 56.2958 - (79.12745 * volt) + (43.04363 * Math.pow(volt, 2.0)) - (7.753581 * Math.pow(volt, 3.0)); + return cm / 2.54; + } +} diff --git a/src/main/java/com/spartronics4915/lib/hardware/sensors/IRSensor.java b/src/main/java/com/spartronics4915/lib/hardware/sensors/IRSensor.java new file mode 100644 index 0000000..3819668 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/sensors/IRSensor.java @@ -0,0 +1,41 @@ +package com.spartronics4915.lib.hardware.sensors; + +import edu.wpi.first.wpilibj.AnalogInput; + +public abstract class IRSensor +{ + AnalogInput mAnalogInput; + + public IRSensor(int port) + { + mAnalogInput = new AnalogInput(port); + mAnalogInput.setAverageBits(12); + } + + public double getVoltage() + { + double v = mAnalogInput.getAverageVoltage(); + if (v < .001) + v = .001; + return v; + } + + public abstract double getDistance(); // inches + + public boolean isTargetInVoltageRange(double min, double max) + { + double v = getVoltage(); + return v > min && v < max; + } + + /** + * @param minDist in inches + * @param maxDist in inches + * @return is within the distance + */ + public boolean isTargetInDistanceRange(double minDist, double maxDist) + { + double d = getDistance(); + return d > minDist && d < maxDist; + } +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/hardware/sensors/RPLidarA1.java b/src/main/java/com/spartronics4915/lib/hardware/sensors/RPLidarA1.java new file mode 100644 index 0000000..fdfde06 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/sensors/RPLidarA1.java @@ -0,0 +1,520 @@ +package com.spartronics4915.lib.hardware.sensors; + +import java.io.IOException; +import java.io.InputStream; +import java.io.OutputStream; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Formatter; +import java.util.List; +import java.util.Optional; +import java.util.concurrent.atomic.AtomicBoolean; +import java.util.function.Consumer; + +import com.fazecast.jSerialComm.SerialPort; +import com.spartronics4915.lib.math.twodim.geometry.Pose2d; +import com.spartronics4915.lib.math.twodim.geometry.Rotation2d; +import com.spartronics4915.lib.math.twodim.geometry.Translation2d; +import com.spartronics4915.lib.subsystems.estimator.RobotStateMap; + +import edu.wpi.first.wpilibj.Timer; + +/** + * This is a single-file Java driver for the RPLidarA1. This driver supports + * starting and stopping scans, getting individual points in a stream, getting + * device-relative pointclouds, and getting robot-relative pointclouds. + * + * This driver may work with the A2, but it does not use in express scan mode, + * which means that you cannot use the A2 at its highest rated speed. + * + * @author Declan Freeman-Gleason + */ +public class RPLidarA1 { + + private static enum OutgoingPacket { + RESET(0x20), STOP(0x5A), SCAN(0x20), GET_INFO(0x50), GET_HEALTH(0xF0); + + public final byte header; + + private OutgoingPacket(int header) { + this.header = (byte) header; + } + } + + private static enum IncomingPacket { + INFO(0x04, 20), HEALTH(0x06, 3), SCAN(0x81, 5); + + public final byte header; + public final int size; + + private IncomingPacket(int header, int size) { + this.header = (byte) header; + this.size = size; + } + } + + public static class DeviceHealth { + + public static enum HealthStatus { + GOOD, WARNING, ERROR + } + + public final HealthStatus status; + public final int errorCode; + + public DeviceHealth(int status, int errorCode) { + this.status = HealthStatus.values()[status]; // With throw if out of range + this.errorCode = errorCode; + } + + @Override + public String toString() { + return status + (status != HealthStatus.GOOD ? " (Error code " + errorCode + ")" : ""); + } + } + + public static class DeviceInfo { + public final int model; + public final int firmwareMinor; + public final int firmwareMajor; + public final int hardwareRevision; + public final byte[] serialNumber; + + public DeviceInfo(int model, int firmwareMinor, int firmwareMajor, int hardwareRevision, byte[] serialNumber) { + this.model = model; + this.firmwareMinor = firmwareMinor; + this.firmwareMajor = firmwareMajor; + this.hardwareRevision = hardwareRevision; + this.serialNumber = serialNumber; + } + + @Override + public String toString() { + try (var formatter = new Formatter()) { + for (byte b : serialNumber) { + formatter.format("%02x", b); + } + return "RPLidar model " + model + ", firmware version " + firmwareMajor + "." + firmwareMinor + + ", hardware revision " + hardwareRevision + ", serial number " + formatter.toString(); + } + } + } + + /* + * This is lowest level user-facing representation of what we get back from the + * sensor. + */ + public static class Measurement { + /** Is this the start of a new scan */ + public final boolean start; + /** Scan quality from 0 (lowest) to 255 (highest) */ + public final int quality; + /** Angle in degrees */ + public final Rotation2d angle; + /** Distance in meters */ + public final double distance; + /** Timer.getFPGATimestamp() when the measurement arrived */ + public final double timestamp; + + public Measurement(boolean start, int quality, double angleDegrees, double distanceMeters, + double timestampSeconds) { + this.start = start; + this.quality = quality; + this.angle = Rotation2d.fromDegrees(angleDegrees); + this.distance = distanceMeters; + this.timestamp = timestampSeconds; + } + + public Translation2d getAsPoint() { + return new Translation2d(angle.getCos() * distance, angle.getSin() * distance); + } + + public boolean isInvalid() { + return distance == 0; + } + + public String toString() { + return "Starting: " + start + ", Quality: " + quality + ", Angle: " + angle + ", Distance: " + distance + + ", Timestamp: " + timestamp; + } + } + + // We have to make these aliases because Java type erasure + public static interface MeasurementConsumer extends Consumer { + } + + public static interface PointConsumer extends Consumer { + } + + public static interface PointcloudConsumer extends Consumer> { + } + + private static final String kPortDescription = "CP2102 USB to UART Bridge Controller"; + private static final byte kSyncByteZero = (byte) 0xA5; + private static final byte kSyncByteOne = (byte) 0x5A; + /** Seconds */ + private static final double kSendBlockingRetryDelay = 0.02; + /** Seconds */ + private static final double kReadThreadDelay = 0.02; + /** Seconds */ + private static final double kSendTimeout = 2; + + private Consumer mMeasurementConsumer; + private final SerialPort mSerialPort; + private InputStream mInStream; + private OutputStream mOutStream; + + // We keep a large buffer as a member because the entire packet isn't always + // available + private byte[] mReadBuffer = new byte[2048]; + private int mEndOfDataIndex = 0; + + private Object mLastRecievedHeaderLock = new Object(); + private int mLastRecievedHeader = 0; + + private Optional mLastDeviceHealth = Optional.empty(); + private Optional mLastDeviceInfo = Optional.empty(); + + // There are two modes: + // 1. Request/response mode, where a each request has a response. + // 2. Many response mode, where one request has many responses. + // + // The first mode is used for all request types except the scan request. The + // scan request puts us into the second mode and we can only get out by sending + // the stop request. + private AtomicBoolean mInScanMode = new AtomicBoolean(false); + private AtomicBoolean mIsStarted = new AtomicBoolean(false); + + // This list will be empty unless the user uses one of the pointcloud ctors + private List mCurrentPointcloud = new ArrayList<>(); + + public RPLidarA1() { + mMeasurementConsumer = (m) -> {}; + mSerialPort = Arrays.stream(SerialPort.getCommPorts()) + .filter((SerialPort p) -> p.getPortDescription().equals(kPortDescription) && !p.isOpen()).findFirst() + .orElseThrow(() -> new RuntimeException("No RPLidar device found")); + + mSerialPort.setComPortParameters(115200, 8, SerialPort.ONE_STOP_BIT, SerialPort.NO_PARITY); + mSerialPort.setFlowControl(SerialPort.FLOW_CONTROL_DISABLED); + mSerialPort.openPort(); + + mInStream = mSerialPort.getInputStream(); + mOutStream = mSerialPort.getOutputStream(); + + new Thread(this::readData).start(); + + stop(); + } + + /** + * @param measurementConsumer A callback to recieve measurements. Note that this + * will be called from an independent read thread. + */ + public void setCallback(MeasurementConsumer measurementConsumer) { + synchronized (mMeasurementConsumer) { + mMeasurementConsumer = measurementConsumer; + } + } + + /** + * @param pointConsumer A callback that accepts single sensor-relative points, + * in meters. Note that this will be called from an + * independant read thread. + */ + public void setCallback(PointConsumer pointConsumer) { + setCallback((Measurement m) -> { + pointConsumer.accept(m.getAsPoint()); + }); + } + + /** + * @param pointcloudConsumer A callback that accepts a field-relative + * pointcloud, with all coordinates in meters. + * @param robotStateMap A robot state map to transform the pointcloud with. + * @param vehicleToLidar Transformation from the vehicle to the lidar sensor + * (i.e. the sensor's offset from the vehicle's + * center). + */ + public void setCallback(PointcloudConsumer pointcloudConsumer, RobotStateMap robotStateMap, Pose2d vehicleToLidar) { + setCallback((Measurement m) -> { + if (m.start && mCurrentPointcloud.size() > 0) { + pointcloudConsumer.accept(new ArrayList<>(mCurrentPointcloud)); + mCurrentPointcloud.clear(); + } + Translation2d point = m.getAsPoint(); + point = robotStateMap.getFieldToVehicle(m.timestamp).transformBy(vehicleToLidar) + .transformBy(new Pose2d(point, new Rotation2d())).getTranslation(); + mCurrentPointcloud.add(point); + }); + } + + /** + * @param pointcloudConsumer A callback that accepts a sensor-relative + * pointcloud, with all coordinates in meters. This is + * susceptible to motion smear if you move during a + * scan. + */ + public void setCallback(PointcloudConsumer pointcloudConsumer) { + setCallback(pointcloudConsumer, new RobotStateMap(), new Pose2d()); + } + + public void start() { + mInScanMode.set(false); + mIsStarted.set(true); + + mSerialPort.clearDTR(); + sendData(OutgoingPacket.SCAN, IncomingPacket.SCAN, kSendTimeout); + } + + public void stop() { + mInScanMode.set(false); + mIsStarted.set(false); + + sendData(OutgoingPacket.RESET); + Timer.delay(0.002); // Docs say to sleep 2ms after a reset + sendData(OutgoingPacket.STOP); + Timer.delay(0.02); // This is also advised by the docs + mSerialPort.setDTR(); + } + + public Optional getHealth() { + sendData(OutgoingPacket.GET_HEALTH, IncomingPacket.HEALTH, kSendTimeout); + synchronized (mLastDeviceHealth) { + return mLastDeviceHealth; + } + } + + public Optional getInfo() { + sendData(OutgoingPacket.GET_INFO, IncomingPacket.INFO, kSendTimeout); + synchronized (mLastDeviceInfo) { + return mLastDeviceInfo; + } + } + + private void readData() { + try { + // Should we allow users to stop the read thread? + while (true) { + if (mInStream.available() > 0 && mIsStarted.get()) { + // Note that there is a chance packets get truncated if we exceed the read + // buffer. + // This shouldn't happen in practice though because messages aren't greater than + // 84 bytes. + int totalRead = mInStream.read(mReadBuffer, mEndOfDataIndex, mReadBuffer.length - mEndOfDataIndex); + mEndOfDataIndex += totalRead; + + // Here we parse the message and shift everything over such that the bytes we + // just read are cleared from the buffer. + int totalUsed = parseData(); + for (int i = 0; i < mEndOfDataIndex - totalUsed; i++) { + mReadBuffer[i] = mReadBuffer[i + totalUsed]; + } + mEndOfDataIndex -= totalUsed; + } else { + Timer.delay(kReadThreadDelay); + } + } + } catch (IOException e) { + throw new RuntimeException(e); + } + } + + /** + * @return The number of bytes that were parsed and used + */ + private int parseData() { + int offset = 0; + while (true) { + if (mInScanMode.get()) { + // There are no more complete scan packets to parse so we just return + if (offset + IncomingPacket.SCAN.size > mEndOfDataIndex) { + return offset; + } + + if (parseScan(offset, IncomingPacket.SCAN.size)) { + offset += IncomingPacket.SCAN.size; + } else { + // Bad packet + System.err.println("Got a bad scan packet"); + offset += 1; + } + } else { + // Check if we have consumed enough bytes to get to the response length field + // (which is 6 bytes in: 2 bytes for the start flags and 4 bytes for the length + // field itself) + if (offset + 6 > mEndOfDataIndex) { + return offset; + } + + if (mReadBuffer[offset] == kSyncByteZero && mReadBuffer[offset + 1] == kSyncByteOne) { + // Packet length is a 30-bit unsigned integer. The 2 least significant bytes + // after those 30 bytes give the current send mode. + + // First convert the bytes to an integer + int packetLength = ByteBuffer.wrap(Arrays.copyOfRange(mReadBuffer, offset + 2, offset + 6)) + .order(ByteOrder.LITTLE_ENDIAN).getInt(); + // Then mask off the first two (least significant) bits + packetLength &= 0x3FFFFFFF; + + // Get the packet header + byte header = mReadBuffer[offset + 6]; + + // 7 is the first two sync bytes, plus the 4 bytes for packet length/send mode, + // plus 1 byte for the data type (header) of the packet. + // Adding this to offset gives us the starting address of the actual packet + // data. + int packetDataOffset = 7; + + if (offset + packetDataOffset + packetLength > mEndOfDataIndex) { + return offset; + } + + if (parsePacket(offset + packetDataOffset, packetLength, header)) { + synchronized (mLastRecievedHeaderLock) { + // AND wiht 0xFF is to convert an unsigned to signed integer + mLastRecievedHeader = header & 0xFF; + offset += packetDataOffset + packetLength; + } + } else { + // The packet was bad; this sometimes happens on startup + offset += 2; + } + } else { + // We should only get here on startup; getting to this point in other situations + // means that we probably got a malformed packet, or that we parsed a packet + // wrong which screwed up our offset + offset++; + } + } + } + } + + private boolean parsePacket(int offset, int length, byte header) { + // Can't do a switch because these aren't constant expressions + if (header == IncomingPacket.INFO.header) { + return parseDeviceInfo(offset, length); + } else if (header == IncomingPacket.HEALTH.header) { + return parseDeviceHealth(offset, length); + } else if (header == IncomingPacket.SCAN.header) { + // We only get here when we recieve the initial scan packet + if (parseScan(offset, length)) { + mInScanMode.set(true); + return true; + } + } + return false; + } + + private boolean parseScan(int offset, int length) { + if (length != IncomingPacket.SCAN.size) { + return false; + } + + // The least significant bit of byte zero indicates if this is thes start of a + // new scan. + // The second to least significant bit should be the oppositive of the least + // significant bit. + // The remaining 6 bits are an unsigned integer that represents the quality of + // the point. + byte byteZero = mReadBuffer[offset]; + // The least significant bit of byte one should always be 1; it is a check bit. + // The remaining 6 bits are part of a fixed-point number representing the angle + // of the sensor at this point. + byte byteOne = mReadBuffer[offset + 1]; + + boolean isStart = (byteZero & 0x01) == 1; + boolean notIsStart = (byteZero & 0x02) >> 1 == 1; + + if (isStart == notIsStart) { + return false; + } + + if ((byteOne & 0x01) != 1) { + // This is probably not a scan packet, which means we have the wrong offset + // That only really happens on startup + return false; + } + + double timestamp = Timer.getFPGATimestamp(); + int quality = (byteZero & 0xFF) >> 2; // Convert to signed int and get rid of the last two bits + int angle = ((byteOne & 0xFF) | ((mReadBuffer[offset + 2] & 0xFF) << 8)) >> 1; // Convert to signed int and extract from multiple bytes + int distance = ((mReadBuffer[offset + 3] & 0xFF) | ((mReadBuffer[offset + 4] & 0xFF) << 8)); // Same as above + + // We negate because the angle they provide is counter clockwise positive + double angleDegrees = -1 * angle / 64d; + double distanceMeters = (distance / 4d) / 1000d; + + var m = new Measurement(isStart, quality, angleDegrees, distanceMeters, timestamp); + synchronized (mMeasurementConsumer) { + mMeasurementConsumer.accept(m); + } + + return true; + } + + private boolean parseDeviceHealth(int offset, int length) { + if (length != IncomingPacket.HEALTH.size) { + System.err.println("Bad health packet"); + return false; + } + + // AND by 0xFF is to convert unsigned integers to signed integers + // The bit shift and OR is to convert the little endian bytes to integers (this + // can be dne with a ByteBuffer, but it's slower and much less concise) + DeviceHealth h = new DeviceHealth(mReadBuffer[offset] & 0xFF, + (mReadBuffer[offset + 1] & 0xFF) | ((mReadBuffer[offset + 2] & 0xFF) << 8)); + synchronized (mLastDeviceHealth) { + mLastDeviceHealth = Optional.of(h); + } + return true; + } + + private boolean parseDeviceInfo(int offset, int length) { + if (length != IncomingPacket.INFO.size) { + System.err.println("Bad device info packet"); + return false; + } + + byte[] serialNumber = new byte[16]; + for (int i = 0; i < serialNumber.length; i++) { + serialNumber[i] = mReadBuffer[offset + i + 4]; + } + + synchronized (mLastDeviceInfo) { + // AND by 0xFF to convert to signed integer from unsigned + mLastDeviceInfo = Optional.of(new DeviceInfo(mReadBuffer[offset] & 0xFF, mReadBuffer[offset + 1] & 0xFF, + mReadBuffer[offset + 2] & 0xFF, mReadBuffer[offset + 3] & 0xFF, serialNumber)); + } + return true; + } + + /** + * Sends data and blocks until the expected packet is recieved, or the timeout + * is hit. + */ + private void sendData(OutgoingPacket packet, IncomingPacket expected, double timeoutSeconds) { + sendData(packet); + + synchronized (mLastRecievedHeaderLock) { + double endTime = Timer.getFPGATimestamp() + timeoutSeconds; + while (endTime >= Timer.getFPGATimestamp() && mLastRecievedHeader != expected.header) { + Timer.delay(kSendBlockingRetryDelay); + } + } + } + + /** + * Sends a packet without blocking. + */ + private void sendData(OutgoingPacket packet) { + try { + mOutStream.write(new byte[] { kSyncByteZero, packet.header }); + mOutStream.flush(); + } catch (IOException e) { + throw new RuntimeException(e); + } + } + +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/hardware/sensors/SpartronicsIMU.java b/src/main/java/com/spartronics4915/lib/hardware/sensors/SpartronicsIMU.java new file mode 100644 index 0000000..862d0c4 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/sensors/SpartronicsIMU.java @@ -0,0 +1,8 @@ +package com.spartronics4915.lib.hardware.sensors; + +import com.spartronics4915.lib.math.twodim.geometry.Rotation2d; + +public interface SpartronicsIMU +{ + Rotation2d getYaw(); +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/hardware/sensors/T265Camera.java b/src/main/java/com/spartronics4915/lib/hardware/sensors/T265Camera.java new file mode 100644 index 0000000..9686c66 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/sensors/T265Camera.java @@ -0,0 +1,249 @@ +package com.spartronics4915.lib.hardware.sensors; + +import java.nio.file.Paths; +import java.util.function.Consumer; + +import com.spartronics4915.lib.math.twodim.geometry.Pose2d; +import com.spartronics4915.lib.math.twodim.geometry.Rotation2d; +import com.spartronics4915.lib.math.twodim.geometry.Twist2d; + +/** + * Provides a convenient Java interface to the Intel RealSense + * T265 V-SLAM camera. Only the subset of the librealsense that is useful + * to robot tracking is exposed in this class. + *

+ * We employ JNI to call librealsense. There are Java bindings for + * librealsense, but they are not complete and do not support our usecase. + *

+ * This class works entirely in 2d, even though the tracking camera supports + * giving us a third dimension (Z). + *

+ * The coordinate system is as follows: + * + X == Camera forwards + * + Y == Camera Left (left is from the perspective of a viewer standing behind the camera) + *

+ * All distance units are meters. All time units are seconds. + */ +public class T265Camera +{ + + static + { + // FIXME: Use System.loadLibrary + System.load(Paths.get(System.getProperty("user.home"), "libspartronicsnative.so").toAbsolutePath().toString()); + + // Cleanup is quite tricky for us, because the native code has no idea when Java + // will be done. This is why we can't use smart pointers in the native code. + // Even worse, trying to cleanup with atexit in the native code is too late and + // results in unfinished callbacks blocking. As a result a shutdown hook is our + // best option. + Runtime.getRuntime().addShutdownHook(new Thread(() -> T265Camera.cleanup())); + } + + public static enum PoseConfidence + { + Failed, + Low, + Medium, + High, + } + + public static class CameraUpdate + { + + /** + * The robot's pose in meters. + */ + public final Pose2d pose; + /** + * The robot's velocity in meters/sec and radians/sec. + */ + public final Twist2d velocity; + public final PoseConfidence confidence; + + public CameraUpdate(Pose2d pose, Twist2d velocity, PoseConfidence confidence) + { + this.pose = pose; + this.velocity = velocity; + this.confidence = confidence; + } + } + + private long mNativeCameraObjectPointer = 0; + private boolean mIsStarted = false; + private Pose2d mRobotOffset; + private Pose2d mZeroingOffset = new Pose2d(); + private Pose2d mLastRecievedPose = new Pose2d(); + private Consumer mPoseConsumer = null; + + /** + * This method constructs a T265 camera and sets it up with the right info. + * {@link T265Camera#start() start} will not be called, you must call it + * yourself. + * + * @param robotOffset Offset of the center of the robot from the center + * of the camera. + * @param odometryCovariance Covariance of the odometry input when doing + * sensor fusion (you probably want to tune this). + */ + public T265Camera(Pose2d robotOffset, double odometryCovariance) + { + this(robotOffset, odometryCovariance, ""); + } + + /** + * This method constructs a T265 camera and sets it up with the right info. + * {@link T265Camera#start() start} will not be called, you must call it + * yourself. + * + * @param robotOffsetMeters Offset of the center of the robot from the center + * of the camera. Units are meters. + * @param odometryCovariance Covariance of the odometry input when doing + * sensor fusion (you probablywant to tune this) + * @param relocMapPath path (including filename) to a relocalization map + * to load. + */ + public T265Camera(Pose2d robotOffsetMeters, double odometryCovariance, String relocMapPath) + { + mNativeCameraObjectPointer = newCamera(relocMapPath); + setOdometryInfo((float) robotOffsetMeters.getTranslation().getX(), (float) robotOffsetMeters.getTranslation().getY(), + (float) robotOffsetMeters.getRotation().getRadians(), odometryCovariance); + mRobotOffset = robotOffsetMeters; + } + + /** + * This allows the user-provided pose recieve callback to recieve data. + * This will also reset the camera's pose to (0, 0) at 0 degrees. + *

+ * This will not restart the camera following exportRelocalizationMap. You will + * have to call {@link T265Camera#free()} and make a new {@link T265Camera}. + * This is related to what appears to be a bug in librealsense. + * + * @param poseConsumer A method to be called every time we recieve a pose from + * from a different thread! You must synchronize + * memory access accross threads! + *

+ * Recieved poses are in meters. + */ + public synchronized void start(Consumer poseConsumer) + { + if (mIsStarted) + throw new RuntimeException("T265 camera is already started"); + setPose(new Pose2d()); + mPoseConsumer = poseConsumer; + mIsStarted = true; + } + + /** + * This allows the callback to recieve data, but it does not internally stop the + * camera. + */ + public synchronized void stop() + { + mIsStarted = false; + } + + /** + * Exports a binary relocalization map file to the given path. + * This will stop the camera. Because of a librealsense bug the camera isn't + * restarted after you call this method. TODO: Fix that. + * + * @param path Path (with filename) to export to + */ + public native void exportRelocalizationMap(String path); + + /** + * Sends robot velocity as computed from wheel encoders. + * + * @param velocity The robot's translational velocity in meters/sec. + */ + public void sendOdometry(Twist2d velocity) + { + Pose2d transVel = velocity.exp(); + // Only 1 odometry sensor is supported for now (index 0) + sendOdometryRaw(0, (float) transVel.getTranslation().getX(), (float) transVel.getTranslation().getY()); + } + + /** + * This zeroes the camera pose to the provided new pose. + * + * @param newPose The pose the camera should be zeroed to. + */ + public synchronized void setPose(Pose2d newPose) + { + mZeroingOffset = newPose.transformBy(new Pose2d(mLastRecievedPose.getTranslation().inverse(), mLastRecievedPose.getRotation().inverse())); + } + + /** + * This will free the underlying native objects. You probably don't want to use + * this; on program shutdown the native code will gracefully stop and delete any + * remaining objects. + */ + public native void free(); + + private native void setOdometryInfo(float robotOffsetX, float robotOffsetY, float robotOffsetRads, double measurementCovariance); + + private native void sendOdometryRaw(int sensorIndex, float xVel, float yVel); + + private native long newCamera(String mapPath); + + private static native void cleanup(); + + private synchronized void consumePoseUpdate(float x, float y, float radians, float dx, float dtheta, int confOrdinal) + { + // First we apply an offset to go from the camera coordinate system to the + // robot coordinate system with an origin at the center of the robot. This + // is not a directional transformation. + // Then we transform the pose our camera is giving us so that it reports is + // the robot's pose, not the camera's. This is a directional transformation. + final Pose2d currentPose = + new Pose2d(x - mRobotOffset.getTranslation().getX(), y - mRobotOffset.getTranslation().getY(), Rotation2d.fromRadians(radians)) + .transformBy(mRobotOffset); + + mLastRecievedPose = currentPose; + + if (!mIsStarted) + return; + + // See + // https://github.com/IntelRealSense/librealsense/blob/7f2ba0de8769620fd672f7b44101f0758e7e2fb3/include/librealsense2/h/rs_types.h#L115 + // for ordinals + PoseConfidence confidence; + switch (confOrdinal) + { + case 0x0: + confidence = PoseConfidence.Failed; + break; + case 0x1: + confidence = PoseConfidence.Low; + break; + case 0x2: + confidence = PoseConfidence.Medium; + break; + case 0x3: + confidence = PoseConfidence.High; + break; + default: + throw new RuntimeException("Unknown confidence ordinal \"" + confOrdinal + "\" passed from native code"); + } + + final Pose2d transformedPose = + new Pose2d(currentPose.getTranslation().translateBy(mZeroingOffset.getTranslation()).rotateBy(mZeroingOffset.getRotation()), + currentPose.getRotation().rotateBy(mZeroingOffset.getRotation())); + mPoseConsumer.accept(new CameraUpdate(transformedPose, new Twist2d(dx, 0.0, Rotation2d.fromRadians(dtheta)), confidence)); + } + + /** + * Thrown if something goes wrong in the native code + */ + public static class CameraJNIException extends RuntimeException + { + + // This must be static _and_ have this constructor if you want it to be + // thrown from native code + public CameraJNIException(String message) + { + super(message); + } + } +} diff --git a/src/main/java/com/spartronics4915/lib/math/PolynomialRegression.java b/src/main/java/com/spartronics4915/lib/math/PolynomialRegression.java new file mode 100644 index 0000000..eb1d6fe --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/PolynomialRegression.java @@ -0,0 +1,204 @@ +// NOTE: This file is available at http://algs4.cs.princeton.edu/14analysis/PolynomialRegression.java.html +package com.spartronics4915.lib.math; + +/****************************************************************************** + * Compilation: javac -cp .:jama.jar PolynomialRegression.java + * Execution: java -cp .:jama.jar PolynomialRegression + * Dependencies: jama.jar StdOut.java + * + * % java -cp .:jama.jar PolynomialRegression + * 0.01 n^3 + -1.64 n^2 + 168.92 n + -2113.73 (R^2 = 0.997) + * + ******************************************************************************/ + +import Jama.Matrix; +import Jama.QRDecomposition; + +/** + * The {@code PolynomialRegression} class performs a polynomial regression on an + * set of N data points ( + * yi, xi). That is, it fits a + * polynomial y = β0 + + * β1 x + β2 x2 + + * ... + βd + * xd (where y is the response variable, + * x is the predictor variable, and + * the βi are the regression coefficients) that + * minimizes the sum of squared residuals of the + * multiple regression model. It also computes associated the coefficient of + * determination R2. + *

+ * This implementation performs a QR-decomposition of the underlying Vandermonde + * matrix, so it is not the fastest or + * most numerically stable way to perform the polynomial regression. + * + * @author Robert Sedgewick + * @author Kevin Wayne + */ +public class PolynomialRegression +{ + + private int degree; // degree of the polynomial regression + private Matrix beta; // the polynomial regression coefficients + private double sse; // sum of squares due to error + private double sst; // total sum of squares + + public PolynomialRegression(double[][] xy, int degree) + { + double[] x = new double[xy.length]; + double[] y = new double[xy.length]; + for (int i = 0; i < xy.length; ++i) + { + x[i] = xy[i][0]; + y[i] = xy[i][1]; + } + solve(x, y, degree); + } + + /** + * Performs a polynomial regression on the data points {@code (y[i], x[i])}. + * + * @param x the values of the predictor variable + * @param y the corresponding values of the response variable + * @param degree the degree of the polynomial to fit + */ + public PolynomialRegression(double[] x, double[] y, int degree) + { + solve(x, y, degree); + } + + private void solve(double[] x, double[] y, int degree) + { + this.degree = degree; + + int n = x.length; + QRDecomposition qr = null; + Matrix matrixX = null; + + // in case Vandermonde matrix does not have full rank, reduce degree until it does + while (true) + { + + // build Vandermonde matrix + double[][] vandermonde = new double[n][this.degree + 1]; + for (int i = 0; i < n; i++) + { + for (int j = 0; j <= this.degree; j++) + { + vandermonde[i][j] = Math.pow(x[i], j); + } + } + matrixX = new Matrix(vandermonde); + + // find least squares solution + qr = new QRDecomposition(matrixX); + if (qr.isFullRank()) + break; + + // decrease degree and try again + this.degree--; + } + + // create matrix from vector + Matrix matrixY = new Matrix(y, n); + + // linear regression coefficients + beta = qr.solve(matrixY); + + // mean of y[] values + double sum = 0.0; + for (int i = 0; i < n; i++) + sum += y[i]; + double mean = sum / n; + + // total variation to be accounted for + for (int i = 0; i < n; i++) + { + double dev = y[i] - mean; + sst += dev * dev; + } + + // variation not accounted for + Matrix residuals = matrixX.times(beta).minus(matrixY); + sse = residuals.norm2() * residuals.norm2(); + } + + /** + * Returns the {@code j}th regression coefficient. + * + * @param j the index + * @return the {@code j}th regression coefficient + */ + public double beta(int j) + { + // to make -0.0 print as 0.0 + if (Math.abs(beta.get(j, 0)) < 1E-4) + return 0.0; + return beta.get(j, 0); + } + + /** + * Returns the degree of the polynomial to fit. + * + * @return the degree of the polynomial to fit + */ + public int degree() + { + return degree; + } + + /** + * Returns the coefficient of determination R2. + * + * @return the coefficient of determination R2, which is a + * real number between 0 and 1 + */ + public double R2() + { + if (sst == 0.0) + return 1.0; // constant function + return 1.0 - sse / sst; + } + + /** + * Returns the expected response {@code y} given the value of the predictor + * variable {@code x}. + * + * @param x the value of the predictor variable + * @return the expected response {@code y} given the value of the predictor + * variable {@code x} + */ + public double predict(double x) + { + // horner's method + double y = 0.0; + for (int j = degree; j >= 0; j--) + y = beta(j) + (x * y); + return y; + } + + @Override + public String toString() + { + StringBuilder s = new StringBuilder(); + int j = degree; + + // ignoring leading zero coefficients + while (j >= 0 && Math.abs(beta(j)) < 1E-5) + j--; + + // create remaining terms + while (j >= 0) + { + if (j == 0) + s.append(String.format("%.2f ", beta(j))); + else if (j == 1) + s.append(String.format("%.2f x + ", beta(j))); + else + s.append(String.format("%.2f x^%d + ", beta(j), j)); + j--; + } + s = s.append(" (R^2 = " + String.format("%.3f", R2()) + ")"); + return s.toString(); + } +} diff --git a/src/main/java/com/spartronics4915/lib/math/Util.java b/src/main/java/com/spartronics4915/lib/math/Util.java new file mode 100644 index 0000000..e3c7b85 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/Util.java @@ -0,0 +1,60 @@ +package com.spartronics4915.lib.math; + +import java.util.List; + +/** + * Contains basic functions that are used often. + */ +public class Util +{ + + public static final double kEpsilon = 1e-9; + + private Util() + { + } + + /** + * Limits the given input to the given magnitude. + */ + public static double limit(double v, double maxMagnitude) + { + return limit(v, -maxMagnitude, maxMagnitude); + } + + public static double limit(double v, double min, double max) + { + return Math.min(max, Math.max(min, v)); + } + + public static double interpolate(double startValue, double endValue, double t) + { + t = limit(t, 0.0, 1.0); + return startValue + (endValue - startValue) * t; + } + + public static boolean epsilonEquals(double a, double b, double epsilon) + { + return (a - epsilon <= b) && (a + epsilon >= b); + } + + public static boolean epsilonEquals(double a, double b) + { + return epsilonEquals(a, b, kEpsilon); + } + + public static boolean epsilonEquals(int a, int b, int epsilon) + { + return (a - epsilon <= b) && (a + epsilon >= b); + } + + public static boolean allCloseTo(final List list, double value, double epsilon) + { + boolean result = true; + for (Double value_in : list) + { + result &= epsilonEquals(value_in, value, epsilon); + } + return result; + } +} diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/control/FeedForwardTracker.java b/src/main/java/com/spartronics4915/lib/math/twodim/control/FeedForwardTracker.java new file mode 100644 index 0000000..9f41c86 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/control/FeedForwardTracker.java @@ -0,0 +1,31 @@ +package com.spartronics4915.lib.math.twodim.control; + +import com.spartronics4915.lib.math.twodim.geometry.Pose2d; +import com.spartronics4915.lib.math.twodim.geometry.Pose2dWithCurvature; +import com.spartronics4915.lib.math.twodim.trajectory.types.TimedTrajectory.TimedIterator; + +/** + * This tracker follows without any external disturbance correction based on + * odometry. Hence it feeds the path's target velocities and accelerations + * forward to the drivetrain, without modification. + */ +public class FeedForwardTracker extends TrajectoryTracker { + + @Override + protected TrajectoryTrackerVelocityOutput calculateState( + TimedIterator iterator, + Pose2d currentRobotPoseMeters + ) { + var referenceState = iterator.getCurrentSample().state; + + double vLinear, vAngular; + vLinear = referenceState.velocity; + vAngular = vLinear * referenceState.state.getCurvature(); + + return new TrajectoryTrackerVelocityOutput( + vLinear, + vAngular + ); + } + +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/control/PurePursuitTracker.java b/src/main/java/com/spartronics4915/lib/math/twodim/control/PurePursuitTracker.java new file mode 100644 index 0000000..a04810d --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/control/PurePursuitTracker.java @@ -0,0 +1,108 @@ +package com.spartronics4915.lib.math.twodim.control; + +import com.spartronics4915.lib.math.twodim.geometry.Pose2d; +import com.spartronics4915.lib.math.twodim.geometry.Pose2dWithCurvature; +import com.spartronics4915.lib.math.twodim.geometry.Rotation2d; +import com.spartronics4915.lib.math.twodim.geometry.Translation2d; +import com.spartronics4915.lib.math.twodim.trajectory.types.TimedTrajectory.TimedIterator; + +/** + * Uses an adaptive pure pursuit controller to steer the robot back onto the + * desired trajectory. From + * https://www.ri.cmu.edu/pub_files/pub3/coulter_r_craig_1992_1/coulter_r_craig_1992_1.pdf + */ +public class PurePursuitTracker extends TrajectoryTracker { + + private final double kLat; + /** Seconds */ + private final double kLookaheadTime; + /** Meters */ + private final double kMinLookaheadDistance; + + /** + * @param lat Latitudinal error gain. This is effectively a + * "P" term in a PID controller. You tune this + * about the same way you would tune P + * normally. + * @param lookaheadTimeSeconds Lookahead time in seconds. Larger values + * mean slower but more stable convergence. + * @param minLookaheadDistanceMeters Minimum lookahead distance in meters. Helps + * with stability, mostly when you're near the + * end of the path. + */ + public PurePursuitTracker(double lat, double lookaheadTimeSeconds, double minLookaheadDistanceMeters) { + kLat = lat; + kLookaheadTime = lookaheadTimeSeconds; + kMinLookaheadDistance = minLookaheadDistanceMeters; + } + + @Override + protected TrajectoryTrackerVelocityOutput calculateState(TimedIterator iterator, Pose2d currentRobotPoseMeters) { + // Target point on the path for the current time + var referencePoint = iterator.getCurrentSample(); + + // Field-relative pose of the lookahead point + Pose2d lookaheadState = calculateLookaheadPose(iterator, currentRobotPoseMeters); + + // Robot-relative transform needed to get to the lookaheda + Pose2d lookaheadTransform = lookaheadState.inFrameReferenceOf(currentRobotPoseMeters); + + // Calculate latitudinal error + double xError = (referencePoint.state.state.getPose().inFrameReferenceOf(currentRobotPoseMeters).getTranslation().getX()); + + // Calculate the velocity at the reference point (meters/sec) + double vd = referencePoint.state.velocity; + + // Distance from the robot to the lookahead (meters) + double l = lookaheadTransform.getTranslation().norm(); + + // Calculate the curvature of the arc that connects the robot and the lookahead point + double curvature = 2 * lookaheadTransform.getTranslation().getY() / Math.pow(l, 2); + + // Adjust the linear velocity proportional to the product of a gain and the error, and the rotational error + double adjustedLinearVelocity = vd * lookaheadTransform.getRotation().getCos() + kLat * xError; + + return new TrajectoryTrackerVelocityOutput( + adjustedLinearVelocity, + adjustedLinearVelocity * curvature // velocity * curvature = angular velocity + ); + } + + private Pose2d calculateLookaheadPose(TimedIterator iterator, Pose2d robotPose) { + Pose2d lookaheadPoseByTime = iterator.preview(kLookaheadTime).state.state.getPose(); + + // If the lookahead point is farther from the robot than the min lookahead + // distance we just use the lookaheadPoseByTime. + if (lookaheadPoseByTime.inFrameReferenceOf(robotPose).getTranslation().norm() >= kMinLookaheadDistance) { + return lookaheadPoseByTime; + } + + Pose2d lookaheadPoseByDistance = iterator.getCurrentSample().state.state.getPose(); + double previewedTime = kLookaheadTime; // Seconds + + // Preview forwards in the trajectory until the distance from the robot to the + // previewed point is greater than the minimum lookahead distance. If we reach + // the end of the trajectry then we just extend the trajectory in the direction + // of the final pose. + while (iterator.getRemainingProgress() > previewedTime) { + previewedTime += 0.02; + + // Remember that TrajectoryIterator::preview previews in an additive manner + // E.g. in this situation iterator.preview(3) would preview at iterator.getProgress() + 3 + lookaheadPoseByDistance = iterator.preview(previewedTime).state.state.getPose(); + double lookaheadDistance = lookaheadPoseByDistance.inFrameReferenceOf(robotPose).getTranslation().norm(); + + if (lookaheadDistance > kMinLookaheadDistance) { + return lookaheadPoseByDistance; + } + } + + // Extend the trajectory + double remaining = + kMinLookaheadDistance - (lookaheadPoseByDistance.inFrameReferenceOf(robotPose).getTranslation().norm()); + return lookaheadPoseByDistance.transformBy( + new Pose2d(new Translation2d(remaining * (iterator.getTrajectory().isReversed() ? -1 : 1), 0.0), new Rotation2d()) + ); + } + +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/control/RamseteTracker.java b/src/main/java/com/spartronics4915/lib/math/twodim/control/RamseteTracker.java new file mode 100644 index 0000000..b4433d6 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/control/RamseteTracker.java @@ -0,0 +1,66 @@ +package com.spartronics4915.lib.math.twodim.control; + +import com.spartronics4915.lib.math.Util; +import com.spartronics4915.lib.math.twodim.geometry.Pose2d; +import com.spartronics4915.lib.math.twodim.geometry.Pose2dWithCurvature; +import com.spartronics4915.lib.math.twodim.trajectory.types.TimedTrajectory.TimedIterator; + +/** + * Uses a time-varying non linear reference controller to steer the robot back + * onto the trajectory. + * + * From https://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf eq 5.12 + * + * https://file.tavsys.net/control/state-space-guide.pdf section 8.5 is also + * helpful and a little more accessible + */ +public class RamseteTracker extends TrajectoryTracker { + // These terms don't work out dimensionally + // But whatever, it works + private final double kBeta, kZeta; + + /** + * @param beta Constant for correction. Increase for more aggresive convergence. + * beta > 0 + * @param zeta Dampening constant. Increase for more dampening. zeta ∈ (0,1) + */ + public RamseteTracker(double beta, double zeta) { + kBeta = beta; + kZeta = zeta; + } + + @Override + protected TrajectoryTrackerVelocityOutput calculateState( + TimedIterator iterator, + Pose2d currentRobotPoseMeters + ) { + var referenceState = iterator.getCurrentSample().state; + + // This can be thought of as the goal pose in robot coordinates + Pose2d error = referenceState.state.getPose().inFrameReferenceOf(currentRobotPoseMeters); + + // Get reference linear and angular velocities (meters/sec, rads/sec) + double vd = referenceState.velocity; + double wd = vd * referenceState.state.getCurvature(); + + // Compute the first time-varying gain + double k1 = 2 * kZeta * Math.sqrt(wd * wd + kBeta * vd * vd); + + // Get bounded angular error (radians) + double angleError = error.getRotation().getRadians(); + + return new TrajectoryTrackerVelocityOutput( + vd * error.getRotation().getCos() + k1 * error.getTranslation().getX(), + wd + kBeta * vd * sinc(angleError) * error.getTranslation().getY() + k1 * angleError + ); + } + + private double sinc(double theta) { + if (Util.epsilonEquals(theta, 0)) { + return 1 - 1 / 6 * theta * theta; + } else { + return Math.sin(theta) / theta; + } + } + +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/control/TrajectoryTracker.java b/src/main/java/com/spartronics4915/lib/math/twodim/control/TrajectoryTracker.java new file mode 100644 index 0000000..f59a7e7 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/control/TrajectoryTracker.java @@ -0,0 +1,113 @@ +package com.spartronics4915.lib.math.twodim.control; + +import com.spartronics4915.lib.math.twodim.geometry.Pose2d; +import com.spartronics4915.lib.math.twodim.geometry.Pose2dWithCurvature; +import com.spartronics4915.lib.math.twodim.physics.DifferentialDrive; +import com.spartronics4915.lib.math.twodim.trajectory.types.TimedTrajectory; +import com.spartronics4915.lib.math.twodim.trajectory.types.TimedTrajectory.TimedIterator; +import com.spartronics4915.lib.math.twodim.trajectory.types.TimedTrajectory.TimedState; +import com.spartronics4915.lib.math.twodim.trajectory.types.Trajectory.TrajectorySamplePoint; +import com.spartronics4915.lib.util.DeltaTime; + +public abstract class TrajectoryTracker { + private final DeltaTime mDeltaTimeController = new DeltaTime(); + + private TimedIterator mTrajectoryIterator = null; + private TrajectoryTrackerVelocityOutput mPreviousVelocity = null; + + public TrajectorySamplePoint> getReferencePoint() { + return mTrajectoryIterator.getCurrentSample(); + } + + public boolean isFinished() { + return mTrajectoryIterator == null ? true : mTrajectoryIterator.isDone(); + } + + public void reset(TimedTrajectory newTrajectory) { + mTrajectoryIterator = new TimedIterator<>(newTrajectory); + mDeltaTimeController.reset(); + mPreviousVelocity = null; + } + + public TrajectoryTrackerOutput nextState( + Pose2d currentRobotPoseMeters, + double currentTimeSeconds + ) { + var iterator = mTrajectoryIterator; + if (iterator == null) { + throw new RuntimeException("You can't get the next state from the TrajectoryTracker without a trajectory! Call TrajectoryTracker::reset first."); + } + double deltaTime = mDeltaTimeController.updateTime(currentTimeSeconds); + iterator.advance(deltaTime); + + TrajectoryTrackerVelocityOutput velocity = + calculateState(iterator, currentRobotPoseMeters); + var previousVelocity = mPreviousVelocity; + mPreviousVelocity = velocity; + + if (previousVelocity == null || deltaTime <= 0) { + // There should be no acceleration initially + return new TrajectoryTrackerOutput( + velocity.linearVelocity, 0.0, velocity.angularVelocity, 0.0 + ); + } else { + return new TrajectoryTrackerOutput( + velocity.linearVelocity, + (velocity.linearVelocity - previousVelocity.linearVelocity) / deltaTime, + velocity.angularVelocity, + (velocity.angularVelocity - previousVelocity.angularVelocity) / deltaTime + ); + } + } + + protected abstract TrajectoryTrackerVelocityOutput calculateState( + TimedIterator iterator, + Pose2d currentRobotPoseMeters + ); + + protected static class TrajectoryTrackerVelocityOutput { + /** Meters/sec */ + public final double linearVelocity; + /** Rads/sec */ + public final double angularVelocity; + + public TrajectoryTrackerVelocityOutput( + double linearVelocityMetersSecond, + double angularVelocityRadsSecond + ) { + this.linearVelocity = linearVelocityMetersSecond; + this.angularVelocity = angularVelocityRadsSecond; + } + } + + public static class TrajectoryTrackerOutput { + /** Meters/sec */ + public final double linearVelocity; + /** Meters/sec^2 */ + public final double linearAcceleration; + /** Rads/sec */ + public final double angularVelocity; + /** Rads/sec^2 */ + public final double angularAcceleration; + + public TrajectoryTrackerOutput( + double linearVelocityMetersSecond, + double linearAccelerationMetersSecondSq, + double angularVelocityRadsSecond, + double angularAccelerationRadsSecondSq + ) { + this.linearVelocity = linearVelocityMetersSecond; + this.linearAcceleration = linearAccelerationMetersSecondSq; + this.angularVelocity = angularVelocityRadsSecond; + this.angularAcceleration = angularAccelerationRadsSecondSq; + } + + public DifferentialDrive.ChassisState getDifferentialDriveAcceleration() { + return new DifferentialDrive.ChassisState(linearAcceleration, angularAcceleration); + } + + public DifferentialDrive.ChassisState getDifferentialDriveVelocity() { + return new DifferentialDrive.ChassisState(linearVelocity, angularVelocity); + } + } +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/geometry/Pose2d.java b/src/main/java/com/spartronics4915/lib/math/twodim/geometry/Pose2d.java new file mode 100644 index 0000000..c9b5e10 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/geometry/Pose2d.java @@ -0,0 +1,199 @@ +package com.spartronics4915.lib.math.twodim.geometry; + +import com.spartronics4915.lib.math.Util; +import com.spartronics4915.lib.math.twodim.trajectory.types.State; + +/** + * Represents a 2d pose (rigid transform) containing translational and + * rotational elements. + */ +public class Pose2d implements State +{ + + private final Translation2d mTranslation; + private final Rotation2d mRotation; + + public Pose2d() + { + mTranslation = new Translation2d(); + mRotation = new Rotation2d(); + } + + public Pose2d(double x, double y, final Rotation2d rotation) + { + mTranslation = new Translation2d(x, y); + mRotation = rotation; + } + + public Pose2d(final Translation2d translation, final Rotation2d rotation) + { + mTranslation = translation; + mRotation = rotation; + } + + public Pose2d(final Pose2d other) + { + mTranslation = new Translation2d(other.mTranslation); + mRotation = new Rotation2d(other.mRotation); + } + + public static Pose2d fromTranslation(final Translation2d translation) + { + return new Pose2d(translation, new Rotation2d()); + } + + public static Pose2d fromRotation(final Rotation2d rotation) + { + return new Pose2d(new Translation2d(), rotation); + } + + /** + * Logical inverse of the above: obtains a Twist2d from a delta-pose + */ + public Twist2d log() + { + final double dtheta = getRotation().getRadians(); + final double halfDtheta = 0.5 * dtheta; + final double cosMinusOne = getRotation().getCos() - 1.0; + double halfCos; // halftheta_by_tan_of_halfdtheta; + if (Math.abs(cosMinusOne) < Util.kEpsilon) + { + halfCos = 1.0 - 1.0 / 12.0 * dtheta * dtheta; + } + else + { + halfCos = -(halfDtheta * getRotation().getSin()) / cosMinusOne; + } + final Translation2d transPart = getTranslation() + .rotateBy(new Rotation2d(halfCos, -halfDtheta, false)); + return new Twist2d(transPart.getX(), transPart.getY(), Rotation2d.fromRadians(dtheta)); + } + + public Translation2d getTranslation() + { + return mTranslation; + } + + public Rotation2d getRotation() + { + return mRotation; + } + + /** + * Transforming this Pose2d means first translating by + * other.translation and then rotating by + * other.rotation + * + * @param other The other transform. + * @return This transform * other + */ + public Pose2d transformBy(final Pose2d other) + { + return new Pose2d(mTranslation.translateBy(other.mTranslation.rotateBy(mRotation)), + mRotation.rotateBy(other.mRotation)); + } + + /** + * This transforms the pose directionally. E.g.: + * if we're at 0, 0, 0 and we transform by 3, + * we would be at 3, 0, 0. + * + * @param scalar A scalar to transform this directionally by + * @return This translated by the scalar in the direction of the rotation + */ + public Pose2d transformBy(final double scalar) + { + return new Pose2d( + this.getRotation().getCos() * scalar + this.getTranslation().getX(), + this.getRotation().getSin() * scalar + this.getTranslation().getY(), + this.getRotation()); + } + + /** + * The inverse of this Pose2d "undoes" the effect of applying our transform. + * + * For p = new Pose2d(10, 10, -30deg) + * np = p.inverse() + * + * @return The opposite of this transform. + */ + public Pose2d inverse() + { + Rotation2d invRot = mRotation.inverse(); + return new Pose2d(mTranslation.inverse().rotateBy(invRot), invRot); + } + + public Pose2d inFrameReferenceOf(Pose2d fieldRelativeOrigin) { + return fieldRelativeOrigin.inverse().transformBy(this); + } + + public Pose2d normal() + { + return new Pose2d(mTranslation, mRotation.normal()); + } + + /** + * Return true if this pose is (nearly) colinear with the another. + */ + public boolean isColinear(final Pose2d other) + { + if (!getRotation().isParallel(other.getRotation())) + return false; + final Twist2d twist = inverse().transformBy(other).log(); + return (Util.epsilonEquals(twist.dy, 0.0) && Util.epsilonEquals(twist.dtheta.getRadians(), 0.0)); + } + + public boolean epsilonEquals(final Pose2d other, double epsilon) + { + return getTranslation().epsilonEquals(other.getTranslation(), epsilon) + && getRotation().isParallel(other.getRotation()); + } + + /** + * Do twist interpolation of this pose assuming constant curvature. + */ + @Override + public Pose2d interpolate(final Pose2d endValue, double t) + { + if (t <= 0) + { + return new Pose2d(this); + } + else if (t >= 1) + { + return new Pose2d(endValue); + } + final Twist2d twist = inverse().transformBy(endValue).log(); + return transformBy(twist.scaled(t).exp()); + } + + @Override + public String toString() + { + return "T:" + mTranslation.toString() + ", R:" + mRotation.toString(); + } + + @Override + public double distance(final Pose2d other) + { + return inverse().transformBy(other).log().norm(); + } + + @Override + public boolean equals(final Object other) + { + if (other == null || !(other instanceof Pose2d)) + return false; + return epsilonEquals((Pose2d) other, Util.kEpsilon); + } + + public Pose2d getPose() + { + return this; + } + + public Pose2d mirror() + { + return new Pose2d(new Translation2d(getTranslation().getX(), -getTranslation().getY()), getRotation().inverse()); + } +} diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/geometry/Pose2dWithCurvature.java b/src/main/java/com/spartronics4915/lib/math/twodim/geometry/Pose2dWithCurvature.java new file mode 100644 index 0000000..b2d3c1a --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/geometry/Pose2dWithCurvature.java @@ -0,0 +1,114 @@ +package com.spartronics4915.lib.math.twodim.geometry; + +import com.spartronics4915.lib.math.Util; +import com.spartronics4915.lib.math.twodim.trajectory.types.State; + +import java.text.DecimalFormat; + +public class Pose2dWithCurvature implements State +{ + + private final Pose2d mPose; + private final double mCurvature; + private final double mDCurvatureDS; + + public Pose2dWithCurvature() + { + mPose = new Pose2d(); + mCurvature = 0.0; + mDCurvatureDS = 0.0; + } + + public Pose2dWithCurvature(final Pose2d pose, double curvature) + { + mPose = pose; + mCurvature = curvature; + mDCurvatureDS = 0.0; + } + + public Pose2dWithCurvature(final Pose2d pose, double curvature, double dCurvatureDS) + { + mPose = pose; + mCurvature = curvature; + mDCurvatureDS = dCurvatureDS; + } + + public Pose2dWithCurvature(final Translation2d translation, final Rotation2d rotation, double curvature) + { + mPose = new Pose2d(translation, rotation); + mCurvature = curvature; + mDCurvatureDS = 0.0; + } + + public Pose2dWithCurvature(final Translation2d translation, final Rotation2d rotation, double curvature, double dCurvatureDS) + { + mPose = new Pose2d(translation, rotation); + mCurvature = curvature; + mDCurvatureDS = dCurvatureDS; + } + + public final Pose2d getPose() + { + return mPose; + } + + public Pose2dWithCurvature transformBy(Pose2d transform) + { + return new Pose2dWithCurvature(getPose().transformBy(transform), getCurvature(), getDCurvatureDs()); + } + + public Pose2dWithCurvature mirror() + { + return new Pose2dWithCurvature(getPose().mirror().getPose(), -getCurvature(), -getDCurvatureDs()); + } + + public double getCurvature() + { + return mCurvature; + } + + public double getDCurvatureDs() + { + return mDCurvatureDS; + } + + public final Translation2d getTranslation() + { + return getPose().getTranslation(); + } + + public final Rotation2d getRotation() + { + return getPose().getRotation(); + } + + @Override + public Pose2dWithCurvature interpolate(final Pose2dWithCurvature endValue, double t) + { + return new Pose2dWithCurvature(getPose().interpolate(endValue.getPose(), t), + Util.interpolate(getCurvature(), endValue.getCurvature(), t), + Util.interpolate(getDCurvatureDs(), endValue.getDCurvatureDs(), t)); + } + + @Override + public double distance(final Pose2dWithCurvature other) + { + return getPose().distance(other.getPose()); + } + + public boolean equals(final Object other) + { + if (other == null || !(other instanceof Pose2dWithCurvature)) + return false; + Pose2dWithCurvature p2dwc = (Pose2dWithCurvature) other; + return getPose().equals(p2dwc.getPose()) && Util.epsilonEquals(getCurvature(), p2dwc.getCurvature()) + && Util.epsilonEquals(getDCurvatureDs(), p2dwc.getDCurvatureDs()); + } + + @Override + public String toString() + { + final DecimalFormat fmt = new DecimalFormat("#0.000"); + return getPose().toString() + ", curvature: " + fmt.format(getCurvature()) + ", dcurvature_ds: " + fmt.format(getDCurvatureDs()); + } +} diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/geometry/Rectangle2d.java b/src/main/java/com/spartronics4915/lib/math/twodim/geometry/Rectangle2d.java new file mode 100644 index 0000000..ff7bf16 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/geometry/Rectangle2d.java @@ -0,0 +1,158 @@ +package com.spartronics4915.lib.math.twodim.geometry; + +import java.util.Arrays; +import java.util.Objects; + +import com.spartronics4915.lib.math.Util; + +public class Rectangle2d { + + private double mX, mY; + private double mWidth, mHeight; + + /** + * @param x X coordinate of the bottom left hand corner. + * @param y Y coordinate of the bottom left hand corner. + */ + public Rectangle2d(double x, double y, double width, double height) { + mX = x; + mY = y; + mWidth = width; + mHeight = height; + } + + public Rectangle2d(Translation2d one, Translation2d two) { + double minX, minY, maxX, maxY; + minX = Math.min(one.getX(), two.getX()); + minY = Math.min(one.getY(), two.getY()); + maxX = Math.max(one.getX(), two.getX()); + maxY = Math.max(one.getY(), two.getY()); + + mX = minX; + mY = minY; + mWidth = maxX - minX; + mHeight = maxY - minY; + } + + public Rectangle2d(Translation2d... points) { + double minX, minY, maxX, maxY; + minX = Arrays.stream(points).map((t) -> t.getX()).min(Double::compare).orElseThrow(); + minY = Arrays.stream(points).map((t) -> t.getY()).min(Double::compare).orElseThrow(); + maxX = Arrays.stream(points).map((t) -> t.getX()).max(Double::compare).orElseThrow(); + maxY = Arrays.stream(points).map((t) -> t.getY()).max(Double::compare).orElseThrow(); + + mX = minX; + mY = minY; + mWidth = maxX - minX; + mHeight = maxY - minY; + } + + public double x() { + return mX; + } + + public double y() { + return mY; + } + + public double width() { + return mWidth; + } + + public double height() { + return mHeight; + } + + public Translation2d getTopLeft() { + return new Translation2d(mX, mY + mHeight); + } + + public Translation2d getTopRight() { + return new Translation2d(mX + mWidth, mY + mHeight); + } + + public Translation2d getBottomLeft() { + return new Translation2d(mX, mY); + } + + public Translation2d getBottomRight() { + return new Translation2d(mX + mWidth, mY); + } + + public Translation2d getCenter() { + return new Translation2d(mX + mWidth / 2, mX + mWidth / 2); + } + + public boolean isIn(Rectangle2d r) { + return mX < r.mX + r.mWidth && mX + mWidth > r.mX && mY < r.mY + r.mHeight && mY + mHeight > r.mY; + } + + public boolean contains(Translation2d t) { + return (t.getX() > mX && t.getX() < mX + mWidth) && (t.getY() > mY && t.getY() < mY + mHeight); + } + + public boolean doesCollide(Rectangle2d rectangle, Translation2d translation) { + if (Util.epsilonEquals(0, translation.getX()) && Util.epsilonEquals(0, translation.getY())) + return false; + // Check if it's even in range + Rectangle2d boxRect = new Rectangle2d(rectangle.getTopLeft(), rectangle.getBottomRight(), + rectangle.getTopLeft().translateBy(translation), rectangle.getBottomRight().translateBy(translation)); + if (!boxRect.isIn(this)) + return false; + + // AABB collision + // Calculate distances + double xInvEntry, xInvExit, yInvEntry, yInvExit; + if (translation.getX() > 0.0) { + xInvEntry = (mX - (rectangle.mX + rectangle.mWidth)); + xInvExit = ((mX + mWidth) - rectangle.mX); + } else { + xInvEntry = ((mX + mWidth) - rectangle.mX); + xInvExit = (mX - (rectangle.mX + rectangle.mWidth)); + } + if (translation.getY() > 0.0) { + yInvEntry = (mY - (rectangle.mY + rectangle.mHeight)); + yInvExit = ((mY + mHeight) - rectangle.mY); + } else { + yInvEntry = ((mY + mHeight) - rectangle.mY); + yInvExit = (mY - (rectangle.mY + rectangle.mHeight)); + } + // Find time of collisions + double xEntry, xExit, yEntry, yExit; + if (Util.epsilonEquals(0, translation.getX())) { + xEntry = Double.NEGATIVE_INFINITY; + xExit = Double.POSITIVE_INFINITY; + } else { + xEntry = xInvEntry / translation.getX(); + xExit = xInvExit / translation.getX(); + } + if (Util.epsilonEquals(0, translation.getY())) { + yEntry = Double.NEGATIVE_INFINITY; + yExit = Double.POSITIVE_INFINITY; + } else { + yEntry = yInvEntry / translation.getY(); + yExit = yInvExit / translation.getY(); + } + double entryTime = Math.max(xEntry, yEntry); + double exitTime = Math.min(xExit, yExit); + + return entryTime <= exitTime && (xEntry >= 0.0 || yEntry >= 0.0) && (xEntry < 1.0 || yEntry < 1.0); + } + + @Override + public boolean equals(Object o) { + if (this == o) + return true; + if (o == null) + return false; + if (!(o instanceof Rectangle2d)) + return false; + return this.mX == ((Rectangle2d) o).mX && this.mY == ((Rectangle2d) o).mY + && this.mWidth == ((Rectangle2d) o).mWidth && this.mHeight == ((Rectangle2d) o).mHeight; + } + + @Override + public int hashCode() { + return Objects.hash(mX, mY, mWidth, mHeight); + } +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/geometry/Rotation2d.java b/src/main/java/com/spartronics4915/lib/math/twodim/geometry/Rotation2d.java new file mode 100644 index 0000000..9359fe5 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/geometry/Rotation2d.java @@ -0,0 +1,183 @@ +package com.spartronics4915.lib.math.twodim.geometry; + +import com.spartronics4915.lib.util.Interpolable; +import com.spartronics4915.lib.math.Util; + +import java.text.DecimalFormat; + +/** + * A rotation in a 2d coordinate frame represented a point on the unit circle + * (cosine and sine). + */ +public class Rotation2d implements Interpolable +{ + + private final double mCosAngle; + private final double mSinAngle; + + public Rotation2d() + { + this(1, 0, false); + } + + public Rotation2d(double x, double y, boolean normalize) + { + if (normalize) + { + // From trig, we know that sin^2 + cos^2 == 1, but as we do math on this object we might accumulate rounding errors. + // Normalizing forces us to re-scale the sin and cos to reset rounding errors. + double magnitude = Math.hypot(x, y); + if (magnitude > Util.kEpsilon) + { + mSinAngle = y / magnitude; + mCosAngle = x / magnitude; + } + else + { + mSinAngle = 0; + mCosAngle = 1; + } + } + else + { + mCosAngle = x; + mSinAngle = y; + } + } + + public Rotation2d(final Rotation2d other) + { + mCosAngle = other.mCosAngle; + mSinAngle = other.mSinAngle; + } + + public Rotation2d(final Translation2d direction, boolean normalize) + { + this(direction.getX(), direction.getY(), normalize); + } + + public static Rotation2d fromRadians(double angleRadians) + { + return new Rotation2d(Math.cos(angleRadians), Math.sin(angleRadians), false); + } + + public static Rotation2d fromDegrees(double angleDegrees) + { + return fromRadians(Math.toRadians(angleDegrees)); + } + + public double getCos() + { + return mCosAngle; + } + + public double getSin() + { + return mSinAngle; + } + + public double getTan() + { + if (Math.abs(mCosAngle) < Util.kEpsilon) + { + if (mSinAngle >= 0.0) + { + return Double.POSITIVE_INFINITY; + } + else + { + return Double.NEGATIVE_INFINITY; + } + } + return mSinAngle / mCosAngle; + } + + public double getRadians() + { + return Math.atan2(mSinAngle, mCosAngle); + } + + public double getDegrees() + { + return Math.toDegrees(getRadians()); + } + + /** + * We can rotate this Rotation2d by adding together the effects of it and + * another rotation. + * + * @param other The other rotation. See: + * https://en.wikipedia.org/wiki/Rotation_matrix + * @return This rotation rotated by other. + */ + public Rotation2d rotateBy(final Rotation2d other) + { + return new Rotation2d(mCosAngle * other.mCosAngle - mSinAngle * other.mSinAngle, + mCosAngle * other.mSinAngle + mSinAngle * other.mCosAngle, true); + } + + public Rotation2d normal() + { + return new Rotation2d(-mSinAngle, mCosAngle, false); + } + + /** + * The inverse of a Rotation2d "undoes" the effect of this rotation. + * + * @return The opposite of this rotation. + */ + public Rotation2d inverse() + { + return new Rotation2d(mCosAngle, -mSinAngle, false); + } + + public boolean isParallel(final Rotation2d other) + { + return Util.epsilonEquals(toTranslation().cross(other.toTranslation()), 0.0); + } + + public Translation2d toTranslation() + { + return new Translation2d(mCosAngle, mSinAngle); + } + + @Override + public Rotation2d interpolate(final Rotation2d endValue, double t) + { + if (t <= 0) + { + return new Rotation2d(this); + } + else if (t >= 1) + { + return new Rotation2d(endValue); + } + double angleDiff = inverse().rotateBy(endValue).getRadians(); + return this.rotateBy(Rotation2d.fromRadians(angleDiff * t)); + } + + @Override + public String toString() + { + final DecimalFormat fmt = new DecimalFormat("#0.000"); + return "(" + fmt.format(getDegrees()) + " deg)"; + } + + public double distance(final Rotation2d other) + { + return inverse().rotateBy(other).getRadians(); + } + + @Override + public boolean equals(final Object other) + { + if (other == null || !(other instanceof Rotation2d)) + return false; + return distance((Rotation2d) other) < Util.kEpsilon; + } + + public Rotation2d getRotation() + { + return this; + } +} diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/geometry/Translation2d.java b/src/main/java/com/spartronics4915/lib/math/twodim/geometry/Translation2d.java new file mode 100644 index 0000000..77cb0ef --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/geometry/Translation2d.java @@ -0,0 +1,160 @@ +package com.spartronics4915.lib.math.twodim.geometry; + +import com.spartronics4915.lib.util.Interpolable; +import com.spartronics4915.lib.math.Util; + +import java.text.DecimalFormat; + +/** + * A translation in a 2d coordinate frame. Translations are simply shifts in an + * (x, y) plane. + */ +public class Translation2d implements Interpolable { + + private final double mX; + private final double mY; + + public Translation2d() { + mX = 0; + mY = 0; + } + + public Translation2d(double x, double y) { + mX = x; + mY = y; + } + + public Translation2d(final Translation2d other) { + mX = other.mX; + mY = other.mY; + } + + public Translation2d(final Translation2d start, final Translation2d end) { + mX = end.mX - start.mX; + mY = end.mY - start.mY; + } + + /** + * The "norm" of a transform is the Euclidean distance in x and y. + * + * @return sqrt(x ^ 2 + y ^ 2) + */ + public double norm() { + return Math.hypot(mX, mY); + } + + public double norm2() { + return mX * mX + mY * mY; + } + + public double getX() { + return mX; + } + + public double getY() { + return mY; + } + + /** + * We can compose Translation2d's by adding together the x and y shifts. + * + * @param other The other translation to add. + * @return The combined effect of translating by this object and the other. + */ + public Translation2d translateBy(final Translation2d other) { + return new Translation2d(mX + other.mX, mY + other.mY); + } + + /** + * We can also rotate Translation2d's. See: + * https://en.wikipedia.org/wiki/Rotation_matrix + * + * @param rotation The rotation to apply. + * @return This translation rotated by rotation. + */ + public Translation2d rotateBy(final Rotation2d rotation) { + return new Translation2d(mX * rotation.getCos() - mY * rotation.getSin(), mX * rotation.getSin() + mY * rotation.getCos()); + } + + public Rotation2d direction() { + return new Rotation2d(mX, mY, true); + } + + /** + * The inverse simply means a Translation2d that "undoes" this object. + * + * @return Translation by -x and -y. + */ + public Translation2d inverse() { + return new Translation2d(-mX, -mY); + } + + @Override + public Translation2d interpolate(final Translation2d endValue, double t) { + if (t <= 0) { + return new Translation2d(this); + } else if (t >= 1) { + return new Translation2d(endValue); + } + return extrapolate(endValue, t); + } + + public Translation2d extrapolate(final Translation2d other, double x) { + return new Translation2d(x * (other.mX - mX) + mX, x * (other.mY - mY) + mY); + } + + public Translation2d scale(double s) { + return new Translation2d(mX * s, mY * s); + } + + public boolean epsilonEquals(final Translation2d other, double epsilon) { + return Util.epsilonEquals(getX(), other.getX(), epsilon) && Util.epsilonEquals(getY(), other.getY(), epsilon); + } + + @Override + public String toString() { + final DecimalFormat fmt = new DecimalFormat("#0.000"); + return "(" + fmt.format(mX) + "," + fmt.format(mY) + ")"; + } + + /** + * @return this · b + */ + public double dot(final Translation2d b) { + return this.mX * b.mX + this.mY * b.mY; + } + + /** + * @return this ⨯ b (also the determinant of a 2x2 matrix where + * this is in the first column and b is in the second + * column) + */ + public double cross(final Translation2d b) { + return this.mX * b.mY - this.mY * b.mX; + } + + public Rotation2d getAngle(final Translation2d b) { + // The below works because a · b = |a||b|cosθ and a ⨯ b = |a||b|sinθ. Because + // both sin and cos are multiplied by the same number -- |a||b| are + // the same (and always nonnegative) -- the ratio is the same. + return Rotation2d.fromRadians(Math.atan2(this.cross(b), this.dot(b))); + } + + // This class could implement TrajectoryState because it has a distance + // method, but you would never use a Translation2d as a TrajectoryState. + // The above applies to Rotation2d and Twist2d as well. + public double getDistance(final Translation2d other) { + return inverse().translateBy(other).norm(); + } + + @Override + public boolean equals(final Object other) { + if (other == null || !(other instanceof Translation2d)) + return false; + return getDistance((Translation2d) other) < Util.kEpsilon; + } + + public Translation2d getTranslation() { + return this; + } +} diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/geometry/Twist2d.java b/src/main/java/com/spartronics4915/lib/math/twodim/geometry/Twist2d.java new file mode 100644 index 0000000..39d5b38 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/geometry/Twist2d.java @@ -0,0 +1,118 @@ +package com.spartronics4915.lib.math.twodim.geometry; + +import com.spartronics4915.lib.util.Interpolable; +import com.spartronics4915.lib.math.Util; + +import java.text.DecimalFormat; + +/** + * A movement along an arc at constant curvature and velocity. We can use ideas + * from "differential calculus" to create + * new Pose2ds from a Twist2d and visa versa. + *

+ * A Twist can be used to represent a difference between two poses, a velocity, + * an acceleration, etc. + */ +public class Twist2d implements Interpolable +{ + + public final double dx; + public final double dy; + public final Rotation2d dtheta; + + private final double mDThetaRads; + + public Twist2d() + { + this.dx = 0; + this.dy = 0; + this.dtheta = new Rotation2d(); + mDThetaRads = 0; + } + + public Twist2d(double dx, double dy, Rotation2d dtheta) + { + this.dx = dx; + this.dy = dy; + this.dtheta = dtheta; + mDThetaRads = dtheta.getRadians(); + } + + public Twist2d(Twist2d src) + { + this.dx = src.dx; + this.dy = src.dy; + this.dtheta = src.dtheta; + mDThetaRads = src.mDThetaRads; + } + + public Twist2d scaled(double scale) + { + return new Twist2d(dx * scale, dy * scale, Rotation2d.fromRadians(mDThetaRads * scale)); + } + + public double norm() + { + // Common case of dy == 0 + if (dy == 0.0) + return Math.abs(dx); + return Math.hypot(dx, dy); + } + + public double curvature() + { + if (Math.abs(mDThetaRads) < Util.kEpsilon && norm() < Util.kEpsilon) + return 0.0; + return mDThetaRads / norm(); + } + + /** + * Obtain a new Pose2d from a (constant curvature) velocity. See: + * https://github.com/strasdat/Sophus/blob/master/sophus/se2.hpp + * + * See also ch 9 of: + * http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf + */ + public Pose2d exp() { + double sin_theta = this.dtheta.getSin(); + double cos_theta = this.dtheta.getCos(); + double s, c; + if (Math.abs(mDThetaRads) < Util.kEpsilon) { + // small angle approximation + s = 1.0 - 1.0 / 6.0 * mDThetaRads * mDThetaRads; + c = .5 * mDThetaRads; + } else { + s = sin_theta / mDThetaRads; + c = (1.0 - cos_theta) / mDThetaRads; + } + Translation2d xlate = new Translation2d(this.dx * s - this.dy * c, this.dx * c + this.dy * s); + return new Pose2d(xlate, new Rotation2d(cos_theta, sin_theta, false)); + } + + /** + * For some applications interpolating a Twist2d may be fraught with peril. + * If you are trying to move from a Pose2d according to a Twist2d + * consider using Pose2d.interpolate + */ + @Override + public Twist2d interpolate(final Twist2d endValue, double t) + { + if (t <= 0) + return this; + else if (t >= 1) + return new Twist2d(endValue); + final Twist2d newTwist = new Twist2d( + this.dx + t*(endValue.dx - this.dx), + this.dy + t*(endValue.dy - this.dy), + Rotation2d.fromRadians(mDThetaRads + t*(endValue.mDThetaRads - this.mDThetaRads))); + // should just return t, no need for scaled + return newTwist.scaled(t); + } + + @Override + public String toString() + { + final DecimalFormat fmt = new DecimalFormat("#0.000"); + return "(" + fmt.format(dx) + "," + fmt.format(dy) + "," + fmt.format(dtheta.getDegrees()) + " deg)"; + } +} diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/lidar/ObjectFinder.java b/src/main/java/com/spartronics4915/lib/math/twodim/lidar/ObjectFinder.java new file mode 100644 index 0000000..be3d226 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/lidar/ObjectFinder.java @@ -0,0 +1,229 @@ +package com.spartronics4915.lib.math.twodim.lidar; + +import java.io.IOException; +import java.util.ArrayList; +import java.util.Hashtable; +import java.util.List; +import java.util.Map; +import java.util.Map.Entry; + +import com.spartronics4915.lib.math.twodim.geometry.Rotation2d; +import com.spartronics4915.lib.math.twodim.geometry.Translation2d; +import com.spartronics4915.lib.util.Units; + +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.Size; +import org.opencv.imgcodecs.Imgcodecs; +import org.opencv.imgproc.Imgproc; + +import edu.wpi.cscore.CameraServerJNI; + +public class ObjectFinder { + + static { + // This is so libopencv_javaVERSION.so (where version is the 3-digit opencv + // version) gets loaded. + try { + CameraServerJNI.forceLoad(); + } catch (IOException e) { + e.printStackTrace(); + } + } + + /** Side length of each binning square in meters */ + private final double mBinWidth; + + public ObjectFinder(double binWidthMeters) { + mBinWidth = binWidthMeters; + } + + public List findCircles(List pointcloud, double radiusMeters, int minVotes, int dilationSize) { + var binned = toMat(pointcloud); + Mat binnedPointcloud = binned.getKey(); + BinnedPoint minPoint = binned.getValue(); + + Mat kernel = Imgproc.getStructuringElement(Imgproc.MORPH_ELLIPSE, new Size(dilationSize, dilationSize)); + Imgproc.dilate(binnedPointcloud, binnedPointcloud, kernel); + + Mat results = new Mat(); + int roundedRadius = (int) Math.round(radiusMeters / mBinWidth); + Imgproc.HoughCircles(binnedPointcloud, results, Imgproc.HOUGH_GRADIENT, 1.0, (radiusMeters * 2) / mBinWidth, + 1, minVotes, roundedRadius - 3, roundedRadius + 3); + + List centers = new ArrayList<>(); + for (int i = 0; i < results.cols(); i++) { + double[] result = results.get(0, i); + centers.add(binnedCoordsToTranslation2d(result[0], result[1], minPoint)); + } + + return centers; + } + + public List findSquares(List pointcloud, Translation2d sensorPosition, double sideLengthMeters, int minVotes, int dilationSize, double maxLineGapMeters) { + var binned = toMat(pointcloud); + Mat binnedPointcloud = binned.getKey(); + BinnedPoint minPoint = binned.getValue(); + + Mat kernel = Imgproc.getStructuringElement(Imgproc.MORPH_ELLIPSE, new Size(dilationSize, dilationSize)); + Imgproc.dilate(binnedPointcloud, binnedPointcloud, kernel); + + Imgcodecs.imwrite("binned.png", binnedPointcloud); + + Mat results = new Mat(); + Imgproc.HoughLinesP(binnedPointcloud, results, 1, Math.PI / 360, minVotes, sideLengthMeters / mBinWidth, maxLineGapMeters / mBinWidth); + + System.out.println(results.dump()); + + List centers = new ArrayList<>(); + for (int i = 0; i < results.cols(); i++) { + double[] points = results.get(0, i); + + var pointOne = binnedCoordsToTranslation2d(points[0], points[1], minPoint); + var pointTwo = binnedCoordsToTranslation2d(points[2], points[3], minPoint); + + if (pointOne.getDistance(pointTwo) > sideLengthMeters + Units.inchesToMeters(2)) { + continue; + } + + System.out.println(pointOne + ", " + pointTwo); + + centers.add(getSquareCenter(pointOne, pointTwo)); + } + + System.out.println(); + + return centers; + } + + private Translation2d binnedCoordsToTranslation2d(double x, double y, BinnedPoint minPoint) { + x += minPoint.x(); + y += minPoint.y(); + + return new Translation2d(mBinWidth * x, mBinWidth * y); + } + + private Translation2d getSquareCenter(Translation2d a, Translation2d b) { + final double sideLength = a.getDistance(b); + + var midpoint = a.translateBy(b).scale(0.5); + + var normalOne = a.translateBy(midpoint.inverse()).rotateBy(Rotation2d.fromDegrees(90)); + var normalTwo = a.translateBy(midpoint.inverse()).rotateBy(Rotation2d.fromDegrees(270)); + + Rotation2d normalOneAngle = normalOne.getAngle(midpoint); + Rotation2d normalTwoAngle = normalTwo.getAngle(midpoint); + + Translation2d center = (Math.abs(normalOneAngle.getRadians()) > Math.abs(normalTwoAngle.getRadians()) + ? normalTwo + : normalOne); + center = center.scale(1 / center.norm()).scale(sideLength).translateBy(midpoint); + + return center; + } + + /** + * @param pointcloud A pointcloud in field coordinates to bin. + * @return An entry (used like a tuple), with the resulting binned mat as the + * key, and the minimum coordinate of the pointcloud as the value. + */ + private Entry toMat(List pointcloud) { + if (pointcloud.size() <= 0) { + System.err.println("toMat got an empty pointcloud"); + return Map.entry(new Mat(), new BinnedPoint(0, 0)); + } + + Hashtable binnedPoints = new Hashtable<>(); + + // Bin and deduplicate the points, and find the min and max coords + BinnedPoint min = null, max = null; + for (Translation2d point : pointcloud) { + BinnedPoint binnedPoint = new BinnedPoint(point); + + Integer density = binnedPoints.get(binnedPoint); + if (density == null) { + binnedPoints.put(binnedPoint, 1); + } else { + binnedPoints.put(binnedPoint, density++); + } + // Density is currently unused + + if (min == null) { + min = binnedPoint; + } + if (max == null) { + max = binnedPoint; + } + + min = new BinnedPoint(Math.min(binnedPoint.mBinX, min.mBinX), Math.min(binnedPoint.mBinY, min.mBinY)); + max = new BinnedPoint(Math.max(binnedPoint.mBinX, max.mBinX), Math.max(binnedPoint.mBinY, max.mBinY)); + } + + Mat binnedPointcloud = Mat.zeros(max.y() - min.y(), max.x() - min.x(), CvType.CV_8UC(1)); + + // Normalize the points to have an origin at 0, 0 and put them into the mat + for (BinnedPoint point : binnedPoints.keySet()) { + binnedPointcloud.put(point.y() - min.y(), point.x() - min.x(), new byte[] { (byte) 0xFF }); + } + // binnedPointcloud.put(0 - min.y(), 0 - min.x(), new byte[] { (byte) 0x80 }); + + return Map.entry(binnedPointcloud, min); + } + + private class BinnedPoint { + + /** Indicies in the bin (i.e. unitless) */ + private final int mBinX, mBinY; + + /** + * @param point A sensor-relative point, with X and Y in meters. + */ + public BinnedPoint(Translation2d point) { + // Overflow possible but very unlikely + mBinX = (int) (point.getX() / mBinWidth); + mBinY = (int) (point.getY() / mBinWidth); + } + + public BinnedPoint(int x, int y) { + mBinX = x; + mBinY = y; + } + + @Override + public boolean equals(Object other) { + if (this == other) { + return true; + } else if (other == null) { + return false; + } else if (this.getClass() != other.getClass()) { + return false; + } + + var bp = (BinnedPoint) other; + return bp.mBinX == this.mBinX && bp.mBinY == this.mBinY; + } + + @Override + public int hashCode() { + int hashX = Integer.valueOf(mBinX).hashCode(); + int hashY = Integer.valueOf(mBinY).hashCode(); + return 31 * hashX + hashY; + } + + public int x() { + return mBinX; + } + + public int y() { + return mBinY; + } + + /** + * @return This binned point converted back to a Translation2d in meters, in the + * sensor-relative coordinate plane. + */ + public Translation2d toTranslation2d() { + return new Translation2d(mBinWidth * mBinX + mBinX / 2.0, mBinWidth * mBinY + mBinY / 2.0); + } + } +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/lidar/TargetTracker.java b/src/main/java/com/spartronics4915/lib/math/twodim/lidar/TargetTracker.java new file mode 100644 index 0000000..c18393e --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/lidar/TargetTracker.java @@ -0,0 +1,120 @@ +package com.spartronics4915.lib.math.twodim.lidar; + +import java.util.ArrayList; +import java.util.List; + +import com.spartronics4915.lib.math.twodim.geometry.Translation2d; +import com.spartronics4915.lib.util.Units; + +import edu.wpi.first.wpilibj.Timer; + +public class TargetTracker { + private static class Target { + /** Meters */ + public final Translation2d translation; + /** Seconds */ + public final double firstSeen; + /** Seconds */ + public final double lastSeen; + public final double confidence; + + public Target(Translation2d translation, double confidencePercentile) { + this.translation = translation; + this.firstSeen = Timer.getFPGATimestamp(); + this.lastSeen = Timer.getFPGATimestamp(); + this.confidence = confidencePercentile; + } + + public Target(Target previous, Translation2d newTranslation, double confidencePercentile) { + if (confidencePercentile == 1) { + confidencePercentile *= kFirstPlaceAdvantageMultiplier; + } + + this.translation = newTranslation; + this.firstSeen = previous.firstSeen; + this.lastSeen = Timer.getFPGATimestamp(); + this.confidence = previous.confidence + confidencePercentile; + } + + /** + * @return Time since last sighting, in seconds. + */ + public double getTimeSinceLastSighting() { + return Timer.getFPGATimestamp() - this.lastSeen; + } + } + + private static final double kMaxTargetMovementMeters = Units.inchesToMeters(8); + private static final double kMaxTimeSinceLastSighting = 5; + + private static final double kFirstPlaceAdvantageMultiplier = 1.5; + + private List mTrackedTargets = new ArrayList<>(); + + /** + * @param targets List of targets, with the first target in the list being the "best". + * @return The target with the highest computed score, or null if no targets were given. + */ + public Translation2d update(List targets) { + List newTrackedTargets = new ArrayList<>(); + + double highestScore = Double.NEGATIVE_INFINITY; + Target bestTarget = null; + for (int i = 0; i < targets.size(); i++) { + var newTargetTranslation = targets.get(i); + + // Correlate with a previous target + double lowestDistance = Double.POSITIVE_INFINITY; + Target bestCorrelatedTarget = null; + for (int j = 0; j < mTrackedTargets.size(); j++) { + var oldTarget = mTrackedTargets.get(j); + + double dist = oldTarget.translation.getDistance(newTargetTranslation); + if (dist < kMaxTargetMovementMeters && dist < lowestDistance) { + bestCorrelatedTarget = mTrackedTargets.remove(j); + lowestDistance = dist; + } + } + + final Target newTarget; + double confidencePercentile = ((double) targets.size() - i) / targets.size(); + if (bestCorrelatedTarget == null) { + // Target has not been seen before + newTarget = new Target(newTargetTranslation, confidencePercentile); + } else { + newTarget = new Target(bestCorrelatedTarget, newTargetTranslation, confidencePercentile); + } + + double score = newTarget.confidence; + if (score > highestScore) { + highestScore = score; + bestTarget = newTarget; + } + + newTrackedTargets.add(newTarget); + } + + for (var oldTarget : mTrackedTargets) { + if (oldTarget.getTimeSinceLastSighting() >= kMaxTimeSinceLastSighting) { + continue; + } + + double score = oldTarget.confidence; + if (score > highestScore) { + highestScore = score; + bestTarget = oldTarget; + } + + newTrackedTargets.add(oldTarget); + } + + if (bestTarget == null) { + return null; + } + + mTrackedTargets = newTrackedTargets; + + return highestScore > 0 ? bestTarget.translation : null; + } + +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/physics/DCMotorTransmission.java b/src/main/java/com/spartronics4915/lib/math/twodim/physics/DCMotorTransmission.java new file mode 100644 index 0000000..6546671 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/physics/DCMotorTransmission.java @@ -0,0 +1,141 @@ +package com.spartronics4915.lib.math.twodim.physics; + +import com.spartronics4915.lib.math.Util; + +/** + * Model of a DC motor rotating a shaft. All parameters refer to the output + * (e.g. should already consider gearing + * and efficiency losses). The motor is assumed to be symmetric forward/reverse. + */ +public class DCMotorTransmission +{ + // All units must be SI! + protected final double mSpeedPerVolt; // rad/s per V (no load) + protected final double mTorquePerVolt; // N m per V (stall) + protected final double mFrictionVoltage; // V + + /** + * @param wheelRadiusMeters Wheel radius in meters. + * @param linearInertiaKg Robot mass in kg + * @param kS Volts to break static friction + * @param kV Volts / radians/sec + * @param kA Volts / radians/sec^2 + */ + public DCMotorTransmission( + double wheelRadiusMeters, double linearInertiaKg, + double kS, double kV, double kA) + { + this( + 1 / kV, + Math.pow(wheelRadiusMeters, 2) * linearInertiaKg / (2.0 * kA), + kS + ); + } + + /** + * @param speedPerVolt Radians/sec / volt + * @param torquePerVolt N m / volt + * @param frictionVoltage Volts to break static friction + */ + public DCMotorTransmission(final double speedPerVolt, + final double torquePerVolt, + final double frictionVoltage) + { + mSpeedPerVolt = speedPerVolt; + mTorquePerVolt = torquePerVolt; + mFrictionVoltage = frictionVoltage; + } + + public double speedPerVolt() + { + return mSpeedPerVolt; + } + + public double torquePerVolt() + { + return mTorquePerVolt; + } + + public double frictionVoltage() + { + return mFrictionVoltage; + } + + public double freeSpeedAtVoltage(final double voltage) + { + if (voltage > Util.kEpsilon) + { + return Math.max(0.0, voltage - frictionVoltage()) * speedPerVolt(); + } + else if (voltage < -Util.kEpsilon) + { + return Math.min(0.0, voltage + frictionVoltage()) * speedPerVolt(); + } + else + { + return 0.0; + } + } + + public double getTorqueForVoltage(final double outputSpeed, final double voltage) + { + double effectiveVoltage = voltage; + if (outputSpeed > Util.kEpsilon) + { + // Forward motion, rolling friction. + effectiveVoltage -= frictionVoltage(); + } + else if (outputSpeed < -Util.kEpsilon) + { + // Reverse motion, rolling friction. + effectiveVoltage += frictionVoltage(); + } + else if (voltage > Util.kEpsilon) + { + // System is static, forward torque. + effectiveVoltage = Math.max(0.0, voltage - frictionVoltage()); + } + else if (voltage < -Util.kEpsilon) + { + // System is static, reverse torque. + effectiveVoltage = Math.min(0.0, voltage + frictionVoltage()); + } + else + { + // System is idle. + return 0.0; + } + return torquePerVolt() * (-outputSpeed / speedPerVolt() + effectiveVoltage); + } + + public double getVoltageForTorque(final double outputSpeed, final double torque) + { + double frictionVoltage; + if (outputSpeed > Util.kEpsilon) + { + // Forward motion, rolling friction. + frictionVoltage = frictionVoltage(); + } + else if (outputSpeed < -Util.kEpsilon) + { + // Reverse motion, rolling friction. + frictionVoltage = -frictionVoltage(); + } + else if (torque > Util.kEpsilon) + { + // System is static, forward torque. + frictionVoltage = frictionVoltage(); + } + else if (torque < -Util.kEpsilon) + { + // System is static, reverse torque. + frictionVoltage = -frictionVoltage(); + } + else + { + // System is idle. + return 0.0; + } + return torque / torquePerVolt() + outputSpeed / speedPerVolt() + frictionVoltage; + } +} diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/physics/DifferentialDrive.java b/src/main/java/com/spartronics4915/lib/math/twodim/physics/DifferentialDrive.java new file mode 100644 index 0000000..bd1d69a --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/physics/DifferentialDrive.java @@ -0,0 +1,469 @@ +package com.spartronics4915.lib.math.twodim.physics; + +import com.spartronics4915.lib.math.Util; + +import java.text.DecimalFormat; +import java.util.Arrays; + +/** + * Dynamic model a differential drive robot. Note: to simplify things, this math + * assumes the center of mass is + * coincident with the kinematic center of rotation (e.g. midpoint of the center + * axle). + */ +public class DifferentialDrive +{ + // All units must be SI! + + // Equivalent mass when accelerating purely linearly, in kg. + // This is "equivalent" in that it also absorbs the effects of drivetrain inertia. + // Measure by doing drivetrain acceleration characterization in a straight line. + /** Kilograms */ + protected final double mMass; + + // Equivalent moment of inertia when accelerating purely angularly, in kg*m^2. + // This is "equivalent" in that it also absorbs the effects of drivetrain inertia. + // Measure by doing drivetrain acceleration characterization while turning in place. + /** kg*m^2 */ + protected final double mMoi; + + // Drag torque (proportional to angular velocity) that resists turning, in N*m/rad/s + // Empirical testing of our drivebase showed that there was an unexplained loss in torque ~proportional to angular + // velocity, likely due to scrub of wheels. + // NOTE: this may not be a purely linear term, and we have done limited testing, but this factor helps our model to + // better match reality. For future seasons, we should investigate what's going on here... + /** N*m/rad/s */ + protected final double mAngularDrag; + + // Self-explanatory. Measure by rolling the robot a known distance and counting encoder ticks. + /** Meters */ + protected final double mWheelRadius; // m + + // "Effective" kinematic wheelbase radius. Might be larger than theoretical to compensate for skid steer. Measure + // by turning the robot in place several times and figuring out what the equivalent wheelbase radius is. + /** Meters */ + protected final double mEffectiveWheelbaseRadius; // m + + // Transmissions for both sides of the drive. + protected final DCMotorTransmission mLeftTransmission; + protected final DCMotorTransmission mRightTransmission; + + public DifferentialDrive( + final double massKilograms, + final double moi, // kg m^2 + final double angularDrag, // N*m/rad/s + final double wheelRadiusMeters, + final double effectiveWheelbaseRadiusMeters, + final DCMotorTransmission leftTransmission, + final DCMotorTransmission rightTransmission) + { + mMass = massKilograms; + mMoi = moi; + mAngularDrag = angularDrag; + mWheelRadius = wheelRadiusMeters; + mEffectiveWheelbaseRadius = effectiveWheelbaseRadiusMeters; + mLeftTransmission = leftTransmission; + mRightTransmission = rightTransmission; + } + + /** + * @return Robot mass in kilograms + * */ + public double mass() + { + return mMass; + } + + /** + * @return Robot moment of inertia in kilograms*meters^2 + */ + public double moi() + { + return mMoi; + } + + /** + * @return Wheel radius in meters + */ + public double wheelRadius() + { + return mWheelRadius; + } + + /** + * @return Effective wheel radius in meters (this is used to calculate scrub) + */ + public double effectiveWheelbaseRadius() + { + return mEffectiveWheelbaseRadius; + } + + public DCMotorTransmission leftTransmission() + { + return mLeftTransmission; + } + + public DCMotorTransmission rightTransmission() + { + return mRightTransmission; + } + + public WheelState getVoltagesFromkV(WheelState velocities) { + return new WheelState( + velocities.left / mLeftTransmission.speedPerVolt() + + mLeftTransmission.frictionVoltage() * Math.signum(velocities.left), + velocities.right / mRightTransmission.speedPerVolt() + + mRightTransmission.frictionVoltage() * Math.signum(velocities.right) + ); + } + + // Input/demand could be either velocity or acceleration...the math is the same. + public ChassisState solveForwardKinematics(final WheelState wheelMotion) + { + ChassisState chassisMotion = new ChassisState(); + chassisMotion.linear = mWheelRadius * (wheelMotion.right + wheelMotion.left) / 2.0; + chassisMotion.angular = mWheelRadius * (wheelMotion.right - wheelMotion.left) / (2.0 * + mEffectiveWheelbaseRadius); + return chassisMotion; + } + + // Input/output could be either velocity or acceleration...the math is the same. + public WheelState solveInverseKinematics(final ChassisState chassisMotion) + { + WheelState wheelMotion = new WheelState(); + wheelMotion.left = (chassisMotion.linear - mEffectiveWheelbaseRadius * chassisMotion.angular) / + mWheelRadius; + wheelMotion.right = (chassisMotion.linear + mEffectiveWheelbaseRadius * chassisMotion.angular) / + mWheelRadius; + return wheelMotion; + } + + // Solve for torques and accelerations. + public DriveDynamics solveForwardDynamics(final ChassisState chassisVelocity, final WheelState voltage) + { + DriveDynamics dynamics = new DriveDynamics(); + dynamics.wheelVelocity = solveInverseKinematics(chassisVelocity); + dynamics.chassisVelocity = chassisVelocity; + dynamics.curvature = dynamics.chassisVelocity.angular / dynamics.chassisVelocity.linear; + if (Double.isNaN(dynamics.curvature)) + dynamics.curvature = 0.0; + dynamics.voltage = voltage; + solveForwardDynamics(dynamics); + return dynamics; + } + + public DriveDynamics solveForwardDynamics(final WheelState wheelVelocity, final WheelState voltage) + { + DriveDynamics dynamics = new DriveDynamics(); + dynamics.wheelVelocity = wheelVelocity; + dynamics.chassisVelocity = solveForwardKinematics(wheelVelocity); + dynamics.curvature = dynamics.chassisVelocity.angular / dynamics.chassisVelocity.linear; + if (Double.isNaN(dynamics.curvature)) + dynamics.curvature = 0.0; + dynamics.voltage = voltage; + solveForwardDynamics(dynamics); + return dynamics; + } + + // Assumptions about dynamics: velocities and voltages provided. + public void solveForwardDynamics(DriveDynamics dynamics) + { + final boolean leftStationary = + Util.epsilonEquals(dynamics.wheelVelocity.left, 0.0) && Math.abs(dynamics.voltage.left) < mLeftTransmission.frictionVoltage(); + final boolean rightStationary = + Util.epsilonEquals(dynamics.wheelVelocity.right, 0.0) && Math.abs(dynamics.voltage.right) < mRightTransmission.frictionVoltage(); + if (leftStationary && rightStationary) + { + // Neither side breaks static friction, so we remain stationary. + dynamics.wheelTorque.left = dynamics.wheelTorque.right = 0.0; + dynamics.chassisAcceleration.linear = dynamics.chassisAcceleration.angular = 0.0; + dynamics.wheelAcceleration.left = dynamics.wheelAcceleration.right = 0.0; + dynamics.dcurvature = 0.0; + return; + } + + // Solve for motor torques generated on each side. + dynamics.wheelTorque.left = mLeftTransmission.getTorqueForVoltage(dynamics.wheelVelocity.left, dynamics.voltage.left); + dynamics.wheelTorque.right = mRightTransmission.getTorqueForVoltage(dynamics.wheelVelocity.right, dynamics.voltage.right); + + // Add forces and torques about the center of mass. + dynamics.chassisAcceleration.linear = (dynamics.wheelTorque.right + dynamics.wheelTorque.left) / + (mWheelRadius * mMass); + // (Tr - Tl) / r_w * r_wb - drag * w = I * angular_accel + dynamics.chassisAcceleration.angular = + mEffectiveWheelbaseRadius * (dynamics.wheelTorque.right - dynamics.wheelTorque.left) / (mWheelRadius * mMoi) + - dynamics.chassisVelocity.angular * mAngularDrag / mMoi; + + // Solve for change in curvature from angular acceleration. + // total angular accel = linear_accel * curvature + v^2 * dcurvature + dynamics.dcurvature = (dynamics.chassisAcceleration.angular - dynamics.chassisAcceleration.linear * dynamics.curvature) / + (dynamics.chassisVelocity.linear * dynamics.chassisVelocity.linear); + if (Double.isNaN(dynamics.dcurvature)) + dynamics.dcurvature = 0.0; + + // Resolve chassis accelerations to each wheel. + dynamics.wheelAcceleration.left = dynamics.chassisAcceleration.linear - dynamics.chassisAcceleration.angular * mEffectiveWheelbaseRadius; + dynamics.wheelAcceleration.right = + dynamics.chassisAcceleration.linear + dynamics.chassisAcceleration.angular * mEffectiveWheelbaseRadius; + } + + // Solve for torques and voltages. + public DriveDynamics solveInverseDynamics(final ChassisState chassisVelocity, final ChassisState chassisAcceleration) + { + DriveDynamics dynamics = new DriveDynamics(); + dynamics.chassisVelocity = chassisVelocity; + dynamics.curvature = dynamics.chassisVelocity.angular / dynamics.chassisVelocity.linear; + if (Double.isNaN(dynamics.curvature)) + dynamics.curvature = 0.0; + dynamics.chassisAcceleration = chassisAcceleration; + dynamics.dcurvature = (dynamics.chassisAcceleration.angular - dynamics.chassisAcceleration.linear * dynamics.curvature) / + (dynamics.chassisVelocity.linear * dynamics.chassisVelocity.linear); + if (Double.isNaN(dynamics.dcurvature)) + dynamics.dcurvature = 0.0; + dynamics.wheelVelocity = solveInverseKinematics(chassisVelocity); + dynamics.wheelAcceleration = solveInverseKinematics(chassisAcceleration); + solveInverseDynamics(dynamics); + return dynamics; + } + + public DriveDynamics solveInverseDynamics(final WheelState wheelVelocity, final WheelState wheelAcceleration) + { + DriveDynamics dynamics = new DriveDynamics(); + dynamics.chassisVelocity = solveForwardKinematics(wheelVelocity); + dynamics.curvature = dynamics.chassisVelocity.angular / dynamics.chassisVelocity.linear; + if (Double.isNaN(dynamics.curvature)) + dynamics.curvature = 0.0; + dynamics.chassisAcceleration = solveForwardKinematics(wheelAcceleration); + dynamics.dcurvature = (dynamics.chassisAcceleration.angular - dynamics.chassisAcceleration.linear * dynamics.curvature) / + (dynamics.chassisVelocity.linear * dynamics.chassisVelocity.linear); + if (Double.isNaN(dynamics.dcurvature)) + dynamics.dcurvature = 0.0; + dynamics.wheelVelocity = wheelVelocity; + dynamics.wheelAcceleration = wheelAcceleration; + solveInverseDynamics(dynamics); + return dynamics; + } + + // Assumptions about dynamics: velocities and accelerations provided, curvature and dcurvature computed. + public void solveInverseDynamics(DriveDynamics dynamics) + { + // Determine the necessary torques on the left and right wheels to produce the desired wheel accelerations. + dynamics.wheelTorque.left = mWheelRadius / 2.0 * (dynamics.chassisAcceleration.linear * mMass - + dynamics.chassisAcceleration.angular * mMoi / mEffectiveWheelbaseRadius - + dynamics.chassisVelocity.angular * mAngularDrag / mEffectiveWheelbaseRadius); + dynamics.wheelTorque.right = mWheelRadius / 2.0 * (dynamics.chassisAcceleration.linear * mMass + + dynamics.chassisAcceleration.angular * mMoi / mEffectiveWheelbaseRadius + + dynamics.chassisVelocity.angular * mAngularDrag / mEffectiveWheelbaseRadius); + + // Solve for input voltages. + dynamics.voltage.left = mLeftTransmission.getVoltageForTorque(dynamics.wheelVelocity.left, dynamics.wheelTorque.left); + dynamics.voltage.right = mRightTransmission.getVoltageForTorque(dynamics.wheelVelocity.right, dynamics.wheelTorque.right); + } + + public double getMaxAbsVelocity(double curvature, /* double dcurvature, */double maxAbsVoltage) + { + // Alternative implementation: + // (Tr - Tl) * r_wb / r_w = I * v^2 * dk + // (Tr + Tl) / r_w = 0 + // T = Tr = -Tl + // 2T * r_wb / r_w = I*v^2*dk + // T = 2*I*v^2*dk*r_w/r_wb + // T = kt*(-vR/kv + V) = -kt*(-vL/vmax + V) + // Vr = v * (1 + k*r_wb) + // 0 = 2*I*dk*r_w/r_wb * v^2 + kt * ((1 + k*r_wb) * v / kv) - kt * V + // solve using quadratic formula? + // -b +/- sqrt(b^2 - 4*a*c) / (2a) + + // k = w / v + // v = r_w*(wr + wl) / 2 + // w = r_w*(wr - wl) / (2 * r_wb) + // Plug in max_abs_voltage for each wheel. + final double leftSpeedAtMaxVoltage = mLeftTransmission.freeSpeedAtVoltage(maxAbsVoltage); + final double rightSpeedAtMaxVoltage = mRightTransmission.freeSpeedAtVoltage(maxAbsVoltage); + if (Util.epsilonEquals(curvature, 0.0)) + { + return mWheelRadius * Math.min(leftSpeedAtMaxVoltage, rightSpeedAtMaxVoltage); + } + if (Double.isInfinite(curvature)) + { + // Turn in place. Return value meaning becomes angular velocity. + final double wheelSpeed = Math.min(leftSpeedAtMaxVoltage, rightSpeedAtMaxVoltage); + return Math.signum(curvature) * mWheelRadius * wheelSpeed / mEffectiveWheelbaseRadius; + } + + final double rightSpeedIfLeftMax = leftSpeedAtMaxVoltage * (mEffectiveWheelbaseRadius * curvature + + 1.0) / (1.0 - mEffectiveWheelbaseRadius * curvature); + if (Math.abs(rightSpeedIfLeftMax) <= rightSpeedAtMaxVoltage + Util.kEpsilon) + { + // Left max is active constraint. + return mWheelRadius * (leftSpeedAtMaxVoltage + rightSpeedIfLeftMax) / 2.0; + } + final double leftSpeedIfRightMax = rightSpeedAtMaxVoltage * (1.0 - mEffectiveWheelbaseRadius * + curvature) / (1.0 + mEffectiveWheelbaseRadius * curvature); + // Right at max is active constraint. + return mWheelRadius * (rightSpeedAtMaxVoltage + leftSpeedIfRightMax) / 2.0; + } + + public static class MinMax + { + + public double min; + public double max; + } + + // Curvature is redundant here in the case that chassis_velocity is not purely angular. It is the responsibility of + // the caller to ensure that curvature = angular vel / linear vel in these cases. + public MinMax getMinMaxAcceleration(final ChassisState chassisVelocity, double curvature, /* double dcurvature, */ double maxAbsVoltage) + { + MinMax result = new MinMax(); + final WheelState wheelVelocities = solveInverseKinematics(chassisVelocity); + result.min = Double.POSITIVE_INFINITY; + result.max = Double.NEGATIVE_INFINITY; + + // Math: + // (Tl + Tr) / r_w = m*a + // (Tr - Tl) / r_w * r_wb - drag*w = i*(a * k + v^2 * dk) + + // 2 equations, 2 unknowns. + // Solve for a and (Tl|Tr) + + final double linearTerm = Double.isInfinite(curvature) ? 0.0 : mMass * mEffectiveWheelbaseRadius; + final double angularTerm = Double.isInfinite(curvature) ? mMoi : mMoi * curvature; + + final double dragTorque = chassisVelocity.angular * mAngularDrag; + + // Check all four cases and record the min and max valid accelerations. + for (boolean left : Arrays.asList(false, true)) + { + for (double sign : Arrays.asList(1.0, -1.0)) + { + final DCMotorTransmission fixedTransmission = left ? mLeftTransmission : mRightTransmission; + final DCMotorTransmission variableTransmission = left ? mRightTransmission : mLeftTransmission; + final double fixedTorque = fixedTransmission.getTorqueForVoltage(wheelVelocities.get(left), sign * + maxAbsVoltage); + double variableTorque = 0.0; + // NOTE: variable_torque is wrong. Units don't work out correctly. We made a math error somewhere... + // Leaving this "as is" for code release so as not to be disingenuous, but this whole function needs + // revisiting in the future... + if (left) + { + variableTorque = + ((/*-moi_ * chassis_velocity.linear * chassis_velocity.linear * dcurvature*/ -dragTorque) * mMass * mWheelRadius + + fixedTorque * + (linearTerm + angularTerm)) + / (linearTerm - angularTerm); + } + else + { + variableTorque = + ((/* moi_ * chassis_velocity.linear * chassis_velocity.linear * dcurvature */ +dragTorque) * mMass * mWheelRadius + + fixedTorque * + (linearTerm - angularTerm)) + / (linearTerm + angularTerm); + } + final double variableVoltage = variableTransmission.getVoltageForTorque(wheelVelocities.get(!left), variableTorque); + if (Math.abs(variableVoltage) <= maxAbsVoltage + Util.kEpsilon) + { + double accel = 0.0; + if (Double.isInfinite(curvature)) + { + accel = (left ? -1.0 : 1.0) * (fixedTorque - variableTorque) * mEffectiveWheelbaseRadius + / (mMoi * mWheelRadius) - dragTorque / mMoi /*- chassis_velocity.linear * chassis_velocity.linear * dcurvature*/; + } + else + { + accel = (fixedTorque + variableTorque) / (mMass * mWheelRadius); + } + result.min = Math.min(result.min, accel); + result.max = Math.max(result.max, accel); + } + } + } + return result; + } + + // Can refer to velocity or acceleration depending on context. + public static class ChassisState + { + + /** Either meters/sec or meters/sec^2 */ + public double linear; + /** Either radians/sec or radians/sec^2 */ + public double angular; + + public ChassisState(double linear, double angular) + { + this.linear = linear; + this.angular = angular; + } + + public ChassisState() + { + } + + @Override + public String toString() + { + DecimalFormat fmt = new DecimalFormat("#0.000"); + return fmt.format(linear) + ", " + fmt.format(angular); + } + } + + // Can refer to velocity, acceleration, torque, voltage, etc., depending on context. + public static class WheelState + { + + /** Either radians/sec, radians/sec^2, Newton meters, or Volts */ + public double left, right; + + public WheelState(double left, double right) + { + this.left = left; + this.right = right; + } + + public WheelState() + { + } + + public double get(boolean getLeft) + { + return getLeft ? left : right; + } + + public void set(boolean setLeft, double val) + { + if (setLeft) + { + left = val; + } + else + { + right = val; + } + } + + @Override + public String toString() + { + DecimalFormat fmt = new DecimalFormat("#0.000"); + return fmt.format(left) + ", " + fmt.format(right); + } + } + + // Full state dynamics of the drivetrain. + public static class DriveDynamics + { + + /** 1/meters */ + public double curvature = 0.0; + /** 1/meters^2 */ + public double dcurvature = 0.0; + public ChassisState chassisVelocity = new ChassisState(); // m/s + public ChassisState chassisAcceleration = new ChassisState(); // m/s^2 + public WheelState wheelVelocity = new WheelState(); // rad/s + public WheelState wheelAcceleration = new WheelState(); // rad/s^2 + public WheelState voltage = new WheelState(); // V + public WheelState wheelTorque = new WheelState(); // N m + } +} diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/physics/DriveCharacterization.java b/src/main/java/com/spartronics4915/lib/math/twodim/physics/DriveCharacterization.java new file mode 100644 index 0000000..a9e8cb7 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/physics/DriveCharacterization.java @@ -0,0 +1,165 @@ +package com.spartronics4915.lib.math.twodim.physics; + +import com.spartronics4915.lib.math.PolynomialRegression; + +import com.spartronics4915.lib.util.Logger; +import com.spartronics4915.lib.math.Util; + +import java.util.List; + +public class DriveCharacterization +{ + + public static class CharacterizationConstants + { + + public double ks; //voltage needed to break static friction + public double kv; + public double ka; + } + + public static class VelocityDataPoint + { + + public final double velocity; + public final double power; + + public VelocityDataPoint(double velocity, double power) + { + this.velocity = velocity; + this.power = power; + } + } + + public static class AccelerationDataPoint + { + + public final double velocity; + public final double power; + public final double acceleration; + + public AccelerationDataPoint(double velocity, double power, double acceleration) + { + this.velocity = velocity; + this.power = power; + this.acceleration = acceleration; + } + } + + public static class CurvatureDataPoint + { + + public final double linearVelocity; + public final double angularVelocity; + public final double leftVoltage; + public final double rightVoltage; + + public CurvatureDataPoint(double linearVelocity, double angularVelocity, double leftVoltage, double rightVoltage) + { + this.linearVelocity = linearVelocity; + this.angularVelocity = angularVelocity; + this.leftVoltage = leftVoltage; + this.rightVoltage = rightVoltage; + } + } + + public static CharacterizationConstants characterizeDrive(List velocityData, List accelerationData) + { + CharacterizationConstants rv = getVelocityCharacterization(getVelocityData(velocityData)); + getAccelerationCharacterization(getAccelerationData(accelerationData, rv), rv); + return rv; + } + + /** + * From "Practical Guide to State Space Control" section 4.5.2. We're solving + * for I (also known as J) in rma=Iα. Jared Russell has posted a similar + * equation that you can use to solve for J, but we're not using it. + * + * @param linearAccelerationData in rad/s^2 + * @param angularAccelerationData in rad/s^2 + * @param wheelRadius in m + * @param robotMass in kg + * @return moment of inertia (angular inertia) in kg m^2 + */ + public static double calculateAngularInertia(List linearAccelerationData, + List angularAccelerationData, double wheelRadius, double robotMass) + { + // We currently throw out samples if things don't match up... FIXME? + int smallestLength = Math.min(angularAccelerationData.size(), linearAccelerationData.size()); + Logger.debug("Linear accel data has " + linearAccelerationData.size() + " samples, angular has " + angularAccelerationData.size()); + + double[] x = new double[smallestLength]; + double[] y = new double[smallestLength]; + + for (int i = 0; i < x.length; i++) + x[i] = angularAccelerationData.get(i).acceleration; + for (int i = 0; i < y.length; i++) + y[i] = linearAccelerationData.get(i).acceleration; + + PolynomialRegression p = new PolynomialRegression(x, y, 1); + Logger.notice("moi r^2: " + p.R2()); + + return p.beta(1) * (wheelRadius * robotMass); + } + + private static CharacterizationConstants getVelocityCharacterization(double[][] points) + { + CharacterizationConstants constants = new CharacterizationConstants(); + if (points == null) + { + return constants; + } + PolynomialRegression p = new PolynomialRegression(points, 1); + Logger.notice("Velocity r^2: " + p.R2()); + constants.ks = p.beta(0); + constants.kv = p.beta(1); + return constants; + } + + private static CharacterizationConstants getAccelerationCharacterization(double[][] points, CharacterizationConstants velocityChacterization) + { + if (points == null) + { + return velocityChacterization; + } + + PolynomialRegression p = new PolynomialRegression(points, 1); + Logger.notice("Acceleration r^2: " + p.R2()); + velocityChacterization.ka = p.beta(1); + return velocityChacterization; + } + + /** + * removes data points with a velocity of zero to get a better line fit + */ + private static double[][] getVelocityData(List input) + { + double[][] output = null; + int startTrim = 0; + for (int i = 0; i < input.size(); ++i) + { + if (input.get(i).velocity > Util.kEpsilon) + { + if (output == null) + { + output = new double[input.size() - i][2]; + startTrim = i; + } + output[i - startTrim][0] = input.get(i).velocity; + output[i - startTrim][1] = input.get(i).power; + } + } + return output; + } + + private static double[][] getAccelerationData(List input, CharacterizationConstants constants) + { + double[][] output = new double[input.size()][2]; + for (int i = 0; i < input.size(); ++i) + { + output[i][0] = input.get(i).acceleration; + output[i][1] = input.get(i).power - constants.kv * input.get(i).velocity - constants.ks; + } + return output; + } +} diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/spline/CubicHermiteSpline.java b/src/main/java/com/spartronics4915/lib/math/twodim/spline/CubicHermiteSpline.java new file mode 100644 index 0000000..0e57e3d --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/spline/CubicHermiteSpline.java @@ -0,0 +1,115 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package com.spartronics4915.lib.math.twodim.spline; + +import org.ejml.simple.SimpleMatrix; + +public class CubicHermiteSpline extends Spline { + private static SimpleMatrix hermiteBasis; + private final SimpleMatrix m_coefficients; + + /** + * Constructs a cubic hermite spline with the specified control vectors. Each + * control vector contains info about the location of the point and its first + * derivative. + * + * @param xInitialControlVector The control vector for the initial point in + * the x dimension. + * @param xFinalControlVector The control vector for the final point in + * the x dimension. + * @param yInitialControlVector The control vector for the initial point in + * the y dimension. + * @param yFinalControlVector The control vector for the final point in + * the y dimension. + */ + @SuppressWarnings("ParameterName") + public CubicHermiteSpline(double[] xInitialControlVector, double[] xFinalControlVector, + double[] yInitialControlVector, double[] yFinalControlVector) { + super(3); + + // Populate the coefficients for the actual spline equations. + // Row 0 is x coefficients + // Row 1 is y coefficients + final var hermite = makeHermiteBasis(); + final var x = getControlVectorFromArrays(xInitialControlVector, xFinalControlVector); + final var y = getControlVectorFromArrays(yInitialControlVector, yFinalControlVector); + + final var xCoeffs = (hermite.mult(x)).transpose(); + final var yCoeffs = (hermite.mult(y)).transpose(); + + m_coefficients = new SimpleMatrix(6, 4); + + for (int i = 0; i < 4; i++) { + m_coefficients.set(0, i, xCoeffs.get(0, i)); + m_coefficients.set(1, i, yCoeffs.get(0, i)); + + // Populate Row 2 and Row 3 with the derivatives of the equations above. + // Then populate row 4 and 5 with the second derivatives. + // Here, we are multiplying by (3 - i) to manually take the derivative. The + // power of the term in index 0 is 3, index 1 is 2 and so on. To find the + // coefficient of the derivative, we can use the power rule and multiply + // the existing coefficient by its power. + m_coefficients.set(2, i, m_coefficients.get(0, i) * (3 - i)); + m_coefficients.set(3, i, m_coefficients.get(1, i) * (3 - i)); + } + + for (int i = 0; i < 3; i++) { + // Here, we are multiplying by (2 - i) to manually take the derivative. The + // power of the term in index 0 is 2, index 1 is 1 and so on. To find the + // coefficient of the derivative, we can use the power rule and multiply + // the existing coefficient by its power. + m_coefficients.set(4, i, m_coefficients.get(2, i) * (2 - i)); + m_coefficients.set(5, i, m_coefficients.get(3, i) * (2 - i)); + } + + } + + /** + * Returns the coefficients matrix. + * + * @return The coefficients matrix. + */ + @Override + protected SimpleMatrix getCoefficients() { + return m_coefficients; + } + + /** + * Returns the hermite basis matrix for cubic hermite spline interpolation. + * + * @return The hermite basis matrix for cubic hermite spline interpolation. + */ + private SimpleMatrix makeHermiteBasis() { + if (hermiteBasis == null) { + hermiteBasis = new SimpleMatrix(4, 4, true, new double[]{ + +2.0, +1.0, -2.0, +1.0, + -3.0, -2.0, +3.0, -1.0, + +0.0, +1.0, +0.0, +0.0, + +1.0, +0.0, +0.0, +0.0 + }); + } + return hermiteBasis; + } + + /** + * Returns the control vector for each dimension as a matrix from the + * user-provided arrays in the constructor. + * + * @param initialVector The control vector for the initial point. + * @param finalVector The control vector for the final point. + * @return The control vector matrix for a dimension. + */ + private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) { + if (initialVector.length != 2 || finalVector.length != 2) { + throw new IllegalArgumentException("Size of vectors must be 2"); + } + return new SimpleMatrix(4, 1, true, new double[]{ + initialVector[0], initialVector[1], + finalVector[0], finalVector[1]}); + } +} diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/spline/QuinticHermiteSpline.java b/src/main/java/com/spartronics4915/lib/math/twodim/spline/QuinticHermiteSpline.java new file mode 100644 index 0000000..fc7a75a --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/spline/QuinticHermiteSpline.java @@ -0,0 +1,116 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package com.spartronics4915.lib.math.twodim.spline; + +import org.ejml.simple.SimpleMatrix; + +public class QuinticHermiteSpline extends Spline { + private static SimpleMatrix hermiteBasis; + private final SimpleMatrix m_coefficients; + + /** + * Constructs a quintic hermite spline with the specified control vectors. + * Each control vector contains into about the location of the point, its + * first derivative, and its second derivative. + * + * @param xInitialControlVector The control vector for the initial point in + * the x dimension. + * @param xFinalControlVector The control vector for the final point in + * the x dimension. + * @param yInitialControlVector The control vector for the initial point in + * the y dimension. + * @param yFinalControlVector The control vector for the final point in + * the y dimension. + */ + @SuppressWarnings("ParameterName") + public QuinticHermiteSpline(double[] xInitialControlVector, double[] xFinalControlVector, + double[] yInitialControlVector, double[] yFinalControlVector) { + super(5); + + // Populate the coefficients for the actual spline equations. + // Row 0 is x coefficients + // Row 1 is y coefficients + final var hermite = makeHermiteBasis(); + final var x = getControlVectorFromArrays(xInitialControlVector, xFinalControlVector); + final var y = getControlVectorFromArrays(yInitialControlVector, yFinalControlVector); + + final var xCoeffs = (hermite.mult(x)).transpose(); + final var yCoeffs = (hermite.mult(y)).transpose(); + + m_coefficients = new SimpleMatrix(6, 6); + + for (int i = 0; i < 6; i++) { + m_coefficients.set(0, i, xCoeffs.get(0, i)); + m_coefficients.set(1, i, yCoeffs.get(0, i)); + } + for (int i = 0; i < 6; i++) { + // Populate Row 2 and Row 3 with the derivatives of the equations above. + // Here, we are multiplying by (5 - i) to manually take the derivative. The + // power of the term in index 0 is 5, index 1 is 4 and so on. To find the + // coefficient of the derivative, we can use the power rule and multiply + // the existing coefficient by its power. + m_coefficients.set(2, i, m_coefficients.get(0, i) * (5 - i)); + m_coefficients.set(3, i, m_coefficients.get(1, i) * (5 - i)); + } + for (int i = 0; i < 5; i++) { + // Then populate row 4 and 5 with the second derivatives. + // Here, we are multiplying by (4 - i) to manually take the derivative. The + // power of the term in index 0 is 4, index 1 is 3 and so on. To find the + // coefficient of the derivative, we can use the power rule and multiply + // the existing coefficient by its power. + m_coefficients.set(4, i, m_coefficients.get(2, i) * (4 - i)); + m_coefficients.set(5, i, m_coefficients.get(3, i) * (4 - i)); + } + } + + /** + * Returns the coefficients matrix. + * + * @return The coefficients matrix. + */ + @Override + protected SimpleMatrix getCoefficients() { + return m_coefficients; + } + + /** + * Returns the hermite basis matrix for quintic hermite spline interpolation. + * + * @return The hermite basis matrix for quintic hermite spline interpolation. + */ + private SimpleMatrix makeHermiteBasis() { + if (hermiteBasis == null) { + hermiteBasis = new SimpleMatrix(6, 6, true, new double[]{ + -06.0, -03.0, -00.5, +06.0, -03.0, +00.5, + +15.0, +08.0, +01.5, -15.0, +07.0, +01.0, + -10.0, -06.0, -01.5, +10.0, -04.0, +00.5, + +00.0, +00.0, +00.5, +00.0, +00.0, +00.0, + +00.0, +01.0, +00.0, +00.0, +00.0, +00.0, + +01.0, +00.0, +00.0, +00.0, +00.0, +00.0 + }); + } + return hermiteBasis; + } + + /** + * Returns the control vector for each dimension as a matrix from the + * user-provided arrays in the constructor. + * + * @param initialVector The control vector for the initial point. + * @param finalVector The control vector for the final point. + * @return The control vector matrix for a dimension. + */ + private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) { + if (initialVector.length != 3 || finalVector.length != 3) { + throw new IllegalArgumentException("Size of vectors must be 3"); + } + return new SimpleMatrix(6, 1, true, new double[]{ + initialVector[0], initialVector[1], initialVector[2], + finalVector[0], finalVector[1], finalVector[2]}); + } +} diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/spline/Spline.java b/src/main/java/com/spartronics4915/lib/math/twodim/spline/Spline.java new file mode 100644 index 0000000..8cb0db9 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/spline/Spline.java @@ -0,0 +1,117 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package com.spartronics4915.lib.math.twodim.spline; + +import java.util.Arrays; + +import org.ejml.simple.SimpleMatrix; + +import com.spartronics4915.lib.math.twodim.geometry.Pose2d; +import com.spartronics4915.lib.math.twodim.geometry.Pose2dWithCurvature; +import com.spartronics4915.lib.math.twodim.geometry.Rotation2d; + +/** + * Represents a two-dimensional parametric spline that interpolates between two + * points. + */ +public abstract class Spline { + private final int m_degree; + + /** + * Constructs a spline with the given degree. + * + * @param degree The degree of the spline. + */ + Spline(int degree) { + m_degree = degree; + } + + /** + * Returns the coefficients of the spline. + * + * @return The coefficients of the spline. + */ + protected abstract SimpleMatrix getCoefficients(); + + /** + * Gets the pose and curvature at some point t on the spline. + * + * @param t The point t + * @return The pose and curvature at that point. + */ + @SuppressWarnings("ParameterName") + public Pose2dWithCurvature getPoint(double t) { + SimpleMatrix polynomialBases = new SimpleMatrix(m_degree + 1, 1); + final var coefficients = getCoefficients(); + + // Populate the polynomial bases. + for (int i = 0; i <= m_degree; i++) { + polynomialBases.set(i, 0, Math.pow(t, m_degree - i)); + } + + // This simply multiplies by the coefficients. We need to divide out t some + // n number of times where n is the derivative we want to take. + SimpleMatrix combined = coefficients.mult(polynomialBases); + + // Get x and y + final double x = combined.get(0, 0); + final double y = combined.get(1, 0); + + double dx; + double dy; + double ddx; + double ddy; + + if (t == 0) { + dx = coefficients.get(2, m_degree - 1); + dy = coefficients.get(3, m_degree - 1); + ddx = coefficients.get(4, m_degree - 2); + ddy = coefficients.get(5, m_degree - 2); + } else { + // Divide out t once for first derivative. + dx = combined.get(2, 0) / t; + dy = combined.get(3, 0) / t; + + // Divide out t twice for second derivative. + ddx = combined.get(4, 0) / t / t; + ddy = combined.get(5, 0) / t / t; + } + + // Find the curvature. + final double curvature = + (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * Math.hypot(dx, dy)); + + return new Pose2dWithCurvature( + new Pose2d(x, y, new Rotation2d(dx, dy, true)), + curvature + ); + } + + /** + * Represents a control vector for a spline. + * + *

Each element in each array represents the value of the derivative at the index. For + * example, the value of x[2] is the second derivative in the x dimension. + */ + @SuppressWarnings("MemberName") + public static class ControlVector { + public double[] x; + public double[] y; + + /** + * Instantiates a control vector. + * @param x The x dimension of the control vector. + * @param y The y dimension of the control vector. + */ + @SuppressWarnings("ParameterName") + public ControlVector(double[] x, double[] y) { + this.x = Arrays.copyOf(x, x.length); + this.y = Arrays.copyOf(y, y.length); + } + } +} diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/spline/SplineHelper.java b/src/main/java/com/spartronics4915/lib/math/twodim/spline/SplineHelper.java new file mode 100644 index 0000000..b33dae8 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/spline/SplineHelper.java @@ -0,0 +1,276 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package com.spartronics4915.lib.math.twodim.spline; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +import com.spartronics4915.lib.math.twodim.geometry.Pose2d; +import com.spartronics4915.lib.math.twodim.geometry.Translation2d; + +public final class SplineHelper { + /** + * Private constructor because this is a utility class. + */ + private SplineHelper() { + } + + /** + * Returns 2 cubic control vectors from a set of exterior waypoints and + * interior translations. + * + * @param start The starting pose. + * @param interiorWaypoints The interior waypoints. + * @param end The ending pose. + * @return 2 cubic control vectors. + */ + public static Spline.ControlVector[] getCubicControlVectorsFromWaypoints( + Pose2d start, Translation2d[] interiorWaypoints, Pose2d end + ) { + // Generate control vectors from poses. + Spline.ControlVector initialCV; + Spline.ControlVector endCV; + + // Chooses a magnitude automatically that makes the splines look better. + if (interiorWaypoints.length < 1) { + double scalar = start.getTranslation().getDistance(end.getTranslation()) * 1.2; + initialCV = getCubicControlVector(scalar, start); + endCV = getCubicControlVector(scalar, end); + } else { + double scalar = start.getTranslation().getDistance(interiorWaypoints[0]) * 1.2; + initialCV = getCubicControlVector(scalar, start); + scalar = end.getTranslation().getDistance(interiorWaypoints[interiorWaypoints.length - 1]) + * 1.2; + endCV = getCubicControlVector(scalar, end); + } + return new Spline.ControlVector[]{initialCV, endCV}; + } + + /** + * Returns quintic control vectors from a set of waypoints. + * + * @param waypoints The waypoints + * @return List of control vectors + */ + public static List getQuinticControlVectorsFromWaypoints( + List waypoints + ) { + List vectors = new ArrayList<>(); + for (int i = 0; i < waypoints.size() - 1; i++) { + var p0 = waypoints.get(i); + var p1 = waypoints.get(i + 1); + + // This just makes the splines look better. + final var scalar = 1.2 * p0.getTranslation().getDistance(p1.getTranslation()); + + vectors.add(getQuinticControlVector(scalar, p0)); + vectors.add(getQuinticControlVector(scalar, p1)); + } + return vectors; + } + + /** + * Returns a set of cubic splines corresponding to the provided control vectors. The + * user is free to set the direction of the start and end point. The + * directions for the middle waypoints are determined automatically to ensure + * continuous curvature throughout the path. + * + * @param start The starting control vector. + * @param waypoints The middle waypoints. This can be left blank if you only + * wish to create a path with two waypoints. + * @param end The ending control vector. + * @return A vector of cubic hermite splines that interpolate through the + * provided waypoints and control vectors. + */ + @SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength", + "PMD.AvoidInstantiatingObjectsInLoops"}) + public static CubicHermiteSpline[] getCubicSplinesFromControlVectors( + Spline.ControlVector start, Translation2d[] waypoints, Spline.ControlVector end) { + + CubicHermiteSpline[] splines = new CubicHermiteSpline[waypoints.length + 1]; + + double[] xInitial = start.x; + double[] yInitial = start.y; + double[] xFinal = end.x; + double[] yFinal = end.y; + + if (waypoints.length > 1) { + Translation2d[] newWaypts = new Translation2d[waypoints.length + 2]; + + // Create an array of all waypoints, including the start and end. + newWaypts[0] = new Translation2d(xInitial[0], yInitial[0]); + System.arraycopy(waypoints, 0, newWaypts, 1, waypoints.length); + newWaypts[newWaypts.length - 1] = new Translation2d(xFinal[0], yFinal[0]); + + // Populate tridiagonal system for clamped cubic + /* See: + https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08 + /undervisningsmateriale/chap7alecture.pdf + */ + // Above-diagonal of tridiagonal matrix, zero-padded + final double[] a = new double[newWaypts.length - 2]; + + // Diagonal of tridiagonal matrix + final double[] b = new double[newWaypts.length - 2]; + Arrays.fill(b, 4.0); + + // Below-diagonal of tridiagonal matrix, zero-padded + final double[] c = new double[newWaypts.length - 2]; + + // rhs vectors + final double[] dx = new double[newWaypts.length - 2]; + final double[] dy = new double[newWaypts.length - 2]; + + // solution vectors + final double[] fx = new double[newWaypts.length - 2]; + final double[] fy = new double[newWaypts.length - 2]; + + // populate above-diagonal and below-diagonal vectors + a[0] = 0.0; + for (int i = 0; i < newWaypts.length - 3; i++) { + a[i + 1] = 1; + c[i] = 1; + } + c[c.length - 1] = 0.0; + + // populate rhs vectors + dx[0] = 3 * (newWaypts[2].getX() - newWaypts[0].getX()) - xInitial[1]; + dy[0] = 3 * (newWaypts[2].getY() - newWaypts[0].getY()) - yInitial[1]; + + if (newWaypts.length > 4) { + for (int i = 1; i <= newWaypts.length - 4; i++) { + dx[i] = 3 * (newWaypts[i + 1].getX() - newWaypts[i - 1].getX()); + dy[i] = 3 * (newWaypts[i + 1].getY() - newWaypts[i - 1].getY()); + } + } + + dx[dx.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getX() + - newWaypts[newWaypts.length - 3].getX()) - xFinal[1]; + dy[dy.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getY() + - newWaypts[newWaypts.length - 3].getY()) - yFinal[1]; + + // Compute solution to tridiagonal system + thomasAlgorithm(a, b, c, dx, fx); + thomasAlgorithm(a, b, c, dy, fy); + + double[] newFx = new double[fx.length + 2]; + double[] newFy = new double[fy.length + 2]; + + newFx[0] = xInitial[1]; + newFy[0] = yInitial[1]; + System.arraycopy(fx, 0, newFx, 1, fx.length); + System.arraycopy(fy, 0, newFy, 1, fy.length); + newFx[newFx.length - 1] = xFinal[1]; + newFy[newFy.length - 1] = yFinal[1]; + + for (int i = 0; i < newFx.length - 1; i++) { + splines[i] = new CubicHermiteSpline( + new double[]{newWaypts[i].getX(), newFx[i]}, + new double[]{newWaypts[i + 1].getX(), newFx[i + 1]}, + new double[]{newWaypts[i].getY(), newFy[i]}, + new double[]{newWaypts[i + 1].getY(), newFy[i + 1]} + ); + } + } else if (waypoints.length == 1) { + final var xDeriv = (3 * (xFinal[0] + - xInitial[0]) + - xFinal[1] - xInitial[1]) + / 4.0; + final var yDeriv = (3 * (yFinal[0] + - yInitial[0]) + - yFinal[1] - yInitial[1]) + / 4.0; + + double[] midXControlVector = {waypoints[0].getX(), xDeriv}; + double[] midYControlVector = {waypoints[0].getY(), yDeriv}; + + splines[0] = new CubicHermiteSpline(xInitial, midXControlVector, + yInitial, midYControlVector); + splines[1] = new CubicHermiteSpline(midXControlVector, xFinal, + midYControlVector, yFinal); + } else { + splines[0] = new CubicHermiteSpline(xInitial, xFinal, + yInitial, yFinal); + } + return splines; + } + + /** + * Returns a set of quintic splines corresponding to the provided control vectors. + * The user is free to set the direction of all control vectors. Continuous + * curvature is guaranteed throughout the path. + * + * @param controlVectors The control vectors. + * @return A vector of quintic hermite splines that interpolate through the + * provided waypoints. + */ + @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"}) + public static QuinticHermiteSpline[] getQuinticSplinesFromControlVectors( + Spline.ControlVector[] controlVectors) { + QuinticHermiteSpline[] splines = new QuinticHermiteSpline[controlVectors.length - 1]; + for (int i = 0; i < controlVectors.length - 1; i++) { + var xInitial = controlVectors[i].x; + var xFinal = controlVectors[i + 1].x; + var yInitial = controlVectors[i].y; + var yFinal = controlVectors[i + 1].y; + splines[i] = new QuinticHermiteSpline(xInitial, xFinal, + yInitial, yFinal); + } + return splines; + } + + /** + * Thomas algorithm for solving tridiagonal systems Af = d. + * + * @param a the values of A above the diagonal + * @param b the values of A on the diagonal + * @param c the values of A below the diagonal + * @param d the vector on the rhs + * @param solutionVector the unknown (solution) vector, modified in-place + */ + @SuppressWarnings({"ParameterName", "LocalVariableName"}) + private static void thomasAlgorithm(double[] a, double[] b, + double[] c, double[] d, double[] solutionVector) { + int N = d.length; + + double[] cStar = new double[N]; + double[] dStar = new double[N]; + + // This updates the coefficients in the first row + // Note that we should be checking for division by zero here + cStar[0] = c[0] / b[0]; + dStar[0] = d[0] / b[0]; + + // Create the c_star and d_star coefficients in the forward sweep + for (int i = 1; i < N; i++) { + double m = 1.0 / (b[i] - a[i] * cStar[i - 1]); + cStar[i] = c[i] * m; + dStar[i] = (d[i] - a[i] * dStar[i - 1]) * m; + } + solutionVector[N - 1] = dStar[N - 1]; + // This is the reverse sweep, used to update the solution vector f + for (int i = N - 2; i >= 0; i--) { + solutionVector[i] = dStar[i] - cStar[i] * solutionVector[i + 1]; + } + } + + private static Spline.ControlVector getCubicControlVector(double scalar, Pose2d point) { + return new Spline.ControlVector( + new double[]{point.getTranslation().getX(), scalar * point.getRotation().getCos()}, + new double[]{point.getTranslation().getY(), scalar * point.getRotation().getSin()} + ); + } + + private static Spline.ControlVector getQuinticControlVector(double scalar, Pose2d point) { + return new Spline.ControlVector( + new double[]{point.getTranslation().getX(), scalar * point.getRotation().getCos(), 0.0}, + new double[]{point.getTranslation().getY(), scalar * point.getRotation().getSin(), 0.0} + ); + } +} diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/spline/SplineParameterizer.java b/src/main/java/com/spartronics4915/lib/math/twodim/spline/SplineParameterizer.java new file mode 100644 index 0000000..ded4f6c --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/spline/SplineParameterizer.java @@ -0,0 +1,156 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +/* + * MIT License + * + * Copyright (c) 2018 Team 254 + * + * 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 com.spartronics4915.lib.math.twodim.spline; + +import java.util.ArrayDeque; +import java.util.ArrayList; +import java.util.List; + +import com.spartronics4915.lib.math.twodim.geometry.Pose2d; +import com.spartronics4915.lib.math.twodim.geometry.Pose2dWithCurvature; +import com.spartronics4915.lib.math.twodim.geometry.Twist2d; + +/** + * Class used to parameterize a spline by its arc length. + */ +public final class SplineParameterizer { + private static final double kMaxDx = 0.127; + private static final double kMaxDy = 0.00127; + private static final double kMaxDtheta = 0.0872; + + /** + * A malformed spline does not actually explode the LIFO stack size. Instead, the stack size + * stays at a relatively small number (e.g. 30) and never decreases. Because of this, we must + * count iterations. Even long, complex paths don't usually go over 300 iterations, so hitting + * this maximum should definitely indicate something has gone wrong. + */ + private static final int kMaxIterations = 5000; + + @SuppressWarnings("MemberName") + private static class StackContents { + final double t1; + final double t0; + + StackContents(double t0, double t1) { + this.t0 = t0; + this.t1 = t1; + } + } + + public static class MalformedSplineException extends RuntimeException { + /** + * Create a new exception with the given message. + * + * @param message the message to pass with the exception + */ + private MalformedSplineException(String message) { + super(message); + } + } + + /** + * Private constructor because this is a utility class. + */ + private SplineParameterizer() { + } + + /** + * Parameterizes the spline. This method breaks up the spline into various + * arcs until their dx, dy, and dtheta are within specific tolerances. + * + * @param spline The spline to parameterize. + * @return A list of poses and curvatures that represents various points on the spline. + * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points + * with approximately opposing headings) + */ + public static List parameterize(Spline spline) { + return parameterize(spline, 0.0, 1.0); + } + + /** + * Parameterizes the spline. This method breaks up the spline into various + * arcs until their dx, dy, and dtheta are within specific tolerances. + * + * @param spline The spline to parameterize. + * @param t0 Starting internal spline parameter. It is recommended to use 0.0. + * @param t1 Ending internal spline parameter. It is recommended to use 1.0. + * @return A list of poses and curvatures that represents various points on the spline. + * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points + * with approximately opposing headings) + */ + @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops") + public static List parameterize(Spline spline, double t0, double t1) { + var splinePoints = new ArrayList(); + + // The parameterization does not add the initial point. Let's add that. + splinePoints.add(spline.getPoint(t0)); + + // We use an "explicit stack" to simulate recursion, instead of a recursive function call + // This give us greater control, instead of a stack overflow + var stack = new ArrayDeque(); + stack.push(new StackContents(t0, t1)); + + StackContents current; + Pose2dWithCurvature start; + Pose2dWithCurvature end; + int iterations = 0; + + while (!stack.isEmpty()) { + current = stack.removeFirst(); + start = spline.getPoint(current.t0); + end = spline.getPoint(current.t1); + + final var twist = end.getPose().inFrameReferenceOf(start.getPose()).log(); + if ( + Math.abs(twist.dy) > kMaxDy + || Math.abs(twist.dx) > kMaxDx + || Math.abs(twist.dtheta.getRadians()) > kMaxDtheta + ) { + stack.addFirst(new StackContents((current.t0 + current.t1) / 2, current.t1)); + stack.addFirst(new StackContents(current.t0, (current.t0 + current.t1) / 2)); + } else { + splinePoints.add(spline.getPoint(current.t1)); + } + + iterations++; + if (iterations >= kMaxIterations) { + throw new MalformedSplineException( + "Could not parameterize a malformed spline. " + + "This means that you probably had two or more adjacent waypoints that were very close " + + "together with headings in opposing directions." + ); + } + } + + return splinePoints; + } +} diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/Line.java b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/Line.java new file mode 100644 index 0000000..3bafded --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/Line.java @@ -0,0 +1,35 @@ +package com.spartronics4915.lib.math.twodim.trajectory; + +import java.util.Objects; + +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; + +public class Line extends org.apache.commons.math3.geometry.euclidean.twod.Line { + + private final Vector2D mP1, mP2; + + public Line(Vector2D p1, Vector2D p2, double tolerance) { + super(p1, p2, tolerance); + mP1 = p1; + mP2 = p2; + } + + @Override + public boolean equals(Object o) { + if (this == o) + return true; + if (o == null) + return false; + if (!(o instanceof Line)) + return false; + // If the vectors are the same then the lines are the same too + // The Apache commons line class doesn't have an equals method + return this.getAngle() == ((Line) o).getAngle() && this.getOriginOffset() == ((Line) o).getOriginOffset(); + } + + @Override + public int hashCode() { + return (int) (Math.random() * 1000000.0); + } + +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/PathFinder.java b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/PathFinder.java new file mode 100644 index 0000000..870f675 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/PathFinder.java @@ -0,0 +1,267 @@ +package com.spartronics4915.lib.math.twodim.trajectory; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Comparator; +import java.util.HashSet; +import java.util.List; +import java.util.Objects; +import java.util.Set; +import java.util.stream.Collectors; +import java.util.stream.Stream; + +import com.spartronics4915.lib.math.Util; +import com.spartronics4915.lib.math.twodim.geometry.Pose2d; +import com.spartronics4915.lib.math.twodim.geometry.Rectangle2d; +import com.spartronics4915.lib.math.twodim.geometry.Translation2d; +import com.spartronics4915.lib.util.Units; + +import com.spartronics4915.lib.math.twodim.trajectory.Line; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; + +public class PathFinder { + + private static class Node { + public final Vector2D point; + /** Meters */ + public double dist; + public Node prev; + + public Node(Vector2D point, double distMeters, Node prev) { + this.point = point; + this.dist = distMeters; + this.prev = prev; + } + } + + private static final Rectangle2d kFieldRectangle = new Rectangle2d(new Translation2d(), + new Translation2d(Units.feetToMeters(54), Units.feetToMeters(27))); + + /** Meters */ + private final double mRobotSize; + /** Meters */ + private final double mRobotSizeCorner; + /** Meters */ + private final Vector2D mRobotCornerTopRight, mRobotCornerTopLeft, mRobotCornerBottomLeft, mRobotCornerBottomRight; + /** Meters */ + private final Translation2d mRobotTopLeftOffset, mRobotBottomRightOffset; + /** Meters */ + private final Rectangle2d mFieldRectangleWithOffset; + private final List mGlobalRestrictedAreas; + + public PathFinder(double robotSizeMeters, Rectangle2d... globalRestrictedAreas) { + mRobotSize = robotSizeMeters; + mGlobalRestrictedAreas = Arrays.asList(globalRestrictedAreas); + + mRobotSizeCorner = Math.sqrt(Math.pow(mRobotSize, 2.0) / 2.0); + mRobotCornerTopLeft = new Vector2D(-mRobotSizeCorner, mRobotSizeCorner); + mRobotCornerTopRight = new Vector2D(mRobotSizeCorner, mRobotSizeCorner); + mRobotCornerBottomLeft = new Vector2D(-mRobotSizeCorner, -mRobotSizeCorner); + mRobotCornerBottomRight = new Vector2D(mRobotSizeCorner, -mRobotSizeCorner); + mRobotTopLeftOffset = new Translation2d(-mRobotSize / 2.0, mRobotSize / 2.0); + mRobotBottomRightOffset = new Translation2d(mRobotSize / 2.0, -mRobotSize / 2.0); + mFieldRectangleWithOffset = new Rectangle2d( + kFieldRectangle.getTopLeft().translateBy(mRobotTopLeftOffset.inverse()), + kFieldRectangle.getBottomRight().translateBy(mRobotBottomRightOffset.inverse())); + } + + public List findInteriorWaypoints(Pose2d start, Pose2d end, + Rectangle2d... additionalRestrictedAreas) { + var waypoints = findWaypoints(toVector(start.getTranslation()), toVector(end.getTranslation()), + additionalRestrictedAreas); + + List ret; + if (waypoints == null) { + return null; + } else if (waypoints.size() < 3) { + ret = new ArrayList<>(); + } else { + ret = waypoints.subList(1, waypoints.size() - 2); + } + return ret.stream().map((it) -> toTranslation(it)).collect(Collectors.toList()); + } + + public List findWaypoints(Vector2D start, Vector2D end, Rectangle2d... additionalRestrictedAreas) { + var effectiveRestrictedAreas = new HashSet<>(mGlobalRestrictedAreas); + effectiveRestrictedAreas.addAll(Arrays.asList(additionalRestrictedAreas)); + + var worldNodes = createNodes(effectiveRestrictedAreas); + worldNodes.addAll(Set.of(start, end)); + + return optimize(start, end, worldNodes, effectiveRestrictedAreas); + } + + private List optimize(Vector2D source, Vector2D target, Set points, + Set effectiveRestrictedAreas) { + final var Q = points.stream().map((it) -> new Node(it, Double.POSITIVE_INFINITY, null)) + .collect(Collectors.toSet()); + for (var node : Q) { + if (node.point.equals(source)) { + node.dist = 0.0; + break; + } + } + + while (!Q.isEmpty()) { + var u = Q.stream().min(Comparator.comparing((n) -> n.dist)).orElseThrow(); + Q.remove(u); + if (u.point.equals(target)) { + var S = new ArrayList(); + var c = u; + while (c != null) { + S.add(0, c.point); + c = c.prev; + } + return S; + } + + var robotRectangle = toRobotRectangle(u.point); + for (var v : Q) { + var toTranslation = toTranslation(v.point.subtract(u.point)); + if (effectiveRestrictedAreas.stream() + .noneMatch((it) -> it.doesCollide(robotRectangle, toTranslation))) { + var alt = u.dist + u.point.distance(v.point); + if (alt < v.dist) { + v.dist = alt; + v.prev = u; + } + } + } + } + + return null; + } + + private Set createNodes(Set effectiveRestrictedAreas) { + Set effectiveRestrictedAreasWithOffsets = effectiveRestrictedAreas.stream() + .map((rect) -> new Rectangle2d(rect.getTopLeft().translateBy(mRobotTopLeftOffset), + rect.getBottomRight().translateBy(mRobotBottomRightOffset))) + .collect(Collectors.toSet()); + System.out.println(effectiveRestrictedAreasWithOffsets.size()); + var result = Pair.combinationPairs(createLines(effectiveRestrictedAreasWithOffsets)).stream() + .map((it) -> it.first.intersection(it.second)).filter((it) -> it != null); + System.out.println(result.collect(Collectors.toList()).size()); + var restrictedCorners = effectiveRestrictedAreas.stream() + .flatMap((rect) -> Set.of(toVector(rect.getTopLeft()).add(mRobotCornerTopLeft), + toVector(rect.getTopRight()).add(mRobotCornerTopRight), + toVector(rect.getBottomLeft()).add(mRobotCornerBottomLeft), + toVector(rect.getBottomRight()).add(mRobotCornerBottomRight)).stream()); + + return Stream.concat(result, restrictedCorners).filter((point) -> { + var translation = toTranslation(point); + return (mFieldRectangleWithOffset.contains(translation) + && !effectiveRestrictedAreas.stream().anyMatch((it) -> it.contains(translation))); + }).collect(Collectors.toSet()); + } + + private Set createLines(Set effectiveRestrictedAreas) { + var restrictedAreas = new HashSet<>(effectiveRestrictedAreas); + restrictedAreas.add(mFieldRectangleWithOffset); + + class LineTriple { + public final Vector2D vecOne, vecTwo; + public final Line line; + + public LineTriple(Translation2d first, Translation2d second) { + this.vecOne = toVector(first); + this.vecTwo = toVector(second); + this.line = new Line(this.vecOne, this.vecTwo, Util.kEpsilon); + } + + @Override + public int hashCode() { + return Objects.hash(vecOne, vecTwo); + } + + @Override + public String toString() { + return "{" + vecOne + "; " + vecTwo + "}"; + } + + @Override + public boolean equals(Object o) { + if (this == o) + return true; + if (o == null) + return false; + if (!(o instanceof LineTriple)) + return false; + // If the vectors are the same then the lines are the same too + // The Apache commons line class doesn't have an equals method + return this.vecOne.equals(((LineTriple) o).vecOne) && this.vecTwo.equals(((LineTriple) o).vecTwo); + } + } + + var restrictedWallLines = restrictedAreas.stream() + .flatMap((rect) -> Set.of(new LineTriple(rect.getTopLeft(), rect.getTopRight()), + new LineTriple(rect.getTopLeft(), rect.getBottomLeft()), + new LineTriple(rect.getBottomRight(), rect.getBottomLeft()), + new LineTriple(rect.getBottomRight(), rect.getTopRight())).stream()) + .collect(Collectors.toSet()); + System.out.println(restrictedWallLines.size()); + System.out.println(Pair.combinationPairs(restrictedWallLines)); + + Set lines = new HashSet<>(); + for (var comboPair : Pair.combinationPairs(restrictedWallLines)) { + if (!comboPair.first.line.isParallelTo(comboPair.second.line) + || comboPair.first.line.getOffset(comboPair.second.line) < mRobotSize / 2.0) { + continue; + } + lines.add(new Line(comboPair.first.vecOne.add(comboPair.second.vecOne).scalarMultiply(0.5), + comboPair.first.vecTwo.add(comboPair.second.vecTwo).scalarMultiply(0.5), Util.kEpsilon)); + } + System.out.println("lines " + lines.size()); + return lines; + } + + private static final class Pair { + private final T first; + private final T second; + + public Pair(T first, T second) { + this.first = first; + this.second = second; + } + + @Override + public boolean equals(Object o) { + if (this == o) + return true; + if (o == null) + return false; + if (!(o instanceof Pair)) + return false; + return first.equals(((Pair) o).first) && second.equals(((Pair) o).second); + } + + @Override + public int hashCode() { + return Objects.hash(first, second); + } + + public static Set> combinationPairs(Set set) { + var result = new HashSet>(); + for (final var p1 : set) { + for (final var p2 : set) { + if (p1.equals(p2) || result.contains(new Pair(p2, p1))) + continue; + result.add(new Pair(p1, p2)); + } + } + return result; + } + } + + private Rectangle2d toRobotRectangle(Vector2D point) { + return new Rectangle2d((point.getX() - mRobotSize / 3), (point.getY() - mRobotSize / 3), (mRobotSize / 3 * 2), + (mRobotSize / 3 * 2)); + } + + private Translation2d toTranslation(Vector2D vec) { + return new Translation2d(vec.getX(), vec.getY()); + } + + private Vector2D toVector(Translation2d tran) { + return new Vector2D(tran.getX(), tran.getY()); + } +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/TrajectoryGenerator.java b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/TrajectoryGenerator.java new file mode 100644 index 0000000..1ed0a64 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/TrajectoryGenerator.java @@ -0,0 +1,387 @@ +package com.spartronics4915.lib.math.twodim.trajectory; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; +import java.util.stream.Collectors; + +import com.spartronics4915.lib.math.Util; +import com.spartronics4915.lib.math.twodim.geometry.Pose2d; +import com.spartronics4915.lib.math.twodim.geometry.Pose2dWithCurvature; +import com.spartronics4915.lib.math.twodim.geometry.Rotation2d; +import com.spartronics4915.lib.math.twodim.geometry.Translation2d; +import com.spartronics4915.lib.math.twodim.spline.QuinticHermiteSpline; +import com.spartronics4915.lib.math.twodim.spline.Spline; +import com.spartronics4915.lib.math.twodim.spline.SplineHelper; +import com.spartronics4915.lib.math.twodim.spline.SplineParameterizer; +import com.spartronics4915.lib.math.twodim.spline.Spline.ControlVector; +import com.spartronics4915.lib.math.twodim.spline.SplineParameterizer.MalformedSplineException; +import com.spartronics4915.lib.math.twodim.trajectory.constraints.TimingConstraint; +import com.spartronics4915.lib.math.twodim.trajectory.types.DistancedTrajectory; +import com.spartronics4915.lib.math.twodim.trajectory.types.IndexedTrajectory; +import com.spartronics4915.lib.math.twodim.trajectory.types.State; +import com.spartronics4915.lib.math.twodim.trajectory.types.TimedTrajectory; +import com.spartronics4915.lib.math.twodim.trajectory.types.TimedTrajectory.TimedState; +import com.spartronics4915.lib.util.Logger; +import com.spartronics4915.lib.util.Units; + +public class TrajectoryGenerator { + + public static final TrajectoryGenerator defaultTrajectoryGenerator = new TrajectoryGenerator( + Units.inchesToMeters(2), Units.inchesToMeters(0.2), Rotation2d.fromDegrees(5)); + private static final TimedTrajectory kDoNothingTrajectory = new TimedTrajectory( + Arrays.asList(new TimedState(new Pose2dWithCurvature(), 0, 0, 0)), false); + + /** Meters */ + private final double kMaxDx, kMaxDy; + private final Rotation2d kMaxDTheta; + + /** + * Instantiates a trajectory generator with a couple spline parameterization + * knobs that give coordinate deltas (in the field frame) which control how + * frequently we parameterize a point on the spline. + */ + public TrajectoryGenerator(double maxDxMeters, double maxDyMeters, Rotation2d maxDTheta) { + kMaxDx = maxDxMeters; + kMaxDy = maxDyMeters; + kMaxDTheta = maxDTheta; + } + + public TimedTrajectory generateTrajectory(Pose2d start, List interiorWaypoints, + Pose2d end, List> constraints, double startVelocityMetersPerSec, + double endVelocityMetersPerSec, double maxVelocityMetersPerSec, double maxAccelerationMeterPerSecSq, + boolean reversed) { + var controlVectors = SplineHelper.getCubicControlVectorsFromWaypoints(start, + interiorWaypoints.toArray(new Translation2d[0]), end); + final var flipTransform = new Pose2d(0, 0, Rotation2d.fromDegrees(180)); + + // Clone the control vectors. + var newInitial = new Spline.ControlVector(controlVectors[0].x, controlVectors[0].y); + var newEnd = new Spline.ControlVector(controlVectors[1].x, controlVectors[1].y); + + // Change the orientation if reversed. + if (reversed) { + newInitial.x[1] *= -1; + newInitial.y[1] *= -1; + newEnd.x[1] *= -1; + newEnd.y[1] *= -1; + } + + // Get the spline points + List points; + try { + points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromControlVectors(newInitial, + interiorWaypoints.toArray(new Translation2d[0]), newEnd)); + } catch (MalformedSplineException e) { + Logger.exception(e); + return kDoNothingTrajectory; + } + + // Change the points back to their original orientation. + var trajectory = new IndexedTrajectory<>(points); + if (reversed) { + trajectory = new IndexedTrajectory<>(trajectory.stream() + .map((state) -> new Pose2dWithCurvature(state.getPose().transformBy(flipTransform), + -state.getCurvature(), state.getDCurvatureDs())) + .collect(Collectors.toList())); + } + + return timeParameterizeTrajectory(new DistancedTrajectory(trajectory), constraints, + startVelocityMetersPerSec, endVelocityMetersPerSec, maxVelocityMetersPerSec, + Math.abs(maxAccelerationMeterPerSecSq), kMaxDx, reversed); + } + + public TimedTrajectory generateTrajectory(List waypoints, + List> constraints, double startVelocityMetersPerSec, + double endVelocityMetersPerSec, double maxVelocityMetersPerSec, double maxAccelerationMeterPerSecSq, + boolean reversed) { + Pose2d flipTransform = new Pose2d(0, 0, Rotation2d.fromDegrees(180)); + + // Make theta normal for trajectory generation if path is trajectoryReversed. + List newWaypoints = waypoints.stream() + .map((point) -> reversed ? point.transformBy(flipTransform) : point).collect(Collectors.toList()); + + var controlVectors = SplineHelper.getQuinticControlVectorsFromWaypoints(newWaypoints); + + List points; + try { + points = splinePointsFromSplines(SplineHelper + .getQuinticSplinesFromControlVectors(controlVectors.toArray(new Spline.ControlVector[] {}))); + } catch (Exception e) { + Logger.exception(e); + return kDoNothingTrajectory; + } + var trajectory = new IndexedTrajectory<>(points); + + // After trajectory generation, flip theta back so it's relative to the field. + // Also fix curvature. + // Derivative of curvature should stay the same because the change in curvature + // will be the same. + if (reversed) { + trajectory = new IndexedTrajectory<>(trajectory.stream() + .map((state) -> new Pose2dWithCurvature(state.getPose().transformBy(flipTransform), + -state.getCurvature(), state.getDCurvatureDs())) + .collect(Collectors.toList())); + } + + return timeParameterizeTrajectory(new DistancedTrajectory(trajectory), constraints, + startVelocityMetersPerSec, endVelocityMetersPerSec, maxVelocityMetersPerSec, + Math.abs(maxAccelerationMeterPerSecSq), kMaxDx, reversed); + } + + public static List splinePointsFromSplines(Spline[] splines) { + // Create the vector of spline points. + var splinePoints = new ArrayList(); + + // Add the first point to the vector. + splinePoints.add(splines[0].getPoint(0.0)); + + // Iterate through the vector and parameterize each spline, adding the + // parameterized points to the final vector. + for (final var spline : splines) { + var points = SplineParameterizer.parameterize(spline); + + // Append the array of poses to the vector. We are removing the first + // point because it's a duplicate of the last point from the previous + // spline. + splinePoints.addAll(points.subList(1, points.size())); + } + return splinePoints; + } + + private > TimedTrajectory timeParameterizeTrajectory( + /* + * Note that we don't follow the units naming convention here. This is usually + * bad, but this is a private method and we're trying to be less verbose. + * + * All units are meters (step size), meters/sec (velocity) or meters/sec^2 + * (acceleration). + */ + DistancedTrajectory distancedTrajectory, List> constraints, double startVelocity, + double endVelocity, double maxVelocity, double maxAcceleration, double stepSize, boolean reversed) { + class ConstrainedState { + public S state; + /** Meters */ + public double distance = 0; + /** Meters/sec */ + public double maxVelocity = 0; + /** Meters/sec^2 */ + public double minAcceleration = 0, maxAcceleration = 0; + } + + var accelLimiter = new Object() { + public void enforceAccelerationLimits(boolean reversed, List> constraints, + ConstrainedState constrainedState) { + for (TimingConstraint constraint : constraints) { + var minMaxAccel = constraint.getMinMaxAcceleration(constrainedState.state, + (reversed ? -1 : 1) * constrainedState.maxVelocity); + if (!minMaxAccel.valid) { + throw new RuntimeException( + "Constraint returned an invalid minMaxAccel for state " + constrainedState.state); + } + + constrainedState.minAcceleration = Math.max(constrainedState.minAcceleration, + reversed ? -minMaxAccel.maxAcceleration : minMaxAccel.minAcceleration); + + constrainedState.maxAcceleration = Math.min(constrainedState.maxAcceleration, + reversed ? -minMaxAccel.minAcceleration : minMaxAccel.maxAcceleration); + } + } + }; + + int distanceViewSteps = (int) Math.ceil( + (distancedTrajectory.getLastInterpolant() - distancedTrajectory.getFirstInterpolant()) / stepSize + 1); + + List states = new ArrayList<>(); + for (int i = 0; i < distanceViewSteps; i++) { + S state = distancedTrajectory.sample(Util.limit(i * stepSize + distancedTrajectory.getFirstInterpolant(), + distancedTrajectory.getFirstInterpolant(), distancedTrajectory.getLastInterpolant())).state; + states.add(state); + } + + List constrainedStates = new ArrayList<>(states.size()); + double epsilon = 1e-6; + + // Note that the forwards and backwards passes (i.e. everything below this + // point) are looking to find one thing, and one thing only, for each state: max + // acheiveable velocity given the constraints and the other adjacent states. All + // the other mins and maxes, and all the acceleration info is just bookkeeping + // to get to that point. + + // Forward pass. We look at pairs of consecutive states, where the start state + // has already been velocity parameterized (though we may adjust the velocity + // downwards during the backwards pass). We wish to find an acceleration that is + // admissible at both the start and end state, as well as an admissible end + // velocity. If there is no admissible end velocity or acceleration, we set the + // end velocity to the state's maximum allowed velocity and will repair the + // acceleration during the backward pass (by slowing down the predecessor). + + var predecessor = new ConstrainedState(); + predecessor.state = states.get(0); + predecessor.distance = 0.0; + predecessor.maxVelocity = startVelocity; + predecessor.minAcceleration = -maxAcceleration; + predecessor.maxAcceleration = maxAcceleration; + + for (S s : states) { + var constrainedState = new ConstrainedState(); + constrainedState.state = s; + /** Meters */ + double ds = constrainedState.state.distance(predecessor.state); + constrainedState.distance = ds + predecessor.distance; + constrainedStates.add(constrainedState); + + // We may need to iterate to find the maximum end velocity and common + // acceleration, since acceleration limits may be a function of velocity. + while (true) { + // Enforce global max velocity and max reachable velocity by global acceleration + // limit. + // vf = sqrt(vi^2 + 2*a*d) + // (This equation finds a final velocity given an initial velocity, + // acceleration, and distance) + constrainedState.maxVelocity = Math.min(maxVelocity, + Math.sqrt(Math.pow(predecessor.maxVelocity, 2) + 2 * predecessor.maxAcceleration * ds)); + if (Double.isNaN(constrainedState.maxVelocity)) { + throw new RuntimeException("Max velocicity is NaN"); + } + // Enforce global max absolute acceleration + constrainedState.minAcceleration = -maxAcceleration; + constrainedState.maxAcceleration = maxAcceleration; + + // At this point, the state is fully constructed, but no constraints have been + // applied aside from predecessor state max accel. + + // Enforce all velocity constraints + for (var constraint : constraints) { + constrainedState.maxVelocity = Math.min(constrainedState.maxVelocity, + constraint.getMaxVelocity(constrainedState.state)); + } + if (constrainedState.maxVelocity < 0.0) { + // This should never happen if constraints are well-behaved + throw new RuntimeException("constrainedState max velocity cannot be negative"); + } + + // Now enforce all acceleration constraints + accelLimiter.enforceAccelerationLimits(reversed, constraints, constrainedState); + if (constrainedState.minAcceleration > constrainedState.maxAcceleration) { + // This should never happen if constraints are well-behaved + throw new RuntimeException( + "constrainedState min acceleration cannot be greater than max acceleration"); + } + + if (ds < epsilon) { + break; + } + + double actualAcceleration = (Math.pow(constrainedState.maxVelocity, 2) + - Math.pow(predecessor.maxVelocity, 2)) / (2 * ds); + if (constrainedState.maxAcceleration < actualAcceleration - epsilon) { + // If the max acceleration for this constraint state is more conservative than + // what we had applied, we need to reduce the max accel at the predecessor state + // and try again. + predecessor.maxAcceleration = constrainedState.maxAcceleration; + } else { + if (actualAcceleration > predecessor.minAcceleration + epsilon) { + predecessor.maxAcceleration = actualAcceleration; + } + // If actual acceleration is less than predecessor min accel, we will repair + // during the backward pass. + break; + } + } + predecessor = constrainedState; + } + + // Backwards pass + var successor = new ConstrainedState(); + successor.state = states.get(states.size() - 1); + successor.distance = constrainedStates.get(states.size() - 1).distance; + successor.maxVelocity = endVelocity; + successor.minAcceleration = -maxAcceleration; + successor.maxAcceleration = maxAcceleration; + for (int i = states.size() - 1; i >= 0; i--) { + ConstrainedState constrainedState = constrainedStates.get(i); + /** Meters */ + double ds = constrainedState.distance - successor.distance; // Will be negative + + while (true) { + // Enforce reverse max reachable velocity limit + // vf = sqrt(vi^2 + 2*a*d), where vi = successor + var newMaxVelocity = Math.sqrt(Math.pow(successor.maxVelocity, 2) + 2 * successor.minAcceleration * ds); + if (newMaxVelocity >= constrainedState.maxVelocity) { + // No new limits to impose + break; + } + constrainedState.maxVelocity = newMaxVelocity; + if (Double.isNaN(constrainedState.maxVelocity)) { + throw new RuntimeException("constrainedState max velocity can't be NaN"); + } + + // Now check all acceleration constraints with lower max velocity + if (constrainedState.minAcceleration > constrainedState.maxAcceleration) { + throw new RuntimeException( + "constrainedState min acceleration cannot be greater than max acceleration"); + } + + if (ds > epsilon) { + break; + } + // If the min acceleration for this constraint state is more conservative than + // what we have applied then we need to reduce the min accel and try again + double actualAcceleration = (Math.pow(constrainedState.maxVelocity, 2) + - Math.pow(successor.maxVelocity, 2)) / (2 * ds); + if (constrainedState.minAcceleration > actualAcceleration + epsilon) { + successor.minAcceleration = constrainedState.minAcceleration; + } else { + // This sets successor.minAcceleration to NaN on the first iteration (because ds + // is 0), but it doesn't matter + successor.minAcceleration = actualAcceleration; + break; + } + } + successor = constrainedState; + } + + // Integrate the constrained states forward in time to obtain the TimedStates + List> timedStates = new ArrayList<>(states.size()); + /** Current time in seconds */ + double t = 0; + /** Current distance in meters */ + double s = 0; + /** Current velocity in meters/sec */ + double v = 0; + for (int i = 0; i < states.size(); i++) { + ConstrainedState constrainedState = constrainedStates.get(i); + + // Here we find the dt to advance t by + // Finding the unknown, dt, requires the current state's max velocity, the last + // state's actual velocity, and ds (delta distance traveled) + double ds = constrainedState.distance - s; + double accel = (Math.pow(constrainedState.maxVelocity, 2) - Math.pow(v, 2)) / (2.0 * ds); + double dt = 0.0; + if (i > 0) { + timedStates.set(i - 1, new TimedState(timedStates.get(i - 1), reversed ? -accel : accel)); + + if (Math.abs(accel) > epsilon) { + dt = (constrainedState.maxVelocity - v) / accel; + } else if (Math.abs(v) > epsilon) { + dt = ds / v; + } else { + throw new RuntimeException( + "Couldn't determine a dt to integrate because robot is too still after the initial pose"); + } + } + t += dt; + if (Double.isNaN(t) || Double.isInfinite(t)) { + throw new RuntimeException(); + } + + v = constrainedState.maxVelocity; + s = constrainedState.distance; + timedStates.add(new TimedState(constrainedState.state, t, reversed ? -v : v, reversed ? -accel : accel)); + } + + return new TimedTrajectory(timedStates, reversed); + } + +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/AngularAccelerationConstraint.java b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/AngularAccelerationConstraint.java new file mode 100644 index 0000000..87b29f1 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/AngularAccelerationConstraint.java @@ -0,0 +1,71 @@ +package com.spartronics4915.lib.math.twodim.trajectory.constraints; + +import com.spartronics4915.lib.math.twodim.geometry.Pose2dWithCurvature; + +public class AngularAccelerationConstraint implements TimingConstraint { + + /** Rads/sec^2 */ + private final double mMaxAngularAcceleration; + + public AngularAccelerationConstraint(double maxAngularAccelRadsPerSecSq) { + if (maxAngularAccelRadsPerSecSq < 0) + throw new RuntimeException("Cannot have negative angular acceleration"); + + mMaxAngularAcceleration = maxAngularAccelRadsPerSecSq; + } + + @Override + public double getMaxVelocity(Pose2dWithCurvature state) { + /* + * This bit ensures that we don't violate our constraint indirectly. I.e. we + * don't want v^2 * dk/ds alone to go over the max angular acceleration. + * + * v^2 * dk/ds = maxAngularAcceleration when linear acceleration = 0. + * + * v = sqrt(maxAngularAcceleration / dk/ds) + */ + + return Math.sqrt(mMaxAngularAcceleration / Math.abs(state.getDCurvatureDs())); + } + + @Override + public MinMaxAcceleration getMinMaxAcceleration(Pose2dWithCurvature state, double velocity) { + // @formatter:off + /* + * We want to limit the acceleration such that we never go above the specified angular acceleration. + * + * Angular acceleration = dw/dt WHERE w = omega = angular velocity + * w = v * k WHERE v = linear velocity, k = curvature + * WHERE s = distance traveled + * + * dw/dt = d/dt (v * k) + * + * By chain rule, + * dw/dt = dv/dt * k + v * dk/dt [1] + * + * We don't have dk/dt, but we do have dk/ds and ds/dt + * dk/ds * ds/dt = dk/dt [2] + * + * Substituting [2] in [1], we get + * dw/dt = acceleration * curvature + velocity * velocity * d_curvature + * WHERE acceleration = dv/dt, velocity = ds/dt or v, d_curvature = dk/dt and curvature = k + * + * We now want to find the linear acceleration such that the angular acceleration (dw/dt) never goes above + * the specified amount. + * + * We can rearrange the last equality we derived to get the below: + * linear acceleration * curvature = dw/dt - (velocity * velocity * d_curvature) + * linear acceleration = (dw/dt - (velocity * velocity * d_curvature)) / curvature + * + * (Above is adapted from a comment originally in FalconLib by Prateek Machiraju) + */ + // @formatter:on + + double maxAbsoluteAccel = Math.abs( + (mMaxAngularAcceleration - (velocity * velocity * state.getDCurvatureDs())) / state.getCurvature() + ); + + return new TimingConstraint.MinMaxAcceleration(-maxAbsoluteAccel, maxAbsoluteAccel); + } + +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/CentripetalAccelerationConstraint.java b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/CentripetalAccelerationConstraint.java new file mode 100644 index 0000000..e47c0ea --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/CentripetalAccelerationConstraint.java @@ -0,0 +1,23 @@ +package com.spartronics4915.lib.math.twodim.trajectory.constraints; + +import com.spartronics4915.lib.math.twodim.geometry.Pose2dWithCurvature; + +public class CentripetalAccelerationConstraint implements TimingConstraint { + + /** Meters/sec^2 */ + private final double mMaxCentripetalAccel; + + public CentripetalAccelerationConstraint(final double maxCentripetalAccelMetersPerSecSq) { + mMaxCentripetalAccel = maxCentripetalAccelMetersPerSecSq; + } + + @Override + public double getMaxVelocity(final Pose2dWithCurvature state) { + return Math.sqrt(Math.abs(mMaxCentripetalAccel / state.getCurvature())); + } + + @Override + public MinMaxAcceleration getMinMaxAcceleration(final Pose2dWithCurvature state, final double velocity) { + return MinMaxAcceleration.kNoLimits; + } +} diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/DifferentialDriveDynamicsConstraint.java b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/DifferentialDriveDynamicsConstraint.java new file mode 100644 index 0000000..092948a --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/DifferentialDriveDynamicsConstraint.java @@ -0,0 +1,33 @@ +package com.spartronics4915.lib.math.twodim.trajectory.constraints; + +import com.spartronics4915.lib.math.twodim.geometry.Pose2dWithCurvature; +import com.spartronics4915.lib.math.twodim.physics.DifferentialDrive; + +public class DifferentialDriveDynamicsConstraint implements TimingConstraint { + + private final DifferentialDrive mDifferentialDrive; + /** Non-normalized volts (i.e. this is in the domain of [0, 12]) */ + private final double mMaxVoltage; + + public DifferentialDriveDynamicsConstraint(DifferentialDrive drive, double maxVoltage) { + mDifferentialDrive = drive; + mMaxVoltage = maxVoltage; + } + + @Override + public double getMaxVelocity(Pose2dWithCurvature state) { + return mDifferentialDrive.getMaxAbsVelocity(state.getCurvature(), mMaxVoltage); + } + + @Override + public MinMaxAcceleration getMinMaxAcceleration(Pose2dWithCurvature state, double velocity) { + DifferentialDrive.MinMax minMax = mDifferentialDrive.getMinMaxAcceleration( + new DifferentialDrive.ChassisState(velocity, velocity * state.getCurvature()), + state.getCurvature(), + mMaxVoltage + ); + + return new MinMaxAcceleration(minMax.min, minMax.max); + } + +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/TimingConstraint.java b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/TimingConstraint.java new file mode 100644 index 0000000..6b1e869 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/TimingConstraint.java @@ -0,0 +1,25 @@ +package com.spartronics4915.lib.math.twodim.trajectory.constraints; + +public interface TimingConstraint { + double getMaxVelocity(S state); + + MinMaxAcceleration getMinMaxAcceleration(S state, double velocity); + + public class MinMaxAcceleration { + + public static final MinMaxAcceleration kNoLimits = + new MinMaxAcceleration(Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY); + + /** Meters/sec^2 */ + public final double minAcceleration, maxAcceleration; + + /** Checks if minAcceleration <= maxAcceleration */ + public final boolean valid; + + public MinMaxAcceleration(double minAccelerationMetersPerSecSq, double maxAccelerationMetersPerSecSq) { + minAcceleration = minAccelerationMetersPerSecSq; + maxAcceleration = maxAccelerationMetersPerSecSq; + valid = minAcceleration <= maxAcceleration; + } + } +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/VelocityLimitRadiusConstraint.java b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/VelocityLimitRadiusConstraint.java new file mode 100644 index 0000000..c4e450a --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/VelocityLimitRadiusConstraint.java @@ -0,0 +1,30 @@ +package com.spartronics4915.lib.math.twodim.trajectory.constraints; + +import com.spartronics4915.lib.math.twodim.geometry.Pose2dWithCurvature; +import com.spartronics4915.lib.math.twodim.geometry.Translation2d; + +public class VelocityLimitRadiusConstraint implements TimingConstraint { + + /** Meters */ + private final Translation2d mCenter; + /** Meters */ + private final double mRadius; + /** Meters/sec */ + private final double mVelocityLimit; + + public VelocityLimitRadiusConstraint(Translation2d centerMeters, double radiusMeters, double velocityLimitMetersPerSec) { + mCenter = centerMeters; + mRadius = radiusMeters; + mVelocityLimit = velocityLimitMetersPerSec; + } + + @Override + public double getMaxVelocity(Pose2dWithCurvature state) { + return state.getTranslation().getDistance(mCenter) <= mRadius ? mVelocityLimit : Double.POSITIVE_INFINITY; + } + + @Override + public MinMaxAcceleration getMinMaxAcceleration(Pose2dWithCurvature state, double velocity) { + return TimingConstraint.MinMaxAcceleration.kNoLimits; + } +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/VelocityLimitRegionConstraint.java b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/VelocityLimitRegionConstraint.java new file mode 100644 index 0000000..8c70811 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/constraints/VelocityLimitRegionConstraint.java @@ -0,0 +1,28 @@ +package com.spartronics4915.lib.math.twodim.trajectory.constraints; + +import com.spartronics4915.lib.math.twodim.geometry.Pose2dWithCurvature; +import com.spartronics4915.lib.math.twodim.geometry.Rectangle2d; + +public class VelocityLimitRegionConstraint implements TimingConstraint { + + /** Meters */ + private Rectangle2d mRegion; + /** Meters/sec */ + private double mVelocityLimit; + + public VelocityLimitRegionConstraint(Rectangle2d regionMeters, double velocityLimitMetersPerSec) { + mRegion = regionMeters; + mVelocityLimit = velocityLimitMetersPerSec; + } + + @Override + public double getMaxVelocity(Pose2dWithCurvature state) { + return mRegion.contains(state.getTranslation()) ? mVelocityLimit : Double.POSITIVE_INFINITY; + } + + @Override + public MinMaxAcceleration getMinMaxAcceleration(Pose2dWithCurvature state, double velocity) { + return TimingConstraint.MinMaxAcceleration.kNoLimits; + } + +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/DistancedTrajectory.java b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/DistancedTrajectory.java new file mode 100644 index 0000000..6867a09 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/DistancedTrajectory.java @@ -0,0 +1,78 @@ +package com.spartronics4915.lib.math.twodim.trajectory.types; + +import java.util.ArrayList; +import java.util.List; +import java.util.ListIterator; +import java.util.stream.IntStream; + +import com.spartronics4915.lib.math.Util; + +public class DistancedTrajectory> extends Trajectory { + + private List mDistances = new ArrayList<>(); + + public DistancedTrajectory(List points) { + super(points); + + mDistances.add(0.0); + + ListIterator iter = mPoints.listIterator(); + while (iter.hasNext()) { + var firstPoint = iter.next(); + if (!iter.hasNext()) + break; + + mDistances.add(mDistances.get(mDistances.size() - 1) + firstPoint.distance(iter.next())); + iter.previous(); // Send the iterator back by 1 (we're basically doing a peek) + } + } + + public DistancedTrajectory(IndexedTrajectory otherTraj) { + this(otherTraj.mPoints); + } + + @Override + public TrajectorySamplePoint sample(double interpolant) { + if (interpolant >= getLastInterpolant()) { + return new TrajectorySamplePoint(getPoint(mPoints.size() - 1)); + } else if (interpolant <= getFirstInterpolant()) { + return new TrajectorySamplePoint(getPoint(0)); + } else { + int index = IntStream.range(0, mPoints.size()).filter((i) -> i != 0 && mDistances.get(i) >= interpolant) + .findFirst().orElseThrow(); // This should never throw because of the above checks + + S upperBoundState = mPoints.get(index); + S lowerBoundState = mPoints.get(index - 1); + + if (Util.epsilonEquals(mDistances.get(index), mDistances.get(index - 1))) { + // XXX: Why don't we do index - 1 and index instead of index and index? + return new TrajectorySamplePoint(lowerBoundState, index, index); + } else { + return new TrajectorySamplePoint( + lowerBoundState.interpolate( + upperBoundState, + (interpolant - mDistances.get(index - 1)) / (mDistances.get(index) - mDistances.get(index - 1)) + ), + index - 1, index + ); + } + } + } + + @Override + public double getFirstInterpolant() { + return 0.0; + } + + @Override + public double getLastInterpolant() { + return mDistances.get(mDistances.size() - 1); + } + + public static class DistancedIterator> extends TrajectoryIterator { + public DistancedIterator(DistancedTrajectory trajectory) { + super(trajectory); + } + } + +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/IndexedTrajectory.java b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/IndexedTrajectory.java new file mode 100644 index 0000000..eed5860 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/IndexedTrajectory.java @@ -0,0 +1,62 @@ +package com.spartronics4915.lib.math.twodim.trajectory.types; + +import java.util.List; +import java.util.stream.Stream; + +import com.spartronics4915.lib.math.Util; + +public class IndexedTrajectory> extends Trajectory { + + public IndexedTrajectory(List points) { + super(points); + } + + @Override + public TrajectorySamplePoint sample(double interpolant) { + if (mPoints.isEmpty()) + throw new IndexOutOfBoundsException("Trajectory is empty"); + + if (interpolant <= 0) { + return new TrajectorySamplePoint(getPoint(0)); + } else if (interpolant >= mPoints.size() - 1) { + return new TrajectorySamplePoint(getPoint(mPoints.size() - 1)); + } else { + int index = (int) Math.floor(interpolant); + double percent = interpolant - index; + + // 254 (and FalconLib) use Double.MIN_VALUE as an epsilon, so we're + // going to as well to ensure consistency + if (Util.epsilonEquals(percent, 0, Double.MIN_VALUE)) { + return new TrajectorySamplePoint(getPoint(index)); + } else if (Util.epsilonEquals(percent, 1, Double.MIN_VALUE)) { + return new TrajectorySamplePoint(getPoint(index + 1)); + } else { + return new TrajectorySamplePoint( + mPoints.get(index).interpolate(mPoints.get(index), percent), + index, + index + 1 + ); + } + } + } + + @Override + public double getFirstInterpolant() { + return 0; + } + + @Override + public double getLastInterpolant() { + return Math.max(0.0, mPoints.size() - 1); + } + + public Stream stream() { + return mPoints.stream(); + } + + public static class IndexedIterator> extends TrajectoryIterator { + public IndexedIterator(IndexedTrajectory trajectory) { + super(trajectory); + } + } +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/State.java b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/State.java new file mode 100644 index 0000000..33e4153 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/State.java @@ -0,0 +1,7 @@ +package com.spartronics4915.lib.math.twodim.trajectory.types; + +import com.spartronics4915.lib.util.Interpolable; + +public interface State extends Interpolable { + double distance(S other); +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/TimedTrajectory.java b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/TimedTrajectory.java new file mode 100644 index 0000000..cfda55e --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/TimedTrajectory.java @@ -0,0 +1,111 @@ +package com.spartronics4915.lib.math.twodim.trajectory.types; + +import java.util.List; +import java.util.stream.IntStream; + +import com.spartronics4915.lib.math.Util; + +public class TimedTrajectory> extends Trajectory> { + + public TimedTrajectory(List> points, boolean reversed) { + super(points); + mReversed = reversed; + } + + @Override + public TrajectorySamplePoint> sample(double interpolant) { + if (interpolant >= getLastInterpolant()) { + return new TrajectorySamplePoint>(getPoint(mPoints.size() - 1)); + } else if (interpolant <= getFirstInterpolant()) { + return new TrajectorySamplePoint>(getPoint(0)); + } else { + int index = IntStream.range(0, mPoints.size()) + .filter((i) -> i != 0 && mPoints.get(i).time >= interpolant) + .findFirst() + .orElseThrow(); // This should never throw because of the above checks + + TimedState upperBoundState = mPoints.get(index); + TimedState lowerBoundState = mPoints.get(index - 1); + + if (Util.epsilonEquals(upperBoundState.time, lowerBoundState.time)) { + return new TrajectorySamplePoint>(lowerBoundState, index, index); + } else { + return new TrajectorySamplePoint>( + lowerBoundState.interpolate( + upperBoundState, + (interpolant - lowerBoundState.time) / (upperBoundState.time - lowerBoundState.time) + ), + index - 1, + index + ); + } + } + } + + @Override + public double getFirstInterpolant() { + return getFirstState().time; + } + + @Override + public double getLastInterpolant() { + return getLastState().time; + } + + /** + * @return Total time interval taken by the trajectory, in seconds. + */ + public double getTotalTime() { + return getLastInterpolant() - getFirstInterpolant(); + } + + public static class TimedState> implements State> { + public final S state; + /** Time in seconds */ + public final double time; + /** Velocity in meters/sec */ + public final double velocity; + /** Acceleration in meters/sec^2 */ + public final double acceleration; + + public TimedState(S state, double timeSeconds, double velocityMetersPerSecond, + double accelerationMetersPerSecondSq) { + this.state = state; + this.time = timeSeconds; + this.velocity = velocityMetersPerSecond; + this.acceleration = accelerationMetersPerSecondSq; + } + + /** Special case copy-constructor */ + public TimedState(TimedState other, double newAccelerationMetersPerSecondSq) { + this(other.state, other.time, other.velocity, newAccelerationMetersPerSecondSq); + } + + @Override + public TimedState interpolate(TimedState endValue, double time) { + double newT = Util.interpolate(this.time, endValue.time, time); + double deltaT = newT - this.time; + if (deltaT < 0.0) + return endValue.interpolate(this, 1.0 - time); + + boolean reversing = velocity < 0.0 || Util.epsilonEquals(velocity, 0.0) && acceleration < 0.0; + + double newV = velocity + acceleration * deltaT; + double newS = (reversing ? -1 : 1) * (velocity * deltaT + 0.5 * acceleration * deltaT * deltaT); + + return new TimedState(state.interpolate(endValue.state, newS / state.distance(endValue.state)), newT, + newV, acceleration); + } + + @Override + public double distance(TimedState endValue) { + return this.state.distance(endValue.state); + } + } + + public static class TimedIterator> extends TrajectoryIterator> { + public TimedIterator(TimedTrajectory trajectory) { + super(trajectory); + } + } +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/Trajectory.java b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/Trajectory.java new file mode 100644 index 0000000..20580cf --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/Trajectory.java @@ -0,0 +1,66 @@ +package com.spartronics4915.lib.math.twodim.trajectory.types; + +import java.util.List; + +import com.spartronics4915.lib.util.Interpolable; + +public abstract class Trajectory> { + protected List mPoints; + protected boolean mReversed = false; + + protected Trajectory(List points) { + mPoints = points; + } + + public TrajectoryPoint getPoint(int index) { + return new TrajectoryPoint(index, mPoints.get(index)); + } + + public int size() { + return mPoints.size(); + } + + public S getFirstState() { + return mPoints.get(0); + } + + public S getLastState() { + return mPoints.get(mPoints.size() - 1); + } + + public boolean isReversed() { + return mReversed; + } + + public abstract TrajectorySamplePoint sample(double interpolant); + + public abstract double getFirstInterpolant(); + public abstract double getLastInterpolant(); + + public static class TrajectoryPoint { + public int index; + public S state; + + public TrajectoryPoint(int index, S state) { + this.index = index; + this.state = state; + } + } + + public static class TrajectorySamplePoint { + public S state; + public int indexFloor, indexCeil; + + public TrajectorySamplePoint(TrajectoryPoint p) { + this.state = p.state; + this.indexFloor = p.index; + this.indexCeil = p.index; + } + + public TrajectorySamplePoint(S state, int indexFloor, int indexCeil) { + this.state = state; + this.indexFloor = indexFloor; + this.indexCeil = indexCeil; + } + } +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/TrajectoryIterator.java b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/TrajectoryIterator.java new file mode 100644 index 0000000..b1880e8 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/math/twodim/trajectory/types/TrajectoryIterator.java @@ -0,0 +1,59 @@ +package com.spartronics4915.lib.math.twodim.trajectory.types; + +import com.spartronics4915.lib.math.Util; +import com.spartronics4915.lib.math.twodim.trajectory.types.Trajectory.TrajectorySamplePoint; + +/** + * This class makes it easy to walk a trajectory by some unit. This is abstract + * so that subclasses can be made that specify which unit they use to index the + * trajectory (note that this hierarchy is merely for readability, and not a + * property of the subclasses enforced by the type system). + * + * @param State that the trajectory holds. + */ +public abstract class TrajectoryIterator> { + + private final Trajectory mTrajectory; + private double mProgress; + private TrajectorySamplePoint mCurrentSample; + + public TrajectoryIterator(final Trajectory trajectory) { + mTrajectory = trajectory; + + mCurrentSample = mTrajectory.sample(mTrajectory.getFirstInterpolant()); + mProgress = mTrajectory.getFirstInterpolant(); + } + + public boolean isDone() { + return getRemainingProgress() <= 0.0; + } + + public double getProgress() { + return mProgress; + } + + public double getRemainingProgress() { + return Math.max(0.0, mTrajectory.getLastInterpolant() - mProgress); + } + + public TrajectorySamplePoint getCurrentSample() { + return mCurrentSample; + } + + public Trajectory getTrajectory() { + return mTrajectory; + } + + public TrajectorySamplePoint advance(double additionalProgress) { + mProgress = Util.limit(mProgress + additionalProgress, mTrajectory.getFirstInterpolant(), + mTrajectory.getLastInterpolant()); + mCurrentSample = mTrajectory.sample(mProgress); + return mCurrentSample; + } + + public TrajectorySamplePoint preview(double additionalProgress) { + double progress = Util.limit(mProgress + additionalProgress, mTrajectory.getFirstInterpolant(), + mTrajectory.getLastInterpolant()); + return mTrajectory.sample(progress); + } +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/subsystems/SpartronicsSubsystem.java b/src/main/java/com/spartronics4915/lib/subsystems/SpartronicsSubsystem.java new file mode 100644 index 0000000..2e4bf1c --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/subsystems/SpartronicsSubsystem.java @@ -0,0 +1,130 @@ +package com.spartronics4915.lib.subsystems; + +import com.spartronics4915.lib.util.Logger; + +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * The Subsystem abstract class, which serves as a basic framework for all robot + * subsystems. Each subsystem outputs + * commands to SmartDashboard, has a stop routine (for after each match), and a + * routine to zero all sensors, which helps + * with calibration. + *

+ * All Subsystems only have one instance (after all, one robot does not have two + * drivetrains), and functions get the + * instance of the drivetrain and act accordingly. + */ +public abstract class SpartronicsSubsystem implements Subsystem +{ + + // All subsystems should set mInitialized upon successful init. + private boolean mInitialized = false; + private String mName = null; + + public boolean isInitialized() + { + return mInitialized; + } + + protected SpartronicsSubsystem() + { + String classname = this.getClass().getName(); + int tail = classname.lastIndexOf('.'); + if (tail == -1) + { + mName = classname; + } + else + { + mName = classname.substring(tail + 1); + } + } + + public String getClassName() + { + return mName; + } + + public void logInitialized(boolean success) + { + mInitialized = success; + if (success) + this.logNotice("init SUCCEEDED"); + else + this.logWarning("init FAILED"); + SmartDashboard.putString(mName + "/Status", mInitialized ? "OK" : "ERROR"); + } + + public void dashboardPutString(String nm, String value) + { + SmartDashboard.putString(mName + "/" + nm, value); + } + + public String dashboardGetString(String nm, String defValue) + { + return SmartDashboard.getString(mName + "/" + nm, defValue); + } + + public void dashboardPutNumber(String nm, Number value) + { + SmartDashboard.putNumber(mName + "/" + nm, value.doubleValue()); + } + + public Number dashboardGetNumber(String nm, Number defaultValue) + { + return SmartDashboard.getNumber(mName + "/" + nm, defaultValue.doubleValue()); + } + + public void dashboardPutBoolean(String nm, Boolean value) + { + SmartDashboard.putBoolean(mName + "/" + nm, value); + } + + public boolean dashboardGetBoolean(String nm, Boolean defValue) + { + return SmartDashboard.getBoolean(mName + "/" + nm, defValue); + } + + // log methods are for conventionalizing format across subsystems + public void logException(String msg, Throwable e) + { + Logger.logThrowableCrash(this.getClassName() + " " + msg, e); + } + + public void logError(String msg) + { + Logger.error(this.getClassName() + " " + msg); + } + + public void logWarning(String msg) + { + Logger.warning(this.getClassName() + " " + msg); + } + + public void logNotice(String msg) + { + Logger.notice(this.getClassName() + " " + msg); + } + + public void logInfo(String msg) + { + Logger.info(this.getClassName() + " " + msg); + } + + public void logDebug(String msg) + { + Logger.debug(this.getClassName() + " " + msg); + } + + public void zeroSensors() + { + } + + @Override + public void periodic() { + dashboardPutString("currentCommand", CommandScheduler.getInstance().requiring(this).getName()); + } +} diff --git a/src/main/java/com/spartronics4915/lib/subsystems/drive/AbstractDrive.java b/src/main/java/com/spartronics4915/lib/subsystems/drive/AbstractDrive.java new file mode 100644 index 0000000..65541de --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/subsystems/drive/AbstractDrive.java @@ -0,0 +1,116 @@ +package com.spartronics4915.lib.subsystems.drive; + +import com.spartronics4915.lib.hardware.motors.SpartronicsMotor; +import com.spartronics4915.lib.hardware.sensors.SpartronicsIMU; +import com.spartronics4915.lib.math.twodim.geometry.Rotation2d; +import com.spartronics4915.lib.math.twodim.physics.DifferentialDrive; +import com.spartronics4915.lib.subsystems.SpartronicsSubsystem; + +public abstract class AbstractDrive extends SpartronicsSubsystem implements DifferentialTrackerDriveBase +{ + protected final SpartronicsMotor mLeftMotor, mRightMotor; + protected final SpartronicsIMU mIMU; + protected final DifferentialDrive mDifferentialDrive; + + protected Rotation2d mIMUOffset = new Rotation2d(); + + /** + * This constructor will set up everything you need. It's protected to allow for + * a singleton drivetrain. + */ + protected AbstractDrive( + SpartronicsMotor leftMotor, + SpartronicsMotor rightMotor, + SpartronicsIMU imu, + DifferentialDrive differentialDrive + ) + { + mLeftMotor = leftMotor; + mRightMotor = rightMotor; + mIMU = imu; + mDifferentialDrive = differentialDrive; + } + + /** + * This sets the heading of the IMU, but this should almost exclusively be + * called by RobotStateEstimator, because that is the single source of truth for + * robot heading. + * + * @param heading Heading to set the IMU to. + */ + public void setIMUHeading(Rotation2d heading) + { + mIMUOffset = getIMUHeading().rotateBy(heading.inverse()); + } + + /** + * This gets the heading of the IMU, but this should almost exclusively be + * called by RobotStateMap, which is the single source of truth for all matters + * of robot pose (including heading). + * + * @return Heading of the IMU. + */ + public Rotation2d getIMUHeading() + { + return mIMU.getYaw().rotateBy(mIMUOffset); + } + + public void arcadeDrive(double linearPercent, double rotationPercent) + { + double maxInput = Math.copySign(Math.max(Math.abs(linearPercent), Math.abs(rotationPercent)), linearPercent); + + double leftMotorOutput, rightMotorOutput; + + if (linearPercent >= 0.0) + { + // First quadrant, else second quadrant + if (rotationPercent >= 0.0) + { + leftMotorOutput = maxInput; + rightMotorOutput = linearPercent - rotationPercent; + } + else + { + leftMotorOutput = linearPercent + rotationPercent; + rightMotorOutput = maxInput; + } + } + else + { + // Third quadrant, else fourth quadrant + if (rotationPercent >= 0.0) + { + leftMotorOutput = linearPercent + rotationPercent; + rightMotorOutput = maxInput; + } + else + { + leftMotorOutput = maxInput; + rightMotorOutput = linearPercent - rotationPercent; + } + } + + tankDrive(leftMotorOutput, rightMotorOutput); + } + + public void tankDrive(double leftPercent, double rightPercent) + { + mLeftMotor.setDutyCycle(leftPercent); + mRightMotor.setDutyCycle(rightPercent); + } + + @Override + public SpartronicsMotor getLeftMotor() { + return mLeftMotor; + } + + @Override + public SpartronicsMotor getRightMotor() { + return mRightMotor; + } + + @Override + public DifferentialDrive getDifferentialDrive() { + return mDifferentialDrive; + } +} diff --git a/src/main/java/com/spartronics4915/lib/subsystems/drive/CharacterizeDriveBaseCommand.java b/src/main/java/com/spartronics4915/lib/subsystems/drive/CharacterizeDriveBaseCommand.java new file mode 100644 index 0000000..e887b67 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/subsystems/drive/CharacterizeDriveBaseCommand.java @@ -0,0 +1,92 @@ +package com.spartronics4915.lib.subsystems.drive; + +import java.util.Set; + +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; + +public class CharacterizeDriveBaseCommand implements Command { + + private final AbstractDrive mDrive; + private final NetworkTableEntry mAutoSpeedEntry = NetworkTableInstance.getDefault().getEntry("/robot/autospeed"); + private final NetworkTableEntry mTelemetryEntry = NetworkTableInstance.getDefault().getEntry("/robot/telemetry"); + /** Meters */ + private final double mWheelCircumference; + + private Number[] mOutputArray = new Number[10]; + private double mLeftInitialPosition = 0.0; + private double mRightInitialPosition = 0.0; + + /** + * This feeds this Python script: + * https://github.com/robotpy/robot-characterization + */ + public CharacterizeDriveBaseCommand(AbstractDrive drive, double wheelDiameterMeters) { + NetworkTableInstance.getDefault().setUpdateRate(0.010); + + mDrive = drive; + mWheelCircumference = wheelDiameterMeters * Math.PI; + } + + @Override + public boolean isFinished() { + // This must be manually stopped by the user via the Disable button + return false; + } + + @Override + public void execute() { + double now = Timer.getFPGATimestamp(); + + // radians and radians/s + double leftPosition = (mDrive.getLeftMotor().getEncoder().getPosition() - mLeftInitialPosition) + / mWheelCircumference * (2 * Math.PI); + double leftVelocity = mDrive.getLeftMotor().getEncoder().getVelocity() / mWheelCircumference * (2 * Math.PI); + + // radians and raians/s + double rightPosition = (mDrive.getRightMotor().getEncoder().getPosition() - mRightInitialPosition) + / mWheelCircumference * (2 * Math.PI); + double rightVelocity = mDrive.getLeftMotor().getEncoder().getVelocity() / mWheelCircumference * (2 * Math.PI); + + // volts + double battery = RobotController.getBatteryVoltage(); + + // volts + double leftMotorVolts = mDrive.getLeftMotor().getVoltageOutput(); + double rightMotorVolts = mDrive.getRightMotor().getVoltageOutput(); + + // percent output on [-1 1] + double autospeed = mAutoSpeedEntry.getDouble(0.0); + + mDrive.tankDrive(autospeed, autospeed); + + // send telemetry data array back to NT + mOutputArray[0] = now; + mOutputArray[1] = battery; + mOutputArray[2] = autospeed; + mOutputArray[3] = leftMotorVolts; + mOutputArray[4] = rightMotorVolts; + mOutputArray[5] = leftPosition; + mOutputArray[6] = rightPosition; + mOutputArray[7] = leftVelocity; + mOutputArray[8] = rightVelocity; + mOutputArray[9] = mDrive.getIMUHeading().getRadians(); + mTelemetryEntry.setNumberArray(mOutputArray); + } + + @Override + public void initialize() { + mLeftInitialPosition = mDrive.getLeftMotor().getEncoder().getPosition(); + mRightInitialPosition = mDrive.getRightMotor().getEncoder().getPosition(); + } + + @Override + public Set getRequirements() { + return Set.of(mDrive); + } + +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/subsystems/drive/DifferentialTrackerDriveBase.java b/src/main/java/com/spartronics4915/lib/subsystems/drive/DifferentialTrackerDriveBase.java new file mode 100644 index 0000000..8bf0652 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/subsystems/drive/DifferentialTrackerDriveBase.java @@ -0,0 +1,56 @@ +package com.spartronics4915.lib.subsystems.drive; + +import com.spartronics4915.lib.math.twodim.control.TrajectoryTracker.TrajectoryTrackerOutput; +import com.spartronics4915.lib.math.twodim.physics.DifferentialDrive; +import com.spartronics4915.lib.math.twodim.physics.DifferentialDrive.ChassisState; +import com.spartronics4915.lib.math.twodim.physics.DifferentialDrive.DriveDynamics; +import com.spartronics4915.lib.math.twodim.physics.DifferentialDrive.WheelState; + +/** + * This interface gives you a little more help than + * {@link TrajectoryTrackerDriveBase}. It makes it so that you don't have to use + * a SpartronicsSubsystem, but also so that you don't need to deal with all the + * kinematics/dynamics stuff yourself. + */ +public interface DifferentialTrackerDriveBase extends TrajectoryTrackerDriveBase { + + public DifferentialDrive getDifferentialDrive(); + + @Override + public default void setOutput(TrajectoryTrackerOutput outputMeters) { + setOutputFromDynamics( + outputMeters.getDifferentialDriveVelocity(), + outputMeters.getDifferentialDriveAcceleration() + ); + } + + public default void setOutputFromKinematics(ChassisState chassisVelocityMetersPerSecond) { + WheelState wheelVelocities = getDifferentialDrive().solveInverseKinematics(chassisVelocityMetersPerSecond); + WheelState feedForwardVoltages = getDifferentialDrive().getVoltagesFromkV(wheelVelocities); + + setOutput(wheelVelocities, feedForwardVoltages); + } + + public default void setOutputFromDynamics( + ChassisState chassisVelocityMetersPerSecond, + ChassisState chassisAccelerationMetersPerSecondSq + ) { + DriveDynamics dynamics = getDifferentialDrive().solveInverseDynamics(chassisVelocityMetersPerSecond, chassisAccelerationMetersPerSecondSq); + + setOutput(dynamics.wheelVelocity, dynamics.voltage); + } + + public default void setOutput( + WheelState wheelVelocitiesRadiansPerSecond, + WheelState wheelVoltages + ) { + getLeftMotor().setVelocity( + (wheelVelocitiesRadiansPerSecond.left * getDifferentialDrive().wheelRadius()), + wheelVoltages.left + ); + getRightMotor().setVelocity( + (wheelVelocitiesRadiansPerSecond.right * getDifferentialDrive().wheelRadius()), + wheelVoltages.right + ); + } +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/subsystems/drive/TrajectoryTrackerCommand.java b/src/main/java/com/spartronics4915/lib/subsystems/drive/TrajectoryTrackerCommand.java new file mode 100644 index 0000000..7e99d94 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/subsystems/drive/TrajectoryTrackerCommand.java @@ -0,0 +1,70 @@ +package com.spartronics4915.lib.subsystems.drive; + +import java.util.Set; +import java.util.function.Supplier; + +import com.spartronics4915.lib.math.twodim.control.TrajectoryTracker; +import com.spartronics4915.lib.math.twodim.geometry.Pose2dWithCurvature; +import com.spartronics4915.lib.math.twodim.trajectory.types.TimedTrajectory; +import com.spartronics4915.lib.subsystems.estimator.RobotStateMap; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; + +public class TrajectoryTrackerCommand implements Command { + + private final Subsystem mSubsystemToRequire; + private final TrajectoryTrackerDriveBase mDriveBase; + private final Supplier> mTrajectory; + private final TrajectoryTracker mTracker; + private final RobotStateMap mRobotPositionMap; + + public TrajectoryTrackerCommand(AbstractDrive driveSubsystem, TimedTrajectory trajectory, + TrajectoryTracker tracker, RobotStateMap robotPositionMap) { + this(driveSubsystem, driveSubsystem, trajectory, tracker, robotPositionMap); + } + + public TrajectoryTrackerCommand(Subsystem driveSubsystem, TrajectoryTrackerDriveBase driveBase, + TimedTrajectory trajectorySupplier, TrajectoryTracker tracker, + RobotStateMap robotPositionMap) { + this(driveSubsystem, driveBase, () -> trajectorySupplier, tracker, robotPositionMap); + } + + public TrajectoryTrackerCommand(Subsystem driveSubsystem, TrajectoryTrackerDriveBase driveBase, + Supplier> trajectorySupplier, TrajectoryTracker tracker, + RobotStateMap robotPositionMap) { + mSubsystemToRequire = driveSubsystem; + mDriveBase = driveBase; + mTrajectory = trajectorySupplier; + mTracker = tracker; + mRobotPositionMap = robotPositionMap; + } + + @Override + public void initialize() { + mTracker.reset(mTrajectory.get()); + } + + @Override + public void execute() { + var nextState = mTracker.nextState(mRobotPositionMap.getFieldToVehicle(Timer.getFPGATimestamp()), + Timer.getFPGATimestamp()); + mDriveBase.setOutput(nextState); + } + + @Override + public void end(boolean interrupted) { + mDriveBase.zeroOutputs(); + } + + @Override + public boolean isFinished() { + return mTracker.isFinished(); + } + + @Override + public Set getRequirements() { + return Set.of(mSubsystemToRequire); + } +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/subsystems/drive/TrajectoryTrackerDriveBase.java b/src/main/java/com/spartronics4915/lib/subsystems/drive/TrajectoryTrackerDriveBase.java new file mode 100644 index 0000000..c36ad57 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/subsystems/drive/TrajectoryTrackerDriveBase.java @@ -0,0 +1,21 @@ +package com.spartronics4915.lib.subsystems.drive; + +import com.spartronics4915.lib.hardware.motors.SpartronicsMotor; +import com.spartronics4915.lib.math.twodim.control.TrajectoryTracker.TrajectoryTrackerOutput; + +/** + * This is the minimal interface you need to implement if you want to use + * TrajectoryTrackerCommand. + */ +public interface TrajectoryTrackerDriveBase { + SpartronicsMotor getLeftMotor(); + + SpartronicsMotor getRightMotor(); + + void setOutput(TrajectoryTrackerOutput output); + + default void zeroOutputs() { + getLeftMotor().setNeutral(); + getRightMotor().setNeutral(); + } +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/subsystems/estimator/RobotStateEstimator.java b/src/main/java/com/spartronics4915/lib/subsystems/estimator/RobotStateEstimator.java new file mode 100644 index 0000000..ac33692 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/subsystems/estimator/RobotStateEstimator.java @@ -0,0 +1,184 @@ +package com.spartronics4915.lib.subsystems.estimator; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import com.spartronics4915.lib.math.twodim.geometry.Pose2d; +import com.spartronics4915.lib.math.twodim.geometry.Rotation2d; +import com.spartronics4915.lib.math.twodim.geometry.Twist2d; +import com.spartronics4915.lib.hardware.sensors.T265Camera; +import com.spartronics4915.lib.hardware.sensors.T265Camera.CameraUpdate; +import com.spartronics4915.lib.subsystems.SpartronicsSubsystem; +import com.spartronics4915.lib.subsystems.drive.AbstractDrive; +import com.spartronics4915.lib.util.Kinematics; +import com.spartronics4915.lib.util.Units; + +/** + * This loop keeps track of robot state whenever the robot is enabled. + * + * TODO: Split out to CameraRobotStateEstimator and DifferentialDriveRobotStateEstimator. + * Also TODO: Remove all the velocity stuff from robot states. + */ +public class RobotStateEstimator extends SpartronicsSubsystem +{ + + /** + * The SLAM camera/encoder RobotStateMap objects represent two views of the + * robot's current state (state is pose, velocity, and distance driven). + * + * All length units are meters. + */ + private RobotStateMap mEncoderStateMap = new RobotStateMap(); + private RobotStateMap mCameraStateMap = new RobotStateMap(); + + private AbstractDrive mDrive; + private Kinematics mKinematics; + private T265Camera mSLAMCamera; + + /** Meters */ + private double mLeftPrevDist = 0.0, mRightPrevDist = 0.0; + + public RobotStateEstimator(AbstractDrive driveSubsystem, Kinematics kinematics, T265Camera slamra) + { + mDrive = driveSubsystem; + mKinematics = kinematics; + mSLAMCamera = slamra; + + resetRobotStateMaps(); + + // Run this at 20 Hz + new Notifier(this::run).startPeriodic(1 / 100.0); + } + + public RobotStateMap getEncoderRobotStateMap() + { + return mEncoderStateMap; + } + + public RobotStateMap getCameraRobotStateMap() + { + return mCameraStateMap; + } + + public void resetRobotStateMaps() + { + resetRobotStateMaps(new Pose2d()); + } + + public synchronized void resetRobotStateMaps(Pose2d pose) + { + double time = Timer.getFPGATimestamp(); + mEncoderStateMap.reset(time, pose); + mCameraStateMap.reset(time, pose); + + mLeftPrevDist = mDrive.getLeftMotor().getEncoder().getPosition(); + mRightPrevDist = mDrive.getRightMotor().getEncoder().getPosition(); + + mSLAMCamera.setPose(pose); + // mDrive.setIMUHeading(pose.getRotation()); TODO: Put back when I get a real IMU + } + + @Override + public void periodic() + { + super.periodic(); + + final RobotStateMap.State estate = mEncoderStateMap.getLatestState(); + Pose2d epose = estate.pose; + SmartDashboard.putString("RobotState/encoderPose", + Units.metersToInches(epose.getTranslation().getX()) + + " " + Units.metersToInches(epose.getTranslation().getY()) + + " " + epose.getRotation().getDegrees()); + SmartDashboard.putNumber("RobotState/encoderVelocity", estate.predictedVelocity.dx); + + final RobotStateMap.State cstate = getCameraRobotStateMap().getLatestState(); + Pose2d cpose = cstate.pose; + SmartDashboard.putString("RobotState/pose", + Units.metersToInches(cpose.getTranslation().getX()) + + " " + Units.metersToInches(cpose.getTranslation().getY()) + + " " + cpose.getRotation().getDegrees()); + SmartDashboard.putNumber("RobotState/velocity", cstate.predictedVelocity.dx); + } + + public void stop() + { + mSLAMCamera.stop(); + } + + public void run() + { + if (DriverStation.getInstance().isDisabled()) + return; + + final RobotStateMap.State last = mEncoderStateMap.getLatestState(); + + /* + * There are two ways to measure current velocity: + * Method 1, integrationVelocity + * Look at the distance traveled since last measurement, consider + * current gyro heading rather than our stored state + * Divide by delta time to produce a velocity. Note that + * 254's implementation doesn't include time computations explicitly. + * In method 1, the implicit time is the time between samples which relates + * to the looper time interval. Thus: leftDelta is measured in + * meters/loopinterval. To the degree that the loop interval isn't a + * constant the result will be noisy. OTH: we can interpret this + * velocity as also a distance traveled since last loop. + */ + final Twist2d iVal; + synchronized (this) { + final double leftDist = mDrive.getLeftMotor().getEncoder().getPosition(); + final double rightDist = mDrive.getRightMotor().getEncoder().getPosition(); + final double leftDelta = leftDist - mLeftPrevDist; + final double rightDelta = rightDist - mRightPrevDist; + final Rotation2d heading = mDrive.getIMUHeading(); + mLeftPrevDist = leftDist; + mRightPrevDist = rightDist; + iVal = mKinematics.forwardKinematics(last.pose.getRotation(), leftDelta, rightDelta, heading); + } + + /* + * Method 2, 'predictedVelocity' + * Directly sample the current wheel velocities. Here, linear velocities + * are measured in meters/sec. Since the integration step below expects + * velocity to be measured in meters/loopinterval, this version of velocity + * can't be used directly. Moreover, the velocity we obtain from the wheel + * encoders is integrated over a different time interval than one + * loop-interval. It's not clear which estimation technique would deliver + * a better result. For visualization purposes velocity2 (in meters/sec) + * is in human-readable form. Also of note, this variant doesn't + * include the gyro heading in its calculation. + */ + final Twist2d pVal = mKinematics.forwardKinematics( + mDrive.getLeftMotor().getEncoder().getVelocity(), + mDrive.getRightMotor().getEncoder().getVelocity()); + + /* + * integrateForward: given a last state and a current velocity, + * estimate a new state (P2 = P1 + dPdt * dt) + */ + final Pose2d nextP = mKinematics.integrateForwardKinematics(last.pose, iVal); + + /* record the new state estimate */ + mEncoderStateMap.addObservations(Timer.getFPGATimestamp(), nextP, iVal, pVal); + + // We convert meters/loopinterval and radians/loopinterval to meters/sec and radians/sec + final double loopintervalToSeconds = 1 / (Timer.getFPGATimestamp() - last.timestamp); + final Twist2d normalizedIVal = iVal.scaled(loopintervalToSeconds); + + mSLAMCamera.sendOdometry(pVal); + } + + public void enable() + { + // Callback is called from a different thread... We avoid data races because RobotSteteMap is thread-safe + mSLAMCamera.stop(); + mSLAMCamera.start((CameraUpdate update) -> + { + mCameraStateMap.addObservations(Timer.getFPGATimestamp(), update.pose, update.velocity, new Twist2d()); + SmartDashboard.putString("RobotState/cameraConfidence", update.confidence.toString()); + }); + } +} diff --git a/src/main/java/com/spartronics4915/lib/subsystems/estimator/RobotStateMap.java b/src/main/java/com/spartronics4915/lib/subsystems/estimator/RobotStateMap.java new file mode 100644 index 0000000..8e1747f --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/subsystems/estimator/RobotStateMap.java @@ -0,0 +1,146 @@ +package com.spartronics4915.lib.subsystems.estimator; + +import com.spartronics4915.lib.math.twodim.geometry.Pose2d; +import com.spartronics4915.lib.math.twodim.geometry.Twist2d; +import com.spartronics4915.lib.util.Interpolable; +import com.spartronics4915.lib.util.InterpolatingDouble; +import com.spartronics4915.lib.util.InterpolatingTreeMap; + +public class RobotStateMap +{ + + private static final int kObservationBufferSize = 100; + + static public class State implements Interpolable + { + + public Pose2d pose; + public Twist2d integrationVelocity, predictedVelocity; + public double timestamp; + + public State() + { + this.pose = new Pose2d(); + this.integrationVelocity = new Twist2d(); + this.predictedVelocity = new Twist2d(); + this.timestamp = 0; + } + + public State(State other) + { + this.pose = other.pose; + this.integrationVelocity = other.integrationVelocity; + this.predictedVelocity = other.predictedVelocity; + this.timestamp = other.timestamp; + } + + public State(Pose2d pose, Twist2d iVel, Twist2d pVel, double ts) + { + this.pose = pose; + this.integrationVelocity = iVel; + this.predictedVelocity = pVel; + this.timestamp = ts; + } + + public State(Pose2d p, double ts) + { + this.pose = p; + this.integrationVelocity = new Twist2d(); + this.predictedVelocity = new Twist2d(); + this.timestamp = ts; + } + + @Override + public State interpolate(final State other, double pct) + { + if (pct <= 0) + return new State(this); + else if (pct >= 0) + return new State(other); + else + { + final State s = new State( + this.pose.interpolate(other.pose, pct), + this.integrationVelocity.interpolate(other.integrationVelocity, pct), + this.predictedVelocity.interpolate(other.predictedVelocity, pct), + this.timestamp + pct * (other.timestamp - this.timestamp)); + return s; + } + } + } + + private InterpolatingTreeMap mStateMap; + private double mDistanceDriven; + + public RobotStateMap() + { + reset(0, new Pose2d()); + } + + /** + * Resets the field to robot transform (robot's position on the field) + */ + public synchronized void reset(double startTime, Pose2d initialPose) + { + mStateMap = new InterpolatingTreeMap<>(kObservationBufferSize); + mStateMap.put(new InterpolatingDouble(startTime), + new State(initialPose, startTime)); + mDistanceDriven = 0.0; + } + + public synchronized void resetDistanceDriven() + { + mDistanceDriven = 0.0; + } + + public synchronized void addObservations(double timestamp, + Pose2d pose, + Twist2d velI, + Twist2d velP) + { + InterpolatingDouble ts = new InterpolatingDouble(timestamp); + mStateMap.put(ts, new State(pose, velI, velP, timestamp)); + mDistanceDriven += velI.dx; // Math.hypot(velocity.dx, velocity.dy); + // do we care about time here? + // no: if dx is measured in distance/loopinterval (loopinterval == 1) + // + // do we care about dy here? + // no: if velocity is in robot coords (no transverse motion expected) + // yes: if velocity is in field coords + + // The answer to both questions is no, btw. + } + + /** + * Returns the robot's state on the field at a certain time. Linearly + * interpolates between stored robot state to fill in the gaps. + */ + public synchronized State get(double ts) + { + return mStateMap.getInterpolated(new InterpolatingDouble(ts)); + } + + /** + * Returns the robot's position on the field at a certain time. Linearly + * interpolates between stored robot positions to fill in the gaps. + */ + public synchronized Pose2d getFieldToVehicle(double timestamp) + { + return this.get(timestamp).pose; + } + + public synchronized Pose2d getLatestFieldToVehicle() + { + return mStateMap.lastEntry().getValue().pose; + } + + public synchronized State getLatestState() + { + return mStateMap.lastEntry().getValue(); + } + + public synchronized double getDistanceDriven() + { + return mDistanceDriven; + } +} diff --git a/src/main/java/com/spartronics4915/lib/util/DeltaTime.java b/src/main/java/com/spartronics4915/lib/util/DeltaTime.java new file mode 100644 index 0000000..0bfc602 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/util/DeltaTime.java @@ -0,0 +1,40 @@ +package com.spartronics4915.lib.util; + +public class DeltaTime { + /** Seconds */ + private double mStartTime = -1; + /** Seconds */ + double mCurrentTime = -1; + /** Seconds */ + double mDeltaTime = 0.0; + + + public double getDeltaTimeSeconds() { + return mDeltaTime; + } + + public double getCurrentTimeSeconds() { + return mCurrentTime; + } + + public double getStartTime() { + if (mStartTime < 0) { + throw new RuntimeException("Can't get start time if DeltaTime has never been updated"); + } + return mStartTime; + } + + public double updateTime(double newTimeSeconds) { + if (mCurrentTime < 0) { + mDeltaTime = 0; + } else { + mDeltaTime = newTimeSeconds - mCurrentTime; + } + mCurrentTime = newTimeSeconds; + return mDeltaTime; + } + + public void reset() { + mCurrentTime = -1; + } +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/util/Interpolable.java b/src/main/java/com/spartronics4915/lib/util/Interpolable.java new file mode 100644 index 0000000..83c5472 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/util/Interpolable.java @@ -0,0 +1,27 @@ +package com.spartronics4915.lib.util; + +/** + * Interpolable is an interface used by an Interpolating Tree as the Value type. + * Given two end points and an + * interpolation parameter on [0, 1], it calculates a new Interpolable + * representing the interpolated value. + * + * @param The Type of Interpolable + * @see InterpolatingTreeMap + */ +public interface Interpolable +{ + + /** + * Interpolates between this value and an other value according to a given + * parameter. If x is 0, the method should + * return this value. If x is 1, the method should return the other value. If 0 + * < x < 1, the return value should be + * interpolated proportionally between the two. + * + * @param endValue The value of the upper bound + * @param x The requested value. Should be between 0 and 1. + * @return Interpolable The estimated average between the surrounding data + */ + public T interpolate(T endValue, double x); +} diff --git a/src/main/java/com/spartronics4915/lib/util/InterpolatingDouble.java b/src/main/java/com/spartronics4915/lib/util/InterpolatingDouble.java new file mode 100644 index 0000000..bb6f6ca --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/util/InterpolatingDouble.java @@ -0,0 +1,60 @@ +package com.spartronics4915.lib.util; + +/** + * A Double that can be interpolated using the InterpolatingTreeMap. + * + * @see InterpolatingTreeMap + */ +public class InterpolatingDouble implements Interpolable, InverseInterpolable, + Comparable +{ + + public Double value = 0.0; + + public InterpolatingDouble(Double val) + { + value = val; + } + + @Override + public InterpolatingDouble interpolate(InterpolatingDouble other, double x) + { + Double dydx = other.value - value; + Double searchY = dydx * x + value; + return new InterpolatingDouble(searchY); + } + + @Override + public double inverseInterpolate(InterpolatingDouble upper, InterpolatingDouble query) + { + double upper_to_lower = upper.value - value; + if (upper_to_lower <= 0) + { + return 0; + } + double query_to_lower = query.value - value; + if (query_to_lower <= 0) + { + return 0; + } + return query_to_lower / upper_to_lower; + } + + @Override + public int compareTo(InterpolatingDouble other) + { + if (other.value < value) + { + return 1; + } + else if (other.value > value) + { + return -1; + } + else + { + return 0; + } + } + +} diff --git a/src/main/java/com/spartronics4915/lib/util/InterpolatingLong.java b/src/main/java/com/spartronics4915/lib/util/InterpolatingLong.java new file mode 100644 index 0000000..9176e78 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/util/InterpolatingLong.java @@ -0,0 +1,59 @@ +package com.spartronics4915.lib.util; + +/** + * A Long that can be interpolated using the InterpolatingTreeMap. + * + * @see InterpolatingTreeMap + */ +public class InterpolatingLong implements Interpolable, InverseInterpolable, + Comparable +{ + + public Long value = 0L; + + public InterpolatingLong(Long val) + { + value = val; + } + + @Override + public InterpolatingLong interpolate(InterpolatingLong other, double x) + { + Long dydx = other.value - value; + Double searchY = dydx * x + value; + return new InterpolatingLong(searchY.longValue()); + } + + @Override + public double inverseInterpolate(InterpolatingLong upper, InterpolatingLong query) + { + long upper_to_lower = upper.value - value; + if (upper_to_lower <= 0) + { + return 0; + } + long query_to_lower = query.value - value; + if (query_to_lower <= 0) + { + return 0; + } + return query_to_lower / (double) upper_to_lower; + } + + @Override + public int compareTo(InterpolatingLong other) + { + if (other.value < value) + { + return 1; + } + else if (other.value > value) + { + return -1; + } + else + { + return 0; + } + } +} diff --git a/src/main/java/com/spartronics4915/lib/util/InterpolatingTreeMap.java b/src/main/java/com/spartronics4915/lib/util/InterpolatingTreeMap.java new file mode 100644 index 0000000..9c897e2 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/util/InterpolatingTreeMap.java @@ -0,0 +1,100 @@ +package com.spartronics4915.lib.util; + +import java.util.Map; +import java.util.TreeMap; + +/** + * Interpolating Tree Maps are used to get values at points that are not defined + * by making a guess from points that are + * defined. This uses linear interpolation. + * + * @param The type of the key (must implement InverseInterpolable) + * @param The type of the value (must implement Interpolable) + */ +public class InterpolatingTreeMap & Comparable, V extends Interpolable> + extends TreeMap +{ + + private static final long serialVersionUID = 8347275262778054124L; + + int max_; + + public InterpolatingTreeMap(int maximumSize) + { + max_ = maximumSize; + } + + public InterpolatingTreeMap() + { + this(0); + } + + /** + * Inserts a key value pair, and trims the tree if a max size is specified + * + * @param key Key for inserted data + * @param value Value for inserted data + * @return the value + */ + @Override + public V put(K key, V value) + { + if (max_ > 0 && max_ <= size()) + { + // "Prune" the tree if it is oversize + K first = firstKey(); + remove(first); + } + + super.put(key, value); + + return value; + } + + @Override + public void putAll(Map map) + { + Logger.error("Unimplemented Method"); + } + + /** + * @param key Lookup for a value (does not have to exist) + * @return V or null; V if it is Interpolable or exists, null if it is at a + * bound and cannot average + */ + public V getInterpolated(K key) + { + V gotval = get(key); + if (gotval == null) + { + /** Get surrounding keys for interpolation */ + K topBound = ceilingKey(key); + K bottomBound = floorKey(key); + + /** + * If attempting interpolation at ends of tree, return the nearest data point + */ + if (topBound == null && bottomBound == null) + { + return null; + } + else if (topBound == null) + { + return get(bottomBound); + } + else if (bottomBound == null) + { + return get(topBound); + } + + /** Get surrounding values for interpolation */ + V topElem = get(topBound); + V bottomElem = get(bottomBound); + return bottomElem.interpolate(topElem, bottomBound.inverseInterpolate(topBound, key)); + } + else + { + return gotval; + } + } +} diff --git a/src/main/java/com/spartronics4915/lib/util/InverseInterpolable.java b/src/main/java/com/spartronics4915/lib/util/InverseInterpolable.java new file mode 100644 index 0000000..35db4cf --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/util/InverseInterpolable.java @@ -0,0 +1,28 @@ +package com.spartronics4915.lib.util; + +/** + * InverseInterpolable is an interface used by an Interpolating Tree as the Key + * type. Given two endpoint keys and a + * third query key, an InverseInterpolable object can calculate the + * interpolation parameter of the query key on the + * interval [0, 1]. + * + * @param The Type of InverseInterpolable + * @see InterpolatingTreeMap + */ +public interface InverseInterpolable +{ + + /** + * Given this point (lower), a query point (query), and an upper point (upper), + * estimate how far (on [0, 1]) between + * 'lower' and 'upper' the query point lies. + * + * @param upper + * @param query + * @return The interpolation parameter on [0, 1] representing how far between + * this point and the upper point the + * query point lies. + */ + public double inverseInterpolate(T upper, T query); +} diff --git a/src/main/java/com/spartronics4915/lib/util/Kinematics.java b/src/main/java/com/spartronics4915/lib/util/Kinematics.java new file mode 100644 index 0000000..9a5f73f --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/util/Kinematics.java @@ -0,0 +1,58 @@ +package com.spartronics4915.lib.util; + +import com.spartronics4915.lib.math.twodim.geometry.Pose2d; +import com.spartronics4915.lib.math.twodim.geometry.Rotation2d; +import com.spartronics4915.lib.math.twodim.geometry.Twist2d; + +/** + * Provides forward and inverse kinematics equations for the robot modeling the + * wheelbase as a differential drive (with a corrective factor to account for + * skidding). + */ +public class Kinematics +{ + // TODO: Deduplicate kinematics once we add in path following stuff (i.e. remove this file) + + private final double mTrackWidthInches, mScrubFactor; + + public Kinematics(double trackWidthInches, double scrubFactor) + { + mTrackWidthInches = trackWidthInches; + mScrubFactor = scrubFactor; + } + + /** + * Forward kinematics using only encoders, rotation is implicit (less accurate + * than below, but useful for predicting motion) + */ + public Twist2d forwardKinematics(double leftWheelDelta, double rightWheelDelta) + { + double delta_rotation = (rightWheelDelta - leftWheelDelta) + / (mTrackWidthInches * mScrubFactor); + return forwardKinematics(leftWheelDelta, rightWheelDelta, delta_rotation); + } + + public Twist2d forwardKinematics(double leftWheelDelta, double rightWheelDelta, + double deltaRotationRads) + { + final double dx = (leftWheelDelta + rightWheelDelta) / 2.0; + return new Twist2d(dx, 0.0, Rotation2d.fromDegrees(deltaRotationRads)); + } + + public Twist2d forwardKinematics(Rotation2d prevHeading, double leftWheelDelta, double rightWheelDelta, + Rotation2d currentHeading) + { + final double dx = (leftWheelDelta + rightWheelDelta) / 2.0; + final double dy = 0.0; + return new Twist2d(dx, dy, prevHeading.inverse().rotateBy(currentHeading)); + } + + /** + * For convenience, integrate forward kinematics with a Twist2d and previous + * rotation. + */ + public Pose2d integrateForwardKinematics(Pose2d currentPose, Twist2d forwardKinematics) + { + return currentPose.transformBy(forwardKinematics.exp()); + } +} diff --git a/src/main/java/com/spartronics4915/lib/util/Logger.java b/src/main/java/com/spartronics4915/lib/util/Logger.java new file mode 100644 index 0000000..840a01c --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/util/Logger.java @@ -0,0 +1,159 @@ +package com.spartronics4915.lib.util; + +import java.io.FileWriter; +import java.io.IOException; +import java.io.PrintWriter; +import java.io.StringWriter; +import java.nio.file.Paths; +import java.text.DateFormat; +import java.text.SimpleDateFormat; +import java.util.Date; +import java.util.UUID; + +/** + * Tracks start-up and caught crash events, logging them to a file which dosn't + * roll over + */ +public class Logger +{ + + private static final UUID sRunInstanceUUID = UUID.randomUUID(); + public static int sVerbosity = 2; // 0: notices and above, 1: info and above, 2: all + private static final DateFormat sDateFormat = new SimpleDateFormat("hh:mm:ss"); + + public static void setVerbosity(String nm) + { + if (nm.equals("NOTICE")) + sVerbosity = 0; + else if (nm.equals("INFO")) + sVerbosity = 1; + else if (nm.equals("DEBUG")) + sVerbosity = 2; + else + error("Logger: unknown verbosity level:" + nm); + } + + public static void logRobotStartup() + { + notice("robot startup"); + } + + public static void logRobotConstruction() + { + notice("robot construction"); + } + + public static void logRobotInit() + { + notice("robot init"); + } + + public static void logTeleopInit() + { + notice("teleop init"); + } + + public static void logAutoInit() + { + notice("auto init"); + } + + public static void logDisabledInit() + { + notice("disabled init"); + } + + public static void logTestInit() + { + notice("test init"); + } + + public static void logThrowableCrash(Throwable throwable) + { + logMarker("Exception", throwable); + } + + public static void logThrowableCrash(String msg, Throwable throwable) + { + logMarker("ERROR " + msg, throwable); + } + + public static void exception(Exception e) + { + StringWriter sw = new StringWriter(); + e.printStackTrace(new PrintWriter(sw)); + logMarker("EXCEPT " + e.getMessage() + + " trace:\n" + sw.toString()); + } + + public static void error(String m) + { + logMarker("ERROR " + m); + } + + public static void warning(String m) + { + logMarker("WARNING " + m); + } + + public static void notice(String m) + { + logMarker("NOTICE " + m); + } + + public static void info(String m) + { + if (sVerbosity > 0) + { + printMarker("INFO " + m); + } + } + + public static void debug(String m) + { + if (sVerbosity > 1) + { + printMarker("DEBUG " + m); + } + } + + private static String getTimeStamp() + { + Date now = new Date(); + String nowstr = sDateFormat.format(now) + " "; + return nowstr; + } + + private static void logMarker(String mark) + { + logMarker(mark, null); + } + + private static void printMarker(String mark) + { + System.out.println(mark); + } + + private static void logMarker(String mark, Throwable nullableException) + { + printMarker(mark); + if (nullableException != null) + nullableException.printStackTrace(); + try (PrintWriter writer = new PrintWriter(new FileWriter(Paths.get(System.getProperty("user.home"), "crash_tracking.txt").toString(), true))) + { + writer.print(sRunInstanceUUID.toString()); + writer.print(", "); + writer.print(mark); + if (nullableException != null) + { + writer.print(", "); + nullableException.printStackTrace(writer); + } + writer.println(); + } + catch (IOException e) + { + e.printStackTrace(); + } + } +} diff --git a/src/main/java/com/spartronics4915/lib/util/Units.java b/src/main/java/com/spartronics4915/lib/util/Units.java new file mode 100644 index 0000000..1e87a8a --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/util/Units.java @@ -0,0 +1,55 @@ +package com.spartronics4915.lib.util; + +public class Units +{ + + public static double rpmToRadsPerSec(double rpm) + { + return rpm * 2.0 * Math.PI / 60.0; + } + + public static double radsPerSecToRpm(double radsPerSec) + { + return radsPerSec * 60.0 / (2.0 * Math.PI); + } + + public static double inchesToMeters(double inches) + { + return inches * 0.0254; + } + + public static double millimetersToInches(double millimeters) + { + return metersToInches(millimeters / 1000); + } + + public static double inchesToMillimeters(double inches) + { + return inchesToMeters(inches) * 1000; + } + + public static double metersToInches(double meters) + { + return meters / 0.0254; + } + + public static double feetToMeters(double feet) + { + return inchesToMeters(feet * 12.0); + } + + public static double metersToFeet(double meters) + { + return metersToInches(meters) / 12.0; + } + + public static double degreesToRadians(double degrees) + { + return Math.toRadians(degrees); + } + + public static double radiansToDegrees(double radians) + { + return Math.toDegrees(radians); + } +} diff --git a/src/test/java/com/spartronics4915/lib/hardware/sensors/TestT265Camera.java b/src/test/java/com/spartronics4915/lib/hardware/sensors/TestT265Camera.java new file mode 100644 index 0000000..d3a644f --- /dev/null +++ b/src/test/java/com/spartronics4915/lib/hardware/sensors/TestT265Camera.java @@ -0,0 +1,100 @@ +package com.spartronics4915.lib.hardware.sensors; + +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.junit.jupiter.api.Assertions.fail; + +import java.nio.file.Path; +import java.nio.file.Paths; + +import com.spartronics4915.lib.math.twodim.geometry.Pose2d; +import com.spartronics4915.lib.math.twodim.geometry.Rotation2d; +import com.spartronics4915.lib.math.twodim.geometry.Twist2d; +import com.spartronics4915.lib.hardware.sensors.T265Camera.CameraUpdate; +import com.spartronics4915.lib.util.Logger; + +import org.junit.jupiter.api.Tag; +import org.junit.jupiter.api.Test; + +public class TestT265Camera +{ + + private boolean mDataRecieved = false; + private final Object mLock = new Object(); + + @Tag("hardwareDependant") + @Test + public void testNewCamera() throws InterruptedException + { + // This one is a little hard to unit test because we haven't simulated the hardware + // We mostly just make sure that we can get through this sequence without throwing an exception + // The rest is just a few sanity checks + + T265Camera cam = null; + try + { + cam = new T265Camera(new Pose2d(), 0); + + // Just make sure this doesn't throw + cam.sendOdometry(new Twist2d(0, 0, new Rotation2d())); + + cam.start((CameraUpdate update) -> + { + synchronized (mLock) + { + mDataRecieved = true; + } + System.out.println("Got pose with confidence " + update.pose); + }); + Logger.debug( + "Waiting 5 seconds to recieve data... Move the camera around in the path of the shape of a cross for best results. This will not work unless you get to High confidence."); + Thread.sleep(5000); + cam.stop(); + synchronized (mLock) + { + assertTrue(mDataRecieved, "No pose data was recieved after 5 seconds... Try moving the camera?"); + } + + Logger.debug("Got pose data, exporting relocalization map to java.io.tmpdir..."); + Path mapPath = Paths.get(System.getProperty("java.io.tmpdir"), "map.bin").toAbsolutePath(); + cam.exportRelocalizationMap(mapPath.toString()); + + if (mapPath.toFile().length() <= 0) + fail("Relocalization map file length was 0"); + Logger.debug("Successfuly saved relocalization map, we will now try to import it"); + + cam.free(); + + // Try making a new camera and importing the map + cam = new T265Camera(new Pose2d(), 0f, mapPath.toString()); + + Logger.debug("Map imported without errors!"); + } + finally + { + if (cam != null) + cam.free(); + } + } + + @Tag("hardwareDependant") + @Test + public void testErrorChecking() + { + T265Camera cam = null; + try + { + cam = new T265Camera(new Pose2d(), 0); + cam.start((CameraUpdate unused) -> {}); + + final T265Camera camTemp = cam; + assertThrows(RuntimeException.class, () -> camTemp.start((CameraUpdate unused) -> {})); + } + finally + { + if (cam != null) + cam.free(); + } + } + +} diff --git a/src/test/java/com/spartronics4915/lib/math/twodim/lidar/TestObjectFinder.java b/src/test/java/com/spartronics4915/lib/math/twodim/lidar/TestObjectFinder.java new file mode 100644 index 0000000..f3b28e6 --- /dev/null +++ b/src/test/java/com/spartronics4915/lib/math/twodim/lidar/TestObjectFinder.java @@ -0,0 +1,181 @@ +package com.spartronics4915.lib.math.twodim.lidar; + +import java.awt.Canvas; +import java.awt.Color; +import java.awt.Dimension; +import java.awt.Frame; +import java.awt.Graphics; +import java.awt.event.KeyEvent; +import java.awt.event.KeyListener; +import java.util.ArrayList; +import java.util.List; + +import com.spartronics4915.lib.hardware.sensors.RPLidarA1; +import com.spartronics4915.lib.math.twodim.geometry.Pose2d; +import com.spartronics4915.lib.math.twodim.geometry.Rotation2d; +import com.spartronics4915.lib.math.twodim.geometry.Translation2d; +import com.spartronics4915.lib.subsystems.estimator.RobotStateMap; +import com.spartronics4915.lib.util.Units; + +import org.junit.jupiter.api.Tag; +import org.junit.jupiter.api.Test; +import org.opencv.core.Mat; + +import edu.wpi.cscore.CameraServerJNI; +import edu.wpi.first.wpilibj.Timer; + +public class TestObjectFinder { + + private List mPointcloud = new ArrayList<>(); + private long mLastResetTime = 0; + + private int mEdgeDetectorValue = 1; + private int mNumVotesNeeded = 5; + + private final TargetTracker mTargetTracker = new TargetTracker(); + + @Test + public void testGeom() { + var poseOne = new Pose2d(0, 0, Rotation2d.fromDegrees(0)); + var poseTwo = new Pose2d(1, 1, Rotation2d.fromDegrees(90)); + System.out.println(poseOne.distance(poseTwo) + ", " + ((1.0/4.0) * 2 * Math.PI) + ", " + poseTwo.inverse().transformBy(poseOne).log() + ", " + poseOne.getTranslation().getDistance(poseTwo.getTranslation())); + // for (int i = 0; i < 20; i++) { + // var poseOne = new Pose2d(0, 0, Rotation2d.fromDegrees(0)); + // var poseTwo = new Pose2d(1, 0, Rotation2d.fromDegrees(180)); + // System.out.println(poseTwo.inFrameReferenceOf(poseOne) + ", " + poseTwo.inFrameReferenceOf(poseOne).log()); + // } + } + + @Test + public void testCoords() { + var point = new Pose2d(0, 1, new Rotation2d()); + + var fieldToVehicle = new Pose2d(0, 0, Rotation2d.fromDegrees(180)); + var vehicleToLidar = new Pose2d(1, -1, Rotation2d.fromDegrees(-90)); + + System.out.println(fieldToVehicle.transformBy(vehicleToLidar).transformBy(point)); + } + + @Tag("hardwareDependant") + @Test + public void interactivePointcloudTest() { + final ObjectFinder finder = new ObjectFinder(0.01); + final double circleRadiusMeters = Units.inchesToMeters(3.5); + + var pointCloudCanvas = new Canvas() { + @Override + public void paint(Graphics g) { + super.paint(g); + + try { + synchronized (mPointcloud) { + if (mPointcloud.size() <= 0) { + return; + } + + mPointcloud.forEach((p) -> drawPoint(p, 255, g)); + + int circleDiameterCentimeters = (int) Math.round(circleRadiusMeters * 100.0 * 2.0); + // var centers = finder.findSquares(pointcloud, new Translation2d(), 0.28, mNumVotesNeeded, 3, 0.3); + var centers = finder.findCircles(mPointcloud, circleRadiusMeters, mNumVotesNeeded, 3); + var center = mTargetTracker.update(centers); + if (center == null) { + System.out.println("No target found"); + return; + } + // for (Translation2d center : centers) { + System.out.println(center); + g.setColor(Color.BLUE); + g.drawOval(toScreenCoordinates(center.getX() - circleRadiusMeters, true), toScreenCoordinates(center.getY() - circleRadiusMeters, false), circleDiameterCentimeters, circleDiameterCentimeters); + // break; + // } + + g.drawString("Edge: " + mEdgeDetectorValue + ", Votes: " + mNumVotesNeeded, 10, 10); + } + } catch (Exception e) { + e.printStackTrace(); + } + } + + // Screen coordinates correspond to centimeters + private int toScreenCoordinates(double coordMeters, boolean isX) { + Dimension size = this.getSize(); + + coordMeters *= 100; + coordMeters += (isX ? size.getWidth() : size.getHeight()) / 2; + + return (int) Math.round(coordMeters); + } + + private void drawPoint(Translation2d point, int quality, Graphics g) { + g.setColor(new Color(255 - quality, 0, 0)); + g.drawOval(toScreenCoordinates(point.getX(), true), toScreenCoordinates(point.getY(), false), 1, 1); + } + }; + pointCloudCanvas.setSize(6000, 6000); + pointCloudCanvas.setBackground(Color.WHITE); + + var frame = new Frame(); + frame.add(pointCloudCanvas); + frame.setSize(6000, 6000); + frame.setVisible(true); + + frame.addKeyListener(new KeyListener() { + + @Override + public void keyPressed(KeyEvent k) { + } + + @Override + public void keyReleased(KeyEvent k) { + System.out.println(k.getKeyChar()); + if (k.getKeyChar() == '-' || k.getKeyChar() == '+') { + double toAdd = (k.getKeyChar() == '+' ? 1 : -1); + + if (k.isAltDown()) { + mEdgeDetectorValue += toAdd; + } else if (k.isControlDown()) { + mNumVotesNeeded += toAdd; + } + } else if (k.getKeyChar() == 'q') { + System.exit(0); + } + } + + @Override + public void keyTyped(KeyEvent k) { + } + + }); + + RPLidarA1 lidar = new RPLidarA1(); + lidar.setCallback((List pointcloud) -> { + synchronized (mPointcloud) { + if (System.currentTimeMillis() - mLastResetTime > 0) { + mPointcloud = pointcloud; + mLastResetTime = System.currentTimeMillis(); + } else { + mPointcloud.addAll(pointcloud); + } + } + }, new RobotStateMap(), new Pose2d(Units.inchesToMeters(-5.5), Units.inchesToMeters(-14), Rotation2d.fromDegrees(180))); + lidar.start(); + + Runtime.getRuntime().addShutdownHook(new Thread(() -> { + lidar.stop(); + System.out.println("Graceful shutdown complete"); + })); + + while (true) { + try { + Thread.sleep(500); + synchronized (mPointcloud) { + pointCloudCanvas.repaint(); + } + } catch (InterruptedException e) { + e.printStackTrace(); + } + } + } + +} \ No newline at end of file diff --git a/src/test/java/com/spartronics4915/lib/math/twodim/spline/CubicHermiteSplineTest.java b/src/test/java/com/spartronics4915/lib/math/twodim/spline/CubicHermiteSplineTest.java new file mode 100644 index 0000000..25ab2e9 --- /dev/null +++ b/src/test/java/com/spartronics4915/lib/math/twodim/spline/CubicHermiteSplineTest.java @@ -0,0 +1,163 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package com.spartronics4915.lib.math.twodim.spline; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +import org.junit.jupiter.api.Test; + +import com.spartronics4915.lib.math.twodim.geometry.Pose2d; +import com.spartronics4915.lib.math.twodim.geometry.Pose2dWithCurvature; +import com.spartronics4915.lib.math.twodim.geometry.Rotation2d; +import com.spartronics4915.lib.math.twodim.geometry.Translation2d; +import com.spartronics4915.lib.math.twodim.spline.SplineParameterizer.MalformedSplineException; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; + + +class CubicHermiteSplineTest { + private static final double kMaxDx = 0.127; + private static final double kMaxDy = 0.00127; + private static final double kMaxDtheta = 0.0872; + + @SuppressWarnings({"ParameterName", "PMD.UnusedLocalVariable"}) + private void run(Pose2d a, List waypoints, Pose2d b) { + // Start the timer. + var start = System.nanoTime(); + + // Generate and parameterize the spline. + var controlVectors = + SplineHelper.getCubicControlVectorsFromWaypoints(a, + waypoints.toArray(new Translation2d[0]), b); + var splines + = SplineHelper.getCubicSplinesFromControlVectors( + controlVectors[0], waypoints.toArray(new Translation2d[0]), controlVectors[1]); + + var poses = new ArrayList(); + + poses.add(splines[0].getPoint(0.0)); + + for (var spline : splines) { + poses.addAll(SplineParameterizer.parameterize(spline)); + } + + // End the timer. + var end = System.nanoTime(); + + // Calculate the duration (used when benchmarking) + var durationMicroseconds = (end - start) / 1000.0; + + for (int i = 0; i < poses.size() - 1; i++) { + var p0 = poses.get(i); + var p1 = poses.get(i + 1); + + // Make sure the twist is under the tolerance defined by the Spline class. + var twist = p1.getPose().inFrameReferenceOf(p0.getPose()).log(); + assertAll( + () -> assertTrue(Math.abs(twist.dx) < kMaxDx), + () -> assertTrue(Math.abs(twist.dy) < kMaxDy), + () -> assertTrue(Math.abs(twist.dtheta.getRadians()) < kMaxDtheta) + ); + } + + // Check first point + assertAll( + () -> assertEquals(a.getTranslation().getX(), + poses.get(0).getPose().getTranslation().getX(), 1E-9), + () -> assertEquals(a.getTranslation().getY(), + poses.get(0).getPose().getTranslation().getY(), 1E-9), + () -> assertEquals(a.getRotation().getRadians(), + poses.get(0).getPose().getRotation().getRadians(), 1E-9) + ); + + // Check interior waypoints + boolean interiorsGood = true; + for (var waypoint : waypoints) { + boolean found = false; + for (var state : poses) { + if (waypoint.getDistance(state.getPose().getTranslation()) == 0) { + found = true; + } + } + interiorsGood &= found; + } + + assertTrue(interiorsGood); + + // Check last point + assertAll( + () -> assertEquals(b.getTranslation().getX(), + poses.get(poses.size() - 1).getPose().getTranslation().getX(), 1E-9), + () -> assertEquals(b.getTranslation().getY(), + poses.get(poses.size() - 1).getPose().getTranslation().getY(), 1E-9), + () -> assertEquals(b.getRotation().getRadians(), + poses.get(poses.size() - 1).getPose().getRotation().getRadians(), 1E-9) + ); + } + + @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert") + @Test + void testStraightLine() { + run(new Pose2d(), new ArrayList<>(), new Pose2d(3, 0, new Rotation2d())); + } + + @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert") + @Test + void testSCurve() { + var start = new Pose2d(0, 0, Rotation2d.fromDegrees(90.0)); + ArrayList waypoints = new ArrayList<>(); + waypoints.add(new Translation2d(1, 1)); + waypoints.add(new Translation2d(2, -1)); + var end = new Pose2d(3, 0, Rotation2d.fromDegrees(90.0)); + + run(start, waypoints, end); + } + + @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert") + @Test + void testOneInterior() { + var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0)); + ArrayList waypoints = new ArrayList<>(); + waypoints.add(new Translation2d(2.0, 0.0)); + var end = new Pose2d(4, 0, Rotation2d.fromDegrees(0.0)); + + run(start, waypoints, end); + } + + @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert") + @Test + void testWindyPath() { + final var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0)); + final ArrayList waypoints = new ArrayList<>(); + waypoints.add(new Translation2d(0.5, 0.5)); + waypoints.add(new Translation2d(0.5, 0.5)); + waypoints.add(new Translation2d(1.0, 0.0)); + waypoints.add(new Translation2d(1.5, 0.5)); + waypoints.add(new Translation2d(2.0, 0.0)); + waypoints.add(new Translation2d(2.5, 0.5)); + final var end = new Pose2d(3.0, 0.0, Rotation2d.fromDegrees(0.0)); + + run(start, waypoints, end); + } + + @Test + void testMalformed() { + assertThrows(MalformedSplineException.class, () -> run( + new Pose2d(0, 0, Rotation2d.fromDegrees(0)), + new ArrayList<>(), new Pose2d(1, 0, Rotation2d.fromDegrees(180)))); + assertThrows(MalformedSplineException.class, () -> run( + new Pose2d(10, 10, Rotation2d.fromDegrees(90)), + Arrays.asList(new Translation2d(10, 10.5)), + new Pose2d(10, 11, Rotation2d.fromDegrees(-90)))); + } +} diff --git a/src/test/java/com/spartronics4915/lib/math/twodim/spline/QuinticHermiteSplineTest.java b/src/test/java/com/spartronics4915/lib/math/twodim/spline/QuinticHermiteSplineTest.java new file mode 100644 index 0000000..08b8642 --- /dev/null +++ b/src/test/java/com/spartronics4915/lib/math/twodim/spline/QuinticHermiteSplineTest.java @@ -0,0 +1,108 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package com.spartronics4915.lib.math.twodim.spline; + +import java.util.List; + +import org.junit.jupiter.api.Test; + +import com.spartronics4915.lib.math.twodim.geometry.Pose2d; +import com.spartronics4915.lib.math.twodim.geometry.Rotation2d; +import com.spartronics4915.lib.math.twodim.spline.SplineParameterizer.MalformedSplineException; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; + +class QuinticHermiteSplineTest { + private static final double kMaxDx = 0.127; + private static final double kMaxDy = 0.00127; + private static final double kMaxDtheta = 0.0872; + + @SuppressWarnings({ "ParameterName", "PMD.UnusedLocalVariable" }) + private void run(Pose2d a, Pose2d b) { + // Start the timer. + var start = System.nanoTime(); + + // Generate and parameterize the spline. + var controlVectors = SplineHelper.getQuinticControlVectorsFromWaypoints(List.of(a, b)); + var spline = SplineHelper.getQuinticSplinesFromControlVectors( + controlVectors.toArray(new Spline.ControlVector[0]))[0]; + var poses = SplineParameterizer.parameterize(spline); + + // End the timer. + var end = System.nanoTime(); + + // Calculate the duration (used when benchmarking) + var durationMicroseconds = (end - start) / 1000.0; + + for (int i = 0; i < poses.size() - 1; i++) { + var p0 = poses.get(i); + var p1 = poses.get(i + 1); + + // Make sure the twist is under the tolerance defined by the Spline class. + var twist = p1.getPose().inFrameReferenceOf(p0.getPose()).log(); + assertAll( + () -> assertTrue(Math.abs(twist.dx) < kMaxDx), + () -> assertTrue(Math.abs(twist.dy) < kMaxDy), + () -> assertTrue(Math.abs(twist.dtheta.getRadians()) < kMaxDtheta)); + } + + // Check first point + assertAll( + () -> assertEquals( + a.getTranslation().getX(), poses.get(0).getPose().getTranslation().getX(), 1E-9), + () -> assertEquals( + a.getTranslation().getY(), poses.get(0).getPose().getTranslation().getY(), 1E-9), + () -> assertEquals( + a.getRotation().getRadians(), poses.get(0).getPose().getRotation().getRadians(), + 1E-9)); + + // Check last point + assertAll( + () -> assertEquals(b.getTranslation().getX(), poses.get(poses.size() - 1) + .getPose().getTranslation().getX(), 1E-9), + () -> assertEquals(b.getTranslation().getY(), poses.get(poses.size() - 1) + .getPose().getTranslation().getY(), 1E-9), + () -> assertEquals(b.getRotation().getRadians(), + poses.get(poses.size() - 1).getPose().getRotation().getRadians(), 1E-9)); + } + + @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert") + @Test + void testStraightLine() { + run(new Pose2d(), new Pose2d(3, 0, new Rotation2d())); + } + + @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert") + @Test + void testSimpleSCurve() { + run(new Pose2d(), new Pose2d(1, 1, new Rotation2d())); + } + + @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert") + @Test + void testSquiggly() { + run( + new Pose2d(0, 0, Rotation2d.fromDegrees(90)), + new Pose2d(-1, 0, Rotation2d.fromDegrees(90))); + } + + @Test + void testMalformed() { + assertThrows(MalformedSplineException.class, + () -> run( + new Pose2d(0, 0, Rotation2d.fromDegrees(0)), + new Pose2d(1, 0, Rotation2d.fromDegrees(180)))); + assertThrows(MalformedSplineException.class, + () -> run( + new Pose2d(10, 10, Rotation2d.fromDegrees(90)), + new Pose2d(10, 11, Rotation2d.fromDegrees(-90)))); + } +} diff --git a/src/test/java/com/spartronics4915/lib/math/twodim/trajectory/PathFinderTest.java b/src/test/java/com/spartronics4915/lib/math/twodim/trajectory/PathFinderTest.java new file mode 100644 index 0000000..4d08eca --- /dev/null +++ b/src/test/java/com/spartronics4915/lib/math/twodim/trajectory/PathFinderTest.java @@ -0,0 +1,125 @@ +package com.spartronics4915.lib.math.twodim.trajectory; + +import static com.spartronics4915.lib.util.Units.feetToMeters; +import static com.spartronics4915.lib.util.Units.inchesToMeters; +import static org.junit.jupiter.api.Assertions.assertNotEquals; + +import java.text.DecimalFormat; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; +import java.util.Set; +import java.util.stream.Collectors; + +import com.spartronics4915.lib.math.twodim.geometry.Pose2d; +import com.spartronics4915.lib.math.twodim.geometry.Pose2dWithCurvature; +import com.spartronics4915.lib.math.twodim.geometry.Rectangle2d; +import com.spartronics4915.lib.math.twodim.geometry.Rotation2d; +import com.spartronics4915.lib.math.twodim.geometry.Translation2d; +import com.spartronics4915.lib.math.twodim.trajectory.constraints.CentripetalAccelerationConstraint; +import com.spartronics4915.lib.math.twodim.trajectory.types.TimedTrajectory.TimedState; +import com.spartronics4915.lib.math.twodim.trajectory.types.Trajectory.TrajectorySamplePoint; + +import org.junit.jupiter.api.Test; +import org.knowm.xchart.XYChartBuilder; +import org.knowm.xchart.SwingWrapper; +import org.knowm.xchart.XYChart; + +import java.awt.Color; +import java.awt.Font; + +class PathFinderTest { + @Test + public void testPathFinder() { + var robotSize = inchesToMeters(33.0); + var pathFinder = new PathFinder(robotSize, + new Rectangle2d(new Translation2d(inchesToMeters(140.0), inchesToMeters(85.25)), + new Translation2d(inchesToMeters(196.0), inchesToMeters(238.75))), + new Rectangle2d(new Translation2d(inchesToMeters(261.47), inchesToMeters(95.25)), + new Translation2d(inchesToMeters(196.0), inchesToMeters(238.75))), + new Rectangle2d(new Translation2d(inchesToMeters(196.0), inchesToMeters(85.25)), + new Translation2d(inchesToMeters(211.0), inchesToMeters(238.75)))); + + double start = System.currentTimeMillis(); + + var startPose = new Pose2d(feetToMeters(1.54), feetToMeters(23.234167), Rotation2d.fromDegrees(0)); + var endPose = new Pose2d(feetToMeters(23.7), feetToMeters(27 - 20.2), Rotation2d.fromDegrees(0)); + var interiors = pathFinder.findInteriorWaypoints(startPose, endPose, + new Rectangle2d(new Translation2d(feetToMeters(0), feetToMeters(0)), + new Translation2d(feetToMeters(10), feetToMeters(10)))); + assertNotEquals(interiors, null); + var traj = TrajectoryGenerator.defaultTrajectoryGenerator.generateTrajectory(startPose, interiors, endPose, + Arrays.asList(new CentripetalAccelerationConstraint(4.0)), 0, 0, feetToMeters(10), feetToMeters(4), + false); + + double end = System.currentTimeMillis(); + System.out.println(end - start); + + List>> refList = new ArrayList<>(); + final var dt = 0.1; + var t = 0.0; + while (t < traj.getTotalTime()) { + refList.add(traj.sample(t)); + t += dt; + } + + var fm = new DecimalFormat("#.###").format(traj.getTotalTime()); + + var chart = new XYChartBuilder().width(1800).height(1520).title(fm.toString() + " seconds.").xAxisTitle("X") + .yAxisTitle("Y").build(); + + chart.getStyler().setMarkerSize(8); + chart.getStyler().setSeriesColors(new Color[] { Color.ORANGE, new Color(151, 60, 67) }); + + chart.getStyler().setChartTitleFont(new Font("Kanit", 1, 40)); + chart.getStyler().setChartTitlePadding(15); + + chart.getStyler().setXAxisMin(1.0); + chart.getStyler().setXAxisMax(26.0); + chart.getStyler().setYAxisMin(1.0); + chart.getStyler().setYAxisMax(26.0); + + chart.getStyler().setChartFontColor(Color.WHITE); + chart.getStyler().setAxisTickLabelsColor(Color.WHITE); + + chart.getStyler().setLegendBackgroundColor(Color.GRAY); + + chart.getStyler().setPlotGridLinesVisible(true); + chart.getStyler().setLegendVisible(true); + + chart.getStyler().setPlotGridLinesColor(Color.GRAY); + chart.getStyler().setChartBackgroundColor(Color.DARK_GRAY); + chart.getStyler().setPlotBackgroundColor(Color.DARK_GRAY); + + chart.addSeries("Trajectory", + refList.stream().map((it) -> it.state.state.getTranslation().getX()).collect(Collectors.toList()), + refList.stream().map((it) -> it.state.state.getTranslation().getY()).collect(Collectors.toList())); + + var restricted = Arrays.asList( + new Rectangle2d(new Translation2d(feetToMeters(0), feetToMeters(0)), + new Translation2d(feetToMeters(10), feetToMeters(10))), + new Rectangle2d(new Translation2d(inchesToMeters(140.0), inchesToMeters(85.25)), + new Translation2d(inchesToMeters(196.0), inchesToMeters(238.75))), + new Rectangle2d(new Translation2d(inchesToMeters(261.47), inchesToMeters(95.25)), + new Translation2d(inchesToMeters(196.0), inchesToMeters(238.75))), + new Rectangle2d(new Translation2d(inchesToMeters(196.0), inchesToMeters(85.25)), + new Translation2d(inchesToMeters(211.0), inchesToMeters(238.75)))); + chart.addSeries("Restricted Areas", + restricted.stream() + .flatMap((it) -> Arrays.asList(it.getBottomLeft().getX(), it.getTopLeft().getX(), it.getBottomRight().getX(), + it.getTopRight().getX()).stream()) + .collect(Collectors.toList()), + restricted.stream() + .flatMap((it) -> Arrays.asList(it.getBottomLeft().getY(), it.getTopLeft().getY(), + it.getBottomRight().getY(), it.getTopRight().getY()).stream()) + .collect(Collectors.toList())); + + new SwingWrapper(chart).displayChart(); + try { + Thread.sleep(10000000); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } +} \ No newline at end of file diff --git a/src/test/java/com/spartronics4915/lib/math/twodim/trajectory/TrajectoryGeneratorTest.java b/src/test/java/com/spartronics4915/lib/math/twodim/trajectory/TrajectoryGeneratorTest.java new file mode 100644 index 0000000..ac203bc --- /dev/null +++ b/src/test/java/com/spartronics4915/lib/math/twodim/trajectory/TrajectoryGeneratorTest.java @@ -0,0 +1,75 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package com.spartronics4915.lib.math.twodim.trajectory; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +import org.junit.jupiter.api.Test; + +import com.spartronics4915.lib.math.twodim.geometry.Pose2d; +import com.spartronics4915.lib.math.twodim.geometry.Pose2dWithCurvature; +import com.spartronics4915.lib.math.twodim.geometry.Rotation2d; +import com.spartronics4915.lib.math.twodim.geometry.Translation2d; +import com.spartronics4915.lib.math.twodim.trajectory.constraints.TimingConstraint; +import com.spartronics4915.lib.math.twodim.trajectory.types.TimedTrajectory; + +import static com.spartronics4915.lib.util.Units.feetToMeters; +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +class TrajectoryGeneratorTest { + static TimedTrajectory getTrajectory(List> constraints) { + final double maxVelocity = feetToMeters(12.0); + final double maxAccel = feetToMeters(12); + + // 2018 cross scale auto waypoints. + var sideStart = new Pose2d(feetToMeters(1.54), feetToMeters(23.23), Rotation2d.fromDegrees(-180)); + var crossScale = new Pose2d(feetToMeters(23.7), feetToMeters(6.8), Rotation2d.fromDegrees(-160)); + + var waypoints = new ArrayList(); + waypoints.add(sideStart); + waypoints.add( + sideStart.transformBy(new Pose2d(new Translation2d(feetToMeters(-13), feetToMeters(0)), new Rotation2d()))); + waypoints.add(sideStart + .transformBy(new Pose2d(new Translation2d(feetToMeters(-19.5), feetToMeters(5)), Rotation2d.fromDegrees(-90)))); + waypoints.add(crossScale); + + return TrajectoryGenerator.defaultTrajectoryGenerator.generateTrajectory(waypoints, constraints, 0, 0, maxVelocity, + maxAccel, true); + } + + @Test + @SuppressWarnings("LocalVariableName") + void testGenerationAndConstraints() { + TimedTrajectory trajectory = getTrajectory(new ArrayList<>()); + + double duration = trajectory.getTotalTime(); + double t = 0.0; + double dt = 0.02; + + while (t < duration) { + var point = trajectory.sample(t); + t += dt; + assertAll(() -> assertTrue(Math.abs(point.state.velocity) < feetToMeters(12.0) + 0.05), + () -> assertTrue(Math.abs(point.state.acceleration) < feetToMeters(12.0) + 0.05)); + } + } + + @Test + void testMalformedTrajectory() { + var traj = TrajectoryGenerator.defaultTrajectoryGenerator.generateTrajectory( + Arrays.asList(new Pose2d(0, 0, Rotation2d.fromDegrees(0)), new Pose2d(1, 0, Rotation2d.fromDegrees(180))), + Arrays.asList(), 0, 0, 12, 12, false); + + assertEquals(traj.size(), 1); + assertEquals(traj.getTotalTime(), 0); + } +} diff --git a/src/test/java/com/spartronics4915/lib/math/twodim/trajectory/TrajectoryIntegrationTest.java b/src/test/java/com/spartronics4915/lib/math/twodim/trajectory/TrajectoryIntegrationTest.java new file mode 100644 index 0000000..4d4536d --- /dev/null +++ b/src/test/java/com/spartronics4915/lib/math/twodim/trajectory/TrajectoryIntegrationTest.java @@ -0,0 +1,160 @@ +package com.spartronics4915.lib.math.twodim.trajectory; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import java.text.DecimalFormat; +import java.util.Arrays; +import java.util.List; + +import com.spartronics4915.lib.math.twodim.control.FeedForwardTracker; +import com.spartronics4915.lib.math.twodim.control.RamseteTracker; +import com.spartronics4915.lib.math.twodim.geometry.Pose2d; +import com.spartronics4915.lib.math.twodim.geometry.Pose2dWithCurvature; +import com.spartronics4915.lib.math.twodim.geometry.Rotation2d; +import com.spartronics4915.lib.math.twodim.geometry.Translation2d; +import com.spartronics4915.lib.math.twodim.physics.DCMotorTransmission; +import com.spartronics4915.lib.math.twodim.physics.DifferentialDrive; +import com.spartronics4915.lib.math.twodim.trajectory.constraints.DifferentialDriveDynamicsConstraint; +import com.spartronics4915.lib.math.twodim.trajectory.types.TimedTrajectory; +import com.spartronics4915.lib.math.twodim.trajectory.types.TimedTrajectory.TimedState; +import com.spartronics4915.lib.math.twodim.trajectory.types.Trajectory.TrajectorySamplePoint; +import com.spartronics4915.lib.util.Units; + +import org.junit.jupiter.api.Test; + +public class TrajectoryIntegrationTest { + + @Test + public void testPathFollower() { + final double kDriveTrackScrubFactor = 1.063; // determine with auto mode + final double kDriveTrackWidth = Units.inchesToMeters(23.75 * kDriveTrackScrubFactor); + final double kDriveWheelDiameter = Units.inchesToMeters(6); + final double kDriveWheelRadius = kDriveWheelDiameter / 2.0; + + final double kDriveRightVIntercept = 0.7714; // V + final double kDriveRightKv = 0.1920; // V per rad/s + final double kDriveRightKa = 0.0533; // V per rad/s^2 + + final double kDriveLeftVIntercept = 0.7939; // V + final double kDriveLeftKv = 0.1849; // V per rad/s + final double kDriveLeftKa = 0.0350; // V per rad/s^2 + + final double kRobotLinearInertia = 27.93; // kg (robot's mass) + final double kRobotAngularInertia = 1.7419; // kg m^2 (use the moi auto mode) + final double kRobotAngularDrag = 12.0; // N*m / (rad/sec) + + final DifferentialDrive kDifferentialDrive = new DifferentialDrive( + kRobotLinearInertia, kRobotAngularInertia, kRobotAngularDrag, + kDriveWheelRadius, kDriveTrackWidth, + new DCMotorTransmission( + kDriveWheelRadius, kRobotLinearInertia, kDriveLeftVIntercept, kDriveLeftKv, kDriveLeftKa + ), + new DCMotorTransmission( + kDriveWheelRadius, kRobotLinearInertia, kDriveRightVIntercept, kDriveRightKv, kDriveRightKa + ) + ); + + var traj = TrajectoryGenerator.defaultTrajectoryGenerator.generateTrajectory( + Arrays.asList(new Pose2d(), new Pose2d(1, 0, new Rotation2d())), + Arrays.asList(new DifferentialDriveDynamicsConstraint(kDifferentialDrive, 10.0)), + 0.0, 0.0, 3.65, 1.83, false + ); + + var tracker = new RamseteTracker(2.0, 0.7); + tracker.reset(traj); + + var iterator = new TimedTrajectory.TimedIterator<>(traj); + double distanceMeters = 0.0; + while (!tracker.isFinished()) { + tracker.getReferencePoint(); + var out = tracker.nextState(iterator.getCurrentSample().state.state.getPose(), iterator.getProgress()); + + System.out.println(out.linearVelocity); + + distanceMeters += out.linearVelocity * 0.1; + + iterator.advance(0.1); + } + System.out.println("Distance = " + distanceMeters); + } + + @Test + public void testSplineTrajectoryGenerator() { + // Specify desired waypoints. + List waypoints = Arrays.asList( + new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0)), + new Pose2d(Units.inchesToMeters(36.0), Units.inchesToMeters(0.0), Rotation2d.fromDegrees(0.0)), + new Pose2d(Units.inchesToMeters(60.0), Units.inchesToMeters(100), Rotation2d.fromDegrees(0.0)), + new Pose2d(Units.inchesToMeters(160.0), Units.inchesToMeters(100.0), Rotation2d.fromDegrees(0.0)), + new Pose2d(Units.inchesToMeters(200.0), Units.inchesToMeters(70), Rotation2d.fromDegrees(45.0)) + ); + + // Create a differential drive. + final double kRobotMassKg = 60.0; + final double kRobotAngularInertia = 80.0; + final double kRobotAngularDrag = 0.0; + final double kWheelRadius = Units.inchesToMeters(2.0); + DCMotorTransmission transmission = new DCMotorTransmission(1.0 / 0.143, + (kWheelRadius * kWheelRadius * kRobotMassKg / 2.0) / 0.02, 0.8); + DifferentialDrive drive = new DifferentialDrive(kRobotMassKg, kRobotAngularInertia, kRobotAngularDrag, + kWheelRadius, Units.inchesToMeters(26.0 / 2.0), transmission, transmission); + + // Create the constraint that the robot must be able to traverse the trajectory + // without ever applying more + // than 10V. + var voltageConstraint = new DifferentialDriveDynamicsConstraint(drive, 10.0); + + // Generate the timed trajectory. + var trajectory = TrajectoryGenerator.defaultTrajectoryGenerator.generateTrajectory( + waypoints, Arrays.asList(voltageConstraint), 0.0, 0.0, Units.inchesToMeters(12 * 14), + Units.inchesToMeters(12 * 10), false + ); + + for (int i = 1; i < trajectory.size(); i++) { + final var prev = trajectory.getPoint(i - 1); + final var next = trajectory.getPoint(i); + final double dt = next.state.time - prev.state.time; + + assertEquals(prev.state.acceleration, (next.state.velocity - prev.state.velocity) / dt, 1E-9); + assertEquals(next.state.velocity, prev.state.velocity + prev.state.acceleration * dt, 1E-9); + assertEquals(next.state.distance(prev.state), + prev.state.velocity * dt + 0.5 * prev.state.acceleration * dt * dt, 1E-9); + } + + // "Follow" the trajectory. + final double kDt = 0.01; + boolean first = true; + var it = new TimedTrajectory.TimedIterator<>(trajectory); + while (!it.isDone()) { + TrajectorySamplePoint> sample; + if (first) { + sample = it.getCurrentSample(); + first = false; + } else { + sample = it.advance(kDt); + } + final TimedState state = sample.state; + + // This is designed to be exactly the same as 2019-DeepSpace output, as a comparison + // XXX: dkds doesn't match... Numerical stability? + // final DecimalFormat fmt = new DecimalFormat("#0.000"); + // System.out.println( + // new Pose2dWithCurvature( + // new Translation2d( + // Units.metersToInches(state.state.getTranslation().x()), Units.metersToInches(state.state.getTranslation().y()) + // ), + // state.state.getRotation(), + // // Units are inverse + // Units.inchesToMeters(state.state.getCurvature()), + // Units.inchesToMeters(state.state.getDCurvatureDs()) + // ).toString() + + // ", t: " + fmt.format(state.time) + + // ", v: " + fmt.format(Units.metersToInches(state.velocity)) + + // ", a: " + fmt.format(Units.metersToInches(state.acceleration)) + // ); + + System.out.println(state.velocity + "," + state.acceleration); + } + } + +} \ No newline at end of file diff --git a/src/test/java/com/spartronics4915/lib/subsystems/TestSubsystem.java b/src/test/java/com/spartronics4915/lib/subsystems/TestSubsystem.java new file mode 100644 index 0000000..4da6f89 --- /dev/null +++ b/src/test/java/com/spartronics4915/lib/subsystems/TestSubsystem.java @@ -0,0 +1,26 @@ +package com.spartronics4915.lib.subsystems; + +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import org.junit.jupiter.api.Test; + +public class TestSubsystem extends SpartronicsSubsystem +{ + @Test + public void testInitChecks() + { + assertFalse(super.isInitialized()); + super.logInitialized(true); + assertTrue(super.isInitialized()); + super.logInitialized(false); + assertFalse(super.isInitialized()); + } + + @Test + public void testGetName() + { + assertTrue(super.getClassName().equals("TestSubsystem")); + } + +} \ No newline at end of file diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index f8d42a4..8b25850 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix.json", "name": "CTRE-Phoenix", - "version": "5.17.3", + "version": "5.17.6", "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ "http://devsite.ctr-electronics.com/maven/release/" @@ -11,19 +11,19 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.17.3" + "version": "5.17.6" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.17.3" + "version": "5.17.6" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.17.3", + "version": "5.17.6", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -35,7 +35,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "diagnostics", - "version": "5.17.3", + "version": "5.17.6", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -47,7 +47,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "canutils", - "version": "5.17.3", + "version": "5.17.6", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -58,7 +58,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "platform-stub", - "version": "5.17.3", + "version": "5.17.6", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -69,7 +69,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "core", - "version": "5.17.3", + "version": "5.17.6", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -83,7 +83,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.17.3", + "version": "5.17.6", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": false, @@ -97,7 +97,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.17.3", + "version": "5.17.6", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": false, @@ -111,7 +111,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.17.3", + "version": "5.17.6", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": false, @@ -125,7 +125,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "diagnostics", - "version": "5.17.3", + "version": "5.17.6", "libName": "CTRE_PhoenixDiagnostics", "headerClassifier": "headers", "sharedLibrary": false, @@ -139,7 +139,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "canutils", - "version": "5.17.3", + "version": "5.17.6", "libName": "CTRE_PhoenixCanutils", "headerClassifier": "headers", "sharedLibrary": false, @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "platform-stub", - "version": "5.17.3", + "version": "5.17.6", "libName": "CTRE_PhoenixPlatform", "headerClassifier": "headers", "sharedLibrary": false, @@ -165,7 +165,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "core", - "version": "5.17.3", + "version": "5.17.6", "libName": "CTRE_PhoenixCore", "headerClassifier": "headers", "sharedLibrary": false, From ae7da559cd6850433d4fab209d1c3e6d9faf30d4 Mon Sep 17 00:00:00 2001 From: Dana Batali Date: Wed, 22 Jan 2020 09:48:10 -0800 Subject: [PATCH 2/2] Disable tests for now. './gradlew build' fails on tests on windows - due to pipe-closure error 'gradle build' doesn't fail. there are still a few tests that fail when operated correctly. --- build.gradle | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/build.gradle b/build.gradle index df79ce9..b626f3f 100644 --- a/build.gradle +++ b/build.gradle @@ -12,7 +12,6 @@ sourceCompatibility = JavaVersion.VERSION_11 targetCompatibility = JavaVersion.VERSION_11 def ROBOT_MAIN_CLASS = "com.spartronics4915.frc2020.Main" -def includeDesktopSupport = true // Set this to true to enable desktop support. // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project EmbeddedTools. @@ -45,10 +44,8 @@ deploy { } } -repositories { - mavenCentral() // needed for JUnit - maven { url 'https://jitpack.io' } -} +// Set this to true to enable desktop support. +def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 4. @@ -69,8 +66,8 @@ dependencies { implementation("com.fazecast:jSerialComm:2.4.1") testImplementation("org.knowm.xchart:xchart:3.2.2") - //testImplementation 'junit:junit:4.12' testImplementation("org.junit.jupiter:junit-jupiter-api:5.3.2") + // testImplementation 'junit:junit:4.12' testRuntime("org.junit.jupiter:junit-jupiter-engine:5.3.2") // Enable simulation gui support. Must check the box in vscode to enable support @@ -102,6 +99,7 @@ jar { // we could exclude source by subdir (see 2019-Deepspace/build.gradle) } +/* test { useJUnitPlatform { // Some tests only work if you have the right stuff plugged in @@ -114,4 +112,5 @@ test { events "PASSED", "SKIPPED", "FAILED" exceptionFormat "full" } -} \ No newline at end of file +} +*/ \ No newline at end of file