Merge branch 'main' into 2027

wpimath/src/main/java/edu/wpi/first/math/MathUtil.java

@@ -107,6 +107,42 @@ public final class MathUtil {
     return applyDeadband(value, deadband, 1);
   }
 
+  /**
+   * Raises the input to the power of the given exponent while preserving its sign.
+   *
+   * <p>The function normalizes the input value to the range [0, 1] based on the maximum magnitude,
+   * raises it to the power of the exponent, then scales the result back to the original range and
+   * copying the sign. This keeps the value in the original range and gives consistent curve
+   * behavior regardless of the input value's scale.
+   *
+   * <p>This is useful for applying smoother or more aggressive control response curves (e.g.
+   * joystick input shaping).
+   *
+   * @param value The input value to transform.
+   * @param exponent The exponent to apply (e.g. 1.0 = linear, 2.0 = squared curve). Must be
+   *     positive.
+   * @param maxMagnitude The maximum expected absolute value of input. Must be positive.
+   * @return The transformed value with the same sign and scaled to the input range.
+   */
+  public static double copySignPow(double value, double exponent, double maxMagnitude) {
+    return Math.copySign(Math.pow(Math.abs(value) / maxMagnitude, exponent), value) * maxMagnitude;
+  }
+
+  /**
+   * Raises the input to the power of the given exponent while preserving its sign.
+   *
+   * <p>This is useful for applying smoother or more aggressive control response curves (e.g.
+   * joystick input shaping).
+   *
+   * @param value The input value to transform.
+   * @param exponent The exponent to apply (e.g. 1.0 = linear, 2.0 = squared curve). Must be
+   *     positive.
+   * @return The transformed value with the same sign.
+   */
+  public static double copySignPow(double value, double exponent) {
+    return copySignPow(value, exponent, 1);
+  }
+
   /**
    * Returns modulus of input.
    *

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

@@ -189,6 +189,7 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
    * @param currentVelocity The current velocity setpoint in radians per second.
    * @param nextVelocity The next velocity setpoint in radians per second.
    * @return The computed feedforward in volts.
+   * @deprecated Use {@link #calculateWithVelocities(double, double, double)} instead.
    */
   public double calculate(double currentAngle, double currentVelocity, double nextVelocity) {
     return ArmFeedforwardJNI.calculate(

wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java

@@ -20,9 +20,9 @@ import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
+import java.util.Collection;
 import java.util.Collections;
 import java.util.Comparator;
-import java.util.List;
 import java.util.Objects;
 
 /** Represents a 2D pose containing translational and rotational elements. */
@@ -347,13 +347,13 @@ public class Pose2d implements Interpolatable<Pose2d>, ProtobufSerializable, Str
   }
 
   /**
-   * Returns the nearest Pose2d from a list of poses. If two or more poses in the list have the same
-   * distance from this pose, return the one with the closest rotation component.
+   * Returns the nearest Pose2d from a collection of poses. If two or more poses in the collection
+   * have the same distance from this pose, return the one with the closest rotation component.
    *
-   * @param poses The list of poses to find the nearest.
-   * @return The nearest Pose2d from the list.
+   * @param poses The collection of poses to find the nearest.
+   * @return The nearest Pose2d from the collection.
    */
-  public Pose2d nearest(List<Pose2d> poses) {
+  public Pose2d nearest(Collection<Pose2d> poses) {
     return Collections.min(
         poses,
         Comparator.comparing(

wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java

@@ -21,9 +21,9 @@ import edu.wpi.first.math.numbers.N4;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
+import java.util.Collection;
 import java.util.Collections;
 import java.util.Comparator;
-import java.util.List;
 import java.util.Objects;
 
 /** Represents a 3D pose containing translational and rotational elements. */
@@ -400,13 +400,13 @@ public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, Str
   }
 
   /**
-   * Returns the nearest Pose3d from a list of poses. If two or more poses in the list have the same
-   * distance from this pose, return the one with the closest rotation component.
+   * Returns the nearest Pose3d from a collection of poses. If two or more poses in the collection
+   * have the same distance from this pose, return the one with the closest rotation component.
    *
-   * @param poses The list of poses to find the nearest.
-   * @return The nearest Pose3d from the list.
+   * @param poses The collection of poses to find the nearest.
+   * @return The nearest Pose3d from the collection.
    */
-  public Pose3d nearest(List<Pose3d> poses) {
+  public Pose3d nearest(Collection<Pose3d> poses) {
     return Collections.min(
         poses,
         Comparator.comparing(

wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java

@@ -20,9 +20,9 @@ import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
+import java.util.Collection;
 import java.util.Collections;
 import java.util.Comparator;
-import java.util.List;
 import java.util.Objects;
 
 /**
@@ -273,12 +273,12 @@ public class Translation2d
   }
 
   /**
-   * Returns the nearest Translation2d from a list of translations.
+   * Returns the nearest Translation2d from a collection of translations.
    *
-   * @param translations The list of translations.
-   * @return The nearest Translation2d from the list.
+   * @param translations The collection of translations.
+   * @return The nearest Translation2d from the collection.
    */
-  public Translation2d nearest(List<Translation2d> translations) {
+  public Translation2d nearest(Collection<Translation2d> translations) {
     return Collections.min(translations, Comparator.comparing(this::getDistance));
   }
 

wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java

@@ -20,9 +20,9 @@ import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
+import java.util.Collection;
 import java.util.Collections;
 import java.util.Comparator;
-import java.util.List;
 import java.util.Objects;
 
 /**
@@ -305,7 +305,7 @@ public class Translation3d
    * @param translations The collection of translations to find the nearest.
    * @return The nearest Translation3d from the collection.
    */
-  public Translation3d nearest(List<Translation3d> translations) {
+  public Translation3d nearest(Collection<Translation3d> translations) {
     return Collections.min(translations, Comparator.comparing(this::getDistance));
   }
 

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

@@ -4,7 +4,12 @@
 
 package edu.wpi.first.math.kinematics;
 
-/** Represents the motor voltages for a mecanum drive drivetrain. */
+/**
+ * Represents the motor voltages for a mecanum drive drivetrain.
+ *
+ * @deprecated Use {@link
+ *     edu.wpi.first.wpilibj2.command.MecanumControllerCommand.MecanumVoltagesConsumer}
+ */
 @Deprecated(since = "2025", forRemoval = true)
 public class MecanumDriveMotorVoltages {
   /** Voltage of the front left motor. */

wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java

@@ -59,10 +59,14 @@ public class TrapezoidProfile {
     /**
      * Constructs constraints for a TrapezoidProfile.
      *
-     * @param maxVelocity maximum velocity
-     * @param maxAcceleration maximum acceleration
+     * @param maxVelocity Maximum velocity, must be non-negative.
+     * @param maxAcceleration Maximum acceleration, must be non-negative.
      */
     public Constraints(double maxVelocity, double maxAcceleration) {
+      if (maxVelocity < 0.0 || maxAcceleration < 0.0) {
+        throw new IllegalArgumentException("Constraints must be non-negative");
+      }
+
       this.maxVelocity = maxVelocity;
       this.maxAcceleration = maxAcceleration;
       MathSharedStore.reportUsage("TrapezoidProfile", "");
@@ -127,8 +131,8 @@ public class TrapezoidProfile {
     m_current = direct(current);
     goal = direct(goal);
 
-    if (m_current.velocity > m_constraints.maxVelocity) {
-      m_current.velocity = m_constraints.maxVelocity;
+    if (Math.abs(m_current.velocity) > m_constraints.maxVelocity) {
+      m_current.velocity = Math.copySign(m_constraints.maxVelocity, m_current.velocity);
     }
 
     // Deal with a possibly truncated motion profile (with nonzero initial or