diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c9534bb2..0877b4ce 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -139,7 +139,7 @@ private void configureBindings() { .onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); operatorController .getRightBumper() - .onTrue(drivetrain.runOnce(() -> drivetrain.resetPose(new Pose2d()))); + .onTrue(drivetrain.runOnce(() -> drivetrain.resetPose(Pose2d.kZero))); } private double deadband(double value, double deadband) { diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 693c7aeb..218cb51f 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -38,6 +38,8 @@ * be used in command-based projects. */ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem { + private final CustomOdometry m_odometry_custom; + private static final double kSimLoopPeriod = 0.005; // 5 ms private Notifier m_simNotifier = null; private double m_lastSimTime; @@ -162,6 +164,9 @@ public CommandSwerveDrivetrain( startSimThread(); } m_applyFieldSpeedsOrbit = generateSwerveSetpointConfig(); + m_odometry_custom = + new CustomOdometry(new CustomInverseKinematics(getModuleLocations()), getPigeon2()); + registerTelemetry(m_odometry_custom::odometryFunction); } /** @@ -185,6 +190,9 @@ public CommandSwerveDrivetrain( } m_applyFieldSpeedsOrbit = generateSwerveSetpointConfig(); + m_odometry_custom = + new CustomOdometry(new CustomInverseKinematics(getModuleLocations()), getPigeon2()); + registerTelemetry(m_odometry_custom::odometryFunction); } /** @@ -217,6 +225,9 @@ public CommandSwerveDrivetrain( } m_applyFieldSpeedsOrbit = generateSwerveSetpointConfig(); + m_odometry_custom = + new CustomOdometry(new CustomInverseKinematics(getModuleLocations()), getPigeon2()); + registerTelemetry(m_odometry_custom::odometryFunction); } private FieldOrientedOrbitSwerveRequest generateSwerveSetpointConfig() { @@ -401,6 +412,18 @@ public void periodic() { Logger.recordOutput("Drive/actualChassisSpeeds", getState().Speeds); Logger.recordOutput("Drive/pose", getState().Pose); + + Logger.recordOutput("Drive/customPose", m_odometry_custom.m_currentPose); + + Logger.recordOutput("Drive/slippingModule", m_odometry_custom.m_maxSlippingWheelIndex); + + Logger.recordOutput("Drive/slippingModuleAmount", m_odometry_custom.m_maxSlippingAmount); + Logger.recordOutput("Drive/slippingModuleRatio", m_odometry_custom.m_maxSlippingRatio); + + Logger.recordOutput("Drive/customOdometryTime", m_odometry_custom.m_lastOdometryTime); + + Logger.recordOutput("Drive/isSlipping", m_odometry_custom.m_isSlipping); + Logger.recordOutput("Drive/isMultiSlipping", m_odometry_custom.m_isMultiwheelSlipping); } private void startSimThread() { diff --git a/src/main/java/frc/robot/subsystems/CustomInverseKinematics.java b/src/main/java/frc/robot/subsystems/CustomInverseKinematics.java new file mode 100644 index 00000000..8eb8713a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/CustomInverseKinematics.java @@ -0,0 +1,222 @@ +package frc.robot.subsystems; + +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.MathUsageId; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import java.util.Arrays; +import org.ejml.simple.SimpleMatrix; + +public class CustomInverseKinematics { + public class CustomKinematicsSolution { + public Twist2d estimatedTwist; + } + + private final SimpleMatrix m_inverseKinematics; + private final SimpleMatrix[] m_inverseKinematicsMissing; + private final SimpleMatrix m_forwardKinematics; + private final SimpleMatrix[] m_forwardKinematicsMissing; + + private final int m_numModules; + private final Translation2d[] m_modules; + private Rotation2d[] m_moduleHeadings; + private Translation2d m_prevCoR = Translation2d.kZero; + + /** + * Constructs a swerve drive kinematics object. This takes in a variable number of module + * locations as Translation2d objects. The order in which you pass in the module locations is + * the same order that you will receive the module states when performing inverse kinematics. It + * is also expected that you pass in the module states in the same order when calling the + * forward kinematics methods. + * + * @param moduleTranslationsMeters The locations of the modules relative to the physical center + * of the robot. + */ + public CustomInverseKinematics(Translation2d... moduleTranslationsMeters) { + if (moduleTranslationsMeters.length < 2) { + throw new IllegalArgumentException("A swerve drive requires at least two modules"); + } + m_numModules = moduleTranslationsMeters.length; + m_modules = Arrays.copyOf(moduleTranslationsMeters, m_numModules); + m_moduleHeadings = new Rotation2d[m_numModules]; + Arrays.fill(m_moduleHeadings, Rotation2d.kZero); + m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3); + + for (int i = 0; i < m_numModules; i++) { + m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY()); + m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX()); + } + + m_forwardKinematics = m_inverseKinematics.pseudoInverse(); + + m_inverseKinematicsMissing = new SimpleMatrix[m_numModules]; + m_forwardKinematicsMissing = new SimpleMatrix[m_numModules]; + + for (int i = 0; i < m_numModules; i++) { + m_inverseKinematicsMissing[i] = + m_inverseKinematics + .rows(0, i * 2) + .concatRows(m_inverseKinematics.rows(i * 2 + 2, m_numModules * 2)); + var x = m_inverseKinematicsMissing[i].getNumRows(); + var y = m_inverseKinematicsMissing[i].getNumCols(); + m_forwardKinematicsMissing[i] = m_inverseKinematicsMissing[i].pseudoInverse(); + } + + MathSharedStore.reportUsage(MathUsageId.kKinematics_SwerveDrive, 1); + } + + public SimpleMatrix toModuleVelocities(SwerveModuleState... moduleStates) { + var moduleStatesMatrix = new SimpleMatrix(moduleStates.length * 2, 1); + + for (int i = 0; i < m_numModules; i++) { + var module = moduleStates[i]; + moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.getCos()); + moduleStatesMatrix.set(i * 2 + 1, module.speedMetersPerSecond * module.angle.getSin()); + } + + return moduleStatesMatrix; + } + + public SimpleMatrix toModuleVelocities(ChassisSpeeds chassisSpeeds) { + var chassisSpeedsVector = new SimpleMatrix(3, 1); + chassisSpeedsVector.set(0, 0, chassisSpeeds.vxMetersPerSecond); + chassisSpeedsVector.set(1, 0, chassisSpeeds.vyMetersPerSecond); + chassisSpeedsVector.set(2, 0, chassisSpeeds.omegaRadiansPerSecond); + + return m_inverseKinematics.mult(chassisSpeedsVector); + } + + public ChassisSpeeds toChassisSpeeds(int missingModule, SwerveModuleState... moduleStates) { + if (moduleStates.length != m_numModules) { + throw new IllegalArgumentException( + "Number of modules is not consistent with number of module locations provided in " + + "constructor"); + } + var moduleStatesMatrix = new SimpleMatrix((m_numModules - 1) * 2, 1); + + for (int i = 0; i < m_numModules - 1; i++) { + int j = i; + if (i >= missingModule) { + j++; + } + var module = moduleStates[j]; + moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.getCos()); + moduleStatesMatrix.set(i * 2 + 1, module.speedMetersPerSecond * module.angle.getSin()); + } + + var chassisSpeedsVector = + m_forwardKinematicsMissing[missingModule].mult(moduleStatesMatrix); + return new ChassisSpeeds( + chassisSpeedsVector.get(0, 0), + chassisSpeedsVector.get(1, 0), + chassisSpeedsVector.get(2, 0)); + } + + public ChassisSpeeds toChassisSpeeds(SwerveModuleState... moduleStates) { + if (moduleStates.length != m_numModules) { + throw new IllegalArgumentException( + "Number of modules is not consistent with number of module locations provided in " + + "constructor"); + } + var moduleStatesMatrix = new SimpleMatrix(m_numModules * 2, 1); + + for (int i = 0; i < m_numModules; i++) { + var module = moduleStates[i]; + moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.getCos()); + moduleStatesMatrix.set(i * 2 + 1, module.speedMetersPerSecond * module.angle.getSin()); + } + + var chassisSpeedsVector = m_forwardKinematics.mult(moduleStatesMatrix); + return new ChassisSpeeds( + chassisSpeedsVector.get(0, 0), + chassisSpeedsVector.get(1, 0), + chassisSpeedsVector.get(2, 0)); + } + + /** + * Performs forward kinematics to return the resulting chassis state from the given module + * states. This method is often used for odometry -- determining the robot's position on the + * field using data from the real-world speed and angle of each module on the robot. + * + * @param moduleDeltas The latest change in position of the modules (as a SwerveModulePosition + * type) as measured from respective encoders and gyros. The order of the swerve module + * states should be same as passed into the constructor of this class. + * @return The resulting Twist2d. + */ + public Twist2d toTwist2d(SwerveModulePosition... moduleDeltas) { + if (moduleDeltas.length != m_numModules) { + throw new IllegalArgumentException( + "Number of modules is not consistent with number of module locations provided in " + + "constructor"); + } + var moduleDeltaMatrix = new SimpleMatrix(m_numModules * 2, 1); + + for (int i = 0; i < m_numModules; i++) { + var module = moduleDeltas[i]; + moduleDeltaMatrix.set(i * 2, 0, module.distanceMeters * module.angle.getCos()); + moduleDeltaMatrix.set(i * 2 + 1, module.distanceMeters * module.angle.getSin()); + } + + var chassisDeltaVector = m_forwardKinematics.mult(moduleDeltaMatrix); + return new Twist2d( + chassisDeltaVector.get(0, 0), + chassisDeltaVector.get(1, 0), + chassisDeltaVector.get(2, 0)); + } + + public Twist2d toTwist2d(int missingModule, SwerveModulePosition... moduleDeltas) { + if (moduleDeltas.length != m_numModules) { + throw new IllegalArgumentException( + "Number of modules is not consistent with number of module locations provided in " + + "constructor"); + } + var moduleDeltaMatrix = new SimpleMatrix((m_numModules - 1) * 2, 1); + + for (int i = 0; i < (m_numModules - 1); i++) { + int j = i; + if (i >= missingModule) { + j++; + } + var module = moduleDeltas[j]; + moduleDeltaMatrix.set(i * 2, 0, module.distanceMeters * module.angle.getCos()); + moduleDeltaMatrix.set(i * 2 + 1, module.distanceMeters * module.angle.getSin()); + } + + var chassisDeltaVector = m_forwardKinematicsMissing[missingModule].mult(moduleDeltaMatrix); + return new Twist2d( + chassisDeltaVector.get(0, 0), + chassisDeltaVector.get(1, 0), + chassisDeltaVector.get(2, 0)); + } + + public Twist2d toTwist2d(SwerveModulePosition[] start, SwerveModulePosition[] end) { + if (start.length != end.length) { + throw new IllegalArgumentException("Inconsistent number of modules!"); + } + var newPositions = new SwerveModulePosition[start.length]; + for (int i = 0; i < start.length; i++) { + newPositions[i] = + new SwerveModulePosition( + end[i].distanceMeters - start[i].distanceMeters, end[i].angle); + } + return toTwist2d(newPositions); + } + + public Twist2d toTwist2d( + int missingModule, SwerveModulePosition[] start, SwerveModulePosition[] end) { + if (start.length != end.length) { + throw new IllegalArgumentException("Inconsistent number of modules!"); + } + var newPositions = new SwerveModulePosition[start.length]; + for (int i = 0; i < start.length; i++) { + newPositions[i] = + new SwerveModulePosition( + end[i].distanceMeters - start[i].distanceMeters, end[i].angle); + } + return toTwist2d(missingModule, newPositions); + } +} diff --git a/src/main/java/frc/robot/subsystems/CustomOdometry.java b/src/main/java/frc/robot/subsystems/CustomOdometry.java new file mode 100644 index 00000000..1f12a566 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/CustomOdometry.java @@ -0,0 +1,171 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.RadiansPerSecond; + +import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj.Timer; +import java.util.function.DoubleSupplier; +import org.ejml.simple.SimpleMatrix; + +public class CustomOdometry { + public Pose2d m_currentPose = new Pose2d(); + private SwerveModulePosition[] m_lastSwerveModulePositionsCustomOdom = + new SwerveModulePosition[4]; + private final CustomInverseKinematics m_kinematics_custom; + private final DoubleSupplier m_gyroAngularVelocitySupplier; + final double m_slippingThreshold = 0.02; + + public boolean m_isSlipping = false; + public boolean m_isMultiwheelSlipping = false; + + public SimpleMatrix m_derotatedWheelVelocities = new SimpleMatrix(8, 1); + public int m_maxSlippingWheelIndex = -1; + + public double m_lastOdometryTime = 0; + + public double m_maxSlippingAmount = 0; + public double m_maxSlippingRatio = 1; + + public CustomOdometry( + CustomInverseKinematics kinematics, DoubleSupplier gyroAngularVelocitySupplier) { + m_kinematics_custom = kinematics; + m_gyroAngularVelocitySupplier = gyroAngularVelocitySupplier; + } + + public CustomOdometry(CustomInverseKinematics kinematics, Pigeon2 pigeon2) { + this( + kinematics, + () -> pigeon2.getAngularVelocityZWorld().refresh().getValue().in(RadiansPerSecond)); + } + + public void odometryFunction(SwerveDrivetrain.SwerveDriveState state) { + try { + double start = Timer.getTimestamp(); + + SimpleMatrix wheel_velocities_no_rot = + m_kinematics_custom + .toModuleVelocities(state.ModuleStates) + .minus( + m_kinematics_custom.toModuleVelocities( + new ChassisSpeeds( + 0, + 0, + m_gyroAngularVelocitySupplier.getAsDouble()))); + + double xVelocity = 0; + double yVelocity = 0; + for (int i = 0; i < 4; i++) { + xVelocity += wheel_velocities_no_rot.get(i * 2, 0); + yVelocity += wheel_velocities_no_rot.get(i * 2 + 1, 0); + } + xVelocity /= 4; + yVelocity /= 4; + + double[] wheelErrors = new double[4]; + + double maxWheelError = 0; + int maxWheelErrorIndex = 0; + + for (int i = 0; i < 4; i++) { + wheelErrors[i] = + Math.hypot( + wheel_velocities_no_rot.get(i * 2, 0) - xVelocity, + wheel_velocities_no_rot.get(i * 2 + 1, 0) - yVelocity); + if (wheelErrors[i] > maxWheelError) { + maxWheelError = wheelErrors[i]; + maxWheelErrorIndex = i; + } + } + + boolean slipping; + + if (maxWheelError > m_slippingThreshold) { + slipping = true; + } else { + slipping = false; + } + + m_maxSlippingAmount = maxWheelError; + + m_maxSlippingRatio = maxWheelError / Math.hypot(xVelocity, yVelocity); + + m_maxSlippingWheelIndex = maxWheelErrorIndex; + + m_isSlipping = slipping; + + boolean multiWheelSlipping = false; + + if (slipping) { + xVelocity = 0; + yVelocity = 0; + for (int i = 0; i < 3; i++) { + int j = i; + if (i >= maxWheelErrorIndex) { + j++; + } + xVelocity += wheel_velocities_no_rot.get(j * 2, 0); + yVelocity += wheel_velocities_no_rot.get(j * 2 + 1, 0); + } + xVelocity /= 3; + yVelocity /= 3; + + maxWheelError = 0; + + for (int i = 0; i < 3; i++) { + int j = i; + if (i >= maxWheelErrorIndex) { + j++; + } + wheelErrors[j] = + Math.hypot( + wheel_velocities_no_rot.get(j * 2, 0) - xVelocity, + wheel_velocities_no_rot.get(j * 2 + 1, 0) - yVelocity); + if (wheelErrors[j] > maxWheelError) { + maxWheelError = wheelErrors[j]; + } + } + + if (maxWheelError > m_slippingThreshold) { + multiWheelSlipping = true; + } + } + + m_isMultiwheelSlipping = multiWheelSlipping; + + Twist2d poseChange; + double translationStds; + double rotationStds; + + if (m_lastSwerveModulePositionsCustomOdom[0] == null) { + m_lastSwerveModulePositionsCustomOdom = state.ModulePositions.clone(); + } + + if (slipping & !multiWheelSlipping) { + poseChange = + m_kinematics_custom.toTwist2d( + maxWheelErrorIndex, + m_lastSwerveModulePositionsCustomOdom, + state.ModulePositions); + } else { + poseChange = + m_kinematics_custom.toTwist2d( + m_lastSwerveModulePositionsCustomOdom, state.ModulePositions); + } + + m_currentPose = m_currentPose.exp(poseChange); + + m_lastSwerveModulePositionsCustomOdom = state.ModulePositions.clone(); + + double end = Timer.getTimestamp(); + m_lastOdometryTime = end - start; + } catch (Exception e) { + System.out.println("Error in odometryFunction: " + e); + e.printStackTrace(); + } + } +}