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

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(),