Merge branch 'main' into 2027

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

@@ -4,6 +4,8 @@
 
 #include "frc/StateSpaceUtil.h"
 
+#include <limits>
+
 namespace frc {
 
 template bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A,
@@ -13,4 +15,57 @@ template bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A,
 template bool IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(
     const Eigen::MatrixXd& A, const Eigen::MatrixXd& B);
 
+template bool IsDetectable<Eigen::Dynamic, Eigen::Dynamic>(
+    const Eigen::MatrixXd& A, const Eigen::MatrixXd& C);
+
+template Eigen::VectorXd ClampInputMaxMagnitude<Eigen::Dynamic>(
+    const Eigen::VectorXd& u, const Eigen::VectorXd& umin,
+    const Eigen::VectorXd& umax);
+
+template Eigen::VectorXd DesaturateInputVector<Eigen::Dynamic>(
+    const Eigen::VectorXd& u, double maxMagnitude);
+
+Eigen::MatrixXd MakeCostMatrix(const std::span<const double> costs) {
+  Eigen::MatrixXd result{costs.size(), costs.size()};
+  result.setZero();
+
+  for (size_t i = 0; i < costs.size(); ++i) {
+    if (costs[i] == std::numeric_limits<double>::infinity()) {
+      result(i, i) = 0.0;
+    } else {
+      result(i, i) = 1.0 / (std::pow(costs[i], 2));
+    }
+  }
+  return result;
+}
+
+Eigen::VectorXd MakeWhiteNoiseVector(const std::span<const double> stdDevs) {
+  std::random_device rd;
+  std::mt19937 gen{rd()};
+
+  Eigen::VectorXd result{stdDevs.size()};
+  for (size_t i = 0; i < stdDevs.size(); ++i) {
+    // Passing a standard deviation of 0.0 to std::normal_distribution is
+    // undefined behavior
+    if (stdDevs[i] == 0.0) {
+      result(i) = 0.0;
+    } else {
+      std::normal_distribution distr{0.0, stdDevs[i]};
+      result(i) = distr(gen);
+    }
+  }
+  return result;
+}
+
+Eigen::MatrixXd MakeCovMatrix(const std::span<const double> stdDevs) {
+  Eigen::MatrixXd result{stdDevs.size(), stdDevs.size()};
+  result.setZero();
+
+  for (size_t i = 0; i < stdDevs.size(); ++i) {
+    result(i, i) = std::pow(stdDevs[i], 2);
+  }
+
+  return result;
+}
+
 }  // namespace frc

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

@@ -7,8 +7,8 @@
 #include <algorithm>
 #include <limits>
 
-#include <sleipnir/autodiff/Gradient.hpp>
-#include <sleipnir/autodiff/Hessian.hpp>
+#include <sleipnir/autodiff/gradient.hpp>
+#include <sleipnir/autodiff/hessian.hpp>
 
 #include "frc/EigenCore.h"
 #include "frc/system/NumericalIntegration.h"
@@ -18,7 +18,7 @@ using namespace frc;
 units::volt_t ArmFeedforward::Calculate(
     units::unit_t<Angle> currentAngle, units::unit_t<Velocity> currentVelocity,
     units::unit_t<Velocity> nextVelocity) const {
-  using VarMat = sleipnir::VariableMatrix;
+  using VarMat = slp::VariableMatrix;
 
   // Small kₐ values make the solver ill-conditioned
   if (kA < units::unit_t<ka_unit>{1e-1}) {
@@ -32,37 +32,37 @@ units::volt_t ArmFeedforward::Calculate(
   Matrixd<2, 1> B{{0.0}, {1.0 / kA.value()}};
   const auto& f = [&](const VarMat& x, const VarMat& u) -> VarMat {
     VarMat c{{0.0},
-             {-(kS / kA).value() * sleipnir::sign(x(1)) -
-              (kG / kA).value() * sleipnir::cos(x(0))}};
+             {-(kS / kA).value() * slp::sign(x[1]) -
+              (kG / kA).value() * slp::cos(x[0])}};
     return A * x + B * u + c;
   };
 
   Vectord<2> r_k{currentAngle.value(), currentVelocity.value()};
 
-  sleipnir::Variable u_k;
+  slp::Variable u_k;
 
   // Initial guess
   auto acceleration = (nextVelocity - currentVelocity) / m_dt;
-  u_k.SetValue((kS * wpi::sgn(currentVelocity.value()) + kV * currentVelocity +
-                kA * acceleration + kG * units::math::cos(currentAngle))
-                   .value());
+  u_k.set_value((kS * wpi::sgn(currentVelocity.value()) + kV * currentVelocity +
+                 kA * acceleration + kG * units::math::cos(currentAngle))
+                    .value());
 
   auto r_k1 = RK4<decltype(f), VarMat, VarMat>(f, r_k, u_k, m_dt);
 
   // Minimize difference between desired and actual next velocity
   auto cost =
-      (nextVelocity.value() - r_k1(1)) * (nextVelocity.value() - r_k1(1));
+      (nextVelocity.value() - r_k1[1]) * (nextVelocity.value() - r_k1[1]);
 
   // Refine solution via Newton's method
   {
     auto xAD = u_k;
-    double x = xAD.Value();
+    double x = xAD.value();
 
-    sleipnir::Gradient gradientF{cost, xAD};
-    Eigen::SparseVector<double> g = gradientF.Value();
+    slp::Gradient gradientF{cost, xAD};
+    Eigen::SparseVector<double> g = gradientF.value();
 
-    sleipnir::Hessian hessianF{cost, xAD};
-    Eigen::SparseMatrix<double> H = hessianF.Value();
+    slp::Hessian hessianF{cost, xAD};
+    Eigen::SparseMatrix<double> H = hessianF.value();
 
     double error_k = std::numeric_limits<double>::infinity();
     double error_k1 = std::abs(g.coeff(0));
@@ -81,31 +81,31 @@ units::volt_t ArmFeedforward::Calculate(
 
       // Shrink step until cost goes down
       {
-        double oldCost = cost.Value();
+        double oldCost = cost.value();
 
         double α = 1.0;
         double trial_x = x + α * p_x;
 
-        xAD.SetValue(trial_x);
+        xAD.set_value(trial_x);
 
-        while (cost.Value() > oldCost) {
+        while (cost.value() > oldCost) {
           α *= 0.5;
           trial_x = x + α * p_x;
 
-          xAD.SetValue(trial_x);
+          xAD.set_value(trial_x);
         }
 
         x = trial_x;
       }
 
-      xAD.SetValue(x);
+      xAD.set_value(x);
 
-      g = gradientF.Value();
-      H = hessianF.Value();
+      g = gradientF.value();
+      H = hessianF.value();
 
       error_k1 = std::abs(g.coeff(0));
     }
   }
 
-  return units::volt_t{u_k.Value()};
+  return units::volt_t{u_k.value()};
 }

wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp

@@ -4,7 +4,7 @@
 
 #include "frc/geometry/Ellipse2d.h"
 
-#include <sleipnir/optimization/OptimizationProblem.hpp>
+#include <sleipnir/optimization/problem.hpp>
 
 using namespace frc;
 
@@ -20,31 +20,29 @@ Translation2d Ellipse2d::Nearest(const Translation2d& point) const {
 
   // Find nearest point
   {
-    namespace slp = sleipnir;
-
-    sleipnir::OptimizationProblem problem;
+    slp::Problem problem;
 
     // Point on ellipse
-    auto x = problem.DecisionVariable();
-    x.SetValue(rotPoint.X().value());
-    auto y = problem.DecisionVariable();
-    y.SetValue(rotPoint.Y().value());
+    auto x = problem.decision_variable();
+    x.set_value(rotPoint.X().value());
+    auto y = problem.decision_variable();
+    y.set_value(rotPoint.Y().value());
 
-    problem.Minimize(slp::pow(x - rotPoint.X().value(), 2) +
+    problem.minimize(slp::pow(x - rotPoint.X().value(), 2) +
                      slp::pow(y - rotPoint.Y().value(), 2));
 
     // (x − x_c)²/a² + (y − y_c)²/b² = 1
     // b²(x − x_c)² + a²(y − y_c)² = a²b²
     double a2 = m_xSemiAxis.value() * m_xSemiAxis.value();
     double b2 = m_ySemiAxis.value() * m_ySemiAxis.value();
-    problem.SubjectTo(b2 * slp::pow(x - m_center.X().value(), 2) +
-                          a2 * slp::pow(y - m_center.Y().value(), 2) ==
-                      a2 * b2);
+    problem.subject_to(b2 * slp::pow(x - m_center.X().value(), 2) +
+                           a2 * slp::pow(y - m_center.Y().value(), 2) ==
+                       a2 * b2);
 
-    problem.Solve();
+    problem.solve();
 
-    rotPoint = frc::Translation2d{units::meter_t{x.Value()},
-                                  units::meter_t{y.Value()}};
+    rotPoint = frc::Translation2d{units::meter_t{x.value()},
+                                  units::meter_t{y.value()}};
   }
 
   // Undo rotation