mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-23 01:21:42 +00:00
[wpimath] Add core State-space classes (#2614)
Co-authored-by: Tyler Veness <calcmogul@gmail.com> Co-authored-by: Claudius Tewari <cttewari@gmail.com> Co-authored-by: Declan Freeman-Gleason <declanfreemangleason@gmail.com>
This commit is contained in:
Matt
@@ -0,0 +1,202 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <functional>
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "frc/system/NumericalJacobian.h"
|
||||
#include "units/time.h"
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* Constructs a control-affine plant inversion model-based feedforward from
|
||||
* given model dynamics.
|
||||
*
|
||||
* If given the vector valued function as f(x, u) where x is the state
|
||||
* vector and u is the input vector, the B matrix(continuous input matrix)
|
||||
* is calculated through a NumericalJacobian. In this case f has to be
|
||||
* control-affine (of the form f(x) + Bu).
|
||||
*
|
||||
* The feedforward is calculated as
|
||||
* <strong> u_ff = B<sup>+</sup> (rDot - f(x)) </strong>, where <strong>
|
||||
* B<sup>+</sup> </strong> is the pseudoinverse of B.
|
||||
*
|
||||
* This feedforward does not account for a dynamic B matrix, B is either
|
||||
* determined or supplied when the feedforward is created and remains constant.
|
||||
*
|
||||
* For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
|
||||
*/
|
||||
template <int States, int Inputs>
|
||||
class ControlAffinePlantInversionFeedforward {
|
||||
public:
|
||||
/**
|
||||
* Constructs a feedforward with given model dynamics as a function
|
||||
* of state and input.
|
||||
*
|
||||
* @param f A vector-valued function of x, the state, and
|
||||
* u, the input, that returns the derivative of
|
||||
* the state vector. HAS to be control-affine
|
||||
* (of the form f(x) + Bu).
|
||||
* @param dt The timestep between calls of calculate().
|
||||
*/
|
||||
ControlAffinePlantInversionFeedforward(
|
||||
std::function<Eigen::Matrix<double, States, 1>(
|
||||
const Eigen::Matrix<double, States, 1>&,
|
||||
const Eigen::Matrix<double, Inputs, 1>&)>
|
||||
f,
|
||||
units::second_t dt)
|
||||
: m_dt(dt), m_f(f) {
|
||||
m_B = NumericalJacobianU<States, States, Inputs>(
|
||||
f, Eigen::Matrix<double, States, 1>::Zero(),
|
||||
Eigen::Matrix<double, Inputs, 1>::Zero());
|
||||
|
||||
m_r.setZero();
|
||||
Reset(m_r);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a feedforward with given model dynamics as a function of state,
|
||||
* and the plant's B matrix(continuous input matrix).
|
||||
*
|
||||
* @param f A vector-valued function of x, the state,
|
||||
* that returns the derivative of the state vector.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param dt The timestep between calls of calculate().
|
||||
*/
|
||||
ControlAffinePlantInversionFeedforward(
|
||||
std::function<Eigen::Matrix<double, States, 1>(
|
||||
const Eigen::Matrix<double, States, 1>&)>
|
||||
f,
|
||||
const Eigen::Matrix<double, States, Inputs>& B, units::second_t dt)
|
||||
: m_B(B), m_dt(dt) {
|
||||
m_f = [=](const Eigen::Matrix<double, States, 1>& x,
|
||||
const Eigen::Matrix<double, Inputs, 1>& u)
|
||||
-> Eigen::Matrix<double, States, 1> { return f(x); };
|
||||
|
||||
m_r.setZero();
|
||||
Reset(m_r);
|
||||
}
|
||||
|
||||
ControlAffinePlantInversionFeedforward(
|
||||
ControlAffinePlantInversionFeedforward&&) = default;
|
||||
ControlAffinePlantInversionFeedforward& operator=(
|
||||
ControlAffinePlantInversionFeedforward&&) = default;
|
||||
|
||||
/**
|
||||
* Returns the previously calculated feedforward as an input vector.
|
||||
*
|
||||
* @return The calculated feedforward.
|
||||
*/
|
||||
const Eigen::Matrix<double, Inputs, 1>& Uff() const { return m_uff; }
|
||||
|
||||
/**
|
||||
* Returns an element of the previously calculated feedforward.
|
||||
*
|
||||
* @param row Row of uff.
|
||||
*
|
||||
* @return The row of the calculated feedforward.
|
||||
*/
|
||||
double Uff(int i) const { return m_uff(i, 0); }
|
||||
|
||||
/**
|
||||
* Returns the current reference vector r.
|
||||
*
|
||||
* @return The current reference vector.
|
||||
*/
|
||||
const Eigen::Matrix<double, States, 1>& R() const { return m_r; }
|
||||
|
||||
/**
|
||||
* Returns an element of the reference vector r.
|
||||
*
|
||||
* @param i Row of r.
|
||||
*
|
||||
* @return The row of the current reference vector.
|
||||
*/
|
||||
double R(int i) const { return m_r(i, 0); }
|
||||
|
||||
/**
|
||||
* Resets the feedforward with a specified initial state vector.
|
||||
*
|
||||
* @param initialState The initial state vector.
|
||||
*/
|
||||
void Reset(const Eigen::Matrix<double, States, 1>& initialState) {
|
||||
m_r = initialState;
|
||||
m_uff.setZero();
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the feedforward with a zero initial state vector.
|
||||
*/
|
||||
void Reset() {
|
||||
m_r.setZero();
|
||||
m_uff.setZero();
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the feedforward with only the desired
|
||||
* future reference. This uses the internally stored "current"
|
||||
* reference.
|
||||
*
|
||||
* If this method is used the initial state of the system is the one
|
||||
* set using Reset(const Eigen::Matrix<double, States, 1>&).
|
||||
* If the initial state is not set it defaults to a zero vector.
|
||||
*
|
||||
* @param nextR The reference state of the future timestep (k + dt).
|
||||
*
|
||||
* @return The calculated feedforward.
|
||||
*/
|
||||
Eigen::Matrix<double, Inputs, 1> Calculate(
|
||||
const Eigen::Matrix<double, States, 1>& nextR) {
|
||||
return Calculate(m_r, nextR);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the feedforward with current and future reference vectors.
|
||||
*
|
||||
* @param r The reference state of the current timestep (k).
|
||||
* @param nextR The reference state of the future timestep (k + dt).
|
||||
*
|
||||
* @return The calculated feedforward.
|
||||
*/
|
||||
Eigen::Matrix<double, Inputs, 1> Calculate(
|
||||
const Eigen::Matrix<double, States, 1>& r,
|
||||
const Eigen::Matrix<double, States, 1>& nextR) {
|
||||
Eigen::Matrix<double, States, 1> rDot = (nextR - r) / m_dt.to<double>();
|
||||
|
||||
m_uff = m_B.householderQr().solve(
|
||||
rDot - m_f(r, Eigen::Matrix<double, Inputs, 1>::Zero()));
|
||||
|
||||
m_r = nextR;
|
||||
return m_uff;
|
||||
}
|
||||
|
||||
private:
|
||||
Eigen::Matrix<double, States, Inputs> m_B;
|
||||
|
||||
units::second_t m_dt;
|
||||
|
||||
/**
|
||||
* The model dynamics.
|
||||
*/
|
||||
std::function<Eigen::Matrix<double, States, 1>(
|
||||
const Eigen::Matrix<double, States, 1>&,
|
||||
const Eigen::Matrix<double, Inputs, 1>&)>
|
||||
m_f;
|
||||
|
||||
// Current reference
|
||||
Eigen::Matrix<double, States, 1> m_r;
|
||||
|
||||
// Computed feedforward
|
||||
Eigen::Matrix<double, Inputs, 1> m_uff;
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
@@ -0,0 +1,162 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <functional>
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "frc/system/Discretization.h"
|
||||
#include "frc/system/LinearSystem.h"
|
||||
#include "units/time.h"
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* Constructs a plant inversion model-based feedforward from a LinearSystem.
|
||||
*
|
||||
* The feedforward is calculated as <strong> u_ff = B<sup>+</sup> (r_k+1 - A
|
||||
* r_k) </strong>, where <strong> B<sup>+</sup> </strong> is the pseudoinverse
|
||||
* of B.
|
||||
*
|
||||
* For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
|
||||
*/
|
||||
template <int States, int Inputs>
|
||||
class LinearPlantInversionFeedforward {
|
||||
public:
|
||||
/**
|
||||
* Constructs a feedforward with the given plant.
|
||||
*
|
||||
* @param plant The plant being controlled.
|
||||
* @param dtSeconds Discretization timestep.
|
||||
*/
|
||||
template <int Outputs>
|
||||
LinearPlantInversionFeedforward(
|
||||
const LinearSystem<States, Inputs, Outputs>& plant, units::second_t dt)
|
||||
: LinearPlantInversionFeedforward(plant.A(), plant.B(), dt) {}
|
||||
|
||||
/**
|
||||
* Constructs a feedforward with the given coefficients.
|
||||
*
|
||||
* @param A Continuous system matrix of the plant being controlled.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param dtSeconds Discretization timestep.
|
||||
*/
|
||||
LinearPlantInversionFeedforward(
|
||||
const Eigen::Matrix<double, States, States>& A,
|
||||
const Eigen::Matrix<double, States, Inputs>& B, units::second_t dt)
|
||||
: m_dt(dt) {
|
||||
DiscretizeAB<States, Inputs>(A, B, dt, &m_A, &m_B);
|
||||
|
||||
m_r.setZero();
|
||||
Reset(m_r);
|
||||
}
|
||||
|
||||
LinearPlantInversionFeedforward(LinearPlantInversionFeedforward&&) = default;
|
||||
LinearPlantInversionFeedforward& operator=(
|
||||
LinearPlantInversionFeedforward&&) = default;
|
||||
|
||||
/**
|
||||
* Returns the previously calculated feedforward as an input vector.
|
||||
*
|
||||
* @return The calculated feedforward.
|
||||
*/
|
||||
const Eigen::Matrix<double, Inputs, 1>& Uff() const { return m_uff; }
|
||||
|
||||
/**
|
||||
* Returns an element of the previously calculated feedforward.
|
||||
*
|
||||
* @param row Row of uff.
|
||||
*
|
||||
* @return The row of the calculated feedforward.
|
||||
*/
|
||||
double Uff(int i) const { return m_uff(i, 0); }
|
||||
|
||||
/**
|
||||
* Returns the current reference vector r.
|
||||
*
|
||||
* @return The current reference vector.
|
||||
*/
|
||||
const Eigen::Matrix<double, States, 1>& R() const { return m_r; }
|
||||
|
||||
/**
|
||||
* Returns an element of the reference vector r.
|
||||
*
|
||||
* @param i Row of r.
|
||||
*
|
||||
* @return The row of the current reference vector.
|
||||
*/
|
||||
double R(int i) const { return m_r(i, 0); }
|
||||
|
||||
/**
|
||||
* Resets the feedforward with a specified initial state vector.
|
||||
*
|
||||
* @param initialState The initial state vector.
|
||||
*/
|
||||
void Reset(const Eigen::Matrix<double, States, 1>& initialState) {
|
||||
m_r = initialState;
|
||||
m_uff.setZero();
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the feedforward with a zero initial state vector.
|
||||
*/
|
||||
void Reset() {
|
||||
m_r.setZero();
|
||||
m_uff.setZero();
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the feedforward with only the desired
|
||||
* future reference. This uses the internally stored "current"
|
||||
* reference.
|
||||
*
|
||||
* If this method is used the initial state of the system is the one
|
||||
* set using Reset(const Eigen::Matrix<double, States, 1>&).
|
||||
* If the initial state is not set it defaults to a zero vector.
|
||||
*
|
||||
* @param nextR The reference state of the future timestep (k + dt).
|
||||
*
|
||||
* @return The calculated feedforward.
|
||||
*/
|
||||
Eigen::Matrix<double, Inputs, 1> Calculate(
|
||||
const Eigen::Matrix<double, States, 1>& nextR) {
|
||||
return Calculate(m_r, nextR);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the feedforward with current and future reference vectors.
|
||||
*
|
||||
* @param r The reference state of the current timestep (k).
|
||||
* @param nextR The reference state of the future timestep (k + dt).
|
||||
*
|
||||
* @return The calculated feedforward.
|
||||
*/
|
||||
Eigen::Matrix<double, Inputs, 1> Calculate(
|
||||
const Eigen::Matrix<double, States, 1>& r,
|
||||
const Eigen::Matrix<double, States, 1>& nextR) {
|
||||
m_uff = m_B.householderQr().solve(nextR - (m_A * r));
|
||||
m_r = nextR;
|
||||
return m_uff;
|
||||
}
|
||||
|
||||
private:
|
||||
Eigen::Matrix<double, States, States> m_A;
|
||||
Eigen::Matrix<double, States, Inputs> m_B;
|
||||
|
||||
units::second_t m_dt;
|
||||
|
||||
// Current reference
|
||||
Eigen::Matrix<double, States, 1> m_r;
|
||||
|
||||
// Computed feedforward
|
||||
Eigen::Matrix<double, Inputs, 1> m_uff;
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
@@ -0,0 +1,422 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "Eigen/QR"
|
||||
#include "drake/math/discrete_algebraic_riccati_equation.h"
|
||||
#include "frc/StateSpaceUtil.h"
|
||||
#include "frc/system/Discretization.h"
|
||||
#include "frc/system/LinearSystem.h"
|
||||
#include "units/time.h"
|
||||
|
||||
namespace frc {
|
||||
namespace detail {
|
||||
|
||||
/**
|
||||
* Contains the controller coefficients and logic for a linear-quadratic
|
||||
* regulator (LQR).
|
||||
* LQRs use the control law u = K(r - x).
|
||||
*
|
||||
* For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
|
||||
*/
|
||||
template <int States, int Inputs>
|
||||
class LinearQuadraticRegulatorImpl {
|
||||
public:
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param plant The plant being controlled.
|
||||
* @param Qelems The maximum desired error tolerance for each state.
|
||||
* @param Relems The maximum desired control effort for each input.
|
||||
* @param dt Discretization timestep.
|
||||
*/
|
||||
template <int Outputs>
|
||||
LinearQuadraticRegulatorImpl(
|
||||
const LinearSystem<States, Inputs, Outputs>& plant,
|
||||
const std::array<double, States>& Qelems,
|
||||
const std::array<double, Inputs>& Relems, units::second_t dt)
|
||||
: LinearQuadraticRegulatorImpl(plant.A(), plant.B(), Qelems, 1.0, Relems,
|
||||
dt) {}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param plant The plant being controlled.
|
||||
* @param Qelems The maximum desired error tolerance for each state.
|
||||
* @param rho A weighting factor that balances control effort and state
|
||||
* excursion. Greater values penalize state excursion more heavily. 1 is a
|
||||
* good starting value.
|
||||
* @param Relems The maximum desired control effort for each input.
|
||||
* @param dt Discretization timestep.
|
||||
*/
|
||||
template <int Outputs>
|
||||
LinearQuadraticRegulatorImpl(
|
||||
const LinearSystem<States, Inputs, Outputs>& plant,
|
||||
const std::array<double, States>& Qelems, const double rho,
|
||||
const std::array<double, Inputs>& Relems, units::second_t dt)
|
||||
: LinearQuadraticRegulatorImpl(plant.A(), plant.B(), Qelems, rho, Relems,
|
||||
dt) {}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param A Continuous system matrix of the plant being controlled.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param Qelems The maximum desired error tolerance for each state.
|
||||
* @param rho A weighting factor that balances control effort and state
|
||||
* excursion. Greater values penalize state excursion more heavily. 1 is a
|
||||
* good starting value.
|
||||
* @param Relems The maximum desired control effort for each input.
|
||||
* @param dt Discretization timestep.
|
||||
*/
|
||||
LinearQuadraticRegulatorImpl(const Eigen::Matrix<double, States, States>& A,
|
||||
const Eigen::Matrix<double, States, Inputs>& B,
|
||||
const std::array<double, States>& Qelems,
|
||||
const std::array<double, Inputs>& Relems,
|
||||
units::second_t dt)
|
||||
: LinearQuadraticRegulatorImpl(A, B, Qelems, 1.0, Relems, dt) {}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param A Continuous system matrix of the plant being controlled.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param Qelems The maximum desired error tolerance for each state.
|
||||
* @param rho A weighting factor that balances control effort and state
|
||||
* excursion. Greater values penalize state excursion more heavily. 1 is a
|
||||
* good starting value.
|
||||
* @param Relems The maximum desired control effort for each input.
|
||||
* @param dt Discretization timestep.
|
||||
*/
|
||||
LinearQuadraticRegulatorImpl(const Eigen::Matrix<double, States, States>& A,
|
||||
const Eigen::Matrix<double, States, Inputs>& B,
|
||||
const std::array<double, States>& Qelems,
|
||||
const double rho,
|
||||
const std::array<double, Inputs>& Relems,
|
||||
units::second_t dt)
|
||||
: LinearQuadraticRegulatorImpl(A, B, MakeCostMatrix(Qelems) * rho,
|
||||
MakeCostMatrix(Relems), dt) {}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param A Continuous system matrix of the plant being controlled.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param Q The state cost matrix.
|
||||
* @param R The input cost matrix.
|
||||
* @param dt Discretization timestep.
|
||||
*/
|
||||
LinearQuadraticRegulatorImpl(const Eigen::Matrix<double, States, States>& A,
|
||||
const Eigen::Matrix<double, States, Inputs>& B,
|
||||
const Eigen::Matrix<double, States, States>& Q,
|
||||
const Eigen::Matrix<double, Inputs, Inputs>& R,
|
||||
units::second_t dt) {
|
||||
Eigen::Matrix<double, States, States> discA;
|
||||
Eigen::Matrix<double, States, Inputs> discB;
|
||||
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
|
||||
|
||||
Eigen::Matrix<double, States, States> S =
|
||||
drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
|
||||
Eigen::Matrix<double, Inputs, Inputs> tmp =
|
||||
discB.transpose() * S * discB + R;
|
||||
m_K = tmp.llt().solve(discB.transpose() * S * discA);
|
||||
|
||||
Reset();
|
||||
}
|
||||
|
||||
LinearQuadraticRegulatorImpl(LinearQuadraticRegulatorImpl&&) = default;
|
||||
LinearQuadraticRegulatorImpl& operator=(LinearQuadraticRegulatorImpl&&) =
|
||||
default;
|
||||
|
||||
/**
|
||||
* Returns the controller matrix K.
|
||||
*/
|
||||
const Eigen::Matrix<double, Inputs, States>& K() const { return m_K; }
|
||||
|
||||
/**
|
||||
* Returns an element of the controller matrix K.
|
||||
*
|
||||
* @param i Row of K.
|
||||
* @param j Column of K.
|
||||
*/
|
||||
double K(int i, int j) const { return m_K(i, j); }
|
||||
|
||||
/**
|
||||
* Returns the reference vector r.
|
||||
*
|
||||
* @return The reference vector.
|
||||
*/
|
||||
const Eigen::Matrix<double, States, 1>& R() const { return m_r; }
|
||||
|
||||
/**
|
||||
* Returns an element of the reference vector r.
|
||||
*
|
||||
* @param i Row of r.
|
||||
*
|
||||
* @return The row of the reference vector.
|
||||
*/
|
||||
double R(int i) const { return m_r(i, 0); }
|
||||
|
||||
/**
|
||||
* Returns the control input vector u.
|
||||
*
|
||||
* @return The control input.
|
||||
*/
|
||||
const Eigen::Matrix<double, Inputs, 1>& U() const { return m_u; }
|
||||
|
||||
/**
|
||||
* Returns an element of the control input vector u.
|
||||
*
|
||||
* @param i Row of u.
|
||||
*
|
||||
* @return The row of the control input vector.
|
||||
*/
|
||||
double U(int i) const { return m_u(i, 0); }
|
||||
|
||||
/**
|
||||
* Resets the controller.
|
||||
*/
|
||||
void Reset() {
|
||||
m_r.setZero();
|
||||
m_u.setZero();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the controller.
|
||||
*
|
||||
* @param x The current state x.
|
||||
*/
|
||||
Eigen::Matrix<double, Inputs, 1> Calculate(
|
||||
const Eigen::Matrix<double, States, 1>& x) {
|
||||
m_u = m_K * (m_r - x);
|
||||
return m_u;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the controller.
|
||||
*
|
||||
* @param x The current state x.
|
||||
* @param nextR The next reference vector r.
|
||||
*/
|
||||
Eigen::Matrix<double, Inputs, 1> Calculate(
|
||||
const Eigen::Matrix<double, States, 1>& x,
|
||||
const Eigen::Matrix<double, States, 1>& nextR) {
|
||||
m_r = nextR;
|
||||
return Calculate(x);
|
||||
}
|
||||
|
||||
private:
|
||||
// Current reference
|
||||
Eigen::Matrix<double, States, 1> m_r;
|
||||
|
||||
// Computed controller output
|
||||
Eigen::Matrix<double, Inputs, 1> m_u;
|
||||
|
||||
// Controller gain
|
||||
Eigen::Matrix<double, Inputs, States> m_K;
|
||||
};
|
||||
|
||||
} // namespace detail
|
||||
|
||||
template <int States, int Inputs>
|
||||
class LinearQuadraticRegulator
|
||||
: public detail::LinearQuadraticRegulatorImpl<States, Inputs> {
|
||||
public:
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param system The plant being controlled.
|
||||
* @param Qelems The maximum desired error tolerance for each state.
|
||||
* @param Relems The maximum desired control effort for each input.
|
||||
* @param dt Discretization timestep.
|
||||
*/
|
||||
template <int Outputs>
|
||||
LinearQuadraticRegulator(const LinearSystem<States, Inputs, Outputs>& plant,
|
||||
const std::array<double, States>& Qelems,
|
||||
const std::array<double, Inputs>& Relems,
|
||||
units::second_t dt)
|
||||
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, 1.0, Relems,
|
||||
dt) {}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param system The plant being controlled.
|
||||
* @param Qelems The maximum desired error tolerance for each state.
|
||||
* @param rho A weighting factor that balances control effort and state
|
||||
* excursion. Greater values penalize state excursion more heavily. 1 is a
|
||||
* good starting value.
|
||||
* @param Relems The maximum desired control effort for each input.
|
||||
* @param dt Discretization timestep.
|
||||
*/
|
||||
template <int Outputs>
|
||||
LinearQuadraticRegulator(const LinearSystem<States, Inputs, Outputs>& plant,
|
||||
const std::array<double, States>& Qelems,
|
||||
const double rho,
|
||||
const std::array<double, Inputs>& Relems,
|
||||
units::second_t dt)
|
||||
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, rho, Relems,
|
||||
dt) {}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param A Continuous system matrix of the plant being controlled.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param Qelems The maximum desired error tolerance for each state.
|
||||
* @param rho A weighting factor that balances control effort and state
|
||||
* excursion. Greater values penalize state excursion more heavily. 1 is a
|
||||
* good starting value.
|
||||
* @param Relems The maximum desired control effort for each input.
|
||||
* @param dt Discretization timestep.
|
||||
*/
|
||||
LinearQuadraticRegulator(const Eigen::Matrix<double, States, States>& A,
|
||||
const Eigen::Matrix<double, States, Inputs>& B,
|
||||
const std::array<double, States>& Qelems,
|
||||
const std::array<double, Inputs>& Relems,
|
||||
units::second_t dt)
|
||||
: LinearQuadraticRegulator(A, B, Qelems, 1.0, Relems, dt) {}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param A Continuous system matrix of the plant being controlled.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param Qelems The maximum desired error tolerance for each state.
|
||||
* @param rho A weighting factor that balances control effort and state
|
||||
* excursion. Greater values penalize state excursion more heavily. 1 is a
|
||||
* good starting value.
|
||||
* @param Relems The maximum desired control effort for each input.
|
||||
* @param dt Discretization timestep.
|
||||
*/
|
||||
LinearQuadraticRegulator(const Eigen::Matrix<double, States, States>& A,
|
||||
const Eigen::Matrix<double, States, Inputs>& B,
|
||||
const std::array<double, States>& Qelems,
|
||||
const double rho,
|
||||
const std::array<double, Inputs>& Relems,
|
||||
units::second_t dt)
|
||||
: detail::LinearQuadraticRegulatorImpl<States, Inputs>{
|
||||
A, B, Qelems, rho, Relems, dt} {}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param A Continuous system matrix of the plant being controlled.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param Q The state cost matrix.
|
||||
* @param R The input cost matrix.
|
||||
* @param dt Discretization timestep.
|
||||
*/
|
||||
LinearQuadraticRegulator(const Eigen::Matrix<double, States, States>& A,
|
||||
const Eigen::Matrix<double, States, Inputs>& B,
|
||||
const Eigen::Matrix<double, States, States>& Q,
|
||||
const Eigen::Matrix<double, Inputs, Inputs>& R,
|
||||
units::second_t dt)
|
||||
: detail::LinearQuadraticRegulatorImpl<States, Inputs>{A, B, Q, R, dt} {}
|
||||
|
||||
LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
|
||||
LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
|
||||
};
|
||||
|
||||
// Template specializations are used here to make common state-input pairs
|
||||
// compile faster.
|
||||
template <>
|
||||
class LinearQuadraticRegulator<1, 1>
|
||||
: public detail::LinearQuadraticRegulatorImpl<1, 1> {
|
||||
public:
|
||||
template <int Outputs>
|
||||
LinearQuadraticRegulator(const LinearSystem<1, 1, Outputs>& plant,
|
||||
const std::array<double, 1>& Qelems,
|
||||
const std::array<double, 1>& Relems,
|
||||
units::second_t dt)
|
||||
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, 1.0, Relems,
|
||||
dt) {}
|
||||
|
||||
template <int Outputs>
|
||||
LinearQuadraticRegulator(const LinearSystem<1, 1, Outputs>& plant,
|
||||
const std::array<double, 1>& Qelems,
|
||||
const double rho,
|
||||
const std::array<double, 1>& Relems,
|
||||
units::second_t dt)
|
||||
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, rho, Relems,
|
||||
dt) {}
|
||||
|
||||
LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
|
||||
const Eigen::Matrix<double, 1, 1>& B,
|
||||
const std::array<double, 1>& Qelems,
|
||||
const std::array<double, 1>& Relems,
|
||||
units::second_t dt);
|
||||
|
||||
LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
|
||||
const Eigen::Matrix<double, 1, 1>& B,
|
||||
const std::array<double, 1>& Qelems,
|
||||
const double rho,
|
||||
const std::array<double, 1>& Relems,
|
||||
units::second_t dt);
|
||||
|
||||
LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
|
||||
const Eigen::Matrix<double, 1, 1>& B,
|
||||
const Eigen::Matrix<double, 1, 1>& Q,
|
||||
const Eigen::Matrix<double, 1, 1>& R,
|
||||
units::second_t dt);
|
||||
|
||||
LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
|
||||
LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
|
||||
};
|
||||
|
||||
// Template specializations are used here to make common state-input pairs
|
||||
// compile faster.
|
||||
template <>
|
||||
class LinearQuadraticRegulator<2, 1>
|
||||
: public detail::LinearQuadraticRegulatorImpl<2, 1> {
|
||||
public:
|
||||
template <int Outputs>
|
||||
LinearQuadraticRegulator(const LinearSystem<2, 1, Outputs>& plant,
|
||||
const std::array<double, 2>& Qelems,
|
||||
const std::array<double, 1>& Relems,
|
||||
units::second_t dt)
|
||||
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, 1.0, Relems,
|
||||
dt) {}
|
||||
|
||||
template <int Outputs>
|
||||
LinearQuadraticRegulator(const LinearSystem<2, 1, Outputs>& plant,
|
||||
const std::array<double, 2>& Qelems,
|
||||
const double rho,
|
||||
const std::array<double, 1>& Relems,
|
||||
units::second_t dt)
|
||||
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, rho, Relems,
|
||||
dt) {}
|
||||
|
||||
LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
|
||||
const Eigen::Matrix<double, 2, 1>& B,
|
||||
const std::array<double, 2>& Qelems,
|
||||
const std::array<double, 1>& Relems,
|
||||
units::second_t dt);
|
||||
|
||||
LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
|
||||
const Eigen::Matrix<double, 2, 1>& B,
|
||||
const std::array<double, 2>& Qelems,
|
||||
const double rho,
|
||||
const std::array<double, 1>& Relems,
|
||||
units::second_t dt);
|
||||
|
||||
LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
|
||||
const Eigen::Matrix<double, 2, 1>& B,
|
||||
const Eigen::Matrix<double, 2, 2>& Q,
|
||||
const Eigen::Matrix<double, 1, 1>& R,
|
||||
units::second_t dt);
|
||||
|
||||
LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
|
||||
LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
Reference in New Issue
Block a user