diff --git a/CMakeLists.txt b/CMakeLists.txt index 46a2e975..46732533 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -171,8 +171,30 @@ install( DESTINATION lib/cmake/Sleipnir ) -# Add benchmark executables if CasADi exists +# Add benchmark executables if(BUILD_BENCHMARKING) + # Perf benchmark + foreach(benchmark "CartPole" "Flywheel") + file( + GLOB ${benchmark}PerfBenchmark_src + benchmarks/perf/*.cpp + benchmarks/perf/${benchmark}/*.cpp + ) + add_executable( + ${benchmark}PerfBenchmark + ${${benchmark}PerfBenchmark_src} + ) + compiler_flags(${benchmark}PerfBenchmark) + target_include_directories( + ${benchmark}PerfBenchmark + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/benchmarks + ${CMAKE_CURRENT_SOURCE_DIR}/benchmarks/perf + ) + target_link_libraries(${benchmark}PerfBenchmark PRIVATE Sleipnir) + endforeach() + + # Scalability benchmark (if CasADi exists) find_package(casadi QUIET) if(casadi_FOUND) foreach(benchmark "CartPole" "Flywheel") @@ -188,7 +210,9 @@ if(BUILD_BENCHMARKING) compiler_flags(${benchmark}ScalabilityBenchmark) target_include_directories( ${benchmark}ScalabilityBenchmark - PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/benchmarks/scalability + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/benchmarks + ${CMAKE_CURRENT_SOURCE_DIR}/benchmarks/scalability ) target_link_libraries( ${benchmark}ScalabilityBenchmark diff --git a/benchmarks/scalability/CmdlineArguments.hpp b/benchmarks/CmdlineArguments.hpp similarity index 100% rename from benchmarks/scalability/CmdlineArguments.hpp rename to benchmarks/CmdlineArguments.hpp diff --git a/benchmarks/scalability/RK4.hpp b/benchmarks/RK4.hpp similarity index 100% rename from benchmarks/scalability/RK4.hpp rename to benchmarks/RK4.hpp diff --git a/benchmarks/perf/CartPole/Main.cpp b/benchmarks/perf/CartPole/Main.cpp new file mode 100644 index 00000000..66ad8fd7 --- /dev/null +++ b/benchmarks/perf/CartPole/Main.cpp @@ -0,0 +1,145 @@ +// Copyright (c) Sleipnir contributors + +#include +#include +#include + +#include +#include + +#include "CmdlineArguments.hpp" +#include "RK4.hpp" + +sleipnir::VariableMatrix CartPoleDynamics(const sleipnir::VariableMatrix& x, + const sleipnir::VariableMatrix& u) { + // https://underactuated.mit.edu/acrobot.html#cart_pole + // + // θ is CCW+ measured from negative y-axis. + // + // q = [x, θ]ᵀ + // q̇ = [ẋ, θ̇]ᵀ + // u = f_x + // + // M(q)q̈ + C(q, q̇)q̇ = τ_g(q) + Bu + // M(q)q̈ = τ_g(q) − C(q, q̇)q̇ + Bu + // q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu) + // + // [ m_c + m_p m_p l cosθ] + // M(q) = [m_p l cosθ m_p l² ] + // + // [0 −m_p lθ̇ sinθ] + // C(q, q̇) = [0 0 ] + // + // [ 0 ] + // τ_g(q) = [−m_p gl sinθ] + // + // [1] + // B = [0] + 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² ] + sleipnir::VariableMatrix M{2, 2}; + M(0, 0) = m_c + m_p; + M(0, 1) = m_p * l * cos(theta); // NOLINT + M(1, 0) = m_p * l * cos(theta); // NOLINT + M(1, 1) = m_p * std::pow(l, 2); + + // [0 −m_p lθ̇ sinθ] + // C(q, q̇) = [0 0 ] + sleipnir::VariableMatrix C{2, 2}; + C(0, 0) = 0; + C(0, 1) = -m_p * l * thetadot * sin(theta); // NOLINT + C(1, 0) = 0; + C(1, 1) = 0; + + // [ 0 ] + // τ_g(q) = [-m_p gl sinθ] + sleipnir::VariableMatrix tau_g{2, 1}; + tau_g(0) = 0; + tau_g(1) = -m_p * g * l * sin(theta); // NOLINT + + // [1] + // B = [0] + Eigen::Matrix B{{1}, {0}}; + + // q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu) + sleipnir::VariableMatrix qddot{4, 1}; + qddot.Segment(0, 2) = qdot; + qddot.Segment(2, 2) = sleipnir::Solve(M, tau_g - C * qdot + B * u); + return qddot; +} + +sleipnir::OptimizationProblem CartPoleProblem(std::chrono::duration dt, + int N) { + 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}}; + + sleipnir::OptimizationProblem problem; + + // x = [q, q̇]ᵀ = [x, θ, ẋ, θ̇]ᵀ + auto X = problem.DecisionVariable(4, N + 1); + + // Initial guess + for (int k = 0; k < N + 1; ++k) { + X(0, k).SetValue( + std::lerp(x_initial(0), x_final(0), static_cast(k) / N)); + X(1, k).SetValue( + std::lerp(x_initial(1), x_final(1), static_cast(k) / N)); + } + + // u = f_x + auto U = problem.DecisionVariable(1, N); + + // Initial conditions + problem.SubjectTo(X.Col(0) == x_initial); + + // Final conditions + problem.SubjectTo(X.Col(N) == x_final); + + // Cart position constraints + problem.SubjectTo(X.Row(0) >= 0.0); + problem.SubjectTo(X.Row(0) <= d_max); + + // Input constraints + problem.SubjectTo(U >= -u_max); + problem.SubjectTo(U <= u_max); + + // Dynamics constraints - RK4 integration + for (int k = 0; k < N; ++k) { + problem.SubjectTo(X.Col(k + 1) == + RK4(CartPoleDynamics, X.Col(k), + U.Col(k), dt)); + } + + // Minimize sum squared inputs + sleipnir::Variable J = 0.0; + for (int k = 0; k < N; ++k) { + J += U.Col(k).T() * U.Col(k); + } + problem.Minimize(J); + + return problem; +} + +int main(int argc, char* argv[]) { + using namespace std::chrono_literals; + + CmdlineArgs args{argv, argc}; + bool diagnostics = args.Contains("--enable-diagnostics"); + + auto problem = CartPoleProblem(5s, 300); + problem.Solve({.diagnostics = diagnostics}); +} diff --git a/benchmarks/perf/Flywheel/Main.cpp b/benchmarks/perf/Flywheel/Main.cpp new file mode 100644 index 00000000..880c245b --- /dev/null +++ b/benchmarks/perf/Flywheel/Main.cpp @@ -0,0 +1,52 @@ +// Copyright (c) Sleipnir contributors + +#include +#include + +#include +#include + +#include "CmdlineArguments.hpp" + +sleipnir::OptimizationProblem FlywheelProblem(std::chrono::duration dt, + int N) { + // Flywheel model: + // States: [velocity] + // Inputs: [voltage] + Eigen::Matrix A{std::exp(-dt.count())}; + Eigen::Matrix B{1.0 - std::exp(-dt.count())}; + + sleipnir::OptimizationProblem problem; + auto X = problem.DecisionVariable(1, N + 1); + auto U = problem.DecisionVariable(1, N); + + // Dynamics constraint + for (int k = 0; k < N; ++k) { + problem.SubjectTo(X.Col(k + 1) == A * X.Col(k) + B * U.Col(k)); + } + + // State and input constraints + problem.SubjectTo(X.Col(0) == 0.0); + problem.SubjectTo(-12 <= U); + problem.SubjectTo(U <= 12); + + // Cost function - minimize error + Eigen::Matrix r{10.0}; + sleipnir::Variable J = 0.0; + for (int k = 0; k < N + 1; ++k) { + J += ((r - X.Col(k)).T() * (r - X.Col(k))); + } + problem.Minimize(J); + + return problem; +} + +int main(int argc, char* argv[]) { + using namespace std::chrono_literals; + + CmdlineArgs args{argv, argc}; + bool diagnostics = args.Contains("--enable-diagnostics"); + + auto problem = FlywheelProblem(5s, 5000); + problem.Solve({.diagnostics = diagnostics}); +} diff --git a/tools/perf-benchmark.sh b/tools/perf-benchmark.sh new file mode 100755 index 00000000..395f9f4e --- /dev/null +++ b/tools/perf-benchmark.sh @@ -0,0 +1,12 @@ +#!/bin/bash +set -e + +if [[ $# -ne 1 ]] || [[ "$1" != "CartPole" && "$1" != "Flywheel" ]]; then + echo "usage: ./tools/perf-benchmark.sh {CartPole,Flywheel}" + exit 1 +fi + +cmake -B build-perf -S . -DCMAKE_BUILD_TYPE=Perf -DBUILD_BENCHMARKING=ON +cmake --build build-perf --target $1PerfBenchmark +./tools/perf-record.sh ./build-perf/$1PerfBenchmark +./tools/perf-report.sh