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] 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

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

@@ -7,7 +7,7 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/spline/Spline.h"
namespace frc {
@@ -40,19 +40,16 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> {
* Returns the coefficients matrix.
* @return The coefficients matrix.
*/
Eigen::Matrix<double, 6, 3 + 1> Coefficients() const override {
return m_coefficients;
}
Matrixd<6, 3 + 1> Coefficients() const override { return m_coefficients; }
private:
Eigen::Matrix<double, 6, 4> m_coefficients =
Eigen::Matrix<double, 6, 4>::Zero();
Matrixd<6, 4> m_coefficients = Matrixd<6, 4>::Zero();
/**
* Returns the hermite basis matrix for cubic hermite spline interpolation.
* @return The hermite basis matrix for cubic hermite spline interpolation.
*/
static Eigen::Matrix<double, 4, 4> MakeHermiteBasis() {
static Matrixd<4, 4> MakeHermiteBasis() {
// Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find
// the coefficients of the spline P(t) = a3 * t^3 + a2 * t^2 + a1 * t + a0.
//
@@ -74,10 +71,10 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> {
// [ a1 ] = [ 0 1 0 0 ][ P(i+1) ]
// [ a0 ] = [ 1 0 0 0 ][ P'(i+1) ]
static const Eigen::Matrix<double, 4, 4> basis{{+2.0, +1.0, -2.0, +1.0},
{-3.0, -2.0, +3.0, -1.0},
{+0.0, +1.0, +0.0, +0.0},
{+1.0, +0.0, +0.0, +0.0}};
static const Matrixd<4, 4> basis{{+2.0, +1.0, -2.0, +1.0},
{-3.0, -2.0, +3.0, -1.0},
{+0.0, +1.0, +0.0, +0.0},
{+1.0, +0.0, +0.0, +0.0}};
return basis;
}

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

@@ -7,7 +7,7 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/spline/Spline.h"
namespace frc {
@@ -40,19 +40,16 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> {
* Returns the coefficients matrix.
* @return The coefficients matrix.
*/
Eigen::Matrix<double, 6, 6> Coefficients() const override {
return m_coefficients;
}
Matrixd<6, 6> Coefficients() const override { return m_coefficients; }
private:
Eigen::Matrix<double, 6, 6> m_coefficients =
Eigen::Matrix<double, 6, 6>::Zero();
Matrixd<6, 6> m_coefficients = Matrixd<6, 6>::Zero();
/**
* Returns the hermite basis matrix for quintic hermite spline interpolation.
* @return The hermite basis matrix for quintic hermite spline interpolation.
*/
static Eigen::Matrix<double, 6, 6> MakeHermiteBasis() {
static Matrixd<6, 6> MakeHermiteBasis() {
// Given P(i), P'(i), P''(i), P(i+1), P'(i+1), P''(i+1), the control
// vectors, we want to find the coefficients of the spline
// P(t) = a5 * t^5 + a4 * t^4 + a3 * t^3 + a2 * t^2 + a1 * t + a0.
@@ -81,7 +78,7 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> {
// [ a1 ] = [ 0.0 1.0 0.0 0.0 0.0 0.0 ][ P'(i+1) ]
// [ a0 ] = [ 1.0 0.0 0.0 0.0 0.0 0.0 ][ P''(i+1) ]
static const Eigen::Matrix<double, 6, 6> basis{
static const Matrixd<6, 6> basis{
{-06.0, -03.0, -00.5, +06.0, -03.0, +00.5},
{+15.0, +08.0, +01.5, -15.0, +07.0, -01.0},
{-10.0, -06.0, -01.5, +10.0, -04.0, +00.5},
@@ -100,11 +97,10 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> {
*
* @return The control vector matrix for a dimension.
*/
static Eigen::Vector<double, 6> ControlVectorFromArrays(
wpi::array<double, 3> initialVector, wpi::array<double, 3> finalVector) {
return Eigen::Vector<double, 6>{initialVector[0], initialVector[1],
initialVector[2], finalVector[0],
finalVector[1], finalVector[2]};
static Vectord<6> ControlVectorFromArrays(wpi::array<double, 3> initialVector,
wpi::array<double, 3> finalVector) {
return Vectord<6>{initialVector[0], initialVector[1], initialVector[2],
finalVector[0], finalVector[1], finalVector[2]};
}
};
} // namespace frc

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

@@ -9,7 +9,7 @@
#include <wpi/array.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/geometry/Pose2d.h"
#include "units/curvature.h"
#include "units/length.h"
@@ -55,7 +55,7 @@ class Spline {
* @return The pose and curvature at that point.
*/
PoseWithCurvature GetPoint(double t) const {
Eigen::Vector<double, Degree + 1> polynomialBases;
Vectord<Degree + 1> polynomialBases;
// Populate the polynomial bases
for (int i = 0; i <= Degree; i++) {
@@ -64,7 +64,7 @@ class Spline {
// This simply multiplies by the coefficients. We need to divide out t some
// n number of times where n is the derivative we want to take.
Eigen::Vector<double, 6> combined = Coefficients() * polynomialBases;
Vectord<6> combined = Coefficients() * polynomialBases;
double dx, dy, ddx, ddy;
@@ -100,7 +100,7 @@ class Spline {
*
* @return The coefficients of the spline.
*/
virtual Eigen::Matrix<double, 6, Degree + 1> Coefficients() const = 0;
virtual Matrixd<6, Degree + 1> Coefficients() const = 0;
/**
* Converts a Translation2d into a vector that is compatible with Eigen.