From b26f1286767cd70dca86fc7563e05911c879533e Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Fri, 25 Feb 2022 21:00:38 -0800 Subject: [PATCH] Add ImplicitModelFollower --- .../controller/ImplicitModelFollower.java | 138 ++++++++++++++++++ .../frc/controller/ImplicitModelFollower.h | 137 +++++++++++++++++ .../frc/controller/LinearQuadraticRegulator.h | 2 +- 3 files changed, 276 insertions(+), 1 deletion(-) create mode 100644 wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java create mode 100644 wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java new file mode 100644 index 00000000000..e8343030a36 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java @@ -0,0 +1,138 @@ +// 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. + +package edu.wpi.first.math.controller; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Num; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.system.Discretization; +import edu.wpi.first.math.system.LinearSystem; +import org.ejml.simple.SimpleMatrix; + +/** + * Contains the controller coefficients and logic for an implicit model follower. + * + *

Implicit model following lets us design a feedback controller that erases the dynamics of our + * system and makes it behave like some other system. This can be used to make a drivetrain more + * controllable during teleop driving by making it behave like a slower or more benign drivetrain. + * + *

For more on the underlying math, read appendix B.3 in + * https://file.tavsys.net/control/controls-engineering-in-frc.pdf. + */ +@SuppressWarnings("ClassTypeParameterName") +public class ImplicitModelFollower { + // Computed controller output + @SuppressWarnings("MemberName") + private Matrix m_u; + + // State space conversion gain + @SuppressWarnings("MemberName") + private Matrix m_A; + + // Input space conversion gain + @SuppressWarnings("MemberName") + private Matrix m_B; + + /** + * Constructs a controller with the given coefficients and plant. + * + * @param plant The plant being controlled. + * @param plantRef The plant whose dynamics should be followed. + * @param dtSeconds Discretization timestep. + */ + public ImplicitModelFollower( + LinearSystem plant, + LinearSystem plantRef, + double dtSeconds) { + this(plant.getA(), plant.getB(), plantRef.getA(), plantRef.getB(), 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 Aref Continuous system matrix whose dynamics should be followed. + * @param Bref Continuous input matrix whose dynamics should be followed. + * @param dtSeconds Discretization timestep. + */ + @SuppressWarnings("ParameterName") + public ImplicitModelFollower( + Matrix A, + Matrix B, + Matrix Aref, + Matrix Bref, + double dtSeconds) { + m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1)); + + // Discretize real dynamics + var discABPair = Discretization.discretizeAB(A, B, dtSeconds); + var discA = discABPair.getFirst(); + var discB = discABPair.getSecond(); + + // Discretize desired dynamics + var discABrefPair = Discretization.discretizeAB(Aref, Bref, dtSeconds); + var discAref = discABrefPair.getFirst(); + var discBref = discABrefPair.getSecond(); + + // Find u_imf that makes real model match reference model. + // + // x_k+1 = Ax_k + Bu_imf + // z_k+1 = Aref z_k + Bref u_k + // + // Let x_k = z_k. + // + // x_k+1 = z_k+1 + // Ax_k + Bu_imf = Aref x_k + Bref u_k + // Bu_imf = Aref x_k - Ax_k + Bref u_k + // Bu_imf = (Aref - A)x_k + Bref u_k + // u_imf = B^+ ((Aref - A)x_k + Bref u_k) + // u_imf = -B^+ (A - Aref)x_k + B^+ Bref u_k + + // The first term makes the open-loop poles that of the reference + // system, and the second term makes the input behave like that of the + // reference system. + m_A = discB.solve(discA.minus(discAref)).times(-1.0); + m_B = discB.solve(discBref); + + reset(); + } + + /** + * Returns the control input vector u. + * + * @return The control input. + */ + public Matrix getU() { + return m_u; + } + + /** + * Returns an element of the control input vector u. + * + * @param i Row of u. + * @return The row of the control input vector. + */ + public double getU(int i) { + return m_u.get(i, 0); + } + + /** Resets the controller. */ + public void reset() { + m_u.fill(0.0); + } + + /** + * Returns the next output of the controller. + * + * @param x The current state x. + * @param u The current input for the original model. + * @return The next controller output. + */ + public Matrix calculate(Matrix x, Matrix u) { + m_u = m_A.times(x).plus(m_B.times(u)); + return m_u; + } +} diff --git a/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h b/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h new file mode 100644 index 00000000000..e8ebcf620be --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h @@ -0,0 +1,137 @@ +// 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 +#include + +#include "Eigen/Core" +#include "Eigen/QR" +#include "units/time.h" + +namespace frc { + +/** + * Contains the controller coefficients and logic for an implicit model + * follower. + * + * Implicit model following lets us design a feedback controller that erases the + * dynamics of our system and makes it behave like some other system. This can + * be used to make a drivetrain more controllable during teleop driving by + * making it behave like a slower or more benign drivetrain. + * + * For more on the underlying math, read appendix B.3 in + * https://file.tavsys.net/control/controls-engineering-in-frc.pdf. + */ +template +class ImplicitModelFollower { + public: + /** + * Constructs a controller with the given coefficients and plant. + * + * @param plant The plant being controlled. + * @param plantRef The plant whose dynamics should be followed. + * @param dt Discretization timestep. + */ + template + ImplicitModelFollower(const LinearSystem& plant, + const LinearSystem& plantRef, + units::second_t dt) + : ImplicitModelFollower(plant.A(), plant.B(), + plantRef.A(), plantRef.B(), 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 Aref Continuous system matrix whose dynamics should be followed. + * @param Bref Continuous input matrix whose dynamics should be followed. + * @param dt Discretization timestep. + */ + ImplicitModelFollower(const Eigen::Matrix& A, + const Eigen::Matrix& B, + const Eigen::Matrix& Aref, + const Eigen::Matrix& Bref, + units::second_t dt) { + // Discretize real dynamics + Eigen::Matrix discA; + Eigen::Matrix discB; + frc::DiscretizeAB(A, B, dt, &discA, &discB); + + // Discretize desired dynamics + Eigen::Matrix discAref; + Eigen::Matrix discBref; + frc::DiscretizeAB(Aref, Bref, dt, &discAref, &discBref); + + // Find u_imf that makes real model match reference model. + // + // x_k+1 = Ax_k + Bu_imf + // z_k+1 = Aref z_k + Bref u_k + // + // Let x_k = z_k. + // + // x_k+1 = z_k+1 + // Ax_k + Bu_imf = Aref x_k + Bref u_k + // Bu_imf = Aref x_k - Ax_k + Bref u_k + // Bu_imf = (Aref - A)x_k + Bref u_k + // u_imf = B^+ ((Aref - A)x_k + Bref u_k) + // u_imf = -B^+ (A - Aref)x_k + B^+ Bref u_k + + // The first term makes the open-loop poles that of the reference + // system, and the second term makes the input behave like that of the + // reference system. + m_A = -discB.householderQr().solve(discA - discAref); + m_B = discB.householderQr().solve(discBref); + + Reset(); + } + + /** + * Returns the control input vector u. + * + * @return The control input. + */ + const Eigen::Vector& U() const { return m_u; } + + /** + * Returns an element of the control input vector u. + * + * @param i Row of u. + * + * @return The row of the control input vector. + */ + double U(int i) const { return m_u(i); } + + /** + * Resets the controller. + */ + void Reset() { m_u.setZero(); } + + /** + * Returns the next output of the controller. + * + * @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) { + m_u = m_A * x + m_B * u; + return m_u; + } + + private: + // Computed controller output + Eigen::Vector m_u; + + // State space conversion gain + Eigen::Matrix m_A; + + // Input space conversion gain + Eigen::Matrix m_B; +}; + +} // 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 44a850178ba..59b07a25d03 100644 --- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h +++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h @@ -189,7 +189,7 @@ class LinearQuadraticRegulatorImpl { * * @return The row of the control input vector. */ - double U(int i) const { return m_u(i, 0); } + double U(int i) const { return m_u(i); } /** * Resets the controller.