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