Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-27 02:01:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add core State-space classes (#2614)

Browse Source 
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
2020-08-14 23:40:33 -07:00
committed by GitHub
parent e5b84e2f87
commit 3b283ab9aa
84 changed files with 11747 additions and 174 deletions
Download Patch File Download Diff File Expand all files Collapse all files

165
wpimath/src/main/native/include/frc/system/Discretization.h Normal file
View File

@@ -0,0 +1,165 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-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 "Eigen/Core"
#include "units/time.h"
#include "unsupported/Eigen/MatrixFunctions"
namespace frc {
/**
* Discretizes the given continuous A matrix.
*
* @param contA Continuous system matrix.
* @param dt Discretization timestep.
* @param discA Storage for discrete system matrix.
*/
template <int States>
void DiscretizeA(const Eigen::Matrix<double, States, States>& contA,
units::second_t dt,
Eigen::Matrix<double, States, States>* discA) {
*discA = (contA * dt.to<double>()).exp();
}
/**
* Discretizes the given continuous A and B matrices.
*
* @param contA Continuous system matrix.
* @param contB Continuous input matrix.
* @param dt Discretization timestep.
* @param discA Storage for discrete system matrix.
* @param discB Storage for discrete input matrix.
*/
template <int States, int Inputs>
void DiscretizeAB(const Eigen::Matrix<double, States, States>& contA,
const Eigen::Matrix<double, States, Inputs>& contB,
units::second_t dt,
Eigen::Matrix<double, States, States>* discA,
Eigen::Matrix<double, States, Inputs>* discB) {
// Matrices are blocked here to minimize matrix exponentiation calculations
Eigen::Matrix<double, States + Inputs, States + Inputs> Mcont;
Mcont.setZero();
Mcont.template block<States, States>(0, 0) = contA * dt.to<double>();
Mcont.template block<States, Inputs>(0, States) = contB * dt.to<double>();
// Discretize A and B with the given timestep
Eigen::Matrix<double, States + Inputs, States + Inputs> Mdisc = Mcont.exp();
*discA = Mdisc.template block<States, States>(0, 0);
*discB = Mdisc.template block<States, Inputs>(0, States);
}
/**
* Discretizes the given continuous A and Q matrices.
*
* @param contA Continuous system matrix.
* @param contQ Continuous process noise covariance matrix.
* @param dt Discretization timestep.
* @param discA Storage for discrete system matrix.
* @param discQ Storage for discrete process noise covariance matrix.
*/
template <int States>
void DiscretizeAQ(const Eigen::Matrix<double, States, States>& contA,
const Eigen::Matrix<double, States, States>& contQ,
units::second_t dt,
Eigen::Matrix<double, States, States>* discA,
Eigen::Matrix<double, States, States>* discQ) {
// Make continuous Q symmetric if it isn't already
Eigen::Matrix<double, States, States> Q = (contQ + contQ.transpose()) / 2.0;
// Set up the matrix M = [[-A, Q], [0, A.T]]
Eigen::Matrix<double, 2 * States, 2 * States> M;
M.template block<States, States>(0, 0) = -contA;
M.template block<States, States>(0, States) = Q;
M.template block<States, States>(States, 0).setZero();
M.template block<States, States>(States, States) = contA.transpose();
Eigen::Matrix<double, 2 * States, 2 * States> phi =
(M * dt.to<double>()).exp();
// Phi12 = phi[0:States, States:2*States]
// Phi22 = phi[States:2*States, States:2*States]
Eigen::Matrix<double, States, States> phi12 =
phi.block(0, States, States, States);
Eigen::Matrix<double, States, States> phi22 =
phi.block(States, States, States, States);
*discA = phi22.transpose();
Q = *discA * phi12;
// Make discrete Q symmetric if it isn't already
*discQ = (Q + Q.transpose()) / 2.0;
}
/**
* Discretizes the given continuous A and Q matrices.
*
* Rather than solving a 2N x 2N matrix exponential like in DiscretizeAQ()
* (which is expensive), we take advantage of the structure of the block matrix
* of A and Q.
*
* 1) The exponential of A*t, which is only N x N, is relatively cheap.
* 2) The upper-right quarter of the 2N x 2N matrix, which we can approximate
* using a taylor series to several terms and still be substantially cheaper
* than taking the big exponential.
*
* @param contA Continuous system matrix.
* @param contQ Continuous process noise covariance matrix.
* @param dt Discretization timestep.
* @param discA Storage for discrete system matrix.
* @param discQ Storage for discrete process noise covariance matrix.
*/
template <int States>
void DiscretizeAQTaylor(const Eigen::Matrix<double, States, States>& contA,
const Eigen::Matrix<double, States, States>& contQ,
units::second_t dt,
Eigen::Matrix<double, States, States>* discA,
Eigen::Matrix<double, States, States>* discQ) {
// Make continuous Q symmetric if it isn't already
Eigen::Matrix<double, States, States> Q = (contQ + contQ.transpose()) / 2.0;
Eigen::Matrix<double, States, States> lastTerm = Q;
double lastCoeff = dt.to<double>();
// A^T^n
Eigen::Matrix<double, States, States> Atn = contA.transpose();
Eigen::Matrix<double, States, States> phi12 = lastTerm * lastCoeff;
// i = 6 i.e. 5th order should be enough precision
for (int i = 2; i < 6; ++i) {
lastTerm = -contA * lastTerm + Q * Atn;
lastCoeff *= dt.to<double>() / static_cast<double>(i);
phi12 += lastTerm * lastCoeff;
Atn *= contA.transpose();
}
DiscretizeA<States>(contA, dt, discA);
Q = *discA * phi12;
// Make discrete Q symmetric if it isn't already
*discQ = (Q + Q.transpose()) / 2.0;
}
/**
* Returns a discretized version of the provided continuous measurement noise
* covariance matrix.
*
* @param R Continuous measurement noise covariance matrix.
* @param dt Discretization timestep.
*/
template <int Outputs>
Eigen::Matrix<double, Outputs, Outputs> DiscretizeR(
const Eigen::Matrix<double, Outputs, Outputs>& R, units::second_t dt) {
return R / dt.to<double>();
}
} // namespace frc

164
wpimath/src/main/native/include/frc/system/LinearSystem.h Normal file
View File

@@ -0,0 +1,164 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-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 <algorithm>
#include <functional>
#include "Eigen/Core"
#include "frc/StateSpaceUtil.h"
#include "frc/system/Discretization.h"
#include "units/time.h"
namespace frc {
/**
* A plant defined using state-space notation.
*
* A plant is a mathematical model of a system's dynamics.
*
* For more on the underlying math, read
* https://file.tavsys.net/control/state-space-guide.pdf.
*/
template <int States, int Inputs, int Outputs>
class LinearSystem {
public:
/**
* Constructs a discrete plant with the given continuous system coefficients.
*
* @param A System matrix.
* @param B Input matrix.
* @param C Output matrix.
* @param D Feedthrough matrix.
*/
LinearSystem(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, Outputs, States>& C,
const Eigen::Matrix<double, Outputs, Inputs>& D) {
m_A = A;
m_B = B;
m_C = C;
m_D = D;
}
LinearSystem(const LinearSystem&) = default;
LinearSystem& operator=(const LinearSystem&) = default;
LinearSystem(LinearSystem&&) = default;
LinearSystem& operator=(LinearSystem&&) = default;
/**
* Returns the system matrix A.
*/
const Eigen::Matrix<double, States, States>& A() const { return m_A; }
/**
* Returns an element of the system matrix A.
*
* @param i Row of A.
* @param j Column of A.
*/
double A(int i, int j) const { return m_A(i, j); }
/**
* Returns the input matrix B.
*/
const Eigen::Matrix<double, States, Inputs>& B() const { return m_B; }
/**
* Returns an element of the input matrix B.
*
* @param i Row of B.
* @param j Column of B.
*/
double B(int i, int j) const { return m_B(i, j); }
/**
* Returns the output matrix C.
*/
const Eigen::Matrix<double, Outputs, States>& C() const { return m_C; }
/**
* Returns an element of the output matrix C.
*
* @param i Row of C.
* @param j Column of C.
*/
double C(int i, int j) const { return m_C(i, j); }
/**
* Returns the feedthrough matrix D.
*/
const Eigen::Matrix<double, Outputs, Inputs>& D() const { return m_D; }
/**
* Returns an element of the feedthrough matrix D.
*
* @param i Row of D.
* @param j Column of D.
*/
double D(int i, int j) const { return m_D(i, j); }
/**
* Computes the new x given the old x and the control input.
*
* This is used by state observers directly to run updates based on state
* estimate.
*
* @param x The current state.
* @param u The control input.
* @param dt Timestep for model update.
*/
Eigen::Matrix<double, States, 1> CalculateX(
const Eigen::Matrix<double, States, 1>& x,
const Eigen::Matrix<double, Inputs, 1>& clampedU,
units::second_t dt) const {
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, Inputs> discB;
DiscretizeAB<States, Inputs>(m_A, m_B, dt, &discA, &discB);
return discA * x + discB * clampedU;
}
/**
* Computes the new y given the control input.
*
* This is used by state observers directly to run updates based on state
* estimate.
*
* @param x The current state.
* @param clampedU The control input.
*/
Eigen::Matrix<double, Outputs, 1> CalculateY(
const Eigen::Matrix<double, States, 1>& x,
const Eigen::Matrix<double, Inputs, 1>& clampedU) const {
return m_C * x + m_D * clampedU;
}
private:
/**
* Continuous system matrix.
*/
Eigen::Matrix<double, States, States> m_A;
/**
* Continuous input matrix.
*/
Eigen::Matrix<double, States, Inputs> m_B;
/**
* Output matrix.
*/
Eigen::Matrix<double, Outputs, States> m_C;
/**
* Feedthrough matrix.
*/
Eigen::Matrix<double, Outputs, Inputs> m_D;
};
} // namespace frc

262
wpimath/src/main/native/include/frc/system/LinearSystemLoop.h Normal file
View File

@@ -0,0 +1,262 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-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 "Eigen/Core"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/controller/LinearQuadraticRegulator.h"
#include "frc/estimator/KalmanFilter.h"
#include "frc/system/LinearSystem.h"
#include "units/time.h"
#include "units/voltage.h"
namespace frc {
/**
* Combines a plant, controller, and observer for controlling a mechanism with
* full state feedback.
*
* For everything in this file, "inputs" and "outputs" are defined from the
* perspective of the plant. This means U is an input and Y is an output
* (because you give the plant U (powers) and it gives you back a Y (sensor
* values). This is the opposite of what they mean from the perspective of the
* controller (U is an output because that's what goes to the motors and Y is an
* input because that's what comes back from the sensors).
*
* For more on the underlying math, read
* https://file.tavsys.net/control/state-space-guide.pdf.
*/
template <int States, int Inputs, int Outputs>
class LinearSystemLoop {
public:
/**
* Constructs a state-space loop with the given plant, controller, and
* observer. By default, the initial reference is all zeros. Users should
* call reset with the initial system state before enabling the loop.
*
* @param plant State-space plant.
* @param controller State-space controller.
* @param feedforward Plant inversion feedforward.
* @param observer State-space observer.
* @param maxVoltageVolts The maximum voltage that can be applied. Assumes
* that the inputs are voltages.
*/
LinearSystemLoop(LinearSystem<States, Inputs, Outputs>& plant,
LinearQuadraticRegulator<States, Inputs>& controller,
LinearPlantInversionFeedforward<States, Inputs>& feedforward,
KalmanFilter<States, Inputs, Outputs>& observer,
units::volt_t maxVoltage)
: LinearSystemLoop(plant, controller, feedforward, observer,
[=](Eigen::Matrix<double, Inputs, 1> u) {
return frc::NormalizeInputVector<Inputs>(
u, maxVoltage.template to<double>());
}) {}
/**
* Constructs a state-space loop with the given plant, controller, and
* observer.
*
* @param plant State-space plant.
* @param controller State-space controller.
* @param feedforward Plant-inversion feedforward.
* @param observer State-space observer.
*/
LinearSystemLoop(LinearSystem<States, Inputs, Outputs>& plant,
LinearQuadraticRegulator<States, Inputs>& controller,
LinearPlantInversionFeedforward<States, Inputs>& feedforward,
KalmanFilter<States, Inputs, Outputs>& observer,
std::function<Eigen::Matrix<double, Inputs, 1>(
const Eigen::Matrix<double, Inputs, 1>&)>
clampFunction)
: m_plant(plant),
m_controller(controller),
m_feedforward(feedforward),
m_observer(observer),
m_clampFunc(clampFunction) {
m_nextR.setZero();
Reset(m_nextR);
}
virtual ~LinearSystemLoop() = default;
LinearSystemLoop(LinearSystemLoop&&) = default;
LinearSystemLoop& operator=(LinearSystemLoop&&) = default;
/**
* Returns the observer's state estimate x-hat.
*/
const Eigen::Matrix<double, States, 1>& Xhat() const {
return m_observer.Xhat();
}
/**
* Returns an element of the observer's state estimate x-hat.
*
* @param i Row of x-hat.
*/
double Xhat(int i) const { return m_observer.Xhat(i); }
/**
* Returns the controller's next reference r.
*/
const Eigen::Matrix<double, States, 1>& NextR() const { return m_nextR; }
/**
* Returns an element of the controller's next reference r.
*
* @param i Row of r.
*/
double NextR(int i) const { return NextR()(i); }
/**
* Returns the controller's calculated control input u.
*/
Eigen::Matrix<double, Inputs, 1> U() const {
return ClampInput(m_controller.U() + m_feedforward.Uff());
}
/**
* Returns an element of the controller's calculated control input u.
*
* @param i Row of u.
*/
double U(int i) const { return U()(i); }
/**
* Set the initial state estimate x-hat.
*
* @param xHat The initial state estimate x-hat.
*/
void SetXhat(const Eigen::Matrix<double, States, 1>& xHat) {
m_observer.SetXhat(xHat);
}
/**
* Set an element of the initial state estimate x-hat.
*
* @param i Row of x-hat.
* @param value Value for element of x-hat.
*/
void SetXhat(int i, double value) { m_observer.SetXhat(i, value); }
/**
* Set the next reference r.
*
* @param nextR Next reference.
*/
void SetNextR(const Eigen::Matrix<double, States, 1>& nextR) {
m_nextR = nextR;
}
/**
* Return the plant used internally.
*/
const LinearSystem<States, Inputs, Outputs>& Plant() const { return m_plant; }
/**
* Return the controller used internally.
*/
const LinearQuadraticRegulator<States, Inputs>& Controller() const {
return m_controller;
}
/**
* Return the feedforward used internally.
*
* @return the feedforward used internally.
*/
const LinearPlantInversionFeedforward<States, Inputs> Feedforward() const {
return m_feedforward;
}
/**
* Return the observer used internally.
*/
const KalmanFilter<States, Inputs, Outputs>& Observer() const {
return m_observer;
}
/**
* Zeroes reference r, controller output u and plant output y.
* The previous reference for PlantInversionFeedforward is set to the
* initial reference.
* @param initialReference The initial reference.
*/
void Reset(Eigen::Matrix<double, States, 1> initialState) {
m_controller.Reset();
m_feedforward.Reset(initialState);
m_observer.Reset();
m_nextR.setZero();
}
/**
* Returns difference between reference r and x-hat.
*/
const Eigen::Matrix<double, States, 1> Error() const {
return m_controller.R() - m_observer.Xhat();
}
/**
* Correct the state estimate x-hat using the measurements in y.
*
* @param y Measurement vector.
*/
void Correct(const Eigen::Matrix<double, Outputs, 1>& y) {
m_observer.Correct(U(), y);
}
/**
* Sets new controller output, projects model forward, and runs observer
* prediction.
*
* After calling this, the user should send the elements of u to the
* actuators.
*
* @param dt Timestep for model update.
*/
void Predict(units::second_t dt) {
Eigen::Matrix<double, Inputs, 1> u =
ClampInput(m_controller.Calculate(m_observer.Xhat(), m_nextR) +
m_feedforward.Calculate(m_nextR));
m_observer.Predict(u, dt);
}
/**
* Clamps input vector between system's minimum and maximum allowable input.
*
* @param u Input vector to clamp.
* @return Clamped input vector.
*/
Eigen::Matrix<double, Inputs, 1> ClampInput(
const Eigen::Matrix<double, Inputs, 1>& u) const {
return m_clampFunc(u);
}
protected:
LinearSystem<States, Inputs, Outputs>& m_plant;
LinearQuadraticRegulator<States, Inputs>& m_controller;
LinearPlantInversionFeedforward<States, Inputs>& m_feedforward;
KalmanFilter<States, Inputs, Outputs>& m_observer;
/**
* Clamping function.
*/
std::function<Eigen::Matrix<double, Inputs, 1>(
const Eigen::Matrix<double, Inputs, 1>&)>
m_clampFunc;
// Reference to go to in the next cycle (used by feedforward controller).
Eigen::Matrix<double, States, 1> m_nextR;
// These are accessible from non-templated subclasses.
static constexpr int kStates = States;
static constexpr int kInputs = Inputs;
static constexpr int kOutputs = Outputs;
};
} // namespace frc

80
wpimath/src/main/native/include/frc/system/NumericalJacobian.h Normal file
View File

@@ -0,0 +1,80 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-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 "Eigen/Core"
namespace frc {
/**
* Returns numerical Jacobian with respect to x for f(x).
*
* @tparam Rows Number of rows in result of f(x).
* @tparam Cols Number of columns in result of f(x).
* @param f Vector-valued function from which to compute Jacobian.
* @param x Vector argument.
*/
template <int Rows, int Cols, typename F>
auto NumericalJacobian(F&& f, const Eigen::Matrix<double, Cols, 1>& x) {
constexpr double kEpsilon = 1e-5;
Eigen::Matrix<double, Rows, Cols> result;
result.setZero();
// It's more expensive, but +- epsilon will be more accurate
for (int i = 0; i < Cols; ++i) {
Eigen::Matrix<double, Cols, 1> dX_plus = x;
dX_plus(i) += kEpsilon;
Eigen::Matrix<double, Cols, 1> dX_minus = x;
dX_minus(i) -= kEpsilon;
result.col(i) = (f(dX_plus) - f(dX_minus)) / (kEpsilon * 2.0);
}
return result;
}
/**
* Returns numerical Jacobian with respect to x for f(x, u, ...).
*
* @tparam Rows Number of rows in result of f(x, u, ...).
* @tparam States Number of rows in x.
* @tparam Inputs Number of rows in u.
* @tparam F Function object type.
* @tparam Args... Remaining arguments to f(x, u, ...).
* @param f Vector-valued function from which to compute Jacobian.
* @param x State vector.
* @param u Input vector.
*/
template <int Rows, int States, int Inputs, typename F, typename... Args>
auto NumericalJacobianX(F&& f, const Eigen::Matrix<double, States, 1>& x,
const Eigen::Matrix<double, Inputs, 1>& u,
Args&&... args) {
return NumericalJacobian<Rows, States>(
[&](Eigen::Matrix<double, States, 1> x) { return f(x, u, args...); }, x);
}
/**
* Returns numerical Jacobian with respect to u for f(x, u, ...).
*
* @tparam Rows Number of rows in result of f(x, u, ...).
* @tparam States Number of rows in x.
* @tparam Inputs Number of rows in u.
* @tparam F Function object type.
* @tparam Args... Remaining arguments to f(x, u, ...).
* @param f Vector-valued function from which to compute Jacobian.
* @param x State vector.
* @param u Input vector.
*/
template <int Rows, int States, int Inputs, typename F, typename... Args>
auto NumericalJacobianU(F&& f, const Eigen::Matrix<double, States, 1>& x,
const Eigen::Matrix<double, Inputs, 1>& u,
Args&&... args) {
return NumericalJacobian<Rows, Inputs>(
[&](Eigen::Matrix<double, Inputs, 1> u) { return f(x, u, args...); }, u);
}
} // namespace frc

69
wpimath/src/main/native/include/frc/system/RungeKutta.h Normal file
View File

@@ -0,0 +1,69 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-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 "Eigen/Core"
#include "units/time.h"
namespace frc {
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
*
* @param f The function to integrate. It must take one argument x.
* @param x The initial value of x.
* @param dt The time over which to integrate.
*/
template <typename F, typename T>
T RungeKutta(F&& f, T x, units::second_t dt) {
const auto halfDt = 0.5 * dt;
T k1 = f(x);
T k2 = f(x + k1 * halfDt.to<double>());
T k3 = f(x + k2 * halfDt.to<double>());
T k4 = f(x + k3 * dt.to<double>());
return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
*
* @param f The function to integrate. It must take two arguments x and u.
* @param x The initial value of x.
* @param u The value u held constant over the integration period.
* @param dt The time over which to integrate.
*/
template <typename F, typename T, typename U>
T RungeKutta(F&& f, T x, U u, units::second_t dt) {
const auto halfDt = 0.5 * dt;
T k1 = f(x, u);
T k2 = f(x + k1 * halfDt.to<double>(), u);
T k3 = f(x + k2 * halfDt.to<double>(), u);
T k4 = f(x + k3 * dt.to<double>(), u);
return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(t, x) for dt.
*
* @param f The function to integrate. It must take two arguments x and t.
* @param x The initial value of x.
* @param t The intial value of t.
* @param dt The time over which to integrate.
*/
template <typename F, typename T>
T RungeKuttaTimeVarying(F&& f, T x, units::second_t t, units::second_t dt) {
const auto halfDt = 0.5 * dt;
T k1 = f(t, x);
T k2 = f(t + halfDt, x + k1 / 2.0);
T k3 = f(t + halfDt, x + k2 / 2.0);
T k4 = f(t + dt, x + k3);
return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
} // namespace frc

157
wpimath/src/main/native/include/frc/system/plant/DCMotor.h Normal file
View File

@@ -0,0 +1,157 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-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 "units/angular_velocity.h"
#include "units/current.h"
#include "units/impedance.h"
#include "units/torque.h"
#include "units/voltage.h"
namespace frc {
/**
* Holds the constants for a DC motor.
*/
class DCMotor {
public:
using radians_per_second_per_volt_t =
units::unit_t<units::compound_unit<units::radians_per_second,
units::inverse<units::volt>>>;
using newton_meters_per_ampere_t =
units::unit_t<units::compound_unit<units::newton_meters,
units::inverse<units::ampere>>>;
units::volt_t nominalVoltage;
units::newton_meter_t stallTorque;
units::ampere_t stallCurrent;
units::ampere_t freeCurrent;
units::radians_per_second_t freeSpeed;
// Resistance of motor
units::ohm_t R;
// Motor velocity constant
radians_per_second_per_volt_t Kv;
// Torque constant
newton_meters_per_ampere_t Kt;
/**
* Constructs a DC motor.
*
* @param nominalVoltage Voltage at which the motor constants were measured.
* @param stallTorque Current draw when stalled.
* @param stallCurrent Current draw when stalled.
* @param freeCurrent Current draw under no load.
* @param freeSpeed Angular velocity under no load.
* @param numMotors Number of motors in a gearbox.
*/
constexpr DCMotor(units::volt_t nominalVoltage,
units::newton_meter_t stallTorque,
units::ampere_t stallCurrent, units::ampere_t freeCurrent,
units::radians_per_second_t freeSpeed, int numMotors = 1)
: nominalVoltage(nominalVoltage),
stallTorque(stallTorque * numMotors),
stallCurrent(stallCurrent),
freeCurrent(freeCurrent),
freeSpeed(freeSpeed),
R(nominalVoltage / stallCurrent),
Kv(freeSpeed / (nominalVoltage - R * freeCurrent)),
Kt(stallTorque * numMotors / stallCurrent) {}
/**
* Returns current drawn by motor with given speed and input voltage.
*
* @param speed The current angular velocity of the motor.
* @param inputVoltage The voltage being applied to the motor.
*/
constexpr units::ampere_t Current(units::radians_per_second_t speed,
units::volt_t inputVoltage) const {
return -1.0 / Kv / R * speed + 1.0 / R * inputVoltage;
}
/**
* Returns instance of CIM.
*/
static constexpr DCMotor CIM(int numMotors = 1) {
return DCMotor(12_V, 2.42_Nm, 133_A, 2.7_A, 5310_rpm, numMotors);
}
/**
* Returns instance of MiniCIM.
*/
static constexpr DCMotor MiniCIM(int numMotors = 1) {
return DCMotor(12_V, 1.41_Nm, 89_A, 3_A, 5840_rpm, numMotors);
}
/**
* Returns instance of Bag motor.
*/
static constexpr DCMotor Bag(int numMotors = 1) {
return DCMotor(12_V, 0.43_Nm, 53_A, 1.8_A, 13180_rpm, numMotors);
}
/**
* Returns instance of Vex 775 Pro.
*/
static constexpr DCMotor Vex775Pro(int numMotors = 1) {
return DCMotor(12_V, 0.71_Nm, 134_A, 0.7_A, 18730_rpm, numMotors);
}
/**
* Returns instance of Andymark RS 775-125.
*/
static constexpr DCMotor RS775_125(int numMotors = 1) {
return DCMotor(12_V, 0.28_Nm, 18_A, 1.6_A, 5800_rpm, numMotors);
}
/**
* Returns instance of Banebots RS 775.
*/
static constexpr DCMotor BanebotsRS775(int numMotors = 1) {
return DCMotor(12_V, 0.72_Nm, 97_A, 2.7_A, 13050_rpm, numMotors);
}
/**
* Returns instance of Andymark 9015.
*/
static constexpr DCMotor Andymark9015(int numMotors = 1) {
return DCMotor(12_V, 0.36_Nm, 71_A, 3.7_A, 14270_rpm, numMotors);
}
/**
* Returns instance of Banebots RS 550.
*/
static constexpr DCMotor BanebotsRS550(int numMotors = 1) {
return DCMotor(12_V, 0.38_Nm, 84_A, 0.4_A, 19000_rpm, numMotors);
}
/**
* Returns instance of NEO brushless motor.
*/
static constexpr DCMotor NEO(int numMotors = 1) {
return DCMotor(12_V, 2.6_Nm, 105_A, 1.8_A, 5676_rpm, numMotors);
}
/**
* Returns instance of NEO 550 brushless motor.
*/
static constexpr DCMotor NEO550(int numMotors = 1) {
return DCMotor(12_V, 0.97_Nm, 100_A, 1.4_A, 11000_rpm, numMotors);
}
/**
* Returns instance of Falcon 500 brushless motor.
*/
static constexpr DCMotor Falcon500(int numMotors = 1) {
return DCMotor(12_V, 4.69_Nm, 257_A, 1.5_A, 6380_rpm, numMotors);
}
};
} // namespace frc

213
wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h Normal file
View File

@@ -0,0 +1,213 @@
/*----------------------------------------------------------------------------*/
/* 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 "frc/StateSpaceUtil.h"
#include "frc/system/LinearSystem.h"
#include "frc/system/plant/DCMotor.h"
#include "units/moment_of_inertia.h"
namespace frc {
class LinearSystemId {
public:
/**
* Constructs the state-space model for an elevator.
*
* States: [[position], [velocity]]
* Inputs: [[voltage]]
* Outputs: [[position]]
*
* @param motor Instance of DCMotor.
* @param m Carriage mass.
* @param r Pulley radius.
* @param G Gear ratio from motor to carriage.
*/
static LinearSystem<2, 1, 1> ElevatorSystem(DCMotor motor,
units::kilogram_t m,
units::meter_t r, double G) {
auto A = frc::MakeMatrix<2, 2>(
0.0, 1.0, 0.0,
(-std::pow(G, 2) * motor.Kt /
(motor.R * units::math::pow<2>(r) * m * motor.Kv))
.to<double>());
auto B = frc::MakeMatrix<2, 1>(
0.0, (G * motor.Kt / (motor.R * r * m)).to<double>());
auto C = frc::MakeMatrix<1, 2>(1.0, 0.0);
auto D = frc::MakeMatrix<1, 1>(0.0);
return LinearSystem<2, 1, 1>(A, B, C, D);
}
/**
* Constructs the state-space model for a single-jointed arm.
*
* States: [[angle], [angular velocity]]
* Inputs: [[voltage]]
* Outputs: [[angle]]
*
* @param motor Instance of DCMotor.
* @param J Moment of inertia.
* @param G Gear ratio from motor to carriage.
*/
static LinearSystem<2, 1, 1> SingleJointedArmSystem(
DCMotor motor, units::kilogram_square_meter_t J, double G) {
auto A = frc::MakeMatrix<2, 2>(
0.0, 1.0, 0.0,
(-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to<double>());
auto B =
frc::MakeMatrix<2, 1>(0.0, (G * motor.Kt / (motor.R * J)).to<double>());
auto C = frc::MakeMatrix<1, 2>(1.0, 0.0);
auto D = frc::MakeMatrix<1, 1>(0.0);
return LinearSystem<2, 1, 1>(A, B, C, D);
}
/**
* Constructs the state-space model for a 1 DOF velocity-only system from
* system identification data.
*
* States: [[velocity]]
* Inputs: [[voltage]]
* Outputs: [[velocity]]
*
* The parameters provided by the user are from this feedforward model:
*
* u = K_v v + K_a a
*
* @param kV The velocity gain, in volt seconds per distance.
* @param kA The acceleration gain, in volt seconds^2 per distance.
*/
static LinearSystem<1, 1, 1> IdentifyVelocitySystem(double kV, double kA) {
auto A = frc::MakeMatrix<1, 1>(-kV / kA);
auto B = frc::MakeMatrix<1, 1>(1.0 / kA);
auto C = frc::MakeMatrix<1, 1>(1.0);
auto D = frc::MakeMatrix<1, 1>(0.0);
return LinearSystem<1, 1, 1>(A, B, C, D);
}
/**
* Constructs the state-space model for a 1 DOF position system from system
* identification data.
*
* States: [[position], [velocity]]
* Inputs: [[voltage]]
* Outputs: [[position]]
*
* The parameters provided by the user are from this feedforward model:
*
* u = K_v v + K_a a
*
* @param kV The velocity gain, in volt seconds per distance.
* @param kA The acceleration gain, in volt seconds^2 per distance.
*/
static LinearSystem<2, 1, 1> IdentifyPositionSystem(double kV, double kA) {
auto A = frc::MakeMatrix<2, 2>(0.0, 1.0, 0.0, -kV / kA);
auto B = frc::MakeMatrix<2, 1>(0.0, 1.0 / kA);
auto C = frc::MakeMatrix<1, 2>(1.0, 0.0);
auto D = frc::MakeMatrix<1, 1>(0.0);
return LinearSystem<2, 1, 1>(A, B, C, D);
}
/**
* Constructs the state-space model for a 2 DOF drivetrain velocity system
* from system identification data.
*
* States: [[left velocity], [right velocity]]
* Inputs: [[left voltage], [right voltage]]
* Outputs: [[left velocity], [right velocity]]
*
* @param kVlinear The linear velocity gain, in volt seconds per distance.
* @param kAlinear The linear acceleration gain, in volt seconds^2 per
* distance.
* @param kVangular The angular velocity gain, in volt seconds per angle.
* @param kAangular The angular acceleration gain, in volt seconds^2 per
* angle.
*/
static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(double kVlinear,
double kAlinear,
double kVangular,
double kAangular) {
double c = 0.5 / (kAlinear * kAangular);
double A1 = c * (-kAlinear * kVangular - kVlinear * kAangular);
double A2 = c * (kAlinear * kVangular - kVlinear * kAangular);
double B1 = c * (kAlinear + kAangular);
double B2 = c * (kAangular - kAlinear);
auto A = frc::MakeMatrix<2, 2>(A1, A2, A2, A1);
auto B = frc::MakeMatrix<2, 2>(B1, B2, B2, B1);
auto C = frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0);
auto D = frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0);
return LinearSystem<2, 2, 2>(A, B, C, D);
}
/**
* Constructs the state-space model for a flywheel.
*
* States: [[angular velocity]]
* Inputs: [[voltage]]
* Outputs: [[angular velocity]]
*
* @param motor Instance of DCMotor.
* @param J Moment of inertia.
* @param G Gear ratio from motor to carriage.
*/
static LinearSystem<1, 1, 1> FlywheelSystem(DCMotor motor,
units::kilogram_square_meter_t J,
double G) {
auto A = frc::MakeMatrix<1, 1>(
(-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to<double>());
auto B = frc::MakeMatrix<1, 1>((G * motor.Kt / (motor.R * J)).to<double>());
auto C = frc::MakeMatrix<1, 1>(1.0);
auto D = frc::MakeMatrix<1, 1>(0.0);
return LinearSystem<1, 1, 1>(A, B, C, D);
}
/**
* Constructs the state-space model for a drivetrain.
*
* States: [[left velocity], [right velocity]]
* Inputs: [[left voltage], [right voltage]]
* Outputs: [[left velocity], [right velocity]]
*
* @param motor Instance of DCMotor.
* @param m Drivetrain mass.
* @param r Wheel radius.
* @param rb Robot radius.
* @param G Gear ratio from motor to wheel.
* @param J Moment of inertia.
*/
static LinearSystem<2, 2, 2> DrivetrainVelocitySystem(
DCMotor motor, units::kilogram_t m, units::meter_t r, units::meter_t rb,
units::kilogram_square_meter_t J, double G) {
auto C1 = -std::pow(G, 2) * motor.Kt /
(motor.Kv * motor.R * units::math::pow<2>(r));
auto C2 = G * motor.Kt / (motor.R * r);
auto A = frc::MakeMatrix<2, 2>(
((1 / m + units::math::pow<2>(rb) / J) * C1).to<double>(),
((1 / m - units::math::pow<2>(rb) / J) * C1).to<double>(),
((1 / m - units::math::pow<2>(rb) / J) * C1).to<double>(),
((1 / m + units::math::pow<2>(rb) / J) * C1).to<double>());
auto B = frc::MakeMatrix<2, 2>(
((1 / m + units::math::pow<2>(rb) / J) * C2).to<double>(),
((1 / m - units::math::pow<2>(rb) / J) * C2).to<double>(),
((1 / m - units::math::pow<2>(rb) / J) * C2).to<double>(),
((1 / m + units::math::pow<2>(rb) / J) * C2).to<double>());
auto C = frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0);
auto D = frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0);
return LinearSystem<2, 2, 2>(A, B, C, D);
}
};
} // namespace frc