[wpimath] Add ImplicitModelFollower (#4056)

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.
+ *
+ * <p>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.
+ *
+ * <p>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<States extends Num, Inputs extends Num, Outputs extends Num> {
+  // Computed controller output
+  @SuppressWarnings("MemberName")
+  private Matrix<Inputs, N1> m_u;
+
+  // State space conversion gain
+  @SuppressWarnings("MemberName")
+  private Matrix<Inputs, States> m_A;
+
+  // Input space conversion gain
+  @SuppressWarnings("MemberName")
+  private Matrix<Inputs, Inputs> 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<States, Inputs, Outputs> plant,
+      LinearSystem<States, Inputs, Outputs> 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<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<States, States> Aref,
+      Matrix<States, Inputs> 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<Inputs, N1> 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<Inputs, N1> calculate(Matrix<States, N1> x, Matrix<Inputs, N1> u) {
+    m_u = m_A.times(x).plus(m_B.times(u));
+    return m_u;
+  }
+}

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 <frc/system/Discretization.h>
+#include <frc/system/LinearSystem.h>
+
+#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 <int States, int Inputs>
+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 <int Outputs>
+  ImplicitModelFollower(const LinearSystem<States, Inputs, Outputs>& plant,
+                        const LinearSystem<States, Inputs, Outputs>& plantRef,
+                        units::second_t dt)
+      : ImplicitModelFollower<States, Inputs>(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<double, States, States>& A,
+                        const Eigen::Matrix<double, States, Inputs>& B,
+                        const Eigen::Matrix<double, States, States>& Aref,
+                        const Eigen::Matrix<double, States, Inputs>& Bref,
+                        units::second_t dt) {
+    // Discretize real dynamics
+    Eigen::Matrix<double, States, States> discA;
+    Eigen::Matrix<double, States, Inputs> discB;
+    frc::DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
+
+    // Discretize desired dynamics
+    Eigen::Matrix<double, States, States> discAref;
+    Eigen::Matrix<double, States, Inputs> discBref;
+    frc::DiscretizeAB<States, Inputs>(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<double, Inputs>& 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<double, Inputs> Calculate(
+      const Eigen::Vector<double, States>& x,
+      const Eigen::Vector<double, Inputs>& u) {
+    m_u = m_A * x + m_B * u;
+    return m_u;
+  }
+
+ private:
+  // Computed controller output
+  Eigen::Vector<double, Inputs> m_u;
+
+  // State space conversion gain
+  Eigen::Matrix<double, Inputs, States> m_A;
+
+  // Input space conversion gain
+  Eigen::Matrix<double, Inputs, Inputs> m_B;
+};
+
+}  // namespace frc

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<N2, N2, N2>(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<N2, N2, N2>(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);
+    }
+  }
+}

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 <gtest/gtest.h>
+
+#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<double, 2> x{0.0, 0.0};
+  Eigen::Vector<double, 2> xImf{0.0, 0.0};
+
+  // Forward
+  Eigen::Vector<double, 2> 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<double, 2>{-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<double, 2>{-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<double, 2> x{0.0, 0.0};
+  Eigen::Vector<double, 2> xImf{0.0, 0.0};
+
+  // Forward
+  Eigen::Vector<double, 2> 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<double, 2>{-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<double, 2>{-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