From 329568fa95315efb677b41d1a0be39900986f5df Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Fri, 26 Dec 2025 18:56:28 -0800 Subject: [PATCH 1/4] move pose age measurement from camera reader to updater --- .../org/team100/frc2025/indicator/LEDIndicator.java | 9 +++++---- .../java/org/team100/frc2025/robot/Machinery.java | 2 +- .../lib/localization/AprilTagRobotLocalizer.java | 13 +------------ .../lib/localization/NudgingVisionUpdater.java | 13 +++++++++++++ 4 files changed, 20 insertions(+), 17 deletions(-) diff --git a/comp/src/main/java/org/team100/frc2025/indicator/LEDIndicator.java b/comp/src/main/java/org/team100/frc2025/indicator/LEDIndicator.java index d3f0a401..333df9c9 100644 --- a/comp/src/main/java/org/team100/frc2025/indicator/LEDIndicator.java +++ b/comp/src/main/java/org/team100/frc2025/indicator/LEDIndicator.java @@ -4,6 +4,7 @@ import org.team100.frc2025.grip.Manipulator; import org.team100.lib.coherence.Takt; import org.team100.lib.localization.AprilTagRobotLocalizer; +import org.team100.lib.localization.NudgingVisionUpdater; import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; @@ -42,7 +43,7 @@ public class LEDIndicator { private final AddressableLEDBuffer m_blackBuffer; private final AddressableLEDBuffer m_whiteBuffer; - private final AprilTagRobotLocalizer m_localizer; + private final NudgingVisionUpdater m_updater; private final Manipulator m_manipulator; private final ClimberIntake m_climberIntake; @@ -55,7 +56,7 @@ public class LEDIndicator { * Since this has logic about the 2025 game it should be in the "comp" project. */ public LEDIndicator( - AprilTagRobotLocalizer localizer, + NudgingVisionUpdater updater, Manipulator manipulator, ClimberIntake climberIntake) { m_led = new AddressableLED(0); @@ -68,7 +69,7 @@ public LEDIndicator( m_blackBuffer = fill(Color.kBlack); m_led.setData(m_redBuffer); m_led.start(); - m_localizer = localizer; + m_updater = updater; m_manipulator = manipulator; m_climberIntake = climberIntake; } @@ -79,7 +80,7 @@ public LEDIndicator( public void periodic() { if (RobotState.isDisabled()) { - if (m_localizer.getPoseAgeSec() < 1) { + if (m_updater.getPoseAgeSec() < 1) { m_led.setData(m_greenBuffer); } else { m_led.setData(m_redBuffer); diff --git a/comp/src/main/java/org/team100/frc2025/robot/Machinery.java b/comp/src/main/java/org/team100/frc2025/robot/Machinery.java index b69494ef..76e65793 100644 --- a/comp/src/main/java/org/team100/frc2025/robot/Machinery.java +++ b/comp/src/main/java/org/team100/frc2025/robot/Machinery.java @@ -162,7 +162,7 @@ public Machinery() { // LED INDICATOR // m_leds = new LEDIndicator( - m_localizer, + visionUpdater, m_manipulator, m_climberIntake); m_beeper = new Beeper(m_mech, m_manipulator, m_drive); diff --git a/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java b/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java index 5886932a..7944840f 100644 --- a/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java +++ b/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java @@ -121,8 +121,6 @@ public class AprilTagRobotLocalizer extends CameraReader { // distance between consecutive updates, and ignore too-far updates. private Pose2d m_prevPose; - private double m_latestTime = 0; - /** use tags closer than this; ignore tags further than this. */ private double m_heedRadiusM = 3.5; @@ -173,15 +171,6 @@ public AprilTagRobotLocalizer( m_log_lag = log.doubleLogger(Level.TRACE, "lag"); } - /** - * The age of the last pose estimate, in microseconds. - * The caller could use this to, say, indicate tag visibility. - */ - public double getPoseAgeSec() { - double now = Takt.get(); - return now - m_latestTime; - } - @Override protected void perValue( Transform3d cameraOffset, @@ -240,6 +229,7 @@ void estimateRobotPose( double correctedTimestamp = valueTimestamp - IMPORTANT_MAGIC_NUMBER; // this seems to always be 1. ???? + // TODO: look more closely at this m_log_lag.log(() -> Takt.get() - correctedTimestamp); if (!optAlliance.isPresent()) { @@ -389,7 +379,6 @@ void estimateRobotPose( stateStdDevs(), visionMeasurementStdDevs(distanceM)); - m_latestTime = Takt.get(); m_prevPose = pose; } } diff --git a/lib/src/main/java/org/team100/lib/localization/NudgingVisionUpdater.java b/lib/src/main/java/org/team100/lib/localization/NudgingVisionUpdater.java index 6034ec3c..57dbb225 100644 --- a/lib/src/main/java/org/team100/lib/localization/NudgingVisionUpdater.java +++ b/lib/src/main/java/org/team100/lib/localization/NudgingVisionUpdater.java @@ -1,5 +1,6 @@ package org.team100.lib.localization; +import org.team100.lib.coherence.Takt; import org.team100.lib.state.ModelR3; import edu.wpi.first.math.geometry.Pose2d; @@ -19,6 +20,8 @@ public class NudgingVisionUpdater implements VisionUpdater { private final SwerveHistory m_history; /** For replay. */ private final OdometryUpdater m_odometryUpdater; + /** To measure time since last update, for indicator. */ + private double m_latestTimeS = 0; public NudgingVisionUpdater( SwerveHistory history, @@ -54,6 +57,16 @@ public void put( new ModelR3(nudged, sample.m_state.velocity()), sample.m_wheelPositions); m_odometryUpdater.replay(timestampS); + m_latestTimeS = Takt.get(); + } + + /** + * The age of the last pose estimate, in seconds. + * The caller could use this to, say, indicate tag visibility. + */ + public double getPoseAgeSec() { + double now = Takt.get(); + return now - m_latestTimeS; } ///////////////////////////////////////// From f27a9dd534d19ce5c5ed37fa7a3a6482f55b79b0 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Fri, 26 Dec 2025 20:00:41 -0800 Subject: [PATCH 2/4] cleaning localizer --- .../localization/AprilTagRobotLocalizer.java | 275 +++++++++--------- .../localization/NudgingVisionUpdater.java | 37 +-- .../team100/lib/localization/Uncertainty.java | 95 ++++++ .../AprilTagRobotLocalizerTest.java | 47 +-- .../lib/localization/UncertaintyTest.java | 70 +++++ .../lib/localization/VisionUpdaterTest.java | 38 +-- 6 files changed, 314 insertions(+), 248 deletions(-) create mode 100644 lib/src/main/java/org/team100/lib/localization/Uncertainty.java create mode 100644 lib/src/test/java/org/team100/lib/localization/UncertaintyTest.java diff --git a/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java b/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java index 7944840f..9e46f8f0 100644 --- a/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java +++ b/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java @@ -42,6 +42,7 @@ */ public class AprilTagRobotLocalizer extends CameraReader { private static final boolean DEBUG = false; + /** Maximum age of the sights we publish for diagnosis. */ private static final double HISTORY_DURATION = 1.0; @@ -58,24 +59,11 @@ public class AprilTagRobotLocalizer extends CameraReader { * always use the camera. */ private static final double TAG_ROTATION_BELIEF_THRESHOLD_M = 0; + /** Discard results further than this from the previous one. */ private static final double VISION_CHANGE_TOLERANCE_M = 0.1; // private static final double VISION_CHANGE_TOLERANCE_M = 1; - /** this is the default value which, in hindsight, seems ridiculously high. */ - private static final double[] defaultStateStdDevs = new double[] { - 0.1, - 0.1, - 0.1 }; - /** - * This value is tuned so that errors scale at 0.2x per second. See - * SwerveDrivePoseEstimator100Test::testFirmerNudge. - */ - static final double[] tightStateStdDevs = new double[] { - 0.001, - 0.001, - 0.1 }; - private final DoubleFunction m_history; private final VisionUpdater m_visionUpdater; private final AprilTagFieldLayoutWithCorrectOrientation m_layout; @@ -242,28 +230,12 @@ void estimateRobotPose( m_log_alliance.log(() -> alliance); m_log_heedRadius.log(() -> m_heedRadiusM); - // The pose from the frame timestamp. - // note this pulls from the *old history* not the *odometry-updated history* - // because we don't care about the latest odometry update: we're trying to - // affect the history from several cycles ago -- and replaying that. it's ok for - // new odometry to be the last thing. - final Pose2d historicalPose = m_history.apply(correctedTimestamp).pose(); - - // The gyro rotation for the frame timestamp - final Rotation2d gyroRotation = historicalPose.getRotation(); - if (DEBUG) { - System.out.printf("gyro rotation %f\n", gyroRotation.getRadians()); - } + Pose2d historicalPose = historicalPose(correctedTimestamp); for (int i = 0; i < blips.length; ++i) { Blip24 blip = blips[i]; - if (DEBUG) { - Translation3d t = blip.getRawPose().getTranslation(); - Rotation3d r = blip.getRawPose().getRotation(); - System.out.printf("blip raw pose %d X %5.2f Y %5.2f Z %5.2f R %5.2f P %5.2f Y %5.2f\n", - blip.getId(), t.getX(), t.getY(), t.getZ(), r.getX(), r.getY(), r.getZ()); - } + printBlip(blip); // Look up the pose of the tag in the field frame. Optional tagInFieldCoordsOptional = m_layout.getTagPose(alliance, blip.getId()); @@ -274,146 +246,165 @@ void estimateRobotPose( } // Field-to-tag, canonical pose from the JSON file - final Pose3d tagInField = tagInFieldCoordsOptional.get(); - - // Camera-to-tag, as it appears in the camera frame. - Transform3d blipTransform = blip.blipToTransform(); - m_log_tag_in_camera.log(() -> blipTransform); - - Transform3d tagInCamera = blipTransform; - - if (DEBUG) { - // This is used for camera offset calibration. Place a tag at a known position, - // observe the offset, and add it to Camera.java, inverted. - Transform3d tagInRobot = cameraOffset.plus(tagInCamera); - System.out.printf("tagInRobot id %d X %5.2f Y %5.2f Z %5.2f R %5.2f P %5.2f Y %5.2f\n", - blip.getId(), tagInRobot.getTranslation().getX(), tagInRobot.getTranslation().getY(), - tagInRobot.getTranslation().getZ(), tagInRobot.getRotation().getX(), - tagInRobot.getRotation().getY(), tagInRobot.getRotation().getZ()); - } + Pose3d tagInField = tagInFieldCoordsOptional.get(); - if (tagInCamera.getTranslation().getNorm() > TAG_ROTATION_BELIEF_THRESHOLD_M) { - // If the tag is further than the threshold, replace the tag rotation with - // a rotation derived from the gyro, if available. - m_log_using_gyro.log(() -> true); - tagInCamera = PoseEstimationHelper.tagInCamera( - cameraOffset, - tagInField, - tagInCamera, - new Rotation3d(gyroRotation)); - } else { - m_log_using_gyro.log(() -> false); - } + Transform3d tagInCamera = tagInCamera(blip); - // Log the estimated tag position, for visualization of vision error + printForCalibration(cameraOffset, blip, tagInCamera); - // Field-to-robot - Pose3d historicalPose3d = new Pose3d(historicalPose); - // Field-to-robot plus robot-to-camera = field-to-camera - Pose3d historicalCameraInField = historicalPose3d.transformBy(cameraOffset); - // given the historical pose, where do we think the tag is? - Pose3d estimatedTagInField = historicalCameraInField.transformBy(tagInCamera); - m_allTags.add(correctedTimestamp, estimatedTagInField); - // clean the used-tags collection in case we don't end up writing to it. - m_usedTags.cleanup(correctedTimestamp); + tagInCamera = maybeOverrideRotation(cameraOffset, historicalPose, tagInField, tagInCamera); + + Pose3d estimatedTagInField = estimatedTagInField(cameraOffset, historicalPose, tagInCamera); - // log the norm of the translational error of the tag. - Transform3d tagError = tagInField.minus(estimatedTagInField); - m_log_tag_error.log(() -> tagError.getTranslation().getNorm()); + m_allTags.add(correctedTimestamp, estimatedTagInField); - // Estimate of robot pose. - Pose3d pose3d = PoseEstimationHelper.robotInField( - cameraOffset, - tagInField, - tagInCamera); + logTagError(tagInField, estimatedTagInField); - // Robot in field frame. Use the gyro for rotation if available. - final Pose2d pose = new Pose2d( - pose3d.getTranslation().toTranslation2d(), - gyroRotation); + Pose2d robotPose2d = robotPose2d(historicalPose, cameraOffset, tagInField, tagInCamera); - m_log_pose.log(() -> pose); - m_pub_pose.set(pose); + // Clean the used-tags collection in case we don't end up writing to it. + m_usedTags.cleanup(correctedTimestamp); + ////////////////////////////////////////////////////////////////// + /// + /// Should we use this update? + /// if (!Experiments.instance.enabled(Experiment.HeedVision)) { - // If we've turned vision off altogether, then don't apply this update to the - // pose estimator. - if (DEBUG) - System.out.println("heedvision is off"); + // No, we've turned vision off. continue; } - - if (blipTransform.getTranslation().getNorm() > m_heedRadiusM) { - if (DEBUG) - System.out.println("tag is too far"); - // Skip too-far tags. + /// + if (tagInCamera.getTranslation().getNorm() > m_heedRadiusM) { + // No, the tag is too far away. continue; } - + /// if (m_prevPose == null) { - // Ignore the very first update since we have no idea if it is far from the - // previous one. - if (DEBUG) - System.out.println("skip first update"); - m_prevPose = pose; + // No, we need another nearby fix to believe either one. + m_prevPose = robotPose2d; continue; } - - final double distanceM = Metrics.translationalDistance(m_prevPose, pose); + /// + double distanceM = Metrics.translationalDistance(m_prevPose, robotPose2d); if (distanceM > VISION_CHANGE_TOLERANCE_M) { - // The new estimate is too far from the previous one: it's probably garbage. - m_prevPose = pose; - if (DEBUG) - System.out.println("too far from previous"); + // No, the new estimate is too far from the previous one. + m_prevPose = robotPose2d; continue; } - - // If we got this far, we're using the tag for localization, so add it to the - // history of used tags. + /// + /// Yes, we should use this update. + /// + ////////////////////////////////////////////////////////////////// m_usedTags.add(correctedTimestamp, estimatedTagInField); - m_visionUpdater.put( correctedTimestamp, - pose, - stateStdDevs(), - visionMeasurementStdDevs(distanceM)); - - m_prevPose = pose; + robotPose2d, + Uncertainty.stateStdDevs(), + Uncertainty.visionMeasurementStdDevs(distanceM)); + m_prevPose = robotPose2d; } } - static double[] stateStdDevs() { - if (Experiments.instance.enabled(Experiment.AvoidVisionJitter)) { - return tightStateStdDevs; + /** + * Project the 3d pose into a Pose2d, but use the historical pose rotation + * (which is the gyro rotation from that timestamp) + */ + private Pose2d robotPose2d( + Pose2d historicalPose, + Transform3d cameraInRobot, + Pose3d tagInField, + Transform3d tagInCamera) { + Pose3d robotPose3d = PoseEstimationHelper.robotInField( + cameraInRobot, tagInField, tagInCamera); + Pose2d robotPose2d = new Pose2d( + robotPose3d.getTranslation().toTranslation2d(), + historicalPose.getRotation()); + m_log_pose.log(() -> robotPose2d); + m_pub_pose.set(robotPose2d); + return robotPose2d; + } + + /** + * If the tag is too far, replace the blip-derived tag rotation with a + * gyro-derived tag rotation. + */ + private Transform3d maybeOverrideRotation( + Transform3d cameraOffset, Pose2d historicalPose, Pose3d tagInField, Transform3d tagInCamera) { + if (tagInCamera.getTranslation().getNorm() > TAG_ROTATION_BELIEF_THRESHOLD_M) { + m_log_using_gyro.log(() -> true); + tagInCamera = PoseEstimationHelper.tagInCamera( + cameraOffset, tagInField, tagInCamera, new Rotation3d(historicalPose.getRotation())); + } else { + m_log_using_gyro.log(() -> false); } - return defaultStateStdDevs; + return tagInCamera; + } + + /** Log the norm of the translational error of the tag. */ + private void logTagError(Pose3d tagInField, Pose3d estimatedTagInField) { + Transform3d tagError = tagInField.minus(estimatedTagInField); + m_log_tag_error.log(() -> tagError.getTranslation().getNorm()); } - /** This is an educated guess. */ - static double[] visionMeasurementStdDevs(double distanceM) { - if (Experiments.instance.enabled(Experiment.AvoidVisionJitter)) { - /* - * NEW (3/12/25), 2 cm std dev seems kinda realistic for 1 m. - * - * If it still jitters, try 0.03 or 0.05, but watch out for slow convergence. - */ - final double K = 0.03; - return new double[] { - (K * distanceM) + 0.01, - (K * distanceM) + 0.01, - Double.MAX_VALUE }; + /** + * Returns the pose from the frame timestamp. + * + * Note this pulls from the *old history*, not the *odometry-updated history*, + * because we don't care about the latest odometry update. + * + * Because the camera delay is much more than the odometry delay, we're always + * trying to write history from several cycles ago (followed by replay). It's ok + * for new odometry to be the last thing. + */ + private Pose2d historicalPose(double correctedTimestamp) { + Pose2d historicalPose = m_history.apply(correctedTimestamp).pose(); + Rotation2d gyroRotation = historicalPose.getRotation(); + if (DEBUG) { + System.out.printf("historical gyro rotation %f\n", gyroRotation.getRadians()); } - /* - * Standard deviation of pose estimate, as a fraction of target range. - * This is a guess based on figure 5 in the Apriltag2 paper: - * https://april.eecs.umich.edu/media/media/pdfs/wang2016iros.pdf - */ - final double K = 0.03; - return new double[] { - K * distanceM, - K * distanceM, - Double.MAX_VALUE }; + return historicalPose; + } + + private void printBlip(Blip24 blip) { + if (!DEBUG) + return; + Translation3d t = blip.getRawPose().getTranslation(); + Rotation3d r = blip.getRawPose().getRotation(); + System.out.printf("blip raw pose %d X %5.2f Y %5.2f Z %5.2f R %5.2f P %5.2f Y %5.2f\n", + blip.getId(), t.getX(), t.getY(), t.getZ(), r.getX(), r.getY(), r.getZ()); + } + + /** + * This is used for camera offset calibration. Place a tag at a known position, + * observe the offset, and add it to Camera.java, inverted. + */ + private void printForCalibration(Transform3d cameraOffset, Blip24 blip, Transform3d tagInCamera) { + if (!DEBUG) + return; + Transform3d tagInRobot = cameraOffset.plus(tagInCamera); + System.out.printf("tagInRobot id %d X %5.2f Y %5.2f Z %5.2f R %5.2f P %5.2f Y %5.2f\n", + blip.getId(), tagInRobot.getTranslation().getX(), tagInRobot.getTranslation().getY(), + tagInRobot.getTranslation().getZ(), tagInRobot.getRotation().getX(), + tagInRobot.getRotation().getY(), tagInRobot.getRotation().getZ()); + } + + /** The estimated tag position, for visualization of vision error. */ + private Pose3d estimatedTagInField( + Transform3d cameraOffset, Pose2d historicalPose, Transform3d tagInCamera) { + // Field-to-robot + Pose3d historicalPose3d = new Pose3d(historicalPose); + // Field-to-robot plus robot-to-camera = field-to-camera + Pose3d historicalCameraInField = historicalPose3d.transformBy(cameraOffset); + // Given the historical pose, where do we think the tag is? + return historicalCameraInField.transformBy(tagInCamera); + } + + /** Camera-to-tag, as it appears in the camera frame. */ + private Transform3d tagInCamera(Blip24 blip) { + Transform3d blipTransform = blip.blipToTransform(); + m_log_tag_in_camera.log(() -> blipTransform); + return blipTransform; } } \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/localization/NudgingVisionUpdater.java b/lib/src/main/java/org/team100/lib/localization/NudgingVisionUpdater.java index 57dbb225..474f7229 100644 --- a/lib/src/main/java/org/team100/lib/localization/NudgingVisionUpdater.java +++ b/lib/src/main/java/org/team100/lib/localization/NudgingVisionUpdater.java @@ -82,43 +82,8 @@ static Pose2d nudge( // Step 2: Measure the twist between the odometry pose and the vision pose. Twist2d twist = sample.log(measurement); // Step 4: Discount the twist based on the sigmas relative to each other - Twist2d scaledTwist = getScaledTwist(stateSigma, visionSigma, twist); + Twist2d scaledTwist = Uncertainty.getScaledTwist(stateSigma, visionSigma, twist); return sample.exp(scaledTwist); } - static Twist2d getScaledTwist( - double[] stateSigma, - double[] visionSigma, - Twist2d twist) { - // discount the vision update by this factor. - final double[] K = getK(stateSigma, visionSigma); - Twist2d scaledTwist = new Twist2d( - K[0] * twist.dx, - K[1] * twist.dy, - K[2] * twist.dtheta); - return scaledTwist; - } - - static double[] getK(double[] stateSigma, double[] visionSigma) { - return new double[] { - mix(Math.pow(stateSigma[0], 2), Math.pow(visionSigma[0], 2)), - mix(Math.pow(stateSigma[1], 2), Math.pow(visionSigma[1], 2)), - mix(Math.pow(stateSigma[2], 2), Math.pow(visionSigma[2], 2)) - }; - } - - /** - * Given q and r stddev's, what mixture should that yield? - * This is the "closed form Kalman gain for continuous Kalman filter with A = 0 - * and C = I. See wpimath/algorithms.md." ... but really it's just a mixer. - * - * @param q state variance - * @param r vision variance - */ - static double mix(final double q, final double r) { - if (q == 0.0) - return 0.0; - return q / (q + Math.sqrt(q * r)); - } - } diff --git a/lib/src/main/java/org/team100/lib/localization/Uncertainty.java b/lib/src/main/java/org/team100/lib/localization/Uncertainty.java new file mode 100644 index 00000000..24a9122e --- /dev/null +++ b/lib/src/main/java/org/team100/lib/localization/Uncertainty.java @@ -0,0 +1,95 @@ +package org.team100.lib.localization; + +import org.team100.lib.experiments.Experiment; +import org.team100.lib.experiments.Experiments; + +import edu.wpi.first.math.geometry.Twist2d; + +/** + * Methods governing vision update uncertainties. + */ +public class Uncertainty { + /** this is the default value which, in hindsight, seems ridiculously high. */ + private static final double[] DEFAULT_STATE_STDDEV = new double[] { + 0.1, + 0.1, + 0.1 }; + + /** + * This value is tuned so that errors scale at 0.2x per second. See + * SwerveDrivePoseEstimator100Test::testFirmerNudge. + */ + static final double[] TIGHT_STATE_STDDEV = new double[] { + 0.001, + 0.001, + 0.1 }; + + /** This is an educated guess. */ + static double[] visionMeasurementStdDevs(double distanceM) { + if (Experiments.instance.enabled(Experiment.AvoidVisionJitter)) { + /* + * NEW (3/12/25), 2 cm std dev seems kinda realistic for 1 m. + * + * If it still jitters, try 0.03 or 0.05, but watch out for slow convergence. + */ + final double K = 0.03; + return new double[] { + (K * distanceM) + 0.01, + (K * distanceM) + 0.01, + Double.MAX_VALUE }; + } + /* + * Standard deviation of pose estimate, as a fraction of target range. + * This is a guess based on figure 5 in the Apriltag2 paper: + * https://april.eecs.umich.edu/media/media/pdfs/wang2016iros.pdf + */ + final double K = 0.03; + return new double[] { + K * distanceM, + K * distanceM, + Double.MAX_VALUE }; + } + + static double[] stateStdDevs() { + if (Experiments.instance.enabled(Experiment.AvoidVisionJitter)) { + return Uncertainty.TIGHT_STATE_STDDEV; + } + return Uncertainty.DEFAULT_STATE_STDDEV; + } + + static Twist2d getScaledTwist( + double[] stateSigma, + double[] visionSigma, + Twist2d twist) { + // discount the vision update by this factor. + final double[] K = Uncertainty.getK(stateSigma, visionSigma); + Twist2d scaledTwist = new Twist2d( + K[0] * twist.dx, + K[1] * twist.dy, + K[2] * twist.dtheta); + return scaledTwist; + } + + static double[] getK(double[] stateSigma, double[] visionSigma) { + return new double[] { + Uncertainty.mix(Math.pow(stateSigma[0], 2), Math.pow(visionSigma[0], 2)), + Uncertainty.mix(Math.pow(stateSigma[1], 2), Math.pow(visionSigma[1], 2)), + Uncertainty.mix(Math.pow(stateSigma[2], 2), Math.pow(visionSigma[2], 2)) + }; + } + + /** + * Given q and r stddev's, what mixture should that yield? + * This is the "closed form Kalman gain for continuous Kalman filter with A = 0 + * and C = I. See wpimath/algorithms.md." ... but really it's just a mixer. + * + * @param q state variance + * @param r vision variance + */ + static double mix(final double q, final double r) { + if (q == 0.0) + return 0.0; + return q / (q + Math.sqrt(q * r)); + } + +} diff --git a/lib/src/test/java/org/team100/lib/localization/AprilTagRobotLocalizerTest.java b/lib/src/test/java/org/team100/lib/localization/AprilTagRobotLocalizerTest.java index c9017426..236bb81a 100644 --- a/lib/src/test/java/org/team100/lib/localization/AprilTagRobotLocalizerTest.java +++ b/lib/src/test/java/org/team100/lib/localization/AprilTagRobotLocalizerTest.java @@ -35,27 +35,6 @@ class AprilTagRobotLocalizerTest implements Timeless { private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); private static final LoggerFactory fieldLogger = new TestLoggerFactory(new TestPrimitiveLogger()); - @Test - void testVisionStdDevs() { - double targetRangeM = 1.0; - double[] visionStdDev = AprilTagRobotLocalizer.visionMeasurementStdDevs(targetRangeM); - assertEquals(3, visionStdDev.length); - assertEquals(0.04, visionStdDev[0], DELTA); - assertEquals(0.04, visionStdDev[1], DELTA); - assertEquals(Double.MAX_VALUE, visionStdDev[2], DELTA); - } - - @Test - void testStateStdDevs() { - // these are the "antijitter" values. - // 1 mm, very low - double[] stateStdDev = AprilTagRobotLocalizer.tightStateStdDevs; - assertEquals(3, stateStdDev.length); - assertEquals(0.001, stateStdDev[0], DELTA); - assertEquals(0.001, stateStdDev[1], DELTA); - assertEquals(0.1, stateStdDev[2], DELTA); - } - @Test void testEndToEnd() throws IOException, InterruptedException { AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); @@ -73,7 +52,7 @@ public void put(double t, Pose2d p, double[] sd1, double[] sd2) { }; AprilTagRobotLocalizer localizer = new AprilTagRobotLocalizer( - logger,fieldLogger, layout, history, visionUpdater); + logger, fieldLogger, layout, history, visionUpdater); // client instance NetworkTableInstance inst = NetworkTableInstance.create(); @@ -144,7 +123,7 @@ public void put(double t, Pose2d p, double[] sd1, double[] sd2) { }; AprilTagRobotLocalizer localizer = new AprilTagRobotLocalizer( - logger, fieldLogger,layout, history, visionUpdater); + logger, fieldLogger, layout, history, visionUpdater); // in red layout blip 7 is on the other side of the field @@ -195,7 +174,7 @@ public void put(double t, Pose2d p, double[] sd1, double[] sd2) { }; AprilTagRobotLocalizer localizer = new AprilTagRobotLocalizer( - logger, fieldLogger,layout, history, visionUpdater); + logger, fieldLogger, layout, history, visionUpdater); // camera sees the tag straight ahead in the center of the frame, // but rotated pi/4 to the left. this is ignored anyway. @@ -272,7 +251,7 @@ public void put(double t, Pose2d p, double[] sd1, double[] sd2) { } }; AprilTagRobotLocalizer localizer = new AprilTagRobotLocalizer( - logger, fieldLogger,layout, history, visionUpdater); + logger, fieldLogger, layout, history, visionUpdater); Blip24 tag4 = new Blip24(4, new Transform3d( new Translation3d(0, 0, 2.4), @@ -316,7 +295,7 @@ public void put(double t, Pose2d p, double[] sd1, double[] sd2) { } }; AprilTagRobotLocalizer localizer = new AprilTagRobotLocalizer( - logger, fieldLogger,layout, history, visionUpdater); + logger, fieldLogger, layout, history, visionUpdater); // tag is 1m away on bore final Blip24 tag4 = new Blip24(4, new Transform3d( @@ -355,7 +334,7 @@ public void put(double t, Pose2d p, double[] sd1, double[] sd2) { }; AprilTagRobotLocalizer localizer = new AprilTagRobotLocalizer( - logger, fieldLogger,layout, history, visionUpdater); + logger, fieldLogger, layout, history, visionUpdater); Blip24 tag4 = new Blip24(4, new Transform3d( new Translation3d(0, 0, 1), @@ -397,7 +376,7 @@ public void put(double t, Pose2d p, double[] sd1, double[] sd2) { }; AprilTagRobotLocalizer localizer = new AprilTagRobotLocalizer( - logger, fieldLogger,layout, history, visionUpdater); + logger, fieldLogger, layout, history, visionUpdater); Blip24 tag3 = new Blip24(3, new Transform3d( new Translation3d(0.561, 0, 1), @@ -440,7 +419,7 @@ public void put(double t, Pose2d p, double[] sd1, double[] sd2) { }; AprilTagRobotLocalizer localizer = new AprilTagRobotLocalizer( - logger, fieldLogger,layout, history, visionUpdater); + logger, fieldLogger, layout, history, visionUpdater); Blip24 tag4 = new Blip24(4, new Transform3d( new Translation3d(0, 0, 1.4142), @@ -482,7 +461,7 @@ public void put(double t, Pose2d p, double[] sd1, double[] sd2) { }; AprilTagRobotLocalizer localizer = new AprilTagRobotLocalizer( - logger, fieldLogger,layout, history, visionUpdater); + logger, fieldLogger, layout, history, visionUpdater); Blip24 tag4 = new Blip24(4, new Transform3d( new Translation3d(-1, 0, 1), @@ -522,7 +501,7 @@ public void put(double t, Pose2d p, double[] sd1, double[] sd2) { }; AprilTagRobotLocalizer localizer = new AprilTagRobotLocalizer( - logger, fieldLogger,layout, history, visionUpdater); + logger, fieldLogger, layout, history, visionUpdater); Blip24 tag4 = new Blip24(4, new Transform3d( new Translation3d(0, 0, 1.4142), @@ -561,7 +540,7 @@ public void put(double t, Pose2d p, double[] sd1, double[] sd2) { } }; AprilTagRobotLocalizer localizer = new AprilTagRobotLocalizer( - logger, fieldLogger,layout, history, visionUpdater); + logger, fieldLogger, layout, history, visionUpdater); Blip24 tag4 = new Blip24(4, new Transform3d( new Translation3d(0, 0, 1.4142), @@ -600,7 +579,7 @@ public void put(double t, Pose2d p, double[] sd1, double[] sd2) { } }; AprilTagRobotLocalizer localizer = new AprilTagRobotLocalizer( - logger, fieldLogger,layout, history, visionUpdater); + logger, fieldLogger, layout, history, visionUpdater); Blip24 tag4 = new Blip24(4, new Transform3d( new Translation3d(0, 0, 2), @@ -642,7 +621,7 @@ public void put(double t, Pose2d p, double[] sd1, double[] sd2) { } }; AprilTagRobotLocalizer localizer = new AprilTagRobotLocalizer( - logger, fieldLogger,layout, history, visionUpdater); + logger, fieldLogger, layout, history, visionUpdater); // 30 degrees, long side is sqrt2, so hypotenuse is sqrt2/sqrt3/2 Blip24 tag4 = new Blip24(4, new Transform3d( diff --git a/lib/src/test/java/org/team100/lib/localization/UncertaintyTest.java b/lib/src/test/java/org/team100/lib/localization/UncertaintyTest.java new file mode 100644 index 00000000..32055b5e --- /dev/null +++ b/lib/src/test/java/org/team100/lib/localization/UncertaintyTest.java @@ -0,0 +1,70 @@ +package org.team100.lib.localization; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.math.geometry.Twist2d; + +public class UncertaintyTest { + private static final double DELTA = 0.01; + + @Test + void testVisionStdDevs() { + double targetRangeM = 1.0; + double[] visionStdDev = Uncertainty.visionMeasurementStdDevs(targetRangeM); + assertEquals(3, visionStdDev.length); + assertEquals(0.04, visionStdDev[0], DELTA); + assertEquals(0.04, visionStdDev[1], DELTA); + assertEquals(Double.MAX_VALUE, visionStdDev[2], DELTA); + } + + @Test + void testStateStdDevs() { + // these are the "antijitter" values. + // 1 mm, very low + double[] stateStdDev = Uncertainty.TIGHT_STATE_STDDEV; + assertEquals(3, stateStdDev.length); + assertEquals(0.001, stateStdDev[0], DELTA); + assertEquals(0.001, stateStdDev[1], DELTA); + assertEquals(0.1, stateStdDev[2], DELTA); + } + + @Test + void testScaledTwist() { + // 1 mm + double[] stateStdDev = Uncertainty.TIGHT_STATE_STDDEV; + double targetRangeM = 1.0; + // 2 cm stdev, 20x + double[] visionStdDev = Uncertainty.visionMeasurementStdDevs(targetRangeM); + // 10 cm of difference between the vision update and the current pose + Twist2d twist = new Twist2d(0.1, 0.1, 0); + Twist2d scaled = Uncertainty.getScaledTwist(stateStdDev, visionStdDev, twist); + // difference is discounted 20x + assertEquals(0.002439, scaled.dx, 1e-6); + assertEquals(0.002439, scaled.dy, 1e-6); + assertEquals(0, scaled.dtheta, 1e-6); + } + + @Test + void testK() { + double[] stateStdDev = Uncertainty.TIGHT_STATE_STDDEV; + double targetRangeM = 1.0; + double[] visionStdDev = Uncertainty.visionMeasurementStdDevs(targetRangeM); + double[] k = Uncertainty.getK(stateStdDev, visionStdDev); + assertEquals(3, k.length); + assertEquals(0.024, k[0], DELTA); + assertEquals(0.024, k[1], DELTA); + assertEquals(0, k[2], DELTA); + } + + @Test + void testMix() { + assertEquals(0.091, Uncertainty.mix(1, 100), DELTA); + assertEquals(0.24, Uncertainty.mix(1, 10), DELTA); + assertEquals(0.5, Uncertainty.mix(1, 1), DELTA); + assertEquals(0.76, Uncertainty.mix(10, 1), DELTA); + assertEquals(0.909, Uncertainty.mix(100, 1), DELTA); + } + +} diff --git a/lib/src/test/java/org/team100/lib/localization/VisionUpdaterTest.java b/lib/src/test/java/org/team100/lib/localization/VisionUpdaterTest.java index 4c021cf6..896bfd09 100644 --- a/lib/src/test/java/org/team100/lib/localization/VisionUpdaterTest.java +++ b/lib/src/test/java/org/team100/lib/localization/VisionUpdaterTest.java @@ -7,27 +7,10 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Twist2d; public class VisionUpdaterTest { private static final double DELTA = 0.001; - @Test - void testScaledTwist() { - // 1 mm - double[] stateStdDev = AprilTagRobotLocalizer.tightStateStdDevs; - double targetRangeM = 1.0; - // 2 cm stdev, 20x - double[] visionStdDev = AprilTagRobotLocalizer.visionMeasurementStdDevs(targetRangeM); - // 10 cm of difference between the vision update and the current pose - Twist2d twist = new Twist2d(0.1, 0.1, 0); - Twist2d scaled = NudgingVisionUpdater.getScaledTwist(stateStdDev, visionStdDev, twist); - // difference is discounted 20x - assertEquals(0.002439, scaled.dx, 1e-6); - assertEquals(0.002439, scaled.dy, 1e-6); - assertEquals(0, scaled.dtheta, 1e-6); - } - /** * If the measurement is the same as the sample, nothing happens. */ @@ -125,25 +108,8 @@ void testFirmerNudge() { } - @Test - void testK() { - double[] stateStdDev = AprilTagRobotLocalizer.tightStateStdDevs; - double targetRangeM = 1.0; - double[] visionStdDev = AprilTagRobotLocalizer.visionMeasurementStdDevs(targetRangeM); - double[] k = NudgingVisionUpdater.getK(stateStdDev, visionStdDev); - assertEquals(3, k.length); - assertEquals(0.024, k[0], DELTA); - assertEquals(0.024, k[1], DELTA); - assertEquals(0, k[2], DELTA); - } - @Test - void testMix() { - assertEquals(0.091, NudgingVisionUpdater.mix(1, 100), DELTA); - assertEquals(0.24, NudgingVisionUpdater.mix(1, 10), DELTA); - assertEquals(0.5, NudgingVisionUpdater.mix(1, 1), DELTA); - assertEquals(0.76, NudgingVisionUpdater.mix(10, 1), DELTA); - assertEquals(0.909, NudgingVisionUpdater.mix(100, 1), DELTA); - } + + } From 649b4be90a6d4e93e7bfcccdb110193fc27b433a Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Fri, 26 Dec 2025 21:59:58 -0800 Subject: [PATCH 3/4] actually call SE(2) classes SE(2) --- .../frc2025/CalgamesArm/CalgamesMech.java | 20 +-- .../frc2025/CalgamesArm/MechTrajectories.java | 4 +- .../ScoreSmart/ScoreCoralSmart.java | 8 +- .../ScoreSmart/ScoreCoralSmartLuke.java | 8 +- .../ScoreSmart/ScoreL1Smart.java | 10 +- .../ScoreSmart/ScoreL2Smart.java | 10 +- .../ScoreSmart/ScoreL3Smart.java | 10 +- .../ScoreSmart/ScoreL4Smart.java | 10 +- .../ScoreSmart/ScoreL4SmartBack.java | 10 +- .../frc2025/Swerve/ManualWithBargeAssist.java | 6 +- .../Swerve/ManualWithProfiledReefLock.java | 6 +- .../org/team100/frc2025/robot/AllAutons.java | 14 +- .../java/org/team100/frc2025/robot/Auton.java | 16 +- .../org/team100/frc2025/robot/Binder.java | 24 +-- .../team100/frc2025/robot/DriveAndScore.java | 14 +- .../team100/frc2025/robot/LolipopAuto.java | 18 +- .../CalgamesArm/TrajectoryJointTest.java | 4 +- .../ControllerFactorySE2.java} | 64 +++---- .../ControllerSE2.java} | 14 +- .../ControllerSE2Base.java} | 44 ++--- .../FeedforwardControllerSE2.java} | 10 +- .../FullStateControllerSE2.java} | 12 +- .../NullControllerSE2.java} | 10 +- .../lib/controller/{r3 => se2}/README.md | 0 .../localization/AprilTagRobotLocalizer.java | 6 +- .../lib/localization/FreshSwerveEstimate.java | 6 +- .../lib/localization/InterpolationRecord.java | 8 +- .../team100/lib/localization/ManualPose.java | 14 +- .../localization/NudgingVisionUpdater.java | 4 +- .../lib/localization/OdometryUpdater.java | 6 +- .../localization/SimulatedTagDetector.java | 6 +- .../lib/localization/SwerveHistory.java | 12 +- .../team100/lib/logging/LoggerFactory.java | 36 ++-- .../{r3 => se2}/FreeRotationProfile.java | 16 +- .../profile/{r3 => se2}/HolonomicProfile.java | 16 +- .../{r3 => se2}/HolonomicProfileFactory.java | 2 +- .../ProfileR3.java => se2/ProfileSE2.java} | 12 +- .../lib/reference/r3/ConstantReferenceR3.java | 39 ---- .../reference/se2/ConstantReferenceSE2.java | 39 ++++ .../ProfileReferenceSE2.java} | 58 +++--- .../ReferenceSE2.java} | 18 +- .../TrajectoryReferenceSE2.java} | 44 ++--- .../java/org/team100/lib/servo/Torque.java | 4 - .../state/{ControlR3.java => ControlSE2.java} | 48 ++--- .../lib/state/{ModelR3.java => ModelSE2.java} | 56 +++--- .../main/java/org/team100/lib/state/README.md | 13 +- .../subsystems/mecanum/MecanumDrive100.java | 10 +- .../subsystems/prr/AnalyticalJacobian.java | 8 +- .../team100/lib/subsystems/prr/Jacobian.java | 4 +- .../subsystems/r3/PositionSubsystemR3.java | 15 -- .../subsystems/se2/PositionSubsystemSE2.java | 15 ++ .../lib/subsystems/{r3 => se2}/README.md | 0 .../SubsystemSE2.java} | 9 +- .../VelocitySubsystemSE2.java} | 6 +- .../{r3 => se2}/commands/DriveStop.java | 8 +- .../commands/DriveToPoseWithProfile.java | 36 ++-- .../commands/DriveToStateWithProfile.java | 40 ++--- .../DriveToTranslationFacingWithProfile.java | 36 ++-- ...DriveToTranslationWithRelativeBearing.java | 38 ++-- .../commands/DriveWithTrajectoryFunction.java | 24 +-- .../commands/FloorPickSequence.java | 14 +- .../commands/FloorPickSequence2.java | 14 +- .../commands/FollowTrajectoryPosition.java | 20 +-- .../commands/GoToPosePosition.java | 18 +- .../{r3 => se2}/commands/HoldPosition.java | 14 +- .../{r3 => se2}/commands/ManualPosition.java | 12 +- .../{r3 => se2}/commands/Pushbroom.java | 38 ++-- .../subsystems/{r3 => se2}/commands/README.md | 0 .../commands/VelocityFeedforwardOnly.java | 28 +-- .../PositionReferenceControllerSE2.java} | 28 +-- .../{r3 => se2}/commands/helper/README.md | 0 .../helper/ReferenceControllerSE2Base.java} | 62 +++---- .../VelocityReferenceControllerSE2.java} | 24 +-- .../commands/test/DriveToStateSimple.java | 32 ++-- .../commands/test/DriveWithTrajectory.java | 24 +-- .../test/DriveWithTrajectoryListFunction.java | 24 +-- .../test/PermissiveTrajectoryListCommand.java | 24 +-- .../team100/lib/subsystems/swerve/README.md | 2 +- .../swerve/SwerveDriveSubsystem.java | 24 +-- .../commands/manual/ChassisSpeedAdapter.java | 6 +- .../commands/manual/ChassisSpeedDriver.java | 6 +- .../swerve/commands/manual/DriveManually.java | 8 +- .../commands/manual/DriveManuallySimple.java | 10 +- .../swerve/commands/manual/DriverAdapter.java | 6 +- .../commands/manual/FieldRelativeAdapter.java | 12 +- .../commands/manual/FieldRelativeDriver.java | 6 +- .../commands/manual/ManualChassisSpeeds.java | 6 +- .../manual/ManualFieldRelativeSpeeds.java | 6 +- .../manual/ManualWithFullStateHeading.java | 6 +- .../manual/ManualWithOptionalTargetLock.java | 6 +- .../manual/ManualWithProfiledHeading.java | 6 +- .../commands/manual/ManualWithTargetLock.java | 6 +- .../commands/manual/ModuleStateAdapter.java | 6 +- .../swerve/commands/manual/StopDriver.java | 12 +- .../swerve/commands/test/Rotate.java | 28 +-- .../lib/subsystems/test/OffsetDrivetrain.java | 14 +- .../test/OffsetDrivetrainWithBoost.java | 14 +- .../subsystems/test/TrivialDrivetrain.java | 18 +- .../team100/lib/subsystems/turret/Turret.java | 12 +- .../lib/targeting/SimulatedTargetWriter.java | 6 +- .../org/team100/lib/targeting/TargetUtil.java | 4 +- .../org/team100/lib/targeting/Targets.java | 6 +- .../java/org/team100/lib/trajectory/README.md | 4 +- .../lib/visualization/BallFactory.java | 6 +- .../org/team100/lib/visualization/BallR2.java | 8 +- .../org/team100/lib/visualization/BallR3.java | 8 +- .../FeedforwardControllerSE2Test.java} | 33 ++-- .../FullStateControllerR3Test.java | 168 +++++++++--------- .../ReferenceControllerSE2Test.java} | 45 ++--- ...AprilTagRobotLocalizerPerformanceTest.java | 4 +- .../AprilTagRobotLocalizerTest.java | 28 +-- .../localization/InterpolationRecordTest.java | 10 +- .../SimulatedTagDetectorTest.java | 4 +- .../SwerveDrivePoseEstimator100Test.java | 8 +- .../{r3 => se2}/HolonomicProfileTest.java | 34 ++-- .../ProfileReferenceSE2Test.java} | 34 ++-- .../{r3 => se2}/TrajectoryReferenceTest.java | 26 +-- .../prr/AnalyticalJacobianTest.java | 30 ++-- .../lib/subsystems/prr/JacobianTest.java | 12 +- .../MockSubsystemSE2.java} | 13 +- .../commands/DriveToPoseWithProfileTest.java | 21 +-- .../DriveWithTrajectoryFunctionTest.java | 17 +- .../{r3 => se2}/commands/PushbroomTest.java | 3 +- .../DriveWithTrajectoryListFunctionTest.java | 11 +- .../test/DriveWithTrajectoryTest.java | 25 +-- .../lib/subsystems/swerve/Fixture.java | 8 +- .../subsystems/swerve/RealisticFixture.java | 8 +- .../subsystems/swerve/SwerveControlTest.java | 18 +- .../subsystems/swerve/SwerveModelTest.java | 18 +- .../manual/ManualChassisSpeedsTest.java | 6 +- .../manual/ManualFieldRelativeSpeedsTest.java | 6 +- .../ManualWithFullStateHeadingTest.java | 46 ++--- .../manual/ManualWithProfiledHeadingTest.java | 46 ++--- .../targeting/SimulatedTargetWriterTest.java | 4 +- .../team100/lib/targeting/TargetUtilTest.java | 20 +-- .../team100/lib/targeting/TargetsTest.java | 12 +- .../lib/trajectory/TrajectoryPlannerTest.java | 8 +- .../team100/lib/visualization/BallR2Test.java | 4 +- .../team100/lib/visualization/BallR3Test.java | 4 +- 139 files changed, 1227 insertions(+), 1203 deletions(-) rename lib/src/main/java/org/team100/lib/controller/{r3/ControllerFactoryR3.java => se2/ControllerFactorySE2.java} (52%) rename lib/src/main/java/org/team100/lib/controller/{r3/ControllerR3.java => se2/ControllerSE2.java} (82%) rename lib/src/main/java/org/team100/lib/controller/{r3/ControllerR3Base.java => se2/ControllerSE2Base.java} (80%) rename lib/src/main/java/org/team100/lib/controller/{r3/FeedforwardControllerR3.java => se2/FeedforwardControllerSE2.java} (82%) rename lib/src/main/java/org/team100/lib/controller/{r3/FullStateControllerR3.java => se2/FullStateControllerSE2.java} (91%) rename lib/src/main/java/org/team100/lib/controller/{r3/NullControllerR3.java => se2/NullControllerSE2.java} (76%) rename lib/src/main/java/org/team100/lib/controller/{r3 => se2}/README.md (100%) rename lib/src/main/java/org/team100/lib/profile/{r3 => se2}/FreeRotationProfile.java (88%) rename lib/src/main/java/org/team100/lib/profile/{r3 => se2}/HolonomicProfile.java (90%) rename lib/src/main/java/org/team100/lib/profile/{r3 => se2}/HolonomicProfileFactory.java (99%) rename lib/src/main/java/org/team100/lib/profile/{r3/ProfileR3.java => se2/ProfileSE2.java} (56%) delete mode 100644 lib/src/main/java/org/team100/lib/reference/r3/ConstantReferenceR3.java create mode 100644 lib/src/main/java/org/team100/lib/reference/se2/ConstantReferenceSE2.java rename lib/src/main/java/org/team100/lib/reference/{r3/ProfileReferenceR3.java => se2/ProfileReferenceSE2.java} (70%) rename lib/src/main/java/org/team100/lib/reference/{r3/ReferenceR3.java => se2/ReferenceSE2.java} (66%) rename lib/src/main/java/org/team100/lib/reference/{r3/TrajectoryReferenceR3.java => se2/TrajectoryReferenceSE2.java} (58%) rename lib/src/main/java/org/team100/lib/state/{ControlR3.java => ControlSE2.java} (74%) rename lib/src/main/java/org/team100/lib/state/{ModelR3.java => ModelSE2.java} (68%) delete mode 100644 lib/src/main/java/org/team100/lib/subsystems/r3/PositionSubsystemR3.java create mode 100644 lib/src/main/java/org/team100/lib/subsystems/se2/PositionSubsystemSE2.java rename lib/src/main/java/org/team100/lib/subsystems/{r3 => se2}/README.md (100%) rename lib/src/main/java/org/team100/lib/subsystems/{r3/SubsystemR3.java => se2/SubsystemSE2.java} (52%) rename lib/src/main/java/org/team100/lib/subsystems/{r3/VelocitySubsystemR3.java => se2/VelocitySubsystemSE2.java} (66%) rename lib/src/main/java/org/team100/lib/subsystems/{r3 => se2}/commands/DriveStop.java (55%) rename lib/src/main/java/org/team100/lib/subsystems/{r3 => se2}/commands/DriveToPoseWithProfile.java (63%) rename lib/src/main/java/org/team100/lib/subsystems/{r3 => se2}/commands/DriveToStateWithProfile.java (65%) rename lib/src/main/java/org/team100/lib/subsystems/{r3 => se2}/commands/DriveToTranslationFacingWithProfile.java (68%) rename lib/src/main/java/org/team100/lib/subsystems/{r3 => se2}/commands/DriveToTranslationWithRelativeBearing.java (79%) rename lib/src/main/java/org/team100/lib/subsystems/{r3 => se2}/commands/DriveWithTrajectoryFunction.java (77%) rename lib/src/main/java/org/team100/lib/subsystems/{r3 => se2}/commands/FloorPickSequence.java (85%) rename lib/src/main/java/org/team100/lib/subsystems/{r3 => se2}/commands/FloorPickSequence2.java (78%) rename lib/src/main/java/org/team100/lib/subsystems/{r3 => se2}/commands/FollowTrajectoryPosition.java (61%) rename lib/src/main/java/org/team100/lib/subsystems/{r3 => se2}/commands/GoToPosePosition.java (77%) rename lib/src/main/java/org/team100/lib/subsystems/{r3 => se2}/commands/HoldPosition.java (64%) rename lib/src/main/java/org/team100/lib/subsystems/{r3 => se2}/commands/ManualPosition.java (83%) rename lib/src/main/java/org/team100/lib/subsystems/{r3 => se2}/commands/Pushbroom.java (85%) rename lib/src/main/java/org/team100/lib/subsystems/{r3 => se2}/commands/README.md (100%) rename lib/src/main/java/org/team100/lib/subsystems/{r3 => se2}/commands/VelocityFeedforwardOnly.java (66%) rename lib/src/main/java/org/team100/lib/subsystems/{r3/commands/helper/PositionReferenceControllerR3.java => se2/commands/helper/PositionReferenceControllerSE2.java} (53%) rename lib/src/main/java/org/team100/lib/subsystems/{r3 => se2}/commands/helper/README.md (100%) rename lib/src/main/java/org/team100/lib/subsystems/{r3/commands/helper/ReferenceControllerR3Base.java => se2/commands/helper/ReferenceControllerSE2Base.java} (60%) rename lib/src/main/java/org/team100/lib/subsystems/{r3/commands/helper/VelocityReferenceControllerR3.java => se2/commands/helper/VelocityReferenceControllerSE2.java} (56%) rename lib/src/main/java/org/team100/lib/subsystems/{r3 => se2}/commands/test/DriveToStateSimple.java (59%) rename lib/src/main/java/org/team100/lib/subsystems/{r3 => se2}/commands/test/DriveWithTrajectory.java (68%) rename lib/src/main/java/org/team100/lib/subsystems/{r3 => se2}/commands/test/DriveWithTrajectoryListFunction.java (79%) rename lib/src/main/java/org/team100/lib/subsystems/{r3 => se2}/commands/test/PermissiveTrajectoryListCommand.java (78%) rename lib/src/test/java/org/team100/lib/controller/{r3/FeedforwardControllerR3Test.java => se2/FeedforwardControllerSE2Test.java} (77%) rename lib/src/test/java/org/team100/lib/controller/{r3 => se2}/FullStateControllerR3Test.java (80%) rename lib/src/test/java/org/team100/lib/controller/{r3/ReferenceControllerR3Test.java => se2/ReferenceControllerSE2Test.java} (82%) rename lib/src/test/java/org/team100/lib/profile/{r3 => se2}/HolonomicProfileTest.java (81%) rename lib/src/test/java/org/team100/lib/reference/{r3/ProfileReferenceR3Test.java => se2/ProfileReferenceSE2Test.java} (70%) rename lib/src/test/java/org/team100/lib/reference/{r3 => se2}/TrajectoryReferenceTest.java (85%) rename lib/src/test/java/org/team100/lib/subsystems/{r3/MockSubsystemR3.java => se2/MockSubsystemSE2.java} (56%) rename lib/src/test/java/org/team100/lib/subsystems/{r3 => se2}/commands/DriveToPoseWithProfileTest.java (70%) rename lib/src/test/java/org/team100/lib/subsystems/{r3 => se2}/commands/DriveWithTrajectoryFunctionTest.java (82%) rename lib/src/test/java/org/team100/lib/subsystems/{r3 => se2}/commands/PushbroomTest.java (95%) rename lib/src/test/java/org/team100/lib/subsystems/{r3 => se2}/commands/test/DriveWithTrajectoryListFunctionTest.java (90%) rename lib/src/test/java/org/team100/lib/subsystems/{r3 => se2}/commands/test/DriveWithTrajectoryTest.java (92%) diff --git a/comp/src/main/java/org/team100/frc2025/CalgamesArm/CalgamesMech.java b/comp/src/main/java/org/team100/frc2025/CalgamesArm/CalgamesMech.java index 9ba192f2..d8b930de 100644 --- a/comp/src/main/java/org/team100/frc2025/CalgamesArm/CalgamesMech.java +++ b/comp/src/main/java/org/team100/frc2025/CalgamesArm/CalgamesMech.java @@ -17,13 +17,13 @@ import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.logging.LoggerFactory.ConfigLogger; import org.team100.lib.logging.LoggerFactory.AccelerationSE2Logger; -import org.team100.lib.logging.LoggerFactory.VelocitySE2Logger; +import org.team100.lib.logging.LoggerFactory.ConfigLogger; import org.team100.lib.logging.LoggerFactory.JointAccelerationsLogger; import org.team100.lib.logging.LoggerFactory.JointForceLogger; import org.team100.lib.logging.LoggerFactory.JointVelocitiesLogger; import org.team100.lib.logging.LoggerFactory.Pose2dLogger; +import org.team100.lib.logging.LoggerFactory.VelocitySE2Logger; import org.team100.lib.mechanism.LinearMechanism; import org.team100.lib.mechanism.RotaryMechanism; import org.team100.lib.motor.MotorPhase; @@ -41,8 +41,8 @@ import org.team100.lib.sensor.position.absolute.wpi.AS5048RotaryPositionSensor; import org.team100.lib.sensor.position.incremental.IncrementalBareEncoder; import org.team100.lib.sensor.position.incremental.ctre.Talon6Encoder; -import org.team100.lib.state.ControlR3; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ControlSE2; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.prr.AnalyticalJacobian; import org.team100.lib.subsystems.prr.EAWConfig; import org.team100.lib.subsystems.prr.ElevatorArmWristKinematics; @@ -51,7 +51,7 @@ import org.team100.lib.subsystems.prr.JointVelocities; import org.team100.lib.subsystems.prr.SubsystemPRR; import org.team100.lib.subsystems.prr.commands.FollowJointProfiles; -import org.team100.lib.subsystems.r3.PositionSubsystemR3; +import org.team100.lib.subsystems.se2.PositionSubsystemSE2; import org.team100.lib.util.CanId; import org.team100.lib.util.RoboRioChannel; import org.team100.lib.util.StrUtil; @@ -61,7 +61,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class CalgamesMech extends SubsystemBase implements Music, PositionSubsystemR3, SubsystemPRR { +public class CalgamesMech extends SubsystemBase implements Music, PositionSubsystemSE2, SubsystemPRR { private static final boolean DEBUG = false; private boolean DISABLED = false; //////////////////////////////////////////////////////// @@ -313,19 +313,19 @@ public JointVelocities getJointVelocity() { } @Override - public ModelR3 getState() { + public ModelSE2 getState() { EAWConfig c = getConfig(); JointVelocities jv = getJointVelocity(); Pose2d p = m_kinematics.forward(c); VelocitySE2 v = m_jacobian.forward(c, jv); - return new ModelR3(p, v); + return new ModelSE2(p, v); } // for testing only public void setVelocity(VelocitySE2 v) { Pose2d pose = getState().pose(); AccelerationSE2 a = new AccelerationSE2(0, 0, 0); - ControlR3 control = new ControlR3(pose, v, a); + ControlSE2 control = new ControlSE2(pose, v, a); JointVelocities jv = m_jacobian.inverse(control.model()); JointAccelerations ja = m_jacobian.inverseA(control); @@ -343,7 +343,7 @@ public void setVelocity(VelocitySE2 v) { /** There are no profiles here, so this control needs to be feasible. */ @Override - public void set(ControlR3 control) { + public void set(ControlSE2 control) { Pose2d pose = control.pose(); EAWConfig config = m_kinematics.inverse(pose); if (DEBUG) { diff --git a/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java b/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java index 19eca592..b16238ec 100644 --- a/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java +++ b/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java @@ -8,13 +8,13 @@ import org.team100.lib.logging.LoggerFactory; import org.team100.lib.subsystems.prr.AnalyticalJacobian; import org.team100.lib.subsystems.prr.ElevatorArmWristKinematics; -import org.team100.lib.subsystems.r3.commands.GoToPosePosition; +import org.team100.lib.subsystems.se2.commands.GoToPosePosition; import org.team100.lib.trajectory.TrajectoryPlanner; import org.team100.lib.trajectory.path.PathFactory; import org.team100.lib.trajectory.timing.ConstantConstraint; -import org.team100.lib.trajectory.timing.TrajectoryFactory; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.TorqueConstraint; +import org.team100.lib.trajectory.timing.TrajectoryFactory; import org.team100.lib.trajectory.timing.YawRateConstraint; import edu.wpi.first.wpilibj2.command.Command; diff --git a/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreCoralSmart.java b/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreCoralSmart.java index ad1cd8c6..0fad638d 100644 --- a/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreCoralSmart.java +++ b/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreCoralSmart.java @@ -12,11 +12,11 @@ import org.team100.frc2025.CalgamesArm.CalgamesMech; import org.team100.frc2025.grip.Manipulator; import org.team100.lib.config.ElevatorUtil.ScoringLevel; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.field.FieldConstants; import org.team100.lib.field.FieldConstants.ReefPoint; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.profile.r3.ProfileR3; +import org.team100.lib.profile.se2.ProfileSE2; import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem; import edu.wpi.first.math.geometry.Pose2d; @@ -30,8 +30,8 @@ public static Command get( LoggerFactory logger, CalgamesMech mech, Manipulator manipulator, - ControllerR3 controller, - ProfileR3 profile, + ControllerSE2 controller, + ProfileSE2 profile, SwerveDriveSubsystem drive, DoubleConsumer heedRadiusM, Supplier level, diff --git a/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreCoralSmartLuke.java b/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreCoralSmartLuke.java index 24954e02..2ce65765 100644 --- a/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreCoralSmartLuke.java +++ b/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreCoralSmartLuke.java @@ -9,11 +9,11 @@ import org.team100.frc2025.CalgamesArm.CalgamesMech; import org.team100.frc2025.grip.Manipulator; import org.team100.lib.config.ElevatorUtil.ScoringLevel; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.field.FieldConstants.ReefPoint; import org.team100.lib.field.FieldConstantsLuke; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.profile.r3.ProfileR3; +import org.team100.lib.profile.se2.ProfileSE2; import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem; import edu.wpi.first.math.geometry.Pose2d; @@ -27,8 +27,8 @@ public static Command get( LoggerFactory logger, CalgamesMech mech, Manipulator manipulator, - ControllerR3 controller, - ProfileR3 profile, + ControllerSE2 controller, + ProfileSE2 profile, SwerveDriveSubsystem drive, DoubleConsumer heedRadiusM, Supplier level, diff --git a/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL1Smart.java b/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL1Smart.java index 844abc14..17952631 100644 --- a/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL1Smart.java +++ b/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL1Smart.java @@ -9,10 +9,10 @@ import org.team100.frc2025.CalgamesArm.CalgamesMech; import org.team100.frc2025.grip.Manipulator; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.profile.r3.ProfileR3; -import org.team100.lib.subsystems.r3.commands.DriveToPoseWithProfile; +import org.team100.lib.profile.se2.ProfileSE2; +import org.team100.lib.subsystems.se2.commands.DriveToPoseWithProfile; import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem; import edu.wpi.first.math.geometry.Pose2d; @@ -23,8 +23,8 @@ public static Command get( LoggerFactory logger, CalgamesMech mech, Manipulator manipulator, - ControllerR3 controller, - ProfileR3 profile, + ControllerSE2 controller, + ProfileSE2 profile, SwerveDriveSubsystem drive, Supplier goal) { DriveToPoseWithProfile toReef = new DriveToPoseWithProfile( diff --git a/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL2Smart.java b/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL2Smart.java index 96476b96..3b52be05 100644 --- a/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL2Smart.java +++ b/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL2Smart.java @@ -9,10 +9,10 @@ import org.team100.frc2025.CalgamesArm.CalgamesMech; import org.team100.frc2025.grip.Manipulator; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.profile.r3.ProfileR3; -import org.team100.lib.subsystems.r3.commands.DriveToPoseWithProfile; +import org.team100.lib.profile.se2.ProfileSE2; +import org.team100.lib.subsystems.se2.commands.DriveToPoseWithProfile; import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem; import edu.wpi.first.math.geometry.Pose2d; @@ -23,8 +23,8 @@ public static Command get( LoggerFactory logger, CalgamesMech mech, Manipulator manipulator, - ControllerR3 controller, - ProfileR3 profile, + ControllerSE2 controller, + ProfileSE2 profile, SwerveDriveSubsystem drive, Supplier goal) { DriveToPoseWithProfile toReef = new DriveToPoseWithProfile( diff --git a/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL3Smart.java b/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL3Smart.java index 836d6bd4..568b8b7f 100644 --- a/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL3Smart.java +++ b/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL3Smart.java @@ -9,10 +9,10 @@ import org.team100.frc2025.CalgamesArm.CalgamesMech; import org.team100.frc2025.grip.Manipulator; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.profile.r3.ProfileR3; -import org.team100.lib.subsystems.r3.commands.DriveToPoseWithProfile; +import org.team100.lib.profile.se2.ProfileSE2; +import org.team100.lib.subsystems.se2.commands.DriveToPoseWithProfile; import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem; import edu.wpi.first.math.geometry.Pose2d; @@ -23,8 +23,8 @@ public static Command get( LoggerFactory logger, CalgamesMech mech, Manipulator manipulator, - ControllerR3 controller, - ProfileR3 profile, + ControllerSE2 controller, + ProfileSE2 profile, SwerveDriveSubsystem m_drive, Supplier goal) { DriveToPoseWithProfile toReef = new DriveToPoseWithProfile( diff --git a/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL4Smart.java b/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL4Smart.java index bbcf8955..6b2ae0e0 100644 --- a/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL4Smart.java +++ b/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL4Smart.java @@ -9,10 +9,10 @@ import org.team100.frc2025.CalgamesArm.CalgamesMech; import org.team100.frc2025.grip.Manipulator; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.profile.r3.ProfileR3; -import org.team100.lib.subsystems.r3.commands.DriveToPoseWithProfile; +import org.team100.lib.profile.se2.ProfileSE2; +import org.team100.lib.subsystems.se2.commands.DriveToPoseWithProfile; import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem; import edu.wpi.first.math.geometry.Pose2d; @@ -23,8 +23,8 @@ public static Command get( LoggerFactory logger, CalgamesMech mech, Manipulator manipulator, - ControllerR3 controller, - ProfileR3 profile, + ControllerSE2 controller, + ProfileSE2 profile, SwerveDriveSubsystem m_drive, Supplier goal) { DriveToPoseWithProfile toReef = new DriveToPoseWithProfile( diff --git a/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL4SmartBack.java b/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL4SmartBack.java index d3d2b1a7..a5f787ad 100644 --- a/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL4SmartBack.java +++ b/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL4SmartBack.java @@ -9,10 +9,10 @@ import org.team100.frc2025.CalgamesArm.CalgamesMech; import org.team100.frc2025.grip.Manipulator; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.profile.r3.ProfileR3; -import org.team100.lib.subsystems.r3.commands.DriveToPoseWithProfile; +import org.team100.lib.profile.se2.ProfileSE2; +import org.team100.lib.subsystems.se2.commands.DriveToPoseWithProfile; import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem; import edu.wpi.first.math.geometry.Pose2d; @@ -23,8 +23,8 @@ public static Command get( LoggerFactory logger, CalgamesMech mech, Manipulator manipulator, - ControllerR3 controller, - ProfileR3 profile, + ControllerSE2 controller, + ProfileSE2 profile, SwerveDriveSubsystem m_drive, Supplier goal) { DriveToPoseWithProfile toReef = new DriveToPoseWithProfile( diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithBargeAssist.java b/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithBargeAssist.java index e498ea57..f061ff42 100644 --- a/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithBargeAssist.java +++ b/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithBargeAssist.java @@ -14,7 +14,7 @@ import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile; import org.team100.lib.state.Control100; import org.team100.lib.state.Model100; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.swerve.commands.manual.FieldRelativeDriver; import org.team100.lib.subsystems.swerve.commands.manual.HeadingLatch; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; @@ -90,7 +90,7 @@ public ManualWithBargeAssist( } @Override - public void reset(ModelR3 state) { + public void reset(ModelSE2 state) { m_thetaSetpoint = state.theta().control(); m_goal = null; m_latch.unlatch(); @@ -113,7 +113,7 @@ public void reset(ModelR3 state) { */ @Override public VelocitySE2 apply( - final ModelR3 state, + final ModelSE2 state, final Velocity twist1_1) { final VelocitySE2 control = clipAndScale(twist1_1); diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithProfiledReefLock.java b/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithProfiledReefLock.java index 153bee59..5271345e 100644 --- a/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithProfiledReefLock.java +++ b/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithProfiledReefLock.java @@ -15,7 +15,7 @@ import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile; import org.team100.lib.state.Control100; import org.team100.lib.state.Model100; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.swerve.commands.manual.FieldRelativeDriver; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.util.Math100; @@ -82,7 +82,7 @@ public ManualWithProfiledReefLock( } @Override - public void reset(ModelR3 state) { + public void reset(ModelSE2 state) { m_thetaSetpoint = state.theta().control(); m_thetaFeedback.reset(); } @@ -103,7 +103,7 @@ public void reset(ModelR3 state) { */ @Override public VelocitySE2 apply( - final ModelR3 state, + final ModelSE2 state, final Velocity twist1_1) { final VelocitySE2 control = clipAndScale(twist1_1); diff --git a/comp/src/main/java/org/team100/frc2025/robot/AllAutons.java b/comp/src/main/java/org/team100/frc2025/robot/AllAutons.java index 23655df0..7f410092 100644 --- a/comp/src/main/java/org/team100/frc2025/robot/AllAutons.java +++ b/comp/src/main/java/org/team100/frc2025/robot/AllAutons.java @@ -5,18 +5,18 @@ import org.team100.lib.config.AnnotatedCommand; import org.team100.lib.config.AutonChooser; import org.team100.lib.config.ElevatorUtil.ScoringLevel; -import org.team100.lib.controller.r3.ControllerFactoryR3; -import org.team100.lib.controller.r3.FullStateControllerR3; +import org.team100.lib.controller.se2.ControllerFactorySE2; +import org.team100.lib.controller.se2.FullStateControllerSE2; import org.team100.lib.field.FieldConstants.ReefPoint; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.Logging; -import org.team100.lib.profile.r3.HolonomicProfileFactory; -import org.team100.lib.profile.r3.ProfileR3; +import org.team100.lib.profile.se2.HolonomicProfileFactory; +import org.team100.lib.profile.se2.ProfileSE2; import org.team100.lib.trajectory.TrajectoryPlanner; import org.team100.lib.trajectory.path.PathFactory; -import org.team100.lib.trajectory.timing.TrajectoryFactory; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.TimingConstraintFactory; +import org.team100.lib.trajectory.timing.TrajectoryFactory; import edu.wpi.first.wpilibj2.command.Command; @@ -33,11 +33,11 @@ public AllAutons(Machinery machinery) { m_autonChooser = new AutonChooser(); LoggerFactory autoLog = Logging.instance().rootLogger.name("Auton"); - final ProfileR3 profile = HolonomicProfileFactory.currentLimitedExponential(1, 2, 4, + final ProfileSE2 profile = HolonomicProfileFactory.currentLimitedExponential(1, 2, 4, machinery.m_swerveKinodynamics.getMaxAngleSpeedRad_S(), machinery.m_swerveKinodynamics.getMaxAngleAccelRad_S2(), 5); - final FullStateControllerR3 controller = ControllerFactoryR3 + final FullStateControllerSE2 controller = ControllerFactorySE2 .auto2025LooseTolerance(autoLog); List constraints = new TimingConstraintFactory(machinery.m_swerveKinodynamics) .medium(autoLog); diff --git a/comp/src/main/java/org/team100/frc2025/robot/Auton.java b/comp/src/main/java/org/team100/frc2025/robot/Auton.java index ccdbdaac..dbe2d132 100644 --- a/comp/src/main/java/org/team100/frc2025/robot/Auton.java +++ b/comp/src/main/java/org/team100/frc2025/robot/Auton.java @@ -17,14 +17,14 @@ import org.team100.frc2025.Swerve.Auto.GoToCoralStation; import org.team100.lib.commands.MoveAndHold; import org.team100.lib.config.ElevatorUtil.ScoringLevel; -import org.team100.lib.controller.r3.FullStateControllerR3; +import org.team100.lib.controller.se2.FullStateControllerSE2; import org.team100.lib.field.FieldConstants; import org.team100.lib.field.FieldConstants.CoralStation; import org.team100.lib.field.FieldConstants.ReefPoint; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.profile.r3.ProfileR3; -import org.team100.lib.subsystems.r3.commands.DriveToPoseWithProfile; -import org.team100.lib.subsystems.r3.commands.DriveWithTrajectoryFunction; +import org.team100.lib.profile.se2.ProfileSE2; +import org.team100.lib.subsystems.se2.commands.DriveToPoseWithProfile; +import org.team100.lib.subsystems.se2.commands.DriveWithTrajectoryFunction; import edu.wpi.first.wpilibj2.command.Command; @@ -33,14 +33,14 @@ public class Auton { private final LoggerFactory m_log; private final Machinery m_machinery; - private final ProfileR3 m_autoProfile; - private final FullStateControllerR3 m_autoController; + private final ProfileSE2 m_autoProfile; + private final FullStateControllerSE2 m_autoController; public Auton( LoggerFactory parent, Machinery machinery, - ProfileR3 autoProfile, - FullStateControllerR3 autoController) { + ProfileSE2 autoProfile, + FullStateControllerSE2 autoController) { m_log = parent.type(this); m_machinery = machinery; m_autoProfile = autoProfile; diff --git a/comp/src/main/java/org/team100/frc2025/robot/Binder.java b/comp/src/main/java/org/team100/frc2025/robot/Binder.java index 9c908d34..c08d6d97 100644 --- a/comp/src/main/java/org/team100/frc2025/robot/Binder.java +++ b/comp/src/main/java/org/team100/frc2025/robot/Binder.java @@ -12,19 +12,19 @@ import org.team100.frc2025.Swerve.Auto.BigLoop; import org.team100.lib.controller.r1.Feedback100; import org.team100.lib.controller.r1.PIDFeedback; -import org.team100.lib.controller.r3.ControllerFactoryR3; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerFactorySE2; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.hid.Buttons2025; import org.team100.lib.hid.DriverXboxControl; import org.team100.lib.hid.OperatorXboxControl; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.Logging; -import org.team100.lib.profile.r3.HolonomicProfileFactory; -import org.team100.lib.profile.r3.ProfileR3; +import org.team100.lib.profile.se2.HolonomicProfileFactory; +import org.team100.lib.profile.se2.ProfileSE2; import org.team100.lib.subsystems.prr.commands.FollowJointProfiles; -import org.team100.lib.subsystems.r3.commands.DriveWithTrajectoryFunction; -import org.team100.lib.subsystems.r3.commands.FloorPickSequence2; -import org.team100.lib.subsystems.r3.commands.ManualPosition; +import org.team100.lib.subsystems.se2.commands.DriveWithTrajectoryFunction; +import org.team100.lib.subsystems.se2.commands.FloorPickSequence2; +import org.team100.lib.subsystems.se2.commands.ManualPosition; import org.team100.lib.subsystems.swerve.commands.SetRotation; import org.team100.lib.subsystems.swerve.commands.manual.DriveManuallySimple; import org.team100.lib.subsystems.swerve.kinodynamics.limiter.SwerveLimiter; @@ -137,7 +137,7 @@ public void bind() { .onFalse(m_machinery.m_mech.profileHomeTerminal()); // go full speed for rotation, so that rotation gets there first. - final ProfileR3 coralPickProfile = HolonomicProfileFactory.freeRotationCurrentLimitedExponential( + final ProfileSE2 coralPickProfile = HolonomicProfileFactory.freeRotationCurrentLimitedExponential( m_machinery.m_swerveKinodynamics, 0.5, 1.0); // Pick a game piece from the floor, based on camera input. @@ -149,7 +149,7 @@ public void bind() { // m_machinery.m_manipulator.centerIntake(), // FloorPickSequence.get( // log, fieldLogger, m_machinery.m_drive, m_machinery.m_targets, - // ControllerFactoryR3.pick(log), coralPickProfile) + // ControllerFactorySE2.pick(log), coralPickProfile) // .withName("Floor Pick")) // .until(m_machinery.m_manipulator::hasCoral)); @@ -160,7 +160,7 @@ public void bind() { fieldLogger, m_machinery.m_drive, m_machinery.m_targets, - ControllerFactoryR3.pick(log), + ControllerFactorySE2.pick(log), coralPickProfile) .withName("FloorPick")); @@ -183,9 +183,9 @@ public void bind() { // whileTrue(driverControl::test, m_mech.homeToL4()).onFalse(m_mech.l4ToHome()); final LoggerFactory coralSequence = rootLogger.name("Coral Sequence"); - final ProfileR3 profile = HolonomicProfileFactory.get( + final ProfileSE2 profile = HolonomicProfileFactory.get( coralSequence, m_machinery.m_swerveKinodynamics, 1, 0.5, 1, 0.2); - final ControllerR3 holonomicController = ControllerFactoryR3.byIdentity(coralSequence); + final ControllerSE2 holonomicController = ControllerFactorySE2.byIdentity(coralSequence); // Drive to a scoring location at the reef and score. whileTrue(driver::b, m_machinery.m_manipulator.centerEject()); diff --git a/comp/src/main/java/org/team100/frc2025/robot/DriveAndScore.java b/comp/src/main/java/org/team100/frc2025/robot/DriveAndScore.java index 27a210c8..babe315c 100644 --- a/comp/src/main/java/org/team100/frc2025/robot/DriveAndScore.java +++ b/comp/src/main/java/org/team100/frc2025/robot/DriveAndScore.java @@ -7,12 +7,12 @@ import org.team100.lib.commands.MoveAndHold; import org.team100.lib.config.ElevatorUtil.ScoringLevel; -import org.team100.lib.controller.r3.FullStateControllerR3; +import org.team100.lib.controller.se2.FullStateControllerSE2; import org.team100.lib.field.FieldConstants; import org.team100.lib.field.FieldConstants.ReefPoint; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.profile.r3.ProfileR3; -import org.team100.lib.subsystems.r3.commands.DriveToPoseWithProfile; +import org.team100.lib.profile.se2.ProfileSE2; +import org.team100.lib.subsystems.se2.commands.DriveToPoseWithProfile; import edu.wpi.first.wpilibj2.command.Command; @@ -26,14 +26,14 @@ public class DriveAndScore { private final LoggerFactory m_logger; private final Machinery m_machinery; - private final ProfileR3 m_autoProfile; - private final FullStateControllerR3 m_autoController; + private final ProfileSE2 m_autoProfile; + private final FullStateControllerSE2 m_autoController; public DriveAndScore( LoggerFactory logger, Machinery machinery, - ProfileR3 autoProfile, - FullStateControllerR3 autoController) { + ProfileSE2 autoProfile, + FullStateControllerSE2 autoController) { m_logger = logger; m_machinery = machinery; m_autoProfile = autoProfile; diff --git a/comp/src/main/java/org/team100/frc2025/robot/LolipopAuto.java b/comp/src/main/java/org/team100/frc2025/robot/LolipopAuto.java index df838b5d..916d95bd 100644 --- a/comp/src/main/java/org/team100/frc2025/robot/LolipopAuto.java +++ b/comp/src/main/java/org/team100/frc2025/robot/LolipopAuto.java @@ -9,15 +9,15 @@ import org.team100.lib.commands.MoveAndHold; import org.team100.lib.config.ElevatorUtil.ScoringLevel; -import org.team100.lib.controller.r3.FullStateControllerR3; +import org.team100.lib.controller.se2.FullStateControllerSE2; import org.team100.lib.field.FieldConstants; import org.team100.lib.field.FieldConstants.ReefPoint; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.profile.r3.ProfileR3; -import org.team100.lib.subsystems.r3.commands.DriveToPoseWithProfile; -import org.team100.lib.subsystems.r3.commands.DriveToTranslationFacingWithProfile; -import org.team100.lib.subsystems.r3.commands.DriveWithTrajectoryFunction; +import org.team100.lib.profile.se2.ProfileSE2; +import org.team100.lib.subsystems.se2.commands.DriveToPoseWithProfile; +import org.team100.lib.subsystems.se2.commands.DriveToTranslationFacingWithProfile; +import org.team100.lib.subsystems.se2.commands.DriveWithTrajectoryFunction; import org.team100.lib.trajectory.TrajectoryPlanner; import edu.wpi.first.math.geometry.Pose2d; @@ -30,15 +30,15 @@ public class LolipopAuto { private static final double HEED_RADIUS_M = 3; private final LoggerFactory m_log; private final Machinery m_machinery; - private final ProfileR3 m_autoProfile; - private final FullStateControllerR3 m_autoController; + private final ProfileSE2 m_autoProfile; + private final FullStateControllerSE2 m_autoController; private final TrajectoryPlanner m_planner; public LolipopAuto( LoggerFactory parent, Machinery machinery, - ProfileR3 autoProfile, - FullStateControllerR3 autoController, + ProfileSE2 autoProfile, + FullStateControllerSE2 autoController, TrajectoryPlanner planner) { m_log = parent.type(this); m_machinery = machinery; diff --git a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java index 74c40c8e..ae6c92a0 100644 --- a/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java +++ b/comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java @@ -9,7 +9,7 @@ import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; -import org.team100.lib.state.ControlR3; +import org.team100.lib.state.ControlSE2; import org.team100.lib.subsystems.prr.AnalyticalJacobian; import org.team100.lib.subsystems.prr.EAWConfig; import org.team100.lib.subsystems.prr.ElevatorArmWristKinematics; @@ -67,7 +67,7 @@ void homeToL4() { .println( "t, x, y, r, vx, vy, vr, ax, ay, ar, q1, q2, q3, q1dot, q2dot, q3dot, q1ddot, q2ddot, q3ddot"); for (double tt = 0; tt < t.duration(); tt += 0.02) { - ControlR3 m = ControlR3.fromTimedState(t.sample(tt)); + ControlSE2 m = ControlSE2.fromTimedState(t.sample(tt)); Pose2d p = m.pose(); VelocitySE2 v = m.velocity(); AccelerationSE2 a = m.acceleration(); diff --git a/lib/src/main/java/org/team100/lib/controller/r3/ControllerFactoryR3.java b/lib/src/main/java/org/team100/lib/controller/se2/ControllerFactorySE2.java similarity index 52% rename from lib/src/main/java/org/team100/lib/controller/r3/ControllerFactoryR3.java rename to lib/src/main/java/org/team100/lib/controller/se2/ControllerFactorySE2.java index a336c459..933e919b 100644 --- a/lib/src/main/java/org/team100/lib/controller/r3/ControllerFactoryR3.java +++ b/lib/src/main/java/org/team100/lib/controller/se2/ControllerFactorySE2.java @@ -1,4 +1,4 @@ -package org.team100.lib.controller.r3; +package org.team100.lib.controller.se2; import org.team100.lib.config.Identity; import org.team100.lib.logging.LoggerFactory; @@ -6,19 +6,19 @@ /** * Known-good controller settings. */ -public class ControllerFactoryR3 { +public class ControllerFactorySE2 { /** For real robots. */ - public static ControllerR3 byIdentity(LoggerFactory log) { + public static ControllerSE2 byIdentity(LoggerFactory log) { switch (Identity.instance) { case COMP_BOT -> { - return new FullStateControllerR3(log, 2.9, 3.5, 0.025, 0.01, 0.02, 0.3, 1, 1); + return new FullStateControllerSE2(log, 2.9, 3.5, 0.025, 0.01, 0.02, 0.3, 1, 1); } case SWERVE_ONE -> { - return new FullStateControllerR3(log, 3, 3.5, 0.05, 0, 0.01, 0.01, 1, 1); + return new FullStateControllerSE2(log, 3, 3.5, 0.05, 0, 0.01, 0.01, 1, 1); } case SWERVE_TWO -> { - return new FullStateControllerR3(log, + return new FullStateControllerSE2(log, 4, // P for x/y 4, // P for theta 0.25, // P for v @@ -29,7 +29,7 @@ public static ControllerR3 byIdentity(LoggerFactory log) { 0.02); // omega tolerance } case ROOKIE_BOT -> { - return new FullStateControllerR3(log, + return new FullStateControllerSE2(log, 3, // P for x/y 3.5, // P for theta 0.05, // P for v @@ -39,10 +39,10 @@ public static ControllerR3 byIdentity(LoggerFactory log) { 1, // v tolerance 1); // omega tolerance // I think this is what @anay was doing on 7/8/25. - // return new FullStateControllerR3(log, 3, 0.5, 0.05, 0, 0.01, 0.01, 1, 1); + // return new FullStateControllerSE2(log, 3, 0.5, 0.05, 0, 0.01, 0.01, 1, 1); } case BETA_BOT -> { - return new FullStateControllerR3(log, + return new FullStateControllerSE2(log, 3, // for x/y 3.5, // P for theta 0.05, // P for v @@ -52,33 +52,33 @@ public static ControllerR3 byIdentity(LoggerFactory log) { 1, // v tolerance 1); // omega tolerance // I think this is what @anay was doing on 7/8/25. - // return new FullStateControllerR3(log, 3, 0.5, 0.05, 0, 0.01, 0.01, 1, 1); + // return new FullStateControllerSE2(log, 3, 0.5, 0.05, 0, 0.01, 0.01, 1, 1); } default -> { // this is for simulation, don't use these values - return new FullStateControllerR3(log, 3.0, 3.5, 0.05, 0, 0.017, 0.017, 0.01, 0.01); + return new FullStateControllerSE2(log, 3.0, 3.5, 0.05, 0, 0.017, 0.017, 0.01, 0.01); } } } - public static FullStateControllerR3 ridiculous(LoggerFactory log) { - return new FullStateControllerR3(log, 3, 3, 0.1, 0.1, 0.01, 0.01, 0.01, 0.01); + public static FullStateControllerSE2 ridiculous(LoggerFactory log) { + return new FullStateControllerSE2(log, 3, 3, 0.1, 0.1, 0.01, 0.01, 0.01, 0.01); } - public static FullStateControllerR3 fieldRelativeFancyPIDF(LoggerFactory log) { - return new FullStateControllerR3(log, 2.4, 1.3, 0.1, 0.1, 0.01, 0.02, 0.01, 0.02); + public static FullStateControllerSE2 fieldRelativeFancyPIDF(LoggerFactory log) { + return new FullStateControllerSE2(log, 2.4, 1.3, 0.1, 0.1, 0.01, 0.02, 0.01, 0.02); } - public static FullStateControllerR3 fieldRelativeGoodPIDF(LoggerFactory log) { - return new FullStateControllerR3(log, 1, 1.3, 0.1, 0.1, 0.01, 0.02, 0.01, 0.02); + public static FullStateControllerSE2 fieldRelativeGoodPIDF(LoggerFactory log) { + return new FullStateControllerSE2(log, 1, 1.3, 0.1, 0.1, 0.01, 0.02, 0.01, 0.02); } - public static FullStateControllerR3 autoFieldRelativePIDF(LoggerFactory log) { - return new FullStateControllerR3(log, 1.5, 1.3, 0, 0, 0.1, 0.1, 0.1, 0.1); + public static FullStateControllerSE2 autoFieldRelativePIDF(LoggerFactory log) { + return new FullStateControllerSE2(log, 1.5, 1.3, 0, 0, 0.1, 0.1, 0.1, 0.1); } - public static FullStateControllerR3 auto2025LooseTolerance(LoggerFactory log) { - return new FullStateControllerR3(log, + public static FullStateControllerSE2 auto2025LooseTolerance(LoggerFactory log) { + return new FullStateControllerSE2(log, 7.2, // p cartesian 3.5, // p theta 0.055, // p cartesian v @@ -89,8 +89,8 @@ public static FullStateControllerR3 auto2025LooseTolerance(LoggerFactory log) { 1); // omega tol } - public static FullStateControllerR3 pick(LoggerFactory log) { - return new FullStateControllerR3(log, + public static FullStateControllerSE2 pick(LoggerFactory log) { + return new FullStateControllerSE2(log, 7.2, // p cartesian 3.5, // p theta 0.055, // p cartesian v @@ -106,24 +106,24 @@ public static FullStateControllerR3 pick(LoggerFactory log) { // don't use these for real robots // - public static FullStateControllerR3 testFieldRelativePIDF(LoggerFactory log) { - return new FullStateControllerR3(log, 2.4, 2.4, 0.1, 0.1, 0.01, 0.02, 0.01, 0.02); + public static FullStateControllerSE2 testFieldRelativePIDF(LoggerFactory log) { + return new FullStateControllerSE2(log, 2.4, 2.4, 0.1, 0.1, 0.01, 0.02, 0.01, 0.02); } - public static FullStateControllerR3 testFieldRelativeFFOnly(LoggerFactory log) { - return new FullStateControllerR3(log, 0, 0, 0, 0, 0.01, 0.02, 0.01, 0.02); + public static FullStateControllerSE2 testFieldRelativeFFOnly(LoggerFactory log) { + return new FullStateControllerSE2(log, 0, 0, 0, 0, 0.01, 0.02, 0.01, 0.02); } - public static FullStateControllerR3 test(LoggerFactory log) { - return new FullStateControllerR3(log, 3.0, 3.5, 0, 0, 0.01, 0.01, 0.01, 0.01); + public static FullStateControllerSE2 test(LoggerFactory log) { + return new FullStateControllerSE2(log, 3.0, 3.5, 0, 0, 0.01, 0.01, 0.01, 0.01); } /** high gains used in tests. */ - public static FullStateControllerR3 test2(LoggerFactory log) { - return new FullStateControllerR3(log, 4, 4, 0.25, 0.25, 0.01, 0.02, 0.01, 0.02); + public static FullStateControllerSE2 test2(LoggerFactory log) { + return new FullStateControllerSE2(log, 4, 4, 0.25, 0.25, 0.01, 0.02, 0.01, 0.02); } - private ControllerFactoryR3() { + private ControllerFactorySE2() { // don't call this } diff --git a/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3.java b/lib/src/main/java/org/team100/lib/controller/se2/ControllerSE2.java similarity index 82% rename from lib/src/main/java/org/team100/lib/controller/r3/ControllerR3.java rename to lib/src/main/java/org/team100/lib/controller/se2/ControllerSE2.java index cadc6d60..89f88abf 100644 --- a/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3.java +++ b/lib/src/main/java/org/team100/lib/controller/se2/ControllerSE2.java @@ -1,13 +1,13 @@ -package org.team100.lib.controller.r3; +package org.team100.lib.controller.se2; import org.team100.lib.geometry.VelocitySE2; -import org.team100.lib.state.ControlR3; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ControlSE2; +import org.team100.lib.state.ModelSE2; /** * Feedback and feedforward control. */ -public interface ControllerR3 { +public interface ControllerSE2 { /** * Feedback should compare the current-instant measurement to the @@ -28,9 +28,9 @@ public interface ControllerR3 { * similar. */ VelocitySE2 calculate( - ModelR3 measurement, - ModelR3 currentReference, - ControlR3 nextReference); + ModelSE2 measurement, + ModelSE2 currentReference, + ControlSE2 nextReference); /** * True if the error is within tolerance of the reference. The definitions of diff --git a/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3Base.java b/lib/src/main/java/org/team100/lib/controller/se2/ControllerSE2Base.java similarity index 80% rename from lib/src/main/java/org/team100/lib/controller/r3/ControllerR3Base.java rename to lib/src/main/java/org/team100/lib/controller/se2/ControllerSE2Base.java index 76859f37..17c0145c 100644 --- a/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3Base.java +++ b/lib/src/main/java/org/team100/lib/controller/se2/ControllerSE2Base.java @@ -1,32 +1,32 @@ -package org.team100.lib.controller.r3; +package org.team100.lib.controller.se2; import org.team100.lib.geometry.DeltaSE2; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.BooleanLogger; -import org.team100.lib.logging.LoggerFactory.ControlR3Logger; +import org.team100.lib.logging.LoggerFactory.ControlSE2Logger; import org.team100.lib.logging.LoggerFactory.DoubleLogger; -import org.team100.lib.logging.LoggerFactory.GlobaDeltaR3Logger; -import org.team100.lib.logging.LoggerFactory.ModelR3Logger; +import org.team100.lib.logging.LoggerFactory.GlobaDeltaSE2Logger; +import org.team100.lib.logging.LoggerFactory.ModelSE2Logger; import org.team100.lib.logging.LoggerFactory.VelocitySE2Logger; -import org.team100.lib.state.ControlR3; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ControlSE2; +import org.team100.lib.state.ModelSE2; import edu.wpi.first.math.geometry.Rotation2d; /** - * Base class for R3 controllers. + * Base class for SE(2) controllers. * * Implements error calculation and tolerances. */ -public abstract class ControllerR3Base implements ControllerR3 { +public abstract class ControllerSE2Base implements ControllerSE2 { - private final ModelR3Logger m_log_measurement; - private final ModelR3Logger m_log_currentReference; - private final ControlR3Logger m_log_nextReference; + private final ModelSE2Logger m_log_measurement; + private final ModelSE2Logger m_log_currentReference; + private final ControlSE2Logger m_log_nextReference; - private final GlobaDeltaR3Logger m_log_position_error; + private final GlobaDeltaSE2Logger m_log_position_error; private final VelocitySE2Logger m_log_velocity_error; /** Error in cartesian distance, i.e. hypot(x, y). */ private final DoubleLogger m_log_cartesianPositionError; @@ -47,7 +47,7 @@ public abstract class ControllerR3Base implements ControllerR3 { /** The velocity error calculated in the most-recent call to calculate. */ private VelocitySE2 m_velocityError; - public ControllerR3Base( + public ControllerSE2Base( LoggerFactory parent, double xTolerance, double thetaTolerance, @@ -55,9 +55,9 @@ public ControllerR3Base( double omegaTolerance) { LoggerFactory log = parent.type(this); - m_log_measurement = log.modelR3Logger(Level.DEBUG, "measurement"); - m_log_currentReference = log.modelR3Logger(Level.DEBUG, "current reference"); - m_log_nextReference = log.controlR3Logger(Level.DEBUG, "next reference"); + m_log_measurement = log.modelSE2Logger(Level.DEBUG, "measurement"); + m_log_currentReference = log.modelSE2Logger(Level.DEBUG, "current reference"); + m_log_nextReference = log.controlSE2Logger(Level.DEBUG, "next reference"); m_log_position_error = log.DeltaSE2Logger(Level.TRACE, "position error"); m_log_velocity_error = log.VelocitySE2Logger(Level.TRACE, "velocity error"); @@ -76,9 +76,9 @@ public ControllerR3Base( @Override public VelocitySE2 calculate( - ModelR3 measurement, - ModelR3 currentReference, - ControlR3 nextReference) { + ModelSE2 measurement, + ModelSE2 currentReference, + ControlSE2 nextReference) { m_log_measurement.log(() -> measurement); m_log_currentReference.log(() -> currentReference); m_log_nextReference.log(() -> nextReference); @@ -96,7 +96,7 @@ public VelocitySE2 calculate( public abstract VelocitySE2 calculate100( DeltaSE2 positionError, VelocitySE2 velocityError, - ControlR3 nextReference); + ControlSE2 nextReference); /** * Uses the position and velocity errors from the previous call to calculate(). @@ -133,7 +133,7 @@ boolean velocityOK(VelocitySE2 velocityError) { /** * Wraps heading. */ - DeltaSE2 positionError(ModelR3 measurement, ModelR3 currentReference) { + DeltaSE2 positionError(ModelSE2 measurement, ModelSE2 currentReference) { DeltaSE2 err = DeltaSE2.delta(measurement.pose(), currentReference.pose()); m_log_position_error.log(() -> err); return err; @@ -142,7 +142,7 @@ DeltaSE2 positionError(ModelR3 measurement, ModelR3 currentReference) { /** * Velocity does not wrap. */ - VelocitySE2 velocityError(ModelR3 measurement, ModelR3 currentReference) { + VelocitySE2 velocityError(ModelSE2 measurement, ModelSE2 currentReference) { VelocitySE2 err = currentReference.velocity().minus(measurement.velocity()); m_log_velocity_error.log(() -> err); return err; diff --git a/lib/src/main/java/org/team100/lib/controller/r3/FeedforwardControllerR3.java b/lib/src/main/java/org/team100/lib/controller/se2/FeedforwardControllerSE2.java similarity index 82% rename from lib/src/main/java/org/team100/lib/controller/r3/FeedforwardControllerR3.java rename to lib/src/main/java/org/team100/lib/controller/se2/FeedforwardControllerSE2.java index 10afa4b6..5b7eed9b 100644 --- a/lib/src/main/java/org/team100/lib/controller/r3/FeedforwardControllerR3.java +++ b/lib/src/main/java/org/team100/lib/controller/se2/FeedforwardControllerSE2.java @@ -1,20 +1,20 @@ -package org.team100.lib.controller.r3; +package org.team100.lib.controller.se2; import org.team100.lib.geometry.DeltaSE2; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.VelocitySE2Logger; -import org.team100.lib.state.ControlR3; +import org.team100.lib.state.ControlSE2; /** * A controller that doesn't do anything except return the "next" setpoint. This * is appropriate if feedback control is outboard. */ -public class FeedforwardControllerR3 extends ControllerR3Base { +public class FeedforwardControllerSE2 extends ControllerSE2Base { private final VelocitySE2Logger m_log_u_FF; - public FeedforwardControllerR3( + public FeedforwardControllerSE2( LoggerFactory parent, double xTolerance, double thetaTolerance, @@ -30,7 +30,7 @@ public FeedforwardControllerR3( public VelocitySE2 calculate100( DeltaSE2 positionError, VelocitySE2 velocityError, - ControlR3 nextReference) { + ControlSE2 nextReference) { m_log_u_FF.log(() -> nextReference.velocity()); return nextReference.velocity(); } diff --git a/lib/src/main/java/org/team100/lib/controller/r3/FullStateControllerR3.java b/lib/src/main/java/org/team100/lib/controller/se2/FullStateControllerSE2.java similarity index 91% rename from lib/src/main/java/org/team100/lib/controller/r3/FullStateControllerR3.java rename to lib/src/main/java/org/team100/lib/controller/se2/FullStateControllerSE2.java index d18df6b9..67f0ac66 100644 --- a/lib/src/main/java/org/team100/lib/controller/r3/FullStateControllerR3.java +++ b/lib/src/main/java/org/team100/lib/controller/se2/FullStateControllerSE2.java @@ -1,16 +1,16 @@ -package org.team100.lib.controller.r3; +package org.team100.lib.controller.se2; import org.team100.lib.geometry.DeltaSE2; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.VelocitySE2Logger; -import org.team100.lib.state.ControlR3; +import org.team100.lib.state.ControlSE2; /** * Velocity feedforward and proportional feedback on position and velocity. */ -public class FullStateControllerR3 extends ControllerR3Base { +public class FullStateControllerSE2 extends ControllerSE2Base { private final VelocitySE2Logger m_log_u_FF; private final VelocitySE2Logger m_log_u_FB; @@ -21,7 +21,7 @@ public class FullStateControllerR3 extends ControllerR3Base { private final double m_kPCartV; private final double m_kPThetaV; - public FullStateControllerR3( + public FullStateControllerSE2( LoggerFactory parent, double kPCart, double kPTheta, @@ -48,7 +48,7 @@ public FullStateControllerR3( public VelocitySE2 calculate100( DeltaSE2 positionError, VelocitySE2 velocityError, - ControlR3 nextReference) { + ControlSE2 nextReference) { VelocitySE2 u_FF = feedforward(nextReference); VelocitySE2 u_FB = fullFeedback(positionError, velocityError); return u_FF.plus(u_FB); @@ -58,7 +58,7 @@ public VelocitySE2 calculate100( // // package-private for testing - VelocitySE2 feedforward(ControlR3 nextReference) { + VelocitySE2 feedforward(ControlSE2 nextReference) { m_log_u_FF.log(() -> nextReference.velocity()); return nextReference.velocity(); } diff --git a/lib/src/main/java/org/team100/lib/controller/r3/NullControllerR3.java b/lib/src/main/java/org/team100/lib/controller/se2/NullControllerSE2.java similarity index 76% rename from lib/src/main/java/org/team100/lib/controller/r3/NullControllerR3.java rename to lib/src/main/java/org/team100/lib/controller/se2/NullControllerSE2.java index 82fad0df..43693dca 100644 --- a/lib/src/main/java/org/team100/lib/controller/r3/NullControllerR3.java +++ b/lib/src/main/java/org/team100/lib/controller/se2/NullControllerSE2.java @@ -1,17 +1,17 @@ -package org.team100.lib.controller.r3; +package org.team100.lib.controller.se2; import org.team100.lib.geometry.DeltaSE2; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.state.ControlR3; +import org.team100.lib.state.ControlSE2; /** * A controller that doesn't do anything except calculate the errors, so that * "atReference" works. */ -public class NullControllerR3 extends ControllerR3Base { +public class NullControllerSE2 extends ControllerSE2Base { - public NullControllerR3( + public NullControllerSE2( LoggerFactory parent, double xTolerance, double thetaTolerance, @@ -24,7 +24,7 @@ public NullControllerR3( public VelocitySE2 calculate100( DeltaSE2 positionError, VelocitySE2 velocityError, - ControlR3 nextReference) { + ControlSE2 nextReference) { return VelocitySE2.ZERO; } diff --git a/lib/src/main/java/org/team100/lib/controller/r3/README.md b/lib/src/main/java/org/team100/lib/controller/se2/README.md similarity index 100% rename from lib/src/main/java/org/team100/lib/controller/r3/README.md rename to lib/src/main/java/org/team100/lib/controller/se2/README.md diff --git a/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java b/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java index 9e46f8f0..852d07f7 100644 --- a/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java +++ b/lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java @@ -17,7 +17,7 @@ import org.team100.lib.logging.LoggerFactory.Pose2dLogger; import org.team100.lib.logging.LoggerFactory.Transform3dLogger; import org.team100.lib.network.CameraReader; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.util.TrailingHistory; import edu.wpi.first.math.geometry.Pose2d; @@ -64,7 +64,7 @@ public class AprilTagRobotLocalizer extends CameraReader { private static final double VISION_CHANGE_TOLERANCE_M = 0.1; // private static final double VISION_CHANGE_TOLERANCE_M = 1; - private final DoubleFunction m_history; + private final DoubleFunction m_history; private final VisionUpdater m_visionUpdater; private final AprilTagFieldLayoutWithCorrectOrientation m_layout; @@ -132,7 +132,7 @@ public AprilTagRobotLocalizer( LoggerFactory parent, LoggerFactory fieldLogger, AprilTagFieldLayoutWithCorrectOrientation layout, - DoubleFunction history, + DoubleFunction history, VisionUpdater visionUpdater) { super(parent, "vision", "blips", StructBuffer.create(Blip24.struct)); LoggerFactory log = parent.type(this); diff --git a/lib/src/main/java/org/team100/lib/localization/FreshSwerveEstimate.java b/lib/src/main/java/org/team100/lib/localization/FreshSwerveEstimate.java index f49cb314..e56ec74c 100644 --- a/lib/src/main/java/org/team100/lib/localization/FreshSwerveEstimate.java +++ b/lib/src/main/java/org/team100/lib/localization/FreshSwerveEstimate.java @@ -4,14 +4,14 @@ import org.team100.lib.coherence.Cache; import org.team100.lib.coherence.SideEffect; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; /** * Proxy the history after making sure it has received any updates that may * mutate it. Some clients want "fresh" estimates, and should use this class; * other clients only need old historical estimates, and should use the history. */ -public class FreshSwerveEstimate implements DoubleFunction { +public class FreshSwerveEstimate implements DoubleFunction { private final SwerveHistory m_history; /** Side effect mutates history. */ private final SideEffect m_vision; @@ -32,7 +32,7 @@ public FreshSwerveEstimate( * making sure any pending updates from vision or odometry have been applied. */ @Override - public ModelR3 apply(double timestampS) { + public ModelSE2 apply(double timestampS) { // run our dependencies if they haven't already m_vision.run(); m_odometry.run(); diff --git a/lib/src/main/java/org/team100/lib/localization/InterpolationRecord.java b/lib/src/main/java/org/team100/lib/localization/InterpolationRecord.java index c260f762..570ffd77 100644 --- a/lib/src/main/java/org/team100/lib/localization/InterpolationRecord.java +++ b/lib/src/main/java/org/team100/lib/localization/InterpolationRecord.java @@ -3,7 +3,7 @@ import java.util.Objects; import org.team100.lib.geometry.VelocitySE2; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveDriveKinematics100; import org.team100.lib.subsystems.swerve.module.state.SwerveModuleDeltas; import org.team100.lib.subsystems.swerve.module.state.SwerveModulePositions; @@ -15,7 +15,7 @@ class InterpolationRecord implements Interpolatable { private final SwerveDriveKinematics100 m_kinematics; - final ModelR3 m_state; + final ModelSE2 m_state; final SwerveModulePositions m_wheelPositions; @@ -29,7 +29,7 @@ class InterpolationRecord implements Interpolatable { */ InterpolationRecord( SwerveDriveKinematics100 kinematics, - ModelR3 state, + ModelSE2 state, SwerveModulePositions wheelPositions) { m_kinematics = kinematics; m_state = state; @@ -76,7 +76,7 @@ public InterpolationRecord interpolate(InterpolationRecord endValue, double t) { VelocitySE2 endVelocity = endValue.m_state.velocity(); VelocitySE2 velocity = startVelocity.plus(endVelocity.minus(startVelocity).times(t)); - ModelR3 newState = new ModelR3(pose, velocity); + ModelSE2 newState = new ModelSE2(pose, velocity); return new InterpolationRecord(m_kinematics, newState, wheelLerp); } diff --git a/lib/src/main/java/org/team100/lib/localization/ManualPose.java b/lib/src/main/java/org/team100/lib/localization/ManualPose.java index 34baa5f4..4750e6eb 100644 --- a/lib/src/main/java/org/team100/lib/localization/ManualPose.java +++ b/lib/src/main/java/org/team100/lib/localization/ManualPose.java @@ -10,7 +10,7 @@ import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleArrayLogger; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -29,9 +29,9 @@ public class ManualPose { private static final double MAX_OMEGA = 1.0; private final DoubleArrayLogger m_log_field_robot; private final Supplier m_v; - private final ObjectCache m_stateCache; + private final ObjectCache m_stateCache; /** Used only by update(). */ - private ModelR3 m_state; + private ModelSE2 m_state; public ManualPose( LoggerFactory fieldLogger, @@ -39,11 +39,11 @@ public ManualPose( Pose2d initial) { m_log_field_robot = fieldLogger.doubleArrayLogger(Level.COMP, "robot"); m_v = v; - m_state = new ModelR3(initial); + m_state = new ModelSE2(initial); m_stateCache = Cache.of(this::update); } - public ModelR3 getState() { + public ModelSE2 getState() { return m_stateCache.get(); } @@ -63,12 +63,12 @@ private double[] poseArray() { pose.getRotation().getDegrees() }; } - private ModelR3 update() { + private ModelSE2 update() { Velocity v = m_v.get(); double vx = v.x() * MAX_V; double vy = v.y() * MAX_V; double omega = v.theta() * MAX_OMEGA; - m_state = new ModelR3( + m_state = new ModelSE2( new Pose2d( m_state.pose().getX() + vx * DT, m_state.pose().getY() + vy * DT, diff --git a/lib/src/main/java/org/team100/lib/localization/NudgingVisionUpdater.java b/lib/src/main/java/org/team100/lib/localization/NudgingVisionUpdater.java index 474f7229..42fc8c50 100644 --- a/lib/src/main/java/org/team100/lib/localization/NudgingVisionUpdater.java +++ b/lib/src/main/java/org/team100/lib/localization/NudgingVisionUpdater.java @@ -1,7 +1,7 @@ package org.team100.lib.localization; import org.team100.lib.coherence.Takt; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Twist2d; @@ -54,7 +54,7 @@ public void put( sample.m_state.pose(), measurement, stateSigma, visionSigma); m_history.put( timestampS, - new ModelR3(nudged, sample.m_state.velocity()), + new ModelSE2(nudged, sample.m_state.velocity()), sample.m_wheelPositions); m_odometryUpdater.replay(timestampS); m_latestTimeS = Takt.get(); diff --git a/lib/src/main/java/org/team100/lib/localization/OdometryUpdater.java b/lib/src/main/java/org/team100/lib/localization/OdometryUpdater.java index f4f5e283..dd74e98b 100644 --- a/lib/src/main/java/org/team100/lib/localization/OdometryUpdater.java +++ b/lib/src/main/java/org/team100/lib/localization/OdometryUpdater.java @@ -8,7 +8,7 @@ import org.team100.lib.geometry.DeltaSE2; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.sensor.gyro.Gyro; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.subsystems.swerve.module.state.SwerveModuleDeltas; import org.team100.lib.subsystems.swerve.module.state.SwerveModulePositions; @@ -123,7 +123,7 @@ private void put( double dt = currentTimeS - lowerEntry.getKey(); InterpolationRecord value = lowerEntry.getValue(); - ModelR3 previousState = value.m_state; + ModelSE2 previousState = value.m_state; if (DEBUG) { System.out.printf("previous x %.6f y %.6f\n", previousState.pose().getX(), previousState.pose().getY()); } @@ -164,7 +164,7 @@ private void put( odoVelo.getY(), gyroRateRad_SNWU); - ModelR3 swerveState = new ModelR3(newPose, velocity); + ModelSE2 swerveState = new ModelSE2(newPose, velocity); m_history.put(currentTimeS, swerveState, wheelPositions); } diff --git a/lib/src/main/java/org/team100/lib/localization/SimulatedTagDetector.java b/lib/src/main/java/org/team100/lib/localization/SimulatedTagDetector.java index 64ae024d..2958c1ee 100644 --- a/lib/src/main/java/org/team100/lib/localization/SimulatedTagDetector.java +++ b/lib/src/main/java/org/team100/lib/localization/SimulatedTagDetector.java @@ -10,7 +10,7 @@ import org.team100.lib.coherence.Takt; import org.team100.lib.config.Camera; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; @@ -55,7 +55,7 @@ public class SimulatedTagDetector { private final List m_cameras; private final AprilTagFieldLayoutWithCorrectOrientation m_layout; - private final DoubleFunction m_history; + private final DoubleFunction m_history; private final Map> m_publishers; /** client instance, not the default */ @@ -71,7 +71,7 @@ public class SimulatedTagDetector { public SimulatedTagDetector( List cameras, AprilTagFieldLayoutWithCorrectOrientation layout, - DoubleFunction history) { + DoubleFunction history) { m_cameras = cameras; m_layout = layout; m_history = history; diff --git a/lib/src/main/java/org/team100/lib/localization/SwerveHistory.java b/lib/src/main/java/org/team100/lib/localization/SwerveHistory.java index 1147295f..b35a433b 100644 --- a/lib/src/main/java/org/team100/lib/localization/SwerveHistory.java +++ b/lib/src/main/java/org/team100/lib/localization/SwerveHistory.java @@ -8,7 +8,7 @@ import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleLogger; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.subsystems.swerve.module.state.SwerveModulePositions; import org.team100.lib.util.TimeInterpolatableBuffer100; @@ -28,7 +28,7 @@ * * Other SwerveModel consumers should use SwerveModelEstimate. */ -public class SwerveHistory implements DoubleFunction { +public class SwerveHistory implements DoubleFunction { /** * The buffer only needs to be long enough to catch stale-but-still-helpful * vision updates. @@ -57,7 +57,7 @@ public SwerveHistory( timestampSeconds, new InterpolationRecord( m_kinodynamics.getKinematics(), - new ModelR3( + new ModelSE2( initialPoseMeters, new VelocitySE2(0, 0, 0)), modulePositions)); @@ -67,7 +67,7 @@ public SwerveHistory( * Sample the state estimate buffer. */ @Override - public ModelR3 apply(double timestampSeconds) { + public ModelSE2 apply(double timestampSeconds) { m_log_timestamp.log(() -> timestampSeconds); return m_poseBuffer.get(timestampSeconds).m_state; } @@ -82,7 +82,7 @@ void reset( timestampSeconds, new InterpolationRecord( m_kinodynamics.getKinematics(), - new ModelR3(pose, new VelocitySE2(0, 0, 0)), + new ModelSE2(pose, new VelocitySE2(0, 0, 0)), modulePositions)); } @@ -94,7 +94,7 @@ void reset( */ void put( double timestamp, - ModelR3 model, + ModelSE2 model, SwerveModulePositions positions) { m_poseBuffer.put( timestamp, diff --git a/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java b/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java index b8cf0269..aba430aa 100644 --- a/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java +++ b/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java @@ -17,9 +17,9 @@ import org.team100.lib.logging.primitive.PrimitiveLogger; import org.team100.lib.reference.r1.SetpointsR1; import org.team100.lib.state.Control100; -import org.team100.lib.state.ControlR3; +import org.team100.lib.state.ControlSE2; import org.team100.lib.state.Model100; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.prr.EAWConfig; import org.team100.lib.subsystems.prr.JointAccelerations; import org.team100.lib.subsystems.prr.JointForce; @@ -579,13 +579,13 @@ public ChassisSpeedsLogger chassisSpeedsLogger(Level level, String leaf) { return new ChassisSpeedsLogger(level, leaf); } - public class GlobaDeltaR3Logger { + public class GlobaDeltaSE2Logger { private final Level m_level; private final DoubleLogger m_xLogger; private final DoubleLogger m_yLogger; private final DoubleLogger m_thetaLogger; - GlobaDeltaR3Logger(Level level, String leaf) { + GlobaDeltaSE2Logger(Level level, String leaf) { m_level = level; m_xLogger = doubleLogger(level, join(leaf, "x m")); m_yLogger = doubleLogger(level, join(leaf, "y m")); @@ -602,8 +602,8 @@ public void log(Supplier vals) { } } - public GlobaDeltaR3Logger DeltaSE2Logger(Level level, String leaf) { - return new GlobaDeltaR3Logger(level, leaf); + public GlobaDeltaSE2Logger DeltaSE2Logger(Level level, String leaf) { + return new GlobaDeltaSE2Logger(level, leaf); } public class VelocitySE2Logger { @@ -755,62 +755,62 @@ public SetpointsR1Logger setpointsR1Logger(Level level, String leaf) { return new SetpointsR1Logger(level, leaf); } - public class ControlR3Logger { + public class ControlSE2Logger { private final Level m_level; private final Control100Logger m_xLogger; private final Control100Logger m_yLogger; private final Control100Logger m_thetaLogger; - ControlR3Logger(Level level, String leaf) { + ControlSE2Logger(Level level, String leaf) { m_level = level; m_xLogger = control100Logger(level, join(leaf, "x")); m_yLogger = control100Logger(level, join(leaf, "y")); m_thetaLogger = control100Logger(level, join(leaf, "theta")); } - public void log(Supplier vals) { + public void log(Supplier vals) { if (!allow(m_level)) return; - ControlR3 val = vals.get(); + ControlSE2 val = vals.get(); m_xLogger.log(val::x); m_yLogger.log(val::y); m_thetaLogger.log(val::theta); } } - public ControlR3Logger controlR3Logger(Level level, String leaf) { - return new ControlR3Logger(level, leaf); + public ControlSE2Logger controlSE2Logger(Level level, String leaf) { + return new ControlSE2Logger(level, leaf); } public Model100Logger model100Logger(Level level, String leaf) { return new Model100Logger(level, leaf); } - public class ModelR3Logger { + public class ModelSE2Logger { private final Level m_level; private final Model100Logger m_xLogger; private final Model100Logger m_yLogger; private final Model100Logger m_thetaLogger; - ModelR3Logger(Level level, String leaf) { + ModelSE2Logger(Level level, String leaf) { m_level = level; m_xLogger = model100Logger(level, join(leaf, "x")); m_yLogger = model100Logger(level, join(leaf, "y")); m_thetaLogger = model100Logger(level, join(leaf, "theta")); } - public void log(Supplier vals) { + public void log(Supplier vals) { if (!allow(m_level)) return; - ModelR3 val = vals.get(); + ModelSE2 val = vals.get(); m_xLogger.log(val::x); m_yLogger.log(val::y); m_thetaLogger.log(val::theta); } } - public ModelR3Logger modelR3Logger(Level level, String leaf) { - return new ModelR3Logger(level, leaf); + public ModelSE2Logger modelSE2Logger(Level level, String leaf) { + return new ModelSE2Logger(level, leaf); } public class SwerveModulePosition100Logger { diff --git a/lib/src/main/java/org/team100/lib/profile/r3/FreeRotationProfile.java b/lib/src/main/java/org/team100/lib/profile/se2/FreeRotationProfile.java similarity index 88% rename from lib/src/main/java/org/team100/lib/profile/r3/FreeRotationProfile.java rename to lib/src/main/java/org/team100/lib/profile/se2/FreeRotationProfile.java index 22a82214..23d858b9 100644 --- a/lib/src/main/java/org/team100/lib/profile/r3/FreeRotationProfile.java +++ b/lib/src/main/java/org/team100/lib/profile/se2/FreeRotationProfile.java @@ -1,11 +1,11 @@ -package org.team100.lib.profile.r3; +package org.team100.lib.profile.se2; import org.team100.lib.framework.TimedRobot100; import org.team100.lib.profile.incremental.IncrementalProfile; import org.team100.lib.state.Control100; -import org.team100.lib.state.ControlR3; +import org.team100.lib.state.ControlSE2; import org.team100.lib.state.Model100; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import edu.wpi.first.math.MathUtil; @@ -16,7 +16,7 @@ * translation, e.g. to make sure the robot arrives with the correct * orientation. */ -public class FreeRotationProfile implements ProfileR3 { +public class FreeRotationProfile implements ProfileSE2 { /** For testing */ private static final boolean DEBUG = false; @@ -52,7 +52,7 @@ public class FreeRotationProfile implements ProfileR3 { * This is a fairly coarse optimization, i.e. ETA within 0.1 sec or so. */ @Override - public void solve(ModelR3 i, ModelR3 g) { + public void solve(ModelSE2 i, ModelSE2 g) { // first find the max ETA if (DEBUG) { System.out.printf("i %s g %s\n", i, g); @@ -79,11 +79,11 @@ public void solve(ModelR3 i, ModelR3 g) { } @Override - public ControlR3 calculate(ModelR3 i, ModelR3 g) { + public ControlSE2 calculate(ModelSE2 i, ModelSE2 g) { if (i == null || g == null) { // this can happen on startup when the initial state hasn't yet been defined, // but the cache refresher is trying to update the references. - return ControlR3.zero(); + return ControlSE2.zero(); } if (DEBUG) { System.out.printf("initial %s goal %s\n", i, g); @@ -95,6 +95,6 @@ public ControlR3 calculate(ModelR3 i, ModelR3 g) { MathUtil.angleModulus(i.theta().x() - g.theta().x()) + g.theta().x(), i.theta().v()); Control100 stateTheta = ptheta.calculate(DT, theta.control(), g.theta()); - return new ControlR3(stateX, stateY, stateTheta); + return new ControlSE2(stateX, stateY, stateTheta); } } diff --git a/lib/src/main/java/org/team100/lib/profile/r3/HolonomicProfile.java b/lib/src/main/java/org/team100/lib/profile/se2/HolonomicProfile.java similarity index 90% rename from lib/src/main/java/org/team100/lib/profile/r3/HolonomicProfile.java rename to lib/src/main/java/org/team100/lib/profile/se2/HolonomicProfile.java index 0b1febc4..ea656e87 100644 --- a/lib/src/main/java/org/team100/lib/profile/r3/HolonomicProfile.java +++ b/lib/src/main/java/org/team100/lib/profile/se2/HolonomicProfile.java @@ -1,11 +1,11 @@ -package org.team100.lib.profile.r3; +package org.team100.lib.profile.se2; import org.team100.lib.framework.TimedRobot100; import org.team100.lib.profile.incremental.IncrementalProfile; import org.team100.lib.state.Control100; -import org.team100.lib.state.ControlR3; +import org.team100.lib.state.ControlSE2; import org.team100.lib.state.Model100; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import edu.wpi.first.math.MathUtil; @@ -16,7 +16,7 @@ * Note that because acceleration is adjusted, but not cruise velocity, the * resulting paths will not be straight, for rest-to-rest profiles. */ -public class HolonomicProfile implements ProfileR3 { +public class HolonomicProfile implements ProfileSE2 { /** For testing */ private static final boolean DEBUG = false; @@ -55,7 +55,7 @@ public class HolonomicProfile implements ProfileR3 { * This is a fairly coarse optimization, i.e. ETA within 0.1 sec or so. */ @Override - public void solve(ModelR3 i, ModelR3 g) { + public void solve(ModelSE2 i, ModelSE2 g) { // first find the max ETA if (DEBUG) { System.out.printf("i %s g %s\n", i, g); @@ -86,11 +86,11 @@ public void solve(ModelR3 i, ModelR3 g) { } @Override - public ControlR3 calculate(ModelR3 i, ModelR3 g) { + public ControlSE2 calculate(ModelSE2 i, ModelSE2 g) { if (i == null || g == null) { // this can happen on startup when the initial state hasn't yet been defined, // but the cache refresher is trying to update the references. - return ControlR3.zero(); + return ControlSE2.zero(); } if (DEBUG) { System.out.printf("initial %s goal %s\n", i, g); @@ -102,6 +102,6 @@ public ControlR3 calculate(ModelR3 i, ModelR3 g) { MathUtil.angleModulus(i.theta().x() - g.theta().x()) + g.theta().x(), i.theta().v()); Control100 stateTheta = pptheta.calculate(DT, theta.control(), g.theta()); - return new ControlR3(stateX, stateY, stateTheta); + return new ControlSE2(stateX, stateY, stateTheta); } } diff --git a/lib/src/main/java/org/team100/lib/profile/r3/HolonomicProfileFactory.java b/lib/src/main/java/org/team100/lib/profile/se2/HolonomicProfileFactory.java similarity index 99% rename from lib/src/main/java/org/team100/lib/profile/r3/HolonomicProfileFactory.java rename to lib/src/main/java/org/team100/lib/profile/se2/HolonomicProfileFactory.java index 3de27141..6d0a2646 100644 --- a/lib/src/main/java/org/team100/lib/profile/r3/HolonomicProfileFactory.java +++ b/lib/src/main/java/org/team100/lib/profile/se2/HolonomicProfileFactory.java @@ -1,4 +1,4 @@ -package org.team100.lib.profile.r3; +package org.team100.lib.profile.se2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; diff --git a/lib/src/main/java/org/team100/lib/profile/r3/ProfileR3.java b/lib/src/main/java/org/team100/lib/profile/se2/ProfileSE2.java similarity index 56% rename from lib/src/main/java/org/team100/lib/profile/r3/ProfileR3.java rename to lib/src/main/java/org/team100/lib/profile/se2/ProfileSE2.java index 032bc0be..dc5449e6 100644 --- a/lib/src/main/java/org/team100/lib/profile/r3/ProfileR3.java +++ b/lib/src/main/java/org/team100/lib/profile/se2/ProfileSE2.java @@ -1,9 +1,9 @@ -package org.team100.lib.profile.r3; +package org.team100.lib.profile.se2; -import org.team100.lib.state.ControlR3; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ControlSE2; +import org.team100.lib.state.ModelSE2; -public interface ProfileR3 { +public interface ProfileSE2 { /** * For coordinated profiles, do whatever precalculation is required. * @@ -11,7 +11,7 @@ public interface ProfileR3 { * @param i initial * @param g goal */ - void solve(ModelR3 i, ModelR3 g); + void solve(ModelSE2 i, ModelSE2 g); /** * Compute the control for the end of the next time step. @@ -20,6 +20,6 @@ public interface ProfileR3 { * @param g goal * @return control */ - ControlR3 calculate(ModelR3 i, ModelR3 g); + ControlSE2 calculate(ModelSE2 i, ModelSE2 g); } \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/reference/r3/ConstantReferenceR3.java b/lib/src/main/java/org/team100/lib/reference/r3/ConstantReferenceR3.java deleted file mode 100644 index e0cc87e0..00000000 --- a/lib/src/main/java/org/team100/lib/reference/r3/ConstantReferenceR3.java +++ /dev/null @@ -1,39 +0,0 @@ -package org.team100.lib.reference.r3; - -import org.team100.lib.state.ControlR3; -import org.team100.lib.state.ModelR3; - -/** Always returns the same reference. */ -public class ConstantReferenceR3 implements ReferenceR3 { - private final ModelR3 m_goal; - - public ConstantReferenceR3(ModelR3 goal) { - m_goal = goal; - } - - @Override - public void initialize(ModelR3 measurement) { - // - } - - @Override - public ModelR3 current() { - return m_goal; - } - - @Override - public ControlR3 next() { - return m_goal.control(); - } - - @Override - public boolean done() { - return false; - } - - @Override - public ModelR3 goal() { - return m_goal; - } - -} diff --git a/lib/src/main/java/org/team100/lib/reference/se2/ConstantReferenceSE2.java b/lib/src/main/java/org/team100/lib/reference/se2/ConstantReferenceSE2.java new file mode 100644 index 00000000..f6aff4d7 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/reference/se2/ConstantReferenceSE2.java @@ -0,0 +1,39 @@ +package org.team100.lib.reference.se2; + +import org.team100.lib.state.ControlSE2; +import org.team100.lib.state.ModelSE2; + +/** Always returns the same reference. */ +public class ConstantReferenceSE2 implements ReferenceSE2 { + private final ModelSE2 m_goal; + + public ConstantReferenceSE2(ModelSE2 goal) { + m_goal = goal; + } + + @Override + public void initialize(ModelSE2 measurement) { + // + } + + @Override + public ModelSE2 current() { + return m_goal; + } + + @Override + public ControlSE2 next() { + return m_goal.control(); + } + + @Override + public boolean done() { + return false; + } + + @Override + public ModelSE2 goal() { + return m_goal; + } + +} diff --git a/lib/src/main/java/org/team100/lib/reference/r3/ProfileReferenceR3.java b/lib/src/main/java/org/team100/lib/reference/se2/ProfileReferenceSE2.java similarity index 70% rename from lib/src/main/java/org/team100/lib/reference/r3/ProfileReferenceR3.java rename to lib/src/main/java/org/team100/lib/reference/se2/ProfileReferenceSE2.java index b425bc99..6639f92d 100644 --- a/lib/src/main/java/org/team100/lib/reference/r3/ProfileReferenceR3.java +++ b/lib/src/main/java/org/team100/lib/reference/se2/ProfileReferenceSE2.java @@ -1,15 +1,15 @@ -package org.team100.lib.reference.r3; +package org.team100.lib.reference.se2; import org.team100.lib.coherence.Cache; import org.team100.lib.coherence.ObjectCache; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.BooleanLogger; -import org.team100.lib.logging.LoggerFactory.ControlR3Logger; -import org.team100.lib.logging.LoggerFactory.ModelR3Logger; -import org.team100.lib.profile.r3.ProfileR3; -import org.team100.lib.state.ControlR3; -import org.team100.lib.state.ModelR3; +import org.team100.lib.logging.LoggerFactory.ControlSE2Logger; +import org.team100.lib.logging.LoggerFactory.ModelSE2Logger; +import org.team100.lib.profile.se2.ProfileSE2; +import org.team100.lib.state.ControlSE2; +import org.team100.lib.state.ModelSE2; /** * Produces references based on profiles. @@ -24,39 +24,39 @@ * continue forever unless we explicitly stop them, so if you use this class, * you need to remember to do that. */ -public class ProfileReferenceR3 implements ReferenceR3 { +public class ProfileReferenceSE2 implements ReferenceSE2 { private static final boolean DEBUG = false; private static final double TOLERANCE = 0.01; /** * Putting these in the same class allows us to refresh them both atomically. */ - private record References(ModelR3 m_current, ControlR3 m_next) { + private record References(ModelSE2 m_current, ControlSE2 m_next) { } private final LoggerFactory m_log; - private final ProfileR3 m_profile; + private final ProfileSE2 m_profile; /** The name is for debugging. */ private final String m_name; private final ObjectCache m_references; - private final ModelR3Logger m_log_current; - private final ControlR3Logger m_log_next; + private final ModelSE2Logger m_log_current; + private final ControlSE2Logger m_log_next; private final BooleanLogger m_log_done; - private final ModelR3Logger m_log_goal; + private final ModelSE2Logger m_log_goal; - private ModelR3 m_goal; + private ModelSE2 m_goal; private boolean m_done; /** * The most-recently calculated "next" reference, which will be a future * "current" reference. */ - private ControlR3 m_next; + private ControlSE2 m_next; - public ProfileReferenceR3( + public ProfileReferenceSE2( LoggerFactory parent, - ProfileR3 profile, + ProfileSE2 profile, String name) { m_log = parent.type(this); m_profile = profile; @@ -64,10 +64,10 @@ public ProfileReferenceR3( // this will keep polling until we stop it. m_references = Cache.of(() -> refresh(m_next == null ? null : m_next.model())); - m_log_current = m_log.modelR3Logger(Level.TRACE, "current"); - m_log_next = m_log.controlR3Logger(Level.TRACE, "next"); + m_log_current = m_log.modelSE2Logger(Level.TRACE, "current"); + m_log_next = m_log.controlSE2Logger(Level.TRACE, "next"); m_log_done = m_log.booleanLogger(Level.TRACE, "done"); - m_log_goal = m_log.modelR3Logger(Level.TRACE, "goal"); + m_log_goal = m_log.modelSE2Logger(Level.TRACE, "goal"); } /** @@ -75,28 +75,28 @@ public ProfileReferenceR3( * initialize(), you'll use the old scales. That's probably fine, if the goal * hasn't moved much, but it's not appropriate to move the goal a lot. */ - public void setGoal(ModelR3 goal) { + public void setGoal(ModelSE2 goal) { m_goal = goal; } /** Immediately overwrite the references. */ @Override - public void initialize(ModelR3 measurement) { + public void initialize(ModelSE2 measurement) { m_profile.solve(measurement, m_goal); m_references.set(refresh(measurement)); m_done = false; } @Override - public ModelR3 current() { - ModelR3 current = m_references.get().m_current; + public ModelSE2 current() { + ModelSE2 current = m_references.get().m_current; m_log_current.log(() -> current); return current; } @Override - public ControlR3 next() { - ControlR3 next = m_references.get().m_next; + public ControlSE2 next() { + ControlSE2 next = m_references.get().m_next; m_log_next.log(() -> next); return next; } @@ -108,7 +108,7 @@ public boolean done() { } @Override - public ModelR3 goal() { + public ModelSE2 goal() { m_log_goal.log(() -> m_goal); return m_goal; } @@ -123,12 +123,12 @@ public void end() { //////////////////////////////////// - private References refresh(ModelR3 newCurrent) { + private References refresh(ModelSE2 newCurrent) { m_next = makeNext(newCurrent); return new References(newCurrent, m_next); } - private ControlR3 makeNext(ModelR3 current) { + private ControlSE2 makeNext(ModelSE2 current) { if (DEBUG) { System.out.printf("ProfileReference refreshing %s\n", m_name); } @@ -139,7 +139,7 @@ private ControlR3 makeNext(ModelR3 current) { if (m_goal == null) { return current.control(); } - ControlR3 next = m_profile.calculate(current, m_goal); + ControlSE2 next = m_profile.calculate(current, m_goal); if (current.near(m_goal, TOLERANCE)) m_done = true; return next; diff --git a/lib/src/main/java/org/team100/lib/reference/r3/ReferenceR3.java b/lib/src/main/java/org/team100/lib/reference/se2/ReferenceSE2.java similarity index 66% rename from lib/src/main/java/org/team100/lib/reference/r3/ReferenceR3.java rename to lib/src/main/java/org/team100/lib/reference/se2/ReferenceSE2.java index f0827117..126542ad 100644 --- a/lib/src/main/java/org/team100/lib/reference/r3/ReferenceR3.java +++ b/lib/src/main/java/org/team100/lib/reference/se2/ReferenceSE2.java @@ -1,31 +1,31 @@ -package org.team100.lib.reference.r3; +package org.team100.lib.reference.se2; -import org.team100.lib.state.ControlR3; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ControlSE2; +import org.team100.lib.state.ModelSE2; /** - * A source of references for control in R3, useful for drivetrain or + * A source of references for control in SE2, useful for drivetrain or * planar mechanism. */ -public interface ReferenceR3 { +public interface ReferenceSE2 { /** * Make current() return the starting point, which might be informed by the * supplied measurement, or not. */ - void initialize(ModelR3 measurement); + void initialize(ModelSE2 measurement); /** * Reference for the current time. This can be null (e.g. between instantiation * and initialization). */ - ModelR3 current(); + ModelSE2 current(); /** * Reference for 0.02 sec in the future. This can be null (e.g. between * instantiation and initialization). */ - ControlR3 next(); + ControlSE2 next(); /** * The reference series has reached the end, and will keep returning the same @@ -36,5 +36,5 @@ public interface ReferenceR3 { /** * For reference sources that have endpoints, what is the endpoint? */ - ModelR3 goal(); + ModelSE2 goal(); } diff --git a/lib/src/main/java/org/team100/lib/reference/r3/TrajectoryReferenceR3.java b/lib/src/main/java/org/team100/lib/reference/se2/TrajectoryReferenceSE2.java similarity index 58% rename from lib/src/main/java/org/team100/lib/reference/r3/TrajectoryReferenceR3.java rename to lib/src/main/java/org/team100/lib/reference/se2/TrajectoryReferenceSE2.java index e58df8be..4b15eb72 100644 --- a/lib/src/main/java/org/team100/lib/reference/r3/TrajectoryReferenceR3.java +++ b/lib/src/main/java/org/team100/lib/reference/se2/TrajectoryReferenceSE2.java @@ -1,56 +1,56 @@ -package org.team100.lib.reference.r3; +package org.team100.lib.reference.se2; import org.team100.lib.coherence.Takt; import org.team100.lib.framework.TimedRobot100; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.BooleanLogger; -import org.team100.lib.logging.LoggerFactory.ControlR3Logger; +import org.team100.lib.logging.LoggerFactory.ControlSE2Logger; import org.team100.lib.logging.LoggerFactory.DoubleLogger; -import org.team100.lib.logging.LoggerFactory.ModelR3Logger; -import org.team100.lib.state.ControlR3; -import org.team100.lib.state.ModelR3; +import org.team100.lib.logging.LoggerFactory.ModelSE2Logger; +import org.team100.lib.state.ControlSE2; +import org.team100.lib.state.ModelSE2; import org.team100.lib.trajectory.Trajectory100; /** Produces references based on a trajectory. */ -public class TrajectoryReferenceR3 implements ReferenceR3 { +public class TrajectoryReferenceSE2 implements ReferenceSE2 { private final LoggerFactory m_log; private final Trajectory100 m_trajectory; - private final ModelR3Logger m_log_current; - private final ControlR3Logger m_log_next; + private final ModelSE2Logger m_log_current; + private final ControlSE2Logger m_log_next; private final BooleanLogger m_log_done; - private final ModelR3Logger m_log_goal; + private final ModelSE2Logger m_log_goal; private final DoubleLogger m_log_progress; private double m_startTimeS; - public TrajectoryReferenceR3( + public TrajectoryReferenceSE2( LoggerFactory parent, Trajectory100 trajectory) { m_log = parent.type(this); m_trajectory = trajectory; m_log_progress = m_log.doubleLogger(Level.TRACE, "progress"); - m_log_current = m_log.modelR3Logger(Level.TRACE, "current"); - m_log_next = m_log.controlR3Logger(Level.TRACE, "next"); + m_log_current = m_log.modelSE2Logger(Level.TRACE, "current"); + m_log_next = m_log.controlSE2Logger(Level.TRACE, "next"); m_log_done = m_log.booleanLogger(Level.TRACE, "done"); - m_log_goal = m_log.modelR3Logger(Level.TRACE, "goal"); + m_log_goal = m_log.modelSE2Logger(Level.TRACE, "goal"); } /** Ignores the measurement, resets the trajectory timer. */ @Override - public void initialize(ModelR3 measurement) { + public void initialize(ModelSE2 measurement) { m_startTimeS = Takt.get(); } @Override - public ModelR3 current() { - ModelR3 current = sample(progress()).model(); + public ModelSE2 current() { + ModelSE2 current = sample(progress()).model(); m_log_current.log(() -> current); return current; } @Override - public ControlR3 next() { - ControlR3 next = sample(progress() + TimedRobot100.LOOP_PERIOD_S); + public ControlSE2 next() { + ControlSE2 next = sample(progress() + TimedRobot100.LOOP_PERIOD_S); m_log_next.log(() -> next); return next; } @@ -63,8 +63,8 @@ public boolean done() { } @Override - public ModelR3 goal() { - ModelR3 goal = ControlR3.fromTimedState(m_trajectory.getLastPoint()).model(); + public ModelSE2 goal() { + ModelSE2 goal = ControlSE2.fromTimedState(m_trajectory.getLastPoint()).model(); m_log_goal.log(() -> goal); return goal; } @@ -77,7 +77,7 @@ private double progress() { return progress; } - private ControlR3 sample(double t) { - return ControlR3.fromTimedState(m_trajectory.sample(t)); + private ControlSE2 sample(double t) { + return ControlSE2.fromTimedState(m_trajectory.sample(t)); } } diff --git a/lib/src/main/java/org/team100/lib/servo/Torque.java b/lib/src/main/java/org/team100/lib/servo/Torque.java index 2abed938..b7deefda 100644 --- a/lib/src/main/java/org/team100/lib/servo/Torque.java +++ b/lib/src/main/java/org/team100/lib/servo/Torque.java @@ -10,10 +10,6 @@ public Torque(DoubleUnaryOperator torque) { m_torque = torque; } - /** - * I know it's weird to use an optional argument here but it keeps the default - * in one place. - */ public double torque(double positionRad) { return m_torque.applyAsDouble(positionRad); } diff --git a/lib/src/main/java/org/team100/lib/state/ControlR3.java b/lib/src/main/java/org/team100/lib/state/ControlSE2.java similarity index 74% rename from lib/src/main/java/org/team100/lib/state/ControlR3.java rename to lib/src/main/java/org/team100/lib/state/ControlSE2.java index 874b8d4f..9793855c 100644 --- a/lib/src/main/java/org/team100/lib/state/ControlR3.java +++ b/lib/src/main/java/org/team100/lib/state/ControlSE2.java @@ -11,8 +11,9 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; /** - * Describes the state of three independent dimensions, each of which is - * represented by position, velocity, and acceleration. + * Describes the state of rigid body transformations in two dimensions, the + * SE(2) manifold (x,y,theta), where each dimension is represented by position, + * velocity, and acceleration. * * This could be used for navigation, or for other applications of rigid-body * transforms in 2d, e.g. planar mechanisms. @@ -21,61 +22,62 @@ * * Do not try to use zero as an initial location; always initialize with the * current location. + * + * Note: the metric used here is not the SE(2) geodesic, it treats the XY plane + * and rotation dimensions independently. */ -public class ControlR3 { +public class ControlSE2 { private final Control100 m_x; private final Control100 m_y; private final Control100 m_theta; - public ControlR3(Control100 x, Control100 y, Control100 theta) { + public ControlSE2(Control100 x, Control100 y, Control100 theta) { m_x = x; m_y = y; m_theta = theta; } - public ControlR3(Pose2d x, VelocitySE2 v) { + public ControlSE2(Pose2d x, VelocitySE2 v) { this( new Control100(x.getX(), v.x(), 0), new Control100(x.getY(), v.y(), 0), new Control100(x.getRotation().getRadians(), v.theta(), 0)); } - public ControlR3(Pose2d x, VelocitySE2 v, AccelerationSE2 a) { + public ControlSE2(Pose2d x, VelocitySE2 v, AccelerationSE2 a) { this( new Control100(x.getX(), v.x(), a.x()), new Control100(x.getY(), v.y(), a.y()), new Control100(x.getRotation().getRadians(), v.theta(), a.theta())); } - public ControlR3(Pose2d x) { + public ControlSE2(Pose2d x) { this(x, new VelocitySE2(0, 0, 0)); } - public ControlR3(Rotation2d x) { + public ControlSE2(Rotation2d x) { this(new Pose2d(0, 0, x)); } - public static ControlR3 zero() { - return new ControlR3(new Control100(), new Control100(), new Control100()); - } - - public ModelR3 model() { - return new ModelR3(m_x.model(), m_y.model(), m_theta.model()); + public static ControlSE2 zero() { + return new ControlSE2(new Control100(), new Control100(), new Control100()); } - public ControlR3 withTheta(double theta) { - return new ControlR3(m_x, m_y, new Control100(theta, m_theta.v(), m_theta.a())); + public ModelSE2 model() { + return new ModelSE2(m_x.model(), m_y.model(), m_theta.model()); } - public ControlR3 minus(ControlR3 other) { - return new ControlR3(x().minus(other.x()), y().minus(other.y()), theta().minus(other.theta())); + /** Component-wise difference (not geodesic) */ + public ControlSE2 minus(ControlSE2 other) { + return new ControlSE2(x().minus(other.x()), y().minus(other.y()), theta().minus(other.theta())); } - public ControlR3 plus(ControlR3 other) { - return new ControlR3(x().plus(other.x()), y().plus(other.y()), theta().plus(other.theta())); + /** Component-wise sum (not geodesic) */ + public ControlSE2 plus(ControlSE2 other) { + return new ControlSE2(x().plus(other.x()), y().plus(other.y()), theta().plus(other.theta())); } - public boolean near(ControlR3 other, double tolerance) { + public boolean near(ControlSE2 other, double tolerance) { return x().near(other.x(), tolerance) && y().near(other.y(), tolerance) && theta().near(other.theta(), tolerance); @@ -124,7 +126,7 @@ public Control100 theta() { * * Correctly accounts for centripetal acceleration. */ - public static ControlR3 fromTimedState(TimedState timedPose) { + public static ControlSE2 fromTimedState(TimedState timedPose) { double xx = timedPose.state().getPose().pose().getTranslation().getX(); double yx = timedPose.state().getPose().pose().getTranslation().getY(); double thetax = timedPose.state().getPose().pose().getRotation().getRadians(); @@ -146,7 +148,7 @@ public static ControlR3 fromTimedState(TimedState timedPose) { double xCa = -1.0 * course.getSin() * centripetalAccelM_s_s; double yCa = course.getCos() * centripetalAccelM_s_s; - return new ControlR3( + return new ControlSE2( new Control100(xx, xv, xa + xCa), new Control100(yx, yv, ya + yCa), new Control100(thetax, thetav, thetaa)); diff --git a/lib/src/main/java/org/team100/lib/state/ModelR3.java b/lib/src/main/java/org/team100/lib/state/ModelSE2.java similarity index 68% rename from lib/src/main/java/org/team100/lib/state/ModelR3.java rename to lib/src/main/java/org/team100/lib/state/ModelSE2.java index 7816cc8e..f4dd0a2e 100644 --- a/lib/src/main/java/org/team100/lib/state/ModelR3.java +++ b/lib/src/main/java/org/team100/lib/state/ModelSE2.java @@ -13,27 +13,31 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; /** - * Describes the state of three independent dimensions, each of which is - * represented by position and velocity. + * Describes the state of rigid body transformations in two dimensions, the + * SE(2) manifold (x,y,theta), where each dimension is represented by position + * and velocity. * * This could be used for navigation, or for other applications of rigid-body * transforms in 2d, e.g. planar mechanisms. * * This type is used for measurement and estimation, which is why it doesn't * include acceleration. + * + * Note: the metric used here is not the SE(2) geodesic, it treats the XY plane + * and rotation dimensions independently. */ -public class ModelR3 { +public class ModelSE2 { private final Model100 m_x; private final Model100 m_y; private final Model100 m_theta; - public ModelR3(Model100 x, Model100 y, Model100 theta) { + public ModelSE2(Model100 x, Model100 y, Model100 theta) { m_x = x; m_y = y; m_theta = theta; } - public ModelR3(Pose2d x, VelocitySE2 v) { + public ModelSE2(Pose2d x, VelocitySE2 v) { this( new Model100(x.getX(), v.x()), new Model100(x.getY(), v.y()), @@ -41,48 +45,50 @@ public ModelR3(Pose2d x, VelocitySE2 v) { } /** Motionless with the specified pose */ - public ModelR3(Pose2d x) { + public ModelSE2(Pose2d x) { this(x, new VelocitySE2(0, 0, 0)); } /** Motionless at the origin with the specified heading */ - public ModelR3(Rotation2d x) { + public ModelSE2(Rotation2d x) { this(new Pose2d(0, 0, x)); } /** Motionless at the origin */ - public ModelR3() { + public ModelSE2() { this(new Model100(), new Model100(), new Model100()); } - public ControlR3 control() { - return new ControlR3(m_x.control(), m_y.control(), m_theta.control()); + public ControlSE2 control() { + return new ControlSE2(m_x.control(), m_y.control(), m_theta.control()); } - public ModelR3 withTheta(double theta) { - return new ModelR3(m_x, m_y, new Model100(theta, m_theta.v())); + public ModelSE2 withTheta(double theta) { + return new ModelSE2(m_x, m_y, new Model100(theta, m_theta.v())); } - public ModelR3 minus(ModelR3 other) { - return new ModelR3(x().minus(other.x()), y().minus(other.y()), theta().minus(other.theta())); + /** Component-wise difference (not geodesic) */ + public ModelSE2 minus(ModelSE2 other) { + return new ModelSE2(x().minus(other.x()), y().minus(other.y()), theta().minus(other.theta())); } - public ModelR3 plus(ModelR3 other) { - return new ModelR3(x().plus(other.x()), y().plus(other.y()), theta().plus(other.theta())); + /** Component-wise sum (not geodesic) */ + public ModelSE2 plus(ModelSE2 other) { + return new ModelSE2(x().plus(other.x()), y().plus(other.y()), theta().plus(other.theta())); } /** * Use the current velocity to evolve the position of each dimension * independently. * - * This is wrong for Pose2d; if you want the correct thing, see Twist2d. + * This does not describe geodesic paths in SE(2). For that, see Twist2d. */ - public ModelR3 evolve(double dt) { - return new ModelR3(m_x.evolve(dt), m_y.evolve(dt), m_theta.evolve(dt)); + public ModelSE2 evolve(double dt) { + return new ModelSE2(m_x.evolve(dt), m_y.evolve(dt), m_theta.evolve(dt)); } - /** all dimensions position and velocity are within (the same) tolerance */ - public boolean near(ModelR3 other, double tolerance) { + /** All dimensions position and velocity are within (the same) tolerance */ + public boolean near(ModelSE2 other, double tolerance) { return x().near(other.x(), tolerance) && y().near(other.y(), tolerance) && theta().near(other.theta(), tolerance); @@ -92,7 +98,7 @@ public Pose2d pose() { return new Pose2d(m_x.x(), m_y.x(), new Rotation2d(m_theta.x())); } - /** Translation of the pose */ + /** Translation of the pose. */ public Translation2d translation() { return new Translation2d(m_x.x(), m_y.x()); } @@ -109,7 +115,7 @@ public GlobalVelocityR2 velocityR2() { return new GlobalVelocityR2(m_x.v(), m_y.v()); } - /** Robot-relative speeds */ + /** Robot-relative speeds. */ public ChassisSpeeds chassisSpeeds() { return SwerveKinodynamics.toInstantaneousChassisSpeeds(velocity(), rotation()); } @@ -129,7 +135,7 @@ public Model100 theta() { /** * Transform timed pose into swerve state. */ - public static ModelR3 fromTimedState(TimedState timedPose) { + public static ModelSE2 fromTimedState(TimedState timedPose) { Pose2dWithMotion state = timedPose.state(); WaypointSE2 pose = state.getPose(); Translation2d translation = pose.pose().getTranslation(); @@ -141,7 +147,7 @@ public static ModelR3 fromTimedState(TimedState timedPose) { double xv = course.getCos() * velocityM_s; double yv = course.getSin() * velocityM_s; double thetav = state.getHeadingRateRad_M() * velocityM_s; - return new ModelR3( + return new ModelSE2( new Model100(xx, xv), new Model100(yx, yv), new Model100(thetax, thetav)); diff --git a/lib/src/main/java/org/team100/lib/state/README.md b/lib/src/main/java/org/team100/lib/state/README.md index 52a11d63..292885f3 100644 --- a/lib/src/main/java/org/team100/lib/state/README.md +++ b/lib/src/main/java/org/team100/lib/state/README.md @@ -2,7 +2,14 @@ This package represents mechanisms: -- `Model100` represents measurements. Measurements never include acceleration, since it is not directly measurable. -- `Control100` represents control outputs, which _do_ contain acceleration, which can translate directly into motor voltages using the "kA" factor of the motor models. +- `Model100` represents measurements. Measurements never include acceleration, +since it is not directly measurable. +- `Control100` represents control outputs, which _do_ contain acceleration, +which can translate directly into motor voltages using the "kA" factor of the motor models. -In the "state space" representation in control theory, the `Model100` is the `x` variable and `Control100` is the `u` variable. \ No newline at end of file +In the "state space" representation in control theory, the `Model100` is +the `x` variable and `Control100` is the `u` variable. + +There are groupings for the SE(2) manifold of 2d transformations, `ModelSE2` +and `ControlSE2`. These treat each dimension independently, not using the +logmap geodesic constant-twist idea. \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/subsystems/mecanum/MecanumDrive100.java b/lib/src/main/java/org/team100/lib/subsystems/mecanum/MecanumDrive100.java index ec8c03f1..10933f4c 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/mecanum/MecanumDrive100.java +++ b/lib/src/main/java/org/team100/lib/subsystems/mecanum/MecanumDrive100.java @@ -7,10 +7,10 @@ import org.team100.lib.logging.LoggerFactory.VelocitySE2Logger; import org.team100.lib.sensor.gyro.Gyro; import org.team100.lib.servo.OutboardLinearVelocityServo; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.mecanum.kinematics.MecanumKinematics100; import org.team100.lib.subsystems.mecanum.kinematics.MecanumKinematics100.Slip; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import edu.wpi.first.math.geometry.Pose2d; @@ -24,7 +24,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; /** Mecanum drive with optional gyro. */ -public class MecanumDrive100 extends SubsystemBase implements VelocitySubsystemR3 { +public class MecanumDrive100 extends SubsystemBase implements VelocitySubsystemSE2 { private final DoubleArrayLogger m_log_field_robot; private final VelocitySE2Logger m_log_input; @@ -81,9 +81,9 @@ public MecanumDrive100( } @Override - public ModelR3 getState() { + public ModelSE2 getState() { // assume the velocity is exactly what was requested. - return new ModelR3(m_pose, m_input); + return new ModelSE2(m_pose, m_input); } /** diff --git a/lib/src/main/java/org/team100/lib/subsystems/prr/AnalyticalJacobian.java b/lib/src/main/java/org/team100/lib/subsystems/prr/AnalyticalJacobian.java index 0e6ee3d5..b2f86763 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/prr/AnalyticalJacobian.java +++ b/lib/src/main/java/org/team100/lib/subsystems/prr/AnalyticalJacobian.java @@ -2,8 +2,8 @@ import org.team100.lib.geometry.AccelerationSE2; import org.team100.lib.geometry.VelocitySE2; -import org.team100.lib.state.ControlR3; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ControlSE2; +import org.team100.lib.state.ModelSE2; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -48,7 +48,7 @@ public VelocitySE2 forward(EAWConfig q, JointVelocities qdot) { * * See README.md equation 5 */ - public JointVelocities inverse(ModelR3 m) { + public JointVelocities inverse(ModelSE2 m) { Pose2d x = m.pose(); VelocitySE2 xdot = m.velocity(); EAWConfig q = m_k.inverse(x); @@ -78,7 +78,7 @@ public AccelerationSE2 forwardA( * * See doc/README.md equation 9 */ - public JointAccelerations inverseA(ControlR3 m) { + public JointAccelerations inverseA(ControlSE2 m) { Pose2d x = m.pose(); VelocitySE2 xdot = m.velocity(); AccelerationSE2 xddot = m.acceleration(); diff --git a/lib/src/main/java/org/team100/lib/subsystems/prr/Jacobian.java b/lib/src/main/java/org/team100/lib/subsystems/prr/Jacobian.java index 87baed17..47ec5ed3 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/prr/Jacobian.java +++ b/lib/src/main/java/org/team100/lib/subsystems/prr/Jacobian.java @@ -4,7 +4,7 @@ import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.optimization.NumericalJacobian100; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -44,7 +44,7 @@ public VelocitySE2 forward(EAWConfig c, JointVelocities v) { * Given a state (position and velocity), what are the joint velocities? * It's a bit weird to use "swerve" here. Maybe rename this SE2Model? */ - public JointVelocities inverse(ModelR3 swerveModel) { + public JointVelocities inverse(ModelSE2 swerveModel) { Pose2d p = swerveModel.pose(); VelocitySE2 v = swerveModel.velocity(); EAWConfig c = m_k.inverse(p); diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/PositionSubsystemR3.java b/lib/src/main/java/org/team100/lib/subsystems/r3/PositionSubsystemR3.java deleted file mode 100644 index 326b99b9..00000000 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/PositionSubsystemR3.java +++ /dev/null @@ -1,15 +0,0 @@ -package org.team100.lib.subsystems.r3; - -import org.team100.lib.state.ControlR3; - -/** - * A subsystem with three independent dimensions, controlled by position. - * - * We use this interface for mechanisms whose position can be controlled via - * outboard closed-loop controllers. - */ -public interface PositionSubsystemR3 extends SubsystemR3 { - - /** Position, velocity, and acceleration may all be used. */ - void set(ControlR3 setpoint); -} \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/subsystems/se2/PositionSubsystemSE2.java b/lib/src/main/java/org/team100/lib/subsystems/se2/PositionSubsystemSE2.java new file mode 100644 index 00000000..d3f0fa85 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/PositionSubsystemSE2.java @@ -0,0 +1,15 @@ +package org.team100.lib.subsystems.se2; + +import org.team100.lib.state.ControlSE2; + +/** + * A planar subsystem controlled by position. + * + * We use this interface for mechanisms whose position can be controlled via + * outboard closed-loop controllers. + */ +public interface PositionSubsystemSE2 extends SubsystemSE2 { + + /** Position, velocity, and acceleration may all be used. */ + void set(ControlSE2 setpoint); +} \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/README.md b/lib/src/main/java/org/team100/lib/subsystems/se2/README.md similarity index 100% rename from lib/src/main/java/org/team100/lib/subsystems/r3/README.md rename to lib/src/main/java/org/team100/lib/subsystems/se2/README.md diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/SubsystemR3.java b/lib/src/main/java/org/team100/lib/subsystems/se2/SubsystemSE2.java similarity index 52% rename from lib/src/main/java/org/team100/lib/subsystems/r3/SubsystemR3.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/SubsystemSE2.java index b0f4e5f3..9390fd9e 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/SubsystemR3.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/SubsystemSE2.java @@ -1,12 +1,13 @@ -package org.team100.lib.subsystems.r3; +package org.team100.lib.subsystems.se2; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import edu.wpi.first.wpilibj2.command.Subsystem; -public interface SubsystemR3 extends Subsystem { +/** A planar subsystem */ +public interface SubsystemSE2 extends Subsystem { /** State for the current Takt. */ - ModelR3 getState(); + ModelSE2 getState(); /** Passthrough to motor stop. This is not "hold position", it is "disable". */ void stop(); diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/VelocitySubsystemR3.java b/lib/src/main/java/org/team100/lib/subsystems/se2/VelocitySubsystemSE2.java similarity index 66% rename from lib/src/main/java/org/team100/lib/subsystems/r3/VelocitySubsystemR3.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/VelocitySubsystemSE2.java index 3de2390c..76a378d3 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/VelocitySubsystemR3.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/VelocitySubsystemSE2.java @@ -1,14 +1,14 @@ -package org.team100.lib.subsystems.r3; +package org.team100.lib.subsystems.se2; import org.team100.lib.geometry.VelocitySE2; /** - * A subsystem with three independent dimensions, controlled by velocity. + * A planar subsystem controlled by velocity. * * We use this interface for robot movement on the floor, where the three * dimensions are the dimensions of Pose2d: x, y, and theta. */ -public interface VelocitySubsystemR3 extends SubsystemR3 { +public interface VelocitySubsystemSE2 extends SubsystemSE2 { /** * No scaling or filtering. diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveStop.java b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/DriveStop.java similarity index 55% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveStop.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/DriveStop.java index bf35bca2..6b824d57 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveStop.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/DriveStop.java @@ -1,6 +1,6 @@ -package org.team100.lib.subsystems.r3.commands; +package org.team100.lib.subsystems.se2.commands; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; import edu.wpi.first.wpilibj2.command.Command; @@ -8,9 +8,9 @@ * Stop the drivetrain. */ public class DriveStop extends Command { - private final VelocitySubsystemR3 m_drive; + private final VelocitySubsystemSE2 m_drive; - public DriveStop(VelocitySubsystemR3 robotDrive) { + public DriveStop(VelocitySubsystemSE2 robotDrive) { m_drive = robotDrive; addRequirements(m_drive); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithProfile.java b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/DriveToPoseWithProfile.java similarity index 63% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithProfile.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/DriveToPoseWithProfile.java index 885d73d5..43962781 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithProfile.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/DriveToPoseWithProfile.java @@ -1,17 +1,17 @@ -package org.team100.lib.subsystems.r3.commands; +package org.team100.lib.subsystems.se2.commands; import java.util.function.Supplier; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.Pose2dLogger; -import org.team100.lib.profile.r3.ProfileR3; -import org.team100.lib.reference.r3.ProfileReferenceR3; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; -import org.team100.lib.subsystems.r3.commands.helper.VelocityReferenceControllerR3; +import org.team100.lib.profile.se2.ProfileSE2; +import org.team100.lib.reference.se2.ProfileReferenceSE2; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; +import org.team100.lib.subsystems.se2.commands.helper.VelocityReferenceControllerSE2; import edu.wpi.first.math.geometry.Pose2d; @@ -20,20 +20,20 @@ */ public class DriveToPoseWithProfile extends MoveAndHold { private final LoggerFactory m_log; - private final VelocitySubsystemR3 m_drive; - private final ControllerR3 m_controller; - private final ProfileR3 m_profile; + private final VelocitySubsystemSE2 m_drive; + private final ControllerSE2 m_controller; + private final ProfileSE2 m_profile; private final Pose2dLogger m_log_goal; private final Supplier m_goal; - private ProfileReferenceR3 m_reference; - private VelocityReferenceControllerR3 m_referenceController; + private ProfileReferenceSE2 m_reference; + private VelocityReferenceControllerSE2 m_referenceController; public DriveToPoseWithProfile( LoggerFactory logger, - VelocitySubsystemR3 drive, - ControllerR3 controller, - ProfileR3 profile, + VelocitySubsystemSE2 drive, + ControllerSE2 controller, + ProfileSE2 profile, Supplier goal) { m_log = logger.type(this); m_log_goal = m_log.pose2dLogger(Level.TRACE, "goal"); @@ -48,9 +48,9 @@ public DriveToPoseWithProfile( public void initialize() { Pose2d goal = m_goal.get(); m_log_goal.log(() -> goal); - m_reference = new ProfileReferenceR3(m_log, m_profile, "embark"); - m_reference.setGoal(new ModelR3(goal)); - m_referenceController = new VelocityReferenceControllerR3( + m_reference = new ProfileReferenceSE2(m_log, m_profile, "embark"); + m_reference.setGoal(new ModelSE2(goal)); + m_referenceController = new VelocityReferenceControllerSE2( m_log, m_drive, m_controller, m_reference); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToStateWithProfile.java b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/DriveToStateWithProfile.java similarity index 65% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToStateWithProfile.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/DriveToStateWithProfile.java index f4c7d11c..19cdb189 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToStateWithProfile.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/DriveToStateWithProfile.java @@ -1,17 +1,17 @@ -package org.team100.lib.subsystems.r3.commands; +package org.team100.lib.subsystems.se2.commands; import java.util.function.Supplier; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleArrayLogger; -import org.team100.lib.profile.r3.ProfileR3; -import org.team100.lib.reference.r3.ProfileReferenceR3; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; -import org.team100.lib.subsystems.r3.commands.helper.VelocityReferenceControllerR3; +import org.team100.lib.profile.se2.ProfileSE2; +import org.team100.lib.reference.se2.ProfileReferenceSE2; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; +import org.team100.lib.subsystems.se2.commands.helper.VelocityReferenceControllerSE2; /** * Drive to the supplied goal state using a profile. Allows the goal to change @@ -20,22 +20,22 @@ public class DriveToStateWithProfile extends MoveAndHold { private final LoggerFactory m_log; private final DoubleArrayLogger m_log_target; - private final Supplier m_goals; - private final VelocitySubsystemR3 m_drive; - private final ControllerR3 m_controller; - private final ProfileR3 m_profile; + private final Supplier m_goals; + private final VelocitySubsystemSE2 m_drive; + private final ControllerSE2 m_controller; + private final ProfileSE2 m_profile; - private ModelR3 m_goal; - private ProfileReferenceR3 m_reference; - private VelocityReferenceControllerR3 m_referenceController; + private ModelSE2 m_goal; + private ProfileReferenceSE2 m_reference; + private VelocityReferenceControllerSE2 m_referenceController; public DriveToStateWithProfile( LoggerFactory parent, LoggerFactory fieldLogger, - Supplier goal, - VelocitySubsystemR3 drive, - ControllerR3 controller, - ProfileR3 profile) { + Supplier goal, + VelocitySubsystemSE2 drive, + ControllerSE2 controller, + ProfileSE2 profile) { m_log = parent.type(this); m_log_target = fieldLogger.doubleArrayLogger(Level.TRACE, "target"); m_goals = goal; @@ -50,9 +50,9 @@ public void initialize() { m_goal = m_goals.get(); if (m_goal == null) return; - m_reference = new ProfileReferenceR3(m_log, m_profile, "Drive to pose with profile"); + m_reference = new ProfileReferenceSE2(m_log, m_profile, "Drive to pose with profile"); m_reference.setGoal(m_goal); - m_referenceController = new VelocityReferenceControllerR3( + m_referenceController = new VelocityReferenceControllerSE2( m_log, m_drive, m_controller, m_reference); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToTranslationFacingWithProfile.java b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/DriveToTranslationFacingWithProfile.java similarity index 68% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToTranslationFacingWithProfile.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/DriveToTranslationFacingWithProfile.java index d92f85de..2ecdea7c 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToTranslationFacingWithProfile.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/DriveToTranslationFacingWithProfile.java @@ -1,17 +1,17 @@ -package org.team100.lib.subsystems.r3.commands; +package org.team100.lib.subsystems.se2.commands; import java.util.function.Supplier; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.Pose2dLogger; -import org.team100.lib.profile.r3.ProfileR3; -import org.team100.lib.reference.r3.ProfileReferenceR3; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; -import org.team100.lib.subsystems.r3.commands.helper.VelocityReferenceControllerR3; +import org.team100.lib.profile.se2.ProfileSE2; +import org.team100.lib.reference.se2.ProfileReferenceSE2; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; +import org.team100.lib.subsystems.se2.commands.helper.VelocityReferenceControllerSE2; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -22,21 +22,21 @@ */ public class DriveToTranslationFacingWithProfile extends MoveAndHold { private final LoggerFactory m_log; - private final VelocitySubsystemR3 m_drive; - private final ControllerR3 m_controller; - private final ProfileR3 m_profile; + private final VelocitySubsystemSE2 m_drive; + private final ControllerSE2 m_controller; + private final ProfileSE2 m_profile; private final Pose2dLogger m_log_goal; private final Supplier m_goal; private final Rotation2d m_sideFacing; - private ProfileReferenceR3 m_reference; - private VelocityReferenceControllerR3 m_referenceController; + private ProfileReferenceSE2 m_reference; + private VelocityReferenceControllerSE2 m_referenceController; public DriveToTranslationFacingWithProfile( LoggerFactory parent, - VelocitySubsystemR3 drive, - ControllerR3 controller, - ProfileR3 profile, + VelocitySubsystemSE2 drive, + ControllerSE2 controller, + ProfileSE2 profile, Supplier goal, Rotation2d sideFacing) { m_log = parent.type(this); @@ -54,9 +54,9 @@ public void initialize() { Pose2d goal = getGoal(m_goal.get(), m_drive.getState().pose().getTranslation()); m_log_goal.log(() -> goal); - m_reference = new ProfileReferenceR3(m_log, m_profile, "embark"); - m_reference.setGoal(new ModelR3(goal)); - m_referenceController = new VelocityReferenceControllerR3( + m_reference = new ProfileReferenceSE2(m_log, m_profile, "embark"); + m_reference.setGoal(new ModelSE2(goal)); + m_referenceController = new VelocityReferenceControllerSE2( m_log, m_drive, m_controller, m_reference); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToTranslationWithRelativeBearing.java b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/DriveToTranslationWithRelativeBearing.java similarity index 79% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToTranslationWithRelativeBearing.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/DriveToTranslationWithRelativeBearing.java index ec4dc3f5..4d03fe82 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveToTranslationWithRelativeBearing.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/DriveToTranslationWithRelativeBearing.java @@ -1,18 +1,18 @@ -package org.team100.lib.subsystems.r3.commands; +package org.team100.lib.subsystems.se2.commands; import java.util.Optional; import java.util.function.Supplier; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleArrayLogger; -import org.team100.lib.profile.r3.ProfileR3; -import org.team100.lib.reference.r3.ProfileReferenceR3; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; -import org.team100.lib.subsystems.r3.commands.helper.VelocityReferenceControllerR3; +import org.team100.lib.profile.se2.ProfileSE2; +import org.team100.lib.reference.se2.ProfileReferenceSE2; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; +import org.team100.lib.subsystems.se2.commands.helper.VelocityReferenceControllerSE2; import org.team100.lib.targeting.TargetUtil; import edu.wpi.first.math.geometry.Pose2d; @@ -32,24 +32,24 @@ public class DriveToTranslationWithRelativeBearing extends MoveAndHold { private final LoggerFactory m_log; private final DoubleArrayLogger m_log_field_ball; private final Supplier> m_targets; - private final VelocitySubsystemR3 m_drive; - private final ControllerR3 m_controller; - private final ProfileR3 m_profile; + private final VelocitySubsystemSE2 m_drive; + private final ControllerSE2 m_controller; + private final ProfileSE2 m_profile; private final Rotation2d m_relativeBearing; private final Translation2d m_relativeTranslation; private Pose2d m_goal; private Translation2d m_translation; - private ProfileReferenceR3 m_reference; - private VelocityReferenceControllerR3 m_referenceController; + private ProfileReferenceSE2 m_reference; + private VelocityReferenceControllerSE2 m_referenceController; public DriveToTranslationWithRelativeBearing( LoggerFactory parent, LoggerFactory field, Supplier> targetes, - VelocitySubsystemR3 drive, - ControllerR3 controller, - ProfileR3 profile, + VelocitySubsystemSE2 drive, + ControllerSE2 controller, + ProfileSE2 profile, Rotation2d relativeBearing, Translation2d relativeTranslation) { m_log = parent.type(this); @@ -68,9 +68,9 @@ public void initialize() { updateGoal(); if (m_goal == null) return; - m_reference = new ProfileReferenceR3(m_log, m_profile, "DriveToTranslationWithRelativeBearing"); - m_reference.setGoal(new ModelR3(m_goal)); - m_referenceController = new VelocityReferenceControllerR3( + m_reference = new ProfileReferenceSE2(m_log, m_profile, "DriveToTranslationWithRelativeBearing"); + m_reference.setGoal(new ModelSE2(m_goal)); + m_referenceController = new VelocityReferenceControllerSE2( m_log, m_drive, m_controller, m_reference); } @@ -78,7 +78,7 @@ public void initialize() { public void execute() { if (m_goal == null || m_referenceController == null) return; - m_reference.setGoal(new ModelR3(m_goal)); + m_reference.setGoal(new ModelSE2(m_goal)); m_referenceController.execute(); m_log_field_ball.log(() -> new double[] { m_goal.getX(), diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveWithTrajectoryFunction.java b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/DriveWithTrajectoryFunction.java similarity index 77% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveWithTrajectoryFunction.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/DriveWithTrajectoryFunction.java index 82e4bb1e..ff963e7d 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/DriveWithTrajectoryFunction.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/DriveWithTrajectoryFunction.java @@ -1,16 +1,16 @@ -package org.team100.lib.subsystems.r3.commands; +package org.team100.lib.subsystems.se2.commands; import java.util.function.Function; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.BooleanLogger; import org.team100.lib.logging.LoggerFactory.DoubleLogger; -import org.team100.lib.reference.r3.TrajectoryReferenceR3; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; -import org.team100.lib.subsystems.r3.commands.helper.VelocityReferenceControllerR3; +import org.team100.lib.reference.se2.TrajectoryReferenceSE2; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; +import org.team100.lib.subsystems.se2.commands.helper.VelocityReferenceControllerSE2; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.visualization.TrajectoryVisualization; @@ -25,8 +25,8 @@ public class DriveWithTrajectoryFunction extends MoveAndHold { private final LoggerFactory m_log; private final BooleanLogger m_logDone; private final DoubleLogger m_logToGo; - private final VelocitySubsystemR3 m_drive; - private final ControllerR3 m_controller; + private final VelocitySubsystemSE2 m_drive; + private final ControllerSE2 m_controller; private final TrajectoryVisualization m_viz; private final Function m_trajectoryFn; @@ -34,12 +34,12 @@ public class DriveWithTrajectoryFunction extends MoveAndHold { * Non-null when the command is active (between initialize and end), null * otherwise. */ - private VelocityReferenceControllerR3 m_referenceController; + private VelocityReferenceControllerSE2 m_referenceController; public DriveWithTrajectoryFunction( LoggerFactory parent, - VelocitySubsystemR3 drive, - ControllerR3 controller, + VelocitySubsystemSE2 drive, + ControllerSE2 controller, TrajectoryVisualization viz, Function trajectoryFn) { m_log = parent.type(this); @@ -56,8 +56,8 @@ public DriveWithTrajectoryFunction( public void initialize() { Trajectory100 trajectory = m_trajectoryFn.apply(m_drive.getState().pose()); m_viz.setViz(trajectory); - TrajectoryReferenceR3 reference = new TrajectoryReferenceR3(m_log, trajectory); - m_referenceController = new VelocityReferenceControllerR3( + TrajectoryReferenceSE2 reference = new TrajectoryReferenceSE2(m_log, trajectory); + m_referenceController = new VelocityReferenceControllerSE2( m_log, m_drive, m_controller, reference); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/FloorPickSequence.java b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/FloorPickSequence.java similarity index 85% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/FloorPickSequence.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/FloorPickSequence.java index 85ef03be..d5a27453 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/FloorPickSequence.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/FloorPickSequence.java @@ -1,14 +1,14 @@ -package org.team100.lib.subsystems.r3.commands; +package org.team100.lib.subsystems.se2.commands; import static edu.wpi.first.wpilibj2.command.Commands.sequence; import java.util.Optional; import java.util.function.Supplier; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.profile.r3.ProfileR3; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; +import org.team100.lib.profile.se2.ProfileSE2; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; import org.team100.lib.targeting.Targets; import edu.wpi.first.math.geometry.Rotation2d; @@ -28,10 +28,10 @@ public class FloorPickSequence { public static Command get( LoggerFactory parent, LoggerFactory fieldLog, - VelocitySubsystemR3 drive, + VelocitySubsystemSE2 drive, Targets targets, - ControllerR3 controller, - ProfileR3 profile) { + ControllerSE2 controller, + ProfileSE2 profile) { LoggerFactory log = parent.type(FloorPickSequence.class); Supplier> target = targets::getClosestTarget; Supplier> runway = () -> { diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/FloorPickSequence2.java b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/FloorPickSequence2.java similarity index 78% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/FloorPickSequence2.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/FloorPickSequence2.java index 59c5f460..682d50f7 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/FloorPickSequence2.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/FloorPickSequence2.java @@ -1,11 +1,11 @@ -package org.team100.lib.subsystems.r3.commands; +package org.team100.lib.subsystems.se2.commands; import static edu.wpi.first.wpilibj2.command.Commands.sequence; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.profile.r3.ProfileR3; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; +import org.team100.lib.profile.se2.ProfileSE2; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; import org.team100.lib.targeting.Targets; import edu.wpi.first.math.geometry.Rotation2d; @@ -26,10 +26,10 @@ public class FloorPickSequence2 { public static Command get( LoggerFactory parent, LoggerFactory fieldLog, - VelocitySubsystemR3 drive, + VelocitySubsystemSE2 drive, Targets targets, - ControllerR3 controller, - ProfileR3 profile) { + ControllerSE2 controller, + ProfileSE2 profile) { LoggerFactory log = parent.type(new FloorPickSequence()); Pushbroom toRunway = new Pushbroom( log, fieldLog, targets::getClosestTarget, drive, diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/FollowTrajectoryPosition.java b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/FollowTrajectoryPosition.java similarity index 61% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/FollowTrajectoryPosition.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/FollowTrajectoryPosition.java index eb912c5b..20f9dbc6 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/FollowTrajectoryPosition.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/FollowTrajectoryPosition.java @@ -1,23 +1,23 @@ -package org.team100.lib.subsystems.r3.commands; +package org.team100.lib.subsystems.se2.commands; import org.team100.lib.commands.MoveAndHold; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.reference.r3.TrajectoryReferenceR3; -import org.team100.lib.subsystems.r3.PositionSubsystemR3; -import org.team100.lib.subsystems.r3.commands.helper.PositionReferenceControllerR3; +import org.team100.lib.reference.se2.TrajectoryReferenceSE2; +import org.team100.lib.subsystems.se2.PositionSubsystemSE2; +import org.team100.lib.subsystems.se2.commands.helper.PositionReferenceControllerSE2; import org.team100.lib.trajectory.Trajectory100; -/** Analogous to DriveWithTrajectory, but for R3 positional control. */ +/** Analogous to DriveWithTrajectory, but for SE2 positional control. */ public class FollowTrajectoryPosition extends MoveAndHold { private final LoggerFactory m_log; - private final PositionSubsystemR3 m_subsystem; + private final PositionSubsystemSE2 m_subsystem; private final Trajectory100 m_trajectory; - private PositionReferenceControllerR3 m_referenceController; + private PositionReferenceControllerSE2 m_referenceController; public FollowTrajectoryPosition( LoggerFactory parent, - PositionSubsystemR3 subsystem, + PositionSubsystemSE2 subsystem, Trajectory100 trajectory) { m_log = parent.type(this); m_subsystem = subsystem; @@ -27,8 +27,8 @@ public FollowTrajectoryPosition( @Override public void initialize() { - m_referenceController = new PositionReferenceControllerR3( - m_log, m_subsystem, new TrajectoryReferenceR3(m_log, m_trajectory)); + m_referenceController = new PositionReferenceControllerSE2( + m_log, m_subsystem, new TrajectoryReferenceSE2(m_log, m_trajectory)); } @Override diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/GoToPosePosition.java similarity index 77% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/GoToPosePosition.java index 0b78d5d7..a3ed6afb 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/GoToPosePosition.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/GoToPosePosition.java @@ -1,4 +1,4 @@ -package org.team100.lib.subsystems.r3.commands; +package org.team100.lib.subsystems.se2.commands; import java.util.List; @@ -6,9 +6,9 @@ import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.reference.r3.TrajectoryReferenceR3; -import org.team100.lib.subsystems.r3.PositionSubsystemR3; -import org.team100.lib.subsystems.r3.commands.helper.PositionReferenceControllerR3; +import org.team100.lib.reference.se2.TrajectoryReferenceSE2; +import org.team100.lib.subsystems.se2.PositionSubsystemSE2; +import org.team100.lib.subsystems.se2.commands.helper.PositionReferenceControllerSE2; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.TrajectoryPlanner; @@ -20,16 +20,16 @@ */ public class GoToPosePosition extends MoveAndHold { private final LoggerFactory m_log; - private final PositionSubsystemR3 m_subsystem; + private final PositionSubsystemSE2 m_subsystem; private final WaypointSE2 m_goal; private final Rotation2d m_course; private final TrajectoryPlanner m_trajectoryPlanner; - private PositionReferenceControllerR3 m_referenceController; + private PositionReferenceControllerSE2 m_referenceController; public GoToPosePosition( LoggerFactory parent, - PositionSubsystemR3 subsystem, + PositionSubsystemSE2 subsystem, Rotation2d course, WaypointSE2 goal, TrajectoryPlanner trajectoryPlanner) { @@ -48,8 +48,8 @@ public void initialize() { DirectionSE2.irrotational(m_course), 1); Trajectory100 m_trajectory = m_trajectoryPlanner.restToRest( List.of(m_currentPose, m_goal)); - m_referenceController = new PositionReferenceControllerR3( - m_log, m_subsystem, new TrajectoryReferenceR3(m_log, m_trajectory)); + m_referenceController = new PositionReferenceControllerSE2( + m_log, m_subsystem, new TrajectoryReferenceSE2(m_log, m_trajectory)); } @Override diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/HoldPosition.java b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/HoldPosition.java similarity index 64% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/HoldPosition.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/HoldPosition.java index ecdabaf4..03012d88 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/HoldPosition.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/HoldPosition.java @@ -1,19 +1,19 @@ -package org.team100.lib.subsystems.r3.commands; +package org.team100.lib.subsystems.se2.commands; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.r3.PositionSubsystemR3; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.se2.PositionSubsystemSE2; /** * Record the current pose at initialization time and hold that pose, * motionless, forever. */ public class HoldPosition extends MoveAndHold { - private final PositionSubsystemR3 m_subsystem; + private final PositionSubsystemSE2 m_subsystem; - private ModelR3 m_state; + private ModelSE2 m_state; - public HoldPosition(PositionSubsystemR3 subsystem) { + public HoldPosition(PositionSubsystemSE2 subsystem) { m_subsystem = subsystem; addRequirements(subsystem); } @@ -21,7 +21,7 @@ public HoldPosition(PositionSubsystemR3 subsystem) { @Override public void initialize() { // motionless at the current location - m_state = new ModelR3(m_subsystem.getState().pose()); + m_state = new ModelSE2(m_subsystem.getState().pose()); } @Override diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/ManualPosition.java b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/ManualPosition.java similarity index 83% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/ManualPosition.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/ManualPosition.java index 261abe7a..e98235a5 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/ManualPosition.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/ManualPosition.java @@ -1,11 +1,11 @@ -package org.team100.lib.subsystems.r3.commands; +package org.team100.lib.subsystems.se2.commands; import java.util.function.Supplier; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.hid.Velocity; -import org.team100.lib.state.ControlR3; -import org.team100.lib.subsystems.r3.PositionSubsystemR3; +import org.team100.lib.state.ControlSE2; +import org.team100.lib.subsystems.se2.PositionSubsystemSE2; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -19,13 +19,13 @@ public class ManualPosition extends Command { private static final boolean DEBUG = false; private final Supplier m_input; - private final PositionSubsystemR3 m_subsystem; + private final PositionSubsystemSE2 m_subsystem; private Pose2d m_pose; public ManualPosition( Supplier input, - PositionSubsystemR3 subsystem) { + PositionSubsystemSE2 subsystem) { m_input = input; m_subsystem = subsystem; addRequirements(subsystem); @@ -54,7 +54,7 @@ public void execute() { Rotation2d r2 = m_pose.getRotation().plus(new Rotation2d(jv.theta() * dt)); m_pose = new Pose2d(x2, y2, r2); // our new goal point - m_subsystem.set(new ControlR3(m_pose)); + m_subsystem.set(new ControlSE2(m_pose)); if (DEBUG) { System.out.printf("pose %s\n", m_pose); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/Pushbroom.java b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/Pushbroom.java similarity index 85% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/Pushbroom.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/Pushbroom.java index f5f7f409..77d1a86a 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/Pushbroom.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/Pushbroom.java @@ -1,18 +1,18 @@ -package org.team100.lib.subsystems.r3.commands; +package org.team100.lib.subsystems.se2.commands; import java.util.Optional; import java.util.function.Supplier; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleArrayLogger; -import org.team100.lib.profile.r3.ProfileR3; -import org.team100.lib.reference.r3.ProfileReferenceR3; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; -import org.team100.lib.subsystems.r3.commands.helper.VelocityReferenceControllerR3; +import org.team100.lib.profile.se2.ProfileSE2; +import org.team100.lib.reference.se2.ProfileReferenceSE2; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; +import org.team100.lib.subsystems.se2.commands.helper.VelocityReferenceControllerSE2; import org.team100.lib.targeting.TargetUtil; import edu.wpi.first.math.geometry.Pose2d; @@ -50,24 +50,24 @@ public class Pushbroom extends MoveAndHold { private final DoubleArrayLogger m_log_field_goal; private final DoubleArrayLogger m_log_field_target; private final Supplier> m_target; - private final VelocitySubsystemR3 m_drive; - private final ControllerR3 m_controller; - private final ProfileR3 m_profile; + private final VelocitySubsystemSE2 m_drive; + private final ControllerSE2 m_controller; + private final ProfileSE2 m_profile; /** End effector relative to robot */ private final Transform2d m_endEffector; private Translation2d target; private Pose2d m_goal; - private ProfileReferenceR3 m_reference; - private VelocityReferenceControllerR3 m_referenceController; + private ProfileReferenceSE2 m_reference; + private VelocityReferenceControllerSE2 m_referenceController; public Pushbroom( LoggerFactory parent, LoggerFactory field, Supplier> target, - VelocitySubsystemR3 drive, - ControllerR3 controller, - ProfileR3 profile, + VelocitySubsystemSE2 drive, + ControllerSE2 controller, + ProfileSE2 profile, Transform2d endEffector) { m_log = parent.type(this); m_log_field_goal = field.doubleArrayLogger(Level.COMP, "goal"); @@ -86,9 +86,9 @@ public void initialize() { updateGoal(); if (m_goal == null) return; - m_reference = new ProfileReferenceR3(m_log, m_profile, "DriveToTranslationWithRelativeBearing"); - m_reference.setGoal(new ModelR3(m_goal)); - m_referenceController = new VelocityReferenceControllerR3( + m_reference = new ProfileReferenceSE2(m_log, m_profile, "DriveToTranslationWithRelativeBearing"); + m_reference.setGoal(new ModelSE2(m_goal)); + m_referenceController = new VelocityReferenceControllerSE2( m_log, m_drive, m_controller, m_reference); } @@ -100,7 +100,7 @@ public void execute() { updateGoal(); if (m_goal == null || m_referenceController == null) return; - m_reference.setGoal(new ModelR3(m_goal)); + m_reference.setGoal(new ModelSE2(m_goal)); m_referenceController.execute(); m_log_field_goal.log(() -> new double[] { m_goal.getX(), diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/README.md b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/README.md similarity index 100% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/README.md rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/README.md diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/VelocityFeedforwardOnly.java b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/VelocityFeedforwardOnly.java similarity index 66% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/VelocityFeedforwardOnly.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/VelocityFeedforwardOnly.java index 92a9d552..f7e618ff 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/VelocityFeedforwardOnly.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/VelocityFeedforwardOnly.java @@ -1,12 +1,12 @@ -package org.team100.lib.subsystems.r3.commands; +package org.team100.lib.subsystems.se2.commands; import org.team100.lib.commands.MoveAndHold; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.profile.r3.ProfileR3; -import org.team100.lib.reference.r3.ProfileReferenceR3; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; +import org.team100.lib.profile.se2.ProfileSE2; +import org.team100.lib.reference.se2.ProfileReferenceSE2; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; import edu.wpi.first.math.geometry.Pose2d; @@ -19,17 +19,17 @@ public class VelocityFeedforwardOnly extends MoveAndHold { private static final boolean DEBUG = false; private final LoggerFactory m_log; - private final ProfileR3 m_profile; + private final ProfileSE2 m_profile; private final Pose2d m_goal; - private final VelocitySubsystemR3 m_drive; + private final VelocitySubsystemSE2 m_drive; - private ProfileReferenceR3 m_reference; + private ProfileReferenceSE2 m_reference; public VelocityFeedforwardOnly( LoggerFactory parent, - ProfileR3 profile, + ProfileSE2 profile, Pose2d goal, - VelocitySubsystemR3 drive) { + VelocitySubsystemSE2 drive) { m_log = parent.type(this); m_profile = profile; m_goal = goal; @@ -39,8 +39,8 @@ public VelocityFeedforwardOnly( @Override public void initialize() { - m_reference = new ProfileReferenceR3(m_log, m_profile, "feedforward only"); - m_reference.setGoal(new ModelR3(m_goal)); + m_reference = new ProfileReferenceSE2(m_log, m_profile, "feedforward only"); + m_reference.setGoal(new ModelSE2(m_goal)); m_reference.initialize(m_drive.getState()); } @@ -65,8 +65,8 @@ public boolean isDone() { @Override public double toGo() { - ModelR3 goal = m_reference.goal(); - ModelR3 measurement = m_drive.getState(); + ModelSE2 goal = m_reference.goal(); + ModelSE2 measurement = m_drive.getState(); return goal.minus(measurement).translation().getNorm(); } } diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/PositionReferenceControllerR3.java b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/helper/PositionReferenceControllerSE2.java similarity index 53% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/PositionReferenceControllerR3.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/helper/PositionReferenceControllerSE2.java index 3259b0d7..b48a58b2 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/PositionReferenceControllerR3.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/helper/PositionReferenceControllerSE2.java @@ -1,12 +1,12 @@ -package org.team100.lib.subsystems.r3.commands.helper; +package org.team100.lib.subsystems.se2.commands.helper; -import org.team100.lib.controller.r3.ControllerR3; -import org.team100.lib.controller.r3.NullControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; +import org.team100.lib.controller.se2.NullControllerSE2; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.reference.r3.ReferenceR3; -import org.team100.lib.state.ControlR3; -import org.team100.lib.subsystems.r3.PositionSubsystemR3; +import org.team100.lib.reference.se2.ReferenceSE2; +import org.team100.lib.state.ControlSE2; +import org.team100.lib.subsystems.se2.PositionSubsystemSE2; /** * Actuates a positional subsystem based on a reference. @@ -17,16 +17,16 @@ * The lifespan of this object is intended to be a single "playback" of a * trajectory, so create it in Command.initialize(). */ -public class PositionReferenceControllerR3 extends ReferenceControllerR3Base { - private final PositionSubsystemR3 m_subsystem; +public class PositionReferenceControllerSE2 extends ReferenceControllerSE2Base { + private final PositionSubsystemSE2 m_subsystem; /** * Call this from Command.initialize(). */ - public PositionReferenceControllerR3( + public PositionReferenceControllerSE2( LoggerFactory parent, - PositionSubsystemR3 subsystem, - ReferenceR3 reference) { + PositionSubsystemSE2 subsystem, + ReferenceSE2 reference) { super(parent, subsystem, nullController(parent), reference); m_subsystem = subsystem; } @@ -36,15 +36,15 @@ public PositionReferenceControllerR3( * @param u ignored, since this uses outboard feedback only. */ @Override - void execute100(ControlR3 next, VelocitySE2 u) { + void execute100(ControlSE2 next, VelocitySE2 u) { m_subsystem.set(next); } /** * Doesn't control anything, just keeps track of atReference() */ - private static ControllerR3 nullController(LoggerFactory parent) { - return new NullControllerR3(parent, 1, 1, 1, 1); + private static ControllerSE2 nullController(LoggerFactory parent) { + return new NullControllerSE2(parent, 1, 1, 1, 1); } } diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/README.md b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/helper/README.md similarity index 100% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/README.md rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/helper/README.md diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/ReferenceControllerR3Base.java b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/helper/ReferenceControllerSE2Base.java similarity index 60% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/ReferenceControllerR3Base.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/helper/ReferenceControllerSE2Base.java index 2a450f88..0705daa7 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/ReferenceControllerR3Base.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/helper/ReferenceControllerSE2Base.java @@ -1,47 +1,47 @@ -package org.team100.lib.subsystems.r3.commands.helper; +package org.team100.lib.subsystems.se2.commands.helper; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.BooleanLogger; -import org.team100.lib.logging.LoggerFactory.ControlR3Logger; +import org.team100.lib.logging.LoggerFactory.ControlSE2Logger; import org.team100.lib.logging.LoggerFactory.DoubleLogger; -import org.team100.lib.logging.LoggerFactory.ModelR3Logger; -import org.team100.lib.reference.r3.ReferenceR3; -import org.team100.lib.state.ControlR3; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.r3.SubsystemR3; +import org.team100.lib.logging.LoggerFactory.ModelSE2Logger; +import org.team100.lib.reference.se2.ReferenceSE2; +import org.team100.lib.state.ControlSE2; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.se2.SubsystemSE2; -public abstract class ReferenceControllerR3Base { +public abstract class ReferenceControllerSE2Base { private final BooleanLogger m_logDone; private final DoubleLogger m_logToGo; - private final SubsystemR3 m_subsystem; - private final ControllerR3 m_controller; - private final ReferenceR3 m_reference; - private final ModelR3Logger m_log_measurement; - private final ModelR3Logger m_log_current; - private final ControlR3Logger m_log_next; - private final ModelR3Logger m_log_error; + private final SubsystemSE2 m_subsystem; + private final ControllerSE2 m_controller; + private final ReferenceSE2 m_reference; + private final ModelSE2Logger m_log_measurement; + private final ModelSE2Logger m_log_current; + private final ControlSE2Logger m_log_next; + private final ModelSE2Logger m_log_error; /** * Initializes the reference with the current measurement. */ - ReferenceControllerR3Base( + ReferenceControllerSE2Base( LoggerFactory parent, - SubsystemR3 subsystem, - ControllerR3 controller, - ReferenceR3 reference) { + SubsystemSE2 subsystem, + ControllerSE2 controller, + ReferenceSE2 reference) { LoggerFactory log = parent.type(this); m_logDone = log.booleanLogger(Level.TRACE, "done"); m_logToGo = log.doubleLogger(Level.TRACE, "to go"); m_subsystem = subsystem; m_controller = controller; m_reference = reference; - m_log_measurement = log.modelR3Logger(Level.TRACE, "measurement"); - m_log_current = log.modelR3Logger(Level.TRACE, "current"); - m_log_next = log.controlR3Logger(Level.TRACE, "next"); - m_log_error = log.modelR3Logger(Level.TRACE, "error"); + m_log_measurement = log.modelSE2Logger(Level.TRACE, "measurement"); + m_log_current = log.modelSE2Logger(Level.TRACE, "current"); + m_log_next = log.controlSE2Logger(Level.TRACE, "next"); + m_log_error = log.modelSE2Logger(Level.TRACE, "error"); // initialize here so that the "done" state knows about the clock m_reference.initialize(subsystem.getState()); } @@ -52,17 +52,17 @@ public abstract class ReferenceControllerR3Base { * @param next The next control setpoint. * @param u The controller output for the next dt */ - abstract void execute100(ControlR3 next, VelocitySE2 u); + abstract void execute100(ControlSE2 next, VelocitySE2 u); /** * This should be called in Command.execute(). */ public void execute() { try { - ModelR3 measurement = m_subsystem.getState(); - ModelR3 current = m_reference.current(); - ControlR3 next = m_reference.next(); - ModelR3 error = current.minus(measurement); + ModelSE2 measurement = m_subsystem.getState(); + ModelSE2 current = m_reference.current(); + ControlSE2 next = m_reference.next(); + ModelSE2 error = current.minus(measurement); // u represents the time from now until now+dt, so it's also // what the mechanism should be doing at the next time step VelocitySE2 u = m_controller.calculate(measurement, current, next); @@ -97,8 +97,8 @@ public boolean isDone() { * running, e.g. "start doing X if Y is near the goal." */ public double toGo() { - ModelR3 goal = m_reference.goal(); - ModelR3 measurement = m_subsystem.getState(); + ModelSE2 goal = m_reference.goal(); + ModelSE2 measurement = m_subsystem.getState(); double togo = goal.minus(measurement).translation().getNorm(); m_logToGo.log(() -> togo); return togo; diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/VelocityReferenceControllerR3.java b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/helper/VelocityReferenceControllerSE2.java similarity index 56% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/VelocityReferenceControllerR3.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/helper/VelocityReferenceControllerSE2.java index a98cbd40..364a9873 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/VelocityReferenceControllerR3.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/helper/VelocityReferenceControllerSE2.java @@ -1,11 +1,11 @@ -package org.team100.lib.subsystems.r3.commands.helper; +package org.team100.lib.subsystems.se2.commands.helper; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.reference.r3.ReferenceR3; -import org.team100.lib.state.ControlR3; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; +import org.team100.lib.reference.se2.ReferenceSE2; +import org.team100.lib.state.ControlSE2; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; /** * Actuates a velocity subsystem based on a reference. @@ -13,17 +13,17 @@ * The lifespan of this object is intended to be a single "playback" of a * trajectory, so create it in Command.initialize(). */ -public class VelocityReferenceControllerR3 extends ReferenceControllerR3Base { - private final VelocitySubsystemR3 m_subsystem; +public class VelocityReferenceControllerSE2 extends ReferenceControllerSE2Base { + private final VelocitySubsystemSE2 m_subsystem; /** * Call this from Command.initialize(). */ - public VelocityReferenceControllerR3( + public VelocityReferenceControllerSE2( LoggerFactory parent, - VelocitySubsystemR3 subsystem, - ControllerR3 controller, - ReferenceR3 reference) { + VelocitySubsystemSE2 subsystem, + ControllerSE2 controller, + ReferenceSE2 reference) { super(parent, subsystem, controller, reference); m_subsystem = subsystem; } @@ -35,7 +35,7 @@ public VelocityReferenceControllerR3( * velocity to this */ @Override - void execute100(ControlR3 next, VelocitySE2 u) { + void execute100(ControlSE2 next, VelocitySE2 u) { m_subsystem.setVelocity(u); } } diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/test/DriveToStateSimple.java b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/test/DriveToStateSimple.java similarity index 59% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/test/DriveToStateSimple.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/test/DriveToStateSimple.java index 03f73e06..35fbbd24 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/test/DriveToStateSimple.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/test/DriveToStateSimple.java @@ -1,13 +1,13 @@ -package org.team100.lib.subsystems.r3.commands.test; +package org.team100.lib.subsystems.se2.commands.test; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.reference.r3.ConstantReferenceR3; -import org.team100.lib.reference.r3.ReferenceR3; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; -import org.team100.lib.subsystems.r3.commands.helper.VelocityReferenceControllerR3; +import org.team100.lib.reference.se2.ConstantReferenceSE2; +import org.team100.lib.reference.se2.ReferenceSE2; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; +import org.team100.lib.subsystems.se2.commands.helper.VelocityReferenceControllerSE2; /** * Given a swerve state, drive there using only the feedback in the holonomic @@ -21,27 +21,27 @@ */ public class DriveToStateSimple extends MoveAndHold { private final LoggerFactory m_log; - private final ControllerR3 m_controller; - private final VelocitySubsystemR3 m_drive; - private final ReferenceR3 m_reference; + private final ControllerSE2 m_controller; + private final VelocitySubsystemSE2 m_drive; + private final ReferenceSE2 m_reference; - private VelocityReferenceControllerR3 m_referenceController; + private VelocityReferenceControllerSE2 m_referenceController; public DriveToStateSimple( LoggerFactory parent, - ControllerR3 controller, - VelocitySubsystemR3 drive, - ModelR3 goal) { + ControllerSE2 controller, + VelocitySubsystemSE2 drive, + ModelSE2 goal) { m_log = parent.type(this); m_controller = controller; m_drive = drive; - m_reference = new ConstantReferenceR3(goal); + m_reference = new ConstantReferenceSE2(goal); addRequirements(drive); } @Override public void initialize() { - m_referenceController = new VelocityReferenceControllerR3( + m_referenceController = new VelocityReferenceControllerSE2( m_log, m_drive, m_controller, m_reference); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectory.java b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectory.java similarity index 68% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectory.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectory.java index 01d73b62..474fdc48 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectory.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectory.java @@ -1,11 +1,11 @@ -package org.team100.lib.subsystems.r3.commands.test; +package org.team100.lib.subsystems.se2.commands.test; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.reference.r3.TrajectoryReferenceR3; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; -import org.team100.lib.subsystems.r3.commands.helper.VelocityReferenceControllerR3; +import org.team100.lib.reference.se2.TrajectoryReferenceSE2; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; +import org.team100.lib.subsystems.se2.commands.helper.VelocityReferenceControllerSE2; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.visualization.TrajectoryVisualization; @@ -19,17 +19,17 @@ */ public class DriveWithTrajectory extends MoveAndHold { private final LoggerFactory m_log; - private final VelocitySubsystemR3 m_drive; - private final ControllerR3 m_controller; + private final VelocitySubsystemSE2 m_drive; + private final ControllerSE2 m_controller; private final Trajectory100 m_trajectory; private final TrajectoryVisualization m_viz; - private VelocityReferenceControllerR3 m_referenceController; + private VelocityReferenceControllerSE2 m_referenceController; public DriveWithTrajectory( LoggerFactory parent, - VelocitySubsystemR3 drive, - ControllerR3 controller, + VelocitySubsystemSE2 drive, + ControllerSE2 controller, Trajectory100 trajectory, TrajectoryVisualization viz) { m_log = parent.type(this); @@ -42,8 +42,8 @@ public DriveWithTrajectory( @Override public void initialize() { - TrajectoryReferenceR3 reference = new TrajectoryReferenceR3(m_log, m_trajectory); - m_referenceController = new VelocityReferenceControllerR3( + TrajectoryReferenceSE2 reference = new TrajectoryReferenceSE2(m_log, m_trajectory); + m_referenceController = new VelocityReferenceControllerSE2( m_log, m_drive, m_controller, reference); m_viz.setViz(m_trajectory); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectoryListFunction.java b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryListFunction.java similarity index 79% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectoryListFunction.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryListFunction.java index 2bf63d3c..003714a7 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectoryListFunction.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryListFunction.java @@ -1,15 +1,15 @@ -package org.team100.lib.subsystems.r3.commands.test; +package org.team100.lib.subsystems.se2.commands.test; import java.util.Iterator; import java.util.List; import java.util.function.Function; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.reference.r3.TrajectoryReferenceR3; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; -import org.team100.lib.subsystems.r3.commands.helper.VelocityReferenceControllerR3; +import org.team100.lib.reference.se2.TrajectoryReferenceSE2; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; +import org.team100.lib.subsystems.se2.commands.helper.VelocityReferenceControllerSE2; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.visualization.TrajectoryVisualization; @@ -26,18 +26,18 @@ */ public class DriveWithTrajectoryListFunction extends MoveAndHold { private final LoggerFactory m_log; - private final VelocitySubsystemR3 m_drive; - private final ControllerR3 m_controller; + private final VelocitySubsystemSE2 m_drive; + private final ControllerSE2 m_controller; private final Function> m_trajectories; private final TrajectoryVisualization m_viz; private Iterator m_trajectoryIter; - private VelocityReferenceControllerR3 m_referenceController; + private VelocityReferenceControllerSE2 m_referenceController; public DriveWithTrajectoryListFunction( LoggerFactory parent, - VelocitySubsystemR3 swerve, - ControllerR3 controller, + VelocitySubsystemSE2 swerve, + ControllerSE2 controller, Function> trajectories, TrajectoryVisualization viz) { m_log = parent.type(this); @@ -60,8 +60,8 @@ public void execute() { // get the next trajectory if (m_trajectoryIter.hasNext()) { Trajectory100 m_trajectory = m_trajectoryIter.next(); - TrajectoryReferenceR3 reference = new TrajectoryReferenceR3(m_log, m_trajectory); - m_referenceController = new VelocityReferenceControllerR3( + TrajectoryReferenceSE2 reference = new TrajectoryReferenceSE2(m_log, m_trajectory); + m_referenceController = new VelocityReferenceControllerSE2( m_log, m_drive, m_controller, reference); m_viz.setViz(m_trajectory); } else { diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/test/PermissiveTrajectoryListCommand.java b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/test/PermissiveTrajectoryListCommand.java similarity index 78% rename from lib/src/main/java/org/team100/lib/subsystems/r3/commands/test/PermissiveTrajectoryListCommand.java rename to lib/src/main/java/org/team100/lib/subsystems/se2/commands/test/PermissiveTrajectoryListCommand.java index 8e8f4344..a81f6eda 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/test/PermissiveTrajectoryListCommand.java +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/test/PermissiveTrajectoryListCommand.java @@ -1,15 +1,15 @@ -package org.team100.lib.subsystems.r3.commands.test; +package org.team100.lib.subsystems.se2.commands.test; import java.util.Iterator; import java.util.List; import java.util.function.Function; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.reference.r3.TrajectoryReferenceR3; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; -import org.team100.lib.subsystems.r3.commands.helper.VelocityReferenceControllerR3; +import org.team100.lib.reference.se2.TrajectoryReferenceSE2; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; +import org.team100.lib.subsystems.se2.commands.helper.VelocityReferenceControllerSE2; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.visualization.TrajectoryVisualization; @@ -22,18 +22,18 @@ */ public class PermissiveTrajectoryListCommand extends MoveAndHold { private final LoggerFactory m_log; - private final VelocitySubsystemR3 m_drive; - private final ControllerR3 m_controller; + private final VelocitySubsystemSE2 m_drive; + private final ControllerSE2 m_controller; private final List> m_trajectories; private final TrajectoryVisualization m_viz; private Iterator> m_trajectoryIter; - private VelocityReferenceControllerR3 m_referenceController; + private VelocityReferenceControllerSE2 m_referenceController; public PermissiveTrajectoryListCommand( LoggerFactory parent, - VelocitySubsystemR3 swerve, - ControllerR3 controller, + VelocitySubsystemSE2 swerve, + ControllerSE2 controller, List> trajectories, TrajectoryVisualization viz) { m_log = parent.type(this); @@ -56,8 +56,8 @@ public void execute() { // get the next trajectory if (m_trajectoryIter.hasNext()) { Trajectory100 trajectory = m_trajectoryIter.next().apply(m_drive.getState().pose()); - TrajectoryReferenceR3 reference = new TrajectoryReferenceR3(m_log, trajectory); - m_referenceController = new VelocityReferenceControllerR3( + TrajectoryReferenceSE2 reference = new TrajectoryReferenceSE2(m_log, trajectory); + m_referenceController = new VelocityReferenceControllerSE2( m_log, m_drive, m_controller, reference); m_viz.setViz(trajectory); } else { diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/README.md b/lib/src/main/java/org/team100/lib/subsystems/swerve/README.md index c8bd6e34..3da336ec 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/README.md +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/README.md @@ -14,4 +14,4 @@ The subsystem has two layers: * `SwerveLocal` is only aware of robot-relative control, and performs inverse kinematics to execute it in SE(2), using discretization. -* `SwerveDriveSubsystem` is aware of field-relative continuous control in R3. +* `SwerveDriveSubsystem` is aware of field-relative continuous control in SE(2). diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveDriveSubsystem.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveDriveSubsystem.java index 4a970f7e..04ac98a1 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveDriveSubsystem.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveDriveSubsystem.java @@ -16,11 +16,11 @@ import org.team100.lib.logging.LoggerFactory.DoubleLogger; import org.team100.lib.logging.LoggerFactory.EnumLogger; import org.team100.lib.logging.LoggerFactory.VelocitySE2Logger; -import org.team100.lib.logging.LoggerFactory.ModelR3Logger; +import org.team100.lib.logging.LoggerFactory.ModelSE2Logger; import org.team100.lib.music.Music; import org.team100.lib.music.Player; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.subsystems.swerve.kinodynamics.limiter.SwerveLimiter; import org.team100.lib.subsystems.swerve.module.state.SwerveModulePositions; @@ -32,7 +32,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class SwerveDriveSubsystem extends SubsystemBase implements VelocitySubsystemR3, Music { +public class SwerveDriveSubsystem extends SubsystemBase implements VelocitySubsystemSE2, Music { // DEBUG produces a LOT of output, you should only enable it while you're // looking at it. private static final boolean DEBUG = false; @@ -42,10 +42,10 @@ public class SwerveDriveSubsystem extends SubsystemBase implements VelocitySubsy private final SwerveLimiter m_limiter; // CACHES - private final ObjectCache m_stateCache; + private final ObjectCache m_stateCache; // LOGGERS - private final ModelR3Logger m_log_state; + private final ModelSE2Logger m_log_state; private final DoubleLogger m_log_turning; private final DoubleArrayLogger m_log_pose_array; private final EnumLogger m_log_skill; @@ -66,7 +66,7 @@ public SwerveDriveSubsystem( m_limiter = limiter; m_stateCache = Cache.of(this::update); stop(); - m_log_state = log.modelR3Logger(Level.COMP, "state"); + m_log_state = log.modelSE2Logger(Level.COMP, "state"); m_log_turning = log.doubleLogger(Level.TRACE, "Tur Deg"); m_log_pose_array = log.doubleArrayLogger(Level.COMP, "pose array"); m_log_skill = log.enumLogger(Level.TRACE, "skill level"); @@ -92,9 +92,9 @@ public void setVelocity(VelocitySE2 nextV) { // Actuation is constant for the whole control period, which means // that to calculate robot-relative speed from field-relative speed, // we need to use the robot rotation *at the future time*. - ModelR3 currentState = getState(); + ModelSE2 currentState = getState(); // Note this may add a bit of noise. - ModelR3 nextState = currentState.evolve(TimedRobot100.LOOP_PERIOD_S); + ModelSE2 nextState = currentState.evolve(TimedRobot100.LOOP_PERIOD_S); Rotation2d nextTheta = nextState.rotation(); ChassisSpeeds nextSpeed = SwerveKinodynamics.toInstantaneousChassisSpeeds( @@ -160,7 +160,7 @@ public void resetPose(Pose2d robotPose) { * acceleration. */ @Override - public ModelR3 getState() { + public ModelSE2 getState() { return m_stateCache.get(); } @@ -204,13 +204,13 @@ public void close() { * Compute the current state. This is a fairly heavyweight thing to do, so it * should be cached (thus refreshed once per cycle). */ - private ModelR3 update() { + private ModelSE2 update() { double now = Takt.get(); SwerveModulePositions positions = m_swerveLocal.positions(); // now that the pose estimator uses the SideEffect thing, we don't need this. // m_odometryUpdater.update(); // m_cameraUpdater.run(); - ModelR3 swerveModel = m_estimate.apply(now); + ModelSE2 swerveModel = m_estimate.apply(now); if (DEBUG) { System.out.printf("update() positions %s estimated pose: %s\n", positions, swerveModel); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ChassisSpeedAdapter.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ChassisSpeedAdapter.java index 2e78c846..13f5423c 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ChassisSpeedAdapter.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ChassisSpeedAdapter.java @@ -1,7 +1,7 @@ package org.team100.lib.subsystems.swerve.commands.manual; import org.team100.lib.hid.Velocity; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem; public class ChassisSpeedAdapter implements DriverAdapter { @@ -16,7 +16,7 @@ public ChassisSpeedAdapter(SwerveDriveSubsystem drive, ChassisSpeedDriver driver } @Override - public void apply(ModelR3 s, Velocity t) { + public void apply(ModelSE2 s, Velocity t) { if (DEBUG) { System.out.printf("ChassisSpeedDriver %s\n", t); } @@ -24,7 +24,7 @@ public void apply(ModelR3 s, Velocity t) { } @Override - public void reset(ModelR3 p) { + public void reset(ModelSE2 p) { m_driver.reset(p); } } diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ChassisSpeedDriver.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ChassisSpeedDriver.java index bb4a297c..c1ce234b 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ChassisSpeedDriver.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ChassisSpeedDriver.java @@ -1,13 +1,13 @@ package org.team100.lib.subsystems.swerve.commands.manual; import org.team100.lib.hid.Velocity; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import edu.wpi.first.math.kinematics.ChassisSpeeds; public interface ChassisSpeedDriver { - ChassisSpeeds apply(ModelR3 state, Velocity input); + ChassisSpeeds apply(ModelSE2 state, Velocity input); - void reset(ModelR3 state); + void reset(ModelSE2 state); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/DriveManually.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/DriveManually.java index 623c6ab7..f8f172d3 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/DriveManually.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/DriveManually.java @@ -6,7 +6,7 @@ import java.util.function.Supplier; import org.team100.lib.hid.Velocity; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem; import org.team100.lib.subsystems.swerve.kinodynamics.limiter.SwerveLimiter; import org.team100.lib.util.NamedChooser; @@ -71,7 +71,7 @@ public void initialize() { m_heedRadiusM.accept(HEED_RADIUS_M); // make sure the limiter knows what we're doing m_limiter.updateSetpoint(m_drive.getVelocity()); - ModelR3 p = m_drive.getState(); + ModelSE2 p = m_drive.getState(); for (DriverAdapter d : m_drivers.values()) { d.reset(p); } @@ -87,7 +87,7 @@ public void execute() { if (!(manualMode.equals(currentManualMode))) { currentManualMode = manualMode; // there's state in there we'd like to forget - ModelR3 p = m_drive.getState(); + ModelSE2 p = m_drive.getState(); for (DriverAdapter d : m_drivers.values()) { d.reset(p); } @@ -95,7 +95,7 @@ public void execute() { // input in [-1,1] control units Velocity input = m_twistSupplier.get(); - ModelR3 state = m_drive.getState(); + ModelSE2 state = m_drive.getState(); DriverAdapter d = m_drivers.getOrDefault(manualMode, m_defaultDriver); d.apply(state, input); diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/DriveManuallySimple.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/DriveManuallySimple.java index 44f8fe55..8f691658 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/DriveManuallySimple.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/DriveManuallySimple.java @@ -9,8 +9,8 @@ import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.hid.Velocity; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem; import org.team100.lib.subsystems.swerve.kinodynamics.limiter.SwerveLimiter; @@ -31,7 +31,7 @@ public class DriveManuallySimple extends Command { */ private final Supplier m_twistSupplier; private final DoubleConsumer m_heedRadiusM; - private final VelocitySubsystemR3 m_drive; + private final VelocitySubsystemSE2 m_drive; private final SwerveLimiter m_limiter; private final FieldRelativeDriver m_defaultDriver; private final FieldRelativeDriver m_alternativeDriver; @@ -68,7 +68,7 @@ public void initialize() { @Override public void execute() { - ModelR3 state = m_drive.getState(); + ModelSE2 state = m_drive.getState(); if (m_useAlternative.get() != wasUsingAlternative) { // switch modes m_alternativeDriver.reset(state); @@ -91,7 +91,7 @@ public void end(boolean interrupted) { m_drive.stop(); } - private VelocitySE2 desiredVelocity(ModelR3 state, Velocity input) { + private VelocitySE2 desiredVelocity(ModelSE2 state, Velocity input) { if (m_useAlternative.get()) { return m_alternativeDriver.apply(state, input); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/DriverAdapter.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/DriverAdapter.java index 39757984..96e8fa0e 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/DriverAdapter.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/DriverAdapter.java @@ -1,14 +1,14 @@ package org.team100.lib.subsystems.swerve.commands.manual; import org.team100.lib.hid.Velocity; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; /** * Adapts various types of manual drivers so `DriveManually` can switch between * them easily. */ public interface DriverAdapter { - void apply(ModelR3 s, Velocity t); + void apply(ModelSE2 s, Velocity t); - void reset(ModelR3 s); + void reset(ModelSE2 s); } \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/FieldRelativeAdapter.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/FieldRelativeAdapter.java index 5516b396..86bc34be 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/FieldRelativeAdapter.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/FieldRelativeAdapter.java @@ -6,8 +6,8 @@ import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.hid.Velocity; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; import org.team100.lib.subsystems.swerve.kinodynamics.limiter.SwerveLimiter; /** Uses the limited drive subsystem since this is manual driving. */ @@ -15,19 +15,19 @@ public class FieldRelativeAdapter implements DriverAdapter { private static final boolean DEBUG = false; private final SwerveLimiter m_limiter; - private final VelocitySubsystemR3 m_drive; + private final VelocitySubsystemSE2 m_drive; private final FieldRelativeDriver m_driver; public FieldRelativeAdapter( SwerveLimiter limiter, - VelocitySubsystemR3 drive, + VelocitySubsystemSE2 drive, FieldRelativeDriver driver) { m_limiter = limiter; m_drive = drive; m_driver = driver; } - public void apply(ModelR3 s, Velocity t) { + public void apply(ModelSE2 s, Velocity t) { if (DEBUG) { System.out.printf("FieldRelativeDriver %s\n", t); } @@ -42,7 +42,7 @@ public void apply(ModelR3 s, Velocity t) { m_drive.setVelocity(scaled); } - public void reset(ModelR3 p) { + public void reset(ModelSE2 p) { m_driver.reset(p); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/FieldRelativeDriver.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/FieldRelativeDriver.java index 1fe5521c..55708d55 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/FieldRelativeDriver.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/FieldRelativeDriver.java @@ -2,7 +2,7 @@ import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.hid.Velocity; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import edu.wpi.first.math.MathUtil; @@ -13,9 +13,9 @@ public interface FieldRelativeDriver { * @param input control units [-1,1] * @return feasible field-relative velocity in m/s and rad/s */ - VelocitySE2 apply(ModelR3 state, Velocity input); + VelocitySE2 apply(ModelSE2 state, Velocity input); - void reset(ModelR3 state); + void reset(ModelSE2 state); /** * Scales driver input to field-relative velocity. diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualChassisSpeeds.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualChassisSpeeds.java index ef350cf4..546dfe3a 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualChassisSpeeds.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualChassisSpeeds.java @@ -4,7 +4,7 @@ import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.ChassisSpeedsLogger; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import edu.wpi.first.math.MathUtil; @@ -32,7 +32,7 @@ public ManualChassisSpeeds(LoggerFactory parent, SwerveKinodynamics swerveKinody * feasible) speeds. */ @Override - public ChassisSpeeds apply(ModelR3 state, Velocity input) { + public ChassisSpeeds apply(ModelSE2 state, Velocity input) { // clip the input to the unit circle final Velocity clipped = input.clip(1.0); // scale to max in both translation and rotation @@ -63,7 +63,7 @@ public static ChassisSpeeds scaleChassisSpeeds(Velocity twist, double maxSpeed, maxRot * MathUtil.clamp(twist.theta(), -1, 1)); } - public void reset(ModelR3 p) { + public void reset(ModelSE2 p) { // } } diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualFieldRelativeSpeeds.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualFieldRelativeSpeeds.java index ac575da1..bfd8b04f 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualFieldRelativeSpeeds.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualFieldRelativeSpeeds.java @@ -5,7 +5,7 @@ import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.VelocitySE2Logger; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; /** @@ -29,7 +29,7 @@ public ManualFieldRelativeSpeeds(LoggerFactory parent, SwerveKinodynamics swerve * feasible) speeds. */ @Override - public VelocitySE2 apply(ModelR3 state, Velocity input) { + public VelocitySE2 apply(ModelSE2 state, Velocity input) { // clip the input to the unit circle final Velocity clipped = input.clip(1.0); @@ -44,7 +44,7 @@ public VelocitySE2 apply(ModelR3 state, Velocity input) { } @Override - public void reset(ModelR3 p) { + public void reset(ModelSE2 p) { // } } diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithFullStateHeading.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithFullStateHeading.java index 4bef49e4..87175307 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithFullStateHeading.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithFullStateHeading.java @@ -14,7 +14,7 @@ import org.team100.lib.logging.LoggerFactory.DoubleLogger; import org.team100.lib.state.Control100; import org.team100.lib.state.Model100; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.util.Math100; @@ -89,7 +89,7 @@ public ManualWithFullStateHeading( } @Override - public void reset(ModelR3 state) { + public void reset(ModelSE2 state) { m_thetaSetpoint = state.theta().control(); m_goal = null; m_latch.unlatch(); @@ -111,7 +111,7 @@ public void reset(ModelR3 state) { */ @Override public VelocitySE2 apply( - final ModelR3 state, + final ModelSE2 state, final Velocity twist1_1) { final Model100 thetaState = state.theta(); final double yawMeasurement = thetaState.x(); diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithOptionalTargetLock.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithOptionalTargetLock.java index 3ffe4d65..0da0e721 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithOptionalTargetLock.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithOptionalTargetLock.java @@ -15,7 +15,7 @@ import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile; import org.team100.lib.state.Control100; import org.team100.lib.state.Model100; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.targeting.TargetUtil; import org.team100.lib.util.Math100; @@ -76,7 +76,7 @@ public ManualWithOptionalTargetLock( } @Override - public void reset(ModelR3 state) { + public void reset(ModelSE2 state) { m_thetaSetpoint = state.theta().control(); m_controller.reset(); } @@ -91,7 +91,7 @@ public void reset(ModelR3 state) { */ @Override public VelocitySE2 apply( - final ModelR3 state, + final ModelSE2 state, final Velocity input) { // diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeading.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeading.java index bf7548bd..6d18804e 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeading.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeading.java @@ -14,7 +14,7 @@ import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile; import org.team100.lib.state.Control100; import org.team100.lib.state.Model100; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.util.Math100; @@ -92,7 +92,7 @@ public ManualWithProfiledHeading( } @Override - public void reset(ModelR3 state) { + public void reset(ModelSE2 state) { m_thetaSetpoint = state.theta().control(); m_goal = null; m_latch.unlatch(); @@ -115,7 +115,7 @@ public void reset(ModelR3 state) { */ @Override public VelocitySE2 apply( - final ModelR3 state, + final ModelSE2 state, final Velocity twist1_1) { final VelocitySE2 control = clipAndScale(twist1_1); diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithTargetLock.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithTargetLock.java index 79b8bdb3..14a5a394 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithTargetLock.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithTargetLock.java @@ -14,7 +14,7 @@ import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile; import org.team100.lib.state.Control100; import org.team100.lib.state.Model100; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.targeting.TargetUtil; import org.team100.lib.util.Math100; @@ -71,7 +71,7 @@ public ManualWithTargetLock( } @Override - public void reset(ModelR3 state) { + public void reset(ModelSE2 state) { m_thetaSetpoint = state.theta().control(); m_thetaController.reset(); } @@ -90,7 +90,7 @@ public void reset(ModelR3 state) { */ @Override public VelocitySE2 apply( - final ModelR3 state, + final ModelSE2 state, final Velocity input) { // diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ModuleStateAdapter.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ModuleStateAdapter.java index 3d776c4b..68f0e7ec 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ModuleStateAdapter.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ModuleStateAdapter.java @@ -1,7 +1,7 @@ package org.team100.lib.subsystems.swerve.commands.manual; import org.team100.lib.hid.Velocity; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem; public class ModuleStateAdapter implements DriverAdapter { @@ -16,7 +16,7 @@ public ModuleStateAdapter(SwerveDriveSubsystem drive, ModuleStateDriver driver) } @Override - public void apply(ModelR3 s, Velocity t) { + public void apply(ModelSE2 s, Velocity t) { if (DEBUG) { System.out.printf("ModuleStateDriver %s\n", t ); } @@ -24,7 +24,7 @@ public void apply(ModelR3 s, Velocity t) { } @Override - public void reset(ModelR3 p) { + public void reset(ModelSE2 p) { // } } diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/StopDriver.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/StopDriver.java index 670a24c6..537e3b65 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/StopDriver.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/StopDriver.java @@ -1,22 +1,22 @@ package org.team100.lib.subsystems.swerve.commands.manual; import org.team100.lib.hid.Velocity; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; public class StopDriver implements DriverAdapter { - private final VelocitySubsystemR3 m_drive; + private final VelocitySubsystemSE2 m_drive; - public StopDriver(VelocitySubsystemR3 drive) { + public StopDriver(VelocitySubsystemSE2 drive) { m_drive = drive; } - public void apply(ModelR3 s, Velocity t) { + public void apply(ModelSE2 s, Velocity t) { m_drive.stop(); } - public void reset(ModelR3 p) { + public void reset(ModelSE2 p) { // } diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/test/Rotate.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/test/Rotate.java index fd0c56bb..4369ac92 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/test/Rotate.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/test/Rotate.java @@ -1,13 +1,13 @@ package org.team100.lib.subsystems.swerve.commands.test; import org.team100.lib.commands.MoveAndHold; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.profile.r3.HolonomicProfileFactory; -import org.team100.lib.profile.r3.ProfileR3; -import org.team100.lib.reference.r3.ProfileReferenceR3; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.r3.commands.helper.VelocityReferenceControllerR3; +import org.team100.lib.profile.se2.HolonomicProfileFactory; +import org.team100.lib.profile.se2.ProfileSE2; +import org.team100.lib.reference.se2.ProfileReferenceSE2; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.se2.commands.helper.VelocityReferenceControllerSE2; import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; @@ -29,18 +29,18 @@ public class Rotate extends MoveAndHold { private final LoggerFactory m_log; private final SwerveDriveSubsystem m_drive; - private final ControllerR3 m_controller; + private final ControllerSE2 m_controller; private final SwerveKinodynamics m_swerveKinodynamics; private final Rotation2d m_target; - private final ProfileR3 m_profile; + private final ProfileSE2 m_profile; - private ProfileReferenceR3 m_reference; - private VelocityReferenceControllerR3 m_referenceController; + private ProfileReferenceSE2 m_reference; + private VelocityReferenceControllerSE2 m_referenceController; public Rotate( LoggerFactory parent, SwerveDriveSubsystem drive, - ControllerR3 controller, + ControllerSE2 controller, SwerveKinodynamics swerveKinodynamics, double targetAngleRadians) { m_log = parent.type(this); @@ -70,9 +70,9 @@ public void initialize() { // instead, pick a goal at the stopping distance in the current direction. Translation2d dx = m_drive.getVelocity().stopping(m_swerveKinodynamics.getMaxDriveAccelerationM_S2()); Pose2d goal = new Pose2d(measurement.getX() + dx.getX(), measurement.getY() + dx.getY(), m_target); - m_reference = new ProfileReferenceR3(m_log, m_profile, "rotate"); - m_reference.setGoal(new ModelR3(goal)); - m_referenceController = new VelocityReferenceControllerR3( + m_reference = new ProfileReferenceSE2(m_log, m_profile, "rotate"); + m_reference.setGoal(new ModelSE2(goal)); + m_referenceController = new VelocityReferenceControllerSE2( m_log, m_drive, m_controller, m_reference); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrain.java b/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrain.java index d1a6ea49..baa6bd2c 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrain.java +++ b/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrain.java @@ -2,8 +2,8 @@ import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.VelocitySE2; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; @@ -23,8 +23,8 @@ * * This version of the offset drivetrain does not include boosting. */ -public class OffsetDrivetrain implements VelocitySubsystemR3 { - private final VelocitySubsystemR3 m_delegate; +public class OffsetDrivetrain implements VelocitySubsystemSE2 { + private final VelocitySubsystemSE2 m_delegate; private final Translation2d m_offset; /** @@ -32,14 +32,14 @@ public class OffsetDrivetrain implements VelocitySubsystemR3 { * @param offset from delegate to toolpoint */ public OffsetDrivetrain( - VelocitySubsystemR3 delegate, Translation2d offset) { + VelocitySubsystemSE2 delegate, Translation2d offset) { m_delegate = delegate; m_offset = offset; } @Override - public ModelR3 getState() { - return new ModelR3(toolpointPose(), toolpointVelocity()); + public ModelSE2 getState() { + return new ModelSE2(toolpointPose(), toolpointVelocity()); } /** diff --git a/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrainWithBoost.java b/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrainWithBoost.java index cd1eb671..8f6bfcc6 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrainWithBoost.java +++ b/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrainWithBoost.java @@ -2,8 +2,8 @@ import org.team100.lib.geometry.GeometryUtil; import org.team100.lib.geometry.VelocitySE2; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; @@ -28,13 +28,13 @@ * essentially edits the output of the controller, so we can leave the * controller alone. */ -public class OffsetDrivetrainWithBoost implements VelocitySubsystemR3 { +public class OffsetDrivetrainWithBoost implements VelocitySubsystemSE2 { /** * How much of the perpendicular speed to mix in. This interacts with the * controller "P" values, so should be tuned together with them. */ private static final double OMEGA_MIXER = 2.0; - private final VelocitySubsystemR3 m_delegate; + private final VelocitySubsystemSE2 m_delegate; private final Translation2d m_offset; /** @@ -42,14 +42,14 @@ public class OffsetDrivetrainWithBoost implements VelocitySubsystemR3 { * @param offset from delegate to toolpoint */ public OffsetDrivetrainWithBoost( - VelocitySubsystemR3 delegate, Translation2d offset) { + VelocitySubsystemSE2 delegate, Translation2d offset) { m_delegate = delegate; m_offset = offset; } @Override - public ModelR3 getState() { - return new ModelR3(toolpointPose(), toolpointVelocity()); + public ModelSE2 getState() { + return new ModelSE2(toolpointPose(), toolpointVelocity()); } /** diff --git a/lib/src/main/java/org/team100/lib/subsystems/test/TrivialDrivetrain.java b/lib/src/main/java/org/team100/lib/subsystems/test/TrivialDrivetrain.java index f44a87dc..7c9e0ea2 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/test/TrivialDrivetrain.java +++ b/lib/src/main/java/org/team100/lib/subsystems/test/TrivialDrivetrain.java @@ -4,29 +4,29 @@ import org.team100.lib.coherence.ObjectCache; import org.team100.lib.framework.TimedRobot100; import org.team100.lib.geometry.VelocitySE2; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.r3.VelocitySubsystemR3; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; import edu.wpi.first.math.geometry.Pose2d; /** * Executes desired velocity exactly. */ -public class TrivialDrivetrain implements VelocitySubsystemR3 { +public class TrivialDrivetrain implements VelocitySubsystemSE2 { private static final double DT = TimedRobot100.LOOP_PERIOD_S; - private final ObjectCache m_stateCache; + private final ObjectCache m_stateCache; /** setpoint for the next timestep. used only by update(). */ private VelocitySE2 m_setpoint; - private ModelR3 m_state; + private ModelSE2 m_state; public TrivialDrivetrain(Pose2d initial) { m_setpoint = new VelocitySE2(0, 0, 0); - m_state = new ModelR3(initial); + m_state = new ModelSE2(initial); m_stateCache = Cache.of(this::update); } @Override - public ModelR3 getState() { + public ModelSE2 getState() { return m_stateCache.get(); } @@ -40,8 +40,8 @@ public void setVelocity(VelocitySE2 setpoint) { m_setpoint = setpoint; } - private ModelR3 update() { - m_state = new ModelR3( + private ModelSE2 update() { + m_state = new ModelSE2( m_setpoint.integrate(m_state.pose(), DT), m_setpoint); return m_state; diff --git a/lib/src/main/java/org/team100/lib/subsystems/turret/Turret.java b/lib/src/main/java/org/team100/lib/subsystems/turret/Turret.java index bd16f7bc..2967d473 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/turret/Turret.java +++ b/lib/src/main/java/org/team100/lib/subsystems/turret/Turret.java @@ -20,7 +20,7 @@ import org.team100.lib.sensor.position.incremental.IncrementalBareEncoder; import org.team100.lib.servo.AngularPositionServo; import org.team100.lib.servo.OnboardAngularPositionServo; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.targeting.Drag; import org.team100.lib.targeting.Intercept; import org.team100.lib.targeting.Range; @@ -55,7 +55,7 @@ public record Solution(Rotation2d azimuth, Rotation2d elevation) { */ private static final double DELAY = 0.2; private final DoubleArrayLogger m_log_field_turret; - private final Supplier m_state; + private final Supplier m_state; private final Supplier m_target; private final AngularPositionServo m_pivot; private final AngularPositionServo m_elevation; @@ -74,7 +74,7 @@ public record Solution(Rotation2d azimuth, Rotation2d elevation) { public Turret( LoggerFactory parent, LoggerFactory field, - Supplier state, + Supplier state, Supplier target, double speed) { LoggerFactory log = parent.type(this); @@ -170,7 +170,7 @@ private Optional getSolution() { * motion of either one. */ private Optional getAbsoluteBearingInstantaneous() { - ModelR3 state = m_state.get(); + ModelSE2 state = m_state.get(); Translation2d target = m_target.get(); return Optional.of( new Solution( @@ -179,7 +179,7 @@ private Optional getAbsoluteBearingInstantaneous() { } private Optional getShootingMethod() { - ModelR3 state = m_state.get(); + ModelSE2 state = m_state.get(); Translation2d robotPosition = state.translation(); GlobalVelocityR2 robotVelocity = state.velocityR2(); Translation2d targetPosition = m_target.get(); @@ -194,7 +194,7 @@ private Optional getShootingMethod() { * moving robot. */ private Optional getAbsoluteBearingForIntercept() { - ModelR3 state = m_state.get(); + ModelSE2 state = m_state.get(); Translation2d robotPosition = state.translation(); GlobalVelocityR2 robotVelocity = state.velocityR2(); diff --git a/lib/src/main/java/org/team100/lib/targeting/SimulatedTargetWriter.java b/lib/src/main/java/org/team100/lib/targeting/SimulatedTargetWriter.java index c2efda36..003ed830 100644 --- a/lib/src/main/java/org/team100/lib/targeting/SimulatedTargetWriter.java +++ b/lib/src/main/java/org/team100/lib/targeting/SimulatedTargetWriter.java @@ -12,7 +12,7 @@ import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleLogger; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation3d; @@ -38,7 +38,7 @@ public class SimulatedTargetWriter { private final Map> m_publishers; private final DoubleLogger m_log_poseTimestamp; private final List m_cameras; - private final DoubleFunction m_history; + private final DoubleFunction m_history; /** For now, a fixed list of targets */ private final Translation2d[] m_targets; @@ -48,7 +48,7 @@ public class SimulatedTargetWriter { public SimulatedTargetWriter( LoggerFactory parent, List cameras, - DoubleFunction history, + DoubleFunction history, Translation2d[] targets) { LoggerFactory log = parent.type(this); m_log_poseTimestamp = log.doubleLogger(Level.TRACE, "pose timestamp (s)"); diff --git a/lib/src/main/java/org/team100/lib/targeting/TargetUtil.java b/lib/src/main/java/org/team100/lib/targeting/TargetUtil.java index c1c5cf91..122d62c4 100644 --- a/lib/src/main/java/org/team100/lib/targeting/TargetUtil.java +++ b/lib/src/main/java/org/team100/lib/targeting/TargetUtil.java @@ -1,7 +1,7 @@ package org.team100.lib.targeting; import org.team100.lib.geometry.VelocitySE2; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -36,7 +36,7 @@ public static Rotation2d absoluteBearing(Translation2d robot, Translation2d targ * @param target field-relative target position * @return apparent rotation of the target around the robot, rad/s */ - public static double targetMotion(ModelR3 state, Translation2d target) { + public static double targetMotion(ModelSE2 state, Translation2d target) { VelocitySE2 velocity = state.velocity(); if (velocity.angle().isEmpty()) { // If there's no robot motion, there's no target motion. diff --git a/lib/src/main/java/org/team100/lib/targeting/Targets.java b/lib/src/main/java/org/team100/lib/targeting/Targets.java index f45e7825..46f02574 100644 --- a/lib/src/main/java/org/team100/lib/targeting/Targets.java +++ b/lib/src/main/java/org/team100/lib/targeting/Targets.java @@ -16,7 +16,7 @@ import org.team100.lib.logging.LoggerFactory.DoubleLogger; import org.team100.lib.logging.LoggerFactory.IntLogger; import org.team100.lib.network.CameraReader; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.util.CoalescingCollection; import org.team100.lib.util.TrailingHistory; @@ -47,7 +47,7 @@ public class Targets extends CameraReader { public final DoubleArrayLogger m_log_coalescedTargets; /** state = f(takt seconds) from history. */ - private final DoubleFunction m_history; + private final DoubleFunction m_history; /** Accumulation of targets we see; this is really for logging only. */ private final TrailingHistory m_allTargets; /** Coalesced targets */ @@ -61,7 +61,7 @@ public class Targets extends CameraReader { public Targets( LoggerFactory parent, LoggerFactory fieldLogger, - DoubleFunction history) { + DoubleFunction history) { super(parent, "objectVision", "Rotation3d", StructBuffer.create(Rotation3d.struct)); LoggerFactory log = parent.type(this); m_log_historySize = log.intLogger(Level.TRACE, "history size"); diff --git a/lib/src/main/java/org/team100/lib/trajectory/README.md b/lib/src/main/java/org/team100/lib/trajectory/README.md index 0cfc98de..3e83f7d7 100644 --- a/lib/src/main/java/org/team100/lib/trajectory/README.md +++ b/lib/src/main/java/org/team100/lib/trajectory/README.md @@ -13,9 +13,7 @@ The process of constructing a trajectory has three stages: 2. Construct a list of points along the spline, such that straight lines connecting the points ("secant lines") don't deviate too much from the true spline, and aren't too far apart from each other. (This uses recursive bisection.) These points will be close together where the curvature is high, and far apart along straighter sections of the spline. The list is created by `PathFactory`, producing `Path100`, which integrates along the list to find the distance. See the `lib.trajectory.path` package. -3. Construct a list of points interpolated along the secant lines, such that the points aren't too far apart. - -4. Using a list of kinodynamic constraints (see `lib.trajectory.timing`), assign a time for each point. The resulting list of `TimedState`s is created by `TrajectoryFactory`, producing `Trajectory100`. +3. Using a list of kinodynamic constraints (see `lib.trajectory.timing`), assign a time for each point. The resulting list of `TimedState`s is created by `TrajectoryFactory`, producing `Trajectory100`. To use a trajectory, you `sample()` it, with time (in seconds) as the parameter. The resulting `TimedState` is interpolated between from the list above. diff --git a/lib/src/main/java/org/team100/lib/visualization/BallFactory.java b/lib/src/main/java/org/team100/lib/visualization/BallFactory.java index 94e8b68c..9c697a67 100644 --- a/lib/src/main/java/org/team100/lib/visualization/BallFactory.java +++ b/lib/src/main/java/org/team100/lib/visualization/BallFactory.java @@ -3,7 +3,7 @@ import java.util.function.Supplier; import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.targeting.Drag; import edu.wpi.first.math.geometry.Rotation2d; @@ -12,7 +12,7 @@ public class BallFactory { public static Ball get2d( LoggerFactory field, - Supplier robot, + Supplier robot, Supplier azimuth, double speed) { return new BallR2(field, robot, azimuth, speed); @@ -21,7 +21,7 @@ public static Ball get2d( public static Ball get3d( LoggerFactory field, - Supplier robot, + Supplier robot, Supplier azimuth, Supplier elevation, double speed, diff --git a/lib/src/main/java/org/team100/lib/visualization/BallR2.java b/lib/src/main/java/org/team100/lib/visualization/BallR2.java index 72c4c1b6..5349a352 100644 --- a/lib/src/main/java/org/team100/lib/visualization/BallR2.java +++ b/lib/src/main/java/org/team100/lib/visualization/BallR2.java @@ -7,7 +7,7 @@ import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleArrayLogger; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -20,7 +20,7 @@ public class BallR2 implements Ball { private static final double DT = TimedRobot100.LOOP_PERIOD_S; private final DoubleArrayLogger m_log_field_ball; - private final Supplier m_robot; + private final Supplier m_robot; private final Supplier m_azimuth; /** Projectile speed m/s */ private final double m_speed; @@ -31,13 +31,13 @@ public class BallR2 implements Ball { /** * @param field log - * @param robot state (pose2d, velocityR3) + * @param robot state (pose2d, velocitySE2) * @param azimuth absolute * @param speed muzzle speed */ public BallR2( LoggerFactory field, - Supplier robot, + Supplier robot, Supplier azimuth, double speed) { m_log_field_ball = field.doubleArrayLogger(Level.COMP, "ball"); diff --git a/lib/src/main/java/org/team100/lib/visualization/BallR3.java b/lib/src/main/java/org/team100/lib/visualization/BallR3.java index 2a2c33a3..ffc59552 100644 --- a/lib/src/main/java/org/team100/lib/visualization/BallR3.java +++ b/lib/src/main/java/org/team100/lib/visualization/BallR3.java @@ -7,7 +7,7 @@ import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleArrayLogger; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.targeting.Drag; import edu.wpi.first.math.Matrix; @@ -19,13 +19,13 @@ import edu.wpi.first.math.system.NumericalIntegration; /** - * Simulated projectile in three dimensions + * Simulated projectile in three dimensions (x, y, z) */ public class BallR3 implements Ball { private static final double DT = TimedRobot100.LOOP_PERIOD_S; private final DoubleArrayLogger m_log_field_ball; private final Drag m_drag; - private final Supplier m_robot; + private final Supplier m_robot; private final Supplier m_azimuth; private final Supplier m_elevation; private final double m_speed; @@ -51,7 +51,7 @@ public class BallR3 implements Ball { public BallR3( LoggerFactory field, Drag drag, - Supplier robot, + Supplier robot, Supplier azimuth, Supplier elevation, double speed, diff --git a/lib/src/test/java/org/team100/lib/controller/r3/FeedforwardControllerR3Test.java b/lib/src/test/java/org/team100/lib/controller/se2/FeedforwardControllerSE2Test.java similarity index 77% rename from lib/src/test/java/org/team100/lib/controller/r3/FeedforwardControllerR3Test.java rename to lib/src/test/java/org/team100/lib/controller/se2/FeedforwardControllerSE2Test.java index e507a378..18ccb0f8 100644 --- a/lib/src/test/java/org/team100/lib/controller/r3/FeedforwardControllerR3Test.java +++ b/lib/src/test/java/org/team100/lib/controller/se2/FeedforwardControllerSE2Test.java @@ -1,37 +1,38 @@ -package org.team100.lib.controller.r3; +package org.team100.lib.controller.se2; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; import org.junit.jupiter.api.Test; +import org.team100.lib.controller.se2.FeedforwardControllerSE2; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; import org.team100.lib.state.Control100; -import org.team100.lib.state.ControlR3; +import org.team100.lib.state.ControlSE2; import org.team100.lib.state.Model100; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; -public class FeedforwardControllerR3Test { +public class FeedforwardControllerSE2Test { private static final double DELTA = 0.001; private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); @Test void testMotionless() { - FeedforwardControllerR3 c = new FeedforwardControllerR3(logger, 0.01, 0.01, 0.01, 0.01); + FeedforwardControllerSE2 c = new FeedforwardControllerSE2(logger, 0.01, 0.01, 0.01, 0.01); assertFalse(c.atReference()); VelocitySE2 v = c.calculate( - new ModelR3( + new ModelSE2( new Model100(0, 0), new Model100(0, 0), new Model100(0, 0)), - new ModelR3( + new ModelSE2( new Model100(0, 0), new Model100(0, 0), new Model100(0, 0)), - new ControlR3( + new ControlSE2( new Control100(0, 0), new Control100(0, 0), new Control100(0, 0))); @@ -43,18 +44,18 @@ void testMotionless() { @Test void testNotAtReference() { - FeedforwardControllerR3 c = new FeedforwardControllerR3(logger, 0.01, 0.01, 0.01, 0.01); + FeedforwardControllerSE2 c = new FeedforwardControllerSE2(logger, 0.01, 0.01, 0.01, 0.01); assertFalse(c.atReference()); VelocitySE2 v = c.calculate( - new ModelR3( + new ModelSE2( new Model100(1, 0), new Model100(0, 0), new Model100(0, 0)), - new ModelR3( + new ModelSE2( new Model100(0, 0), new Model100(0, 0), new Model100(0, 0)), - new ControlR3( + new ControlSE2( new Control100(0, 0), new Control100(0, 0), new Control100(0, 0))); @@ -66,18 +67,18 @@ void testNotAtReference() { @Test void testFeedforward() { - FeedforwardControllerR3 c = new FeedforwardControllerR3(logger, 0.01, 0.01, 0.01, 0.01); + FeedforwardControllerSE2 c = new FeedforwardControllerSE2(logger, 0.01, 0.01, 0.01, 0.01); assertFalse(c.atReference()); VelocitySE2 v = c.calculate( - new ModelR3( + new ModelSE2( new Model100(0, 0), new Model100(0, 0), new Model100(0, 0)), - new ModelR3( + new ModelSE2( new Model100(0, 0), new Model100(0, 0), new Model100(0, 0)), - new ControlR3( + new ControlSE2( new Control100(0, 1), new Control100(0, 0), new Control100(0, 0))); diff --git a/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java b/lib/src/test/java/org/team100/lib/controller/se2/FullStateControllerR3Test.java similarity index 80% rename from lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java rename to lib/src/test/java/org/team100/lib/controller/se2/FullStateControllerR3Test.java index 9a8fe615..19e051df 100644 --- a/lib/src/test/java/org/team100/lib/controller/r3/FullStateControllerR3Test.java +++ b/lib/src/test/java/org/team100/lib/controller/se2/FullStateControllerR3Test.java @@ -1,45 +1,47 @@ -package org.team100.lib.controller.r3; +package org.team100.lib.controller.se2; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; import org.junit.jupiter.api.Test; +import org.team100.lib.controller.se2.ControllerFactorySE2; +import org.team100.lib.controller.se2.FullStateControllerSE2; import org.team100.lib.geometry.DeltaSE2; -import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.geometry.VelocitySE2; +import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; import org.team100.lib.state.Control100; -import org.team100.lib.state.ControlR3; +import org.team100.lib.state.ControlSE2; import org.team100.lib.state.Model100; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.testing.Timeless; import org.team100.lib.trajectory.timing.TimedState; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -class FullStateControllerR3Test implements Timeless { +class FullStateControllerSE2Test implements Timeless { private static final double DELTA = 0.001; private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); @Test void testAtRest() { - FullStateControllerR3 c = ControllerFactoryR3.test2(logger); + FullStateControllerSE2 c = ControllerFactorySE2.test2(logger); assertFalse(c.atReference()); VelocitySE2 t = c.calculate( - new ModelR3( + new ModelSE2( new Model100(0, 0), new Model100(0, 0), new Model100(0, 0)), - new ModelR3( + new ModelSE2( new Model100(0, 0), new Model100(0, 0), new Model100(0, 0)), - new ControlR3( + new ControlSE2( new Control100(0, 0), new Control100(0, 0), new Control100(0, 0))); @@ -51,19 +53,19 @@ void testAtRest() { @Test void testFar() { - FullStateControllerR3 c = ControllerFactoryR3.test2(logger); + FullStateControllerSE2 c = ControllerFactorySE2.test2(logger); assertFalse(c.atReference()); // no velocity, no feedforward VelocitySE2 t = c.calculate( - new ModelR3( + new ModelSE2( new Model100(0, 0), new Model100(0, 0), new Model100(0, 0)), - new ModelR3( + new ModelSE2( new Model100(1, 0), new Model100(0, 0), new Model100(0, 0)), - new ControlR3( + new ControlSE2( new Control100(1, 0), new Control100(0, 0), new Control100(0, 0))); @@ -76,18 +78,18 @@ void testFar() { @Test void testFast() { - FullStateControllerR3 c = ControllerFactoryR3.test2(logger); + FullStateControllerSE2 c = ControllerFactorySE2.test2(logger); assertFalse(c.atReference()); VelocitySE2 t = c.calculate( - new ModelR3( + new ModelSE2( new Model100(0, 0), new Model100(0, 0), new Model100(0, 0)), - new ModelR3( + new ModelSE2( new Model100(0, 1), // produces error = 1 new Model100(0, 0), new Model100(0, 0)), - new ControlR3( + new ControlSE2( new Control100(0, 1), // produces FF = 1 new Control100(0, 0), new Control100(0, 0))); @@ -101,18 +103,18 @@ void testFast() { @Test void testOnTrack() { - FullStateControllerR3 c = ControllerFactoryR3.test2(logger); + FullStateControllerSE2 c = ControllerFactorySE2.test2(logger); assertFalse(c.atReference()); VelocitySE2 t = c.calculate( - new ModelR3( + new ModelSE2( new Model100(0, 0), new Model100(0, 0), new Model100(0, 0)), - new ModelR3( + new ModelSE2( new Model100(-1, 0.5), // position + velocity error new Model100(0, 0), new Model100(0, 0)), - new ControlR3( + new ControlSE2( new Control100(-1, 0.5), // velocity reference new Control100(0, 0), new Control100(0, 0))); @@ -125,19 +127,19 @@ void testOnTrack() { @Test void testAllAxes() { - FullStateControllerR3 c = ControllerFactoryR3.test2(logger); + FullStateControllerSE2 c = ControllerFactorySE2.test2(logger); assertFalse(c.atReference()); // none of these have any velocity so there's no feedforward. VelocitySE2 t = c.calculate( - new ModelR3( + new ModelSE2( new Model100(0, 0), new Model100(0, 0), new Model100(0, 0)), - new ModelR3( + new ModelSE2( new Model100(1, 0), new Model100(2, 0), new Model100(3, 0)), - new ControlR3( + new ControlSE2( new Control100(2, 0), new Control100(4, 0), new Control100(6, 0))); @@ -150,18 +152,18 @@ void testAllAxes() { @Test void testRotation() { - FullStateControllerR3 c = ControllerFactoryR3.test2(logger); + FullStateControllerSE2 c = ControllerFactorySE2.test2(logger); assertFalse(c.atReference()); VelocitySE2 t = c.calculate( - new ModelR3( + new ModelSE2( new Model100(0, 0), new Model100(0, 0), new Model100(3, 0)), - new ModelR3( + new ModelSE2( new Model100(0, 0), new Model100(0, 0), new Model100(-3, 0)), - new ControlR3( + new ControlSE2( new Control100(0, 0), new Control100(0, 0), new Control100(-3, 0))); @@ -174,10 +176,10 @@ void testRotation() { @Test void testErrZero() { - FullStateControllerR3 controller = new FullStateControllerR3(logger, 2.4, 2.4, 0.0, 0.0, 0.01, 0.02, + FullStateControllerSE2 controller = new FullStateControllerSE2(logger, 2.4, 2.4, 0.0, 0.0, 0.01, 0.02, 0.01, 0.02); - ModelR3 measurement = new ModelR3(); - ModelR3 currentReference = ModelR3 + ModelSE2 measurement = new ModelSE2(); + ModelSE2 currentReference = ModelSE2 .fromTimedState(new TimedState( new Pose2dWithMotion( WaypointSE2.irrotational( @@ -192,10 +194,10 @@ void testErrZero() { @Test void testErrXAhead() { - FullStateControllerR3 controller = new FullStateControllerR3(logger, 2.4, 2.4, 0.0, 0.0, 0.01, 0.02, + FullStateControllerSE2 controller = new FullStateControllerSE2(logger, 2.4, 2.4, 0.0, 0.0, 0.01, 0.02, 0.01, 0.02); - ModelR3 measurement = new ModelR3(new Pose2d(1, 0, new Rotation2d())); - ModelR3 currentReference = ModelR3 + ModelSE2 measurement = new ModelSE2(new Pose2d(1, 0, new Rotation2d())); + ModelSE2 currentReference = ModelSE2 .fromTimedState(new TimedState(new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), @@ -208,10 +210,10 @@ void testErrXAhead() { @Test void testErrXBehind() { - FullStateControllerR3 controller = new FullStateControllerR3(logger, 2.4, 2.4, 0.0, 0.0, 0.01, 0.02, + FullStateControllerSE2 controller = new FullStateControllerSE2(logger, 2.4, 2.4, 0.0, 0.0, 0.01, 0.02, 0.01, 0.02); - ModelR3 measurement = new ModelR3(new Pose2d(0, 0, new Rotation2d())); - ModelR3 currentReference = ModelR3 + ModelSE2 measurement = new ModelSE2(new Pose2d(0, 0, new Rotation2d())); + ModelSE2 currentReference = ModelSE2 .fromTimedState(new TimedState(new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(1, 0, new Rotation2d(0)), 0, 1.2), @@ -225,10 +227,10 @@ void testErrXBehind() { /** Rotation should not matter. */ @Test void testErrXAheadWithRotation() { - FullStateControllerR3 controller = new FullStateControllerR3(logger, 2.4, 2.4, 0.0, 0.0, 0.01, 0.02, + FullStateControllerSE2 controller = new FullStateControllerSE2(logger, 2.4, 2.4, 0.0, 0.0, 0.01, 0.02, 0.01, 0.02); - ModelR3 measurement = new ModelR3(new Pose2d(1, 0, new Rotation2d(1))); - ModelR3 currentReference = ModelR3 + ModelSE2 measurement = new ModelSE2(new Pose2d(1, 0, new Rotation2d(1))); + ModelSE2 currentReference = ModelSE2 .fromTimedState(new TimedState(new Pose2dWithMotion( WaypointSE2.irrotational( new Pose2d(0, 0, new Rotation2d(1)), 0, 1.2), @@ -241,10 +243,10 @@ void testErrXAheadWithRotation() { @Test void testErrorAhead() { - FullStateControllerR3 controller = new FullStateControllerR3(logger, 2.4, 2.4, 0.0, 0.0, 0.01, 0.02, + FullStateControllerSE2 controller = new FullStateControllerSE2(logger, 2.4, 2.4, 0.0, 0.0, 0.01, 0.02, 0.01, 0.02); // measurement is at the origin, facing ahead - ModelR3 measurement = new ModelR3(new Pose2d()); + ModelSE2 measurement = new ModelSE2(new Pose2d()); // motion is in a straight line, down the x axis // setpoint is also at the origin @@ -258,7 +260,7 @@ void testErrorAhead() { // constant speed double acceleration = 0; // we're exactly on the setpoint so zero error - ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); + ModelSE2 currentReference = ModelSE2.fromTimedState(new TimedState(state, t, velocity, acceleration)); DeltaSE2 positionError = controller.positionError(measurement, currentReference); assertEquals(0, positionError.getX(), DELTA); assertEquals(0, positionError.getY(), DELTA); @@ -267,10 +269,10 @@ void testErrorAhead() { @Test void testErrorSideways() { - FullStateControllerR3 controller = new FullStateControllerR3(logger, 2.4, 2.4, 0.0, 0.0, 0.01, 0.02, + FullStateControllerSE2 controller = new FullStateControllerSE2(logger, 2.4, 2.4, 0.0, 0.0, 0.01, 0.02, 0.01, 0.02); // measurement is at the origin, facing down y - ModelR3 measurement = new ModelR3(new Pose2d(0, 0, Rotation2d.kCCW_Pi_2)); + ModelSE2 measurement = new ModelSE2(new Pose2d(0, 0, Rotation2d.kCCW_Pi_2)); // motion is in a straight line, down the x axis // setpoint is +x, facing down y Pose2dWithMotion state = new Pose2dWithMotion( @@ -282,7 +284,7 @@ void testErrorSideways() { double velocity = 1; // constant speed double acceleration = 0; - ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); + ModelSE2 currentReference = ModelSE2.fromTimedState(new TimedState(state, t, velocity, acceleration)); DeltaSE2 positionError = controller.positionError(measurement, currentReference); assertEquals(1, positionError.getX(), DELTA); assertEquals(0, positionError.getY(), DELTA); @@ -291,10 +293,10 @@ void testErrorSideways() { @Test void testVelocityErrorZero() { - FullStateControllerR3 controller = new FullStateControllerR3(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, + FullStateControllerSE2 controller = new FullStateControllerSE2(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, 0.02); // measurement position doesn't matter, rotation here matches velocity below - ModelR3 measurement = new ModelR3( + ModelSE2 measurement = new ModelSE2( new Pose2d(1, 2, new Rotation2d(Math.PI)), new VelocitySE2(1, 0, 0)); // motion is in a straight line, down the x axis @@ -306,7 +308,7 @@ void testVelocityErrorZero() { double velocity = 1; // constant speed double acceleration = 0; - ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); + ModelSE2 currentReference = ModelSE2.fromTimedState(new TimedState(state, t, velocity, acceleration)); VelocitySE2 error = controller.velocityError(measurement, currentReference); // we're exactly on the setpoint so zero error assertEquals(0, error.x(), DELTA); @@ -316,11 +318,11 @@ void testVelocityErrorZero() { @Test void testVelocityErrorAhead() { - FullStateControllerR3 controller = new FullStateControllerR3(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, + FullStateControllerSE2 controller = new FullStateControllerSE2(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, 0.02); // measurement is at the origin, facing ahead // measurement is the wrong velocity - ModelR3 measurement = new ModelR3( + ModelSE2 measurement = new ModelSE2( new Pose2d(), new VelocitySE2(0, 1, 0)); // motion is in a straight line, down the x axis @@ -335,7 +337,7 @@ void testVelocityErrorAhead() { // constant speed double acceleration = 0; - ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); + ModelSE2 currentReference = ModelSE2.fromTimedState(new TimedState(state, t, velocity, acceleration)); VelocitySE2 error = controller.velocityError(measurement, currentReference); // error should include both components assertEquals(1, error.x(), DELTA); @@ -345,7 +347,7 @@ void testVelocityErrorAhead() { @Test void testFeedForwardAhead() { - FullStateControllerR3 controller = new FullStateControllerR3(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, + FullStateControllerSE2 controller = new FullStateControllerSE2(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, 0.02); // motion is in a straight line, down the x axis // setpoint is also at the origin @@ -360,7 +362,7 @@ void testFeedForwardAhead() { double acceleration = 0; TimedState setpoint = new TimedState(state, t, velocity, acceleration); // feedforward should be straight ahead, no rotation. - ControlR3 nextReference = ControlR3.fromTimedState(setpoint); + ControlSE2 nextReference = ControlSE2.fromTimedState(setpoint); VelocitySE2 speeds = controller.feedforward(nextReference); assertEquals(1, speeds.x(), DELTA); assertEquals(0, speeds.y(), DELTA); @@ -369,7 +371,7 @@ void testFeedForwardAhead() { @Test void testFeedForwardSideways() { - FullStateControllerR3 controller = new FullStateControllerR3(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, + FullStateControllerSE2 controller = new FullStateControllerSE2(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, 0.02); // motion is in a straight line, down the x axis // setpoint is the same @@ -384,7 +386,7 @@ void testFeedForwardSideways() { double acceleration = 0; TimedState setpoint = new TimedState(state, t, velocity, acceleration); // feedforward should be -y, robot relative, no rotation. - ControlR3 nextReference = ControlR3.fromTimedState(setpoint); + ControlSE2 nextReference = ControlSE2.fromTimedState(setpoint); VelocitySE2 speeds = controller.feedforward(nextReference); assertEquals(1, speeds.x(), DELTA); assertEquals(0, speeds.y(), DELTA); @@ -393,7 +395,7 @@ void testFeedForwardSideways() { @Test void testFeedForwardTurning() { - FullStateControllerR3 controller = new FullStateControllerR3(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, + FullStateControllerSE2 controller = new FullStateControllerSE2(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, 0.02); // motion is tangential to the x axis but turning left // setpoint is also at the origin @@ -406,7 +408,7 @@ void testFeedForwardTurning() { double velocity = 1; // constant speed double acceleration = 0; - ControlR3 nextReference = ControlR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); + ControlSE2 nextReference = ControlSE2.fromTimedState(new TimedState(state, t, velocity, acceleration)); VelocitySE2 speeds = controller.feedforward(nextReference); // feedforward should be ahead and rotating. assertEquals(1, speeds.x(), DELTA); @@ -416,7 +418,7 @@ void testFeedForwardTurning() { @Test void testFeedbackAhead() { - FullStateControllerR3 controller = new FullStateControllerR3(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, + FullStateControllerSE2 controller = new FullStateControllerSE2(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, 0.02); // measurement is at the origin, facing ahead Pose2d currentState = new Pose2d(); @@ -435,11 +437,11 @@ void testFeedbackAhead() { double velocity = 1; // constant speed double acceleration = 0; - ModelR3 measurement = new ModelR3( + ModelSE2 measurement = new ModelSE2( currentState, new VelocitySE2(1, 0, 0)); // feedforward should be straight ahead, no rotation. - ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); + ModelSE2 currentReference = ModelSE2.fromTimedState(new TimedState(state, t, velocity, acceleration)); DeltaSE2 err = DeltaSE2.delta(measurement.pose(), currentReference.pose()); VelocitySE2 speeds = controller.positionFeedback(err); // we're exactly on the setpoint so zero feedback @@ -450,7 +452,7 @@ void testFeedbackAhead() { @Test void testFeedbackAheadPlusY() { - FullStateControllerR3 controller = new FullStateControllerR3(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, + FullStateControllerSE2 controller = new FullStateControllerSE2(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, 0.02); // measurement is plus-Y, facing ahead Pose2d currentState = new Pose2d(0, 1, Rotation2d.kZero); @@ -469,11 +471,11 @@ void testFeedbackAheadPlusY() { double velocity = 1; // constant speed double acceleration = 0; - ModelR3 measurement = new ModelR3( + ModelSE2 measurement = new ModelSE2( currentState, new VelocitySE2(1, 0, 0)); // feedforward should be straight ahead, no rotation. - ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); + ModelSE2 currentReference = ModelSE2.fromTimedState(new TimedState(state, t, velocity, acceleration)); DeltaSE2 err = DeltaSE2.delta(measurement.pose(), currentReference.pose()); VelocitySE2 speeds = controller.positionFeedback(err); // setpoint should be negative y @@ -484,7 +486,7 @@ void testFeedbackAheadPlusY() { @Test void testFeedbackAheadPlusTheta() { - FullStateControllerR3 controller = new FullStateControllerR3(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, + FullStateControllerSE2 controller = new FullStateControllerSE2(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, 0.02); // measurement is plus-theta Pose2d currentState = new Pose2d(0, 0, new Rotation2d(1.0)); @@ -503,11 +505,11 @@ void testFeedbackAheadPlusTheta() { double velocity = 1; // constant speed double acceleration = 0; - ModelR3 measurement = new ModelR3( + ModelSE2 measurement = new ModelSE2( currentState, new VelocitySE2(1, 0, 0)); // feedforward should be straight ahead, no rotation. - ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); + ModelSE2 currentReference = ModelSE2.fromTimedState(new TimedState(state, t, velocity, acceleration)); DeltaSE2 err = DeltaSE2.delta(measurement.pose(), currentReference.pose()); VelocitySE2 speeds = controller.positionFeedback(err); // robot is on the setpoint in translation @@ -520,7 +522,7 @@ void testFeedbackAheadPlusTheta() { @Test void testFeedbackSideways() { - FullStateControllerR3 controller = new FullStateControllerR3(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, + FullStateControllerSE2 controller = new FullStateControllerSE2(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, 0.02); // measurement is at the origin, facing down the y axis @@ -540,10 +542,10 @@ void testFeedbackSideways() { double velocity = 1; // constant speed double acceleration = 0; - ModelR3 measurement = new ModelR3( + ModelSE2 measurement = new ModelSE2( currentState, new VelocitySE2(1, 0, 0)); - ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); + ModelSE2 currentReference = ModelSE2.fromTimedState(new TimedState(state, t, velocity, acceleration)); DeltaSE2 err = DeltaSE2.delta(measurement.pose(), currentReference.pose()); VelocitySE2 speeds = controller.positionFeedback(err); // on target @@ -554,7 +556,7 @@ void testFeedbackSideways() { @Test void testFeedbackSidewaysPlusY() { - FullStateControllerR3 controller = new FullStateControllerR3(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, + FullStateControllerSE2 controller = new FullStateControllerSE2(logger, 1, 1, 0, 0, 0.01, 0.02, 0.01, 0.02); // measurement is plus-y, facing down the y axis Pose2d currentState = new Pose2d(0, 1, Rotation2d.kCCW_Pi_2); @@ -573,10 +575,10 @@ void testFeedbackSidewaysPlusY() { double velocity = 1; // constant speed double acceleration = 0; - ModelR3 measurement = new ModelR3( + ModelSE2 measurement = new ModelSE2( currentState, new VelocitySE2(1, 0, 0)); - ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); + ModelSE2 currentReference = ModelSE2.fromTimedState(new TimedState(state, t, velocity, acceleration)); DeltaSE2 err = DeltaSE2.delta(measurement.pose(), currentReference.pose()); VelocitySE2 speeds = controller.positionFeedback(err); // feedback is -y field relative @@ -587,7 +589,7 @@ void testFeedbackSidewaysPlusY() { @Test void testFullFeedbackAhead() { - FullStateControllerR3 controller = new FullStateControllerR3(logger, 1, 1, 1, 1, 0.01, 0.02, 0.01, + FullStateControllerSE2 controller = new FullStateControllerSE2(logger, 1, 1, 1, 1, 0.01, 0.02, 0.01, 0.02); // measurement is at the origin, facing ahead Pose2d currentState = new Pose2d(); @@ -607,10 +609,10 @@ void testFullFeedbackAhead() { // constant speed double acceleration = 0; // motion is on setpoint - ModelR3 measurement = new ModelR3( + ModelSE2 measurement = new ModelSE2( currentState, new VelocitySE2(1, 0, 0)); - ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); + ModelSE2 currentReference = ModelSE2.fromTimedState(new TimedState(state, t, velocity, acceleration)); DeltaSE2 perr = DeltaSE2.delta(measurement.pose(), currentReference.pose()); VelocitySE2 verr = currentReference.velocity().minus(measurement.velocity()); VelocitySE2 speeds = controller.fullFeedback(perr, verr); @@ -622,7 +624,7 @@ void testFullFeedbackAhead() { @Test void testFullFeedbackSideways() { - FullStateControllerR3 controller = new FullStateControllerR3(logger, 1, 1, 1, 1, 0.01, 0.02, 0.01, + FullStateControllerSE2 controller = new FullStateControllerSE2(logger, 1, 1, 1, 1, 0.01, 0.02, 0.01, 0.02); // measurement is at the origin, facing +y @@ -643,10 +645,10 @@ void testFullFeedbackSideways() { // constant speed double acceleration = 0; // measurement is too slow - ModelR3 measurement = new ModelR3( + ModelSE2 measurement = new ModelSE2( currentPose, new VelocitySE2(0.5, 0, 0)); - ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); + ModelSE2 currentReference = ModelSE2.fromTimedState(new TimedState(state, t, velocity, acceleration)); DeltaSE2 perr = DeltaSE2.delta(measurement.pose(), currentReference.pose()); VelocitySE2 verr = currentReference.velocity().minus(measurement.velocity()); VelocitySE2 speeds = controller.fullFeedback(perr, verr); @@ -658,12 +660,12 @@ void testFullFeedbackSideways() { @Test void testFullFeedbackSidewaysWithRotation() { - FullStateControllerR3 controller = new FullStateControllerR3(logger, 1, 1, 1, 1, 0.01, 0.02, 0.01, + FullStateControllerSE2 controller = new FullStateControllerSE2(logger, 1, 1, 1, 1, 0.01, 0.02, 0.01, 0.02); // measurement is ahead in x and y and theta // measurement is too slow - ModelR3 measurement = new ModelR3( + ModelSE2 measurement = new ModelSE2( new Pose2d(0.1, 0.1, Rotation2d.kCCW_Pi_2.plus(new Rotation2d(0.1))), new VelocitySE2(0.5, 0, 0)); @@ -684,7 +686,7 @@ void testFullFeedbackSidewaysWithRotation() { // constant speed double acceleration = 0; - ModelR3 currentReference = ModelR3.fromTimedState(new TimedState(state, t, velocity, acceleration)); + ModelSE2 currentReference = ModelSE2.fromTimedState(new TimedState(state, t, velocity, acceleration)); // feedforward should be straight ahead, no rotation. DeltaSE2 perr = DeltaSE2.delta(measurement.pose(), currentReference.pose()); diff --git a/lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java b/lib/src/test/java/org/team100/lib/controller/se2/ReferenceControllerSE2Test.java similarity index 82% rename from lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java rename to lib/src/test/java/org/team100/lib/controller/se2/ReferenceControllerSE2Test.java index 7e3a4f35..470a9fbc 100644 --- a/lib/src/test/java/org/team100/lib/controller/r3/ReferenceControllerR3Test.java +++ b/lib/src/test/java/org/team100/lib/controller/se2/ReferenceControllerSE2Test.java @@ -1,4 +1,4 @@ -package org.team100.lib.controller.r3; +package org.team100.lib.controller.se2; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertFalse; @@ -9,16 +9,19 @@ import java.util.List; import org.junit.jupiter.api.Test; +import org.team100.lib.controller.se2.ControllerFactorySE2; +import org.team100.lib.controller.se2.ControllerSE2; +import org.team100.lib.controller.se2.FullStateControllerSE2; import org.team100.lib.geometry.DirectionSE2; import org.team100.lib.geometry.VelocitySE2; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; -import org.team100.lib.reference.r3.TrajectoryReferenceR3; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.r3.MockSubsystemR3; -import org.team100.lib.subsystems.r3.commands.helper.VelocityReferenceControllerR3; +import org.team100.lib.reference.se2.TrajectoryReferenceSE2; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.se2.MockSubsystemSE2; +import org.team100.lib.subsystems.se2.commands.helper.VelocityReferenceControllerSE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamicsFactory; import org.team100.lib.testing.Timeless; @@ -27,15 +30,15 @@ import org.team100.lib.trajectory.examples.TrajectoryExamples; import org.team100.lib.trajectory.path.Path100; import org.team100.lib.trajectory.path.PathFactory; -import org.team100.lib.trajectory.timing.TrajectoryFactory; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.TimingConstraintFactory; +import org.team100.lib.trajectory.timing.TrajectoryFactory; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -public class ReferenceControllerR3Test implements Timeless { +public class ReferenceControllerSE2Test implements Timeless { private static final boolean DEBUG = false; private static final double DELTA = 0.001; @@ -55,13 +58,13 @@ void testTrajectoryStart() { new Pose2d(1, 0, Rotation2d.kZero)); // first state is motionless assertEquals(0, t.sample(0).velocityM_S(), DELTA); - ControllerR3 controller = ControllerFactoryR3.test(logger); + ControllerSE2 controller = ControllerFactorySE2.test(logger); // initially at rest - MockSubsystemR3 drive = new MockSubsystemR3(new ModelR3()); + MockSubsystemSE2 drive = new MockSubsystemSE2(new ModelSE2()); - TrajectoryReferenceR3 reference = new TrajectoryReferenceR3(logger, t); - VelocityReferenceControllerR3 c = new VelocityReferenceControllerR3( + TrajectoryReferenceSE2 reference = new TrajectoryReferenceSE2(logger, t); + VelocityReferenceControllerSE2 c = new VelocityReferenceControllerSE2( logger, drive, controller, reference); stepTime(); @@ -111,13 +114,13 @@ void testTrajectoryDone() { new Pose2d(1, 0, Rotation2d.kZero)); // first state is motionless assertEquals(0, t.sample(0).velocityM_S(), DELTA); - ControllerR3 controller = ControllerFactoryR3.test(logger); + ControllerSE2 controller = ControllerFactorySE2.test(logger); // initially at rest - MockSubsystemR3 drive = new MockSubsystemR3(new ModelR3()); + MockSubsystemSE2 drive = new MockSubsystemSE2(new ModelSE2()); - TrajectoryReferenceR3 reference = new TrajectoryReferenceR3(logger, t); - VelocityReferenceControllerR3 c = new VelocityReferenceControllerR3( + TrajectoryReferenceSE2 reference = new TrajectoryReferenceSE2(logger, t); + VelocityReferenceControllerSE2 c = new VelocityReferenceControllerSE2( logger, drive, controller, reference); // the measurement never changes but that doesn't affect "done" as far as the @@ -128,7 +131,7 @@ void testTrajectoryDone() { if (DEBUG) System.out.printf("%s\n", drive.m_setpoint); // we have magically reached the end (immediately) - drive.m_state = new ModelR3(new Pose2d(1, 0, Rotation2d.kZero)); + drive.m_state = new ModelSE2(new Pose2d(1, 0, Rotation2d.kZero)); } assertTrue(c.isDone()); @@ -167,16 +170,16 @@ void testFieldRelativeTrajectory() { if (DEBUG) System.out.printf("TRAJECTORY:\n%s\n", trajectory); - FullStateControllerR3 swerveController = new FullStateControllerR3( + FullStateControllerSE2 swerveController = new FullStateControllerSE2( logger, 2.4, 2.4, 0.1, 0.1, 0.01, 0.02, 0.01, 0.02); - MockSubsystemR3 drive = new MockSubsystemR3(new ModelR3()); - TrajectoryReferenceR3 reference = new TrajectoryReferenceR3(logger, trajectory); - VelocityReferenceControllerR3 referenceController = new VelocityReferenceControllerR3( + MockSubsystemSE2 drive = new MockSubsystemSE2(new ModelSE2()); + TrajectoryReferenceSE2 reference = new TrajectoryReferenceSE2(logger, trajectory); + VelocityReferenceControllerSE2 referenceController = new VelocityReferenceControllerSE2( logger, drive, swerveController, reference); Pose2d pose = trajectory.sample(0).state().getPose().pose(); @@ -188,7 +191,7 @@ void testFieldRelativeTrajectory() { if (++i > 500) break; stepTime(); - drive.m_state = new ModelR3(pose, velocity); + drive.m_state = new ModelSE2(pose, velocity); referenceController.execute(); velocity = drive.m_recentSetpoint; pose = new Pose2d( diff --git a/lib/src/test/java/org/team100/lib/localization/AprilTagRobotLocalizerPerformanceTest.java b/lib/src/test/java/org/team100/lib/localization/AprilTagRobotLocalizerPerformanceTest.java index 61fb5988..9ec221c8 100644 --- a/lib/src/test/java/org/team100/lib/localization/AprilTagRobotLocalizerPerformanceTest.java +++ b/lib/src/test/java/org/team100/lib/localization/AprilTagRobotLocalizerPerformanceTest.java @@ -14,7 +14,7 @@ import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -36,7 +36,7 @@ void testEstimateRobotPose2() throws IOException { AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); List poseEstimate = new ArrayList(); List timeEstimate = new ArrayList(); - DoubleFunction history = t -> new ModelR3(new Rotation2d(-Math.PI / 4)); + DoubleFunction history = t -> new ModelSE2(new Rotation2d(-Math.PI / 4)); VisionUpdater visionUpdater = new VisionUpdater() { @Override diff --git a/lib/src/test/java/org/team100/lib/localization/AprilTagRobotLocalizerTest.java b/lib/src/test/java/org/team100/lib/localization/AprilTagRobotLocalizerTest.java index 236bb81a..98c4d9f1 100644 --- a/lib/src/test/java/org/team100/lib/localization/AprilTagRobotLocalizerTest.java +++ b/lib/src/test/java/org/team100/lib/localization/AprilTagRobotLocalizerTest.java @@ -14,7 +14,7 @@ import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.testing.Timeless; import edu.wpi.first.hal.AllianceStationID; @@ -41,7 +41,7 @@ void testEndToEnd() throws IOException, InterruptedException { // these lists receive the updates final List poseEstimate = new ArrayList(); final List timeEstimate = new ArrayList(); - DoubleFunction history = t -> new ModelR3(); + DoubleFunction history = t -> new ModelSE2(); VisionUpdater visionUpdater = new VisionUpdater() { @Override @@ -112,7 +112,7 @@ void testEstimateRobotPose() throws IOException { final List poseEstimate = new ArrayList(); final List timeEstimate = new ArrayList(); - DoubleFunction history = t -> new ModelR3(); + DoubleFunction history = t -> new ModelSE2(); VisionUpdater visionUpdater = new VisionUpdater() { @Override @@ -164,7 +164,7 @@ void testEstimateRobotPose2() throws IOException { AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); final List poseEstimate = new ArrayList(); final List timeEstimate = new ArrayList(); - DoubleFunction history = t -> new ModelR3(new Rotation2d(-Math.PI / 4)); + DoubleFunction history = t -> new ModelSE2(new Rotation2d(-Math.PI / 4)); VisionUpdater visionUpdater = new VisionUpdater() { @Override public void put(double t, Pose2d p, double[] sd1, double[] sd2) { @@ -241,7 +241,7 @@ void testCase1() throws IOException { AprilTagFieldLayoutWithCorrectOrientation layout = new AprilTagFieldLayoutWithCorrectOrientation(); - DoubleFunction history = t -> new ModelR3(new Rotation2d(3 * Math.PI / 4)); + DoubleFunction history = t -> new ModelSE2(new Rotation2d(3 * Math.PI / 4)); VisionUpdater visionUpdater = new VisionUpdater() { @Override @@ -286,7 +286,7 @@ void testCase2() throws IOException { assertEquals(1.914, tag4pose.getY(), DELTA); assertEquals(1.868, tag4pose.getZ(), DELTA); - DoubleFunction history = t -> new ModelR3(new Rotation2d(Math.PI)); + DoubleFunction history = t -> new ModelSE2(new Rotation2d(Math.PI)); VisionUpdater visionUpdater = new VisionUpdater() { @Override public void put(double t, Pose2d p, double[] sd1, double[] sd2) { @@ -324,7 +324,7 @@ void testCase2WithOffset() throws IOException { assertEquals(1.914, tag4pose.getY(), DELTA); assertEquals(1.868, tag4pose.getZ(), DELTA); - DoubleFunction history = t -> new ModelR3(new Rotation2d(Math.PI)); + DoubleFunction history = t -> new ModelSE2(new Rotation2d(Math.PI)); VisionUpdater visionUpdater = new VisionUpdater() { @Override public void put(double t, Pose2d p, double[] sd1, double[] sd2) { @@ -365,7 +365,7 @@ void testCase2WithTriangulation() throws IOException { assertEquals(1.914, tag4pose.getY(), DELTA); assertEquals(1.868, tag4pose.getZ(), DELTA); - DoubleFunction history = t -> new ModelR3(new Rotation2d(Math.PI)); + DoubleFunction history = t -> new ModelSE2(new Rotation2d(Math.PI)); VisionUpdater visionUpdater = new VisionUpdater() { @Override @@ -408,7 +408,7 @@ void testCase2tilt() throws IOException { assertEquals(1.914, tag4pose.getY(), DELTA); assertEquals(1.868, tag4pose.getZ(), DELTA); - DoubleFunction history = t -> new ModelR3(new Rotation2d(Math.PI)); + DoubleFunction history = t -> new ModelSE2(new Rotation2d(Math.PI)); VisionUpdater visionUpdater = new VisionUpdater() { @Override @@ -450,7 +450,7 @@ void testCase3() throws IOException { assertEquals(1.914, tag4pose.getY(), DELTA); assertEquals(1.868, tag4pose.getZ(), DELTA); - DoubleFunction history = t -> new ModelR3(new Rotation2d(Math.PI)); + DoubleFunction history = t -> new ModelSE2(new Rotation2d(Math.PI)); VisionUpdater visionUpdater = new VisionUpdater() { @Override @@ -490,7 +490,7 @@ void testCase4() throws IOException { assertEquals(1.914, tag4pose.getY(), DELTA); assertEquals(1.868, tag4pose.getZ(), DELTA); - DoubleFunction history = t -> new ModelR3(new Rotation2d(-3 * Math.PI / 4)); + DoubleFunction history = t -> new ModelSE2(new Rotation2d(-3 * Math.PI / 4)); VisionUpdater visionUpdater = new VisionUpdater() { @Override @@ -530,7 +530,7 @@ void testCase5() throws IOException { assertEquals(1.914, tag4pose.getY(), DELTA); assertEquals(1.868, tag4pose.getZ(), DELTA); - DoubleFunction history = t -> new ModelR3(new Rotation2d(3 * Math.PI / 4)); + DoubleFunction history = t -> new ModelSE2(new Rotation2d(3 * Math.PI / 4)); VisionUpdater visionUpdater = new VisionUpdater() { @Override @@ -569,7 +569,7 @@ void testCase6() throws IOException { assertEquals(1.914, tag4pose.getY(), DELTA); assertEquals(1.868, tag4pose.getZ(), DELTA); - DoubleFunction history = t -> new ModelR3(new Rotation2d(3 * Math.PI / 4)); + DoubleFunction history = t -> new ModelSE2(new Rotation2d(3 * Math.PI / 4)); VisionUpdater visionUpdater = new VisionUpdater() { @Override @@ -611,7 +611,7 @@ void testCase7() throws IOException { assertEquals(1.914, tag4pose.getY(), DELTA); assertEquals(1.868, tag4pose.getZ(), DELTA); - DoubleFunction history = t -> new ModelR3(new Rotation2d(3 * Math.PI / 4)); + DoubleFunction history = t -> new ModelSE2(new Rotation2d(3 * Math.PI / 4)); VisionUpdater visionUpdater = new VisionUpdater() { @Override diff --git a/lib/src/test/java/org/team100/lib/localization/InterpolationRecordTest.java b/lib/src/test/java/org/team100/lib/localization/InterpolationRecordTest.java index d50d9a1d..ebf722bc 100644 --- a/lib/src/test/java/org/team100/lib/localization/InterpolationRecordTest.java +++ b/lib/src/test/java/org/team100/lib/localization/InterpolationRecordTest.java @@ -9,7 +9,7 @@ import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; import org.team100.lib.state.Model100; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamicsFactory; import org.team100.lib.subsystems.swerve.module.state.SwerveModulePosition100; @@ -28,7 +28,7 @@ class InterpolationRecordTest implements Timeless { void testInterp0() { // initially at rest, finally in motion. // what does the interpolator do? - ModelR3 s0 = new ModelR3(); + ModelSE2 s0 = new ModelSE2(); SwerveModulePositions p0 = new SwerveModulePositions( new SwerveModulePosition100(0, Optional.empty()), new SwerveModulePosition100(0, Optional.empty()), @@ -46,7 +46,7 @@ void testInterp0() { // so // 1 = 1/2 * 1 * t; t = 2, a = 0.5. - ModelR3 s1 = new ModelR3(new Model100(), new Model100(), new Model100(1, 1)); + ModelSE2 s1 = new ModelSE2(new Model100(), new Model100(), new Model100(1, 1)); SwerveModulePositions p1 = new SwerveModulePositions( new SwerveModulePosition100(Math.sqrt(2) / 2, Optional.of(new Rotation2d(3 * Math.PI / 4))), new SwerveModulePosition100(Math.sqrt(2) / 2, Optional.of(new Rotation2d(Math.PI / 4))), @@ -85,7 +85,7 @@ void testInterp0() { @Test void testInterp1() { - ModelR3 s0 = new ModelR3(); + ModelSE2 s0 = new ModelSE2(); // initally driving straight SwerveModulePositions p0 = new SwerveModulePositions( new SwerveModulePosition100(0, Optional.of(new Rotation2d())), @@ -104,7 +104,7 @@ void testInterp1() { // so // 1 = 1/2 * 1 * t; t = 2, a = 0.5. - ModelR3 s1 = new ModelR3(new Model100(), new Model100(), new Model100(1, 1)); + ModelSE2 s1 = new ModelSE2(new Model100(), new Model100(), new Model100(1, 1)); SwerveModulePositions p1 = new SwerveModulePositions( new SwerveModulePosition100(Math.sqrt(2) / 2, Optional.of(new Rotation2d(3 * Math.PI / 4))), new SwerveModulePosition100(Math.sqrt(2) / 2, Optional.of(new Rotation2d(Math.PI / 4))), diff --git a/lib/src/test/java/org/team100/lib/localization/SimulatedTagDetectorTest.java b/lib/src/test/java/org/team100/lib/localization/SimulatedTagDetectorTest.java index 484397f1..8eff61d4 100644 --- a/lib/src/test/java/org/team100/lib/localization/SimulatedTagDetectorTest.java +++ b/lib/src/test/java/org/team100/lib/localization/SimulatedTagDetectorTest.java @@ -10,7 +10,7 @@ import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; import org.team100.lib.config.Camera; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import edu.wpi.first.hal.AllianceStationID; import edu.wpi.first.hal.HAL; @@ -47,7 +47,7 @@ void testSimple() throws IOException { SimulatedTagDetector sim = new SimulatedTagDetector( cameras, layout, - x -> new ModelR3(new Pose2d(2.6576, 4.0259, Rotation2d.kZero))); + x -> new ModelSE2(new Pose2d(2.6576, 4.0259, Rotation2d.kZero))); // sim uses alliance from driver station DriverStationSim.setAllianceStationId(AllianceStationID.Red1); DriverStationSim.notifyNewData(); diff --git a/lib/src/test/java/org/team100/lib/localization/SwerveDrivePoseEstimator100Test.java b/lib/src/test/java/org/team100/lib/localization/SwerveDrivePoseEstimator100Test.java index 8b6f1c24..0ae29071 100644 --- a/lib/src/test/java/org/team100/lib/localization/SwerveDrivePoseEstimator100Test.java +++ b/lib/src/test/java/org/team100/lib/localization/SwerveDrivePoseEstimator100Test.java @@ -17,7 +17,7 @@ import org.team100.lib.logging.primitive.TestPrimitiveLogger; import org.team100.lib.sensor.gyro.Gyro; import org.team100.lib.sensor.gyro.MockGyro; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamicsFactory; import org.team100.lib.subsystems.swerve.module.state.SwerveModulePosition100; @@ -51,14 +51,14 @@ class SwerveDrivePoseEstimator100Test implements Timeless { private SwerveModulePositions positions; - private static void verify(double x, ModelR3 state) { + private static void verify(double x, ModelSE2 state) { Pose2d estimate = state.pose(); assertEquals(x, estimate.getX(), DELTA); assertEquals(0, estimate.getY(), DELTA); assertEquals(0, estimate.getRotation().getRadians(), DELTA); } - private static void verifyVelocity(double xV, ModelR3 state) { + private static void verifyVelocity(double xV, ModelSE2 state) { VelocitySE2 v = state.velocity(); assertEquals(xV, v.x(), DELTA); } @@ -859,7 +859,7 @@ public void periodic() { } ou.update(t); - ModelR3 xHat = estimator.apply(t); + ModelSE2 xHat = estimator.apply(t); double error = groundTruthState.poseMeters.getTranslation().getDistance(xHat.pose().getTranslation()); if (error > maxError) { diff --git a/lib/src/test/java/org/team100/lib/profile/r3/HolonomicProfileTest.java b/lib/src/test/java/org/team100/lib/profile/se2/HolonomicProfileTest.java similarity index 81% rename from lib/src/test/java/org/team100/lib/profile/r3/HolonomicProfileTest.java rename to lib/src/test/java/org/team100/lib/profile/se2/HolonomicProfileTest.java index c6061630..ae7ee5af 100644 --- a/lib/src/test/java/org/team100/lib/profile/r3/HolonomicProfileTest.java +++ b/lib/src/test/java/org/team100/lib/profile/se2/HolonomicProfileTest.java @@ -1,4 +1,4 @@ -package org.team100.lib.profile.r3; +package org.team100.lib.profile.se2; import static org.junit.jupiter.api.Assertions.assertEquals; @@ -8,8 +8,10 @@ import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; -import org.team100.lib.state.ControlR3; -import org.team100.lib.state.ModelR3; +import org.team100.lib.profile.se2.HolonomicProfile; +import org.team100.lib.profile.se2.HolonomicProfileFactory; +import org.team100.lib.state.ControlSE2; +import org.team100.lib.state.ModelSE2; import org.team100.lib.testing.Timeless; import edu.wpi.first.math.geometry.Pose2d; @@ -23,9 +25,9 @@ class HolonomicProfileTest implements Timeless { @Test void testSolve() { HolonomicProfile hp = HolonomicProfileFactory.trapezoidal(logger, 1, 1, 0.01, 1, 1, 0.01); - ModelR3 i = new ModelR3( + ModelSE2 i = new ModelSE2( new Pose2d(0, 0, Rotation2d.kZero), new VelocitySE2(1, 0, 0)); - ModelR3 g = new ModelR3( + ModelSE2 g = new ModelSE2( new Pose2d(0, 2, Rotation2d.kZero), new VelocitySE2(0, 0, 0)); hp.solve(i, g); // scale factors @@ -45,10 +47,10 @@ void testSolve() { @Test void test2d() { HolonomicProfile hp = HolonomicProfileFactory.trapezoidal(logger, 1, 1, 0.01, 1, 1, 0.01); - ModelR3 i = new ModelR3(); - ModelR3 g = new ModelR3(new Pose2d(1, 5, Rotation2d.kZero)); + ModelSE2 i = new ModelSE2(); + ModelSE2 g = new ModelSE2(new Pose2d(1, 5, Rotation2d.kZero)); hp.solve(i, g); - ControlR3 s = i.control(); + ControlSE2 s = i.control(); for (double t = 0; t < 10; t += 0.02) { s = hp.calculate(s.model(), g); if (DEBUG) @@ -63,10 +65,10 @@ void test2d() { @Test void test2dExp() { HolonomicProfile hp = HolonomicProfileFactory.currentLimitedExponential(1, 1, 2, 1, 1, 2); - ModelR3 i = new ModelR3(); - ModelR3 g = new ModelR3(new Pose2d(1, 5, Rotation2d.kZero)); + ModelSE2 i = new ModelSE2(); + ModelSE2 g = new ModelSE2(new Pose2d(1, 5, Rotation2d.kZero)); hp.solve(i, g); - ControlR3 s = i.control(); + ControlSE2 s = i.control(); for (double t = 0; t < 10; t += 0.02) { s = hp.calculate(s.model(), g); if (DEBUG) @@ -77,10 +79,10 @@ void test2dExp() { @Test void test2dWithEntrySpeed() { HolonomicProfile hp = HolonomicProfileFactory.trapezoidal(logger, 1, 1, 0.01, 1, 1, 0.01); - ModelR3 i = new ModelR3(new Pose2d(), new VelocitySE2(1, 0, 0)); - ModelR3 g = new ModelR3(new Pose2d(0, 1, Rotation2d.kZero)); + ModelSE2 i = new ModelSE2(new Pose2d(), new VelocitySE2(1, 0, 0)); + ModelSE2 g = new ModelSE2(new Pose2d(0, 1, Rotation2d.kZero)); hp.solve(i, g); - ControlR3 s = i.control(); + ControlSE2 s = i.control(); for (double t = 0; t < 10; t += 0.02) { s = hp.calculate(s.model(), g); if (DEBUG) @@ -104,8 +106,8 @@ void test2dWithEntrySpeed() { @Test void testSolvePerformance() { HolonomicProfile hp = HolonomicProfileFactory.trapezoidal(logger, 1, 1, 0.01, 1, 1, 0.01); - ModelR3 i = new ModelR3(new Pose2d(), new VelocitySE2(1, 0, 0)); - ModelR3 g = new ModelR3(new Pose2d(0, 1, Rotation2d.kZero)); + ModelSE2 i = new ModelSE2(new Pose2d(), new VelocitySE2(1, 0, 0)); + ModelSE2 g = new ModelSE2(new Pose2d(0, 1, Rotation2d.kZero)); int N = 10000; double t0 = Takt.actual(); for (int ii = 0; ii < N; ++ii) { diff --git a/lib/src/test/java/org/team100/lib/reference/r3/ProfileReferenceR3Test.java b/lib/src/test/java/org/team100/lib/reference/se2/ProfileReferenceSE2Test.java similarity index 70% rename from lib/src/test/java/org/team100/lib/reference/r3/ProfileReferenceR3Test.java rename to lib/src/test/java/org/team100/lib/reference/se2/ProfileReferenceSE2Test.java index c8e1bc08..a420ced8 100644 --- a/lib/src/test/java/org/team100/lib/reference/r3/ProfileReferenceR3Test.java +++ b/lib/src/test/java/org/team100/lib/reference/se2/ProfileReferenceSE2Test.java @@ -1,4 +1,4 @@ -package org.team100.lib.reference.r3; +package org.team100.lib.reference.se2; import static org.junit.jupiter.api.Assertions.assertEquals; @@ -6,41 +6,41 @@ import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; -import org.team100.lib.profile.r3.HolonomicProfile; -import org.team100.lib.profile.r3.HolonomicProfileFactory; -import org.team100.lib.state.ControlR3; -import org.team100.lib.state.ModelR3; +import org.team100.lib.profile.se2.HolonomicProfile; +import org.team100.lib.profile.se2.HolonomicProfileFactory; +import org.team100.lib.state.ControlSE2; +import org.team100.lib.state.ModelSE2; import org.team100.lib.testing.Timeless; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -public class ProfileReferenceR3Test implements Timeless { +public class ProfileReferenceSE2Test implements Timeless { private static final double DELTA = 0.001; private final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); @Test void testSimple() { - ModelR3 measurement = new ModelR3(new Pose2d(0, 0, Rotation2d.kZero)); - ModelR3 goal = new ModelR3(new Pose2d(1, 0, Rotation2d.kZero)); + ModelSE2 measurement = new ModelSE2(new Pose2d(0, 0, Rotation2d.kZero)); + ModelSE2 goal = new ModelSE2(new Pose2d(1, 0, Rotation2d.kZero)); HolonomicProfile hp = HolonomicProfileFactory.trapezoidal(logger, 1, 1, 0.01, 1, 1, 0.01); - ProfileReferenceR3 r = new ProfileReferenceR3(logger, hp, "test"); + ProfileReferenceSE2 r = new ProfileReferenceSE2(logger, hp, "test"); r.setGoal(goal); r.initialize(measurement); { - ModelR3 c = r.current(); + ModelSE2 c = r.current(); assertEquals(0, c.velocity().x(), DELTA); assertEquals(0, c.pose().getX(), DELTA); - ControlR3 n = r.next(); + ControlSE2 n = r.next(); assertEquals(0.02, n.velocity().x(), DELTA); assertEquals(0, n.pose().getX(), DELTA); } // no time step, nothing changes { - ModelR3 c = r.current(); + ModelSE2 c = r.current(); assertEquals(0, c.velocity().x(), DELTA); assertEquals(0, c.pose().getX(), DELTA); - ControlR3 n = r.next(); + ControlSE2 n = r.next(); assertEquals(0.02, n.velocity().x(), DELTA); // x is very small but not zero assertEquals(0.0002, n.pose().getX(), 0.00001); @@ -48,10 +48,10 @@ void testSimple() { // stepping time gets the next references stepTime(); { - ModelR3 c = r.current(); + ModelSE2 c = r.current(); assertEquals(0.02, c.velocity().x(), DELTA); assertEquals(0, c.pose().getX(), DELTA); - ControlR3 n = r.next(); + ControlSE2 n = r.next(); assertEquals(0.04, n.velocity().x(), DELTA); assertEquals(0.00078, n.pose().getX(), DELTA); } @@ -60,10 +60,10 @@ void testSimple() { stepTime(); } { - ModelR3 c = r.current(); + ModelSE2 c = r.current(); assertEquals(0, c.velocity().x(), DELTA); assertEquals(1, c.pose().getX(), DELTA); - ControlR3 n = r.next(); + ControlSE2 n = r.next(); assertEquals(0, n.velocity().x(), DELTA); assertEquals(1, n.pose().getX(), DELTA); } diff --git a/lib/src/test/java/org/team100/lib/reference/r3/TrajectoryReferenceTest.java b/lib/src/test/java/org/team100/lib/reference/se2/TrajectoryReferenceTest.java similarity index 85% rename from lib/src/test/java/org/team100/lib/reference/r3/TrajectoryReferenceTest.java rename to lib/src/test/java/org/team100/lib/reference/se2/TrajectoryReferenceTest.java index 2798b0cb..81e409e4 100644 --- a/lib/src/test/java/org/team100/lib/reference/r3/TrajectoryReferenceTest.java +++ b/lib/src/test/java/org/team100/lib/reference/se2/TrajectoryReferenceTest.java @@ -1,4 +1,4 @@ -package org.team100.lib.reference.r3; +package org.team100.lib.reference.se2; import static org.junit.jupiter.api.Assertions.assertEquals; @@ -9,8 +9,8 @@ import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; -import org.team100.lib.state.ControlR3; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ControlSE2; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamicsFactory; import org.team100.lib.testing.Timeless; @@ -42,23 +42,23 @@ void testSimple() { Trajectory100 t = ex.restToRest( new Pose2d(0, 0, Rotation2d.kZero), new Pose2d(1, 0, Rotation2d.kZero)); - TrajectoryReferenceR3 r = new TrajectoryReferenceR3(logger, t); + TrajectoryReferenceSE2 r = new TrajectoryReferenceSE2(logger, t); // measurement is irrelevant - r.initialize(new ModelR3()); + r.initialize(new ModelSE2()); { - ModelR3 c = r.current(); + ModelSE2 c = r.current(); assertEquals(0, c.velocity().x(), DELTA); assertEquals(0, c.pose().getX(), DELTA); - ControlR3 n = r.next(); + ControlSE2 n = r.next(); assertEquals(0.033, n.velocity().x(), DELTA); assertEquals(0, n.pose().getX(), DELTA); } // no time step, nothing changes { - ModelR3 c = r.current(); + ModelSE2 c = r.current(); assertEquals(0, c.velocity().x(), DELTA); assertEquals(0, c.pose().getX(), DELTA); - ControlR3 n = r.next(); + ControlSE2 n = r.next(); assertEquals(0.033, n.velocity().x(), DELTA); // x is very small but not zero assertEquals(0.0003266, n.pose().getX(), 0.0000001); @@ -66,10 +66,10 @@ void testSimple() { // stepping time gets the next references stepTime(); { - ModelR3 c = r.current(); + ModelSE2 c = r.current(); assertEquals(0.033, c.velocity().x(), DELTA); assertEquals(0, c.pose().getX(), DELTA); - ControlR3 n = r.next(); + ControlSE2 n = r.next(); assertEquals(0.065, n.velocity().x(), DELTA); assertEquals(0.001, n.pose().getX(), DELTA); } @@ -78,10 +78,10 @@ void testSimple() { stepTime(); } { - ModelR3 c = r.current(); + ModelSE2 c = r.current(); assertEquals(0, c.velocity().x(), DELTA); assertEquals(1, c.pose().getX(), DELTA); - ControlR3 n = r.next(); + ControlSE2 n = r.next(); assertEquals(0, n.velocity().x(), DELTA); assertEquals(1, n.pose().getX(), DELTA); } diff --git a/lib/src/test/java/org/team100/lib/subsystems/prr/AnalyticalJacobianTest.java b/lib/src/test/java/org/team100/lib/subsystems/prr/AnalyticalJacobianTest.java index 25cdb68d..a526cdde 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/prr/AnalyticalJacobianTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/prr/AnalyticalJacobianTest.java @@ -10,16 +10,16 @@ import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; -import org.team100.lib.state.ControlR3; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ControlSE2; +import org.team100.lib.state.ModelSE2; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.TrajectoryPlanner; import org.team100.lib.trajectory.examples.TrajectoryExamples; import org.team100.lib.trajectory.path.PathFactory; import org.team100.lib.trajectory.timing.ConstantConstraint; -import org.team100.lib.trajectory.timing.TrajectoryFactory; import org.team100.lib.trajectory.timing.TimedState; import org.team100.lib.trajectory.timing.TimingConstraint; +import org.team100.lib.trajectory.timing.TrajectoryFactory; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -83,7 +83,7 @@ void testInverse() { // some example velocities // zero velocity - ModelR3 v = new ModelR3(p); + ModelSE2 v = new ModelSE2(p); JointVelocities jv = j.inverse(v); assertEquals(0, jv.elevator(), DELTA); @@ -91,21 +91,21 @@ void testInverse() { assertEquals(0, jv.wrist(), DELTA); // +x - v = new ModelR3(p, new VelocitySE2(1, 0, 0)); + v = new ModelSE2(p, new VelocitySE2(1, 0, 0)); jv = j.inverse(v); assertEquals(1, jv.elevator(), DELTA); assertEquals(0, jv.shoulder(), DELTA); assertEquals(0, jv.wrist(), DELTA); // +y - v = new ModelR3(p, new VelocitySE2(0, 1, 0)); + v = new ModelSE2(p, new VelocitySE2(0, 1, 0)); jv = j.inverse(v); assertEquals(0, jv.elevator(), DELTA); assertEquals(0.5, jv.shoulder(), DELTA); assertEquals(-0.5, jv.wrist(), DELTA); // +theta - v = new ModelR3(p, new VelocitySE2(0, 0, 1)); + v = new ModelSE2(p, new VelocitySE2(0, 0, 1)); jv = j.inverse(v); assertEquals(0, jv.elevator(), DELTA); assertEquals(-0.5, jv.shoulder(), DELTA); @@ -180,7 +180,7 @@ void testInverseA() { EAWConfig c = new EAWConfig(0, 0, 0); Pose2d p = k.forward(c); VelocitySE2 v = new VelocitySE2(0, 0, 0); - ControlR3 m = new ControlR3(p, v, new AccelerationSE2(0, 0, 0)); + ControlSE2 m = new ControlSE2(p, v, new AccelerationSE2(0, 0, 0)); JointAccelerations ja = j.inverseA(m); assertEquals(0, ja.elevator(), DELTA); assertEquals(0, ja.shoulder(), DELTA); @@ -190,7 +190,7 @@ void testInverseA() { c = new EAWConfig(0, 0, 0); p = k.forward(c); v = new VelocitySE2(0, 0, 0); - m = new ControlR3(p, v, new AccelerationSE2(1, 0, 0)); + m = new ControlSE2(p, v, new AccelerationSE2(1, 0, 0)); ja = j.inverseA(m); assertEquals(1, ja.elevator(), DELTA); assertEquals(0, ja.shoulder(), DELTA); @@ -200,7 +200,7 @@ void testInverseA() { c = new EAWConfig(0, 0, 0); p = k.forward(c); v = new VelocitySE2(0, 0, 0); - m = new ControlR3(p, v, new AccelerationSE2(0, 1, 0)); + m = new ControlSE2(p, v, new AccelerationSE2(0, 1, 0)); ja = j.inverseA(m); assertEquals(0, ja.elevator(), DELTA); assertEquals(0.5, ja.shoulder(), DELTA); @@ -210,7 +210,7 @@ void testInverseA() { c = new EAWConfig(0, 0, 0); p = k.forward(c); v = new VelocitySE2(0, 0, 0); - m = new ControlR3(p, v, new AccelerationSE2(0, 0, 1)); + m = new ControlSE2(p, v, new AccelerationSE2(0, 0, 1)); ja = j.inverseA(m); assertEquals(0, ja.elevator(), DELTA); assertEquals(-0.5, ja.shoulder(), DELTA); @@ -221,7 +221,7 @@ void testInverseA() { c = new EAWConfig(0, Math.PI / 4, 0); p = k.forward(c); v = new VelocitySE2(0, 0, 0); - m = new ControlR3(p, v, new AccelerationSE2(1, 0, 0)); + m = new ControlSE2(p, v, new AccelerationSE2(1, 0, 0)); ja = j.inverseA(m); assertEquals(1, ja.elevator(), DELTA); assertEquals(0, ja.shoulder(), DELTA); @@ -232,7 +232,7 @@ void testInverseA() { // using 45 deg because of singularity at 90 p = k.forward(c); v = new VelocitySE2(0, 0, 0); - m = new ControlR3(p, v, new AccelerationSE2(0, 1, 0)); + m = new ControlSE2(p, v, new AccelerationSE2(0, 1, 0)); ja = j.inverseA(m); assertEquals(1, ja.elevator(), DELTA); assertEquals(0.707, ja.shoulder(), DELTA); @@ -243,7 +243,7 @@ void testInverseA() { // using 45 deg because of singularity at 90 p = k.forward(c); v = new VelocitySE2(0, 0, 0); - m = new ControlR3(p, v, new AccelerationSE2(0, 1, 0)); + m = new ControlSE2(p, v, new AccelerationSE2(0, 1, 0)); ja = j.inverseA(m); assertEquals(1, ja.elevator(), DELTA); assertEquals(0.707, ja.shoulder(), DELTA); @@ -267,7 +267,7 @@ void testTrajectory() { double dt = d / 20; for (double time = 0; time < d; time += dt) { TimedState tp = t.sample(time); - ModelR3 sm = ModelR3.fromTimedState(tp); + ModelSE2 sm = ModelSE2.fromTimedState(tp); Pose2d p = sm.pose(); VelocitySE2 v = sm.velocity(); EAWConfig c = k.inverse(p); diff --git a/lib/src/test/java/org/team100/lib/subsystems/prr/JacobianTest.java b/lib/src/test/java/org/team100/lib/subsystems/prr/JacobianTest.java index 83889227..14d2893f 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/prr/JacobianTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/prr/JacobianTest.java @@ -12,7 +12,7 @@ import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; import org.team100.lib.optimization.NumericalJacobian100; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.TrajectoryPlanner; import org.team100.lib.trajectory.examples.TrajectoryExamples; @@ -134,7 +134,7 @@ void test05() { // some example velocities // zero velocity - ModelR3 v = new ModelR3(p); + ModelSE2 v = new ModelSE2(p); JointVelocities jv = j.inverse(v); assertEquals(0, jv.elevator(), DELTA); @@ -142,21 +142,21 @@ void test05() { assertEquals(0, jv.wrist(), DELTA); // +x - v = new ModelR3(p, new VelocitySE2(1, 0, 0)); + v = new ModelSE2(p, new VelocitySE2(1, 0, 0)); jv = j.inverse(v); assertEquals(1, jv.elevator(), DELTA); assertEquals(0, jv.shoulder(), DELTA); assertEquals(0, jv.wrist(), DELTA); // +y - v = new ModelR3(p, new VelocitySE2(0, 1, 0)); + v = new ModelSE2(p, new VelocitySE2(0, 1, 0)); jv = j.inverse(v); assertEquals(0, jv.elevator(), DELTA); assertEquals(0.5, jv.shoulder(), DELTA); assertEquals(-0.5, jv.wrist(), DELTA); // +theta - v = new ModelR3(p, new VelocitySE2(0, 0, 1)); + v = new ModelSE2(p, new VelocitySE2(0, 0, 1)); jv = j.inverse(v); assertEquals(0, jv.elevator(), DELTA); assertEquals(-0.5, jv.shoulder(), DELTA); @@ -244,7 +244,7 @@ void testTrajectory() { double dt = d / 20; for (double time = 0; time < d; time += dt) { TimedState tp = t.sample(time); - ModelR3 sm = ModelR3.fromTimedState(tp); + ModelSE2 sm = ModelSE2.fromTimedState(tp); Pose2d p = sm.pose(); VelocitySE2 v = sm.velocity(); EAWConfig c = k.inverse(p); diff --git a/lib/src/test/java/org/team100/lib/subsystems/r3/MockSubsystemR3.java b/lib/src/test/java/org/team100/lib/subsystems/se2/MockSubsystemSE2.java similarity index 56% rename from lib/src/test/java/org/team100/lib/subsystems/r3/MockSubsystemR3.java rename to lib/src/test/java/org/team100/lib/subsystems/se2/MockSubsystemSE2.java index 55998205..e1c2f4ee 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/r3/MockSubsystemR3.java +++ b/lib/src/test/java/org/team100/lib/subsystems/se2/MockSubsystemSE2.java @@ -1,19 +1,20 @@ -package org.team100.lib.subsystems.r3; +package org.team100.lib.subsystems.se2; import org.team100.lib.geometry.VelocitySE2; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.se2.VelocitySubsystemSE2; -public class MockSubsystemR3 implements VelocitySubsystemR3 { +public class MockSubsystemSE2 implements VelocitySubsystemSE2 { public VelocitySE2 m_setpoint; public VelocitySE2 m_recentSetpoint; - public ModelR3 m_state; + public ModelSE2 m_state; - public MockSubsystemR3(ModelR3 initial) { + public MockSubsystemSE2(ModelSE2 initial) { m_state = initial; } @Override - public ModelR3 getState() { + public ModelSE2 getState() { return m_state; } diff --git a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithProfileTest.java b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/DriveToPoseWithProfileTest.java similarity index 70% rename from lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithProfileTest.java rename to lib/src/test/java/org/team100/lib/subsystems/se2/commands/DriveToPoseWithProfileTest.java index 11eb2214..84461f05 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveToPoseWithProfileTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/DriveToPoseWithProfileTest.java @@ -1,15 +1,16 @@ -package org.team100.lib.subsystems.r3.commands; +package org.team100.lib.subsystems.se2.commands; import org.junit.jupiter.api.Test; -import org.team100.lib.controller.r3.ControllerFactoryR3; -import org.team100.lib.controller.r3.FullStateControllerR3; +import org.team100.lib.controller.se2.ControllerFactorySE2; +import org.team100.lib.controller.se2.FullStateControllerSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; -import org.team100.lib.profile.r3.HolonomicProfile; -import org.team100.lib.profile.r3.HolonomicProfileFactory; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.r3.MockSubsystemR3; +import org.team100.lib.profile.se2.HolonomicProfile; +import org.team100.lib.profile.se2.HolonomicProfileFactory; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.se2.MockSubsystemSE2; +import org.team100.lib.subsystems.se2.commands.DriveToPoseWithProfile; import org.team100.lib.testing.Timeless; import edu.wpi.first.math.geometry.Pose2d; @@ -24,8 +25,8 @@ public class DriveToPoseWithProfileTest implements Timeless { private static final boolean DEBUG = false; LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger()); - MockSubsystemR3 subsystem = new MockSubsystemR3(new ModelR3()); - FullStateControllerR3 controller = ControllerFactoryR3.test(log); + MockSubsystemSE2 subsystem = new MockSubsystemSE2(new ModelSE2()); + FullStateControllerSE2 controller = ControllerFactorySE2.test(log); HolonomicProfile profile = HolonomicProfileFactory.wpi(1, 1, 1, 1); /** @@ -41,7 +42,7 @@ void testDemo() { System.out.println("x, y, theta"); for (int i = 0; i < 200; ++i) { drive.execute(); - subsystem.m_state = new ModelR3(subsystem.m_state.pose(), subsystem.m_setpoint); + subsystem.m_state = new ModelSE2(subsystem.m_state.pose(), subsystem.m_setpoint); subsystem.m_state = subsystem.m_state.evolve(0.02); Pose2d p = subsystem.m_state.pose(); if (DEBUG) diff --git a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveWithTrajectoryFunctionTest.java b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/DriveWithTrajectoryFunctionTest.java similarity index 82% rename from lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveWithTrajectoryFunctionTest.java rename to lib/src/test/java/org/team100/lib/subsystems/se2/commands/DriveWithTrajectoryFunctionTest.java index fb40fff4..1146893f 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/DriveWithTrajectoryFunctionTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/DriveWithTrajectoryFunctionTest.java @@ -1,16 +1,17 @@ -package org.team100.lib.subsystems.r3.commands; +package org.team100.lib.subsystems.se2.commands; import java.util.List; import org.junit.jupiter.api.Test; -import org.team100.lib.controller.r3.ControllerFactoryR3; -import org.team100.lib.controller.r3.FullStateControllerR3; +import org.team100.lib.controller.se2.ControllerFactorySE2; +import org.team100.lib.controller.se2.FullStateControllerSE2; import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.r3.MockSubsystemR3; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.se2.MockSubsystemSE2; +import org.team100.lib.subsystems.se2.commands.DriveWithTrajectoryFunction; import org.team100.lib.testing.Timeless; import org.team100.lib.trajectory.Trajectory100; import org.team100.lib.trajectory.TrajectoryPlanner; @@ -35,8 +36,8 @@ public class DriveWithTrajectoryFunctionTest implements Timeless { private static final boolean DEBUG = false; LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger()); - MockSubsystemR3 subsystem = new MockSubsystemR3(new ModelR3()); - FullStateControllerR3 controller = ControllerFactoryR3.test(log); + MockSubsystemSE2 subsystem = new MockSubsystemSE2(new ModelSE2()); + FullStateControllerSE2 controller = ControllerFactorySE2.test(log); TrajectoryVisualization viz = new TrajectoryVisualization(log); List constraints = List.of( new ConstantConstraint(log, 2, 2), @@ -66,7 +67,7 @@ void testDemo() { System.out.println("x, y, theta"); for (int i = 0; i < 200; ++i) { drive.execute(); - subsystem.m_state = new ModelR3(subsystem.m_state.pose(), subsystem.m_setpoint); + subsystem.m_state = new ModelSE2(subsystem.m_state.pose(), subsystem.m_setpoint); subsystem.m_state = subsystem.m_state.evolve(0.02); Pose2d p = subsystem.m_state.pose(); if (DEBUG) diff --git a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/PushbroomTest.java b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/PushbroomTest.java similarity index 95% rename from lib/src/test/java/org/team100/lib/subsystems/r3/commands/PushbroomTest.java rename to lib/src/test/java/org/team100/lib/subsystems/se2/commands/PushbroomTest.java index 24ecd814..4e3684bd 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/PushbroomTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/PushbroomTest.java @@ -1,8 +1,9 @@ -package org.team100.lib.subsystems.r3.commands; +package org.team100.lib.subsystems.se2.commands; import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; +import org.team100.lib.subsystems.se2.commands.Pushbroom; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectoryListFunctionTest.java b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryListFunctionTest.java similarity index 90% rename from lib/src/test/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectoryListFunctionTest.java rename to lib/src/test/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryListFunctionTest.java index 1583b6eb..23aaf6bf 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectoryListFunctionTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryListFunctionTest.java @@ -1,4 +1,4 @@ -package org.team100.lib.subsystems.r3.commands.test; +package org.team100.lib.subsystems.se2.commands.test; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertFalse; @@ -9,22 +9,23 @@ import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; -import org.team100.lib.controller.r3.ControllerFactoryR3; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerFactorySE2; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.experiments.Experiment; import org.team100.lib.experiments.Experiments; import org.team100.lib.framework.TimedRobot100; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; +import org.team100.lib.subsystems.se2.commands.test.DriveWithTrajectoryListFunction; import org.team100.lib.subsystems.swerve.Fixture; import org.team100.lib.testing.Timeless; import org.team100.lib.trajectory.TrajectoryPlanner; import org.team100.lib.trajectory.examples.TrajectoryExamples; import org.team100.lib.trajectory.path.PathFactory; -import org.team100.lib.trajectory.timing.TrajectoryFactory; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.TimingConstraintFactory; +import org.team100.lib.trajectory.timing.TrajectoryFactory; import org.team100.lib.visualization.TrajectoryVisualization; import edu.wpi.first.wpilibj.DataLogManager; @@ -51,7 +52,7 @@ void testSimple() throws IOException { // this initial step is required since the timebase is different? stepTime(); Experiments.instance.testOverride(Experiment.UseSetpointGenerator, true); - ControllerR3 control = ControllerFactoryR3.test(logger); + ControllerSE2 control = ControllerFactorySE2.test(logger); DriveWithTrajectoryListFunction c = new DriveWithTrajectoryListFunction( logger, fixture.drive, diff --git a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectoryTest.java b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryTest.java similarity index 92% rename from lib/src/test/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectoryTest.java rename to lib/src/test/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryTest.java index fd7bb6b1..96d6fc2f 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectoryTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryTest.java @@ -1,4 +1,4 @@ -package org.team100.lib.subsystems.r3.commands.test; +package org.team100.lib.subsystems.se2.commands.test; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; @@ -7,8 +7,8 @@ import java.util.List; import org.junit.jupiter.api.Test; -import org.team100.lib.controller.r3.ControllerFactoryR3; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerFactorySE2; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.experiments.Experiment; import org.team100.lib.experiments.Experiments; import org.team100.lib.localization.AprilTagFieldLayoutWithCorrectOrientation; @@ -22,8 +22,9 @@ import org.team100.lib.logging.primitive.TestPrimitiveLogger; import org.team100.lib.sensor.gyro.Gyro; import org.team100.lib.sensor.gyro.SimulatedGyro; -import org.team100.lib.state.ModelR3; -import org.team100.lib.subsystems.r3.MockSubsystemR3; +import org.team100.lib.state.ModelSE2; +import org.team100.lib.subsystems.se2.MockSubsystemSE2; +import org.team100.lib.subsystems.se2.commands.test.DriveWithTrajectory; import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem; import org.team100.lib.subsystems.swerve.SwerveLocal; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; @@ -36,9 +37,9 @@ import org.team100.lib.trajectory.TrajectoryPlanner; import org.team100.lib.trajectory.examples.TrajectoryExamples; import org.team100.lib.trajectory.path.PathFactory; -import org.team100.lib.trajectory.timing.TrajectoryFactory; import org.team100.lib.trajectory.timing.TimingConstraint; import org.team100.lib.trajectory.timing.TimingConstraintFactory; +import org.team100.lib.trajectory.timing.TrajectoryFactory; import org.team100.lib.visualization.TrajectoryVisualization; import edu.wpi.first.math.geometry.Pose2d; @@ -64,10 +65,10 @@ void testTrajectoryStart() { new Pose2d(1, 0, Rotation2d.kZero)); // first state is motionless assertEquals(0, t.sample(0).velocityM_S(), DELTA); - ControllerR3 controller = ControllerFactoryR3.test(logger); + ControllerSE2 controller = ControllerFactorySE2.test(logger); // initially at rest - MockSubsystemR3 d = new MockSubsystemR3(new ModelR3()); + MockSubsystemSE2 d = new MockSubsystemSE2(new ModelSE2()); DriveWithTrajectory c = new DriveWithTrajectory(logger, d, controller, t, viz); @@ -121,10 +122,10 @@ void testTrajectoryDone() { new Pose2d(1, 0, Rotation2d.kZero)); // first state is motionless assertEquals(0, t.sample(0).velocityM_S(), DELTA); - ControllerR3 controller = ControllerFactoryR3.test(logger); + ControllerSE2 controller = ControllerFactorySE2.test(logger); // initially at rest - MockSubsystemR3 d = new MockSubsystemR3(new ModelR3()); + MockSubsystemSE2 d = new MockSubsystemSE2(new ModelSE2()); DriveWithTrajectory c = new DriveWithTrajectory(logger, d, controller, t, viz); c.initialize(); @@ -133,7 +134,7 @@ void testTrajectoryDone() { stepTime(); c.execute(); // we have magically reached the end - d.m_state = new ModelR3(new Pose2d(1, 0, Rotation2d.kZero)); + d.m_state = new ModelSE2(new Pose2d(1, 0, Rotation2d.kZero)); } assertTrue(c.isDone()); @@ -164,7 +165,7 @@ void testRealDrive() throws IOException { new Pose2d(1, 0, Rotation2d.kZero)); // first state is motionless assertEquals(0, trajectory.sample(0).velocityM_S(), DELTA); - ControllerR3 controller = ControllerFactoryR3.test(logger); + ControllerSE2 controller = ControllerFactorySE2.test(logger); Gyro gyro = new SimulatedGyro(logger, swerveKinodynamics, collection); SwerveHistory history = new SwerveHistory( diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/Fixture.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/Fixture.java index 18ba1d7d..256fae9c 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/Fixture.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/Fixture.java @@ -2,8 +2,8 @@ import java.io.IOException; -import org.team100.lib.controller.r3.ControllerFactoryR3; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerFactorySE2; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.localization.AprilTagFieldLayoutWithCorrectOrientation; import org.team100.lib.localization.AprilTagRobotLocalizer; import org.team100.lib.localization.FreshSwerveEstimate; @@ -41,7 +41,7 @@ public class Fixture { public SwerveLocal swerveLocal; public OdometryUpdater odometryUpdater; public SwerveDriveSubsystem drive; - public ControllerR3 controller; + public ControllerSE2 controller; public LoggerFactory logger; public LoggerFactory fieldLogger; @@ -81,7 +81,7 @@ public Fixture() throws IOException { swerveLocal, limiter); - controller = ControllerFactoryR3.test(logger); + controller = ControllerFactorySE2.test(logger); } public void close() { diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/RealisticFixture.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/RealisticFixture.java index 0069b3ce..fe740d88 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/RealisticFixture.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/RealisticFixture.java @@ -2,8 +2,8 @@ import java.io.IOException; -import org.team100.lib.controller.r3.ControllerFactoryR3; -import org.team100.lib.controller.r3.ControllerR3; +import org.team100.lib.controller.se2.ControllerFactorySE2; +import org.team100.lib.controller.se2.ControllerSE2; import org.team100.lib.localization.AprilTagFieldLayoutWithCorrectOrientation; import org.team100.lib.localization.AprilTagRobotLocalizer; import org.team100.lib.localization.FreshSwerveEstimate; @@ -40,7 +40,7 @@ public class RealisticFixture { public SwerveKinodynamics swerveKinodynamics; public SwerveLocal swerveLocal; public SwerveDriveSubsystem drive; - public ControllerR3 controller; + public ControllerSE2 controller; public LoggerFactory logger; public LoggerFactory fieldLogger; @@ -78,7 +78,7 @@ public RealisticFixture() throws IOException { swerveLocal, limiter); - controller = ControllerFactoryR3.test(logger); + controller = ControllerFactorySE2.test(logger); } public void close() { diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java index 4a9c13d0..05cb3d3e 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java @@ -3,10 +3,10 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.geometry.VelocitySE2; -import org.team100.lib.state.ControlR3; +import org.team100.lib.geometry.WaypointSE2; +import org.team100.lib.state.ControlSE2; import org.team100.lib.trajectory.timing.TimedState; import edu.wpi.first.math.geometry.Pose2d; @@ -21,13 +21,13 @@ class SwerveControlTest { void testTransform() { Pose2d p = new Pose2d(new Translation2d(1, 1), new Rotation2d(1)); VelocitySE2 t = new VelocitySE2(1, 1, 1); - ControlR3 s = new ControlR3(p, t); + ControlSE2 s = new ControlSE2(p, t); assertEquals(1, s.x().x(), DELTA); } @Test void testTimedState() { - ControlR3 s = ControlR3.fromTimedState( + ControlSE2 s = ControlSE2.fromTimedState( new TimedState( new Pose2dWithMotion( WaypointSE2.irrotational( @@ -44,7 +44,7 @@ void testTimedState() { @Test void testTimedState2() { - ControlR3 s = ControlR3.fromTimedState( + ControlSE2 s = ControlSE2.fromTimedState( new TimedState( new Pose2dWithMotion( WaypointSE2.irrotational( @@ -61,7 +61,7 @@ void testTimedState2() { @Test void testTimedState3() { - ControlR3 s = ControlR3.fromTimedState( + ControlSE2 s = ControlSE2.fromTimedState( new TimedState( new Pose2dWithMotion( WaypointSE2.irrotational( @@ -79,7 +79,7 @@ void testTimedState3() { /** +x motion, positive curvature => +y accel. */ @Test void testTimedState4() { - ControlR3 s = ControlR3.fromTimedState( + ControlSE2 s = ControlSE2.fromTimedState( new TimedState( new Pose2dWithMotion( WaypointSE2.irrotational( @@ -96,7 +96,7 @@ void testTimedState4() { @Test void testChassisSpeeds0() { - ControlR3 state = new ControlR3( + ControlSE2 state = new ControlSE2( new Pose2d(new Translation2d(0, 0), Rotation2d.kPi), new VelocitySE2(1, 0, 0)); ChassisSpeeds speeds = state.chassisSpeeds(); @@ -107,7 +107,7 @@ void testChassisSpeeds0() { @Test void testChassisSpeeds1() { - ControlR3 state = new ControlR3( + ControlSE2 state = new ControlSE2( new Pose2d(new Translation2d(0, 0), Rotation2d.kCCW_Pi_2), new VelocitySE2(1, 0, 1)); ChassisSpeeds speeds = state.chassisSpeeds(); diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java index ff266cef..9652eee1 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java @@ -7,7 +7,7 @@ import org.team100.lib.geometry.WaypointSE2; import org.team100.lib.geometry.Pose2dWithMotion; import org.team100.lib.geometry.VelocitySE2; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.trajectory.timing.TimedState; import edu.wpi.first.math.geometry.Pose2d; @@ -15,20 +15,20 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -class ModelR3Test { +class ModelSE2Test { private static final double DELTA = 0.001; @Test void testTransform() { Pose2d p = new Pose2d(new Translation2d(1, 1), new Rotation2d(1)); VelocitySE2 t = new VelocitySE2(1, 1, 1); - ModelR3 s = new ModelR3(p, t); + ModelSE2 s = new ModelSE2(p, t); assertEquals(1, s.x().x(), DELTA); } @Test void testTimedState() { - ModelR3 s = ModelR3.fromTimedState( + ModelSE2 s = ModelSE2.fromTimedState( new TimedState( new Pose2dWithMotion( WaypointSE2.irrotational( @@ -45,7 +45,7 @@ void testTimedState() { @Test void testTimedState2() { - ModelR3 s = ModelR3.fromTimedState( + ModelSE2 s = ModelSE2.fromTimedState( new TimedState( new Pose2dWithMotion( WaypointSE2.irrotational( @@ -62,7 +62,7 @@ void testTimedState2() { @Test void testTimedState3() { - ModelR3 s = ModelR3.fromTimedState( + ModelSE2 s = ModelSE2.fromTimedState( new TimedState( new Pose2dWithMotion( WaypointSE2.irrotational( @@ -80,7 +80,7 @@ void testTimedState3() { /** +x motion, positive curvature => +y accel. */ @Test void testTimedState4() { - ModelR3 s = ModelR3.fromTimedState( + ModelSE2 s = ModelSE2.fromTimedState( new TimedState( new Pose2dWithMotion( WaypointSE2.irrotational( @@ -97,7 +97,7 @@ void testTimedState4() { @Test void testChassisSpeeds0() { - ModelR3 state = new ModelR3( + ModelSE2 state = new ModelSE2( new Pose2d(new Translation2d(0, 0), Rotation2d.kPi), new VelocitySE2(1, 0, 0)); ChassisSpeeds speeds = state.chassisSpeeds(); @@ -108,7 +108,7 @@ void testChassisSpeeds0() { @Test void testChassisSpeeds1() { - ModelR3 state = new ModelR3( + ModelSE2 state = new ModelSE2( new Pose2d(new Translation2d(0, 0), Rotation2d.kCCW_Pi_2), new VelocitySE2(1, 0, 1)); ChassisSpeeds speeds = state.chassisSpeeds(); diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualChassisSpeedsTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualChassisSpeedsTest.java index b20e3d06..50565788 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualChassisSpeedsTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualChassisSpeedsTest.java @@ -7,7 +7,7 @@ import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamicsFactory; @@ -22,7 +22,7 @@ void testChassisSpeedsZero() { SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest(logger); ManualChassisSpeeds manual = new ManualChassisSpeeds(logger, limits); Velocity input = new Velocity(0, 0, 0); - ChassisSpeeds speeds = manual.apply(new ModelR3(), input); + ChassisSpeeds speeds = manual.apply(new ModelSE2(), input); assertEquals(0, speeds.vxMetersPerSecond, DELTA); assertEquals(0, speeds.vyMetersPerSecond, DELTA); assertEquals(0, speeds.omegaRadiansPerSecond, DELTA); @@ -36,7 +36,7 @@ void testChassisSpeedsNonzero() { ManualChassisSpeeds manual = new ManualChassisSpeeds(logger, limits); // clipping to the unit circle Velocity input = new Velocity(1, 2, 3); - ChassisSpeeds speeds = manual.apply(new ModelR3(), input); + ChassisSpeeds speeds = manual.apply(new ModelSE2(), input); assertEquals(0.447, speeds.vxMetersPerSecond, DELTA); assertEquals(0.894, speeds.vyMetersPerSecond, DELTA); assertEquals(2.828, speeds.omegaRadiansPerSecond, DELTA); diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualFieldRelativeSpeedsTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualFieldRelativeSpeedsTest.java index f2847aee..748115fd 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualFieldRelativeSpeedsTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualFieldRelativeSpeedsTest.java @@ -8,7 +8,7 @@ import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamicsFactory; @@ -21,7 +21,7 @@ void testTwistZero() { SwerveKinodynamics limits = SwerveKinodynamicsFactory.forTest(logger); ManualFieldRelativeSpeeds manual = new ManualFieldRelativeSpeeds(logger, limits); Velocity input = new Velocity(0, 0, 0); - ModelR3 s = new ModelR3(); + ModelSE2 s = new ModelSE2(); VelocitySE2 twist = manual.apply(s, input); assertEquals(0, twist.x(), DELTA); assertEquals(0, twist.y(), DELTA); @@ -34,7 +34,7 @@ void testTwistNonzero() { ManualFieldRelativeSpeeds manual = new ManualFieldRelativeSpeeds(logger, limits); // these inputs are clipped Velocity input = new Velocity(1, 2, 3); - ModelR3 s = new ModelR3(); + ModelSE2 s = new ModelSE2(); VelocitySE2 twist = manual.apply(s, input); assertEquals(0.447, twist.x(), DELTA); assertEquals(0.894, twist.y(), DELTA); diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithFullStateHeadingTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithFullStateHeadingTest.java index 4aedfff5..a33d9901 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithFullStateHeadingTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithFullStateHeadingTest.java @@ -19,7 +19,7 @@ import org.team100.lib.sensor.gyro.MockGyro; import org.team100.lib.state.Control100; import org.team100.lib.state.Model100; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamicsFactory; @@ -43,11 +43,11 @@ void testModeSwitching() { swerveKinodynamics, rotationSupplier, new double[] { 1.0, 1.0 }); - m_manualWithHeading.reset(new ModelR3()); + m_manualWithHeading.reset(new ModelSE2()); Velocity twist1_1 = new Velocity(0, 0, 0); - VelocitySE2 twistM_S = m_manualWithHeading.apply(new ModelR3(), twist1_1); + VelocitySE2 twistM_S = m_manualWithHeading.apply(new ModelSE2(), twist1_1); verify(0, 0, 0, twistM_S); // with a non-null desired rotation we're in snap mode @@ -55,7 +55,7 @@ void testModeSwitching() { desiredRotation = null; twist1_1 = new Velocity(0, 0, 1); - twistM_S = m_manualWithHeading.apply(new ModelR3(), twist1_1); + twistM_S = m_manualWithHeading.apply(new ModelSE2(), twist1_1); // with a nonzero desired twist, we're out of snap mode assertNull(m_manualWithHeading.m_goal); @@ -72,7 +72,7 @@ void testNotSnapMode() { rotationSupplier, new double[] { 1.0, 1.0 }); - m_manualWithHeading.reset(new ModelR3()); + m_manualWithHeading.reset(new ModelSE2()); // no desired rotation desiredRotation = null; @@ -80,7 +80,7 @@ void testNotSnapMode() { Velocity twist1_1 = new Velocity(0, 0, 1); VelocitySE2 twistM_S = m_manualWithHeading.apply( - new ModelR3(), + new ModelSE2(), twist1_1); // not in snap mode @@ -89,7 +89,7 @@ void testNotSnapMode() { twist1_1 = new Velocity(1, 0, 0); - twistM_S = m_manualWithHeading.apply(new ModelR3(Pose2d.kZero, twistM_S), twist1_1); + twistM_S = m_manualWithHeading.apply(new ModelSE2(Pose2d.kZero, twistM_S), twist1_1); assertNull(m_manualWithHeading.m_goal); verify(1, 0, 0, twistM_S); } @@ -106,7 +106,7 @@ void testSnapMode() { new double[] { 1.0, 0.1 }); // facing +x - m_manualWithHeading.reset(new ModelR3()); + m_manualWithHeading.reset(new ModelSE2()); // reset means setpoint is currentpose. assertEquals(0, m_manualWithHeading.m_thetaSetpoint.x(), DELTA); assertEquals(0, m_manualWithHeading.m_thetaSetpoint.v(), DELTA); @@ -117,7 +117,7 @@ void testSnapMode() { final Velocity zeroVelocity = new Velocity(0, 0, 0); VelocitySE2 twistM_S = m_manualWithHeading.apply( - new ModelR3(), + new ModelSE2(), zeroVelocity); // in snap mode assertNotNull(m_manualWithHeading.m_goal); @@ -137,7 +137,7 @@ void testSnapMode() { Pose2d currentPose = new Pose2d(0, 0, new Rotation2d(0.5)); // cheat the setpoint for the test m_manualWithHeading.m_thetaSetpoint = new Control100(0.5, 1); - twistM_S = m_manualWithHeading.apply(new ModelR3(currentPose, twistM_S), zeroVelocity); + twistM_S = m_manualWithHeading.apply(new ModelSE2(currentPose, twistM_S), zeroVelocity); // setpoint is the goal assertEquals(0.0, m_manualWithHeading.m_thetaSetpoint.v(), DELTA); assertNotNull(m_manualWithHeading.m_goal); @@ -148,7 +148,7 @@ void testSnapMode() { currentPose = new Pose2d(0, 0, new Rotation2d(1.55)); // cheat the setpoint for the test m_manualWithHeading.m_thetaSetpoint = new Control100(1.55, 0.2); - twistM_S = m_manualWithHeading.apply(new ModelR3(currentPose, twistM_S), zeroVelocity); + twistM_S = m_manualWithHeading.apply(new ModelSE2(currentPose, twistM_S), zeroVelocity); // setpoint is the goal assertEquals(0.0, m_manualWithHeading.m_thetaSetpoint.v(), DELTA); assertNotNull(m_manualWithHeading.m_goal); @@ -158,7 +158,7 @@ void testSnapMode() { // done currentPose = new Pose2d(0, 0, new Rotation2d(Math.PI / 2)); m_manualWithHeading.m_thetaSetpoint = new Control100(Math.PI / 2, 0); - twistM_S = m_manualWithHeading.apply(new ModelR3(currentPose, twistM_S), zeroVelocity); + twistM_S = m_manualWithHeading.apply(new ModelSE2(currentPose, twistM_S), zeroVelocity); assertNotNull(m_manualWithHeading.m_goal); // there should be no more profile to follow @@ -180,7 +180,7 @@ void testSnapHeld() { // currently facing +x Pose2d currentPose = Pose2d.kZero; - m_manualWithHeading.reset(new ModelR3()); + m_manualWithHeading.reset(new ModelSE2()); // want to face towards +y desiredRotation = Rotation2d.kCCW_Pi_2; @@ -189,7 +189,7 @@ void testSnapHeld() { // no stick input Velocity twist1_1 = new Velocity(0, 0, 0); VelocitySE2 v = m_manualWithHeading.apply( - new ModelR3(currentPose, new VelocitySE2(0, 0, 0)), + new ModelSE2(currentPose, new VelocitySE2(0, 0, 0)), twist1_1); // in snap mode @@ -203,7 +203,7 @@ void testSnapHeld() { // cheat the setpoint for the test m_manualWithHeading.m_thetaSetpoint = new Control100(0.5, 1); - v = m_manualWithHeading.apply(new ModelR3(currentPose, v), twist1_1); + v = m_manualWithHeading.apply(new ModelSE2(currentPose, v), twist1_1); // the setpoint is the goal assertEquals(0.0, m_manualWithHeading.m_thetaSetpoint.v(), DELTA); assertNotNull(m_manualWithHeading.m_goal); @@ -213,7 +213,7 @@ void testSnapHeld() { currentPose = new Pose2d(0, 0, new Rotation2d(1.555)); // cheat the setpoint for the test m_manualWithHeading.m_thetaSetpoint = new Control100(1.555, 0.2); - v = m_manualWithHeading.apply(new ModelR3(currentPose, v), twist1_1); + v = m_manualWithHeading.apply(new ModelSE2(currentPose, v), twist1_1); // the setpoint is the goal assertEquals(0.0, m_manualWithHeading.m_thetaSetpoint.v(), DELTA); assertNotNull(m_manualWithHeading.m_goal); @@ -224,7 +224,7 @@ void testSnapHeld() { // at the setpoint currentPose = new Pose2d(0, 0, new Rotation2d(Math.PI / 2)); m_manualWithHeading.m_thetaSetpoint = new Control100(Math.PI / 2, 0); - v = m_manualWithHeading.apply(new ModelR3(currentPose, v), twist1_1); + v = m_manualWithHeading.apply(new ModelSE2(currentPose, v), twist1_1); assertNotNull(m_manualWithHeading.m_goal); // there should be no more profile to follow verify(0, 0, 0, v); @@ -246,7 +246,7 @@ void testStickyHeading() { // driver rotates a bit Velocity twist1_1 = new Velocity(0, 0, 1); - ModelR3 currentState = new ModelR3( + ModelSE2 currentState = new ModelSE2( Pose2d.kZero, new VelocitySE2(0, 0, 0)); // no POV @@ -259,7 +259,7 @@ void testStickyHeading() { verify(0, 0, 2.828, v); // already going full speed: - currentState = new ModelR3( + currentState = new ModelSE2( Pose2d.kZero, new VelocitySE2(0, 0, 2.828)); // gyro indicates the correct speed @@ -271,7 +271,7 @@ void testStickyHeading() { // let go of the stick twist1_1 = new Velocity(0, 0, 0); - currentState = new ModelR3( + currentState = new ModelSE2( Pose2d.kZero, new VelocitySE2(0, 0, 2.828)); // gyro rate is still full speed. @@ -302,7 +302,7 @@ void testStickyHeading2() { // driver rotates a bit Velocity twist1_1 = new Velocity(0, 0, 1); - ModelR3 currentState = new ModelR3( + ModelSE2 currentState = new ModelSE2( Pose2d.kZero, new VelocitySE2(0, 0, 0)); // no POV @@ -315,7 +315,7 @@ void testStickyHeading2() { verify(0, 0, 2.828, v); // already going full speed: - currentState = new ModelR3( + currentState = new ModelSE2( Pose2d.kZero, new VelocitySE2(0, 0, 2.828)); // gyro indicates the correct speed @@ -327,7 +327,7 @@ void testStickyHeading2() { // let go of the stick twist1_1 = new Velocity(0, 0, 0); - currentState = new ModelR3( + currentState = new ModelSE2( Pose2d.kZero, new VelocitySE2(0, 0, 2.828)); // gyro rate is still full speed. diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeadingTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeadingTest.java index d2e1f4a8..a1129680 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeadingTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeadingTest.java @@ -20,7 +20,7 @@ import org.team100.lib.sensor.gyro.MockGyro; import org.team100.lib.state.Control100; import org.team100.lib.state.Model100; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamicsFactory; import org.team100.lib.testing.Timeless; @@ -47,11 +47,11 @@ void testModeSwitching() { swerveKinodynamics, rotationSupplier, thetaFeedback); - m_manualWithHeading.reset(new ModelR3()); + m_manualWithHeading.reset(new ModelSE2()); Velocity twist1_1 = new Velocity(0, 0, 0); - VelocitySE2 twistM_S = m_manualWithHeading.apply(new ModelR3(), twist1_1); + VelocitySE2 twistM_S = m_manualWithHeading.apply(new ModelSE2(), twist1_1); verify(0, 0, 0, twistM_S); // with a non-null desired rotation we're in snap mode @@ -59,7 +59,7 @@ void testModeSwitching() { desiredRotation = null; twist1_1 = new Velocity(0, 0, 1); - twistM_S = m_manualWithHeading.apply(new ModelR3(), twist1_1); + twistM_S = m_manualWithHeading.apply(new ModelSE2(), twist1_1); // with a nonzero desired twist, we're out of snap mode assertNull(m_manualWithHeading.m_goal); @@ -78,7 +78,7 @@ void testNotSnapMode() { rotationSupplier, thetaFeedback); - m_manualWithHeading.reset(new ModelR3()); + m_manualWithHeading.reset(new ModelSE2()); // no desired rotation desiredRotation = null; @@ -86,7 +86,7 @@ void testNotSnapMode() { Velocity twist1_1 = new Velocity(0, 0, 1); VelocitySE2 twistM_S = m_manualWithHeading.apply( - new ModelR3(), + new ModelSE2(), twist1_1); // not in snap mode @@ -95,7 +95,7 @@ void testNotSnapMode() { twist1_1 = new Velocity(1, 0, 0); - twistM_S = m_manualWithHeading.apply(new ModelR3(Pose2d.kZero, twistM_S), twist1_1); + twistM_S = m_manualWithHeading.apply(new ModelSE2(Pose2d.kZero, twistM_S), twist1_1); assertNull(m_manualWithHeading.m_goal); verify(1, 0, 0, twistM_S); } @@ -113,7 +113,7 @@ void testSnapMode() { rotationSupplier, thetaFeedback); - m_manualWithHeading.reset(new ModelR3()); + m_manualWithHeading.reset(new ModelSE2()); // reset means setpoint is currentpose. assertEquals(0, m_manualWithHeading.m_thetaSetpoint.x(), DELTA); assertEquals(0, m_manualWithHeading.m_thetaSetpoint.v(), DELTA); @@ -125,7 +125,7 @@ void testSnapMode() { // initial state is motionless VelocitySE2 twistM_S = m_manualWithHeading.apply( - new ModelR3(), + new ModelSE2(), twist1_1); // in snap mode assertNotNull(m_manualWithHeading.m_goal); @@ -145,7 +145,7 @@ void testSnapMode() { // say we've rotated a little. m_manualWithHeading.m_thetaSetpoint = new Control100(0.5, 1); twistM_S = m_manualWithHeading.apply( - new ModelR3( + new ModelSE2( new Pose2d(0, 0, new Rotation2d(0.5)), new VelocitySE2(0, 0, 0.1)), twist1_1); @@ -156,7 +156,7 @@ void testSnapMode() { // mostly rotated m_manualWithHeading.m_thetaSetpoint = new Control100(1.55, 0.2); twistM_S = m_manualWithHeading.apply( - new ModelR3( + new ModelSE2( new Pose2d(0, 0, new Rotation2d(1.55)), new VelocitySE2(0, 0, 0.2)), twist1_1); @@ -168,7 +168,7 @@ void testSnapMode() { // done m_manualWithHeading.m_thetaSetpoint = new Control100(Math.PI / 2, 0); twistM_S = m_manualWithHeading.apply( - new ModelR3( + new ModelSE2( new Pose2d(0, 0, new Rotation2d(Math.PI / 2)), new VelocitySE2(0, 0, 0)), twist1_1); @@ -194,7 +194,7 @@ void testSnapHeld() { thetaFeedback); // currently facing +x - m_manualWithHeading.reset(new ModelR3()); + m_manualWithHeading.reset(new ModelSE2()); // want to face towards +y desiredRotation = Rotation2d.kCCW_Pi_2; @@ -203,7 +203,7 @@ void testSnapHeld() { // no stick input final Velocity twist1_1 = new Velocity(0, 0, 0); VelocitySE2 v = m_manualWithHeading.apply( - new ModelR3(), + new ModelSE2(), twist1_1); // in snap mode @@ -215,7 +215,7 @@ void testSnapHeld() { // say we've rotated a little. m_manualWithHeading.m_thetaSetpoint = new Control100(0.5, 1); v = m_manualWithHeading.apply( - new ModelR3( + new ModelSE2( new Pose2d(0, 0, new Rotation2d(0.5)), new VelocitySE2(0, 0, 1)), twist1_1); @@ -226,7 +226,7 @@ void testSnapHeld() { // mostly rotated, so the FB controller is calm m_manualWithHeading.m_thetaSetpoint = new Control100(1.555, 0.2); v = m_manualWithHeading.apply( - new ModelR3( + new ModelSE2( new Pose2d(0, 0, new Rotation2d(1.555)), new VelocitySE2(0, 0, 0.2)), twist1_1); @@ -239,7 +239,7 @@ void testSnapHeld() { // at the setpoint m_manualWithHeading.m_thetaSetpoint = new Control100(Math.PI / 2, 0); v = m_manualWithHeading.apply( - new ModelR3( + new ModelSE2( new Pose2d(0, 0, new Rotation2d(Math.PI / 2)), new VelocitySE2(0, 0, 0)), twist1_1); @@ -268,7 +268,7 @@ void testStickyHeading() { // driver rotates a bit Velocity control = new Velocity(0, 0, 1); - ModelR3 currentState = new ModelR3( + ModelSE2 currentState = new ModelSE2( Pose2d.kZero, new VelocitySE2(0, 0, 0)); // no POV @@ -281,7 +281,7 @@ void testStickyHeading() { verify(0, 0, 2.828, v); // already going full speed: - currentState = new ModelR3( + currentState = new ModelSE2( Pose2d.kZero, new VelocitySE2(0, 0, 2.828)); // gyro indicates the correct speed @@ -293,7 +293,7 @@ void testStickyHeading() { // let go of the stick control = new Velocity(0, 0, 0); - currentState = new ModelR3( + currentState = new ModelSE2( Pose2d.kZero, new VelocitySE2(0, 0, 2.828)); // gyro rate is still full speed. @@ -339,7 +339,7 @@ void testStickyHeading2() { // driver rotates a bit Velocity twist1_1 = new Velocity(0, 0, 1); - ModelR3 currentState = new ModelR3( + ModelSE2 currentState = new ModelSE2( Pose2d.kZero, new VelocitySE2(0, 0, 0)); // no POV @@ -352,7 +352,7 @@ void testStickyHeading2() { verify(0, 0, 2.828, v); // already going full speed: - currentState = new ModelR3( + currentState = new ModelSE2( Pose2d.kZero, new VelocitySE2(0, 0, 2.828)); // gyro indicates the correct speed @@ -364,7 +364,7 @@ void testStickyHeading2() { // let go of the stick twist1_1 = new Velocity(0, 0, 0); - currentState = new ModelR3( + currentState = new ModelSE2( Pose2d.kZero, new VelocitySE2(0, 0, 2.828)); // gyro rate is still full speed. diff --git a/lib/src/test/java/org/team100/lib/targeting/SimulatedTargetWriterTest.java b/lib/src/test/java/org/team100/lib/targeting/SimulatedTargetWriterTest.java index bf16434c..cba9f1e9 100644 --- a/lib/src/test/java/org/team100/lib/targeting/SimulatedTargetWriterTest.java +++ b/lib/src/test/java/org/team100/lib/targeting/SimulatedTargetWriterTest.java @@ -9,7 +9,7 @@ import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.testing.Timeless; import edu.wpi.first.math.geometry.Translation2d; @@ -27,7 +27,7 @@ void testOne() throws InterruptedException { Thread.sleep(50); stepTime(); - ModelR3 p = new ModelR3(); + ModelSE2 p = new ModelSE2(); Targets reader = new Targets(logger, logger, x -> p); Thread.sleep(50); SimulatedTargetWriter writer = new SimulatedTargetWriter( diff --git a/lib/src/test/java/org/team100/lib/targeting/TargetUtilTest.java b/lib/src/test/java/org/team100/lib/targeting/TargetUtilTest.java index 8f057a7f..99a42aee 100644 --- a/lib/src/test/java/org/team100/lib/targeting/TargetUtilTest.java +++ b/lib/src/test/java/org/team100/lib/targeting/TargetUtilTest.java @@ -4,7 +4,7 @@ import org.junit.jupiter.api.Test; import org.team100.lib.geometry.VelocitySE2; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -46,7 +46,7 @@ void testBearing() { @Test void testTargetMotion() { // at the origin moving 1 m/s +x - ModelR3 state = new ModelR3(new Pose2d(), new VelocitySE2(1, 0, 0)); + ModelSE2 state = new ModelSE2(new Pose2d(), new VelocitySE2(1, 0, 0)); // target is 1m to the left Translation2d target = new Translation2d(0, 1); // so it appears to move clockwise @@ -56,7 +56,7 @@ void testTargetMotion() { @Test void testTargetMotionFaster() { // at the origin moving 2 m/s +x - ModelR3 state = new ModelR3(new Pose2d(), new VelocitySE2(2, 0, 0)); + ModelSE2 state = new ModelSE2(new Pose2d(), new VelocitySE2(2, 0, 0)); // target is 1m to the left Translation2d target = new Translation2d(0, 1); // so it appears to move clockwise @@ -66,7 +66,7 @@ void testTargetMotionFaster() { @Test void testTargetMotionElsewhere() { // somewhere else, moving 1 m/s +x - ModelR3 state = new ModelR3(new Pose2d(1, 1, new Rotation2d()), new VelocitySE2(1, 0, 0)); + ModelSE2 state = new ModelSE2(new Pose2d(1, 1, new Rotation2d()), new VelocitySE2(1, 0, 0)); // target is 1m to the left Translation2d target = new Translation2d(1, 2); // so it appears to move clockwise @@ -76,7 +76,7 @@ void testTargetMotionElsewhere() { @Test void testTargetMotionReverse() { // at the origin, moving 1m/s +x - ModelR3 state = new ModelR3(new Pose2d(), new VelocitySE2(1, 0, 0)); + ModelSE2 state = new ModelSE2(new Pose2d(), new VelocitySE2(1, 0, 0)); // target is 1m to the right Translation2d target = new Translation2d(0, -1); // so it appears to move counterclockwise @@ -86,7 +86,7 @@ void testTargetMotionReverse() { @Test void testTargetMotionAhead() { // at the origin, moving 1m/s +x - ModelR3 state = new ModelR3(new Pose2d(), new VelocitySE2(1, 0, 0)); + ModelSE2 state = new ModelSE2(new Pose2d(), new VelocitySE2(1, 0, 0)); // target is dead ahead Translation2d target = new Translation2d(2, 0); // no apparent motion @@ -96,7 +96,7 @@ void testTargetMotionAhead() { @Test void testTargetMotionOblique() { // at the origin, moving 1m/s +x - ModelR3 state = new ModelR3(new Pose2d(), new VelocitySE2(1, 0, 0)); + ModelSE2 state = new ModelSE2(new Pose2d(), new VelocitySE2(1, 0, 0)); // target is at 45 Translation2d target = new Translation2d(1, 1); // apparent motion is slower @@ -106,7 +106,7 @@ void testTargetMotionOblique() { @Test void testTargetMotionY() { // at the origin, moving 1m/s +y - ModelR3 state = new ModelR3(new Pose2d(), new VelocitySE2(0, 1, 0)); + ModelSE2 state = new ModelSE2(new Pose2d(), new VelocitySE2(0, 1, 0)); // target is dead ahead Translation2d target = new Translation2d(1, 0); // target moves the other way @@ -116,7 +116,7 @@ void testTargetMotionY() { @Test void testTargetMotionYReversed() { // in front of the origin, facing back to it, moving 1m/s +y, - ModelR3 state = new ModelR3( + ModelSE2 state = new ModelSE2( new Pose2d(1, 0, Rotation2d.kPi), new VelocitySE2(0, 1, 0)); // target is dead ahead @@ -128,7 +128,7 @@ void testTargetMotionYReversed() { @Test void testTargetMotionZero() { // not moving, no motion - ModelR3 state = new ModelR3(new Pose2d(), new VelocitySE2(0, 0, 0)); + ModelSE2 state = new ModelSE2(new Pose2d(), new VelocitySE2(0, 0, 0)); // target is 1m to the left Translation2d target = new Translation2d(0, 1); // it should not move diff --git a/lib/src/test/java/org/team100/lib/targeting/TargetsTest.java b/lib/src/test/java/org/team100/lib/targeting/TargetsTest.java index 96940a0c..1697beae 100644 --- a/lib/src/test/java/org/team100/lib/targeting/TargetsTest.java +++ b/lib/src/test/java/org/team100/lib/targeting/TargetsTest.java @@ -12,7 +12,7 @@ import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.testing.Timeless; import edu.wpi.first.math.geometry.Rotation3d; @@ -35,7 +35,7 @@ void testTargets() throws InterruptedException { Thread.sleep(50); stepTime(); - ModelR3 p = new ModelR3(); + ModelSE2 p = new ModelSE2(); Targets t = new Targets(logger, logger, (x) -> p); t.update(); assertTrue(t.getTargets().isEmpty()); @@ -80,7 +80,7 @@ void testTranslations() throws InterruptedException { Thread.sleep(50); stepTime(); - ModelR3 p = new ModelR3(); + ModelSE2 p = new ModelSE2(); Targets reader = new Targets(logger, logger, (x) -> p); Thread.sleep(200); SimulatedTargetWriter writer = new SimulatedTargetWriter( @@ -119,7 +119,7 @@ void testMultipleCameras() throws InterruptedException { Thread.sleep(50); stepTime(); - ModelR3 p = new ModelR3(); + ModelSE2 p = new ModelSE2(); Targets reader = new Targets(logger, logger, (x) -> p); Thread.sleep(100); SimulatedTargetWriter writer = new SimulatedTargetWriter( @@ -159,7 +159,7 @@ void testMultipleTargets() throws InterruptedException { Thread.sleep(100); stepTime(); - ModelR3 p = new ModelR3(); + ModelSE2 p = new ModelSE2(); Targets reader = new Targets(logger, logger, x -> p); Thread.sleep(100); SimulatedTargetWriter writer = new SimulatedTargetWriter( @@ -200,7 +200,7 @@ void testMultipleTargetsAndCameras() throws InterruptedException { Thread.sleep(50); stepTime(); - ModelR3 p = new ModelR3(); + ModelSE2 p = new ModelSE2(); Targets reader = new Targets(logger, logger, (x) -> p); Thread.sleep(50); SimulatedTargetWriter writer = new SimulatedTargetWriter( diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java index 28bfca0c..452d59fc 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java @@ -14,7 +14,7 @@ import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamicsFactory; import org.team100.lib.testing.Timeless; @@ -146,7 +146,7 @@ void testRestToRest() { TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); PathFactory pathFactory = new PathFactory(); TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); - ModelR3 start = new ModelR3(Pose2d.kZero, new VelocitySE2(0, 0, 0)); + ModelSE2 start = new ModelSE2(Pose2d.kZero, new VelocitySE2(0, 0, 0)); Pose2d end = new Pose2d(1, 0, Rotation2d.kZero); TrajectoryExamples ex = new TrajectoryExamples(planner); Trajectory100 trajectory = ex.restToRest(start.pose(), end); @@ -177,7 +177,7 @@ void testMovingToRest() { TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); PathFactory pathFactory = new PathFactory(); TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); - ModelR3 start = new ModelR3(Pose2d.kZero, new VelocitySE2(1, 0, 0)); + ModelSE2 start = new ModelSE2(Pose2d.kZero, new VelocitySE2(1, 0, 0)); Pose2d end = new Pose2d(1, 0, Rotation2d.kZero); VelocitySE2 startVelocity = start.velocity(); @@ -222,7 +222,7 @@ void test2d() { TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints); PathFactory pathFactory = new PathFactory(); TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory); - ModelR3 start = new ModelR3(Pose2d.kZero, new VelocitySE2(0, 1, 0)); + ModelSE2 start = new ModelSE2(Pose2d.kZero, new VelocitySE2(0, 1, 0)); Pose2d end = new Pose2d(1, 0, Rotation2d.kZero); VelocitySE2 startVelocity = start.velocity(); diff --git a/lib/src/test/java/org/team100/lib/visualization/BallR2Test.java b/lib/src/test/java/org/team100/lib/visualization/BallR2Test.java index cc64e887..f0851e8c 100644 --- a/lib/src/test/java/org/team100/lib/visualization/BallR2Test.java +++ b/lib/src/test/java/org/team100/lib/visualization/BallR2Test.java @@ -7,14 +7,14 @@ import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import edu.wpi.first.math.geometry.Rotation2d; public class BallR2Test { private static final boolean DEBUG = false; - ModelR3 robot = new ModelR3(); + ModelSE2 robot = new ModelSE2(); Rotation2d azimuth = new Rotation2d(); /** 2d ball proceeds forever. */ diff --git a/lib/src/test/java/org/team100/lib/visualization/BallR3Test.java b/lib/src/test/java/org/team100/lib/visualization/BallR3Test.java index ae9ecb8f..b0460a99 100644 --- a/lib/src/test/java/org/team100/lib/visualization/BallR3Test.java +++ b/lib/src/test/java/org/team100/lib/visualization/BallR3Test.java @@ -7,7 +7,7 @@ import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.TestLoggerFactory; import org.team100.lib.logging.primitive.TestPrimitiveLogger; -import org.team100.lib.state.ModelR3; +import org.team100.lib.state.ModelSE2; import org.team100.lib.targeting.Drag; import edu.wpi.first.math.geometry.Rotation2d; @@ -16,7 +16,7 @@ public class BallR3Test { private static final boolean DEBUG = false; private static final double DELTA = 0.001; - ModelR3 robot = new ModelR3(); + ModelSE2 robot = new ModelSE2(); Rotation2d azimuth = new Rotation2d(); Rotation2d elevation = new Rotation2d(); From c519aaa487ca46816a0045fd10467d1357211d4f Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Fri, 26 Dec 2025 22:10:24 -0800 Subject: [PATCH 4/4] cleaning --- .../main/java/org/team100/lib/controller/README.md | 3 +-- .../java/org/team100/lib/controller/se2/README.md | 2 +- .../geometry/{Centroid2d.java => CentroidR2.java} | 2 +- ...GlobalVelocity3d.java => GlobalVelocityR3.java} | 14 +++++++------- .../lib/geometry/{Near2d.java => NearR2.java} | 4 ++-- .../main/java/org/team100/lib/geometry/README.md | 4 ++-- .../java/org/team100/lib/localization/README.md | 2 +- .../main/java/org/team100/lib/logging/README.md | 2 +- .../main/java/org/team100/lib/reference/README.md | 2 +- .../java/org/team100/lib/subsystems/se2/README.md | 4 ++-- .../team100/lib/subsystems/se2/commands/README.md | 6 +++--- .../java/org/team100/lib/targeting/Targets.java | 8 ++++---- .../java/org/team100/lib/visualization/BallR3.java | 14 +++++++------- 13 files changed, 33 insertions(+), 34 deletions(-) rename lib/src/main/java/org/team100/lib/geometry/{Centroid2d.java => CentroidR2.java} (88%) rename lib/src/main/java/org/team100/lib/geometry/{GlobalVelocity3d.java => GlobalVelocityR3.java} (64%) rename lib/src/main/java/org/team100/lib/geometry/{Near2d.java => NearR2.java} (79%) diff --git a/lib/src/main/java/org/team100/lib/controller/README.md b/lib/src/main/java/org/team100/lib/controller/README.md index 70d58c48..9caefb78 100644 --- a/lib/src/main/java/org/team100/lib/controller/README.md +++ b/lib/src/main/java/org/team100/lib/controller/README.md @@ -7,5 +7,4 @@ actual mechanism state and applying some control law. There are two packages here: * `r1` for single-dimension control -* `r3` bundles together three independent dimensions, for drivetrain or planar -mechanism control. \ No newline at end of file +* `se2` describes planar mechanisms and drivetrain motion in the SE(2) manifold. \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/controller/se2/README.md b/lib/src/main/java/org/team100/lib/controller/se2/README.md index 86dea5d6..ec2a8380 100644 --- a/lib/src/main/java/org/team100/lib/controller/se2/README.md +++ b/lib/src/main/java/org/team100/lib/controller/se2/README.md @@ -1,4 +1,4 @@ -# lib.controller.r3 +# lib.controller.se2 This package contains control methods for three dimensional coordinates, where the three coordinates are orthogonal. Our high-level diff --git a/lib/src/main/java/org/team100/lib/geometry/Centroid2d.java b/lib/src/main/java/org/team100/lib/geometry/CentroidR2.java similarity index 88% rename from lib/src/main/java/org/team100/lib/geometry/Centroid2d.java rename to lib/src/main/java/org/team100/lib/geometry/CentroidR2.java index 832dfc69..86567e4f 100644 --- a/lib/src/main/java/org/team100/lib/geometry/Centroid2d.java +++ b/lib/src/main/java/org/team100/lib/geometry/CentroidR2.java @@ -6,7 +6,7 @@ import edu.wpi.first.math.geometry.Translation2d; /** Yields the centroid of the translations. */ -public class Centroid2d implements Function, Translation2d> { +public class CentroidR2 implements Function, Translation2d> { @Override public Translation2d apply(Collection c) { Translation2d sum = new Translation2d(); diff --git a/lib/src/main/java/org/team100/lib/geometry/GlobalVelocity3d.java b/lib/src/main/java/org/team100/lib/geometry/GlobalVelocityR3.java similarity index 64% rename from lib/src/main/java/org/team100/lib/geometry/GlobalVelocity3d.java rename to lib/src/main/java/org/team100/lib/geometry/GlobalVelocityR3.java index a2d98ff7..4b420d46 100644 --- a/lib/src/main/java/org/team100/lib/geometry/GlobalVelocity3d.java +++ b/lib/src/main/java/org/team100/lib/geometry/GlobalVelocityR3.java @@ -8,23 +8,23 @@ * This is different from VelocitySE2, which is the companion to Pose2d, * i.e. velocity of planar rigid transforms. */ -public record GlobalVelocity3d(double x, double y, double z) { +public record GlobalVelocityR3(double x, double y, double z) { - public static GlobalVelocity3d fromPolar( + public static GlobalVelocityR3 fromPolar( Rotation2d azimuth, Rotation2d elevation, double speed) { double vx = speed * azimuth.getCos() * elevation.getCos(); double vy = speed * azimuth.getSin() * elevation.getCos(); double vz = speed * elevation.getSin(); - return new GlobalVelocity3d(vx, vy, vz); + return new GlobalVelocityR3(vx, vy, vz); } /** Pick up the translation component of v, in the XY plane. */ - public static GlobalVelocity3d fromSe2(VelocitySE2 v) { - return new GlobalVelocity3d(v.x(), v.y(), 0); + public static GlobalVelocityR3 fromSe2(VelocitySE2 v) { + return new GlobalVelocityR3(v.x(), v.y(), 0); } - public GlobalVelocity3d plus(GlobalVelocity3d other) { - return new GlobalVelocity3d(x + other.x, y + other.y, z + other.z); + public GlobalVelocityR3 plus(GlobalVelocityR3 other) { + return new GlobalVelocityR3(x + other.x, y + other.y, z + other.z); } public double normXY() { diff --git a/lib/src/main/java/org/team100/lib/geometry/Near2d.java b/lib/src/main/java/org/team100/lib/geometry/NearR2.java similarity index 79% rename from lib/src/main/java/org/team100/lib/geometry/Near2d.java rename to lib/src/main/java/org/team100/lib/geometry/NearR2.java index aa924715..82372a92 100644 --- a/lib/src/main/java/org/team100/lib/geometry/Near2d.java +++ b/lib/src/main/java/org/team100/lib/geometry/NearR2.java @@ -5,11 +5,11 @@ import edu.wpi.first.math.geometry.Translation2d; /** True if the two translations are near each other. */ -public class Near2d implements BiPredicate { +public class NearR2 implements BiPredicate { private final double m_threshold; - public Near2d(double threshold) { + public NearR2(double threshold) { m_threshold = threshold; } diff --git a/lib/src/main/java/org/team100/lib/geometry/README.md b/lib/src/main/java/org/team100/lib/geometry/README.md index de9b8d25..6547eefc 100644 --- a/lib/src/main/java/org/team100/lib/geometry/README.md +++ b/lib/src/main/java/org/team100/lib/geometry/README.md @@ -15,9 +15,9 @@ and the correct derivative takes this into account: see `Pose2d.exp()` and We mostly do not use `exp()` and `log()`. We treat global velocity and acceleration as if all the components were independent, -i.e. using the R3 vector space, not the SE(2) Lie group. +i.e. using the R2xS1 vector space, not the SE(2) Lie group. -Why? Because at large scales, we think in R3, e.g. we define +Why? Because at large scales, we think in R2xS1, e.g. we define trajectories and means to follow them without worrying about the coupling in SE(2). We *do* handle SE(2) correctly at the smallest scale, one robot-clock step at a time, where, for example, drivetrain actuation diff --git a/lib/src/main/java/org/team100/lib/localization/README.md b/lib/src/main/java/org/team100/lib/localization/README.md index cde50cbd..380bf323 100644 --- a/lib/src/main/java/org/team100/lib/localization/README.md +++ b/lib/src/main/java/org/team100/lib/localization/README.md @@ -19,7 +19,7 @@ robot (see `Camera`), and each tag relative to the field (see `AprilTagFieldLayoutWithCorrectOrientation`). The main complexity here is __accommodating delay.__ Camera input is delayed between -around 75 and 100, and so it can't be used to directly adjust the robot's __current__ pose +around 75 and 100 ms, and so it can't be used to directly adjust the robot's __current__ pose estimate. Instead, we apply camera input to __old__ pose estimates, recorded by `SwerveHistory`, and then __replay__ the subsequent odometry differentials to get the current pose. diff --git a/lib/src/main/java/org/team100/lib/logging/README.md b/lib/src/main/java/org/team100/lib/logging/README.md index f9447a9e..335c6026 100644 --- a/lib/src/main/java/org/team100/lib/logging/README.md +++ b/lib/src/main/java/org/team100/lib/logging/README.md @@ -1,4 +1,4 @@ -# lib.ogging +# lib.logging This package is what we use to put stuff on the dashboard and in the log file. diff --git a/lib/src/main/java/org/team100/lib/reference/README.md b/lib/src/main/java/org/team100/lib/reference/README.md index cafaecbe..315b2309 100644 --- a/lib/src/main/java/org/team100/lib/reference/README.md +++ b/lib/src/main/java/org/team100/lib/reference/README.md @@ -5,7 +5,7 @@ Classes here supply "references" (controller setpoints). There are two dimensionalities: * R1: one-dimensional, used for single-DOF mechanisms. -* R3: three-dimensional, i.e. `Pose2d`, used for drivetrain and planar mechanism. +* SE(2): planar, i.e. `Pose2d`, used for drivetrain and planar mechanism. The reference sources can wrap two different suppliers: diff --git a/lib/src/main/java/org/team100/lib/subsystems/se2/README.md b/lib/src/main/java/org/team100/lib/subsystems/se2/README.md index 6da73c1a..7e643b64 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/se2/README.md +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/README.md @@ -4,5 +4,5 @@ These general interfaces make it easier to reuse controllers etc for different, but similarly controlled, subsystems. For example, a Mecanum drivetrain and a Swerve drivetrain have some things -in common, and are both velocity controlled in R3, so they are -implementations of `VelocitySubsystemR3`. \ No newline at end of file +in common, and are both velocity controlled in SE(2), so they are +implementations of `VelocitySubsystemSE2`. \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/subsystems/se2/commands/README.md b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/README.md index 43401403..14682fad 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/se2/commands/README.md +++ b/lib/src/main/java/org/team100/lib/subsystems/se2/commands/README.md @@ -1,7 +1,7 @@ -# lib.subsystems.r3.commands +# lib.subsystems.se2.commands -This package contains commands for motion in R3, any space -with three orthogonal axes. These are useful for drivetrains, +This package contains commands for motion in the space of 2-dimensional +rigid transforms, SE(2). These are useful for drivetrains, or any planar mechanism. Commands we used in 2025: diff --git a/lib/src/main/java/org/team100/lib/targeting/Targets.java b/lib/src/main/java/org/team100/lib/targeting/Targets.java index 46f02574..1368536f 100644 --- a/lib/src/main/java/org/team100/lib/targeting/Targets.java +++ b/lib/src/main/java/org/team100/lib/targeting/Targets.java @@ -8,8 +8,8 @@ import org.team100.lib.coherence.Cache; import org.team100.lib.coherence.SideEffect; import org.team100.lib.coherence.Takt; -import org.team100.lib.geometry.Centroid2d; -import org.team100.lib.geometry.Near2d; +import org.team100.lib.geometry.CentroidR2; +import org.team100.lib.geometry.NearR2; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleArrayLogger; @@ -73,8 +73,8 @@ public Targets( m_allTargets = new TrailingHistory<>(HISTORY_DURATION); m_targets = new CoalescingCollection<>( new TrailingHistory<>(HISTORY_DURATION), - new Near2d(NEARNESS_THRESHOLD), - new Centroid2d()); + new NearR2(NEARNESS_THRESHOLD), + new CentroidR2()); m_vision = Cache.ofSideEffect(this::update); } diff --git a/lib/src/main/java/org/team100/lib/visualization/BallR3.java b/lib/src/main/java/org/team100/lib/visualization/BallR3.java index ffc59552..c1e89f7b 100644 --- a/lib/src/main/java/org/team100/lib/visualization/BallR3.java +++ b/lib/src/main/java/org/team100/lib/visualization/BallR3.java @@ -3,7 +3,7 @@ import java.util.function.Supplier; import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.GlobalVelocity3d; +import org.team100.lib.geometry.GlobalVelocityR3; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.logging.LoggerFactory.DoubleArrayLogger; @@ -68,13 +68,13 @@ public BallR3( @Override public void launch() { // Velocity due only to the gun - GlobalVelocity3d v = GlobalVelocity3d.fromPolar( + GlobalVelocityR3 v = GlobalVelocityR3.fromPolar( m_azimuth.get(), m_elevation.get(), m_speed); // velocity due to robot translation - GlobalVelocity3d mv = GlobalVelocity3d.fromSe2(m_robot.get().velocity()); + GlobalVelocityR3 mv = GlobalVelocityR3.fromSe2(m_robot.get().velocity()); // Initial position is on the floor. TODO: offsets. m_location = new Translation3d(m_robot.get().pose().getTranslation()); - GlobalVelocity3d m_velocity = v.plus(mv); + GlobalVelocityR3 m_velocity = v.plus(mv); // velocity in the XY plane double vxy = m_velocity.normXY(); double vz = m_velocity.z(); @@ -122,13 +122,13 @@ Translation3d location() { } // for testing - GlobalVelocity3d velocity() { + GlobalVelocityR3 velocity() { if (m_x == null) { - return GlobalVelocity3d.fromSe2(m_robot.get().velocity()); + return GlobalVelocityR3.fromSe2(m_robot.get().velocity()); } double vxy = m_x.get(3, 0); double vz = m_x.get(4, 0); - return new GlobalVelocity3d( + return new GlobalVelocityR3( vxy * m_az.getCos(), vxy * m_az.getSin(), vz);