diff --git a/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp b/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp index df1f89d984..fbfdde2227 100644 --- a/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp +++ b/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp @@ -64,8 +64,14 @@ units::volt_t ArmFeedforward::Calculate( sleipnir::Hessian hessianF{cost, xAD}; Eigen::SparseMatrix H = hessianF.Value(); - double error = std::numeric_limits::infinity(); - while (error > 1e-8) { + double error_k = std::numeric_limits::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)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java index 3c8748d35f..b18b81641e 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java @@ -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 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 A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0.0, 1.0, 0.0, -kv / ka}); final Matrix 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)), diff --git a/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp index 66060c2020..157b10da75 100644 --- a/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp @@ -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); }