diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index 9f4dd863ff..cfe7c2b892 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java @@ -4,6 +4,10 @@ package edu.wpi.first.math.controller; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.system.plant.LinearSystemId; + /** * A helper class that computes feedforward outputs for a simple elevator (modeled as a motor acting * against the force of gravity). @@ -53,6 +57,48 @@ public class ElevatorFeedforward { return ks * Math.signum(velocity) + kg + kv * velocity + ka * acceleration; } + /** + * Calculates the feedforward from the gains and setpoints. + * + *

Note this method is inaccurate when the velocity crosses 0. + * + * @param currentVelocity The current velocity setpoint. + * @param nextVelocity The next velocity setpoint. + * @param dtSeconds Time between velocity setpoints in seconds. + * @return The computed feedforward. + */ + public double calculate(double currentVelocity, double nextVelocity, double dtSeconds) { + // Discretize the affine model. + // + // dx/dt = Ax + Bu + c + // dx/dt = Ax + B(u + B⁺c) + // xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ) + // xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ) + // xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ + // + // Solve for uₖ. + // + // B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ) + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ + // + // For an elevator with the model + // dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x), + // A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka sgn(x)). Substitute in B + // assuming sgn(x) is a constant for the duration of the step. + // + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka sgn(x))) + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka sgn(x)) + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks sgn(x) + var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka); + var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds); + + var r = Matrix.mat(Nat.N1(), Nat.N1()).fill(currentVelocity); + var nextR = Matrix.mat(Nat.N1(), Nat.N1()).fill(nextVelocity); + + return kg + ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0); + } + /** * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be * zero). diff --git a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h index bbe7720686..62a7bad867 100644 --- a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h @@ -6,6 +6,9 @@ #include +#include "frc/EigenCore.h" +#include "frc/controller/LinearPlantInversionFeedforward.h" +#include "frc/system/plant/LinearSystemId.h" #include "units/length.h" #include "units/time.h" #include "units/voltage.h" @@ -54,6 +57,50 @@ class ElevatorFeedforward { return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration; } + /** + * Calculates the feedforward from the gains and setpoints. + * + * @param currentVelocity The current velocity setpoint, in distance per + * second. + * @param nextVelocity The next velocity setpoint, in distance per second. + * @param dt Time between velocity setpoints in seconds. + * @return The computed feedforward, in volts. + */ + units::volt_t Calculate(units::unit_t currentVelocity, + units::unit_t nextVelocity, + units::second_t dt) const { + // Discretize the affine model. + // + // dx/dt = Ax + Bu + c + // dx/dt = Ax + B(u + B⁺c) + // xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ) + // xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ) + // xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ + // + // Solve for uₖ. + // + // B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ) + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ + // + // For an elevator with the model + // dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x), + // A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka sgn(x)). Substitute in B + // assuming sgn(x) is a constant for the duration of the step. + // + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka sgn(x))) + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka sgn(x)) + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks sgn(x) + auto plant = LinearSystemId::IdentifyVelocitySystem(kV, kA); + LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt}; + + Vectord<1> r{currentVelocity.value()}; + Vectord<1> nextR{nextVelocity.value()}; + + return kG + kS * wpi::sgn(currentVelocity.value()) + + units::volt_t{feedforward.Calculate(r, nextR)(0)}; + } + // Rearranging the main equation from the calculate() method yields the // formulas for the methods below: diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java index c701f38d1b..6c978ac986 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java @@ -6,6 +6,10 @@ package edu.wpi.first.math.controller; import static org.junit.jupiter.api.Assertions.assertEquals; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.numbers.N1; import org.junit.jupiter.api.Test; class ElevatorFeedforwardTest { @@ -22,6 +26,18 @@ class ElevatorFeedforwardTest { assertEquals(4.5, m_elevatorFF.calculate(2), 0.002); assertEquals(6.5, m_elevatorFF.calculate(2, 1), 0.002); assertEquals(-0.5, m_elevatorFF.calculate(-2, 1), 0.002); + + var A = Matrix.mat(Nat.N1(), Nat.N1()).fill(-kv / ka); + var B = Matrix.mat(Nat.N1(), Nat.N1()).fill(1.0 / ka); + final double dt = 0.02; + var plantInversion = new LinearPlantInversionFeedforward(A, B, dt); + + var r = VecBuilder.fill(2.0); + var nextR = VecBuilder.fill(3.0); + assertEquals( + plantInversion.calculate(r, nextR).get(0, 0) + ks + kg, + m_elevatorFF.calculate(2.0, 3.0, dt), + 0.002); } @Test diff --git a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp index bf2c1ba3b0..a2db70dfd5 100644 --- a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp @@ -6,7 +6,9 @@ #include +#include "frc/EigenCore.h" #include "frc/controller/ElevatorFeedforward.h" +#include "frc/controller/LinearPlantInversionFeedforward.h" #include "units/acceleration.h" #include "units/length.h" #include "units/time.h" @@ -18,12 +20,23 @@ static constexpr auto Kg = 1_V; TEST(ElevatorFeedforwardTest, Calculate) { frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka}; + EXPECT_NEAR(elevatorFF.Calculate(0_m / 1_s).value(), Kg.value(), 0.002); EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s).value(), 4.5, 0.002); EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s, 1_m / 1_s / 1_s).value(), 6.5, 0.002); EXPECT_NEAR(elevatorFF.Calculate(-2_m / 1_s, 1_m / 1_s / 1_s).value(), -0.5, 0.002); + + frc::Matrixd<1, 1> A{-Kv.value() / Ka.value()}; + frc::Matrixd<1, 1> B{1.0 / Ka.value()}; + constexpr units::second_t dt = 20_ms; + frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt}; + + frc::Vectord<1> r{2.0}; + frc::Vectord<1> nextR{3.0}; + EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value() + Kg.value(), + elevatorFF.Calculate(2_mps, 3_mps, dt).value(), 0.002); } TEST(ElevatorFeedforwardTest, AchievableVelocity) {