Merge branch 'main' into 2027

wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java

@@ -16,16 +16,16 @@ import edu.wpi.first.util.struct.StructSerializable;
  */
 public class ArmFeedforward implements ProtobufSerializable, StructSerializable {
   /** The static gain, in volts. */
-  private final double ks;
+  private double ks;
 
   /** The gravity gain, in volts. */
-  private final double kg;
+  private double kg;
 
   /** The velocity gain, in V/(rad/s). */
-  private final double kv;
+  private double kv;
 
   /** The acceleration gain, in V/(rad/s²). */
-  private final double ka;
+  private double ka;
 
   /** The period, in seconds. */
   private final double m_dt;
@@ -85,6 +85,42 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
     this(ks, kg, kv, 0);
   }
 
+  /**
+   * Sets the static gain.
+   *
+   * @param ks The static gain in volts.
+   */
+  public void setKs(double ks) {
+    this.ks = ks;
+  }
+
+  /**
+   * Sets the gravity gain.
+   *
+   * @param kg The gravity gain in volts.
+   */
+  public void setKg(double kg) {
+    this.kg = kg;
+  }
+
+  /**
+   * Sets the velocity gain.
+   *
+   * @param kv The velocity gain in V/(rad/s).
+   */
+  public void setKv(double kv) {
+    this.kv = kv;
+  }
+
+  /**
+   * Sets the acceleration gain.
+   *
+   * @param ka The acceleration gain in V/(rad/s²).
+   */
+  public void setKa(double ka) {
+    this.ka = ka;
+  }
+
   /**
    * Returns the static gain in volts.
    *

wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java

@@ -15,16 +15,16 @@ import edu.wpi.first.util.struct.StructSerializable;
  */
 public class ElevatorFeedforward implements ProtobufSerializable, StructSerializable {
   /** The static gain, in volts. */
-  private final double ks;
+  private double ks;
 
   /** The gravity gain, in volts. */
-  private final double kg;
+  private double kg;
 
   /** The velocity gain, in V/(m/s). */
-  private final double kv;
+  private double kv;
 
   /** The acceleration gain, in V/(m/s²). */
-  private final double ka;
+  private double ka;
 
   /** The period, in seconds. */
   private final double m_dt;
@@ -85,6 +85,42 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
     this(ks, kg, kv, 0);
   }
 
+  /**
+   * Sets the static gain.
+   *
+   * @param ks The static gain in volts.
+   */
+  public void setKs(double ks) {
+    this.ks = ks;
+  }
+
+  /**
+   * Sets the gravity gain.
+   *
+   * @param kg The gravity gain in volts.
+   */
+  public void setKg(double kg) {
+    this.kg = kg;
+  }
+
+  /**
+   * Sets the velocity gain.
+   *
+   * @param kv The velocity gain in V/(m/s).
+   */
+  public void setKv(double kv) {
+    this.kv = kv;
+  }
+
+  /**
+   * Sets the acceleration gain.
+   *
+   * @param ka The acceleration gain in V/(m/s²).
+   */
+  public void setKa(double ka) {
+    this.ka = ka;
+  }
+
   /**
    * Returns the static gain in volts.
    *

wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java

@@ -12,13 +12,13 @@ import edu.wpi.first.util.struct.StructSerializable;
 /** A helper class that computes feedforward outputs for a simple permanent-magnet DC motor. */
 public class SimpleMotorFeedforward implements ProtobufSerializable, StructSerializable {
   /** The static gain, in volts. */
-  private final double ks;
+  private double ks;
 
   /** The velocity gain, in V/(units/s). */
-  private final double kv;
+  private double kv;
 
   /** The acceleration gain, in V/(units/s²). */
-  private final double ka;
+  private double ka;
 
   /** The period, in seconds. */
   private final double m_dt;
@@ -82,6 +82,33 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
     this(ks, kv, 0);
   }
 
+  /**
+   * Sets the static gain.
+   *
+   * @param ks The static gain in volts.
+   */
+  public void setKs(double ks) {
+    this.ks = ks;
+  }
+
+  /**
+   * Sets the velocity gain.
+   *
+   * @param kv The velocity gain in V/(units/s).
+   */
+  public void setKv(double kv) {
+    this.kv = kv;
+  }
+
+  /**
+   * Sets the acceleration gain.
+   *
+   * @param ka The acceleration gain in V/(units/s²).
+   */
+  public void setKa(double ka) {
+    this.ka = ka;
+  }
+
   /**
    * Returns the static gain in volts.
    *

wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java

@@ -90,7 +90,10 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
    * along the y-axis, and omega * dt around the z-axis).
    *
    * <p>This is useful for compensating for translational skew when translating and rotating a
-   * swerve drivetrain.
+   * holonomic (swerve or mecanum) drivetrain. However, scaling down the ChassisSpeeds after
+   * discretizing (e.g., when desaturating swerve module speeds) rotates the direction of net motion
+   * in the opposite direction of rotational velocity, introducing a different translational skew
+   * which is not accounted for by discretization.
    *
    * @param dt The duration of the timestep in seconds the speeds should be applied for.
    * @return Discretized ChassisSpeeds.

wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java

@@ -257,6 +257,10 @@ public class SwerveDriveKinematics
    * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
    * absolute threshold, while maintaining the ratio of speeds between modules.
    *
+   * <p>Scaling down the module speeds rotates the direction of net motion in the opposite direction
+   * of rotational velocity, which makes discretizing the chassis speeds inaccurate because the
+   * discretization did not account for this translational skew.
+   *
    * @param moduleStates Reference to array of module states. The array will be mutated with the
    *     normalized speeds!
    * @param attainableMaxSpeed The absolute max speed in meters per second that a module can reach.
@@ -282,6 +286,10 @@ public class SwerveDriveKinematics
    * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
    * absolute threshold, while maintaining the ratio of speeds between modules.
    *
+   * <p>Scaling down the module speeds rotates the direction of net motion in the opposite direction
+   * of rotational velocity, which makes discretizing the chassis speeds inaccurate because the
+   * discretization did not account for this translational skew.
+   *
    * @param moduleStates Reference to array of module states. The array will be mutated with the
    *     normalized speeds!
    * @param attainableMaxSpeed The absolute max speed in meters per second that a module can reach.
@@ -300,6 +308,10 @@ public class SwerveDriveKinematics
    * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
    * absolute threshold, while maintaining the ratio of speeds between modules.
    *
+   * <p>Scaling down the module speeds rotates the direction of net motion in the opposite direction
+   * of rotational velocity, which makes discretizing the chassis speeds inaccurate because the
+   * discretization did not account for this translational skew.
+   *
    * @param moduleStates Reference to array of module states. The array will be mutated with the
    *     normalized speeds!
    * @param desiredChassisSpeed The desired speed of the robot
@@ -346,6 +358,10 @@ public class SwerveDriveKinematics
    * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
    * absolute threshold, while maintaining the ratio of speeds between modules.
    *
+   * <p>Scaling down the module speeds rotates the direction of net motion in the opposite direction
+   * of rotational velocity, which makes discretizing the chassis speeds inaccurate because the
+   * discretization did not account for this translational skew.
+   *
    * @param moduleStates Reference to array of module states. The array will be mutated with the
    *     normalized speeds!
    * @param desiredChassisSpeed The desired speed of the robot

wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp

@@ -20,6 +20,13 @@ units::volt_t ArmFeedforward::Calculate(
     units::unit_t<Velocity> nextVelocity) const {
   using VarMat = sleipnir::VariableMatrix;
 
+  // Small kₐ values make the solver ill-conditioned
+  if (kA < units::unit_t<ka_unit>{1e-1}) {
+    auto acceleration = (nextVelocity - currentVelocity) / m_dt;
+    return kS * wpi::sgn(currentVelocity.value()) + kV * currentVelocity +
+           kA * acceleration + kG * units::math::cos(currentAngle);
+  }
+
   // Arm dynamics
   Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
   Matrixd<2, 1> B{{0.0}, {1.0 / kA.value()}};

wpimath/src/main/native/include/frc/controller/ArmFeedforward.h

@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <cstdlib>
+
 #include <wpi/MathExtras.h>
 #include <wpi/SymbolExports.h>
 
@@ -242,6 +244,34 @@ class WPILIB_DLLEXPORT ArmFeedforward {
     return MaxAchievableAcceleration(-maxVoltage, angle, velocity);
   }
 
+  /**
+   * Sets the static gain.
+   *
+   * @param kS The static gain.
+   */
+  constexpr void SetKs(units::volt_t kS) { this->kS = kS; }
+
+  /**
+   * Sets the gravity gain.
+   *
+   * @param kG The gravity gain.
+   */
+  constexpr void SetKg(units::volt_t kG) { this->kG = kG; }
+
+  /**
+   * Sets the velocity gain.
+   *
+   * @param kV The velocity gain.
+   */
+  constexpr void SetKv(units::unit_t<kv_unit> kV) { this->kV = kV; }
+
+  /**
+   * Sets the acceleration gain.
+   *
+   * @param kA The acceleration gain.
+   */
+  constexpr void SetKa(units::unit_t<ka_unit> kA) { this->kA = kA; }
+
   /**
    * Returns the static gain.
    *

wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h

@@ -216,6 +216,34 @@ class ElevatorFeedforward {
     return MaxAchievableAcceleration(-maxVoltage, velocity);
   }
 
+  /**
+   * Sets the static gain.
+   *
+   * @param kS The static gain.
+   */
+  constexpr void SetKs(units::volt_t kS) { this->kS = kS; }
+
+  /**
+   * Sets the gravity gain.
+   *
+   * @param kG The gravity gain.
+   */
+  constexpr void SetKg(units::volt_t kG) { this->kG = kG; }
+
+  /**
+   * Sets the velocity gain.
+   *
+   * @param kV The velocity gain.
+   */
+  constexpr void SetKv(units::unit_t<kv_unit> kV) { this->kV = kV; }
+
+  /**
+   * Sets the acceleration gain.
+   *
+   * @param kA The acceleration gain.
+   */
+  constexpr void SetKa(units::unit_t<ka_unit> kA) { this->kA = kA; }
+
   /**
    * Returns the static gain.
    *

wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h

@@ -178,6 +178,27 @@ class SimpleMotorFeedforward {
     return MaxAchievableAcceleration(-maxVoltage, velocity);
   }
 
+  /**
+   * Sets the static gain.
+   *
+   * @param kS The static gain.
+   */
+  constexpr void SetKs(units::volt_t kS) { this->kS = kS; }
+
+  /**
+   * Sets the velocity gain.
+   *
+   * @param kV The velocity gain.
+   */
+  constexpr void SetKv(units::unit_t<kv_unit> kV) { this->kV = kV; }
+
+  /**
+   * Sets the acceleration gain.
+   *
+   * @param kA The acceleration gain.
+   */
+  constexpr void SetKa(units::unit_t<ka_unit> kA) { this->kA = kA; }
+
   /**
    * Returns the static gain.
    *

wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h

@@ -59,7 +59,11 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
    * y-axis, and omega * dt around the z-axis).
    *
    * This is useful for compensating for translational skew when translating and
-   * rotating a swerve drivetrain.
+   * rotating a holonomic (swerve or mecanum) drivetrain. However, scaling down
+   * the ChassisSpeeds after discretizing (e.g., when desaturating swerve module
+   * speeds) rotates the direction of net motion in the opposite direction of
+   * rotational velocity, introducing a different translational skew which is
+   * not accounted for by discretization.
    *
    * @param dt The duration of the timestep the speeds should be applied for.
    * @return Discretized ChassisSpeeds.

wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h

@@ -333,6 +333,11 @@ class SwerveDriveKinematics
    * the absolute threshold, while maintaining the ratio of speeds between
    * modules.
    *
+   * Scaling down the module speeds rotates the direction of net motion in the
+   * opposite direction of rotational velocity, which makes discretizing the
+   * chassis speeds inaccurate because the discretization did not account for
+   * this translational skew.
+   *
    * @param moduleStates Reference to array of module states. The array will be
    * mutated with the normalized speeds!
    * @param attainableMaxSpeed The absolute max speed that a module can reach.
@@ -368,6 +373,11 @@ class SwerveDriveKinematics
    * the absolute threshold, while maintaining the ratio of speeds between
    * modules.
    *
+   * Scaling down the module speeds rotates the direction of net motion in the
+   * opposite direction of rotational velocity, which makes discretizing the
+   * chassis speeds inaccurate because the discretization did not account for
+   * this translational skew.
+   *
    * @param moduleStates Reference to array of module states. The array will be
    * mutated with the normalized speeds!
    * @param desiredChassisSpeed The desired speed of the robot

wpimath/src/main/native/include/units/base.h

@@ -1759,7 +1759,7 @@ namespace units
 
 	namespace traits
 	{
-#ifdef FOR_DOXYGEN_PURPOSOES_ONLY
+#ifdef FOR_DOXYGEN_PURPOSES_ONLY
 		/**
 		* @ingroup		TypeTraits
 		* @brief		Trait for accessing the publicly defined types of `units::unit_t`