[wpimath, wpiutil] Add wpi::array for compile time size checking

wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp

@@ -31,7 +31,7 @@ class SwerveControllerCommandTest : public ::testing::Test {
   frc2::Timer m_timer;
   frc::Rotation2d m_angle{0_rad};
 
-  std::array<frc::SwerveModuleState, 4> m_moduleStates{
+  wpi::array<frc::SwerveModuleState, 4> m_moduleStates{
       frc::SwerveModuleState{}, frc::SwerveModuleState{},
       frc::SwerveModuleState{}, frc::SwerveModuleState{}};
 
@@ -60,7 +60,7 @@ class SwerveControllerCommandTest : public ::testing::Test {
 
   void TearDown() override { frc::sim::ResumeTiming(); }
 
-  std::array<frc::SwerveModuleState, 4> getCurrentWheelSpeeds() {
+  wpi::array<frc::SwerveModuleState, 4> getCurrentWheelSpeeds() {
     return m_moduleStates;
   }
 

wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp

@@ -65,7 +65,7 @@ void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
 }
 
 void DriveSubsystem::SetModuleStates(
-    std::array<frc::SwerveModuleState, 4> desiredStates) {
+    wpi::array<frc::SwerveModuleState, 4> desiredStates) {
   kDriveKinematics.NormalizeWheelSpeeds(&desiredStates,
                                         AutoConstants::kMaxSpeed);
   m_frontLeft.SetDesiredState(desiredStates[0]);

wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h

@@ -53,7 +53,7 @@ class DriveSubsystem : public frc2::SubsystemBase {
   /**
    * Sets the drive SpeedControllers to a power from -1 to 1.
    */
-  void SetModuleStates(std::array<frc::SwerveModuleState, 4> desiredStates);
+  void SetModuleStates(wpi::array<frc::SwerveModuleState, 4> desiredStates);
 
   /**
    * Returns the heading of the robot.

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

@@ -8,7 +8,7 @@ namespace frc {
 
 LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
     const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
-    const std::array<double, 1>& Qelems, const std::array<double, 1>& Relems,
+    const wpi::array<double, 1>& Qelems, const wpi::array<double, 1>& Relems,
     units::second_t dt)
     : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
                                MakeCostMatrix(Relems), dt) {}
@@ -21,7 +21,7 @@ LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
 
 LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
     const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
-    const std::array<double, 2>& Qelems, const std::array<double, 1>& Relems,
+    const wpi::array<double, 2>& Qelems, const wpi::array<double, 1>& Relems,
     units::second_t dt)
     : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
                                MakeCostMatrix(Relems), dt) {}

wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp

@@ -13,9 +13,9 @@ using namespace frc;
 
 DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
     const Rotation2d& gyroAngle, const Pose2d& initialPose,
-    const std::array<double, 5>& stateStdDevs,
-    const std::array<double, 3>& localMeasurementStdDevs,
-    const std::array<double, 3>& visionMeasurmentStdDevs,
+    const wpi::array<double, 5>& stateStdDevs,
+    const wpi::array<double, 3>& localMeasurementStdDevs,
+    const wpi::array<double, 3>& visionMeasurmentStdDevs,
     units::second_t nominalDt)
     : m_observer(
           &DifferentialDrivePoseEstimator::F,
@@ -46,7 +46,7 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
 }
 
 void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs(
-    const std::array<double, 3>& visionMeasurmentStdDevs) {
+    const wpi::array<double, 3>& visionMeasurmentStdDevs) {
   // Create R (covariances) for vision measurements.
   Eigen::Matrix<double, 3, 3> visionContR =
       frc::MakeCovMatrix(visionMeasurmentStdDevs);
@@ -126,9 +126,9 @@ Eigen::Matrix<double, 5, 1> DifferentialDrivePoseEstimator::F(
 }
 
 template <int Dim>
-std::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
+wpi::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
     const Eigen::Matrix<double, Dim, 1>& stdDevs) {
-  std::array<double, Dim> array;
+  wpi::array<double, Dim> array;
   for (size_t i = 0; i < Dim; ++i) {
     array[i] = stdDevs(i);
   }

wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp

@@ -7,14 +7,14 @@
 namespace frc {
 
 KalmanFilter<1, 1, 1>::KalmanFilter(
-    LinearSystem<1, 1, 1>& plant, const std::array<double, 1>& stateStdDevs,
-    const std::array<double, 1>& measurementStdDevs, units::second_t dt)
+    LinearSystem<1, 1, 1>& plant, const wpi::array<double, 1>& stateStdDevs,
+    const wpi::array<double, 1>& measurementStdDevs, units::second_t dt)
     : detail::KalmanFilterImpl<1, 1, 1>{plant, stateStdDevs, measurementStdDevs,
                                         dt} {}
 
 KalmanFilter<2, 1, 1>::KalmanFilter(
-    LinearSystem<2, 1, 1>& plant, const std::array<double, 2>& stateStdDevs,
-    const std::array<double, 1>& measurementStdDevs, units::second_t dt)
+    LinearSystem<2, 1, 1>& plant, const wpi::array<double, 2>& stateStdDevs,
+    const wpi::array<double, 1>& measurementStdDevs, units::second_t dt)
     : detail::KalmanFilterImpl<2, 1, 1>{plant, stateStdDevs, measurementStdDevs,
                                         dt} {}
 

wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp

@@ -14,9 +14,9 @@ using namespace frc;
 frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
     const Rotation2d& gyroAngle, const Pose2d& initialPose,
     MecanumDriveKinematics kinematics,
-    const std::array<double, 3>& stateStdDevs,
-    const std::array<double, 1>& localMeasurementStdDevs,
-    const std::array<double, 3>& visionMeasurementStdDevs,
+    const wpi::array<double, 3>& stateStdDevs,
+    const wpi::array<double, 1>& localMeasurementStdDevs,
+    const wpi::array<double, 3>& visionMeasurementStdDevs,
     units::second_t nominalDt)
     : m_observer([](const Eigen::Matrix<double, 3, 1>& x,
                     const Eigen::Matrix<double, 3, 1>& u) { return u; },
@@ -51,7 +51,7 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
 }
 
 void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs(
-    const std::array<double, 3>& visionMeasurmentStdDevs) {
+    const wpi::array<double, 3>& visionMeasurmentStdDevs) {
   // Create R (covariances) for vision measurements.
   Eigen::Matrix<double, 3, 3> visionContR =
       frc::MakeCovMatrix(visionMeasurmentStdDevs);

wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp

@@ -7,10 +7,10 @@
 using namespace frc;
 
 CubicHermiteSpline::CubicHermiteSpline(
-    std::array<double, 2> xInitialControlVector,
-    std::array<double, 2> xFinalControlVector,
-    std::array<double, 2> yInitialControlVector,
-    std::array<double, 2> yFinalControlVector) {
+    wpi::array<double, 2> xInitialControlVector,
+    wpi::array<double, 2> xFinalControlVector,
+    wpi::array<double, 2> yInitialControlVector,
+    wpi::array<double, 2> yFinalControlVector) {
   const auto hermite = MakeHermiteBasis();
   const auto x =
       ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);

wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp

@@ -7,10 +7,10 @@
 using namespace frc;
 
 QuinticHermiteSpline::QuinticHermiteSpline(
-    std::array<double, 3> xInitialControlVector,
-    std::array<double, 3> xFinalControlVector,
-    std::array<double, 3> yInitialControlVector,
-    std::array<double, 3> yFinalControlVector) {
+    wpi::array<double, 3> xInitialControlVector,
+    wpi::array<double, 3> xFinalControlVector,
+    wpi::array<double, 3> yInitialControlVector,
+    wpi::array<double, 3> yFinalControlVector) {
   const auto hermite = MakeHermiteBasis();
   const auto x =
       ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);

wpimath/src/main/native/cpp/spline/SplineHelper.cpp

@@ -13,10 +13,10 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
     const Spline<3>::ControlVector& end) {
   std::vector<CubicHermiteSpline> splines;
 
-  std::array<double, 2> xInitial = start.x;
-  std::array<double, 2> yInitial = start.y;
-  std::array<double, 2> xFinal = end.x;
-  std::array<double, 2> yFinal = end.y;
+  wpi::array<double, 2> xInitial = start.x;
+  wpi::array<double, 2> yInitial = start.y;
+  wpi::array<double, 2> xFinal = end.x;
+  wpi::array<double, 2> yFinal = end.y;
 
   if (waypoints.size() > 1) {
     waypoints.emplace(waypoints.begin(),
@@ -102,9 +102,9 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
     const double yDeriv =
         (3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0;
 
-    std::array<double, 2> midXControlVector{waypoints[0].X().to<double>(),
+    wpi::array<double, 2> midXControlVector{waypoints[0].X().to<double>(),
                                             xDeriv};
-    std::array<double, 2> midYControlVector{waypoints[0].Y().to<double>(),
+    wpi::array<double, 2> midYControlVector{waypoints[0].Y().to<double>(),
                                             yDeriv};
 
     splines.emplace_back(xInitial, midXControlVector, yInitial,
@@ -134,7 +134,7 @@ SplineHelper::QuinticSplinesFromControlVectors(
   return splines;
 }
 
-std::array<Spline<3>::ControlVector, 2>
+wpi::array<Spline<3>::ControlVector, 2>
 SplineHelper::CubicControlVectorsFromWaypoints(
     const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
     const Pose2d& end) {

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

@@ -4,7 +4,7 @@
 
 #pragma once
 
-#include <array>
+#include <wpi/array.h>
 
 #include "Eigen/Core"
 #include "Eigen/src/Cholesky/LLT.h"
@@ -44,8 +44,8 @@ class LinearQuadraticRegulatorImpl {
   template <int Outputs>
   LinearQuadraticRegulatorImpl(
       const LinearSystem<States, Inputs, Outputs>& plant,
-      const std::array<double, States>& Qelems,
-      const std::array<double, Inputs>& Relems, units::second_t dt)
+      const wpi::array<double, States>& Qelems,
+      const wpi::array<double, Inputs>& Relems, units::second_t dt)
       : LinearQuadraticRegulatorImpl(plant.A(), plant.B(), Qelems, Relems, dt) {
   }
 
@@ -60,8 +60,8 @@ class LinearQuadraticRegulatorImpl {
    */
   LinearQuadraticRegulatorImpl(const Eigen::Matrix<double, States, States>& A,
                                const Eigen::Matrix<double, States, Inputs>& B,
-                               const std::array<double, States>& Qelems,
-                               const std::array<double, Inputs>& Relems,
+                               const wpi::array<double, States>& Qelems,
+                               const wpi::array<double, Inputs>& Relems,
                                units::second_t dt)
       : LinearQuadraticRegulatorImpl(A, B, MakeCostMatrix(Qelems),
                                      MakeCostMatrix(Relems), dt) {}
@@ -226,8 +226,8 @@ class LinearQuadraticRegulator
    */
   template <int Outputs>
   LinearQuadraticRegulator(const LinearSystem<States, Inputs, Outputs>& plant,
-                           const std::array<double, States>& Qelems,
-                           const std::array<double, Inputs>& Relems,
+                           const wpi::array<double, States>& Qelems,
+                           const wpi::array<double, Inputs>& Relems,
                            units::second_t dt)
       : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
 
@@ -242,8 +242,8 @@ class LinearQuadraticRegulator
    */
   LinearQuadraticRegulator(const Eigen::Matrix<double, States, States>& A,
                            const Eigen::Matrix<double, States, Inputs>& B,
-                           const std::array<double, States>& Qelems,
-                           const std::array<double, Inputs>& Relems,
+                           const wpi::array<double, States>& Qelems,
+                           const wpi::array<double, Inputs>& Relems,
                            units::second_t dt)
       : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
                                  MakeCostMatrix(Relems), dt) {}
@@ -276,15 +276,15 @@ class LinearQuadraticRegulator<1, 1>
  public:
   template <int Outputs>
   LinearQuadraticRegulator(const LinearSystem<1, 1, Outputs>& plant,
-                           const std::array<double, 1>& Qelems,
-                           const std::array<double, 1>& Relems,
+                           const wpi::array<double, 1>& Qelems,
+                           const wpi::array<double, 1>& Relems,
                            units::second_t dt)
       : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
 
   LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
                            const Eigen::Matrix<double, 1, 1>& B,
-                           const std::array<double, 1>& Qelems,
-                           const std::array<double, 1>& Relems,
+                           const wpi::array<double, 1>& Qelems,
+                           const wpi::array<double, 1>& Relems,
                            units::second_t dt);
 
   LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
@@ -305,15 +305,15 @@ class LinearQuadraticRegulator<2, 1>
  public:
   template <int Outputs>
   LinearQuadraticRegulator(const LinearSystem<2, 1, Outputs>& plant,
-                           const std::array<double, 2>& Qelems,
-                           const std::array<double, 1>& Relems,
+                           const wpi::array<double, 2>& Qelems,
+                           const wpi::array<double, 1>& Relems,
                            units::second_t dt)
       : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
 
   LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
                            const Eigen::Matrix<double, 2, 1>& B,
-                           const std::array<double, 2>& Qelems,
-                           const std::array<double, 1>& Relems,
+                           const wpi::array<double, 2>& Qelems,
+                           const wpi::array<double, 1>& Relems,
                            units::second_t dt);
 
   LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,

wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h

@@ -4,7 +4,7 @@
 
 #pragma once
 
-#include <array>
+#include <wpi/array.h>
 
 #include "Eigen/Core"
 #include "frc/estimator/KalmanFilterLatencyCompensator.h"
@@ -74,9 +74,9 @@ class DifferentialDrivePoseEstimator {
    */
   DifferentialDrivePoseEstimator(
       const Rotation2d& gyroAngle, const Pose2d& initialPose,
-      const std::array<double, 5>& stateStdDevs,
-      const std::array<double, 3>& localMeasurementStdDevs,
-      const std::array<double, 3>& visionMeasurementStdDevs,
+      const wpi::array<double, 5>& stateStdDevs,
+      const wpi::array<double, 3>& localMeasurementStdDevs,
+      const wpi::array<double, 3>& visionMeasurementStdDevs,
       units::second_t nominalDt = 0.02_s);
 
   /**
@@ -92,7 +92,7 @@ class DifferentialDrivePoseEstimator {
    *                                 radians.
    */
   void SetVisionMeasurementStdDevs(
-      const std::array<double, 3>& visionMeasurementStdDevs);
+      const wpi::array<double, 3>& visionMeasurementStdDevs);
 
   /**
    * Resets the robot's position on the field.
@@ -183,7 +183,7 @@ class DifferentialDrivePoseEstimator {
   Rotation2d m_previousAngle;
 
   template <int Dim>
-  static std::array<double, Dim> StdDevMatrixToArray(
+  static wpi::array<double, Dim> StdDevMatrixToArray(
       const Eigen::Matrix<double, Dim, 1>& stdDevs);
 
   static Eigen::Matrix<double, 5, 1> F(const Eigen::Matrix<double, 5, 1>& x,

wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h

@@ -4,9 +4,10 @@
 
 #pragma once
 
-#include <array>
 #include <functional>
 
+#include <wpi/array.h>
+
 #include "Eigen/Core"
 #include "Eigen/src/Cholesky/LDLT.h"
 #include "drake/math/discrete_algebraic_riccati_equation.h"
@@ -40,8 +41,8 @@ class ExtendedKalmanFilter {
                            const Eigen::Matrix<double, States, 1>&,
                            const Eigen::Matrix<double, Inputs, 1>&)>
                            h,
-                       const std::array<double, States>& stateStdDevs,
-                       const std::array<double, Outputs>& measurementStdDevs,
+                       const wpi::array<double, States>& stateStdDevs,
+                       const wpi::array<double, Outputs>& measurementStdDevs,
                        units::second_t dt)
       : m_f(f), m_h(h) {
     m_contQ = MakeCovMatrix(stateStdDevs);