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

[wpimath] Add typedefs for common types

Browse Source 
This makes complex code significantly easier to read.

frc::Vectord<Size> = Eigen::Vector<double, Size>
frc::Matrixd<Rows, Cols> = Eigen::Matrix<double, Rows, Cols>
This commit is contained in:
Peter Johnson
2022-04-29 22:29:20 -07:00
parent 97c493241f
commit e767605e94
76 changed files with 1136 additions and 1449 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
View File

@@ -6,8 +6,8 @@
#include <wpi/SymbolExports.h>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
@@ -115,8 +115,8 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics {
const MecanumDriveWheelSpeeds& wheelSpeeds) const;
private:
mutable Eigen::Matrix<double, 4, 3> m_inverseKinematics;
Eigen::HouseholderQR<Eigen::Matrix<double, 4, 3>> m_forwardKinematics;
mutable Matrixd<4, 3> m_inverseKinematics;
Eigen::HouseholderQR<Matrixd<4, 3>> m_forwardKinematics;
Translation2d m_frontLeftWheel;
Translation2d m_frontRightWheel;
Translation2d m_rearLeftWheel;

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

@@ -9,8 +9,8 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
@@ -179,9 +179,8 @@ class SwerveDriveKinematics {
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;
mutable Matrixd<NumModules * 2, 3> m_inverseKinematics;
Eigen::HouseholderQR<Matrixd<NumModules * 2, 3>> m_forwardKinematics;
wpi::array<Translation2d, NumModules> m_modules;
mutable Translation2d m_previousCoR;

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

@@ -25,7 +25,7 @@ SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
for (size_t i = 0; i < NumModules; i++) {
// clang-format off
m_inverseKinematics.template block<2, 3>(i * 2, 0) =
Eigen::Matrix<double, 2, 3>{
Matrixd<2, 3>{
{1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).value()},
{0, 1, (+m_modules[i].X() - centerOfRotation.X()).value()}};
// clang-format on
@@ -37,13 +37,13 @@ SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
chassisSpeeds.vy.value(),
chassisSpeeds.omega.value()};
Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix =
Matrixd<NumModules * 2, 1> moduleStateMatrix =
m_inverseKinematics * chassisSpeedsVector;
wpi::array<SwerveModuleState, NumModules> moduleStates{wpi::empty_array};
for (size_t i = 0; i < NumModules; i++) {
units::meters_per_second_t x{moduleStatesMatrix(i * 2, 0)};
units::meters_per_second_t y{moduleStatesMatrix(i * 2 + 1, 0)};
units::meters_per_second_t x{moduleStateMatrix(i * 2, 0)};
units::meters_per_second_t y{moduleStateMatrix(i * 2 + 1, 0)};
auto speed = units::math::hypot(x, y);
Rotation2d rotation{x.value(), y.value()};
@@ -70,17 +70,16 @@ ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
template <size_t NumModules>
ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
wpi::array<SwerveModuleState, NumModules> moduleStates) const {
Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix;
Matrixd<NumModules * 2, 1> moduleStateMatrix;
for (size_t i = 0; i < NumModules; ++i) {
SwerveModuleState module = moduleStates[i];
moduleStatesMatrix(i * 2, 0) = module.speed.value() * module.angle.Cos();
moduleStatesMatrix(i * 2 + 1, 0) =
module.speed.value() * module.angle.Sin();
moduleStateMatrix(i * 2, 0) = module.speed.value() * module.angle.Cos();
moduleStateMatrix(i * 2 + 1, 0) = module.speed.value() * module.angle.Sin();
}
Eigen::Vector3d chassisSpeedsVector =
m_forwardKinematics.solve(moduleStatesMatrix);
m_forwardKinematics.solve(moduleStateMatrix);
return {units::meters_per_second_t{chassisSpeedsVector(0)},
units::meters_per_second_t{chassisSpeedsVector(1)},