From e767605e94cc76abbdb3adafa2c478f33f3b6f81 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Fri, 29 Apr 2022 22:29:20 -0700 Subject: [PATCH] [wpimath] Add typedefs for common types This makes complex code significantly easier to read. frc::Vectord = Eigen::Vector frc::Matrixd = Eigen::Matrix --- .../main/native/cpp/simulation/DCMotorSim.cpp | 2 +- .../simulation/DifferentialDrivetrainSim.cpp | 20 +-- .../native/cpp/simulation/ElevatorSim.cpp | 18 +- .../native/cpp/simulation/FlywheelSim.cpp | 2 +- .../cpp/simulation/SingleJointedArmSim.cpp | 25 ++- .../simulation/DifferentialDrivetrainSim.h | 19 +- .../include/frc/simulation/ElevatorSim.h | 5 +- .../include/frc/simulation/LinearSystemSim.h | 28 +-- .../frc/simulation/SingleJointedArmSim.h | 5 +- .../DifferentialDrivetrainSimTest.cpp | 8 +- .../native/cpp/simulation/ElevatorSimTest.cpp | 19 +- .../simulation/SingleJointedArmSimTest.cpp | 4 +- .../cpp/simulation/StateSpaceSimTest.cpp | 4 +- .../cpp/examples/ArmSimulation/cpp/Robot.cpp | 4 +- .../examples/ElevatorSimulation/cpp/Robot.cpp | 2 +- .../cpp/examples/StateSpaceArm/cpp/Robot.cpp | 10 +- .../examples/StateSpaceElevator/cpp/Robot.cpp | 10 +- .../examples/StateSpaceFlywheel/cpp/Robot.cpp | 8 +- .../StateSpaceFlywheelSysId/cpp/Robot.cpp | 8 +- .../src/main/native/cpp/StateSpaceUtil.cpp | 28 ++- .../DifferentialDriveAccelerationLimiter.cpp | 12 +- .../controller/LinearQuadraticRegulator.cpp | 48 ++--- .../DifferentialDrivePoseEstimator.cpp | 40 ++--- .../estimator/MecanumDrivePoseEstimator.cpp | 32 ++-- .../cpp/kinematics/MecanumDriveKinematics.cpp | 14 +- .../src/main/native/include/frc/EigenCore.h | 23 +++ .../main/native/include/frc/StateSpaceUtil.h | 67 ++++--- .../ControlAffinePlantInversionFeedforward.h | 58 +++--- .../frc/controller/ImplicitModelFollower.h | 25 +-- .../LinearPlantInversionFeedforward.h | 34 ++-- .../frc/controller/LinearQuadraticRegulator.h | 151 +++++++--------- .../frc/controller/SimpleMotorFeedforward.h | 6 +- .../include/frc/estimator/AngleStatistics.h | 35 ++-- .../DifferentialDrivePoseEstimator.h | 19 +- .../frc/estimator/ExtendedKalmanFilter.h | 131 ++++++-------- .../frc/estimator/ExtendedKalmanFilter.inc | 138 +++++---------- .../include/frc/estimator/KalmanFilter.h | 29 +-- .../include/frc/estimator/KalmanFilter.inc | 24 ++- .../KalmanFilterLatencyCompensator.h | 27 ++- .../frc/estimator/MecanumDrivePoseEstimator.h | 8 +- .../frc/estimator/MerweScaledSigmaPoints.h | 24 ++- .../frc/estimator/SwerveDrivePoseEstimator.h | 29 ++- .../frc/estimator/UnscentedKalmanFilter.h | 165 +++++++----------- .../frc/estimator/UnscentedKalmanFilter.inc | 142 +++++---------- .../frc/estimator/UnscentedTransform.h | 29 ++- .../native/include/frc/filter/LinearFilter.h | 8 +- .../frc/kinematics/MecanumDriveKinematics.h | 6 +- .../frc/kinematics/SwerveDriveKinematics.h | 7 +- .../frc/kinematics/SwerveDriveKinematics.inc | 17 +- .../include/frc/spline/CubicHermiteSpline.h | 19 +- .../include/frc/spline/QuinticHermiteSpline.h | 22 +-- .../main/native/include/frc/spline/Spline.h | 8 +- .../include/frc/system/Discretization.h | 62 +++---- .../native/include/frc/system/LinearSystem.h | 44 ++--- .../include/frc/system/LinearSystemLoop.h | 62 +++---- .../include/frc/system/NumericalJacobian.h | 26 ++- .../include/frc/system/plant/LinearSystemId.h | 88 +++++----- wpimath/src/test/native/cpp/EigenTest.cpp | 21 ++- .../src/test/native/cpp/StateSpaceTest.cpp | 7 +- .../test/native/cpp/StateSpaceUtilTest.cpp | 60 +++---- ...rolAffinePlantInversionFeedforwardTest.cpp | 31 ++-- ...fferentialDriveAccelerationLimiterTest.cpp | 69 ++++---- .../controller/ImplicitModelFollowerTest.cpp | 20 +-- .../LinearPlantInversionFeedforwardTest.cpp | 10 +- .../LinearQuadraticRegulatorTest.cpp | 64 +++---- .../controller/SimpleMotorFeedforwardTest.cpp | 10 +- .../cpp/estimator/AngleStatisticsTest.cpp | 4 +- .../estimator/ExtendedKalmanFilterTest.cpp | 42 ++--- .../estimator/MerweScaledSigmaPointsTest.cpp | 12 +- .../estimator/UnscentedKalmanFilterTest.cpp | 42 ++--- .../native/cpp/system/DiscretizationTest.cpp | 141 ++++++++------- .../native/cpp/system/LinearSystemIDTest.cpp | 53 +++--- .../cpp/system/NumericalIntegrationTest.cpp | 38 ++-- .../cpp/system/NumericalJacobianTest.cpp | 35 ++-- .../cpp/system/RungeKuttaTimeVaryingTest.cpp | 16 +- .../frc/system/RungeKuttaTimeVarying.h | 2 +- 76 files changed, 1136 insertions(+), 1449 deletions(-) create mode 100644 wpimath/src/main/native/include/frc/EigenCore.h diff --git a/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp index 7860e32098..bda202095c 100644 --- a/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp @@ -42,5 +42,5 @@ units::ampere_t DCMotorSim::GetCurrentDraw() const { } void DCMotorSim::SetInputVoltage(units::volt_t voltage) { - SetInput(Eigen::Vector{voltage.value()}); + SetInput(Vectord<1>{voltage.value()}); } diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp index 78b9dc79b6..407746c0ae 100644 --- a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp @@ -41,8 +41,7 @@ DifferentialDrivetrainSim::DifferentialDrivetrainSim( driveMotor, mass, wheelRadius, trackWidth / 2.0, J, gearing), trackWidth, driveMotor, gearing, wheelRadius, measurementStdDevs) {} -Eigen::Vector DifferentialDrivetrainSim::ClampInput( - const Eigen::Vector& u) { +Vectord<2> DifferentialDrivetrainSim::ClampInput(const Vectord<2>& u) { return frc::DesaturateInputVector<2>(u, frc::RobotController::GetInputVoltage()); } @@ -66,11 +65,11 @@ double DifferentialDrivetrainSim::GetGearing() const { return m_currentGearing; } -Eigen::Vector DifferentialDrivetrainSim::GetOutput() const { +Vectord<7> DifferentialDrivetrainSim::GetOutput() const { return m_y; } -Eigen::Vector DifferentialDrivetrainSim::GetState() const { +Vectord<7> DifferentialDrivetrainSim::GetState() const { return m_x; } @@ -116,8 +115,7 @@ units::ampere_t DifferentialDrivetrainSim::GetCurrentDraw() const { return GetLeftCurrentDraw() + GetRightCurrentDraw(); } -void DifferentialDrivetrainSim::SetState( - const Eigen::Vector& state) { +void DifferentialDrivetrainSim::SetState(const Vectord<7>& state) { m_x = state; } @@ -129,19 +127,19 @@ void DifferentialDrivetrainSim::SetPose(const frc::Pose2d& pose) { m_x(State::kRightPosition) = 0; } -Eigen::Vector DifferentialDrivetrainSim::Dynamics( - const Eigen::Vector& x, const Eigen::Vector& u) { +Vectord<7> DifferentialDrivetrainSim::Dynamics(const Vectord<7>& x, + const Vectord<2>& u) { // Because G^2 can be factored out of A, we can divide by the old ratio // squared and multiply by the new ratio squared to get a new drivetrain // model. - Eigen::Matrix B; + Matrixd<4, 2> B; B.block<2, 2>(0, 0) = m_plant.B() * m_currentGearing * m_currentGearing / m_originalGearing / m_originalGearing; B.block<2, 2>(2, 0).setZero(); // Because G can be factored out of B, we can divide by the old ratio and // multiply by the new ratio to get a new drivetrain model. - Eigen::Matrix A; + Matrixd<4, 4> A; A.block<2, 2>(0, 0) = m_plant.A() * m_currentGearing / m_originalGearing; A.block<2, 2>(2, 0).setIdentity(); @@ -149,7 +147,7 @@ Eigen::Vector DifferentialDrivetrainSim::Dynamics( double v = (x(State::kLeftVelocity) + x(State::kRightVelocity)) / 2.0; - Eigen::Vector xdot; + Vectord<7> xdot; xdot(0) = v * std::cos(x(State::kHeading)); xdot(1) = v * std::sin(x(State::kHeading)); xdot(2) = diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index 64c7aceaea..7adff3a6ec 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -80,29 +80,27 @@ units::ampere_t ElevatorSim::GetCurrentDraw() const { } void ElevatorSim::SetInputVoltage(units::volt_t voltage) { - SetInput(Eigen::Vector{voltage.value()}); + SetInput(Vectord<1>{voltage.value()}); } -Eigen::Vector ElevatorSim::UpdateX( - const Eigen::Vector& currentXhat, - const Eigen::Vector& u, units::second_t dt) { +Vectord<2> ElevatorSim::UpdateX(const Vectord<2>& currentXhat, + const Vectord<1>& u, units::second_t dt) { auto updatedXhat = RKDP( - [&](const Eigen::Vector& x, - const Eigen::Vector& u_) -> Eigen::Vector { - Eigen::Vector xdot = m_plant.A() * x + m_plant.B() * u; + [&](const Vectord<2>& x, const Vectord<1>& u_) -> Vectord<2> { + Vectord<2> xdot = m_plant.A() * x + m_plant.B() * u; if (m_simulateGravity) { - xdot += Eigen::Vector{0.0, -9.8}; + xdot += Vectord<2>{0.0, -9.8}; } return xdot; }, currentXhat, u, dt); // Check for collision after updating x-hat. if (WouldHitLowerLimit(units::meter_t(updatedXhat(0)))) { - return Eigen::Vector{m_minHeight.value(), 0.0}; + return Vectord<2>{m_minHeight.value(), 0.0}; } if (WouldHitUpperLimit(units::meter_t(updatedXhat(0)))) { - return Eigen::Vector{m_maxHeight.value(), 0.0}; + return Vectord<2>{m_maxHeight.value(), 0.0}; } return updatedXhat; } diff --git a/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp index f4371b12bb..95dcb9e5ed 100644 --- a/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp @@ -38,5 +38,5 @@ units::ampere_t FlywheelSim::GetCurrentDraw() const { } void FlywheelSim::SetInputVoltage(units::volt_t voltage) { - SetInput(Eigen::Vector{voltage.value()}); + SetInput(Vectord<1>{voltage.value()}); } diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp index a6fad8f975..e4fa2a6e01 100644 --- a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp @@ -72,12 +72,12 @@ units::ampere_t SingleJointedArmSim::GetCurrentDraw() const { } void SingleJointedArmSim::SetInputVoltage(units::volt_t voltage) { - SetInput(Eigen::Vector{voltage.value()}); + SetInput(Vectord<1>{voltage.value()}); } -Eigen::Vector SingleJointedArmSim::UpdateX( - const Eigen::Vector& currentXhat, - const Eigen::Vector& u, units::second_t dt) { +Vectord<2> SingleJointedArmSim::UpdateX(const Vectord<2>& currentXhat, + const Vectord<1>& u, + units::second_t dt) { // Horizontal case: // Torque = F * r = I * alpha // alpha = F * r / I @@ -88,15 +88,14 @@ Eigen::Vector SingleJointedArmSim::UpdateX( // We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I * // std::cos(theta)]] - Eigen::Vector updatedXhat = RKDP( - [&](const auto& x, const auto& u) -> Eigen::Vector { - Eigen::Vector xdot = m_plant.A() * x + m_plant.B() * u; + Vectord<2> updatedXhat = RKDP( + [&](const auto& x, const auto& u) -> Vectord<2> { + Vectord<2> xdot = m_plant.A() * x + m_plant.B() * u; if (m_simulateGravity) { - xdot += Eigen::Vector{ - 0.0, (m_armMass * m_r * -9.8 * 3.0 / (m_armMass * m_r * m_r) * - std::cos(x(0))) - .value()}; + xdot += Vectord<2>{0.0, (m_armMass * m_r * -9.8 * 3.0 / + (m_armMass * m_r * m_r) * std::cos(x(0))) + .value()}; } return xdot; }, @@ -104,9 +103,9 @@ Eigen::Vector SingleJointedArmSim::UpdateX( // Check for collisions. if (WouldHitLowerLimit(units::radian_t(updatedXhat(0)))) { - return Eigen::Vector{m_minAngle.value(), 0.0}; + return Vectord<2>{m_minAngle.value(), 0.0}; } else if (WouldHitUpperLimit(units::radian_t(updatedXhat(0)))) { - return Eigen::Vector{m_maxAngle.value(), 0.0}; + return Vectord<2>{m_maxAngle.value(), 0.0}; } return updatedXhat; } diff --git a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h index 7c53eb7f01..420ff4eea1 100644 --- a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h @@ -4,11 +4,11 @@ #pragma once +#include #include #include #include -#include #include #include #include @@ -80,7 +80,7 @@ class DifferentialDrivetrainSim { * @param u The input vector. * @return The normalized input. */ - Eigen::Vector ClampInput(const Eigen::Vector& u); + Vectord<2> ClampInput(const Vectord<2>& u); /** * Sets the applied voltage to the drivetrain. Note that positive voltage must @@ -178,7 +178,7 @@ class DifferentialDrivetrainSim { * * @param state The state. */ - void SetState(const Eigen::Vector& state); + void SetState(const Vectord<7>& state); /** * Sets the system pose. @@ -187,8 +187,7 @@ class DifferentialDrivetrainSim { */ void SetPose(const frc::Pose2d& pose); - Eigen::Vector Dynamics(const Eigen::Vector& x, - const Eigen::Vector& u); + Vectord<7> Dynamics(const Vectord<7>& x, const Vectord<2>& u); class State { public: @@ -301,7 +300,7 @@ class DifferentialDrivetrainSim { /** * Returns the current output vector y. */ - Eigen::Vector GetOutput() const; + Vectord<7> GetOutput() const; /** * Returns an element of the state vector. Note that this will not include @@ -314,7 +313,7 @@ class DifferentialDrivetrainSim { /** * Returns the current state vector x. Note that this will not include noise! */ - Eigen::Vector GetState() const; + Vectord<7> GetState() const; LinearSystem<2, 2, 2> m_plant; units::meter_t m_rb; @@ -325,9 +324,9 @@ class DifferentialDrivetrainSim { double m_originalGearing; double m_currentGearing; - Eigen::Vector m_x; - Eigen::Vector m_u; - Eigen::Vector m_y; + Vectord<7> m_x; + Vectord<2> m_u; + Vectord<7> m_y; std::array m_measurementStdDevs; }; } // namespace frc::sim diff --git a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h index 0965fdd4fb..cf408107c3 100644 --- a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h @@ -127,9 +127,8 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { * @param u The system inputs (voltage). * @param dt The time difference between controller updates. */ - Eigen::Vector UpdateX(const Eigen::Vector& currentXhat, - const Eigen::Vector& u, - units::second_t dt) override; + Vectord<2> UpdateX(const Vectord<2>& currentXhat, const Vectord<1>& u, + units::second_t dt) override; private: DCMotor m_gearbox; diff --git a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h index 731fad36b1..b0145a2c7a 100644 --- a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h @@ -6,10 +6,10 @@ #include -#include #include #include +#include "frc/EigenCore.h" #include "frc/RobotController.h" #include "frc/StateSpaceUtil.h" #include "frc/system/LinearSystem.h" @@ -40,9 +40,9 @@ class LinearSystemSim { const LinearSystem& system, const std::array& measurementStdDevs = {}) : m_plant(system), m_measurementStdDevs(measurementStdDevs) { - m_x = Eigen::Vector::Zero(); - m_y = Eigen::Vector::Zero(); - m_u = Eigen::Vector::Zero(); + m_x = Vectord::Zero(); + m_y = Vectord::Zero(); + m_u = Vectord::Zero(); } virtual ~LinearSystemSim() = default; @@ -71,7 +71,7 @@ class LinearSystemSim { * * @return The current output of the plant. */ - const Eigen::Vector& GetOutput() const { return m_y; } + const Vectord& GetOutput() const { return m_y; } /** * Returns an element of the current output of the plant. @@ -86,7 +86,7 @@ class LinearSystemSim { * * @param u The system inputs. */ - void SetInput(const Eigen::Vector& u) { m_u = ClampInput(u); } + void SetInput(const Vectord& u) { m_u = ClampInput(u); } /* * Sets the system inputs. @@ -104,7 +104,7 @@ class LinearSystemSim { * * @param state The new state. */ - void SetState(const Eigen::Vector& state) { m_x = state; } + void SetState(const Vectord& state) { m_x = state; } /** * Returns the current drawn by this simulated system. Override this method to @@ -124,9 +124,9 @@ class LinearSystemSim { * @param u The system inputs (usually voltage). * @param dt The time difference between controller updates. */ - virtual Eigen::Vector UpdateX( - const Eigen::Vector& currentXhat, - const Eigen::Vector& u, units::second_t dt) { + virtual Vectord UpdateX(const Vectord& currentXhat, + const Vectord& u, + units::second_t dt) { return m_plant.CalculateX(currentXhat, u, dt); } @@ -137,16 +137,16 @@ class LinearSystemSim { * @param u The input vector. * @return The normalized input. */ - Eigen::Vector ClampInput(Eigen::Vector u) { + Vectord ClampInput(Vectord u) { return frc::DesaturateInputVector( u, frc::RobotController::GetInputVoltage()); } LinearSystem m_plant; - Eigen::Vector m_x; - Eigen::Vector m_y; - Eigen::Vector m_u; + Vectord m_x; + Vectord m_y; + Vectord m_u; std::array m_measurementStdDevs; }; } // namespace frc::sim diff --git a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h index 6c5a34ba87..57ad6b1a36 100644 --- a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h @@ -142,9 +142,8 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> { * @param u The system inputs (voltage). * @param dt The time difference between controller updates. */ - Eigen::Vector UpdateX(const Eigen::Vector& currentXhat, - const Eigen::Vector& u, - units::second_t dt) override; + Vectord<2> UpdateX(const Vectord<2>& currentXhat, const Vectord<1>& u, + units::second_t dt) override; private: units::meter_t m_r; diff --git a/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp index 8ccfcd60b2..778f99d762 100644 --- a/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp @@ -30,10 +30,10 @@ TEST(DifferentialDrivetrainSimTest, Convergence) { frc::LinearPlantInversionFeedforward feedforward{plant, 20_ms}; frc::RamseteController ramsete; - feedforward.Reset(Eigen::Vector{0.0, 0.0}); + feedforward.Reset(frc::Vectord<2>{0.0, 0.0}); // Ground truth. - Eigen::Vector groundTruthX = Eigen::Vector::Zero(); + frc::Vectord<7> groundTruthX = frc::Vectord<7>::Zero(); frc::TrajectoryConfig config{1_mps, 1_mps_sq}; config.AddConstraint( @@ -48,7 +48,7 @@ TEST(DifferentialDrivetrainSimTest, Convergence) { auto [l, r] = kinematics.ToWheelSpeeds(ramseteOut); auto voltages = - feedforward.Calculate(Eigen::Vector{l.value(), r.value()}); + feedforward.Calculate(frc::Vectord<2>{l.value(), r.value()}); // Sim periodic code. sim.SetInputs(units::volt_t(voltages(0, 0)), units::volt_t(voltages(1, 0))); @@ -56,7 +56,7 @@ TEST(DifferentialDrivetrainSimTest, Convergence) { // Update ground truth. groundTruthX = frc::RK4( - [&sim](const auto& x, const auto& u) -> Eigen::Vector { + [&sim](const auto& x, const auto& u) -> frc::Vectord<7> { return sim.Dynamics(x, u); }, groundTruthX, voltages, 20_ms); diff --git a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp index 9d623c0464..b43b6fd52b 100644 --- a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp @@ -34,8 +34,7 @@ TEST(ElevatorSimTest, StateSpaceSim) { auto nextVoltage = controller.Calculate(encoderSim.GetDistance()); motor.Set(nextVoltage / frc::RobotController::GetInputVoltage()); - Eigen::Vector u{motor.Get() * - frc::RobotController::GetInputVoltage()}; + frc::Vectord<1> u{motor.Get() * frc::RobotController::GetInputVoltage()}; sim.SetInput(u); sim.Update(20_ms); @@ -51,7 +50,7 @@ TEST(ElevatorSimTest, MinMax) { units::meter_t(0.75 * 25.4 / 1000.0), 0_m, 1_m, true, {0.01}); for (size_t i = 0; i < 100; ++i) { - sim.SetInput(Eigen::Vector{0.0}); + sim.SetInput(frc::Vectord<1>{0.0}); sim.Update(20_ms); auto height = sim.GetPosition(); @@ -59,7 +58,7 @@ TEST(ElevatorSimTest, MinMax) { } for (size_t i = 0; i < 100; ++i) { - sim.SetInput(Eigen::Vector{12.0}); + sim.SetInput(frc::Vectord<1>{12.0}); sim.Update(20_ms); auto height = sim.GetPosition(); @@ -71,16 +70,16 @@ TEST(ElevatorSimTest, Stability) { frc::sim::ElevatorSim sim{ frc::DCMotor::Vex775Pro(4), 100, 4_kg, 0.5_in, 0_m, 10_m, true}; - sim.SetState(Eigen::Vector{0.0, 0.0}); - sim.SetInput(Eigen::Vector{12.0}); + sim.SetState(frc::Vectord<2>{0.0, 0.0}); + sim.SetInput(frc::Vectord<1>{12.0}); for (int i = 0; i < 50; ++i) { sim.Update(20_ms); } frc::LinearSystem<2, 1, 1> system = frc::LinearSystemId::ElevatorSystem( frc::DCMotor::Vex775Pro(4), 4_kg, 0.5_in, 100); - EXPECT_NEAR_UNITS(units::meter_t{system.CalculateX( - Eigen::Vector{0.0, 0.0}, - Eigen::Vector{12.0}, 20_ms * 50)(0)}, - sim.GetPosition(), 1_cm); + EXPECT_NEAR_UNITS( + units::meter_t{system.CalculateX(frc::Vectord<2>{0.0, 0.0}, + frc::Vectord<1>{12.0}, 20_ms * 50)(0)}, + sim.GetPosition(), 1_cm); } diff --git a/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp index 03df12b8fa..dbcffb07c4 100644 --- a/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp @@ -10,10 +10,10 @@ TEST(SingleJointedArmTest, Disabled) { frc::sim::SingleJointedArmSim sim(frc::DCMotor::Vex775Pro(2), 100, 3_kg_sq_m, 9.5_in, -180_deg, 0_deg, 10_lb, true); - sim.SetState(Eigen::Vector{0.0, 0.0}); + sim.SetState(frc::Vectord<2>{0.0, 0.0}); for (size_t i = 0; i < 12 / 0.02; ++i) { - sim.SetInput(Eigen::Vector{0.0}); + sim.SetInput(frc::Vectord<1>{0.0}); sim.Update(20_ms); } diff --git a/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp index 23af83df09..b54d8b7aab 100644 --- a/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp @@ -46,8 +46,8 @@ TEST(StateSpaceSimTest, FlywheelSim) { // Then, SimulationPeriodic runs frc::sim::RoboRioSim::SetVInVoltage( frc::sim::BatterySim::Calculate({sim.GetCurrentDraw()})); - sim.SetInput(Eigen::Vector{ - motor.Get() * frc::RobotController::GetInputVoltage()}); + sim.SetInput( + frc::Vectord<1>{motor.Get() * frc::RobotController::GetInputVoltage()}); sim.Update(20_ms); encoderSim.SetRate(sim.GetAngularVelocity().value()); } diff --git a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp index 330ddc44fa..2b52dddaf7 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp @@ -101,8 +101,8 @@ class Robot : public frc::TimedRobot { void SimulationPeriodic() override { // In this method, we update our simulation of what our arm is doing // First, we set our "inputs" (voltages) - m_armSim.SetInput(Eigen::Vector{ - m_motor.Get() * frc::RobotController::GetInputVoltage()}); + m_armSim.SetInput(frc::Vectord<1>{m_motor.Get() * + frc::RobotController::GetInputVoltage()}); // Next, we update it. The standard loop time is 20ms. m_armSim.Update(20_ms); diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp index 34609eddfa..2f7382a4d1 100644 --- a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp @@ -88,7 +88,7 @@ class Robot : public frc::TimedRobot { void SimulationPeriodic() override { // In this method, we update our simulation of what our elevator is doing // First, we set our "inputs" (voltages) - m_elevatorSim.SetInput(Eigen::Vector{ + m_elevatorSim.SetInput(frc::Vectord<1>{ m_motor.Get() * frc::RobotController::GetInputVoltage()}); // Next, we update it. The standard loop time is 20ms. diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp index 89e98984b5..c29f3353d8 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp @@ -97,8 +97,7 @@ class Robot : public frc::TimedRobot { } void TeleopInit() override { - m_loop.Reset( - Eigen::Vector{m_encoder.GetDistance(), m_encoder.GetRate()}); + m_loop.Reset(frc::Vectord<2>{m_encoder.GetDistance(), m_encoder.GetRate()}); m_lastProfiledReference = { units::radian_t(m_encoder.GetDistance()), @@ -121,12 +120,11 @@ class Robot : public frc::TimedRobot { m_lastProfiledReference)) .Calculate(20_ms); - m_loop.SetNextR( - Eigen::Vector{m_lastProfiledReference.position.value(), - m_lastProfiledReference.velocity.value()}); + m_loop.SetNextR(frc::Vectord<2>{m_lastProfiledReference.position.value(), + m_lastProfiledReference.velocity.value()}); // Correct our Kalman filter's state vector estimate with encoder data. - m_loop.Correct(Eigen::Vector{m_encoder.GetDistance()}); + m_loop.Correct(frc::Vectord<1>{m_encoder.GetDistance()}); // Update our LQR to generate new voltage commands and use the voltages to // predict the next state with out Kalman filter. diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp index 4c9d65e2f7..a4e74506ef 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp @@ -96,8 +96,7 @@ class Robot : public frc::TimedRobot { void TeleopInit() override { // Reset our loop to make sure it's in a known state. - m_loop.Reset( - Eigen::Vector{m_encoder.GetDistance(), m_encoder.GetRate()}); + m_loop.Reset(frc::Vectord<2>{m_encoder.GetDistance(), m_encoder.GetRate()}); m_lastProfiledReference = {units::meter_t(m_encoder.GetDistance()), units::meters_per_second_t(m_encoder.GetRate())}; @@ -119,12 +118,11 @@ class Robot : public frc::TimedRobot { m_lastProfiledReference)) .Calculate(20_ms); - m_loop.SetNextR( - Eigen::Vector{m_lastProfiledReference.position.value(), - m_lastProfiledReference.velocity.value()}); + m_loop.SetNextR(frc::Vectord<2>{m_lastProfiledReference.position.value(), + m_lastProfiledReference.velocity.value()}); // Correct our Kalman filter's state vector estimate with encoder data. - m_loop.Correct(Eigen::Vector{m_encoder.GetDistance()}); + m_loop.Correct(frc::Vectord<1>{m_encoder.GetDistance()}); // Update our LQR to generate new voltage commands and use the voltages to // predict the next state with out Kalman filter. diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp index 636916f41f..c09173997e 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp @@ -85,7 +85,7 @@ class Robot : public frc::TimedRobot { } void TeleopInit() override { - m_loop.Reset(Eigen::Vector{m_encoder.GetRate()}); + m_loop.Reset(frc::Vectord<1>{m_encoder.GetRate()}); } void TeleopPeriodic() override { @@ -93,14 +93,14 @@ class Robot : public frc::TimedRobot { // setpoint of a PID controller. if (m_joystick.GetRightBumper()) { // We pressed the bumper, so let's set our next reference - m_loop.SetNextR(Eigen::Vector{kSpinup.value()}); + m_loop.SetNextR(frc::Vectord<1>{kSpinup.value()}); } else { // We released the bumper, so let's spin down - m_loop.SetNextR(Eigen::Vector{0.0}); + m_loop.SetNextR(frc::Vectord<1>{0.0}); } // Correct our Kalman filter's state vector estimate with encoder data. - m_loop.Correct(Eigen::Vector{m_encoder.GetRate()}); + m_loop.Correct(frc::Vectord<1>{m_encoder.GetRate()}); // Update our LQR to generate new voltage commands and use the voltages to // predict the next state with out Kalman filter. diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp index ed588249ac..738c64bfeb 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp @@ -85,7 +85,7 @@ class Robot : public frc::TimedRobot { } void TeleopInit() override { - m_loop.Reset(Eigen::Vector{m_encoder.GetRate()}); + m_loop.Reset(frc::Vectord<1>{m_encoder.GetRate()}); } void TeleopPeriodic() override { @@ -93,14 +93,14 @@ class Robot : public frc::TimedRobot { // setpoint of a PID controller. if (m_joystick.GetRightBumper()) { // We pressed the bumper, so let's set our next reference - m_loop.SetNextR(Eigen::Vector{kSpinup.value()}); + m_loop.SetNextR(frc::Vectord<1>{kSpinup.value()}); } else { // We released the bumper, so let's spin down - m_loop.SetNextR(Eigen::Vector{0.0}); + m_loop.SetNextR(frc::Vectord<1>{0.0}); } // Correct our Kalman filter's state vector estimate with encoder data. - m_loop.Correct(Eigen::Vector{m_encoder.GetRate()}); + m_loop.Correct(frc::Vectord<1>{m_encoder.GetRate()}); // Update our LQR to generate new voltage commands and use the voltages to // predict the next state with out Kalman filter. diff --git a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp index 8f1145d96d..fc532dbd12 100644 --- a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp +++ b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp @@ -6,33 +6,31 @@ namespace frc { -Eigen::Vector PoseTo3dVector(const Pose2d& pose) { - return Eigen::Vector{pose.Translation().X().value(), - pose.Translation().Y().value(), - pose.Rotation().Radians().value()}; +Vectord<3> PoseTo3dVector(const Pose2d& pose) { + return Vectord<3>{pose.Translation().X().value(), + pose.Translation().Y().value(), + pose.Rotation().Radians().value()}; } -Eigen::Vector PoseTo4dVector(const Pose2d& pose) { - return Eigen::Vector{pose.Translation().X().value(), - pose.Translation().Y().value(), - pose.Rotation().Cos(), pose.Rotation().Sin()}; +Vectord<4> PoseTo4dVector(const Pose2d& pose) { + return Vectord<4>{pose.Translation().X().value(), + pose.Translation().Y().value(), pose.Rotation().Cos(), + pose.Rotation().Sin()}; } template <> -bool IsStabilizable<1, 1>(const Eigen::Matrix& A, - const Eigen::Matrix& B) { +bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B) { return detail::IsStabilizableImpl<1, 1>(A, B); } template <> -bool IsStabilizable<2, 1>(const Eigen::Matrix& A, - const Eigen::Matrix& B) { +bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B) { return detail::IsStabilizableImpl<2, 1>(A, B); } -Eigen::Vector PoseToVector(const Pose2d& pose) { - return Eigen::Vector{pose.X().value(), pose.Y().value(), - pose.Rotation().Radians().value()}; +Vectord<3> PoseToVector(const Pose2d& pose) { + return Vectord<3>{pose.X().value(), pose.Y().value(), + pose.Rotation().Radians().value()}; } } // namespace frc diff --git a/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp index 1d03ffd47a..5727d87f90 100644 --- a/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp +++ b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp @@ -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 u{leftVoltage.value(), rightVoltage.value()}; + Vectord<2> u{leftVoltage.value(), rightVoltage.value()}; // Find unconstrained wheel accelerations - Eigen::Vector x{leftVelocity.value(), rightVelocity.value()}; - Eigen::Vector 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 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 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()) { diff --git a/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp b/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp index 4d2fbe92fb..bb062846af 100644 --- a/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp +++ b/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp @@ -7,60 +7,60 @@ namespace frc { LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator( - const Eigen::Matrix& A, const Eigen::Matrix& B, + const Matrixd<1, 1>& A, const Matrixd<1, 1>& B, const wpi::array& Qelems, const wpi::array& Relems, units::second_t dt) : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems), MakeCostMatrix(Relems), dt) {} -LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator( - const Eigen::Matrix& A, const Eigen::Matrix& B, - const Eigen::Matrix& Q, const Eigen::Matrix& 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& A, const Eigen::Matrix& B, - const Eigen::Matrix& Q, const Eigen::Matrix& R, - const Eigen::Matrix& 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& A, const Eigen::Matrix& B, + const Matrixd<2, 2>& A, const Matrixd<2, 1>& B, const wpi::array& Qelems, const wpi::array& Relems, units::second_t dt) : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems), MakeCostMatrix(Relems), dt) {} -LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator( - const Eigen::Matrix& A, const Eigen::Matrix& B, - const Eigen::Matrix& Q, const Eigen::Matrix& 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& A, const Eigen::Matrix& B, - const Eigen::Matrix& Q, const Eigen::Matrix& R, - const Eigen::Matrix& 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& A, const Eigen::Matrix& B, + const Matrixd<2, 2>& A, const Matrixd<2, 2>& B, const wpi::array& Qelems, const wpi::array& Relems, units::second_t dt) : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems), MakeCostMatrix(Relems), dt) {} -LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator( - const Eigen::Matrix& A, const Eigen::Matrix& B, - const Eigen::Matrix& Q, const Eigen::Matrix& 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& A, const Eigen::Matrix& B, - const Eigen::Matrix& Q, const Eigen::Matrix& R, - const Eigen::Matrix& 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 diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index bf67b71474..7b25629861 100644 --- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -19,9 +19,8 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( units::second_t nominalDt) : m_observer( &DifferentialDrivePoseEstimator::F, - [](const Eigen::Vector& x, - const Eigen::Vector& u) { - return Eigen::Vector{x(3, 0), x(4, 0), x(2, 0)}; + [](const Vectord<5>& x, const Vectord<3>& u) { + return Vectord<3>{x(3, 0), x(4, 0), x(2, 0)}; }, stateStdDevs, localMeasurementStdDevs, frc::AngleMean<5, 5>(2), frc::AngleMean<3, 5>(2), frc::AngleResidual<5>(2), @@ -30,11 +29,10 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( SetVisionMeasurementStdDevs(visionMeasurmentStdDevs); // Create correction mechanism for vision measurements. - m_visionCorrect = [&](const Eigen::Vector& u, - const Eigen::Vector& y) { + m_visionCorrect = [&](const Vectord<3>& u, const Vectord<3>& y) { m_observer.Correct<3>( u, y, - [](const Eigen::Vector& x, const Eigen::Vector&) { + [](const Vectord<5>& x, const Vectord<3>&) { return x.block<3, 1>(0, 0); }, m_visionContR, frc::AngleMean<3, 5>(2), frc::AngleResidual<3>(2), @@ -73,7 +71,7 @@ Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const { void DifferentialDrivePoseEstimator::AddVisionMeasurement( const Pose2d& visionRobotPose, units::second_t timestamp) { if (auto sample = m_poseBuffer.Sample(timestamp)) { - m_visionCorrect(Eigen::Vector::Zero(), + m_visionCorrect(Vectord<3>::Zero(), PoseTo3dVector(GetEstimatedPosition().TransformBy( visionRobotPose - sample.value()))); } @@ -97,13 +95,13 @@ Pose2d DifferentialDrivePoseEstimator::UpdateWithTime( auto angle = gyroAngle + m_gyroOffset; auto omega = (gyroAngle - m_previousAngle).Radians() / dt; - auto u = Eigen::Vector{ - (wheelSpeeds.left + wheelSpeeds.right).value() / 2.0, 0.0, omega.value()}; + auto u = Vectord<3>{(wheelSpeeds.left + wheelSpeeds.right).value() / 2.0, 0.0, + omega.value()}; m_previousAngle = angle; - auto localY = Eigen::Vector{ - leftDistance.value(), rightDistance.value(), angle.Radians().value()}; + auto localY = Vectord<3>{leftDistance.value(), rightDistance.value(), + angle.Radians().value()}; m_poseBuffer.AddSample(currentTime, GetEstimatedPosition()); m_observer.Predict(u, dt); @@ -112,24 +110,24 @@ Pose2d DifferentialDrivePoseEstimator::UpdateWithTime( return GetEstimatedPosition(); } -Eigen::Vector DifferentialDrivePoseEstimator::F( - const Eigen::Vector& x, const Eigen::Vector& u) { +Vectord<5> DifferentialDrivePoseEstimator::F(const Vectord<5>& x, + const Vectord<3>& u) { // Apply a rotation matrix. Note that we do not add x because Runge-Kutta does // that for us. auto& theta = x(2); - Eigen::Matrix toFieldRotation{ + Matrixd<5, 5> toFieldRotation{ {std::cos(theta), -std::sin(theta), 0.0, 0.0, 0.0}, {std::sin(theta), std::cos(theta), 0.0, 0.0, 0.0}, {0.0, 0.0, 1.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 1.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 1.0}}; return toFieldRotation * - Eigen::Vector{u(0, 0), u(1, 0), u(2, 0), u(0, 0), u(1, 0)}; + Vectord<5>{u(0, 0), u(1, 0), u(2, 0), u(0, 0), u(1, 0)}; } template wpi::array DifferentialDrivePoseEstimator::StdDevMatrixToArray( - const Eigen::Vector& stdDevs) { + const Vectord& stdDevs) { wpi::array array; for (size_t i = 0; i < Dim; ++i) { array[i] = stdDevs(i); @@ -137,11 +135,11 @@ wpi::array DifferentialDrivePoseEstimator::StdDevMatrixToArray( return array; } -Eigen::Vector DifferentialDrivePoseEstimator::FillStateVector( +Vectord<5> DifferentialDrivePoseEstimator::FillStateVector( const Pose2d& pose, units::meter_t leftDistance, units::meter_t rightDistance) { - return Eigen::Vector{pose.Translation().X().value(), - pose.Translation().Y().value(), - pose.Rotation().Radians().value(), - leftDistance.value(), rightDistance.value()}; + return Vectord<5>{pose.Translation().X().value(), + pose.Translation().Y().value(), + pose.Rotation().Radians().value(), leftDistance.value(), + rightDistance.value()}; } diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index 62ae045224..e411eaa879 100644 --- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -18,26 +18,21 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( const wpi::array& localMeasurementStdDevs, const wpi::array& visionMeasurementStdDevs, units::second_t nominalDt) - : m_observer( - [](const Eigen::Vector& x, - const Eigen::Vector& u) { return u; }, - [](const Eigen::Vector& x, - const Eigen::Vector& u) { return x.block<1, 1>(2, 0); }, - stateStdDevs, localMeasurementStdDevs, frc::AngleMean<3, 3>(2), - frc::AngleMean<1, 3>(0), frc::AngleResidual<3>(2), - frc::AngleResidual<1>(0), frc::AngleAdd<3>(2), nominalDt), + : m_observer([](const Vectord<3>& x, const Vectord<3>& u) { return u; }, + [](const Vectord<3>& x, const Vectord<3>& u) { + return x.block<1, 1>(2, 0); + }, + stateStdDevs, localMeasurementStdDevs, frc::AngleMean<3, 3>(2), + frc::AngleMean<1, 3>(0), frc::AngleResidual<3>(2), + frc::AngleResidual<1>(0), frc::AngleAdd<3>(2), nominalDt), m_kinematics(kinematics), m_nominalDt(nominalDt) { SetVisionMeasurementStdDevs(visionMeasurementStdDevs); // Create vision correction mechanism. - m_visionCorrect = [&](const Eigen::Vector& u, - const Eigen::Vector& y) { + m_visionCorrect = [&](const Vectord<3>& u, const Vectord<3>& y) { m_observer.Correct<3>( - u, y, - [](const Eigen::Vector& x, const Eigen::Vector&) { - return x; - }, + u, y, [](const Vectord<3>& x, const Vectord<3>&) { return x; }, m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2), frc::AngleResidual<3>(2), frc::AngleAdd<3>(2)); }; @@ -76,7 +71,7 @@ Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const { void frc::MecanumDrivePoseEstimator::AddVisionMeasurement( const Pose2d& visionRobotPose, units::second_t timestamp) { if (auto sample = m_poseBuffer.Sample(timestamp)) { - m_visionCorrect(Eigen::Vector::Zero(), + m_visionCorrect(Vectord<3>::Zero(), PoseTo3dVector(GetEstimatedPosition().TransformBy( visionRobotPose - sample.value()))); } @@ -102,11 +97,10 @@ Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime( Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s) .RotateBy(angle); - Eigen::Vector u{fieldRelativeVelocities.X().value(), - fieldRelativeVelocities.Y().value(), - omega.value()}; + Vectord<3> u{fieldRelativeVelocities.X().value(), + fieldRelativeVelocities.Y().value(), omega.value()}; - Eigen::Vector localY{angle.Radians().value()}; + Vectord<1> localY{angle.Radians().value()}; m_previousAngle = angle; m_poseBuffer.AddSample(currentTime, GetEstimatedPosition()); diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp index c4a71fdf24..cf13c9f0e3 100644 --- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp +++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp @@ -25,8 +25,7 @@ MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds( chassisSpeeds.vy.value(), chassisSpeeds.omega.value()}; - Eigen::Vector wheelsVector = - m_inverseKinematics * chassisSpeedsVector; + Vectord<4> wheelsVector = m_inverseKinematics * chassisSpeedsVector; MecanumDriveWheelSpeeds wheelSpeeds; wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsVector(0)}; @@ -38,7 +37,7 @@ MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds( ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds( const MecanumDriveWheelSpeeds& wheelSpeeds) const { - Eigen::Vector wheelSpeedsVector{ + Vectord<4> wheelSpeedsVector{ wheelSpeeds.frontLeft.value(), wheelSpeeds.frontRight.value(), wheelSpeeds.rearLeft.value(), wheelSpeeds.rearRight.value()}; @@ -54,9 +53,8 @@ void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl, Translation2d fr, Translation2d rl, Translation2d rr) const { - m_inverseKinematics = - Eigen::Matrix{{1, -1, (-(fl.X() + fl.Y())).value()}, - {1, 1, (fr.X() - fr.Y()).value()}, - {1, 1, (rl.X() - rl.Y()).value()}, - {1, -1, (-(rr.X() + rr.Y())).value()}}; + m_inverseKinematics = Matrixd<4, 3>{{1, -1, (-(fl.X() + fl.Y())).value()}, + {1, 1, (fr.X() - fr.Y()).value()}, + {1, 1, (rl.X() - rl.Y()).value()}, + {1, -1, (-(rr.X() + rr.Y())).value()}}; } diff --git a/wpimath/src/main/native/include/frc/EigenCore.h b/wpimath/src/main/native/include/frc/EigenCore.h new file mode 100644 index 0000000000..b33e9e23fb --- /dev/null +++ b/wpimath/src/main/native/include/frc/EigenCore.h @@ -0,0 +1,23 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "Eigen/Core" + +namespace frc { + +template +using Vectord = Eigen::Vector; + +template +using Matrixd = Eigen::Matrix; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h index 17ed68c42a..da1e57dc3e 100644 --- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h +++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h @@ -13,9 +13,9 @@ #include #include -#include "Eigen/Core" #include "Eigen/Eigenvalues" #include "Eigen/QR" +#include "frc/EigenCore.h" #include "frc/geometry/Pose2d.h" namespace frc { @@ -64,9 +64,9 @@ void WhiteNoiseVectorImpl(Matrix& result, T elem, Ts... elems) { } template -bool IsStabilizableImpl(const Eigen::Matrix& A, - const Eigen::Matrix& B) { - Eigen::EigenSolver> es{A}; +bool IsStabilizableImpl(const Matrixd& A, + const Matrixd& B) { + Eigen::EigenSolver> es{A}; for (int i = 0; i < States; ++i) { if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() + @@ -107,12 +107,12 @@ template ...>>> WPI_DEPRECATED( "Use Eigen::Matrix or Eigen::Vector initializer list constructor") -Eigen::Matrix MakeMatrix(Ts... elems) { +Matrixd MakeMatrix(Ts... elems) { static_assert( sizeof...(elems) == Rows * Cols, "Number of provided elements doesn't match matrix dimensionality"); - Eigen::Matrix result; + Matrixd result; detail::MatrixImpl(result, elems...); return result; } @@ -132,8 +132,7 @@ Eigen::Matrix MakeMatrix(Ts... elems) { */ template ...>>> -Eigen::Matrix MakeCostMatrix( - Ts... tolerances) { +Matrixd MakeCostMatrix(Ts... tolerances) { Eigen::DiagonalMatrix result; detail::CostMatrixImpl(result.diagonal(), tolerances...); return result; @@ -153,8 +152,7 @@ Eigen::Matrix MakeCostMatrix( */ template ...>>> -Eigen::Matrix MakeCovMatrix( - Ts... stdDevs) { +Matrixd MakeCovMatrix(Ts... stdDevs) { Eigen::DiagonalMatrix result; detail::CovMatrixImpl(result.diagonal(), stdDevs...); return result; @@ -173,7 +171,7 @@ Eigen::Matrix MakeCovMatrix( * @return State excursion or control effort cost matrix. */ template -Eigen::Matrix MakeCostMatrix(const std::array& costs) { +Matrixd MakeCostMatrix(const std::array& costs) { Eigen::DiagonalMatrix result; auto& diag = result.diagonal(); for (size_t i = 0; i < N; ++i) { @@ -195,8 +193,7 @@ Eigen::Matrix MakeCostMatrix(const std::array& costs) { * @return Process noise or measurement noise covariance matrix. */ template -Eigen::Matrix MakeCovMatrix( - const std::array& stdDevs) { +Matrixd MakeCovMatrix(const std::array& stdDevs) { Eigen::DiagonalMatrix result; auto& diag = result.diagonal(); for (size_t i = 0; i < N; ++i) { @@ -207,8 +204,8 @@ Eigen::Matrix MakeCovMatrix( template ...>>> -Eigen::Matrix MakeWhiteNoiseVector(Ts... stdDevs) { - Eigen::Matrix result; +Matrixd MakeWhiteNoiseVector(Ts... stdDevs) { + Matrixd result; detail::WhiteNoiseVectorImpl(result, stdDevs...); return result; } @@ -222,12 +219,11 @@ Eigen::Matrix MakeWhiteNoiseVector(Ts... stdDevs) { * @return White noise vector. */ template -Eigen::Vector MakeWhiteNoiseVector( - const std::array& stdDevs) { +Vectord MakeWhiteNoiseVector(const std::array& stdDevs) { std::random_device rd; std::mt19937 gen{rd()}; - Eigen::Vector result; + Vectord result; for (int i = 0; i < N; ++i) { // Passing a standard deviation of 0.0 to std::normal_distribution is // undefined behavior @@ -249,7 +245,7 @@ Eigen::Vector MakeWhiteNoiseVector( * @return The vector. */ WPILIB_DLLEXPORT -Eigen::Vector PoseTo3dVector(const Pose2d& pose); +Vectord<3> PoseTo3dVector(const Pose2d& pose); /** * Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)]. @@ -259,7 +255,7 @@ Eigen::Vector PoseTo3dVector(const Pose2d& pose); * @return The vector. */ WPILIB_DLLEXPORT -Eigen::Vector PoseTo4dVector(const Pose2d& pose); +Vectord<4> PoseTo4dVector(const Pose2d& pose); /** * Returns true if (A, B) is a stabilizable pair. @@ -274,8 +270,8 @@ Eigen::Vector PoseTo4dVector(const Pose2d& pose); * @param B Input matrix. */ template -bool IsStabilizable(const Eigen::Matrix& A, - const Eigen::Matrix& B) { +bool IsStabilizable(const Matrixd& A, + const Matrixd& B) { return detail::IsStabilizableImpl(A, B); } @@ -292,8 +288,8 @@ bool IsStabilizable(const Eigen::Matrix& A, * @param C Output matrix. */ template -bool IsDetectable(const Eigen::Matrix& A, - const Eigen::Matrix& C) { +bool IsDetectable(const Matrixd& A, + const Matrixd& C) { return detail::IsStabilizableImpl(A.transpose(), C.transpose()); } @@ -301,14 +297,14 @@ bool IsDetectable(const Eigen::Matrix& A, // Template specializations are used here to make common state-input pairs // compile faster. template <> -WPILIB_DLLEXPORT bool IsStabilizable<1, 1>( - const Eigen::Matrix& A, const Eigen::Matrix& B); +WPILIB_DLLEXPORT bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A, + const Matrixd<1, 1>& B); // Template specializations are used here to make common state-input pairs // compile faster. template <> -WPILIB_DLLEXPORT bool IsStabilizable<2, 1>( - const Eigen::Matrix& A, const Eigen::Matrix& B); +WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A, + const Matrixd<2, 1>& B); /** * Converts a Pose2d into a vector of [x, y, theta]. @@ -318,7 +314,7 @@ WPILIB_DLLEXPORT bool IsStabilizable<2, 1>( * @return The vector. */ WPILIB_DLLEXPORT -Eigen::Vector PoseToVector(const Pose2d& pose); +Vectord<3> PoseToVector(const Pose2d& pose); /** * Clamps input vector between system's minimum and maximum allowable input. @@ -330,11 +326,10 @@ Eigen::Vector PoseToVector(const Pose2d& pose); * @return Clamped input vector. */ template -Eigen::Vector ClampInputMaxMagnitude( - const Eigen::Vector& u, - const Eigen::Vector& umin, - const Eigen::Vector& umax) { - Eigen::Vector result; +Vectord ClampInputMaxMagnitude(const Vectord& u, + const Vectord& umin, + const Vectord& umax) { + Vectord result; for (int i = 0; i < Inputs; ++i) { result(i) = std::clamp(u(i), umin(i), umax(i)); } @@ -351,8 +346,8 @@ Eigen::Vector ClampInputMaxMagnitude( * @return The normalizedInput */ template -Eigen::Vector DesaturateInputVector( - const Eigen::Vector& u, double maxMagnitude) { +Vectord DesaturateInputVector(const Vectord& u, + double maxMagnitude) { double maxValue = u.template lpNorm(); if (maxValue > maxMagnitude) { diff --git a/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h index 75d85a7bc6..21aad251e7 100644 --- a/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h @@ -7,8 +7,8 @@ #include #include -#include "Eigen/Core" #include "Eigen/QR" +#include "frc/EigenCore.h" #include "frc/system/NumericalJacobian.h" #include "units/time.h" @@ -39,6 +39,9 @@ namespace frc { template class ControlAffinePlantInversionFeedforward { public: + using StateVector = Vectord; + using InputVector = Vectord; + /** * Constructs a feedforward with given model dynamics as a function * of state and input. @@ -50,15 +53,11 @@ class ControlAffinePlantInversionFeedforward { * @param dt The timestep between calls of calculate(). */ ControlAffinePlantInversionFeedforward( - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> - f, + std::function f, units::second_t dt) : m_dt(dt), m_f(f) { - m_B = NumericalJacobianU( - f, Eigen::Vector::Zero(), - Eigen::Vector::Zero()); + m_B = NumericalJacobianU(f, StateVector::Zero(), + InputVector::Zero()); Reset(); } @@ -73,14 +72,12 @@ class ControlAffinePlantInversionFeedforward { * @param dt The timestep between calls of calculate(). */ ControlAffinePlantInversionFeedforward( - std::function< - Eigen::Vector(const Eigen::Vector&)> - f, - const Eigen::Matrix& B, units::second_t dt) + std::function f, + const Matrixd& B, units::second_t dt) : m_B(B), m_dt(dt) { - m_f = [=](const Eigen::Vector& x, - const Eigen::Vector& u) - -> Eigen::Vector { return f(x); }; + m_f = [=](const StateVector& x, const InputVector& u) -> StateVector { + return f(x); + }; Reset(); } @@ -95,7 +92,7 @@ class ControlAffinePlantInversionFeedforward { * * @return The calculated feedforward. */ - const Eigen::Vector& Uff() const { return m_uff; } + const InputVector& Uff() const { return m_uff; } /** * Returns an element of the previously calculated feedforward. @@ -111,7 +108,7 @@ class ControlAffinePlantInversionFeedforward { * * @return The current reference vector. */ - const Eigen::Vector& R() const { return m_r; } + const StateVector& R() const { return m_r; } /** * Returns an element of the reference vector r. @@ -127,7 +124,7 @@ class ControlAffinePlantInversionFeedforward { * * @param initialState The initial state vector. */ - void Reset(const Eigen::Vector& initialState) { + void Reset(const StateVector& initialState) { m_r = initialState; m_uff.setZero(); } @@ -146,15 +143,14 @@ class ControlAffinePlantInversionFeedforward { * reference. * * If this method is used the initial state of the system is the one set using - * Reset(const Eigen::Vector&). If the initial state is not + * Reset(const StateVector&). If the initial state is not * set it defaults to a zero vector. * * @param nextR The reference state of the future timestep (k + dt). * * @return The calculated feedforward. */ - Eigen::Vector Calculate( - const Eigen::Vector& nextR) { + InputVector Calculate(const StateVector& nextR) { return Calculate(m_r, nextR); } @@ -166,36 +162,30 @@ class ControlAffinePlantInversionFeedforward { * * @return The calculated feedforward. */ - Eigen::Vector Calculate( - const Eigen::Vector& r, - const Eigen::Vector& nextR) { - Eigen::Vector rDot = (nextR - r) / m_dt.value(); + InputVector Calculate(const StateVector& r, const StateVector& nextR) { + StateVector rDot = (nextR - r) / m_dt.value(); - m_uff = m_B.householderQr().solve( - rDot - m_f(r, Eigen::Vector::Zero())); + m_uff = m_B.householderQr().solve(rDot - m_f(r, InputVector::Zero())); m_r = nextR; return m_uff; } private: - Eigen::Matrix m_B; + Matrixd m_B; units::second_t m_dt; /** * The model dynamics. */ - std::function( - const Eigen::Vector&, - const Eigen::Vector&)> - m_f; + std::function m_f; // Current reference - Eigen::Vector m_r; + StateVector m_r; // Computed feedforward - Eigen::Vector m_uff; + InputVector m_uff; }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h b/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h index e093582bfd..eef1fc0d10 100644 --- a/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h +++ b/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h @@ -6,8 +6,8 @@ #include -#include "Eigen/Core" #include "Eigen/QR" +#include "frc/EigenCore.h" #include "units/time.h" namespace frc { @@ -27,6 +27,9 @@ namespace frc { template class ImplicitModelFollower { public: + using StateVector = Vectord; + using InputVector = Vectord; + /** * Constructs a controller with the given coefficients and plant. * @@ -47,10 +50,10 @@ class ImplicitModelFollower { * @param Aref Continuous system matrix whose dynamics should be followed. * @param Bref Continuous input matrix whose dynamics should be followed. */ - ImplicitModelFollower(const Eigen::Matrix& A, - const Eigen::Matrix& B, - const Eigen::Matrix& Aref, - const Eigen::Matrix& Bref) { + ImplicitModelFollower(const Matrixd& A, + const Matrixd& B, + const Matrixd& Aref, + const Matrixd& Bref) { // Find u_imf that makes real model match reference model. // // dx/dt = Ax + Bu_imf @@ -79,7 +82,7 @@ class ImplicitModelFollower { * * @return The control input. */ - const Eigen::Vector& U() const { return m_u; } + const InputVector& U() const { return m_u; } /** * Returns an element of the control input vector u. @@ -101,22 +104,20 @@ class ImplicitModelFollower { * @param x The current state x. * @param u The current input for the original model. */ - Eigen::Vector Calculate( - const Eigen::Vector& x, - const Eigen::Vector& u) { + InputVector Calculate(const StateVector& x, const InputVector& u) { m_u = m_A * x + m_B * u; return m_u; } private: // Computed controller output - Eigen::Vector m_u; + InputVector m_u; // State space conversion gain - Eigen::Matrix m_A; + Matrixd m_A; // Input space conversion gain - Eigen::Matrix m_B; + Matrixd m_B; }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h index eccc483327..d4cc3c426d 100644 --- a/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h @@ -7,8 +7,8 @@ #include #include -#include "Eigen/Core" #include "Eigen/QR" +#include "frc/EigenCore.h" #include "frc/system/Discretization.h" #include "frc/system/LinearSystem.h" #include "units/time.h" @@ -31,6 +31,9 @@ namespace frc { template class LinearPlantInversionFeedforward { public: + using StateVector = Vectord; + using InputVector = Vectord; + /** * Constructs a feedforward with the given plant. * @@ -50,9 +53,9 @@ class LinearPlantInversionFeedforward { * @param B Continuous input matrix of the plant being controlled. * @param dt Discretization timestep. */ - LinearPlantInversionFeedforward( - const Eigen::Matrix& A, - const Eigen::Matrix& B, units::second_t dt) + LinearPlantInversionFeedforward(const Matrixd& A, + const Matrixd& B, + units::second_t dt) : m_dt(dt) { DiscretizeAB(A, B, dt, &m_A, &m_B); Reset(); @@ -63,7 +66,7 @@ class LinearPlantInversionFeedforward { * * @return The calculated feedforward. */ - const Eigen::Vector& Uff() const { return m_uff; } + const InputVector& Uff() const { return m_uff; } /** * Returns an element of the previously calculated feedforward. @@ -79,7 +82,7 @@ class LinearPlantInversionFeedforward { * * @return The current reference vector. */ - const Eigen::Vector& R() const { return m_r; } + const StateVector& R() const { return m_r; } /** * Returns an element of the reference vector r. @@ -95,7 +98,7 @@ class LinearPlantInversionFeedforward { * * @param initialState The initial state vector. */ - void Reset(const Eigen::Vector& initialState) { + void Reset(const StateVector& initialState) { m_r = initialState; m_uff.setZero(); } @@ -114,15 +117,14 @@ class LinearPlantInversionFeedforward { * reference. * * If this method is used the initial state of the system is the one set using - * Reset(const Eigen::Vector&). If the initial state is not + * Reset(const StateVector&). If the initial state is not * set it defaults to a zero vector. * * @param nextR The reference state of the future timestep (k + dt). * * @return The calculated feedforward. */ - Eigen::Vector Calculate( - const Eigen::Vector& nextR) { + InputVector Calculate(const StateVector& nextR) { return Calculate(m_r, nextR); } @@ -134,25 +136,23 @@ class LinearPlantInversionFeedforward { * * @return The calculated feedforward. */ - Eigen::Vector Calculate( - const Eigen::Vector& r, - const Eigen::Vector& nextR) { + InputVector Calculate(const StateVector& r, const StateVector& nextR) { m_uff = m_B.householderQr().solve(nextR - (m_A * r)); m_r = nextR; return m_uff; } private: - Eigen::Matrix m_A; - Eigen::Matrix m_B; + Matrixd m_A; + Matrixd m_B; units::second_t m_dt; // Current reference - Eigen::Vector m_r; + StateVector m_r; // Computed feedforward - Eigen::Vector m_uff; + InputVector m_uff; }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h index c0e5dfd5de..4bc0083331 100644 --- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h +++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h @@ -12,9 +12,9 @@ #include #include "Eigen/Cholesky" -#include "Eigen/Core" #include "Eigen/Eigenvalues" #include "drake/math/discrete_algebraic_riccati_equation.h" +#include "frc/EigenCore.h" #include "frc/StateSpaceUtil.h" #include "frc/system/Discretization.h" #include "frc/system/LinearSystem.h" @@ -39,6 +39,12 @@ namespace detail { template class LinearQuadraticRegulatorImpl { public: + using StateVector = Vectord; + using InputVector = Vectord; + + using StateArray = wpi::array; + using InputArray = wpi::array; + /** * Constructs a controller with the given coefficients and plant. * @@ -50,8 +56,7 @@ class LinearQuadraticRegulatorImpl { template LinearQuadraticRegulatorImpl( const LinearSystem& plant, - const wpi::array& Qelems, - const wpi::array& Relems, units::second_t dt) + const StateArray& Qelems, const InputArray& Relems, units::second_t dt) : LinearQuadraticRegulatorImpl(plant.A(), plant.B(), Qelems, Relems, dt) { } @@ -64,11 +69,10 @@ class LinearQuadraticRegulatorImpl { * @param Relems The maximum desired control effort for each input. * @param dt Discretization timestep. */ - LinearQuadraticRegulatorImpl(const Eigen::Matrix& A, - const Eigen::Matrix& B, - const wpi::array& Qelems, - const wpi::array& Relems, - units::second_t dt) + LinearQuadraticRegulatorImpl(const Matrixd& A, + const Matrixd& B, + const StateArray& Qelems, + const InputArray& Relems, units::second_t dt) : LinearQuadraticRegulatorImpl(A, B, MakeCostMatrix(Qelems), MakeCostMatrix(Relems), dt) {} @@ -81,13 +85,13 @@ class LinearQuadraticRegulatorImpl { * @param R The input cost matrix. * @param dt Discretization timestep. */ - LinearQuadraticRegulatorImpl(const Eigen::Matrix& A, - const Eigen::Matrix& B, - const Eigen::Matrix& Q, - const Eigen::Matrix& R, + LinearQuadraticRegulatorImpl(const Matrixd& A, + const Matrixd& B, + const Matrixd& Q, + const Matrixd& R, units::second_t dt) { - Eigen::Matrix discA; - Eigen::Matrix discB; + Matrixd discA; + Matrixd discB; DiscretizeAB(A, B, dt, &discA, &discB); if (!IsStabilizable(discA, discB)) { @@ -100,7 +104,7 @@ class LinearQuadraticRegulatorImpl { throw std::invalid_argument(msg); } - Eigen::Matrix S = + Matrixd S = drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R); // K = (BᵀSB + R)⁻¹BᵀSA @@ -121,17 +125,17 @@ class LinearQuadraticRegulatorImpl { * @param N The state-input cross-term cost matrix. * @param dt Discretization timestep. */ - LinearQuadraticRegulatorImpl(const Eigen::Matrix& A, - const Eigen::Matrix& B, - const Eigen::Matrix& Q, - const Eigen::Matrix& R, - const Eigen::Matrix& N, + LinearQuadraticRegulatorImpl(const Matrixd& A, + const Matrixd& B, + const Matrixd& Q, + const Matrixd& R, + const Matrixd& N, units::second_t dt) { - Eigen::Matrix discA; - Eigen::Matrix discB; + Matrixd discA; + Matrixd discB; DiscretizeAB(A, B, dt, &discA, &discB); - Eigen::Matrix S = + Matrixd S = drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N); // K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ) @@ -149,7 +153,7 @@ class LinearQuadraticRegulatorImpl { /** * Returns the controller matrix K. */ - const Eigen::Matrix& K() const { return m_K; } + const Matrixd& K() const { return m_K; } /** * Returns an element of the controller matrix K. @@ -164,7 +168,7 @@ class LinearQuadraticRegulatorImpl { * * @return The reference vector. */ - const Eigen::Vector& R() const { return m_r; } + const StateVector& R() const { return m_r; } /** * Returns an element of the reference vector r. @@ -180,7 +184,7 @@ class LinearQuadraticRegulatorImpl { * * @return The control input. */ - const Eigen::Vector& U() const { return m_u; } + const InputVector& U() const { return m_u; } /** * Returns an element of the control input vector u. @@ -204,8 +208,7 @@ class LinearQuadraticRegulatorImpl { * * @param x The current state x. */ - Eigen::Vector Calculate( - const Eigen::Vector& x) { + InputVector Calculate(const StateVector& x) { m_u = m_K * (m_r - x); return m_u; } @@ -216,9 +219,7 @@ class LinearQuadraticRegulatorImpl { * @param x The current state x. * @param nextR The next reference vector r. */ - Eigen::Vector Calculate( - const Eigen::Vector& x, - const Eigen::Vector& nextR) { + InputVector Calculate(const StateVector& x, const StateVector& nextR) { m_r = nextR; return Calculate(x); } @@ -242,8 +243,8 @@ class LinearQuadraticRegulatorImpl { template void LatencyCompensate(const LinearSystem& plant, units::second_t dt, units::second_t inputDelay) { - Eigen::Matrix discA; - Eigen::Matrix discB; + Matrixd discA; + Matrixd discB; DiscretizeAB(plant.A(), plant.B(), dt, &discA, &discB); m_K = m_K * (discA - discB * m_K).pow(inputDelay / dt); @@ -251,13 +252,13 @@ class LinearQuadraticRegulatorImpl { private: // Current reference - Eigen::Vector m_r; + StateVector m_r; // Computed controller output - Eigen::Vector m_u; + InputVector m_u; // Controller gain - Eigen::Matrix m_K; + Matrixd m_K; }; } // namespace detail @@ -291,8 +292,8 @@ class LinearQuadraticRegulator * @param Relems The maximum desired control effort for each input. * @param dt Discretization timestep. */ - LinearQuadraticRegulator(const Eigen::Matrix& A, - const Eigen::Matrix& B, + LinearQuadraticRegulator(const Matrixd& A, + const Matrixd& B, const wpi::array& Qelems, const wpi::array& Relems, units::second_t dt) @@ -308,11 +309,10 @@ class LinearQuadraticRegulator * @param R The input cost matrix. * @param dt Discretization timestep. */ - LinearQuadraticRegulator(const Eigen::Matrix& A, - const Eigen::Matrix& B, - const Eigen::Matrix& Q, - const Eigen::Matrix& R, - units::second_t dt) + LinearQuadraticRegulator(const Matrixd& A, + const Matrixd& B, + const Matrixd& Q, + const Matrixd& R, units::second_t dt) : detail::LinearQuadraticRegulatorImpl{A, B, Q, R, dt} {} /** @@ -325,12 +325,11 @@ class LinearQuadraticRegulator * @param N The state-input cross-term cost matrix. * @param dt Discretization timestep. */ - LinearQuadraticRegulator(const Eigen::Matrix& A, - const Eigen::Matrix& B, - const Eigen::Matrix& Q, - const Eigen::Matrix& R, - const Eigen::Matrix& N, - units::second_t dt) + LinearQuadraticRegulator(const Matrixd& A, + const Matrixd& B, + const Matrixd& Q, + const Matrixd& R, + const Matrixd& N, units::second_t dt) : detail::LinearQuadraticRegulatorImpl{A, B, Q, R, N, dt} {} @@ -351,24 +350,18 @@ class WPILIB_DLLEXPORT LinearQuadraticRegulator<1, 1> units::second_t dt) : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {} - LinearQuadraticRegulator(const Eigen::Matrix& A, - const Eigen::Matrix& B, + LinearQuadraticRegulator(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B, const wpi::array& Qelems, const wpi::array& Relems, units::second_t dt); - LinearQuadraticRegulator(const Eigen::Matrix& A, - const Eigen::Matrix& B, - const Eigen::Matrix& Q, - const Eigen::Matrix& R, + 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); - LinearQuadraticRegulator(const Eigen::Matrix& A, - const Eigen::Matrix& B, - const Eigen::Matrix& Q, - const Eigen::Matrix& R, - const Eigen::Matrix& N, - units::second_t dt); + LinearQuadraticRegulator(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); LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default; LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default; @@ -387,24 +380,18 @@ class WPILIB_DLLEXPORT LinearQuadraticRegulator<2, 1> units::second_t dt) : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {} - LinearQuadraticRegulator(const Eigen::Matrix& A, - const Eigen::Matrix& B, + LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B, const wpi::array& Qelems, const wpi::array& Relems, units::second_t dt); - LinearQuadraticRegulator(const Eigen::Matrix& A, - const Eigen::Matrix& B, - const Eigen::Matrix& Q, - const Eigen::Matrix& R, + 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); - LinearQuadraticRegulator(const Eigen::Matrix& A, - const Eigen::Matrix& B, - const Eigen::Matrix& Q, - const Eigen::Matrix& R, - const Eigen::Matrix& N, - units::second_t dt); + LinearQuadraticRegulator(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); LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default; LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default; @@ -423,24 +410,18 @@ class WPILIB_DLLEXPORT LinearQuadraticRegulator<2, 2> units::second_t dt) : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {} - LinearQuadraticRegulator(const Eigen::Matrix& A, - const Eigen::Matrix& B, + LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 2>& B, const wpi::array& Qelems, const wpi::array& Relems, units::second_t dt); - LinearQuadraticRegulator(const Eigen::Matrix& A, - const Eigen::Matrix& B, - const Eigen::Matrix& Q, - const Eigen::Matrix& R, + 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); - LinearQuadraticRegulator(const Eigen::Matrix& A, - const Eigen::Matrix& B, - const Eigen::Matrix& Q, - const Eigen::Matrix& R, - const Eigen::Matrix& N, - units::second_t dt); + LinearQuadraticRegulator(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); LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default; LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default; diff --git a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h index df1a52cad7..27852f7499 100644 --- a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h @@ -6,7 +6,7 @@ #include -#include "Eigen/Core" +#include "frc/EigenCore.h" #include "frc/controller/LinearPlantInversionFeedforward.h" #include "frc/system/plant/LinearSystemId.h" #include "units/time.h" @@ -70,8 +70,8 @@ class SimpleMotorFeedforward { auto plant = LinearSystemId::IdentifyVelocitySystem(kV, kA); LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt}; - Eigen::Vector r{currentVelocity.value()}; - Eigen::Vector nextR{nextVelocity.value()}; + Vectord<1> r{currentVelocity.value()}; + Vectord<1> nextR{nextVelocity.value()}; return kS * wpi::sgn(currentVelocity.value()) + units::volt_t{feedforward.Calculate(r, nextR)(0)}; diff --git a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h index 3ddabc1019..5186d441c9 100644 --- a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h +++ b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h @@ -6,7 +6,7 @@ #include -#include "Eigen/Core" +#include "frc/EigenCore.h" #include "frc/MathUtil.h" namespace frc { @@ -21,10 +21,9 @@ namespace frc { * @param angleStateIdx The row containing angles to be normalized. */ template -Eigen::Vector AngleResidual( - const Eigen::Vector& a, - const Eigen::Vector& b, int angleStateIdx) { - Eigen::Vector ret = a - b; +Vectord AngleResidual(const Vectord& a, + const Vectord& b, int angleStateIdx) { + Vectord ret = a - b; ret[angleStateIdx] = AngleModulus(units::radian_t{ret[angleStateIdx]}).value(); return ret; @@ -38,8 +37,7 @@ Eigen::Vector AngleResidual( * @param angleStateIdx The row containing angles to be normalized. */ template -std::function( - const Eigen::Vector&, const Eigen::Vector&)> +std::function(const Vectord&, const Vectord&)> AngleResidual(int angleStateIdx) { return [=](auto a, auto b) { return AngleResidual(a, b, angleStateIdx); @@ -56,10 +54,9 @@ AngleResidual(int angleStateIdx) { * @param angleStateIdx The row containing angles to be normalized. */ template -Eigen::Vector AngleAdd(const Eigen::Vector& a, - const Eigen::Vector& b, - int angleStateIdx) { - Eigen::Vector ret = a + b; +Vectord AngleAdd(const Vectord& a, const Vectord& b, + int angleStateIdx) { + Vectord ret = a + b; ret[angleStateIdx] = InputModulus(ret[angleStateIdx], -wpi::numbers::pi, wpi::numbers::pi); return ret; @@ -73,8 +70,7 @@ Eigen::Vector AngleAdd(const Eigen::Vector& a, * @param angleStateIdx The row containing angles to be normalized. */ template -std::function( - const Eigen::Vector&, const Eigen::Vector&)> +std::function(const Vectord&, const Vectord&)> AngleAdd(int angleStateIdx) { return [=](auto a, auto b) { return AngleAdd(a, b, angleStateIdx); }; } @@ -91,9 +87,9 @@ AngleAdd(int angleStateIdx) { * @param angleStatesIdx The row containing the angles. */ template -Eigen::Vector AngleMean( - const Eigen::Matrix& sigmas, - const Eigen::Vector& Wm, int angleStatesIdx) { +Vectord AngleMean(const Matrixd& sigmas, + const Vectord<2 * States + 1>& Wm, + int angleStatesIdx) { double sumSin = sigmas.row(angleStatesIdx) .unaryExpr([](auto it) { return std::sin(it); }) .sum(); @@ -101,7 +97,7 @@ Eigen::Vector AngleMean( .unaryExpr([](auto it) { return std::cos(it); }) .sum(); - Eigen::Vector ret = sigmas * Wm; + Vectord ret = sigmas * Wm; ret[angleStatesIdx] = std::atan2(sumSin, sumCos); return ret; } @@ -116,9 +112,8 @@ Eigen::Vector AngleMean( * @param angleStateIdx The row containing the angles. */ template -std::function( - const Eigen::Matrix&, - const Eigen::Vector&)> +std::function(const Matrixd&, + const Vectord<2 * States + 1>&)> AngleMean(int angleStateIdx) { return [=](auto sigmas, auto Wm) { return AngleMean(sigmas, Wm, angleStateIdx); diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h index 45022eff86..afe6bed98f 100644 --- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h @@ -7,7 +7,7 @@ #include #include -#include "Eigen/Core" +#include "frc/EigenCore.h" #include "frc/estimator/UnscentedKalmanFilter.h" #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation2d.h" @@ -223,11 +223,9 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator { private: UnscentedKalmanFilter<5, 3, 3> m_observer; TimeInterpolatableBuffer m_poseBuffer{1.5_s}; - std::function& u, - const Eigen::Vector& y)> - m_visionCorrect; + std::function& u, const Vectord<3>& y)> m_visionCorrect; - Eigen::Matrix m_visionContR; + Matrixd<3, 3> m_visionContR; units::second_t m_nominalDt; units::second_t m_prevTime = -1_s; @@ -237,13 +235,12 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator { template static wpi::array StdDevMatrixToArray( - const Eigen::Vector& stdDevs); + const Vectord& stdDevs); - static Eigen::Vector F(const Eigen::Vector& x, - const Eigen::Vector& u); - static Eigen::Vector FillStateVector(const Pose2d& pose, - units::meter_t leftDistance, - units::meter_t rightDistance); + static Vectord<5> F(const Vectord<5>& x, const Vectord<3>& u); + static Vectord<5> FillStateVector(const Pose2d& pose, + units::meter_t leftDistance, + units::meter_t rightDistance); }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h index 4e40931dec..b09e8d9d94 100644 --- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h @@ -8,7 +8,7 @@ #include -#include "Eigen/Core" +#include "frc/EigenCore.h" #include "units/time.h" namespace frc { @@ -40,6 +40,15 @@ namespace frc { template class ExtendedKalmanFilter { public: + using StateVector = Vectord; + using InputVector = Vectord; + using OutputVector = Vectord; + + using StateArray = wpi::array; + using OutputArray = wpi::array; + + using StateMatrix = Matrixd; + /** * Constructs an extended Kalman filter. * @@ -51,17 +60,11 @@ class ExtendedKalmanFilter { * @param measurementStdDevs Standard deviations of measurements. * @param dt Nominal discretization timestep. */ - ExtendedKalmanFilter(std::function( - const Eigen::Vector&, - const Eigen::Vector&)> - f, - std::function( - const Eigen::Vector&, - const Eigen::Vector&)> - h, - const wpi::array& stateStdDevs, - const wpi::array& measurementStdDevs, - units::second_t dt); + ExtendedKalmanFilter( + std::function f, + std::function h, + const StateArray& stateStdDevs, const OutputArray& measurementStdDevs, + units::second_t dt); /** * Constructs an extended Kalman filter. @@ -77,30 +80,20 @@ class ExtendedKalmanFilter { * @param addFuncX A function that adds two state vectors. * @param dt Nominal discretization timestep. */ - ExtendedKalmanFilter(std::function( - const Eigen::Vector&, - const Eigen::Vector&)> - f, - std::function( - const Eigen::Vector&, - const Eigen::Vector&)> - h, - const wpi::array& stateStdDevs, - const wpi::array& measurementStdDevs, - std::function( - const Eigen::Vector&, - const Eigen::Vector&)> - residualFuncY, - std::function( - const Eigen::Vector&, - const Eigen::Vector&)> - addFuncX, - units::second_t dt); + ExtendedKalmanFilter( + std::function f, + std::function h, + const StateArray& stateStdDevs, const OutputArray& measurementStdDevs, + std::function + residualFuncY, + std::function + addFuncX, + units::second_t dt); /** * Returns the error covariance matrix P. */ - const Eigen::Matrix& P() const { return m_P; } + const StateMatrix& P() const { return m_P; } /** * Returns an element of the error covariance matrix P. @@ -115,12 +108,12 @@ class ExtendedKalmanFilter { * * @param P The error covariance matrix P. */ - void SetP(const Eigen::Matrix& P) { m_P = P; } + void SetP(const StateMatrix& P) { m_P = P; } /** * Returns the state estimate x-hat. */ - const Eigen::Vector& Xhat() const { return m_xHat; } + const StateVector& Xhat() const { return m_xHat; } /** * Returns an element of the state estimate x-hat. @@ -134,7 +127,7 @@ class ExtendedKalmanFilter { * * @param xHat The state estimate x-hat. */ - void SetXhat(const Eigen::Vector& xHat) { m_xHat = xHat; } + void SetXhat(const StateVector& xHat) { m_xHat = xHat; } /** * Set an element of the initial state estimate x-hat. @@ -158,7 +151,7 @@ class ExtendedKalmanFilter { * @param u New control input from controller. * @param dt Timestep for prediction. */ - void Predict(const Eigen::Vector& u, units::second_t dt); + void Predict(const InputVector& u, units::second_t dt); /** * Correct the state estimate x-hat using the measurements in y. @@ -166,19 +159,15 @@ class ExtendedKalmanFilter { * @param u Same control input used in the predict step. * @param y Measurement vector. */ - void Correct(const Eigen::Vector& u, - const Eigen::Vector& y) { + void Correct(const InputVector& u, const OutputVector& y) { Correct(u, y, m_h, m_contR, m_residualFuncY, m_addFuncX); } template - void Correct(const Eigen::Vector& u, - const Eigen::Vector& y, - std::function( - const Eigen::Vector&, - const Eigen::Vector&)> - h, - const Eigen::Matrix& R); + void Correct( + const InputVector& u, const Vectord& y, + std::function(const StateVector&, const InputVector&)> h, + const Matrixd& R); /** * Correct the state estimate x-hat using the measurements in y. @@ -197,46 +186,28 @@ class ExtendedKalmanFilter { * @param addFuncX A function that adds two state vectors. */ template - void Correct(const Eigen::Vector& u, - const Eigen::Vector& y, - std::function( - const Eigen::Vector&, - const Eigen::Vector&)> - h, - const Eigen::Matrix& R, - std::function( - const Eigen::Vector&, - const Eigen::Vector&)> - residualFuncY, - std::function( - const Eigen::Vector&, - const Eigen::Vector)> - addFuncX); + void Correct( + const InputVector& u, const Vectord& y, + std::function(const StateVector&, const InputVector&)> h, + const Matrixd& R, + std::function(const Vectord&, const Vectord&)> + residualFuncY, + std::function + addFuncX); private: - std::function( - const Eigen::Vector&, - const Eigen::Vector&)> - m_f; - std::function( - const Eigen::Vector&, - const Eigen::Vector&)> - m_h; - std::function( - const Eigen::Vector&, - const Eigen::Vector)> + std::function m_f; + std::function m_h; + std::function m_residualFuncY; - std::function( - const Eigen::Vector&, - const Eigen::Vector)> - m_addFuncX; - Eigen::Vector m_xHat; - Eigen::Matrix m_P; - Eigen::Matrix m_contQ; - Eigen::Matrix m_contR; + std::function m_addFuncX; + StateVector m_xHat; + StateMatrix m_P; + StateMatrix m_contQ; + Matrixd m_contR; units::second_t m_dt; - Eigen::Matrix m_initP; + StateMatrix m_initP; }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc index 5a50bb4c87..a56b8b5c66 100644 --- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc +++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc @@ -16,72 +16,47 @@ namespace frc { template ExtendedKalmanFilter::ExtendedKalmanFilter( - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> - f, - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> - h, - const wpi::array& stateStdDevs, - const wpi::array& measurementStdDevs, units::second_t dt) + std::function f, + std::function h, + const StateArray& stateStdDevs, const OutputArray& measurementStdDevs, + units::second_t dt) : m_f(f), m_h(h) { m_contQ = MakeCovMatrix(stateStdDevs); m_contR = MakeCovMatrix(measurementStdDevs); - m_residualFuncY = [](auto a, auto b) -> Eigen::Vector { - return a - b; - }; - m_addFuncX = [](auto a, auto b) -> Eigen::Vector { - return a + b; - }; + m_residualFuncY = [](auto a, auto b) -> OutputVector { return a - b; }; + m_addFuncX = [](auto a, auto b) -> StateVector { return a + b; }; m_dt = dt; Reset(); - Eigen::Matrix contA = - NumericalJacobianX( - m_f, m_xHat, Eigen::Vector::Zero()); - Eigen::Matrix C = - NumericalJacobianX( - m_h, m_xHat, Eigen::Vector::Zero()); + StateMatrix contA = NumericalJacobianX( + m_f, m_xHat, InputVector::Zero()); + Matrixd C = NumericalJacobianX( + m_h, m_xHat, InputVector::Zero()); - Eigen::Matrix discA; - Eigen::Matrix discQ; + StateMatrix discA; + StateMatrix discQ; DiscretizeAQTaylor(contA, m_contQ, dt, &discA, &discQ); - Eigen::Matrix discR = - DiscretizeR(m_contR, dt); + Matrixd discR = DiscretizeR(m_contR, dt); if (IsDetectable(discA, C) && Outputs <= States) { m_initP = drake::math::DiscreteAlgebraicRiccatiEquation( discA.transpose(), C.transpose(), discQ, discR); } else { - m_initP = Eigen::Matrix::Zero(); + m_initP = StateMatrix::Zero(); } m_P = m_initP; } template ExtendedKalmanFilter::ExtendedKalmanFilter( - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> - f, - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> - h, - const wpi::array& stateStdDevs, - const wpi::array& measurementStdDevs, - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> + std::function f, + std::function h, + const StateArray& stateStdDevs, const OutputArray& measurementStdDevs, + std::function residualFuncY, - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> - addFuncX, + std::function addFuncX, units::second_t dt) : m_f(f), m_h(h), m_residualFuncY(residualFuncY), m_addFuncX(addFuncX) { m_contQ = MakeCovMatrix(stateStdDevs); @@ -90,39 +65,36 @@ ExtendedKalmanFilter::ExtendedKalmanFilter( Reset(); - Eigen::Matrix contA = - NumericalJacobianX( - m_f, m_xHat, Eigen::Vector::Zero()); - Eigen::Matrix C = - NumericalJacobianX( - m_h, m_xHat, Eigen::Vector::Zero()); + StateMatrix contA = NumericalJacobianX( + m_f, m_xHat, InputVector::Zero()); + Matrixd C = NumericalJacobianX( + m_h, m_xHat, InputVector::Zero()); - Eigen::Matrix discA; - Eigen::Matrix discQ; + StateMatrix discA; + StateMatrix discQ; DiscretizeAQTaylor(contA, m_contQ, dt, &discA, &discQ); - Eigen::Matrix discR = - DiscretizeR(m_contR, dt); + Matrixd discR = DiscretizeR(m_contR, dt); if (IsDetectable(discA, C) && Outputs <= States) { m_initP = drake::math::DiscreteAlgebraicRiccatiEquation( discA.transpose(), C.transpose(), discQ, discR); } else { - m_initP = Eigen::Matrix::Zero(); + m_initP = StateMatrix::Zero(); } m_P = m_initP; } template void ExtendedKalmanFilter::Predict( - const Eigen::Vector& u, units::second_t dt) { + const InputVector& u, units::second_t dt) { // Find continuous A - Eigen::Matrix contA = + StateMatrix contA = NumericalJacobianX(m_f, m_xHat, u); // Find discrete A and Q - Eigen::Matrix discA; - Eigen::Matrix discQ; + StateMatrix discA; + StateMatrix discQ; DiscretizeAQTaylor(contA, m_contQ, dt, &discA, &discQ); m_xHat = RK4(m_f, m_xHat, u, dt); @@ -136,44 +108,29 @@ void ExtendedKalmanFilter::Predict( template template void ExtendedKalmanFilter::Correct( - const Eigen::Vector& u, - const Eigen::Vector& y, - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> - h, - const Eigen::Matrix& R) { - auto residualFuncY = [](auto a, auto b) -> Eigen::Vector { - return a - b; - }; - auto addFuncX = [](auto a, auto b) -> Eigen::Vector { - return a + b; - }; + const InputVector& u, const Vectord& y, + std::function(const StateVector&, const InputVector&)> h, + const Matrixd& R) { + auto residualFuncY = [](auto a, auto b) -> Vectord { return a - b; }; + auto addFuncX = [](auto a, auto b) -> StateVector { return a + b; }; Correct(u, y, h, R, residualFuncY, addFuncX); } template template void ExtendedKalmanFilter::Correct( - const Eigen::Vector& u, - const Eigen::Vector& y, - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> - h, - const Eigen::Matrix& R, - std::function( - const Eigen::Vector&, const Eigen::Vector&)> + const InputVector& u, const Vectord& y, + std::function(const StateVector&, const InputVector&)> h, + const Matrixd& R, + std::function(const Vectord&, const Vectord&)> residualFuncY, - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector)> + std::function addFuncX) { - const Eigen::Matrix C = + const Matrixd C = NumericalJacobianX(h, m_xHat, u); - const Eigen::Matrix discR = DiscretizeR(R, m_dt); + const Matrixd discR = DiscretizeR(R, m_dt); - Eigen::Matrix S = C * m_P * C.transpose() + discR; + Matrixd S = C * m_P * C.transpose() + discR; // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more // efficiently. @@ -187,7 +144,7 @@ void ExtendedKalmanFilter::Correct( // // Kᵀ = Sᵀ.solve(CPᵀ) // K = (Sᵀ.solve(CPᵀ))ᵀ - Eigen::Matrix K = + Matrixd K = S.transpose().ldlt().solve(C * m_P.transpose()).transpose(); // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + Kₖ₊₁(y − h(x̂ₖ₊₁⁻, uₖ₊₁)) @@ -195,9 +152,8 @@ void ExtendedKalmanFilter::Correct( // Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ // Use Joseph form for numerical stability - m_P = (Eigen::Matrix::Identity() - K * C) * m_P * - (Eigen::Matrix::Identity() - K * C) - .transpose() + + m_P = (StateMatrix::Identity() - K * C) * m_P * + (StateMatrix::Identity() - K * C).transpose() + K * discR * K.transpose(); } diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h index dd0abc3edf..aa8db1e745 100644 --- a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h @@ -7,7 +7,7 @@ #include #include -#include "Eigen/Core" +#include "frc/EigenCore.h" #include "frc/system/LinearSystem.h" #include "units/time.h" @@ -36,6 +36,13 @@ namespace frc { template class KalmanFilter { public: + using StateVector = Vectord; + using InputVector = Vectord; + using OutputVector = Vectord; + + using StateArray = wpi::array; + using OutputArray = wpi::array; + /** * Constructs a state-space observer with the given plant. * @@ -45,9 +52,8 @@ class KalmanFilter { * @param dt Nominal discretization timestep. */ KalmanFilter(LinearSystem& plant, - const wpi::array& stateStdDevs, - const wpi::array& measurementStdDevs, - units::second_t dt); + const StateArray& stateStdDevs, + const OutputArray& measurementStdDevs, units::second_t dt); KalmanFilter(KalmanFilter&&) = default; KalmanFilter& operator=(KalmanFilter&&) = default; @@ -55,7 +61,7 @@ class KalmanFilter { /** * Returns the steady-state Kalman gain matrix K. */ - const Eigen::Matrix& K() const { return m_K; } + const Matrixd& K() const { return m_K; } /** * Returns an element of the steady-state Kalman gain matrix K. @@ -68,7 +74,7 @@ class KalmanFilter { /** * Returns the state estimate x-hat. */ - const Eigen::Vector& Xhat() const { return m_xHat; } + const StateVector& Xhat() const { return m_xHat; } /** * Returns an element of the state estimate x-hat. @@ -82,7 +88,7 @@ class KalmanFilter { * * @param xHat The state estimate x-hat. */ - void SetXhat(const Eigen::Vector& xHat) { m_xHat = xHat; } + void SetXhat(const StateVector& xHat) { m_xHat = xHat; } /** * Set an element of the initial state estimate x-hat. @@ -103,7 +109,7 @@ class KalmanFilter { * @param u New control input from controller. * @param dt Timestep for prediction. */ - void Predict(const Eigen::Vector& u, units::second_t dt); + void Predict(const InputVector& u, units::second_t dt); /** * Correct the state estimate x-hat using the measurements in y. @@ -111,8 +117,7 @@ class KalmanFilter { * @param u Same control input used in the last predict step. * @param y Measurement vector. */ - void Correct(const Eigen::Vector& u, - const Eigen::Vector& y); + void Correct(const InputVector& u, const OutputVector& y); private: LinearSystem* m_plant; @@ -120,12 +125,12 @@ class KalmanFilter { /** * The steady-state Kalman gain matrix. */ - Eigen::Matrix m_K; + Matrixd m_K; /** * The state estimate. */ - Eigen::Vector m_xHat; + StateVector m_xHat; }; extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT) diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc index df5fb91baa..81f2cc0467 100644 --- a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc +++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc @@ -21,15 +21,15 @@ namespace frc { template KalmanFilter::KalmanFilter( LinearSystem& plant, - const wpi::array& stateStdDevs, - const wpi::array& measurementStdDevs, units::second_t dt) { + const StateArray& stateStdDevs, const OutputArray& measurementStdDevs, + units::second_t dt) { m_plant = &plant; auto contQ = MakeCovMatrix(stateStdDevs); auto contR = MakeCovMatrix(measurementStdDevs); - Eigen::Matrix discA; - Eigen::Matrix discQ; + Matrixd discA; + Matrixd discQ; DiscretizeAQTaylor(plant.A(), contQ, dt, &discA, &discQ); auto discR = DiscretizeR(contR, dt); @@ -46,12 +46,11 @@ KalmanFilter::KalmanFilter( throw std::invalid_argument(msg); } - Eigen::Matrix P = - drake::math::DiscreteAlgebraicRiccatiEquation( - discA.transpose(), C.transpose(), discQ, discR); + Matrixd P = drake::math::DiscreteAlgebraicRiccatiEquation( + discA.transpose(), C.transpose(), discQ, discR); // S = CPCᵀ + R - Eigen::Matrix S = C * P * C.transpose() + discR; + Matrixd S = C * P * C.transpose() + discR; // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more // efficiently. @@ -71,15 +70,14 @@ KalmanFilter::KalmanFilter( } template -void KalmanFilter::Predict( - const Eigen::Vector& u, units::second_t dt) { +void KalmanFilter::Predict(const InputVector& u, + units::second_t dt) { m_xHat = m_plant->CalculateX(m_xHat, u, dt); } template -void KalmanFilter::Correct( - const Eigen::Vector& u, - const Eigen::Vector& y) { +void KalmanFilter::Correct(const InputVector& u, + const OutputVector& y) { // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁)) m_xHat += m_K * (y - (m_plant->C() * m_xHat + m_plant->D() * u)); } diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h b/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h index 5cb8eb2ea1..9156efa66f 100644 --- a/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h +++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h @@ -10,7 +10,7 @@ #include #include -#include "Eigen/Core" +#include "frc/EigenCore.h" #include "units/math.h" #include "units/time.h" @@ -20,14 +20,13 @@ template class KalmanFilterLatencyCompensator { public: struct ObserverSnapshot { - Eigen::Vector xHat; - Eigen::Matrix errorCovariances; - Eigen::Vector inputs; - Eigen::Vector localMeasurements; + Vectord xHat; + Matrixd errorCovariances; + Vectord inputs; + Vectord localMeasurements; - ObserverSnapshot(const KalmanFilterType& observer, - const Eigen::Vector& u, - const Eigen::Vector& localY) + ObserverSnapshot(const KalmanFilterType& observer, const Vectord& u, + const Vectord& localY) : xHat(observer.Xhat()), errorCovariances(observer.P()), inputs(u), @@ -47,10 +46,8 @@ class KalmanFilterLatencyCompensator { * @param localY The local output at the timestamp * @param timestamp The timesnap of the state. */ - void AddObserverState(const KalmanFilterType& observer, - Eigen::Vector u, - Eigen::Vector localY, - units::second_t timestamp) { + void AddObserverState(const KalmanFilterType& observer, Vectord u, + Vectord localY, units::second_t timestamp) { // Add the new state into the vector. m_pastObserverSnapshots.emplace_back(timestamp, ObserverSnapshot{observer, u, localY}); @@ -74,10 +71,8 @@ class KalmanFilterLatencyCompensator { */ template void ApplyPastGlobalMeasurement( - KalmanFilterType* observer, units::second_t nominalDt, - Eigen::Vector y, - std::function& u, - const Eigen::Vector& y)> + KalmanFilterType* observer, units::second_t nominalDt, Vectord y, + std::function& u, const Vectord& y)> globalMeasurementCorrect, units::second_t timestamp) { if (m_pastObserverSnapshots.size() == 0) { diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h index 5b3b139c94..eb8219cfd1 100644 --- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h @@ -9,7 +9,7 @@ #include #include -#include "Eigen/Core" +#include "frc/EigenCore.h" #include "frc/estimator/UnscentedKalmanFilter.h" #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation2d.h" @@ -213,9 +213,7 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator { UnscentedKalmanFilter<3, 3, 1> m_observer; MecanumDriveKinematics m_kinematics; TimeInterpolatableBuffer m_poseBuffer{1.5_s}; - std::function& u, - const Eigen::Vector& y)> - m_visionCorrect; + std::function& u, const Vectord<3>& y)> m_visionCorrect; Eigen::Matrix3d m_visionContR; @@ -227,7 +225,7 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator { template static wpi::array StdDevMatrixToArray( - const Eigen::Vector& vector) { + const Vectord& vector) { wpi::array array; for (size_t i = 0; i < Dim; ++i) { array[i] = vector(i); diff --git a/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h b/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h index 42f5593cc3..1c46834041 100644 --- a/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h +++ b/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h @@ -7,7 +7,7 @@ #include #include "Eigen/Cholesky" -#include "Eigen/Core" +#include "frc/EigenCore.h" namespace frc { @@ -62,14 +62,12 @@ class MerweScaledSigmaPoints { * Xi_0, Xi_{1..n}, Xi_{n+1..2n}. * */ - Eigen::Matrix SigmaPoints( - const Eigen::Vector& x, - const Eigen::Matrix& P) { + Matrixd SigmaPoints( + const Vectord& x, const Matrixd& P) { double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States; - Eigen::Matrix U = - ((lambda + States) * P).llt().matrixL(); + Matrixd U = ((lambda + States) * P).llt().matrixL(); - Eigen::Matrix sigmas; + Matrixd sigmas; sigmas.template block(0, 0) = x; for (int k = 0; k < States; ++k) { sigmas.template block(0, k + 1) = @@ -84,7 +82,7 @@ class MerweScaledSigmaPoints { /** * Returns the weight for each sigma point for the mean. */ - const Eigen::Vector& Wm() const { return m_Wm; } + const Vectord<2 * States + 1>& Wm() const { return m_Wm; } /** * Returns an element of the weight for each sigma point for the mean. @@ -96,7 +94,7 @@ class MerweScaledSigmaPoints { /** * Returns the weight for each sigma point for the covariance. */ - const Eigen::Vector& Wc() const { return m_Wc; } + const Vectord<2 * States + 1>& Wc() const { return m_Wc; } /** * Returns an element of the weight for each sigma point for the covariance. @@ -106,8 +104,8 @@ class MerweScaledSigmaPoints { double Wc(int i) const { return m_Wc(i, 0); } private: - Eigen::Vector m_Wm; - Eigen::Vector m_Wc; + Vectord<2 * States + 1> m_Wm; + Vectord<2 * States + 1> m_Wc; double m_alpha; int m_kappa; @@ -120,8 +118,8 @@ class MerweScaledSigmaPoints { double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States; double c = 0.5 / (States + lambda); - m_Wm = Eigen::Vector::Constant(c); - m_Wc = Eigen::Vector::Constant(c); + m_Wm = Vectord<2 * States + 1>::Constant(c); + m_Wc = Vectord<2 * States + 1>::Constant(c); m_Wm(0) = lambda / (States + lambda); m_Wc(0) = lambda / (States + lambda) + (1 - std::pow(m_alpha, 2) + beta); diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index 16f8439ce8..2fe8ddf037 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -10,7 +10,7 @@ #include #include -#include "Eigen/Core" +#include "frc/EigenCore.h" #include "frc/StateSpaceUtil.h" #include "frc/estimator/AngleStatistics.h" #include "frc/estimator/UnscentedKalmanFilter.h" @@ -83,10 +83,8 @@ class SwerveDrivePoseEstimator { const wpi::array& localMeasurementStdDevs, const wpi::array& visionMeasurementStdDevs, units::second_t nominalDt = 0.02_s) - : m_observer([](const Eigen::Vector& x, - const Eigen::Vector& u) { return u; }, - [](const Eigen::Vector& x, - const Eigen::Vector& u) { + : m_observer([](const Vectord<3>& x, const Vectord<3>& u) { return u; }, + [](const Vectord<3>& x, const Vectord<3>& u) { return x.block<1, 1>(2, 0); }, stateStdDevs, localMeasurementStdDevs, @@ -98,12 +96,9 @@ class SwerveDrivePoseEstimator { SetVisionMeasurementStdDevs(visionMeasurementStdDevs); // Create correction mechanism for vision measurements. - m_visionCorrect = [&](const Eigen::Vector& u, - const Eigen::Vector& y) { + m_visionCorrect = [&](const Vectord<3>& u, const Vectord<3>& y) { m_observer.Correct<3>( - u, y, - [](const Eigen::Vector& x, - const Eigen::Vector& u) { return x; }, + u, y, [](const Vectord<3>& x, const Vectord<3>& u) { return x; }, m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2), frc::AngleResidual<3>(2), frc::AngleAdd<3>(2)); }; @@ -190,7 +185,7 @@ class SwerveDrivePoseEstimator { void AddVisionMeasurement(const Pose2d& visionRobotPose, units::second_t timestamp) { if (auto sample = m_poseBuffer.Sample(timestamp)) { - m_visionCorrect(Eigen::Vector::Zero(), + m_visionCorrect(Vectord<3>::Zero(), PoseTo3dVector(GetEstimatedPosition().TransformBy( visionRobotPose - sample.value()))); } @@ -280,10 +275,10 @@ class SwerveDrivePoseEstimator { Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s) .RotateBy(angle); - Eigen::Vector u{fieldRelativeSpeeds.X().value(), - fieldRelativeSpeeds.Y().value(), omega.value()}; + Vectord<3> u{fieldRelativeSpeeds.X().value(), + fieldRelativeSpeeds.Y().value(), omega.value()}; - Eigen::Vector localY{angle.Radians().value()}; + Vectord<1> localY{angle.Radians().value()}; m_previousAngle = angle; m_poseBuffer.AddSample(currentTime, GetEstimatedPosition()); @@ -298,9 +293,7 @@ class SwerveDrivePoseEstimator { UnscentedKalmanFilter<3, 3, 1> m_observer; SwerveDriveKinematics& m_kinematics; TimeInterpolatableBuffer m_poseBuffer{1.5_s}; - std::function& u, - const Eigen::Vector& y)> - m_visionCorrect; + std::function& u, const Vectord<3>& y)> m_visionCorrect; Eigen::Matrix3d m_visionContR; @@ -312,7 +305,7 @@ class SwerveDrivePoseEstimator { template static wpi::array StdDevMatrixToArray( - const Eigen::Vector& vector) { + const Vectord& vector) { wpi::array array; for (size_t i = 0; i < Dim; ++i) { array[i] = vector(i); diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h index 4b20d32a26..8a434578ef 100644 --- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h @@ -9,7 +9,7 @@ #include #include -#include "Eigen/Core" +#include "frc/EigenCore.h" #include "frc/estimator/MerweScaledSigmaPoints.h" #include "units/time.h" @@ -42,6 +42,15 @@ namespace frc { template class UnscentedKalmanFilter { public: + using StateVector = Vectord; + using InputVector = Vectord; + using OutputVector = Vectord; + + using StateArray = wpi::array; + using OutputArray = wpi::array; + + using StateMatrix = Matrixd; + /** * Constructs an unscented Kalman filter. * @@ -53,17 +62,11 @@ class UnscentedKalmanFilter { * @param measurementStdDevs Standard deviations of measurements. * @param dt Nominal discretization timestep. */ - UnscentedKalmanFilter(std::function( - const Eigen::Vector&, - const Eigen::Vector&)> - f, - std::function( - const Eigen::Vector&, - const Eigen::Vector&)> - h, - const wpi::array& stateStdDevs, - const wpi::array& measurementStdDevs, - units::second_t dt); + UnscentedKalmanFilter( + std::function f, + std::function h, + const StateArray& stateStdDevs, const OutputArray& measurementStdDevs, + units::second_t dt); /** * Constructs an unscented Kalman filter with custom mean, residual, and @@ -90,42 +93,27 @@ class UnscentedKalmanFilter { * @param dt Nominal discretization timestep. */ UnscentedKalmanFilter( - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> - f, - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> - h, - const wpi::array& stateStdDevs, - const wpi::array& measurementStdDevs, - std::function( - const Eigen::Matrix&, - const Eigen::Vector&)> + std::function f, + std::function h, + const StateArray& stateStdDevs, const OutputArray& measurementStdDevs, + std::function&, + const Vectord<2 * States + 1>&)> meanFuncX, - std::function( - const Eigen::Matrix&, - const Eigen::Vector&)> + std::function&, + const Vectord<2 * States + 1>&)> meanFuncY, - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> + std::function residualFuncX, - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> + std::function residualFuncY, - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> + std::function addFuncX, units::second_t dt); /** * Returns the error covariance matrix P. */ - const Eigen::Matrix& P() const { return m_P; } + const StateMatrix& P() const { return m_P; } /** * Returns an element of the error covariance matrix P. @@ -140,12 +128,12 @@ class UnscentedKalmanFilter { * * @param P The error covariance matrix P. */ - void SetP(const Eigen::Matrix& P) { m_P = P; } + void SetP(const StateMatrix& P) { m_P = P; } /** * Returns the state estimate x-hat. */ - const Eigen::Vector& Xhat() const { return m_xHat; } + const StateVector& Xhat() const { return m_xHat; } /** * Returns an element of the state estimate x-hat. @@ -159,7 +147,7 @@ class UnscentedKalmanFilter { * * @param xHat The state estimate x-hat. */ - void SetXhat(const Eigen::Vector& xHat) { m_xHat = xHat; } + void SetXhat(const StateVector& xHat) { m_xHat = xHat; } /** * Set an element of the initial state estimate x-hat. @@ -184,7 +172,7 @@ class UnscentedKalmanFilter { * @param u New control input from controller. * @param dt Timestep for prediction. */ - void Predict(const Eigen::Vector& u, units::second_t dt); + void Predict(const InputVector& u, units::second_t dt); /** * Correct the state estimate x-hat using the measurements in y. @@ -192,8 +180,7 @@ class UnscentedKalmanFilter { * @param u Same control input used in the predict step. * @param y Measurement vector. */ - void Correct(const Eigen::Vector& u, - const Eigen::Vector& y) { + void Correct(const InputVector& u, const OutputVector& y) { Correct(u, y, m_h, m_contR, m_meanFuncY, m_residualFuncY, m_residualFuncX, m_addFuncX); } @@ -212,13 +199,10 @@ class UnscentedKalmanFilter { * @param R Measurement noise covariance matrix (continuous-time). */ template - void Correct(const Eigen::Vector& u, - const Eigen::Vector& y, - std::function( - const Eigen::Vector&, - const Eigen::Vector&)> - h, - const Eigen::Matrix& R); + void Correct( + const InputVector& u, const Vectord& y, + std::function(const StateVector&, const InputVector&)> h, + const Matrixd& R); /** * Correct the state estimate x-hat using the measurements in y. @@ -241,64 +225,39 @@ class UnscentedKalmanFilter { * @param addFuncX A function that adds two state vectors. */ template - void Correct(const Eigen::Vector& u, - const Eigen::Vector& y, - std::function( - const Eigen::Vector&, - const Eigen::Vector&)> - h, - const Eigen::Matrix& R, - std::function( - const Eigen::Matrix&, - const Eigen::Vector&)> - meanFuncY, - std::function( - const Eigen::Vector&, - const Eigen::Vector&)> - residualFuncY, - std::function( - const Eigen::Vector&, - const Eigen::Vector&)> - residualFuncX, - std::function( - const Eigen::Vector&, - const Eigen::Vector)> - addFuncX); + void Correct( + const InputVector& u, const Vectord& y, + std::function(const StateVector&, const InputVector&)> h, + const Matrixd& R, + std::function(const Matrixd&, + const Vectord<2 * States + 1>&)> + meanFuncY, + std::function(const Vectord&, const Vectord&)> + residualFuncY, + std::function + residualFuncX, + std::function + addFuncX); private: - std::function( - const Eigen::Vector&, - const Eigen::Vector&)> - m_f; - std::function( - const Eigen::Vector&, - const Eigen::Vector&)> - m_h; - std::function( - const Eigen::Matrix&, - const Eigen::Vector&)> + std::function m_f; + std::function m_h; + std::function&, + const Vectord<2 * States + 1>&)> m_meanFuncX; - std::function( - const Eigen::Matrix&, - const Eigen::Vector&)> + std::function&, + const Vectord<2 * States + 1>&)> m_meanFuncY; - std::function( - const Eigen::Vector&, - const Eigen::Vector&)> + std::function m_residualFuncX; - std::function( - const Eigen::Vector&, - const Eigen::Vector)> + std::function m_residualFuncY; - std::function( - const Eigen::Vector&, - const Eigen::Vector)> - m_addFuncX; - Eigen::Vector m_xHat; - Eigen::Matrix m_P; - Eigen::Matrix m_contQ; - Eigen::Matrix m_contR; - Eigen::Matrix m_sigmasF; + std::function m_addFuncX; + StateVector m_xHat; + StateMatrix m_P; + StateMatrix m_contQ; + Matrixd m_contR; + Matrixd m_sigmasF; units::second_t m_dt; MerweScaledSigmaPoints m_pts; diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc index 55ead68d8d..b2ba677cf2 100644 --- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc +++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc @@ -16,34 +16,20 @@ namespace frc { template UnscentedKalmanFilter::UnscentedKalmanFilter( - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> - f, - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> - h, - const wpi::array& stateStdDevs, - const wpi::array& measurementStdDevs, units::second_t dt) + std::function f, + std::function h, + const StateArray& stateStdDevs, const OutputArray& measurementStdDevs, + units::second_t dt) : m_f(f), m_h(h) { m_contQ = MakeCovMatrix(stateStdDevs); m_contR = MakeCovMatrix(measurementStdDevs); - m_meanFuncX = [](auto sigmas, auto Wm) -> Eigen::Vector { - return sigmas * Wm; - }; - m_meanFuncY = [](auto sigmas, auto Wc) -> Eigen::Vector { + m_meanFuncX = [](auto sigmas, auto Wm) -> StateVector { return sigmas * Wm; }; + m_meanFuncY = [](auto sigmas, auto Wc) -> OutputVector { return sigmas * Wc; }; - m_residualFuncX = [](auto a, auto b) -> Eigen::Vector { - return a - b; - }; - m_residualFuncY = [](auto a, auto b) -> Eigen::Vector { - return a - b; - }; - m_addFuncX = [](auto a, auto b) -> Eigen::Vector { - return a + b; - }; + m_residualFuncX = [](auto a, auto b) -> StateVector { return a - b; }; + m_residualFuncY = [](auto a, auto b) -> OutputVector { return a - b; }; + m_addFuncX = [](auto a, auto b) -> StateVector { return a + b; }; m_dt = dt; Reset(); @@ -51,36 +37,20 @@ UnscentedKalmanFilter::UnscentedKalmanFilter( template UnscentedKalmanFilter::UnscentedKalmanFilter( - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> - f, - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> - h, - const wpi::array& stateStdDevs, - const wpi::array& measurementStdDevs, - std::function( - const Eigen::Matrix&, - const Eigen::Vector&)> + std::function f, + std::function h, + const StateArray& stateStdDevs, const OutputArray& measurementStdDevs, + std::function&, + const Vectord<2 * States + 1>&)> meanFuncX, - std::function( - const Eigen::Matrix&, - const Eigen::Vector&)> + std::function&, + const Vectord<2 * States + 1>&)> meanFuncY, - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> + std::function residualFuncX, - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> + std::function residualFuncY, - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> - addFuncX, + std::function addFuncX, units::second_t dt) : m_f(f), m_h(h), @@ -98,21 +68,20 @@ UnscentedKalmanFilter::UnscentedKalmanFilter( template void UnscentedKalmanFilter::Predict( - const Eigen::Vector& u, units::second_t dt) { + const InputVector& u, units::second_t dt) { m_dt = dt; // Discretize Q before projecting mean and covariance forward - Eigen::Matrix contA = + StateMatrix contA = NumericalJacobianX(m_f, m_xHat, u); - Eigen::Matrix discA; - Eigen::Matrix discQ; + StateMatrix discA; + StateMatrix discQ; DiscretizeAQTaylor(contA, m_contQ, dt, &discA, &discQ); - Eigen::Matrix sigmas = - m_pts.SigmaPoints(m_xHat, m_P); + Matrixd sigmas = m_pts.SigmaPoints(m_xHat, m_P); for (int i = 0; i < m_pts.NumSigmas(); ++i) { - Eigen::Vector x = sigmas.template block(0, i); + StateVector x = sigmas.template block(0, i); m_sigmasF.template block(0, i) = RK4(m_f, x, u, dt); } @@ -127,59 +96,38 @@ void UnscentedKalmanFilter::Predict( template template void UnscentedKalmanFilter::Correct( - const Eigen::Vector& u, - const Eigen::Vector& y, - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> - h, - const Eigen::Matrix& R) { - auto meanFuncY = [](auto sigmas, auto Wc) -> Eigen::Vector { + const InputVector& u, const Vectord& y, + std::function(const StateVector&, const InputVector&)> h, + const Matrixd& R) { + auto meanFuncY = [](auto sigmas, auto Wc) -> Vectord { return sigmas * Wc; }; - auto residualFuncX = [](auto a, auto b) -> Eigen::Vector { - return a - b; - }; - auto residualFuncY = [](auto a, auto b) -> Eigen::Vector { - return a - b; - }; - auto addFuncX = [](auto a, auto b) -> Eigen::Vector { - return a + b; - }; + auto residualFuncX = [](auto a, auto b) -> StateVector { return a - b; }; + auto residualFuncY = [](auto a, auto b) -> Vectord { return a - b; }; + auto addFuncX = [](auto a, auto b) -> StateVector { return a + b; }; Correct(u, y, h, R, meanFuncY, residualFuncY, residualFuncX, addFuncX); } template template void UnscentedKalmanFilter::Correct( - const Eigen::Vector& u, - const Eigen::Vector& y, - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> - h, - const Eigen::Matrix& R, - std::function( - const Eigen::Matrix&, - const Eigen::Vector&)> + const InputVector& u, const Vectord& y, + std::function(const StateVector&, const InputVector&)> h, + const Matrixd& R, + std::function(const Matrixd&, + const Vectord<2 * States + 1>&)> meanFuncY, - std::function( - const Eigen::Vector&, const Eigen::Vector&)> + std::function(const Vectord&, const Vectord&)> residualFuncY, - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector&)> + std::function residualFuncX, - std::function< - Eigen::Vector(const Eigen::Vector&, - const Eigen::Vector)> + std::function addFuncX) { - const Eigen::Matrix discR = DiscretizeR(R, m_dt); + const Matrixd discR = DiscretizeR(R, m_dt); // Transform sigma points into measurement space - Eigen::Matrix sigmasH; - Eigen::Matrix sigmas = - m_pts.SigmaPoints(m_xHat, m_P); + Matrixd sigmasH; + Matrixd sigmas = m_pts.SigmaPoints(m_xHat, m_P); for (int i = 0; i < m_pts.NumSigmas(); ++i) { sigmasH.template block(0, i) = h(sigmas.template block(0, i), u); @@ -191,7 +139,7 @@ void UnscentedKalmanFilter::Correct( Py += discR; // Compute cross covariance of the state and the measurements - Eigen::Matrix Pxy; + Matrixd Pxy; Pxy.setZero(); for (int i = 0; i < m_pts.NumSigmas(); ++i) { // Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i] @@ -206,7 +154,7 @@ void UnscentedKalmanFilter::Correct( // P_yᵀKᵀ = P_{xy}ᵀ // Kᵀ = P_yᵀ.solve(P_{xy}ᵀ) // K = (P_yᵀ.solve(P_{xy}ᵀ)ᵀ - Eigen::Matrix K = + Matrixd K = Py.transpose().ldlt().solve(Pxy.transpose()).transpose(); // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ) diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h index 2df0a47040..1859316909 100644 --- a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h +++ b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h @@ -6,7 +6,7 @@ #include -#include "Eigen/Core" +#include "frc/EigenCore.h" namespace frc { @@ -31,33 +31,30 @@ namespace frc { * passing through the transform. */ template -std::tuple, Eigen::Matrix> -UnscentedTransform(const Eigen::Matrix& sigmas, - const Eigen::Vector& Wm, - const Eigen::Vector& Wc, - std::function( - const Eigen::Matrix&, - const Eigen::Vector&)> - meanFunc, - std::function( - const Eigen::Vector&, - const Eigen::Vector&)> - residualFunc) { +std::tuple, Matrixd> UnscentedTransform( + const Matrixd& sigmas, + const Vectord<2 * States + 1>& Wm, const Vectord<2 * States + 1>& Wc, + std::function(const Matrixd&, + const Vectord<2 * States + 1>&)> + meanFunc, + std::function(const Vectord&, + const Vectord&)> + residualFunc) { // New mean is usually just the sum of the sigmas * weight: // n // dot = Σ W[k] Xᵢ[k] // k=1 - Eigen::Vector x = meanFunc(sigmas, Wm); + Vectord x = meanFunc(sigmas, Wm); // New covariance is the sum of the outer product of the residuals times the // weights - Eigen::Matrix y; + Matrixd y; for (int i = 0; i < 2 * States + 1; ++i) { // y[:, i] = sigmas[:, i] - x y.template block(0, i) = residualFunc(sigmas.template block(0, i), x); } - Eigen::Matrix P = + Matrixd P = y * Eigen::DiagonalMatrix(Wc) * y.transpose(); return std::make_tuple(x, P); diff --git a/wpimath/src/main/native/include/frc/filter/LinearFilter.h b/wpimath/src/main/native/include/frc/filter/LinearFilter.h index 62953271dd..7f21daa772 100644 --- a/wpimath/src/main/native/include/frc/filter/LinearFilter.h +++ b/wpimath/src/main/native/include/frc/filter/LinearFilter.h @@ -14,8 +14,8 @@ #include #include -#include "Eigen/Core" #include "Eigen/QR" +#include "frc/EigenCore.h" #include "units/time.h" #include "wpimath/MathShared.h" @@ -209,7 +209,7 @@ class LinearFilter { static_assert(Derivative < Samples, "Order of derivative must be less than number of samples."); - Eigen::Matrix S; + Matrixd S; for (int row = 0; row < Samples; ++row) { for (int col = 0; col < Samples; ++col) { S(row, col) = std::pow(stencil[col], row); @@ -217,12 +217,12 @@ class LinearFilter { } // Fill in Kronecker deltas: https://en.wikipedia.org/wiki/Kronecker_delta - Eigen::Vector d; + Vectord d; for (int i = 0; i < Samples; ++i) { d(i) = (i == Derivative) ? Factorial(Derivative) : 0.0; } - Eigen::Vector a = + Vectord a = S.householderQr().solve(d) / std::pow(period.value(), Derivative); // Reverse gains list diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h index 9a7cef9e56..ba854da270 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h @@ -6,8 +6,8 @@ #include -#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 m_inverseKinematics; - Eigen::HouseholderQR> m_forwardKinematics; + mutable Matrixd<4, 3> m_inverseKinematics; + Eigen::HouseholderQR> m_forwardKinematics; Translation2d m_frontLeftWheel; Translation2d m_frontRightWheel; Translation2d m_rearLeftWheel; diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index 3e605549e5..33e126f687 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -9,8 +9,8 @@ #include #include -#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 m_inverseKinematics; - Eigen::HouseholderQR> - m_forwardKinematics; + mutable Matrixd m_inverseKinematics; + Eigen::HouseholderQR> m_forwardKinematics; wpi::array m_modules; mutable Translation2d m_previousCoR; diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc index 7b958c1ed1..b549a87aa7 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc @@ -25,7 +25,7 @@ SwerveDriveKinematics::ToSwerveModuleStates( for (size_t i = 0; i < NumModules; i++) { // clang-format off m_inverseKinematics.template block<2, 3>(i * 2, 0) = - Eigen::Matrix{ + 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::ToSwerveModuleStates( chassisSpeeds.vy.value(), chassisSpeeds.omega.value()}; - Eigen::Matrix moduleStatesMatrix = + Matrixd moduleStateMatrix = m_inverseKinematics * chassisSpeedsVector; wpi::array 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::ToChassisSpeeds( template ChassisSpeeds SwerveDriveKinematics::ToChassisSpeeds( wpi::array moduleStates) const { - Eigen::Matrix moduleStatesMatrix; + Matrixd 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)}, diff --git a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h index 8126349d8a..771f7ab1fe 100644 --- a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h +++ b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h @@ -7,7 +7,7 @@ #include #include -#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 Coefficients() const override { - return m_coefficients; - } + Matrixd<6, 3 + 1> Coefficients() const override { return m_coefficients; } private: - Eigen::Matrix m_coefficients = - Eigen::Matrix::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 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 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; } diff --git a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h index 5ba3e2a18f..5743ee9069 100644 --- a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h +++ b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h @@ -7,7 +7,7 @@ #include #include -#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 Coefficients() const override { - return m_coefficients; - } + Matrixd<6, 6> Coefficients() const override { return m_coefficients; } private: - Eigen::Matrix m_coefficients = - Eigen::Matrix::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 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 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 ControlVectorFromArrays( - wpi::array initialVector, wpi::array finalVector) { - return Eigen::Vector{initialVector[0], initialVector[1], - initialVector[2], finalVector[0], - finalVector[1], finalVector[2]}; + static Vectord<6> ControlVectorFromArrays(wpi::array initialVector, + wpi::array finalVector) { + return Vectord<6>{initialVector[0], initialVector[1], initialVector[2], + finalVector[0], finalVector[1], finalVector[2]}; } }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/spline/Spline.h b/wpimath/src/main/native/include/frc/spline/Spline.h index 2dd248a528..8f2da6520e 100644 --- a/wpimath/src/main/native/include/frc/spline/Spline.h +++ b/wpimath/src/main/native/include/frc/spline/Spline.h @@ -9,7 +9,7 @@ #include -#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 polynomialBases; + Vectord 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 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 Coefficients() const = 0; + virtual Matrixd<6, Degree + 1> Coefficients() const = 0; /** * Converts a Translation2d into a vector that is compatible with Eigen. diff --git a/wpimath/src/main/native/include/frc/system/Discretization.h b/wpimath/src/main/native/include/frc/system/Discretization.h index 722bb5f923..8ae03f4848 100644 --- a/wpimath/src/main/native/include/frc/system/Discretization.h +++ b/wpimath/src/main/native/include/frc/system/Discretization.h @@ -4,7 +4,7 @@ #pragma once -#include "Eigen/Core" +#include "frc/EigenCore.h" #include "units/time.h" #include "unsupported/Eigen/MatrixFunctions" @@ -19,9 +19,8 @@ namespace frc { * @param discA Storage for discrete system matrix. */ template -void DiscretizeA(const Eigen::Matrix& contA, - units::second_t dt, - Eigen::Matrix* discA) { +void DiscretizeA(const Matrixd& contA, units::second_t dt, + Matrixd* discA) { *discA = (contA * dt.value()).exp(); } @@ -37,19 +36,18 @@ void DiscretizeA(const Eigen::Matrix& contA, * @param discB Storage for discrete input matrix. */ template -void DiscretizeAB(const Eigen::Matrix& contA, - const Eigen::Matrix& contB, - units::second_t dt, - Eigen::Matrix* discA, - Eigen::Matrix* discB) { +void DiscretizeAB(const Matrixd& contA, + const Matrixd& contB, units::second_t dt, + Matrixd* discA, + Matrixd* discB) { // Matrices are blocked here to minimize matrix exponentiation calculations - Eigen::Matrix Mcont; + Matrixd Mcont; Mcont.setZero(); Mcont.template block(0, 0) = contA * dt.value(); Mcont.template block(0, States) = contB * dt.value(); // Discretize A and B with the given timestep - Eigen::Matrix Mdisc = Mcont.exp(); + Matrixd Mdisc = Mcont.exp(); *discA = Mdisc.template block(0, 0); *discB = Mdisc.template block(0, States); } @@ -65,29 +63,26 @@ void DiscretizeAB(const Eigen::Matrix& contA, * @param discQ Storage for discrete process noise covariance matrix. */ template -void DiscretizeAQ(const Eigen::Matrix& contA, - const Eigen::Matrix& contQ, - units::second_t dt, - Eigen::Matrix* discA, - Eigen::Matrix* discQ) { +void DiscretizeAQ(const Matrixd& contA, + const Matrixd& contQ, units::second_t dt, + Matrixd* discA, + Matrixd* discQ) { // Make continuous Q symmetric if it isn't already - Eigen::Matrix Q = (contQ + contQ.transpose()) / 2.0; + Matrixd Q = (contQ + contQ.transpose()) / 2.0; // Set up the matrix M = [[-A, Q], [0, A.T]] - Eigen::Matrix M; + Matrixd<2 * States, 2 * States> M; M.template block(0, 0) = -contA; M.template block(0, States) = Q; M.template block(States, 0).setZero(); M.template block(States, States) = contA.transpose(); - Eigen::Matrix phi = (M * dt.value()).exp(); + Matrixd<2 * States, 2 * States> phi = (M * dt.value()).exp(); // Phi12 = phi[0:States, States:2*States] // Phi22 = phi[States:2*States, States:2*States] - Eigen::Matrix phi12 = - phi.block(0, States, States, States); - Eigen::Matrix phi22 = - phi.block(States, States, States, States); + Matrixd phi12 = phi.block(0, States, States, States); + Matrixd phi22 = phi.block(States, States, States, States); *discA = phi22.transpose(); @@ -117,21 +112,20 @@ void DiscretizeAQ(const Eigen::Matrix& contA, * @param discQ Storage for discrete process noise covariance matrix. */ template -void DiscretizeAQTaylor(const Eigen::Matrix& contA, - const Eigen::Matrix& contQ, - units::second_t dt, - Eigen::Matrix* discA, - Eigen::Matrix* discQ) { +void DiscretizeAQTaylor(const Matrixd& contA, + const Matrixd& contQ, + units::second_t dt, Matrixd* discA, + Matrixd* discQ) { // Make continuous Q symmetric if it isn't already - Eigen::Matrix Q = (contQ + contQ.transpose()) / 2.0; + Matrixd Q = (contQ + contQ.transpose()) / 2.0; - Eigen::Matrix lastTerm = Q; + Matrixd lastTerm = Q; double lastCoeff = dt.value(); // Aᵀⁿ - Eigen::Matrix Atn = contA.transpose(); + Matrixd Atn = contA.transpose(); - Eigen::Matrix phi12 = lastTerm * lastCoeff; + Matrixd phi12 = lastTerm * lastCoeff; // i = 6 i.e. 5th order should be enough precision for (int i = 2; i < 6; ++i) { @@ -159,8 +153,8 @@ void DiscretizeAQTaylor(const Eigen::Matrix& contA, * @param dt Discretization timestep. */ template -Eigen::Matrix DiscretizeR( - const Eigen::Matrix& R, units::second_t dt) { +Matrixd DiscretizeR(const Matrixd& R, + units::second_t dt) { return R / dt.value(); } diff --git a/wpimath/src/main/native/include/frc/system/LinearSystem.h b/wpimath/src/main/native/include/frc/system/LinearSystem.h index bd3ff40300..dff24338ac 100644 --- a/wpimath/src/main/native/include/frc/system/LinearSystem.h +++ b/wpimath/src/main/native/include/frc/system/LinearSystem.h @@ -8,7 +8,7 @@ #include #include -#include "Eigen/Core" +#include "frc/EigenCore.h" #include "frc/StateSpaceUtil.h" #include "frc/system/Discretization.h" #include "units/time.h" @@ -30,6 +30,10 @@ namespace frc { template class LinearSystem { public: + using StateVector = Vectord; + using InputVector = Vectord; + using OutputVector = Vectord; + /** * Constructs a discrete plant with the given continuous system coefficients. * @@ -39,10 +43,10 @@ class LinearSystem { * @param D Feedthrough matrix. * @throws std::domain_error if any matrix element isn't finite. */ - LinearSystem(const Eigen::Matrix& A, - const Eigen::Matrix& B, - const Eigen::Matrix& C, - const Eigen::Matrix& D) { + LinearSystem(const Matrixd& A, + const Matrixd& B, + const Matrixd& C, + const Matrixd& D) { if (!A.allFinite()) { throw std::domain_error( "Elements of A aren't finite. This is usually due to model " @@ -78,7 +82,7 @@ class LinearSystem { /** * Returns the system matrix A. */ - const Eigen::Matrix& A() const { return m_A; } + const Matrixd& A() const { return m_A; } /** * Returns an element of the system matrix A. @@ -91,7 +95,7 @@ class LinearSystem { /** * Returns the input matrix B. */ - const Eigen::Matrix& B() const { return m_B; } + const Matrixd& B() const { return m_B; } /** * Returns an element of the input matrix B. @@ -104,7 +108,7 @@ class LinearSystem { /** * Returns the output matrix C. */ - const Eigen::Matrix& C() const { return m_C; } + const Matrixd& C() const { return m_C; } /** * Returns an element of the output matrix C. @@ -117,7 +121,7 @@ class LinearSystem { /** * Returns the feedthrough matrix D. */ - const Eigen::Matrix& D() const { return m_D; } + const Matrixd& D() const { return m_D; } /** * Returns an element of the feedthrough matrix D. @@ -137,11 +141,10 @@ class LinearSystem { * @param clampedU The control input. * @param dt Timestep for model update. */ - Eigen::Vector CalculateX( - const Eigen::Vector& x, - const Eigen::Vector& clampedU, units::second_t dt) const { - Eigen::Matrix discA; - Eigen::Matrix discB; + StateVector CalculateX(const StateVector& x, const InputVector& clampedU, + units::second_t dt) const { + Matrixd discA; + Matrixd discB; DiscretizeAB(m_A, m_B, dt, &discA, &discB); return discA * x + discB * clampedU; @@ -156,9 +159,8 @@ class LinearSystem { * @param x The current state. * @param clampedU The control input. */ - Eigen::Vector CalculateY( - const Eigen::Vector& x, - const Eigen::Vector& clampedU) const { + OutputVector CalculateY(const StateVector& x, + const InputVector& clampedU) const { return m_C * x + m_D * clampedU; } @@ -166,22 +168,22 @@ class LinearSystem { /** * Continuous system matrix. */ - Eigen::Matrix m_A; + Matrixd m_A; /** * Continuous input matrix. */ - Eigen::Matrix m_B; + Matrixd m_B; /** * Output matrix. */ - Eigen::Matrix m_C; + Matrixd m_C; /** * Feedthrough matrix. */ - Eigen::Matrix m_D; + Matrixd m_D; }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h index a18c03bb19..dd95cf5d1b 100644 --- a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h +++ b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h @@ -4,7 +4,7 @@ #pragma once -#include "Eigen/Core" +#include "frc/EigenCore.h" #include "frc/controller/LinearPlantInversionFeedforward.h" #include "frc/controller/LinearQuadraticRegulator.h" #include "frc/estimator/KalmanFilter.h" @@ -35,6 +35,10 @@ namespace frc { template class LinearSystemLoop { public: + using StateVector = Vectord; + using InputVector = Vectord; + using OutputVector = Vectord; + /** * Constructs a state-space loop with the given plant, controller, and * observer. By default, the initial reference is all zeros. Users should @@ -53,7 +57,7 @@ class LinearSystemLoop { units::volt_t maxVoltage, units::second_t dt) : LinearSystemLoop( plant, controller, observer, - [=](const Eigen::Vector& u) { + [=](const InputVector& u) { return frc::DesaturateInputVector(u, maxVoltage.value()); }, dt) {} @@ -73,9 +77,7 @@ class LinearSystemLoop { LinearSystemLoop(LinearSystem& plant, LinearQuadraticRegulator& controller, KalmanFilter& observer, - std::function( - const Eigen::Vector&)> - clampFunction, + std::function clampFunction, units::second_t dt) : LinearSystemLoop( controller, @@ -97,11 +99,10 @@ class LinearSystemLoop { LinearQuadraticRegulator& controller, const LinearPlantInversionFeedforward& feedforward, KalmanFilter& observer, units::volt_t maxVoltage) - : LinearSystemLoop(controller, feedforward, observer, - [=](const Eigen::Vector& u) { - return frc::DesaturateInputVector( - u, maxVoltage.value()); - }) {} + : LinearSystemLoop( + controller, feedforward, observer, [=](const InputVector& u) { + return frc::DesaturateInputVector(u, maxVoltage.value()); + }) {} /** * Constructs a state-space loop with the given controller, feedforward, @@ -117,9 +118,7 @@ class LinearSystemLoop { LinearQuadraticRegulator& controller, const LinearPlantInversionFeedforward& feedforward, KalmanFilter& observer, - std::function< - Eigen::Vector(const Eigen::Vector&)> - clampFunction) + std::function clampFunction) : m_controller(&controller), m_feedforward(feedforward), m_observer(&observer), @@ -134,9 +133,7 @@ class LinearSystemLoop { /** * Returns the observer's state estimate x-hat. */ - const Eigen::Vector& Xhat() const { - return m_observer->Xhat(); - } + const StateVector& Xhat() const { return m_observer->Xhat(); } /** * Returns an element of the observer's state estimate x-hat. @@ -148,7 +145,7 @@ class LinearSystemLoop { /** * Returns the controller's next reference r. */ - const Eigen::Vector& NextR() const { return m_nextR; } + const StateVector& NextR() const { return m_nextR; } /** * Returns an element of the controller's next reference r. @@ -160,7 +157,7 @@ class LinearSystemLoop { /** * Returns the controller's calculated control input u. */ - Eigen::Vector U() const { + InputVector U() const { return ClampInput(m_controller->U() + m_feedforward.Uff()); } @@ -176,9 +173,7 @@ class LinearSystemLoop { * * @param xHat The initial state estimate x-hat. */ - void SetXhat(const Eigen::Vector& xHat) { - m_observer->SetXhat(xHat); - } + void SetXhat(const StateVector& xHat) { m_observer->SetXhat(xHat); } /** * Set an element of the initial state estimate x-hat. @@ -193,7 +188,7 @@ class LinearSystemLoop { * * @param nextR Next reference. */ - void SetNextR(const Eigen::Vector& nextR) { m_nextR = nextR; } + void SetNextR(const StateVector& nextR) { m_nextR = nextR; } /** * Return the controller used internally. @@ -225,7 +220,7 @@ class LinearSystemLoop { * * @param initialState The initial state. */ - void Reset(const Eigen::Vector& initialState) { + void Reset(const StateVector& initialState) { m_nextR.setZero(); m_controller->Reset(); m_feedforward.Reset(initialState); @@ -235,18 +230,14 @@ class LinearSystemLoop { /** * Returns difference between reference r and current state x-hat. */ - Eigen::Vector Error() const { - return m_controller->R() - m_observer->Xhat(); - } + StateVector Error() const { return m_controller->R() - m_observer->Xhat(); } /** * Correct the state estimate x-hat using the measurements in y. * * @param y Measurement vector. */ - void Correct(const Eigen::Vector& y) { - m_observer->Correct(U(), y); - } + void Correct(const OutputVector& y) { m_observer->Correct(U(), y); } /** * Sets new controller output, projects model forward, and runs observer @@ -258,7 +249,7 @@ class LinearSystemLoop { * @param dt Timestep for model update. */ void Predict(units::second_t dt) { - Eigen::Vector u = + InputVector u = ClampInput(m_controller->Calculate(m_observer->Xhat(), m_nextR) + m_feedforward.Calculate(m_nextR)); m_observer->Predict(u, dt); @@ -270,10 +261,7 @@ class LinearSystemLoop { * @param u Input vector to clamp. * @return Clamped input vector. */ - Eigen::Vector ClampInput( - const Eigen::Vector& u) const { - return m_clampFunc(u); - } + InputVector ClampInput(const InputVector& u) const { return m_clampFunc(u); } protected: LinearQuadraticRegulator* m_controller; @@ -283,12 +271,10 @@ class LinearSystemLoop { /** * Clamping function. */ - std::function( - const Eigen::Vector&)> - m_clampFunc; + std::function m_clampFunc; // Reference to go to in the next cycle (used by feedforward controller). - Eigen::Vector m_nextR; + StateVector m_nextR; // These are accessible from non-templated subclasses. static constexpr int kStates = States; diff --git a/wpimath/src/main/native/include/frc/system/NumericalJacobian.h b/wpimath/src/main/native/include/frc/system/NumericalJacobian.h index 5f1bc78de3..c4cb6959dd 100644 --- a/wpimath/src/main/native/include/frc/system/NumericalJacobian.h +++ b/wpimath/src/main/native/include/frc/system/NumericalJacobian.h @@ -4,7 +4,7 @@ #pragma once -#include "Eigen/Core" +#include "frc/EigenCore.h" namespace frc { @@ -17,16 +17,16 @@ namespace frc { * @param x Vector argument. */ template -auto NumericalJacobian(F&& f, const Eigen::Vector& x) { +auto NumericalJacobian(F&& f, const Vectord& x) { constexpr double kEpsilon = 1e-5; - Eigen::Matrix result; + Matrixd result; result.setZero(); // It's more expensive, but +- epsilon will be more accurate for (int i = 0; i < Cols; ++i) { - Eigen::Vector dX_plus = x; + Vectord dX_plus = x; dX_plus(i) += kEpsilon; - Eigen::Vector dX_minus = x; + Vectord dX_minus = x; dX_minus(i) -= kEpsilon; result.col(i) = (f(dX_plus) - f(dX_minus)) / (kEpsilon * 2.0); } @@ -48,12 +48,10 @@ auto NumericalJacobian(F&& f, const Eigen::Vector& x) { * @param args Remaining arguments to f(x, u, ...). */ template -auto NumericalJacobianX(F&& f, const Eigen::Vector& x, - const Eigen::Vector& u, - Args&&... args) { +auto NumericalJacobianX(F&& f, const Vectord& x, + const Vectord& u, Args&&... args) { return NumericalJacobian( - [&](const Eigen::Vector& x) { return f(x, u, args...); }, - x); + [&](const Vectord& x) { return f(x, u, args...); }, x); } /** @@ -70,12 +68,10 @@ auto NumericalJacobianX(F&& f, const Eigen::Vector& x, * @param args Remaining arguments to f(x, u, ...). */ template -auto NumericalJacobianU(F&& f, const Eigen::Vector& x, - const Eigen::Vector& u, - Args&&... args) { +auto NumericalJacobianU(F&& f, const Vectord& x, + const Vectord& u, Args&&... args) { return NumericalJacobian( - [&](const Eigen::Vector& u) { return f(x, u, args...); }, - u); + [&](const Vectord& u) { return f(x, u, args...); }, u); } } // namespace frc diff --git a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h index 26142bf889..9c9768b570 100644 --- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h +++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h @@ -56,15 +56,13 @@ class WPILIB_DLLEXPORT LinearSystemId { throw std::domain_error("G must be greater than zero."); } - Eigen::Matrix A{ - {0.0, 1.0}, - {0.0, (-std::pow(G, 2) * motor.Kt / - (motor.R * units::math::pow<2>(r) * m * motor.Kv)) - .value()}}; - Eigen::Matrix B{0.0, - (G * motor.Kt / (motor.R * r * m)).value()}; - Eigen::Matrix C{1.0, 0.0}; - Eigen::Matrix D{0.0}; + Matrixd<2, 2> A{{0.0, 1.0}, + {0.0, (-std::pow(G, 2) * motor.Kt / + (motor.R * units::math::pow<2>(r) * m * motor.Kv)) + .value()}}; + Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * r * m)).value()}; + Matrixd<1, 2> C{1.0, 0.0}; + Matrixd<1, 1> D{0.0}; return LinearSystem<2, 1, 1>(A, B, C, D); } @@ -90,12 +88,12 @@ class WPILIB_DLLEXPORT LinearSystemId { throw std::domain_error("G must be greater than zero."); } - Eigen::Matrix A{ + Matrixd<2, 2> A{ {0.0, 1.0}, {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}}; - Eigen::Matrix B{0.0, (G * motor.Kt / (motor.R * J)).value()}; - Eigen::Matrix C{1.0, 0.0}; - Eigen::Matrix D{0.0}; + Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()}; + Matrixd<1, 2> C{1.0, 0.0}; + Matrixd<1, 1> D{0.0}; return LinearSystem<2, 1, 1>(A, B, C, D); } @@ -134,10 +132,10 @@ class WPILIB_DLLEXPORT LinearSystemId { throw std::domain_error("Ka must be greater than zero."); } - Eigen::Matrix A{-kV.value() / kA.value()}; - Eigen::Matrix B{1.0 / kA.value()}; - Eigen::Matrix C{1.0}; - Eigen::Matrix D{0.0}; + Matrixd<1, 1> A{-kV.value() / kA.value()}; + Matrixd<1, 1> B{1.0 / kA.value()}; + Matrixd<1, 1> C{1.0}; + Matrixd<1, 1> D{0.0}; return LinearSystem<1, 1, 1>(A, B, C, D); } @@ -176,10 +174,10 @@ class WPILIB_DLLEXPORT LinearSystemId { throw std::domain_error("Ka must be greater than zero."); } - Eigen::Matrix A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}}; - Eigen::Matrix B{0.0, 1.0 / kA.value()}; - Eigen::Matrix C{1.0, 0.0}; - Eigen::Matrix D{0.0}; + Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}}; + Matrixd<2, 1> B{0.0, 1.0 / kA.value()}; + Matrixd<1, 2> C{1.0, 0.0}; + Matrixd<1, 1> D{0.0}; return LinearSystem<2, 1, 1>(A, B, C, D); } @@ -224,12 +222,10 @@ class WPILIB_DLLEXPORT LinearSystemId { double B1 = 1.0 / kAlinear.value() + 1.0 / kAangular.value(); double B2 = 1.0 / kAlinear.value() - 1.0 / kAangular.value(); - Eigen::Matrix A = - 0.5 * Eigen::Matrix{{A1, A2}, {A2, A1}}; - Eigen::Matrix B = - 0.5 * Eigen::Matrix{{B1, B2}, {B2, B1}}; - Eigen::Matrix C{{1.0, 0.0}, {0.0, 1.0}}; - Eigen::Matrix D{{0.0, 0.0}, {0.0, 0.0}}; + Matrixd<2, 2> A = 0.5 * Matrixd<2, 2>{{A1, A2}, {A2, A1}}; + Matrixd<2, 2> B = 0.5 * Matrixd<2, 2>{{B1, B2}, {B2, B1}}; + Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; + Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}}; return LinearSystem<2, 2, 2>(A, B, C, D); } @@ -311,11 +307,11 @@ class WPILIB_DLLEXPORT LinearSystemId { throw std::domain_error("G must be greater than zero."); } - Eigen::Matrix A{ + Matrixd<1, 1> A{ (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}; - Eigen::Matrix B{(G * motor.Kt / (motor.R * J)).value()}; - Eigen::Matrix C{1.0}; - Eigen::Matrix D{0.0}; + Matrixd<1, 1> B{(G * motor.Kt / (motor.R * J)).value()}; + Matrixd<1, 1> C{1.0}; + Matrixd<1, 1> D{0.0}; return LinearSystem<1, 1, 1>(A, B, C, D); } @@ -342,12 +338,12 @@ class WPILIB_DLLEXPORT LinearSystemId { throw std::domain_error("G must be greater than zero."); } - Eigen::Matrix A{ + Matrixd<2, 2> A{ {0.0, 1.0}, {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}}; - Eigen::Matrix B{0.0, (G * motor.Kt / (motor.R * J)).value()}; - Eigen::Matrix C{{1.0, 0.0}, {0.0, 1.0}}; - Eigen::Matrix D{0.0, 0.0}; + Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()}; + Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; + Matrixd<2, 1> D{0.0, 0.0}; return LinearSystem<2, 1, 2>(A, B, C, D); } @@ -391,18 +387,16 @@ class WPILIB_DLLEXPORT LinearSystemId { (motor.Kv * motor.R * units::math::pow<2>(r)); auto C2 = G * motor.Kt / (motor.R * r); - Eigen::Matrix A{ - {((1 / m + units::math::pow<2>(rb) / J) * C1).value(), - ((1 / m - units::math::pow<2>(rb) / J) * C1).value()}, - {((1 / m - units::math::pow<2>(rb) / J) * C1).value(), - ((1 / m + units::math::pow<2>(rb) / J) * C1).value()}}; - Eigen::Matrix B{ - {((1 / m + units::math::pow<2>(rb) / J) * C2).value(), - ((1 / m - units::math::pow<2>(rb) / J) * C2).value()}, - {((1 / m - units::math::pow<2>(rb) / J) * C2).value(), - ((1 / m + units::math::pow<2>(rb) / J) * C2).value()}}; - Eigen::Matrix C{{1.0, 0.0}, {0.0, 1.0}}; - Eigen::Matrix D{{0.0, 0.0}, {0.0, 0.0}}; + Matrixd<2, 2> A{{((1 / m + units::math::pow<2>(rb) / J) * C1).value(), + ((1 / m - units::math::pow<2>(rb) / J) * C1).value()}, + {((1 / m - units::math::pow<2>(rb) / J) * C1).value(), + ((1 / m + units::math::pow<2>(rb) / J) * C1).value()}}; + Matrixd<2, 2> B{{((1 / m + units::math::pow<2>(rb) / J) * C2).value(), + ((1 / m - units::math::pow<2>(rb) / J) * C2).value()}, + {((1 / m - units::math::pow<2>(rb) / J) * C2).value(), + ((1 / m + units::math::pow<2>(rb) / J) * C2).value()}}; + Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; + Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}}; return LinearSystem<2, 2, 2>(A, B, C, D); } diff --git a/wpimath/src/test/native/cpp/EigenTest.cpp b/wpimath/src/test/native/cpp/EigenTest.cpp index c1786c3a96..03be9b3734 100644 --- a/wpimath/src/test/native/cpp/EigenTest.cpp +++ b/wpimath/src/test/native/cpp/EigenTest.cpp @@ -2,34 +2,34 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -#include "Eigen/Core" #include "Eigen/LU" +#include "frc/EigenCore.h" #include "gtest/gtest.h" TEST(EigenTest, Multiplication) { - Eigen::Matrix m1{{2, 1}, {0, 1}}; - Eigen::Matrix m2{{3, 0}, {0, 2.5}}; + frc::Matrixd<2, 2> m1{{2, 1}, {0, 1}}; + frc::Matrixd<2, 2> m2{{3, 0}, {0, 2.5}}; const auto result = m1 * m2; - Eigen::Matrix expectedResult{{6.0, 2.5}, {0.0, 2.5}}; + frc::Matrixd<2, 2> expectedResult{{6.0, 2.5}, {0.0, 2.5}}; EXPECT_TRUE(expectedResult.isApprox(result)); - Eigen::Matrix m3{{1.0, 3.0, 0.5}, {2.0, 4.3, 1.2}}; - Eigen::Matrix m4{ + frc::Matrixd<2, 3> m3{{1.0, 3.0, 0.5}, {2.0, 4.3, 1.2}}; + frc::Matrixd<3, 4> m4{ {3.0, 1.5, 2.0, 4.5}, {2.3, 1.0, 1.6, 3.1}, {5.2, 2.1, 2.0, 1.0}}; const auto result2 = m3 * m4; - Eigen::Matrix expectedResult2{{12.5, 5.55, 7.8, 14.3}, - {22.13, 9.82, 13.28, 23.53}}; + frc::Matrixd<2, 4> expectedResult2{{12.5, 5.55, 7.8, 14.3}, + {22.13, 9.82, 13.28, 23.53}}; EXPECT_TRUE(expectedResult2.isApprox(result2)); } TEST(EigenTest, Transpose) { - Eigen::Vector vec{1, 2, 3}; + frc::Vectord<3> vec{1, 2, 3}; const auto transpose = vec.transpose(); @@ -39,8 +39,7 @@ TEST(EigenTest, Transpose) { } TEST(EigenTest, Inverse) { - Eigen::Matrix mat{ - {1.0, 3.0, 2.0}, {5.0, 2.0, 1.5}, {0.0, 1.3, 2.5}}; + frc::Matrixd<3, 3> mat{{1.0, 3.0, 2.0}, {5.0, 2.0, 1.5}, {0.0, 1.3, 2.5}}; const auto inverse = mat.inverse(); const auto identity = Eigen::MatrixXd::Identity(3, 3); diff --git a/wpimath/src/test/native/cpp/StateSpaceTest.cpp b/wpimath/src/test/native/cpp/StateSpaceTest.cpp index 0dc3978b8e..ee7842cbb4 100644 --- a/wpimath/src/test/native/cpp/StateSpaceTest.cpp +++ b/wpimath/src/test/native/cpp/StateSpaceTest.cpp @@ -7,7 +7,7 @@ #include #include -#include "Eigen/Core" +#include "frc/EigenCore.h" #include "frc/controller/LinearPlantInversionFeedforward.h" #include "frc/controller/LinearQuadraticRegulator.h" #include "frc/estimator/KalmanFilter.h" @@ -45,8 +45,7 @@ class StateSpaceTest : public testing::Test { void Update(const LinearSystem<2, 1, 1>& plant, LinearSystemLoop<2, 1, 1>& loop, double noise) { - Eigen::Vector y = - plant.CalculateY(loop.Xhat(), loop.U()) + Eigen::Vector{noise}; + Vectord<1> y = plant.CalculateY(loop.Xhat(), loop.U()) + Vectord<1>{noise}; loop.Correct(y); loop.Predict(kDt); } @@ -55,7 +54,7 @@ TEST_F(StateSpaceTest, CorrectPredictLoop) { std::default_random_engine generator; std::normal_distribution dist{0.0, kPositionStddev}; - Eigen::Vector references{2.0, 0.0}; + Vectord<2> references{2.0, 0.0}; loop.SetNextR(references); for (int i = 0; i < 1000; i++) { diff --git a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp index 57b93bb42b..60efa066dc 100644 --- a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp +++ b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp @@ -6,13 +6,13 @@ #include -#include "Eigen/Core" +#include "frc/EigenCore.h" #include "frc/StateSpaceUtil.h" #include "frc/system/NumericalIntegration.h" TEST(StateSpaceUtilTest, MakeMatrix) { // Column vector - Eigen::Vector mat1 = frc::MakeMatrix<2, 1>(1.0, 2.0); + frc::Vectord<2> mat1 = frc::MakeMatrix<2, 1>(1.0, 2.0); EXPECT_NEAR(mat1(0), 1.0, 1e-3); EXPECT_NEAR(mat1(1), 2.0, 1e-3); @@ -22,15 +22,14 @@ TEST(StateSpaceUtilTest, MakeMatrix) { EXPECT_NEAR(mat2(1), 2.0, 1e-3); // Square matrix - Eigen::Matrix mat3 = frc::MakeMatrix<2, 2>(1.0, 2.0, 3.0, 4.0); + frc::Matrixd<2, 2> mat3 = frc::MakeMatrix<2, 2>(1.0, 2.0, 3.0, 4.0); EXPECT_NEAR(mat3(0, 0), 1.0, 1e-3); EXPECT_NEAR(mat3(0, 1), 2.0, 1e-3); EXPECT_NEAR(mat3(1, 0), 3.0, 1e-3); EXPECT_NEAR(mat3(1, 1), 4.0, 1e-3); // Nonsquare matrix with more rows than columns - Eigen::Matrix mat4 = - frc::MakeMatrix<3, 2>(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); + frc::Matrixd<3, 2> mat4 = frc::MakeMatrix<3, 2>(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); EXPECT_NEAR(mat4(0, 0), 1.0, 1e-3); EXPECT_NEAR(mat4(0, 1), 2.0, 1e-3); EXPECT_NEAR(mat4(1, 0), 3.0, 1e-3); @@ -39,8 +38,7 @@ TEST(StateSpaceUtilTest, MakeMatrix) { EXPECT_NEAR(mat4(2, 1), 6.0, 1e-3); // Nonsquare matrix with more columns than rows - Eigen::Matrix mat5 = - frc::MakeMatrix<2, 3>(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); + frc::Matrixd<2, 3> mat5 = frc::MakeMatrix<2, 3>(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); EXPECT_NEAR(mat5(0, 0), 1.0, 1e-3); EXPECT_NEAR(mat5(0, 1), 2.0, 1e-3); EXPECT_NEAR(mat5(0, 2), 3.0, 1e-3); @@ -50,7 +48,7 @@ TEST(StateSpaceUtilTest, MakeMatrix) { } TEST(StateSpaceUtilTest, CostParameterPack) { - Eigen::Matrix mat = frc::MakeCostMatrix(1.0, 2.0, 3.0); + frc::Matrixd<3, 3> mat = frc::MakeCostMatrix(1.0, 2.0, 3.0); EXPECT_NEAR(mat(0, 0), 1.0, 1e-3); EXPECT_NEAR(mat(0, 1), 0.0, 1e-3); EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); @@ -63,7 +61,7 @@ TEST(StateSpaceUtilTest, CostParameterPack) { } TEST(StateSpaceUtilTest, CostArray) { - Eigen::Matrix mat = frc::MakeCostMatrix<3>({1.0, 2.0, 3.0}); + frc::Matrixd<3, 3> mat = frc::MakeCostMatrix<3>({1.0, 2.0, 3.0}); EXPECT_NEAR(mat(0, 0), 1.0, 1e-3); EXPECT_NEAR(mat(0, 1), 0.0, 1e-3); EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); @@ -76,7 +74,7 @@ TEST(StateSpaceUtilTest, CostArray) { } TEST(StateSpaceUtilTest, CovParameterPack) { - Eigen::Matrix mat = frc::MakeCovMatrix(1.0, 2.0, 3.0); + frc::Matrixd<3, 3> mat = frc::MakeCovMatrix(1.0, 2.0, 3.0); EXPECT_NEAR(mat(0, 0), 1.0, 1e-3); EXPECT_NEAR(mat(0, 1), 0.0, 1e-3); EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); @@ -89,7 +87,7 @@ TEST(StateSpaceUtilTest, CovParameterPack) { } TEST(StateSpaceUtilTest, CovArray) { - Eigen::Matrix mat = frc::MakeCovMatrix<3>({1.0, 2.0, 3.0}); + frc::Matrixd<3, 3> mat = frc::MakeCovMatrix<3>({1.0, 2.0, 3.0}); EXPECT_NEAR(mat(0, 0), 1.0, 1e-3); EXPECT_NEAR(mat(0, 1), 0.0, 1e-3); EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); @@ -102,59 +100,59 @@ TEST(StateSpaceUtilTest, CovArray) { } TEST(StateSpaceUtilTest, WhiteNoiseVectorParameterPack) { - Eigen::Vector vec = frc::MakeWhiteNoiseVector(2.0, 3.0); + frc::Vectord<2> vec = frc::MakeWhiteNoiseVector(2.0, 3.0); static_cast(vec); } TEST(StateSpaceUtilTest, WhiteNoiseVectorArray) { - Eigen::Vector vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0}); + frc::Vectord<2> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0}); static_cast(vec); } TEST(StateSpaceUtilTest, IsStabilizable) { - Eigen::Matrix B{0, 1}; + frc::Matrixd<2, 1> B{0, 1}; // First eigenvalue is uncontrollable and unstable. // Second eigenvalue is controllable and stable. - EXPECT_FALSE((frc::IsStabilizable<2, 1>( - Eigen::Matrix{{1.2, 0}, {0, 0.5}}, B))); + EXPECT_FALSE( + (frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{1.2, 0}, {0, 0.5}}, B))); // First eigenvalue is uncontrollable and marginally stable. // Second eigenvalue is controllable and stable. - EXPECT_FALSE((frc::IsStabilizable<2, 1>( - Eigen::Matrix{{1, 0}, {0, 0.5}}, B))); + EXPECT_FALSE( + (frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{1, 0}, {0, 0.5}}, B))); // First eigenvalue is uncontrollable and stable. // Second eigenvalue is controllable and stable. - EXPECT_TRUE((frc::IsStabilizable<2, 1>( - Eigen::Matrix{{0.2, 0}, {0, 0.5}}, B))); + EXPECT_TRUE( + (frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 0.5}}, B))); // First eigenvalue is uncontrollable and stable. // Second eigenvalue is controllable and unstable. - EXPECT_TRUE((frc::IsStabilizable<2, 1>( - Eigen::Matrix{{0.2, 0}, {0, 1.2}}, B))); + EXPECT_TRUE( + (frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 1.2}}, B))); } TEST(StateSpaceUtilTest, IsDetectable) { - Eigen::Matrix C{0, 1}; + frc::Matrixd<1, 2> C{0, 1}; // First eigenvalue is unobservable and unstable. // Second eigenvalue is observable and stable. - EXPECT_FALSE((frc::IsDetectable<2, 1>( - Eigen::Matrix{{1.2, 0}, {0, 0.5}}, C))); + EXPECT_FALSE( + (frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{1.2, 0}, {0, 0.5}}, C))); // First eigenvalue is unobservable and marginally stable. // Second eigenvalue is observable and stable. - EXPECT_FALSE((frc::IsDetectable<2, 1>( - Eigen::Matrix{{1, 0}, {0, 0.5}}, C))); + EXPECT_FALSE( + (frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{1, 0}, {0, 0.5}}, C))); // First eigenvalue is unobservable and stable. // Second eigenvalue is observable and stable. - EXPECT_TRUE((frc::IsDetectable<2, 1>( - Eigen::Matrix{{0.2, 0}, {0, 0.5}}, C))); + EXPECT_TRUE( + (frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 0.5}}, C))); // First eigenvalue is unobservable and stable. // Second eigenvalue is observable and unstable. - EXPECT_TRUE((frc::IsDetectable<2, 1>( - Eigen::Matrix{{0.2, 0}, {0, 1.2}}, C))); + EXPECT_TRUE( + (frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 1.2}}, C))); } diff --git a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp index 354ed18637..7bb36afd8f 100644 --- a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp @@ -6,47 +6,46 @@ #include -#include "Eigen/Core" +#include "frc/EigenCore.h" #include "frc/controller/ControlAffinePlantInversionFeedforward.h" #include "units/time.h" namespace frc { -Eigen::Vector Dynamics(const Eigen::Vector& x, - const Eigen::Vector& u) { - return Eigen::Matrix{{1.0, 0.0}, {0.0, 1.0}} * x + - Eigen::Matrix{0.0, 1.0} * u; +Vectord<2> Dynamics(const Vectord<2>& x, const Vectord<1>& u) { + return Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x + + Matrixd<2, 1>{0.0, 1.0} * u; } -Eigen::Vector StateDynamics(const Eigen::Vector& x) { - return Eigen::Matrix{{1.0, 0.0}, {0.0, 1.0}} * x; +Vectord<2> StateDynamics(const Vectord<2>& x) { + return Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x; } TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) { - std::function(const Eigen::Vector&, - const Eigen::Vector&)> + std::function(const Vectord<2>&, const Vectord<1>&)> modelDynamics = [](auto& x, auto& u) { return Dynamics(x, u); }; frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{ modelDynamics, units::second_t{0.02}}; - Eigen::Vector r{2, 2}; - Eigen::Vector nextR{3, 3}; + Vectord<2> r{2, 2}; + Vectord<2> nextR{3, 3}; EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6); } TEST(ControlAffinePlantInversionFeedforwardTest, CalculateState) { - std::function(const Eigen::Vector&)> - modelDynamics = [](auto& x) { return StateDynamics(x); }; + std::function(const Vectord<2>&)> modelDynamics = [](auto& x) { + return StateDynamics(x); + }; - Eigen::Matrix B{0, 1}; + Matrixd<2, 1> B{0, 1}; frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{ modelDynamics, B, units::second_t(0.02)}; - Eigen::Vector r{2, 2}; - Eigen::Vector nextR{3, 3}; + Vectord<2> r{2, 2}; + Vectord<2> nextR{3, 3}; EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6); } diff --git a/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp b/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp index 31927ff2f8..2987a5200c 100644 --- a/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp +++ b/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp @@ -24,40 +24,37 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) { DifferentialDriveAccelerationLimiter accelLimiter{plant, trackwidth, maxA, maxAlpha}; - Eigen::Vector x{0.0, 0.0}; - Eigen::Vector xAccelLimiter{0.0, 0.0}; + Vectord<2> x{0.0, 0.0}; + Vectord<2> xAccelLimiter{0.0, 0.0}; // Ensure voltage exceeds acceleration before limiting { - Eigen::Vector accels = - plant.A() * xAccelLimiter + - plant.B() * Eigen::Vector{12.0, 12.0}; + Vectord<2> accels = + plant.A() * xAccelLimiter + plant.B() * Vectord<2>{12.0, 12.0}; units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0}; EXPECT_GT(units::math::abs(a), maxA); } { - Eigen::Vector accels = - plant.A() * xAccelLimiter + - plant.B() * Eigen::Vector{-12.0, 12.0}; + Vectord<2> accels = + plant.A() * xAccelLimiter + plant.B() * Vectord<2>{-12.0, 12.0}; units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) / trackwidth.value()}; EXPECT_GT(units::math::abs(alpha), maxAlpha); } // Forward - Eigen::Vector u{12.0, 12.0}; + Vectord<2> u{12.0, 12.0}; for (auto t = 0_s; t < 3_s; t += dt) { x = plant.CalculateX(x, u, dt); auto [left, right] = accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)}, units::meters_per_second_t{xAccelLimiter(1)}, units::volt_t{u(0)}, units::volt_t{u(1)}); - xAccelLimiter = plant.CalculateX(xAccelLimiter, - Eigen::Vector{left, right}, dt); + xAccelLimiter = + plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt); - Eigen::Vector accels = - plant.A() * xAccelLimiter + - plant.B() * Eigen::Vector{left, right}; + Vectord<2> accels = + plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right}; units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0}; units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) / trackwidth.value()}; @@ -66,19 +63,18 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) { } // Backward - u = Eigen::Vector{-12.0, -12.0}; + u = Vectord<2>{-12.0, -12.0}; for (auto t = 0_s; t < 3_s; t += dt) { x = plant.CalculateX(x, u, dt); auto [left, right] = accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)}, units::meters_per_second_t{xAccelLimiter(1)}, units::volt_t{u(0)}, units::volt_t{u(1)}); - xAccelLimiter = plant.CalculateX(xAccelLimiter, - Eigen::Vector{left, right}, dt); + xAccelLimiter = + plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt); - Eigen::Vector accels = - plant.A() * xAccelLimiter + - plant.B() * Eigen::Vector{left, right}; + Vectord<2> accels = + plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right}; units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0}; units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) / trackwidth.value()}; @@ -87,19 +83,18 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) { } // Rotate CCW - u = Eigen::Vector{-12.0, 12.0}; + u = Vectord<2>{-12.0, 12.0}; for (auto t = 0_s; t < 3_s; t += dt) { x = plant.CalculateX(x, u, dt); auto [left, right] = accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)}, units::meters_per_second_t{xAccelLimiter(1)}, units::volt_t{u(0)}, units::volt_t{u(1)}); - xAccelLimiter = plant.CalculateX(xAccelLimiter, - Eigen::Vector{left, right}, dt); + xAccelLimiter = + plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt); - Eigen::Vector accels = - plant.A() * xAccelLimiter + - plant.B() * Eigen::Vector{left, right}; + Vectord<2> accels = + plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right}; units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0}; units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) / trackwidth.value()}; @@ -123,19 +118,19 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) { DifferentialDriveAccelerationLimiter accelLimiter{ plant, trackwidth, 1e3_mps_sq, 1e3_rad_per_s_sq}; - Eigen::Vector x{0.0, 0.0}; - Eigen::Vector xAccelLimiter{0.0, 0.0}; + Vectord<2> x{0.0, 0.0}; + Vectord<2> xAccelLimiter{0.0, 0.0}; // Forward - Eigen::Vector u{12.0, 12.0}; + Vectord<2> u{12.0, 12.0}; for (auto t = 0_s; t < 3_s; t += dt) { x = plant.CalculateX(x, u, dt); auto [left, right] = accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)}, units::meters_per_second_t{xAccelLimiter(1)}, units::volt_t{u(0)}, units::volt_t{u(1)}); - xAccelLimiter = plant.CalculateX(xAccelLimiter, - Eigen::Vector{left, right}, dt); + xAccelLimiter = + plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt); EXPECT_DOUBLE_EQ(x(0), xAccelLimiter(0)); EXPECT_DOUBLE_EQ(x(1), xAccelLimiter(1)); @@ -144,15 +139,15 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) { // Backward x.setZero(); xAccelLimiter.setZero(); - u = Eigen::Vector{-12.0, -12.0}; + u = Vectord<2>{-12.0, -12.0}; for (auto t = 0_s; t < 3_s; t += dt) { x = plant.CalculateX(x, u, dt); auto [left, right] = accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)}, units::meters_per_second_t{xAccelLimiter(1)}, units::volt_t{u(0)}, units::volt_t{u(1)}); - xAccelLimiter = plant.CalculateX(xAccelLimiter, - Eigen::Vector{left, right}, dt); + xAccelLimiter = + plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt); EXPECT_DOUBLE_EQ(x(0), xAccelLimiter(0)); EXPECT_DOUBLE_EQ(x(1), xAccelLimiter(1)); @@ -161,15 +156,15 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) { // Rotate CCW x.setZero(); xAccelLimiter.setZero(); - u = Eigen::Vector{-12.0, 12.0}; + u = Vectord<2>{-12.0, 12.0}; for (auto t = 0_s; t < 3_s; t += dt) { x = plant.CalculateX(x, u, dt); auto [left, right] = accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)}, units::meters_per_second_t{xAccelLimiter(1)}, units::volt_t{u(0)}, units::volt_t{u(1)}); - xAccelLimiter = plant.CalculateX(xAccelLimiter, - Eigen::Vector{left, right}, dt); + xAccelLimiter = + plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt); EXPECT_DOUBLE_EQ(x(0), xAccelLimiter(0)); EXPECT_DOUBLE_EQ(x(1), xAccelLimiter(1)); diff --git a/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp b/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp index 9916370ef9..afa491a7b1 100644 --- a/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp @@ -19,11 +19,11 @@ TEST(ImplicitModelFollowerTest, SameModel) { ImplicitModelFollower<2, 2> imf{plant, plant}; - Eigen::Vector x{0.0, 0.0}; - Eigen::Vector xImf{0.0, 0.0}; + Vectord<2> x{0.0, 0.0}; + Vectord<2> xImf{0.0, 0.0}; // Forward - Eigen::Vector u{12.0, 12.0}; + Vectord<2> u{12.0, 12.0}; for (auto t = 0_s; t < 3_s; t += dt) { x = plant.CalculateX(x, u, dt); xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt); @@ -33,7 +33,7 @@ TEST(ImplicitModelFollowerTest, SameModel) { } // Backward - u = Eigen::Vector{-12.0, -12.0}; + u = Vectord<2>{-12.0, -12.0}; for (auto t = 0_s; t < 3_s; t += dt) { x = plant.CalculateX(x, u, dt); xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt); @@ -43,7 +43,7 @@ TEST(ImplicitModelFollowerTest, SameModel) { } // Rotate CCW - u = Eigen::Vector{-12.0, 12.0}; + u = Vectord<2>{-12.0, 12.0}; for (auto t = 0_s; t < 3_s; t += dt) { x = plant.CalculateX(x, u, dt); xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt); @@ -68,11 +68,11 @@ TEST(ImplicitModelFollowerTest, SlowerRefModel) { ImplicitModelFollower<2, 2> imf{plant, plantRef}; - Eigen::Vector x{0.0, 0.0}; - Eigen::Vector xImf{0.0, 0.0}; + Vectord<2> x{0.0, 0.0}; + Vectord<2> xImf{0.0, 0.0}; // Forward - Eigen::Vector u{12.0, 12.0}; + Vectord<2> u{12.0, 12.0}; for (auto t = 0_s; t < 3_s; t += dt) { x = plant.CalculateX(x, u, dt); xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt); @@ -84,7 +84,7 @@ TEST(ImplicitModelFollowerTest, SlowerRefModel) { // Backward x.setZero(); xImf.setZero(); - u = Eigen::Vector{-12.0, -12.0}; + u = Vectord<2>{-12.0, -12.0}; for (auto t = 0_s; t < 3_s; t += dt) { x = plant.CalculateX(x, u, dt); xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt); @@ -96,7 +96,7 @@ TEST(ImplicitModelFollowerTest, SlowerRefModel) { // Rotate CCW x.setZero(); xImf.setZero(); - u = Eigen::Vector{-12.0, 12.0}; + u = Vectord<2>{-12.0, 12.0}; for (auto t = 0_s; t < 3_s; t += dt) { x = plant.CalculateX(x, u, dt); xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt); diff --git a/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp index 6e61706ff0..139d6fbf40 100644 --- a/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp @@ -6,21 +6,21 @@ #include -#include "Eigen/Core" +#include "frc/EigenCore.h" #include "frc/controller/LinearPlantInversionFeedforward.h" #include "units/time.h" namespace frc { TEST(LinearPlantInversionFeedforwardTest, Calculate) { - Eigen::Matrix A{{1, 0}, {0, 1}}; - Eigen::Matrix B{0, 1}; + Matrixd<2, 2> A{{1, 0}, {0, 1}}; + Matrixd<2, 1> B{0, 1}; frc::LinearPlantInversionFeedforward<2, 1> feedforward{A, B, units::second_t(0.02)}; - Eigen::Vector r{2, 2}; - Eigen::Vector nextR{3, 3}; + Vectord<2> r{2, 2}; + Vectord<2> nextR{3, 3}; EXPECT_NEAR(47.502599, feedforward.Calculate(r, nextR)(0, 0), 0.002); } diff --git a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp index 7055530d71..1127fc21d4 100644 --- a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp +++ b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp @@ -6,7 +6,7 @@ #include -#include "Eigen/Core" +#include "frc/EigenCore.h" #include "frc/controller/LinearQuadraticRegulator.h" #include "frc/system/LinearSystem.h" #include "frc/system/plant/DCMotor.h" @@ -30,7 +30,7 @@ TEST(LinearQuadraticRegulatorTest, ElevatorGains) { return frc::LinearSystemId::ElevatorSystem(motors, m, r, G); }(); - Eigen::Matrix K = + Matrixd<1, 2> K = LinearQuadraticRegulator<2, 1>{plant, {0.02, 0.4}, {12.0}, 5.05_ms}.K(); EXPECT_NEAR(522.15314269, K(0, 0), 1e-6); @@ -54,7 +54,7 @@ TEST(LinearQuadraticRegulatorTest, ArmGains) { motors, 1.0 / 3.0 * m * r * r, G); }(); - Eigen::Matrix K = + Matrixd<1, 2> K = LinearQuadraticRegulator<2, 1>{plant, {0.01745, 0.08726}, {12.0}, 5.05_ms} .K(); @@ -77,7 +77,7 @@ TEST(LinearQuadraticRegulatorTest, FourMotorElevator) { return frc::LinearSystemId::ElevatorSystem(motors, m, r, G); }(); - Eigen::Matrix K = + Matrixd<1, 2> K = LinearQuadraticRegulator<2, 1>{plant, {0.1, 0.2}, {12.0}, 20_ms}.K(); EXPECT_NEAR(10.38, K(0, 0), 1e-1); @@ -99,50 +99,44 @@ TEST(LinearQuadraticRegulatorTest, FourMotorElevator) { * @param dt Discretization timestep. */ template -Eigen::Matrix GetImplicitModelFollowingK( - const Eigen::Matrix& A, - const Eigen::Matrix& B, - const Eigen::Matrix& Q, - const Eigen::Matrix& R, - const Eigen::Matrix& Aref, units::second_t dt) { +Matrixd GetImplicitModelFollowingK( + const Matrixd& A, const Matrixd& B, + const Matrixd& Q, const Matrixd& R, + const Matrixd& Aref, units::second_t dt) { // Discretize real dynamics - Eigen::Matrix discA; - Eigen::Matrix discB; + Matrixd discA; + Matrixd discB; DiscretizeAB(A, B, dt, &discA, &discB); // Discretize desired dynamics - Eigen::Matrix discAref; + Matrixd discAref; DiscretizeA(Aref, dt, &discAref); - Eigen::Matrix Qimf = + Matrixd Qimf = (discA - discAref).transpose() * Q * (discA - discAref); - Eigen::Matrix Rimf = - discB.transpose() * Q * discB + R; - Eigen::Matrix Nimf = - (discA - discAref).transpose() * Q * discB; + Matrixd Rimf = discB.transpose() * Q * discB + R; + Matrixd Nimf = (discA - discAref).transpose() * Q * discB; return LinearQuadraticRegulator{A, B, Qimf, Rimf, Nimf, dt} .K(); } TEST(LinearQuadraticRegulatorTest, MatrixOverloadsWithSingleIntegrator) { - Eigen::Matrix A{Eigen::Matrix::Zero()}; - Eigen::Matrix B{Eigen::Matrix::Identity()}; - Eigen::Matrix Q{Eigen::Matrix::Identity()}; - Eigen::Matrix R{Eigen::Matrix::Identity()}; + Matrixd<2, 2> A{Matrixd<2, 2>::Zero()}; + Matrixd<2, 2> B{Matrixd<2, 2>::Identity()}; + Matrixd<2, 2> Q{Matrixd<2, 2>::Identity()}; + Matrixd<2, 2> R{Matrixd<2, 2>::Identity()}; // QR overload - Eigen::Matrix K = - LinearQuadraticRegulator<2, 2>{A, B, Q, R, 5_ms}.K(); + Matrixd<2, 2> K = LinearQuadraticRegulator<2, 2>{A, B, Q, R, 5_ms}.K(); EXPECT_NEAR(0.99750312499512261, K(0, 0), 1e-10); EXPECT_NEAR(0.0, K(0, 1), 1e-10); EXPECT_NEAR(0.0, K(1, 0), 1e-10); EXPECT_NEAR(0.99750312499512261, K(1, 1), 1e-10); // QRN overload - Eigen::Matrix N{Eigen::Matrix::Identity()}; - Eigen::Matrix Kimf = - LinearQuadraticRegulator<2, 2>{A, B, Q, R, N, 5_ms}.K(); + Matrixd<2, 2> N{Matrixd<2, 2>::Identity()}; + Matrixd<2, 2> Kimf = LinearQuadraticRegulator<2, 2>{A, B, Q, R, N, 5_ms}.K(); EXPECT_NEAR(1.0, Kimf(0, 0), 1e-10); EXPECT_NEAR(0.0, Kimf(0, 1), 1e-10); EXPECT_NEAR(0.0, Kimf(1, 0), 1e-10); @@ -153,21 +147,19 @@ TEST(LinearQuadraticRegulatorTest, MatrixOverloadsWithDoubleIntegrator) { double Kv = 3.02; double Ka = 0.642; - Eigen::Matrix A{{0, 1}, {0, -Kv / Ka}}; - Eigen::Matrix B{{0}, {1.0 / Ka}}; - Eigen::Matrix Q{{1, 0}, {0, 0.2}}; - Eigen::Matrix R{0.25}; + Matrixd<2, 2> A{{0, 1}, {0, -Kv / Ka}}; + Matrixd<2, 1> B{{0}, {1.0 / Ka}}; + Matrixd<2, 2> Q{{1, 0}, {0, 0.2}}; + Matrixd<1, 1> R{0.25}; // QR overload - Eigen::Matrix K = - LinearQuadraticRegulator<2, 1>{A, B, Q, R, 5_ms}.K(); + Matrixd<1, 2> K = LinearQuadraticRegulator<2, 1>{A, B, Q, R, 5_ms}.K(); EXPECT_NEAR(1.9960017786537287, K(0, 0), 1e-10); EXPECT_NEAR(0.51182128351092726, K(0, 1), 1e-10); // QRN overload - Eigen::Matrix Aref{{0, 1}, {0, -Kv / (Ka * 2.0)}}; - Eigen::Matrix Kimf = - GetImplicitModelFollowingK<2, 1>(A, B, Q, R, Aref, 5_ms); + Matrixd<2, 2> Aref{{0, 1}, {0, -Kv / (Ka * 2.0)}}; + Matrixd<1, 2> Kimf = GetImplicitModelFollowingK<2, 1>(A, B, Q, R, Aref, 5_ms); EXPECT_NEAR(0.0, Kimf(0, 0), 1e-10); EXPECT_NEAR(-5.367540084534802e-05, Kimf(0, 1), 1e-10); } diff --git a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp index 3cf944ef49..6d10e051d6 100644 --- a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp @@ -6,7 +6,7 @@ #include -#include "Eigen/Core" +#include "frc/EigenCore.h" #include "frc/controller/LinearPlantInversionFeedforward.h" #include "frc/controller/SimpleMotorFeedforward.h" #include "units/acceleration.h" @@ -21,16 +21,16 @@ TEST(SimpleMotorFeedforwardTest, Calculate) { double Ka = 0.6; auto dt = 0.02_s; - Eigen::Matrix A{-Kv / Ka}; - Eigen::Matrix B{1.0 / Ka}; + Matrixd<1, 1> A{-Kv / Ka}; + Matrixd<1, 1> B{1.0 / Ka}; frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt}; frc::SimpleMotorFeedforward simpleMotor{ units::volt_t{Ks}, units::volt_t{Kv} / 1_mps, units::volt_t{Ka} / 1_mps_sq}; - Eigen::Vector r{2.0}; - Eigen::Vector nextR{3.0}; + Vectord<1> r{2.0}; + Vectord<1> nextR{3.0}; EXPECT_NEAR(37.524995834325161 + Ks, simpleMotor.Calculate(2_mps, 3_mps, dt).value(), 0.002); diff --git a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp index ee1da7f03c..e050cd7bb8 100644 --- a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp @@ -6,11 +6,11 @@ #include -#include "Eigen/Core" +#include "frc/EigenCore.h" #include "frc/estimator/AngleStatistics.h" TEST(AngleStatisticsTest, Mean) { - Eigen::Matrix sigmas{ + frc::Matrixd<3, 3> sigmas{ {1, 1.2, 0}, {359 * wpi::numbers::pi / 180, 3 * wpi::numbers::pi / 180, 0}, {1, 2, 0}}; diff --git a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp index 6d51185661..1924b73f39 100644 --- a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp @@ -7,8 +7,8 @@ #include #include -#include "Eigen/Core" #include "Eigen/QR" +#include "frc/EigenCore.h" #include "frc/StateSpaceUtil.h" #include "frc/estimator/ExtendedKalmanFilter.h" #include "frc/system/NumericalJacobian.h" @@ -18,8 +18,7 @@ namespace { -Eigen::Vector Dynamics(const Eigen::Vector& x, - const Eigen::Vector& u) { +frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) { auto motors = frc::DCMotor::CIM(2); // constexpr double Glow = 15.32; // Low gear ratio @@ -41,7 +40,7 @@ Eigen::Vector Dynamics(const Eigen::Vector& x, units::volt_t Vr{u(1)}; auto v = 0.5 * (vl + vr); - return Eigen::Vector{ + return frc::Vectord<5>{ v.value() * std::cos(x(2)), v.value() * std::sin(x(2)), ((vr - vl) / (2.0 * rb)).value(), k1.value() * ((C1 * vl).value() + (C2 * Vl).value()) + @@ -50,16 +49,16 @@ Eigen::Vector Dynamics(const Eigen::Vector& x, k1.value() * ((C1 * vr).value() + (C2 * Vr).value())}; } -Eigen::Vector LocalMeasurementModel( - const Eigen::Vector& x, const Eigen::Vector& u) { +frc::Vectord<3> LocalMeasurementModel(const frc::Vectord<5>& x, + const frc::Vectord<2>& u) { static_cast(u); - return Eigen::Vector{x(2), x(3), x(4)}; + return frc::Vectord<3>{x(2), x(3), x(4)}; } -Eigen::Vector GlobalMeasurementModel( - const Eigen::Vector& x, const Eigen::Vector& u) { +frc::Vectord<5> GlobalMeasurementModel(const frc::Vectord<5>& x, + const frc::Vectord<2>& u) { static_cast(u); - return Eigen::Vector{x(0), x(1), x(2), x(3), x(4)}; + return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)}; } } // namespace @@ -71,7 +70,7 @@ TEST(ExtendedKalmanFilterTest, Init) { {0.5, 0.5, 10.0, 1.0, 1.0}, {0.0001, 0.01, 0.01}, dt}; - Eigen::Vector u{12.0, 12.0}; + frc::Vectord<2> u{12.0, 12.0}; observer.Predict(u, dt); auto localY = LocalMeasurementModel(observer.Xhat(), u); @@ -98,14 +97,13 @@ TEST(ExtendedKalmanFilterTest, Convergence) { auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory( waypoints, {8.8_mps, 0.1_mps_sq}); - Eigen::Vector r = Eigen::Vector::Zero(); - Eigen::Vector u = Eigen::Vector::Zero(); + frc::Vectord<5> r = frc::Vectord<5>::Zero(); + frc::Vectord<2> u = frc::Vectord<2>::Zero(); - auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics, - Eigen::Vector::Zero(), - Eigen::Vector::Zero()); + auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics, frc::Vectord<5>::Zero(), + frc::Vectord<2>::Zero()); - observer.SetXhat(Eigen::Vector{ + observer.SetXhat(frc::Vectord<5>{ trajectory.InitialPose().Translation().X().value(), trajectory.InitialPose().Translation().Y().value(), trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0}); @@ -118,17 +116,15 @@ TEST(ExtendedKalmanFilterTest, Convergence) { units::meters_per_second_t vr = ref.velocity * (1 + (ref.curvature * rb).value()); - Eigen::Vector nextR{ + frc::Vectord<5> nextR{ ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(), ref.pose.Rotation().Radians().value(), vl.value(), vr.value()}; - auto localY = - LocalMeasurementModel(nextR, Eigen::Vector::Zero()); + auto localY = LocalMeasurementModel(nextR, frc::Vectord<2>::Zero()); observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5)); - Eigen::Vector rdot = (nextR - r) / dt.value(); - u = B.householderQr().solve(rdot - - Dynamics(r, Eigen::Vector::Zero())); + frc::Vectord<5> rdot = (nextR - r) / dt.value(); + u = B.householderQr().solve(rdot - Dynamics(r, frc::Vectord<2>::Zero())); observer.Predict(u, dt); diff --git a/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp b/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp index c012435949..9fd92b6677 100644 --- a/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp @@ -11,12 +11,10 @@ namespace { TEST(MerweScaledSigmaPointsTest, ZeroMean) { frc::MerweScaledSigmaPoints<2> sigmaPoints; auto points = sigmaPoints.SigmaPoints( - Eigen::Vector{0.0, 0.0}, - Eigen::Matrix{{1.0, 0.0}, {0.0, 1.0}}); + frc::Vectord<2>{0.0, 0.0}, frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}); EXPECT_TRUE( - (points - - Eigen::Matrix{{0.0, 0.00173205, 0.0, -0.00173205, 0.0}, + (points - frc::Matrixd<2, 5>{{0.0, 0.00173205, 0.0, -0.00173205, 0.0}, {0.0, 0.0, 0.00173205, 0.0, -0.00173205}}) .norm() < 1e-3); } @@ -24,12 +22,10 @@ TEST(MerweScaledSigmaPointsTest, ZeroMean) { TEST(MerweScaledSigmaPointsTest, NonzeroMean) { frc::MerweScaledSigmaPoints<2> sigmaPoints; auto points = sigmaPoints.SigmaPoints( - Eigen::Vector{1.0, 2.0}, - Eigen::Matrix{{1.0, 0.0}, {0.0, 10.0}}); + frc::Vectord<2>{1.0, 2.0}, frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 10.0}}); EXPECT_TRUE( - (points - - Eigen::Matrix{{1.0, 1.00173205, 1.0, 0.998268, 1.0}, + (points - frc::Matrixd<2, 5>{{1.0, 1.00173205, 1.0, 0.998268, 1.0}, {2.0, 2.0, 2.00548, 2.0, 1.99452}}) .norm() < 1e-3); } diff --git a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp index 9665442bf3..b5b36d65e3 100644 --- a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp @@ -7,8 +7,8 @@ #include #include -#include "Eigen/Core" #include "Eigen/QR" +#include "frc/EigenCore.h" #include "frc/StateSpaceUtil.h" #include "frc/estimator/AngleStatistics.h" #include "frc/estimator/UnscentedKalmanFilter.h" @@ -20,8 +20,7 @@ namespace { -Eigen::Vector Dynamics(const Eigen::Vector& x, - const Eigen::Vector& u) { +frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) { auto motors = frc::DCMotor::CIM(2); // constexpr double Glow = 15.32; // Low gear ratio @@ -43,7 +42,7 @@ Eigen::Vector Dynamics(const Eigen::Vector& x, units::volt_t Vr{u(1)}; auto v = 0.5 * (vl + vr); - return Eigen::Vector{ + return frc::Vectord<5>{ v.value() * std::cos(x(2)), v.value() * std::sin(x(2)), ((vr - vl) / (2.0 * rb)).value(), k1.value() * ((C1 * vl).value() + (C2 * Vl).value()) + @@ -52,16 +51,16 @@ Eigen::Vector Dynamics(const Eigen::Vector& x, k1.value() * ((C1 * vr).value() + (C2 * Vr).value())}; } -Eigen::Vector LocalMeasurementModel( - const Eigen::Vector& x, const Eigen::Vector& u) { +frc::Vectord<3> LocalMeasurementModel(const frc::Vectord<5>& x, + const frc::Vectord<2>& u) { static_cast(u); - return Eigen::Vector{x(2), x(3), x(4)}; + return frc::Vectord<3>{x(2), x(3), x(4)}; } -Eigen::Vector GlobalMeasurementModel( - const Eigen::Vector& x, const Eigen::Vector& u) { +frc::Vectord<5> GlobalMeasurementModel(const frc::Vectord<5>& x, + const frc::Vectord<2>& u) { static_cast(u); - return Eigen::Vector{x(0), x(1), x(2), x(3), x(4)}; + return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)}; } } // namespace @@ -73,7 +72,7 @@ TEST(UnscentedKalmanFilterTest, Init) { {0.5, 0.5, 10.0, 1.0, 1.0}, {0.0001, 0.01, 0.01}, dt}; - Eigen::Vector u{12.0, 12.0}; + frc::Vectord<2> u{12.0, 12.0}; observer.Predict(u, dt); auto localY = LocalMeasurementModel(observer.Xhat(), u); @@ -102,14 +101,13 @@ TEST(UnscentedKalmanFilterTest, Convergence) { auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory( waypoints, {8.8_mps, 0.1_mps_sq}); - Eigen::Vector r = Eigen::Vector::Zero(); - Eigen::Vector u = Eigen::Vector::Zero(); + frc::Vectord<5> r = frc::Vectord<5>::Zero(); + frc::Vectord<2> u = frc::Vectord<2>::Zero(); - auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics, - Eigen::Vector::Zero(), - Eigen::Vector::Zero()); + auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics, frc::Vectord<5>::Zero(), + frc::Vectord<2>::Zero()); - observer.SetXhat(Eigen::Vector{ + observer.SetXhat(frc::Vectord<5>{ trajectory.InitialPose().Translation().X().value(), trajectory.InitialPose().Translation().Y().value(), trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0}); @@ -124,17 +122,15 @@ TEST(UnscentedKalmanFilterTest, Convergence) { units::meters_per_second_t vr = ref.velocity * (1 + (ref.curvature * rb).value()); - Eigen::Vector nextR{ + frc::Vectord<5> nextR{ ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(), ref.pose.Rotation().Radians().value(), vl.value(), vr.value()}; - auto localY = - LocalMeasurementModel(trueXhat, Eigen::Vector::Zero()); + auto localY = LocalMeasurementModel(trueXhat, frc::Vectord<2>::Zero()); observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5)); - Eigen::Vector rdot = (nextR - r) / dt.value(); - u = B.householderQr().solve(rdot - - Dynamics(r, Eigen::Vector::Zero())); + frc::Vectord<5> rdot = (nextR - r) / dt.value(); + u = B.householderQr().solve(rdot - Dynamics(r, frc::Vectord<2>::Zero())); observer.Predict(u, dt); diff --git a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp index b5a0fdc807..9a25c58adb 100644 --- a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp +++ b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp @@ -6,8 +6,8 @@ #include -#include "Eigen/Core" #include "Eigen/Eigenvalues" +#include "frc/EigenCore.h" #include "frc/system/Discretization.h" #include "frc/system/NumericalIntegration.h" #include "frc/system/RungeKuttaTimeVarying.h" @@ -15,17 +15,16 @@ // Check that for a simple second-order system that we can easily analyze // analytically, TEST(DiscretizationTest, DiscretizeA) { - Eigen::Matrix contA{{0, 1}, {0, 0}}; + frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}}; - Eigen::Vector x0{1, 1}; - Eigen::Matrix discA; + frc::Vectord<2> x0{1, 1}; + frc::Matrixd<2, 2> discA; frc::DiscretizeA<2>(contA, 1_s, &discA); - Eigen::Vector x1Discrete = discA * x0; + frc::Vectord<2> x1Discrete = discA * x0; // We now have pos = vel = 1 and accel = 0, which should give us: - Eigen::Vector x1Truth{1.0 * x0(0) + 1.0 * x0(1), - 0.0 * x0(0) + 1.0 * x0(1)}; + frc::Vectord<2> x1Truth{1.0 * x0(0) + 1.0 * x0(1), 0.0 * x0(0) + 1.0 * x0(1)}; EXPECT_EQ(x1Truth, x1Discrete); } @@ -33,20 +32,20 @@ TEST(DiscretizationTest, DiscretizeA) { // Check that for a simple second-order system that we can easily analyze // analytically, TEST(DiscretizationTest, DiscretizeAB) { - Eigen::Matrix contA{{0, 1}, {0, 0}}; - Eigen::Matrix contB{0, 1}; + frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}}; + frc::Matrixd<2, 1> contB{0, 1}; - Eigen::Vector x0{1, 1}; - Eigen::Vector u{1}; - Eigen::Matrix discA; - Eigen::Matrix discB; + frc::Vectord<2> x0{1, 1}; + frc::Vectord<1> u{1}; + frc::Matrixd<2, 2> discA; + frc::Matrixd<2, 1> discB; frc::DiscretizeAB<2, 1>(contA, contB, 1_s, &discA, &discB); - Eigen::Vector x1Discrete = discA * x0 + discB * u; + frc::Vectord<2> x1Discrete = discA * x0 + discB * u; // We now have pos = vel = accel = 1, which should give us: - Eigen::Vector x1Truth{1.0 * x0(0) + 1.0 * x0(1) + 0.5 * u(0), - 0.0 * x0(0) + 1.0 * x0(1) + 1.0 * u(0)}; + frc::Vectord<2> x1Truth{1.0 * x0(0) + 1.0 * x0(1) + 0.5 * u(0), + 0.0 * x0(0) + 1.0 * x0(1) + 1.0 * u(0)}; EXPECT_EQ(x1Truth, x1Discrete); } @@ -55,24 +54,23 @@ TEST(DiscretizationTest, DiscretizeAB) { // Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ // 0 TEST(DiscretizationTest, DiscretizeSlowModelAQ) { - Eigen::Matrix contA{{0, 1}, {0, 0}}; - Eigen::Matrix contQ{{1, 0}, {0, 1}}; + frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}}; + frc::Matrixd<2, 2> contQ{{1, 0}, {0, 1}}; constexpr auto dt = 1_s; - Eigen::Matrix discQIntegrated = frc::RungeKuttaTimeVarying< - std::function( - units::second_t, const Eigen::Matrix&)>, - Eigen::Matrix>( - [&](units::second_t t, const Eigen::Matrix&) { - return Eigen::Matrix( - (contA * t.value()).exp() * contQ * - (contA.transpose() * t.value()).exp()); + frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying< + std::function(units::second_t, + const frc::Matrixd<2, 2>&)>, + frc::Matrixd<2, 2>>( + [&](units::second_t t, const frc::Matrixd<2, 2>&) { + return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ * + (contA.transpose() * t.value()).exp()); }, - 0_s, Eigen::Matrix::Zero(), dt); + 0_s, frc::Matrixd<2, 2>::Zero(), dt); - Eigen::Matrix discA; - Eigen::Matrix discQ; + frc::Matrixd<2, 2> discA; + frc::Matrixd<2, 2> discQ; frc::DiscretizeAQ<2>(contA, contQ, dt, &discA, &discQ); EXPECT_LT((discQIntegrated - discQ).norm(), 1e-10) @@ -85,24 +83,23 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQ) { // Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ // 0 TEST(DiscretizationTest, DiscretizeFastModelAQ) { - Eigen::Matrix contA{{0, 1}, {0, -1406.29}}; - Eigen::Matrix contQ{{0.0025, 0}, {0, 1}}; + frc::Matrixd<2, 2> contA{{0, 1}, {0, -1406.29}}; + frc::Matrixd<2, 2> contQ{{0.0025, 0}, {0, 1}}; constexpr auto dt = 5_ms; - Eigen::Matrix discQIntegrated = frc::RungeKuttaTimeVarying< - std::function( - units::second_t, const Eigen::Matrix&)>, - Eigen::Matrix>( - [&](units::second_t t, const Eigen::Matrix&) { - return Eigen::Matrix( - (contA * t.value()).exp() * contQ * - (contA.transpose() * t.value()).exp()); + frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying< + std::function(units::second_t, + const frc::Matrixd<2, 2>&)>, + frc::Matrixd<2, 2>>( + [&](units::second_t t, const frc::Matrixd<2, 2>&) { + return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ * + (contA.transpose() * t.value()).exp()); }, - 0_s, Eigen::Matrix::Zero(), dt); + 0_s, frc::Matrixd<2, 2>::Zero(), dt); - Eigen::Matrix discA; - Eigen::Matrix discQ; + frc::Matrixd<2, 2> discA; + frc::Matrixd<2, 2> discQ; frc::DiscretizeAQ<2>(contA, contQ, dt, &discA, &discQ); EXPECT_LT((discQIntegrated - discQ).norm(), 1e-3) @@ -113,14 +110,14 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) { // Test that the Taylor series discretization produces nearly identical results. TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) { - Eigen::Matrix contA{{0, 1}, {0, 0}}; - Eigen::Matrix contQ{{1, 0}, {0, 1}}; + frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}}; + frc::Matrixd<2, 2> contQ{{1, 0}, {0, 1}}; constexpr auto dt = 1_s; - Eigen::Matrix discQTaylor; - Eigen::Matrix discA; - Eigen::Matrix discATaylor; + frc::Matrixd<2, 2> discQTaylor; + frc::Matrixd<2, 2> discA; + frc::Matrixd<2, 2> discATaylor; // Continuous Q should be positive semidefinite Eigen::SelfAdjointEigenSolver esCont{contQ}; @@ -128,16 +125,15 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) { EXPECT_GE(esCont.eigenvalues()[i], 0); } - Eigen::Matrix discQIntegrated = frc::RungeKuttaTimeVarying< - std::function( - units::second_t, const Eigen::Matrix&)>, - Eigen::Matrix>( - [&](units::second_t t, const Eigen::Matrix&) { - return Eigen::Matrix( - (contA * t.value()).exp() * contQ * - (contA.transpose() * t.value()).exp()); + frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying< + std::function(units::second_t, + const frc::Matrixd<2, 2>&)>, + frc::Matrixd<2, 2>>( + [&](units::second_t t, const frc::Matrixd<2, 2>&) { + return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ * + (contA.transpose() * t.value()).exp()); }, - 0_s, Eigen::Matrix::Zero(), dt); + 0_s, frc::Matrixd<2, 2>::Zero(), dt); frc::DiscretizeA<2>(contA, dt, &discA); frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor); @@ -157,14 +153,14 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) { // Test that the Taylor series discretization produces nearly identical results. TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) { - Eigen::Matrix contA{{0, 1}, {0, -1500}}; - Eigen::Matrix contQ{{0.0025, 0}, {0, 1}}; + frc::Matrixd<2, 2> contA{{0, 1}, {0, -1500}}; + frc::Matrixd<2, 2> contQ{{0.0025, 0}, {0, 1}}; constexpr auto dt = 5_ms; - Eigen::Matrix discQTaylor; - Eigen::Matrix discA; - Eigen::Matrix discATaylor; + frc::Matrixd<2, 2> discQTaylor; + frc::Matrixd<2, 2> discA; + frc::Matrixd<2, 2> discATaylor; // Continuous Q should be positive semidefinite Eigen::SelfAdjointEigenSolver esCont(contQ); @@ -172,16 +168,15 @@ TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) { EXPECT_GE(esCont.eigenvalues()[i], 0); } - Eigen::Matrix discQIntegrated = frc::RungeKuttaTimeVarying< - std::function( - units::second_t, const Eigen::Matrix&)>, - Eigen::Matrix>( - [&](units::second_t t, const Eigen::Matrix&) { - return Eigen::Matrix( - (contA * t.value()).exp() * contQ * - (contA.transpose() * t.value()).exp()); + frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying< + std::function(units::second_t, + const frc::Matrixd<2, 2>&)>, + frc::Matrixd<2, 2>>( + [&](units::second_t t, const frc::Matrixd<2, 2>&) { + return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ * + (contA.transpose() * t.value()).exp()); }, - 0_s, Eigen::Matrix::Zero(), dt); + 0_s, frc::Matrixd<2, 2>::Zero(), dt); frc::DiscretizeA<2>(contA, dt, &discA); frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor); @@ -201,10 +196,10 @@ TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) { // Test that DiscretizeR() works TEST(DiscretizationTest, DiscretizeR) { - Eigen::Matrix contR{{2.0, 0.0}, {0.0, 1.0}}; - Eigen::Matrix discRTruth{{4.0, 0.0}, {0.0, 2.0}}; + frc::Matrixd<2, 2> contR{{2.0, 0.0}, {0.0, 1.0}}; + frc::Matrixd<2, 2> discRTruth{{4.0, 0.0}, {0.0, 2.0}}; - Eigen::Matrix discR = frc::DiscretizeR<2>(contR, 500_ms); + frc::Matrixd<2, 2> discR = frc::DiscretizeR<2>(contR, 500_ms); EXPECT_LT((discRTruth - discR).norm(), 1e-10) << "Expected these to be nearly equal:\ndiscR:\n" diff --git a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp index fbf86f1c57..dbc4284e10 100644 --- a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp +++ b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp @@ -16,49 +16,43 @@ TEST(LinearSystemIDTest, IdentifyDrivetrainVelocitySystem) { frc::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0); ASSERT_TRUE(model.A().isApprox( - Eigen::Matrix{{-10.14132, 3.06598}, {3.06598, -10.14132}}, - 0.001)); + frc::Matrixd<2, 2>{{-10.14132, 3.06598}, {3.06598, -10.14132}}, 0.001)); ASSERT_TRUE(model.B().isApprox( - Eigen::Matrix{{4.2590, -1.28762}, {-1.2876, 4.2590}}, - 0.001)); - ASSERT_TRUE(model.C().isApprox( - Eigen::Matrix{{1.0, 0.0}, {0.0, 1.0}}, 0.001)); - ASSERT_TRUE(model.D().isApprox( - Eigen::Matrix{{0.0, 0.0}, {0.0, 0.0}}, 0.001)); + frc::Matrixd<2, 2>{{4.2590, -1.28762}, {-1.2876, 4.2590}}, 0.001)); + ASSERT_TRUE( + model.C().isApprox(frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001)); + ASSERT_TRUE( + model.D().isApprox(frc::Matrixd<2, 2>{{0.0, 0.0}, {0.0, 0.0}}, 0.001)); } TEST(LinearSystemIDTest, ElevatorSystem) { auto model = frc::LinearSystemId::ElevatorSystem(frc::DCMotor::NEO(2), 5_kg, 0.05_m, 12); ASSERT_TRUE(model.A().isApprox( - Eigen::Matrix{{0.0, 1.0}, {0.0, -99.05473}}, 0.001)); - ASSERT_TRUE( - model.B().isApprox(Eigen::Matrix{0.0, 20.8}, 0.001)); - ASSERT_TRUE(model.C().isApprox(Eigen::Matrix{1.0, 0.0}, 0.001)); - ASSERT_TRUE(model.D().isApprox(Eigen::Matrix{0.0}, 0.001)); + frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -99.05473}}, 0.001)); + ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0.0, 20.8}, 0.001)); + ASSERT_TRUE(model.C().isApprox(frc::Matrixd<1, 2>{1.0, 0.0}, 0.001)); + ASSERT_TRUE(model.D().isApprox(frc::Matrixd<1, 1>{0.0}, 0.001)); } TEST(LinearSystemIDTest, FlywheelSystem) { auto model = frc::LinearSystemId::FlywheelSystem(frc::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0); - ASSERT_TRUE( - model.A().isApprox(Eigen::Matrix{-26.87032}, 0.001)); - ASSERT_TRUE( - model.B().isApprox(Eigen::Matrix{1354.166667}, 0.001)); - ASSERT_TRUE(model.C().isApprox(Eigen::Matrix{1.0}, 0.001)); - ASSERT_TRUE(model.D().isApprox(Eigen::Matrix{0.0}, 0.001)); + ASSERT_TRUE(model.A().isApprox(frc::Matrixd<1, 1>{-26.87032}, 0.001)); + ASSERT_TRUE(model.B().isApprox(frc::Matrixd<1, 1>{1354.166667}, 0.001)); + ASSERT_TRUE(model.C().isApprox(frc::Matrixd<1, 1>{1.0}, 0.001)); + ASSERT_TRUE(model.D().isApprox(frc::Matrixd<1, 1>{0.0}, 0.001)); } TEST(LinearSystemIDTest, DCMotorSystem) { auto model = frc::LinearSystemId::DCMotorSystem(frc::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0); - ASSERT_TRUE(model.A().isApprox( - Eigen::Matrix{{0, 1}, {0, -26.87032}}, 0.001)); ASSERT_TRUE( - model.B().isApprox(Eigen::Matrix{0, 1354.166667}, 0.001)); - ASSERT_TRUE(model.C().isApprox( - Eigen::Matrix{{1.0, 0.0}, {0.0, 1.0}}, 0.001)); - ASSERT_TRUE(model.D().isApprox(Eigen::Matrix{0.0, 0.0}, 0.001)); + model.A().isApprox(frc::Matrixd<2, 2>{{0, 1}, {0, -26.87032}}, 0.001)); + ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0, 1354.166667}, 0.001)); + ASSERT_TRUE( + model.C().isApprox(frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001)); + ASSERT_TRUE(model.D().isApprox(frc::Matrixd<2, 1>{0.0, 0.0}, 0.001)); } TEST(LinearSystemIDTest, IdentifyPositionSystem) { @@ -70,9 +64,8 @@ TEST(LinearSystemIDTest, IdentifyPositionSystem) { kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq); ASSERT_TRUE(model.A().isApprox( - Eigen::Matrix{{0.0, 1.0}, {0.0, -kv / ka}}, 0.001)); - ASSERT_TRUE( - model.B().isApprox(Eigen::Matrix{0.0, 1.0 / ka}, 0.001)); + frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -kv / ka}}, 0.001)); + ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0.0, 1.0 / ka}, 0.001)); } TEST(LinearSystemIDTest, IdentifyVelocitySystem) { @@ -84,6 +77,6 @@ TEST(LinearSystemIDTest, IdentifyVelocitySystem) { auto model = frc::LinearSystemId::IdentifyVelocitySystem( kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq); - ASSERT_TRUE(model.A().isApprox(Eigen::Matrix{-kv / ka}, 0.001)); - ASSERT_TRUE(model.B().isApprox(Eigen::Matrix{1.0 / ka}, 0.001)); + ASSERT_TRUE(model.A().isApprox(frc::Matrixd<1, 1>{-kv / ka}, 0.001)); + ASSERT_TRUE(model.B().isApprox(frc::Matrixd<1, 1>{1.0 / ka}, 0.001)); } diff --git a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp index fd9c039ca0..f8e475fa17 100644 --- a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp +++ b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp @@ -10,48 +10,46 @@ // Tests that integrating dx/dt = e^x works. TEST(NumericalIntegrationTest, Exponential) { - Eigen::Vector y0{0.0}; + frc::Vectord<1> y0{0.0}; - Eigen::Vector y1 = frc::RK4( - [](const Eigen::Vector& x) { - return Eigen::Vector{std::exp(x(0))}; - }, + frc::Vectord<1> y1 = frc::RK4( + [](const frc::Vectord<1>& x) { return frc::Vectord<1>{std::exp(x(0))}; }, y0, 0.1_s); EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3); } // Tests that integrating dx/dt = e^x works when we provide a U. TEST(NumericalIntegrationTest, ExponentialWithU) { - Eigen::Vector y0{0.0}; + frc::Vectord<1> y0{0.0}; - Eigen::Vector y1 = frc::RK4( - [](const Eigen::Vector& x, const Eigen::Vector& u) { - return Eigen::Vector{std::exp(u(0) * x(0))}; + frc::Vectord<1> y1 = frc::RK4( + [](const frc::Vectord<1>& x, const frc::Vectord<1>& u) { + return frc::Vectord<1>{std::exp(u(0) * x(0))}; }, - y0, Eigen::Vector{1.0}, 0.1_s); + y0, frc::Vectord<1>{1.0}, 0.1_s); EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3); } // Tests that integrating dx/dt = e^x works with RKF45. TEST(NumericalIntegrationTest, ExponentialRKF45) { - Eigen::Vector y0{0.0}; + frc::Vectord<1> y0{0.0}; - Eigen::Vector y1 = frc::RKF45( - [](const Eigen::Vector& x, const Eigen::Vector& u) { - return Eigen::Vector{std::exp(x(0))}; + frc::Vectord<1> y1 = frc::RKF45( + [](const frc::Vectord<1>& x, const frc::Vectord<1>& u) { + return frc::Vectord<1>{std::exp(x(0))}; }, - y0, Eigen::Vector{0.0}, 0.1_s); + y0, frc::Vectord<1>{0.0}, 0.1_s); EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3); } // Tests that integrating dx/dt = e^x works with RKDP TEST(NumericalIntegrationTest, ExponentialRKDP) { - Eigen::Vector y0{0.0}; + frc::Vectord<1> y0{0.0}; - Eigen::Vector y1 = frc::RKDP( - [](const Eigen::Vector& x, const Eigen::Vector& u) { - return Eigen::Vector{std::exp(x(0))}; + frc::Vectord<1> y1 = frc::RKDP( + [](const frc::Vectord<1>& x, const frc::Vectord<1>& u) { + return frc::Vectord<1>{std::exp(x(0))}; }, - y0, Eigen::Vector{0.0}, 0.1_s); + y0, frc::Vectord<1>{0.0}, 0.1_s); EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3); } diff --git a/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp b/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp index 4e64825e12..513684b755 100644 --- a/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp +++ b/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp @@ -6,53 +6,46 @@ #include "frc/system/NumericalJacobian.h" -Eigen::Matrix A{ - {1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}, {1, 1, 3, 7}}; -Eigen::Matrix B{{1, 1}, {2, 1}, {3, 2}, {3, 7}}; +frc::Matrixd<4, 4> A{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}, {1, 1, 3, 7}}; +frc::Matrixd<4, 2> B{{1, 1}, {2, 1}, {3, 2}, {3, 7}}; // Function from which to recover A and B -Eigen::Vector AxBuFn(const Eigen::Vector& x, - const Eigen::Vector& u) { +frc::Vectord<4> AxBuFn(const frc::Vectord<4>& x, const frc::Vectord<2>& u) { return A * x + B * u; } // Test that we can recover A from AxBuFn() pretty accurately TEST(NumericalJacobianTest, Ax) { - Eigen::Matrix newA = - frc::NumericalJacobianX<4, 4, 2>(AxBuFn, Eigen::Vector::Zero(), - Eigen::Vector::Zero()); + frc::Matrixd<4, 4> newA = frc::NumericalJacobianX<4, 4, 2>( + AxBuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero()); EXPECT_TRUE(newA.isApprox(A)); } // Test that we can recover B from AxBuFn() pretty accurately TEST(NumericalJacobianTest, Bu) { - Eigen::Matrix newB = - frc::NumericalJacobianU<4, 4, 2>(AxBuFn, Eigen::Vector::Zero(), - Eigen::Vector::Zero()); + frc::Matrixd<4, 2> newB = frc::NumericalJacobianU<4, 4, 2>( + AxBuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero()); EXPECT_TRUE(newB.isApprox(B)); } -Eigen::Matrix C{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}}; -Eigen::Matrix D{{1, 1}, {2, 1}, {3, 2}}; +frc::Matrixd<3, 4> C{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}}; +frc::Matrixd<3, 2> D{{1, 1}, {2, 1}, {3, 2}}; // Function from which to recover C and D -Eigen::Vector CxDuFn(const Eigen::Vector& x, - const Eigen::Vector& u) { +frc::Vectord<3> CxDuFn(const frc::Vectord<4>& x, const frc::Vectord<2>& u) { return C * x + D * u; } // Test that we can recover C from CxDuFn() pretty accurately TEST(NumericalJacobianTest, Cx) { - Eigen::Matrix newC = - frc::NumericalJacobianX<3, 4, 2>(CxDuFn, Eigen::Vector::Zero(), - Eigen::Vector::Zero()); + frc::Matrixd<3, 4> newC = frc::NumericalJacobianX<3, 4, 2>( + CxDuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero()); EXPECT_TRUE(newC.isApprox(C)); } // Test that we can recover D from CxDuFn() pretty accurately TEST(NumericalJacobianTest, Du) { - Eigen::Matrix newD = - frc::NumericalJacobianU<3, 4, 2>(CxDuFn, Eigen::Vector::Zero(), - Eigen::Vector::Zero()); + frc::Matrixd<3, 2> newD = frc::NumericalJacobianU<3, 4, 2>( + CxDuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero()); EXPECT_TRUE(newD.isApprox(D)); } diff --git a/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp index f1be861580..d5311f85ab 100644 --- a/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp +++ b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp @@ -9,9 +9,9 @@ #include "frc/system/RungeKuttaTimeVarying.h" namespace { -Eigen::Vector RungeKuttaTimeVaryingSolution(double t) { - return Eigen::Vector{12.0 * std::exp(t) / - (std::pow(std::exp(t) + 1.0, 2.0))}; +frc::Vectord<1> RungeKuttaTimeVaryingSolution(double t) { + return frc::Vectord<1>{12.0 * std::exp(t) / + (std::pow(std::exp(t) + 1.0, 2.0))}; } } // namespace @@ -23,12 +23,12 @@ Eigen::Vector RungeKuttaTimeVaryingSolution(double t) { // // x(t) = 12 * e^t / ((e^t + 1)^2) TEST(RungeKuttaTimeVaryingTest, RungeKuttaTimeVarying) { - Eigen::Vector y0 = RungeKuttaTimeVaryingSolution(5.0); + frc::Vectord<1> y0 = RungeKuttaTimeVaryingSolution(5.0); - Eigen::Vector y1 = frc::RungeKuttaTimeVarying( - [](units::second_t t, const Eigen::Vector& x) { - return Eigen::Vector{ - x(0) * (2.0 / (std::exp(t.value()) + 1.0) - 1.0)}; + frc::Vectord<1> y1 = frc::RungeKuttaTimeVarying( + [](units::second_t t, const frc::Vectord<1>& x) { + return frc::Vectord<1>{x(0) * + (2.0 / (std::exp(t.value()) + 1.0) - 1.0)}; }, 5_s, y0, 1_s); EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3); diff --git a/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h b/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h index 23a20e80d9..5e7b165708 100644 --- a/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h +++ b/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h @@ -6,7 +6,7 @@ #include -#include "Eigen/Core" +#include "frc/EigenCore.h" #include "units/time.h" namespace frc {