Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +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()};
}