[wpimath] Add typedefs for common types

This makes complex code significantly easier to read.

frc::Vectord<Size> = Eigen::Vector<double, Size>
frc::Matrixd<Rows, Cols> = Eigen::Matrix<double, Rows, Cols>

wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp

@@ -42,5 +42,5 @@ units::ampere_t DCMotorSim::GetCurrentDraw() const {
 }
 
 void DCMotorSim::SetInputVoltage(units::volt_t voltage) {
-  SetInput(Eigen::Vector<double, 1>{voltage.value()});
+  SetInput(Vectord<1>{voltage.value()});
 }

wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp

@@ -41,8 +41,7 @@ DifferentialDrivetrainSim::DifferentialDrivetrainSim(
               driveMotor, mass, wheelRadius, trackWidth / 2.0, J, gearing),
           trackWidth, driveMotor, gearing, wheelRadius, measurementStdDevs) {}
 
-Eigen::Vector<double, 2> DifferentialDrivetrainSim::ClampInput(
-    const Eigen::Vector<double, 2>& u) {
+Vectord<2> DifferentialDrivetrainSim::ClampInput(const Vectord<2>& u) {
   return frc::DesaturateInputVector<2>(u,
                                        frc::RobotController::GetInputVoltage());
 }
@@ -66,11 +65,11 @@ double DifferentialDrivetrainSim::GetGearing() const {
   return m_currentGearing;
 }
 
-Eigen::Vector<double, 7> DifferentialDrivetrainSim::GetOutput() const {
+Vectord<7> DifferentialDrivetrainSim::GetOutput() const {
   return m_y;
 }
 
-Eigen::Vector<double, 7> DifferentialDrivetrainSim::GetState() const {
+Vectord<7> DifferentialDrivetrainSim::GetState() const {
   return m_x;
 }
 
@@ -116,8 +115,7 @@ units::ampere_t DifferentialDrivetrainSim::GetCurrentDraw() const {
   return GetLeftCurrentDraw() + GetRightCurrentDraw();
 }
 
-void DifferentialDrivetrainSim::SetState(
-    const Eigen::Vector<double, 7>& state) {
+void DifferentialDrivetrainSim::SetState(const Vectord<7>& state) {
   m_x = state;
 }
 
@@ -129,19 +127,19 @@ void DifferentialDrivetrainSim::SetPose(const frc::Pose2d& pose) {
   m_x(State::kRightPosition) = 0;
 }
 
-Eigen::Vector<double, 7> DifferentialDrivetrainSim::Dynamics(
-    const Eigen::Vector<double, 7>& x, const Eigen::Vector<double, 2>& u) {
+Vectord<7> DifferentialDrivetrainSim::Dynamics(const Vectord<7>& x,
+                                               const Vectord<2>& u) {
   // Because G^2 can be factored out of A, we can divide by the old ratio
   // squared and multiply by the new ratio squared to get a new drivetrain
   // model.
-  Eigen::Matrix<double, 4, 2> B;
+  Matrixd<4, 2> B;
   B.block<2, 2>(0, 0) = m_plant.B() * m_currentGearing * m_currentGearing /
                         m_originalGearing / m_originalGearing;
   B.block<2, 2>(2, 0).setZero();
 
   // Because G can be factored out of B, we can divide by the old ratio and
   // multiply by the new ratio to get a new drivetrain model.
-  Eigen::Matrix<double, 4, 4> A;
+  Matrixd<4, 4> A;
   A.block<2, 2>(0, 0) = m_plant.A() * m_currentGearing / m_originalGearing;
 
   A.block<2, 2>(2, 0).setIdentity();
@@ -149,7 +147,7 @@ Eigen::Vector<double, 7> DifferentialDrivetrainSim::Dynamics(
 
   double v = (x(State::kLeftVelocity) + x(State::kRightVelocity)) / 2.0;
 
-  Eigen::Vector<double, 7> xdot;
+  Vectord<7> xdot;
   xdot(0) = v * std::cos(x(State::kHeading));
   xdot(1) = v * std::sin(x(State::kHeading));
   xdot(2) =

wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp

@@ -80,29 +80,27 @@ units::ampere_t ElevatorSim::GetCurrentDraw() const {
 }
 
 void ElevatorSim::SetInputVoltage(units::volt_t voltage) {
-  SetInput(Eigen::Vector<double, 1>{voltage.value()});
+  SetInput(Vectord<1>{voltage.value()});
 }
 
-Eigen::Vector<double, 2> ElevatorSim::UpdateX(
-    const Eigen::Vector<double, 2>& currentXhat,
-    const Eigen::Vector<double, 1>& u, units::second_t dt) {
+Vectord<2> ElevatorSim::UpdateX(const Vectord<2>& currentXhat,
+                                const Vectord<1>& u, units::second_t dt) {
   auto updatedXhat = RKDP(
-      [&](const Eigen::Vector<double, 2>& x,
-          const Eigen::Vector<double, 1>& u_) -> Eigen::Vector<double, 2> {
-        Eigen::Vector<double, 2> xdot = m_plant.A() * x + m_plant.B() * u;
+      [&](const Vectord<2>& x, const Vectord<1>& u_) -> Vectord<2> {
+        Vectord<2> xdot = m_plant.A() * x + m_plant.B() * u;
 
         if (m_simulateGravity) {
-          xdot += Eigen::Vector<double, 2>{0.0, -9.8};
+          xdot += Vectord<2>{0.0, -9.8};
         }
         return xdot;
       },
       currentXhat, u, dt);
   // Check for collision after updating x-hat.
   if (WouldHitLowerLimit(units::meter_t(updatedXhat(0)))) {
-    return Eigen::Vector<double, 2>{m_minHeight.value(), 0.0};
+    return Vectord<2>{m_minHeight.value(), 0.0};
   }
   if (WouldHitUpperLimit(units::meter_t(updatedXhat(0)))) {
-    return Eigen::Vector<double, 2>{m_maxHeight.value(), 0.0};
+    return Vectord<2>{m_maxHeight.value(), 0.0};
   }
   return updatedXhat;
 }

wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp

@@ -38,5 +38,5 @@ units::ampere_t FlywheelSim::GetCurrentDraw() const {
 }
 
 void FlywheelSim::SetInputVoltage(units::volt_t voltage) {
-  SetInput(Eigen::Vector<double, 1>{voltage.value()});
+  SetInput(Vectord<1>{voltage.value()});
 }

wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp

@@ -72,12 +72,12 @@ units::ampere_t SingleJointedArmSim::GetCurrentDraw() const {
 }
 
 void SingleJointedArmSim::SetInputVoltage(units::volt_t voltage) {
-  SetInput(Eigen::Vector<double, 1>{voltage.value()});
+  SetInput(Vectord<1>{voltage.value()});
 }
 
-Eigen::Vector<double, 2> SingleJointedArmSim::UpdateX(
-    const Eigen::Vector<double, 2>& currentXhat,
-    const Eigen::Vector<double, 1>& u, units::second_t dt) {
+Vectord<2> SingleJointedArmSim::UpdateX(const Vectord<2>& currentXhat,
+                                        const Vectord<1>& u,
+                                        units::second_t dt) {
   // Horizontal case:
   // Torque = F * r = I * alpha
   // alpha = F * r / I
@@ -88,15 +88,14 @@ Eigen::Vector<double, 2> SingleJointedArmSim::UpdateX(
   // We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I *
   // std::cos(theta)]]
 
-  Eigen::Vector<double, 2> updatedXhat = RKDP(
-      [&](const auto& x, const auto& u) -> Eigen::Vector<double, 2> {
-        Eigen::Vector<double, 2> xdot = m_plant.A() * x + m_plant.B() * u;
+  Vectord<2> updatedXhat = RKDP(
+      [&](const auto& x, const auto& u) -> Vectord<2> {
+        Vectord<2> xdot = m_plant.A() * x + m_plant.B() * u;
 
         if (m_simulateGravity) {
-          xdot += Eigen::Vector<double, 2>{
-              0.0, (m_armMass * m_r * -9.8 * 3.0 / (m_armMass * m_r * m_r) *
-                    std::cos(x(0)))
-                       .value()};
+          xdot += Vectord<2>{0.0, (m_armMass * m_r * -9.8 * 3.0 /
+                                   (m_armMass * m_r * m_r) * std::cos(x(0)))
+                                      .value()};
         }
         return xdot;
       },
@@ -104,9 +103,9 @@ Eigen::Vector<double, 2> SingleJointedArmSim::UpdateX(
 
   // Check for collisions.
   if (WouldHitLowerLimit(units::radian_t(updatedXhat(0)))) {
-    return Eigen::Vector<double, 2>{m_minAngle.value(), 0.0};
+    return Vectord<2>{m_minAngle.value(), 0.0};
   } else if (WouldHitUpperLimit(units::radian_t(updatedXhat(0)))) {
-    return Eigen::Vector<double, 2>{m_maxAngle.value(), 0.0};
+    return Vectord<2>{m_maxAngle.value(), 0.0};
   }
   return updatedXhat;
 }

wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h

@@ -4,11 +4,11 @@
 
 #pragma once
 
+#include <frc/EigenCore.h>
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/system/LinearSystem.h>
 #include <frc/system/plant/DCMotor.h>
 
-#include <Eigen/Core>
 #include <units/length.h>
 #include <units/moment_of_inertia.h>
 #include <units/time.h>
@@ -80,7 +80,7 @@ class DifferentialDrivetrainSim {
    * @param u The input vector.
    * @return The normalized input.
    */
-  Eigen::Vector<double, 2> ClampInput(const Eigen::Vector<double, 2>& u);
+  Vectord<2> ClampInput(const Vectord<2>& u);
 
   /**
    * Sets the applied voltage to the drivetrain. Note that positive voltage must
@@ -178,7 +178,7 @@ class DifferentialDrivetrainSim {
    *
    * @param state The state.
    */
-  void SetState(const Eigen::Vector<double, 7>& state);
+  void SetState(const Vectord<7>& state);
 
   /**
    * Sets the system pose.
@@ -187,8 +187,7 @@ class DifferentialDrivetrainSim {
    */
   void SetPose(const frc::Pose2d& pose);
 
-  Eigen::Vector<double, 7> Dynamics(const Eigen::Vector<double, 7>& x,
-                                    const Eigen::Vector<double, 2>& u);
+  Vectord<7> Dynamics(const Vectord<7>& x, const Vectord<2>& u);
 
   class State {
    public:
@@ -301,7 +300,7 @@ class DifferentialDrivetrainSim {
   /**
    * Returns the current output vector y.
    */
-  Eigen::Vector<double, 7> GetOutput() const;
+  Vectord<7> GetOutput() const;
 
   /**
    * Returns an element of the state vector. Note that this will not include
@@ -314,7 +313,7 @@ class DifferentialDrivetrainSim {
   /**
    * Returns the current state vector x. Note that this will not include noise!
    */
-  Eigen::Vector<double, 7> GetState() const;
+  Vectord<7> GetState() const;
 
   LinearSystem<2, 2, 2> m_plant;
   units::meter_t m_rb;
@@ -325,9 +324,9 @@ class DifferentialDrivetrainSim {
   double m_originalGearing;
   double m_currentGearing;
 
-  Eigen::Vector<double, 7> m_x;
-  Eigen::Vector<double, 2> m_u;
-  Eigen::Vector<double, 7> m_y;
+  Vectord<7> m_x;
+  Vectord<2> m_u;
+  Vectord<7> m_y;
   std::array<double, 7> m_measurementStdDevs;
 };
 }  // namespace frc::sim

wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h

@@ -127,9 +127,8 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
    * @param u           The system inputs (voltage).
    * @param dt          The time difference between controller updates.
    */
-  Eigen::Vector<double, 2> UpdateX(const Eigen::Vector<double, 2>& currentXhat,
-                                   const Eigen::Vector<double, 1>& u,
-                                   units::second_t dt) override;
+  Vectord<2> UpdateX(const Vectord<2>& currentXhat, const Vectord<1>& u,
+                     units::second_t dt) override;
 
  private:
   DCMotor m_gearbox;

wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h

@@ -6,10 +6,10 @@
 
 #include <array>
 
-#include <Eigen/Core>
 #include <units/current.h>
 #include <units/time.h>
 
+#include "frc/EigenCore.h"
 #include "frc/RobotController.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/system/LinearSystem.h"
@@ -40,9 +40,9 @@ class LinearSystemSim {
       const LinearSystem<States, Inputs, Outputs>& system,
       const std::array<double, Outputs>& measurementStdDevs = {})
       : m_plant(system), m_measurementStdDevs(measurementStdDevs) {
-    m_x = Eigen::Vector<double, States>::Zero();
-    m_y = Eigen::Vector<double, Outputs>::Zero();
-    m_u = Eigen::Vector<double, Inputs>::Zero();
+    m_x = Vectord<States>::Zero();
+    m_y = Vectord<Outputs>::Zero();
+    m_u = Vectord<Inputs>::Zero();
   }
 
   virtual ~LinearSystemSim() = default;
@@ -71,7 +71,7 @@ class LinearSystemSim {
    *
    * @return The current output of the plant.
    */
-  const Eigen::Vector<double, Outputs>& GetOutput() const { return m_y; }
+  const Vectord<Outputs>& GetOutput() const { return m_y; }
 
   /**
    * Returns an element of the current output of the plant.
@@ -86,7 +86,7 @@ class LinearSystemSim {
    *
    * @param u The system inputs.
    */
-  void SetInput(const Eigen::Vector<double, Inputs>& u) { m_u = ClampInput(u); }
+  void SetInput(const Vectord<Inputs>& u) { m_u = ClampInput(u); }
 
   /*
    * Sets the system inputs.
@@ -104,7 +104,7 @@ class LinearSystemSim {
    *
    * @param state The new state.
    */
-  void SetState(const Eigen::Vector<double, States>& state) { m_x = state; }
+  void SetState(const Vectord<States>& state) { m_x = state; }
 
   /**
    * Returns the current drawn by this simulated system. Override this method to
@@ -124,9 +124,9 @@ class LinearSystemSim {
    * @param u           The system inputs (usually voltage).
    * @param dt          The time difference between controller updates.
    */
-  virtual Eigen::Vector<double, States> UpdateX(
-      const Eigen::Vector<double, States>& currentXhat,
-      const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
+  virtual Vectord<States> UpdateX(const Vectord<States>& currentXhat,
+                                  const Vectord<Inputs>& u,
+                                  units::second_t dt) {
     return m_plant.CalculateX(currentXhat, u, dt);
   }
 
@@ -137,16 +137,16 @@ class LinearSystemSim {
    * @param u          The input vector.
    * @return The normalized input.
    */
-  Eigen::Vector<double, Inputs> ClampInput(Eigen::Vector<double, Inputs> u) {
+  Vectord<Inputs> ClampInput(Vectord<Inputs> u) {
     return frc::DesaturateInputVector<Inputs>(
         u, frc::RobotController::GetInputVoltage());
   }
 
   LinearSystem<States, Inputs, Outputs> m_plant;
 
-  Eigen::Vector<double, States> m_x;
-  Eigen::Vector<double, Outputs> m_y;
-  Eigen::Vector<double, Inputs> m_u;
+  Vectord<States> m_x;
+  Vectord<Outputs> m_y;
+  Vectord<Inputs> m_u;
   std::array<double, Outputs> m_measurementStdDevs;
 };
 }  // namespace frc::sim

wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h

@@ -142,9 +142,8 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
    * @param u           The system inputs (voltage).
    * @param dt          The time difference between controller updates.
    */
-  Eigen::Vector<double, 2> UpdateX(const Eigen::Vector<double, 2>& currentXhat,
-                                   const Eigen::Vector<double, 1>& u,
-                                   units::second_t dt) override;
+  Vectord<2> UpdateX(const Vectord<2>& currentXhat, const Vectord<1>& u,
+                     units::second_t dt) override;
 
  private:
   units::meter_t m_r;