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.Discretization;
011import edu.wpi.first.math.system.LinearSystem;
012import org.ejml.simple.SimpleMatrix;
013
014/**
015 * Constructs a plant inversion model-based feedforward from a {@link LinearSystem}.
016 *
017 * <p>The feedforward is calculated as <strong> u_ff = B<sup>+</sup> (r_k+1 - A r_k) </strong>,
018 * where <strong> B<sup>+</sup> </strong> is the pseudoinverse of B.
019 *
020 * <p>For more on the underlying math, read
021 * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
022 */
023@SuppressWarnings({"ParameterName", "LocalVariableName", "MemberName", "ClassTypeParameterName"})
024public class LinearPlantInversionFeedforward<
025    States extends Num, Inputs extends Num, Outputs extends Num> {
026  /** The current reference state. */
027  @SuppressWarnings("MemberName")
028  private Matrix<States, N1> m_r;
029
030  /** The computed feedforward. */
031  private Matrix<Inputs, N1> m_uff;
032
033  @SuppressWarnings("MemberName")
034  private final Matrix<States, Inputs> m_B;
035
036  @SuppressWarnings("MemberName")
037  private final Matrix<States, States> m_A;
038
039  /**
040   * Constructs a feedforward with the given plant.
041   *
042   * @param plant The plant being controlled.
043   * @param dtSeconds Discretization timestep.
044   */
045  public LinearPlantInversionFeedforward(
046      LinearSystem<States, Inputs, Outputs> plant, double dtSeconds) {
047    this(plant.getA(), plant.getB(), dtSeconds);
048  }
049
050  /**
051   * Constructs a feedforward with the given coefficients.
052   *
053   * @param A Continuous system matrix of the plant being controlled.
054   * @param B Continuous input matrix of the plant being controlled.
055   * @param dtSeconds Discretization timestep.
056   */
057  @SuppressWarnings({"ParameterName", "LocalVariableName"})
058  public LinearPlantInversionFeedforward(
059      Matrix<States, States> A, Matrix<States, Inputs> B, double dtSeconds) {
060    var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
061    this.m_A = discABPair.getFirst();
062    this.m_B = discABPair.getSecond();
063
064    m_r = new Matrix<>(new SimpleMatrix(B.getNumRows(), 1));
065    m_uff = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
066
067    reset();
068  }
069
070  /**
071   * Returns the previously calculated feedforward as an input vector.
072   *
073   * @return The calculated feedforward.
074   */
075  public Matrix<Inputs, N1> getUff() {
076    return m_uff;
077  }
078
079  /**
080   * Returns an element of the previously calculated feedforward.
081   *
082   * @param row Row of uff.
083   * @return The row of the calculated feedforward.
084   */
085  public double getUff(int row) {
086    return m_uff.get(row, 0);
087  }
088
089  /**
090   * Returns the current reference vector r.
091   *
092   * @return The current reference vector.
093   */
094  public Matrix<States, N1> getR() {
095    return m_r;
096  }
097
098  /**
099   * Returns an element of the current reference vector r.
100   *
101   * @param row Row of r.
102   * @return The row of the current reference vector.
103   */
104  public double getR(int row) {
105    return m_r.get(row, 0);
106  }
107
108  /**
109   * Resets the feedforward with a specified initial state vector.
110   *
111   * @param initialState The initial state vector.
112   */
113  public void reset(Matrix<States, N1> initialState) {
114    m_r = initialState;
115    m_uff.fill(0.0);
116  }
117
118  /** Resets the feedforward with a zero initial state vector. */
119  public void reset() {
120    m_r.fill(0.0);
121    m_uff.fill(0.0);
122  }
123
124  /**
125   * Calculate the feedforward with only the desired future reference. This uses the internally
126   * stored "current" reference.
127   *
128   * <p>If this method is used the initial state of the system is the one set using {@link
129   * LinearPlantInversionFeedforward#reset(Matrix)}. If the initial state is not set it defaults to
130   * a zero vector.
131   *
132   * @param nextR The reference state of the future timestep (k + dt).
133   * @return The calculated feedforward.
134   */
135  public Matrix<Inputs, N1> calculate(Matrix<States, N1> nextR) {
136    return calculate(m_r, nextR);
137  }
138
139  /**
140   * Calculate the feedforward with current and future reference vectors.
141   *
142   * @param r The reference state of the current timestep (k).
143   * @param nextR The reference state of the future timestep (k + dt).
144   * @return The calculated feedforward.
145   */
146  @SuppressWarnings({"ParameterName", "LocalVariableName"})
147  public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
148    m_uff = new Matrix<>(m_B.solve(nextR.minus(m_A.times(r))));
149
150    m_r = nextR;
151    return m_uff;
152  }
153}