Merge branch 'main' into 2027

wpimath/src/main/native/include/frc/MathUtil.h

@@ -94,6 +94,42 @@ constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
   }
 }
 
+/**
+ * Raises the input to the power of the given exponent while preserving its
+ * sign.
+ *
+ * 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.
+ *
+ * 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.
+ */
+template <typename T>
+  requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
+constexpr T CopySignPow(T value, double exponent, T maxMagnitude = T{1.0}) {
+  if constexpr (std::is_arithmetic_v<T>) {
+    return gcem::copysign(
+        gcem::pow(gcem::abs(value) / maxMagnitude, exponent) * maxMagnitude,
+        value);
+  } else {
+    return units::math::copysign(
+        gcem::pow((units::math::abs(value) / maxMagnitude).value(), exponent) *
+            maxMagnitude,
+        value);
+  }
+}
+
 /**
  * Returns modulus of input.
  *

wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h

@@ -76,8 +76,8 @@ class TrapezoidProfile {
     /**
      * Constructs constraints for a Trapezoid Profile.
      *
-     * @param maxVelocity Maximum velocity.
-     * @param maxAcceleration Maximum acceleration.
+     * @param maxVelocity Maximum velocity, must be non-negative.
+     * @param maxAcceleration Maximum acceleration, must be non-negative.
      */
     constexpr Constraints(Velocity_t maxVelocity,
                           Acceleration_t maxAcceleration)
@@ -85,6 +85,10 @@ class TrapezoidProfile {
       if (!std::is_constant_evaluated()) {
         wpi::math::MathSharedStore::ReportUsage("TrapezoidProfile", "");
       }
+
+      if (maxVelocity < Velocity_t{0} || maxAcceleration < Acceleration_t{0}) {
+        throw std::domain_error("Constraints must be non-negative");
+      }
     }
   };
 
@@ -129,8 +133,9 @@ class TrapezoidProfile {
     m_direction = ShouldFlipAcceleration(current, goal) ? -1 : 1;
     m_current = Direct(current);
     goal = Direct(goal);
-    if (m_current.velocity > m_constraints.maxVelocity) {
-      m_current.velocity = m_constraints.maxVelocity;
+    if (units::math::abs(m_current.velocity) > m_constraints.maxVelocity) {
+      m_current.velocity =
+          units::math::copysign(m_constraints.maxVelocity, m_current.velocity);
     }
 
     // Deal with a possibly truncated motion profile (with nonzero initial or