Replace SFINAE with concepts (#5361)

Concepts are cleaner to use and result in much better error messages for incorrect template use.

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

@@ -5,6 +5,7 @@
 #pragma once
 
 #include <numbers>
+#include <type_traits>
 
 #include <wpi/SymbolExports.h>
 
@@ -25,12 +26,11 @@ namespace frc {
  * be infinite.
  * @return The value after the deadband is applied.
  */
-template <typename T,
-          typename = std::enable_if_t<std::disjunction_v<
-              std::is_floating_point<T>, units::traits::is_unit_t<T>>>>
+template <typename T>
+  requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
 T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
   T magnitude;
-  if constexpr (std::is_floating_point_v<T>) {
+  if constexpr (std::is_arithmetic_v<T>) {
     magnitude = std::abs(value);
   } else {
     magnitude = units::math::abs(value);

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

@@ -8,9 +8,9 @@
 #include <cmath>
 #include <limits>
 #include <random>
-#include <type_traits>
 
 #include <wpi/SymbolExports.h>
+#include <wpi/concepts.h>
 #include <wpi/deprecated.h>
 
 #include "Eigen/Eigenvalues"
@@ -96,8 +96,7 @@ bool IsStabilizableImpl(const Matrixd<States, States>& A,
  *                   of the control inputs from no actuation.
  * @return State excursion or control effort cost matrix.
  */
-template <typename... Ts, typename = std::enable_if_t<
-                              std::conjunction_v<std::is_same<double, Ts>...>>>
+template <std::same_as<double>... Ts>
 Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(Ts... tolerances) {
   Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
   detail::CostMatrixImpl(result.diagonal(), tolerances...);
@@ -116,8 +115,7 @@ Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(Ts... tolerances) {
  *                output measurement.
  * @return Process noise or measurement noise covariance matrix.
  */
-template <typename... Ts, typename = std::enable_if_t<
-                              std::conjunction_v<std::is_same<double, Ts>...>>>
+template <std::same_as<double>... Ts>
 Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs) {
   Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
   detail::CovMatrixImpl(result.diagonal(), stdDevs...);
@@ -173,8 +171,7 @@ Matrixd<N, N> MakeCovMatrix(const std::array<double, N>& stdDevs) {
   return result;
 }
 
-template <typename... Ts, typename = std::enable_if_t<
-                              std::conjunction_v<std::is_same<double, Ts>...>>>
+template <std::same_as<double>... Ts>
 Matrixd<sizeof...(Ts), 1> MakeWhiteNoiseVector(Ts... stdDevs) {
   Matrixd<sizeof...(Ts), 1> result;
   detail::WhiteNoiseVectorImpl(result, stdDevs...);

wpimath/src/main/native/include/frc/fmt/Eigen.h

@@ -4,9 +4,8 @@
 
 #pragma once
 
-#include <type_traits>
-
 #include <fmt/format.h>
+#include <wpi/concepts.h>
 
 #include "Eigen/Core"
 #include "Eigen/SparseCore"
@@ -16,12 +15,9 @@
  * Eigen::SparseCompressedBase<Derived>.
  */
 template <typename Derived, typename CharT>
-struct fmt::formatter<
-    Derived, CharT,
-    std::enable_if_t<
-        std::is_base_of_v<Eigen::MatrixBase<Derived>, Derived> ||
-            std::is_base_of_v<Eigen::SparseCompressedBase<Derived>, Derived>,
-        void>> {
+  requires std::derived_from<Derived, Eigen::MatrixBase<Derived>> ||
+           std::derived_from<Derived, Eigen::SparseCompressedBase<Derived>>
+struct fmt::formatter<Derived, CharT> {
   constexpr auto parse(fmt::format_parse_context& ctx) {
     return m_underlying.parse(ctx);
   }

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

@@ -5,10 +5,10 @@
 #pragma once
 
 #include <cstddef>
-#include <type_traits>
 
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
+#include <wpi/concepts.h>
 
 #include "Eigen/QR"
 #include "frc/EigenCore.h"
@@ -49,23 +49,19 @@ class SwerveDriveKinematics {
  public:
   /**
    * Constructs a swerve drive kinematics object. This takes in a variable
-   * number of wheel locations as Translation2ds. The order in which you pass in
-   * the wheel locations is the same order that you will receive the module
+   * number of module locations as Translation2ds. The order in which you pass
+   * in the module locations is the same order that you will receive the module
    * states when performing inverse kinematics. It is also expected that you
    * pass in the module states in the same order when calling the forward
    * kinematics methods.
    *
-   * @param wheel  The location of the first wheel relative to the physical
-   *               center of the robot.
-   * @param wheels The locations of the other wheels relative to the physical
-   *               center of the robot.
+   * @param moduleTranslations The locations of the modules relative to the
+   *                           physical center of the robot.
    */
-  template <typename... Wheels>
-  explicit SwerveDriveKinematics(Translation2d wheel, Wheels&&... wheels)
-      : m_modules{wheel, wheels...}, m_moduleStates(wpi::empty_array) {
-    static_assert(sizeof...(wheels) >= 1,
-                  "A swerve drive requires at least two modules");
-
+  template <std::convertible_to<Translation2d>... ModuleTranslations>
+    requires(sizeof...(ModuleTranslations) == NumModules)
+  explicit SwerveDriveKinematics(ModuleTranslations&&... moduleTranslations)
+      : m_modules{moduleTranslations...}, m_moduleStates(wpi::empty_array) {
     for (size_t i = 0; i < NumModules; i++) {
       // clang-format off
       m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
@@ -81,8 +77,8 @@ class SwerveDriveKinematics {
   }
 
   explicit SwerveDriveKinematics(
-      const wpi::array<Translation2d, NumModules>& wheels)
-      : m_modules{wheels}, m_moduleStates(wpi::empty_array) {
+      const wpi::array<Translation2d, NumModules>& modules)
+      : m_modules{modules}, m_moduleStates(wpi::empty_array) {
     for (size_t i = 0; i < NumModules; i++) {
       // clang-format off
       m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
@@ -140,17 +136,18 @@ class SwerveDriveKinematics {
    * the robot's position on the field using data from the real-world speed and
    * angle of each module on the robot.
    *
-   * @param wheelStates The state of the modules (as a SwerveModuleState type)
+   * @param moduleStates The state of the modules (as a SwerveModuleState type)
    * as measured from respective encoders and gyros. The order of the swerve
    * module states should be same as passed into the constructor of this class.
    *
    * @return The resulting chassis speed.
    */
-  template <typename... ModuleStates>
-    requires(std::is_same_v<std::remove_reference_t<ModuleStates>,
-                            SwerveModuleState> &&
-             ...)
-  ChassisSpeeds ToChassisSpeeds(ModuleStates&&... wheelStates) const;
+  template <std::convertible_to<SwerveModuleState>... ModuleStates>
+    requires(sizeof...(ModuleStates) == NumModules)
+  ChassisSpeeds ToChassisSpeeds(ModuleStates&&... moduleStates) const {
+    return this->ToChassisSpeeds(
+        wpi::array<SwerveModuleState, NumModules>{moduleStates...});
+  }
 
   /**
    * Performs forward kinematics to return the resulting chassis state from the
@@ -174,15 +171,19 @@ class SwerveDriveKinematics {
    * determining the robot's position on the field using data from the
    * real-world position delta and angle of each module on the robot.
    *
-   * @param wheelDeltas The latest change in position of the modules (as a
+   * @param moduleDeltas The latest change in position of the modules (as a
    * SwerveModulePosition type) as measured from respective encoders and gyros.
    * The order of the swerve module states should be same as passed into the
    * constructor of this class.
    *
    * @return The resulting Twist2d.
    */
-  template <typename... ModuleDeltas>
-  Twist2d ToTwist2d(ModuleDeltas&&... wheelDeltas) const;
+  template <std::convertible_to<SwerveModulePosition>... ModuleDeltas>
+    requires(sizeof...(ModuleDeltas) == NumModules)
+  Twist2d ToTwist2d(ModuleDeltas&&... moduleDeltas) const {
+    return this->ToTwist2d(
+        wpi::array<SwerveModulePosition, NumModules>{moduleDeltas...});
+  }
 
   /**
    * Performs forward kinematics to return the resulting Twist2d from the
@@ -190,7 +191,7 @@ class SwerveDriveKinematics {
    * determining the robot's position on the field using data from the
    * real-world position delta and angle of each module on the robot.
    *
-   * @param wheelDeltas The latest change in position of the modules (as a
+   * @param moduleDeltas The latest change in position of the modules (as a
    * SwerveModulePosition type) as measured from respective encoders and gyros.
    * The order of the swerve module states should be same as passed into the
    * constructor of this class.
@@ -198,7 +199,7 @@ class SwerveDriveKinematics {
    * @return The resulting Twist2d.
    */
   Twist2d ToTwist2d(
-      wpi::array<SwerveModulePosition, NumModules> wheelDeltas) const;
+      wpi::array<SwerveModulePosition, NumModules> moduleDeltas) const;
 
   /**
    * Renormalizes the wheel speeds if any individual speed is above the

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

@@ -13,9 +13,9 @@
 
 namespace frc {
 
-template <class... Wheels>
-SwerveDriveKinematics(Translation2d, Wheels...)
-    -> SwerveDriveKinematics<1 + sizeof...(Wheels)>;
+template <typename ModuleTranslation, typename... ModuleTranslations>
+SwerveDriveKinematics(ModuleTranslation, ModuleTranslations...)
+    -> SwerveDriveKinematics<1 + sizeof...(ModuleTranslations)>;
 
 template <size_t NumModules>
 wpi::array<SwerveModuleState, NumModules>
@@ -64,22 +64,6 @@ SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
   return m_moduleStates;
 }
 
-template <size_t NumModules>
-template <typename... ModuleStates>
-  requires(std::is_same_v<std::remove_reference_t<ModuleStates>,
-                          SwerveModuleState> &&
-           ...)
-ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
-    ModuleStates&&... wheelStates) const {
-  static_assert(sizeof...(wheelStates) == NumModules,
-                "Number of modules is not consistent with number of wheel "
-                "locations provided in constructor.");
-
-  wpi::array<SwerveModuleState, NumModules> moduleStates{wheelStates...};
-
-  return this->ToChassisSpeeds(moduleStates);
-}
-
 template <size_t NumModules>
 ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
     const wpi::array<SwerveModuleState, NumModules>& moduleStates) const {
@@ -99,19 +83,6 @@ ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
           units::radians_per_second_t{chassisSpeedsVector(2)}};
 }
 
-template <size_t NumModules>
-template <typename... ModuleDeltas>
-Twist2d SwerveDriveKinematics<NumModules>::ToTwist2d(
-    ModuleDeltas&&... wheelDeltas) const {
-  static_assert(sizeof...(wheelDeltas) == NumModules,
-                "Number of modules is not consistent with number of wheel "
-                "locations provided in constructor.");
-
-  wpi::array<SwerveModulePosition, NumModules> moduleDeltas{wheelDeltas...};
-
-  return this->ToTwist2d(moduleDeltas);
-}
-
 template <size_t NumModules>
 Twist2d SwerveDriveKinematics<NumModules>::ToTwist2d(
     wpi::array<SwerveModulePosition, NumModules> moduleDeltas) const {

wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h

@@ -7,6 +7,7 @@
 #include <stdexcept>
 
 #include <wpi/SymbolExports.h>
+#include <wpi/concepts.h>
 
 #include "frc/system/LinearSystem.h"
 #include "frc/system/plant/DCMotor.h"
@@ -76,9 +77,9 @@ class WPILIB_DLLEXPORT LinearSystemId {
    * @param kA The acceleration gain, in volts/(unit/sec²).
    * @throws std::domain_error if kV <= 0 or kA <= 0.
    */
-  template <typename Distance, typename = std::enable_if_t<
-                                   std::is_same_v<units::meter, Distance> ||
-                                   std::is_same_v<units::radian, Distance>>>
+  template <typename Distance>
+    requires std::same_as<units::meter, Distance> ||
+             std::same_as<units::radian, Distance>
   static LinearSystem<1, 1, 1> IdentifyVelocitySystem(
       decltype(1_V / Velocity_t<Distance>(1)) kV,
       decltype(1_V / Acceleration_t<Distance>(1)) kA) {
@@ -117,9 +118,9 @@ class WPILIB_DLLEXPORT LinearSystemId {
    *
    * @throws std::domain_error if kV <= 0 or kA <= 0.
    */
-  template <typename Distance, typename = std::enable_if_t<
-                                   std::is_same_v<units::meter, Distance> ||
-                                   std::is_same_v<units::radian, Distance>>>
+  template <typename Distance>
+    requires std::same_as<units::meter, Distance> ||
+             std::same_as<units::radian, Distance>
   static LinearSystem<2, 1, 1> IdentifyPositionSystem(
       decltype(1_V / Velocity_t<Distance>(1)) kV,
       decltype(1_V / Acceleration_t<Distance>(1)) kA) {

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

@@ -9,6 +9,7 @@
 #include <vector>
 
 #include <wpi/SymbolExports.h>
+#include <wpi/concepts.h>
 
 #include "frc/kinematics/DifferentialDriveKinematics.h"
 #include "frc/kinematics/MecanumDriveKinematics.h"
@@ -74,8 +75,7 @@ class WPILIB_DLLEXPORT TrajectoryConfig {
    * Adds a user-defined constraint to the trajectory.
    * @param constraint The user-defined constraint.
    */
-  template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
-                                     TrajectoryConstraint, Constraint>>>
+  template <std::derived_from<TrajectoryConstraint> Constraint>
   void AddConstraint(Constraint constraint) {
     m_constraints.emplace_back(std::make_unique<Constraint>(constraint));
   }

wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h

@@ -6,6 +6,8 @@
 
 #include <limits>
 
+#include <wpi/concepts.h>
+
 #include "frc/geometry/Rotation2d.h"
 #include "frc/geometry/Translation2d.h"
 #include "frc/trajectory/constraint/TrajectoryConstraint.h"
@@ -15,8 +17,7 @@ namespace frc {
 /**
  * Enforces a particular constraint only within an elliptical region.
  */
-template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
-                                   TrajectoryConstraint, Constraint>>>
+template <std::derived_from<TrajectoryConstraint> Constraint>
 class EllipticalRegionConstraint : public TrajectoryConstraint {
  public:
   /**

wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h

@@ -6,6 +6,8 @@
 
 #include <limits>
 
+#include <wpi/concepts.h>
+
 #include "frc/geometry/Rotation2d.h"
 #include "frc/geometry/Translation2d.h"
 #include "frc/trajectory/constraint/TrajectoryConstraint.h"
@@ -14,8 +16,7 @@ namespace frc {
 /**
  * Enforces a particular constraint only within a rectangular region.
  */
-template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
-                                   TrajectoryConstraint, Constraint>>>
+template <std::derived_from<TrajectoryConstraint> Constraint>
 class RectangularRegionConstraint : public TrajectoryConstraint {
  public:
   /**