diff --git a/wpimath/CMakeLists.txt b/wpimath/CMakeLists.txt index 0d11252619..ca49610175 100644 --- a/wpimath/CMakeLists.txt +++ b/wpimath/CMakeLists.txt @@ -19,6 +19,7 @@ protobuf_generate_cpp( file( GLOB wpimath_jni_src + src/main/native/cpp/jni/WPIMathJNI_ArmFeedforward.cpp src/main/native/cpp/jni/WPIMathJNI_DARE.cpp src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java index a60bcbae5f..412c1b1f74 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java +++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java @@ -44,6 +44,33 @@ public final class WPIMathJNI { libraryLoaded = true; } + // ArmFeedforward wrappers + + /** + * Obtain a feedforward voltage from a single jointed arm feedforward object. + * + *

Constructs an ArmFeedforward object and runs its currentVelocity and nextVelocity overload + * + * @param ks The ArmFeedforward's static gain in volts. + * @param kv The ArmFeedforward's velocity gain in volt seconds per radian. + * @param ka The ArmFeedforward's acceleration gain in volt seconds² per radian. + * @param kg The ArmFeedforward's gravity gain in volts. + * @param currentAngle The current angle in the calculation in radians. + * @param currentVelocity The current velocity in the calculation in radians per second. + * @param nextVelocity The next velocity in the calculation in radians per second. + * @param dt The time between velocity setpoints in seconds. + * @return The calculated feedforward in volts. + */ + public static native double calculate( + double ks, + double kv, + double ka, + double kg, + double currentAngle, + double currentVelocity, + double nextVelocity, + double dt); + // DARE wrappers /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java index 0d8b11b465..0d47941806 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java @@ -4,6 +4,7 @@ package edu.wpi.first.math.controller; +import edu.wpi.first.math.WPIMathJNI; import edu.wpi.first.math.controller.proto.ArmFeedforwardProto; import edu.wpi.first.math.controller.struct.ArmFeedforwardStruct; import edu.wpi.first.util.protobuf.ProtobufSerializable; @@ -100,6 +101,22 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable return calculate(positionRadians, velocity, 0); } + /** + * Calculates the feedforward from the gains and setpoints. + * + * @param currentAngle The current angle in radians. This angle should be measured from the + * horizontal (i.e. if the provided angle is 0, the arm should be parallel to the floor). If + * your encoder does not follow this convention, an offset should be added. + * @param currentVelocity The current velocity setpoint in radians per second. + * @param nextVelocity The next velocity setpoint in radians per second. + * @param dt Time between velocity setpoints in seconds. + * @return The computed feedforward in volts. + */ + public double calculate( + double currentAngle, double currentVelocity, double nextVelocity, double dt) { + return WPIMathJNI.calculate(ks, kv, ka, kg, currentAngle, currentVelocity, nextVelocity, dt); + } + // Rearranging the main equation from the calculate() method yields the // formulas for the methods below: diff --git a/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp b/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp new file mode 100644 index 0000000000..3c677c0267 --- /dev/null +++ b/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp @@ -0,0 +1,100 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/controller/ArmFeedforward.h" + +#include + +#include +#include + +#include "frc/EigenCore.h" +#include "frc/system/NumericalIntegration.h" + +using namespace frc; + +units::volt_t ArmFeedforward::Calculate(units::unit_t currentAngle, + units::unit_t currentVelocity, + units::unit_t nextVelocity, + units::second_t dt) const { + using VarMat = sleipnir::VariableMatrix; + + // Arm dynamics + Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}}; + 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))}}; + return A * x + B * u + c; + }; + + Vectord<2> r_k{currentAngle.value(), currentVelocity.value()}; + + sleipnir::Variable u_k; + + // Initial guess + auto acceleration = (nextVelocity - currentVelocity) / dt; + u_k.SetValue((kS * wpi::sgn(currentVelocity.value()) + kV * currentVelocity + + kA * acceleration + kG * units::math::cos(currentAngle)) + .value()); + + auto r_k1 = RK4(f, r_k, u_k, dt); + + // Minimize difference between desired and actual next velocity + auto cost = + (nextVelocity.value() - r_k1(1)) * (nextVelocity.value() - r_k1(1)); + + // Refine solution via Newton's method + { + auto xAD = u_k; + double x = xAD.Value(); + + sleipnir::Gradient gradientF{cost, xAD}; + Eigen::SparseVector g = gradientF.Value(); + + sleipnir::Hessian hessianF{cost, xAD}; + Eigen::SparseMatrix H = hessianF.Value(); + + double error = std::numeric_limits::infinity(); + while (error > 1e-8) { + // Iterate via Newton's method. + // + // xₖ₊₁ = xₖ − H⁻¹g + // + // The Hessian is regularized to at least 1e-4. + double p_x = -g.coeff(0) / std::max(H.coeff(0, 0), 1e-4); + + // Shrink step until cost goes down + { + double oldCost = cost.Value(); + + double α = 1.0; + double trial_x = x + α * p_x; + + xAD.SetValue(trial_x); + cost.Update(); + + while (cost.Value() > oldCost) { + α *= 0.5; + trial_x = x + α * p_x; + + xAD.SetValue(trial_x); + cost.Update(); + } + + x = trial_x; + } + + xAD.SetValue(x); + + g = gradientF.Value(); + H = hessianF.Value(); + + error = std::abs(g.coeff(0)); + } + } + + return units::volt_t{u_k.Value()}; +} diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_ArmFeedforward.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_ArmFeedforward.cpp new file mode 100644 index 0000000000..3428c52688 --- /dev/null +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_ArmFeedforward.cpp @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include + +#include "edu_wpi_first_math_WPIMathJNI.h" +#include "frc/controller/ArmFeedforward.h" + +using namespace wpi::java; + +extern "C" { + +/* + * Class: edu_wpi_first_math_WPIMathJNI + * Method: calculate + * Signature: (DDDDDDDD)D + */ +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_math_WPIMathJNI_calculate + (JNIEnv* env, jclass, jdouble ks, jdouble kv, jdouble ka, jdouble kg, + jdouble currentAngle, jdouble currentVelocity, jdouble nextVelocity, + jdouble dt) +{ + return frc::ArmFeedforward{units::volt_t{ks}, units::volt_t{kg}, + units::unit_t{kv}, + units::unit_t{ka}} + .Calculate(units::radian_t{currentAngle}, + units::radians_per_second_t{currentVelocity}, + units::radians_per_second_t{nextVelocity}, units::second_t{dt}) + .value(); +} + +} // extern "C" diff --git a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h index 69091ccd9a..28713003c9 100644 --- a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h @@ -76,6 +76,23 @@ class WPILIB_DLLEXPORT ArmFeedforward { kV * velocity + kA * acceleration; } + /** + * Calculates the feedforward from the gains and setpoints. + * + * @param currentAngle The current angle in radians. This angle should be + * measured from the horizontal (i.e. if the provided angle is 0, the arm + * should be parallel to the floor). If your encoder does not follow this + * convention, an offset should be added. + * @param currentVelocity The current velocity setpoint in radians per second. + * @param nextVelocity The next velocity setpoint in radians per second. + * @param dt Time between velocity setpoints in seconds. + * @return The computed feedforward in volts. + */ + units::volt_t Calculate(units::unit_t currentAngle, + units::unit_t currentVelocity, + units::unit_t nextVelocity, + units::second_t dt) const; + // 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/ArmFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java index 7b5cf77380..16d0269d0e 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 @@ -6,6 +6,13 @@ package edu.wpi.first.math.controller; import static org.junit.jupiter.api.Assertions.assertEquals; +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.NumericalIntegration; +import java.util.function.BiFunction; import org.junit.jupiter.api.Test; class ArmFeedforwardTest { @@ -15,12 +22,69 @@ class ArmFeedforwardTest { 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 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. + * @return The final state as a 2-vector of angle and angular velocity. + */ + private Matrix simulate( + 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}); + + final BiFunction, Matrix, Matrix> f = + (x, u) -> { + Matrix c = + MatBuilder.fill( + Nat.N2(), + Nat.N1(), + 0.0, + Math.signum(x.get(1, 0)) * (-ks / ka) - (kg / ka) * Math.cos(x.get(0, 0))); + return A.times(x).plus(B.times(u)).plus(c); + }; + + return NumericalIntegration.rk4( + f, + MatBuilder.fill(Nat.N2(), Nat.N1(), currentAngle, currentVelocity), + MatBuilder.fill(Nat.N1(), Nat.N1(), input), + dt); + } + + /** + * Calculates a feedforward voltage using overload taking angle, two angular velocities, and dt; + * then simulates an arm using that voltage to verify correctness. + * + * @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.calculate(currentAngle, currentVelocity, nextVelocity, dt); + assertEquals(nextVelocity, simulate(currentAngle, currentVelocity, input, dt).get(1, 0), 1e-12); + } + @Test void testCalculate() { + // calculate(angle, angular velocity) assertEquals(0.5, m_armFF.calculate(Math.PI / 3, 0), 0.002); assertEquals(2.5, m_armFF.calculate(Math.PI / 3, 1), 0.002); + + // calculate(angle, angular velocity, angular acceleration) assertEquals(6.5, m_armFF.calculate(Math.PI / 3, 1, 2), 0.002); assertEquals(2.5, m_armFF.calculate(Math.PI / 3, -1, 2), 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); } @Test diff --git a/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp index b7853b19ba..615f10ae89 100644 --- a/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp @@ -7,46 +7,111 @@ #include +#include "frc/EigenCore.h" #include "frc/controller/ArmFeedforward.h" -#include "units/acceleration.h" -#include "units/length.h" +#include "frc/system/NumericalIntegration.h" +#include "units/angular_acceleration.h" +#include "units/angular_velocity.h" #include "units/time.h" +#include "units/voltage.h" static constexpr auto Ks = 0.5_V; -static constexpr auto Kv = 1.5_V * 1_s / 1_rad; -static constexpr auto Ka = 2_V * 1_s * 1_s / 1_rad; +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 { + +/** + * Simulates a single-jointed arm and returns the final state. + * + * @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, + 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()}}; + + return frc::RK4( + [&](const frc::Matrixd<2, 1>& x, + const frc::Matrixd<1, 1>& u) -> frc::Matrixd<2, 1> { + frc::Matrixd<2, 1> c{0.0, -Ks.value() / Ka.value() * wpi::sgn(x(1)) - + Kg.value() / Ka.value() * std::cos(x(0))}; + return A * x + B * u + c; + }, + frc::Matrixd<2, 1>{currentAngle.value(), currentVelocity.value()}, + frc::Matrixd<1, 1>{input.value()}, dt); +} + +/** + * Simulates a single-jointed arm and returns the final state. + * + * @param armFF The feedforward object. + * @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, + 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, dt); + EXPECT_NEAR(nextVelocity.value(), + Simulate(currentAngle, currentVelocity, input, dt)(1), 1e-12); +} + +} // namespace + TEST(ArmFeedforwardTest, Calculate) { frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka}; + + // Calculate(angle, angular velocity) EXPECT_NEAR( - armFF.Calculate(std::numbers::pi * 1_rad / 3, 0_rad / 1_s).value(), 0.5, + armFF.Calculate(std::numbers::pi / 3 * 1_rad, 0_rad_per_s).value(), 0.5, 0.002); EXPECT_NEAR( - armFF.Calculate(std::numbers::pi * 1_rad / 3, 1_rad / 1_s).value(), 2.5, + armFF.Calculate(std::numbers::pi / 3 * 1_rad, 1_rad_per_s).value(), 2.5, 0.002); - EXPECT_NEAR(armFF - .Calculate(std::numbers::pi * 1_rad / 3, 1_rad / 1_s, - 2_rad / 1_s / 1_s) - .value(), - 6.5, 0.002); - EXPECT_NEAR(armFF - .Calculate(std::numbers::pi * 1_rad / 3, -1_rad / 1_s, - 2_rad / 1_s / 1_s) - .value(), - 2.5, 0.002); + + // Calculate(angle, angular velocity, angular acceleration) + EXPECT_NEAR( + armFF.Calculate(std::numbers::pi / 3 * 1_rad, 1_rad_per_s, 2_rad_per_s_sq) + .value(), + 6.5, 0.002); + EXPECT_NEAR( + armFF + .Calculate(std::numbers::pi / 3 * 1_rad, -1_rad_per_s, 2_rad_per_s_sq) + .value(), + 2.5, 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); } TEST(ArmFeedforwardTest, AchievableVelocity) { frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka}; EXPECT_NEAR(armFF - .MaxAchievableVelocity(12_V, std::numbers::pi * 1_rad / 3, - 1_rad / 1_s / 1_s) + .MaxAchievableVelocity(12_V, std::numbers::pi / 3 * 1_rad, + 1_rad_per_s_sq) .value(), 6, 0.002); EXPECT_NEAR(armFF - .MinAchievableVelocity(11.5_V, std::numbers::pi * 1_rad / 3, - 1_rad / 1_s / 1_s) + .MinAchievableVelocity(11.5_V, std::numbers::pi / 3 * 1_rad, + 1_rad_per_s_sq) .value(), -9, 0.002); } @@ -54,23 +119,23 @@ TEST(ArmFeedforwardTest, AchievableVelocity) { TEST(ArmFeedforwardTest, AchievableAcceleration) { frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka}; EXPECT_NEAR(armFF - .MaxAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3, - 1_rad / 1_s) + .MaxAchievableAcceleration(12_V, std::numbers::pi / 3 * 1_rad, + 1_rad_per_s) .value(), 4.75, 0.002); EXPECT_NEAR(armFF - .MaxAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3, - -1_rad / 1_s) + .MaxAchievableAcceleration(12_V, std::numbers::pi / 3 * 1_rad, + -1_rad_per_s) .value(), 6.75, 0.002); EXPECT_NEAR(armFF - .MinAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3, - 1_rad / 1_s) + .MinAchievableAcceleration(12_V, std::numbers::pi / 3 * 1_rad, + 1_rad_per_s) .value(), -7.25, 0.002); EXPECT_NEAR(armFF - .MinAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3, - -1_rad / 1_s) + .MinAchievableAcceleration(12_V, std::numbers::pi / 3 * 1_rad, + -1_rad_per_s) .value(), -5.25, 0.002); }