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

119
wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java
View File

@@ -18,15 +18,13 @@ import java.util.function.BiFunction;
import org.junit.jupiter.api.Test;
class ArmFeedforwardTest {
private static final double ks = 0.5;
private static final double kg = 1;
private static final double kv = 1.5;
private static final double ka = 2;
private final ArmFeedforward m_armFF = new ArmFeedforward(ks, kg, kv, ka);
/**
* Simulates a single-jointed arm and returns the final state.
*
* @param ks The static gain, in volts.
* @param kv The velocity gain, in volt seconds per radian.
* @param ka The acceleration gain, in volt seconds² per radian.
* @param kg The gravity gain, in volts.
* @param currentAngle The starting angle in radians.
* @param currentVelocity The starting angular velocity in radians per second.
* @param input The input voltage.
@@ -34,7 +32,14 @@ class ArmFeedforwardTest {
* @return The final state as a 2-vector of angle and angular velocity.
*/
private Matrix<N2, N1> simulate(
double currentAngle, double currentVelocity, double input, double dt) {
double ks,
double kv,
double ka,
double kg,
double currentAngle,
double currentVelocity,
double input,
double dt) {
final Matrix<N2, N2> A =
new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0.0, 1.0, 0.0, -kv / ka});
final Matrix<N2, N1> B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0.0, 1.0 / ka});
@@ -61,47 +66,115 @@ class ArmFeedforwardTest {
* Calculates a feedforward voltage using overload taking angle, two angular velocities, and dt;
* then simulates an arm using that voltage to verify correctness.
*
* @param armFF The feedforward object.
* @param ks The static gain, in volts.
* @param kv The velocity gain, in volt seconds per radian.
* @param ka The acceleration gain, in volt seconds² per radian.
* @param kg The gravity gain, in volts.
* @param currentAngle The starting angle in radians.
* @param currentVelocity The starting angular velocity in radians per second.
* @param input The input voltage.
* @param dt The simulation time in seconds.
*/
private void calculateAndSimulate(
double currentAngle, double currentVelocity, double nextVelocity, double dt) {
final double input =
m_armFF.calculateWithVelocities(currentAngle, currentVelocity, nextVelocity);
assertEquals(nextVelocity, simulate(currentAngle, currentVelocity, input, dt).get(1, 0), 1e-12);
ArmFeedforward armFF,
double ks,
double kv,
double ka,
double kg,
double currentAngle,
double currentVelocity,
double nextVelocity,
double dt) {
final double input = armFF.calculateWithVelocities(currentAngle, currentVelocity, nextVelocity);
assertEquals(
nextVelocity,
simulate(ks, kv, ka, kg, currentAngle, currentVelocity, input, dt).get(1, 0),
1e-4);
}
@Test
void testCalculate() {
final double ks = 0.5;
final double kv = 1.5;
final double ka = 2;
final double kg = 1;
final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
// calculate(angle, angular velocity)
assertEquals(0.5, m_armFF.calculate(Math.PI / 3, 0.0), 0.002);
assertEquals(2.5, m_armFF.calculate(Math.PI / 3, 1.0), 0.002);
assertEquals(0.5, armFF.calculate(Math.PI / 3, 0.0), 0.002);
assertEquals(2.5, armFF.calculate(Math.PI / 3, 1.0), 0.002);
// calculate(currentAngle, currentVelocity, nextAngle, dt)
calculateAndSimulate(Math.PI / 3, 1.0, 1.05, 0.020);
calculateAndSimulate(Math.PI / 3, 1.0, 0.95, 0.020);
calculateAndSimulate(-Math.PI / 3, 1.0, 1.05, 0.020);
calculateAndSimulate(-Math.PI / 3, 1.0, 0.95, 0.020);
calculateAndSimulate(armFF, ks, kv, ka, kg, Math.PI / 3, 1.0, 1.05, 0.020);
calculateAndSimulate(armFF, ks, kv, ka, kg, Math.PI / 3, 1.0, 0.95, 0.020);
calculateAndSimulate(armFF, ks, kv, ka, kg, -Math.PI / 3, 1.0, 1.05, 0.020);
calculateAndSimulate(armFF, ks, kv, ka, kg, -Math.PI / 3, 1.0, 0.95, 0.020);
}
@Test
void testCalculateIllConditionedModel() {
final double ks = 0.39671;
final double kv = 2.7167;
final double ka = 1e-2;
final double kg = 0.2708;
final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
final double currentAngle = 1.0;
final double currentVelocity = 0.02;
final double nextVelocity = 0.0;
var averageAccel = (nextVelocity - currentVelocity) / 0.02;
assertEquals(
armFF.calculateWithVelocities(currentAngle, currentVelocity, nextVelocity),
ks + kv * currentVelocity + ka * averageAccel + kg * Math.cos(currentAngle));
}
@Test
void testCalculateIllConditionedGradient() {
final double ks = 0.39671;
final double kv = 2.7167;
final double ka = 0.50799;
final double kg = 0.2708;
final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
calculateAndSimulate(armFF, ks, kv, ka, kg, 1.0, 0.02, 0.0, 0.02);
}
@Test
void testAchievableVelocity() {
assertEquals(6, m_armFF.maxAchievableVelocity(12, Math.PI / 3, 1), 0.002);
assertEquals(-9, m_armFF.minAchievableVelocity(11.5, Math.PI / 3, 1), 0.002);
final double ks = 0.5;
final double kv = 1.5;
final double ka = 2;
final double kg = 1;
final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
assertEquals(6, armFF.maxAchievableVelocity(12, Math.PI / 3, 1), 0.002);
assertEquals(-9, armFF.minAchievableVelocity(11.5, Math.PI / 3, 1), 0.002);
}
@Test
void testAchievableAcceleration() {
assertEquals(4.75, m_armFF.maxAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
assertEquals(6.75, m_armFF.maxAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
assertEquals(-7.25, m_armFF.minAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
assertEquals(-5.25, m_armFF.minAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
final double ks = 0.5;
final double kv = 1.5;
final double ka = 2;
final double kg = 1;
final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
assertEquals(4.75, armFF.maxAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
assertEquals(6.75, armFF.maxAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
assertEquals(-7.25, armFF.minAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
assertEquals(-5.25, armFF.minAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
}
@Test
void testNegativeGains() {
final double ks = 0.5;
final double kv = 1.5;
final double ka = 2;
final double kg = 1;
assertAll(
() ->
assertThrows(IllegalArgumentException.class, () -> new ArmFeedforward(ks, kg, -kv, ka)),

100
wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp
View File

@@ -15,27 +15,32 @@
#include "units/time.h"
#include "units/voltage.h"
static constexpr auto Ks = 0.5_V;
static constexpr auto Kv = 1.5_V / 1_rad_per_s;
static constexpr auto Ka = 2_V / 1_rad_per_s_sq;
static constexpr auto Kg = 1_V;
namespace {
using Ks_unit = decltype(1_V);
using Kv_unit = decltype(1_V / 1_rad_per_s);
using Ka_unit = decltype(1_V / 1_rad_per_s_sq);
using Kg_unit = decltype(1_V);
/**
* Simulates a single-jointed arm and returns the final state.
*
* @param Ks The static gain, in volts.
* @param Kv The velocity gain, in volt seconds per radian.
* @param Ka The acceleration gain, in volt seconds² per radian.
* @param Kg The gravity gain, in volts.
* @param currentAngle The starting angle.
* @param currentVelocity The starting angular velocity.
* @param input The input voltage.
* @param dt The simulation time.
* @return The final state as a 2-vector of angle and angular velocity.
*/
frc::Matrixd<2, 1> Simulate(units::radian_t currentAngle,
frc::Matrixd<2, 1> Simulate(Ks_unit Ks, Kv_unit Kv, Ka_unit Ka, Kg_unit Kg,
units::radian_t currentAngle,
units::radians_per_second_t currentVelocity,
units::volt_t input, units::second_t dt) {
constexpr frc::Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -Kv.value() / Ka.value()}};
constexpr frc::Matrixd<2, 1> B{{0.0}, {1.0 / Ka.value()}};
frc::Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -Kv.value() / Ka.value()}};
frc::Matrixd<2, 1> B{{0.0}, {1.0 / Ka.value()}};
return frc::RK4(
[&](const frc::Matrixd<2, 1>& x,
@@ -52,24 +57,35 @@ frc::Matrixd<2, 1> Simulate(units::radian_t currentAngle,
* Simulates a single-jointed arm and returns the final state.
*
* @param armFF The feedforward object.
* @param Ks The static gain, in volts.
* @param Kv The velocity gain, in volt seconds per radian.
* @param Ka The acceleration gain, in volt seconds² per radian.
* @param Kg The gravity gain, in volts.
* @param currentAngle The starting angle.
* @param currentVelocity The starting angular velocity.
* @param input The input voltage.
* @param dt The simulation time.
*/
void CalculateAndSimulate(const frc::ArmFeedforward& armFF,
void CalculateAndSimulate(const frc::ArmFeedforward& armFF, Ks_unit Ks,
Kv_unit Kv, Ka_unit Ka, Kg_unit Kg,
units::radian_t currentAngle,
units::radians_per_second_t currentVelocity,
units::radians_per_second_t nextVelocity,
units::second_t dt) {
auto input = armFF.Calculate(currentAngle, currentVelocity, nextVelocity);
EXPECT_NEAR(nextVelocity.value(),
Simulate(currentAngle, currentVelocity, input, dt)(1), 1e-12);
EXPECT_NEAR(
nextVelocity.value(),
Simulate(Ks, Kv, Ka, Kg, currentAngle, currentVelocity, input, dt)(1),
1e-4);
}
} // namespace
TEST(ArmFeedforwardTest, Calculate) {
constexpr auto Ks = 0.5_V;
constexpr auto Kv = 1.5_V / 1_rad_per_s;
constexpr auto Ka = 2_V / 1_rad_per_s_sq;
constexpr auto Kg = 1_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
// Calculate(angle, angular velocity)
@@ -81,18 +97,54 @@ TEST(ArmFeedforwardTest, Calculate) {
0.002);
// Calculate(currentAngle, currentVelocity, nextAngle, dt)
CalculateAndSimulate(armFF, std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
1.05_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
0.95_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, -std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
1.05_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, -std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
0.95_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, std::numbers::pi / 3 * 1_rad,
1_rad_per_s, 1.05_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, std::numbers::pi / 3 * 1_rad,
1_rad_per_s, 0.95_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, -std::numbers::pi / 3 * 1_rad,
1_rad_per_s, 1.05_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, -std::numbers::pi / 3 * 1_rad,
1_rad_per_s, 0.95_rad_per_s, 20_ms);
}
TEST(ArmFeedforwardTest, CalculateIllConditionedModel) {
constexpr auto Ks = 0.39671_V;
constexpr auto Kv = 2.7167_V / 1_rad_per_s;
constexpr auto Ka = 1e-2_V / 1_rad_per_s_sq;
constexpr auto Kg = 0.2708_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
constexpr auto currentAngle = 1_rad;
constexpr auto currentVelocity = 0.02_rad_per_s;
constexpr auto nextVelocity = 0_rad_per_s;
constexpr auto averageAccel = (nextVelocity - currentVelocity) / 20_ms;
EXPECT_DOUBLE_EQ(
armFF.Calculate(currentAngle, currentVelocity, nextVelocity).value(),
(Ks + Kv * currentVelocity + Ka * averageAccel +
Kg * units::math::cos(currentAngle))
.value());
}
TEST(ArmFeedforwardTest, CalculateIllConditionedGradient) {
constexpr auto Ks = 0.39671_V;
constexpr auto Kv = 2.7167_V / 1_rad_per_s;
constexpr auto Ka = 0.50799_V / 1_rad_per_s_sq;
constexpr auto Kg = 0.2708_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, 1_rad, 0.02_rad_per_s,
0_rad_per_s, 20_ms);
}
TEST(ArmFeedforwardTest, AchievableVelocity) {
constexpr auto Ks = 0.5_V;
constexpr auto Kv = 1.5_V / 1_rad_per_s;
constexpr auto Ka = 2_V / 1_rad_per_s_sq;
constexpr auto Kg = 1_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(armFF
.MaxAchievableVelocity(12_V, std::numbers::pi / 3 * 1_rad,
1_rad_per_s_sq)
@@ -106,7 +158,12 @@ TEST(ArmFeedforwardTest, AchievableVelocity) {
}
TEST(ArmFeedforwardTest, AchievableAcceleration) {
constexpr auto Ks = 0.5_V;
constexpr auto Kv = 1.5_V / 1_rad_per_s;
constexpr auto Ka = 2_V / 1_rad_per_s_sq;
constexpr auto Kg = 1_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(armFF
.MaxAchievableAcceleration(12_V, std::numbers::pi / 3 * 1_rad,
1_rad_per_s)
@@ -130,7 +187,12 @@ TEST(ArmFeedforwardTest, AchievableAcceleration) {
}
TEST(ArmFeedforwardTest, NegativeGains) {
constexpr auto Ks = 0.5_V;
constexpr auto Kv = 1.5_V / 1_rad_per_s;
constexpr auto Ka = 2_V / 1_rad_per_s_sq;
constexpr auto Kg = 1_V;
frc::ArmFeedforward armFF{Ks, Kg, -Kv, -Ka};
EXPECT_EQ(armFF.GetKv().value(), 0);
EXPECT_EQ(armFF.GetKa().value(), 0);
}