diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp index 6d5c5bd8fc..90d4d102e4 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp @@ -70,10 +70,6 @@ class Robot : public frc::TimedRobot { // Decrease this to more heavily penalize state excursion, or make the // controller behave more aggressively. {1.0 * 2.0 * wpi::math::pi / 360.0, 10.0 * 2.0 * wpi::math::pi / 360.0}, - // rho balances Q and R. Increasing this will penalize state excursion - // more heavily, while decreasing this will penalize control effort more - // heavily. - 1.0, // relms. Control effort (voltage) tolerance. Decrease this to more // heavily penalize control effort, or make the controller less // aggressive. 12 is a good starting point because that is the diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp index 4c70fbc4f0..ac94c6fafa 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp @@ -67,10 +67,6 @@ class Robot : public frc::TimedRobot { // Decrease this to more heavily penalize state excursion, or make the // controller behave more aggressively. {0.0254, 0.254}, - // rho balances Q and R. Increasing this will penalize state excursion - // more heavily, while decreasing this will penalize control effort more - // heavily. - 1.0, // relms. Control effort (voltage) tolerance. Decrease this to more // heavily penalize control effort, or make the controller less // aggressive. 12 is a good starting point because that is the diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp index e143df8bd8..b066ac68c8 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp @@ -63,10 +63,6 @@ class Robot : public frc::TimedRobot { // to more heavily penalize state excursion, or make the controller behave // more aggressively. {8.0}, - // rho balances Q and R, or velocity and voltage weights. Increasing this - // will penalize state excursion more heavily, while decreasing this will - // penalize control effort more heavily. - 1.0, // relms. Control effort (voltage) tolerance. Decrease this to more // heavily penalize control effort, or make the controller less // aggressive. 12 is a good starting point because that is the diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp index 34f4246a6d..7721decd5d 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp @@ -63,10 +63,6 @@ class Robot : public frc::TimedRobot { // to more heavily penalize state excursion, or make the controller behave // more aggressively. {8.0}, - // rho balances Q and R, or velocity and voltage weights. Increasing this - // will penalize state excursion more heavily, while decreasing this will - // penalize control effort more heavily. - 1.0, // relms. Control effort (voltage) tolerance. Decrease this to more // heavily penalize control effort, or make the controller less // aggressive. 12 is a good starting point because that is the diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java index 5875567581..508102dd52 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java @@ -78,10 +78,6 @@ public class Robot extends TimedRobot { // to more heavily penalize state excursion, or make the controller behave more // aggressively. In this example we weight position much more highly than velocity, but this // can be tuned to balance the two. - 1.0, // rho balances Q and R, or velocity and voltage weights. Increasing this - // will penalize state excursion more heavily, while decreasing this will penalize control - // effort more heavily. Useful for balancing weights for systems with more states such - // as drivetrains. VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more // heavily penalize control effort, or make the controller less aggressive. 12 is a good // starting point because that is the (approximate) maximum voltage of a battery. diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java index 4c59e89412..f6836a3de3 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java @@ -80,10 +80,6 @@ public class Robot extends TimedRobot { // heavily penalize state excursion, or make the controller behave more aggressively. In // this example we weight position much more highly than velocity, but this can be // tuned to balance the two. - 1.0, // rho balances Q and R, or velocity and voltage weights. Increasing this - // will penalize state excursion more heavily, while decreasing this will penalize control - // effort more heavily. Useful for balancing weights for systems with more states such - // as drivetrains. VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more // heavily penalize control effort, or make the controller less aggressive. 12 is a good // starting point because that is the (approximate) maximum voltage of a battery. diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java index 4a3a0a4d63..f9d113123a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java @@ -65,10 +65,6 @@ public class Robot extends TimedRobot { VecBuilder.fill(8.0), // qelms. Velocity error tolerance, in radians per second. Decrease // this to more heavily penalize state excursion, or make the controller behave more // aggressively. - 1.0, // rho balances Q and R, or velocity and voltage weights. Increasing this - // will penalize state excursion more heavily, while decreasing this will penalize control - // effort more heavily. Useful for balancing weights for systems with more states such - // as drivetrains. VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more // heavily penalize control effort, or make the controller less aggressive. 12 is a good // starting point because that is the (approximate) maximum voltage of a battery. diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulator.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulator.java index 1db5eba5e1..195ba4f3bc 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulator.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulator.java @@ -61,28 +61,8 @@ public class LinearQuadraticRegulator relms, double dtSeconds ) { - this(plant.getA(), plant.getB(), qelms, 1.0, relms, dtSeconds); - } - - /** - * Constructs a controller with the given coefficients and plant. - * - * @param plant The plant being controlled. - * @param qelms The maximum desired error tolerance for each state. - * @param rho A weighting factor that balances control effort and state excursion. - * Greater values penalize state excursion more heavily. 1 is a good starting - * value. - * @param relms The maximum desired control effort for each input. - * @param dtSeconds Discretization timestep. - */ - public LinearQuadraticRegulator( - LinearSystem plant, - Vector qelms, - double rho, - Vector relms, - double dtSeconds - ) { - this(plant.getA(), plant.getB(), qelms, rho, relms, dtSeconds); + this(plant.getA(), plant.getB(), StateSpaceUtil.makeCostMatrix(qelms), + StateSpaceUtil.makeCostMatrix(relms), dtSeconds); } /** @@ -99,28 +79,8 @@ public class LinearQuadraticRegulator qelms, Vector relms, double dtSeconds ) { - this(A, B, qelms, 1.0, relms, dtSeconds); - } - - /** - * Constructs a controller with the given coefficients and plant. - * - * @param A Continuous system matrix of the plant being controlled. - * @param B Continuous input matrix of the plant being controlled. - * @param qelms The maximum desired error tolerance for each state. - * @param rho A weighting factor that balances control effort and state excursion. - * Greater - * values penalize state excursion more heavily. 1 is a good starting value. - * @param relms The maximum desired control effort for each input. - * @param dtSeconds Discretization timestep. - */ - @SuppressWarnings({"ParameterName", "LocalVariableName"}) - public LinearQuadraticRegulator(Matrix A, Matrix B, - Vector qelms, double rho, Vector relms, - double dtSeconds - ) { - this(A, B, StateSpaceUtil.makeCostMatrix(qelms).times(rho), - StateSpaceUtil.makeCostMatrix(relms), dtSeconds); + this(A, B, StateSpaceUtil.makeCostMatrix(qelms), StateSpaceUtil.makeCostMatrix(relms), + dtSeconds); } /** diff --git a/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp b/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp index 543a2e95a9..ae58440fdf 100644 --- a/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp +++ b/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp @@ -13,14 +13,8 @@ LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator( const Eigen::Matrix& A, const Eigen::Matrix& B, const std::array& Qelems, const std::array& Relems, units::second_t dt) - : LinearQuadraticRegulator(A, B, Qelems, 1.0, Relems, dt) {} - -LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator( - const Eigen::Matrix& A, const Eigen::Matrix& B, - const std::array& Qelems, const double rho, - const std::array& Relems, units::second_t dt) - : detail::LinearQuadraticRegulatorImpl<1, 1>{A, B, Qelems, - rho, Relems, dt} {} + : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems), + MakeCostMatrix(Relems), dt) {} LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator( const Eigen::Matrix& A, const Eigen::Matrix& B, @@ -32,14 +26,8 @@ LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator( const Eigen::Matrix& A, const Eigen::Matrix& B, const std::array& Qelems, const std::array& Relems, units::second_t dt) - : LinearQuadraticRegulator(A, B, Qelems, 1.0, Relems, dt) {} - -LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator( - const Eigen::Matrix& A, const Eigen::Matrix& B, - const std::array& Qelems, const double rho, - const std::array& Relems, units::second_t dt) - : detail::LinearQuadraticRegulatorImpl<2, 1>{A, B, Qelems, - rho, Relems, dt} {} + : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems), + MakeCostMatrix(Relems), dt) {} LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator( const Eigen::Matrix& A, const Eigen::Matrix& B, diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h index b6b8d9a87f..31f60bd3f1 100644 --- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h +++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h @@ -44,27 +44,8 @@ class LinearQuadraticRegulatorImpl { const LinearSystem& plant, const std::array& Qelems, const std::array& Relems, units::second_t dt) - : LinearQuadraticRegulatorImpl(plant.A(), plant.B(), Qelems, 1.0, Relems, - dt) {} - - /** - * Constructs a controller with the given coefficients and plant. - * - * @param plant The plant being controlled. - * @param Qelems The maximum desired error tolerance for each state. - * @param rho A weighting factor that balances control effort and state - * excursion. Greater values penalize state excursion more heavily. 1 is a - * good starting value. - * @param Relems The maximum desired control effort for each input. - * @param dt Discretization timestep. - */ - template - LinearQuadraticRegulatorImpl( - const LinearSystem& plant, - const std::array& Qelems, const double rho, - const std::array& Relems, units::second_t dt) - : LinearQuadraticRegulatorImpl(plant.A(), plant.B(), Qelems, rho, Relems, - dt) {} + : LinearQuadraticRegulatorImpl(plant.A(), plant.B(), Qelems, Relems, dt) { + } /** * Constructs a controller with the given coefficients and plant. @@ -72,9 +53,6 @@ class LinearQuadraticRegulatorImpl { * @param A Continuous system matrix of the plant being controlled. * @param B Continuous input matrix of the plant being controlled. * @param Qelems The maximum desired error tolerance for each state. - * @param rho A weighting factor that balances control effort and state - * excursion. Greater values penalize state excursion more heavily. 1 is a - * good starting value. * @param Relems The maximum desired control effort for each input. * @param dt Discretization timestep. */ @@ -83,27 +61,7 @@ class LinearQuadraticRegulatorImpl { const std::array& Qelems, const std::array& Relems, units::second_t dt) - : LinearQuadraticRegulatorImpl(A, B, Qelems, 1.0, Relems, dt) {} - - /** - * Constructs a controller with the given coefficients and plant. - * - * @param A Continuous system matrix of the plant being controlled. - * @param B Continuous input matrix of the plant being controlled. - * @param Qelems The maximum desired error tolerance for each state. - * @param rho A weighting factor that balances control effort and state - * excursion. Greater values penalize state excursion more heavily. 1 is a - * good starting value. - * @param Relems The maximum desired control effort for each input. - * @param dt Discretization timestep. - */ - LinearQuadraticRegulatorImpl(const Eigen::Matrix& A, - const Eigen::Matrix& B, - const std::array& Qelems, - const double rho, - const std::array& Relems, - units::second_t dt) - : LinearQuadraticRegulatorImpl(A, B, MakeCostMatrix(Qelems) * rho, + : LinearQuadraticRegulatorImpl(A, B, MakeCostMatrix(Qelems), MakeCostMatrix(Relems), dt) {} /** @@ -244,28 +202,7 @@ class LinearQuadraticRegulator const std::array& Qelems, const std::array& Relems, units::second_t dt) - : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, 1.0, Relems, - dt) {} - - /** - * Constructs a controller with the given coefficients and plant. - * - * @param system The plant being controlled. - * @param Qelems The maximum desired error tolerance for each state. - * @param rho A weighting factor that balances control effort and state - * excursion. Greater values penalize state excursion more heavily. 1 is a - * good starting value. - * @param Relems The maximum desired control effort for each input. - * @param dt Discretization timestep. - */ - template - LinearQuadraticRegulator(const LinearSystem& plant, - const std::array& Qelems, - const double rho, - const std::array& Relems, - units::second_t dt) - : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, rho, Relems, - dt) {} + : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {} /** * Constructs a controller with the given coefficients and plant. @@ -273,9 +210,6 @@ class LinearQuadraticRegulator * @param A Continuous system matrix of the plant being controlled. * @param B Continuous input matrix of the plant being controlled. * @param Qelems The maximum desired error tolerance for each state. - * @param rho A weighting factor that balances control effort and state - * excursion. Greater values penalize state excursion more heavily. 1 is a - * good starting value. * @param Relems The maximum desired control effort for each input. * @param dt Discretization timestep. */ @@ -284,28 +218,8 @@ class LinearQuadraticRegulator const std::array& Qelems, const std::array& Relems, units::second_t dt) - : LinearQuadraticRegulator(A, B, Qelems, 1.0, Relems, dt) {} - - /** - * Constructs a controller with the given coefficients and plant. - * - * @param A Continuous system matrix of the plant being controlled. - * @param B Continuous input matrix of the plant being controlled. - * @param Qelems The maximum desired error tolerance for each state. - * @param rho A weighting factor that balances control effort and state - * excursion. Greater values penalize state excursion more heavily. 1 is a - * good starting value. - * @param Relems The maximum desired control effort for each input. - * @param dt Discretization timestep. - */ - LinearQuadraticRegulator(const Eigen::Matrix& A, - const Eigen::Matrix& B, - const std::array& Qelems, - const double rho, - const std::array& Relems, - units::second_t dt) - : detail::LinearQuadraticRegulatorImpl{ - A, B, Qelems, rho, Relems, dt} {} + : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems), + MakeCostMatrix(Relems), dt) {} /** * Constructs a controller with the given coefficients and plant. @@ -338,17 +252,7 @@ class LinearQuadraticRegulator<1, 1> const std::array& Qelems, const std::array& Relems, units::second_t dt) - : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, 1.0, Relems, - dt) {} - - template - LinearQuadraticRegulator(const LinearSystem<1, 1, Outputs>& plant, - const std::array& Qelems, - const double rho, - const std::array& Relems, - units::second_t dt) - : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, rho, Relems, - dt) {} + : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {} LinearQuadraticRegulator(const Eigen::Matrix& A, const Eigen::Matrix& B, @@ -356,13 +260,6 @@ class LinearQuadraticRegulator<1, 1> const std::array& Relems, units::second_t dt); - LinearQuadraticRegulator(const Eigen::Matrix& A, - const Eigen::Matrix& B, - const std::array& Qelems, - const double rho, - const std::array& Relems, - units::second_t dt); - LinearQuadraticRegulator(const Eigen::Matrix& A, const Eigen::Matrix& B, const Eigen::Matrix& Q, @@ -384,17 +281,7 @@ class LinearQuadraticRegulator<2, 1> const std::array& Qelems, const std::array& Relems, units::second_t dt) - : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, 1.0, Relems, - dt) {} - - template - LinearQuadraticRegulator(const LinearSystem<2, 1, Outputs>& plant, - const std::array& Qelems, - const double rho, - const std::array& Relems, - units::second_t dt) - : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, rho, Relems, - dt) {} + : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {} LinearQuadraticRegulator(const Eigen::Matrix& A, const Eigen::Matrix& B, @@ -402,13 +289,6 @@ class LinearQuadraticRegulator<2, 1> const std::array& Relems, units::second_t dt); - LinearQuadraticRegulator(const Eigen::Matrix& A, - const Eigen::Matrix& B, - const std::array& Qelems, - const double rho, - const std::array& Relems, - units::second_t dt); - LinearQuadraticRegulator(const Eigen::Matrix& A, const Eigen::Matrix& B, const Eigen::Matrix& Q,