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