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

[wpimath] Fix another infinite loop in ArmFeedforward (#7823)

Browse Source
This commit is contained in:
Tyler Veness
2025-02-25 19:04:31 -08:00
committed by GitHub
parent b0e588fd49
commit 517344fe80
3 changed files with 186 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -64,8 +64,14 @@ units::volt_t ArmFeedforward::Calculate(
sleipnir::Hessian hessianF{cost, xAD};
Eigen::SparseMatrix<double> H = hessianF.Value();
double error = std::numeric_limits<double>::infinity();
while (error > 1e-8) {
double error_k = std::numeric_limits<double>::infinity();
double error_k1 = std::abs(g.coeff(0));
// Loop until error stops decreasing or max iterations is reached
for (size_t iteration = 0;
iteration < 50 && error_k1 < (1.0 - 1e-10) * error_k; ++iteration) {
error_k = error_k1;
// Iterate via Newton's method.
//
// xₖ₊₁ = xₖ − H⁻¹g
@@ -97,7 +103,7 @@ units::volt_t ArmFeedforward::Calculate(
g = gradientF.Value();
H = hessianF.Value();
error = std::abs(g.coeff(0));
error_k1 = std::abs(g.coeff(0));
}
}