diff --git a/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp b/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp index c3a00eadd8..4d2fbe92fb 100644 --- a/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp +++ b/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp @@ -44,4 +44,23 @@ LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator( const Eigen::Matrix& 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 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) + : 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) + : detail::LinearQuadraticRegulatorImpl<2, 2>(A, B, Q, R, N, dt) {} + } // 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 2101a3499d..8b6be757bc 100644 --- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h +++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h @@ -388,4 +388,40 @@ class LinearQuadraticRegulator<2, 1> LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default; }; +// Template specializations are used here to make common state-input pairs +// compile faster. +template <> +class LinearQuadraticRegulator<2, 2> + : public detail::LinearQuadraticRegulatorImpl<2, 2> { + public: + template + LinearQuadraticRegulator(const LinearSystem<2, 2, Outputs>& plant, + const wpi::array& Qelems, + const wpi::array& Relems, + units::second_t dt) + : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {} + + LinearQuadraticRegulator(const Eigen::Matrix& A, + const Eigen::Matrix& 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, + 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(LinearQuadraticRegulator&&) = default; + LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default; +}; + } // namespace frc