From 0f7bf158165e3de0aa1a7461849aaa22aea7f18d Mon Sep 17 00:00:00 2001 From: Kavin Muralikrishnan Date: Mon, 11 Nov 2024 20:30:07 -0500 Subject: [PATCH] Implemented Tsitouras 5th order numerical integrator --- sysid/src/main/native/cpp/analysis/ArmSim.cpp | 2 +- .../simulation/DifferentialDrivetrainSim.cpp | 3 +- .../native/cpp/simulation/ElevatorSim.cpp | 2 +- .../cpp/simulation/SingleJointedArmSim.cpp | 2 +- .../DifferentialDrivetrainSimTest.cpp | 4 +- .../simulation/DifferentialDrivetrainSim.java | 2 +- .../first/wpilibj/simulation/ElevatorSim.java | 2 +- .../simulation/SingleJointedArmSim.java | 2 +- .../DifferentialDrivetrainSimTest.java | 4 +- .../math/system/NumericalIntegration.java | 318 ++++++++++++++++++ .../include/frc/system/NumericalIntegration.h | 182 +++++++++- .../LTVDifferentialDriveControllerTest.java | 2 +- .../math/system/NumericalIntegrationTest.java | 54 +++ .../LTVDifferentialDriveControllerTest.cpp | 6 +- .../native/cpp/system/DiscretizationTest.cpp | 12 +- .../cpp/system/NumericalIntegrationTest.cpp | 45 ++- 16 files changed, 619 insertions(+), 23 deletions(-) diff --git a/sysid/src/main/native/cpp/analysis/ArmSim.cpp b/sysid/src/main/native/cpp/analysis/ArmSim.cpp index a72f50569d1..2b3962b837f 100644 --- a/sysid/src/main/native/cpp/analysis/ArmSim.cpp +++ b/sysid/src/main/native/cpp/analysis/ArmSim.cpp @@ -42,7 +42,7 @@ void ArmSim::Update(units::volt_t voltage, units::second_t dt) { // small for ill-conditioned data (e.g., high velocities with sharp spikes in // acceleration). Eigen::Vector u{voltage.value()}; - m_x = frc::RKDP(f, m_x, u, dt, 0.25); + m_x = frc::Tsit5(f, m_x, u, dt, 0.25); } double ArmSim::GetPosition() const { diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp index 19f3ab6c7d3..c9dbddb7886 100644 --- a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp @@ -59,7 +59,8 @@ void DifferentialDrivetrainSim::SetGearing(double newGearing) { } void DifferentialDrivetrainSim::Update(units::second_t dt) { - m_x = RKDP([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x, m_u, dt); + m_x = + Tsit5([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x, m_u, dt); m_y = m_x + frc::MakeWhiteNoiseVector<7>(m_measurementStdDevs); } diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index 4553dc4d0a1..0e0bc4e885b 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -104,7 +104,7 @@ void ElevatorSim::SetInputVoltage(units::volt_t voltage) { Vectord<2> ElevatorSim::UpdateX(const Vectord<2>& currentXhat, const Vectord<1>& u, units::second_t dt) { - auto updatedXhat = RKDP( + auto updatedXhat = Tsit5( [&](const Vectord<2>& x, const Vectord<1>& u_) -> Vectord<2> { Vectord<2> xdot = m_plant.A() * x + m_plant.B() * u; diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp index f318d083000..23945eac774 100644 --- a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp @@ -112,7 +112,7 @@ Vectord<2> SingleJointedArmSim::UpdateX(const Vectord<2>& currentXhat, // f(x, u) = Ax + Bu + [0 α]ᵀ // f(x, u) = Ax + Bu + [0 3/2⋅g⋅cos(θ)/L]ᵀ - Vectord<2> updatedXhat = RKDP( + Vectord<2> updatedXhat = Tsit5( [&](const auto& x, const auto& u) -> Vectord<2> { Vectord<2> xdot = m_plant.A() * x + m_plant.B() * u; diff --git a/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp index 9ed23624de3..43d4fee5c03 100644 --- a/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp @@ -55,7 +55,7 @@ TEST(DifferentialDrivetrainSimTest, Convergence) { sim.Update(20_ms); // Update ground truth. - groundTruthX = frc::RKDP( + groundTruthX = frc::Tsit5( [&sim](const auto& x, const auto& u) -> frc::Vectord<7> { return sim.Dynamics(x, u); }, @@ -63,7 +63,7 @@ TEST(DifferentialDrivetrainSimTest, Convergence) { } // 2 inch tolerance is OK since our ground truth is an approximation of the - // ODE solution using RKDP anyway + // ODE solution using Tsit5 anyway EXPECT_NEAR(groundTruthX(0, 0), sim.GetPose().X().value(), 0.05); EXPECT_NEAR(groundTruthX(1, 0), sim.GetPose().Y().value(), 0.05); EXPECT_NEAR(groundTruthX(2, 0), sim.GetHeading().Radians().value(), 0.01); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java index 6e25b344393..6f78c77bd3e 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java @@ -148,7 +148,7 @@ public void setInputs(double leftVoltageVolts, double rightVoltageVolts) { * @param dtSeconds the time difference */ public void update(double dtSeconds) { - m_x = NumericalIntegration.rkdp(this::getDynamics, m_x, m_u, dtSeconds); + m_x = NumericalIntegration.tsit5(this::getDynamics, m_x, m_u, dtSeconds); m_y = m_x; if (m_measurementStdDevs != null) { m_y = m_y.plus(StateSpaceUtil.makeWhiteNoiseVector(m_measurementStdDevs)); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index 2f7ce3e768d..d9cbda4f50a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -235,7 +235,7 @@ public void setInputVoltage(double volts) { protected Matrix updateX(Matrix currentXhat, Matrix u, double dtSeconds) { // Calculate updated x-hat from Runge-Kutta. var updatedXhat = - NumericalIntegration.rkdp( + NumericalIntegration.tsit5( (x, _u) -> { Matrix xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u)); if (m_simulateGravity) { diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java index 7d7ae78284d..32eba10cb1a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java @@ -244,7 +244,7 @@ protected Matrix updateX(Matrix currentXhat, Matrix u, d // f(x, u) = Ax + Bu + [0 3/2⋅g⋅cos(θ)/L]ᵀ Matrix updatedXhat = - NumericalIntegration.rkdp( + NumericalIntegration.tsit5( (Matrix x, Matrix _u) -> { Matrix xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u)); if (m_simulateGravity) { diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java index 25bbdb59834..1889a606791 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java @@ -76,11 +76,11 @@ void testConvergence() { sim.update(0.020); // Update our ground truth - groundTruthX = NumericalIntegration.rkdp(sim::getDynamics, groundTruthX, voltages, 0.020); + groundTruthX = NumericalIntegration.tsit5(sim::getDynamics, groundTruthX, voltages, 0.020); } // 2 inch tolerance is OK since our ground truth is an approximation of the - // ODE solution using RKDP anyway + // ODE solution using tsit5 anyway assertEquals( groundTruthX.get(DifferentialDrivetrainSim.State.kX.value, 0), sim.getState(DifferentialDrivetrainSim.State.kX), diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java index 1dcddea8404..6ee23174ff1 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java @@ -144,8 +144,10 @@ public static Matrix rk4( * @param u The value u held constant over the integration period. * @param dtSeconds The time over which to integrate. * @return the integration of dx/dt = f(x, u) for dt. + * @deprecated Use tsit5() instead. */ @SuppressWarnings("overloads") + @Deprecated(forRemoval = true, since = "2025") public static Matrix rkdp( BiFunction, Matrix, Matrix> f, Matrix x, @@ -165,8 +167,10 @@ public static Matrix rkdp( * @param dtSeconds The time over which to integrate. * @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6. * @return the integration of dx/dt = f(x, u) for dt. + * @deprecated use tsit5() instead. */ @SuppressWarnings("overloads") + @Deprecated(forRemoval = true, since = "2025") public static Matrix rkdp( BiFunction, Matrix, Matrix> f, Matrix x, @@ -290,8 +294,10 @@ public static Matrix rkdp( * @param dtSeconds The time over which to integrate. * @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6. * @return the integration of dx/dt = f(x, u) for dt. + * @deprecated Use tsit5() instead */ @SuppressWarnings("overloads") + @Deprecated(forRemoval = true, since = "2025") public static Matrix rkdp( BiFunction, Matrix> f, double t, @@ -406,4 +412,316 @@ public static Matrix rkdp( return y; } + + /** + * Performs adaptive Dormand-Prince integration of dx/dt = f(x, u) for dt. By default, the max + * error is 1e-6. + * + * @param A Num representing the states of the system to integrate. + * @param A Num representing the inputs of the system to integrate. + * @param f The function to integrate. It must take two arguments x and u. + * @param x The initial value of x. + * @param u The value u held constant over the integration period. + * @param dtSeconds The time over which to integrate. + * @return the integration of dx/dt = f(x, u) for dt. + */ + @SuppressWarnings("overloads") + public static Matrix tsit5( + BiFunction, Matrix, Matrix> f, + Matrix x, + Matrix u, + double dtSeconds) { + return tsit5(f, x, u, dtSeconds, 1e-6); + } + + /** + * Performs adaptive Tsitouras 5th Order integration of dx/dt = f(x, u) for dt. + * + * @param A Num representing the states of the system to integrate. + * @param A Num representing the inputs of the system to integrate. + * @param f The function to integrate. It must take two arguments x and u. + * @param x The initial value of x. + * @param u The value u held constant over the integration period. + * @param dtSeconds The time over which to integrate. + * @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6. + * @return the integration of dx/dt = f(x, u) for dt. + */ + @SuppressWarnings("overloads") + public static Matrix tsit5( + BiFunction, Matrix, Matrix> f, + Matrix x, + Matrix u, + double dtSeconds, + double maxError) { + // See http://users.uoa.gr/~tsitourasc/RK54_new_v2.pdf Table 1 for the + // Butcher tableau the following arrays came from. + + // final double[6][6] + final double[][] A = { + {0.161}, + {-0.008480655492357, 0.3354806554923570}, + {2.897153057105494, -6.359448489975075, 4.362295432869581}, + {5.32586482843925895, -11.74888356406283, 7.495539342889836, -0.09249506636175525}, + { + 5.86145544294642038, + -12.92096931784711, + 8.159367898576159, + -0.07158497328140100, + -0.02826905039406838 + }, + { //a₇ᵢ = bᵢ, i = 1, 2, ··· , 6 + 0.09646076681806523, + 0.01, + 0.4798896504144996, + 1.379008574103742, + -3.290069515436081, + 2.324710524099774 + } + }; + + // final double[7] + final double[] b1 = { + 0.09646076681806523, + 0.01, + 0.4798896504144996, + 1.379008574103742, + -3.290069515436081, + 2.324710524099774, + 0.0 + }; + + // final double[7] + final double[] b2 = { + 0.001780011052226, + 0.000816434459657, + -0.007880878010262, + 0.144711007173263, + -0.582357165452555, + 0.458082105929187, + 1.0 / 66.0 + }; + + Matrix newX; + double truncationError; + + double dtElapsed = 0.0; + double h = dtSeconds; + + // Loop until we've gotten to our desired dt + while (dtElapsed < dtSeconds) { + do { + // Only allow us to advance up to the dt remaining + h = Math.min(h, dtSeconds - dtElapsed); + + var k1 = f.apply(x, u); + var k2 = f.apply(x.plus(k1.times(A[0][0]).times(h)), u); + var k3 = f.apply(x.plus(k1.times(A[1][0]).plus(k2.times(A[1][1])).times(h)), u); + var k4 = + f.apply( + x.plus(k1.times(A[2][0]).plus(k2.times(A[2][1])).plus(k3.times(A[2][2])).times(h)), + u); + var k5 = + f.apply( + x.plus( + k1.times(A[3][0]) + .plus(k2.times(A[3][1])) + .plus(k3.times(A[3][2])) + .plus(k4.times(A[3][3])) + .times(h)), + u); + var k6 = + f.apply( + x.plus( + k1.times(A[4][0]) + .plus(k2.times(A[4][1])) + .plus(k3.times(A[4][2])) + .plus(k4.times(A[4][3])) + .plus(k5.times(A[4][4])) + .times(h)), + u); + + // Since the final row of A and the array b1 have the same coefficients + // and k7 has no effect on newX, we can reuse the calculation. + newX = + x.plus( + k1.times(A[5][0]) + .plus(k2.times(A[5][1])) + .plus(k3.times(A[5][2])) + .plus(k4.times(A[5][3])) + .plus(k5.times(A[5][4])) + .plus(k6.times(A[5][5])) + .times(h)); + var k7 = f.apply(newX, u); + + truncationError = + (k1.times(b1[0] - b2[0]) + .plus(k2.times(b1[1] - b2[1])) + .plus(k3.times(b1[2] - b2[2])) + .plus(k4.times(b1[3] - b2[3])) + .plus(k5.times(b1[4] - b2[4])) + .plus(k6.times(b1[5] - b2[5])) + .plus(k7.times(b1[6] - b2[6])) + .times(h)) + .normF(); + + if (truncationError == 0.0) { + h = dtSeconds - dtElapsed; + } else { + h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0); + } + } while (truncationError > maxError); + + dtElapsed += h; + x = newX; + } + + return x; + } + + /** + * Performs adaptive Tsitouras 5th Order integration of dx/dt = f(t, y) for dt. + * + * @param Rows in y. + * @param Columns in y. + * @param f The function to integrate. It must take two arguments t and y. + * @param t The initial value of t. + * @param y The initial value of y. + * @param dtSeconds The time over which to integrate. + * @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6. + * @return the integration of dx/dt = f(x, u) for dt. + */ + @SuppressWarnings("overloads") + public static Matrix tsit5( + BiFunction, Matrix> f, + double t, + Matrix y, + double dtSeconds, + double maxError) { + // See http://users.uoa.gr/~tsitourasc/RK54_new_v2.pdf Table 1 for the + // Butcher tableau the following arrays came from. + + // final double[6][6] + final double[][] A = { + {0.161}, + {-0.008480655492357, 0.3354806554923570}, + {2.897153057105494, -6.359448489975075, 4.362295432869581}, + {5.32586482843925895, -11.74888356406283, 7.495539342889836, -0.09249506636175525}, + { + 5.86145544294642038, + -12.92096931784711, + 8.159367898576159, + -0.07158497328140100, + -0.02826905039406838 + }, + { //a₇ᵢ = bᵢ, i = 1, 2, ··· , 6 + 0.09646076681806523, + 0.01, + 0.4798896504144996, + 1.379008574103742, + -3.290069515436081, + 2.324710524099774 + } + }; + + // final double[7] + final double[] b1 = { + 0.09646076681806523, + 0.01, + 0.4798896504144996, + 1.379008574103742, + -3.290069515436081, + 2.324710524099774, + 0.0 + }; + + // final double[7] + final double[] b2 = { + 0.001780011052226, + 0.000816434459657, + -0.007880878010262, + 0.144711007173263, + -0.582357165452555, + 0.458082105929187, + 1.0 / 66.0 + }; + + // final double[6] + final double[] c = {0.161, 0.327, 0.9, 0.9800255409045097, 1.0, 1.0}; + + Matrix newY; + double truncationError; + + double dtElapsed = 0.0; + double h = dtSeconds; + + // Loop until we've gotten to our desired dt + while (dtElapsed < dtSeconds) { + do { + // Only allow us to advance up to the dt remaining + h = Math.min(h, dtSeconds - dtElapsed); + + var k1 = f.apply(t, y); + var k2 = f.apply(t + h * c[0], y.plus(k1.times(A[0][0]).times(h))); + var k3 = f.apply(t + h * c[1], y.plus(k1.times(A[1][0]).plus(k2.times(A[1][1])).times(h))); + var k4 = + f.apply( + t + h * c[2], + y.plus(k1.times(A[2][0]).plus(k2.times(A[2][1])).plus(k3.times(A[2][2])).times(h))); + var k5 = + f.apply( + t + h * c[3], + y.plus( + k1.times(A[3][0]) + .plus(k2.times(A[3][1])) + .plus(k3.times(A[3][2])) + .plus(k4.times(A[3][3])) + .times(h))); + var k6 = + f.apply( + t + h * c[4], + y.plus( + k1.times(A[4][0]) + .plus(k2.times(A[4][1])) + .plus(k3.times(A[4][2])) + .plus(k4.times(A[4][3])) + .plus(k5.times(A[4][4])) + .times(h))); + + // Since the final row of A and the array b1 have the same coefficients + // and k7 has no effect on newY, we can reuse the calculation. + newY = + y.plus( + k1.times(A[5][0]) + .plus(k2.times(A[5][1])) + .plus(k3.times(A[5][2])) + .plus(k4.times(A[5][3])) + .plus(k5.times(A[5][4])) + .plus(k6.times(A[5][5])) + .times(h)); + var k7 = f.apply(t + h * c[5], newY); + + truncationError = + (k1.times(b1[0] - b2[0]) + .plus(k2.times(b1[1] - b2[1])) + .plus(k3.times(b1[2] - b2[2])) + .plus(k4.times(b1[3] - b2[3])) + .plus(k5.times(b1[4] - b2[4])) + .plus(k6.times(b1[5] - b2[5])) + .plus(k7.times(b1[6] - b2[6])) + .times(h)) + .normF(); + + if (truncationError == 0.0) { + h = dtSeconds - dtElapsed; + } else { + h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0); + } + } while (truncationError > maxError); + + dtElapsed += h; + y = newY; + } + + return y; + } } diff --git a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h index 2fe1946f0d3..5b67a0bfdde 100644 --- a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h +++ b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h @@ -81,8 +81,10 @@ T RK4(F&& f, units::second_t t, T y, units::second_t dt) { * @param dt The time over which to integrate. * @param maxError The maximum acceptable truncation error. Usually a small * number like 1e-6. + * @deprecated Use Tsit5() instead */ template +[[deprecated("Use AprilTagFieldLayout::LoadField() instead")]] T RKDP(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) { // See https://en.wikipedia.org/wiki/Dormand%E2%80%93Prince_method for the // Butcher tableau the following arrays came from. @@ -96,7 +98,7 @@ T RKDP(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) { { 44.0 / 45.0, -56.0 / 15.0, 32.0 / 9.0}, {19372.0 / 6561.0, -25360.0 / 2187.0, 64448.0 / 6561.0, -212.0 / 729.0}, { 9017.0 / 3168.0, -355.0 / 33.0, 46732.0 / 5247.0, 49.0 / 176.0, -5103.0 / 18656.0}, - { 35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0, 11.0 / 84.0}}; + { 35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0, 11.0 / 84.0}}; // clang-format on constexpr std::array b1{ @@ -164,8 +166,10 @@ T RKDP(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) { * @param dt The time over which to integrate. * @param maxError The maximum acceptable truncation error. Usually a small * number like 1e-6. + * @deprecated Use Tsit5() instead */ template +[[deprecated("Use AprilTagFieldLayout::LoadField() instead")]] T RKDP(F&& f, units::second_t t, T y, units::second_t dt, double maxError = 1e-6) { // See https://en.wikipedia.org/wiki/Dormand%E2%80%93Prince_method for the @@ -237,4 +241,180 @@ T RKDP(F&& f, units::second_t t, T y, units::second_t dt, return y; } +/** + * Performs adaptive Tsitouras 5th Order integration of dx/dt = f(x, u) for dt. + * + * @param f The function to integrate. It must take two arguments x and + * u. + * @param x The initial value of x. + * @param u The value u held constant over the integration period. + * @param dt The time over which to integrate. + * @param maxError The maximum acceptable truncation error. Usually a small + * number like 1e-6. + */ +template +T Tsit5(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) { + // See http://users.uoa.gr/~tsitourasc/RK54_new_v2.pdf Table 1 for the + // Butcher tableau the following arrays came from. + + constexpr int kDim = 7; + + // clang-format off + constexpr double A[kDim - 1][kDim - 1]{ + { 0.161}, + { -0.008480655492357, 0.3354806554923570}, + { 2.897153057105494, -6.359448489975075, 4.362295432869581}, + { 5.32586482843925895, -11.74888356406283, 7.495539342889836, -0.09249506636175525}, + { 5.86145544294642038, -12.92096931784711, 8.159367898576159, -0.07158497328140100, -0.02826905039406838}, + //a₇ᵢ = bᵢ, i = 1, 2, ··· , 6 + { 0.09646076681806523, 0.01, 0.4798896504144996, 1.379008574103742, -3.290069515436081, 2.324710524099774}}; + // clang-format on + + constexpr std::array b1{0.09646076681806523, + 0.01, + 0.4798896504144996, + 1.379008574103742, + -3.290069515436081, + 2.324710524099774, + 0.0}; + constexpr std::array b2{0.001780011052226, 0.000816434459657, + -0.007880878010262, 0.144711007173263, + -0.582357165452555, 0.458082105929187, + 1.0 / 66.0}; + + T newX; + double truncationError; + + double dtElapsed = 0.0; + double h = dt.value(); + + // Loop until we've gotten to our desired dt + while (dtElapsed < dt.value()) { + do { + // Only allow us to advance up to the dt remaining + h = (std::min)(h, dt.value() - dtElapsed); + + // clang-format off + T k1 = f(x, u); + T k2 = f(x + h * (A[0][0] * k1), u); + T k3 = f(x + h * (A[1][0] * k1 + A[1][1] * k2), u); + T k4 = f(x + h * (A[2][0] * k1 + A[2][1] * k2 + A[2][2] * k3), u); + T k5 = f(x + h * (A[3][0] * k1 + A[3][1] * k2 + A[3][2] * k3 + A[3][3] * k4), u); + T k6 = f(x + h * (A[4][0] * k1 + A[4][1] * k2 + A[4][2] * k3 + A[4][3] * k4 + A[4][4] * k5), u); + // clang-format on + + // Since the final row of A and the array b1 have the same coefficients + // and k7 has no effect on newX, we can reuse the calculation. + newX = x + h * (A[5][0] * k1 + A[5][1] * k2 + A[5][2] * k3 + + A[5][3] * k4 + A[5][4] * k5 + A[5][5] * k6); + T k7 = f(newX, u); + + truncationError = (h * ((b1[0] - b2[0]) * k1 + (b1[1] - b2[1]) * k2 + + (b1[2] - b2[2]) * k3 + (b1[3] - b2[3]) * k4 + + (b1[4] - b2[4]) * k5 + (b1[5] - b2[5]) * k6 + + (b1[6] - b2[6]) * k7)) + .norm(); + + if (truncationError == 0.0) { + h = dt.value() - dtElapsed; + } else { + h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0); + } + } while (truncationError > maxError); + + dtElapsed += h; + x = newX; + } + + return x; +} + +/** + * Performs adaptive Tsitouras 5th Order integration of dy/dt = f(t, y) for dt. + * + * @param f The function to integrate. It must take two arguments t and + * y. + * @param t The initial value of t. + * @param y The initial value of y. + * @param dt The time over which to integrate. + * @param maxError The maximum acceptable truncation error. Usually a small + * number like 1e-6. + */ +template +T Tsit5(F&& f, units::second_t t, T y, units::second_t dt, + double maxError = 1e-6) { + // See http://users.uoa.gr/~tsitourasc/RK54_new_v2.pdf Table 1 for the + // Butcher tableau the following arrays came from. + + constexpr int kDim = 7; + + // clang-format off + constexpr double A[kDim - 1][kDim - 1]{ + { 0.161}, + { -0.008480655492357, 0.3354806554923570}, + { 2.897153057105494, -6.359448489975075, 4.362295432869581}, + { 5.32586482843925895, -11.74888356406283, 7.495539342889836, -0.09249506636175525}, + { 5.86145544294642038, -12.92096931784711, 8.159367898576159, -0.07158497328140100, -0.02826905039406838}, + //a₇ᵢ = bᵢ, i = 1, 2, ··· , 6 + { 0.09646076681806523, 0.01, 0.4798896504144996, 1.379008574103742, -3.290069515436081, 2.324710524099774}}; + // clang-format on + + constexpr std::array b1{0.09646076681806523, + 0.01, + 0.4798896504144996, + 1.379008574103742, + -3.290069515436081, + 2.324710524099774, + 0.0}; + constexpr std::array b2{0.001780011052226, 0.000816434459657, + -0.007880878010262, 0.144711007173263, + -0.582357165452555, 0.458082105929187, + 1.0 / 66.0}; + + constexpr std::array c{ + 0.161, 0.327, 0.9, 0.9800255409045097, 1.0, 1.0}; + + T newY; + double truncationError; + + double dtElapsed = 0.0; + double h = dt.to(); + + // Loop until we've gotten to our desired dt + while (dtElapsed < dt.to()) { + do { + // Only allow us to advance up to the dt remaining + h = std::min(h, dt.to() - dtElapsed); + + // clang-format off + T k1 = f(t, y); + T k2 = f(t + units::second_t{h} * c[0], y + h * (A[0][0] * k1)); + T k3 = f(t + units::second_t{h} * c[1], y + h * (A[1][0] * k1 + A[1][1] * k2)); + T k4 = f(t + units::second_t{h} * c[2], y + h * (A[2][0] * k1 + A[2][1] * k2 + A[2][2] * k3)); + T k5 = f(t + units::second_t{h} * c[3], y + h * (A[3][0] * k1 + A[3][1] * k2 + A[3][2] * k3 + A[3][3] * k4)); + T k6 = f(t + units::second_t{h} * c[4], y + h * (A[4][0] * k1 + A[4][1] * k2 + A[4][2] * k3 + A[4][3] * k4 + A[4][4] * k5)); + // clang-format on + + // Since the final row of A and the array b1 have the same coefficients + // and k7 has no effect on newY, we can reuse the calculation. + newY = y + h * (A[5][0] * k1 + A[5][1] * k2 + A[5][2] * k3 + + A[5][3] * k4 + A[5][4] * k5 + A[5][5] * k6); + T k7 = f(t + units::second_t{h} * c[5], newY); + + truncationError = (h * ((b1[0] - b2[0]) * k1 + (b1[1] - b2[1]) * k2 + + (b1[2] - b2[2]) * k3 + (b1[3] - b2[3]) * k4 + + (b1[4] - b2[4]) * k5 + (b1[5] - b2[5]) * k6 + + (b1[6] - b2[6]) * k7)) + .norm(); + + h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0); + } while (truncationError > maxError); + + dtElapsed += h; + y = newY; + } + + return y; +} + } // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java index 99bc544bd04..76cbcab810e 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java @@ -107,7 +107,7 @@ void testReachesReference() { robotPose, x.get(State.kLeftVelocity, 0), x.get(State.kRightVelocity, 0), state); x = - NumericalIntegration.rkdp( + NumericalIntegration.tsit5( LTVDifferentialDriveControllerTest::dynamics, x, VecBuilder.fill(output.left, output.right), diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java index db7cf5c6d1c..6c74a670dbd 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java @@ -54,6 +54,7 @@ void testRK4TimeVarying() { } @Test + @SuppressWarnings("removal") void testZeroRKDP() { var y1 = NumericalIntegration.rkdp( @@ -63,6 +64,7 @@ void testZeroRKDP() { } @Test + @SuppressWarnings("removal") void testExponentialRKDP() { Matrix y0 = VecBuilder.fill(0.0); @@ -89,6 +91,7 @@ void testExponentialRKDP() { // // x(t) = 12eᵗ/(eᵗ + 1)² @Test + @SuppressWarnings("removal") void testRKDPTimeVarying() { final var y0 = VecBuilder.fill(12.0 * Math.exp(5.0) / Math.pow(Math.exp(5.0) + 1.0, 2.0)); @@ -103,4 +106,55 @@ void testRKDPTimeVarying() { 1e-12); assertEquals(12.0 * Math.exp(6.0) / Math.pow(Math.exp(6.0) + 1.0, 2.0), y1.get(0, 0), 1e-3); } + + @Test + void testZeroTsit5() { + var y1 = + NumericalIntegration.tsit5( + (x, u) -> VecBuilder.fill(0), VecBuilder.fill(0), VecBuilder.fill(0), 0.1); + + assertEquals(0.0, y1.get(0, 0), 1e-3); + } + + @Test + void testExponentialTsit5() { + Matrix y0 = VecBuilder.fill(0.0); + + var y1 = + NumericalIntegration.tsit5( + (x, u) -> { + var y = new Matrix<>(Nat.N1(), Nat.N1()); + y.set(0, 0, Math.exp(x.get(0, 0))); + return y; + }, + y0, + VecBuilder.fill(0), + 0.1); + + assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3); + } + + // Tests Tsit5 with a time varying solution. From + // http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html: + // + // dx/dt = x(2/(eᵗ + 1) - 1) + // + // The true (analytical) solution is: + // + // x(t) = 12eᵗ/(eᵗ + 1)² + @Test + void testTsit5TimeVarying() { + final var y0 = VecBuilder.fill(12.0 * Math.exp(5.0) / Math.pow(Math.exp(5.0) + 1.0, 2.0)); + + final var y1 = + NumericalIntegration.tsit5( + (Double t, Matrix y) -> + MatBuilder.fill( + Nat.N1(), Nat.N1(), y.get(0, 0) * (2.0 / (Math.exp(t) + 1.0) - 1.0)), + 5.0, + y0, + 1.0, + 1e-12); + assertEquals(12.0 * Math.exp(6.0) / Math.pow(Math.exp(6.0) + 1.0, 2.0), y1.get(0, 0), 1e-3); + } } diff --git a/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp b/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp index f602c702448..e1d1b6e85e4 100644 --- a/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp +++ b/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp @@ -88,9 +88,9 @@ TEST(LTVDifferentialDriveControllerTest, ReachesReference) { robotPose, units::meters_per_second_t{x(State::kLeftVelocity)}, units::meters_per_second_t{x(State::kRightVelocity)}, state); - x = frc::RKDP(&Dynamics, x, - frc::Vectord<2>{leftVoltage.value(), rightVoltage.value()}, - kDt); + x = frc::Tsit5(&Dynamics, x, + frc::Vectord<2>{leftVoltage.value(), rightVoltage.value()}, + kDt); } auto& endPose = trajectory.States().back().pose; diff --git a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp index 055ee49736b..38f2879cdf3 100644 --- a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp +++ b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp @@ -62,9 +62,9 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQ) { // Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ // 0 frc::Matrixd<2, 2> discQIntegrated = - frc::RKDP(units::second_t, - const frc::Matrixd<2, 2>&)>, - frc::Matrixd<2, 2>>( + frc::Tsit5(units::second_t, + const frc::Matrixd<2, 2>&)>, + frc::Matrixd<2, 2>>( [&](units::second_t t, const frc::Matrixd<2, 2>&) { return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ * (contA.transpose() * t.value()).exp()); @@ -94,9 +94,9 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) { // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ // 0 frc::Matrixd<2, 2> discQIntegrated = - frc::RKDP(units::second_t, - const frc::Matrixd<2, 2>&)>, - frc::Matrixd<2, 2>>( + frc::Tsit5(units::second_t, + const frc::Matrixd<2, 2>&)>, + frc::Matrixd<2, 2>>( [&](units::second_t t, const frc::Matrixd<2, 2>&) { return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ * (contA.transpose() * t.value()).exp()); diff --git a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp index 05ec0a48265..b59657d4f09 100644 --- a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp +++ b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp @@ -82,7 +82,7 @@ TEST(NumericalIntegrationTest, ExponentialRKDP) { // The true (analytical) solution is: // // x(t) = 12eᵗ/(eᵗ + 1)² -TEST(NumericalIntegrationTest, RKDPTimeVarying) { +TEST(NumericalIntegrationTest, TRKDP5TimeVarying) { frc::Vectord<1> y0{12.0 * std::exp(5.0) / std::pow(std::exp(5.0) + 1.0, 2.0)}; frc::Vectord<1> y1 = frc::RKDP( @@ -94,3 +94,46 @@ TEST(NumericalIntegrationTest, RKDPTimeVarying) { EXPECT_NEAR(y1(0), 12.0 * std::exp(6.0) / std::pow(std::exp(6.0) + 1.0, 2.0), 1e-3); } + +// Tests that integrating dx/dt = 0 works with Tsit5 +TEST(NumericalIntegrationTest, ZeroTsit5) { + frc::Vectord<1> y1 = frc::Tsit5( + [](const frc::Vectord<1>& x, const frc::Vectord<1>& u) { + return frc::Vectord<1>::Zero(); + }, + frc::Vectord<1>{0.0}, frc::Vectord<1>{0.0}, 0.1_s); + EXPECT_NEAR(y1(0), 0.0, 1e-3); +} + +// Tests that integrating dx/dt = eˣ works with Tsit5 +TEST(NumericalIntegrationTest, ExponentialTsit5) { + frc::Vectord<1> y0{0.0}; + + frc::Vectord<1> y1 = frc::Tsit5( + [](const frc::Vectord<1>& x, const frc::Vectord<1>& u) { + return frc::Vectord<1>{std::exp(x(0))}; + }, + y0, frc::Vectord<1>{0.0}, 0.1_s); + EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3); +} + +// Tests Tsit5 with a time varying solution. From +// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html: +// +// dx/dt = x(2/(eᵗ + 1) - 1) +// +// The true (analytical) solution is: +// +// x(t) = 12eᵗ/(eᵗ + 1)² +TEST(NumericalIntegrationTest, Tsit5TimeVarying) { + frc::Vectord<1> y0{12.0 * std::exp(5.0) / std::pow(std::exp(5.0) + 1.0, 2.0)}; + + frc::Vectord<1> y1 = frc::Tsit5( + [](units::second_t t, const frc::Vectord<1>& x) { + return frc::Vectord<1>{x(0) * + (2.0 / (std::exp(t.value()) + 1.0) - 1.0)}; + }, + 5_s, y0, 1_s, 1e-12); + EXPECT_NEAR(y1(0), 12.0 * std::exp(6.0) / std::pow(std::exp(6.0) + 1.0, 2.0), + 1e-3); +}