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

58
wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
View File

@@ -7,8 +7,8 @@
#include <array>
#include <functional>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/system/NumericalJacobian.h"
#include "units/time.h"
@@ -39,6 +39,9 @@ namespace frc {
template <int States, int Inputs>
class ControlAffinePlantInversionFeedforward {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
/**
* Constructs a feedforward with given model dynamics as a function
* of state and input.
@@ -50,15 +53,11 @@ class ControlAffinePlantInversionFeedforward {
* @param dt The timestep between calls of calculate().
*/
ControlAffinePlantInversionFeedforward(
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
f,
std::function<StateVector(const StateVector&, const InputVector&)> f,
units::second_t dt)
: m_dt(dt), m_f(f) {
m_B = NumericalJacobianU<States, States, Inputs>(
f, Eigen::Vector<double, States>::Zero(),
Eigen::Vector<double, Inputs>::Zero());
m_B = NumericalJacobianU<States, States, Inputs>(f, StateVector::Zero(),
InputVector::Zero());
Reset();
}
@@ -73,14 +72,12 @@ class ControlAffinePlantInversionFeedforward {
* @param dt The timestep between calls of calculate().
*/
ControlAffinePlantInversionFeedforward(
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&)>
f,
const Eigen::Matrix<double, States, Inputs>& B, units::second_t dt)
std::function<StateVector(const StateVector&)> f,
const Matrixd<States, Inputs>& B, units::second_t dt)
: m_B(B), m_dt(dt) {
m_f = [=](const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, Inputs>& u)
-> Eigen::Vector<double, States> { return f(x); };
m_f = [=](const StateVector& x, const InputVector& u) -> StateVector {
return f(x);
};
Reset();
}
@@ -95,7 +92,7 @@ class ControlAffinePlantInversionFeedforward {
*
* @return The calculated feedforward.
*/
const Eigen::Vector<double, Inputs>& Uff() const { return m_uff; }
const InputVector& Uff() const { return m_uff; }
/**
* Returns an element of the previously calculated feedforward.
@@ -111,7 +108,7 @@ class ControlAffinePlantInversionFeedforward {
*
* @return The current reference vector.
*/
const Eigen::Vector<double, States>& R() const { return m_r; }
const StateVector& R() const { return m_r; }
/**
* Returns an element of the reference vector r.
@@ -127,7 +124,7 @@ class ControlAffinePlantInversionFeedforward {
*
* @param initialState The initial state vector.
*/
void Reset(const Eigen::Vector<double, States>& initialState) {
void Reset(const StateVector& initialState) {
m_r = initialState;
m_uff.setZero();
}
@@ -146,15 +143,14 @@ class ControlAffinePlantInversionFeedforward {
* reference.
*
* If this method is used the initial state of the system is the one set using
* Reset(const Eigen::Vector<double, States>&). If the initial state is not
* Reset(const StateVector&). 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::Vector<double, Inputs> Calculate(
const Eigen::Vector<double, States>& nextR) {
InputVector Calculate(const StateVector& nextR) {
return Calculate(m_r, nextR);
}
@@ -166,36 +162,30 @@ class ControlAffinePlantInversionFeedforward {
*
* @return The calculated feedforward.
*/
Eigen::Vector<double, Inputs> Calculate(
const Eigen::Vector<double, States>& r,
const Eigen::Vector<double, States>& nextR) {
Eigen::Vector<double, States> rDot = (nextR - r) / m_dt.value();
InputVector Calculate(const StateVector& r, const StateVector& nextR) {
StateVector rDot = (nextR - r) / m_dt.value();
m_uff = m_B.householderQr().solve(
rDot - m_f(r, Eigen::Vector<double, Inputs>::Zero()));
m_uff = m_B.householderQr().solve(rDot - m_f(r, InputVector::Zero()));
m_r = nextR;
return m_uff;
}
private:
Eigen::Matrix<double, States, Inputs> m_B;
Matrixd<States, Inputs> m_B;
units::second_t m_dt;
/**
* The model dynamics.
*/
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
m_f;
std::function<StateVector(const StateVector&, const InputVector&)> m_f;
// Current reference
Eigen::Vector<double, States> m_r;
StateVector m_r;
// Computed feedforward
Eigen::Vector<double, Inputs> m_uff;
InputVector m_uff;
};
} // namespace frc

25
wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h
View File

@@ -6,8 +6,8 @@
#include <frc/system/LinearSystem.h>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "units/time.h"
namespace frc {
@@ -27,6 +27,9 @@ namespace frc {
template <int States, int Inputs>
class ImplicitModelFollower {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
/**
* Constructs a controller with the given coefficients and plant.
*
@@ -47,10 +50,10 @@ class ImplicitModelFollower {
* @param Aref Continuous system matrix whose dynamics should be followed.
* @param Bref Continuous input matrix whose dynamics should be followed.
*/
ImplicitModelFollower(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Aref,
const Eigen::Matrix<double, States, Inputs>& Bref) {
ImplicitModelFollower(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Aref,
const Matrixd<States, Inputs>& Bref) {
// Find u_imf that makes real model match reference model.
//
// dx/dt = Ax + Bu_imf
@@ -79,7 +82,7 @@ class ImplicitModelFollower {
*
* @return The control input.
*/
const Eigen::Vector<double, Inputs>& U() const { return m_u; }
const InputVector& U() const { return m_u; }
/**
* Returns an element of the control input vector u.
@@ -101,22 +104,20 @@ class ImplicitModelFollower {
* @param x The current state x.
* @param u The current input for the original model.
*/
Eigen::Vector<double, Inputs> Calculate(
const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, Inputs>& u) {
InputVector Calculate(const StateVector& x, const InputVector& u) {
m_u = m_A * x + m_B * u;
return m_u;
}
private:
// Computed controller output
Eigen::Vector<double, Inputs> m_u;
InputVector m_u;
// State space conversion gain
Eigen::Matrix<double, Inputs, States> m_A;
Matrixd<Inputs, States> m_A;
// Input space conversion gain
Eigen::Matrix<double, Inputs, Inputs> m_B;
Matrixd<Inputs, Inputs> m_B;
};
} // namespace frc

34
wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
View File

@@ -7,8 +7,8 @@
#include <array>
#include <functional>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/system/Discretization.h"
#include "frc/system/LinearSystem.h"
#include "units/time.h"
@@ -31,6 +31,9 @@ namespace frc {
template <int States, int Inputs>
class LinearPlantInversionFeedforward {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
/**
* Constructs a feedforward with the given plant.
*
@@ -50,9 +53,9 @@ class LinearPlantInversionFeedforward {
* @param B Continuous input matrix of the plant being controlled.
* @param dt Discretization timestep.
*/
LinearPlantInversionFeedforward(
const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B, units::second_t dt)
LinearPlantInversionFeedforward(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
units::second_t dt)
: m_dt(dt) {
DiscretizeAB<States, Inputs>(A, B, dt, &m_A, &m_B);
Reset();
@@ -63,7 +66,7 @@ class LinearPlantInversionFeedforward {
*
* @return The calculated feedforward.
*/
const Eigen::Vector<double, Inputs>& Uff() const { return m_uff; }
const InputVector& Uff() const { return m_uff; }
/**
* Returns an element of the previously calculated feedforward.
@@ -79,7 +82,7 @@ class LinearPlantInversionFeedforward {
*
* @return The current reference vector.
*/
const Eigen::Vector<double, States>& R() const { return m_r; }
const StateVector& R() const { return m_r; }
/**
* Returns an element of the reference vector r.
@@ -95,7 +98,7 @@ class LinearPlantInversionFeedforward {
*
* @param initialState The initial state vector.
*/
void Reset(const Eigen::Vector<double, States>& initialState) {
void Reset(const StateVector& initialState) {
m_r = initialState;
m_uff.setZero();
}
@@ -114,15 +117,14 @@ class LinearPlantInversionFeedforward {
* reference.
*
* If this method is used the initial state of the system is the one set using
* Reset(const Eigen::Vector<double, States>&). If the initial state is not
* Reset(const StateVector&). 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::Vector<double, Inputs> Calculate(
const Eigen::Vector<double, States>& nextR) {
InputVector Calculate(const StateVector& nextR) {
return Calculate(m_r, nextR);
}
@@ -134,25 +136,23 @@ class LinearPlantInversionFeedforward {
*
* @return The calculated feedforward.
*/
Eigen::Vector<double, Inputs> Calculate(
const Eigen::Vector<double, States>& r,
const Eigen::Vector<double, States>& nextR) {
InputVector Calculate(const StateVector& r, const StateVector& 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;
Matrixd<States, States> m_A;
Matrixd<States, Inputs> m_B;
units::second_t m_dt;
// Current reference
Eigen::Vector<double, States> m_r;
StateVector m_r;
// Computed feedforward
Eigen::Vector<double, Inputs> m_uff;
InputVector m_uff;
};
} // namespace frc

151
wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
View File

@@ -12,9 +12,9 @@
#include <wpi/array.h>
#include "Eigen/Cholesky"
#include "Eigen/Core"
#include "Eigen/Eigenvalues"
#include "drake/math/discrete_algebraic_riccati_equation.h"
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/Discretization.h"
#include "frc/system/LinearSystem.h"
@@ -39,6 +39,12 @@ namespace detail {
template <int States, int Inputs>
class LinearQuadraticRegulatorImpl {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
using StateArray = wpi::array<double, States>;
using InputArray = wpi::array<double, Inputs>;
/**
* Constructs a controller with the given coefficients and plant.
*
@@ -50,8 +56,7 @@ class LinearQuadraticRegulatorImpl {
template <int Outputs>
LinearQuadraticRegulatorImpl(
const LinearSystem<States, Inputs, Outputs>& plant,
const wpi::array<double, States>& Qelems,
const wpi::array<double, Inputs>& Relems, units::second_t dt)
const StateArray& Qelems, const InputArray& Relems, units::second_t dt)
: LinearQuadraticRegulatorImpl(plant.A(), plant.B(), Qelems, Relems, dt) {
}
@@ -64,11 +69,10 @@ class LinearQuadraticRegulatorImpl {
* @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 wpi::array<double, States>& Qelems,
const wpi::array<double, Inputs>& Relems,
units::second_t dt)
LinearQuadraticRegulatorImpl(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const StateArray& Qelems,
const InputArray& Relems, units::second_t dt)
: LinearQuadraticRegulatorImpl(A, B, MakeCostMatrix(Qelems),
MakeCostMatrix(Relems), dt) {}
@@ -81,13 +85,13 @@ class LinearQuadraticRegulatorImpl {
* @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,
LinearQuadraticRegulatorImpl(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Q,
const Matrixd<Inputs, Inputs>& R,
units::second_t dt) {
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, Inputs> discB;
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
if (!IsStabilizable<States, Inputs>(discA, discB)) {
@@ -100,7 +104,7 @@ class LinearQuadraticRegulatorImpl {
throw std::invalid_argument(msg);
}
Eigen::Matrix<double, States, States> S =
Matrixd<States, States> S =
drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
// K = (BᵀSB + R)⁻¹BᵀSA
@@ -121,17 +125,17 @@ class LinearQuadraticRegulatorImpl {
* @param N The state-input cross-term 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,
const Eigen::Matrix<double, States, Inputs>& N,
LinearQuadraticRegulatorImpl(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Q,
const Matrixd<Inputs, Inputs>& R,
const Matrixd<States, Inputs>& N,
units::second_t dt) {
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, Inputs> discB;
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
Eigen::Matrix<double, States, States> S =
Matrixd<States, States> S =
drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
// K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
@@ -149,7 +153,7 @@ class LinearQuadraticRegulatorImpl {
/**
* Returns the controller matrix K.
*/
const Eigen::Matrix<double, Inputs, States>& K() const { return m_K; }
const Matrixd<Inputs, States>& K() const { return m_K; }
/**
* Returns an element of the controller matrix K.
@@ -164,7 +168,7 @@ class LinearQuadraticRegulatorImpl {
*
* @return The reference vector.
*/
const Eigen::Vector<double, States>& R() const { return m_r; }
const StateVector& R() const { return m_r; }
/**
* Returns an element of the reference vector r.
@@ -180,7 +184,7 @@ class LinearQuadraticRegulatorImpl {
*
* @return The control input.
*/
const Eigen::Vector<double, Inputs>& U() const { return m_u; }
const InputVector& U() const { return m_u; }
/**
* Returns an element of the control input vector u.
@@ -204,8 +208,7 @@ class LinearQuadraticRegulatorImpl {
*
* @param x The current state x.
*/
Eigen::Vector<double, Inputs> Calculate(
const Eigen::Vector<double, States>& x) {
InputVector Calculate(const StateVector& x) {
m_u = m_K * (m_r - x);
return m_u;
}
@@ -216,9 +219,7 @@ class LinearQuadraticRegulatorImpl {
* @param x The current state x.
* @param nextR The next reference vector r.
*/
Eigen::Vector<double, Inputs> Calculate(
const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, States>& nextR) {
InputVector Calculate(const StateVector& x, const StateVector& nextR) {
m_r = nextR;
return Calculate(x);
}
@@ -242,8 +243,8 @@ class LinearQuadraticRegulatorImpl {
template <int Outputs>
void LatencyCompensate(const LinearSystem<States, Inputs, Outputs>& plant,
units::second_t dt, units::second_t inputDelay) {
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, Inputs> discB;
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(plant.A(), plant.B(), dt, &discA, &discB);
m_K = m_K * (discA - discB * m_K).pow(inputDelay / dt);
@@ -251,13 +252,13 @@ class LinearQuadraticRegulatorImpl {
private:
// Current reference
Eigen::Vector<double, States> m_r;
StateVector m_r;
// Computed controller output
Eigen::Vector<double, Inputs> m_u;
InputVector m_u;
// Controller gain
Eigen::Matrix<double, Inputs, States> m_K;
Matrixd<Inputs, States> m_K;
};
} // namespace detail
@@ -291,8 +292,8 @@ class LinearQuadraticRegulator
* @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,
LinearQuadraticRegulator(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const wpi::array<double, States>& Qelems,
const wpi::array<double, Inputs>& Relems,
units::second_t dt)
@@ -308,11 +309,10 @@ class LinearQuadraticRegulator
* @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)
LinearQuadraticRegulator(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Q,
const Matrixd<Inputs, Inputs>& R, units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<States, Inputs>{A, B, Q, R, dt} {}
/**
@@ -325,12 +325,11 @@ class LinearQuadraticRegulator
* @param N The state-input cross-term 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,
const Eigen::Matrix<double, States, Inputs>& N,
units::second_t dt)
LinearQuadraticRegulator(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Q,
const Matrixd<Inputs, Inputs>& R,
const Matrixd<States, Inputs>& N, units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<States, Inputs>{A, B, Q,
R, N, dt} {}
@@ -351,24 +350,18 @@ class WPILIB_DLLEXPORT LinearQuadraticRegulator<1, 1>
units::second_t dt)
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
const Eigen::Matrix<double, 1, 1>& B,
LinearQuadraticRegulator(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B,
const wpi::array<double, 1>& Qelems,
const wpi::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,
LinearQuadraticRegulator(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B,
const Matrixd<1, 1>& Q, const Matrixd<1, 1>& R,
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,
const Eigen::Matrix<double, 1, 1>& N,
units::second_t dt);
LinearQuadraticRegulator(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B,
const Matrixd<1, 1>& Q, const Matrixd<1, 1>& R,
const Matrixd<1, 1>& N, units::second_t dt);
LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
@@ -387,24 +380,18 @@ class WPILIB_DLLEXPORT LinearQuadraticRegulator<2, 1>
units::second_t dt)
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B,
LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B,
const wpi::array<double, 2>& Qelems,
const wpi::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,
LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B,
const Matrixd<2, 2>& Q, const Matrixd<1, 1>& R,
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,
const Eigen::Matrix<double, 2, 1>& N,
units::second_t dt);
LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B,
const Matrixd<2, 2>& Q, const Matrixd<1, 1>& R,
const Matrixd<2, 1>& N, units::second_t dt);
LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
@@ -423,24 +410,18 @@ class WPILIB_DLLEXPORT LinearQuadraticRegulator<2, 2>
units::second_t dt)
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 2>& B,
LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 2>& B,
const wpi::array<double, 2>& Qelems,
const wpi::array<double, 2>& Relems,
units::second_t dt);
LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 2, 2>& R,
LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 2>& B,
const Matrixd<2, 2>& Q, const Matrixd<2, 2>& R,
units::second_t dt);
LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 2, 2>& R,
const Eigen::Matrix<double, 2, 2>& N,
units::second_t dt);
LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 2>& B,
const Matrixd<2, 2>& Q, const Matrixd<2, 2>& R,
const Matrixd<2, 2>& N, units::second_t dt);
LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;

6
wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
View File

@@ -6,7 +6,7 @@
#include <wpi/MathExtras.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/system/plant/LinearSystemId.h"
#include "units/time.h"
@@ -70,8 +70,8 @@ class SimpleMotorFeedforward {
auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
Eigen::Vector<double, 1> r{currentVelocity.value()};
Eigen::Vector<double, 1> nextR{nextVelocity.value()};
Vectord<1> r{currentVelocity.value()};
Vectord<1> nextR{nextVelocity.value()};
return kS * wpi::sgn(currentVelocity.value()) +
units::volt_t{feedforward.Calculate(r, nextR)(0)};