diff --git a/benchmark/src/main/java/wpilib/robot/Main.java b/benchmark/src/main/java/wpilib/robot/Main.java index e0635e6e4d..975e3401aa 100644 --- a/benchmark/src/main/java/wpilib/robot/Main.java +++ b/benchmark/src/main/java/wpilib/robot/Main.java @@ -12,44 +12,18 @@ import org.openjdk.jmh.annotations.OutputTimeUnit; import org.openjdk.jmh.profile.GCProfiler; import org.openjdk.jmh.runner.Runner; import org.openjdk.jmh.runner.RunnerException; -import org.openjdk.jmh.runner.options.Options; import org.openjdk.jmh.runner.options.OptionsBuilder; import org.openjdk.jmh.runner.options.TimeValue; import org.wpilib.math.geometry.Pose2d; -import org.wpilib.math.geometry.Rotation2d; -import org.wpilib.math.path.TravelingSalesman; public class Main { - private static final Pose2d[] poses = { - new Pose2d(-1, 1, Rotation2d.kCW_90deg), - new Pose2d(-1, 2, Rotation2d.kCCW_90deg), - new Pose2d(0, 0, Rotation2d.kZero), - new Pose2d(0, 3, Rotation2d.kCW_90deg), - new Pose2d(1, 1, Rotation2d.kCCW_90deg), - new Pose2d(1, 2, Rotation2d.kCCW_90deg), - }; - private static final int iterations = 100; - - private static final TravelingSalesman transformTraveler = - new TravelingSalesman( - (pose1, pose2) -> { - var transform = pose2.minus(pose1); - return Math.hypot(transform.getX(), transform.getY()); - }); - private static final TravelingSalesman twistTraveler = - new TravelingSalesman( - (pose1, pose2) -> { - var twist = pose2.minus(pose1).log(); - return Math.hypot(twist.dx, twist.dy); - }); - /** * Main function. * * @param args The (unused) arguments to the program. */ public static void main(String... args) throws RunnerException { - Options opt = + var opt = new OptionsBuilder() .include(Main.class.getSimpleName()) .addProfiler(GCProfiler.class) @@ -66,14 +40,14 @@ public class Main { @Benchmark @BenchmarkMode(Mode.AverageTime) @OutputTimeUnit(TimeUnit.MICROSECONDS) - public Pose2d[] transform() { - return transformTraveler.solve(poses, iterations); + public Pose2d[] travelingSalesmanTransform() { + return TravelingSalesmanBenchmark.transform(); } @Benchmark @BenchmarkMode(Mode.AverageTime) @OutputTimeUnit(TimeUnit.MICROSECONDS) - public Pose2d[] twist() { - return twistTraveler.solve(poses, iterations); + public Pose2d[] travelingSalesmanTwist() { + return TravelingSalesmanBenchmark.twist(); } } diff --git a/benchmark/src/main/java/wpilib/robot/TravelingSalesmanBenchmark.java b/benchmark/src/main/java/wpilib/robot/TravelingSalesmanBenchmark.java new file mode 100644 index 0000000000..a1e3796612 --- /dev/null +++ b/benchmark/src/main/java/wpilib/robot/TravelingSalesmanBenchmark.java @@ -0,0 +1,48 @@ +// 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. + +package wpilib.robot; + +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.path.TravelingSalesman; + +public final class TravelingSalesmanBenchmark { + private TravelingSalesmanBenchmark() { + // Utility class. + } + + private static final Pose2d[] poses = { + new Pose2d(-1, 1, Rotation2d.kCW_90deg), + new Pose2d(-1, 2, Rotation2d.kCCW_90deg), + new Pose2d(0, 0, Rotation2d.kZero), + new Pose2d(0, 3, Rotation2d.kCW_90deg), + new Pose2d(1, 1, Rotation2d.kCCW_90deg), + new Pose2d(1, 2, Rotation2d.kCCW_90deg), + }; + private static final int iterations = 100; + + private static final TravelingSalesman transformTraveler = + new TravelingSalesman( + (pose1, pose2) -> { + var transform = pose2.minus(pose1); + return Math.hypot(transform.getX(), transform.getY()); + }); + private static final TravelingSalesman twistTraveler = + new TravelingSalesman( + (pose1, pose2) -> { + var twist = pose2.minus(pose1).log(); + return Math.hypot(twist.dx, twist.dy); + }); + + /** TravelingSalesman transform benchmark. */ + public static Pose2d[] transform() { + return transformTraveler.solve(poses, iterations); + } + + /** TravelingSalesman twist benchmark. */ + public static Pose2d[] twist() { + return twistTraveler.solve(poses, iterations); + } +} diff --git a/benchmark/src/main/native/cpp/CartPoleBenchmark.hpp b/benchmark/src/main/native/cpp/CartPoleBenchmark.hpp new file mode 100644 index 0000000000..8ff790f8cd --- /dev/null +++ b/benchmark/src/main/native/cpp/CartPoleBenchmark.hpp @@ -0,0 +1,115 @@ +// 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. + +#pragma once + +#include + +#include + +#include + +#include "wpi/math/system/NumericalIntegration.hpp" + +inline slp::VariableMatrix CartPoleDynamics(const slp::VariableMatrix& x, + const slp::VariableMatrix& u) { + constexpr double m_c = 5.0; // Cart mass (kg) + constexpr double m_p = 0.5; // Pole mass (kg) + constexpr double l = 0.5; // Pole length (m) + constexpr double g = 9.806; // Acceleration due to gravity (m/s²) + + auto q = x.segment(0, 2); + auto qdot = x.segment(2, 2); + auto theta = q[1]; + auto thetadot = qdot[1]; + + // [ m_c + m_p m_p l cosθ] + // M(q) = [m_p l cosθ m_p l² ] + slp::VariableMatrix M{{m_c + m_p, m_p * l * cos(theta)}, + {m_p * l * cos(theta), m_p * std::pow(l, 2)}}; + + // [0 −m_p lθ̇ sinθ] + // C(q, q̇) = [0 0 ] + slp::VariableMatrix C{{0, -m_p * l * thetadot * sin(theta)}, {0, 0}}; + + // [ 0 ] + // τ_g(q) = [-m_p gl sinθ] + slp::VariableMatrix tau_g{{0}, {-m_p * g * l * sin(theta)}}; + + // [1] + // B = [0] + constexpr Eigen::Matrix B{{1}, {0}}; + + // q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu) + slp::VariableMatrix qddot{4}; + qddot.segment(0, 2) = qdot; + qddot.segment(2, 2) = solve(M, tau_g - C * qdot + B * u); + return qddot; +} + +inline void BM_CartPole(benchmark::State& state) { + using namespace std::chrono_literals; + + // NOLINTNEXTLINE(clang-analyzer-deadcode.DeadStores) + for (auto _ : state) { + constexpr std::chrono::duration T = 5s; + constexpr std::chrono::duration dt = 50ms; + constexpr int N = T / dt; + + constexpr double u_max = 20.0; // N + constexpr double d_max = 2.0; // m + + constexpr Eigen::Vector x_initial{{0.0, 0.0, 0.0, 0.0}}; + constexpr Eigen::Vector x_final{ + {1.0, std::numbers::pi, 0.0, 0.0}}; + + slp::Problem problem; + + // x = [q, q̇]ᵀ = [x, θ, ẋ, θ̇]ᵀ + auto X = problem.decision_variable(4, N + 1); + + // Initial guess + for (int k = 0; k < N + 1; ++k) { + X(0, k).set_value( + std::lerp(x_initial[0], x_final[0], static_cast(k) / N)); + X(1, k).set_value( + std::lerp(x_initial[1], x_final[1], static_cast(k) / N)); + } + + // u = f_x + auto U = problem.decision_variable(1, N); + + // Initial conditions + problem.subject_to(X.col(0) == x_initial); + + // Final conditions + problem.subject_to(X.col(N) == x_final); + + // Cart position constraints + problem.subject_to(X.row(0) >= 0.0); + problem.subject_to(X.row(0) <= d_max); + + // Input constraints + problem.subject_to(U >= -u_max); + problem.subject_to(U <= u_max); + + // Dynamics constraints - RK4 integration + for (int k = 0; k < N; ++k) { + problem.subject_to( + X.col(k + 1) == + wpi::math::RK4(CartPoleDynamics, X.col(k), + U.col(k), dt)); + } + + // Minimize sum squared inputs + slp::Variable J = 0.0; + for (int k = 0; k < N; ++k) { + J += U.col(k).T() * U.col(k); + } + problem.minimize(J); + + problem.solve({.diagnostics = true}); + } +} diff --git a/benchmark/src/main/native/cpp/Main.cpp b/benchmark/src/main/native/cpp/Main.cpp index 406245c12e..ac879aa5ff 100644 --- a/benchmark/src/main/native/cpp/Main.cpp +++ b/benchmark/src/main/native/cpp/Main.cpp @@ -4,41 +4,11 @@ #include -#include "wpi/math/geometry/Pose2d.hpp" -#include "wpi/math/path/TravelingSalesman.hpp" -#include "wpi/units/angle.hpp" -#include "wpi/units/length.hpp" -#include "wpi/util/array.hpp" +#include "CartPoleBenchmark.hpp" +#include "TravelingSalesmanBenchmark.hpp" -static constexpr wpi::util::array poses{ - wpi::math::Pose2d{-1_m, 1_m, -90_deg}, wpi::math::Pose2d{-1_m, 2_m, 90_deg}, - wpi::math::Pose2d{0_m, 0_m, 0_deg}, wpi::math::Pose2d{0_m, 3_m, -90_deg}, - wpi::math::Pose2d{1_m, 1_m, 90_deg}, wpi::math::Pose2d{1_m, 2_m, 90_deg}, -}; -static constexpr int iterations = 100; - -void BM_Transform(benchmark::State& state) { - wpi::math::TravelingSalesman traveler{[](auto pose1, auto pose2) { - auto transform = pose2 - pose1; - return wpi::units::math::hypot(transform.X(), transform.Y()).value(); - }}; - // NOLINTNEXTLINE(clang-analyzer-deadcode.DeadStores) - for (auto _ : state) { - traveler.Solve(poses, iterations); - } -} -BENCHMARK(BM_Transform); - -void BM_Twist(benchmark::State& state) { - wpi::math::TravelingSalesman traveler{[](auto pose1, auto pose2) { - auto twist = (pose2 - pose1).Log(); - return wpi::units::math::hypot(twist.dx, twist.dy).value(); - }}; - // NOLINTNEXTLINE(clang-analyzer-deadcode.DeadStores) - for (auto _ : state) { - traveler.Solve(poses, iterations); - } -} -BENCHMARK(BM_Twist); +BENCHMARK(BM_CartPole); +BENCHMARK(BM_TravelingSalesman_Transform); +BENCHMARK(BM_TravelingSalesman_Twist); BENCHMARK_MAIN(); diff --git a/benchmark/src/main/native/cpp/TravelingSalesmanBenchmark.hpp b/benchmark/src/main/native/cpp/TravelingSalesmanBenchmark.hpp new file mode 100644 index 0000000000..e70fb847f1 --- /dev/null +++ b/benchmark/src/main/native/cpp/TravelingSalesmanBenchmark.hpp @@ -0,0 +1,42 @@ +// 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. + +#pragma once + +#include + +#include "wpi/math/geometry/Pose2d.hpp" +#include "wpi/math/path/TravelingSalesman.hpp" +#include "wpi/units/angle.hpp" +#include "wpi/units/length.hpp" +#include "wpi/util/array.hpp" + +static constexpr wpi::util::array poses{ + wpi::math::Pose2d{-1_m, 1_m, -90_deg}, wpi::math::Pose2d{-1_m, 2_m, 90_deg}, + wpi::math::Pose2d{0_m, 0_m, 0_deg}, wpi::math::Pose2d{0_m, 3_m, -90_deg}, + wpi::math::Pose2d{1_m, 1_m, 90_deg}, wpi::math::Pose2d{1_m, 2_m, 90_deg}, +}; +static constexpr int iterations = 100; + +inline void BM_TravelingSalesman_Transform(benchmark::State& state) { + wpi::math::TravelingSalesman traveler{[](auto pose1, auto pose2) { + auto transform = pose2 - pose1; + return wpi::units::math::hypot(transform.X(), transform.Y()).value(); + }}; + // NOLINTNEXTLINE(clang-analyzer-deadcode.DeadStores) + for (auto _ : state) { + traveler.Solve(poses, iterations); + } +} + +inline void BM_TravelingSalesman_Twist(benchmark::State& state) { + wpi::math::TravelingSalesman traveler{[](auto pose1, auto pose2) { + auto twist = (pose2 - pose1).Log(); + return wpi::units::math::hypot(twist.dx, twist.dy).value(); + }}; + // NOLINTNEXTLINE(clang-analyzer-deadcode.DeadStores) + for (auto _ : state) { + traveler.Solve(poses, iterations); + } +}