[wpimath] Clean up Eigen usage

* Replace Matrix<> with Vector<> where vectors are explicitly intended. I found these via `rg "Eigen::Matrix<double, \w+, 1>"`. * Pass all Eigen matrices by const reference. I found these via `rg "\(Eigen"` on main (the initializer list constructors make more false positives). * Replace MakeMatrix() and operator<< usage with initializer list constructors. I found these via `rg MakeMatrix` and `rg "<<"` respectively. * Deprecate MakeMatrix()
2026-07-02 02:51:42 +00:00 · 2021-08-19 00:23:48 -07:00
parent 72716f51ce
commit 9359431bad
63 changed files with 821 additions and 955 deletions
--- a/wpimath/src/main/native/include/frc/system/LinearSystem.h
+++ b/wpimath/src/main/native/include/frc/system/LinearSystem.h
@@ -110,10 +110,9 @@ class LinearSystem {
   * @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::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;
    DiscretizeAB<States, Inputs>(m_A, m_B, dt, &discA, &discB);
@@ -130,9 +129,9 @@ class LinearSystem {
   * @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 {
+  Eigen::Vector<double, Outputs> CalculateY(
+      const Eigen::Vector<double, States>& x,
+      const Eigen::Vector<double, Inputs>& clampedU) const {
    return m_C * x + m_D * clampedU;
  }

--- a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
+++ b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
@@ -48,7 +48,7 @@ class LinearSystemLoop {
                   units::volt_t maxVoltage, units::second_t dt)
      : LinearSystemLoop(
            plant, controller, observer,
-            [=](Eigen::Matrix<double, Inputs, 1> u) {
+            [=](const Eigen::Vector<double, Inputs>& u) {
              return frc::NormalizeInputVector<Inputs>(
                  u, maxVoltage.template to<double>());
            },
@@ -69,8 +69,8 @@ class LinearSystemLoop {
  LinearSystemLoop(LinearSystem<States, Inputs, Outputs>& plant,
                   LinearQuadraticRegulator<States, Inputs>& controller,
                   KalmanFilter<States, Inputs, Outputs>& observer,
-                   std::function<Eigen::Matrix<double, Inputs, 1>(
-                       const Eigen::Matrix<double, Inputs, 1>&)>
+                   std::function<Eigen::Vector<double, Inputs>(
+                       const Eigen::Vector<double, Inputs>&)>
                       clampFunction,
                   units::second_t dt)
      : LinearSystemLoop(
@@ -94,7 +94,7 @@ class LinearSystemLoop {
      const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
      KalmanFilter<States, Inputs, Outputs>& observer, units::volt_t maxVoltage)
      : LinearSystemLoop(controller, feedforward, observer,
-                         [=](Eigen::Matrix<double, Inputs, 1> u) {
+                         [=](const Eigen::Vector<double, Inputs>& u) {
                           return frc::NormalizeInputVector<Inputs>(
                               u, maxVoltage.template to<double>());
                         }) {}
@@ -113,8 +113,8 @@ class LinearSystemLoop {
      LinearQuadraticRegulator<States, Inputs>& controller,
      const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
      KalmanFilter<States, Inputs, Outputs>& observer,
-      std::function<Eigen::Matrix<double, Inputs, 1>(
-          const Eigen::Matrix<double, Inputs, 1>&)>
+      std::function<
+          Eigen::Vector<double, Inputs>(const Eigen::Vector<double, Inputs>&)>
          clampFunction)
      : m_controller(&controller),
        m_feedforward(feedforward),
@@ -130,7 +130,7 @@ class LinearSystemLoop {
  /**
   * Returns the observer's state estimate x-hat.
   */
-  const Eigen::Matrix<double, States, 1>& Xhat() const {
+  const Eigen::Vector<double, States>& Xhat() const {
    return m_observer->Xhat();
  }

@@ -144,7 +144,7 @@ class LinearSystemLoop {
  /**
   * Returns the controller's next reference r.
   */
-  const Eigen::Matrix<double, States, 1>& NextR() const { return m_nextR; }
+  const Eigen::Vector<double, States>& NextR() const { return m_nextR; }

  /**
   * Returns an element of the controller's next reference r.
@@ -156,7 +156,7 @@ class LinearSystemLoop {
  /**
   * Returns the controller's calculated control input u.
   */
-  Eigen::Matrix<double, Inputs, 1> U() const {
+  Eigen::Vector<double, Inputs> U() const {
    return ClampInput(m_controller->U() + m_feedforward.Uff());
  }

@@ -172,7 +172,7 @@ class LinearSystemLoop {
   *
   * @param xHat The initial state estimate x-hat.
   */
-  void SetXhat(const Eigen::Matrix<double, States, 1>& xHat) {
+  void SetXhat(const Eigen::Vector<double, States>& xHat) {
    m_observer->SetXhat(xHat);
  }

@@ -189,9 +189,7 @@ class LinearSystemLoop {
   *
   * @param nextR Next reference.
   */
-  void SetNextR(const Eigen::Matrix<double, States, 1>& nextR) {
-    m_nextR = nextR;
-  }
+  void SetNextR(const Eigen::Vector<double, States>& nextR) { m_nextR = nextR; }

  /**
   * Return the controller used internally.
@@ -223,7 +221,7 @@ class LinearSystemLoop {
   *
   * @param initialState The initial state.
   */
-  void Reset(Eigen::Matrix<double, States, 1> initialState) {
+  void Reset(const Eigen::Vector<double, States>& initialState) {
    m_nextR.setZero();
    m_controller->Reset();
    m_feedforward.Reset(initialState);
@@ -233,7 +231,7 @@ class LinearSystemLoop {
  /**
   * Returns difference between reference r and current state x-hat.
   */
-  const Eigen::Matrix<double, States, 1> Error() const {
+  Eigen::Vector<double, States> Error() const {
    return m_controller->R() - m_observer->Xhat();
  }

@@ -242,7 +240,7 @@ class LinearSystemLoop {
   *
   * @param y Measurement vector.
   */
-  void Correct(const Eigen::Matrix<double, Outputs, 1>& y) {
+  void Correct(const Eigen::Vector<double, Outputs>& y) {
    m_observer->Correct(U(), y);
  }

@@ -256,7 +254,7 @@ class LinearSystemLoop {
   * @param dt Timestep for model update.
   */
  void Predict(units::second_t dt) {
-    Eigen::Matrix<double, Inputs, 1> u =
+    Eigen::Vector<double, Inputs> u =
        ClampInput(m_controller->Calculate(m_observer->Xhat(), m_nextR) +
                   m_feedforward.Calculate(m_nextR));
    m_observer->Predict(u, dt);
@@ -268,8 +266,8 @@ class LinearSystemLoop {
   * @param u Input vector to clamp.
   * @return Clamped input vector.
   */
-  Eigen::Matrix<double, Inputs, 1> ClampInput(
-      const Eigen::Matrix<double, Inputs, 1>& u) const {
+  Eigen::Vector<double, Inputs> ClampInput(
+      const Eigen::Vector<double, Inputs>& u) const {
    return m_clampFunc(u);
  }

@@ -281,12 +279,12 @@ class LinearSystemLoop {
  /**
   * Clamping function.
   */
-  std::function<Eigen::Matrix<double, Inputs, 1>(
-      const Eigen::Matrix<double, Inputs, 1>&)>
+  std::function<Eigen::Vector<double, Inputs>(
+      const Eigen::Vector<double, Inputs>&)>
      m_clampFunc;

  // Reference to go to in the next cycle (used by feedforward controller).
-  Eigen::Matrix<double, States, 1> m_nextR;
+  Eigen::Vector<double, States> m_nextR;

  // These are accessible from non-templated subclasses.
  static constexpr int kStates = States;
--- a/wpimath/src/main/native/include/frc/system/NumericalJacobian.h
+++ b/wpimath/src/main/native/include/frc/system/NumericalJacobian.h
@@ -17,16 +17,16 @@ namespace frc {
 * @param x     Vector argument.
 */
 template <int Rows, int Cols, typename F>
-auto NumericalJacobian(F&& f, const Eigen::Matrix<double, Cols, 1>& x) {
+auto NumericalJacobian(F&& f, const Eigen::Vector<double, Cols>& 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;
+    Eigen::Vector<double, Cols> dX_plus = x;
    dX_plus(i) += kEpsilon;
-    Eigen::Matrix<double, Cols, 1> dX_minus = x;
+    Eigen::Vector<double, Cols> dX_minus = x;
    dX_minus(i) -= kEpsilon;
    result.col(i) = (f(dX_plus) - f(dX_minus)) / (kEpsilon * 2.0);
  }
@@ -47,11 +47,12 @@ auto NumericalJacobian(F&& f, const Eigen::Matrix<double, Cols, 1>& x) {
 * @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,
+auto NumericalJacobianX(F&& f, const Eigen::Vector<double, States>& x,
+                        const Eigen::Vector<double, Inputs>& u,
                        Args&&... args) {
  return NumericalJacobian<Rows, States>(
-      [&](Eigen::Matrix<double, States, 1> x) { return f(x, u, args...); }, x);
+      [&](const Eigen::Vector<double, States>& x) { return f(x, u, args...); },
+      x);
 }

 /**
@@ -67,11 +68,12 @@ auto NumericalJacobianX(F&& f, const Eigen::Matrix<double, States, 1>& x,
 * @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,
+auto NumericalJacobianU(F&& f, const Eigen::Vector<double, States>& x,
+                        const Eigen::Vector<double, Inputs>& u,
                        Args&&... args) {
  return NumericalJacobian<Rows, Inputs>(
-      [&](Eigen::Matrix<double, Inputs, 1> u) { return f(x, u, args...); }, u);
+      [&](const Eigen::Vector<double, Inputs>& u) { return f(x, u, args...); },
+      u);
 }

 }  // namespace frc
--- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
+++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
@@ -43,15 +43,15 @@ class WPILIB_DLLEXPORT LinearSystemId {
  static LinearSystem<2, 1, 1> ElevatorSystem(DCMotor motor,
                                              units::kilogram_t m,
                                              units::meter_t r, double G) {
-    Eigen::Matrix<double, 2, 2> 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>());
-    Eigen::Matrix<double, 2, 1> B = frc::MakeMatrix<2, 1>(
-        0.0, (G * motor.Kt / (motor.R * r * m)).to<double>());
-    Eigen::Matrix<double, 1, 2> C = frc::MakeMatrix<1, 2>(1.0, 0.0);
-    Eigen::Matrix<double, 1, 1> D = frc::MakeMatrix<1, 1>(0.0);
+    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))
+                  .to<double>()}};
+    Eigen::Matrix<double, 2, 1> B{
+        0.0, (G * motor.Kt / (motor.R * r * m)).to<double>()};
+    Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
+    Eigen::Matrix<double, 1, 1> D{0.0};

    return LinearSystem<2, 1, 1>(A, B, C, D);
  }
@@ -69,13 +69,14 @@ class WPILIB_DLLEXPORT LinearSystemId {
   */
  static LinearSystem<2, 1, 1> SingleJointedArmSystem(
      DCMotor motor, units::kilogram_square_meter_t J, double G) {
-    Eigen::Matrix<double, 2, 2> A = frc::MakeMatrix<2, 2>(
-        0.0, 1.0, 0.0,
-        (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to<double>());
-    Eigen::Matrix<double, 2, 1> B =
-        frc::MakeMatrix<2, 1>(0.0, (G * motor.Kt / (motor.R * J)).to<double>());
-    Eigen::Matrix<double, 1, 2> C = frc::MakeMatrix<1, 2>(1.0, 0.0);
-    Eigen::Matrix<double, 1, 1> D = frc::MakeMatrix<1, 1>(0.0);
+    Eigen::Matrix<double, 2, 2> A{
+        {0.0, 1.0},
+        {0.0,
+         (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to<double>()}};
+    Eigen::Matrix<double, 2, 1> B{0.0,
+                                  (G * motor.Kt / (motor.R * J)).to<double>()};
+    Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
+    Eigen::Matrix<double, 1, 1> D{0.0};

    return LinearSystem<2, 1, 1>(A, B, C, D);
  }
@@ -106,12 +107,11 @@ class WPILIB_DLLEXPORT LinearSystemId {
  static LinearSystem<1, 1, 1> IdentifyVelocitySystem(
      decltype(1_V / Velocity_t<Distance>(1)) kV,
      decltype(1_V / Acceleration_t<Distance>(1)) kA) {
-    Eigen::Matrix<double, 1, 1> A = frc::MakeMatrix<1, 1>(
-        -kV.template to<double>() / kA.template to<double>());
-    Eigen::Matrix<double, 1, 1> B =
-        frc::MakeMatrix<1, 1>(1.0 / kA.template to<double>());
-    Eigen::Matrix<double, 1, 1> C = frc::MakeMatrix<1, 1>(1.0);
-    Eigen::Matrix<double, 1, 1> D = frc::MakeMatrix<1, 1>(0.0);
+    Eigen::Matrix<double, 1, 1> A{-kV.template to<double>() /
+                                  kA.template to<double>()};
+    Eigen::Matrix<double, 1, 1> B{1.0 / kA.template to<double>()};
+    Eigen::Matrix<double, 1, 1> C{1.0};
+    Eigen::Matrix<double, 1, 1> D{0.0};

    return LinearSystem<1, 1, 1>(A, B, C, D);
  }
@@ -142,12 +142,12 @@ class WPILIB_DLLEXPORT LinearSystemId {
  static LinearSystem<2, 1, 1> IdentifyPositionSystem(
      decltype(1_V / Velocity_t<Distance>(1)) kV,
      decltype(1_V / Acceleration_t<Distance>(1)) kA) {
-    Eigen::Matrix<double, 2, 2> A = frc::MakeMatrix<2, 2>(
-        0.0, 1.0, 0.0, -kV.template to<double>() / kA.template to<double>());
-    Eigen::Matrix<double, 2, 1> B =
-        frc::MakeMatrix<2, 1>(0.0, 1.0 / kA.template to<double>());
-    Eigen::Matrix<double, 1, 2> C = frc::MakeMatrix<1, 2>(1.0, 0.0);
-    Eigen::Matrix<double, 1, 1> D = frc::MakeMatrix<1, 1>(0.0);
+    Eigen::Matrix<double, 2, 2> A{
+        {0.0, 1.0},
+        {0.0, -kV.template to<double>() / kA.template to<double>()}};
+    Eigen::Matrix<double, 2, 1> B{0.0, 1.0 / kA.template to<double>()};
+    Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
+    Eigen::Matrix<double, 1, 1> D{0.0};

    return LinearSystem<2, 1, 1>(A, B, C, D);
  }
@@ -177,10 +177,12 @@ class WPILIB_DLLEXPORT LinearSystemId {
    double B1 = 1.0 / kAlinear.to<double>() + 1.0 / kAangular.to<double>();
    double B2 = 1.0 / kAlinear.to<double>() - 1.0 / kAangular.to<double>();

-    Eigen::Matrix<double, 2, 2> A = 0.5 * frc::MakeMatrix<2, 2>(A1, A2, A2, A1);
-    Eigen::Matrix<double, 2, 2> B = 0.5 * frc::MakeMatrix<2, 2>(B1, B2, B2, B1);
-    Eigen::Matrix<double, 2, 2> C = frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0);
-    Eigen::Matrix<double, 2, 2> D = frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0);
+    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}};

    return LinearSystem<2, 2, 2>(A, B, C, D);
  }
@@ -236,12 +238,11 @@ class WPILIB_DLLEXPORT LinearSystemId {
  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>());
-    Eigen::Matrix<double, 1, 1> B =
-        frc::MakeMatrix<1, 1>((G * motor.Kt / (motor.R * J)).to<double>());
-    Eigen::Matrix<double, 1, 1> C = frc::MakeMatrix<1, 1>(1.0);
-    Eigen::Matrix<double, 1, 1> D = frc::MakeMatrix<1, 1>(0.0);
+    Eigen::Matrix<double, 1, 1> A{
+        (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to<double>()};
+    Eigen::Matrix<double, 1, 1> B{(G * motor.Kt / (motor.R * J)).to<double>()};
+    Eigen::Matrix<double, 1, 1> C{1.0};
+    Eigen::Matrix<double, 1, 1> D{0.0};

    return LinearSystem<1, 1, 1>(A, B, C, D);
  }
@@ -267,18 +268,18 @@ 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 = 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>());
-    Eigen::Matrix<double, 2, 2> 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>());
-    Eigen::Matrix<double, 2, 2> C = frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0);
-    Eigen::Matrix<double, 2, 2> D = frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0);
+    Eigen::Matrix<double, 2, 2> A{
+        {((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>()}};
+    Eigen::Matrix<double, 2, 2> B{
+        {((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>()}};
+    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}};

    return LinearSystem<2, 2, 2>(A, B, C, D);
  }