Merge branch 'main' into 2027

wpimath/src/main/native/include/frc/StateSpaceUtil.h

@@ -123,6 +123,22 @@ constexpr Matrixd<N, N> MakeCostMatrix(const std::array<double, N>& costs) {
   return result;
 }
 
+/**
+ * Creates a cost matrix from the given vector for use with LQR.
+ *
+ * The cost matrix is constructed using Bryson's rule. The inverse square of
+ * each element in the input is placed on the cost matrix diagonal. If a
+ * tolerance is infinity, its cost matrix entry is set to zero.
+ *
+ * @param costs A possibly variable length container. For a Q matrix, its
+ *              elements are the maximum allowed excursions of the states from
+ *              the reference. For an R matrix, its elements are the maximum
+ *              allowed excursions of the control inputs from no actuation.
+ * @return State excursion or control effort cost matrix.
+ */
+WPILIB_DLLEXPORT Eigen::MatrixXd MakeCostMatrix(
+    const std::span<const double> costs);
+
 /**
  * Creates a covariance matrix from the given vector for use with Kalman
  * filters.
@@ -152,6 +168,21 @@ constexpr Matrixd<N, N> MakeCovMatrix(const std::array<double, N>& stdDevs) {
   return result;
 }
 
+/**
+ * Creates a covariance matrix from the given vector for use with Kalman
+ * filters.
+ *
+ * Each element is squared and placed on the covariance matrix diagonal.
+ *
+ * @param stdDevs A possibly variable length container. For a Q matrix, its
+ *                elements are the standard deviations of each state from how
+ *                the model behaves. For an R matrix, its elements are the
+ *                standard deviations for each output measurement.
+ * @return Process noise or measurement noise covariance matrix.
+ */
+WPILIB_DLLEXPORT Eigen::MatrixXd MakeCovMatrix(
+    const std::span<const double> stdDevs);
+
 template <std::same_as<double>... Ts>
 Vectord<sizeof...(Ts)> MakeWhiteNoiseVector(Ts... stdDevs) {
   std::random_device rd;
@@ -200,6 +231,17 @@ Vectord<N> MakeWhiteNoiseVector(const std::array<double, N>& stdDevs) {
   return result;
 }
 
+/**
+ * Creates a vector of normally distributed white noise with the given noise
+ * intensities for each element.
+ *
+ * @param stdDevs A possibly variable length container whose elements are the
+ *                standard deviations of each element of the noise vector.
+ * @return White noise vector.
+ */
+WPILIB_DLLEXPORT Eigen::VectorXd MakeWhiteNoiseVector(
+    const std::span<const double> stdDevs);
+
 /**
  * Converts a Pose2d into a vector of [x, y, theta].
  *
@@ -311,6 +353,10 @@ bool IsDetectable(const Matrixd<States, States>& A,
   return IsStabilizable<States, Outputs>(A.transpose(), C.transpose());
 }
 
+extern template WPILIB_DLLEXPORT bool
+IsDetectable<Eigen::Dynamic, Eigen::Dynamic>(const Eigen::MatrixXd& A,
+                                             const Eigen::MatrixXd& C);
+
 /**
  * Converts a Pose2d into a vector of [x, y, theta].
  *
@@ -341,12 +387,17 @@ constexpr Vectord<Inputs> ClampInputMaxMagnitude(const Vectord<Inputs>& u,
                                                  const Vectord<Inputs>& umin,
                                                  const Vectord<Inputs>& umax) {
   Vectord<Inputs> result;
-  for (int i = 0; i < Inputs; ++i) {
+  for (int i = 0; i < u.rows(); ++i) {
     result(i) = std::clamp(u(i), umin(i), umax(i));
   }
   return result;
 }
 
+extern template WPILIB_DLLEXPORT Eigen::VectorXd
+ClampInputMaxMagnitude<Eigen::Dynamic>(const Eigen::VectorXd& u,
+                                       const Eigen::VectorXd& umin,
+                                       const Eigen::VectorXd& umax);
+
 /**
  * Renormalize all inputs if any exceeds the maximum magnitude. Useful for
  * systems such as differential drivetrains.
@@ -366,4 +417,9 @@ Vectord<Inputs> DesaturateInputVector(const Vectord<Inputs>& u,
   }
   return u;
 }
+
+extern template WPILIB_DLLEXPORT Eigen::VectorXd
+DesaturateInputVector<Eigen::Dynamic>(const Eigen::VectorXd& u,
+                                      double maxMagnitude);
+
 }  // namespace frc

wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h

@@ -23,11 +23,11 @@ namespace frc {
  * This holonomic drive controller can be used to follow trajectories using a
  * holonomic drivetrain (i.e. swerve or mecanum). Holonomic trajectory following
  * is a much simpler problem to solve compared to skid-steer style drivetrains
- * because it is possible to individually control forward, sideways, and angular
- * velocity.
+ * because it is possible to individually control field-relative x, y, and
+ * angular velocity.
  *
  * The holonomic drive controller takes in one PID controller for each
- * direction, forward and sideways, and one profiled PID controller for the
+ * direction, field-relative x and y, and one profiled PID controller for the
  * angular direction. Because the heading dynamics are decoupled from
  * translations, users can specify a custom heading that the drivetrain should
  * point toward. This heading reference is profiled for smoothness.

wpimath/src/main/native/include/frc/system/NumericalJacobian.h

@@ -34,6 +34,36 @@ auto NumericalJacobian(F&& f, const Vectord<Cols>& x) {
   return result;
 }
 
+/**
+ * Returns numerical Jacobian with respect to x for f(x).
+ *
+ * @param f     Vector-valued function from which to compute Jacobian.
+ * @param x     Vector argument.
+ */
+
+template <typename F>
+Eigen::MatrixXd NumericalJacobian(F&& f, const Eigen::VectorXd& x) {
+  constexpr double kEpsilon = 1e-5;
+  Eigen::MatrixXd result;
+
+  // It's more expensive, but +- epsilon will be more accurate
+  for (int i = 0; i < x.rows(); ++i) {
+    Eigen::VectorXd dX_plus = x;
+    dX_plus(i) += kEpsilon;
+    Eigen::VectorXd dX_minus = x;
+    dX_minus(i) -= kEpsilon;
+    Eigen::VectorXd partialDerivative =
+        (f(dX_plus) - f(dX_minus)) / (kEpsilon * 2.0);
+    if (i == 0) {
+      result.resize(partialDerivative.rows(), x.rows());
+      result.setZero();
+    }
+    result.col(i) = partialDerivative;
+  }
+
+  return result;
+}
+
 /**
  * Returns numerical Jacobian with respect to x for f(x, u, ...).
  *
@@ -54,6 +84,23 @@ auto NumericalJacobianX(F&& f, const Vectord<States>& x,
       [&](const Vectord<States>& x) { return f(x, u, args...); }, x);
 }
 
+/**
+ * Returns numerical Jacobian with respect to x for f(x, u, ...).
+ *
+ * @tparam F       Function object type.
+ * @tparam Args... Types of remaining arguments to f(x, u, ...).
+ * @param f        Vector-valued function from which to compute Jacobian.
+ * @param x        State vector.
+ * @param u        Input vector.
+ * @param args     Remaining arguments to f(x, u, ...).
+ */
+template <typename F, typename... Args>
+auto NumericalJacobianX(F&& f, const Eigen::VectorXd& x,
+                        const Eigen::VectorXd& u, Args&&... args) {
+  return NumericalJacobian(
+      [&](const Eigen::VectorXd& x) { return f(x, u, args...); }, x);
+}
+
 /**
  * Returns numerical Jacobian with respect to u for f(x, u, ...).
  *
@@ -74,4 +121,21 @@ auto NumericalJacobianU(F&& f, const Vectord<States>& x,
       [&](const Vectord<Inputs>& u) { return f(x, u, args...); }, u);
 }
 
+/**
+ * Returns numerical Jacobian with respect to u for f(x, u, ...).
+ *
+ * @tparam F       Function object type.
+ * @tparam Args... Types of remaining arguments to f(x, u, ...).
+ * @param f        Vector-valued function from which to compute Jacobian.
+ * @param x        State vector.
+ * @param u        Input vector.
+ * @param args     Remaining arguments to f(x, u, ...).
+ */
+template <typename F, typename... Args>
+auto NumericalJacobianU(F&& f, const Eigen::VectorXd& x,
+                        const Eigen::VectorXd& u, Args&&... args) {
+  return NumericalJacobian(
+      [&](const Eigen::VectorXd& u) { return f(x, u, args...); }, u);
+}
+
 }  // namespace frc