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
021 * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
022 */
023public class ImplicitModelFollower<States extends Num, Inputs extends Num, Outputs extends Num> {
024  // Computed controller output
025  private Matrix<Inputs, N1> m_u;
026
027  // State space conversion gain
028  private Matrix<Inputs, States> m_A;
029
030  // Input space conversion gain
031  private Matrix<Inputs, Inputs> m_B;
032
033  /**
034   * Constructs a controller with the given coefficients and plant.
035   *
036   * @param plant The plant being controlled.
037   * @param plantRef The plant whose dynamics should be followed.
038   */
039  public ImplicitModelFollower(
040      LinearSystem<States, Inputs, Outputs> plant, LinearSystem<States, Inputs, Outputs> plantRef) {
041    this(plant.getA(), plant.getB(), plantRef.getA(), plantRef.getB());
042  }
043
044  /**
045   * Constructs a controller with the given coefficients and plant.
046   *
047   * @param A Continuous system matrix of the plant being controlled.
048   * @param B Continuous input matrix of the plant being controlled.
049   * @param Aref Continuous system matrix whose dynamics should be followed.
050   * @param Bref Continuous input matrix whose dynamics should be followed.
051   */
052  public ImplicitModelFollower(
053      Matrix<States, States> A,
054      Matrix<States, Inputs> B,
055      Matrix<States, States> Aref,
056      Matrix<States, Inputs> Bref) {
057    m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
058
059    // Find u_imf that makes real model match reference model.
060    //
061    // dx/dt = Ax + Bu_imf
062    // dz/dt = A_ref z + B_ref u
063    //
064    // Let x = z.
065    //
066    // dx/dt = dz/dt
067    // Ax + Bu_imf = Aref x + B_ref u
068    // Bu_imf = A_ref x - Ax + B_ref u
069    // Bu_imf = (A_ref - A)x + B_ref u
070    // u_imf = B⁻¹((A_ref - A)x + Bref u)
071    // u_imf = -B⁻¹(A - A_ref)x + B⁻¹B_ref u
072
073    // The first term makes the open-loop poles that of the reference
074    // system, and the second term makes the input behave like that of the
075    // reference system.
076    m_A = B.solve(A.minus(Aref)).times(-1.0);
077    m_B = B.solve(Bref);
078
079    reset();
080  }
081
082  /**
083   * Returns the control input vector u.
084   *
085   * @return The control input.
086   */
087  public Matrix<Inputs, N1> getU() {
088    return m_u;
089  }
090
091  /**
092   * Returns an element of the control input vector u.
093   *
094   * @param i Row of u.
095   * @return The row of the control input vector.
096   */
097  public double getU(int i) {
098    return m_u.get(i, 0);
099  }
100
101  /** Resets the controller. */
102  public void reset() {
103    m_u.fill(0.0);
104  }
105
106  /**
107   * Returns the next output of the controller.
108   *
109   * @param x The current state x.
110   * @param u The current input for the original model.
111   * @return The next controller output.
112   */
113  public Matrix<Inputs, N1> calculate(Matrix<States, N1> x, Matrix<Inputs, N1> u) {
114    m_u = m_A.times(x).plus(m_B.times(u));
115    return m_u;
116  }
117}