001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.controller;
006
007import edu.wpi.first.math.Matrix;
008import edu.wpi.first.math.Num;
009import edu.wpi.first.math.numbers.N1;
010import edu.wpi.first.math.system.LinearSystem;
011import org.ejml.simple.SimpleMatrix;
012
013/**
014 * Contains the controller coefficients and logic for an implicit model follower.
015 *
016 * <p>Implicit model following lets us design a feedback controller that erases the dynamics of our
017 * system and makes it behave like some other system. This can be used to make a drivetrain more
018 * controllable during teleop driving by making it behave like a slower or more benign drivetrain.
019 *
020 * <p>For more on the underlying math, read appendix B.3 in <a
021 * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>.
022 *
023 * @param <States> Number of states.
024 * @param <Inputs> Number of inputs.
025 * @param <Outputs> Number of outputs.
026 */
027public class ImplicitModelFollower<States extends Num, Inputs extends Num, Outputs extends Num> {
028  // Computed controller output
029  private Matrix<Inputs, N1> m_u;
030
031  // State space conversion gain
032  private Matrix<Inputs, States> m_A;
033
034  // Input space conversion gain
035  private Matrix<Inputs, Inputs> m_B;
036
037  /**
038   * Constructs a controller with the given coefficients and plant.
039   *
040   * @param plant The plant being controlled.
041   * @param plantRef The plant whose dynamics should be followed.
042   */
043  public ImplicitModelFollower(
044      LinearSystem<States, Inputs, Outputs> plant, LinearSystem<States, Inputs, Outputs> plantRef) {
045    this(plant.getA(), plant.getB(), plantRef.getA(), plantRef.getB());
046  }
047
048  /**
049   * Constructs a controller with the given coefficients and plant.
050   *
051   * @param A Continuous system matrix of the plant being controlled.
052   * @param B Continuous input matrix of the plant being controlled.
053   * @param Aref Continuous system matrix whose dynamics should be followed.
054   * @param Bref Continuous input matrix whose dynamics should be followed.
055   */
056  public ImplicitModelFollower(
057      Matrix<States, States> A,
058      Matrix<States, Inputs> B,
059      Matrix<States, States> Aref,
060      Matrix<States, Inputs> Bref) {
061    m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
062
063    // Find u_imf that makes real model match reference model.
064    //
065    // dx/dt = Ax + Bu_imf
066    // dz/dt = A_ref z + B_ref u
067    //
068    // Let x = z.
069    //
070    // dx/dt = dz/dt
071    // Ax + Bu_imf = Aref x + B_ref u
072    // Bu_imf = A_ref x - Ax + B_ref u
073    // Bu_imf = (A_ref - A)x + B_ref u
074    // u_imf = B⁻¹((A_ref - A)x + Bref u)
075    // u_imf = -B⁻¹(A - A_ref)x + B⁻¹B_ref u
076
077    // The first term makes the open-loop poles that of the reference
078    // system, and the second term makes the input behave like that of the
079    // reference system.
080    m_A = B.solve(A.minus(Aref)).times(-1.0);
081    m_B = B.solve(Bref);
082
083    reset();
084  }
085
086  /**
087   * Returns the control input vector u.
088   *
089   * @return The control input.
090   */
091  public Matrix<Inputs, N1> getU() {
092    return m_u;
093  }
094
095  /**
096   * Returns an element of the control input vector u.
097   *
098   * @param i Row of u.
099   * @return The row of the control input vector.
100   */
101  public double getU(int i) {
102    return m_u.get(i, 0);
103  }
104
105  /** Resets the controller. */
106  public final void reset() {
107    m_u.fill(0.0);
108  }
109
110  /**
111   * Returns the next output of the controller.
112   *
113   * @param x The current state x.
114   * @param u The current input for the original model.
115   * @return The next controller output.
116   */
117  public Matrix<Inputs, N1> calculate(Matrix<States, N1> x, Matrix<Inputs, N1> u) {
118    m_u = m_A.times(x).plus(m_B.times(u));
119    return m_u;
120  }
121}