From 8d79dc873801ab4e2c90ae66d499402fbfceda47 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 20 Mar 2022 00:36:12 -0700 Subject: [PATCH] [wpimath] Add ImplicitModelFollower (#4056) --- .../controller/ImplicitModelFollower.java | 138 ++++++++++++++++++ .../frc/controller/ImplicitModelFollower.h | 137 +++++++++++++++++ .../controller/ImplicitModelFollowerTest.java | 109 ++++++++++++++ .../controller/ImplicitModelFollowerTest.cpp | 109 ++++++++++++++ 4 files changed, 493 insertions(+) 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 create mode 100644 wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java create mode 100644 wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp 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 0000000000..e8343030a3 --- /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 0000000000..e8ebcf620b --- /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/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java new file mode 100644 index 0000000000..e16dcb629d --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java @@ -0,0 +1,109 @@ +// 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 static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.plant.LinearSystemId; +import org.junit.jupiter.api.Test; + +class ImplicitModelFollowerTest { + @Test + @SuppressWarnings("LocalVariableName") + void testSameModel() { + final double dt = 0.005; + + var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0); + + var imf = new ImplicitModelFollower(plant, plant, dt); + + var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + + // Forward + var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0); + for (double t = 0.0; t < 3.0; t += dt) { + x = plant.calculateX(x, u, dt); + xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); + + assertEquals(x.get(0, 0), xImf.get(0, 0)); + assertEquals(x.get(1, 0), xImf.get(1, 0)); + } + + // Backward + u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0); + for (double t = 0.0; t < 3.0; t += dt) { + x = plant.calculateX(x, u, dt); + xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); + + assertEquals(x.get(0, 0), xImf.get(0, 0)); + assertEquals(x.get(1, 0), xImf.get(1, 0)); + } + + // Rotate CCW + u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0); + for (double t = 0.0; t < 3.0; t += dt) { + x = plant.calculateX(x, u, dt); + xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); + + assertEquals(x.get(0, 0), xImf.get(0, 0)); + assertEquals(x.get(1, 0), xImf.get(1, 0)); + } + } + + @Test + @SuppressWarnings("LocalVariableName") + void testSlowerRefModel() { + final double dt = 0.005; + + var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0); + + // Linear acceleration is slower, but angular acceleration is the same + var plantRef = LinearSystemId.identifyDrivetrainSystem(1.0, 2.0, 1.0, 1.0); + + var imf = new ImplicitModelFollower(plant, plantRef, dt); + + var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + + // Forward + var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0); + for (double t = 0.0; t < 3.0; t += dt) { + x = plant.calculateX(x, u, dt); + xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); + + assertTrue(x.get(0, 0) >= xImf.get(0, 0)); + assertTrue(x.get(1, 0) >= xImf.get(1, 0)); + } + + // Backward + x.fill(0.0); + xImf.fill(0.0); + u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0); + for (double t = 0.0; t < 3.0; t += dt) { + x = plant.calculateX(x, u, dt); + xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); + + assertTrue(x.get(0, 0) <= xImf.get(0, 0)); + assertTrue(x.get(1, 0) <= xImf.get(1, 0)); + } + + // Rotate CCW + x.fill(0.0); + xImf.fill(0.0); + u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0); + for (double t = 0.0; t < 3.0; t += dt) { + x = plant.calculateX(x, u, dt); + xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); + + assertEquals(x.get(0, 0), xImf.get(0, 0), 1e-5); + assertEquals(x.get(1, 0), xImf.get(1, 0), 1e-5); + } + } +} diff --git a/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp b/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp new file mode 100644 index 0000000000..05a6b7100c --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp @@ -0,0 +1,109 @@ +// 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. + +#include + +#include "frc/controller/ImplicitModelFollower.h" +#include "frc/system/plant/LinearSystemId.h" + +namespace frc { + +TEST(ImplicitModelFollowerTest, SameModel) { + constexpr auto dt = 5_ms; + + using Kv_t = decltype(1_V / 1_mps); + using Ka_t = decltype(1_V / 1_mps_sq); + auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0}, + Kv_t{1.0}, Ka_t{1.0}); + + ImplicitModelFollower<2, 2> imf{plant, plant, dt}; + + Eigen::Vector x{0.0, 0.0}; + Eigen::Vector xImf{0.0, 0.0}; + + // Forward + Eigen::Vector 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); + + EXPECT_DOUBLE_EQ(x(0), xImf(0)); + EXPECT_DOUBLE_EQ(x(1), xImf(1)); + } + + // Backward + u = Eigen::Vector{-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); + + EXPECT_DOUBLE_EQ(x(0), xImf(0)); + EXPECT_DOUBLE_EQ(x(1), xImf(1)); + } + + // Rotate CCW + u = Eigen::Vector{-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); + + EXPECT_DOUBLE_EQ(x(0), xImf(0)); + EXPECT_DOUBLE_EQ(x(1), xImf(1)); + } +} + +TEST(ImplicitModelFollowerTest, SlowerRefModel) { + constexpr auto dt = 5_ms; + + using Kv_t = decltype(1_V / 1_mps); + using Ka_t = decltype(1_V / 1_mps_sq); + + auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0}, + Kv_t{1.0}, Ka_t{1.0}); + + // Linear acceleration is slower, but angular acceleration is the same + auto plantRef = LinearSystemId::IdentifyDrivetrainSystem( + Kv_t{1.0}, Ka_t{2.0}, Kv_t{1.0}, Ka_t{1.0}); + + ImplicitModelFollower<2, 2> imf{plant, plantRef, dt}; + + Eigen::Vector x{0.0, 0.0}; + Eigen::Vector xImf{0.0, 0.0}; + + // Forward + Eigen::Vector 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); + + EXPECT_GE(x(0), xImf(0)); + EXPECT_GE(x(1), xImf(1)); + } + + // Backward + x.setZero(); + xImf.setZero(); + u = Eigen::Vector{-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); + + EXPECT_LE(x(0), xImf(0)); + EXPECT_LE(x(1), xImf(1)); + } + + // Rotate CCW + x.setZero(); + xImf.setZero(); + u = Eigen::Vector{-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); + + EXPECT_NEAR(x(0), xImf(0), 1e-5); + EXPECT_NEAR(x(1), xImf(1), 1e-5); + } +} + +} // namespace frc