diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp index bf9d8083bc5..357dd80be4f 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp +++ b/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 m_moduleStates{ + wpi::array 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 getCurrentWheelSpeeds() { + wpi::array getCurrentWheelSpeeds() { return m_moduleStates; } diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp index 094669d7fd0..7ef4af32416 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp +++ b/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 desiredStates) { + wpi::array desiredStates) { kDriveKinematics.NormalizeWheelSpeeds(&desiredStates, AutoConstants::kMaxSpeed); m_frontLeft.SetDesiredState(desiredStates[0]); diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h index 1513b5ed651..9fd548cffd7 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h +++ b/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 desiredStates); + void SetModuleStates(wpi::array desiredStates); /** * Returns the heading of the robot. diff --git a/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp b/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp index dd99dca221f..140ffaa1383 100644 --- a/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp +++ b/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp @@ -8,7 +8,7 @@ namespace frc { LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator( const Eigen::Matrix& A, const Eigen::Matrix& B, - const std::array& Qelems, const std::array& Relems, + const wpi::array& Qelems, const wpi::array& 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& A, const Eigen::Matrix& B, - const std::array& Qelems, const std::array& Relems, + const wpi::array& Qelems, const wpi::array& Relems, units::second_t dt) : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems), MakeCostMatrix(Relems), dt) {} diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index 54ece5998fc..c5e50b6aaef 100644 --- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/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& stateStdDevs, - const std::array& localMeasurementStdDevs, - const std::array& visionMeasurmentStdDevs, + const wpi::array& stateStdDevs, + const wpi::array& localMeasurementStdDevs, + const wpi::array& visionMeasurmentStdDevs, units::second_t nominalDt) : m_observer( &DifferentialDrivePoseEstimator::F, @@ -46,7 +46,7 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( } void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs( - const std::array& visionMeasurmentStdDevs) { + const wpi::array& visionMeasurmentStdDevs) { // Create R (covariances) for vision measurements. Eigen::Matrix visionContR = frc::MakeCovMatrix(visionMeasurmentStdDevs); @@ -126,9 +126,9 @@ Eigen::Matrix DifferentialDrivePoseEstimator::F( } template -std::array DifferentialDrivePoseEstimator::StdDevMatrixToArray( +wpi::array DifferentialDrivePoseEstimator::StdDevMatrixToArray( const Eigen::Matrix& stdDevs) { - std::array array; + wpi::array array; for (size_t i = 0; i < Dim; ++i) { array[i] = stdDevs(i); } diff --git a/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp b/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp index 2ab7528cc85..1209eae0bb2 100644 --- a/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp +++ b/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& stateStdDevs, - const std::array& measurementStdDevs, units::second_t dt) + LinearSystem<1, 1, 1>& plant, const wpi::array& stateStdDevs, + const wpi::array& 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& stateStdDevs, - const std::array& measurementStdDevs, units::second_t dt) + LinearSystem<2, 1, 1>& plant, const wpi::array& stateStdDevs, + const wpi::array& measurementStdDevs, units::second_t dt) : detail::KalmanFilterImpl<2, 1, 1>{plant, stateStdDevs, measurementStdDevs, dt} {} diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index 8f1fc2fa7e2..44453b390f1 100644 --- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/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& stateStdDevs, - const std::array& localMeasurementStdDevs, - const std::array& visionMeasurementStdDevs, + const wpi::array& stateStdDevs, + const wpi::array& localMeasurementStdDevs, + const wpi::array& visionMeasurementStdDevs, units::second_t nominalDt) : m_observer([](const Eigen::Matrix& x, const Eigen::Matrix& u) { return u; }, @@ -51,7 +51,7 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( } void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs( - const std::array& visionMeasurmentStdDevs) { + const wpi::array& visionMeasurmentStdDevs) { // Create R (covariances) for vision measurements. Eigen::Matrix visionContR = frc::MakeCovMatrix(visionMeasurmentStdDevs); diff --git a/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp b/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp index 82c01c7f90d..b643849aaa6 100644 --- a/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp +++ b/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp @@ -7,10 +7,10 @@ using namespace frc; CubicHermiteSpline::CubicHermiteSpline( - std::array xInitialControlVector, - std::array xFinalControlVector, - std::array yInitialControlVector, - std::array yFinalControlVector) { + wpi::array xInitialControlVector, + wpi::array xFinalControlVector, + wpi::array yInitialControlVector, + wpi::array yFinalControlVector) { const auto hermite = MakeHermiteBasis(); const auto x = ControlVectorFromArrays(xInitialControlVector, xFinalControlVector); diff --git a/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp b/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp index d0b57233469..5362b7c6ba4 100644 --- a/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp +++ b/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp @@ -7,10 +7,10 @@ using namespace frc; QuinticHermiteSpline::QuinticHermiteSpline( - std::array xInitialControlVector, - std::array xFinalControlVector, - std::array yInitialControlVector, - std::array yFinalControlVector) { + wpi::array xInitialControlVector, + wpi::array xFinalControlVector, + wpi::array yInitialControlVector, + wpi::array yFinalControlVector) { const auto hermite = MakeHermiteBasis(); const auto x = ControlVectorFromArrays(xInitialControlVector, xFinalControlVector); diff --git a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp index 42652b0d950..c35c58d83b4 100644 --- a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp +++ b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp @@ -13,10 +13,10 @@ std::vector SplineHelper::CubicSplinesFromControlVectors( const Spline<3>::ControlVector& end) { std::vector splines; - std::array xInitial = start.x; - std::array yInitial = start.y; - std::array xFinal = end.x; - std::array yFinal = end.y; + wpi::array xInitial = start.x; + wpi::array yInitial = start.y; + wpi::array xFinal = end.x; + wpi::array yFinal = end.y; if (waypoints.size() > 1) { waypoints.emplace(waypoints.begin(), @@ -102,9 +102,9 @@ std::vector SplineHelper::CubicSplinesFromControlVectors( const double yDeriv = (3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0; - std::array midXControlVector{waypoints[0].X().to(), + wpi::array midXControlVector{waypoints[0].X().to(), xDeriv}; - std::array midYControlVector{waypoints[0].Y().to(), + wpi::array midYControlVector{waypoints[0].Y().to(), yDeriv}; splines.emplace_back(xInitial, midXControlVector, yInitial, @@ -134,7 +134,7 @@ SplineHelper::QuinticSplinesFromControlVectors( return splines; } -std::array::ControlVector, 2> +wpi::array::ControlVector, 2> SplineHelper::CubicControlVectorsFromWaypoints( const Pose2d& start, const std::vector& interiorWaypoints, const Pose2d& end) { diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h index df7a721b7a2..9662710c1e2 100644 --- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h +++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h @@ -4,7 +4,7 @@ #pragma once -#include +#include #include "Eigen/Core" #include "Eigen/src/Cholesky/LLT.h" @@ -44,8 +44,8 @@ class LinearQuadraticRegulatorImpl { template LinearQuadraticRegulatorImpl( const LinearSystem& plant, - const std::array& Qelems, - const std::array& Relems, units::second_t dt) + const wpi::array& Qelems, + const wpi::array& Relems, units::second_t dt) : LinearQuadraticRegulatorImpl(plant.A(), plant.B(), Qelems, Relems, dt) { } @@ -60,8 +60,8 @@ class LinearQuadraticRegulatorImpl { */ LinearQuadraticRegulatorImpl(const Eigen::Matrix& A, const Eigen::Matrix& B, - const std::array& Qelems, - const std::array& Relems, + const wpi::array& Qelems, + const wpi::array& Relems, units::second_t dt) : LinearQuadraticRegulatorImpl(A, B, MakeCostMatrix(Qelems), MakeCostMatrix(Relems), dt) {} @@ -226,8 +226,8 @@ class LinearQuadraticRegulator */ template LinearQuadraticRegulator(const LinearSystem& plant, - const std::array& Qelems, - const std::array& Relems, + const wpi::array& Qelems, + const wpi::array& Relems, units::second_t dt) : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {} @@ -242,8 +242,8 @@ class LinearQuadraticRegulator */ LinearQuadraticRegulator(const Eigen::Matrix& A, const Eigen::Matrix& B, - const std::array& Qelems, - const std::array& Relems, + const wpi::array& Qelems, + const wpi::array& Relems, units::second_t dt) : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems), MakeCostMatrix(Relems), dt) {} @@ -276,15 +276,15 @@ class LinearQuadraticRegulator<1, 1> public: template LinearQuadraticRegulator(const LinearSystem<1, 1, Outputs>& plant, - const std::array& Qelems, - const std::array& Relems, + const wpi::array& Qelems, + const wpi::array& Relems, units::second_t dt) : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {} LinearQuadraticRegulator(const Eigen::Matrix& A, const Eigen::Matrix& B, - const std::array& Qelems, - const std::array& Relems, + const wpi::array& Qelems, + const wpi::array& Relems, units::second_t dt); LinearQuadraticRegulator(const Eigen::Matrix& A, @@ -305,15 +305,15 @@ class LinearQuadraticRegulator<2, 1> public: template LinearQuadraticRegulator(const LinearSystem<2, 1, Outputs>& plant, - const std::array& Qelems, - const std::array& Relems, + const wpi::array& Qelems, + const wpi::array& Relems, units::second_t dt) : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {} LinearQuadraticRegulator(const Eigen::Matrix& A, const Eigen::Matrix& B, - const std::array& Qelems, - const std::array& Relems, + const wpi::array& Qelems, + const wpi::array& Relems, units::second_t dt); LinearQuadraticRegulator(const Eigen::Matrix& A, diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h index 91fe669a0cf..ced768a4d48 100644 --- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h @@ -4,7 +4,7 @@ #pragma once -#include +#include #include "Eigen/Core" #include "frc/estimator/KalmanFilterLatencyCompensator.h" @@ -74,9 +74,9 @@ class DifferentialDrivePoseEstimator { */ DifferentialDrivePoseEstimator( const Rotation2d& gyroAngle, const Pose2d& initialPose, - const std::array& stateStdDevs, - const std::array& localMeasurementStdDevs, - const std::array& visionMeasurementStdDevs, + const wpi::array& stateStdDevs, + const wpi::array& localMeasurementStdDevs, + const wpi::array& visionMeasurementStdDevs, units::second_t nominalDt = 0.02_s); /** @@ -92,7 +92,7 @@ class DifferentialDrivePoseEstimator { * radians. */ void SetVisionMeasurementStdDevs( - const std::array& visionMeasurementStdDevs); + const wpi::array& visionMeasurementStdDevs); /** * Resets the robot's position on the field. @@ -183,7 +183,7 @@ class DifferentialDrivePoseEstimator { Rotation2d m_previousAngle; template - static std::array StdDevMatrixToArray( + static wpi::array StdDevMatrixToArray( const Eigen::Matrix& stdDevs); static Eigen::Matrix F(const Eigen::Matrix& x, diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h index d56ca7e6670..ac8ddc86ed9 100644 --- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h @@ -4,9 +4,10 @@ #pragma once -#include #include +#include + #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&, const Eigen::Matrix&)> h, - const std::array& stateStdDevs, - const std::array& measurementStdDevs, + const wpi::array& stateStdDevs, + const wpi::array& measurementStdDevs, units::second_t dt) : m_f(f), m_h(h) { m_contQ = MakeCovMatrix(stateStdDevs); diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h index 84c5923f26a..2e8a7afe545 100644 --- a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h @@ -4,9 +4,10 @@ #pragma once -#include #include +#include + #include "Eigen/Core" #include "Eigen/src/Cholesky/LDLT.h" #include "drake/math/discrete_algebraic_riccati_equation.h" @@ -47,8 +48,8 @@ class KalmanFilterImpl { * @param dt Nominal discretization timestep. */ KalmanFilterImpl(LinearSystem& plant, - const std::array& stateStdDevs, - const std::array& measurementStdDevs, + const wpi::array& stateStdDevs, + const wpi::array& measurementStdDevs, units::second_t dt) { m_plant = &plant; @@ -193,8 +194,8 @@ class KalmanFilter : public detail::KalmanFilterImpl { * @param dt Nominal discretization timestep. */ KalmanFilter(LinearSystem& plant, - const std::array& stateStdDevs, - const std::array& measurementStdDevs, + const wpi::array& stateStdDevs, + const wpi::array& measurementStdDevs, units::second_t dt) : detail::KalmanFilterImpl{ plant, stateStdDevs, measurementStdDevs, dt} {} @@ -209,8 +210,8 @@ template <> class KalmanFilter<1, 1, 1> : public detail::KalmanFilterImpl<1, 1, 1> { public: KalmanFilter(LinearSystem<1, 1, 1>& plant, - const std::array& stateStdDevs, - const std::array& measurementStdDevs, + const wpi::array& stateStdDevs, + const wpi::array& measurementStdDevs, units::second_t dt); KalmanFilter(KalmanFilter&&) = default; @@ -223,8 +224,8 @@ template <> class KalmanFilter<2, 1, 1> : public detail::KalmanFilterImpl<2, 1, 1> { public: KalmanFilter(LinearSystem<2, 1, 1>& plant, - const std::array& stateStdDevs, - const std::array& measurementStdDevs, + const wpi::array& stateStdDevs, + const wpi::array& measurementStdDevs, units::second_t dt); KalmanFilter(KalmanFilter&&) = default; diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h index c8b08963fa1..c7fb8590bae 100644 --- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h @@ -6,6 +6,8 @@ #include +#include + #include "Eigen/Core" #include "frc/estimator/KalmanFilterLatencyCompensator.h" #include "frc/estimator/UnscentedKalmanFilter.h" @@ -71,9 +73,9 @@ class MecanumDrivePoseEstimator { MecanumDrivePoseEstimator( const Rotation2d& gyroAngle, const Pose2d& initialPose, MecanumDriveKinematics kinematics, - const std::array& stateStdDevs, - const std::array& localMeasurementStdDevs, - const std::array& visionMeasurementStdDevs, + const wpi::array& stateStdDevs, + const wpi::array& localMeasurementStdDevs, + const wpi::array& visionMeasurementStdDevs, units::second_t nominalDt = 0.02_s); /** @@ -89,7 +91,7 @@ class MecanumDrivePoseEstimator { * radians. */ void SetVisionMeasurementStdDevs( - const std::array& visionMeasurementStdDevs); + const wpi::array& visionMeasurementStdDevs); /** * Resets the robot's position on the field. @@ -177,9 +179,9 @@ class MecanumDrivePoseEstimator { Rotation2d m_previousAngle; template - static std::array StdDevMatrixToArray( + static wpi::array StdDevMatrixToArray( const Eigen::Matrix& vector) { - std::array array; + wpi::array array; for (size_t i = 0; i < Dim; ++i) { array[i] = vector(i); } diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index 4a86b5af618..c776f48c325 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -4,9 +4,9 @@ #pragma once -#include #include +#include #include #include "Eigen/Core" @@ -77,9 +77,9 @@ class SwerveDrivePoseEstimator { SwerveDrivePoseEstimator( const Rotation2d& gyroAngle, const Pose2d& initialPose, SwerveDriveKinematics& kinematics, - const std::array& stateStdDevs, - const std::array& localMeasurementStdDevs, - const std::array& visionMeasurementStdDevs, + const wpi::array& stateStdDevs, + const wpi::array& localMeasurementStdDevs, + const wpi::array& visionMeasurementStdDevs, units::second_t nominalDt = 0.02_s) : m_observer([](const Eigen::Matrix& x, const Eigen::Matrix& u) { return u; }, @@ -158,7 +158,7 @@ class SwerveDrivePoseEstimator { * radians. */ void SetVisionMeasurementStdDevs( - const std::array& visionMeasurementStdDevs) { + const wpi::array& visionMeasurementStdDevs) { // Create R (covariances) for vision measurements. Eigen::Matrix visionContR = frc::MakeCovMatrix(visionMeasurementStdDevs); @@ -266,9 +266,9 @@ class SwerveDrivePoseEstimator { Rotation2d m_previousAngle; template - static std::array StdDevMatrixToArray( + static wpi::array StdDevMatrixToArray( const Eigen::Matrix& vector) { - std::array array; + wpi::array array; for (size_t i = 0; i < Dim; ++i) { array[i] = vector(i); } diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h index 33a24c19bd8..b5c1e51899e 100644 --- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h @@ -4,9 +4,10 @@ #pragma once -#include #include +#include + #include "Eigen/Core" #include "Eigen/src/Cholesky/LDLT.h" #include "frc/StateSpaceUtil.h" @@ -41,8 +42,8 @@ class UnscentedKalmanFilter { const Eigen::Matrix&, const Eigen::Matrix&)> h, - const std::array& stateStdDevs, - const std::array& measurementStdDevs, + const wpi::array& stateStdDevs, + const wpi::array& measurementStdDevs, units::second_t dt) : m_f(f), m_h(h) { m_contQ = MakeCovMatrix(stateStdDevs); @@ -101,8 +102,8 @@ class UnscentedKalmanFilter { const Eigen::Matrix&, const Eigen::Matrix&)> h, - const std::array& stateStdDevs, - const std::array& measurementStdDevs, + const wpi::array& stateStdDevs, + const wpi::array& measurementStdDevs, std::function( const Eigen::Matrix&, const Eigen::Matrix&)> diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index 5660dc8bdc5..11298a1355f 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -4,9 +4,10 @@ #pragma once -#include #include +#include + #include "Eigen/Core" #include "Eigen/QR" #include "frc/geometry/Rotation2d.h" @@ -74,7 +75,7 @@ class SwerveDriveKinematics { } explicit SwerveDriveKinematics( - const std::array& wheels) + const wpi::array& wheels) : m_modules{wheels} { for (size_t i = 0; i < NumModules; i++) { // clang-format off @@ -119,7 +120,7 @@ class SwerveDriveKinematics { * auto [fl, fr, bl, br] = kinematics.ToSwerveModuleStates(chassisSpeeds); * @endcode */ - std::array ToSwerveModuleStates( + wpi::array ToSwerveModuleStates( const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation = Translation2d()) const; @@ -144,7 +145,7 @@ 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 moduleStates The state of the modules as an std::array of type + * @param moduleStates The state of the modules as an wpi::array of type * SwerveModuleState, NumModules long 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. @@ -152,7 +153,7 @@ class SwerveDriveKinematics { * @return The resulting chassis speed. */ ChassisSpeeds ToChassisSpeeds( - std::array moduleStates) const; + wpi::array moduleStates) const; /** * Normalizes the wheel speeds using some max attainable speed. Sometimes, @@ -167,14 +168,14 @@ class SwerveDriveKinematics { * @param attainableMaxSpeed The absolute max speed that a module can reach. */ static void NormalizeWheelSpeeds( - std::array* moduleStates, + wpi::array* moduleStates, units::meters_per_second_t attainableMaxSpeed); private: mutable Eigen::Matrix m_inverseKinematics; Eigen::HouseholderQR> m_forwardKinematics; - std::array m_modules; + wpi::array m_modules; mutable Translation2d m_previousCoR; }; diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc index 9fbbc9ef6c0..374db562d7d 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc @@ -16,7 +16,7 @@ SwerveDriveKinematics(Translation2d, Wheels...) -> SwerveDriveKinematics<1 + sizeof...(Wheels)>; template -std::array +wpi::array SwerveDriveKinematics::ToSwerveModuleStates( const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation) const { @@ -38,7 +38,7 @@ SwerveDriveKinematics::ToSwerveModuleStates( Eigen::Matrix moduleStatesMatrix = m_inverseKinematics * chassisSpeedsVector; - std::array moduleStates; + wpi::array moduleStates{wpi::empty_array}; for (size_t i = 0; i < NumModules; i++) { units::meters_per_second_t x = @@ -63,14 +63,14 @@ ChassisSpeeds SwerveDriveKinematics::ToChassisSpeeds( "Number of modules is not consistent with number of wheel " "locations provided in constructor."); - std::array moduleStates{wheelStates...}; + wpi::array moduleStates{wheelStates...}; return this->ToChassisSpeeds(moduleStates); } template ChassisSpeeds SwerveDriveKinematics::ToChassisSpeeds( - std::array moduleStates) const { + wpi::array moduleStates) const { Eigen::Matrix moduleStatesMatrix; for (size_t i = 0; i < NumModules; i++) { @@ -91,7 +91,7 @@ ChassisSpeeds SwerveDriveKinematics::ToChassisSpeeds( template void SwerveDriveKinematics::NormalizeWheelSpeeds( - std::array* moduleStates, + wpi::array* moduleStates, units::meters_per_second_t attainableMaxSpeed) { auto& states = *moduleStates; auto realMaxSpeed = std::max_element(states.begin(), states.end(), diff --git a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h index 38f3840f563..c30a8a89a84 100644 --- a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h +++ b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h @@ -4,7 +4,7 @@ #pragma once -#include +#include #include "Eigen/Core" #include "frc/spline/Spline.h" @@ -29,10 +29,10 @@ class CubicHermiteSpline : public Spline<3> { * @param yFinalControlVector The control vector for the final point in * the y dimension. */ - CubicHermiteSpline(std::array xInitialControlVector, - std::array xFinalControlVector, - std::array yInitialControlVector, - std::array yFinalControlVector); + CubicHermiteSpline(wpi::array xInitialControlVector, + wpi::array xFinalControlVector, + wpi::array yInitialControlVector, + wpi::array yFinalControlVector); protected: /** @@ -72,7 +72,7 @@ class CubicHermiteSpline : public Spline<3> { * @return The control vector matrix for a dimension. */ static Eigen::Vector4d ControlVectorFromArrays( - std::array initialVector, std::array finalVector) { + wpi::array initialVector, wpi::array finalVector) { return (Eigen::Vector4d() << initialVector[0], initialVector[1], finalVector[0], finalVector[1]) .finished(); diff --git a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h index 0a85be63271..ff9369ce27b 100644 --- a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h +++ b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h @@ -4,7 +4,7 @@ #pragma once -#include +#include #include "Eigen/Core" #include "frc/spline/Spline.h" @@ -29,10 +29,10 @@ class QuinticHermiteSpline : public Spline<5> { * @param yFinalControlVector The control vector for the final point in * the y dimension. */ - QuinticHermiteSpline(std::array xInitialControlVector, - std::array xFinalControlVector, - std::array yInitialControlVector, - std::array yFinalControlVector); + QuinticHermiteSpline(wpi::array xInitialControlVector, + wpi::array xFinalControlVector, + wpi::array yInitialControlVector, + wpi::array yFinalControlVector); protected: /** @@ -74,7 +74,7 @@ class QuinticHermiteSpline : public Spline<5> { * @return The control vector matrix for a dimension. */ static Eigen::Matrix ControlVectorFromArrays( - std::array initialVector, std::array finalVector) { + wpi::array initialVector, wpi::array finalVector) { return (Eigen::Matrix() << initialVector[0], initialVector[1], initialVector[2], finalVector[0], finalVector[1], finalVector[2]) .finished(); diff --git a/wpimath/src/main/native/include/frc/spline/Spline.h b/wpimath/src/main/native/include/frc/spline/Spline.h index d048aed4749..a04564ac4d0 100644 --- a/wpimath/src/main/native/include/frc/spline/Spline.h +++ b/wpimath/src/main/native/include/frc/spline/Spline.h @@ -4,10 +4,11 @@ #pragma once -#include #include #include +#include + #include "Eigen/Core" #include "frc/geometry/Pose2d.h" #include "units/curvature.h" @@ -43,8 +44,8 @@ class Spline { * dimension. */ struct ControlVector { - std::array x; - std::array y; + wpi::array x; + wpi::array y; }; /** diff --git a/wpimath/src/main/native/include/frc/spline/SplineHelper.h b/wpimath/src/main/native/include/frc/spline/SplineHelper.h index d6dc3c69483..4af3a8a9f8b 100644 --- a/wpimath/src/main/native/include/frc/spline/SplineHelper.h +++ b/wpimath/src/main/native/include/frc/spline/SplineHelper.h @@ -4,10 +4,11 @@ #pragma once -#include #include #include +#include + #include "frc/spline/CubicHermiteSpline.h" #include "frc/spline/QuinticHermiteSpline.h" @@ -27,7 +28,7 @@ class SplineHelper { * @param end The ending pose. * @return 2 cubic control vectors. */ - static std::array::ControlVector, 2> + static wpi::array::ControlVector, 2> CubicControlVectorsFromWaypoints( const Pose2d& start, const std::vector& interiorWaypoints, const Pose2d& end); diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp index 20ad4407f1b..18923a96ba1 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp @@ -168,7 +168,7 @@ TEST_F(SwerveDriveKinematicsTest, NormalizeTest) { SwerveModuleState state3{4.0_mps, Rotation2d()}; SwerveModuleState state4{7.0_mps, Rotation2d()}; - std::array arr{state1, state2, state3, state4}; + wpi::array arr{state1, state2, state3, state4}; SwerveDriveKinematics<4>::NormalizeWheelSpeeds(&arr, 5.5_mps); double kFactor = 5.5 / 7.0; diff --git a/wpiutil/src/main/native/include/wpi/array.h b/wpiutil/src/main/native/include/wpi/array.h new file mode 100644 index 00000000000..5a0337179c6 --- /dev/null +++ b/wpiutil/src/main/native/include/wpi/array.h @@ -0,0 +1,101 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include +#include +#include + +namespace wpi { + +struct empty_array_t {}; +constexpr empty_array_t empty_array; + +/** + * This class is a wrapper around std::array that does compile time size + * checking. + * + * std::array's implicit constructor can lead result in uninitialized elements + * if the number of arguments doesn't match the std::array size. + */ +template +class array : public std::array { + public: + explicit array(empty_array_t) {} + + template + array(T arg, Ts&&... args) // NOLINT + : std::array{arg, std::forward(args)...} { + static_assert(1 + sizeof...(args) == N, "Dimension mismatch"); + } + + array(const array&) = default; + array& operator=(const array&) = default; + array(array&&) = default; + array& operator=(array&&) = default; + + array(const std::array& rhs) { // NOLINT + *static_cast*>(this) = rhs; + } + + array& operator=(const std::array& rhs) { + *static_cast*>(this) = rhs; + return *this; + } + + array(std::array&& rhs) { // NOLINT + *static_cast*>(this) = rhs; + } + + array& operator=(std::array&& rhs) { + *static_cast*>(this) = rhs; + return *this; + } +}; + +template +array(T, Ts...) -> array && ...), T>, + 1 + sizeof...(Ts)>; + +} // namespace wpi + +template +constexpr T& get(wpi::array& arr) noexcept { + static_assert(I < N, "array index is within bounds"); + return std::get(static_cast>(arr)); +} + +template +constexpr T&& get(wpi::array&& arr) noexcept { + static_assert(I < N, "array index is within bounds"); + return std::move(std::get(arr)); +} + +template +constexpr const T& get(const wpi::array& arr) noexcept { + static_assert(I < N, "array index is within bounds"); + return std::get(static_cast>(arr)); +} + +template +constexpr const T&& get(const wpi::array&& arr) noexcept { + static_assert(I < N, "array index is within bounds"); + return std::move(std::get(arr)); +} + +// Enables structured bindings +namespace std { // NOLINT +// Partial specialization for wpi::array +template +struct tuple_size> : public integral_constant {}; + +// Partial specialization for wpi::array +template +struct tuple_element> { + static_assert(I < N, "index is out of bounds"); + using type = T; +}; +} // namespace std