From 21d949daa8d4546bedf06f78d36526708de84bee Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 2 Oct 2020 17:37:26 -0700 Subject: [PATCH] [wpilibc] Add LinearSystemLoop C++ ctor to match Java (#2755) Co-authored-by: Prateek Machiraju --- .../cpp/examples/StateSpaceArm/cpp/Robot.cpp | 8 +- .../examples/StateSpaceElevator/cpp/Robot.cpp | 7 +- .../examples/StateSpaceFlywheel/cpp/Robot.cpp | 7 +- .../StateSpaceFlywheelSysId/cpp/Robot.cpp | 7 +- .../LinearPlantInversionFeedforward.h | 4 - .../include/frc/system/LinearSystemLoop.h | 91 ++++++++++++++----- .../src/test/native/cpp/StateSpaceTest.cpp | 4 +- 7 files changed, 74 insertions(+), 54 deletions(-) diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp index 90d4d102e4..5efe04c6b4 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp @@ -79,14 +79,10 @@ class Robot : public frc::TimedRobot { // using notifiers. 20_ms}; - // Plant-inversion feedforward calculates the voltages necessary to reach our - // reference. - frc::LinearPlantInversionFeedforward<2, 1> m_feedforward{m_armPlant, 20_ms}; - // The state-space loop combines a controller, observer, feedforward and plant // for easy control. - frc::LinearSystemLoop<2, 1, 1> m_loop{m_armPlant, m_controller, m_feedforward, - m_observer, 12_V}; + frc::LinearSystemLoop<2, 1, 1> m_loop{m_armPlant, m_controller, m_observer, + 12_V, 20_ms}; // An encoder set up to measure arm position in radians per second. frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel}; diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp index ac94c6fafa..e0eda4c99c 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp @@ -76,15 +76,10 @@ class Robot : public frc::TimedRobot { // using notifiers. 20_ms}; - // Plant-inversion feedforward calculates the voltages necessary to reach our - // reference. - frc::LinearPlantInversionFeedforward<2, 1> m_feedforward{m_elevatorPlant, - 20_ms}; - // The state-space loop combines a controller, observer, feedforward and plant // for easy control. frc::LinearSystemLoop<2, 1, 1> m_loop{m_elevatorPlant, m_controller, - m_feedforward, m_observer, 12_V}; + m_observer, 12_V, 20_ms}; // An encoder set up to measure elevator height in meters. frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel}; diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp index b066ac68c8..ca3d90907d 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp @@ -72,15 +72,10 @@ class Robot : public frc::TimedRobot { // using notifiers. 20_ms}; - // Plant-inversion feedforward calculates the voltages necessary to reach our - // reference. - frc::LinearPlantInversionFeedforward<1, 1> m_feedforward{m_flywheelPlant, - 20_ms}; - // The state-space loop combines a controller, observer, feedforward and plant // for easy control. frc::LinearSystemLoop<1, 1, 1> m_loop{m_flywheelPlant, m_controller, - m_feedforward, m_observer, 12_V}; + m_observer, 12_V, 20_ms}; // An encoder set up to measure flywheel velocity in radians per second. frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel}; diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp index 7721decd5d..5487a899e1 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp @@ -72,15 +72,10 @@ class Robot : public frc::TimedRobot { // using notifiers. 20_ms}; - // Plant-inversion feedforward calculates the voltages necessary to reach our - // reference. - frc::LinearPlantInversionFeedforward<1, 1> m_feedforward{m_flywheelPlant, - 20_ms}; - // The state-space loop combines a controller, observer, feedforward and plant // for easy control. frc::LinearSystemLoop<1, 1, 1> m_loop{m_flywheelPlant, m_controller, - m_feedforward, m_observer, 12_V}; + m_observer, 12_V, 20_ms}; // An encoder set up to measure flywheel velocity in radians per second. frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel}; diff --git a/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h index 4bdfff13bc..ea86d90350 100644 --- a/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h @@ -59,10 +59,6 @@ class LinearPlantInversionFeedforward { Reset(m_r); } - LinearPlantInversionFeedforward(LinearPlantInversionFeedforward&&) = default; - LinearPlantInversionFeedforward& operator=( - LinearPlantInversionFeedforward&&) = default; - /** * Returns the previously calculated feedforward as an input vector. * diff --git a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h index fc370c097a..01ef1adb3a 100644 --- a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h +++ b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h @@ -37,20 +37,68 @@ class LinearSystemLoop { /** * Constructs a state-space loop with the given plant, controller, and * observer. By default, the initial reference is all zeros. Users should - * call reset with the initial system state before enabling the loop. + * call reset with the initial system state before enabling the loop. This + * constructor assumes that the input(s) to this system are voltage. * * @param plant State-space plant. * @param controller State-space controller. - * @param feedforward Plant inversion feedforward. * @param observer State-space observer. - * @param maxVoltageVolts The maximum voltage that can be applied. Assumes - * that the inputs are voltages. + * @param maxVoltage The maximum voltage that can be applied. Commonly 12. + * @param dt The nominal timestep. */ LinearSystemLoop(LinearSystem& plant, LinearQuadraticRegulator& controller, - LinearPlantInversionFeedforward& feedforward, KalmanFilter& observer, - units::volt_t maxVoltage) + units::volt_t maxVoltage, units::second_t dt) + : LinearSystemLoop( + plant, controller, observer, + [=](Eigen::Matrix u) { + return frc::NormalizeInputVector( + u, maxVoltage.template to()); + }, + dt) {} + + /** + * Constructs a state-space loop with the given plant, controller, and + * observer. By default, the initial reference is all zeros. Users should + * call reset with the initial system state before enabling the loop. This + * constructor assumes that the input(s) to this system are voltage. + * + * @param plant State-space plant. + * @param controller State-space controller. + * @param observer State-space observer. + * @param clampFunction The function used to clamp the input vector. + * @param dt The nominal timestep. + */ + LinearSystemLoop(LinearSystem& plant, + LinearQuadraticRegulator& controller, + KalmanFilter& observer, + std::function( + const Eigen::Matrix&)> + clampFunction, + units::second_t dt) + : LinearSystemLoop( + plant, controller, + LinearPlantInversionFeedforward{plant, dt}, + observer, clampFunction) {} + + /** + * Constructs a state-space loop with the given plant, controller, and + * observer. By default, the initial reference is all zeros. Users should + * call reset with the initial system state before enabling the loop. + * + * @param plant State-space plant. + * @param controller State-space controller. + * @param feedforward Plant inversion feedforward. + * @param observer State-space observer. + * @param maxVoltage The maximum voltage that can be applied. Assumes + * that the inputs are voltages. + */ + LinearSystemLoop( + LinearSystem& plant, + LinearQuadraticRegulator& controller, + const LinearPlantInversionFeedforward& feedforward, + KalmanFilter& observer, units::volt_t maxVoltage) : LinearSystemLoop(plant, controller, feedforward, observer, [=](Eigen::Matrix u) { return frc::NormalizeInputVector( @@ -61,18 +109,20 @@ class LinearSystemLoop { * Constructs a state-space loop with the given plant, controller, and * observer. * - * @param plant State-space plant. - * @param controller State-space controller. - * @param feedforward Plant-inversion feedforward. - * @param observer State-space observer. + * @param plant State-space plant. + * @param controller State-space controller. + * @param feedforward Plant-inversion feedforward. + * @param observer State-space observer. + * @param clampFunction The function used to clamp the input vector. */ - LinearSystemLoop(LinearSystem& plant, - LinearQuadraticRegulator& controller, - LinearPlantInversionFeedforward& feedforward, - KalmanFilter& observer, - std::function( - const Eigen::Matrix&)> - clampFunction) + LinearSystemLoop( + LinearSystem& plant, + LinearQuadraticRegulator& controller, + const LinearPlantInversionFeedforward& feedforward, + KalmanFilter& observer, + std::function( + const Eigen::Matrix&)> + clampFunction) : m_plant(plant), m_controller(controller), m_feedforward(feedforward), @@ -82,11 +132,6 @@ class LinearSystemLoop { Reset(m_nextR); } - virtual ~LinearSystemLoop() = default; - - LinearSystemLoop(LinearSystemLoop&&) = default; - LinearSystemLoop& operator=(LinearSystemLoop&&) = default; - /** * Returns the observer's state estimate x-hat. */ @@ -240,7 +285,7 @@ class LinearSystemLoop { protected: LinearSystem& m_plant; LinearQuadraticRegulator& m_controller; - LinearPlantInversionFeedforward& m_feedforward; + LinearPlantInversionFeedforward m_feedforward; KalmanFilter& m_observer; /** diff --git a/wpimath/src/test/native/cpp/StateSpaceTest.cpp b/wpimath/src/test/native/cpp/StateSpaceTest.cpp index fa16c18cca..a1600fca6f 100644 --- a/wpimath/src/test/native/cpp/StateSpaceTest.cpp +++ b/wpimath/src/test/native/cpp/StateSpaceTest.cpp @@ -43,9 +43,7 @@ class StateSpace : public testing::Test { }(); LinearQuadraticRegulator<2, 1> controller{plant, {0.02, 0.4}, {12.0}, kDt}; KalmanFilter<2, 1, 1> observer{plant, {0.05, 1.0}, {0.0001}, kDt}; - LinearPlantInversionFeedforward<2, 1> feedforward{plant, kDt}; - LinearSystemLoop<2, 1, 1> loop{plant, controller, feedforward, observer, - 12_V}; + LinearSystemLoop<2, 1, 1> loop{plant, controller, observer, 12_V, kDt}; }; void Update(LinearSystemLoop<2, 1, 1>& loop, double noise) {