diff --git a/build.gradle b/build.gradle index 06056c1..b626f3f 100644 --- a/build.gradle +++ b/build.gradle @@ -1,74 +1,116 @@ -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" + +// 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' + } + } +} + +// 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" + + 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("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 + // 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,