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

12
wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp
View File

@@ -24,21 +24,21 @@ DifferentialDriveAccelerationLimiter::Calculate(
units::meters_per_second_t leftVelocity,
units::meters_per_second_t rightVelocity, units::volt_t leftVoltage,
units::volt_t rightVoltage) {
Eigen::Vector<double, 2> u{leftVoltage.value(), rightVoltage.value()};
Vectord<2> u{leftVoltage.value(), rightVoltage.value()};
// Find unconstrained wheel accelerations
Eigen::Vector<double, 2> x{leftVelocity.value(), rightVelocity.value()};
Eigen::Vector<double, 2> dxdt = m_system.A() * x + m_system.B() * u;
Vectord<2> x{leftVelocity.value(), rightVelocity.value()};
Vectord<2> dxdt = m_system.A() * x + m_system.B() * u;
// Converts from wheel accelerations to linear and angular acceleration
// a = (dxdt(0) + dxdt(1)) / 2.0
// alpha = (dxdt(1) - dxdt(0)) / trackwidth
Eigen::Matrix<double, 2, 2> M{
{0.5, 0.5}, {-1.0 / m_trackwidth.value(), 1.0 / m_trackwidth.value()}};
Matrixd<2, 2> M{{0.5, 0.5},
{-1.0 / m_trackwidth.value(), 1.0 / m_trackwidth.value()}};
// Convert to linear and angular accelerations, constrain them, then convert
// back
Eigen::Vector<double, 2> accels = M * dxdt;
Vectord<2> accels = M * dxdt;
if (accels(0) > m_maxLinearAccel.value()) {
accels(0) = m_maxLinearAccel.value();
} else if (accels(0) < -m_maxLinearAccel.value()) {

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

@@ -7,60 +7,60 @@
namespace frc {
LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
const Matrixd<1, 1>& A, const Matrixd<1, 1>& B,
const wpi::array<double, 1>& Qelems, const wpi::array<double, 1>& Relems,
units::second_t dt)
: LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
MakeCostMatrix(Relems), dt) {}
LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
const Eigen::Matrix<double, 1, 1>& Q, const Eigen::Matrix<double, 1, 1>& R,
units::second_t dt)
LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(const Matrixd<1, 1>& A,
const Matrixd<1, 1>& B,
const Matrixd<1, 1>& Q,
const Matrixd<1, 1>& R,
units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, dt) {}
LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
const Eigen::Matrix<double, 1, 1>& Q, const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 1, 1>& N, units::second_t dt)
const Matrixd<1, 1>& A, const Matrixd<1, 1>& B, const Matrixd<1, 1>& Q,
const Matrixd<1, 1>& R, const Matrixd<1, 1>& N, units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, N, dt) {}
LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
const Matrixd<2, 2>& A, const Matrixd<2, 1>& B,
const wpi::array<double, 2>& Qelems, const wpi::array<double, 1>& Relems,
units::second_t dt)
: LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
MakeCostMatrix(Relems), dt) {}
LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R,
units::second_t dt)
LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(const Matrixd<2, 2>& A,
const Matrixd<2, 1>& B,
const Matrixd<2, 2>& Q,
const Matrixd<1, 1>& R,
units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, dt) {}
LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 2, 1>& N, units::second_t dt)
const Matrixd<2, 2>& A, const Matrixd<2, 1>& B, const Matrixd<2, 2>& Q,
const Matrixd<1, 1>& R, const Matrixd<2, 1>& N, units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, N, dt) {}
LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
const Matrixd<2, 2>& A, const Matrixd<2, 2>& B,
const wpi::array<double, 2>& Qelems, const wpi::array<double, 2>& Relems,
units::second_t dt)
: LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
MakeCostMatrix(Relems), dt) {}
LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R,
units::second_t dt)
LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(const Matrixd<2, 2>& A,
const Matrixd<2, 2>& B,
const Matrixd<2, 2>& Q,
const Matrixd<2, 2>& R,
units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<2, 2>(A, B, Q, R, dt) {}
LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R,
const Eigen::Matrix<double, 2, 2>& N, units::second_t dt)
const Matrixd<2, 2>& A, const Matrixd<2, 2>& B, const Matrixd<2, 2>& Q,
const Matrixd<2, 2>& R, const Matrixd<2, 2>& N, units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<2, 2>(A, B, Q, R, N, dt) {}
} // namespace frc