Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[upstream_utils] Upgrade Sleipnir (#7973)

Browse Source
This commit is contained in:
Tyler Veness
2025-05-27 07:24:15 -07:00
committed by GitHub
parent 5368e8c6ed
commit de718f7ae5
90 changed files with 11188 additions and 7917 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp
View File

@@ -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()};
}

28
wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp
View File

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