[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>

wpimath/src/main/native/cpp/StateSpaceUtil.cpp

@@ -6,33 +6,31 @@
 
 namespace frc {
 
-Eigen::Vector<double, 3> PoseTo3dVector(const Pose2d& pose) {
-  return Eigen::Vector<double, 3>{pose.Translation().X().value(),
-                                  pose.Translation().Y().value(),
-                                  pose.Rotation().Radians().value()};
+Vectord<3> PoseTo3dVector(const Pose2d& pose) {
+  return Vectord<3>{pose.Translation().X().value(),
+                    pose.Translation().Y().value(),
+                    pose.Rotation().Radians().value()};
 }
 
-Eigen::Vector<double, 4> PoseTo4dVector(const Pose2d& pose) {
-  return Eigen::Vector<double, 4>{pose.Translation().X().value(),
-                                  pose.Translation().Y().value(),
-                                  pose.Rotation().Cos(), pose.Rotation().Sin()};
+Vectord<4> PoseTo4dVector(const Pose2d& pose) {
+  return Vectord<4>{pose.Translation().X().value(),
+                    pose.Translation().Y().value(), pose.Rotation().Cos(),
+                    pose.Rotation().Sin()};
 }
 
 template <>
-bool IsStabilizable<1, 1>(const Eigen::Matrix<double, 1, 1>& A,
-                          const Eigen::Matrix<double, 1, 1>& B) {
+bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B) {
   return detail::IsStabilizableImpl<1, 1>(A, B);
 }
 
 template <>
-bool IsStabilizable<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
-                          const Eigen::Matrix<double, 2, 1>& B) {
+bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B) {
   return detail::IsStabilizableImpl<2, 1>(A, B);
 }
 
-Eigen::Vector<double, 3> PoseToVector(const Pose2d& pose) {
-  return Eigen::Vector<double, 3>{pose.X().value(), pose.Y().value(),
-                                  pose.Rotation().Radians().value()};
+Vectord<3> PoseToVector(const Pose2d& pose) {
+  return Vectord<3>{pose.X().value(), pose.Y().value(),
+                    pose.Rotation().Radians().value()};
 }
 
 }  // namespace frc

wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp

@@ -24,21 +24,21 @@ DifferentialDriveAccelerationLimiter::Calculate(
     units::meters_per_second_t leftVelocity,
     units::meters_per_second_t rightVelocity, units::volt_t leftVoltage,
     units::volt_t rightVoltage) {
-  Eigen::Vector<double, 2> u{leftVoltage.value(), rightVoltage.value()};
+  Vectord<2> u{leftVoltage.value(), rightVoltage.value()};
 
   // Find unconstrained wheel accelerations
-  Eigen::Vector<double, 2> x{leftVelocity.value(), rightVelocity.value()};
-  Eigen::Vector<double, 2> dxdt = m_system.A() * x + m_system.B() * u;
+  Vectord<2> x{leftVelocity.value(), rightVelocity.value()};
+  Vectord<2> dxdt = m_system.A() * x + m_system.B() * u;
 
   // Converts from wheel accelerations to linear and angular acceleration
   // a = (dxdt(0) + dxdt(1)) / 2.0
   // alpha = (dxdt(1) - dxdt(0)) / trackwidth
-  Eigen::Matrix<double, 2, 2> M{
-      {0.5, 0.5}, {-1.0 / m_trackwidth.value(), 1.0 / m_trackwidth.value()}};
+  Matrixd<2, 2> M{{0.5, 0.5},
+                  {-1.0 / m_trackwidth.value(), 1.0 / m_trackwidth.value()}};
 
   // Convert to linear and angular accelerations, constrain them, then convert
   // back
-  Eigen::Vector<double, 2> accels = M * dxdt;
+  Vectord<2> accels = M * dxdt;
   if (accels(0) > m_maxLinearAccel.value()) {
     accels(0) = m_maxLinearAccel.value();
   } else if (accels(0) < -m_maxLinearAccel.value()) {

wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp

@@ -7,60 +7,60 @@
 namespace frc {
 
 LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
-    const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
+    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(A, B, MakeCostMatrix(Qelems),
                                MakeCostMatrix(Relems), dt) {}
 
-LinearQuadraticRegulator<1, 1>::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,
-    units::second_t dt)
+LinearQuadraticRegulator<1, 1>::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)
     : detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, dt) {}
 
 LinearQuadraticRegulator<1, 1>::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)
+    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)
     : detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, N, dt) {}
 
 LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
-    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
+    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(A, B, MakeCostMatrix(Qelems),
                                MakeCostMatrix(Relems), dt) {}
 
-LinearQuadraticRegulator<2, 1>::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,
-    units::second_t dt)
+LinearQuadraticRegulator<2, 1>::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)
     : detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, dt) {}
 
 LinearQuadraticRegulator<2, 1>::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)
+    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)
     : detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, N, dt) {}
 
 LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(
-    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
+    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(A, B, MakeCostMatrix(Qelems),
                                MakeCostMatrix(Relems), dt) {}
 
-LinearQuadraticRegulator<2, 2>::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,
-    units::second_t dt)
+LinearQuadraticRegulator<2, 2>::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)
     : detail::LinearQuadraticRegulatorImpl<2, 2>(A, B, Q, R, dt) {}
 
 LinearQuadraticRegulator<2, 2>::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)
+    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)
     : detail::LinearQuadraticRegulatorImpl<2, 2>(A, B, Q, R, N, dt) {}
 
 }  // namespace frc

wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp

@@ -19,9 +19,8 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
     units::second_t nominalDt)
     : m_observer(
           &DifferentialDrivePoseEstimator::F,
-          [](const Eigen::Vector<double, 5>& x,
-             const Eigen::Vector<double, 3>& u) {
-            return Eigen::Vector<double, 3>{x(3, 0), x(4, 0), x(2, 0)};
+          [](const Vectord<5>& x, const Vectord<3>& u) {
+            return Vectord<3>{x(3, 0), x(4, 0), x(2, 0)};
           },
           stateStdDevs, localMeasurementStdDevs, frc::AngleMean<5, 5>(2),
           frc::AngleMean<3, 5>(2), frc::AngleResidual<5>(2),
@@ -30,11 +29,10 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
   SetVisionMeasurementStdDevs(visionMeasurmentStdDevs);
 
   // Create correction mechanism for vision measurements.
-  m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
-                        const Eigen::Vector<double, 3>& y) {
+  m_visionCorrect = [&](const Vectord<3>& u, const Vectord<3>& y) {
     m_observer.Correct<3>(
         u, y,
-        [](const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 3>&) {
+        [](const Vectord<5>& x, const Vectord<3>&) {
           return x.block<3, 1>(0, 0);
         },
         m_visionContR, frc::AngleMean<3, 5>(2), frc::AngleResidual<3>(2),
@@ -73,7 +71,7 @@ Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
 void DifferentialDrivePoseEstimator::AddVisionMeasurement(
     const Pose2d& visionRobotPose, units::second_t timestamp) {
   if (auto sample = m_poseBuffer.Sample(timestamp)) {
-    m_visionCorrect(Eigen::Vector<double, 3>::Zero(),
+    m_visionCorrect(Vectord<3>::Zero(),
                     PoseTo3dVector(GetEstimatedPosition().TransformBy(
                         visionRobotPose - sample.value())));
   }
@@ -97,13 +95,13 @@ Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
   auto angle = gyroAngle + m_gyroOffset;
   auto omega = (gyroAngle - m_previousAngle).Radians() / dt;
 
-  auto u = Eigen::Vector<double, 3>{
-      (wheelSpeeds.left + wheelSpeeds.right).value() / 2.0, 0.0, omega.value()};
+  auto u = Vectord<3>{(wheelSpeeds.left + wheelSpeeds.right).value() / 2.0, 0.0,
+                      omega.value()};
 
   m_previousAngle = angle;
 
-  auto localY = Eigen::Vector<double, 3>{
-      leftDistance.value(), rightDistance.value(), angle.Radians().value()};
+  auto localY = Vectord<3>{leftDistance.value(), rightDistance.value(),
+                           angle.Radians().value()};
 
   m_poseBuffer.AddSample(currentTime, GetEstimatedPosition());
   m_observer.Predict(u, dt);
@@ -112,24 +110,24 @@ Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
   return GetEstimatedPosition();
 }
 
-Eigen::Vector<double, 5> DifferentialDrivePoseEstimator::F(
-    const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 3>& u) {
+Vectord<5> DifferentialDrivePoseEstimator::F(const Vectord<5>& x,
+                                             const Vectord<3>& u) {
   // Apply a rotation matrix. Note that we do not add x because Runge-Kutta does
   // that for us.
   auto& theta = x(2);
-  Eigen::Matrix<double, 5, 5> toFieldRotation{
+  Matrixd<5, 5> toFieldRotation{
       {std::cos(theta), -std::sin(theta), 0.0, 0.0, 0.0},
       {std::sin(theta), std::cos(theta), 0.0, 0.0, 0.0},
       {0.0, 0.0, 1.0, 0.0, 0.0},
       {0.0, 0.0, 0.0, 1.0, 0.0},
       {0.0, 0.0, 0.0, 0.0, 1.0}};
   return toFieldRotation *
-         Eigen::Vector<double, 5>{u(0, 0), u(1, 0), u(2, 0), u(0, 0), u(1, 0)};
+         Vectord<5>{u(0, 0), u(1, 0), u(2, 0), u(0, 0), u(1, 0)};
 }
 
 template <int Dim>
 wpi::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
-    const Eigen::Vector<double, Dim>& stdDevs) {
+    const Vectord<Dim>& stdDevs) {
   wpi::array<double, Dim> array;
   for (size_t i = 0; i < Dim; ++i) {
     array[i] = stdDevs(i);
@@ -137,11 +135,11 @@ wpi::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
   return array;
 }
 
-Eigen::Vector<double, 5> DifferentialDrivePoseEstimator::FillStateVector(
+Vectord<5> DifferentialDrivePoseEstimator::FillStateVector(
     const Pose2d& pose, units::meter_t leftDistance,
     units::meter_t rightDistance) {
-  return Eigen::Vector<double, 5>{pose.Translation().X().value(),
-                                  pose.Translation().Y().value(),
-                                  pose.Rotation().Radians().value(),
-                                  leftDistance.value(), rightDistance.value()};
+  return Vectord<5>{pose.Translation().X().value(),
+                    pose.Translation().Y().value(),
+                    pose.Rotation().Radians().value(), leftDistance.value(),
+                    rightDistance.value()};
 }

wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp

@@ -18,26 +18,21 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
     const wpi::array<double, 1>& localMeasurementStdDevs,
     const wpi::array<double, 3>& visionMeasurementStdDevs,
     units::second_t nominalDt)
-    : m_observer(
-          [](const Eigen::Vector<double, 3>& x,
-             const Eigen::Vector<double, 3>& u) { return u; },
-          [](const Eigen::Vector<double, 3>& x,
-             const Eigen::Vector<double, 3>& u) { return x.block<1, 1>(2, 0); },
-          stateStdDevs, localMeasurementStdDevs, frc::AngleMean<3, 3>(2),
-          frc::AngleMean<1, 3>(0), frc::AngleResidual<3>(2),
-          frc::AngleResidual<1>(0), frc::AngleAdd<3>(2), nominalDt),
+    : m_observer([](const Vectord<3>& x, const Vectord<3>& u) { return u; },
+                 [](const Vectord<3>& x, const Vectord<3>& u) {
+                   return x.block<1, 1>(2, 0);
+                 },
+                 stateStdDevs, localMeasurementStdDevs, frc::AngleMean<3, 3>(2),
+                 frc::AngleMean<1, 3>(0), frc::AngleResidual<3>(2),
+                 frc::AngleResidual<1>(0), frc::AngleAdd<3>(2), nominalDt),
       m_kinematics(kinematics),
       m_nominalDt(nominalDt) {
   SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
 
   // Create vision correction mechanism.
-  m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
-                        const Eigen::Vector<double, 3>& y) {
+  m_visionCorrect = [&](const Vectord<3>& u, const Vectord<3>& y) {
     m_observer.Correct<3>(
-        u, y,
-        [](const Eigen::Vector<double, 3>& x, const Eigen::Vector<double, 3>&) {
-          return x;
-        },
+        u, y, [](const Vectord<3>& x, const Vectord<3>&) { return x; },
         m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2),
         frc::AngleResidual<3>(2), frc::AngleAdd<3>(2));
   };
@@ -76,7 +71,7 @@ Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const {
 void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
     const Pose2d& visionRobotPose, units::second_t timestamp) {
   if (auto sample = m_poseBuffer.Sample(timestamp)) {
-    m_visionCorrect(Eigen::Vector<double, 3>::Zero(),
+    m_visionCorrect(Vectord<3>::Zero(),
                     PoseTo3dVector(GetEstimatedPosition().TransformBy(
                         visionRobotPose - sample.value())));
   }
@@ -102,11 +97,10 @@ Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
       Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s)
           .RotateBy(angle);
 
-  Eigen::Vector<double, 3> u{fieldRelativeVelocities.X().value(),
-                             fieldRelativeVelocities.Y().value(),
-                             omega.value()};
+  Vectord<3> u{fieldRelativeVelocities.X().value(),
+               fieldRelativeVelocities.Y().value(), omega.value()};
 
-  Eigen::Vector<double, 1> localY{angle.Radians().value()};
+  Vectord<1> localY{angle.Radians().value()};
   m_previousAngle = angle;
 
   m_poseBuffer.AddSample(currentTime, GetEstimatedPosition());

wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp

@@ -25,8 +25,7 @@ MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
                                       chassisSpeeds.vy.value(),
                                       chassisSpeeds.omega.value()};
 
-  Eigen::Vector<double, 4> wheelsVector =
-      m_inverseKinematics * chassisSpeedsVector;
+  Vectord<4> wheelsVector = m_inverseKinematics * chassisSpeedsVector;
 
   MecanumDriveWheelSpeeds wheelSpeeds;
   wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsVector(0)};
@@ -38,7 +37,7 @@ MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
 
 ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
     const MecanumDriveWheelSpeeds& wheelSpeeds) const {
-  Eigen::Vector<double, 4> wheelSpeedsVector{
+  Vectord<4> wheelSpeedsVector{
       wheelSpeeds.frontLeft.value(), wheelSpeeds.frontRight.value(),
       wheelSpeeds.rearLeft.value(), wheelSpeeds.rearRight.value()};
 
@@ -54,9 +53,8 @@ void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
                                                   Translation2d fr,
                                                   Translation2d rl,
                                                   Translation2d rr) const {
-  m_inverseKinematics =
-      Eigen::Matrix<double, 4, 3>{{1, -1, (-(fl.X() + fl.Y())).value()},
-                                  {1, 1, (fr.X() - fr.Y()).value()},
-                                  {1, 1, (rl.X() - rl.Y()).value()},
-                                  {1, -1, (-(rr.X() + rr.Y())).value()}};
+  m_inverseKinematics = Matrixd<4, 3>{{1, -1, (-(fl.X() + fl.Y())).value()},
+                                      {1, 1, (fr.X() - fr.Y()).value()},
+                                      {1, 1, (rl.X() - rl.Y()).value()},
+                                      {1, -1, (-(rr.X() + rr.Y())).value()}};
 }