Merge pull request #525 from truher/main

profile allows over-limit entry, also some GTSAM fiddling

comp/swerve100/src/main/java/org/team100/frc2024/RobotContainer.java

@@ -82,6 +82,7 @@
 import org.team100.lib.profile.HolonomicProfile;
 import org.team100.lib.sensors.Gyro;
 import org.team100.lib.sensors.GyroFactory;
+import org.team100.lib.swerve.AsymSwerveSetpointGenerator;
 import org.team100.lib.timing.ConstantConstraint;
 import org.team100.lib.trajectory.StraightLineTrajectory;
 import org.team100.lib.trajectory.TrajectoryMaker;
@@ -93,6 +94,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
@@ -174,7 +176,11 @@ public RobotContainer(TimedRobot100 robot) throws IOException {
                 m_layout,
                 poseEstimator);
 
-        final SwerveLocal swerveLocal = new SwerveLocal(driveLog, swerveKinodynamics, m_modules);
+        final AsymSwerveSetpointGenerator setpointGenerator = new AsymSwerveSetpointGenerator(
+                driveLog,
+                swerveKinodynamics,
+                RobotController::getBatteryVoltage);
+        final SwerveLocal swerveLocal = new SwerveLocal(driveLog, swerveKinodynamics, setpointGenerator, m_modules);
 
         m_drive = new SwerveDriveSubsystem(
                 fieldLogger,
@@ -258,11 +264,11 @@ public RobotContainer(TimedRobot100 robot) throws IOException {
         final DriveTrajectoryFollower drivePID = driveControllerFactory.goodPIDF(PIDFlog);
 
         whileTrue(driverControl::driveToNote,
-        new ParallelDeadlineGroup(new DriveWithProfileRotation(
-                () -> Optional.of(new Translation2d()),
-                m_drive,
-                halfFullStateController,
-                swerveKinodynamics), intake.run(intake::intakeSmart)));
+                new ParallelDeadlineGroup(new DriveWithProfileRotation(
+                        () -> Optional.of(new Translation2d()),
+                        m_drive,
+                        halfFullStateController,
+                        swerveKinodynamics), intake.run(intake::intakeSmart)));
         whileTrue(driverControl::actualCircle,
                 new DriveInACircle(comLog, m_drive, controller, -1, viz));
 

comp/swerve100/src/main/java/org/team100/frc2024/RobotContainerParkingLot.java

@@ -33,8 +33,8 @@
 import org.team100.lib.localization.AprilTagFieldLayoutWithCorrectOrientation;
 import org.team100.lib.localization.SwerveDrivePoseEstimator100;
 import org.team100.lib.localization.VisionDataProvider24;
-import org.team100.lib.logging.Logging;
 import org.team100.lib.logging.LoggerFactory;
+import org.team100.lib.logging.Logging;
 import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem;
 import org.team100.lib.motion.drivetrain.SwerveLocal;
 import org.team100.lib.motion.drivetrain.kinodynamics.FieldRelativeVelocity;
@@ -43,6 +43,7 @@
 import org.team100.lib.motion.drivetrain.module.SwerveModuleCollection;
 import org.team100.lib.sensors.Gyro;
 import org.team100.lib.sensors.GyroFactory;
+import org.team100.lib.swerve.AsymSwerveSetpointGenerator;
 import org.team100.lib.timing.TimingConstraint;
 import org.team100.lib.timing.TimingConstraintFactory;
 import org.team100.lib.trajectory.TrajectoryMaker;
@@ -52,6 +53,7 @@
 import com.choreo.lib.ChoreoTrajectory;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -114,7 +116,11 @@ public class RobotContainerParkingLot implements Glassy {
                 driveLogger,
                 m_layout,
                 poseEstimator);
-        SwerveLocal swerveLocal = new SwerveLocal(driveLogger, swerveKinodynamics, m_modules);
+        final AsymSwerveSetpointGenerator setpointGenerator = new AsymSwerveSetpointGenerator(
+                driveLogger,
+                swerveKinodynamics,
+                RobotController::getBatteryVoltage);
+        SwerveLocal swerveLocal = new SwerveLocal(driveLogger, swerveKinodynamics, setpointGenerator, m_modules);
         m_drive = new SwerveDriveSubsystem(
                 fieldLogger,
                 driveLogger,

comp/swerve100/src/test/java/org/team100/frc2024/motion/AutoMakerTest.java

@@ -23,6 +23,7 @@
 import org.team100.lib.motion.drivetrain.module.SwerveModuleCollection;
 import org.team100.lib.sensors.Gyro;
 import org.team100.lib.sensors.SimulatedGyro;
+import org.team100.lib.swerve.AsymSwerveSetpointGenerator;
 import org.team100.lib.visualization.TrajectoryVisualization;
 
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
@@ -47,7 +48,11 @@ void testAll() {
         swerveKinodynamics = SwerveKinodynamicsFactory.forTest();
         collection = SwerveModuleCollection.get(logger, 10, 20, swerveKinodynamics);
         gyro = new SimulatedGyro(swerveKinodynamics, collection);
-        swerveLocal = new SwerveLocal(logger, swerveKinodynamics, collection);
+        final AsymSwerveSetpointGenerator setpointGenerator = new AsymSwerveSetpointGenerator(
+                logger,
+                swerveKinodynamics,
+                () -> 12);
+        swerveLocal = new SwerveLocal(logger, swerveKinodynamics, setpointGenerator, collection);
         poseEstimator = swerveKinodynamics.newPoseEstimator(
                 logger,
                 gyro.getYawNWU(),

lib/src/main/java/org/team100/lib/commands/drivetrain/HeadingLatch.java

@@ -12,10 +12,15 @@
  */
 public class HeadingLatch {
     private static final double unlatch = 0.01;
-    private static final double maxARad_S2 = 10;
+
     private Rotation2d m_desiredRotation = null;
 
+    /**
+     * @param maxARad_S2 supply an acceleration that matches whatever profile or
+     *                   expectations you have for angular acceleration.
+     */
     public Rotation2d latchedRotation(
+            double maxARad_S2,
             State100 state,
             Rotation2d pov,
             double inputOmega) {

lib/src/main/java/org/team100/lib/commands/drivetrain/manual/ManualWithFullStateHeading.java

@@ -137,7 +137,11 @@ public FieldRelativeVelocity apply(SwerveState state, DriverControl.Velocity twi
         double yawRate = getYawRateNWURad_S();
 
         Rotation2d pov = m_desiredRotation.get();
-        m_goal = m_latch.latchedRotation(state.theta(), pov, twistM_S.theta());
+        m_goal = m_latch.latchedRotation(
+                m_swerveKinodynamics.getMaxAngleAccelRad_S2(),
+                state.theta(),
+                pov,
+                twistM_S.theta());
         if (m_goal == null) {
             // we're not in snap mode, so it's pure manual
             // in this case there is no setpoint

lib/src/main/java/org/team100/lib/commands/drivetrain/manual/ManualWithMinTimeHeading.java

@@ -147,7 +147,11 @@ public FieldRelativeVelocity apply(SwerveState state, DriverControl.Velocity twi
         // double yawRate = getYawRateNWURad_S();
 
         Rotation2d pov = m_desiredRotation.get();
-        m_goal = m_latch.latchedRotation(state.theta(), pov, twistM_S.theta());
+        m_goal = m_latch.latchedRotation(
+                m_swerveKinodynamics.getMaxAngleAccelRad_S2(),
+                state.theta(),
+                pov,
+                twistM_S.theta());
         if (m_goal == null) {
             // we're not in snap mode, so it's pure manual
             // in this case there is no setpoint
@@ -185,7 +189,6 @@ public FieldRelativeVelocity apply(SwerveState state, DriverControl.Velocity twi
         // the snap overrides the user input for omega.
         final double thetaFF = getThetaFF();
 
-
         final double omega = MathUtil.clamp(
                 thetaFF,
                 -m_swerveKinodynamics.getMaxAngleSpeedRad_S(),

lib/src/main/java/org/team100/lib/commands/drivetrain/manual/ManualWithProfiledHeading.java

@@ -34,9 +34,14 @@
  * Rotation uses a profile, velocity feedforward, and positional feedback.
  */
 public class ManualWithProfiledHeading implements FieldRelativeDriver {
+    // don't try to go full speed
+    private static final double PROFILE_SPEED = 0.5;
+    // accelerate gently to avoid upset
+    private static final double PROFILE_ACCEL = 0.5;
     private static final double OMEGA_FB_DEADBAND = 0.1;
     private static final double THETA_FB_DEADBAND = 0.1;
     private final SwerveKinodynamics m_swerveKinodynamics;
+    // TODO: get rid of this, use the state estimator instead
     private final Gyro m_gyro;
     /** Absolute input supplier, null if free */
     private final Supplier<Rotation2d> m_desiredRotation;
@@ -136,27 +141,26 @@ private void updateSetpoint(double x, double v) {
      * @return feasible field-relative velocity in m/s and rad/s
      */
     public FieldRelativeVelocity apply(SwerveState state, DriverControl.Velocity twist1_1) {
-        // clip the input to the unit circle
-        DriverControl.Velocity clipped = DriveUtil.clampTwist(twist1_1, 1.0);
-        // scale to max in both translation and rotation
-        FieldRelativeVelocity twistM_S = DriveUtil.scale(
-                clipped,
-                m_swerveKinodynamics.getMaxDriveVelocityM_S(),
-                m_swerveKinodynamics.getMaxAngleSpeedRad_S());
+        final FieldRelativeVelocity control = clipAndScale(twist1_1);
 
-        double yawMeasurement = state.theta().x();
-        // TODO: use the state instead
-        double yawRate = getYawRateNWURad_S();
+        final double yawMeasurement = state.theta().x();
+        final double yawRateMeasurement = state.theta().v();
+
+        final TrapezoidProfile100 m_profile = makeProfile(control, yawRateMeasurement);
 
         Rotation2d pov = m_desiredRotation.get();
-        m_goal = m_latch.latchedRotation(state.theta(), pov, twistM_S.theta());
+        m_goal = m_latch.latchedRotation(
+                m_profile.getMaxAcceleration(),
+                state.theta(),
+                pov,
+                control.theta());
         if (m_goal == null) {
             // we're not in snap mode, so it's pure manual
             // in this case there is no setpoint
             m_thetaSetpoint = null;
             m_log_mode.log(() -> "free");
             // desaturate to feasibility
-            return m_swerveKinodynamics.analyticDesaturation(twistM_S);
+            return m_swerveKinodynamics.analyticDesaturation(control);
         }
 
         // take the short path
@@ -166,7 +170,7 @@ public FieldRelativeVelocity apply(SwerveState state, DriverControl.Velocity twi
         // if this is the first run since the latch, then the setpoint should be
         // whatever the measurement is
         if (m_thetaSetpoint == null) {
-            updateSetpoint(yawMeasurement, yawRate);
+            updateSetpoint(yawMeasurement, yawRateMeasurement);
         }
 
         // use the modulus closest to the measurement
@@ -178,60 +182,28 @@ public FieldRelativeVelocity apply(SwerveState state, DriverControl.Velocity twi
         // the omega goal in snap mode is always zero.
         State100 goalState = new State100(m_goal.getRadians(), 0);
 
-        // the profile has no state and is ~free to instantiate so make a new one every
-        // time. the max speed adapts to the observed speed (plus a little).
-        // the max speed should be half of the absolute max, to compromise translation
-        // and rotation, unless the actual translation speed is less, in which case we
-        // can rotate faster.
-
-        // how fast do we want to go?
-        double xySpeed = twistM_S.norm();
-        // fraction of the maximum speed
-        double xyRatio = Math.min(1, xySpeed / m_swerveKinodynamics.getMaxDriveVelocityM_S());
-        // fraction left for rotation
-        double oRatio = 1 - xyRatio;
-        // actual speed is at least half
-        double kRotationSpeed = Math.max(0.5, oRatio);
-
-        // finally reduce the speed to make it easier
-        final double lessV = 0.5;
-        // kinodynamic max A seems too high?
-        final double lessA = 0.1;
-
-        double maxSpeedRad_S = Math.max(Math.abs(yawRate) + 0.001,
-                m_swerveKinodynamics.getMaxAngleSpeedRad_S() * kRotationSpeed) * lessV;
-        double maxAccelRad_S2 = m_swerveKinodynamics.getMaxAngleAccelRad_S2() * kRotationSpeed * lessA;
-
-        m_log_max_speed.log(() -> maxSpeedRad_S);
-        m_log_max_accel.log(() -> maxAccelRad_S2);
-
-        final TrapezoidProfile100 m_profile = new TrapezoidProfile100(
-                maxSpeedRad_S,
-                maxAccelRad_S2,
-                0.01);
-
         m_thetaSetpoint = m_profile.calculate(TimedRobot100.LOOP_PERIOD_S, m_thetaSetpoint, goalState);
 
         // the snap overrides the user input for omega.
         double thetaFF = m_thetaSetpoint.v();
 
         final double thetaFB = getThetaFB(yawMeasurement);
 
-        final double omegaFB = getOmegaFB(yawRate);
+        final double omegaFB = getOmegaFB(yawRateMeasurement);
 
         double omega = MathUtil.clamp(
                 thetaFF + thetaFB + omegaFB,
                 -m_swerveKinodynamics.getMaxAngleSpeedRad_S(),
                 m_swerveKinodynamics.getMaxAngleSpeedRad_S());
-        FieldRelativeVelocity twistWithSnapM_S = new FieldRelativeVelocity(twistM_S.x(), twistM_S.y(), omega);
+        FieldRelativeVelocity twistWithSnapM_S = new FieldRelativeVelocity(control.x(), control.y(), omega);
 
         m_log_mode.log(() -> "snap");
         m_log_goal_theta.log(m_goal::getRadians);
         m_log_setpoint_theta.log(() -> m_thetaSetpoint);
         m_log_measurement_theta.log(() -> yawMeasurement);
-        m_log_measurement_omega.log(() -> yawRate);
+        m_log_measurement_omega.log(() -> yawRateMeasurement);
         m_log_error_theta.log(() -> m_thetaSetpoint.x() - yawMeasurement);
-        m_log_error_omega.log(() -> m_thetaSetpoint.v() - yawRate);
+        m_log_error_omega.log(() -> m_thetaSetpoint.v() - yawRateMeasurement);
         m_log_theta_FF.log(() -> thetaFF);
         m_log_theta_FB.log(() -> thetaFB);
         m_log_omega_FB.log(() -> omegaFB);
@@ -243,6 +215,47 @@ public FieldRelativeVelocity apply(SwerveState state, DriverControl.Velocity twi
         return twistWithSnapM_S;
     }
 
+    public FieldRelativeVelocity clipAndScale(DriverControl.Velocity twist1_1) {
+        // clip the input to the unit circle
+        DriverControl.Velocity clipped = DriveUtil.clampTwist(twist1_1, 1.0);
+        // scale to max in both translation and rotation
+        return DriveUtil.scale(
+                clipped,
+                m_swerveKinodynamics.getMaxDriveVelocityM_S(),
+                m_swerveKinodynamics.getMaxAngleSpeedRad_S());
+    }
+
+    /**
+     * the profile has no state and is ~free to instantiate so make a new one every
+     * time. the max speed adapts to the observed speed (plus a little).
+     * the max speed should be half of the absolute max, to compromise
+     * translation and rotation, unless the actual translation speed is less, in
+     * which case we can rotate faster.
+     */
+    public TrapezoidProfile100 makeProfile(FieldRelativeVelocity twistM_S, double yawRate) {
+        // how fast do we want to go?
+        double xySpeed = twistM_S.norm();
+        // fraction of the maximum speed
+        double xyRatio = Math.min(1, xySpeed / m_swerveKinodynamics.getMaxDriveVelocityM_S());
+        // fraction left for rotation
+        double oRatio = 1 - xyRatio;
+        // actual speed is at least half
+        double kRotationSpeed = Math.max(0.5, oRatio);
+
+        double maxSpeedRad_S = Math.max(Math.abs(yawRate) + 0.001,
+                m_swerveKinodynamics.getMaxAngleSpeedRad_S() * kRotationSpeed) * PROFILE_SPEED;
+
+        double maxAccelRad_S2 = m_swerveKinodynamics.getMaxAngleAccelRad_S2() * kRotationSpeed * PROFILE_ACCEL;
+
+        m_log_max_speed.log(() -> maxSpeedRad_S);
+        m_log_max_accel.log(() -> maxAccelRad_S2);
+
+        return new TrapezoidProfile100(
+                maxSpeedRad_S,
+                maxAccelRad_S2,
+                0.01);
+    }
+
     private double getOmegaFB(double headingRate) {
         double omegaFB = m_omegaController.calculate(headingRate, m_thetaSetpoint.v());
 

lib/src/main/java/org/team100/lib/hid/InterLinkDX.java

@@ -4,8 +4,6 @@
 import static org.team100.lib.hid.ControlUtil.deadband;
 import static org.team100.lib.hid.ControlUtil.expo;
 
-import org.team100.lib.logging.LoggerFactory;
-
 import edu.wpi.first.wpilibj.GenericHID;
 
 /**

lib/src/main/java/org/team100/lib/logging/primitive/UdpPrimitiveLogger.java

@@ -8,7 +8,6 @@
 import java.util.Map;
 import java.util.function.BooleanSupplier;
 import java.util.function.Consumer;
-import java.util.stream.Stream;
 
 import org.team100.lib.util.Util;
 

lib/src/main/java/org/team100/lib/motion/drivetrain/SwerveLocal.java

@@ -61,6 +61,7 @@ public class SwerveLocal implements Glassy, SwerveLocalObserver {
     public SwerveLocal(
             LoggerFactory parent,
             SwerveKinodynamics swerveKinodynamics,
+            AsymSwerveSetpointGenerator setpointGenerator,
             SwerveModuleCollection modules) {
         LoggerFactory child = parent.child(this);
         m_log_desired = child.chassisSpeedsLogger(Level.DEBUG, "desired chassis speed");
@@ -69,7 +70,7 @@ public SwerveLocal(
         m_log_setpoint = child.chassisSpeedsLogger(Level.DEBUG, "setpoint chassis speed");
         m_swerveKinodynamics = swerveKinodynamics;
         m_modules = modules;
-        m_SwerveSetpointGenerator = new AsymSwerveSetpointGenerator(child, m_swerveKinodynamics);
+        m_SwerveSetpointGenerator = setpointGenerator;
         m_prevSetpoint = new SwerveSetpoint();
     }