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

[wpimath] Add typedefs for common types

Browse Source 
This makes complex code significantly easier to read.

frc::Vectord<Size> = Eigen::Vector<double, Size>
frc::Matrixd<Rows, Cols> = Eigen::Matrix<double, Rows, Cols>
This commit is contained in:
Peter Johnson
2022-04-29 22:29:20 -07:00
parent 97c493241f
commit e767605e94
76 changed files with 1136 additions and 1449 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
wpimath/src/main/native/include/frc/system/Discretization.h
View File

@@ -4,7 +4,7 @@
#pragma once
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "units/time.h"
#include "unsupported/Eigen/MatrixFunctions"
@@ -19,9 +19,8 @@ namespace frc {
* @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) {
void DiscretizeA(const Matrixd<States, States>& contA, units::second_t dt,
Matrixd<States, States>* discA) {
*discA = (contA * dt.value()).exp();
}
@@ -37,19 +36,18 @@ void DiscretizeA(const Eigen::Matrix<double, States, States>& contA,
* @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) {
void DiscretizeAB(const Matrixd<States, States>& contA,
const Matrixd<States, Inputs>& contB, units::second_t dt,
Matrixd<States, States>* discA,
Matrixd<States, Inputs>* discB) {
// Matrices are blocked here to minimize matrix exponentiation calculations
Eigen::Matrix<double, States + Inputs, States + Inputs> Mcont;
Matrixd<States + Inputs, States + Inputs> Mcont;
Mcont.setZero();
Mcont.template block<States, States>(0, 0) = contA * dt.value();
Mcont.template block<States, Inputs>(0, States) = contB * dt.value();
// Discretize A and B with the given timestep
Eigen::Matrix<double, States + Inputs, States + Inputs> Mdisc = Mcont.exp();
Matrixd<States + Inputs, States + Inputs> Mdisc = Mcont.exp();
*discA = Mdisc.template block<States, States>(0, 0);
*discB = Mdisc.template block<States, Inputs>(0, States);
}
@@ -65,29 +63,26 @@ void DiscretizeAB(const Eigen::Matrix<double, States, States>& contA,
* @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) {
void DiscretizeAQ(const Matrixd<States, States>& contA,
const Matrixd<States, States>& contQ, units::second_t dt,
Matrixd<States, States>* discA,
Matrixd<States, States>* discQ) {
// Make continuous Q symmetric if it isn't already
Eigen::Matrix<double, States, States> Q = (contQ + contQ.transpose()) / 2.0;
Matrixd<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;
Matrixd<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.value()).exp();
Matrixd<2 * States, 2 * States> phi = (M * dt.value()).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);
Matrixd<States, States> phi12 = phi.block(0, States, States, States);
Matrixd<States, States> phi22 = phi.block(States, States, States, States);
*discA = phi22.transpose();
@@ -117,21 +112,20 @@ void DiscretizeAQ(const Eigen::Matrix<double, States, States>& contA,
* @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) {
void DiscretizeAQTaylor(const Matrixd<States, States>& contA,
const Matrixd<States, States>& contQ,
units::second_t dt, Matrixd<States, States>* discA,
Matrixd<States, States>* discQ) {
// Make continuous Q symmetric if it isn't already
Eigen::Matrix<double, States, States> Q = (contQ + contQ.transpose()) / 2.0;
Matrixd<States, States> Q = (contQ + contQ.transpose()) / 2.0;
Eigen::Matrix<double, States, States> lastTerm = Q;
Matrixd<States, States> lastTerm = Q;
double lastCoeff = dt.value();
// Aᵀⁿ
Eigen::Matrix<double, States, States> Atn = contA.transpose();
Matrixd<States, States> Atn = contA.transpose();
Eigen::Matrix<double, States, States> phi12 = lastTerm * lastCoeff;
Matrixd<States, States> phi12 = lastTerm * lastCoeff;
// i = 6 i.e. 5th order should be enough precision
for (int i = 2; i < 6; ++i) {
@@ -159,8 +153,8 @@ void DiscretizeAQTaylor(const Eigen::Matrix<double, States, States>& contA,
* @param dt Discretization timestep.
*/
template <int Outputs>
Eigen::Matrix<double, Outputs, Outputs> DiscretizeR(
const Eigen::Matrix<double, Outputs, Outputs>& R, units::second_t dt) {
Matrixd<Outputs, Outputs> DiscretizeR(const Matrixd<Outputs, Outputs>& R,
units::second_t dt) {
return R / dt.value();
}

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

@@ -8,7 +8,7 @@
#include <functional>
#include <stdexcept>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/Discretization.h"
#include "units/time.h"
@@ -30,6 +30,10 @@ namespace frc {
template <int States, int Inputs, int Outputs>
class LinearSystem {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
using OutputVector = Vectord<Outputs>;
/**
* Constructs a discrete plant with the given continuous system coefficients.
*
@@ -39,10 +43,10 @@ class LinearSystem {
* @param D Feedthrough matrix.
* @throws std::domain_error if any matrix element isn't finite.
*/
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) {
LinearSystem(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const Matrixd<Outputs, States>& C,
const Matrixd<Outputs, Inputs>& D) {
if (!A.allFinite()) {
throw std::domain_error(
"Elements of A aren't finite. This is usually due to model "
@@ -78,7 +82,7 @@ class LinearSystem {
/**
* Returns the system matrix A.
*/
const Eigen::Matrix<double, States, States>& A() const { return m_A; }
const Matrixd<States, States>& A() const { return m_A; }
/**
* Returns an element of the system matrix A.
@@ -91,7 +95,7 @@ class LinearSystem {
/**
* Returns the input matrix B.
*/
const Eigen::Matrix<double, States, Inputs>& B() const { return m_B; }
const Matrixd<States, Inputs>& B() const { return m_B; }
/**
* Returns an element of the input matrix B.
@@ -104,7 +108,7 @@ class LinearSystem {
/**
* Returns the output matrix C.
*/
const Eigen::Matrix<double, Outputs, States>& C() const { return m_C; }
const Matrixd<Outputs, States>& C() const { return m_C; }
/**
* Returns an element of the output matrix C.
@@ -117,7 +121,7 @@ class LinearSystem {
/**
* Returns the feedthrough matrix D.
*/
const Eigen::Matrix<double, Outputs, Inputs>& D() const { return m_D; }
const Matrixd<Outputs, Inputs>& D() const { return m_D; }
/**
* Returns an element of the feedthrough matrix D.
@@ -137,11 +141,10 @@ class LinearSystem {
* @param clampedU The control input.
* @param dt Timestep for model update.
*/
Eigen::Vector<double, States> CalculateX(
const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, Inputs>& clampedU, units::second_t dt) const {
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, Inputs> discB;
StateVector CalculateX(const StateVector& x, const InputVector& clampedU,
units::second_t dt) const {
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(m_A, m_B, dt, &discA, &discB);
return discA * x + discB * clampedU;
@@ -156,9 +159,8 @@ class LinearSystem {
* @param x The current state.
* @param clampedU The control input.
*/
Eigen::Vector<double, Outputs> CalculateY(
const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, Inputs>& clampedU) const {
OutputVector CalculateY(const StateVector& x,
const InputVector& clampedU) const {
return m_C * x + m_D * clampedU;
}
@@ -166,22 +168,22 @@ class LinearSystem {
/**
* Continuous system matrix.
*/
Eigen::Matrix<double, States, States> m_A;
Matrixd<States, States> m_A;
/**
* Continuous input matrix.
*/
Eigen::Matrix<double, States, Inputs> m_B;
Matrixd<States, Inputs> m_B;
/**
* Output matrix.
*/
Eigen::Matrix<double, Outputs, States> m_C;
Matrixd<Outputs, States> m_C;
/**
* Feedthrough matrix.
*/
Eigen::Matrix<double, Outputs, Inputs> m_D;
Matrixd<Outputs, Inputs> m_D;
};
} // namespace frc

62
wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
View File

@@ -4,7 +4,7 @@
#pragma once
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/controller/LinearQuadraticRegulator.h"
#include "frc/estimator/KalmanFilter.h"
@@ -35,6 +35,10 @@ namespace frc {
template <int States, int Inputs, int Outputs>
class LinearSystemLoop {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
using OutputVector = Vectord<Outputs>;
/**
* Constructs a state-space loop with the given plant, controller, and
* observer. By default, the initial reference is all zeros. Users should
@@ -53,7 +57,7 @@ class LinearSystemLoop {
units::volt_t maxVoltage, units::second_t dt)
: LinearSystemLoop(
plant, controller, observer,
[=](const Eigen::Vector<double, Inputs>& u) {
[=](const InputVector& u) {
return frc::DesaturateInputVector<Inputs>(u, maxVoltage.value());
},
dt) {}
@@ -73,9 +77,7 @@ class LinearSystemLoop {
LinearSystemLoop(LinearSystem<States, Inputs, Outputs>& plant,
LinearQuadraticRegulator<States, Inputs>& controller,
KalmanFilter<States, Inputs, Outputs>& observer,
std::function<Eigen::Vector<double, Inputs>(
const Eigen::Vector<double, Inputs>&)>
clampFunction,
std::function<InputVector(const InputVector&)> clampFunction,
units::second_t dt)
: LinearSystemLoop(
controller,
@@ -97,11 +99,10 @@ class LinearSystemLoop {
LinearQuadraticRegulator<States, Inputs>& controller,
const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
KalmanFilter<States, Inputs, Outputs>& observer, units::volt_t maxVoltage)
: LinearSystemLoop(controller, feedforward, observer,
[=](const Eigen::Vector<double, Inputs>& u) {
return frc::DesaturateInputVector<Inputs>(
u, maxVoltage.value());
}) {}
: LinearSystemLoop(
controller, feedforward, observer, [=](const InputVector& u) {
return frc::DesaturateInputVector<Inputs>(u, maxVoltage.value());
}) {}
/**
* Constructs a state-space loop with the given controller, feedforward,
@@ -117,9 +118,7 @@ class LinearSystemLoop {
LinearQuadraticRegulator<States, Inputs>& controller,
const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
KalmanFilter<States, Inputs, Outputs>& observer,
std::function<
Eigen::Vector<double, Inputs>(const Eigen::Vector<double, Inputs>&)>
clampFunction)
std::function<InputVector(const InputVector&)> clampFunction)
: m_controller(&controller),
m_feedforward(feedforward),
m_observer(&observer),
@@ -134,9 +133,7 @@ class LinearSystemLoop {
/**
* Returns the observer's state estimate x-hat.
*/
const Eigen::Vector<double, States>& Xhat() const {
return m_observer->Xhat();
}
const StateVector& Xhat() const { return m_observer->Xhat(); }
/**
* Returns an element of the observer's state estimate x-hat.
@@ -148,7 +145,7 @@ class LinearSystemLoop {
/**
* Returns the controller's next reference r.
*/
const Eigen::Vector<double, States>& NextR() const { return m_nextR; }
const StateVector& NextR() const { return m_nextR; }
/**
* Returns an element of the controller's next reference r.
@@ -160,7 +157,7 @@ class LinearSystemLoop {
/**
* Returns the controller's calculated control input u.
*/
Eigen::Vector<double, Inputs> U() const {
InputVector U() const {
return ClampInput(m_controller->U() + m_feedforward.Uff());
}
@@ -176,9 +173,7 @@ class LinearSystemLoop {
*
* @param xHat The initial state estimate x-hat.
*/
void SetXhat(const Eigen::Vector<double, States>& xHat) {
m_observer->SetXhat(xHat);
}
void SetXhat(const StateVector& xHat) { m_observer->SetXhat(xHat); }
/**
* Set an element of the initial state estimate x-hat.
@@ -193,7 +188,7 @@ class LinearSystemLoop {
*
* @param nextR Next reference.
*/
void SetNextR(const Eigen::Vector<double, States>& nextR) { m_nextR = nextR; }
void SetNextR(const StateVector& nextR) { m_nextR = nextR; }
/**
* Return the controller used internally.
@@ -225,7 +220,7 @@ class LinearSystemLoop {
*
* @param initialState The initial state.
*/
void Reset(const Eigen::Vector<double, States>& initialState) {
void Reset(const StateVector& initialState) {
m_nextR.setZero();
m_controller->Reset();
m_feedforward.Reset(initialState);
@@ -235,18 +230,14 @@ class LinearSystemLoop {
/**
* Returns difference between reference r and current state x-hat.
*/
Eigen::Vector<double, States> Error() const {
return m_controller->R() - m_observer->Xhat();
}
StateVector 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::Vector<double, Outputs>& y) {
m_observer->Correct(U(), y);
}
void Correct(const OutputVector& y) { m_observer->Correct(U(), y); }
/**
* Sets new controller output, projects model forward, and runs observer
@@ -258,7 +249,7 @@ class LinearSystemLoop {
* @param dt Timestep for model update.
*/
void Predict(units::second_t dt) {
Eigen::Vector<double, Inputs> u =
InputVector u =
ClampInput(m_controller->Calculate(m_observer->Xhat(), m_nextR) +
m_feedforward.Calculate(m_nextR));
m_observer->Predict(u, dt);
@@ -270,10 +261,7 @@ class LinearSystemLoop {
* @param u Input vector to clamp.
* @return Clamped input vector.
*/
Eigen::Vector<double, Inputs> ClampInput(
const Eigen::Vector<double, Inputs>& u) const {
return m_clampFunc(u);
}
InputVector ClampInput(const InputVector& u) const { return m_clampFunc(u); }
protected:
LinearQuadraticRegulator<States, Inputs>* m_controller;
@@ -283,12 +271,10 @@ class LinearSystemLoop {
/**
* Clamping function.
*/
std::function<Eigen::Vector<double, Inputs>(
const Eigen::Vector<double, Inputs>&)>
m_clampFunc;
std::function<InputVector(const InputVector&)> m_clampFunc;
// Reference to go to in the next cycle (used by feedforward controller).
Eigen::Vector<double, States> m_nextR;
StateVector m_nextR;
// These are accessible from non-templated subclasses.
static constexpr int kStates = States;

26
wpimath/src/main/native/include/frc/system/NumericalJacobian.h
View File

@@ -4,7 +4,7 @@
#pragma once
#include "Eigen/Core"
#include "frc/EigenCore.h"
namespace frc {
@@ -17,16 +17,16 @@ namespace frc {
* @param x Vector argument.
*/
template <int Rows, int Cols, typename F>
auto NumericalJacobian(F&& f, const Eigen::Vector<double, Cols>& x) {
auto NumericalJacobian(F&& f, const Vectord<Cols>& x) {
constexpr double kEpsilon = 1e-5;
Eigen::Matrix<double, Rows, Cols> result;
Matrixd<Rows, Cols> result;
result.setZero();
// It's more expensive, but +- epsilon will be more accurate
for (int i = 0; i < Cols; ++i) {
Eigen::Vector<double, Cols> dX_plus = x;
Vectord<Cols> dX_plus = x;
dX_plus(i) += kEpsilon;
Eigen::Vector<double, Cols> dX_minus = x;
Vectord<Cols> dX_minus = x;
dX_minus(i) -= kEpsilon;
result.col(i) = (f(dX_plus) - f(dX_minus)) / (kEpsilon * 2.0);
}
@@ -48,12 +48,10 @@ auto NumericalJacobian(F&& f, const Eigen::Vector<double, Cols>& x) {
* @param args Remaining arguments to f(x, u, ...).
*/
template <int Rows, int States, int Inputs, typename F, typename... Args>
auto NumericalJacobianX(F&& f, const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, Inputs>& u,
Args&&... args) {
auto NumericalJacobianX(F&& f, const Vectord<States>& x,
const Vectord<Inputs>& u, Args&&... args) {
return NumericalJacobian<Rows, States>(
[&](const Eigen::Vector<double, States>& x) { return f(x, u, args...); },
x);
[&](const Vectord<States>& x) { return f(x, u, args...); }, x);
}
/**
@@ -70,12 +68,10 @@ auto NumericalJacobianX(F&& f, const Eigen::Vector<double, States>& x,
* @param args Remaining arguments to f(x, u, ...).
*/
template <int Rows, int States, int Inputs, typename F, typename... Args>
auto NumericalJacobianU(F&& f, const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, Inputs>& u,
Args&&... args) {
auto NumericalJacobianU(F&& f, const Vectord<States>& x,
const Vectord<Inputs>& u, Args&&... args) {
return NumericalJacobian<Rows, Inputs>(
[&](const Eigen::Vector<double, Inputs>& u) { return f(x, u, args...); },
u);
[&](const Vectord<Inputs>& u) { return f(x, u, args...); }, u);
}
} // namespace frc

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

@@ -56,15 +56,13 @@ class WPILIB_DLLEXPORT LinearSystemId {
throw std::domain_error("G must be greater than zero.");
}
Eigen::Matrix<double, 2, 2> A{
{0.0, 1.0},
{0.0, (-std::pow(G, 2) * motor.Kt /
(motor.R * units::math::pow<2>(r) * m * motor.Kv))
.value()}};
Eigen::Matrix<double, 2, 1> B{0.0,
(G * motor.Kt / (motor.R * r * m)).value()};
Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
Eigen::Matrix<double, 1, 1> D{0.0};
Matrixd<2, 2> A{{0.0, 1.0},
{0.0, (-std::pow(G, 2) * motor.Kt /
(motor.R * units::math::pow<2>(r) * m * motor.Kv))
.value()}};
Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * r * m)).value()};
Matrixd<1, 2> C{1.0, 0.0};
Matrixd<1, 1> D{0.0};
return LinearSystem<2, 1, 1>(A, B, C, D);
}
@@ -90,12 +88,12 @@ class WPILIB_DLLEXPORT LinearSystemId {
throw std::domain_error("G must be greater than zero.");
}
Eigen::Matrix<double, 2, 2> A{
Matrixd<2, 2> A{
{0.0, 1.0},
{0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
Eigen::Matrix<double, 2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
Eigen::Matrix<double, 1, 1> D{0.0};
Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
Matrixd<1, 2> C{1.0, 0.0};
Matrixd<1, 1> D{0.0};
return LinearSystem<2, 1, 1>(A, B, C, D);
}
@@ -134,10 +132,10 @@ class WPILIB_DLLEXPORT LinearSystemId {
throw std::domain_error("Ka must be greater than zero.");
}
Eigen::Matrix<double, 1, 1> A{-kV.value() / kA.value()};
Eigen::Matrix<double, 1, 1> B{1.0 / kA.value()};
Eigen::Matrix<double, 1, 1> C{1.0};
Eigen::Matrix<double, 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);
}
@@ -176,10 +174,10 @@ class WPILIB_DLLEXPORT LinearSystemId {
throw std::domain_error("Ka must be greater than zero.");
}
Eigen::Matrix<double, 2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
Eigen::Matrix<double, 2, 1> B{0.0, 1.0 / kA.value()};
Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
Eigen::Matrix<double, 1, 1> D{0.0};
Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
Matrixd<2, 1> B{0.0, 1.0 / kA.value()};
Matrixd<1, 2> C{1.0, 0.0};
Matrixd<1, 1> D{0.0};
return LinearSystem<2, 1, 1>(A, B, C, D);
}
@@ -224,12 +222,10 @@ class WPILIB_DLLEXPORT LinearSystemId {
double B1 = 1.0 / kAlinear.value() + 1.0 / kAangular.value();
double B2 = 1.0 / kAlinear.value() - 1.0 / kAangular.value();
Eigen::Matrix<double, 2, 2> A =
0.5 * Eigen::Matrix<double, 2, 2>{{A1, A2}, {A2, A1}};
Eigen::Matrix<double, 2, 2> B =
0.5 * Eigen::Matrix<double, 2, 2>{{B1, B2}, {B2, B1}};
Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Eigen::Matrix<double, 2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
Matrixd<2, 2> A = 0.5 * Matrixd<2, 2>{{A1, A2}, {A2, A1}};
Matrixd<2, 2> B = 0.5 * Matrixd<2, 2>{{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);
}
@@ -311,11 +307,11 @@ class WPILIB_DLLEXPORT LinearSystemId {
throw std::domain_error("G must be greater than zero.");
}
Eigen::Matrix<double, 1, 1> A{
Matrixd<1, 1> A{
(-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()};
Eigen::Matrix<double, 1, 1> B{(G * motor.Kt / (motor.R * J)).value()};
Eigen::Matrix<double, 1, 1> C{1.0};
Eigen::Matrix<double, 1, 1> D{0.0};
Matrixd<1, 1> B{(G * 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);
}
@@ -342,12 +338,12 @@ class WPILIB_DLLEXPORT LinearSystemId {
throw std::domain_error("G must be greater than zero.");
}
Eigen::Matrix<double, 2, 2> A{
Matrixd<2, 2> A{
{0.0, 1.0},
{0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
Eigen::Matrix<double, 2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Eigen::Matrix<double, 2, 1> D{0.0, 0.0};
Matrixd<2, 1> B{0.0, (G * 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);
}
@@ -391,18 +387,16 @@ class WPILIB_DLLEXPORT LinearSystemId {
(motor.Kv * motor.R * units::math::pow<2>(r));
auto C2 = G * motor.Kt / (motor.R * r);
Eigen::Matrix<double, 2, 2> A{
{((1 / m + units::math::pow<2>(rb) / J) * C1).value(),
((1 / m - units::math::pow<2>(rb) / J) * C1).value()},
{((1 / m - units::math::pow<2>(rb) / J) * C1).value(),
((1 / m + units::math::pow<2>(rb) / J) * C1).value()}};
Eigen::Matrix<double, 2, 2> B{
{((1 / m + units::math::pow<2>(rb) / J) * C2).value(),
((1 / m - units::math::pow<2>(rb) / J) * C2).value()},
{((1 / m - units::math::pow<2>(rb) / J) * C2).value(),
((1 / m + units::math::pow<2>(rb) / J) * C2).value()}};
Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Eigen::Matrix<double, 2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
Matrixd<2, 2> A{{((1 / m + units::math::pow<2>(rb) / J) * C1).value(),
((1 / m - units::math::pow<2>(rb) / J) * C1).value()},
{((1 / m - units::math::pow<2>(rb) / J) * C1).value(),
((1 / m + units::math::pow<2>(rb) / J) * C1).value()}};
Matrixd<2, 2> B{{((1 / m + units::math::pow<2>(rb) / J) * C2).value(),
((1 / m - units::math::pow<2>(rb) / J) * C2).value()},
{((1 / m - units::math::pow<2>(rb) / J) * C2).value(),
((1 / m + 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);
}