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

[wpimath] Make controllers and some trajectory classes constexpr (#7343)

Browse Source
This commit is contained in:
Tyler Veness
2024-11-07 13:02:11 -08:00
committed by GitHub
parent 44a45d44e2
commit a66fa339dc
71 changed files with 1512 additions and 1900 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
wpimath/src/main/native/include/frc/system/LinearSystem.h
View File

@@ -6,14 +6,14 @@
#include <algorithm>
#include <concepts>
#include <functional>
#include <stdexcept>
#include <type_traits>
#include <gcem.hpp>
#include <wpi/Algorithm.h>
#include <wpi/SmallVector.h>
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/Discretization.h"
#include "units/time.h"
@@ -47,26 +47,41 @@ class LinearSystem {
* @param D Feedthrough matrix.
* @throws std::domain_error if any matrix element isn't finite.
*/
LinearSystem(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const Matrixd<Outputs, States>& C,
const Matrixd<Outputs, Inputs>& D) {
if (!A.allFinite()) {
constexpr LinearSystem(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const Matrixd<Outputs, States>& C,
const Matrixd<Outputs, Inputs>& D) {
auto allFinite = [](const auto& mat) {
if (std::is_constant_evaluated()) {
for (int row = 0; row < mat.rows(); ++row) {
for (int col = 0; col < mat.cols(); ++col) {
if (!gcem::internal::is_finite(mat.coeff(row, col))) {
return false;
}
}
}
return true;
} else {
return mat.allFinite();
}
};
if (!allFinite(A)) {
throw std::domain_error(
"Elements of A aren't finite. This is usually due to model "
"implementation errors.");
}
if (!B.allFinite()) {
if (!allFinite(B)) {
throw std::domain_error(
"Elements of B aren't finite. This is usually due to model "
"implementation errors.");
}
if (!C.allFinite()) {
if (!allFinite(C)) {
throw std::domain_error(
"Elements of C aren't finite. This is usually due to model "
"implementation errors.");
}
if (!D.allFinite()) {
if (!allFinite(D)) {
throw std::domain_error(
"Elements of D aren't finite. This is usually due to model "
"implementation errors.");
@@ -78,15 +93,15 @@ class LinearSystem {
m_D = D;
}
LinearSystem(const LinearSystem&) = default;
LinearSystem& operator=(const LinearSystem&) = default;
LinearSystem(LinearSystem&&) = default;
LinearSystem& operator=(LinearSystem&&) = default;
constexpr LinearSystem(const LinearSystem&) = default;
constexpr LinearSystem& operator=(const LinearSystem&) = default;
constexpr LinearSystem(LinearSystem&&) = default;
constexpr LinearSystem& operator=(LinearSystem&&) = default;
/**
* Returns the system matrix A.
*/
const Matrixd<States, States>& A() const { return m_A; }
constexpr const Matrixd<States, States>& A() const { return m_A; }
/**
* Returns an element of the system matrix A.
@@ -94,12 +109,12 @@ class LinearSystem {
* @param i Row of A.
* @param j Column of A.
*/
double A(int i, int j) const { return m_A(i, j); }
constexpr double A(int i, int j) const { return m_A(i, j); }
/**
* Returns the input matrix B.
*/
const Matrixd<States, Inputs>& B() const { return m_B; }
constexpr const Matrixd<States, Inputs>& B() const { return m_B; }
/**
* Returns an element of the input matrix B.
@@ -107,12 +122,12 @@ class LinearSystem {
* @param i Row of B.
* @param j Column of B.
*/
double B(int i, int j) const { return m_B(i, j); }
constexpr double B(int i, int j) const { return m_B(i, j); }
/**
* Returns the output matrix C.
*/
const Matrixd<Outputs, States>& C() const { return m_C; }
constexpr const Matrixd<Outputs, States>& C() const { return m_C; }
/**
* Returns an element of the output matrix C.
@@ -120,12 +135,12 @@ class LinearSystem {
* @param i Row of C.
* @param j Column of C.
*/
double C(int i, int j) const { return m_C(i, j); }
constexpr double C(int i, int j) const { return m_C(i, j); }
/**
* Returns the feedthrough matrix D.
*/
const Matrixd<Outputs, Inputs>& D() const { return m_D; }
constexpr const Matrixd<Outputs, Inputs>& D() const { return m_D; }
/**
* Returns an element of the feedthrough matrix D.
@@ -133,7 +148,7 @@ class LinearSystem {
* @param i Row of D.
* @param j Column of D.
*/
double D(int i, int j) const { return m_D(i, j); }
constexpr double D(int i, int j) const { return m_D(i, j); }
/**
* Computes the new x given the old x and the control input.

217
wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
View File

@@ -7,6 +7,7 @@
#include <concepts>
#include <stdexcept>
#include <gcem.hpp>
#include <wpi/SymbolExports.h>
#include "frc/system/LinearSystem.h"
@@ -44,10 +45,32 @@ class WPILIB_DLLEXPORT LinearSystemId {
* @param gearing Gear ratio from motor to carriage.
* @throws std::domain_error if mass <= 0, radius <= 0, or gearing <= 0.
*/
static LinearSystem<2, 1, 2> ElevatorSystem(DCMotor motor,
units::kilogram_t mass,
units::meter_t radius,
double gearing);
static constexpr LinearSystem<2, 1, 2> ElevatorSystem(DCMotor motor,
units::kilogram_t mass,
units::meter_t radius,
double gearing) {
if (mass <= 0_kg) {
throw std::domain_error("mass must be greater than zero.");
}
if (radius <= 0_m) {
throw std::domain_error("radius must be greater than zero.");
}
if (gearing <= 0.0) {
throw std::domain_error("gearing must be greater than zero.");
}
Matrixd<2, 2> A{
{0.0, 1.0},
{0.0, (-gcem::pow(gearing, 2) * motor.Kt /
(motor.R * units::math::pow<2>(radius) * mass * motor.Kv))
.value()}};
Matrixd<2, 1> B{{0.0},
{(gearing * motor.Kt / (motor.R * radius * mass)).value()}};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 1> D{{0.0}, {0.0}};
return LinearSystem<2, 1, 2>(A, B, C, D);
}
/**
* Create a state-space model of a single-jointed arm system.The states of the
@@ -59,8 +82,25 @@ class WPILIB_DLLEXPORT LinearSystemId {
* @param gearing Gear ratio from motor to arm.
* @throws std::domain_error if J <= 0 or gearing <= 0.
*/
static LinearSystem<2, 1, 2> SingleJointedArmSystem(
DCMotor motor, units::kilogram_square_meter_t J, double gearing);
static constexpr LinearSystem<2, 1, 2> SingleJointedArmSystem(
DCMotor motor, units::kilogram_square_meter_t J, double gearing) {
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
if (gearing <= 0.0) {
throw std::domain_error("gearing must be greater than zero.");
}
Matrixd<2, 2> A{
{0.0, 1.0},
{0.0, (-gcem::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J))
.value()}};
Matrixd<2, 1> B{{0.0}, {(gearing * motor.Kt / (motor.R * J)).value()}};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 1> D{{0.0}, {0.0}};
return LinearSystem<2, 1, 2>(A, B, C, D);
}
/**
* Create a state-space model for a 1 DOF velocity system from its kV
@@ -86,7 +126,7 @@ class WPILIB_DLLEXPORT LinearSystemId {
template <typename Distance>
requires std::same_as<units::meter, Distance> ||
std::same_as<units::radian, Distance>
static LinearSystem<1, 1, 1> IdentifyVelocitySystem(
static constexpr LinearSystem<1, 1, 1> IdentifyVelocitySystem(
decltype(1_V / Velocity_t<Distance>(1)) kV,
decltype(1_V / Acceleration_t<Distance>(1)) kA) {
if (kV < decltype(kV){0}) {
@@ -96,10 +136,10 @@ class WPILIB_DLLEXPORT LinearSystemId {
throw std::domain_error("Ka must be greater than zero.");
}
Matrixd<1, 1> A{-kV.value() / kA.value()};
Matrixd<1, 1> B{1.0 / kA.value()};
Matrixd<1, 1> C{1.0};
Matrixd<1, 1> D{0.0};
Matrixd<1, 1> A{{-kV.value() / kA.value()}};
Matrixd<1, 1> B{{1.0 / kA.value()}};
Matrixd<1, 1> C{{1.0}};
Matrixd<1, 1> D{{0.0}};
return LinearSystem<1, 1, 1>(A, B, C, D);
}
@@ -129,7 +169,7 @@ class WPILIB_DLLEXPORT LinearSystemId {
template <typename Distance>
requires std::same_as<units::meter, Distance> ||
std::same_as<units::radian, Distance>
static LinearSystem<2, 1, 1> IdentifyPositionSystem(
static constexpr LinearSystem<2, 1, 1> IdentifyPositionSystem(
decltype(1_V / Velocity_t<Distance>(1)) kV,
decltype(1_V / Acceleration_t<Distance>(1)) kA) {
if (kV < decltype(kV){0}) {
@@ -169,9 +209,41 @@ class WPILIB_DLLEXPORT LinearSystemId {
* @see <a
* href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
*/
static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(
static constexpr LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(
decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular);
decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular) {
if (kVLinear <= decltype(kVLinear){0}) {
throw std::domain_error("Kv,linear must be greater than zero.");
}
if (kALinear <= decltype(kALinear){0}) {
throw std::domain_error("Ka,linear must be greater than zero.");
}
if (kVAngular <= decltype(kVAngular){0}) {
throw std::domain_error("Kv,angular must be greater than zero.");
}
if (kAAngular <= decltype(kAAngular){0}) {
throw std::domain_error("Ka,angular must be greater than zero.");
}
double A1 = -(kVLinear.value() / kALinear.value() +
kVAngular.value() / kAAngular.value());
double A2 = -(kVLinear.value() / kALinear.value() -
kVAngular.value() / kAAngular.value());
double B1 = 1.0 / kALinear.value() + 1.0 / kAAngular.value();
double B2 = 1.0 / kALinear.value() - 1.0 / kAAngular.value();
A1 /= 2.0;
A2 /= 2.0;
B1 /= 2.0;
B2 /= 2.0;
Matrixd<2, 2> A{{A1, A2}, {A2, A1}};
Matrixd<2, 2> B{{B1, B2}, {B2, B1}};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
return LinearSystem<2, 2, 2>(A, B, C, D);
}
/**
* Identify a differential drive drivetrain given the drivetrain's kV and kA
@@ -198,10 +270,41 @@ class WPILIB_DLLEXPORT LinearSystemId {
* @see <a
* href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
*/
static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(
static constexpr LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(
decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
decltype(1_V / 1_rad_per_s) kVAngular,
decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth);
decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth) {
if (kVLinear <= decltype(kVLinear){0}) {
throw std::domain_error("Kv,linear must be greater than zero.");
}
if (kALinear <= decltype(kALinear){0}) {
throw std::domain_error("Ka,linear must be greater than zero.");
}
if (kVAngular <= decltype(kVAngular){0}) {
throw std::domain_error("Kv,angular must be greater than zero.");
}
if (kAAngular <= decltype(kAAngular){0}) {
throw std::domain_error("Ka,angular must be greater than zero.");
}
if (trackwidth <= 0_m) {
throw std::domain_error("r must be greater than zero.");
}
// We want to find a factor to include in Kv,angular that will convert
// `u = Kv,angular omega` to `u = Kv,angular v`.
//
// v = omega r
// omega = v/r
// omega = 1/r v
// omega = 1/(trackwidth/2) v
// omega = 2/trackwidth v
//
// So multiplying by 2/trackwidth converts the angular gains from V/(rad/s)
// to V/(m/s).
return IdentifyDrivetrainSystem(kVLinear, kALinear,
kVAngular * 2.0 / trackwidth * 1_rad,
kAAngular * 2.0 / trackwidth * 1_rad);
}
/**
* Create a state-space model of a flywheel system, the states of the system
@@ -213,9 +316,24 @@ class WPILIB_DLLEXPORT LinearSystemId {
* @param gearing Gear ratio from motor to flywheel.
* @throws std::domain_error if J <= 0 or gearing <= 0.
*/
static LinearSystem<1, 1, 1> FlywheelSystem(DCMotor motor,
units::kilogram_square_meter_t J,
double gearing);
static constexpr LinearSystem<1, 1, 1> FlywheelSystem(
DCMotor motor, units::kilogram_square_meter_t J, double gearing) {
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
if (gearing <= 0.0) {
throw std::domain_error("gearing must be greater than zero.");
}
Matrixd<1, 1> A{
{(-gcem::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J))
.value()}};
Matrixd<1, 1> B{{(gearing * motor.Kt / (motor.R * J)).value()}};
Matrixd<1, 1> C{{1.0}};
Matrixd<1, 1> D{{0.0}};
return LinearSystem<1, 1, 1>(A, B, C, D);
}
/**
* Create a state-space model of a DC motor system. The states of the system
@@ -229,9 +347,25 @@ class WPILIB_DLLEXPORT LinearSystemId {
* @see <a
* href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
*/
static LinearSystem<2, 1, 2> DCMotorSystem(DCMotor motor,
units::kilogram_square_meter_t J,
double gearing);
static constexpr LinearSystem<2, 1, 2> DCMotorSystem(
DCMotor motor, units::kilogram_square_meter_t J, double gearing) {
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
if (gearing <= 0.0) {
throw std::domain_error("gearing must be greater than zero.");
}
Matrixd<2, 2> A{
{0.0, 1.0},
{0.0, (-gcem::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J))
.value()}};
Matrixd<2, 1> B{{0.0}, {(gearing * motor.Kt / (motor.R * J)).value()}};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 1> D{{0.0}, {0.0}};
return LinearSystem<2, 1, 2>(A, B, C, D);
}
/**
* Create a state-space model of a DC motor system from its kV
@@ -256,7 +390,7 @@ class WPILIB_DLLEXPORT LinearSystemId {
template <typename Distance>
requires std::same_as<units::meter, Distance> ||
std::same_as<units::radian, Distance>
static LinearSystem<2, 1, 2> DCMotorSystem(
static constexpr LinearSystem<2, 1, 2> DCMotorSystem(
decltype(1_V / Velocity_t<Distance>(1)) kV,
decltype(1_V / Acceleration_t<Distance>(1)) kA) {
if (kV < decltype(kV){0}) {
@@ -289,9 +423,42 @@ class WPILIB_DLLEXPORT LinearSystemId {
* @throws std::domain_error if mass <= 0, r <= 0, rb <= 0, J <= 0, or
* gearing <= 0.
*/
static LinearSystem<2, 2, 2> DrivetrainVelocitySystem(
static constexpr LinearSystem<2, 2, 2> DrivetrainVelocitySystem(
const DCMotor& motor, units::kilogram_t mass, units::meter_t r,
units::meter_t rb, units::kilogram_square_meter_t J, double gearing);
units::meter_t rb, units::kilogram_square_meter_t J, double gearing) {
if (mass <= 0_kg) {
throw std::domain_error("mass must be greater than zero.");
}
if (r <= 0_m) {
throw std::domain_error("r must be greater than zero.");
}
if (rb <= 0_m) {
throw std::domain_error("rb must be greater than zero.");
}
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
if (gearing <= 0.0) {
throw std::domain_error("gearing must be greater than zero.");
}
auto C1 = -gcem::pow(gearing, 2) * motor.Kt /
(motor.Kv * motor.R * units::math::pow<2>(r));
auto C2 = gearing * motor.Kt / (motor.R * r);
Matrixd<2, 2> A{{((1 / mass + units::math::pow<2>(rb) / J) * C1).value(),
((1 / mass - units::math::pow<2>(rb) / J) * C1).value()},
{((1 / mass - units::math::pow<2>(rb) / J) * C1).value(),
((1 / mass + units::math::pow<2>(rb) / J) * C1).value()}};
Matrixd<2, 2> B{{((1 / mass + units::math::pow<2>(rb) / J) * C2).value(),
((1 / mass - units::math::pow<2>(rb) / J) * C2).value()},
{((1 / mass - units::math::pow<2>(rb) / J) * C2).value(),
((1 / mass + units::math::pow<2>(rb) / J) * C2).value()}};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
return LinearSystem<2, 2, 2>(A, B, C, D);
}
};
} // namespace frc