steering velocity feedforward, and drive visualizer refactor

comp/src/main/java/org/team100/frc2025/robot/Machinery.java

@@ -29,6 +29,7 @@
 import org.team100.lib.targeting.SimulatedTargetWriter;
 import org.team100.lib.targeting.Targets;
 import org.team100.lib.util.CanId;
+import org.team100.lib.visualization.RobotPoseVisualization;
 import org.team100.lib.visualization.TrajectoryVisualization;
 
 import edu.wpi.first.math.geometry.Pose2d;
@@ -52,6 +53,7 @@ public class Machinery {
     private static final LoggerFactory logger = Logging.instance().rootLogger;
     private static final LoggerFactory fieldLogger = Logging.instance().fieldLogger;
 
+    private final Runnable m_robotViz;
     private final Runnable m_combinedViz;
     private final Runnable m_climberViz;
     private final SwerveModuleCollection m_modules;
@@ -143,14 +145,15 @@ public Machinery() {
         // DRIVETRAIN
         //
         m_drive = SwerveDriveFactory.get(
-                fieldLogger,
                 driveLog,
                 m_swerveKinodynamics,
                 m_localizer,
                 odometryUpdater,
                 history,
                 m_modules);
         m_drive.resetPose(new Pose2d(m_drive.getPose().getTranslation(), new Rotation2d(Math.PI)));
+        m_robotViz = new RobotPoseVisualization(
+                fieldLogger, () -> m_drive.getState().pose(), "robot");
 
         ////////////////////////////////////////////////////////////
         //
@@ -171,6 +174,7 @@ public void periodic() {
         // show the closest target on field2d
         m_targets.periodic();
         m_leds.periodic();
+        m_robotViz.run();
         m_combinedViz.run();
         m_climberViz.run();
     }

lib/src/main/java/org/team100/lib/controller/r3/ControllerR3.java

@@ -7,7 +7,7 @@
 /**
  * Feedback and feedforward control.
  */
-public interface ControllerR3  {
+public interface ControllerR3 {
 
     /**
      * Feedback should compare the current-instant measurement to the
@@ -20,8 +20,12 @@ public interface ControllerR3  {
      * @param measurement      Current measurement state in field coordinates
      * @param currentReference Current reference state i.e. setpoint
      * @param nextReference    Reference for dt in the future, used for feedforward.
-     * @return Control output, should be given to
-     *         SwerveDriveSubsystem.driveInFieldCoords() or something similar.
+     * @return Control output for the period during dt, so it's also what the next
+     *         measurement should be. If there's no current error (i.e. feedback is
+     *         zero), then this output is usually just the feedforward, i.e. the
+     *         next reference velocity.
+     *         Give this to SwerveDriveSubsystem.driveInFieldCoords() or something
+     *         similar.
      */
     GlobalVelocityR3 calculate(
             ModelR3 measurement,

lib/src/main/java/org/team100/lib/controller/r3/ControllerR3Base.java

@@ -87,6 +87,12 @@ public GlobalVelocityR3 calculate(
         return calculate100(m_positionError, m_velocityError, nextReference);
     }
 
+    /**
+     * @param positionError current reference minus current measurement
+     * @param velocityError current reference minus current measurement
+     * @param nextReference velocity for dt from now
+     * @returns control output for the period during dt
+     */
     public abstract GlobalVelocityR3 calculate100(
             GlobalDeltaR3 positionError,
             GlobalVelocityR3 velocityError,

lib/src/main/java/org/team100/lib/subsystems/mecanum/MecanumDrive100.java

@@ -86,18 +86,22 @@ public ModelR3 getState() {
         return new ModelR3(m_pose, m_input);
     }
 
-    /** Use inverse kinematics to set wheel speeds. */
+    /**
+     * Use inverse kinematics to set wheel speeds.
+     * 
+     * @param nextV for the next timestep
+     */
     @Override
-    public void setVelocity(GlobalVelocityR3 input) {
+    public void setVelocity(GlobalVelocityR3 nextV) {
         Rotation2d yaw = getYaw();
         ChassisSpeeds speed = SwerveKinodynamics.toInstantaneousChassisSpeeds(
-                input, yaw);
+                nextV, yaw);
         MecanumDriveWheelSpeeds mSpeed = m_kinematics.toWheelSpeeds(speed);
         m_frontLeft.setVelocity(mSpeed.frontLeftMetersPerSecond);
         m_frontRight.setVelocity(mSpeed.frontRightMetersPerSecond);
         m_rearLeft.setVelocity(mSpeed.rearLeftMetersPerSecond);
         m_rearRight.setVelocity(mSpeed.rearRightMetersPerSecond);
-        m_log_input.log(() -> input);
+        m_log_input.log(() -> nextV);
     }
 
     private Rotation2d getYaw() {

lib/src/main/java/org/team100/lib/subsystems/r3/VelocitySubsystemR3.java

@@ -10,6 +10,10 @@
  */
 public interface VelocitySubsystemR3 extends SubsystemR3 {
 
-    /** No scaling or filtering. */
-    void setVelocity(GlobalVelocityR3 setpoint);
+    /**
+     * No scaling or filtering.
+     * 
+     * @param nextV for the next timestep.
+     */
+    void setVelocity(GlobalVelocityR3 nextV);
 }

lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/PositionReferenceControllerR3.java

@@ -31,6 +31,10 @@ public PositionReferenceControllerR3(
         m_subsystem = subsystem;
     }
 
+    /**
+     * @param next set the subsystem position to this
+     * @param u    ignored, since this uses outboard feedback only.
+     */
     @Override
     void execute100(ControlR3 next, GlobalVelocityR3 u) {
         m_subsystem.set(next);

lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/ReferenceControllerR3Base.java

@@ -50,7 +50,7 @@ public abstract class ReferenceControllerR3Base {
      * Actuate the subsystem here.
      * 
      * @param next The next control setpoint.
-     * @param u    The controller output.
+     * @param u    The controller output for the next dt
      */
     abstract void execute100(ControlR3 next, GlobalVelocityR3 u);
 
@@ -63,6 +63,8 @@ public void execute() {
             ModelR3 current = m_reference.current();
             ControlR3 next = m_reference.next();
             ModelR3 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
             GlobalVelocityR3 u = m_controller.calculate(measurement, current, next);
             execute100(next, u);
             m_log_measurement.log(() -> measurement);

lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/VelocityReferenceControllerR3.java

@@ -28,6 +28,12 @@ public VelocityReferenceControllerR3(
         m_subsystem = subsystem;
     }
 
+    /**
+     * @param next ignored
+     * @param u    represents the control for the next dt, so it's also what the
+     *             subsystem should be doing at the next timestep. set the subsystem
+     *             velocity to this
+     */
     @Override
     void execute100(ControlR3 next, GlobalVelocityR3 u) {
         m_subsystem.setVelocity(u);

lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveDriveFactory.java

@@ -18,7 +18,6 @@
 public class SwerveDriveFactory {
 
     public static SwerveDriveSubsystem get(
-            LoggerFactory fieldLogger,
             LoggerFactory driveLog,
             SwerveKinodynamics swerveKinodynamics,
             AprilTagRobotLocalizer localizer,
@@ -39,7 +38,6 @@ public static SwerveDriveSubsystem get(
                 swerveKinodynamics,
                 RobotController::getBatteryVoltage);
         return new SwerveDriveSubsystem(
-                fieldLogger,
                 driveLog,
                 odometryUpdater,
                 estimate,

lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveDriveSubsystem.java

@@ -6,6 +6,7 @@
 import org.team100.lib.coherence.ObjectCache;
 import org.team100.lib.coherence.Takt;
 import org.team100.lib.config.DriverSkill;
+import org.team100.lib.framework.TimedRobot100;
 import org.team100.lib.geometry.GlobalVelocityR3;
 import org.team100.lib.localization.FreshSwerveEstimate;
 import org.team100.lib.localization.OdometryUpdater;
@@ -47,15 +48,12 @@ public class SwerveDriveSubsystem extends SubsystemBase implements VelocitySubsy
     private final ModelR3Logger m_log_state;
     private final DoubleLogger m_log_turning;
     private final DoubleArrayLogger m_log_pose_array;
-    // TODO: pull the field logger out into a separate observer.
-    private final DoubleArrayLogger m_log_field_robot;
     private final EnumLogger m_log_skill;
     private final GlobalVelocityR3Logger m_log_input;
 
     private final List<Player> m_players;
 
     public SwerveDriveSubsystem(
-            LoggerFactory fieldLogger,
             LoggerFactory parent,
             OdometryUpdater odometryUpdater,
             FreshSwerveEstimate estimate,
@@ -71,7 +69,6 @@ public SwerveDriveSubsystem(
         m_log_state = log.modelR3Logger(Level.COMP, "state");
         m_log_turning = log.doubleLogger(Level.TRACE, "Tur Deg");
         m_log_pose_array = log.doubleArrayLogger(Level.COMP, "pose array");
-        m_log_field_robot = fieldLogger.doubleArrayLogger(Level.COMP, "robot");
         m_log_skill = log.enumLogger(Level.TRACE, "skill level");
         m_log_input = log.globalVelocityR3Logger(Level.TRACE, "drive input");
         m_players = m_swerveLocal.players();
@@ -82,17 +79,28 @@ public SwerveDriveSubsystem(
     // ACTUATORS
     //
 
-    /** Skip all scaling, limits generator, etc. */
+    /**
+     * Skip all scaling, limits generator, etc.
+     * 
+     * @param nextV for the next timestep
+     */
     @Override
-    public void setVelocity(GlobalVelocityR3 input) {
+    public void setVelocity(GlobalVelocityR3 nextV) {
         // keep the limiter up to date on what we're doing
-        m_limiter.updateSetpoint(input);
+        m_limiter.updateSetpoint(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();
+        // Note this may add a bit of noise.
+        ModelR3 nextState = currentState.evolve(TimedRobot100.LOOP_PERIOD_S);
+        Rotation2d nextTheta = nextState.rotation();
 
-        Rotation2d theta = getPose().getRotation();
-        ChassisSpeeds targetChassisSpeeds = SwerveKinodynamics.toInstantaneousChassisSpeeds(
-                input, theta);
-        m_swerveLocal.setChassisSpeeds(targetChassisSpeeds);
-        m_log_input.log(() -> input);
+        ChassisSpeeds nextSpeed = SwerveKinodynamics.toInstantaneousChassisSpeeds(
+                nextV, nextTheta);
+        m_swerveLocal.setChassisSpeeds(nextSpeed);
+        m_log_input.log(() -> nextV);
     }
 
     /**
@@ -174,10 +182,6 @@ public void periodic() {
         m_log_turning.log(() -> getPose().getRotation().getDegrees());
         m_log_pose_array.log(this::poseArray);
 
-        // Update the Field2d widget
-        // the name "field" is used by Field2d.
-        // the name "robot" can be anything.
-        m_log_field_robot.log(this::poseArray);
         m_log_skill.log(() -> DriverSkill.level());
         m_swerveLocal.periodic();
     }

lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveLocal.java

@@ -62,11 +62,13 @@ public List<Player> players() {
     /**
      * Discretizes the speeds, calculates the inverse kinematic module states, and
      * sets the module states.
+     * 
+     * @param nextSpeed for the next timestep.
      */
-    void setChassisSpeeds(ChassisSpeeds speeds) {
-        SwerveModuleStates states = m_swerveKinodynamics.toSwerveModuleStates(speeds);
+    void setChassisSpeeds(ChassisSpeeds nextSpeed) {
+        SwerveModuleStates states = m_swerveKinodynamics.toSwerveModuleStates(nextSpeed);
         setModuleStates(states);
-        m_log_chassis_speed.log(() -> speeds);
+        m_log_chassis_speed.log(() -> nextSpeed);
     }
 
     void stop() {
@@ -118,7 +120,10 @@ void periodic() {
 
     /////////////////////////////////////////////////////////
 
-    private void setModuleStates(SwerveModuleStates states) {
-        m_modules.setDesiredStates(states);
+    /**
+     * @param nextStates for now+dt
+     */
+    private void setModuleStates(SwerveModuleStates nextStates) {
+        m_modules.setDesiredStates(nextStates);
     }
 }

lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/SwerveKinodynamics.java

@@ -203,26 +203,30 @@ public double getMaxCapsizeAccelM_S2() {
      * translational acceleration.
      * 
      * States may include empty angles for motionless wheels.
+     * 
+     * @param nextSpeed represents the desired speed for now+dt.
      */
-    public SwerveModuleStates toSwerveModuleStates(ChassisSpeeds in) {
-        return toSwerveModuleStates(in, TimedRobot100.LOOP_PERIOD_S);
+    public SwerveModuleStates toSwerveModuleStates(ChassisSpeeds nextSpeed) {
+        return toSwerveModuleStates(nextSpeed, TimedRobot100.LOOP_PERIOD_S);
     }
 
     /**
      * Discretizes.
      * 
      * States may include empty angles for motionless wheels.
      * Otherwise angle is always within [-pi, pi].
+     * 
+     * @param nextSpeed represents the desired speed for now+dt.
      */
-    SwerveModuleStates toSwerveModuleStates(ChassisSpeeds in, double dt) {
+    SwerveModuleStates toSwerveModuleStates(ChassisSpeeds nextSpeed, double dt) {
         // This is the extra correction angle ...
-        Rotation2d angle = new Rotation2d(VeeringCorrection.correctionRad(in.omegaRadiansPerSecond));
+        Rotation2d angle = new Rotation2d(VeeringCorrection.correctionRad(nextSpeed.omegaRadiansPerSecond));
         // ... which is subtracted here; this isn't really a field-relative
         // transformation it's just a rotation.
         ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
-                in.vxMetersPerSecond,
-                in.vyMetersPerSecond,
-                in.omegaRadiansPerSecond,
+                nextSpeed.vxMetersPerSecond,
+                nextSpeed.vyMetersPerSecond,
+                nextSpeed.omegaRadiansPerSecond,
                 angle);
         // discretization does not affect omega
         DiscreteSpeed descretized = discretize(chassisSpeeds, dt);