diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index ccbf83b3c0..a2951ecf86 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -85,7 +85,7 @@ void ElevatorSim::SetInputVoltage(units::volt_t voltage) { Eigen::Matrix ElevatorSim::UpdateX( const Eigen::Matrix& currentXhat, const Eigen::Matrix& u, units::second_t dt) { - auto updatedXhat = RKF45( + auto updatedXhat = RKDP( [&](const Eigen::Matrix& x, const Eigen::Matrix& u_) -> Eigen::Matrix { diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp index 3b1e943b9b..908a340528 100644 --- a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp @@ -88,7 +88,7 @@ Eigen::Matrix SingleJointedArmSim::UpdateX( // We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I * // std::cos(theta)]] - Eigen::Matrix updatedXhat = RKF45( + Eigen::Matrix updatedXhat = RKDP( [&](const auto& x, const auto& u) -> Eigen::Matrix { Eigen::Matrix xdot = m_plant.A() * x + m_plant.B() * u; diff --git a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp index 6e9bfbd93a..1a52036c1c 100644 --- a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp @@ -80,7 +80,7 @@ TEST(ElevatorSim, Stability) { Eigen::Matrix x1 = frc::MakeMatrix<2, 1>(0.0, 0.0); for (size_t i = 0; i < 50; i++) { - x1 = frc::RKF45( + x1 = frc::RKDP( [&](Eigen::Matrix x, Eigen::Matrix u) -> Eigen::Matrix { return system.A() * x + system.B() * u; 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 753d61afb7..ecd237961e 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 @@ -221,7 +221,7 @@ public class ElevatorSim extends LinearSystemSim { protected Matrix updateX(Matrix currentXhat, Matrix u, double dtSeconds) { // Calculate updated x-hat from Runge-Kutta. var updatedXhat = - NumericalIntegration.rkf45( + NumericalIntegration.rkdp( (x, u_) -> (m_plant.getA().times(x)) .plus(m_plant.getB().times(u_)) 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 3891090892..87981f71dd 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 @@ -282,7 +282,7 @@ public class SingleJointedArmSim extends LinearSystemSim { // We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I * // cos(theta)]] Matrix updatedXhat = - NumericalIntegration.rkf45( + NumericalIntegration.rkdp( (Matrix x, Matrix u_) -> { Matrix xdot = m_plant.getA().times(x).plus(m_plant.getB().times(u_)); if (m_simulateGravity) { 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 1d74ba2cdd..5035634594 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 @@ -5,12 +5,8 @@ package edu.wpi.first.math.system; import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Nat; import edu.wpi.first.math.Num; -import edu.wpi.first.math.Pair; import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N5; -import edu.wpi.first.math.numbers.N6; import java.util.function.BiFunction; import java.util.function.DoubleFunction; import java.util.function.Function; @@ -145,142 +141,234 @@ public final class NumericalIntegration { Matrix u, double dtSeconds, double maxError) { - double dtElapsed = 0; - double previousH = dtSeconds; + // See + // https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method + // for the Butcher tableau the following arrays came from. + + // This is used for time-varying integration + // // final double[5] + // final double[] A = { + // 1.0 / 4.0, 3.0 / 8.0, 12.0 / 13.0, 1.0, 1.0 / 2.0}; + + // final double[5][5] + final double[][] B = { + {1.0 / 4.0}, + {3.0 / 32.0, 9.0 / 32.0}, + {1932.0 / 2197.0, -7200.0 / 2197.0, 7296.0 / 2197.0}, + {439.0 / 216.0, -8.0, 3680.0 / 513.0, -845.0 / 4104.0}, + {-8.0 / 27.0, 2.0, -3544.0 / 2565.0, 1859.0 / 4104.0, -11.0 / 40.0} + }; + + // final double[6] + final double[] C1 = { + 16.0 / 135.0, 0.0, 6656.0 / 12825.0, 28561.0 / 56430.0, -9.0 / 50.0, 2.0 / 55.0 + }; + + // final double[6] + final double[] C2 = {25.0 / 216.0, 0.0, 1408.0 / 2565.0, 2197.0 / 4104.0, -1.0 / 5.0, 0.0}; + + Matrix newX; + double truncationError; + + double dtElapsed = 0.0; + double h = dtSeconds; + // Loop until we've gotten to our desired dt while (dtElapsed < dtSeconds) { - // RKF45 will give us an updated x and a dt back. - // We use the new dt (h) as the initial dt for our next loop - var ret = rkf45Impl(f, x, u, previousH, maxError, dtSeconds - dtElapsed); - dtElapsed += ret.getSecond(); - previousH = ret.getSecond(); - x = ret.getFirst(); + do { + // Only allow us to advance up to the dt remaining + h = Math.min(h, dtSeconds - dtElapsed); + + // Notice how the derivative in the Wikipedia notation is dy/dx. + // That means their y is our x and their x is our t + var k1 = f.apply(x, u).times(h); + var k2 = f.apply(x.plus(k1.times(B[0][0])), u).times(h); + var k3 = f.apply(x.plus(k1.times(B[1][0])).plus(k2.times(B[1][1])), u).times(h); + var k4 = + f.apply(x.plus(k1.times(B[2][0])).plus(k2.times(B[2][1])).plus(k3.times(B[2][2])), u) + .times(h); + var k5 = + f.apply( + x.plus(k1.times(B[3][0])) + .plus(k2.times(B[3][1])) + .plus(k3.times(B[3][2])) + .plus(k4.times(B[3][3])), + u) + .times(h); + var k6 = + f.apply( + x.plus(k1.times(B[4][0])) + .plus(k2.times(B[4][1])) + .plus(k3.times(B[4][2])) + .plus(k4.times(B[4][3])) + .plus(k5.times(B[4][4])), + u) + .times(h); + + newX = + x.plus(k1.times(C1[0])) + .plus(k2.times(C1[1])) + .plus(k3.times(C1[2])) + .plus(k4.times(C1[3])) + .plus(k5.times(C1[4])) + .plus(k6.times(C1[5])); + truncationError = + (k1.times(C1[0] - C2[0]) + .plus(k2.times(C1[1] - C2[1])) + .plus(k3.times(C1[2] - C2[2])) + .plus(k4.times(C1[3] - C2[3])) + .plus(k5.times(C1[4] - C2[4])) + .plus(k6.times(C1[5] - C2[5]))) + .normF(); + + h = 0.9 * h * Math.pow(maxError / truncationError, 1.0 / 5.0); + } while (truncationError > maxError); + + dtElapsed += h; + x = newX; } + return x; } - static final double[] ch = {47 / 450.0, 0, 12 / 25.0, 32 / 225.0, 1 / 30.0, 6 / 25.0}; - static final double[] ct = {-1 / 150.0, 0, 3 / 100.0, -16 / 75.0, -1 / 20.0, 6 / 25.0}; - static final Matrix Bs = - Matrix.mat(Nat.N6(), Nat.N5()) - .fill( - 0, - 0, - 0, - 0, - 0, - 2 / 9.0, - 0, - 0, - 0, - 0, - 1 / 12.0, - 1 / 4.0, - 0, - 0, - 0, - 69 / 128.0, - -243 / 128.0, - 135 / 64.0, - 0, - 0, - -17 / 12.0, - 27 / 4.0, - -27 / 5.0, - 16 / 15.0, - 0, - 65 / 432.0, - -5 / 16.0, - 13 / 16.0, - 4 / 27.0, - 5 / 144.0); - /** - * Implements one loop of RKF45. This takes an initial state, dt guess, and max truncation error, - * and returns a new x and the dt over which that x was updated. This should be called until there - * is no dt remaining. + * Performs adaptive Dormand-Prince integration of dx/dt = f(x, u) for dt. By default, the max + * error is 1e-6. * - * @param Num representing the states of the system to integrate. + * @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 initialH The initial dt guess. This is refined to clamp truncation error to the - * specified max. - * @param maxTruncationError The max truncation error acceptable. Usually a small number like - * 1e-6. - * @param dtRemaining How much time is left to integrate over. Used to clamp h. + * @param dtSeconds The time over which to integrate. * @return the integration of dx/dt = f(x, u) for dt. */ @SuppressWarnings("MethodTypeParameterName") - private static - Pair, Double> rkf45Impl( - BiFunction, Matrix, Matrix> f, - Matrix x, - Matrix u, - double initialH, - double maxTruncationError, - double dtRemaining) { - double truncationErr; - double h = initialH; + public static Matrix rkdp( + BiFunction, Matrix, Matrix> f, + Matrix x, + Matrix u, + double dtSeconds) { + return rkdp(f, x, u, dtSeconds, 1e-6); + } + + /** + * Performs adaptive Dormand-Prince 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("MethodTypeParameterName") + public static Matrix rkdp( + BiFunction, Matrix, Matrix> f, + Matrix x, + Matrix u, + double dtSeconds, + double maxError) { + // See https://en.wikipedia.org/wiki/Dormand%E2%80%93Prince_method for the + // Butcher tableau the following arrays came from. + + // This is used for time-varying integration + // // final double[6] + // final double[] A = { + // 1.0 / 5.0, 3.0 / 10.0, 4.0 / 5.0, 8.0 / 9.0, 1.0, 1.0}; + + // final double[6][6] + final double[][] B = { + {1.0 / 5.0}, + {3.0 / 40.0, 9.0 / 40.0}, + {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} + }; + + // final double[7] + final double[] C1 = { + 35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0, 11.0 / 84.0, 0.0 + }; + + // final double[7] + final double[] C2 = { + 5179.0 / 57600.0, + 0.0, + 7571.0 / 16695.0, + 393.0 / 640.0, + -92097.0 / 339200.0, + 187.0 / 2100.0, + 1.0 / 40.0 + }; + Matrix newX; + double truncationError; - do { - // only allow us to advance up to the dt remaining - h = Math.min(h, dtRemaining); + double dtElapsed = 0.0; + double h = dtSeconds; - // Notice how the derivative in the Wikipedia notation is dy/dx. - // That means their y is our x and their x is our t - Matrix k1 = f.apply(x, u).times(h); - Matrix k2 = f.apply(x.plus(k1.times(Bs.get(1, 0))), u).times(h); - Matrix k3 = - f.apply(x.plus(k1.times(Bs.get(2, 0))).plus(k2.times(Bs.get(2, 1))), u).times(h); - Matrix k4 = - f.apply( - x.plus(k1.times(Bs.get(3, 0))) - .plus(k2.times(Bs.get(3, 1))) - .plus(k3.times(Bs.get(3, 2))), - u) - .times(h); - Matrix k5 = - f.apply( - x.plus(k1.times(Bs.get(4, 0))) - .plus(k2.times(Bs.get(4, 1))) - .plus(k3.times(Bs.get(4, 2))) - .plus(k4.times(Bs.get(4, 3))), - u) - .times(h); - Matrix k6 = - f.apply( - x.plus(k1.times(Bs.get(5, 0))) - .plus(k2.times(Bs.get(5, 1))) - .plus(k3.times(Bs.get(5, 2))) - .plus(k4.times(Bs.get(5, 3))) - .plus(k5.times(Bs.get(5, 4))), - u) - .times(h); + // 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); - newX = - x.plus(k1.times(ch[0])) - .plus(k2.times(ch[1])) - .plus(k3.times(ch[2])) - .plus(k4.times(ch[3])) - .plus(k5.times(ch[4])) - .plus(k6.times(ch[5])); + var k1 = f.apply(x, u).times(h); + var k2 = f.apply(x.plus(k1.times(B[0][0])), u).times(h); + var k3 = f.apply(x.plus(k1.times(B[1][0])).plus(k2.times(B[1][1])), u).times(h); + var k4 = + f.apply(x.plus(k1.times(B[2][0])).plus(k2.times(B[2][1])).plus(k3.times(B[2][2])), u) + .times(h); + var k5 = + f.apply( + x.plus(k1.times(B[3][0])) + .plus(k2.times(B[3][1])) + .plus(k3.times(B[3][2])) + .plus(k4.times(B[3][3])), + u) + .times(h); + var k6 = + f.apply( + x.plus(k1.times(B[4][0])) + .plus(k2.times(B[4][1])) + .plus(k3.times(B[4][2])) + .plus(k4.times(B[4][3])) + .plus(k5.times(B[4][4])), + u) + .times(h); - truncationErr = - k1.times(ct[0]) - .plus(k2.times(ct[1])) - .plus(k3.times(ct[2])) - .plus(k4.times(ct[3])) - .plus(k5.times(ct[4])) - .plus(k6.times(ct[5])) - .normF(); + // Since the final row of B and the array C1 have the same coefficients + // and k7 has no effect on newX, we can reuse the calculation. + newX = + x.plus(k1.times(B[5][0])) + .plus(k2.times(B[5][1])) + .plus(k3.times(B[5][2])) + .plus(k4.times(B[5][3])) + .plus(k5.times(B[5][4])) + .plus(k6.times(B[5][5])); + var k7 = f.apply(newX, u).times(h); - h = 0.9 * h * Math.pow(maxTruncationError / truncationErr, 1 / 5.0); - } while (truncationErr > maxTruncationError); + truncationError = + (k1.times(C1[0] - C2[0]) + .plus(k2.times(C1[1] - C2[1])) + .plus(k3.times(C1[2] - C2[2])) + .plus(k4.times(C1[3] - C2[3])) + .plus(k5.times(C1[4] - C2[4])) + .plus(k6.times(C1[5] - C2[5])) + .plus(k7.times(C1[6] - C2[6]))) + .normF(); - // Return the new x, and the timestep - return Pair.of(newX, h); + h = 0.9 * h * Math.pow(maxError / truncationError, 1.0 / 5.0); + } while (truncationError > maxError); + + dtElapsed += h; + x = newX; + } + + return x; } } diff --git a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h index edc05255cc..c731e464fe 100644 --- a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h +++ b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h @@ -7,6 +7,7 @@ #include #include +#include #include "Eigen/Core" #include "units/time.h" @@ -67,56 +68,6 @@ T RungeKuttaTimeVarying(F&& f, T x, units::second_t t, units::second_t dt) { return x + dt.to() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4); } -template -T RKF45Impl(F&& f, T x, U u, double& h, double maxTruncationError, - double dtRemaining) { - double truncationErr; - T newX; - - static double ch[] = {47 / 450.0, 0, 12 / 25.0, - 32 / 225.0, 1 / 30.0, 6 / 25.0}; - static double ct[] = {-1 / 150.0, 0, 3 / 100.0, - -16 / 75.0, -1 / 20.0, 6 / 25.0}; - static Eigen::Matrix Bs = frc::MakeMatrix<6, 5>( - 0.0, 0.0, 0.0, 0.0, 0.0, 2 / 9.0, 0.0, 0.0, 0.0, 0.0, 1 / 12.0, 1 / 4.0, - 0.0, 0.0, 0.0, 69 / 128.0, -243 / 128.0, 135 / 64.0, 0.0, 0.0, -17 / 12.0, - 27 / 4.0, -27 / 5.0, 16 / 15.0, 0.0, 65 / 432.0, -5 / 16.0, 13 / 16.0, - 4 / 27.0, 5 / 144.0); - - do { - // only allow us to advance up to the dt remaining - h = std::min(h, dtRemaining); - - // Notice how the derivative in the Wikipedia notation is dy/dx. - // That means their y is our x and their x is our t - T k1 = f(x, u) * h; - T k2 = f(x + (k1 * Bs(1, 0)), u) * h; - T k3 = f(x + (k1 * (Bs(2, 0))) + (k2 * (Bs(2, 1))), u) * h; - T k4 = - f(x + (k1 * (Bs(3, 0))) + (k2 * (Bs(3, 1))) + (k3 * (Bs(3, 2))), u) * h; - T k5 = f(x + (k1 * (Bs(4, 0))) + (k2 * (Bs(4, 1))) + (k3 * (Bs(4, 2))) + - (k4 * (Bs(4, 3))), - u) * - h; - T k6 = f(x + (k1 * (Bs(5, 0))) + (k2 * (Bs(5, 1))) + (k3 * (Bs(5, 2))) + - (k4 * (Bs(5, 3))) + (k5 * (Bs(5, 4))), - u) * - h; - - newX = x + (k1 * (ch[0])) + (k2 * (ch[1])) + (k3 * (ch[2])) + - (k4 * (ch[3])) + (k5 * (ch[4])) + (k6 * (ch[5])); - - truncationErr = (k1 * (ct[0]) + (k2 * (ct[1])) + (k3 * (ct[2])) + - (k4 * (ct[3])) + (k5 * (ct[4])) + (k6 * (ct[5]))) - .norm(); - - h = 0.9 * h * std::pow(maxTruncationError / truncationErr, 1 / 5.); - } while (truncationErr > maxTruncationError); - - // Return the new x. Dt is changed by reference - return newX; -} - /** * Performs adaptive RKF45 integration of dx/dt = f(x, u) for dt, as described * in https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method @@ -131,19 +82,150 @@ T RKF45Impl(F&& f, T x, U u, double& h, double maxTruncationError, */ template T RKF45(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) { - double dtElapsed = 0; - double dtSeconds = dt.to(); - double previousH = dt.to(); + // See + // https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method + // for the Butcher tableau the following arrays came from. + constexpr int kDim = 6; + + // This is used for time-varying integration + // constexpr std::array A{ + // 1.0 / 4.0, 3.0 / 8.0, 12.0 / 13.0, 1.0, 1.0 / 2.0}; + + // clang-format off + constexpr double B[kDim - 1][kDim - 1]{ + { 1.0 / 4.0}, + { 3.0 / 32.0, 9.0 / 32.0}, + {1932.0 / 2197.0, -7200.0 / 2197.0, 7296.0 / 2197.0}, + { 439.0 / 216.0, -8.0, 3680.0 / 513.0, -845.0 / 4104.0}, + { -8.0 / 27.0, 2.0, -3544.0 / 2565.0, 1859.0 / 4104.0, -11.0 / 40.0}}; + // clang-format on + + constexpr std::array C1{16.0 / 135.0, 0.0, + 6656.0 / 12825.0, 28561.0 / 56430.0, + -9.0 / 50.0, 2.0 / 55.0}; + constexpr std::array C2{ + 25.0 / 216.0, 0.0, 1408.0 / 2565.0, 2197.0 / 4104.0, -1.0 / 5.0, 0.0}; + + T newX; + double truncationError; + + double dtElapsed = 0.0; + double h = dt.to(); // Loop until we've gotten to our desired dt - while (dtElapsed < dtSeconds) { - // RKF45 will give us an updated x and a dt back. - // We use the new dt (h) as the initial dt for our next loop - // previousH is changed by-reference - T ret = RKF45Impl(f, x, u, previousH, maxError, dtSeconds - dtElapsed); - dtElapsed += previousH; - x = ret; + while (dtElapsed < dt.to()) { + do { + // Only allow us to advance up to the dt remaining + h = std::min(h, dt.to() - dtElapsed); + + // Notice how the derivative in the Wikipedia notation is dy/dx. + // That means their y is our x and their x is our t + // clang-format off + T k1 = f(x, u) * h; + T k2 = f(x + k1 * B[0][0], u) * h; + T k3 = f(x + k1 * B[1][0] + k2 * B[1][1], u) * h; + T k4 = f(x + k1 * B[2][0] + k2 * B[2][1] + k3 * B[2][2], u) * h; + T k5 = f(x + k1 * B[3][0] + k2 * B[3][1] + k3 * B[3][2] + k4 * B[3][3], u) * h; + T k6 = f(x + k1 * B[4][0] + k2 * B[4][1] + k3 * B[4][2] + k4 * B[4][3] + k5 * B[4][4], u) * h; + // clang-format on + + newX = x + k1 * C1[0] + k2 * C1[1] + k3 * C1[2] + k4 * C1[3] + + k5 * C1[4] + k6 * C1[5]; + truncationError = + (k1 * (C1[0] - C2[0]) + k2 * (C1[1] - C2[1]) + k3 * (C1[2] - C2[2]) + + k4 * (C1[3] - C2[3]) + k5 * (C1[4] - C2[4]) + k6 * (C1[5] - C2[5])) + .norm(); + + h = 0.9 * h * std::pow(maxError / truncationError, 1.0 / 5.0); + } while (truncationError > maxError); + + dtElapsed += h; + x = newX; } + + return x; +} + +/** + * Performs adaptive Dormand-Prince 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 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. + + constexpr int kDim = 7; + + // This is used for time-varying integration + // constexpr std::array A{ + // 1.0 / 5.0, 3.0 / 10.0, 4.0 / 5.0, 8.0 / 9.0, 1.0, 1.0}; + + // clang-format off + constexpr double B[kDim - 1][kDim - 1]{ + { 1.0 / 5.0}, + { 3.0 / 40.0, 9.0 / 40.0}, + { 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}}; + // clang-format on + + constexpr std::array C1{ + 35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0, + 11.0 / 84.0, 0.0}; + constexpr std::array C2{5179.0 / 57600.0, 0.0, + 7571.0 / 16695.0, 393.0 / 640.0, + -92097.0 / 339200.0, 187.0 / 2100.0, + 1.0 / 40.0}; + + T newX; + 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(x, u) * h; + T k2 = f(x + k1 * B[0][0], u) * h; + T k3 = f(x + k1 * B[1][0] + k2 * B[1][1], u) * h; + T k4 = f(x + k1 * B[2][0] + k2 * B[2][1] + k3 * B[2][2], u) * h; + T k5 = f(x + k1 * B[3][0] + k2 * B[3][1] + k3 * B[3][2] + k4 * B[3][3], u) * h; + T k6 = f(x + k1 * B[4][0] + k2 * B[4][1] + k3 * B[4][2] + k4 * B[4][3] + k5 * B[4][4], u) * h; + // clang-format on + + // Since the final row of B and the array C1 have the same coefficients + // and k7 has no effect on newX, we can reuse the calculation. + newX = x + k1 * B[5][0] + k2 * B[5][1] + k3 * B[5][2] + k4 * B[5][3] + + k5 * B[5][4] + k6 * B[5][5]; + T k7 = f(newX, u) * h; + + truncationError = + (k1 * (C1[0] - C2[0]) + k2 * (C1[1] - C2[1]) + k3 * (C1[2] - C2[2]) + + k4 * (C1[3] - C2[3]) + k5 * (C1[4] - C2[4]) + k6 * (C1[5] - C2[5]) + + k7 * (C1[6] - C2[6])) + .norm(); + + h = 0.9 * h * std::pow(maxError / truncationError, 1.0 / 5.0); + } while (truncationError > maxError); + + dtElapsed += h; + x = newX; + } + return x; } 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 b7cfbd18ea..93cedd443e 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 @@ -34,7 +34,7 @@ public class NumericalIntegrationTest { @Test @SuppressWarnings({"ParameterName", "LocalVariableName"}) - public void testExponentialAdaptive() { + public void testExponentialRKF45() { Matrix y0 = VecBuilder.fill(0.0); //noinspection SuspiciousNameCombination @@ -51,4 +51,24 @@ public class NumericalIntegrationTest { assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3); } + + @Test + @SuppressWarnings({"ParameterName", "LocalVariableName"}) + public void testExponentialRKDP() { + Matrix y0 = VecBuilder.fill(0.0); + + //noinspection SuspiciousNameCombination + var y1 = + NumericalIntegration.rkdp( + (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); + } } diff --git a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp index c2e6a3d05b..208ab5d274 100644 --- a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp +++ b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp @@ -38,8 +38,8 @@ TEST(NumericalIntegrationTest, ExponentialWithU) { EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3); } -// Tests that integrating dx/dt = e^x works when we provide a U. -TEST(NumericalIntegrationTest, ExponentialWithUAdaptive) { +// Tests that integrating dx/dt = e^x works with RKF45. +TEST(NumericalIntegrationTest, ExponentialRKF45) { Eigen::Matrix y0; y0(0) = 0.0; @@ -53,6 +53,21 @@ TEST(NumericalIntegrationTest, ExponentialWithUAdaptive) { EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3); } +// Tests that integrating dx/dt = e^x works with RKDP +TEST(NumericalIntegrationTest, ExponentialRKDP) { + Eigen::Matrix y0; + y0(0) = 0.0; + + Eigen::Matrix y1 = frc::RKDP( + [](Eigen::Matrix x, Eigen::Matrix u) { + Eigen::Matrix y; + y(0) = std::exp(x(0)); + return y; + }, + y0, (Eigen::Matrix() << 0.0).finished(), 0.1_s); + EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3); +} + namespace { Eigen::Matrix RungeKuttaTimeVaryingSolution(double t) { return (Eigen::Matrix()