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.Nat; 009import edu.wpi.first.math.Num; 010import edu.wpi.first.math.numbers.N1; 011import edu.wpi.first.math.system.NumericalJacobian; 012import java.util.function.BiFunction; 013import java.util.function.Function; 014 015/** 016 * Constructs a control-affine plant inversion model-based feedforward from given model dynamics. 017 * 018 * <p>If given the vector valued function as f(x, u) where x is the state vector and u is the input 019 * vector, the B matrix(continuous input matrix) is calculated through a {@link NumericalJacobian}. 020 * In this case f has to be control-affine (of the form f(x) + Bu). 021 * 022 * <p>The feedforward is calculated as <strong> u_ff = B<sup>+</sup> (rDot - f(x))</strong>, where 023 * <strong> B<sup>+</sup> </strong> is the pseudoinverse of B. 024 * 025 * <p>This feedforward does not account for a dynamic B matrix, B is either determined or supplied 026 * when the feedforward is created and remains constant. 027 * 028 * <p>For more on the underlying math, read <a 029 * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>. 030 */ 031public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs extends Num> { 032 /** The current reference state. */ 033 private Matrix<States, N1> m_r; 034 035 /** The computed feedforward. */ 036 private Matrix<Inputs, N1> m_uff; 037 038 private final Matrix<States, Inputs> m_B; 039 040 private final Nat<Inputs> m_inputs; 041 042 private final double m_dt; 043 044 /** The model dynamics. */ 045 private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> m_f; 046 047 /** 048 * Constructs a feedforward with given model dynamics as a function of state and input. 049 * 050 * @param states A {@link Nat} representing the number of states. 051 * @param inputs A {@link Nat} representing the number of inputs. 052 * @param f A vector-valued function of x, the state, and u, the input, that returns the 053 * derivative of the state vector. HAS to be control-affine (of the form f(x) + Bu). 054 * @param dtSeconds The timestep between calls of calculate(). 055 */ 056 public ControlAffinePlantInversionFeedforward( 057 Nat<States> states, 058 Nat<Inputs> inputs, 059 BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f, 060 double dtSeconds) { 061 this.m_dt = dtSeconds; 062 this.m_f = f; 063 this.m_inputs = inputs; 064 065 this.m_B = 066 NumericalJacobian.numericalJacobianU( 067 states, inputs, m_f, new Matrix<>(states, Nat.N1()), new Matrix<>(inputs, Nat.N1())); 068 069 m_r = new Matrix<>(states, Nat.N1()); 070 m_uff = new Matrix<>(inputs, Nat.N1()); 071 072 reset(); 073 } 074 075 /** 076 * Constructs a feedforward with given model dynamics as a function of state, and the plant's 077 * B(continuous input matrix) matrix. 078 * 079 * @param states A {@link Nat} representing the number of states. 080 * @param inputs A {@link Nat} representing the number of inputs. 081 * @param f A vector-valued function of x, the state, that returns the derivative of the state 082 * vector. 083 * @param B Continuous input matrix of the plant being controlled. 084 * @param dtSeconds The timestep between calls of calculate(). 085 */ 086 public ControlAffinePlantInversionFeedforward( 087 Nat<States> states, 088 Nat<Inputs> inputs, 089 Function<Matrix<States, N1>, Matrix<States, N1>> f, 090 Matrix<States, Inputs> B, 091 double dtSeconds) { 092 this.m_dt = dtSeconds; 093 this.m_inputs = inputs; 094 095 this.m_f = (x, u) -> f.apply(x); 096 this.m_B = B; 097 098 m_r = new Matrix<>(states, Nat.N1()); 099 m_uff = new Matrix<>(inputs, Nat.N1()); 100 101 reset(); 102 } 103 104 /** 105 * Returns the previously calculated feedforward as an input vector. 106 * 107 * @return The calculated feedforward. 108 */ 109 public Matrix<Inputs, N1> getUff() { 110 return m_uff; 111 } 112 113 /** 114 * Returns an element of the previously calculated feedforward. 115 * 116 * @param row Row of uff. 117 * @return The row of the calculated feedforward. 118 */ 119 public double getUff(int row) { 120 return m_uff.get(row, 0); 121 } 122 123 /** 124 * Returns the current reference vector r. 125 * 126 * @return The current reference vector. 127 */ 128 public Matrix<States, N1> getR() { 129 return m_r; 130 } 131 132 /** 133 * Returns an element of the current reference vector r. 134 * 135 * @param row Row of r. 136 * @return The row of the current reference vector. 137 */ 138 public double getR(int row) { 139 return m_r.get(row, 0); 140 } 141 142 /** 143 * Resets the feedforward with a specified initial state vector. 144 * 145 * @param initialState The initial state vector. 146 */ 147 public final void reset(Matrix<States, N1> initialState) { 148 m_r = initialState; 149 m_uff.fill(0.0); 150 } 151 152 /** Resets the feedforward with a zero initial state vector. */ 153 public final void reset() { 154 m_r.fill(0.0); 155 m_uff.fill(0.0); 156 } 157 158 /** 159 * Calculate the feedforward with only the desired future reference. This uses the internally 160 * stored "current" reference. 161 * 162 * <p>If this method is used the initial state of the system is the one set using {@link 163 * LinearPlantInversionFeedforward#reset(Matrix)}. If the initial state is not set it defaults to 164 * a zero vector. 165 * 166 * @param nextR The reference state of the future timestep (k + dt). 167 * @return The calculated feedforward. 168 */ 169 public Matrix<Inputs, N1> calculate(Matrix<States, N1> nextR) { 170 return calculate(m_r, nextR); 171 } 172 173 /** 174 * Calculate the feedforward with current and future reference vectors. 175 * 176 * @param r The reference state of the current timestep (k). 177 * @param nextR The reference state of the future timestep (k + dt). 178 * @return The calculated feedforward. 179 */ 180 public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) { 181 var rDot = (nextR.minus(r)).div(m_dt); 182 183 // ṙ = f(r) + Bu 184 // Bu = ṙ − f(r) 185 // u = B⁺(ṙ − f(r)) 186 m_uff = m_B.solve(rDot.minus(m_f.apply(r, new Matrix<>(m_inputs, Nat.N1())))); 187 188 m_r = nextR; 189 return m_uff; 190 } 191}