Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +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

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