Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath, wpiutil] Add wpi::array for compile time size checking (#3087)

Browse Source 
The wpimath APIs use std::array, which doesn't do size checking. Passing
an array with the wrong size can result in uninitialized elements
instead of a compilation error.

This is a breaking change but is worthwhile to avoid hard-to-debug errors.
This commit is contained in:
Tyler Veness
2021-01-16 20:26:17 -08:00
committed by GitHub
parent d6ed20c1e4
commit f393989a5b
25 changed files with 227 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp
View File

@@ -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;
}

2
wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -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]);

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

@@ -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.

4
wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp
View File

@@ -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) {}

12
wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
View File

@@ -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);
}

8
wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp
View File

@@ -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} {}

8
wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
View File

@@ -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);

8
wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp
View File

@@ -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);

8
wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
View File

@@ -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);

14
wpimath/src/main/native/cpp/spline/SplineHelper.cpp
View File

@@ -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) {

34
wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
View File

@@ -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,

12
wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
View File

@@ -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,

7
wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
View File

@@ -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);

19
wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
View File

@@ -4,9 +4,10 @@
#pragma once
#include <array>
#include <cmath>
#include <wpi/array.h>
#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<States, Inputs, Outputs>& plant,
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_plant = &plant;
@@ -193,8 +194,8 @@ class KalmanFilter : public detail::KalmanFilterImpl<States, Inputs, Outputs> {
* @param dt Nominal discretization timestep.
*/
KalmanFilter(LinearSystem<States, Inputs, Outputs>& plant,
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)
: detail::KalmanFilterImpl<States, Inputs, Outputs>{
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<double, 1>& stateStdDevs,
const std::array<double, 1>& measurementStdDevs,
const wpi::array<double, 1>& stateStdDevs,
const wpi::array<double, 1>& 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<double, 2>& stateStdDevs,
const std::array<double, 1>& measurementStdDevs,
const wpi::array<double, 2>& stateStdDevs,
const wpi::array<double, 1>& measurementStdDevs,
units::second_t dt);
KalmanFilter(KalmanFilter&&) = default;

14
wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
View File

@@ -6,6 +6,8 @@
#include <functional>
#include <wpi/array.h>
#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<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 = 0.02_s);
/**
@@ -89,7 +91,7 @@ class MecanumDrivePoseEstimator {
* radians.
*/
void SetVisionMeasurementStdDevs(
const std::array<double, 3>& visionMeasurementStdDevs);
const wpi::array<double, 3>& visionMeasurementStdDevs);
/**
* Resets the robot's position on the field.
@@ -177,9 +179,9 @@ class MecanumDrivePoseEstimator {
Rotation2d m_previousAngle;
template <int Dim>
static std::array<double, Dim> StdDevMatrixToArray(
static wpi::array<double, Dim> StdDevMatrixToArray(
const Eigen::Matrix<double, Dim, 1>& vector) {
std::array<double, Dim> array;
wpi::array<double, Dim> array;
for (size_t i = 0; i < Dim; ++i) {
array[i] = vector(i);
}

14
wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
View File

@@ -4,9 +4,9 @@
#pragma once
#include <array>
#include <limits>
#include <wpi/array.h>
#include <wpi/timestamp.h>
#include "Eigen/Core"
@@ -77,9 +77,9 @@ class SwerveDrivePoseEstimator {
SwerveDrivePoseEstimator(
const Rotation2d& gyroAngle, const Pose2d& initialPose,
SwerveDriveKinematics<NumModules>& 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 = 0.02_s)
: m_observer([](const Eigen::Matrix<double, 3, 1>& x,
const Eigen::Matrix<double, 3, 1>& u) { return u; },
@@ -158,7 +158,7 @@ class SwerveDrivePoseEstimator {
* radians.
*/
void SetVisionMeasurementStdDevs(
const std::array<double, 3>& visionMeasurementStdDevs) {
const wpi::array<double, 3>& visionMeasurementStdDevs) {
// Create R (covariances) for vision measurements.
Eigen::Matrix<double, 3, 3> visionContR =
frc::MakeCovMatrix(visionMeasurementStdDevs);
@@ -266,9 +266,9 @@ class SwerveDrivePoseEstimator {
Rotation2d m_previousAngle;
template <int Dim>
static std::array<double, Dim> StdDevMatrixToArray(
static wpi::array<double, Dim> StdDevMatrixToArray(
const Eigen::Matrix<double, Dim, 1>& vector) {
std::array<double, Dim> array;
wpi::array<double, Dim> array;
for (size_t i = 0; i < Dim; ++i) {
array[i] = vector(i);
}

11
wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
View File

@@ -4,9 +4,10 @@
#pragma once
#include <array>
#include <functional>
#include <wpi/array.h>
#include "Eigen/Core"
#include "Eigen/src/Cholesky/LDLT.h"
#include "frc/StateSpaceUtil.h"
@@ -41,8 +42,8 @@ class UnscentedKalmanFilter {
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);
@@ -101,8 +102,8 @@ class UnscentedKalmanFilter {
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,
std::function<Eigen::Matrix<double, States, 1>(
const Eigen::Matrix<double, States, 2 * States + 1>&,
const Eigen::Matrix<double, 2 * States + 1, 1>&)>

15
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
View File

@@ -4,9 +4,10 @@
#pragma once
#include <array>
#include <cstddef>
#include <wpi/array.h>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/geometry/Rotation2d.h"
@@ -74,7 +75,7 @@ class SwerveDriveKinematics {
}
explicit SwerveDriveKinematics(
const std::array<Translation2d, NumModules>& wheels)
const wpi::array<Translation2d, NumModules>& 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<SwerveModuleState, NumModules> ToSwerveModuleStates(
wpi::array<SwerveModuleState, NumModules> 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<SwerveModuleState, NumModules> moduleStates) const;
wpi::array<SwerveModuleState, NumModules> 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<SwerveModuleState, NumModules>* moduleStates,
wpi::array<SwerveModuleState, NumModules>* moduleStates,
units::meters_per_second_t attainableMaxSpeed);
private:
mutable Eigen::Matrix<double, NumModules * 2, 3> m_inverseKinematics;
Eigen::HouseholderQR<Eigen::Matrix<double, NumModules * 2, 3>>
m_forwardKinematics;
std::array<Translation2d, NumModules> m_modules;
wpi::array<Translation2d, NumModules> m_modules;
mutable Translation2d m_previousCoR;
};

10
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
View File

@@ -16,7 +16,7 @@ SwerveDriveKinematics(Translation2d, Wheels...)
-> SwerveDriveKinematics<1 + sizeof...(Wheels)>;
template <size_t NumModules>
std::array<SwerveModuleState, NumModules>
wpi::array<SwerveModuleState, NumModules>
SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
const ChassisSpeeds& chassisSpeeds,
const Translation2d& centerOfRotation) const {
@@ -38,7 +38,7 @@ SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix =
m_inverseKinematics * chassisSpeedsVector;
std::array<SwerveModuleState, NumModules> moduleStates;
wpi::array<SwerveModuleState, NumModules> moduleStates{wpi::empty_array};
for (size_t i = 0; i < NumModules; i++) {
units::meters_per_second_t x =
@@ -63,14 +63,14 @@ ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
"Number of modules is not consistent with number of wheel "
"locations provided in constructor.");
std::array<SwerveModuleState, NumModules> moduleStates{wheelStates...};
wpi::array<SwerveModuleState, NumModules> moduleStates{wheelStates...};
return this->ToChassisSpeeds(moduleStates);
}
template <size_t NumModules>
ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
std::array<SwerveModuleState, NumModules> moduleStates) const {
wpi::array<SwerveModuleState, NumModules> moduleStates) const {
Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix;
for (size_t i = 0; i < NumModules; i++) {
@@ -91,7 +91,7 @@ ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
template <size_t NumModules>
void SwerveDriveKinematics<NumModules>::NormalizeWheelSpeeds(
std::array<SwerveModuleState, NumModules>* moduleStates,
wpi::array<SwerveModuleState, NumModules>* moduleStates,
units::meters_per_second_t attainableMaxSpeed) {
auto& states = *moduleStates;
auto realMaxSpeed = std::max_element(states.begin(), states.end(),

12
wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
View File

@@ -4,7 +4,7 @@
#pragma once
#include <array>
#include <wpi/array.h>
#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<double, 2> xInitialControlVector,
std::array<double, 2> xFinalControlVector,
std::array<double, 2> yInitialControlVector,
std::array<double, 2> yFinalControlVector);
CubicHermiteSpline(wpi::array<double, 2> xInitialControlVector,
wpi::array<double, 2> xFinalControlVector,
wpi::array<double, 2> yInitialControlVector,
wpi::array<double, 2> 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<double, 2> initialVector, std::array<double, 2> finalVector) {
wpi::array<double, 2> initialVector, wpi::array<double, 2> finalVector) {
return (Eigen::Vector4d() << initialVector[0], initialVector[1],
finalVector[0], finalVector[1])
.finished();

12
wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
View File

@@ -4,7 +4,7 @@
#pragma once
#include <array>
#include <wpi/array.h>
#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<double, 3> xInitialControlVector,
std::array<double, 3> xFinalControlVector,
std::array<double, 3> yInitialControlVector,
std::array<double, 3> yFinalControlVector);
QuinticHermiteSpline(wpi::array<double, 3> xInitialControlVector,
wpi::array<double, 3> xFinalControlVector,
wpi::array<double, 3> yInitialControlVector,
wpi::array<double, 3> yFinalControlVector);
protected:
/**
@@ -74,7 +74,7 @@ class QuinticHermiteSpline : public Spline<5> {
* @return The control vector matrix for a dimension.
*/
static Eigen::Matrix<double, 6, 1> ControlVectorFromArrays(
std::array<double, 3> initialVector, std::array<double, 3> finalVector) {
wpi::array<double, 3> initialVector, wpi::array<double, 3> finalVector) {
return (Eigen::Matrix<double, 6, 1>() << initialVector[0], initialVector[1],
initialVector[2], finalVector[0], finalVector[1], finalVector[2])
.finished();

7
wpimath/src/main/native/include/frc/spline/Spline.h
View File

@@ -4,10 +4,11 @@
#pragma once
#include <array>
#include <utility>
#include <vector>
#include <wpi/array.h>
#include "Eigen/Core"
#include "frc/geometry/Pose2d.h"
#include "units/curvature.h"
@@ -43,8 +44,8 @@ class Spline {
* dimension.
*/
struct ControlVector {
std::array<double, (Degree + 1) / 2> x;
std::array<double, (Degree + 1) / 2> y;
wpi::array<double, (Degree + 1) / 2> x;
wpi::array<double, (Degree + 1) / 2> y;
};
/**

5
wpimath/src/main/native/include/frc/spline/SplineHelper.h
View File

@@ -4,10 +4,11 @@
#pragma once
#include <array>
#include <utility>
#include <vector>
#include <wpi/array.h>
#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<Spline<3>::ControlVector, 2>
static wpi::array<Spline<3>::ControlVector, 2>
CubicControlVectorsFromWaypoints(
const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
const Pose2d& end);

2
wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
View File

@@ -168,7 +168,7 @@ TEST_F(SwerveDriveKinematicsTest, NormalizeTest) {
SwerveModuleState state3{4.0_mps, Rotation2d()};
SwerveModuleState state4{7.0_mps, Rotation2d()};
std::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
wpi::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::NormalizeWheelSpeeds(&arr, 5.5_mps);
double kFactor = 5.5 / 7.0;

101
wpiutil/src/main/native/include/wpi/array.h Normal file
View File

@@ -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 <array>
#include <cstddef>
#include <tuple>
#include <utility>
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 <typename T, size_t N>
class array : public std::array<T, N> {
public:
explicit array(empty_array_t) {}
template <typename... Ts>
array(T arg, Ts&&... args) // NOLINT
: std::array<T, N>{arg, std::forward<Ts>(args)...} {
static_assert(1 + sizeof...(args) == N, "Dimension mismatch");
}
array(const array<T, N>&) = default;
array& operator=(const array<T, N>&) = default;
array(array<T, N>&&) = default;
array& operator=(array<T, N>&&) = default;
array(const std::array<T, N>& rhs) { // NOLINT
*static_cast<std::array<T, N>*>(this) = rhs;
}
array& operator=(const std::array<T, N>& rhs) {
*static_cast<std::array<T, N>*>(this) = rhs;
return *this;
}
array(std::array<T, N>&& rhs) { // NOLINT
*static_cast<std::array<T, N>*>(this) = rhs;
}
array& operator=(std::array<T, N>&& rhs) {
*static_cast<std::array<T, N>*>(this) = rhs;
return *this;
}
};
template <typename T, typename... Ts>
array(T, Ts...) -> array<std::enable_if_t<(std::is_same_v<T, Ts> && ...), T>,
1 + sizeof...(Ts)>;
} // namespace wpi
template <size_t I, typename T, size_t N>
constexpr T& get(wpi::array<T, N>& arr) noexcept {
static_assert(I < N, "array index is within bounds");
return std::get<I>(static_cast<std::array<T, N>>(arr));
}
template <size_t I, typename T, size_t N>
constexpr T&& get(wpi::array<T, N>&& arr) noexcept {
static_assert(I < N, "array index is within bounds");
return std::move(std::get<I>(arr));
}
template <size_t I, typename T, size_t N>
constexpr const T& get(const wpi::array<T, N>& arr) noexcept {
static_assert(I < N, "array index is within bounds");
return std::get<I>(static_cast<std::array<T, N>>(arr));
}
template <size_t I, typename T, size_t N>
constexpr const T&& get(const wpi::array<T, N>&& arr) noexcept {
static_assert(I < N, "array index is within bounds");
return std::move(std::get<I>(arr));
}
// Enables structured bindings
namespace std { // NOLINT
// Partial specialization for wpi::array
template <typename T, size_t N>
struct tuple_size<wpi::array<T, N>> : public integral_constant<size_t, N> {};
// Partial specialization for wpi::array
template <size_t I, typename T, size_t N>
struct tuple_element<I, wpi::array<T, N>> {
static_assert(I < N, "index is out of bounds");
using type = T;
};
} // namespace std