From d2d122f38dc22a72e8b5535d745c537679106583 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 5 Nov 2024 18:12:32 -0800 Subject: [PATCH] remove second-order swerve control (drive accel and steer velocity); it doesn't seem to make much difference, and it's complicated. --- .../team100/lib/experiments/Experiment.java | 4 - .../lib/motion/drivetrain/SwerveLocal.java | 11 +- .../SwerveDriveKinematics100.java | 132 +---------- .../kinodynamics/SwerveKinodynamics.java | 50 ---- .../drivetrain/module/SwerveModule100.java | 13 +- .../swerve/AsymSwerveSetpointGenerator.java | 43 +--- .../lib/swerve/SteeringRateLimiter.java | 22 +- .../org/team100/lib/swerve/SwerveUtil.java | 40 ---- .../SwerveDriveKinematics100Test.java | 221 ------------------ .../lib/swerve/SteeringRateLimiterTest.java | 4 - 10 files changed, 14 insertions(+), 526 deletions(-) diff --git a/lib/src/main/java/org/team100/lib/experiments/Experiment.java b/lib/src/main/java/org/team100/lib/experiments/Experiment.java index f709d4694..5d2e0ee99 100644 --- a/lib/src/main/java/org/team100/lib/experiments/Experiment.java +++ b/lib/src/main/java/org/team100/lib/experiments/Experiment.java @@ -35,10 +35,6 @@ public enum Experiment { * Use softer vision update gains */ AvoidVisionJitter, - /** - * - */ - UseSecondDerivativeSwerve, /** * Filter rotational output to remove oscillation */ diff --git a/lib/src/main/java/org/team100/lib/motion/drivetrain/SwerveLocal.java b/lib/src/main/java/org/team100/lib/motion/drivetrain/SwerveLocal.java index 0a4236ba9..d4967769f 100644 --- a/lib/src/main/java/org/team100/lib/motion/drivetrain/SwerveLocal.java +++ b/lib/src/main/java/org/team100/lib/motion/drivetrain/SwerveLocal.java @@ -226,17 +226,10 @@ public void resetSetpoint(SwerveSetpoint setpoint) { public void setChassisSpeedsNormally(ChassisSpeeds speeds, double gyroRateRad_S) { // Informs SwerveDriveKinematics of the module states. - SwerveModuleStates states; - if (Experiments.instance.enabled(Experiment.UseSecondDerivativeSwerve)) { - states = m_swerveKinodynamics.toSwerveModuleStates(speeds, m_prevSetpoint.getChassisSpeeds(), - m_prevSetpoint.getModuleStates(), gyroRateRad_S); - } else { - states = m_swerveKinodynamics.toSwerveModuleStates(speeds, gyroRateRad_S); - // System.out.println(speeds); - m_log_chassis_speed.log(() -> speeds); - } + SwerveModuleStates states = m_swerveKinodynamics.toSwerveModuleStates(speeds, gyroRateRad_S); setModuleStates(states); m_prevSetpoint = new SwerveSetpoint(speeds, states); + m_log_chassis_speed.log(() -> speeds); } /** Updates visualization. */ diff --git a/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveDriveKinematics100.java b/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveDriveKinematics100.java index b037da603..450361ed2 100644 --- a/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveDriveKinematics100.java +++ b/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveDriveKinematics100.java @@ -14,9 +14,6 @@ /** * Helper class that converts between chassis state and module state. * - * Note: sometimes one wheel will slip more than the others; we should ignore it - * in the forward calculation. see https://github.com/Team100/all24/issues/348 - * * Note: forward kinematics is never more accurate than the gyro and we * absolutely cannot operate without a functional gyro, so we should use the * gyro instead. see https://github.com/Team100/all24/issues/350 @@ -25,7 +22,6 @@ public class SwerveDriveKinematics100 { private static final double kEpsilon = 1e-6; private final int m_numModules; private final Translation2d[] m_moduleLocations; - private final SimpleMatrix[] m_mat = new SimpleMatrix[4]; /** * this (2n x 3) matrix looks something like @@ -107,15 +103,9 @@ public SwerveDriveKinematics100(Translation2d... moduleTranslationsM) { checkModuleCount(moduleTranslationsM); m_numModules = moduleTranslationsM.length; m_moduleLocations = Arrays.copyOf(moduleTranslationsM, m_numModules); - for (int i = 0; i < m_moduleLocations.length; i++) { - m_mat[i] = new SimpleMatrix(2, 3); - m_mat[i].setRow(0, 0, 1, 0, -m_moduleLocations[i].getY()); - m_mat[i].setRow(1, 0, 0, 1, m_moduleLocations[i].getX()); - } m_inverseKinematics = inverseMatrix(m_moduleLocations); m_forwardKinematics = m_inverseKinematics.pseudoInverse(); - // try to avoid startup transient - // m_moduleHeadings = zeros(m_numModules); + // nulls avoid startup transient m_moduleHeadings = nulls(); } @@ -145,46 +135,6 @@ public SwerveModuleStates toSwerveModuleStates(ChassisSpeeds chassisSpeeds) { return states; } - public SwerveModuleStates toSwerveModuleStates( - ChassisSpeeds chassisSpeeds, - ChassisSpeeds chassisSpeedsAcceleration) { - if (fullStop(chassisSpeeds)) { - return constantModuleHeadings(); // avoid steering when stopped - } - // [vx; vy; omega] (3 x 1) - SimpleMatrix chassisSpeedsVector = chassisSpeeds2Vector(chassisSpeeds); - SimpleMatrix chassisSpeedsAccelerationVector = chassisSpeeds2Vector(chassisSpeedsAcceleration); - // [v cos; v sin; ...] (2n x 1) - SwerveModuleStates prevStates = new SwerveModuleStates( - new SwerveModuleState100(0, Optional.of(new Rotation2d())), - new SwerveModuleState100(0, Optional.of(new Rotation2d())), - new SwerveModuleState100(0, Optional.of(new Rotation2d())), - new SwerveModuleState100(0, Optional.of(new Rotation2d()))); - SwerveModuleStates states = accelerationFromVector(chassisSpeedsVector, chassisSpeedsAccelerationVector, - prevStates); - updateHeadings(states); - return states; - } - - public SwerveModuleStates toSwerveModuleStates( - ChassisSpeeds chassisSpeeds, - ChassisSpeeds chassisSpeedsAcceleration, - SwerveModuleStates prevStates) { - if (fullStop(chassisSpeeds)) { - return constantModuleHeadings(); // avoid steering when stopped - } - // [vx; vy; omega] (3 x 1) - SimpleMatrix chassisSpeedsVector = chassisSpeeds2Vector(chassisSpeeds); - SimpleMatrix chassisSpeedsAccelerationVector = chassisSpeeds2Vector(chassisSpeedsAcceleration); - // [v cos; v sin; ...] (2n x 1) - SwerveModuleStates states = accelerationFromVector( - chassisSpeedsVector, - chassisSpeedsAccelerationVector, - prevStates); - updateHeadings(states); - return states; - } - /** * INVERSE: twist -> module position deltas * @@ -439,83 +389,10 @@ private SwerveModuleState100 stateFromVector(double x, double y) { } } - /** - * // TODO: test this and keep it or toss it - * - * https://www.chiefdelphi.com/uploads/short-url/qzj4k2LyBs7rLxAem0YajNIlStH.pdf - */ - public SwerveModuleStates accelerationFromVector( - SimpleMatrix chassisSpeedsMatrix, - SimpleMatrix chassisSpeedsAccelerationMatrix, - SwerveModuleStates prevStates) { - return new SwerveModuleStates( - oneAccel(0, chassisSpeedsMatrix, chassisSpeedsAccelerationMatrix), - oneAccel(1, chassisSpeedsMatrix, chassisSpeedsAccelerationMatrix), - oneAccel(2, chassisSpeedsMatrix, chassisSpeedsAccelerationMatrix), - oneAccel(3, chassisSpeedsMatrix, chassisSpeedsAccelerationMatrix)); - } - - private SwerveModuleState100 oneAccel( - int moduleLocation, - SimpleMatrix chassisSpeedsMatrix, - SimpleMatrix chassisSpeedsAccelerationMatrix) { - - SimpleMatrix dmodulexy = m_mat[moduleLocation].mult(chassisSpeedsMatrix); - double vx = dmodulexy.get(0, 0); - double vy = dmodulexy.get(1, 0); - double speed = Math.hypot(vx, vy); - Rotation2d angle; - if (speed <= 1e-6) { - // avoid the garbage rotation, extrapolate the angle using omega - // double dtheta = prevStates[i].omega * dt; - // angle = new Rotation2d(MathUtil.angleModulus( - // angle2.get().getRadians() + dtheta)); - // actually we really have no idea what the current state should be - // TODO: confirm this is OK - return new SwerveModuleState100(0, Optional.empty()); - } else { - angle = new Rotation2d(vx, vy); - } - SimpleMatrix multiplier = new SimpleMatrix(2, 2); - multiplier.setRow(0, 0, angle.getCos(), angle.getSin()); - multiplier.setRow(1, 0, -1.0 * angle.getSin(), angle.getCos()); - SimpleMatrix moduleAccelerationXY = getModuleAccelerationXY( - moduleLocation, - chassisSpeedsAccelerationMatrix); - SimpleMatrix moduleAccelMat = multiplier.mult(moduleAccelerationXY); - if (speed != 0) { - moduleAccelMat.set(1, 0, (moduleAccelMat.get(1, 0) / speed)); - } else { - // TODO: what is this 100000? - moduleAccelMat.set(1, 0, moduleAccelMat.get(1, 0) * 100000); - } - double accelMetersPerSecond_2 = moduleAccelMat.get(0, 0); - double omega = moduleAccelMat.get(1, 0); - return new SwerveModuleState100( - speed, - Optional.of(angle), - accelMetersPerSecond_2, - omega); - } - public Translation2d[] getModuleLocations() { return m_moduleLocations; } - /** - * Outputs a 2x1 matrix of acceleration of the module in x and y - */ - public SimpleMatrix getModuleAccelerationXY( - int moduleLocation, - SimpleMatrix chassisSpeedsAccelerationMatrix) { - SimpleMatrix acceleration2vector = new SimpleMatrix(3, 1); - acceleration2vector.setColumn(0, 0, - chassisSpeedsAccelerationMatrix.get(0, 0), - chassisSpeedsAccelerationMatrix.get(1, 0), - chassisSpeedsAccelerationMatrix.get(2, 0)); - return m_mat[moduleLocation].mult(acceleration2vector); - } - /** * The resulting distance is always positive. * @@ -570,11 +447,4 @@ private void checkModuleCount(Translation2d... moduleTranslationsM) { throw new IllegalArgumentException("Swerve requires at least two modules"); } } - - private void checkLength(Object[] objs) { - int ct = objs.length; - if (ct != m_numModules) { - throw new IllegalArgumentException("Wrong module count: " + ct); - } - } } diff --git a/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveKinodynamics.java b/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveKinodynamics.java index 7788f9c7b..4807e73ea 100644 --- a/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveKinodynamics.java +++ b/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveKinodynamics.java @@ -318,56 +318,6 @@ SwerveModuleStates toSwerveModuleStates(ChassisSpeeds in, double gyroRateRad_S, return m_kinematics.toSwerveModuleStates(descretized); } - /** - * Inverse kinematics, chassis speeds => module states. - * - * The resulting state speeds are always positive. - * - * This version does **DISCRETIZATION** to correct for swerve veering. - * - * It also does extra veering correction proportional to rotation rate and - * translational acceleration. - * - * @param in chassis speeds to transform - * @param gyroRateRad_S current gyro rate, or the trajectory gyro rate - * @param accelM_S magnitude of acceleration - */ - public SwerveModuleStates toSwerveModuleStates( - ChassisSpeeds in, - ChassisSpeeds prevIn, - SwerveModuleStates prevSwerveModuleState, - double gyroRateRad_S) { - // This is the extra correction angle ... - Rotation2d angle = new Rotation2d(VeeringCorrection.correctionRad(gyroRateRad_S)); - // ... which is subtracted here; this isn't really a field-relative - // transformation it's just a rotation. - ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( - in.vxMetersPerSecond, - in.vyMetersPerSecond, - in.omegaRadiansPerSecond, - angle); - ChassisSpeeds descretized = ChassisSpeeds.discretize(chassisSpeeds, TimedRobot100.LOOP_PERIOD_S); - - ChassisSpeeds prevChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( - prevIn.vxMetersPerSecond, - prevIn.vyMetersPerSecond, - prevIn.omegaRadiansPerSecond, - angle); - ChassisSpeeds prevDescretized = ChassisSpeeds.discretize(prevChassisSpeeds, TimedRobot100.LOOP_PERIOD_S); - - ChassisSpeeds acceleration = (chassisSpeeds.minus(prevDescretized)).div(TimedRobot100.LOOP_PERIOD_S); - - return m_kinematics.toSwerveModuleStates(descretized, acceleration, prevSwerveModuleState); - } - - /** - * The resulting state speeds are always positive. - */ - public SwerveModuleStates toSwerveModuleStatesWithoutDiscretization(ChassisSpeeds speeds, - ChassisSpeeds prevChassisSpeeds, SwerveModuleStates prevModuleStates) { - return m_kinematics.toSwerveModuleStates(speeds, prevChassisSpeeds, prevModuleStates); - } - /** * The resulting state speeds are always positive. */ diff --git a/lib/src/main/java/org/team100/lib/motion/drivetrain/module/SwerveModule100.java b/lib/src/main/java/org/team100/lib/motion/drivetrain/module/SwerveModule100.java index e274071ce..e6020bdb7 100644 --- a/lib/src/main/java/org/team100/lib/motion/drivetrain/module/SwerveModule100.java +++ b/lib/src/main/java/org/team100/lib/motion/drivetrain/module/SwerveModule100.java @@ -4,8 +4,6 @@ import java.util.OptionalDouble; import org.team100.lib.dashboard.Glassy; -import org.team100.lib.experiments.Experiment; -import org.team100.lib.experiments.Experiments; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModulePosition100; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleState100; import org.team100.lib.motion.servo.AngularPositionServo; @@ -36,7 +34,7 @@ protected SwerveModule100( void setDesiredState(SwerveModuleState100 desiredState) { OptionalDouble position = m_turningServo.getPosition(); - if(position.isPresent()){ + if (position.isPresent()) { previousPosition = new Rotation2d(position.getAsDouble()); } @@ -63,13 +61,8 @@ void setRawDesiredState(SwerveModuleState100 state) { return; // throw new IllegalArgumentException(); } - if (Experiments.instance.enabled(Experiment.UseSecondDerivativeSwerve)) { - m_driveServo.setVelocity(state.speedMetersPerSecond, state.accelMetersPerSecond_2); - m_turningServo.setPositionWithVelocity(state.angle.get().getRadians(), state.omega, 0); - } else { - m_driveServo.setVelocityM_S(state.speedMetersPerSecond); - m_turningServo.setPosition(state.angle.get().getRadians(), 0); - } + m_driveServo.setVelocityM_S(state.speedMetersPerSecond); + m_turningServo.setPosition(state.angle.get().getRadians(), 0); } /** For testing */ diff --git a/lib/src/main/java/org/team100/lib/swerve/AsymSwerveSetpointGenerator.java b/lib/src/main/java/org/team100/lib/swerve/AsymSwerveSetpointGenerator.java index f439834db..9f6e78273 100644 --- a/lib/src/main/java/org/team100/lib/swerve/AsymSwerveSetpointGenerator.java +++ b/lib/src/main/java/org/team100/lib/swerve/AsymSwerveSetpointGenerator.java @@ -4,8 +4,6 @@ import java.util.function.DoubleSupplier; import org.team100.lib.dashboard.Glassy; -import org.team100.lib.experiments.Experiment; -import org.team100.lib.experiments.Experiments; import org.team100.lib.framework.TimedRobot100; import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.logging.LoggerFactory; @@ -72,14 +70,8 @@ public SwerveSetpoint generateSetpoint( ChassisSpeeds desiredState) { SwerveModuleStates prevModuleStates = prevSetpoint.getModuleStates(); // the desired module state speeds are always positive. - SwerveModuleStates desiredModuleStates; - if (Experiments.instance.enabled(Experiment.UseSecondDerivativeSwerve)) { - desiredModuleStates = m_limits.toSwerveModuleStatesWithoutDiscretization( - desiredState, prevSetpoint.getChassisSpeeds(), prevModuleStates); - } else { - desiredModuleStates = m_limits.toSwerveModuleStatesWithoutDiscretization( - desiredState); - } + SwerveModuleStates desiredModuleStates = m_limits.toSwerveModuleStatesWithoutDiscretization( + desiredState); desiredState = desaturate(desiredState, desiredModuleStates); boolean desiredIsStopped = SwerveUtil.desiredIsStopped(desiredState, desiredModuleStates, prevModuleStates); @@ -93,7 +85,6 @@ public SwerveSetpoint generateSetpoint( double[] desired_vy = computeVy(desiredModuleStates); // elements may be null. Rotation2d[] desired_heading = computeHeading(desiredModuleStates); - double[] desired_heading_velocity = computeHeadingVelocity(desiredModuleStates); boolean shouldStopAndReverse = shouldStopAndReverse(prev_heading, desired_heading); if (shouldStopAndReverse @@ -150,7 +141,6 @@ public SwerveSetpoint generateSetpoint( desired_vx, desired_vy, desired_heading, - desired_heading_velocity, overrideSteering); min_s = Math.min(min_s, steering_min_s); } @@ -227,18 +217,6 @@ private Rotation2d[] computeHeading(SwerveModuleStates states) { return heading; } - /** - * Which way each module is actually going, taking speed polarity into account. - */ - private double[] computeHeadingVelocity(SwerveModuleStates states) { - return new double[] { - states.frontLeft().omega, - states.frontRight().omega, - states.rearLeft().omega, - states.rearRight().omega, - }; - } - /** * If we want to go back the way we came, it might be faster to stop * and then reverse. This is certainly true for near-180 degree turns, but @@ -284,20 +262,10 @@ private SwerveSetpoint makeSetpoint( dy, dtheta, min_s); - SwerveModuleStates setpointStates; // the speeds in these states are always positive. - if (Experiments.instance.enabled(Experiment.UseSecondDerivativeSwerve)) { - setpointStates = m_limits.toSwerveModuleStates( - setpointSpeeds, - prevSetpoint.getChassisSpeeds(), - prevModuleStates, - setpointSpeeds.omegaRadiansPerSecond); - } else { - setpointStates = m_limits.toSwerveModuleStates( - setpointSpeeds, - setpointSpeeds.omegaRadiansPerSecond); - } - + SwerveModuleStates setpointStates = m_limits.toSwerveModuleStates( + setpointSpeeds, + setpointSpeeds.omegaRadiansPerSecond); applyOverrides(overrideSteering, setpointStates); flipIfRequired(prevModuleStates, setpointStates); @@ -322,7 +290,6 @@ private void applyOverrides(Rotation2d[] overrides, SwerveModuleStates states) { } } - private void flipIfRequired(SwerveModuleStates prevStates, SwerveModuleStates setpointStates) { SwerveModuleState100[] prevStatesAll = prevStates.all(); SwerveModuleState100[] setpointStatesAll = setpointStates.all(); diff --git a/lib/src/main/java/org/team100/lib/swerve/SteeringRateLimiter.java b/lib/src/main/java/org/team100/lib/swerve/SteeringRateLimiter.java index a53a30cd1..d60ef435e 100644 --- a/lib/src/main/java/org/team100/lib/swerve/SteeringRateLimiter.java +++ b/lib/src/main/java/org/team100/lib/swerve/SteeringRateLimiter.java @@ -1,8 +1,6 @@ package org.team100.lib.swerve; import org.team100.lib.dashboard.Glassy; -import org.team100.lib.experiments.Experiment; -import org.team100.lib.experiments.Experiments; import org.team100.lib.framework.TimedRobot100; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; @@ -38,7 +36,6 @@ public double enforceSteeringLimit( double[] desired_vx, double[] desired_vy, Rotation2d[] desired_heading, // nullable entries - double[] desired_heading_velocity, Rotation2d[] overrideSteering) { double min_s = 1.0; @@ -52,33 +49,20 @@ public double enforceSteeringLimit( // ignore overridden wheels continue; } - double s; - if (Experiments.instance.enabled(Experiment.UseSecondDerivativeSwerve)) { - s = SwerveUtil.findSteeringMaxS( + double s = SwerveUtil.findSteeringMaxS( prev_vx[i], prev_vy[i], prev_heading[i].getRadians(), desired_vx[i], desired_vy[i], desired_heading[i].getRadians(), - desired_heading_velocity[i] * TimedRobot100.LOOP_PERIOD_S, TimedRobot100.LOOP_PERIOD_S * m_limits.getMaxSteeringVelocityRad_S(), kMaxIterations); - } else { - s = SwerveUtil.findSteeringMaxS( - prev_vx[i], - prev_vy[i], - prev_heading[i].getRadians(), - desired_vx[i], - desired_vy[i], - desired_heading[i].getRadians(), - TimedRobot100.LOOP_PERIOD_S * m_limits.getMaxSteeringVelocityRad_S(), - kMaxIterations); - } + min_s = Math.min(min_s, s); } double s = min_s; - m_log_s.log( () -> s); + m_log_s.log(() -> s); return min_s; } diff --git a/lib/src/main/java/org/team100/lib/swerve/SwerveUtil.java b/lib/src/main/java/org/team100/lib/swerve/SwerveUtil.java index d98967896..3bd6365e6 100644 --- a/lib/src/main/java/org/team100/lib/swerve/SwerveUtil.java +++ b/lib/src/main/java/org/team100/lib/swerve/SwerveUtil.java @@ -37,46 +37,6 @@ public static double unwrapAngle(double ref, double angle) { } } - /** - * - * @param x_0 previous vx - * @param y_0 previoux vy - * @param f_0 previous steering angle - * @param x_1 desired vx - * @param y_1 desired vy - * @param f_1 desired steering angle - * @param max_deviation max angle step - * @param max_iterations - * @return - */ - public static double findSteeringMaxS( - double x_0, - double y_0, - double f_0, - double x_1, - double y_1, - double f_1, - double f_2, - double max_deviation, - int max_iterations) { - f_1 = SwerveUtil.unwrapAngle(f_0, f_1); - - if (Math.abs(f_2) <= max_deviation) { - // Can go all the way to s=1. - return 1.0; - } - - double offset = f_0 + Math.signum(f_2) * max_deviation; - - DoubleBinaryOperator func = (x, y) -> SwerveUtil.unwrapAngle(f_0, Math.atan2(y, x)) - offset; - - return Math100.findRoot( - func, - x_0, y_0, f_0 - offset, - x_1, y_1, f_1 - offset, - max_iterations); - } - /** * * @param x_0 previous vx diff --git a/lib/src/test/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveDriveKinematics100Test.java b/lib/src/test/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveDriveKinematics100Test.java index 73f218f7a..1d586c926 100644 --- a/lib/src/test/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveDriveKinematics100Test.java +++ b/lib/src/test/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveDriveKinematics100Test.java @@ -6,11 +6,9 @@ import java.util.Optional; -import org.ejml.simple.SimpleMatrix; import org.junit.jupiter.api.Test; import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.util.DriveUtil; -import org.team100.lib.util.Util; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -667,225 +665,6 @@ void testOffCenterCORRotationAndTranslationForwardKinematicsWithDeltas() { () -> assertEquals(1.5, twist.dtheta, 0.1)); } - @Test - void testModuleKinematics() { - SwerveDriveKinematics100 kinematics = new SwerveDriveKinematics100( - new Translation2d(0.5, 0.5), - new Translation2d(0.5, -0.5), - new Translation2d(-0.5, 0.5), - new Translation2d(-0.5, -0.5)); - SimpleMatrix accelerations = new SimpleMatrix(3, 1); - SimpleMatrix expected = new SimpleMatrix(2, 1); - accelerations.setColumn(0, 0, 0, 0, 0); - expected.setColumn(0, 0, 0, 0); - SimpleMatrix output = kinematics.getModuleAccelerationXY(0, accelerations); - Util.println(output.get(0, 0) + " " + output.get(1, 0)); - assertEquals(0, output.get(0, 0), 0.0001); - assertEquals(0, output.get(1, 0), 0.0001); - } - - @Test - void testModuleKinematics2() { - // one-meter drive base - SwerveDriveKinematics100 kinematics = new SwerveDriveKinematics100( - new Translation2d(0.5, 0.5), - new Translation2d(0.5, -0.5), - new Translation2d(-0.5, 0.5), - new Translation2d(-0.5, -0.5)); - - SwerveModuleStates prev = new SwerveModuleStates( - new SwerveModuleState100(0, Optional.of(new Rotation2d())), - new SwerveModuleState100(0, Optional.of(new Rotation2d())), - new SwerveModuleState100(0, Optional.of(new Rotation2d())), - new SwerveModuleState100(0, Optional.of(new Rotation2d()))); - - { - SimpleMatrix v = new SimpleMatrix(3, 1); - v.setColumn(0, 0, 0, 0, 1); - SwerveModuleStates output2 = kinematics.statesFromVector(v); - - SimpleMatrix a = new SimpleMatrix(3, 1); - a.setColumn(0, 0, 0, 0, 0); - SwerveModuleStates output = kinematics.accelerationFromVector(v, a, prev); - - Util.println(output.frontLeft().toString()); - - assertEquals(0, output.frontLeft().accelMetersPerSecond_2, 0.0001); - - SwerveModuleState100[] outputAll = output.all(); - SwerveModuleState100[] output2All = output2.all(); - for (int i = 0; i < outputAll.length; i++) { - assertEquals(outputAll[i].angle, output2All[i].angle); - assertEquals(outputAll[i].speedMetersPerSecond, output2All[i].speedMetersPerSecond); - } - } - - { - SimpleMatrix v = new SimpleMatrix(3, 1); - v.setColumn(0, 0, 2, 1, 1); - SwerveModuleStates output4 = kinematics.statesFromVector(v); - SimpleMatrix a = new SimpleMatrix(3, 1); - a.setColumn(0, 0, 2, 2, 2); - SwerveModuleStates output3 = kinematics.accelerationFromVector(v, a, prev); - - SwerveModuleState100[] output3All = output3.all(); - SwerveModuleState100[] output4All = output4.all(); - - for (int i = 0; i < output3All.length; i++) { - assertEquals(output3All[i].angle, output4All[i].angle); - assertEquals(output3All[i].speedMetersPerSecond, output4All[i].speedMetersPerSecond); - } - } - - { - SimpleMatrix v = new SimpleMatrix(3, 1); - v.setColumn(0, 0, 2.3, 0, 1.1); - - SwerveModuleStates output5 = kinematics.statesFromVector(v); - - SimpleMatrix a = new SimpleMatrix(3, 1); - a.setColumn(0, 0, 1.32, 1.2, 2.2); - - SwerveModuleStates output6 = kinematics.accelerationFromVector(v, a, prev); - - SwerveModuleState100[] output5All = output5.all(); - SwerveModuleState100[] output6All = output6.all(); - - for (int i = 0; i < output5All.length; i++) { - assertEquals(output5All[i].angle, output6All[i].angle); - assertEquals(output5All[i].speedMetersPerSecond, output6All[i].speedMetersPerSecond); - } - } - - { - // motionless - SimpleMatrix v = new SimpleMatrix(3, 1); - v.setColumn(0, 0, 0, 0, 0); - SwerveModuleStates output7 = kinematics.statesFromVector(v); - SimpleMatrix a = new SimpleMatrix(3, 1); - a.setColumn(0, 0, 1, 1.1, 2); - SwerveModuleStates output8 = kinematics.accelerationFromVector(v, a, prev); - - SwerveModuleState100[] output7All = output7.all(); - SwerveModuleState100[] output8All = output8.all(); - - for (int i = 0; i < output7All.length; i++) { - assertEquals(output7All[i].angle, output8All[i].angle); - assertEquals(output7All[i].speedMetersPerSecond, output8All[i].speedMetersPerSecond); - } - } - } - - @Test - void testModuleKinematics3a() { - SwerveDriveKinematics100 kinematics = new SwerveDriveKinematics100( - new Translation2d(0.5, 0.5), - new Translation2d(0.5, -0.5), - new Translation2d(-0.5, 0.5), - new Translation2d(-0.5, -0.5)); - SwerveModuleStates prevStates = new SwerveModuleStates( - new SwerveModuleState100(0, Optional.of(new Rotation2d())), - new SwerveModuleState100(0, Optional.of(new Rotation2d())), - new SwerveModuleState100(0, Optional.of(new Rotation2d())), - new SwerveModuleState100(0, Optional.of(new Rotation2d()))); - - // no velocity - SimpleMatrix velocities = new SimpleMatrix(3, 1); - velocities.setColumn(0, 0, 0, 0, 0); - - // accelerating in x i guess - SimpleMatrix acceleration = new SimpleMatrix(3, 1); - acceleration.setColumn(0, 0, 1, 0, 0); - - SwerveModuleStates output = kinematics.accelerationFromVector( - velocities, - acceleration, - prevStates); - - for (SwerveModuleState100 state : output.all()) { - // assertEquals(state.angle.get().getRadians(), 0, 0.0001); - // since the module isn't yet moving it has an indeterminate angle. - assertTrue(state.angle.isEmpty()); - assertEquals(0, state.omega, 0.0001); - assertEquals(0, state.speedMetersPerSecond, 0.0001); - // TODO: fix this test - // assertEquals(state.accelMetersPerSecond_2, 1, 0.0001); - } - - } - - @Test - void testModuleKinematics3b() { - SwerveDriveKinematics100 kinematics = new SwerveDriveKinematics100( - new Translation2d(0.5, 0.5), - new Translation2d(0.5, -0.5), - new Translation2d(-0.5, 0.5), - new Translation2d(-0.5, -0.5)); - SwerveModuleStates prevStates = new SwerveModuleStates( - new SwerveModuleState100(0, Optional.of(new Rotation2d())), - new SwerveModuleState100(0, Optional.of(new Rotation2d())), - new SwerveModuleState100(0, Optional.of(new Rotation2d())), - new SwerveModuleState100(0, Optional.of(new Rotation2d()))); - - SimpleMatrix velocities = new SimpleMatrix(3, 1); - SimpleMatrix acceleration = new SimpleMatrix(3, 1); - - velocities.setColumn(0, 0, 1, 0, 0); - acceleration.setColumn(0, 0, 1, 0, 0); - - SwerveModuleStates output = kinematics.accelerationFromVector(velocities, acceleration, prevStates); - - for (SwerveModuleState100 state : output.all()) { - assertEquals(0, state.angle.get().getRadians(), 0.0001); - assertEquals(0, state.omega, 0.0001); - assertEquals(1, state.speedMetersPerSecond, 0.0001); - assertEquals(1, state.accelMetersPerSecond_2, 0.0001); - } - - } - - @Test - void testModuleKinematics3c() { - SwerveDriveKinematics100 kinematics = new SwerveDriveKinematics100( - new Translation2d(0.5, 0.5), - new Translation2d(0.5, -0.5), - new Translation2d(-0.5, 0.5), - new Translation2d(-0.5, -0.5)); - SwerveModuleStates prevStates = new SwerveModuleStates( - new SwerveModuleState100(0, Optional.of(new Rotation2d())), - new SwerveModuleState100(0, Optional.of(new Rotation2d())), - new SwerveModuleState100(0, Optional.of(new Rotation2d())), - new SwerveModuleState100(0, Optional.of(new Rotation2d()))); - - SimpleMatrix velocities = new SimpleMatrix(3, 1); - SimpleMatrix acceleration = new SimpleMatrix(3, 1); - - velocities.setColumn(0, 0, 1, 0, 0); - acceleration.setColumn(0, 0, 0, 0, 1); - SwerveModuleStates output = kinematics.accelerationFromVector(velocities, - acceleration, prevStates); - - assertEquals(0, output.frontLeft().angle.get().getRadians(), 0.0001); - assertEquals(1, output.frontLeft().speedMetersPerSecond, 0.0001); - assertEquals(0.5, output.frontLeft().omega, 0.0001); - assertEquals(-0.5, output.frontLeft().accelMetersPerSecond_2, 0.0001); - - assertEquals(0, output.frontRight().angle.get().getRadians(), 0.0001); - assertEquals(1, output.frontRight().speedMetersPerSecond, 0.0001); - assertEquals(0.5, output.frontRight().omega, 0.0001); - assertEquals(0.5, output.frontRight().accelMetersPerSecond_2, 0.0001); - - assertEquals(0, output.rearLeft().angle.get().getRadians(), 0.0001); - assertEquals(1, output.rearLeft().speedMetersPerSecond, 0.0001); - assertEquals(-0.5, output.rearLeft().omega, 0.0001); - assertEquals(-0.5, output.rearLeft().accelMetersPerSecond_2, 0.0001); - - assertEquals(0, output.rearRight().angle.get().getRadians(), 0.0001); - assertEquals(1, output.rearRight().speedMetersPerSecond, 0.0001); - assertEquals(-0.5, output.rearRight().omega, 0.0001); - assertEquals(0.5, output.rearRight().accelMetersPerSecond_2, 0.0001); - } - @Test void testDesaturate() { SwerveModuleState100 fl = new SwerveModuleState100(5, Optional.of(new Rotation2d())); diff --git a/lib/src/test/java/org/team100/lib/swerve/SteeringRateLimiterTest.java b/lib/src/test/java/org/team100/lib/swerve/SteeringRateLimiterTest.java index 2c606b2bb..9c5a53fcd 100644 --- a/lib/src/test/java/org/team100/lib/swerve/SteeringRateLimiterTest.java +++ b/lib/src/test/java/org/team100/lib/swerve/SteeringRateLimiterTest.java @@ -26,7 +26,6 @@ void testUnconstrained() { double[] desired_vx = new double[] { 0 }; double[] desired_vy = new double[] { 0 }; Rotation2d[] desired_heading = new Rotation2d[] { GeometryUtil.kRotationZero }; - double[] desired_heading_velocity = new double[] { 0 }; Rotation2d[] overrideSteering = new Rotation2d[1]; double s = c.enforceSteeringLimit( @@ -36,7 +35,6 @@ void testUnconstrained() { desired_vx, desired_vy, desired_heading, - desired_heading_velocity, overrideSteering); assertEquals(1.0, s, kDelta); @@ -53,7 +51,6 @@ void testConstrained() { double[] desired_vx = new double[] { 0 }; double[] desired_vy = new double[] { 1 }; Rotation2d[] desired_heading = new Rotation2d[] { GeometryUtil.kRotation90 }; - double[] desired_heading_velocity = new double[] { 0 }; Rotation2d[] overrideSteering = new Rotation2d[1]; double s = c.enforceSteeringLimit( @@ -63,7 +60,6 @@ void testConstrained() { desired_vx, desired_vy, desired_heading, - desired_heading_velocity, overrideSteering); // s = 0 stops the drive motors assertEquals(0, s, kDelta);