diff --git a/comp/swerve100/swerve100.code-workspace b/comp/swerve100/swerve100.code-workspace index 221b3e870..26e9242b6 100644 --- a/comp/swerve100/swerve100.code-workspace +++ b/comp/swerve100/swerve100.code-workspace @@ -13,7 +13,7 @@ "terminal.integrated.scrollback": 100000, "workbench.colorTheme": "Default Light+", "files.autoSave": "afterDelay", - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx16G -Xms100m -Xlog:disable", + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx32G -Xms100m -Xlog:disable", "workbench.editor.revealIfOpen": true, "editor.minimap.enabled": false, "editor.inlayHints.enabled": "off", diff --git a/comp/swerve100/vendordeps/ReduxLib_2024.json b/comp/swerve100/vendordeps/ReduxLib_2024.json index f8e048e7e..f694966df 100644 --- a/comp/swerve100/vendordeps/ReduxLib_2024.json +++ b/comp/swerve100/vendordeps/ReduxLib_2024.json @@ -1,55 +1,55 @@ { - "fileName": "ReduxLib_2024.json", - "name": "ReduxLib", - "version": "2024.3.0", - "frcYear": 2024, - "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", - "mavenUrls": [ - "https://maven.reduxrobotics.com/" - ], - "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2024.json", - "javaDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-java", - "version": "2024.3.0" - } - ], - "jniDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-driver", - "version": "2024.3.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm32", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-cpp", - "version": "2024.3.0", - "libName": "ReduxLib-cpp", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm32", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - } - ] + "fileName": "ReduxLib_2024.json", + "name": "ReduxLib", + "version": "2024.3.1", + "frcYear": 2024, + "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", + "mavenUrls": [ + "https://maven.reduxrobotics.com/" + ], + "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2024.json", + "javaDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-java", + "version": "2024.3.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-driver", + "version": "2024.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-cpp", + "version": "2024.3.1", + "libName": "ReduxLib-cpp", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ] } \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/localization/SwerveDrivePoseEstimator100.java b/lib/src/main/java/org/team100/lib/localization/SwerveDrivePoseEstimator100.java index 3b8e57ad8..94f259ed0 100644 --- a/lib/src/main/java/org/team100/lib/localization/SwerveDrivePoseEstimator100.java +++ b/lib/src/main/java/org/team100/lib/localization/SwerveDrivePoseEstimator100.java @@ -14,6 +14,7 @@ import org.team100.lib.motion.drivetrain.kinodynamics.FieldRelativeDelta; import org.team100.lib.motion.drivetrain.kinodynamics.FieldRelativeVelocity; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics; +import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleDelta; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModulePosition100; import org.team100.lib.util.DriveUtil; import org.team100.lib.util.Util; @@ -202,13 +203,14 @@ public void put( // the entry right before this one, the basis for integration. Entry lowerEntry = consistentPair.get(0); - // System.out.println("SwerveDrivePoseEstiamtor.put() lowerEntry " + lowerEntry); + // System.out.println("SwerveDrivePoseEstiamtor.put() lowerEntry " + + // lowerEntry); double t1 = currentTimeS - lowerEntry.getKey(); InterpolationRecord value = lowerEntry.getValue(); SwerveState previousState = value.m_state; - SwerveModulePosition100[] modulePositionDelta = DriveUtil.modulePositionDelta( + SwerveModuleDelta[] modulePositionDelta = DriveUtil.modulePositionDelta( value.m_wheelPositions, wheelPositions); @@ -231,7 +233,8 @@ public void put( deltaTransform.getX(), deltaTransform.getY(), deltaTransform.getRotation().getRadians()); - // System.out.println("SwerveDrivePoseEstimator.put() current velocity " + velocity); + // System.out.println("SwerveDrivePoseEstimator.put() current velocity " + + // velocity); // calculate acceleration if possible FieldRelativeAcceleration accel = new FieldRelativeAcceleration(0, 0, 0); @@ -246,7 +249,8 @@ public void put( // for acceleration we recalculate from position, since position might have been // updated by the cameras. Map.Entry earlierEntry = consistentPair.get(1); - // System.out.println("SwerveDrivePoseEstiamtor.put() earlierEntry " + earlierEntry); + // System.out.println("SwerveDrivePoseEstiamtor.put() earlierEntry " + + // earlierEntry); double t0 = lowerEntry.getKey() - earlierEntry.getKey(); // System.out.println("SwerveDrivePoseEstimator.put() accel " + accel); 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 195c9080f..c2aa70f0b 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 @@ -4,7 +4,7 @@ import java.util.Optional; import org.ejml.simple.SimpleMatrix; -import org.team100.lib.geometry.Vector2d; +import org.team100.lib.util.DriveUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -55,7 +55,7 @@ public class SwerveDriveKinematics100 { * */ - private final SimpleMatrix m_inverseKinematics; + final SimpleMatrix m_inverseKinematics; /** * like this: * this (3 x 2n) matrix is the pseudo-inverse of above, which ends up something @@ -86,7 +86,7 @@ public class SwerveDriveKinematics100 { * some combination depending on dimensions * */ - private final SimpleMatrix m_forwardKinematics; + final SimpleMatrix m_forwardKinematics; /** * Used when velocity is zero, to keep the steering the same. * elements are nullable. @@ -94,6 +94,13 @@ public class SwerveDriveKinematics100 { private Rotation2d[] m_moduleHeadings; /** + * array order: + * + * frontLeft + * frontRight + * rearLeft + * rearRight + * * @param moduleTranslationsM relative to the center of rotation */ public SwerveDriveKinematics100(Translation2d... moduleTranslationsM) { @@ -114,6 +121,13 @@ public SwerveDriveKinematics100(Translation2d... moduleTranslationsM) { /** * Reset the internal swerve module headings + * + * array order: + * + * frontLeft + * frontRight + * rearLeft + * rearRight * * arg elements are nullable */ @@ -139,24 +153,6 @@ public SwerveModuleState100[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds) return states; } - /** - * INVERSE: chassis speeds -> module states - * - * The resulting module state speeds are always positive. - * TODO(vasili): i think this is unfinished work from 2nd order control??? - */ - public SwerveModuleState100[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds, SwerveModuleState100[] prevStates) { - if (fullStop(chassisSpeeds)) { - return constantModuleHeadings(); // avoid steering when stopped - } - // [vx; vy; omega] (3 x 1) - SimpleMatrix chassisSpeedsVector = chassisSpeeds2Vector(chassisSpeeds); - // [v cos; v sin; ...] (2n x 1) - SwerveModuleState100[] states = statesFromVector(chassisSpeedsVector); - updateHeadings(states); - return states; - } - public SwerveModuleState100[] toSwerveModuleStates( ChassisSpeeds chassisSpeeds, ChassisSpeeds chassisSpeedsAcceleration) { @@ -178,8 +174,10 @@ public SwerveModuleState100[] toSwerveModuleStates( return states; } - public SwerveModuleState100[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds, - ChassisSpeeds chassisSpeedsAcceleration, SwerveModuleState100[] prevStates) { + public SwerveModuleState100[] toSwerveModuleStates( + ChassisSpeeds chassisSpeeds, + ChassisSpeeds chassisSpeedsAcceleration, + SwerveModuleState100[] prevStates) { if (fullStop(chassisSpeeds)) { return constantModuleHeadings(); // avoid steering when stopped } @@ -187,7 +185,9 @@ public SwerveModuleState100[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds, SimpleMatrix chassisSpeedsVector = chassisSpeeds2Vector(chassisSpeeds); SimpleMatrix chassisSpeedsAccelerationVector = chassisSpeeds2Vector(chassisSpeedsAcceleration); // [v cos; v sin; ...] (2n x 1) - SwerveModuleState100[] states = accelerationFromVector(chassisSpeedsVector, chassisSpeedsAccelerationVector, + SwerveModuleState100[] states = accelerationFromVector( + chassisSpeedsVector, + chassisSpeedsAccelerationVector, prevStates); updateHeadings(states); return states; @@ -195,8 +195,10 @@ public SwerveModuleState100[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds, /** * INVERSE: twist -> module position deltas + * + * This assumes the wheel paths are geodesics; steering does not change. */ - public SwerveModulePosition100[] toSwerveModulePosition(Twist2d twist) { + public SwerveModuleDelta[] toSwerveModuleDelta(Twist2d twist) { if (fullStop(twist)) { return constantModulePositions(); } @@ -204,23 +206,19 @@ public SwerveModulePosition100[] toSwerveModulePosition(Twist2d twist) { SimpleMatrix twistVector = twist2Vector(twist); // [d cos; d sin; ...] (2n x 1) SimpleMatrix deltaVector = m_inverseKinematics.mult(twistVector); - SwerveModulePosition100[] deltas = deltasFromVector(deltaVector); + SwerveModuleDelta[] deltas = deltasFromVector(deltaVector); updateHeadings(deltas); return deltas; } - /** This is wrong, it assigns zero radians to the indeterminate case. */ - public Vector2d[] pos2vec(SwerveModulePosition100[] m) { - Vector2d[] vec = new Vector2d[m_numModules]; - for (int i = 0; i < m_numModules; ++i) { - SwerveModulePosition100 p = m[i]; - if (p.angle.isEmpty()) { - vec[i] = new Vector2d(0, 0); - } else { - vec[i] = new Vector2d(p.distanceMeters, p.angle.get()); - } - } - return vec; + /** + * Find the module deltas and apply them to the given initial positions. + */ + public SwerveModulePosition100[] toSwerveModulePositions( + SwerveModulePosition100[] initial, + Twist2d twist) { + SwerveModuleDelta[] deltas = toSwerveModuleDelta(twist); + return DriveUtil.modulePositionFromDelta(initial, deltas); } /** @@ -240,9 +238,11 @@ public ChassisSpeeds toChassisSpeeds(SwerveModuleState100... states) { /** * FORWARD: module deltas -> twist. * + * assumes the module deltas represent straight lines. + * * NOTE: do not use the returned dtheta, use the gyro instead. */ - public Twist2d toTwist2d(SwerveModulePosition100... deltas) { + public Twist2d toTwist2d(SwerveModuleDelta... deltas) { checkLength(deltas); // [d cos; d sin; ...] (2n x 1) SimpleMatrix deltaVector = deltas2Vector(deltas); @@ -343,11 +343,16 @@ private SimpleMatrix states2Vector(SwerveModuleState100... moduleStates) { return moduleStatesMatrix; } - /** deltas -> [d cos; d sin; ... ] (2n x 1) */ - private SimpleMatrix deltas2Vector(SwerveModulePosition100... moduleDeltas) { + /** + * produces a vector of corner dx and dy, assuming the module deltas represent + * straight line paths. + * + * @param moduleDeltas [d cos; d sin; ... ] (2n x 1) + */ + private SimpleMatrix deltas2Vector(SwerveModuleDelta... moduleDeltas) { SimpleMatrix moduleDeltaMatrix = new SimpleMatrix(m_numModules * 2, 1); for (int i = 0; i < m_numModules; i++) { - SwerveModulePosition100 module = moduleDeltas[i]; + SwerveModuleDelta module = moduleDeltas[i]; if (Math.abs(module.distanceMeters) < 1e-6 || module.angle.isEmpty()) { moduleDeltaMatrix.set(i * 2, 0, 0); moduleDeltaMatrix.set(i * 2 + 1, 0, 0); @@ -371,15 +376,12 @@ private SimpleMatrix chassisSpeeds2Vector(ChassisSpeeds chassisSpeeds) { return chassisSpeedsVector; } - private SimpleMatrix twist2Vector(Twist2d twist) { - SimpleMatrix twistVector = new SimpleMatrix(3, 1); - twistVector.setColumn( - 0, - 0, + /** Twist as a 3x1 column vector: [dx; dy; dtheta] */ + private static SimpleMatrix twist2Vector(Twist2d twist) { + return new SimpleMatrix(new double[] { twist.dx, twist.dy, - twist.dtheta); - return twistVector; + twist.dtheta }); } /** [vx; vy; omega] (3 x 1) -> ChassisSpeeds */ @@ -418,13 +420,13 @@ private SwerveModuleState100[] constantModuleHeadings() { return mods; } - private SwerveModulePosition100[] constantModulePositions() { - SwerveModulePosition100[] mods = new SwerveModulePosition100[m_numModules]; + private SwerveModuleDelta[] constantModulePositions() { + SwerveModuleDelta[] mods = new SwerveModuleDelta[m_numModules]; for (int i = 0; i < m_numModules; i++) { if (m_moduleHeadings[i] == null) { - mods[i] = new SwerveModulePosition100(0.0, Optional.empty()); + mods[i] = new SwerveModuleDelta(0.0, Optional.empty()); } else { - mods[i] = new SwerveModulePosition100(0.0, Optional.of(m_moduleHeadings[i])); + mods[i] = new SwerveModuleDelta(0.0, Optional.of(m_moduleHeadings[i])); } } return mods; @@ -434,8 +436,10 @@ private SwerveModulePosition100[] constantModulePositions() { * [v cos; v sin; ... ] (2n x 1) -> states[] * * The resulting module speed is always positive. + * + * @param chassisSpeedsVector [vx0; vy0; vx1; ...] */ - public SwerveModuleState100[] statesFromVector(SimpleMatrix chassisSpeedsVector) { + SwerveModuleState100[] statesFromVector(SimpleMatrix chassisSpeedsVector) { SimpleMatrix moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector); SwerveModuleState100[] moduleStates = new SwerveModuleState100[m_numModules]; for (int i = 0; i < m_numModules; i++) { @@ -479,7 +483,7 @@ public SwerveModuleState100[] accelerationFromVector( // 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 + // TODO: confirm this is OK moduleStates[i] = new SwerveModuleState100(0, Optional.empty()); continue; } else { @@ -488,7 +492,8 @@ public SwerveModuleState100[] accelerationFromVector( 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(i, + SimpleMatrix moduleAccelerationXY = getModuleAccelerationXY( + i, chassisSpeedsAccelerationMatrix); SimpleMatrix moduleAccelMat = multiplier.mult(moduleAccelerationXY); if (speed != 0) { @@ -497,8 +502,12 @@ public SwerveModuleState100[] accelerationFromVector( // TODO: what is this 100000? moduleAccelMat.set(1, 0, moduleAccelMat.get(1, 0) * 100000); } - moduleStates[i] = new SwerveModuleState100(speed, Optional.of(angle), moduleAccelMat.get(0, 0), - moduleAccelMat.get(1, 0)); + double accelMetersPerSecond_2 = moduleAccelMat.get(0, 0); + double omega = moduleAccelMat.get(1, 0); + moduleStates[i] = new SwerveModuleState100(speed, + Optional.of(angle), + accelMetersPerSecond_2, + omega); } return moduleStates; } @@ -510,22 +519,29 @@ public Translation2d[] getModuleLocations() { /** * Outputs a 2x1 matrix of acceleration of the module in x and y */ - public SimpleMatrix getModuleAccelerationXY(int moduleLocation, SimpleMatrix chassisSpeedsAccelerationMatrix) { + 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)); + 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. + * + * @param moduleDeltaVector [d cos; d sin; ...] (2n x 1), + * equivalently [dx0; dy0; dx1; ...] */ - private SwerveModulePosition100[] deltasFromVector(SimpleMatrix moduleDeltaVector) { - SwerveModulePosition100[] moduleDeltas = new SwerveModulePosition100[m_numModules]; + private SwerveModuleDelta[] deltasFromVector(SimpleMatrix moduleDeltaVector) { + SwerveModuleDelta[] moduleDeltas = new SwerveModuleDelta[m_numModules]; for (int i = 0; i < m_numModules; i++) { double x = moduleDeltaVector.get(i * 2, 0); double y = moduleDeltaVector.get(i * 2 + 1, 0); - moduleDeltas[i] = new SwerveModulePosition100(x, y); + moduleDeltas[i] = new SwerveModuleDelta(x, y); } return moduleDeltas; } @@ -541,7 +557,7 @@ private void updateHeadings(SwerveModuleState100[] moduleStates) { } } - private void updateHeadings(SwerveModulePosition100[] mods) { + private void updateHeadings(SwerveModuleDelta[] mods) { for (int i = 0; i < m_numModules; i++) { if (mods[i].angle.isEmpty()) { // skip the update, remember the most-recent not-invalid value diff --git a/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveModuleDelta.java b/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveModuleDelta.java new file mode 100644 index 000000000..3a50e68cd --- /dev/null +++ b/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveModuleDelta.java @@ -0,0 +1,68 @@ +package org.team100.lib.motion.drivetrain.kinodynamics; + +import java.util.Optional; + +import org.team100.lib.motion.drivetrain.kinodynamics.struct.SwerveModuleDeltaStruct; + +import edu.wpi.first.math.geometry.Rotation2d; + +/** + * For kinematics, the module delta is the dx and dy of each corner, i.e. it's a + * distance and an angle. + * + * In the real world, consecutive module positions will have different angles, + * and this class needs to make some assumption about what happened in between. + * The WPI code assumes that the second angle covers the whole period in + * between. For awhile, we assumed the angle was smoothly varying between start + * and end, and computed something like the chord line, but I think that + * produced inconsistent results. + * + * Actuation uses module "state" i.e. velocity, which is applied to the modules, + * which are assumed to respond instantly. + * + * At higher levels we constrain steering rate to be achievable, but the + * maximum rate is quite high, over 10 rad/s, so the change in one 0.02s dt is + * tenths of radians, not negligible. + * + * But the inverse kinematics produces constant-steering deltas, and so should + * we here, so we use the ending angle for the whole path. + */ +public class SwerveModuleDelta { + /** Straight line distance from start to end. */ + public double distanceMeters; + + /** + * Angle of the straight line path. It can be empty, in cases where the angle is + * indeterminate (e.g. calculating the angle required for zero speed). + * TODO: make this private. + */ + public Optional angle = Optional.empty(); + + public static final SwerveModuleDeltaStruct struct = new SwerveModuleDeltaStruct(); + + /** Zero distance, empty angle. */ + public SwerveModuleDelta() { + // + } + + public SwerveModuleDelta(double distanceMeters, Optional angle) { + this.distanceMeters = distanceMeters; + this.angle = angle; + } + + /** + * This is only meaningful when using position as a delta. + * dx and dy are in meters; if both are very small, the rotation is undefined. + */ + public SwerveModuleDelta(double dx, double dy) { + if (Math.abs(dx) < 1e-6 && Math.abs(dy) < 1e-6) { + // avoid the garbage rotation. + this.distanceMeters = 0.0; + this.angle = Optional.empty(); + } else { + this.distanceMeters = Math.hypot(dx, dy); + this.angle = Optional.of(new Rotation2d(dx, dy)); + } + } + +} diff --git a/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveModulePosition100.java b/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveModulePosition100.java index 195fe2eca..969a2acc1 100644 --- a/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveModulePosition100.java +++ b/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveModulePosition100.java @@ -1,7 +1,5 @@ package org.team100.lib.motion.drivetrain.kinodynamics; -import static edu.wpi.first.units.Units.Meters; - import java.util.Objects; import java.util.Optional; @@ -10,8 +8,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.interpolation.Interpolatable; -import edu.wpi.first.units.Distance; -import edu.wpi.first.units.Measure; import edu.wpi.first.util.struct.StructSerializable; /** @@ -38,7 +34,7 @@ public class SwerveModulePosition100 /** SwerveModulePosition struct for serialization. */ public static final SwerveModulePosition100Struct struct = new SwerveModulePosition100Struct(); - /** Constructs a SwerveModulePosition with zeros for distance and angle. */ + /** Zero distance and empty angle. */ public SwerveModulePosition100() { } @@ -53,28 +49,6 @@ public SwerveModulePosition100(double distanceMeters, Optional angle this.angle = angle; } - /** - * Constructs a SwerveModulePosition. - * - * @param distance The distance measured by the wheel of the module. - * @param angle The angle of the module. - */ - public SwerveModulePosition100(Measure distance, Optional angle) { - this(distance.in(Meters), angle); - } - - /** */ - public SwerveModulePosition100(double x, double y) { - if (Math.abs(x) < 1e-6 && Math.abs(y) < 1e-6) { - // avoid the garbage rotation. - this.distanceMeters = 0.0; - this.angle = Optional.empty(); - } else { - this.distanceMeters = Math.hypot(x, y); - this.angle = Optional.of(new Rotation2d(x, y)); - } - } - @Override public boolean equals(Object obj) { return obj instanceof SwerveModulePosition100 other diff --git a/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveModuleState100.java b/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveModuleState100.java index 06f6dfed6..9c8a503dc 100644 --- a/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveModuleState100.java +++ b/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveModuleState100.java @@ -1,21 +1,16 @@ package org.team100.lib.motion.drivetrain.kinodynamics; -import static edu.wpi.first.units.Units.MetersPerSecond; +import java.util.Objects; +import java.util.Optional; + +import org.team100.lib.util.Util; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.proto.SwerveModuleStateProto; import edu.wpi.first.math.kinematics.struct.SwerveModuleStateStruct; -import edu.wpi.first.units.Distance; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.Velocity; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; -import java.util.Objects; -import java.util.Optional; - -import org.team100.lib.util.Util; - /** * This is a copy of {@link edu.wpi.first.math.kinematics.SwerveModuleState} but * with second-order fields. @@ -70,27 +65,20 @@ public SwerveModuleState100(double speedMetersPerSecond, Optional an /** * Constructs a SwerveModuleState. * - * @param accelMetersPerSecond The speed of the wheel of the module. + * @param speedMetersPerSecond The speed of the wheel of the module. * @param angle The angle of the module. * @param accelMetersPerSecond_2 The acceleration of the wheel of the module. - * @param angle_2 The angular velocity of the module. + * @param omega The angular velocity of the module. */ - public SwerveModuleState100(double accelMetersPerSecond, Optional angle, double accelMetersPerSecond_2, - double angle_2) { - this.speedMetersPerSecond = accelMetersPerSecond; + public SwerveModuleState100( + double speedMetersPerSecond, + Optional angle, + double accelMetersPerSecond_2, + double omega) { + this.speedMetersPerSecond = speedMetersPerSecond; this.angle = angle; this.accelMetersPerSecond_2 = accelMetersPerSecond_2; - this.omega = angle_2; - } - - /** - * Constructs a SwerveModuleState. - * - * @param speed The speed of the wheel of the module. - * @param angle The angle of the module. - */ - public SwerveModuleState100(Measure> speed, Optional angle) { - this(speed.in(MetersPerSecond), angle); + this.omega = omega; } @Override diff --git a/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/struct/SwerveModuleDeltaStruct.java b/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/struct/SwerveModuleDeltaStruct.java new file mode 100644 index 000000000..4f0f41860 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/struct/SwerveModuleDeltaStruct.java @@ -0,0 +1,48 @@ +package org.team100.lib.motion.drivetrain.kinodynamics.struct; + +import java.nio.ByteBuffer; + +import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleDelta; +import org.team100.lib.util.OptionalRotation2d; + +import edu.wpi.first.util.struct.Struct; + +public class SwerveModuleDeltaStruct implements Struct { + @Override + public Class getTypeClass() { + return SwerveModuleDelta.class; + } + + @Override + public String getTypeString() { + return "struct:SwerveModuleDelta"; + } + + @Override + public int getSize() { + return kSizeDouble + OptionalRotation2d.struct.getSize(); + } + + @Override + public String getSchema() { + return "double distance;OptionalRotation2d angle"; + } + + @Override + public Struct[] getNested() { + return new Struct[] { OptionalRotation2d.struct }; + } + + @Override + public SwerveModuleDelta unpack(ByteBuffer bb) { + double distance = bb.getDouble(); + OptionalRotation2d angle = OptionalRotation2d.struct.unpack(bb); + return new SwerveModuleDelta(distance, angle.get()); + } + + @Override + public void pack(ByteBuffer bb, SwerveModuleDelta value) { + bb.putDouble(value.distanceMeters); + OptionalRotation2d.struct.pack(bb, new OptionalRotation2d(value.angle)); + } +} diff --git a/lib/src/main/java/org/team100/lib/util/DriveUtil.java b/lib/src/main/java/org/team100/lib/util/DriveUtil.java index d272b2583..5ec3d3964 100644 --- a/lib/src/main/java/org/team100/lib/util/DriveUtil.java +++ b/lib/src/main/java/org/team100/lib/util/DriveUtil.java @@ -4,6 +4,7 @@ import org.team100.lib.hid.DriverControl; import org.team100.lib.motion.drivetrain.kinodynamics.FieldRelativeVelocity; +import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleDelta; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModulePosition100; import edu.wpi.first.math.MathUtil; @@ -108,39 +109,56 @@ public static void checkTwist(Twist2d twist) { } /** - * Path between start and end is assumed to be a circular arc so the - * angle of the delta is the angle of the chord between the endpoints, - * i.e. the average angle. This might not be a good assumption if the positional - * control is at a lower level, so that the motion is not uniform during the - * control period. - * - * Note the arc is assumed to be the same length as the chord, though, i.e. the - * angles are assumed to be close to each other. + * The inverse kinematics wants this to represent a geodesic, which + * means that the steering doesn't change between start and end. */ - public static SwerveModulePosition100[] modulePositionDelta( + public static SwerveModuleDelta[] modulePositionDelta( SwerveModulePosition100[] start, SwerveModulePosition100[] end) { if (start.length != end.length) { throw new IllegalArgumentException("Inconsistent number of modules!"); } - SwerveModulePosition100[] newPositions = new SwerveModulePosition100[start.length]; + SwerveModuleDelta[] deltas = new SwerveModuleDelta[start.length]; for (int i = 0; i < start.length; i++) { - SwerveModulePosition100 startModule = start[i]; - SwerveModulePosition100 endModule = end[i]; - // these positions might be null, if the encoder has failed (which can seem to - // happen if the robot is *severely* overrunning). - double deltaM = endModule.distanceMeters - startModule.distanceMeters; - if (startModule.angle.isPresent() && endModule.angle.isPresent()) { - newPositions[i] = new SwerveModulePosition100( - deltaM, - // this change breaks the odometry test on line 66, the 90 degree turn case. - // endModule.angle); - Optional.of(endModule.angle.get().interpolate(startModule.angle.get(), 0.5))); - } else { - newPositions[i] = new SwerveModulePosition100(deltaM, Optional.empty()); - } + deltas[i] = delta(start[i], end[i]); + } + return deltas; + } + + public static SwerveModulePosition100[] modulePositionFromDelta( + SwerveModulePosition100[] initial, + SwerveModuleDelta[] delta) { + SwerveModulePosition100[] new_positions = new SwerveModulePosition100[initial.length]; + for (int i = 0; i < initial.length; ++i) { + new_positions[i] = plus(initial[i], delta[i]); + } + return new_positions; + } + + /** + * Delta for one module, straight line path using the end angle. + */ + public static SwerveModuleDelta delta( + SwerveModulePosition100 start, + SwerveModulePosition100 end) { + double deltaM = end.distanceMeters - start.distanceMeters; + if (end.angle.isPresent()) { + return new SwerveModuleDelta(deltaM, end.angle); + } + // the angle might be empty, if the encoder has failed + // (which can seem to happen if the robot is *severely* overrunning). + return new SwerveModuleDelta(0, Optional.empty()); + } + + public static SwerveModulePosition100 plus( + SwerveModulePosition100 start, + SwerveModuleDelta delta) { + double posM = start.distanceMeters + delta.distanceMeters; + if (delta.angle.isPresent()) { + return new SwerveModulePosition100(posM, delta.angle); } - return newPositions; + // if there's no delta angle, we're not going anywhere. + return start; } private DriveUtil() { 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 d00c66911..36631d362 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 @@ -8,8 +8,11 @@ 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; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; @@ -18,6 +21,280 @@ class SwerveDriveKinematics100Test { private static final double kDelta = 0.001; + @Test + void testCrab() { + // in this case the wheels are assumed to turn immediately to the new + // angle, so this is straight back. + SwerveDriveKinematics100 kinematics = new SwerveDriveKinematics100( + new Translation2d(0.5, 0.5), // 1,1 + new Translation2d(0.5, -0.5), // 1,0 + new Translation2d(-0.5, 0.5), // 0,1 + new Translation2d(-0.5, -0.5) // origin + ); + SwerveModulePosition100[] start = { + new SwerveModulePosition100( + 0, + Optional.of(Rotation2d.fromRadians(Math.PI / 2))), + new SwerveModulePosition100( + 0, + Optional.of(Rotation2d.fromRadians(Math.PI / 2))), + new SwerveModulePosition100( + 0, + Optional.of(Rotation2d.fromRadians(Math.PI / 2))), + new SwerveModulePosition100( + 0.0, + Optional.of(Rotation2d.fromRadians(Math.PI / 2))) + }; + SwerveModulePosition100[] end = { + new SwerveModulePosition100( + 1, + Optional.of(Rotation2d.fromRadians(Math.PI))), + new SwerveModulePosition100( + 1, + Optional.of(Rotation2d.fromRadians(Math.PI))), + new SwerveModulePosition100( + 1, + Optional.of(Rotation2d.fromRadians(Math.PI))), + new SwerveModulePosition100( + 1, + Optional.of(Rotation2d.fromRadians(Math.PI))) + }; + + SwerveModuleDelta[] delta = DriveUtil.modulePositionDelta(start, end); + + assertEquals(1, delta[0].distanceMeters, kDelta); + assertEquals(Math.PI, delta[0].angle.get().getRadians(), kDelta); + + Twist2d twist = kinematics.toTwist2d(delta); + assertEquals(-1, twist.dx, kDelta); + assertEquals(0, twist.dy, kDelta); + assertEquals(0, twist.dtheta, kDelta); + + // it transforms the starting pose correctly + Pose2d pStart = new Pose2d(0.5, 0.5, GeometryUtil.kRotationZero); + Pose2d pEnd = pStart.exp(twist); + assertEquals(-0.5, pEnd.getX(), kDelta); + assertEquals(0.5, pEnd.getY(), kDelta); + assertEquals(0, pEnd.getRotation().getRadians(), kDelta); + } + + @Test + void testCrabInverse() { + // inverse kinematics for the above case + SwerveDriveKinematics100 m_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)); + + Pose2d pStart = new Pose2d(0.5, 0.5, GeometryUtil.kRotationZero); + Pose2d pEnd = new Pose2d(-0.5, 1.5, GeometryUtil.kRotationZero); + Twist2d t = pStart.log(pEnd); + assertEquals(-1, t.dx, kDelta); + assertEquals(1, t.dy, kDelta); + assertEquals(0, t.dtheta, kDelta); + + // the inverse kinematics really just finds the dx and dy for each + // corner; it doesn't know the path the corners take to get there + // so it assumes the corner paths are straight lines. + SwerveModuleDelta[] p = m_kinematics.toSwerveModuleDelta(t); + assertEquals(Math.sqrt(2), p[0].distanceMeters, kDelta); + assertEquals(3 * Math.PI / 4, p[0].angle.get().getRadians(), kDelta); + assertEquals(Math.sqrt(2), p[1].distanceMeters, kDelta); + assertEquals(3 * Math.PI / 4, p[1].angle.get().getRadians(), kDelta); + assertEquals(Math.sqrt(2), p[2].distanceMeters, kDelta); + assertEquals(3 * Math.PI / 4, p[2].angle.get().getRadians(), kDelta); + assertEquals(Math.sqrt(2), p[3].distanceMeters, kDelta); + assertEquals(3 * Math.PI / 4, p[3].angle.get().getRadians(), kDelta); + } + + @Test + void testRollDelta() { + // a rotate-and-move case you can do in your head + // face +x with right rear at origin + // keep origin corner still, rotate around it + // in this maneuver the steering doesn't change + SwerveDriveKinematics100 kinematics = new SwerveDriveKinematics100( + new Translation2d(0.5, 0.5), // 1,1 + new Translation2d(0.5, -0.5), // 1,0 + new Translation2d(-0.5, 0.5), // 0,1 + new Translation2d(-0.5, -0.5) // origin + ); + SwerveModulePosition100[] start = { + new SwerveModulePosition100( + 0, + Optional.of(Rotation2d.fromRadians(3 * Math.PI / 4))), + new SwerveModulePosition100( + 0, + Optional.of(Rotation2d.fromRadians(Math.PI / 2))), + new SwerveModulePosition100( + 0, + Optional.of(Rotation2d.fromRadians(Math.PI))), + new SwerveModulePosition100( + 0.0, + Optional.empty()) + }; + SwerveModulePosition100[] end = { + new SwerveModulePosition100( + Math.sqrt(2) * Math.PI / 2, + Optional.of(Rotation2d.fromRadians(3 * Math.PI / 4))), + new SwerveModulePosition100( + Math.PI / 2, + Optional.of(Rotation2d.fromRadians(Math.PI / 2))), + new SwerveModulePosition100( + Math.PI / 2, + Optional.of(Rotation2d.fromRadians(Math.PI))), + new SwerveModulePosition100( + 0, + Optional.empty()) + }; + + SwerveModuleDelta[] delta = DriveUtil.modulePositionDelta(start, end); + assertEquals(Math.sqrt(2) * Math.PI / 2, delta[0].distanceMeters, kDelta); + assertEquals(3 * Math.PI / 4, delta[0].angle.get().getRadians(), kDelta); + assertEquals(Math.PI / 2, delta[1].distanceMeters, kDelta); + assertEquals(Math.PI / 2, delta[1].angle.get().getRadians(), kDelta); + assertEquals(Math.PI / 2, delta[2].distanceMeters, kDelta); + assertEquals(Math.PI, delta[2].angle.get().getRadians(), kDelta); + assertEquals(0, delta[3].distanceMeters, kDelta); + assertTrue(delta[3].angle.isEmpty()); + + Twist2d twist = kinematics.toTwist2d(delta); + + assertEquals(-0.5 * Math.PI / 2, twist.dx, kDelta); + assertEquals(0.5 * Math.PI / 2, twist.dy, kDelta); + assertEquals(Math.PI / 2, twist.dtheta, kDelta); + } + + @Test + void testRoll() { + // a rotate-and-move case you can do in your head + // face +x with right rear at origin + // keep origin corner still, rotate around it + // in this maneuver the steering doesn't change + SwerveDriveKinematics100 kinematics = new SwerveDriveKinematics100( + new Translation2d(0.5, 0.5), // 1,1 + new Translation2d(0.5, -0.5), // 1,0 + new Translation2d(-0.5, 0.5), // 0,1 + new Translation2d(-0.5, -0.5) // origin + ); + Twist2d twist = kinematics.toTwist2d( + new SwerveModuleDelta( + Math.sqrt(2) * Math.PI / 2, + Optional.of(Rotation2d.fromRadians(3 * Math.PI / 4))), + new SwerveModuleDelta( + Math.PI / 2, + Optional.of(Rotation2d.fromRadians(Math.PI / 2))), + new SwerveModuleDelta( + Math.PI / 2, + Optional.of(Rotation2d.fromRadians(Math.PI))), + // this wheel doesn't move + new SwerveModuleDelta( + 0.0, + Optional.empty())); + + assertEquals(-0.5 * Math.PI / 2, twist.dx, kDelta); + assertEquals(0.5 * Math.PI / 2, twist.dy, kDelta); + assertEquals(Math.PI / 2, twist.dtheta, kDelta); + } + + @Test + void testRollInverse() { + // inverse kinematics for the above case + SwerveDriveKinematics100 m_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)); + + // move diagonally while turning 90 degrees; this should leave + // one of the wheels in place. + Twist2d t = new Twist2d( + -0.5 * Math.PI / 2, + 0.5 * Math.PI / 2, + Math.PI / 2); + + // check that the exp is correct + Pose2d pStart = new Pose2d(0.5, 0.5, GeometryUtil.kRotationZero); + Pose2d pEnd = pStart.exp(t); + assertEquals(-0.5, pEnd.getX(), kDelta); + assertEquals(0.5, pEnd.getY(), kDelta); + assertEquals(Math.PI / 2, pEnd.getRotation().getRadians(), kDelta); + + // check that the twist is really really correct + Twist2d t2 = pStart.log(pEnd); + assertEquals(t, t2); + + SwerveModuleDelta[] p = m_kinematics.toSwerveModuleDelta(t); + assertEquals(Math.sqrt(2) * Math.PI / 2, p[0].distanceMeters, kDelta); + assertEquals(3 * Math.PI / 4, p[0].angle.get().getRadians(), kDelta); + assertEquals(Math.PI / 2, p[1].distanceMeters, kDelta); + assertEquals(Math.PI / 2, p[1].angle.get().getRadians(), kDelta); + assertEquals(Math.PI / 2, p[2].distanceMeters, kDelta); + assertEquals(Math.PI, p[2].angle.get().getRadians(), kDelta); + // this is the one that shouldn't move + assertEquals(0, p[3].distanceMeters, kDelta); + assertTrue(p[3].angle.isEmpty()); + } + + /** + * array order: + * + * frontLeft + * frontRight + * rearLeft + * rearRight + */ + @Test + void testInverse() { + 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)); + assertEquals(1, kinematics.m_inverseKinematics.get(0, 0)); + assertEquals(0, kinematics.m_inverseKinematics.get(0, 1)); + assertEquals(-0.5, kinematics.m_inverseKinematics.get(0, 2)); + assertEquals(0, kinematics.m_inverseKinematics.get(1, 0)); + assertEquals(1, kinematics.m_inverseKinematics.get(1, 1)); + assertEquals(0.5, kinematics.m_inverseKinematics.get(1, 2)); + assertEquals(1, kinematics.m_inverseKinematics.get(2, 0)); + assertEquals(0, kinematics.m_inverseKinematics.get(2, 1)); + assertEquals(0.5, kinematics.m_inverseKinematics.get(2, 2)); + assertEquals(0, kinematics.m_inverseKinematics.get(3, 0)); + assertEquals(1, kinematics.m_inverseKinematics.get(3, 1)); + assertEquals(0.5, kinematics.m_inverseKinematics.get(3, 2)); + } + + /** + * array order: + * + * frontLeft + * frontRight + * rearLeft + * rearRight + */ + @Test + void testForward() { + 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)); + assertEquals(0.25, kinematics.m_forwardKinematics.get(0, 0), kDelta); + assertEquals(0, kinematics.m_forwardKinematics.get(1, 0), kDelta); + assertEquals(-0.25, kinematics.m_forwardKinematics.get(2, 0), kDelta); + assertEquals(0, kinematics.m_forwardKinematics.get(0, 1), kDelta); + assertEquals(0.25, kinematics.m_forwardKinematics.get(1, 1), kDelta); + assertEquals(0.25, kinematics.m_forwardKinematics.get(2, 1), kDelta); + assertEquals(0.25, kinematics.m_forwardKinematics.get(0, 2), kDelta); + assertEquals(0, kinematics.m_forwardKinematics.get(1, 2), kDelta); + assertEquals(0.25, kinematics.m_forwardKinematics.get(2, 2), kDelta); + assertEquals(0, kinematics.m_forwardKinematics.get(0, 3), kDelta); + assertEquals(0.25, kinematics.m_forwardKinematics.get(1, 3), kDelta); + assertEquals(0.25, kinematics.m_forwardKinematics.get(2, 3), kDelta); + } + @Test void testTwistStraight() { SwerveDriveKinematics100 kinematics = new SwerveDriveKinematics100( @@ -27,10 +304,10 @@ void testTwistStraight() { new Translation2d(-0.5, -0.5)); // 0.1m straight ahead, all same. Twist2d twist = kinematics.toTwist2d( - new SwerveModulePosition100(0.1, Optional.of(Rotation2d.fromDegrees(0))), - new SwerveModulePosition100(0.1, Optional.of(Rotation2d.fromDegrees(0))), - new SwerveModulePosition100(0.1, Optional.of(Rotation2d.fromDegrees(0))), - new SwerveModulePosition100(0.1, Optional.of(Rotation2d.fromDegrees(0)))); + new SwerveModuleDelta(0.1, Optional.of(Rotation2d.fromDegrees(0))), + new SwerveModuleDelta(0.1, Optional.of(Rotation2d.fromDegrees(0))), + new SwerveModuleDelta(0.1, Optional.of(Rotation2d.fromDegrees(0))), + new SwerveModuleDelta(0.1, Optional.of(Rotation2d.fromDegrees(0)))); assertEquals(0.1, twist.dx, kDelta); assertEquals(0, twist.dy, kDelta); @@ -46,10 +323,10 @@ void testTwistSpin() { new Translation2d(-0.5, -0.5)); Twist2d twist = kinematics.toTwist2d( - new SwerveModulePosition100(0.1, Optional.of(Rotation2d.fromDegrees(135))), - new SwerveModulePosition100(0.1, Optional.of(Rotation2d.fromDegrees(45))), - new SwerveModulePosition100(0.1, Optional.of(Rotation2d.fromDegrees(-135))), - new SwerveModulePosition100(0.1, Optional.of(Rotation2d.fromDegrees(-45)))); + new SwerveModuleDelta(0.1, Optional.of(Rotation2d.fromDegrees(135))), + new SwerveModuleDelta(0.1, Optional.of(Rotation2d.fromDegrees(45))), + new SwerveModuleDelta(0.1, Optional.of(Rotation2d.fromDegrees(-135))), + new SwerveModuleDelta(0.1, Optional.of(Rotation2d.fromDegrees(-45)))); assertEquals(0, twist.dx, kDelta); assertEquals(0, twist.dy, kDelta); @@ -65,10 +342,10 @@ void testWithTime() { new Translation2d(-0.5, -0.5)); // 0.1m straight ahead, all same. Twist2d twist = kinematics.toTwist2d( - new SwerveModulePosition100(0.1, Optional.of(Rotation2d.fromDegrees(0))), - new SwerveModulePosition100(0.1, Optional.of(Rotation2d.fromDegrees(0))), - new SwerveModulePosition100(0.1, Optional.of(Rotation2d.fromDegrees(0))), - new SwerveModulePosition100(0.1, Optional.of(Rotation2d.fromDegrees(0)))); + new SwerveModuleDelta(0.1, Optional.of(Rotation2d.fromDegrees(0))), + new SwerveModuleDelta(0.1, Optional.of(Rotation2d.fromDegrees(0))), + new SwerveModuleDelta(0.1, Optional.of(Rotation2d.fromDegrees(0))), + new SwerveModuleDelta(0.1, Optional.of(Rotation2d.fromDegrees(0)))); assertEquals(0.1, twist.dx, kDelta); assertEquals(0, twist.dy, kDelta); @@ -121,13 +398,12 @@ void testStraightLineForwardKinematics() { // test forward kinematics going in a @Test void testStraightLineForwardKinematicsWithDeltas() { // test forward kinematics going in a straight line - SwerveModulePosition100 delta = new SwerveModulePosition100(5.0, Optional.of(Rotation2d.fromDegrees(0.0))); - var twist = m_kinematics.toTwist2d(delta, delta, delta, delta); + SwerveModuleDelta delta = new SwerveModuleDelta(5.0, Optional.of(Rotation2d.fromDegrees(0.0))); + Twist2d twist = m_kinematics.toTwist2d(delta, delta, delta, delta); - assertAll( - () -> assertEquals(5.0, twist.dx, kEpsilon), - () -> assertEquals(0.0, twist.dy, kEpsilon), - () -> assertEquals(0.0, twist.dtheta, kEpsilon)); + assertEquals(5.0, twist.dx, kEpsilon); + assertEquals(0.0, twist.dy, kEpsilon); + assertEquals(0.0, twist.dtheta, kEpsilon); } @Test @@ -159,13 +435,14 @@ void testStraightStrafeForwardKinematics() { @Test void testStraightStrafeForwardKinematicsWithDeltas() { - SwerveModulePosition100 delta = new SwerveModulePosition100(5.0, Optional.of(Rotation2d.fromDegrees(90.0))); - var twist = m_kinematics.toTwist2d(delta, delta, delta, delta); - - assertAll( - () -> assertEquals(0.0, twist.dx, kEpsilon), - () -> assertEquals(5.0, twist.dy, kEpsilon), - () -> assertEquals(0.0, twist.dtheta, kEpsilon)); + SwerveModuleDelta delta = new SwerveModuleDelta( + 5.0, + Optional.of(Rotation2d.fromDegrees(90.0))); + Twist2d twist = m_kinematics.toTwist2d(delta, delta, delta, delta); + + assertEquals(0.0, twist.dx, kEpsilon); + assertEquals(5.0, twist.dy, kEpsilon); + assertEquals(0.0, twist.dtheta, kEpsilon); } @Test @@ -250,20 +527,20 @@ void testTurnInPlaceForwardKinematics() { @Test void testTurnInPlaceForwardKinematicsWithDeltas() { - SwerveModulePosition100 flDelta = new SwerveModulePosition100(106.629, + SwerveModuleDelta flDelta = new SwerveModuleDelta(106.629, Optional.of(Rotation2d.fromDegrees(135))); - SwerveModulePosition100 frDelta = new SwerveModulePosition100(106.629, Optional.of(Rotation2d.fromDegrees(45))); - SwerveModulePosition100 blDelta = new SwerveModulePosition100(106.629, + SwerveModuleDelta frDelta = new SwerveModuleDelta(106.629, + Optional.of(Rotation2d.fromDegrees(45))); + SwerveModuleDelta blDelta = new SwerveModuleDelta(106.629, Optional.of(Rotation2d.fromDegrees(-135))); - SwerveModulePosition100 brDelta = new SwerveModulePosition100(106.629, + SwerveModuleDelta brDelta = new SwerveModuleDelta(106.629, Optional.of(Rotation2d.fromDegrees(-45))); - var twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta); + Twist2d twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta); - assertAll( - () -> assertEquals(0.0, twist.dx, kEpsilon), - () -> assertEquals(0.0, twist.dy, kEpsilon), - () -> assertEquals(2 * Math.PI, twist.dtheta, 0.1)); + assertEquals(0.0, twist.dx, kEpsilon); + assertEquals(0.0, twist.dy, kEpsilon); + assertEquals(2 * Math.PI, twist.dtheta, 0.1); } @Test @@ -296,15 +573,16 @@ void testOffCenterCORRotationForwardKinematics() { @Test void testOffCenterCORRotationForwardKinematicsWithDeltas() { - SwerveModulePosition100 flDelta = new SwerveModulePosition100(0.0, Optional.of(Rotation2d.fromDegrees(0.0))); - SwerveModulePosition100 frDelta = new SwerveModulePosition100(150.796, + SwerveModuleDelta flDelta = new SwerveModuleDelta(0.0, + Optional.of(Rotation2d.fromDegrees(0.0))); + SwerveModuleDelta frDelta = new SwerveModuleDelta(150.796, Optional.of(Rotation2d.fromDegrees(0.0))); - SwerveModulePosition100 blDelta = new SwerveModulePosition100(150.796, + SwerveModuleDelta blDelta = new SwerveModuleDelta(150.796, Optional.of(Rotation2d.fromDegrees(-90))); - SwerveModulePosition100 brDelta = new SwerveModulePosition100(213.258, + SwerveModuleDelta brDelta = new SwerveModuleDelta(213.258, Optional.of(Rotation2d.fromDegrees(-45))); - var twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta); + Twist2d twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta); /* * We already know that our omega should be 2π from the previous test. Next, we @@ -351,16 +629,16 @@ void testOffCenterCORRotationAndTranslationForwardKinematics() { @Test void testOffCenterCORRotationAndTranslationForwardKinematicsWithDeltas() { - SwerveModulePosition100 flDelta = new SwerveModulePosition100(23.43, + SwerveModuleDelta flDelta = new SwerveModuleDelta(23.43, Optional.of(Rotation2d.fromDegrees(-140.19))); - SwerveModulePosition100 frDelta = new SwerveModulePosition100(23.43, + SwerveModuleDelta frDelta = new SwerveModuleDelta(23.43, Optional.of(Rotation2d.fromDegrees(-39.81))); - SwerveModulePosition100 blDelta = new SwerveModulePosition100(54.08, + SwerveModuleDelta blDelta = new SwerveModuleDelta(54.08, Optional.of(Rotation2d.fromDegrees(-109.44))); - SwerveModulePosition100 brDelta = new SwerveModulePosition100(54.08, + SwerveModuleDelta brDelta = new SwerveModuleDelta(54.08, Optional.of(Rotation2d.fromDegrees(-70.56))); - var twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta); + Twist2d twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta); /* * From equation (13.17), we know that chassis motion is th dot product of the diff --git a/lib/src/test/java/org/team100/lib/util/DriveUtilTest.java b/lib/src/test/java/org/team100/lib/util/DriveUtilTest.java index 0c9ff1d81..91dfd062f 100644 --- a/lib/src/test/java/org/team100/lib/util/DriveUtilTest.java +++ b/lib/src/test/java/org/team100/lib/util/DriveUtilTest.java @@ -2,8 +2,18 @@ import static org.junit.jupiter.api.Assertions.assertEquals; +import java.util.Optional; +import java.util.Random; + import org.junit.jupiter.api.Test; +import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.hid.DriverControl; +import org.team100.lib.motion.drivetrain.kinodynamics.SwerveDriveKinematics100; +import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleDelta; +import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModulePosition100; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; class DriveUtilTest { private static final double kDelta = 0.001; @@ -34,4 +44,72 @@ void testClampTwist() { } } + @Test + void testRoundTripModuleDeltas() { + SwerveDriveKinematics100 m_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)); + + { + // straight diagonal path + Twist2d t = new Twist2d(1, 1, 0); + SwerveModuleDelta[] p = m_kinematics.toSwerveModuleDelta(t); + Twist2d t2 = m_kinematics.toTwist2d(p); + assertEquals(t, t2); + } + { + // turning and moving + Twist2d t = new Twist2d(1, 1, 1); + SwerveModuleDelta[] p = m_kinematics.toSwerveModuleDelta(t); + Twist2d t2 = m_kinematics.toTwist2d(p); + assertEquals(t, t2); + } + Random random = new Random(0); + for (int i = 0; i < 500; ++i) { + // inverse always works + Twist2d t = new Twist2d( + random.nextDouble(), + random.nextDouble(), + random.nextDouble()); + SwerveModuleDelta[] p = m_kinematics.toSwerveModuleDelta(t); + Twist2d t2 = m_kinematics.toTwist2d(p); + assertEquals(t, t2); + } + } + + @Test + void testOneModule() { + { + SwerveModulePosition100 start = new SwerveModulePosition100( + 0, Optional.of(GeometryUtil.kRotationZero)); + SwerveModulePosition100 end = new SwerveModulePosition100( + 0, Optional.of(GeometryUtil.kRotationZero)); + SwerveModuleDelta delta = DriveUtil.delta(start, end); + assertEquals(0, delta.distanceMeters, kDelta); + assertEquals(0, delta.angle.get().getRadians(), kDelta); + } + { + SwerveModulePosition100 start = new SwerveModulePosition100( + 0, Optional.of(GeometryUtil.kRotationZero)); + SwerveModulePosition100 end = new SwerveModulePosition100( + 1, Optional.of(GeometryUtil.kRotationZero)); + SwerveModuleDelta delta = DriveUtil.delta(start, end); + assertEquals(1, delta.distanceMeters, kDelta); + assertEquals(0, delta.angle.get().getRadians(), kDelta); + } + { + // this ignores the initial zero rotation, and acts as if + // the path is at 90 degrees the whole time. + SwerveModulePosition100 start = new SwerveModulePosition100( + 0, Optional.of(GeometryUtil.kRotationZero)); + SwerveModulePosition100 end = new SwerveModulePosition100( + 1, Optional.of(GeometryUtil.kRotation90)); + SwerveModuleDelta delta = DriveUtil.delta(start, end); + assertEquals(1, delta.distanceMeters, kDelta); + assertEquals(1.571, delta.angle.get().getRadians(), kDelta); + } + } + } diff --git a/lib/vendordeps/ReduxLib_2024.json b/lib/vendordeps/ReduxLib_2024.json index f8e048e7e..f694966df 100644 --- a/lib/vendordeps/ReduxLib_2024.json +++ b/lib/vendordeps/ReduxLib_2024.json @@ -1,55 +1,55 @@ { - "fileName": "ReduxLib_2024.json", - "name": "ReduxLib", - "version": "2024.3.0", - "frcYear": 2024, - "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", - "mavenUrls": [ - "https://maven.reduxrobotics.com/" - ], - "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2024.json", - "javaDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-java", - "version": "2024.3.0" - } - ], - "jniDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-driver", - "version": "2024.3.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm32", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-cpp", - "version": "2024.3.0", - "libName": "ReduxLib-cpp", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm32", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - } - ] + "fileName": "ReduxLib_2024.json", + "name": "ReduxLib", + "version": "2024.3.1", + "frcYear": 2024, + "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", + "mavenUrls": [ + "https://maven.reduxrobotics.com/" + ], + "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2024.json", + "javaDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-java", + "version": "2024.3.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-driver", + "version": "2024.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-cpp", + "version": "2024.3.1", + "libName": "ReduxLib-cpp", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ] } \ No newline at end of file diff --git a/raspberry_pi/.vscode/launch.json b/raspberry_pi/.vscode/launch.json deleted file mode 100644 index 6b76b4fab..000000000 --- a/raspberry_pi/.vscode/launch.json +++ /dev/null @@ -1,15 +0,0 @@ -{ - // Use IntelliSense to learn about possible attributes. - // Hover to view descriptions of existing attributes. - // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 - "version": "0.2.0", - "configurations": [ - { - "name": "Python Debugger: Current File", - "type": "debugpy", - "request": "launch", - "program": "${file}", - "console": "integratedTerminal" - } - ] -} \ No newline at end of file diff --git a/raspberry_pi/.vscode/settings.json b/raspberry_pi/.vscode/settings.json index c04b7e64f..68277bc87 100644 --- a/raspberry_pi/.vscode/settings.json +++ b/raspberry_pi/.vscode/settings.json @@ -10,11 +10,8 @@ "python.testing.pytestEnabled": false, "python.testing.unittestEnabled": true, "workbench.colorTheme": "Default Light Modern", - "mypy-type-checker.args": [ - "--strict", - ], "editor.minimap.enabled": false, - "python.analysis.typeCheckingMode": "strict", + "python.analysis.typeCheckingMode": "off", "mypy-type-checker.reportingScope": "workspace", "markdown.validate.enabled": true, "tidyMarkdown.disableFormatter": true, @@ -23,5 +20,6 @@ "**/.gradle": true, "**/.mypy_cache": true, "**/*.pyc": true - } + }, + "python.analysis.autoImportCompletions": true } \ No newline at end of file diff --git a/raspberry_pi/app/gtsam/README.md b/raspberry_pi/app/gtsam/README.md deleted file mode 100644 index 43bd55ee1..000000000 --- a/raspberry_pi/app/gtsam/README.md +++ /dev/null @@ -1,12 +0,0 @@ -# GTSAM - -Georgia Tech Smoothing And Mapping (GTSAM) is a framework for "factor graph" computations, which uses iterative linearization and LM solving for high-dimensionality problems in robotics. - -We use GTSAM to solve two problems: - - -* Incremental sensor fusion ("smoothing"). Using recent camera, gyro, and odometry measurements, find the current robot pose, and the uncertainty in that pose. This is a more sophisticated version of the WPILib "Pose Estimator". - -* Camera alignment and calibration. With many cameras, it is a pain to get all the offsets and distortion measurements just right. Instead, we can provide a lot of input using camera, gyro, and odometry, and GTSAM can solve for offset and distortion consistent with the gyro and odometry measurements. - -For more on the design of the Team 100 GTSAM applications, see the [design doc](https://docs.google.com/document/d/1z58ovdQ_rDQaW90we5c4xnuo2mpjdJYAjIfBhLHy2UQ/edit). \ No newline at end of file diff --git a/raspberry_pi/app/localization/tag_detector.py b/raspberry_pi/app/localization/tag_detector.py index c07e62300..af2417fae 100644 --- a/raspberry_pi/app/localization/tag_detector.py +++ b/raspberry_pi/app/localization/tag_detector.py @@ -7,8 +7,7 @@ import numpy as np from cv2 import undistortImagePoints from numpy.typing import NDArray -from robotpy_apriltag import (AprilTagDetection, AprilTagDetector, - AprilTagPoseEstimator) +from robotpy_apriltag import AprilTagDetection, AprilTagDetector, AprilTagPoseEstimator from typing_extensions import override from app.camera.camera_protocol import Camera, Request, Size @@ -97,6 +96,11 @@ def analyze(self, req: Request) -> None: # UNDISTORT EACH ITEM # undistortPoints is at least 10X faster than undistort on the whole image. + # the order here is: + # lower left + # lower right + # upper right + # upper left corners: tuple[ float, float, float, float, float, float, float, float ] = result_item.getCorners((0, 0, 0, 0, 0, 0, 0, 0)) diff --git a/raspberry_pi/app/pose_estimator/README.md b/raspberry_pi/app/pose_estimator/README.md new file mode 100644 index 000000000..d105efbae --- /dev/null +++ b/raspberry_pi/app/pose_estimator/README.md @@ -0,0 +1,11 @@ +# GTSAM + +Georgia Tech Smoothing And Mapping (GTSAM) is a framework for "factor graph" computations, which uses iterative linearization and LM solving for high-dimensionality problems in robotics. + +We use GTSAM to solve two problems: + +* __Incremental sensor fusion ("smoothing").__ Using recent camera, gyro, and odometry measurements, find the current robot pose, and the uncertainty in that pose. This is a more sophisticated version of the WPILib "Pose Estimator". + +* __Camera alignment and calibration.__ With many cameras, it is a pain to get all the offsets and distortion measurements just right. Instead, we can provide a lot of input using camera, gyro, and odometry, and GTSAM can solve for offset and distortion consistent with the gyro and odometry measurements. + +For more on the design of the Team 100 GTSAM applications, see the [design doc](https://docs.google.com/document/d/1z58ovdQ_rDQaW90we5c4xnuo2mpjdJYAjIfBhLHy2UQ/edit). diff --git a/raspberry_pi/app/pose_estimator/accelerometer.py b/raspberry_pi/app/pose_estimator/accelerometer.py new file mode 100644 index 000000000..1e0f46ad2 --- /dev/null +++ b/raspberry_pi/app/pose_estimator/accelerometer.py @@ -0,0 +1,108 @@ +"""Factory for accelerometer factors. + +The GTSAM version of this idea is kind of buried in the +IMUFactor. + +See https://github.com/borglab/gtsam/blob/develop/doc/ImuFactor.pdf + +This is simpler. + +TODO: measure some accelerometers to find a reasonable noise model. + +The Redux guy says his thing is an "instantaneous" measurement but the +reality is that all sensors do some sort of filtering, so there's some +investigation to do here. + +For example, the LSM6DSOX has two stages, an analog stage to prevent +ADC aliasing which probably runs at 1.5khz, and a digital stage that runs +at ODR/2 (Nyquist) or slower. + +Eeshwar says they do 1ms which kind of implies a 1khz filter, so when +we get CAN packets (at 50 or 100hz) they are only doing points near the +CAN output, not averaging or filtering in a useful way at all. + +In that case, we should supplement the Redux box with something like +the LSM6DSOX with an ODR that matches our actual accel need. +""" + +# pylint: disable=C0103,E0611,E1101,R0913 + +import gtsam +import numpy as np +from gtsam.noiseModel import Base as SharedNoiseModel + +from app.pose_estimator.numerical_derivative import ( + numericalDerivative31, + numericalDerivative32, + numericalDerivative33, +) + + +def h( + p0: gtsam.Pose2, p1: gtsam.Pose2, p2: gtsam.Pose2, dt1: float, dt2: float +) -> np.ndarray: + """Estimated tangential acceleration at p2. + Computes the second-order backward finite difference, in tangent space. + TODO: something better than the dt's here? + In 2d, the translation velocity and angular velocity are always perpendicular + so the coriolis force is always -2*omega*v""" + twist1 = p0.logmap(p1) + v1 = twist1 / dt1 + twist2 = p1.logmap(p2) + v2 = twist2 / dt2 + # the translational velocity in the manifold + v = np.copy(v2) + v[2] = 0 + # the rotational velocity in the manifold + omega = np.copy(v2) + omega[0] = 0 + omega[1] = 0 + # coriolis accel + coriolis = -2.0 * np.cross(omega, v) + inertial = (v2 - v1) / dt2 + return coriolis + inertial + + +def h_H( + measured: np.ndarray, + p0: gtsam.Pose2, + p1: gtsam.Pose2, + p2: gtsam.Pose2, + dt1: float, + dt2: float, + H: list[np.ndarray], +): + """Error function including Jacobians.""" + result = h(p0, p1, p2, dt1, dt2) - measured + if H is not None: + H[0] = numericalDerivative31(lambda x, y, z: h(x, y, z, dt1, dt2), p0, p1, p2) + H[1] = numericalDerivative32(lambda x, y, z: h(x, y, z, dt1, dt2), p0, p1, p2) + H[2] = numericalDerivative33(lambda x, y, z: h(x, y, z, dt1, dt2), p0, p1, p2) + return result + + +def factor( + x: float, + y: float, + dt1: float, + dt2: float, + model: SharedNoiseModel, + p0_key: gtsam.Symbol, + p1_key: gtsam.Symbol, + p2_key: gtsam.Symbol, +) -> gtsam.NonlinearFactor: + # TODO: something other than dt1 and dt2? + # this is the robot-frame acceleration vector. + measured = np.array([x, y, 0]) + + def error_func( + this: gtsam.CustomFactor, v: gtsam.Values, H: list[np.ndarray] + ) -> np.ndarray: + p0: gtsam.Pose2 = v.atPose2(this.keys()[0]) + p1: gtsam.Pose2 = v.atPose2(this.keys()[1]) + p2: gtsam.Pose2 = v.atPose2(this.keys()[2]) + return h_H(measured, p0, p1, p2, dt1, dt2, H) + + return gtsam.CustomFactor( + model, gtsam.KeyVector([p0_key, p1_key, p2_key]), error_func + ) diff --git a/raspberry_pi/app/pose_estimator/apriltag.py b/raspberry_pi/app/pose_estimator/apriltag.py new file mode 100644 index 000000000..304aadaae --- /dev/null +++ b/raspberry_pi/app/pose_estimator/apriltag.py @@ -0,0 +1,87 @@ +"""Factory for AprilTag detectors. + +One type of factor uses constant camera parameters, the other treats +camera parameters as a model variable, for calibration. + +This also serves as a template for parameterized estimators -- in this +case, the parameter is the landmark location. +""" + +# pylint: disable=C0103,E0611,E1101,R0913 +from typing import Callable + +import gtsam +import numpy as np +from gtsam.noiseModel import Base as SharedNoiseModel + +from app.pose_estimator.numerical_derivative import ( + numericalDerivative31, + numericalDerivative32, + numericalDerivative33, +) + +# camera "zero" is facing +z; this turns it to face +x +CAM_COORD = gtsam.Pose3( + gtsam.Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])), gtsam.Point3(0, 0, 0) +) + + +def h_fn( + landmark: np.ndarray, +) -> Callable[[gtsam.Pose2, gtsam.Pose3, gtsam.Cal3DS2], np.ndarray]: + """landmark is field position of a tag corner.""" + + def h(p0: gtsam.Pose2, offset: gtsam.Pose3, calib: gtsam.Cal3DS2) -> np.ndarray: + """estimated pixel location of the target""" + # this is x-forward z-up + offset_pose = gtsam.Pose3(p0).compose(offset) + # this is z-forward y-down + camera_pose = offset_pose.compose(CAM_COORD) + camera = gtsam.PinholeCameraCal3DS2(camera_pose, calib) + return camera.project(landmark) + + return h + + +def h_H( + landmark: np.ndarray, + measured: np.array, + p0: gtsam.Pose2, + offset: gtsam.Pose3, + calib: gtsam.Cal3DS2, + H: list[np.ndarray], +) -> np.ndarray: + h = h_fn(landmark) + result = h(p0, offset, calib) - measured + if H is not None: + H[0] = numericalDerivative31(h, p0, offset, calib) + H[1] = numericalDerivative32(h, p0, offset, calib) + H[2] = numericalDerivative33(h, p0, offset, calib) + return result + + +def factor( + landmark: np.ndarray, + measured: np.ndarray, + model: SharedNoiseModel, + p0_key: gtsam.Symbol, + offset_key: gtsam.Symbol, + calib_key: gtsam.Symbol, +) -> gtsam.NonlinearFactor: + """landmark: field coordinates + measured: pixel coordinate""" + + def error_func( + this: gtsam.CustomFactor, v: gtsam.Values, H: list[np.ndarray] + ) -> np.ndarray: + p0: gtsam.Pose2 = v.atPose2(this.keys()[0]) + offset: gtsam.Pose3 = v.atPose3(this.keys()[1]) + calib: gtsam.Cal3DS2 = v.atCal3DS2(this.keys()[2]) + + return h_H(landmark, measured, p0, offset, calib, H) + + return gtsam.CustomFactor( + model, + gtsam.KeyVector([p0_key, offset_key, calib_key]), + error_func, + ) diff --git a/raspberry_pi/app/pose_estimator/calibrate.py b/raspberry_pi/app/pose_estimator/calibrate.py new file mode 100644 index 000000000..250c2791a --- /dev/null +++ b/raspberry_pi/app/pose_estimator/calibrate.py @@ -0,0 +1,6 @@ +""" Run a GTSAM model that calibrates the camera.""" + + +class Calibrate: + def __init__(self) -> None: + pass diff --git a/raspberry_pi/app/pose_estimator/drive_util.py b/raspberry_pi/app/pose_estimator/drive_util.py new file mode 100644 index 000000000..fdce6ef44 --- /dev/null +++ b/raspberry_pi/app/pose_estimator/drive_util.py @@ -0,0 +1,50 @@ +# pylint: disable=C0200,R0903 + +from wpimath.geometry import Rotation2d + +from app.pose_estimator.swerve_module_delta import SwerveModuleDelta +from app.pose_estimator.swerve_module_position import ( + OptionalRotation2d, + SwerveModulePosition100, +) + + +class DriveUtil: + @staticmethod + def module_position_delta( + start: list[SwerveModulePosition100], end: list[SwerveModulePosition100] + ) -> list[SwerveModuleDelta]: + """Uses the end angle to cover the whole interval.""" + deltas: list[SwerveModuleDelta] = [] + for i in range(len(start)): + deltas.append(DriveUtil._delta(start[i], end[i])) + return deltas + + @staticmethod + def module_position_from_delta( + start: list[SwerveModulePosition100], delta: list[SwerveModuleDelta] + ) -> list [SwerveModulePosition100]: + new_positions: list[SwerveModulePosition100] = [] + for i in range(len(start)): + new_positions.append(DriveUtil._plus(start[i], delta[i])) + return new_positions + + @staticmethod + def _delta( + start: SwerveModulePosition100, end: SwerveModulePosition100 + ) -> SwerveModuleDelta: + delta_m: float = end.distance_m - start.distance_m + if end.angle.present: + return SwerveModuleDelta(delta_m, end.angle) + # these positions might be null, if the encoder has failed (which can seem to + # happen if the robot is *severely* overrunning). + return SwerveModuleDelta(0, OptionalRotation2d(False, Rotation2d(0))) + + def _plus( + start:SwerveModulePosition100, delta:SwerveModuleDelta) -> SwerveModulePosition100: + new_distance_m = start.distance_m + delta.distance_m + if delta.angle.present: + return SwerveModulePosition100(new_distance_m, delta.angle) + # if there's no angle, we're not going anywhere. + return start + \ No newline at end of file diff --git a/raspberry_pi/app/pose_estimator/estimate.py b/raspberry_pi/app/pose_estimator/estimate.py new file mode 100644 index 000000000..ecd74a566 --- /dev/null +++ b/raspberry_pi/app/pose_estimator/estimate.py @@ -0,0 +1,117 @@ +""" Run a GTSAM model that uses constant camera calibration. + +The general idea is to run a free-running loop, polling for inputs. +Those inputs are asserted using one of the input methods here. +""" + +# pylint: disable=C0301,E0611,E1101,R0903 + +import gtsam +import numpy as np +from gtsam import noiseModel +from gtsam.symbol_shorthand import X +from wpimath.geometry import Pose2d, Rotation2d, Translation2d, Twist2d + +import app.pose_estimator.odometry as odometry +from app.pose_estimator.drive_util import DriveUtil +from app.pose_estimator.swerve_drive_kinematics import SwerveDriveKinematics100 +from app.pose_estimator.swerve_module_delta import SwerveModuleDelta +from app.pose_estimator.swerve_module_position import (OptionalRotation2d, + SwerveModulePosition100) + +# odometry noise. TODO: real noise estimate. +NOISE3 = noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.1])) + + +class Estimate: + def __init__(self) -> None: + """Initialize the model + initial module positions are at their origins. + TODO: some other initial positions?""" + self.isam: gtsam.FixedLagSmoother = self.make_smoother() + self.result: gtsam.Values + # between updates we accumulate inputs here + self.new_factors = gtsam.NonlinearFactorGraph() + self.new_values = gtsam.Values() + self.new_timestamps = {} + # TODO: correct wheelbase etc + self.kinematics = SwerveDriveKinematics100( + [ + Translation2d(0.5, 0.5), + Translation2d(0.5, -0.5), + Translation2d(-0.5, 0.5), + Translation2d(-0.5, -0.5), + ] + ) + # TODO: reset position + self.positions = [ + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + ] + self.timestamp: int = 0 + + def init(self, initial_pose: Pose2d) -> None: + """Add a state at zero""" + prior_mean = gtsam.Pose2(initial_pose.X(), initial_pose.Y(), initial_pose.rotation().radians()) + prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) + self.new_factors.push_back( + gtsam.PriorFactorPose2(X(0), prior_mean, prior_noise) + ) + self.new_values.insert(X(0), prior_mean) + self.new_timestamps[X(0)] = 0.0 + + def odometry(self, time_us: int, positions: list[SwerveModulePosition100]) -> None: + """Add an odometry measurement + + time_us: network tables timestamp in integer microseconds. + """ + # odometry is not guaranteed to arrive in order, but for now, it is. + # TODO: out-of-order odometry + # each odometry update maps exactly to a "between" factor + # remember a "twist" is a robot-relative concept + deltas: list[SwerveModuleDelta] = DriveUtil.module_position_delta( + self.positions, positions + ) + # this is the tangent-space (twist) measurement + measurement: Twist2d = self.kinematics.to_twist_2d(deltas) + # each odometry update makes a new state + # if you're using the batch smoother, the initial value almost doesn't matter: + # TODO: use the previous pose as the initial value + initial_value = gtsam.Pose2() + self.new_values.insert(X(time_us), initial_value) + self.new_timestamps[X(time_us)] = time_us + + self.new_factors.add( + odometry.factor( + measurement, + NOISE3, + X(self.timestamp), + X(time_us), + ) + ) + + self.positions = positions + self.timestamp = time_us + + def update(self) -> None: + """Run the solver""" + self.isam.update(self.new_factors, self.new_values, self.new_timestamps) + self.result = self.isam.calculateEstimate() # type: ignore + # reset the accumulators + self.new_factors.resize(0) + self.new_values.clear() + self.new_timestamps.clear() + + def make_smoother(self) -> gtsam.FixedLagSmoother: + # experimenting with the size of the lag buffer. + # the python odometry factor is intolerably slow + # but the native one is quite fast. + # i'm not sure what sort of window we really need; maybe + # just long enough to span periods of blindness, so, + # like a second or two? + lag_s = 30 + lag_us = lag_s * 1e6 + lm_params = gtsam.LevenbergMarquardtParams() + return gtsam.BatchFixedLagSmoother(lag_us, lm_params) diff --git a/raspberry_pi/app/pose_estimator/gyro.py b/raspberry_pi/app/pose_estimator/gyro.py new file mode 100644 index 000000000..6597defd9 --- /dev/null +++ b/raspberry_pi/app/pose_estimator/gyro.py @@ -0,0 +1,51 @@ +"""Factory for gyro factors. + +The Redux gyro provides integrated position and high-bandwidth +instantaneous velocity, but not average velocity over the report +period, so we use position measurement and calculate deltas, +just like we do for odometry. You can think of the gyro as a +rotation-only odometry factor. +""" + +# pylint: disable=C0103,E0611,E1101 + +import gtsam +import numpy as np +from gtsam.noiseModel import Base as SharedNoiseModel + +from app.pose_estimator.numerical_derivative import (numericalDerivative21, + numericalDerivative22) + + +def h(p0: gtsam.Pose2, p1: gtsam.Pose2) -> np.ndarray: + """Estimated change in yaw, as a 1x1 array.""" + # logmap in this case is really just "minus" + # since the rotation manifold is the tangent space. + return p0.rotation().logmap(p1.rotation()) + + +def h_H( + measured: np.ndarray, p0: gtsam.Pose2, p1: gtsam.Pose2, H: list[np.ndarray] +) -> np.ndarray: + """measured: 1x1 array""" + result = h(p0, p1) - measured + if H is not None: + H[0] = numericalDerivative21(h, p0, p1) + H[1] = numericalDerivative22(h, p0, p1) + return result + + +def factor( + measured: np.ndarray, + model: SharedNoiseModel, + p0_key: gtsam.Symbol, + p1_key: gtsam.Symbol, +) -> gtsam.NonlinearFactor: + def error_func( + this: gtsam.CustomFactor, v: gtsam.Values, H: list[np.ndarray] + ) -> np.ndarray: + p0: gtsam.Pose2 = v.atPose2(this.keys()[0]) + p1: gtsam.Pose2 = v.atPose2(this.keys()[1]) + return h_H(measured, p0, p1, H) + + return gtsam.CustomFactor(model, gtsam.KeyVector([p0_key, p1_key]), error_func) diff --git a/raspberry_pi/app/pose_estimator/numerical_derivative.py b/raspberry_pi/app/pose_estimator/numerical_derivative.py new file mode 100644 index 000000000..913f39946 --- /dev/null +++ b/raspberry_pi/app/pose_estimator/numerical_derivative.py @@ -0,0 +1,98 @@ +# pylint: disable=C0103,C0114,C0116,E0611,R0913 +# mypy: disable-error-code="import-untyped" +# see numericalDerivative.h + +# pybind wants to wrap concrete types, which would have been +# a whole lot of them, so i reimplemented the part of this that +# i needed, using the python approach to "generic" typing. + +from typing import Callable, TypeVar +import numpy as np + +Y = TypeVar("Y") +X = TypeVar("X") +X1 = TypeVar("X1") +X2 = TypeVar("X2") +X3 = TypeVar("X3") +X4 = TypeVar("X4") +X5 = TypeVar("X5") +X6 = TypeVar("X6") + + +def local(a: Y, b: Y) -> np.ndarray: + if type(a) is not type(b): + raise TypeError(f"a {type(a)} b {type(b)}") + if isinstance(a, np.ndarray): + return b - a + if isinstance(a, (float, int)): + return np.array([[b - a]]) # type:ignore + # there is no common superclass for Y + return a.localCoordinates(b) # type:ignore + + +def retract(a, b): + if isinstance(a, (np.ndarray, float, int)): + return a + b + return a.retract(b) + + +def numericalDerivative11(h: Callable[[X], Y], x: X, delta=1e-5) -> np.ndarray: + hx: Y = h(x) + zeroY = local(hx, hx) + m = zeroY.shape[0] + zeroX = local(x, x) + N = zeroX.shape[0] + dx = np.zeros(N) + H = np.zeros((m, N)) + factor: float = 1.0 / (2.0 * delta) + for j in range(N): + dx[j] = delta + dy1 = local(hx, h(retract(x, dx))) + dx[j] = -delta + dy2 = local(hx, h(retract(x, dx))) + dx[j] = 0 + H[:, j] = (dy1 - dy2) * factor + return H + + +def numericalDerivative21( + h: Callable[[X1, X2], Y], x1: X1, x2: X2, delta=1e-5 +) -> np.array: + return numericalDerivative11(lambda x: h(x, x2), x1, delta) + + +def numericalDerivative22( + h: Callable[[X1, X2], Y], x1: X1, x2: X2, delta=1e-5 +) -> np.array: + return numericalDerivative11(lambda x: h(x1, x), x2, delta) + + +def numericalDerivative31( + h: Callable[[X1, X2, X3], Y], x1: X1, x2: X2, x3: X3, delta=1e-5 +) -> np.array: + return numericalDerivative11(lambda x: h(x, x2, x3), x1, delta) + + +def numericalDerivative32( + h: Callable[[X1, X2, X3], Y], x1: X1, x2: X2, x3: X3, delta=1e-5 +) -> np.array: + return numericalDerivative11(lambda x: h(x1, x, x3), x2, delta) + + +def numericalDerivative33( + h: Callable[[X1, X2, X3], Y], x1: X1, x2: X2, x3: X3, delta=1e-5 +) -> np.array: + return numericalDerivative11(lambda x: h(x1, x2, x), x3, delta) + + +def numericalDerivative61( + h: Callable[[X1, X2, X3, X4, X5, X6], Y], + x1: X1, + x2: X2, + x3: X3, + x4: X4, + x5: X5, + x6: X6, + delta=1e-5, +) -> np.array: + return numericalDerivative11(lambda x: h(x, x2, x3, x4, x5, x6), x1, delta) diff --git a/raspberry_pi/app/pose_estimator/odometry.py b/raspberry_pi/app/pose_estimator/odometry.py new file mode 100644 index 000000000..64c36d7d8 --- /dev/null +++ b/raspberry_pi/app/pose_estimator/odometry.py @@ -0,0 +1,92 @@ +"""Factory for odometry factors. + +This also serves as a template for other factors. The pattern should always be +the same: an "h" function that performs the estimated measurement, an +"h_H" function that wraps it with numerical derivatives (to avoid confusion +with analytic Jacobians), and a "factor" factory that wraps that in a +CustomFactor. + +There's some discussion relevant to these operations here: +https://groups.google.com/g/gtsam-users/c/c-BhH8mfqbo/m/IMk1RQ84AwAJ + +NOTE! the native GTSAM BetweenFactor is much faster than the python +CustomFactor. For a computation budget of 10ms, the python factor can +only handle about 45 values (i.e. a window of about 1 sec at 50 hz) +but the native factor can handle about 600 (i.e. a 12 sec window). + +For production, we should use the native factor. :-) +""" + +# pylint: disable=C0103,E0611,E1101 + +import gtsam +import numpy as np +from gtsam.noiseModel import Base as SharedNoiseModel +from wpimath.geometry import Twist2d, Pose2d + +from app.pose_estimator.numerical_derivative import ( + numericalDerivative21, + numericalDerivative22, +) + + +def h(p0: gtsam.Pose2, p1: gtsam.Pose2) -> np.ndarray: + """Estimated tangential difference between p0 and p1. + This is identical to the WPILib "Twist2d" idea.""" + return p0.logmap(p1) + + +def h_H( + measured: np.ndarray, p0: gtsam.Pose2, p1: gtsam.Pose2, H: list[np.ndarray] +) -> np.ndarray: + """Error function including Jacobians. + Returns the difference between the measured and estimated tangent-space odometry.""" + result = h(p0, p1) - measured + if H is not None: + H[0] = numericalDerivative21(h, p0, p1) + # H[1] = np.eye(3) # even slower than the numerical derivative! + H[1] = numericalDerivative22(h, p0, p1) + return result + + +def factorCustom( + t: Twist2d, + model: SharedNoiseModel, + p0_key: gtsam.Symbol, + p1_key: gtsam.Symbol, +) -> gtsam.NonlinearFactor: + """Uses a python CustomFactor.""" + measured = np.array([t.dx, t.dy, t.dtheta]) + + def error_func( + this: gtsam.CustomFactor, v: gtsam.Values, H: list[np.ndarray] + ) -> np.ndarray: + p0: gtsam.Pose2 = v.atPose2(this.keys()[0]) + p1: gtsam.Pose2 = v.atPose2(this.keys()[1]) + return h_H(measured, p0, p1, H) + + return gtsam.CustomFactor(model, gtsam.KeyVector([p0_key, p1_key]), error_func) + + +def factorNative( + t: Twist2d, + model: SharedNoiseModel, + p0_key: gtsam.Symbol, + p1_key: gtsam.Symbol, +) -> gtsam.NonlinearFactor: + """Uses the GTSAM BetweenFactor.""" + # the gtsam between factor uses a relative pose, not a twist. + p = Pose2d().exp(t) + gp = gtsam.Pose2(p.x, p.y, p.rotation().radians()) + return gtsam.BetweenFactorPose2(p0_key, p1_key, gp, model) + + +def factor( + t: Twist2d, + model: SharedNoiseModel, + p0_key: gtsam.Symbol, + p1_key: gtsam.Symbol, +) -> gtsam.NonlinearFactor: + """Factory for a factor implementing odometry using tangent-space measurements.""" + return factorNative(t, model, p0_key, p1_key) + # return factorCustom(t, model, p0_key, p1_key) diff --git a/raspberry_pi/app/gtsam/gtsam_loop.py b/raspberry_pi/app/pose_estimator/pose_estimator_loop.py similarity index 100% rename from raspberry_pi/app/gtsam/gtsam_loop.py rename to raspberry_pi/app/pose_estimator/pose_estimator_loop.py diff --git a/raspberry_pi/app/pose_estimator/swerve_drive_kinematics.py b/raspberry_pi/app/pose_estimator/swerve_drive_kinematics.py new file mode 100644 index 000000000..dd6836320 --- /dev/null +++ b/raspberry_pi/app/pose_estimator/swerve_drive_kinematics.py @@ -0,0 +1,119 @@ +"""SwerveDriveKinematics100""" + +# pylint: disable=C0301,E0611,R0903 +import numpy as np +import numpy.typing as npt +from wpimath.geometry import Translation2d, Twist2d + +from app.pose_estimator.drive_util import DriveUtil +from app.pose_estimator.swerve_module_delta import SwerveModuleDelta +from app.pose_estimator.swerve_module_position import SwerveModulePosition100 + + +class SwerveDriveKinematics100: + """ + * array order: + * + * frontLeft + * frontRight + * rearLeft + * rearRight + """ + + def __init__(self, module_translations_m: list[Translation2d]) -> None: + self.num_modules = len(module_translations_m) + self.module_locations = module_translations_m + self.inverse_kinematics: npt.NDArray[np.float64] = self._inverse_matrix( + self.module_locations + ) + self.forward_kinematics = np.linalg.pinv(self.inverse_kinematics) + + def to_twist_2d(self, deltas: list[SwerveModuleDelta]) -> Twist2d: + """FORWARD: module deltas -> twist.""" + # [d cos; d sin; ...] (2n x 1) + delta_vector: npt.NDArray[np.float64] = self._deltas_2_vector(deltas) + # [dx ;dy; dtheta] + twist_vector: npt.NDArray[np.float64] = np.matmul( + self.forward_kinematics, delta_vector + ) + return self._vector_2_twist(twist_vector) + + def to_swerve_module_delta(self, twist: Twist2d) -> list[SwerveModuleDelta]: + """ + INVERSE: twist -> module position deltas + + This assumes the wheel paths are geodesics; steering does not change. + """ + + # [dx; dy; dtheta] (3 x 1) + twist_vector = self.twist_2_vector(twist) + # [d cos; d sin; ...] (2n x 1) + delta_vector = np.matmul(self.inverse_kinematics, twist_vector) + return self.deltas_from_vector(delta_vector) + + def to_swerve_module_positions( + self, initial: list[SwerveModulePosition100], twist: Twist2d + ) -> list[SwerveModulePosition100]: + """Find the module deltas and apply them to the given initial positions.""" + deltas: list[SwerveModuleDelta] = self.to_swerve_module_delta(twist) + return DriveUtil.module_position_from_delta(initial, deltas) + + @staticmethod + def twist_2_vector(twist: Twist2d) -> npt.NDArray[np.float64]: + return np.array([[twist.dx], [twist.dy], [twist.dtheta]]) + + def deltas_from_vector( + self, module_delta_vector: npt.NDArray[np.float64] + ) -> list[SwerveModuleDelta]: + """ + The resulting distance is always positive. + + @param moduleDeltaVector [d cos; d sin; ...] (2n x 1), equivalently [dx0; dy0; dx1; ...] + """ + module_deltas: list[SwerveModuleDelta] = [] + for i in range(self.num_modules): + x: float = module_delta_vector[i * 2, 0] + y: float = module_delta_vector[i * 2 + 1, 0] + module_deltas.append(SwerveModuleDelta.of(x, y)) + + return module_deltas + + def _deltas_2_vector( + self, module_deltas: list[SwerveModuleDelta] + ) -> npt.NDArray[np.float64]: + """deltas -> [d cos; d sin; ... ] (2n x 1) */""" + module_delta_matrix = np.zeros((self.num_modules * 2, 1)) + for i in range(self.num_modules): + module: SwerveModuleDelta = module_deltas[i] + if abs(module.distance_m) < 1e-6 or not module.angle.present: + module_delta_matrix[i * 2, 0] = 0 + module_delta_matrix[i * 2 + 1, 0] = 0 + else: + module_delta_matrix[i * 2, 0] = ( + module.distance_m * module.angle.value.cos() + ) + module_delta_matrix[i * 2 + 1, 0] = ( + module.distance_m * module.angle.value.sin() + ) + + return module_delta_matrix + + @staticmethod + def _vector_2_twist(v: npt.NDArray[np.float64]) -> Twist2d: + """[dx; dy; dtheta] (3 x 1) -> Twist2d""" + return Twist2d(v[0, 0], v[1, 0], v[2, 0]) + + @staticmethod + def _inverse_matrix( + module_locations: list[Translation2d], + ) -> npt.NDArray[np.float64]: + num_modules = len(module_locations) + inverse_kinematics = np.zeros((num_modules * 2, 3)) + for i in range(num_modules): + inverse_kinematics[i * 2 + 0, 0] = 1 + inverse_kinematics[i * 2 + 0, 1] = 0 + inverse_kinematics[i * 2 + 0, 2] = -module_locations[i].Y() + inverse_kinematics[i * 2 + 1, 0] = 0 + inverse_kinematics[i * 2 + 1, 1] = 1 + inverse_kinematics[i * 2 + 1, 2] = +module_locations[i].X() + return inverse_kinematics diff --git a/raspberry_pi/app/pose_estimator/swerve_module_delta.py b/raspberry_pi/app/pose_estimator/swerve_module_delta.py new file mode 100644 index 000000000..a232981fb --- /dev/null +++ b/raspberry_pi/app/pose_estimator/swerve_module_delta.py @@ -0,0 +1,22 @@ +import math + +from wpimath.geometry import Rotation2d + +from app.pose_estimator.swerve_module_position import OptionalRotation2d + +# pylint: disable=C0200,R0903 + + +class SwerveModuleDelta: + def __init__(self, distance_m: float, angle: OptionalRotation2d) -> None: + self.distance_m = distance_m + self.angle = angle + + @staticmethod + def of(dx: float, dy: float) -> "SwerveModuleDelta": + if abs(dx) < 1e-6 and abs(dy) < 1e-6: + # avoid the garbage rotation. + return SwerveModuleDelta(0.0, OptionalRotation2d(False, Rotation2d(0))) + return SwerveModuleDelta( + math.hypot(dx, dy), OptionalRotation2d(True, Rotation2d(dx, dy)) + ) diff --git a/raspberry_pi/app/pose_estimator/swerve_module_position.py b/raspberry_pi/app/pose_estimator/swerve_module_position.py new file mode 100644 index 000000000..3e5281381 --- /dev/null +++ b/raspberry_pi/app/pose_estimator/swerve_module_position.py @@ -0,0 +1,20 @@ +""" SwerveModulePosition100""" + +import dataclasses + +from wpimath.geometry import Rotation2d +from wpiutil import wpistruct + + +@wpistruct.make_wpistruct # type: ignore +@dataclasses.dataclass +class OptionalRotation2d: + present: bool + value: Rotation2d + + +@wpistruct.make_wpistruct # type: ignore +@dataclasses.dataclass +class SwerveModulePosition100: + distance_m: float + angle: OptionalRotation2d diff --git a/raspberry_pi/doc/gtsam_model.dot b/raspberry_pi/doc/gtsam_model.dot index 1afd7968d..6e24003e4 100644 --- a/raspberry_pi/doc/gtsam_model.dot +++ b/raspberry_pi/doc/gtsam_model.dot @@ -7,32 +7,67 @@ graph { Xp [color = "blue";shape = "point";width = "0.2";xlabel = "prior";]; X0 [label = "X0";shape = "circle";pos = "10,12!";margin = -0.1;]; O0 [color = "green";shape = "point";width = "0.2";xlabel = "odometry";]; + O1 [color = "green";shape = "point";width = "0.2";xlabel = "odometry";]; X1 [label = "X1";shape = "circle";pos = "12,12!";margin = -0.1;]; - Xe [label = "";shape = "none";pos = "13,12!";]; - t0 [label = "Tag1";shape = "box";pos = "11,15!";]; - c0 [label = "Off";shape = "circle";margin = -0.1;pos = "11,14!";]; + X2 [label = "X2";shape = "circle";pos = "14,12!";margin = -0.1;]; + Xe [label = "";shape = "none";pos = "16,12!";]; + t0 [label = "Tag";shape = "box";pos = "12,16!";]; + c0 [label = "Off";shape = "circle";margin = -0.1;pos = "9,15!";]; cp [color = "blue";shape = "point";width = "0.2";xlabel = "prior";]; - k0 [label = "Cal";shape = "circle";margin = -0.1;pos = "11,13!";]; + k0 [label = "Cal";shape = "circle";margin = -0.1;pos = "9,13!";]; kp [color = "blue";shape = "point";width = "0.2";xlabel = "prior";]; - v0 [color = "green";shape = "point";width = "0.2";xlabel = "vision";]; - v1 [color = "green";shape = "point";width = "0.2";xlabel = "vision ";]; + bp [color = "blue";shape = "point";width = "0.2";xlabel = "prior";]; + v0 [color = "green";shape = "point";width = "0.2";pos="11,14!";xlabel = "vision";]; + v1 [color = "green";shape = "point";width = "0.2";pos="12,14!";xlabel = "vision ";]; + v2 [color = "green";shape = "point";width = "0.2";pos="13,14!"; xlabel = "vision ";]; g0 [color = "green";shape = "point";width = "0.2";xlabel = "gyro";]; g1 [color = "green";shape = "point";width = "0.2";xlabel = "gyro ";]; + g2 [color = "green";shape = "point";width = "0.2";xlabel = "gyro ";]; + b0 [label = "b0";shape = "circle";pos = "10,10!";margin = -0.1;]; + b1 [label = "b1";shape = "circle";pos = "12,10!";margin = -0.1;]; + b2 [label = "b2";shape = "circle";pos = "14,10!";margin = -0.1;]; + bb [color = "green";shape = "point";width = "0.2";xlabel = "gyro bias";]; + bc [color = "green";shape = "point";width = "0.2";xlabel = "gyro bias";]; + bd [label = "";shape = "none";pos = "16,10!";]; + br [color = "blue";shape = "point";width = "0.2";xlabel = "reset";]; + a0 [color="green";shape="point";width="0.2";pos="12.25,11.25!";xlabel="accel";]; Xp -- X0; X0 -- O0; O0 -- X1; - X1 -- Xe [style = "dotted";]; + X1 -- O1; + O1 -- X2; + X2 -- Xe [style = "dotted";]; + X0 -- a0; + X1 -- a0; + X2 -- a0; X0 -- v0; + X0 -- g1; + X1 -- g2; X1 -- v1; + X2 -- v2; v0 -- t0; v1 -- t0; + v2 -- t0; cp -- c0; v0 -- c0; v1 -- c0; + v2 -- c0; kp -- k0; v0 -- k0; v1 -- k0; + v2 -- k0; g0 -- X0; g1 -- X1; + g2 -- X2; + b0 -- g0; + b1 -- g1; + b2 -- g2; + b0 -- bb; + bp -- b0; + bb -- b1; + b1 -- bc; + bc -- b2; + b2 -- bd [style = "dotted";]; + br -- X1; } \ No newline at end of file diff --git a/raspberry_pi/doc/gtsam_model.png b/raspberry_pi/doc/gtsam_model.png index 707457e77..5eae6c935 100644 Binary files a/raspberry_pi/doc/gtsam_model.png and b/raspberry_pi/doc/gtsam_model.png differ diff --git a/raspberry_pi/doc/testing.md b/raspberry_pi/doc/testing.md new file mode 100644 index 000000000..2d0ecf968 --- /dev/null +++ b/raspberry_pi/doc/testing.md @@ -0,0 +1,8 @@ +# Testing + +The vscode "Testing" gadget works with the unittests as set up here. +Click on the little Erlenmeyer flask on the left side to use it. + +What doesn't work is the little "run" triangles in the upper-right corner +of the editor. I think i tries to run the python file as a "script" +rather than a "module," and there's no easy way to change that behavior. \ No newline at end of file diff --git a/raspberry_pi/tests/gtsam/gtsam_test.py b/raspberry_pi/tests/gtsam/gtsam_test.py deleted file mode 100644 index 8eddf086a..000000000 --- a/raspberry_pi/tests/gtsam/gtsam_test.py +++ /dev/null @@ -1,7 +0,0 @@ -import unittest - - -class GtsamTest(unittest.TestCase): - - def testSimple(self) -> None: - self.assertTrue(True) diff --git a/raspberry_pi/tests/gtsam/__init__.py b/raspberry_pi/tests/pose_estimator/__init__.py similarity index 100% rename from raspberry_pi/tests/gtsam/__init__.py rename to raspberry_pi/tests/pose_estimator/__init__.py diff --git a/raspberry_pi/tests/pose_estimator/accelerometer_test.py b/raspberry_pi/tests/pose_estimator/accelerometer_test.py new file mode 100644 index 000000000..aca7baf10 --- /dev/null +++ b/raspberry_pi/tests/pose_estimator/accelerometer_test.py @@ -0,0 +1,296 @@ +# pylint: disable=C0103,E0611,E1101,R0402 + +import unittest + +import gtsam +import numpy as np + +import app.pose_estimator.accelerometer as accelerometer + +NOISE3 = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.1])) + + +class AccelerometerTest(unittest.TestCase): + def test_h_0(self) -> None: + """Verify h alone, motionless""" + p0 = gtsam.Pose2() + p1 = gtsam.Pose2() + p2 = gtsam.Pose2() + dt1 = 0.02 + dt2 = 0.02 + result: np.ndarray = accelerometer.h(p0, p1, p2, dt1, dt2) + self.assertEqual(3, len(result)) + self.assertAlmostEqual(0, result[0]) + self.assertAlmostEqual(0, result[1]) + self.assertAlmostEqual(0, result[2]) + + def test_h_1(self) -> None: + """Verify h alone, constant velocity""" + p0 = gtsam.Pose2(0, 0, 0) + p1 = gtsam.Pose2(1, 0, 0) + p2 = gtsam.Pose2(2, 0, 0) + dt1 = 0.02 + dt2 = 0.02 + result: np.ndarray = accelerometer.h(p0, p1, p2, dt1, dt2) + self.assertEqual(3, len(result)) + self.assertAlmostEqual(0, result[0]) + self.assertAlmostEqual(0, result[1]) + self.assertAlmostEqual(0, result[2]) + + def test_h_1a(self) -> None: + """Verify h alone, constant angular velocity""" + p0 = gtsam.Pose2(0, 0, 0) + p1 = gtsam.Pose2(0, 0, 1) + p2 = gtsam.Pose2(0, 0, 2) + dt1 = 0.02 + dt2 = 0.02 + result: np.ndarray = accelerometer.h(p0, p1, p2, dt1, dt2) + self.assertEqual(3, len(result)) + self.assertAlmostEqual(0, result[0]) + self.assertAlmostEqual(0, result[1]) + self.assertAlmostEqual(0, result[2]) + + def test_h_2(self) -> None: + """Verify h alone, linear acceleration""" + p0 = gtsam.Pose2(0, 0, 0) + p1 = gtsam.Pose2(1, 0, 0) + p2 = gtsam.Pose2(4, 0, 0) + dt1 = 0.02 + dt2 = 0.02 + result: np.ndarray = accelerometer.h(p0, p1, p2, dt1, dt2) + self.assertEqual(3, len(result)) + # v1 is 50 + # v2 is 150 + # delta v is 100, delta t is 0.02, so a is 5000 + self.assertAlmostEqual(5000, result[0]) + self.assertAlmostEqual(0, result[1]) + self.assertAlmostEqual(0, result[2]) + + def test_h_2a(self) -> None: + """Verify h alone, angular acceleration""" + p0 = gtsam.Pose2(0, 0, 0) + p1 = gtsam.Pose2(0, 0, 1) + p2 = gtsam.Pose2(0, 0, 4) + dt1 = 0.02 + dt2 = 0.02 + result: np.ndarray = accelerometer.h(p0, p1, p2, dt1, dt2) + self.assertEqual(3, len(result)) + # v1 is 50 + # v2 is 150 + # delta v is 100, delta t is 0.02, so a is 5000 + self.assertAlmostEqual(0, result[0]) + self.assertAlmostEqual(0, result[1]) + self.assertAlmostEqual(5000, result[2]) + + def test_h_coriolis(self) -> None: + """Verify h alone, constant linear and angular speed""" + p0 = gtsam.Pose2(0, 0, 0) + p1 = gtsam.Pose2(1, 0, 1) + p2 = gtsam.Pose2(2, 0, 2) + dt1 = 0.02 + dt2 = 0.02 + result: np.ndarray = accelerometer.h(p0, p1, p2, dt1, dt2) + self.assertEqual(3, len(result)) + # turning to the left, so we need -y and -x to keep going the same direction. + self.assertAlmostEqual(-7305, result[0], 0) + self.assertAlmostEqual(-1720, result[1], 0) + self.assertAlmostEqual(0, result[2]) + + def test_h_coriolis_realistic(self) -> None: + """As above with smaller numbers""" + p0 = gtsam.Pose2(0, 0, 0) + p1 = gtsam.Pose2(0.02, 0, 0.02) + p2 = gtsam.Pose2(0.04, 0, 0.04) + dt1 = 0.02 + dt2 = 0.02 + result: np.ndarray = accelerometer.h(p0, p1, p2, dt1, dt2) + self.assertEqual(3, len(result)) + # we've only turned a little, so only slowed a little + self.assertAlmostEqual(-0.08, result[0], 2) + # but it's still a sizeable acceleration in y + # is this right? + # translation velocity is 1 + # y component goes from -0.01 to -0.03 which is + # delta-v of just about 0.02, in time 0.02, for a velocity + # of about -1. to maintain the correct path, the inertial force is + # also about -1, and the reference frame is carried (with respect to itself) + # with that same velocity, so the true velocity (i.e. the coriolis term) + # is about -2, and the net is -3. + self.assertAlmostEqual(-3, result[1], 2) + self.assertAlmostEqual(0, result[2]) + + def test_H(self) -> None: + p0 = gtsam.Pose2() + p1 = gtsam.Pose2() + p2 = gtsam.Pose2() + dt1 = 0.02 + dt2 = 0.02 + measured = np.array([0, 0, 0]) + H = [np.zeros((3, 3)), np.zeros((3, 3)), np.zeros((3, 3))] + result: np.ndarray = accelerometer.h_H(measured, p0, p1, p2, dt1, dt2, H) + self.assertEqual(3, len(result)) + self.assertAlmostEqual(0, result[0]) + self.assertAlmostEqual(0, result[1]) + self.assertAlmostEqual(0, result[2]) + print("H[0]:\n", H[0]) + print("H[1]:\n", H[1]) + print("H[2]:\n", H[2]) + self.assertTrue( + np.allclose( + np.array( + [ + [2500, 0, 0], + [0, 2500, 0], + [0, 0, 2500], + ] + ), + H[0], + atol=0.001, + ) + ) + self.assertTrue( + np.allclose( + np.array( + [ + [-5000, 0, 0], + [0, -5000, 0], + [0, 0, -5000], + ] + ), + H[1], + atol=0.001, + ) + ) + self.assertTrue( + np.allclose( + np.array( + [ + [2500, 0, 0], + [0, 2500, 0], + [0, 0, 2500], + ] + ), + H[2], + atol=0.001, + ) + ) + + def test_H_accel(self) -> None: + p0 = gtsam.Pose2(0, 0, 0) + p1 = gtsam.Pose2(1, 0, 0) + p2 = gtsam.Pose2(4, 0, 0) + dt1 = 0.02 + dt2 = 0.02 + measured = np.array([0, 0, 0]) + H = [np.zeros((3, 3)), np.zeros((3, 3)), np.zeros((3, 3))] + result: np.ndarray = accelerometer.h_H(measured, p0, p1, p2, dt1, dt2, H) + self.assertEqual(3, len(result)) + self.assertAlmostEqual(5000, result[0]) + self.assertAlmostEqual(0, result[1]) + self.assertAlmostEqual(0, result[2]) + print("H[0]:\n", H[0]) + print("H[1]:\n", H[1]) + print("H[2]:\n", H[2]) + # i dunno if these jacobians are right. + self.assertTrue( + np.allclose( + np.array( + [ + [2500, 0, 0], + [0, 2500, 1250], + [0, 0, 2500], + ] + ), + H[0], + atol=1, + ) + ) + self.assertTrue( + np.allclose( + np.array( + [ + [-5000, 0, 0], + [0, -5000, 12500], + [0, 0, -5000], + ] + ), + H[1], + atol=1, + ) + ) + self.assertTrue( + np.allclose( + np.array( + [ + [2500, 0, 0], + [0, 2500, -18750], + [0, 0, 2500], + ] + ), + H[2], + atol=1, + ) + ) + + def test_accel_factor(self) -> None: + x_accel = 0 + y_accel = 0 + dt = 0.02 + + f: gtsam.NonlinearFactor = accelerometer.factor( + x_accel, y_accel, dt, dt, NOISE3, 0, 1, 2 + ) + v = gtsam.Values() + p0 = gtsam.Pose2() + p1 = gtsam.Pose2() + p2 = gtsam.Pose2() + v.insert(0, p0) + v.insert(1, p1) + v.insert(2, p2) + result = f.unwhitenedError(v) + self.assertEqual(3, len(result)) + self.assertAlmostEqual(0, result[0]) + self.assertAlmostEqual(0, result[1]) + self.assertAlmostEqual(0, result[2]) + + def test_accel_factor2(self) -> None: + x_accel = 0 + y_accel = 0 + dt = 0.02 + + f: gtsam.NonlinearFactor = accelerometer.factor( + x_accel, y_accel, dt, dt, NOISE3, 0, 1, 2 + ) + v = gtsam.Values() + p0 = gtsam.Pose2(0, 0, 0) + p1 = gtsam.Pose2(0.01, 0, 0) + p2 = gtsam.Pose2(0.04, 0, 0) + v.insert(0, p0) + v.insert(1, p1) + v.insert(2, p2) + result = f.unwhitenedError(v) + self.assertEqual(3, len(result)) + self.assertAlmostEqual(50, result[0]) + self.assertAlmostEqual(0, result[1]) + self.assertAlmostEqual(0, result[2]) + + def test_accel_factor3(self) -> None: + x_accel = 0 + y_accel = 0 + dt = 0.02 + + f: gtsam.NonlinearFactor = accelerometer.factor( + x_accel, y_accel, dt, dt, NOISE3, 0, 1, 2 + ) + v = gtsam.Values() + p0 = gtsam.Pose2(0, 0, 0) + p1 = gtsam.Pose2(0.02, 0, 0.02) + p2 = gtsam.Pose2(0.04, 0, 0.04) + v.insert(0, p0) + v.insert(1, p1) + v.insert(2, p2) + result = f.unwhitenedError(v) + self.assertEqual(3, len(result)) + self.assertAlmostEqual(-0.08, result[0], 2) + self.assertAlmostEqual(-3, result[1], 2) + self.assertAlmostEqual(0, result[2]) diff --git a/raspberry_pi/tests/pose_estimator/apriltag_test.py b/raspberry_pi/tests/pose_estimator/apriltag_test.py new file mode 100644 index 000000000..b7bfdbfba --- /dev/null +++ b/raspberry_pi/tests/pose_estimator/apriltag_test.py @@ -0,0 +1,120 @@ +# pylint: disable=C0103,E0611,E1101,R0402 + + +import unittest + +import gtsam +import numpy as np + +import app.pose_estimator.apriltag as apriltag + +KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1, 0.0, 0.0) +NOISE2 = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1])) + + +class AprilTagTest(unittest.TestCase): + def test_h_center_0(self) -> None: + landmark = np.array([1, 0, 0]) + p0 = gtsam.Pose2() + offset = gtsam.Pose3() + calib = gtsam.Cal3DS2() + result: np.ndarray = apriltag.h_fn(landmark)(p0, offset, calib) + self.assertEqual(2, len(result)) + self.assertAlmostEqual(0, result[0]) + self.assertAlmostEqual(0, result[1]) + + def test_h_center_1(self) -> None: + landmark = np.array([1, 0, 0]) + p0 = gtsam.Pose2() + offset = gtsam.Pose3() + result: np.ndarray = apriltag.h_fn(landmark)(p0, offset, KCAL) + self.assertEqual(2, len(result)) + self.assertAlmostEqual(200, result[0]) + self.assertAlmostEqual(200, result[1]) + + def test_h_side_0(self) -> None: + # higher than the camera + landmark = np.array([1, 0, 1]) + p0 = gtsam.Pose2() + offset = gtsam.Pose3() + result: np.ndarray = apriltag.h_fn(landmark)(p0, offset, KCAL) + self.assertEqual(2, len(result)) + self.assertAlmostEqual(200, result[0]) + self.assertAlmostEqual(20, result[1]) + + def test_h_upper_left(self) -> None: + # higher than the camera + landmark = np.array([1, 1, 1]) + p0 = gtsam.Pose2() + offset = gtsam.Pose3() + result: np.ndarray = apriltag.h_fn(landmark)(p0, offset, KCAL) + self.assertEqual(2, len(result)) + self.assertAlmostEqual(0, result[0]) + self.assertAlmostEqual(0, result[1]) + + def test_H_upper_left(self) -> None: + # higher than the camera + measured = np.array([0, 0]) + landmark = np.array([1, 1, 1]) + p0 = gtsam.Pose2() + offset = gtsam.Pose3() + H = [np.zeros((3, 2)), np.zeros((6, 2)), np.zeros((9, 2))] + result: np.ndarray = apriltag.h_H(landmark, measured, p0, offset, KCAL, H) + self.assertEqual(2, len(result)) + self.assertAlmostEqual(0, result[0]) + self.assertAlmostEqual(0, result[1]) + print("H[0]:\n", H[0]) + print("H[1]:\n", H[1]) + print("H[2]:\n", H[2]) + self.assertTrue( + np.allclose( + np.array( + [ + [-360, 280, 640], + [-360, 80, 440], + ] + ), + H[0], + atol=0.001, + ) + ) + self.assertTrue( + np.allclose( + np.array( + [ + [-200, -440, 640, -360, 280, 80], + [200, -640, 440, -360, 80, 280], + ] + ), + H[1], + atol=0.001, + ) + ) + self.assertTrue( + np.allclose( + np.array( + [ + [-1, 0, -1, 1, 0, -400, -800, 400, 800], + [0, -1, 0, 0, 1, -400, -800, 800, 400], + ] + ), + H[2], + atol=0.001, + ) + ) + + def test_factor(self) -> None: + # higher than the camera + measured = np.array([0, 0]) + landmark = np.array([1, 1, 1]) + p0 = gtsam.Pose2() + offset = gtsam.Pose3() + f: gtsam.NonlinearFactor = apriltag.factor(landmark, measured, NOISE2, 0, 1, 2) + v = gtsam.Values() + v.insert(0, p0) + v.insert(1, offset) + v.insert(2, KCAL) + result = f.unwhitenedError(v) + self.assertEqual(2, len(result)) + self.assertAlmostEqual(0, result[0], 2) + self.assertAlmostEqual(0, result[1], 2) diff --git a/raspberry_pi/tests/pose_estimator/drive_util_test.py b/raspberry_pi/tests/pose_estimator/drive_util_test.py new file mode 100644 index 000000000..11fb3c686 --- /dev/null +++ b/raspberry_pi/tests/pose_estimator/drive_util_test.py @@ -0,0 +1,106 @@ +import unittest + +import random + +from wpimath.geometry import Rotation2d, Translation2d, Twist2d + +from app.pose_estimator.drive_util import DriveUtil +from app.pose_estimator.swerve_drive_kinematics import SwerveDriveKinematics100 +from app.pose_estimator.swerve_module_position import ( + OptionalRotation2d, + SwerveModulePosition100, +) + +# pylint: disable=C0200,R0903 + + +class DriveUtilTest(unittest.TestCase): + def test_round_trip_module_deltas(self) -> None: + k = SwerveDriveKinematics100( + [ + Translation2d(0.5, 0.5), + Translation2d(0.5, -0.5), + Translation2d(-0.5, 0.5), + Translation2d(-0.5, -0.5), + ] + ) + self.assertEqual(4, k.num_modules) + + # straight diagonal path + t = Twist2d(1, 1, 0) + p = k.to_swerve_module_delta(t) + self.assertEqual(4, len(p)) + t2 = k.to_twist_2d(p) + self.assertAlmostEqual(t.dx, t2.dx) + self.assertAlmostEqual(t.dy, t2.dy) + self.assertAlmostEqual(t.dtheta, t2.dtheta) + + # turning and moving + t = Twist2d(1, 1, 1) + p = k.to_swerve_module_delta(t) + t2 = k.to_twist_2d(p) + self.assertAlmostEqual(t.dx, t2.dx) + self.assertAlmostEqual(t.dy, t2.dy) + self.assertAlmostEqual(t.dtheta, t2.dtheta) + + for _ in range(500): + # inverse always works + t = Twist2d(random.random(), random.random(), random.random()) + p = k.to_swerve_module_delta(t) + t2 = k.to_twist_2d(p) + self.assertAlmostEqual(t.dx, t2.dx) + self.assertAlmostEqual(t.dy, t2.dy) + self.assertAlmostEqual(t.dtheta, t2.dtheta) + + def test_delta(self) -> None: + start = [ + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + ] + end = [ + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + ] + delta = DriveUtil.module_position_delta(start, end) + self.assertAlmostEqual(0, delta[0].distance_m) + self.assertAlmostEqual(0, delta[0].angle.value.radians()) + + def test_delta2(self) -> None: + start = [ + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + ] + end = [ + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(1))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(1))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(1))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(1))), + ] + # there is no interpolation, we use the ending value. + delta = DriveUtil.module_position_delta(start, end) + self.assertAlmostEqual(0, delta[0].distance_m) + self.assertAlmostEqual(1, delta[0].angle.value.radians()) + + def test_delta3(self) -> None: + start = [ + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + ] + end = [ + SwerveModulePosition100(1, OptionalRotation2d(True, Rotation2d(1))), + SwerveModulePosition100(1, OptionalRotation2d(True, Rotation2d(1))), + SwerveModulePosition100(1, OptionalRotation2d(True, Rotation2d(1))), + SwerveModulePosition100(1, OptionalRotation2d(True, Rotation2d(1))), + ] + # there is no interpolation, we use the ending value. + delta = DriveUtil.module_position_delta(start, end) + self.assertAlmostEqual(1, delta[0].distance_m) + self.assertAlmostEqual(1, delta[0].angle.value.radians()) diff --git a/raspberry_pi/tests/pose_estimator/estimate_simulate_test.py b/raspberry_pi/tests/pose_estimator/estimate_simulate_test.py new file mode 100644 index 000000000..a362745c6 --- /dev/null +++ b/raspberry_pi/tests/pose_estimator/estimate_simulate_test.py @@ -0,0 +1,45 @@ +# pylint: disable=C0301,E0611,R0903 + +import math +import time +import unittest + +import numpy as np +from gtsam import Cal3DS2, Point2, Point3, Pose2, Pose3, Rot3 +from gtsam.symbol_shorthand import X +from wpimath.geometry import Pose2d + +from app.pose_estimator.estimate import Estimate +from tests.pose_estimator.simulator import Simulator + + +class EstimateSimulateTest(unittest.TestCase): + def test_simple(self) -> None: + sim = Simulator() + est = Estimate() + est.init(sim.wpi_pose) + + print() + for i in range(1, 100): + t0 = time.time_ns() + time_us = 20000 * i + est.odometry(time_us, sim.positions) + est.update() + t1 = time.time_ns() + et = t1 - t0 + # print(f"{et/1e9} {est.result.size()}") + t = i * 0.02 + gt_x = sim.gt_x + gt_y = sim.gt_y + gt_theta = sim.gt_theta + + # using just odometry without noise, the error + # is exactly zero, all the time. :-) + p: Pose2 = est.result.atPose2(X(time_us)) + est_x = p.x() + est_y = p.y() + est_theta = p.theta() + + print(f"{t:7.4f}, {gt_x:7.4f}, {gt_y:7.4f}, {gt_theta:7.4f}, {est_x:7.4f}, {est_y:7.4f}, {est_theta:7.4f}") + + sim.step(0.02) diff --git a/raspberry_pi/tests/pose_estimator/estimate_test.py b/raspberry_pi/tests/pose_estimator/estimate_test.py new file mode 100644 index 000000000..fc2cb47bb --- /dev/null +++ b/raspberry_pi/tests/pose_estimator/estimate_test.py @@ -0,0 +1,85 @@ +""" Evaluate the estimation model. """ + +# pylint: disable=C0301,E0611,R0903 + +import math +import unittest + +import gtsam +from gtsam.symbol_shorthand import X +from wpimath.geometry import Rotation2d, Pose2d + +from app.pose_estimator.estimate import Estimate +from app.pose_estimator.swerve_module_position import ( + OptionalRotation2d, + SwerveModulePosition100, +) + + +class EstimateTest(unittest.TestCase): + def test_eval(self) -> None: + # initial position at origin + est = Estimate() + est.init(Pose2d()) + # drive straight ahead 0.1 + positions = [ + SwerveModulePosition100(0.1, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0.1, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0.1, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0.1, OptionalRotation2d(True, Rotation2d(0))), + ] + time_us: int = 1 + est.odometry(time_us, positions) + est.update() + print(est.result) + self.assertEqual(2, est.result.size()) + p0: gtsam.Pose2 = est.result.atPose2(X(0)) + self.assertAlmostEqual(0, p0.x()) + self.assertAlmostEqual(0, p0.y()) + self.assertAlmostEqual(0, p0.theta()) + p1: gtsam.Pose2 = est.result.atPose2(X(1)) + # this is the x value from above + self.assertAlmostEqual(0.1, p1.x()) + self.assertAlmostEqual(0, p1.y()) + self.assertAlmostEqual(0, p1.theta()) + + def test_eval2(self) -> None: + """array order: + * + * frontLeft + * frontRight + * rearLeft + * rearRight + """ + # initial position at origin, 1m wheelbase + est = Estimate() + est.init(Pose2d()) + # This is a rotating translation that should end up rotated 90 to the left + # and 1m behind + positions = [ + SwerveModulePosition100( + math.sqrt(2) * math.pi / 2, + OptionalRotation2d(True, Rotation2d.fromDegrees(135)), + ), + SwerveModulePosition100( + math.pi / 2, OptionalRotation2d(True, Rotation2d.fromDegrees(90)) + ), + SwerveModulePosition100( + math.pi / 2, OptionalRotation2d(True, Rotation2d.fromDegrees(180)) + ), + SwerveModulePosition100(0.0, OptionalRotation2d(False, Rotation2d(0))), + ] + time_us: int = 1 + est.odometry(time_us, positions) + est.update() + print(est.result) + self.assertEqual(2, est.result.size()) + p0: gtsam.Pose2 = est.result.atPose2(X(0)) + self.assertAlmostEqual(0, p0.x()) + self.assertAlmostEqual(0, p0.y()) + self.assertAlmostEqual(0, p0.theta()) + p1: gtsam.Pose2 = est.result.atPose2(X(1)) + # this is the x value from above + self.assertAlmostEqual(-1, p1.x()) + self.assertAlmostEqual(0, p1.y()) + self.assertAlmostEqual(math.pi / 2, p1.theta()) diff --git a/raspberry_pi/tests/pose_estimator/gyro_test.py b/raspberry_pi/tests/pose_estimator/gyro_test.py new file mode 100644 index 000000000..655d01473 --- /dev/null +++ b/raspberry_pi/tests/pose_estimator/gyro_test.py @@ -0,0 +1,104 @@ +# pylint: disable=C0103,E0611,E1101,R0402 + +import unittest + +import gtsam +import numpy as np + +import app.pose_estimator.gyro as gyro + +NOISE1 = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1])) + + +class GyroTest(unittest.TestCase): + def test_h_0(self) -> None: + """motionless""" + p0 = gtsam.Pose2() + p1 = gtsam.Pose2() + result: np.ndarray = gyro.h(p0, p1) + self.assertEqual(1, len(result)) + self.assertAlmostEqual(0, result[0]) + + def test_h_1(self) -> None: + """dtheta = 1""" + p0 = gtsam.Pose2(0, 0, 0) + p1 = gtsam.Pose2(0, 0, 1) + result: np.ndarray = gyro.h(p0, p1) + self.assertEqual(1, len(result)) + self.assertAlmostEqual(1, result[0]) + + def test_H_0(self) -> None: + p0 = gtsam.Pose2(0, 0, 0) + p1 = gtsam.Pose2(0, 0, 0) + measured = np.array([0]) + H = [np.zeros((3, 3)), np.zeros((3, 3))] + result: np.ndarray = gyro.h_H(measured, p0, p1, H) + self.assertEqual(1, len(result)) + self.assertAlmostEqual(0, result[0]) + print("H[0]:\n", H[0]) + print("H[1]:\n", H[1]) + self.assertTrue( + np.allclose( + np.array([[0, 0, -1]]), + H[0], + atol=0.001, + ) + ) + self.assertTrue( + np.allclose( + np.array([[0, 0, 1]]), + H[1], + atol=0.001, + ) + ) + + def test_H_1(self) -> None: + # estimate is 1, measurement is 0, so error is 1 + p0 = gtsam.Pose2(0, 0, 1) + p1 = gtsam.Pose2(0, 0, 2) + measured = np.array([0]) + H = [np.zeros((3, 3)), np.zeros((3, 3))] + result: np.ndarray = gyro.h_H(measured, p0, p1, H) + self.assertEqual(1, len(result)) + self.assertAlmostEqual(1, result[0]) + print("H[0]:\n", H[0]) + print("H[1]:\n", H[1]) + self.assertTrue( + np.allclose( + np.array([[0, 0, -1]]), + H[0], + atol=0.001, + ) + ) + self.assertTrue( + np.allclose( + np.array([[0, 0, 1]]), + H[1], + atol=0.001, + ) + ) + + def test_factor_0(self) -> None: + measured = np.array([0]) + f: gtsam.NonlinearFactor = gyro.factor(measured, NOISE1, 0, 1) + v = gtsam.Values() + p0 = gtsam.Pose2(0, 0, 0) + p1 = gtsam.Pose2(0, 0, 0) + v.insert(0, p0) + v.insert(1, p1) + result = f.unwhitenedError(v) + self.assertEqual(1, len(result)) + self.assertAlmostEqual(0, result[0]) + + def test_factor_1(self) -> None: + """both estimate and measurement are 1""" + measured = np.array([1]) + f: gtsam.NonlinearFactor = gyro.factor(measured, NOISE1, 0, 1) + v = gtsam.Values() + p0 = gtsam.Pose2(0, 0, 0) + p1 = gtsam.Pose2(0, 0, 1) + v.insert(0, p0) + v.insert(1, p1) + result = f.unwhitenedError(v) + self.assertEqual(1, len(result)) + self.assertAlmostEqual(0, result[0]) \ No newline at end of file diff --git a/raspberry_pi/tests/pose_estimator/odometry_test.py b/raspberry_pi/tests/pose_estimator/odometry_test.py new file mode 100644 index 000000000..901f8fef7 --- /dev/null +++ b/raspberry_pi/tests/pose_estimator/odometry_test.py @@ -0,0 +1,228 @@ +# pylint: disable=C0103,E0611,E1101 + +import math +import unittest + +import gtsam +import numpy as np +from wpimath.geometry import Pose2d, Twist2d + +import app.pose_estimator.odometry as odometry + +NOISE3 = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.1])) + + +class OdometryTest(unittest.TestCase): + def test_odometry_factor(self) -> None: + """No movement in either estimate or measurement, so no error.""" + # The CustomFactor wrapper makes this hard to test more thoroughly. + t = Twist2d(0, 0, 0) + f: gtsam.NonlinearFactor = odometry.factor(t, NOISE3, 0, 1) + print("keys", f.keys()) + v = gtsam.Values() + p0 = gtsam.Pose2() + p1 = gtsam.Pose2() + v.insert(0, p0) + v.insert(1, p1) + result = f.unwhitenedError(v) + self.assertEqual(3, len(result)) + self.assertAlmostEqual(0, result[0]) + self.assertAlmostEqual(0, result[1]) + self.assertAlmostEqual(0, result[2]) + + def test_odo_H(self) -> None: + """No movement, also include Jacobians.""" + # use the inner odo_H factor without the CustomFactor wrapper. + t = Twist2d(0, 0, 0) + measured = [t.dx, t.dy, t.dtheta] + p0 = gtsam.Pose2() + p1 = gtsam.Pose2() + H = [np.zeros((3, 3)), np.zeros((3, 3))] + result = odometry.h_H(measured, p0, p1, H) + self.assertEqual(3, len(result)) + self.assertAlmostEqual(0, result[0]) + self.assertAlmostEqual(0, result[1]) + self.assertAlmostEqual(0, result[2]) + self.assertTrue( + np.allclose( + np.array( + [ + [-1, 0, 0], + [0, -1, 0], + [0, 0, -1], + ] + ), + H[0], + ) + ) + self.assertTrue( + np.allclose( + np.array( + [ + [1, 0, 0], + [0, 1, 0], + [0, 0, 1], + ] + ), + H[1], + ) + ) + + def test_h(self) -> None: + """Verify h alone""" + # (1,1) facing +y + p0 = gtsam.Pose2() + p1 = gtsam.Pose2(1, 1, math.pi / 2) + result: np.ndarray = odometry.h(p0, p1) + self.assertEqual(3, len(result)) + self.assertAlmostEqual(math.pi / 2, result[0]) + self.assertAlmostEqual(0, result[1]) + self.assertAlmostEqual(math.pi / 2, result[2]) + + def test_odo_H2(self) -> None: + """Estimate and measurement are the same, drive ahead while turning left.""" + t = Twist2d(math.pi / 2, 0, math.pi / 2) + testP0 = Pose2d() + testP1 = testP0.exp(t) + # we should end up at (1,1) facing +y. + self.assertAlmostEqual(1, testP1.X()) + self.assertAlmostEqual(1, testP1.Y()) + self.assertAlmostEqual(math.pi / 2, testP1.rotation().radians()) + measured = [t.dx, t.dy, t.dtheta] + p0 = gtsam.Pose2() + # here's (1,1) facing +y again + p1 = gtsam.Pose2(1, 1, math.pi / 2) + H = [np.zeros((3, 3)), np.zeros((3, 3))] + result = odometry.h_H(measured, p0, p1, H) + self.assertEqual(3, len(result)) + # no error + self.assertAlmostEqual(0, result[0]) + self.assertAlmostEqual(0, result[1]) + self.assertAlmostEqual(0, result[2]) + print("H[0]:\n", H[0]) + print("H[1]:\n", H[1]) + self.assertTrue( + np.allclose( + np.array( + [ + [-0.785, -0.785, -0.214], + [0.785, -0.785, -0.785], + [0, 0, -1], + ] + ), + H[0], + atol=0.001, + ) + ) + self.assertTrue( + np.allclose( + np.array( + [ + [0.785, -0.785, 0.214], + [0.785, 0.785, -0.785], + [0, 0, 1], + ] + ), + H[1], + atol=0.001, + ) + ) + + def test_odo_H3(self) -> None: + """Same left turn, but the estimate is behind a little.""" + # drive ahead while turning left + t = Twist2d(math.pi / 2, 0, math.pi / 2) + testP0 = Pose2d() + testP1 = testP0.exp(t) + # we should end up here + self.assertAlmostEqual(1, testP1.X()) + self.assertAlmostEqual(1, testP1.Y()) + self.assertAlmostEqual(math.pi / 2, testP1.rotation().radians()) + measured = [t.dx, t.dy, t.dtheta] + p0 = gtsam.Pose2() + # now there's an error which should map to a pure x error in the twist + # this position is due to just not driving far enough + p1 = gtsam.Pose2(0.9, 0.9, math.pi / 2) + H = [np.zeros((3, 3)), np.zeros((3, 3))] + result = odometry.h_H(measured, p0, p1, H) + self.assertEqual(3, len(result)) + # the tangent-space error is purely in x + self.assertAlmostEqual(-0.157, result[0], 2) + self.assertAlmostEqual(0, result[1], 2) + self.assertAlmostEqual(0, result[2]) + print("H[0]:\n", H[0]) + print("H[1]:\n", H[1]) + self.assertTrue( + np.allclose( + np.array( + [ + [-0.785, -0.785, -0.193], + [0.785, -0.785, -0.706], + [0, 0, -1], + ] + ), + H[0], + atol=0.001, + ) + ) + self.assertTrue( + np.allclose( + np.array( + [ + [0.785, -0.785, 0.193], + [0.785, 0.785, -0.706], + [0, 0, 1], + ] + ), + H[1], + atol=0.001, + ) + ) + + def test_odo_H4(self) -> None: + """move diagonally without rotating""" + t = Twist2d(1, 1, 0) + testP0 = Pose2d() + testP1 = testP0.exp(t) + # we should end up here + self.assertAlmostEqual(1, testP1.X()) + self.assertAlmostEqual(1, testP1.Y()) + self.assertAlmostEqual(0, testP1.rotation().radians()) + measured = [t.dx, t.dy, t.dtheta] + p0 = gtsam.Pose2() + # now there's an error which should be symmetrical + p1 = gtsam.Pose2(0.9, 0.9, 0) + H = [np.zeros((3, 3)), np.zeros((3, 3))] + result = odometry.h_H(measured, p0, p1, H) + self.assertEqual(3, len(result)) + self.assertAlmostEqual(-0.1, result[0], 2) + self.assertAlmostEqual(-0.1, result[1], 2) + self.assertAlmostEqual(0, result[2]) + print("H[0]:\n", H[0]) + print("H[1]:\n", H[1]) + self.assertTrue( + np.allclose( + np.array( + [ + [-1, 0, 0.45], + [0, -1, -0.45], + [0, 0, -1], + ] + ), + H[0], + atol=0.001, + ) + ) + self.assertTrue( + np.allclose( + np.array( + [ + [1, 0, 0.45], + [0, 1, -0.45], + [0, 0, 1], + ] + ), + H[1], + atol=0.001, + ) + ) diff --git a/raspberry_pi/tests/pose_estimator/pose_estimator_test.py b/raspberry_pi/tests/pose_estimator/pose_estimator_test.py new file mode 100644 index 000000000..18dd3ccab --- /dev/null +++ b/raspberry_pi/tests/pose_estimator/pose_estimator_test.py @@ -0,0 +1,7 @@ +import unittest + + +class PoseEstimatorTest(unittest.TestCase): + + def test_simple(self) -> None: + self.assertTrue(True) diff --git a/raspberry_pi/tests/pose_estimator/simulator.py b/raspberry_pi/tests/pose_estimator/simulator.py new file mode 100644 index 000000000..3ccb4ad55 --- /dev/null +++ b/raspberry_pi/tests/pose_estimator/simulator.py @@ -0,0 +1,160 @@ +# pylint: disable=C0301,E0611,R0903 +# mypy: disable-error-code="import-untyped" +"""Minimal simulated telemetry generator for both smoothing and calibration. + +The field is 4x4 meters +The field has one tag on the far (+x) end, 1m off the floor +The robot has one camera with the same pose as the robot, except 1m high. +The robot path is a circle of radius 1m, centered 3m away from the tag, followed at 1m/s. +The robot rotates back and forth with an amplitude of 0.5 rad, at 3x the frequency of the circular path (so 4 rad/s) + +Because the gyro drift is a random walk, the whole thing needs to be sequential. + +TODO: +use realistic 6mm lens and GS sensor parameters. +add camera noise, white noise +/- 1 px +add gyro noise, random walk, plus white noise +/- 0.01 rad. +""" + +import math + +import numpy as np +from gtsam import Cal3DS2 # includes distortion +from gtsam import PinholeCameraCal3DS2, Point2, Point3, Pose2, Pose3, Rot3 +from wpimath.geometry import Rotation2d, Translation2d, Twist2d, Pose2d + +from app.pose_estimator.swerve_drive_kinematics import SwerveDriveKinematics100 +from app.pose_estimator.swerve_module_position import ( + OptionalRotation2d, + SwerveModulePosition100, +) + +TAG_SIZE_M: float = 0.1651 +TAG_X: float = 4 +TAG_Y: float = 0 +TAG_Z: float = 1 +PATH_CENTER_X_M = 1 +PATH_CENTER_Y_M = 0 +PATH_RADIUS_M = 1 +PATH_PERIOD_S = 2.0 * math.pi +PAN_PERIOD_S = PATH_PERIOD_S / 3 +PAN_SCALE_RAD = 0.5 +# camera "zero" is facing +z; this turns it to face +x +CAM_COORD = Pose3( + Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])), + Point3(0, 0, 0), # type: ignore +) +CALIB = Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1, 0.0, 0.0) + + +class Simulator: + def __init__(self) -> None: + # TODO: actual wheelbase etc + self.kinematics = SwerveDriveKinematics100( + [ + Translation2d(0.5, 0.5), + Translation2d(0.5, -0.5), + Translation2d(-0.5, 0.5), + Translation2d(-0.5, -0.5), + ] + ) + self.positions = [ + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + ] + # cheating the initial pose + # TODO: more clever init + self.wpi_pose = Pose2d(PATH_CENTER_X_M + PATH_RADIUS_M, 0, 0) + self.time_s: float = 0 + self.gt_x: float + self.gt_y: float + self.gt_theta: float + # the order is the same as the detector getCorners order + # lower left + # lower right + # upper right + # upper left + self.gt_pixels: list[Point2] + # initialize + self.step(0) + # set positions back to zero + # TODO: more clever init + self.positions = [ + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + ] + + def step(self, dt_s: float) -> None: + """set all the state according to the supplied time""" + self.time_s += dt_s + self.gt_x = PATH_CENTER_X_M + PATH_RADIUS_M * math.cos( + 2 * math.pi * self.time_s / PATH_PERIOD_S + ) + self.gt_y = PATH_CENTER_Y_M + PATH_RADIUS_M * math.sin( + 2 * math.pi * self.time_s / PATH_PERIOD_S + ) + self.gt_theta = PAN_SCALE_RAD * math.sin( + 2 * math.pi * self.time_s / PAN_PERIOD_S + ) + + # find the wheel positions + new_wpi_pose = Pose2d(self.gt_x, self.gt_y, self.gt_theta) + twist = self.wpi_pose.log(new_wpi_pose) + self.wpi_pose = new_wpi_pose + self.positions = self.kinematics.to_swerve_module_positions( + self.positions, twist + ) + + robot_pose = Pose2(self.gt_x, self.gt_y, self.gt_theta) + camera_offset = Pose3(Rot3(), np.array([0, 0, 1])) + # lower left + p0 = self.px( + Point3(TAG_X, TAG_Y + (TAG_SIZE_M / 2), TAG_Z - (TAG_SIZE_M / 2)), + robot_pose, + camera_offset, + CALIB, + ) + # lower right + p1 = self.px( + Point3(TAG_X, TAG_Y - (TAG_SIZE_M / 2), TAG_Z - (TAG_SIZE_M / 2)), + robot_pose, + camera_offset, + CALIB, + ) + # upper right + p2 = self.px( + Point3(TAG_X, TAG_Y - (TAG_SIZE_M / 2), TAG_Z + (TAG_SIZE_M / 2)), + robot_pose, + camera_offset, + CALIB, + ) + # upper left + p3 = self.px( + Point3(TAG_X, TAG_Y + (TAG_SIZE_M / 2), TAG_Z + (TAG_SIZE_M / 2)), + robot_pose, + camera_offset, + CALIB, + ) + self.gt_pixels = [p0, p1, p2, p3] + + def px( # type: ignore + self, + landmark: Point3, # type: ignore + robot_pose: Pose2, + camera_offset: Pose3, + calib: Cal3DS2, + ) -> Point2: + """robot_pose and camera_offset are x-forward, z-up""" + # ctor a pose3 with x,y,yaw, x-forward, z-up + offset_pose = Pose3(robot_pose).compose(camera_offset) # type: ignore + # print("offset pose ", offset_pose) + camera_pose = offset_pose.compose(CAM_COORD) # type: ignore + # camera constructor expects z-forward y-down + # print("camera pose ", camera_pose) + # print("landmark ", landmark) + camera = PinholeCameraCal3DS2(camera_pose, calib) + return camera.project(landmark) # type: ignore diff --git a/raspberry_pi/tests/pose_estimator/simulator_test.py b/raspberry_pi/tests/pose_estimator/simulator_test.py new file mode 100644 index 000000000..43433b8df --- /dev/null +++ b/raspberry_pi/tests/pose_estimator/simulator_test.py @@ -0,0 +1,104 @@ +# pylint: disable=C0301,E0611,R0903 + +import math +import unittest + +import numpy as np +from gtsam import Cal3DS2, Point2, Point3, Pose2, Pose3, Rot3 +from wpimath.geometry import Rotation2d, Translation2d + +from app.pose_estimator.drive_util import DriveUtil +from app.pose_estimator.swerve_drive_kinematics import SwerveDriveKinematics100 +from app.pose_estimator.swerve_module_position import ( + OptionalRotation2d, + SwerveModulePosition100, +) +from tests.pose_estimator.simulator import Simulator + + +class SimulatorTest(unittest.TestCase): + def test_simple(self) -> None: + sim = Simulator() + self.assertAlmostEqual(2, sim.gt_x) + self.assertAlmostEqual(0, sim.gt_y) + self.assertAlmostEqual(0, sim.gt_theta) + self.assertAlmostEqual(0, sim.positions[0].distance_m) + self.assertAlmostEqual(0, sim.positions[0].angle.value.radians()) + + # lower left + self.assertAlmostEqual(192, sim.gt_pixels[0][0], 0) + self.assertAlmostEqual(208, sim.gt_pixels[0][1], 0) + # lower right + self.assertAlmostEqual(208, sim.gt_pixels[1][0], 0) + self.assertAlmostEqual(208, sim.gt_pixels[1][1], 0) + # upper right + self.assertAlmostEqual(208, sim.gt_pixels[2][0], 0) + self.assertAlmostEqual(192, sim.gt_pixels[2][1], 0) + # upper left + self.assertAlmostEqual(192, sim.gt_pixels[3][0], 0) + self.assertAlmostEqual(192, sim.gt_pixels[3][1], 0) + + sim.step(math.pi / 2) + self.assertAlmostEqual(1, sim.gt_x) + self.assertAlmostEqual(1, sim.gt_y) + self.assertAlmostEqual(-0.5, sim.gt_theta) + sim.step(math.pi / 2) + self.assertAlmostEqual(0, sim.gt_x) + self.assertAlmostEqual(0, sim.gt_y) + self.assertAlmostEqual(0, sim.gt_theta) + sim.step(math.pi / 2) + self.assertAlmostEqual(1, sim.gt_x) + self.assertAlmostEqual(-1, sim.gt_y) + self.assertAlmostEqual(0.5, sim.gt_theta) + + def test_camera(self) -> None: + sim = Simulator() + # this is the lower right corner + landmark = Point3(4, -(0.1651 / 2.0), 1 - (0.1651 / 2)) + robot_pose = Pose2(2, 0, 0) + camera_offset = Pose3(Rot3(), np.array([0, 0, 1])) + calib = Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1, 0.0, 0.0) + px: Point2 = sim.px(landmark, robot_pose, camera_offset, calib) + # pixel should be in the lower right quadrant + # remember x+right, y+down + self.assertAlmostEqual(208, px[0], 0) + self.assertAlmostEqual(208, px[1], 0) + + def test_full(self) -> None: + sim = Simulator() + k = SwerveDriveKinematics100( + [ + Translation2d(0.5, 0.5), + Translation2d(0.5, -0.5), + Translation2d(-0.5, 0.5), + Translation2d(-0.5, -0.5), + ] + ) + positions = [ + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + SwerveModulePosition100(0, OptionalRotation2d(True, Rotation2d(0))), + ] + pose = sim.wpi_pose + print() + for i in range(500): + + # make sure position-derived pose matches the sim pose + deltas = DriveUtil.module_position_delta(positions, sim.positions) + twist = k.to_twist_2d(deltas) + new_pose = pose.exp(twist) + pose = new_pose + positions = sim.positions + self.assertAlmostEqual(new_pose.x, sim.wpi_pose.x) + self.assertAlmostEqual(new_pose.y, sim.wpi_pose.y) + self.assertAlmostEqual(new_pose.rotation().radians(), sim.wpi_pose.rotation().radians()) + + t = i * 0.02 + x = sim.gt_x + y = sim.gt_y + theta = sim.gt_theta + d0 = sim.positions[0].distance_m + a0 = sim.positions[0].angle.value.radians() + print(f"{t:5.2f} {x:5.2f} {y:5.2f} {theta:5.2f} {d0:5.2f} {a0:5.2f}") + sim.step(0.02) diff --git a/raspberry_pi/tests/pose_estimator/swerve_drive_kinematics_test.py b/raspberry_pi/tests/pose_estimator/swerve_drive_kinematics_test.py new file mode 100644 index 000000000..3b7c1dad3 --- /dev/null +++ b/raspberry_pi/tests/pose_estimator/swerve_drive_kinematics_test.py @@ -0,0 +1,187 @@ +import math +import unittest + +from wpimath.geometry import Translation2d, Rotation2d + +from app.pose_estimator.swerve_drive_kinematics import SwerveDriveKinematics100 +from app.pose_estimator.swerve_module_position import ( + OptionalRotation2d, + SwerveModulePosition100, +) + + +class SwerveDriveKinematics100Test(unittest.TestCase): + def test_inverse(self) -> None: + """Same as java""" + k = SwerveDriveKinematics100( + [ + Translation2d(0.5, 0.5), + Translation2d(0.5, -0.5), + Translation2d(-0.5, 0.5), + Translation2d(-0.5, -0.5), + ] + ) + self.assertEqual(8, k.inverse_kinematics.shape[0]) + self.assertEqual(3, k.inverse_kinematics.shape[1]) + + self.assertAlmostEqual(1, k.inverse_kinematics[0, 0]) + self.assertAlmostEqual(0, k.inverse_kinematics[0, 1]) + self.assertAlmostEqual(-0.5, k.inverse_kinematics[0, 2]) + self.assertAlmostEqual(0, k.inverse_kinematics[1, 0]) + self.assertAlmostEqual(1, k.inverse_kinematics[1, 1]) + self.assertAlmostEqual(0.5, k.inverse_kinematics[1, 2]) + self.assertAlmostEqual(1, k.inverse_kinematics[2, 0]) + self.assertAlmostEqual(0, k.inverse_kinematics[2, 1]) + self.assertAlmostEqual(0.5, k.inverse_kinematics[2, 2]) + self.assertAlmostEqual(0, k.inverse_kinematics[3, 0]) + self.assertAlmostEqual(1, k.inverse_kinematics[3, 1]) + self.assertAlmostEqual(0.5, k.inverse_kinematics[3, 2]) + + def test_forward(self) -> None: + """Same as java""" + k = SwerveDriveKinematics100( + [ + Translation2d(0.5, 0.5), + Translation2d(0.5, -0.5), + Translation2d(-0.5, 0.5), + Translation2d(-0.5, -0.5), + ] + ) + self.assertEqual(3, k.forward_kinematics.shape[0]) + self.assertEqual(8, k.forward_kinematics.shape[1]) + self.assertAlmostEqual(0.25, k.forward_kinematics[0, 0]) + self.assertAlmostEqual(0, k.forward_kinematics[1, 0]) + self.assertAlmostEqual(-0.25, k.forward_kinematics[2, 0]) + self.assertAlmostEqual(0, k.forward_kinematics[0, 1]) + self.assertAlmostEqual(0.25, k.forward_kinematics[1, 1]) + self.assertAlmostEqual(0.25, k.forward_kinematics[2, 1]) + self.assertAlmostEqual(0.25, k.forward_kinematics[0, 2]) + self.assertAlmostEqual(0, k.forward_kinematics[1, 2]) + self.assertAlmostEqual(0.25, k.forward_kinematics[2, 2]) + self.assertAlmostEqual(0, k.forward_kinematics[0, 3]) + self.assertAlmostEqual(0.25, k.forward_kinematics[1, 3]) + self.assertAlmostEqual(0.25, k.forward_kinematics[2, 3]) + + def test_twist_straight(self) -> None: + kinematics = SwerveDriveKinematics100( + [ + Translation2d(0.5, 0.5), + Translation2d(0.5, -0.5), + Translation2d(-0.5, 0.5), + Translation2d(-0.5, -0.5), + ] + ) + # 0.1m straight ahead, all same. + twist = kinematics.to_twist_2d( + [ + SwerveModulePosition100( + 0.1, OptionalRotation2d(True, Rotation2d.fromDegrees(0)) + ), + SwerveModulePosition100( + 0.1, OptionalRotation2d(True, Rotation2d.fromDegrees(0)) + ), + SwerveModulePosition100( + 0.1, OptionalRotation2d(True, Rotation2d.fromDegrees(0)) + ), + SwerveModulePosition100( + 0.1, OptionalRotation2d(True, Rotation2d.fromDegrees(0)) + ), + ] + ) + + self.assertAlmostEqual(0.1, twist.dx, 9) + self.assertAlmostEqual(0, twist.dy, 9) + self.assertAlmostEqual(0, twist.dtheta, 9) + + def test_twist_spin(self) -> None: + kinematics = SwerveDriveKinematics100( + [ + Translation2d(0.5, 0.5), + Translation2d(0.5, -0.5), + Translation2d(-0.5, 0.5), + Translation2d(-0.5, -0.5), + ] + ) + + twist = kinematics.to_twist_2d( + [ + SwerveModulePosition100( + 0.1, OptionalRotation2d(True, Rotation2d.fromDegrees(135)) + ), + SwerveModulePosition100( + 0.1, OptionalRotation2d(True, Rotation2d.fromDegrees(45)) + ), + SwerveModulePosition100( + 0.1, OptionalRotation2d(True, Rotation2d.fromDegrees(-135)) + ), + SwerveModulePosition100( + 0.1, OptionalRotation2d(True, Rotation2d.fromDegrees(-45)) + ), + ] + ) + + self.assertAlmostEqual(0, twist.dx, 9) + self.assertAlmostEqual(0, twist.dy, 9) + self.assertAlmostEqual(0.141, twist.dtheta, 3) + + def test_straight_line_forward_kinematics_with_deltas(self) -> None: + m_fl = Translation2d(12, 12) + m_fr = Translation2d(12, -12) + m_bl = Translation2d(-12, 12) + m_br = Translation2d(-12, -12) + + m_kinematics = SwerveDriveKinematics100([m_fl, m_fr, m_bl, m_br]) + + # test forward kinematics going in a straight line + delta = SwerveModulePosition100( + 5.0, OptionalRotation2d(True, Rotation2d.fromDegrees(0.0)) + ) + twist = m_kinematics.to_twist_2d([delta, delta, delta, delta]) + + self.assertAlmostEqual(5.0, twist.dx, 9) + self.assertAlmostEqual(0.0, twist.dy, 9) + self.assertAlmostEqual(0.0, twist.dtheta, 9) + + def test_straight_strafe_forward_kinematics_with_deltas(self) -> None: + m_fl = Translation2d(12, 12) + m_fr = Translation2d(12, -12) + m_bl = Translation2d(-12, 12) + m_br = Translation2d(-12, -12) + + m_kinematics = SwerveDriveKinematics100([m_fl, m_fr, m_bl, m_br]) + + delta = SwerveModulePosition100( + 5.0, OptionalRotation2d(True, Rotation2d.fromDegrees(90.0)) + ) + twist = m_kinematics.to_twist_2d([delta, delta, delta, delta]) + + self.assertAlmostEqual(0.0, twist.dx, 9) + self.assertAlmostEqual(5.0, twist.dy, 9) + self.assertAlmostEqual(0.0, twist.dtheta, 9) + + def test_turn_in_place_forward_kinematics_with_deltas(self) -> None: + m_fl = Translation2d(12, 12) + m_fr = Translation2d(12, -12) + m_bl = Translation2d(-12, 12) + m_br = Translation2d(-12, -12) + + m_kinematics = SwerveDriveKinematics100([m_fl, m_fr, m_bl, m_br]) + + fl_delta = SwerveModulePosition100( + 106.629, OptionalRotation2d(True, Rotation2d.fromDegrees(135)) + ) + fr_delta = SwerveModulePosition100( + 106.629, OptionalRotation2d(True, Rotation2d.fromDegrees(45)) + ) + bl_delta = SwerveModulePosition100( + 106.629, OptionalRotation2d(True, Rotation2d.fromDegrees(-135)) + ) + br_delta = SwerveModulePosition100( + 106.629, OptionalRotation2d(True, Rotation2d.fromDegrees(-45)) + ) + + twist = m_kinematics.to_twist_2d([fl_delta, fr_delta, bl_delta, br_delta]) + + self.assertAlmostEqual(0.0, twist.dx, 9) + self.assertAlmostEqual(0.0, twist.dy, 9) + self.assertAlmostEqual(2 * math.pi, twist.dtheta, 1) diff --git a/studies/redux_gyro/networktables.json b/studies/redux_gyro/networktables.json new file mode 100644 index 000000000..fe51488c7 --- /dev/null +++ b/studies/redux_gyro/networktables.json @@ -0,0 +1 @@ +[] diff --git a/studies/redux_gyro/simgui-ds.json b/studies/redux_gyro/simgui-ds.json new file mode 100644 index 000000000..73cc713cb --- /dev/null +++ b/studies/redux_gyro/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/studies/redux_gyro/simgui.json b/studies/redux_gyro/simgui.json new file mode 100644 index 000000000..b01f62697 --- /dev/null +++ b/studies/redux_gyro/simgui.json @@ -0,0 +1,17 @@ +{ + "HALProvider": { + "Other Devices": { + "window": { + "visible": false + } + } + }, + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo" + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/studies/slippery_tires/src/main/java/org/team100/lib/util/SwerveDriveKinematics100.java b/studies/slippery_tires/src/main/java/org/team100/lib/util/SwerveDriveKinematics100.java new file mode 100644 index 000000000..dd5f84b38 --- /dev/null +++ b/studies/slippery_tires/src/main/java/org/team100/lib/util/SwerveDriveKinematics100.java @@ -0,0 +1,601 @@ + +// copied here because there are bits of this (the "vector" part) that are only used by the tire model + +package org.team100.lib.motion.drivetrain.kinodynamics; + +import java.util.Arrays; +import java.util.Optional; + +import org.ejml.simple.SimpleMatrix; +import org.team100.lib.geometry.Vector2d; + +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; + +/** + * 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 + */ +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 + * + * 
+     * 1   0  -y1
+     * 0   1   x1
+     * 1   0  -y2
+     * 0   1   x2
+     * ...
+     *
+ * + * the chassis speed vector is something like + * + * 
+     * vx  vy  omega
+     *
+ * + * when these are multiplied, the result is + * + * 
+     * vx - y1 omega = vx1
+     * vy + x1 omega = vy1
+     * vx - y2 omega = vx2
+     * vy + x2 omega = xy2
+     * ...
+     *
+ */ + + final SimpleMatrix m_inverseKinematics; + /** + * like this: + * this (3 x 2n) matrix is the pseudo-inverse of above, which ends up something + * + * 
+     *  0.25 0.00 0.25 0.00 ...
+     *  0.00 0.25 0.00 0.25 ...
+     * -1.00 1.00 1.00 1.00 ...
+     *
+ * + * the last row depends on the drive dimensions. + * + * the module state vector is something like + * + * 
+     * vx1
+     * vy1
+     * vx2
+     * vy2
+     * ...
+     *
+ * + * so when multiplied, the result is + * + * 
+     * mean(vx)
+     * mean(vy)
+     * some combination depending on dimensions
+     *
+ */ + final SimpleMatrix m_forwardKinematics; + /** + * Used when velocity is zero, to keep the steering the same. + * elements are nullable. + */ + private Rotation2d[] m_moduleHeadings; + + /** + * array order: + * + * frontLeft + * frontRight + * rearLeft + * rearRight + * + * @param moduleTranslationsM relative to the center of rotation + */ + 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); + m_moduleHeadings = nulls(m_numModules); + } + + /** + * Reset the internal swerve module headings + * + * array order: + * + * frontLeft + * frontRight + * rearLeft + * rearRight + * + * arg elements are nullable + */ + public void resetHeadings(Rotation2d... moduleHeadings) { + checkLength(moduleHeadings); + m_moduleHeadings = Arrays.copyOf(moduleHeadings, m_numModules); + } + + /** + * INVERSE: chassis speeds -> module states + * + * The resulting module state speeds are always positive. + */ + public SwerveModuleState100[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds) { + if (fullStop(chassisSpeeds)) { + return constantModuleHeadings(); // avoid steering when stopped + } + // [vx; vy; omega] (3 x 1) + SimpleMatrix chassisSpeedsVector = chassisSpeeds2Vector(chassisSpeeds); + // [v cos; v sin; ...] (2n x 1) + SwerveModuleState100[] states = statesFromVector(chassisSpeedsVector); + updateHeadings(states); + return states; + } + + /** + * INVERSE: chassis speeds -> module states + * + * The resulting module state speeds are always positive. + * TODO(vasili): i think this is unfinished work from 2nd order control??? + */ + public SwerveModuleState100[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds, SwerveModuleState100[] prevStates) { + if (fullStop(chassisSpeeds)) { + return constantModuleHeadings(); // avoid steering when stopped + } + // [vx; vy; omega] (3 x 1) + SimpleMatrix chassisSpeedsVector = chassisSpeeds2Vector(chassisSpeeds); + // [v cos; v sin; ...] (2n x 1) + SwerveModuleState100[] states = statesFromVector(chassisSpeedsVector); + updateHeadings(states); + return states; + } + + public SwerveModuleState100[] 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) + SwerveModuleState100[] prevStates = { + 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())) }; + SwerveModuleState100[] states = accelerationFromVector(chassisSpeedsVector, chassisSpeedsAccelerationVector, + prevStates); + updateHeadings(states); + return states; + } + + public SwerveModuleState100[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds, + ChassisSpeeds chassisSpeedsAcceleration, SwerveModuleState100[] 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) + SwerveModuleState100[] states = accelerationFromVector(chassisSpeedsVector, chassisSpeedsAccelerationVector, + prevStates); + updateHeadings(states); + return states; + } + + /** + * INVERSE: twist -> module position deltas + */ + public SwerveModulePosition100[] toSwerveModulePosition(Twist2d twist) { + if (fullStop(twist)) { + return constantModulePositions(); + } + // [dx; dy; dtheta] (3 x 1) + SimpleMatrix twistVector = twist2Vector(twist); + // [d cos; d sin; ...] (2n x 1) + SimpleMatrix deltaVector = m_inverseKinematics.mult(twistVector); + SwerveModulePosition100[] deltas = deltasFromVector(deltaVector); + updateHeadings(deltas); + return deltas; + } + + /** This is wrong, it assigns zero radians to the indeterminate case. */ + public Vector2d[] pos2vec(SwerveModulePosition100[] m) { + Vector2d[] vec = new Vector2d[m_numModules]; + for (int i = 0; i < m_numModules; ++i) { + SwerveModulePosition100 p = m[i]; + if (p.angle.isEmpty()) { + vec[i] = new Vector2d(0, 0); + } else { + vec[i] = new Vector2d(p.distanceMeters, p.angle.get()); + } + } + return vec; + } + + /** + * FORWARD: module states -> chassis speeds + * + * NOTE: do not use the returned omega, use the gyro instead. + */ + public ChassisSpeeds toChassisSpeeds(SwerveModuleState100... states) { + checkLength(states); + // [v cos; v sin; ...] (2n x 1) + SimpleMatrix statesVector = states2Vector(states); + // [vx; vy; omega] + SimpleMatrix chassisSpeedsVector = m_forwardKinematics.mult(statesVector); + return vector2ChassisSpeeds(chassisSpeedsVector); + } + + /** + * FORWARD: module deltas -> twist. + * + * NOTE: do not use the returned dtheta, use the gyro instead. + */ + public Twist2d toTwist2d(SwerveModulePosition100... deltas) { + checkLength(deltas); + // [d cos; d sin; ...] (2n x 1) + SimpleMatrix deltaVector = deltas2Vector(deltas); + // [dx ;dy; dtheta] + SimpleMatrix twistVector = m_forwardKinematics.mult(deltaVector); + return vector2Twist(twistVector); + } + + /** + * Scale wheel speeds to limit maximum. + * + * array order: + * + * frontLeft + * frontRight + * rearLeft + * rearRight + * + * @param states WILL BE MUTATED! + * @param maxSpeedM_s Max module speed + * @param maxAccelM_s2 Max module acceleration + * @param maxDeccelM_s2 Max module deceleration + */ + public static void desaturateWheelSpeeds(SwerveModuleState100[] states, double maxSpeedM_s, double maxAccelM_s2, + double maxDeccelM_s2, double maxTurnVelocityM_s) { + double realMaxSpeed = 0; + double realMaxAccel = 0; + double realMaxDeccel = 0; + double realTurnVelocity = 0; + for (SwerveModuleState100 moduleState : states) { + realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speedMetersPerSecond)); + realMaxAccel = Math.max(realMaxAccel, moduleState.accelMetersPerSecond_2); + realMaxDeccel = Math.min(realMaxDeccel, moduleState.accelMetersPerSecond_2); + realTurnVelocity = Math.max(maxTurnVelocityM_s, Math.abs(moduleState.omega)); + } + if (realMaxSpeed > maxSpeedM_s) { + for (SwerveModuleState100 moduleState : states) { + moduleState.speedMetersPerSecond = moduleState.speedMetersPerSecond / realMaxSpeed + * maxSpeedM_s; + } + } + if (realMaxAccel > maxAccelM_s2) { + for (SwerveModuleState100 moduleState : states) { + moduleState.accelMetersPerSecond_2 = moduleState.accelMetersPerSecond_2 / realMaxAccel + * maxAccelM_s2; + } + } + if (realMaxDeccel < -1.0 * maxDeccelM_s2) { + for (SwerveModuleState100 moduleState : states) { + moduleState.accelMetersPerSecond_2 = moduleState.accelMetersPerSecond_2 / (-1.0 * realMaxDeccel) + * maxDeccelM_s2; + } + } + if (realTurnVelocity > maxTurnVelocityM_s) { + for (SwerveModuleState100 moduleState : states) { + moduleState.omega = moduleState.omega / realTurnVelocity + * maxTurnVelocityM_s; + } + } + } + + /** + * Scale wheel speeds to limit maximum. + * + * @param states WILL BE MUTATED! + * @param maxSpeedM_s Max module speed + */ + public static void desaturateWheelSpeeds(SwerveModuleState100[] states, double maxSpeedM_s) { + double realMaxSpeed = 0; + for (SwerveModuleState100 moduleState : states) { + realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speedMetersPerSecond)); + } + if (realMaxSpeed > maxSpeedM_s) { + for (SwerveModuleState100 moduleState : states) { + moduleState.speedMetersPerSecond = moduleState.speedMetersPerSecond / realMaxSpeed + * maxSpeedM_s; + } + } + } + + /////////////////////////////////////// + + /** states -> [v cos; v sin; ... v cos; v sin] (2n x 1) */ + private SimpleMatrix states2Vector(SwerveModuleState100... moduleStates) { + SimpleMatrix moduleStatesMatrix = new SimpleMatrix(m_numModules * 2, 1); + for (int i = 0; i < m_numModules; i++) { + SwerveModuleState100 module = moduleStates[i]; + if (Math.abs(module.speedMetersPerSecond) < 1e-6 || module.angle.isEmpty()) { + // wheel is stopped, or angle is invalid so pretend it's stopped. + moduleStatesMatrix.set(i * 2, 0, 0); + moduleStatesMatrix.set(i * 2 + 1, 0, 0); + } else { + moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.get().getCos()); + moduleStatesMatrix.set(i * 2 + 1, 0, module.speedMetersPerSecond * module.angle.get().getSin()); + + } + } + return moduleStatesMatrix; + } + + /** deltas -> [d cos; d sin; ... ] (2n x 1) */ + private SimpleMatrix deltas2Vector(SwerveModulePosition100... moduleDeltas) { + SimpleMatrix moduleDeltaMatrix = new SimpleMatrix(m_numModules * 2, 1); + for (int i = 0; i < m_numModules; i++) { + SwerveModulePosition100 module = moduleDeltas[i]; + if (Math.abs(module.distanceMeters) < 1e-6 || module.angle.isEmpty()) { + moduleDeltaMatrix.set(i * 2, 0, 0); + moduleDeltaMatrix.set(i * 2 + 1, 0, 0); + } else { + moduleDeltaMatrix.set(i * 2, 0, module.distanceMeters * module.angle.get().getCos()); + moduleDeltaMatrix.set(i * 2 + 1, 0, module.distanceMeters * module.angle.get().getSin()); + } + } + return moduleDeltaMatrix; + } + + /** ChassisSpeeds -> [vx; vy; omega] (3 x 1) */ + private SimpleMatrix chassisSpeeds2Vector(ChassisSpeeds chassisSpeeds) { + SimpleMatrix chassisSpeedsVector = new SimpleMatrix(3, 1); + chassisSpeedsVector.setColumn( + 0, + 0, + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond, + chassisSpeeds.omegaRadiansPerSecond); + return chassisSpeedsVector; + } + + private SimpleMatrix twist2Vector(Twist2d twist) { + SimpleMatrix twistVector = new SimpleMatrix(3, 1); + twistVector.setColumn( + 0, + 0, + twist.dx, + twist.dy, + twist.dtheta); + return twistVector; + } + + /** [vx; vy; omega] (3 x 1) -> ChassisSpeeds */ + private static ChassisSpeeds vector2ChassisSpeeds(SimpleMatrix v) { + return new ChassisSpeeds(v.get(0, 0), v.get(1, 0), v.get(2, 0)); + } + + /** [dx; dy; dtheta] (3 x 1) -> Twist2d */ + private static Twist2d vector2Twist(SimpleMatrix v) { + return new Twist2d(v.get(0, 0), v.get(1, 0), v.get(2, 0)); + } + + /** True if speeds are (nearly) stopped. Deadband upstream for this to work. */ + private boolean fullStop(ChassisSpeeds chassisSpeeds) { + return Math.abs(chassisSpeeds.vxMetersPerSecond) < kEpsilon + && Math.abs(chassisSpeeds.vyMetersPerSecond) < kEpsilon + && Math.abs(chassisSpeeds.omegaRadiansPerSecond) < kEpsilon; + } + + private boolean fullStop(Twist2d twist) { + return Math.abs(twist.dx) < kEpsilon + && Math.abs(twist.dy) < kEpsilon + && Math.abs(twist.dtheta) < kEpsilon; + } + + /** Zero velocity, same heading as before. */ + private SwerveModuleState100[] constantModuleHeadings() { + SwerveModuleState100[] mods = new SwerveModuleState100[m_numModules]; + for (int i = 0; i < m_numModules; i++) { + if (m_moduleHeadings[i] == null) { + mods[i] = new SwerveModuleState100(0.0, Optional.empty()); + } else { + mods[i] = new SwerveModuleState100(0.0, Optional.of(m_moduleHeadings[i])); + } + } + return mods; + } + + private SwerveModulePosition100[] constantModulePositions() { + SwerveModulePosition100[] mods = new SwerveModulePosition100[m_numModules]; + for (int i = 0; i < m_numModules; i++) { + if (m_moduleHeadings[i] == null) { + mods[i] = new SwerveModulePosition100(0.0, Optional.empty()); + } else { + mods[i] = new SwerveModulePosition100(0.0, Optional.of(m_moduleHeadings[i])); + } + } + return mods; + } + + /** + * [v cos; v sin; ... ] (2n x 1) -> states[] + * + * The resulting module speed is always positive. + */ + public SwerveModuleState100[] statesFromVector(SimpleMatrix chassisSpeedsVector) { + SimpleMatrix moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector); + SwerveModuleState100[] moduleStates = new SwerveModuleState100[m_numModules]; + for (int i = 0; i < m_numModules; i++) { + double x = moduleStatesMatrix.get(i * 2, 0); + double y = moduleStatesMatrix.get(i * 2 + 1, 0); + if (Math.abs(x) < 1e-6 && Math.abs(y) < 1e-6) { + moduleStates[i] = new SwerveModuleState100(0.0, Optional.empty()); + } else { + moduleStates[i] = new SwerveModuleState100(Math.hypot(x, y), + Optional.of(new Rotation2d(x, y))); + } + } + return moduleStates; + } + + /** + * // TODO: test this and keep it or toss it + * + * https://www.chiefdelphi.com/uploads/short-url/qzj4k2LyBs7rLxAem0YajNIlStH.pdf + */ + public SwerveModuleState100[] accelerationFromVector( + SimpleMatrix chassisSpeedsMatrix, + SimpleMatrix chassisSpeedsAccelerationMatrix, + SwerveModuleState100[] prevStates) { + SwerveModuleState100[] moduleStates = new SwerveModuleState100[m_numModules]; + for (int i = 0; i < m_numModules; i++) { + Optional angle2 = prevStates[i].angle; + + if (angle2.isEmpty()) { + throw new IllegalArgumentException(); + } + + SimpleMatrix dmodulexy = m_mat[i].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 + moduleStates[i] = new SwerveModuleState100(0, Optional.empty()); + continue; + } 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(i, + 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); + } + moduleStates[i] = new SwerveModuleState100(speed, Optional.of(angle), moduleAccelMat.get(0, 0), + moduleAccelMat.get(1, 0)); + } + return moduleStates; + } + + 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. + */ + private SwerveModulePosition100[] deltasFromVector(SimpleMatrix moduleDeltaVector) { + SwerveModulePosition100[] moduleDeltas = new SwerveModulePosition100[m_numModules]; + for (int i = 0; i < m_numModules; i++) { + double x = moduleDeltaVector.get(i * 2, 0); + double y = moduleDeltaVector.get(i * 2 + 1, 0); + moduleDeltas[i] = new SwerveModulePosition100(x, y); + } + return moduleDeltas; + } + + /** Keep a copy of headings in case we need them for full-stop. */ + private void updateHeadings(SwerveModuleState100[] moduleStates) { + for (int i = 0; i < m_numModules; i++) { + if (moduleStates[i].angle.isEmpty()) { + // skip the update, remember the most-recent not-invalid value. + continue; + } + m_moduleHeadings[i] = moduleStates[i].angle.get(); + } + } + + private void updateHeadings(SwerveModulePosition100[] mods) { + for (int i = 0; i < m_numModules; i++) { + if (mods[i].angle.isEmpty()) { + // skip the update, remember the most-recent not-invalid value + continue; + } + m_moduleHeadings[i] = mods[i].angle.get(); + } + } + + /** Module headings null to start to avoid transients? */ + private static Rotation2d[] nulls(int numModules) { + Rotation2d[] moduleHeadings = new Rotation2d[numModules]; + Arrays.fill(moduleHeadings, null); + return moduleHeadings; + } + + /** module locations -> inverse kinematics matrix (2n x 3) */ + private static SimpleMatrix inverseMatrix(Translation2d[] moduleLocations) { + int numModules = moduleLocations.length; + SimpleMatrix inverseKinematics = new SimpleMatrix(numModules * 2, 3); + for (int i = 0; i < numModules; i++) { + inverseKinematics.setRow(i * 2 + 0, 0, 1, 0, -moduleLocations[i].getY()); + inverseKinematics.setRow(i * 2 + 1, 0, 0, 1, +moduleLocations[i].getX()); + } + return inverseKinematics; + } + + private void checkModuleCount(Translation2d... moduleTranslationsM) { + if (moduleTranslationsM.length < 2) { + 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); + } + } +}