WPILibC++ 2023.4.3
ControlAffinePlantInversionFeedforward.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <array>
8#include <functional>
9
10#include "Eigen/QR"
11#include "frc/EigenCore.h"
13#include "units/time.h"
14
15namespace frc {
16
17/**
18 * Constructs a control-affine plant inversion model-based feedforward from
19 * given model dynamics.
20 *
21 * If given the vector valued function as f(x, u) where x is the state
22 * vector and u is the input vector, the B matrix(continuous input matrix)
23 * is calculated through a NumericalJacobian. In this case f has to be
24 * control-affine (of the form f(x) + Bu).
25 *
26 * The feedforward is calculated as
27 * <strong> u_ff = B<sup>+</sup> (rDot - f(x)) </strong>, where <strong>
28 * B<sup>+</sup> </strong> is the pseudoinverse of B.
29 *
30 * This feedforward does not account for a dynamic B matrix, B is either
31 * determined or supplied when the feedforward is created and remains constant.
32 *
33 * For more on the underlying math, read
34 * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
35 *
36 * @tparam States The number of states.
37 * @tparam Inputs the number of inputs.
38 */
39template <int States, int Inputs>
41 public:
44
45 /**
46 * Constructs a feedforward with given model dynamics as a function
47 * of state and input.
48 *
49 * @param f A vector-valued function of x, the state, and
50 * u, the input, that returns the derivative of
51 * the state vector. HAS to be control-affine
52 * (of the form f(x) + Bu).
53 * @param dt The timestep between calls of calculate().
54 */
56 std::function<StateVector(const StateVector&, const InputVector&)> f,
57 units::second_t dt)
58 : m_dt(dt), m_f(f) {
59 m_B = NumericalJacobianU<States, States, Inputs>(f, StateVector::Zero(),
60 InputVector::Zero());
61
62 Reset();
63 }
64
65 /**
66 * Constructs a feedforward with given model dynamics as a function of state,
67 * and the plant's B matrix(continuous input matrix).
68 *
69 * @param f A vector-valued function of x, the state,
70 * that returns the derivative of the state vector.
71 * @param B Continuous input matrix of the plant being controlled.
72 * @param dt The timestep between calls of calculate().
73 */
75 std::function<StateVector(const StateVector&)> f,
76 const Matrixd<States, Inputs>& B, units::second_t dt)
77 : m_B(B), m_dt(dt) {
78 m_f = [=](const StateVector& x, const InputVector& u) -> StateVector {
79 return f(x);
80 };
81
82 Reset();
83 }
84
89
90 /**
91 * Returns the previously calculated feedforward as an input vector.
92 *
93 * @return The calculated feedforward.
94 */
95 const InputVector& Uff() const { return m_uff; }
96
97 /**
98 * Returns an element of the previously calculated feedforward.
99 *
100 * @param i Row of uff.
101 *
102 * @return The row of the calculated feedforward.
103 */
104 double Uff(int i) const { return m_uff(i); }
105
106 /**
107 * Returns the current reference vector r.
108 *
109 * @return The current reference vector.
110 */
111 const StateVector& R() const { return m_r; }
112
113 /**
114 * Returns an element of the reference vector r.
115 *
116 * @param i Row of r.
117 *
118 * @return The row of the current reference vector.
119 */
120 double R(int i) const { return m_r(i); }
121
122 /**
123 * Resets the feedforward with a specified initial state vector.
124 *
125 * @param initialState The initial state vector.
126 */
127 void Reset(const StateVector& initialState) {
128 m_r = initialState;
129 m_uff.setZero();
130 }
131
132 /**
133 * Resets the feedforward with a zero initial state vector.
134 */
135 void Reset() {
136 m_r.setZero();
137 m_uff.setZero();
138 }
139
140 /**
141 * Calculate the feedforward with only the desired
142 * future reference. This uses the internally stored "current"
143 * reference.
144 *
145 * If this method is used the initial state of the system is the one set using
146 * Reset(const StateVector&). If the initial state is not
147 * set it defaults to a zero vector.
148 *
149 * @param nextR The reference state of the future timestep (k + dt).
150 *
151 * @return The calculated feedforward.
152 */
154 return Calculate(m_r, nextR);
155 }
156
157 /**
158 * Calculate the feedforward with current and future reference vectors.
159 *
160 * @param r The reference state of the current timestep (k).
161 * @param nextR The reference state of the future timestep (k + dt).
162 *
163 * @return The calculated feedforward.
164 */
166 StateVector rDot = (nextR - r) / m_dt.value();
167
168 m_uff = m_B.householderQr().solve(rDot - m_f(r, InputVector::Zero()));
169
170 m_r = nextR;
171 return m_uff;
172 }
173
174 private:
176
177 units::second_t m_dt;
178
179 /**
180 * The model dynamics.
181 */
182 std::function<StateVector(const StateVector&, const InputVector&)> m_f;
183
184 // Current reference
185 StateVector m_r;
186
187 // Computed feedforward
188 InputVector m_uff;
189};
190
191} // namespace frc
The matrix class, also used for vectors and row-vectors.
Definition: Matrix.h:180
Constructs a control-affine plant inversion model-based feedforward from given model dynamics.
Definition: ControlAffinePlantInversionFeedforward.h:40
const InputVector & Uff() const
Returns the previously calculated feedforward as an input vector.
Definition: ControlAffinePlantInversionFeedforward.h:95
InputVector Calculate(const StateVector &nextR)
Calculate the feedforward with only the desired future reference.
Definition: ControlAffinePlantInversionFeedforward.h:153
Vectord< Inputs > InputVector
Definition: ControlAffinePlantInversionFeedforward.h:43
double R(int i) const
Returns an element of the reference vector r.
Definition: ControlAffinePlantInversionFeedforward.h:120
InputVector Calculate(const StateVector &r, const StateVector &nextR)
Calculate the feedforward with current and future reference vectors.
Definition: ControlAffinePlantInversionFeedforward.h:165
void Reset()
Resets the feedforward with a zero initial state vector.
Definition: ControlAffinePlantInversionFeedforward.h:135
Vectord< States > StateVector
Definition: ControlAffinePlantInversionFeedforward.h:42
ControlAffinePlantInversionFeedforward(std::function< StateVector(const StateVector &, const InputVector &)> f, units::second_t dt)
Constructs a feedforward with given model dynamics as a function of state and input.
Definition: ControlAffinePlantInversionFeedforward.h:55
void Reset(const StateVector &initialState)
Resets the feedforward with a specified initial state vector.
Definition: ControlAffinePlantInversionFeedforward.h:127
const StateVector & R() const
Returns the current reference vector r.
Definition: ControlAffinePlantInversionFeedforward.h:111
ControlAffinePlantInversionFeedforward(std::function< StateVector(const StateVector &)> f, const Matrixd< States, Inputs > &B, units::second_t dt)
Constructs a feedforward with given model dynamics as a function of state, and the plant's B matrix(c...
Definition: ControlAffinePlantInversionFeedforward.h:74
double Uff(int i) const
Returns an element of the previously calculated feedforward.
Definition: ControlAffinePlantInversionFeedforward.h:104
ControlAffinePlantInversionFeedforward(ControlAffinePlantInversionFeedforward &&)=default
ControlAffinePlantInversionFeedforward & operator=(ControlAffinePlantInversionFeedforward &&)=default
Definition: AprilTagFieldLayout.h:22
Eigen::Vector< double, Size > Vectord
Definition: EigenCore.h:12