From de718f7ae568087bc5cdeff4cdb63a395da2f42d Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 27 May 2025 07:24:15 -0700 Subject: [PATCH] [upstream_utils] Upgrade Sleipnir (#7973) --- upstream_utils/sleipnir.py | 39 +- .../sleipnir_patches/0001-Use-fmtlib.patch | 43 +- .../0002-Use-wpi-SmallVector.patch | 639 +----- ...-Suppress-clang-tidy-false-positives.patch | 31 - .../0003-Use-wpi-byteswap.patch | 41 + .../0004-Replace-std-to_underlying.patch | 63 + .../0005-Replace-std-views-zip.patch | 53 + ...-Suppress-clang-tidy-false-positives.patch | 22 + ...ppress-GCC-12-warning-false-positive.patch | 34 + ...dimensional-array-subscript-operator.patch | 915 +++++++++ .../native/cpp/controller/ArmFeedforward.cpp | 46 +- .../main/native/cpp/geometry/Ellipse2d.cpp | 28 +- .../native/thirdparty/sleipnir/.styleguide | 3 +- .../thirdparty/sleipnir/include/.styleguide | 5 +- .../sleipnir/include/gch/small_vector.hpp | 12 + .../include/sleipnir/autodiff/Expression.hpp | 1144 ----------- .../sleipnir/autodiff/ExpressionGraph.hpp | 244 --- .../sleipnir/autodiff/ExpressionType.hpp | 27 - .../include/sleipnir/autodiff/Hessian.hpp | 79 - .../include/sleipnir/autodiff/Jacobian.hpp | 155 -- .../include/sleipnir/autodiff/Profiler.hpp | 79 - .../sleipnir/autodiff/VariableBlock.hpp | 765 ------- .../sleipnir/autodiff/VariableMatrix.hpp | 1096 ---------- .../autodiff/adjoint_expression_graph.hpp | 178 ++ .../include/sleipnir/autodiff/expression.hpp | 1758 +++++++++++++++++ .../sleipnir/autodiff/expression_graph.hpp | 94 + .../sleipnir/autodiff/expression_type.hpp | 56 + .../autodiff/{Gradient.hpp => gradient.hpp} | 35 +- .../include/sleipnir/autodiff/hessian.hpp | 169 ++ .../include/sleipnir/autodiff/jacobian.hpp | 158 ++ .../autodiff/{Slice.hpp => slice.hpp} | 46 +- .../autodiff/{Variable.hpp => variable.hpp} | 324 +-- .../sleipnir/autodiff/variable_block.hpp | 872 ++++++++ .../sleipnir/autodiff/variable_matrix.hpp | 1283 ++++++++++++ .../control/{OCPSolver.hpp => ocp.hpp} | 342 ++-- .../optimization/OptimizationProblem.hpp | 386 ---- .../sleipnir/optimization/SolverConfig.hpp | 54 - .../optimization/SolverExitCondition.hpp | 80 - .../sleipnir/optimization/SolverStatus.hpp | 32 - .../{Multistart.hpp => multistart.hpp} | 50 +- .../include/sleipnir/optimization/problem.hpp | 342 ++++ .../optimization/solver/InteriorPoint.hpp | 55 - .../sleipnir/optimization/solver/SQP.hpp | 46 - .../optimization/solver/exit_status.hpp | 82 + .../optimization/solver/interior_point.hpp | 232 +++ .../iteration_info.hpp} | 11 +- .../sleipnir/optimization/solver/newton.hpp | 113 ++ .../sleipnir/optimization/solver/options.hpp | 94 + .../sleipnir/optimization/solver/sqp.hpp | 171 ++ .../sleipnir/include/sleipnir/util/Assert.hpp | 25 - .../include/sleipnir/util/Concepts.hpp | 33 - .../sleipnir/include/sleipnir/util/Spy.hpp | 89 - .../sleipnir/include/sleipnir/util/assert.hpp | 27 + .../include/sleipnir/util/concepts.hpp | 42 + .../{FunctionRef.hpp => function_ref.hpp} | 26 +- ...SharedPtr.hpp => intrusive_shared_ptr.hpp} | 142 +- .../sleipnir/util/{Pool.hpp => pool.hpp} | 93 +- .../sleipnir/util/{Print.hpp => print.hpp} | 19 +- .../sleipnir/include/sleipnir/util/spy.hpp | 127 ++ .../{SymbolExports.hpp => symbol_exports.hpp} | 0 .../thirdparty/sleipnir/src/.styleguide | 2 +- .../sleipnir/src/autodiff/VariableMatrix.cpp | 91 - .../sleipnir/src/autodiff/variable_matrix.cpp | 259 +++ .../sleipnir/src/optimization/Inertia.hpp | 58 - .../src/optimization/RegularizedLDLT.hpp | 179 -- .../sleipnir/src/optimization/bounds.hpp | 220 +++ .../sleipnir/src/optimization/inertia.hpp | 53 + .../sleipnir/src/optimization/problem.cpp | 391 ++++ .../src/optimization/regularized_ldlt.hpp | 227 +++ .../src/optimization/solver/InteriorPoint.cpp | 837 -------- .../sleipnir/src/optimization/solver/SQP.cpp | 559 ------ .../optimization/solver/interior_point.cpp | 664 +++++++ .../src/optimization/solver/newton.cpp | 259 +++ .../sleipnir/src/optimization/solver/sqp.cpp | 480 +++++ .../solver/util/FeasibilityRestoration.hpp | 400 ---- .../src/optimization/solver/util/Filter.hpp | 183 -- .../{ErrorEstimate.hpp => error_estimate.hpp} | 57 +- .../src/optimization/solver/util/filter.hpp | 197 ++ ....hpp => fraction_to_the_boundary_rule.hpp} | 6 +- ...feasible.hpp => is_locally_infeasible.hpp} | 10 +- .../util/{KKTError.hpp => kkt_error.hpp} | 42 +- .../thirdparty/sleipnir/src/util/Pool.cpp | 12 - .../sleipnir/src/util/ToMilliseconds.hpp | 21 - .../thirdparty/sleipnir/src/util/pool.cpp | 12 + .../sleipnir/src/util/print_diagnostics.hpp | 357 ++++ .../util/{ScopeExit.hpp => scope_exit.hpp} | 4 +- .../sleipnir/src/util/scoped_profiler.hpp | 78 + .../sleipnir/src/util/setup_profiler.hpp | 68 + .../sleipnir/src/util/solve_profiler.hpp | 105 + wpimath/src/test/native/cpp/SleipnirTest.cpp | 25 +- 90 files changed, 11188 insertions(+), 7917 deletions(-) delete mode 100644 upstream_utils/sleipnir_patches/0003-Suppress-clang-tidy-false-positives.patch create mode 100644 upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch create mode 100644 upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch create mode 100644 upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch create mode 100644 upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch create mode 100644 upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch create mode 100644 upstream_utils/sleipnir_patches/0008-Revert-Use-multidimensional-array-subscript-operator.patch create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/gch/small_vector.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionGraph.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionType.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Hessian.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Jacobian.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Profiler.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/adjoint_expression_graph.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_type.hpp rename wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/{Gradient.hpp => gradient.hpp} (61%) create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp rename wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/{Slice.hpp => slice.hpp} (79%) rename wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/{Variable.hpp => variable.hpp} (67%) create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_block.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp rename wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/{OCPSolver.hpp => ocp.hpp} (50%) delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverConfig.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverStatus.hpp rename wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/{Multistart.hpp => multistart.hpp} (54%) create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/InteriorPoint.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/exit_status.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp rename wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/{SolverIterationInfo.hpp => solver/iteration_info.hpp} (72%) create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/options.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Assert.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Concepts.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Spy.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/concepts.hpp rename wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/{FunctionRef.hpp => function_ref.hpp} (86%) rename wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/{IntrusiveSharedPtr.hpp => intrusive_shared_ptr.hpp} (60%) rename wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/{Pool.hpp => pool.hpp} (58%) rename wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/{Print.hpp => print.hpp} (81%) create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/spy.hpp rename wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/{SymbolExports.hpp => symbol_exports.hpp} (100%) delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/VariableMatrix.cpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/variable_matrix.cpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/optimization/Inertia.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/optimization/RegularizedLDLT.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/optimization/bounds.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/optimization/inertia.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/optimization/problem.cpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/optimization/regularized_ldlt.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/interior_point.cpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/newton.cpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/sqp.cpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp rename wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/{ErrorEstimate.hpp => error_estimate.hpp} (66%) create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/filter.hpp rename wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/{FractionToTheBoundaryRule.hpp => fraction_to_the_boundary_rule.hpp} (92%) rename wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/{IsLocallyInfeasible.hpp => is_locally_infeasible.hpp} (87%) rename wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/{KKTError.hpp => kkt_error.hpp} (61%) delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/util/Pool.cpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/util/ToMilliseconds.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/util/pool.cpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/util/print_diagnostics.hpp rename wpimath/src/main/native/thirdparty/sleipnir/src/util/{ScopeExit.hpp => scope_exit.hpp} (92%) create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/util/scoped_profiler.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/util/setup_profiler.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/util/solve_profiler.hpp diff --git a/upstream_utils/sleipnir.py b/upstream_utils/sleipnir.py index ac25370ff8..ba42c3fd78 100755 --- a/upstream_utils/sleipnir.py +++ b/upstream_utils/sleipnir.py @@ -24,10 +24,7 @@ def copy_upstream_src(wpilib_root): # Copy Sleipnir header files into allwpilib include_files = [ - os.path.join(dp, f) - for dp, dn, fn in os.walk("include") - for f in fn - if not f.endswith("small_vector.hpp") + os.path.join(dp, f) for dp, dn, fn in os.walk("include") for f in fn ] include_files = copy_to( include_files, os.path.join(wpimath, "src/main/native/thirdparty/sleipnir") @@ -44,12 +41,42 @@ def copy_upstream_src(wpilib_root): os.path.join(wpimath, "src/main/native/thirdparty/sleipnir", filename), ) + # Write shim for wpi::SmallVector + try: + os.mkdir( + os.path.join(wpimath, "src/main/native/thirdparty/sleipnir/include/gch") + ) + except: + pass + with open( + os.path.join( + wpimath, + "src/main/native/thirdparty/sleipnir/include/gch/small_vector.hpp", + ), + "w", + ) as f: + f.write( + """// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +namespace gch { + +template +using small_vector = wpi::SmallVector; + +} // namespace gch +""" + ) + def main(): name = "sleipnir" url = "https://github.com/SleipnirGroup/Sleipnir" - # main on 2024-12-07 - tag = "01206ab17d741f4c45a7faeb56b8a5442df1681c" + # main on 2025-05-18 + tag = "2cc18ff6d25ee0a9bd0f9993a0a41a61a28bda3e" sleipnir = Lib(name, url, tag, copy_upstream_src) sleipnir.main() diff --git a/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch b/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch index deaaf7b5cb..2baae9b2ab 100644 --- a/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch +++ b/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch @@ -1,30 +1,32 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 29 May 2024 16:29:55 -0700 -Subject: [PATCH 1/3] Use fmtlib +Subject: [PATCH 1/8] Use fmtlib --- include/.styleguide | 1 + - include/sleipnir/util/Print.hpp | 31 ++++++++++++++++++------------- - 2 files changed, 19 insertions(+), 13 deletions(-) + include/sleipnir/util/print.hpp | 31 ++++++++++++++++++------------- + src/optimization/problem.cpp | 2 +- + 3 files changed, 20 insertions(+), 14 deletions(-) diff --git a/include/.styleguide b/include/.styleguide -index 8fb61fdf9cc5ceff633d3126f0579eef25a1326f..6a7f8ed28f9cb037c9746a7e0ef5e110481d9825 100644 +index 1b6652d3d5886cf8c9eca0d855c21031775bad7c..4f4c76204071f90bf49eddb8c2aceb583b5e09ba 100644 --- a/include/.styleguide +++ b/include/.styleguide -@@ -12,4 +12,5 @@ licenseUpdateExclude { +@@ -8,5 +8,6 @@ cppSrcFileInclude { includeOtherLibs { ^Eigen/ + ^fmt/ + ^gch/ } -diff --git a/include/sleipnir/util/Print.hpp b/include/sleipnir/util/Print.hpp -index a746cb77b70f095bb15f4c493295cb925bc74cd3..c01fd4ac679df854b885293d681ea1e0984626fa 100644 ---- a/include/sleipnir/util/Print.hpp -+++ b/include/sleipnir/util/Print.hpp -@@ -3,52 +3,57 @@ - #pragma once +diff --git a/include/sleipnir/util/print.hpp b/include/sleipnir/util/print.hpp +index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b1159aeb2a57 100644 +--- a/include/sleipnir/util/print.hpp ++++ b/include/sleipnir/util/print.hpp +@@ -4,10 +4,15 @@ + #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS #include -#include #include @@ -36,7 +38,11 @@ index a746cb77b70f095bb15f4c493295cb925bc74cd3..c01fd4ac679df854b885293d681ea1e0 +#include +#endif + - namespace sleipnir { + #endif + + namespace slp { +@@ -15,45 +20,45 @@ namespace slp { + #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS /** - * Wrapper around std::print() that squelches write failure exceptions. @@ -93,3 +99,16 @@ index a746cb77b70f095bb15f4c493295cb925bc74cd3..c01fd4ac679df854b885293d681ea1e0 } catch (const std::system_error&) { } } +diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp +index 31115490867146ec166604bcc61731d7891a9f22..81863808d329a53d4162ce0624a3b8e8afc32dfc 100644 +--- a/src/optimization/problem.cpp ++++ b/src/optimization/problem.cpp +@@ -335,7 +335,7 @@ void Problem::print_exit_conditions([[maybe_unused]] const Options& options) { + slp::println(" ↳ executed {} iterations", options.max_iterations); + } + if (std::isfinite(options.timeout.count())) { +- slp::println(" ↳ {} elapsed", options.timeout); ++ slp::println(" ↳ {} elapsed", options.timeout.count()); + } + } + diff --git a/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch index 6b816784a7..df21b64a10 100644 --- a/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch +++ b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch @@ -1,610 +1,77 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 16 Jun 2024 12:08:49 -0700 -Subject: [PATCH 2/3] Use wpi::SmallVector +Subject: [PATCH 2/8] Use wpi::SmallVector --- - include/.styleguide | 1 + - include/sleipnir/autodiff/Expression.hpp | 13 +++++++------ - include/sleipnir/autodiff/ExpressionGraph.hpp | 15 ++++++++------- - include/sleipnir/autodiff/Hessian.hpp | 4 ++-- - include/sleipnir/autodiff/Jacobian.hpp | 10 +++++----- - include/sleipnir/autodiff/Variable.hpp | 10 +++++----- - include/sleipnir/autodiff/VariableMatrix.hpp | 4 ++-- - include/sleipnir/optimization/Multistart.hpp | 7 ++++--- - .../optimization/OptimizationProblem.hpp | 8 ++++---- - include/sleipnir/util/Pool.hpp | 7 ++++--- - include/sleipnir/util/Spy.hpp | 4 ++-- - src/.styleguide | 1 + - src/optimization/solver/InteriorPoint.cpp | 4 ++-- - src/optimization/solver/SQP.cpp | 4 ++-- - .../solver/util/FeasibilityRestoration.hpp | 18 +++++++++--------- - src/optimization/solver/util/Filter.hpp | 4 ++-- - 16 files changed, 60 insertions(+), 54 deletions(-) + include/sleipnir/autodiff/expression.hpp | 4 ++-- + include/sleipnir/autodiff/variable.hpp | 5 ++--- + include/sleipnir/autodiff/variable_matrix.hpp | 4 ++-- + 3 files changed, 6 insertions(+), 7 deletions(-) -diff --git a/include/.styleguide b/include/.styleguide -index 6a7f8ed28f9cb037c9746a7e0ef5e110481d9825..efa36cee1fb593ae810032340c64f1854fbbc523 100644 ---- a/include/.styleguide -+++ b/include/.styleguide -@@ -13,4 +13,5 @@ licenseUpdateExclude { - includeOtherLibs { - ^Eigen/ - ^fmt/ -+ ^wpi/ - } -diff --git a/include/sleipnir/autodiff/Expression.hpp b/include/sleipnir/autodiff/Expression.hpp -index dd53755ccae88e3975d1b5e6b13ac464bd4efcce..ef9a15bf69d8cae6b2196513b72ec4b359cc8752 100644 ---- a/include/sleipnir/autodiff/Expression.hpp -+++ b/include/sleipnir/autodiff/Expression.hpp -@@ -11,11 +11,12 @@ - #include - #include +diff --git a/include/sleipnir/autodiff/expression.hpp b/include/sleipnir/autodiff/expression.hpp +index 873e1c27559d92eb1b3a217890ca41bdc65af122..1c5f84d22a0bed70869121acabd527825ba90adb 100644 +--- a/include/sleipnir/autodiff/expression.hpp ++++ b/include/sleipnir/autodiff/expression.hpp +@@ -30,7 +30,7 @@ inline constexpr bool USE_POOL_ALLOCATOR = true; + struct Expression; -+#include -+ - #include "sleipnir/autodiff/ExpressionType.hpp" - #include "sleipnir/util/IntrusiveSharedPtr.hpp" - #include "sleipnir/util/Pool.hpp" - #include "sleipnir/util/SymbolExports.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir::detail { - -@@ -29,8 +30,8 @@ inline constexpr bool kUsePoolAllocator = true; - - struct SLEIPNIR_DLLEXPORT Expression; - --inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr); --inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr); -+inline void IntrusiveSharedPtrIncRefCount(Expression* expr); -+inline void IntrusiveSharedPtrDecRefCount(Expression* expr); + inline constexpr void inc_ref_count(Expression* expr); +-inline constexpr void dec_ref_count(Expression* expr); ++inline void dec_ref_count(Expression* expr); /** * Typedef for intrusive shared pointer to Expression. -@@ -418,7 +419,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt(const ExpressionPtr& x); +@@ -680,7 +680,7 @@ inline constexpr void inc_ref_count(Expression* expr) { * * @param expr The shared pointer's managed object. */ --inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) { -+inline void IntrusiveSharedPtrIncRefCount(Expression* expr) { - ++expr->refCount; - } - -@@ -427,12 +428,12 @@ inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) { - * - * @param expr The shared pointer's managed object. - */ --inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr) { -+inline void IntrusiveSharedPtrDecRefCount(Expression* expr) { +-inline constexpr void dec_ref_count(Expression* expr) { ++inline void dec_ref_count(Expression* expr) { // If a deeply nested tree is being deallocated all at once, calling the // Expression destructor when expr's refcount reaches zero can cause a stack // overflow. Instead, we iterate over its children to decrement their - // refcounts and deallocate them. -- small_vector stack; -+ wpi::SmallVector stack; - stack.emplace_back(expr); - - while (!stack.empty()) { -diff --git a/include/sleipnir/autodiff/ExpressionGraph.hpp b/include/sleipnir/autodiff/ExpressionGraph.hpp -index c614195d82ad022dfd0c3f6f8240b042c0056c2f..714bcbb82907e754138347334c7fca8a7ccf055d 100644 ---- a/include/sleipnir/autodiff/ExpressionGraph.hpp -+++ b/include/sleipnir/autodiff/ExpressionGraph.hpp -@@ -4,10 +4,11 @@ - - #include - -+#include -+ - #include "sleipnir/autodiff/Expression.hpp" - #include "sleipnir/util/FunctionRef.hpp" - #include "sleipnir/util/SymbolExports.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir::detail { - -@@ -36,7 +37,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph { - // https://en.wikipedia.org/wiki/Breadth-first_search - - // BFS list sorted from parent to child. -- small_vector stack; -+ wpi::SmallVector stack; - - stack.emplace_back(root.Get()); - -@@ -119,7 +120,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph { - * - * @param wrt Variables with respect to which to compute the gradient. +diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp +index 14eb1d3b95069e143699e1488f3081c4cd9de07c..9f79a82763213dc712cce4c2a322289d57645032 100644 +--- a/include/sleipnir/autodiff/variable.hpp ++++ b/include/sleipnir/autodiff/variable.hpp +@@ -47,7 +47,7 @@ class SLEIPNIR_DLLEXPORT Variable { + /** + * Constructs an empty Variable. */ -- small_vector GenerateGradientTree( -+ wpi::SmallVector GenerateGradientTree( - std::span wrt) const { - // Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation - // for background on reverse accumulation automatic differentiation. -@@ -128,7 +129,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph { - wrt[row]->row = row; - } - -- small_vector grad; -+ wpi::SmallVector grad; - grad.reserve(wrt.size()); - for (size_t row = 0; row < wrt.size(); ++row) { - grad.emplace_back(MakeExpressionPtr()); -@@ -231,13 +232,13 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph { - - private: - // List that maps nodes to their respective row. -- small_vector m_rowList; -+ wpi::SmallVector m_rowList; - - // List for updating adjoints -- small_vector m_adjointList; -+ wpi::SmallVector m_adjointList; - - // List for updating values -- small_vector m_valueList; -+ wpi::SmallVector m_valueList; - }; - - } // namespace sleipnir::detail -diff --git a/include/sleipnir/autodiff/Hessian.hpp b/include/sleipnir/autodiff/Hessian.hpp -index 4563aa234bd7b0ec22e12d2fc0b6f4569bee7f39..2e60d89e95280bdac422b0d7dab955ba111b0059 100644 ---- a/include/sleipnir/autodiff/Hessian.hpp -+++ b/include/sleipnir/autodiff/Hessian.hpp -@@ -6,6 +6,7 @@ - - #include - #include -+#include - - #include "sleipnir/autodiff/ExpressionGraph.hpp" - #include "sleipnir/autodiff/Jacobian.hpp" -@@ -13,7 +14,6 @@ - #include "sleipnir/autodiff/Variable.hpp" - #include "sleipnir/autodiff/VariableMatrix.hpp" - #include "sleipnir/util/SymbolExports.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir { - -@@ -36,7 +36,7 @@ class SLEIPNIR_DLLEXPORT Hessian { - Hessian(Variable variable, const VariableMatrix& wrt) noexcept - : m_jacobian{ - [&] { -- small_vector wrtVec; -+ wpi::SmallVector wrtVec; - wrtVec.reserve(wrt.size()); - for (auto& elem : wrt) { - wrtVec.emplace_back(elem.expr); -diff --git a/include/sleipnir/autodiff/Jacobian.hpp b/include/sleipnir/autodiff/Jacobian.hpp -index ac00c11ef8c947cbe95c58081bdbfb42bf901051..0c660156c80f94539383b8f0d01d7853e41e0297 100644 ---- a/include/sleipnir/autodiff/Jacobian.hpp -+++ b/include/sleipnir/autodiff/Jacobian.hpp -@@ -5,13 +5,13 @@ - #include - - #include -+#include - - #include "sleipnir/autodiff/ExpressionGraph.hpp" - #include "sleipnir/autodiff/Profiler.hpp" - #include "sleipnir/autodiff/Variable.hpp" - #include "sleipnir/autodiff/VariableMatrix.hpp" - #include "sleipnir/util/SymbolExports.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir { - -@@ -81,7 +81,7 @@ class SLEIPNIR_DLLEXPORT Jacobian { - VariableMatrix Get() const { - VariableMatrix result{m_variables.Rows(), m_wrt.Rows()}; - -- small_vector wrtVec; -+ wpi::SmallVector wrtVec; - wrtVec.reserve(m_wrt.size()); - for (auto& elem : m_wrt) { - wrtVec.emplace_back(elem.expr); -@@ -138,16 +138,16 @@ class SLEIPNIR_DLLEXPORT Jacobian { - VariableMatrix m_variables; - VariableMatrix m_wrt; - -- small_vector m_graphs; -+ wpi::SmallVector m_graphs; - - Eigen::SparseMatrix m_J{m_variables.Rows(), m_wrt.Rows()}; - - // Cached triplets for gradients of linear rows -- small_vector> m_cachedTriplets; -+ wpi::SmallVector> m_cachedTriplets; - - // List of row indices for nonlinear rows whose graients will be computed in - // Value() -- small_vector m_nonlinearRows; -+ wpi::SmallVector m_nonlinearRows; - - Profiler m_profiler; - }; -diff --git a/include/sleipnir/autodiff/Variable.hpp b/include/sleipnir/autodiff/Variable.hpp -index c04d629f482efe59497570ca1fd9b09a4af2ae1e..d192fb96e7984b7c0ca30262668c41e5e84ca34e 100644 ---- a/include/sleipnir/autodiff/Variable.hpp -+++ b/include/sleipnir/autodiff/Variable.hpp -@@ -10,6 +10,7 @@ - #include - - #include -+#include - - #include "sleipnir/autodiff/Expression.hpp" - #include "sleipnir/autodiff/ExpressionGraph.hpp" -@@ -17,7 +18,6 @@ - #include "sleipnir/util/Concepts.hpp" - #include "sleipnir/util/Print.hpp" - #include "sleipnir/util/SymbolExports.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir { - -@@ -445,8 +445,8 @@ template - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) --small_vector MakeConstraints(LHS&& lhs, RHS&& rhs) { -- small_vector constraints; -+wpi::SmallVector MakeConstraints(LHS&& lhs, RHS&& rhs) { -+ wpi::SmallVector constraints; - - if constexpr (ScalarLike> && - ScalarLike>) { -@@ -534,7 +534,7 @@ small_vector MakeConstraints(LHS&& lhs, RHS&& rhs) { - */ - struct SLEIPNIR_DLLEXPORT EqualityConstraints { - /// A vector of scalar equality constraints. -- small_vector constraints; -+ wpi::SmallVector constraints; +- explicit constexpr Variable(std::nullptr_t) : expr{nullptr} {} ++ explicit Variable(std::nullptr_t) : expr{nullptr} {} /** - * Concatenates multiple equality constraints. -@@ -596,7 +596,7 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints { - */ - struct SLEIPNIR_DLLEXPORT InequalityConstraints { - /// A vector of scalar inequality constraints. -- small_vector constraints; -+ wpi::SmallVector constraints; + * Constructs a Variable from a floating point type. +@@ -77,8 +77,7 @@ class SLEIPNIR_DLLEXPORT Variable { + * + * @param expr The autodiff variable. + */ +- explicit constexpr Variable(detail::ExpressionPtr&& expr) +- : expr{std::move(expr)} {} ++ explicit Variable(detail::ExpressionPtr&& expr) : expr{std::move(expr)} {} /** - * Concatenates multiple inequality constraints. -diff --git a/include/sleipnir/autodiff/VariableMatrix.hpp b/include/sleipnir/autodiff/VariableMatrix.hpp -index 47452b8988b3a1a96a78d28644200b1c4cdc89c9..57b09913d15e9590873ad7bf62e2baff9fbc5df9 100644 ---- a/include/sleipnir/autodiff/VariableMatrix.hpp -+++ b/include/sleipnir/autodiff/VariableMatrix.hpp -@@ -11,6 +11,7 @@ - #include - - #include -+#include - - #include "sleipnir/autodiff/Slice.hpp" - #include "sleipnir/autodiff/Variable.hpp" -@@ -18,7 +19,6 @@ - #include "sleipnir/util/Assert.hpp" - #include "sleipnir/util/FunctionRef.hpp" - #include "sleipnir/util/SymbolExports.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir { - -@@ -946,7 +946,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { - } - - private: -- small_vector m_storage; -+ wpi::SmallVector m_storage; - int m_rows = 0; - int m_cols = 0; - }; -diff --git a/include/sleipnir/optimization/Multistart.hpp b/include/sleipnir/optimization/Multistart.hpp -index 8055713a2492a9c8473f047a2fb9fe7ca57753c3..09b1b2f3bf33c35ae0aeddb9b5d47c0d80c68cec 100644 ---- a/include/sleipnir/optimization/Multistart.hpp -+++ b/include/sleipnir/optimization/Multistart.hpp -@@ -6,9 +6,10 @@ - #include - #include - -+#include -+ - #include "sleipnir/optimization/SolverStatus.hpp" - #include "sleipnir/util/FunctionRef.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir { - -@@ -44,14 +45,14 @@ MultistartResult Multistart( - const DecisionVariables& initialGuess)> - solve, - std::span initialGuesses) { -- small_vector>> futures; -+ wpi::SmallVector>> futures; - futures.reserve(initialGuesses.size()); - - for (const auto& initialGuess : initialGuesses) { - futures.emplace_back(std::async(std::launch::async, solve, initialGuess)); - } - -- small_vector> results; -+ wpi::SmallVector> results; - results.reserve(futures.size()); - - for (auto& future : futures) { -diff --git a/include/sleipnir/optimization/OptimizationProblem.hpp b/include/sleipnir/optimization/OptimizationProblem.hpp -index 569dcdee507881ceb412585ca811927072552c15..66883fed98ad087010fb153bd91effce6e047928 100644 ---- a/include/sleipnir/optimization/OptimizationProblem.hpp -+++ b/include/sleipnir/optimization/OptimizationProblem.hpp -@@ -11,6 +11,7 @@ - #include - - #include -+#include - - #include "sleipnir/autodiff/Variable.hpp" - #include "sleipnir/autodiff/VariableMatrix.hpp" -@@ -22,7 +23,6 @@ - #include "sleipnir/optimization/solver/SQP.hpp" - #include "sleipnir/util/Print.hpp" - #include "sleipnir/util/SymbolExports.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir { - -@@ -364,16 +364,16 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem { - private: - // The list of decision variables, which are the root of the problem's - // expression tree -- small_vector m_decisionVariables; -+ wpi::SmallVector m_decisionVariables; - - // The cost function: f(x) - std::optional m_f; - - // The list of equality constraints: cₑ(x) = 0 -- small_vector m_equalityConstraints; -+ wpi::SmallVector m_equalityConstraints; - - // The list of inequality constraints: cᵢ(x) ≥ 0 -- small_vector m_inequalityConstraints; -+ wpi::SmallVector m_inequalityConstraints; - - // The user callback - std::function m_callback = -diff --git a/include/sleipnir/util/Pool.hpp b/include/sleipnir/util/Pool.hpp -index 441fa701d4972bc14973c6269d56d4a124deaee5..1951bd1ee8f3bee8d4a3c044c99354b0fd10031d 100644 ---- a/include/sleipnir/util/Pool.hpp -+++ b/include/sleipnir/util/Pool.hpp -@@ -5,8 +5,9 @@ - #include - #include - -+#include -+ - #include "sleipnir/util/SymbolExports.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir { - -@@ -78,8 +79,8 @@ class SLEIPNIR_DLLEXPORT PoolResource { - } - - private: -- small_vector> m_buffer; -- small_vector m_freeList; -+ wpi::SmallVector> m_buffer; -+ wpi::SmallVector m_freeList; - size_t blocksPerChunk; + * Assignment operator for double. +diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp +index 410f12873cfdf5d0d484653c6c3dac74ed96348a..1c6f9e8dade8bebce7aec18bbb9b5491acb1d977 100644 +--- a/include/sleipnir/autodiff/variable_matrix.hpp ++++ b/include/sleipnir/autodiff/variable_matrix.hpp +@@ -1120,14 +1120,14 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + * + * @return Begin iterator. + */ +- const_iterator cbegin() const { return const_iterator{m_storage.cbegin()}; } ++ const_iterator cbegin() const { return const_iterator{m_storage.begin()}; } /** -diff --git a/include/sleipnir/util/Spy.hpp b/include/sleipnir/util/Spy.hpp -index cb9b4e191545e96c2bade5f8f99b0bec376b656b..7f526a2d9968e76b385a0ddfb2edf5bab7274fb0 100644 ---- a/include/sleipnir/util/Spy.hpp -+++ b/include/sleipnir/util/Spy.hpp -@@ -7,9 +7,9 @@ - #include + * Returns end iterator. + * + * @return End iterator. + */ +- const_iterator cend() const { return const_iterator{m_storage.cend()}; } ++ const_iterator cend() const { return const_iterator{m_storage.end()}; } - #include -+#include - - #include "sleipnir/util/SymbolExports.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir { - -@@ -32,7 +32,7 @@ SLEIPNIR_DLLEXPORT inline void Spy(std::ostream& file, - const int cells_width = mat.cols() + 1; - const int cells_height = mat.rows(); - -- small_vector cells; -+ wpi::SmallVector cells; - - // Allocate space for matrix of characters plus trailing newlines - cells.reserve(cells_width * cells_height); -diff --git a/src/.styleguide b/src/.styleguide -index f3b2f0cf9e60b3a86b9654ff2b381f9c48734ff6..ad739cea6dce6f6cb586f538d1d30b92503484c1 100644 ---- a/src/.styleguide -+++ b/src/.styleguide -@@ -8,4 +8,5 @@ cppSrcFileInclude { - - includeOtherLibs { - ^Eigen/ -+ ^wpi/ - } -diff --git a/src/optimization/solver/InteriorPoint.cpp b/src/optimization/solver/InteriorPoint.cpp -index a09d9866d05731c8ce53122b3d1a850803883df4..d3981c59d163927e3e5ba602c3323f6e1429c475 100644 ---- a/src/optimization/solver/InteriorPoint.cpp -+++ b/src/optimization/solver/InteriorPoint.cpp -@@ -9,6 +9,7 @@ - #include - - #include -+#include - - #include "optimization/RegularizedLDLT.hpp" - #include "optimization/solver/util/ErrorEstimate.hpp" -@@ -23,7 +24,6 @@ - #include "sleipnir/optimization/SolverExitCondition.hpp" - #include "sleipnir/util/Print.hpp" - #include "sleipnir/util/Spy.hpp" --#include "sleipnir/util/small_vector.hpp" - #include "util/ScopeExit.hpp" - #include "util/ToMilliseconds.hpp" - -@@ -226,7 +226,7 @@ void InteriorPoint(std::span decisionVariables, - }; - - // Kept outside the loop so its storage can be reused -- small_vector> triplets; -+ wpi::SmallVector> triplets; - - RegularizedLDLT solver; - -diff --git a/src/optimization/solver/SQP.cpp b/src/optimization/solver/SQP.cpp -index 77b9632e1da37361c995a8579d1d83a2756d6d88..662abc2fb6e311767b0fbb3a61121ce78549a3f6 100644 ---- a/src/optimization/solver/SQP.cpp -+++ b/src/optimization/solver/SQP.cpp -@@ -9,6 +9,7 @@ - #include - - #include -+#include - - #include "optimization/RegularizedLDLT.hpp" - #include "optimization/solver/util/ErrorEstimate.hpp" -@@ -22,7 +23,6 @@ - #include "sleipnir/optimization/SolverExitCondition.hpp" - #include "sleipnir/util/Print.hpp" - #include "sleipnir/util/Spy.hpp" --#include "sleipnir/util/small_vector.hpp" - #include "util/ScopeExit.hpp" - #include "util/ToMilliseconds.hpp" - -@@ -155,7 +155,7 @@ void SQP(std::span decisionVariables, - Filter filter{f}; - - // Kept outside the loop so its storage can be reused -- small_vector> triplets; -+ wpi::SmallVector> triplets; - - RegularizedLDLT solver; - -diff --git a/src/optimization/solver/util/FeasibilityRestoration.hpp b/src/optimization/solver/util/FeasibilityRestoration.hpp -index feefe137adf0832b094a36d61201b15962138ded..79b5d99ae27de6049ba098888a965291e6b677fa 100644 ---- a/src/optimization/solver/util/FeasibilityRestoration.hpp -+++ b/src/optimization/solver/util/FeasibilityRestoration.hpp -@@ -8,6 +8,7 @@ - #include - - #include -+#include - - #include "sleipnir/autodiff/Variable.hpp" - #include "sleipnir/autodiff/VariableMatrix.hpp" -@@ -16,7 +17,6 @@ - #include "sleipnir/optimization/SolverStatus.hpp" - #include "sleipnir/optimization/solver/InteriorPoint.hpp" - #include "sleipnir/util/FunctionRef.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir { - -@@ -57,7 +57,7 @@ inline void FeasibilityRestoration( - constexpr double ρ = 1000.0; - double μ = config.tolerance / 10.0; - -- small_vector fr_decisionVariables; -+ wpi::SmallVector fr_decisionVariables; - fr_decisionVariables.reserve(decisionVariables.size() + - 2 * equalityConstraints.size()); - -@@ -70,7 +70,7 @@ inline void FeasibilityRestoration( - fr_decisionVariables.emplace_back(); - } - -- auto it = fr_decisionVariables.cbegin(); -+ auto it = fr_decisionVariables.begin(); - - VariableMatrix xAD{std::span{it, it + decisionVariables.size()}}; - it += decisionVariables.size(); -@@ -128,7 +128,7 @@ inline void FeasibilityRestoration( - } - - // cₑ(x) - pₑ + nₑ = 0 -- small_vector fr_equalityConstraints; -+ wpi::SmallVector fr_equalityConstraints; - fr_equalityConstraints.assign(equalityConstraints.begin(), - equalityConstraints.end()); - for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) { -@@ -137,7 +137,7 @@ inline void FeasibilityRestoration( - } - - // cᵢ(x) - s - pᵢ + nᵢ = 0 -- small_vector fr_inequalityConstraints; -+ wpi::SmallVector fr_inequalityConstraints; - - // pₑ ≥ 0 - std::copy(p_e.begin(), p_e.end(), -@@ -227,7 +227,7 @@ inline void FeasibilityRestoration( - - constexpr double ρ = 1000.0; - -- small_vector fr_decisionVariables; -+ wpi::SmallVector fr_decisionVariables; - fr_decisionVariables.reserve(decisionVariables.size() + - 2 * equalityConstraints.size() + - 2 * inequalityConstraints.size()); -@@ -243,7 +243,7 @@ inline void FeasibilityRestoration( - fr_decisionVariables.emplace_back(); - } - -- auto it = fr_decisionVariables.cbegin(); -+ auto it = fr_decisionVariables.begin(); - - VariableMatrix xAD{std::span{it, it + decisionVariables.size()}}; - it += decisionVariables.size(); -@@ -319,7 +319,7 @@ inline void FeasibilityRestoration( - } - - // cₑ(x) - pₑ + nₑ = 0 -- small_vector fr_equalityConstraints; -+ wpi::SmallVector fr_equalityConstraints; - fr_equalityConstraints.assign(equalityConstraints.begin(), - equalityConstraints.end()); - for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) { -@@ -328,7 +328,7 @@ inline void FeasibilityRestoration( - } - - // cᵢ(x) - s - pᵢ + nᵢ = 0 -- small_vector fr_inequalityConstraints; -+ wpi::SmallVector fr_inequalityConstraints; - fr_inequalityConstraints.assign(inequalityConstraints.begin(), - inequalityConstraints.end()); - for (size_t row = 0; row < fr_inequalityConstraints.size(); ++row) { -diff --git a/src/optimization/solver/util/Filter.hpp b/src/optimization/solver/util/Filter.hpp -index f19236928c59623bc0a3ce87b659f0756997f821..0c14efd7b8afa6cef398f5a7d383c54dbf64ec68 100644 ---- a/src/optimization/solver/util/Filter.hpp -+++ b/src/optimization/solver/util/Filter.hpp -@@ -8,9 +8,9 @@ - #include - - #include -+#include - - #include "sleipnir/autodiff/Variable.hpp" --#include "sleipnir/util/small_vector.hpp" - - // See docs/algorithms.md#Works_cited for citation definitions. - -@@ -177,7 +177,7 @@ class Filter { - - private: - Variable* m_f = nullptr; -- small_vector m_filter; -+ wpi::SmallVector m_filter; - }; - - } // namespace sleipnir + /** + * Returns number of elements in matrix. diff --git a/upstream_utils/sleipnir_patches/0003-Suppress-clang-tidy-false-positives.patch b/upstream_utils/sleipnir_patches/0003-Suppress-clang-tidy-false-positives.patch deleted file mode 100644 index 90ae6e261b..0000000000 --- a/upstream_utils/sleipnir_patches/0003-Suppress-clang-tidy-false-positives.patch +++ /dev/null @@ -1,31 +0,0 @@ -From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 -From: Tyler Veness -Date: Wed, 26 Jun 2024 12:13:33 -0700 -Subject: [PATCH 3/3] Suppress clang-tidy false positives - ---- - include/sleipnir/autodiff/Variable.hpp | 4 ++-- - 1 file changed, 2 insertions(+), 2 deletions(-) - -diff --git a/include/sleipnir/autodiff/Variable.hpp b/include/sleipnir/autodiff/Variable.hpp -index d192fb96e7984b7c0ca30262668c41e5e84ca34e..f25c6d153310a01700ee2390ecf35ffa8af7df11 100644 ---- a/include/sleipnir/autodiff/Variable.hpp -+++ b/include/sleipnir/autodiff/Variable.hpp -@@ -541,7 +541,7 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints { - * - * @param equalityConstraints The list of EqualityConstraints to concatenate. - */ -- EqualityConstraints( -+ EqualityConstraints( // NOLINT - std::initializer_list equalityConstraints) { - for (const auto& elem : equalityConstraints) { - constraints.insert(constraints.end(), elem.constraints.begin(), -@@ -604,7 +604,7 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints { - * @param inequalityConstraints The list of InequalityConstraints to - * concatenate. - */ -- InequalityConstraints( -+ InequalityConstraints( // NOLINT - std::initializer_list inequalityConstraints) { - for (const auto& elem : inequalityConstraints) { - constraints.insert(constraints.end(), elem.constraints.begin(), diff --git a/upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch b/upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch new file mode 100644 index 0000000000..02eb660bc0 --- /dev/null +++ b/upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch @@ -0,0 +1,41 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +Date: Tue, 28 Jan 2025 22:19:14 -0800 +Subject: [PATCH 3/8] Use wpi::byteswap() + +--- + include/.styleguide | 1 + + include/sleipnir/util/spy.hpp | 3 ++- + 2 files changed, 3 insertions(+), 1 deletion(-) + +diff --git a/include/.styleguide b/include/.styleguide +index 4f4c76204071f90bf49eddb8c2aceb583b5e09ba..03938557c2600a7a1f72c6b93c935602f5acb2b2 100644 +--- a/include/.styleguide ++++ b/include/.styleguide +@@ -10,4 +10,5 @@ includeOtherLibs { + ^Eigen/ + ^fmt/ + ^gch/ ++ ^wpi/ + } +diff --git a/include/sleipnir/util/spy.hpp b/include/sleipnir/util/spy.hpp +index a2f94803e3744cee771669210d1af883160e9896..8cd7d4353aad20153af5cd7a818fa55889d35721 100644 +--- a/include/sleipnir/util/spy.hpp ++++ b/include/sleipnir/util/spy.hpp +@@ -12,6 +12,7 @@ + #include + + #include ++#include + + #include "sleipnir/util/symbol_exports.hpp" + +@@ -115,7 +116,7 @@ class SLEIPNIR_DLLEXPORT Spy { + */ + void write32le(int32_t num) { + if constexpr (std::endian::native != std::endian::little) { +- num = std::byteswap(num); ++ num = wpi::byteswap(num); + } + m_file.write(reinterpret_cast(&num), sizeof(num)); + } diff --git a/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch b/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch new file mode 100644 index 0000000000..1b5f998cf8 --- /dev/null +++ b/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch @@ -0,0 +1,63 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +Date: Tue, 28 Jan 2025 22:19:31 -0800 +Subject: [PATCH 4/8] Replace std::to_underlying() + +--- + src/optimization/problem.cpp | 9 ++++----- + src/util/print_diagnostics.hpp | 6 +++--- + 2 files changed, 7 insertions(+), 8 deletions(-) + +diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp +index 81863808d329a53d4162ce0624a3b8e8afc32dfc..c3319fc0a927cf452871a2db08d5edff87ac8eea 100644 +--- a/src/optimization/problem.cpp ++++ b/src/optimization/problem.cpp +@@ -7,7 +7,6 @@ + #include + #include + #include +-#include + + #include + #include +@@ -346,11 +345,11 @@ void Problem::print_problem_analysis() { + // Print problem structure + slp::println("\nProblem structure:"); + slp::println(" ↳ {} cost function", +- types[std::to_underlying(cost_function_type())]); ++ types[static_cast(cost_function_type())]); + slp::println(" ↳ {} equality constraints", +- types[std::to_underlying(equality_constraint_type())]); ++ types[static_cast(equality_constraint_type())]); + slp::println(" ↳ {} inequality constraints", +- types[std::to_underlying(inequality_constraint_type())]); ++ types[static_cast(inequality_constraint_type())]); + + if (m_decision_variables.size() == 1) { + slp::print("\n1 decision variable\n"); +@@ -362,7 +361,7 @@ void Problem::print_problem_analysis() { + [](const gch::small_vector& constraints) { + std::array counts{}; + for (const auto& constraint : constraints) { +- ++counts[std::to_underlying(constraint.type())]; ++ ++counts[static_cast(constraint.type())]; + } + for (const auto& [count, name] : + std::views::zip(counts, std::array{"empty", "constant", "linear", +diff --git a/src/util/print_diagnostics.hpp b/src/util/print_diagnostics.hpp +index fde36957c0258f6e3cd435ef6224d60407012ff7..82e0e082b0e40153dcb2fcd2c655a412a8a9540a 100644 +--- a/src/util/print_diagnostics.hpp ++++ b/src/util/print_diagnostics.hpp +@@ -238,9 +238,9 @@ void print_iteration_diagnostics(int iterations, IterationType type, + slp::println( + "│{:4} {:4} {:9.3f} {:12e} {:13e} {:12e} {:12e} {:.2e} {:<5} {:.2e} " + "{:.2e} {:2d}│", +- iterations, ITERATION_TYPES[std::to_underlying(type)], to_ms(time), error, +- cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, dual_α, +- backtracks); ++ iterations, ITERATION_TYPES[static_cast(type)], to_ms(time), ++ error, cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, ++ dual_α, backtracks); + } + #else + #define print_iteration_diagnostics(...) diff --git a/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch b/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch new file mode 100644 index 0000000000..2885d850de --- /dev/null +++ b/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch @@ -0,0 +1,53 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +Date: Sat, 8 Feb 2025 13:42:36 -0800 +Subject: [PATCH 5/8] Replace std::views::zip() + +--- + include/sleipnir/autodiff/adjoint_expression_graph.hpp | 5 ++++- + src/optimization/problem.cpp | 9 +++++---- + 2 files changed, 9 insertions(+), 5 deletions(-) + +diff --git a/include/sleipnir/autodiff/adjoint_expression_graph.hpp b/include/sleipnir/autodiff/adjoint_expression_graph.hpp +index 4b4f3303faed766d3ac39829870514f50d9a582f..4576e19c9695caf4407fbbb592afe32d8252a0db 100644 +--- a/include/sleipnir/autodiff/adjoint_expression_graph.hpp ++++ b/include/sleipnir/autodiff/adjoint_expression_graph.hpp +@@ -155,7 +155,10 @@ class AdjointExpressionGraph { + } + } + } else { +- for (const auto& [col, node] : std::views::zip(m_col_list, m_top_list)) { ++ for (size_t i = 0; i < m_top_list.size(); ++i) { ++ const auto& col = m_col_list[i]; ++ const auto& node = m_top_list[i]; ++ + // Append adjoints of wrt to sparse matrix triplets + if (col != -1 && node->adjoint != 0.0) { + triplets.emplace_back(row, col, node->adjoint); +diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp +index c3319fc0a927cf452871a2db08d5edff87ac8eea..5532b3962409e2140132e79241da4fba0f36bc78 100644 +--- a/src/optimization/problem.cpp ++++ b/src/optimization/problem.cpp +@@ -6,7 +6,6 @@ + #include + #include + #include +-#include + + #include + #include +@@ -363,9 +362,11 @@ void Problem::print_problem_analysis() { + for (const auto& constraint : constraints) { + ++counts[static_cast(constraint.type())]; + } +- for (const auto& [count, name] : +- std::views::zip(counts, std::array{"empty", "constant", "linear", +- "quadratic", "nonlinear"})) { ++ for (size_t i = 0; i < counts.size(); ++i) { ++ constexpr std::array names{"empty", "constant", "linear", "quadratic", ++ "nonlinear"}; ++ const auto& count = counts[i]; ++ const auto& name = names[i]; + if (count > 0) { + slp::println(" ↳ {} {}", count, name); + } diff --git a/upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch b/upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch new file mode 100644 index 0000000000..c71e612919 --- /dev/null +++ b/upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch @@ -0,0 +1,22 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +Date: Mon, 10 Feb 2025 11:37:02 -0800 +Subject: [PATCH 6/8] Suppress clang-tidy false positives + +--- + include/sleipnir/autodiff/variable.hpp | 2 +- + 1 file changed, 1 insertion(+), 1 deletion(-) + +diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp +index 9f79a82763213dc712cce4c2a322289d57645032..17e7eb7cc2c7c7599eaba97d8ec80972524c1599 100644 +--- a/include/sleipnir/autodiff/variable.hpp ++++ b/include/sleipnir/autodiff/variable.hpp +@@ -626,7 +626,7 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints { + * @param inequality_constraints The list of InequalityConstraints to + * concatenate. + */ +- InequalityConstraints( ++ InequalityConstraints( // NOLINT + std::initializer_list inequality_constraints) { + for (const auto& elem : inequality_constraints) { + constraints.insert(constraints.end(), elem.constraints.begin(), diff --git a/upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch b/upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch new file mode 100644 index 0000000000..a4841664ed --- /dev/null +++ b/upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch @@ -0,0 +1,34 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +Date: Mon, 24 Feb 2025 15:12:03 -0800 +Subject: [PATCH 7/8] Suppress GCC 12 warning false positive + +--- + include/sleipnir/autodiff/variable_matrix.hpp | 7 +++++++ + 1 file changed, 7 insertions(+) + +diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp +index 1c6f9e8dade8bebce7aec18bbb9b5491acb1d977..dee43f926d304e1f4900bd57b99cd613e808f58e 100644 +--- a/include/sleipnir/autodiff/variable_matrix.hpp ++++ b/include/sleipnir/autodiff/variable_matrix.hpp +@@ -573,6 +573,10 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + VariableMatrix result(VariableMatrix::empty, lhs.rows(), rhs.cols()); + ++#if __GNUC__ >= 12 ++#pragma GCC diagnostic push ++#pragma GCC diagnostic ignored "-Wmaybe-uninitialized" ++#endif + for (int i = 0; i < lhs.rows(); ++i) { + for (int j = 0; j < rhs.cols(); ++j) { + Variable sum; +@@ -590,6 +594,9 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + result[i, j] = sum; + } + } ++#if __GNUC__ >= 12 ++#pragma GCC diagnostic pop ++#endif + + return result; + } diff --git a/upstream_utils/sleipnir_patches/0008-Revert-Use-multidimensional-array-subscript-operator.patch b/upstream_utils/sleipnir_patches/0008-Revert-Use-multidimensional-array-subscript-operator.patch new file mode 100644 index 0000000000..d31ba4bcc7 --- /dev/null +++ b/upstream_utils/sleipnir_patches/0008-Revert-Use-multidimensional-array-subscript-operator.patch @@ -0,0 +1,915 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +Date: Sat, 12 Apr 2025 16:28:47 -0700 +Subject: [PATCH 8/8] Revert "Use multidimensional array subscript operator + (#843)" + +This reverts commit f9b2c450bbbf6f14b194b8b81708d032a6431ee0. +--- + include/sleipnir/autodiff/hessian.hpp | 4 +- + include/sleipnir/autodiff/jacobian.hpp | 4 +- + include/sleipnir/autodiff/variable.hpp | 26 +---- + include/sleipnir/autodiff/variable_block.hpp | 70 +++++------ + include/sleipnir/autodiff/variable_matrix.hpp | 110 ++++++------------ + include/sleipnir/control/ocp.hpp | 14 +-- + include/sleipnir/optimization/problem.hpp | 6 +- + src/autodiff/variable_matrix.cpp | 66 +++++------ + 8 files changed, 118 insertions(+), 182 deletions(-) + +diff --git a/include/sleipnir/autodiff/hessian.hpp b/include/sleipnir/autodiff/hessian.hpp +index 4ad097a8117dac47566a3c6896d281004147be70..8b048ab3ba0d671397cfdadcd137ac67bef1b441 100644 +--- a/include/sleipnir/autodiff/hessian.hpp ++++ b/include/sleipnir/autodiff/hessian.hpp +@@ -103,9 +103,9 @@ class SLEIPNIR_DLLEXPORT Hessian { + auto grad = m_graphs[row].generate_gradient_tree(m_wrt); + for (int col = 0; col < m_wrt.rows(); ++col) { + if (grad[col].expr != nullptr) { +- result[row, col] = std::move(grad[col]); ++ result(row, col) = std::move(grad[col]); + } else { +- result[row, col] = Variable{0.0}; ++ result(row, col) = Variable{0.0}; + } + } + } +diff --git a/include/sleipnir/autodiff/jacobian.hpp b/include/sleipnir/autodiff/jacobian.hpp +index 787fca8ccd3fd6e46c5d31ab980704e6a5e99402..7e7e1340d065d35412f43b27fac7d8a719b7e5b5 100644 +--- a/include/sleipnir/autodiff/jacobian.hpp ++++ b/include/sleipnir/autodiff/jacobian.hpp +@@ -95,9 +95,9 @@ class SLEIPNIR_DLLEXPORT Jacobian { + auto grad = m_graphs[row].generate_gradient_tree(m_wrt); + for (int col = 0; col < m_wrt.rows(); ++col) { + if (grad[col].expr != nullptr) { +- result[row, col] = std::move(grad[col]); ++ result(row, col) = std::move(grad[col]); + } else { +- result[row, col] = Variable{0.0}; ++ result(row, col) = Variable{0.0}; + } + } + } +diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp +index 17e7eb7cc2c7c7599eaba97d8ec80972524c1599..03b929c778c03186cc5b461a2e855da23034457a 100644 +--- a/include/sleipnir/autodiff/variable.hpp ++++ b/include/sleipnir/autodiff/variable.hpp +@@ -505,11 +505,7 @@ gch::small_vector make_constraints(LHS&& lhs, RHS&& rhs) { + for (int row = 0; row < rhs.rows(); ++row) { + for (int col = 0; col < rhs.cols(); ++col) { + // Make right-hand side zero +- if constexpr (EigenMatrixLike>) { +- constraints.emplace_back(lhs - rhs(row, col)); +- } else { +- constraints.emplace_back(lhs - rhs[row, col]); +- } ++ constraints.emplace_back(lhs - rhs(row, col)); + } + } + } else if constexpr (MatrixLike && ScalarLike) { +@@ -518,11 +514,7 @@ gch::small_vector make_constraints(LHS&& lhs, RHS&& rhs) { + for (int row = 0; row < lhs.rows(); ++row) { + for (int col = 0; col < lhs.cols(); ++col) { + // Make right-hand side zero +- if constexpr (EigenMatrixLike>) { +- constraints.emplace_back(lhs(row, col) - rhs); +- } else { +- constraints.emplace_back(lhs[row, col] - rhs); +- } ++ constraints.emplace_back(lhs(row, col) - rhs); + } + } + } else if constexpr (MatrixLike && MatrixLike) { +@@ -532,19 +524,7 @@ gch::small_vector make_constraints(LHS&& lhs, RHS&& rhs) { + for (int row = 0; row < lhs.rows(); ++row) { + for (int col = 0; col < lhs.cols(); ++col) { + // Make right-hand side zero +- if constexpr (EigenMatrixLike> && +- EigenMatrixLike>) { +- constraints.emplace_back(lhs(row, col) - rhs(row, col)); +- } else if constexpr (EigenMatrixLike> && +- SleipnirMatrixLike>) { +- constraints.emplace_back(lhs(row, col) - rhs[row, col]); +- } else if constexpr (SleipnirMatrixLike> && +- EigenMatrixLike>) { +- constraints.emplace_back(lhs[row, col] - rhs(row, col)); +- } else if constexpr (SleipnirMatrixLike> && +- SleipnirMatrixLike>) { +- constraints.emplace_back(lhs[row, col] - rhs[row, col]); +- } ++ constraints.emplace_back(lhs(row, col) - rhs(row, col)); + } + } + } +diff --git a/include/sleipnir/autodiff/variable_block.hpp b/include/sleipnir/autodiff/variable_block.hpp +index f1c1ca0dc3fde663c3e74f6fca4b89b119cf377d..632d44beb5b3dae29b9829c52a6168fee39fe537 100644 +--- a/include/sleipnir/autodiff/variable_block.hpp ++++ b/include/sleipnir/autodiff/variable_block.hpp +@@ -50,7 +50,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] = values[row, col]; ++ (*this)(row, col) = values(row, col); + } + } + } +@@ -85,7 +85,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] = values[row, col]; ++ (*this)(row, col) = values(row, col); + } + } + } +@@ -152,7 +152,7 @@ class VariableBlock { + VariableBlock& operator=(ScalarLike auto value) { + slp_assert(rows() == 1 && cols() == 1); + +- (*this)[0, 0] = value; ++ (*this)(0, 0) = value; + + return *this; + } +@@ -167,7 +167,7 @@ class VariableBlock { + void set_value(double value) { + slp_assert(rows() == 1 && cols() == 1); + +- (*this)[0, 0].set_value(value); ++ (*this)(0, 0).set_value(value); + } + + /** +@@ -182,7 +182,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] = values(row, col); ++ (*this)(row, col) = values(row, col); + } + } + +@@ -201,7 +201,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col].set_value(values(row, col)); ++ (*this)(row, col).set_value(values(row, col)); + } + } + } +@@ -217,7 +217,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] = values[row, col]; ++ (*this)(row, col) = values(row, col); + } + } + return *this; +@@ -234,7 +234,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] = std::move(values[row, col]); ++ (*this)(row, col) = std::move(values(row, col)); + } + } + return *this; +@@ -247,13 +247,13 @@ class VariableBlock { + * @param col The scalar subblock's column. + * @return A scalar subblock at the given row and column. + */ +- Variable& operator[](int row, int col) ++ Variable& operator()(int row, int col) + requires(!std::is_const_v) + { + slp_assert(row >= 0 && row < rows()); + slp_assert(col >= 0 && col < cols()); +- return (*m_mat)[m_row_slice.start + row * m_row_slice.step, +- m_col_slice.start + col * m_col_slice.step]; ++ return (*m_mat)(m_row_slice.start + row * m_row_slice.step, ++ m_col_slice.start + col * m_col_slice.step); + } + + /** +@@ -263,11 +263,11 @@ class VariableBlock { + * @param col The scalar subblock's column. + * @return A scalar subblock at the given row and column. + */ +- const Variable& operator[](int row, int col) const { ++ const Variable& operator()(int row, int col) const { + slp_assert(row >= 0 && row < rows()); + slp_assert(col >= 0 && col < cols()); +- return (*m_mat)[m_row_slice.start + row * m_row_slice.step, +- m_col_slice.start + col * m_col_slice.step]; ++ return (*m_mat)(m_row_slice.start + row * m_row_slice.step, ++ m_col_slice.start + col * m_col_slice.step); + } + + /** +@@ -280,7 +280,7 @@ class VariableBlock { + requires(!std::is_const_v) + { + slp_assert(row >= 0 && row < rows() * cols()); +- return (*this)[row / cols(), row % cols()]; ++ return (*this)(row / cols(), row % cols()); + } + + /** +@@ -291,7 +291,7 @@ class VariableBlock { + */ + const Variable& operator[](int row) const { + slp_assert(row >= 0 && row < rows() * cols()); +- return (*this)[row / cols(), row % cols()]; ++ return (*this)(row / cols(), row % cols()); + } + + /** +@@ -309,8 +309,8 @@ class VariableBlock { + slp_assert(col_offset >= 0 && col_offset <= cols()); + slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset); + slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset); +- return (*this)[Slice{row_offset, row_offset + block_rows, 1}, +- Slice{col_offset, col_offset + block_cols, 1}]; ++ return (*this)({row_offset, row_offset + block_rows, 1}, ++ {col_offset, col_offset + block_cols, 1}); + } + + /** +@@ -328,8 +328,8 @@ class VariableBlock { + slp_assert(col_offset >= 0 && col_offset <= cols()); + slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset); + slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset); +- return (*this)[Slice{row_offset, row_offset + block_rows, 1}, +- Slice{col_offset, col_offset + block_cols, 1}]; ++ return (*this)({row_offset, row_offset + block_rows, 1}, ++ {col_offset, col_offset + block_cols, 1}); + } + + /** +@@ -339,7 +339,7 @@ class VariableBlock { + * @param col_slice The column slice. + * @return A slice of the variable matrix. + */ +- VariableBlock operator[](Slice row_slice, Slice col_slice) { ++ VariableBlock operator()(Slice row_slice, Slice col_slice) { + int row_slice_length = row_slice.adjust(m_row_slice_length); + int col_slice_length = col_slice.adjust(m_col_slice_length); + return VariableBlock{ +@@ -359,7 +359,7 @@ class VariableBlock { + * @param col_slice The column slice. + * @return A slice of the variable matrix. + */ +- const VariableBlock operator[](Slice row_slice, ++ const VariableBlock operator()(Slice row_slice, + Slice col_slice) const { + int row_slice_length = row_slice.adjust(m_row_slice_length); + int col_slice_length = col_slice.adjust(m_col_slice_length); +@@ -385,7 +385,7 @@ class VariableBlock { + * @param col_slice_length The column slice length. + * @return A slice of the variable matrix. + */ +- VariableBlock operator[](Slice row_slice, int row_slice_length, ++ VariableBlock operator()(Slice row_slice, int row_slice_length, + Slice col_slice, int col_slice_length) { + return VariableBlock{ + *m_mat, +@@ -409,7 +409,7 @@ class VariableBlock { + * @param col_slice_length The column slice length. + * @return A slice of the variable matrix. + */ +- const VariableBlock operator[](Slice row_slice, ++ const VariableBlock operator()(Slice row_slice, + int row_slice_length, + Slice col_slice, + int col_slice_length) const { +@@ -524,7 +524,7 @@ class VariableBlock { + VariableBlock& operator*=(const ScalarLike auto& rhs) { + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] *= rhs; ++ (*this)(row, col) *= rhs; + } + } + +@@ -542,7 +542,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] /= rhs[0, 0]; ++ (*this)(row, col) /= rhs(0, 0); + } + } + +@@ -558,7 +558,7 @@ class VariableBlock { + VariableBlock& operator/=(const ScalarLike auto& rhs) { + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] /= rhs; ++ (*this)(row, col) /= rhs; + } + } + +@@ -576,7 +576,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] += rhs[row, col]; ++ (*this)(row, col) += rhs(row, col); + } + } + +@@ -594,7 +594,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] += rhs; ++ (*this)(row, col) += rhs; + } + } + +@@ -612,7 +612,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] -= rhs[row, col]; ++ (*this)(row, col) -= rhs(row, col); + } + } + +@@ -630,7 +630,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] -= rhs; ++ (*this)(row, col) -= rhs; + } + } + +@@ -655,7 +655,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- result[col, row] = (*this)[row, col]; ++ result(col, row) = (*this)(row, col); + } + } + +@@ -686,8 +686,8 @@ class VariableBlock { + double value(int row, int col) { + slp_assert(row >= 0 && row < rows()); + slp_assert(col >= 0 && col < cols()); +- return (*m_mat)[m_row_slice.start + row * m_row_slice.step, +- m_col_slice.start + col * m_col_slice.step] ++ return (*m_mat)(m_row_slice.start + row * m_row_slice.step, ++ m_col_slice.start + col * m_col_slice.step) + .value(); + } + +@@ -731,7 +731,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- result[row, col] = unary_op((*this)[row, col]); ++ result(row, col) = unary_op((*this)(row, col)); + } + } + +diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp +index dee43f926d304e1f4900bd57b99cd613e808f58e..4dc2cea00cb9491035a9b4795be3562186991c7a 100644 +--- a/include/sleipnir/autodiff/variable_matrix.hpp ++++ b/include/sleipnir/autodiff/variable_matrix.hpp +@@ -211,7 +211,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < values.rows(); ++row) { + for (int col = 0; col < values.cols(); ++col) { +- (*this)[row, col] = values(row, col); ++ (*this)(row, col) = values(row, col); + } + } + +@@ -229,7 +229,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + VariableMatrix& operator=(ScalarLike auto value) { + slp_assert(rows() == 1 && cols() == 1); + +- (*this)[0, 0] = value; ++ (*this)(0, 0) = value; + + return *this; + } +@@ -246,7 +246,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < values.rows(); ++row) { + for (int col = 0; col < values.cols(); ++col) { +- (*this)[row, col].set_value(values(row, col)); ++ (*this)(row, col).set_value(values(row, col)); + } + } + } +@@ -280,7 +280,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + m_storage.reserve(rows() * cols()); + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- m_storage.emplace_back(values[row, col]); ++ m_storage.emplace_back(values(row, col)); + } + } + } +@@ -295,7 +295,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + m_storage.reserve(rows() * cols()); + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- m_storage.emplace_back(values[row, col]); ++ m_storage.emplace_back(values(row, col)); + } + } + } +@@ -340,7 +340,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + * @param col The block column. + * @return A block pointing to the given row and column. + */ +- Variable& operator[](int row, int col) { ++ Variable& operator()(int row, int col) { + slp_assert(row >= 0 && row < rows()); + slp_assert(col >= 0 && col < cols()); + return m_storage[row * cols() + col]; +@@ -353,7 +353,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + * @param col The block column. + * @return A block pointing to the given row and column. + */ +- const Variable& operator[](int row, int col) const { ++ const Variable& operator()(int row, int col) const { + slp_assert(row >= 0 && row < rows()); + slp_assert(col >= 0 && col < cols()); + return m_storage[row * cols() + col]; +@@ -426,7 +426,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + * @param col_slice The column slice. + * @return A slice of the variable matrix. + */ +- VariableBlock operator[](Slice row_slice, Slice col_slice) { ++ VariableBlock operator()(Slice row_slice, Slice col_slice) { + int row_slice_length = row_slice.adjust(rows()); + int col_slice_length = col_slice.adjust(cols()); + return VariableBlock{*this, std::move(row_slice), row_slice_length, +@@ -440,7 +440,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + * @param col_slice The column slice. + * @return A slice of the variable matrix. + */ +- const VariableBlock operator[](Slice row_slice, ++ const VariableBlock operator()(Slice row_slice, + Slice col_slice) const { + int row_slice_length = row_slice.adjust(rows()); + int col_slice_length = col_slice.adjust(cols()); +@@ -461,7 +461,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + * @return A slice of the variable matrix. + * + */ +- VariableBlock operator[](Slice row_slice, ++ VariableBlock operator()(Slice row_slice, + int row_slice_length, + Slice col_slice, + int col_slice_length) { +@@ -481,7 +481,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + * @param col_slice_length The column slice length. + * @return A slice of the variable matrix. + */ +- const VariableBlock operator[]( ++ const VariableBlock operator()( + Slice row_slice, int row_slice_length, Slice col_slice, + int col_slice_length) const { + return VariableBlock{*this, std::move(row_slice), row_slice_length, +@@ -581,17 +581,9 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + for (int j = 0; j < rhs.cols(); ++j) { + Variable sum; + for (int k = 0; k < lhs.cols(); ++k) { +- if constexpr (SleipnirMatrixLike && SleipnirMatrixLike) { +- sum += lhs[i, k] * rhs[k, j]; +- } else if constexpr (SleipnirMatrixLike && +- EigenMatrixLike) { +- sum += lhs[i, k] * rhs(k, j); +- } else if constexpr (EigenMatrixLike && +- SleipnirMatrixLike) { +- sum += lhs(i, k) * rhs[k, j]; +- } ++ sum += lhs(i, k) * rhs(k, j); + } +- result[i, j] = sum; ++ result(i, j) = sum; + } + } + #if __GNUC__ >= 12 +@@ -613,7 +605,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- result[row, col] = lhs[row, col] * rhs; ++ result(row, col) = lhs(row, col) * rhs; + } + } + +@@ -632,11 +624,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- if constexpr (SleipnirMatrixLike) { +- result[row, col] = lhs[row, col] * rhs; +- } else { +- result[row, col] = lhs(row, col) * rhs; +- } ++ result(row, col) = lhs(row, col) * rhs; + } + } + +@@ -655,7 +643,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- result[row, col] = rhs[row, col] * lhs; ++ result(row, col) = rhs(row, col) * lhs; + } + } + +@@ -674,11 +662,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- if constexpr (SleipnirMatrixLike) { +- result[row, col] = rhs[row, col] * lhs; +- } else { +- result[row, col] = rhs(row, col) * lhs; +- } ++ result(row, col) = rhs(row, col) * lhs; + } + } + +@@ -698,13 +682,9 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + for (int j = 0; j < rhs.cols(); ++j) { + Variable sum; + for (int k = 0; k < cols(); ++k) { +- if constexpr (SleipnirMatrixLike) { +- sum += (*this)[i, k] * rhs[k, j]; +- } else { +- sum += (*this)[i, k] * rhs(k, j); +- } ++ sum += (*this)(i, k) * rhs(k, j); + } +- (*this)[i, j] = sum; ++ (*this)(i, j) = sum; + } + } + +@@ -720,7 +700,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + VariableMatrix& operator*=(const ScalarLike auto& rhs) { + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < rhs.cols(); ++col) { +- (*this)[row, col] *= rhs; ++ (*this)(row, col) *= rhs; + } + } + +@@ -740,11 +720,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- if constexpr (SleipnirMatrixLike) { +- result[row, col] = lhs[row, col] / rhs; +- } else { +- result[row, col] = lhs(row, col) / rhs; +- } ++ result(row, col) = lhs(row, col) / rhs; + } + } + +@@ -760,7 +736,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + VariableMatrix& operator/=(const ScalarLike auto& rhs) { + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] /= rhs; ++ (*this)(row, col) /= rhs; + } + } + +@@ -784,13 +760,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- if constexpr (SleipnirMatrixLike && SleipnirMatrixLike) { +- result[row, col] = lhs[row, col] + rhs[row, col]; +- } else if constexpr (SleipnirMatrixLike && EigenMatrixLike) { +- result[row, col] = lhs[row, col] + rhs(row, col); +- } else if constexpr (EigenMatrixLike && SleipnirMatrixLike) { +- result[row, col] = lhs(row, col) + rhs[row, col]; +- } ++ result(row, col) = lhs(row, col) + rhs(row, col); + } + } + +@@ -808,11 +778,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- if constexpr (SleipnirMatrixLike) { +- (*this)[row, col] += rhs[row, col]; +- } else { +- (*this)[row, col] += rhs(row, col); +- } ++ (*this)(row, col) += rhs(row, col); + } + } + +@@ -830,7 +796,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] += rhs; ++ (*this)(row, col) += rhs; + } + } + +@@ -854,13 +820,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- if constexpr (SleipnirMatrixLike && SleipnirMatrixLike) { +- result[row, col] = lhs[row, col] - rhs[row, col]; +- } else if constexpr (SleipnirMatrixLike && EigenMatrixLike) { +- result[row, col] = lhs[row, col] - rhs(row, col); +- } else if constexpr (EigenMatrixLike && SleipnirMatrixLike) { +- result[row, col] = lhs(row, col) - rhs[row, col]; +- } ++ result(row, col) = lhs(row, col) - rhs(row, col); + } + } + +@@ -878,11 +838,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- if constexpr (SleipnirMatrixLike) { +- (*this)[row, col] -= rhs[row, col]; +- } else { +- (*this)[row, col] -= rhs(row, col); +- } ++ (*this)(row, col) -= rhs(row, col); + } + } + +@@ -900,7 +856,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] -= rhs; ++ (*this)(row, col) -= rhs; + } + } + +@@ -918,7 +874,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- result[row, col] = -lhs[row, col]; ++ result(row, col) = -lhs(row, col); + } + } + +@@ -930,7 +886,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + */ + operator Variable() const { // NOLINT + slp_assert(rows() == 1 && cols() == 1); +- return (*this)[0, 0]; ++ return (*this)(0, 0); + } + + /** +@@ -943,7 +899,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- result[col, row] = (*this)[row, col]; ++ result(col, row) = (*this)(row, col); + } + } + +@@ -1017,7 +973,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- result[row, col] = unary_op((*this)[row, col]); ++ result(row, col) = unary_op((*this)(row, col)); + } + } + +@@ -1199,7 +1155,7 @@ SLEIPNIR_DLLEXPORT inline VariableMatrix cwise_reduce( + + for (int row = 0; row < lhs.rows(); ++row) { + for (int col = 0; col < lhs.cols(); ++col) { +- result[row, col] = binary_op(lhs[row, col], rhs[row, col]); ++ result(row, col) = binary_op(lhs(row, col), rhs(row, col)); + } + } + +diff --git a/include/sleipnir/control/ocp.hpp b/include/sleipnir/control/ocp.hpp +index 282520fb852d8588b96846eb5b4952bf47d1309f..d9174426669281e68a5c09d298cfd5bcd3be3776 100644 +--- a/include/sleipnir/control/ocp.hpp ++++ b/include/sleipnir/control/ocp.hpp +@@ -180,7 +180,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { + if (m_timestep_method == TimestepMethod::FIXED) { + m_DT = VariableMatrix{1, m_num_steps + 1}; + for (int i = 0; i < num_steps + 1; ++i) { +- m_DT[0, i] = m_dt.count(); ++ m_DT(0, i) = m_dt.count(); + } + } else if (m_timestep_method == TimestepMethod::VARIABLE_SINGLE) { + Variable dt = decision_variable(); +@@ -189,12 +189,12 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { + // Set the member variable matrix to track the decision variable + m_DT = VariableMatrix{1, m_num_steps + 1}; + for (int i = 0; i < num_steps + 1; ++i) { +- m_DT[0, i] = dt; ++ m_DT(0, i) = dt; + } + } else if (m_timestep_method == TimestepMethod::VARIABLE) { + m_DT = decision_variable(1, m_num_steps + 1); + for (int i = 0; i < num_steps + 1; ++i) { +- m_DT[0, i].set_value(m_dt.count()); ++ m_DT(0, i).set_value(m_dt.count()); + } + } + +@@ -270,7 +270,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { + for (int i = 0; i < m_num_steps + 1; ++i) { + auto x = X().col(i); + auto u = U().col(i); +- auto dt = this->dt()[0, i]; ++ auto dt = this->dt()(0, i); + callback(time, x, u, dt); + + time += dt; +@@ -377,7 +377,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { + + // Derivation at https://mec560sbu.github.io/2016/09/30/direct_collocation/ + for (int i = 0; i < m_num_steps; ++i) { +- Variable h = dt()[0, i]; ++ Variable h = dt()(0, i); + + auto& f = m_dynamics_function; + +@@ -412,7 +412,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { + auto x_begin = X().col(i); + auto x_end = X().col(i + 1); + auto u = U().col(i); +- Variable dt = this->dt()[0, i]; ++ Variable dt = this->dt()(0, i); + + if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { + subject_to(x_end == rk4dt()[0, i]; ++ Variable dt = this->dt()(0, i); + + if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { + x_end = rk4 #include -#include -#include +#include +#include #include "frc/EigenCore.h" #include "frc/system/NumericalIntegration.h" @@ -18,7 +18,7 @@ using namespace frc; units::volt_t ArmFeedforward::Calculate( units::unit_t currentAngle, units::unit_t currentVelocity, units::unit_t nextVelocity) const { - using VarMat = sleipnir::VariableMatrix; + using VarMat = slp::VariableMatrix; // Small kₐ values make the solver ill-conditioned if (kA < units::unit_t{1e-1}) { @@ -32,37 +32,37 @@ units::volt_t ArmFeedforward::Calculate( 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))}}; + {-(kS / kA).value() * slp::sign(x[1]) - + (kG / kA).value() * slp::cos(x[0])}}; return A * x + B * u + c; }; Vectord<2> r_k{currentAngle.value(), currentVelocity.value()}; - sleipnir::Variable u_k; + slp::Variable u_k; // Initial guess auto acceleration = (nextVelocity - currentVelocity) / m_dt; - u_k.SetValue((kS * wpi::sgn(currentVelocity.value()) + kV * currentVelocity + - kA * acceleration + kG * units::math::cos(currentAngle)) - .value()); + u_k.set_value((kS * wpi::sgn(currentVelocity.value()) + kV * currentVelocity + + kA * acceleration + kG * units::math::cos(currentAngle)) + .value()); auto r_k1 = RK4(f, r_k, u_k, m_dt); // Minimize difference between desired and actual next velocity auto cost = - (nextVelocity.value() - r_k1(1)) * (nextVelocity.value() - r_k1(1)); + (nextVelocity.value() - r_k1[1]) * (nextVelocity.value() - r_k1[1]); // Refine solution via Newton's method { auto xAD = u_k; - double x = xAD.Value(); + double x = xAD.value(); - sleipnir::Gradient gradientF{cost, xAD}; - Eigen::SparseVector g = gradientF.Value(); + slp::Gradient gradientF{cost, xAD}; + Eigen::SparseVector g = gradientF.value(); - sleipnir::Hessian hessianF{cost, xAD}; - Eigen::SparseMatrix H = hessianF.Value(); + slp::Hessian hessianF{cost, xAD}; + Eigen::SparseMatrix H = hessianF.value(); double error_k = std::numeric_limits::infinity(); double error_k1 = std::abs(g.coeff(0)); @@ -81,31 +81,31 @@ units::volt_t ArmFeedforward::Calculate( // Shrink step until cost goes down { - double oldCost = cost.Value(); + double oldCost = cost.value(); double α = 1.0; double trial_x = x + α * p_x; - xAD.SetValue(trial_x); + xAD.set_value(trial_x); - while (cost.Value() > oldCost) { + while (cost.value() > oldCost) { α *= 0.5; trial_x = x + α * p_x; - xAD.SetValue(trial_x); + xAD.set_value(trial_x); } x = trial_x; } - xAD.SetValue(x); + xAD.set_value(x); - g = gradientF.Value(); - H = hessianF.Value(); + g = gradientF.value(); + H = hessianF.value(); error_k1 = std::abs(g.coeff(0)); } } - return units::volt_t{u_k.Value()}; + return units::volt_t{u_k.value()}; } diff --git a/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp b/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp index 9c4fe4fc1e..ab53ecb5d1 100644 --- a/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp @@ -4,7 +4,7 @@ #include "frc/geometry/Ellipse2d.h" -#include +#include using namespace frc; @@ -20,31 +20,29 @@ Translation2d Ellipse2d::Nearest(const Translation2d& point) const { // Find nearest point { - namespace slp = sleipnir; - - sleipnir::OptimizationProblem problem; + slp::Problem problem; // Point on ellipse - auto x = problem.DecisionVariable(); - x.SetValue(rotPoint.X().value()); - auto y = problem.DecisionVariable(); - y.SetValue(rotPoint.Y().value()); + auto x = problem.decision_variable(); + x.set_value(rotPoint.X().value()); + auto y = problem.decision_variable(); + y.set_value(rotPoint.Y().value()); - problem.Minimize(slp::pow(x - rotPoint.X().value(), 2) + + problem.minimize(slp::pow(x - rotPoint.X().value(), 2) + slp::pow(y - rotPoint.Y().value(), 2)); // (x − x_c)²/a² + (y − y_c)²/b² = 1 // b²(x − x_c)² + a²(y − y_c)² = a²b² double a2 = m_xSemiAxis.value() * m_xSemiAxis.value(); double b2 = m_ySemiAxis.value() * m_ySemiAxis.value(); - problem.SubjectTo(b2 * slp::pow(x - m_center.X().value(), 2) + - a2 * slp::pow(y - m_center.Y().value(), 2) == - a2 * b2); + problem.subject_to(b2 * slp::pow(x - m_center.X().value(), 2) + + a2 * slp::pow(y - m_center.Y().value(), 2) == + a2 * b2); - problem.Solve(); + problem.solve(); - rotPoint = frc::Translation2d{units::meter_t{x.Value()}, - units::meter_t{y.Value()}}; + rotPoint = frc::Translation2d{units::meter_t{x.value()}, + units::meter_t{y.value()}}; } // Undo rotation diff --git a/wpimath/src/main/native/thirdparty/sleipnir/.styleguide b/wpimath/src/main/native/thirdparty/sleipnir/.styleguide index 2cf272a115..054f5eb80e 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/.styleguide +++ b/wpimath/src/main/native/thirdparty/sleipnir/.styleguide @@ -10,13 +10,14 @@ modifiableFileExclude { \.patch$ \.png$ \.svg$ - jormungandr/cpp/Docstrings\.hpp$ + jormungandr/cpp/docstrings\.hpp$ jormungandr/py\.typed$ } includeOtherLibs { ^Eigen/ ^catch2/ + ^gch/ ^nanobind/ ^sleipnir/ } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/.styleguide b/wpimath/src/main/native/thirdparty/sleipnir/include/.styleguide index efa36cee1f..03938557c2 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/.styleguide +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/.styleguide @@ -6,12 +6,9 @@ cppSrcFileInclude { \.cpp$ } -licenseUpdateExclude { - include/sleipnir/util/small_vector\.hpp$ -} - includeOtherLibs { ^Eigen/ ^fmt/ + ^gch/ ^wpi/ } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/gch/small_vector.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/gch/small_vector.hpp new file mode 100644 index 0000000000..d3cff4c424 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/gch/small_vector.hpp @@ -0,0 +1,12 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +namespace gch { + +template +using small_vector = wpi::SmallVector; + +} // namespace gch diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp deleted file mode 100644 index ef9a15bf69..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp +++ /dev/null @@ -1,1144 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -#include -#include -#include -#include -#include -#include - -#include - -#include "sleipnir/autodiff/ExpressionType.hpp" -#include "sleipnir/util/IntrusiveSharedPtr.hpp" -#include "sleipnir/util/Pool.hpp" -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir::detail { - -// The global pool allocator uses a thread-local static pool resource, which -// isn't guaranteed to be initialized properly across DLL boundaries on Windows -#ifdef _WIN32 -inline constexpr bool kUsePoolAllocator = false; -#else -inline constexpr bool kUsePoolAllocator = true; -#endif - -struct SLEIPNIR_DLLEXPORT Expression; - -inline void IntrusiveSharedPtrIncRefCount(Expression* expr); -inline void IntrusiveSharedPtrDecRefCount(Expression* expr); - -/** - * Typedef for intrusive shared pointer to Expression. - */ -using ExpressionPtr = IntrusiveSharedPtr; - -/** - * Creates an intrusive shared pointer to an expression from the global pool - * allocator. - * - * @param args Constructor arguments for Expression. - */ -template -static ExpressionPtr MakeExpressionPtr(Args&&... args) { - if constexpr (kUsePoolAllocator) { - return AllocateIntrusiveShared( - GlobalPoolAllocator(), std::forward(args)...); - } else { - return MakeIntrusiveShared(std::forward(args)...); - } -} - -/** - * An autodiff expression node. - */ -struct SLEIPNIR_DLLEXPORT Expression { - /** - * Binary function taking two doubles and returning a double. - */ - using BinaryFuncDouble = double (*)(double, double); - - /** - * Trinary function taking three doubles and returning a double. - */ - using TrinaryFuncDouble = double (*)(double, double, double); - - /** - * Trinary function taking three expressions and returning an expression. - */ - using TrinaryFuncExpr = ExpressionPtr (*)(const ExpressionPtr&, - const ExpressionPtr&, - const ExpressionPtr&); - - /// The value of the expression node. - double value = 0.0; - - /// The adjoint of the expression node used during autodiff. - double adjoint = 0.0; - - /// Tracks the number of instances of this expression yet to be encountered in - /// an expression tree. - uint32_t duplications = 0; - - /// This expression's row in wrt for autodiff gradient, Jacobian, or Hessian. - /// This is -1 if the expression isn't in wrt. - int32_t row = -1; - - /// The adjoint of the expression node used during gradient expression tree - /// generation. - ExpressionPtr adjointExpr; - - /// Expression argument type. - ExpressionType type = ExpressionType::kConstant; - - /// Reference count for intrusive shared pointer. - uint32_t refCount = 0; - - /// Either nullary operator with no arguments, unary operator with one - /// argument, or binary operator with two arguments. This operator is - /// used to update the node's value. - BinaryFuncDouble valueFunc = nullptr; - - /// Functions returning double adjoints of the children expressions. - /// - /// Parameters: - ///
    - ///
  • lhs: Left argument to binary operator.
    • - ///
  • rhs: Right argument to binary operator.
    • - ///
  • parentAdjoint: Adjoint of parent expression.
    • - ///
- std::array gradientValueFuncs{nullptr, nullptr}; - - /// Functions returning Variable adjoints of the children expressions. - /// - /// Parameters: - ///
    - ///
  • lhs: Left argument to binary operator.
    • - ///
  • rhs: Right argument to binary operator.
    • - ///
  • parentAdjoint: Adjoint of parent expression.
    • - ///
- std::array gradientFuncs{nullptr, nullptr}; - - /// Expression arguments. - std::array args{nullptr, nullptr}; - - /** - * Constructs a constant expression with a value of zero. - */ - constexpr Expression() = default; - - /** - * Constructs a nullary expression (an operator with no arguments). - * - * @param value The expression value. - * @param type The expression type. It should be either constant (the default) - * or linear. - */ - explicit constexpr Expression(double value, - ExpressionType type = ExpressionType::kConstant) - : value{value}, type{type} {} - - /** - * Constructs an unary expression (an operator with one argument). - * - * @param type The expression's type. - * @param valueFunc Unary operator that produces this expression's value. - * @param lhsGradientValueFunc Gradient with respect to the operand. - * @param lhsGradientFunc Gradient with respect to the operand. - * @param lhs Unary operator's operand. - */ - constexpr Expression(ExpressionType type, BinaryFuncDouble valueFunc, - TrinaryFuncDouble lhsGradientValueFunc, - TrinaryFuncExpr lhsGradientFunc, ExpressionPtr lhs) - : value{valueFunc(lhs->value, 0.0)}, - type{type}, - valueFunc{valueFunc}, - gradientValueFuncs{lhsGradientValueFunc, nullptr}, - gradientFuncs{lhsGradientFunc, nullptr}, - args{lhs, nullptr} {} - - /** - * Constructs a binary expression (an operator with two arguments). - * - * @param type The expression's type. - * @param valueFunc Unary operator that produces this expression's value. - * @param lhsGradientValueFunc Gradient with respect to the left operand. - * @param rhsGradientValueFunc Gradient with respect to the right operand. - * @param lhsGradientFunc Gradient with respect to the left operand. - * @param rhsGradientFunc Gradient with respect to the right operand. - * @param lhs Binary operator's left operand. - * @param rhs Binary operator's right operand. - */ - constexpr Expression(ExpressionType type, BinaryFuncDouble valueFunc, - TrinaryFuncDouble lhsGradientValueFunc, - TrinaryFuncDouble rhsGradientValueFunc, - TrinaryFuncExpr lhsGradientFunc, - TrinaryFuncExpr rhsGradientFunc, ExpressionPtr lhs, - ExpressionPtr rhs) - : value{valueFunc(lhs->value, rhs->value)}, - type{type}, - valueFunc{valueFunc}, - gradientValueFuncs{lhsGradientValueFunc, rhsGradientValueFunc}, - gradientFuncs{lhsGradientFunc, rhsGradientFunc}, - args{lhs, rhs} {} - - /** - * Returns true if the expression is the given constant. - * - * @param constant The constant. - */ - constexpr bool IsConstant(double constant) const { - return type == ExpressionType::kConstant && value == constant; - } - - /** - * Expression-Expression multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT ExpressionPtr operator*(const ExpressionPtr& lhs, - const ExpressionPtr& rhs) { - using enum ExpressionType; - - // Prune expression - if (lhs->IsConstant(0.0)) { - // Return zero - return lhs; - } else if (rhs->IsConstant(0.0)) { - // Return zero - return rhs; - } else if (lhs->IsConstant(1.0)) { - return rhs; - } else if (rhs->IsConstant(1.0)) { - return lhs; - } - - // Evaluate constant - if (lhs->type == kConstant && rhs->type == kConstant) { - return MakeExpressionPtr(lhs->value * rhs->value); - } - - // Evaluate expression type - ExpressionType type; - if (lhs->type == kConstant) { - type = rhs->type; - } else if (rhs->type == kConstant) { - type = lhs->type; - } else if (lhs->type == kLinear && rhs->type == kLinear) { - type = kQuadratic; - } else { - type = kNonlinear; - } - - return MakeExpressionPtr( - type, [](double lhs, double rhs) { return lhs * rhs; }, - [](double, double rhs, double parentAdjoint) { - return parentAdjoint * rhs; - }, - [](double lhs, double, double parentAdjoint) { - return parentAdjoint * lhs; - }, - [](const ExpressionPtr&, const ExpressionPtr& rhs, - const ExpressionPtr& parentAdjoint) { return parentAdjoint * rhs; }, - [](const ExpressionPtr& lhs, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { return parentAdjoint * lhs; }, - lhs, rhs); - } - - /** - * Expression-Expression division operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT ExpressionPtr operator/(const ExpressionPtr& lhs, - const ExpressionPtr& rhs) { - using enum ExpressionType; - - // Prune expression - if (lhs->IsConstant(0.0)) { - // Return zero - return lhs; - } else if (rhs->IsConstant(1.0)) { - return lhs; - } - - // Evaluate constant - if (lhs->type == kConstant && rhs->type == kConstant) { - return MakeExpressionPtr(lhs->value / rhs->value); - } - - // Evaluate expression type - ExpressionType type; - if (rhs->type == kConstant) { - type = lhs->type; - } else { - type = kNonlinear; - } - - return MakeExpressionPtr( - type, [](double lhs, double rhs) { return lhs / rhs; }, - [](double, double rhs, double parentAdjoint) { - return parentAdjoint / rhs; - }, - [](double lhs, double rhs, double parentAdjoint) { - return parentAdjoint * -lhs / (rhs * rhs); - }, - [](const ExpressionPtr&, const ExpressionPtr& rhs, - const ExpressionPtr& parentAdjoint) { return parentAdjoint / rhs; }, - [](const ExpressionPtr& lhs, const ExpressionPtr& rhs, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * -lhs / (rhs * rhs); - }, - lhs, rhs); - } - - /** - * Expression-Expression addition operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT ExpressionPtr operator+(const ExpressionPtr& lhs, - const ExpressionPtr& rhs) { - using enum ExpressionType; - - // Prune expression - if (lhs == nullptr || lhs->IsConstant(0.0)) { - return rhs; - } else if (rhs == nullptr || rhs->IsConstant(0.0)) { - return lhs; - } - - // Evaluate constant - if (lhs->type == kConstant && rhs->type == kConstant) { - return MakeExpressionPtr(lhs->value + rhs->value); - } - - return MakeExpressionPtr( - std::max(lhs->type, rhs->type), - [](double lhs, double rhs) { return lhs + rhs; }, - [](double, double, double parentAdjoint) { return parentAdjoint; }, - [](double, double, double parentAdjoint) { return parentAdjoint; }, - [](const ExpressionPtr&, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { return parentAdjoint; }, - [](const ExpressionPtr&, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { return parentAdjoint; }, - lhs, rhs); - } - - /** - * Expression-Expression subtraction operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT ExpressionPtr operator-(const ExpressionPtr& lhs, - const ExpressionPtr& rhs) { - using enum ExpressionType; - - // Prune expression - if (lhs->IsConstant(0.0)) { - if (rhs->IsConstant(0.0)) { - // Return zero - return rhs; - } else { - return -rhs; - } - } else if (rhs->IsConstant(0.0)) { - return lhs; - } - - // Evaluate constant - if (lhs->type == kConstant && rhs->type == kConstant) { - return MakeExpressionPtr(lhs->value - rhs->value); - } - - return MakeExpressionPtr( - std::max(lhs->type, rhs->type), - [](double lhs, double rhs) { return lhs - rhs; }, - [](double, double, double parentAdjoint) { return parentAdjoint; }, - [](double, double, double parentAdjoint) { return -parentAdjoint; }, - [](const ExpressionPtr&, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { return parentAdjoint; }, - [](const ExpressionPtr&, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { return -parentAdjoint; }, - lhs, rhs); - } - - /** - * Unary minus operator. - * - * @param lhs Operand of unary minus. - */ - friend SLEIPNIR_DLLEXPORT ExpressionPtr operator-(const ExpressionPtr& lhs) { - using enum ExpressionType; - - // Prune expression - if (lhs->IsConstant(0.0)) { - // Return zero - return lhs; - } - - // Evaluate constant - if (lhs->type == kConstant) { - return MakeExpressionPtr(-lhs->value); - } - - return MakeExpressionPtr( - lhs->type, [](double lhs, double) { return -lhs; }, - [](double, double, double parentAdjoint) { return -parentAdjoint; }, - [](const ExpressionPtr&, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { return -parentAdjoint; }, - lhs); - } - - /** - * Unary plus operator. - * - * @param lhs Operand of unary plus. - */ - friend SLEIPNIR_DLLEXPORT ExpressionPtr operator+(const ExpressionPtr& lhs) { - return lhs; - } -}; - -SLEIPNIR_DLLEXPORT inline ExpressionPtr exp(const ExpressionPtr& x); -SLEIPNIR_DLLEXPORT inline ExpressionPtr sin(const ExpressionPtr& x); -SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x); -SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt(const ExpressionPtr& x); - -/** - * Refcount increment for intrusive shared pointer. - * - * @param expr The shared pointer's managed object. - */ -inline void IntrusiveSharedPtrIncRefCount(Expression* expr) { - ++expr->refCount; -} - -/** - * Refcount decrement for intrusive shared pointer. - * - * @param expr The shared pointer's managed object. - */ -inline void IntrusiveSharedPtrDecRefCount(Expression* expr) { - // If a deeply nested tree is being deallocated all at once, calling the - // Expression destructor when expr's refcount reaches zero can cause a stack - // overflow. Instead, we iterate over its children to decrement their - // refcounts and deallocate them. - wpi::SmallVector stack; - stack.emplace_back(expr); - - while (!stack.empty()) { - auto elem = stack.back(); - stack.pop_back(); - - // Decrement the current node's refcount. If it reaches zero, deallocate the - // node and enqueue its children so their refcounts are decremented too. - if (--elem->refCount == 0) { - if (elem->adjointExpr != nullptr) { - stack.emplace_back(elem->adjointExpr.Get()); - } - for (auto&& arg : elem->args) { - if (arg != nullptr) { - stack.emplace_back(arg.Get()); - } - } - - // Not calling the destructor here is safe because it only decrements - // refcounts, which was already done above. - if constexpr (kUsePoolAllocator) { - auto alloc = GlobalPoolAllocator(); - std::allocator_traits::deallocate(alloc, elem, - sizeof(Expression)); - } - } - } -} - -/** - * std::abs() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr abs( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero - return x; - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::abs(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::abs(x); }, - [](double x, double, double parentAdjoint) { - if (x < 0.0) { - return -parentAdjoint; - } else if (x > 0.0) { - return parentAdjoint; - } else { - return 0.0; - } - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - if (x->value < 0.0) { - return -parentAdjoint; - } else if (x->value > 0.0) { - return parentAdjoint; - } else { - // Return zero - return MakeExpressionPtr(); - } - }, - x); -} - -/** - * std::acos() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr acos( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - return MakeExpressionPtr(std::numbers::pi / 2.0); - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::acos(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::acos(x); }, - [](double x, double, double parentAdjoint) { - return -parentAdjoint / std::sqrt(1.0 - x * x); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return -parentAdjoint / - sleipnir::detail::sqrt(MakeExpressionPtr(1.0) - x * x); - }, - x); -} - -/** - * std::asin() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr asin( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero - return x; - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::asin(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::asin(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / std::sqrt(1.0 - x * x); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint / - sleipnir::detail::sqrt(MakeExpressionPtr(1.0) - x * x); - }, - x); -} - -/** - * std::atan() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr atan( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero - return x; - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::atan(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::atan(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / (1.0 + x * x); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint / (MakeExpressionPtr(1.0) + x * x); - }, - x); -} - -/** - * std::atan2() for Expressions. - * - * @param y The y argument. - * @param x The x argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2( // NOLINT - const ExpressionPtr& y, const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (y->IsConstant(0.0)) { - // Return zero - return y; - } else if (x->IsConstant(0.0)) { - return MakeExpressionPtr(std::numbers::pi / 2.0); - } - - // Evaluate constant - if (y->type == kConstant && x->type == kConstant) { - return MakeExpressionPtr(std::atan2(y->value, x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double y, double x) { return std::atan2(y, x); }, - [](double y, double x, double parentAdjoint) { - return parentAdjoint * x / (y * y + x * x); - }, - [](double y, double x, double parentAdjoint) { - return parentAdjoint * -y / (y * y + x * x); - }, - [](const ExpressionPtr& y, const ExpressionPtr& x, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * x / (y * y + x * x); - }, - [](const ExpressionPtr& y, const ExpressionPtr& x, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * -y / (y * y + x * x); - }, - y, x); -} - -/** - * std::cos() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr cos( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - return MakeExpressionPtr(1.0); - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::cos(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::cos(x); }, - [](double x, double, double parentAdjoint) { - return -parentAdjoint * std::sin(x); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * -sleipnir::detail::sin(x); - }, - x); -} - -/** - * std::cosh() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr cosh( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - return MakeExpressionPtr(1.0); - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::cosh(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::cosh(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint * std::sinh(x); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * sleipnir::detail::sinh(x); - }, - x); -} - -/** - * std::erf() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr erf( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero - return x; - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::erf(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::erf(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint * 2.0 * std::numbers::inv_sqrtpi * - std::exp(-x * x); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * - MakeExpressionPtr(2.0 * std::numbers::inv_sqrtpi) * - sleipnir::detail::exp(-x * x); - }, - x); -} - -/** - * std::exp() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr exp( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - return MakeExpressionPtr(1.0); - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::exp(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::exp(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint * std::exp(x); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * sleipnir::detail::exp(x); - }, - x); -} - -/** - * std::hypot() for Expressions. - * - * @param x The x argument. - * @param y The y argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot( // NOLINT - const ExpressionPtr& x, const ExpressionPtr& y) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - return y; - } else if (y->IsConstant(0.0)) { - return x; - } - - // Evaluate constant - if (x->type == kConstant && y->type == kConstant) { - return MakeExpressionPtr(std::hypot(x->value, y->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double y) { return std::hypot(x, y); }, - [](double x, double y, double parentAdjoint) { - return parentAdjoint * x / std::hypot(x, y); - }, - [](double x, double y, double parentAdjoint) { - return parentAdjoint * y / std::hypot(x, y); - }, - [](const ExpressionPtr& x, const ExpressionPtr& y, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * x / sleipnir::detail::hypot(x, y); - }, - [](const ExpressionPtr& x, const ExpressionPtr& y, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * y / sleipnir::detail::hypot(x, y); - }, - x, y); -} - -/** - * std::log() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr log( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero - return x; - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::log(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::log(x); }, - [](double x, double, double parentAdjoint) { return parentAdjoint / x; }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { return parentAdjoint / x; }, - x); -} - -/** - * std::log10() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr log10( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero - return x; - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::log10(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::log10(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / (std::numbers::ln10 * x); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint / (MakeExpressionPtr(std::numbers::ln10) * x); - }, - x); -} - -/** - * std::pow() for Expressions. - * - * @param base The base. - * @param power The power. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr pow( // NOLINT - const ExpressionPtr& base, const ExpressionPtr& power) { - using enum ExpressionType; - - // Prune expression - if (base->IsConstant(0.0)) { - // Return zero - return base; - } else if (base->IsConstant(1.0)) { - return base; - } - if (power->IsConstant(0.0)) { - return MakeExpressionPtr(1.0); - } else if (power->IsConstant(1.0)) { - return base; - } - - // Evaluate constant - if (base->type == kConstant && power->type == kConstant) { - return MakeExpressionPtr(std::pow(base->value, power->value)); - } - - return MakeExpressionPtr( - base->type == kLinear && power->IsConstant(2.0) ? kQuadratic : kNonlinear, - [](double base, double power) { return std::pow(base, power); }, - [](double base, double power, double parentAdjoint) { - return parentAdjoint * std::pow(base, power - 1) * power; - }, - [](double base, double power, double parentAdjoint) { - // Since x * std::log(x) -> 0 as x -> 0 - if (base == 0.0) { - return 0.0; - } else { - return parentAdjoint * std::pow(base, power - 1) * base * - std::log(base); - } - }, - [](const ExpressionPtr& base, const ExpressionPtr& power, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * - sleipnir::detail::pow(base, power - MakeExpressionPtr(1.0)) * - power; - }, - [](const ExpressionPtr& base, const ExpressionPtr& power, - const ExpressionPtr& parentAdjoint) { - // Since x * std::log(x) -> 0 as x -> 0 - if (base->value == 0.0) { - // Return zero - return base; - } else { - return parentAdjoint * - sleipnir::detail::pow(base, power - MakeExpressionPtr(1.0)) * - base * sleipnir::detail::log(base); - } - }, - base, power); -} - -/** - * sign() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { - using enum ExpressionType; - - // Evaluate constant - if (x->type == kConstant) { - if (x->value < 0.0) { - return MakeExpressionPtr(-1.0); - } else if (x->value == 0.0) { - // Return zero - return x; - } else { - return MakeExpressionPtr(1.0); - } - } - - return MakeExpressionPtr( - kNonlinear, - [](double x, double) { - if (x < 0.0) { - return -1.0; - } else if (x == 0.0) { - return 0.0; - } else { - return 1.0; - } - }, - [](double, double, double) { return 0.0; }, - [](const ExpressionPtr&, const ExpressionPtr&, const ExpressionPtr&) { - // Return zero - return MakeExpressionPtr(); - }, - x); -} - -/** - * std::sin() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero - return x; - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::sin(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::sin(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint * std::cos(x); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * sleipnir::detail::cos(x); - }, - x); -} - -/** - * std::sinh() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero - return x; - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::sinh(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::sinh(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint * std::cosh(x); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * sleipnir::detail::cosh(x); - }, - x); -} - -/** - * std::sqrt() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Evaluate constant - if (x->type == kConstant) { - if (x->value == 0.0) { - // Return zero - return x; - } else if (x->value == 1.0) { - return x; - } else { - return MakeExpressionPtr(std::sqrt(x->value)); - } - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::sqrt(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / (2.0 * std::sqrt(x)); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint / - (MakeExpressionPtr(2.0) * sleipnir::detail::sqrt(x)); - }, - x); -} - -/** - * std::tan() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr tan( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero - return x; - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::tan(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::tan(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / (std::cos(x) * std::cos(x)); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint / - (sleipnir::detail::cos(x) * sleipnir::detail::cos(x)); - }, - x); -} - -/** - * std::tanh() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr tanh(const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero - return x; - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::tanh(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::tanh(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / (std::cosh(x) * std::cosh(x)); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint / - (sleipnir::detail::cosh(x) * sleipnir::detail::cosh(x)); - }, - x); -} - -} // namespace sleipnir::detail diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionGraph.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionGraph.hpp deleted file mode 100644 index 714bcbb829..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionGraph.hpp +++ /dev/null @@ -1,244 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -#include - -#include "sleipnir/autodiff/Expression.hpp" -#include "sleipnir/util/FunctionRef.hpp" -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir::detail { - -/** - * This class is an adaptor type that performs value updates of an expression's - * computational graph in a way that skips duplicates. - */ -class SLEIPNIR_DLLEXPORT ExpressionGraph { - public: - /** - * Generates the deduplicated computational graph for the given expression. - * - * @param root The root node of the expression. - */ - explicit ExpressionGraph(ExpressionPtr& root) { - // If the root type is a constant, Update() is a no-op, so there's no work - // to do - if (root == nullptr || root->type == ExpressionType::kConstant) { - return; - } - - // Breadth-first search (BFS) is used as opposed to a depth-first search - // (DFS) to avoid counting duplicate nodes multiple times. A list of nodes - // ordered from parent to child with no duplicates is generated. - // - // https://en.wikipedia.org/wiki/Breadth-first_search - - // BFS list sorted from parent to child. - wpi::SmallVector stack; - - stack.emplace_back(root.Get()); - - // Initialize the number of instances of each node in the tree - // (Expression::duplications) - while (!stack.empty()) { - auto currentNode = stack.back(); - stack.pop_back(); - - for (auto&& arg : currentNode->args) { - // Only continue if the node is not a constant and hasn't already been - // explored. - if (arg != nullptr && arg->type != ExpressionType::kConstant) { - // If this is the first instance of the node encountered (it hasn't - // been explored yet), add it to stack so it's recursed upon - if (arg->duplications == 0) { - stack.push_back(arg.Get()); - } - ++arg->duplications; - } - } - } - - stack.emplace_back(root.Get()); - - while (!stack.empty()) { - auto currentNode = stack.back(); - stack.pop_back(); - - // BFS lists sorted from parent to child. - m_rowList.emplace_back(currentNode->row); - m_adjointList.emplace_back(currentNode); - if (currentNode->valueFunc != nullptr) { - // Constants are skipped because they have no valueFunc and don't need - // to be updated - m_valueList.emplace_back(currentNode); - } - - for (auto&& arg : currentNode->args) { - // Only add node if it's not a constant and doesn't already exist in the - // tape. - if (arg != nullptr && arg->type != ExpressionType::kConstant) { - // Once the number of node visitations equals the number of - // duplications (the counter hits zero), add it to the stack. Note - // that this means the node is only enqueued once. - --arg->duplications; - if (arg->duplications == 0) { - stack.push_back(arg.Get()); - } - } - } - } - } - - /** - * Update the values of all nodes in this computational tree based on the - * values of their dependent nodes. - */ - void Update() { - // Traverse the BFS list backward from child to parent and update the value - // of each node. - for (auto it = m_valueList.rbegin(); it != m_valueList.rend(); ++it) { - auto& node = *it; - - auto& lhs = node->args[0]; - auto& rhs = node->args[1]; - - if (lhs != nullptr) { - if (rhs != nullptr) { - node->value = node->valueFunc(lhs->value, rhs->value); - } else { - node->value = node->valueFunc(lhs->value, 0.0); - } - } - } - } - - /** - * Returns the variable's gradient tree. - * - * @param wrt Variables with respect to which to compute the gradient. - */ - wpi::SmallVector GenerateGradientTree( - std::span wrt) const { - // Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation - // for background on reverse accumulation automatic differentiation. - - for (size_t row = 0; row < wrt.size(); ++row) { - wrt[row]->row = row; - } - - wpi::SmallVector grad; - grad.reserve(wrt.size()); - for (size_t row = 0; row < wrt.size(); ++row) { - grad.emplace_back(MakeExpressionPtr()); - } - - // Zero adjoints. The root node's adjoint is 1.0 as df/df is always 1. - if (m_adjointList.size() > 0) { - m_adjointList[0]->adjointExpr = MakeExpressionPtr(1.0); - for (auto it = m_adjointList.begin() + 1; it != m_adjointList.end(); - ++it) { - auto& node = *it; - node->adjointExpr = MakeExpressionPtr(); - } - } - - // df/dx = (df/dy)(dy/dx). The adjoint of x is equal to the adjoint of y - // multiplied by dy/dx. If there are multiple "paths" from the root node to - // variable; the variable's adjoint is the sum of each path's adjoint - // contribution. - for (auto node : m_adjointList) { - auto& lhs = node->args[0]; - auto& rhs = node->args[1]; - - if (lhs != nullptr && !lhs->IsConstant(0.0)) { - lhs->adjointExpr = lhs->adjointExpr + - node->gradientFuncs[0](lhs, rhs, node->adjointExpr); - } - if (rhs != nullptr && !rhs->IsConstant(0.0)) { - rhs->adjointExpr = rhs->adjointExpr + - node->gradientFuncs[1](lhs, rhs, node->adjointExpr); - } - - // If variable is a leaf node, assign its adjoint to the gradient. - if (node->row != -1) { - grad[node->row] = node->adjointExpr; - } - } - - // Unlink adjoints to avoid circular references between them and their - // parent expressions. This ensures all expressions are returned to the free - // list. - for (auto node : m_adjointList) { - for (auto& arg : node->args) { - if (arg != nullptr) { - arg->adjointExpr = nullptr; - } - } - } - - for (size_t row = 0; row < wrt.size(); ++row) { - wrt[row]->row = -1; - } - - return grad; - } - - /** - * Updates the adjoints in the expression graph, effectively computing the - * gradient. - * - * @param func A function that takes two arguments: an int for the gradient - * row, and a double for the adjoint (gradient value). - */ - void ComputeAdjoints(function_ref func) { - // Zero adjoints. The root node's adjoint is 1.0 as df/df is always 1. - m_adjointList[0]->adjoint = 1.0; - for (auto it = m_adjointList.begin() + 1; it != m_adjointList.end(); ++it) { - auto& node = *it; - node->adjoint = 0.0; - } - - // df/dx = (df/dy)(dy/dx). The adjoint of x is equal to the adjoint of y - // multiplied by dy/dx. If there are multiple "paths" from the root node to - // variable; the variable's adjoint is the sum of each path's adjoint - // contribution. - for (size_t col = 0; col < m_adjointList.size(); ++col) { - auto& node = m_adjointList[col]; - auto& lhs = node->args[0]; - auto& rhs = node->args[1]; - - if (lhs != nullptr) { - if (rhs != nullptr) { - lhs->adjoint += node->gradientValueFuncs[0](lhs->value, rhs->value, - node->adjoint); - rhs->adjoint += node->gradientValueFuncs[1](lhs->value, rhs->value, - node->adjoint); - } else { - lhs->adjoint += - node->gradientValueFuncs[0](lhs->value, 0.0, node->adjoint); - } - } - - // If variable is a leaf node, assign its adjoint to the gradient. - int row = m_rowList[col]; - if (row != -1) { - func(row, node->adjoint); - } - } - } - - private: - // List that maps nodes to their respective row. - wpi::SmallVector m_rowList; - - // List for updating adjoints - wpi::SmallVector m_adjointList; - - // List for updating values - wpi::SmallVector m_valueList; -}; - -} // namespace sleipnir::detail diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionType.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionType.hpp deleted file mode 100644 index 37825e3b2e..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionType.hpp +++ /dev/null @@ -1,27 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -namespace sleipnir { - -/** - * Expression type. - * - * Used for autodiff caching. - */ -enum class ExpressionType : uint8_t { - /// There is no expression. - kNone, - /// The expression is a constant. - kConstant, - /// The expression is composed of linear and lower-order operators. - kLinear, - /// The expression is composed of quadratic and lower-order operators. - kQuadratic, - /// The expression is composed of nonlinear and lower-order operators. - kNonlinear -}; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Hessian.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Hessian.hpp deleted file mode 100644 index 2e60d89e95..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Hessian.hpp +++ /dev/null @@ -1,79 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -#include -#include -#include - -#include "sleipnir/autodiff/ExpressionGraph.hpp" -#include "sleipnir/autodiff/Jacobian.hpp" -#include "sleipnir/autodiff/Profiler.hpp" -#include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/autodiff/VariableMatrix.hpp" -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** - * This class calculates the Hessian of a variable with respect to a vector of - * variables. - * - * The gradient tree is cached so subsequent Hessian calculations are faster, - * and the Hessian is only recomputed if the variable expression is nonlinear. - */ -class SLEIPNIR_DLLEXPORT Hessian { - public: - /** - * Constructs a Hessian object. - * - * @param variable Variable of which to compute the Hessian. - * @param wrt Vector of variables with respect to which to compute the - * Hessian. - */ - Hessian(Variable variable, const VariableMatrix& wrt) noexcept - : m_jacobian{ - [&] { - wpi::SmallVector wrtVec; - wrtVec.reserve(wrt.size()); - for (auto& elem : wrt) { - wrtVec.emplace_back(elem.expr); - } - - auto grad = - detail::ExpressionGraph{variable.expr}.GenerateGradientTree( - wrtVec); - - VariableMatrix ret{wrt.Rows()}; - for (int row = 0; row < ret.Rows(); ++row) { - ret(row) = Variable{std::move(grad[row])}; - } - return ret; - }(), - wrt} {} - - /** - * Returns the Hessian as a VariableMatrix. - * - * This is useful when constructing optimization problems with derivatives in - * them. - */ - VariableMatrix Get() const { return m_jacobian.Get(); } - - /** - * Evaluates the Hessian at wrt's value. - */ - const Eigen::SparseMatrix& Value() { return m_jacobian.Value(); } - - /** - * Returns the profiler. - */ - Profiler& GetProfiler() { return m_jacobian.GetProfiler(); } - - private: - Jacobian m_jacobian; -}; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Jacobian.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Jacobian.hpp deleted file mode 100644 index 0c660156c8..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Jacobian.hpp +++ /dev/null @@ -1,155 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -#include -#include - -#include "sleipnir/autodiff/ExpressionGraph.hpp" -#include "sleipnir/autodiff/Profiler.hpp" -#include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/autodiff/VariableMatrix.hpp" -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** - * This class calculates the Jacobian of a vector of variables with respect to a - * vector of variables. - * - * The Jacobian is only recomputed if the variable expression is quadratic or - * higher order. - */ -class SLEIPNIR_DLLEXPORT Jacobian { - public: - /** - * Constructs a Jacobian object. - * - * @param variables Vector of variables of which to compute the Jacobian. - * @param wrt Vector of variables with respect to which to compute the - * Jacobian. - */ - Jacobian(const VariableMatrix& variables, const VariableMatrix& wrt) noexcept - : m_variables{std::move(variables)}, m_wrt{std::move(wrt)} { - m_profiler.StartSetup(); - - for (int row = 0; row < m_wrt.Rows(); ++row) { - m_wrt(row).expr->row = row; - } - - for (Variable variable : m_variables) { - m_graphs.emplace_back(variable.expr); - } - - // Reserve triplet space for 99% sparsity - m_cachedTriplets.reserve(m_variables.Rows() * m_wrt.Rows() * 0.01); - - for (int row = 0; row < m_variables.Rows(); ++row) { - if (m_variables(row).Type() == ExpressionType::kLinear) { - // If the row is linear, compute its gradient once here and cache its - // triplets. Constant rows are ignored because their gradients have no - // nonzero triplets. - m_graphs[row].ComputeAdjoints([&](int col, double adjoint) { - m_cachedTriplets.emplace_back(row, col, adjoint); - }); - } else if (m_variables(row).Type() > ExpressionType::kLinear) { - // If the row is quadratic or nonlinear, add it to the list of nonlinear - // rows to be recomputed in Value(). - m_nonlinearRows.emplace_back(row); - } - } - - for (int row = 0; row < m_wrt.Rows(); ++row) { - m_wrt(row).expr->row = -1; - } - - if (m_nonlinearRows.empty()) { - m_J.setFromTriplets(m_cachedTriplets.begin(), m_cachedTriplets.end()); - } - - m_profiler.StopSetup(); - } - - /** - * Returns the Jacobian as a VariableMatrix. - * - * This is useful when constructing optimization problems with derivatives in - * them. - */ - VariableMatrix Get() const { - VariableMatrix result{m_variables.Rows(), m_wrt.Rows()}; - - wpi::SmallVector wrtVec; - wrtVec.reserve(m_wrt.size()); - for (auto& elem : m_wrt) { - wrtVec.emplace_back(elem.expr); - } - - for (int row = 0; row < m_variables.Rows(); ++row) { - auto grad = m_graphs[row].GenerateGradientTree(wrtVec); - for (int col = 0; col < m_wrt.Rows(); ++col) { - result(row, col) = Variable{std::move(grad[col])}; - } - } - - return result; - } - - /** - * Evaluates the Jacobian at wrt's value. - */ - const Eigen::SparseMatrix& Value() { - if (m_nonlinearRows.empty()) { - return m_J; - } - - m_profiler.StartSolve(); - - for (auto& graph : m_graphs) { - graph.Update(); - } - - // Copy the cached triplets so triplets added for the nonlinear rows are - // thrown away at the end of the function - auto triplets = m_cachedTriplets; - - // Compute each nonlinear row of the Jacobian - for (int row : m_nonlinearRows) { - m_graphs[row].ComputeAdjoints([&](int col, double adjoint) { - triplets.emplace_back(row, col, adjoint); - }); - } - - m_J.setFromTriplets(triplets.begin(), triplets.end()); - - m_profiler.StopSolve(); - - return m_J; - } - - /** - * Returns the profiler. - */ - Profiler& GetProfiler() { return m_profiler; } - - private: - VariableMatrix m_variables; - VariableMatrix m_wrt; - - wpi::SmallVector m_graphs; - - Eigen::SparseMatrix m_J{m_variables.Rows(), m_wrt.Rows()}; - - // Cached triplets for gradients of linear rows - wpi::SmallVector> m_cachedTriplets; - - // List of row indices for nonlinear rows whose graients will be computed in - // Value() - wpi::SmallVector m_nonlinearRows; - - Profiler m_profiler; -}; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Profiler.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Profiler.hpp deleted file mode 100644 index 5c22d145b3..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Profiler.hpp +++ /dev/null @@ -1,79 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** - * Records the number of profiler measurements (start/stop pairs) and the - * average duration between each start and stop call. - */ -class SLEIPNIR_DLLEXPORT Profiler { - public: - /** - * Tell the profiler to start measuring setup time. - */ - void StartSetup() { m_setupStartTime = std::chrono::system_clock::now(); } - - /** - * Tell the profiler to stop measuring setup time. - */ - void StopSetup() { - m_setupDuration = std::chrono::system_clock::now() - m_setupStartTime; - } - - /** - * Tell the profiler to start measuring solve time. - */ - void StartSolve() { m_solveStartTime = std::chrono::system_clock::now(); } - - /** - * Tell the profiler to stop measuring solve time, increment the number of - * averages, and incorporate the latest measurement into the average. - */ - void StopSolve() { - auto now = std::chrono::system_clock::now(); - ++m_solveMeasurements; - m_averageSolveDuration = - (m_solveMeasurements - 1.0) / m_solveMeasurements * - m_averageSolveDuration + - 1.0 / m_solveMeasurements * (now - m_solveStartTime); - } - - /** - * The setup duration in milliseconds as a double. - */ - double SetupDuration() const { - using std::chrono::duration_cast; - using std::chrono::nanoseconds; - return duration_cast(m_setupDuration).count() / 1e6; - } - - /** - * The number of solve measurements taken. - */ - int SolveMeasurements() const { return m_solveMeasurements; } - - /** - * The average solve duration in milliseconds as a double. - */ - double AverageSolveDuration() const { - using std::chrono::duration_cast; - using std::chrono::nanoseconds; - return duration_cast(m_averageSolveDuration).count() / 1e6; - } - - private: - std::chrono::system_clock::time_point m_setupStartTime; - std::chrono::duration m_setupDuration{0.0}; - - int m_solveMeasurements = 0; - std::chrono::duration m_averageSolveDuration{0.0}; - std::chrono::system_clock::time_point m_solveStartTime; -}; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp deleted file mode 100644 index 72bb235917..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp +++ /dev/null @@ -1,765 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include -#include -#include - -#include - -#include "sleipnir/autodiff/Slice.hpp" -#include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/util/Assert.hpp" -#include "sleipnir/util/FunctionRef.hpp" - -namespace sleipnir { - -/** - * A submatrix of autodiff variables with reference semantics. - * - * @tparam Mat The type of the matrix whose storage this class points to. - */ -template -class VariableBlock { - public: - VariableBlock(const VariableBlock& values) = default; - - /** - * Assigns a VariableBlock to the block. - * - * @param values VariableBlock of values. - */ - VariableBlock& operator=(const VariableBlock& values) { - if (this == &values) { - return *this; - } - - if (m_mat == nullptr) { - m_mat = values.m_mat; - m_rowSlice = values.m_rowSlice; - m_rowSliceLength = values.m_rowSliceLength; - m_colSlice = values.m_colSlice; - m_colSliceLength = values.m_colSliceLength; - } else { - Assert(Rows() == values.Rows()); - Assert(Cols() == values.Cols()); - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) = values(row, col); - } - } - } - - return *this; - } - - VariableBlock(VariableBlock&&) = default; - - /** - * Assigns a VariableBlock to the block. - * - * @param values VariableBlock of values. - */ - VariableBlock& operator=(VariableBlock&& values) { - if (this == &values) { - return *this; - } - - if (m_mat == nullptr) { - m_mat = values.m_mat; - m_rowSlice = values.m_rowSlice; - m_rowSliceLength = values.m_rowSliceLength; - m_colSlice = values.m_colSlice; - m_colSliceLength = values.m_colSliceLength; - } else { - Assert(Rows() == values.Rows()); - Assert(Cols() == values.Cols()); - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) = values(row, col); - } - } - } - - return *this; - } - - /** - * Constructs a Variable block pointing to all of the given matrix. - * - * @param mat The matrix to which to point. - */ - VariableBlock(Mat& mat) // NOLINT - : m_mat{&mat}, - m_rowSlice{0, mat.Rows(), 1}, - m_rowSliceLength{m_rowSlice.Adjust(mat.Rows())}, - m_colSlice{0, mat.Cols(), 1}, - m_colSliceLength{m_colSlice.Adjust(mat.Cols())} {} - - /** - * Constructs a Variable block pointing to a subset of the given matrix. - * - * @param mat The matrix to which to point. - * @param rowOffset The block's row offset. - * @param colOffset The block's column offset. - * @param blockRows The number of rows in the block. - * @param blockCols The number of columns in the block. - */ - VariableBlock(Mat& mat, int rowOffset, int colOffset, int blockRows, - int blockCols) - : m_mat{&mat}, - m_rowSlice{rowOffset, rowOffset + blockRows, 1}, - m_rowSliceLength{m_rowSlice.Adjust(mat.Rows())}, - m_colSlice{colOffset, colOffset + blockCols, 1}, - m_colSliceLength{m_colSlice.Adjust(mat.Cols())} {} - - /** - * Constructs a Variable block pointing to a subset of the given matrix. - * - * Note that the slices are taken as is rather than adjusted. - * - * @param mat The matrix to which to point. - * @param rowSlice The block's row slice. - * @param rowSliceLength The block's row length. - * @param colSlice The block's column slice. - * @param colSliceLength The block's column length. - */ - VariableBlock(Mat& mat, Slice rowSlice, int rowSliceLength, Slice colSlice, - int colSliceLength) - : m_mat{&mat}, - m_rowSlice{std::move(rowSlice)}, - m_rowSliceLength{rowSliceLength}, - m_colSlice{std::move(colSlice)}, - m_colSliceLength{colSliceLength} {} - - /** - * Assigns a double to the block. - * - * This only works for blocks with one row and one column. - */ - VariableBlock& operator=(double value) { - Assert(Rows() == 1 && Cols() == 1); - - (*this)(0, 0) = value; - - return *this; - } - - /** - * Assigns a double to the block. - * - * This only works for blocks with one row and one column. - * - * @param value Value to assign. - */ - void SetValue(double value) { - Assert(Rows() == 1 && Cols() == 1); - - (*this)(0, 0).SetValue(value); - } - - /** - * Assigns an Eigen matrix to the block. - * - * @param values Eigen matrix of values to assign. - */ - template - VariableBlock& operator=(const Eigen::MatrixBase& values) { - Assert(Rows() == values.rows()); - Assert(Cols() == values.cols()); - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) = values(row, col); - } - } - - return *this; - } - - /** - * Sets block's internal values. - * - * @param values Eigen matrix of values. - */ - template - requires std::same_as - void SetValue(const Eigen::MatrixBase& values) { - Assert(Rows() == values.rows()); - Assert(Cols() == values.cols()); - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col).SetValue(values(row, col)); - } - } - } - - /** - * Assigns a VariableMatrix to the block. - * - * @param values VariableMatrix of values. - */ - VariableBlock& operator=(const Mat& values) { - Assert(Rows() == values.Rows()); - Assert(Cols() == values.Cols()); - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) = values(row, col); - } - } - return *this; - } - - /** - * Assigns a VariableMatrix to the block. - * - * @param values VariableMatrix of values. - */ - VariableBlock& operator=(Mat&& values) { - Assert(Rows() == values.Rows()); - Assert(Cols() == values.Cols()); - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) = std::move(values(row, col)); - } - } - return *this; - } - - /** - * Returns a scalar subblock at the given row and column. - * - * @param row The scalar subblock's row. - * @param col The scalar subblock's column. - */ - Variable& operator()(int row, int col) - requires(!std::is_const_v) - { - Assert(row >= 0 && row < Rows()); - Assert(col >= 0 && col < Cols()); - return (*m_mat)(m_rowSlice.start + row * m_rowSlice.step, - m_colSlice.start + col * m_colSlice.step); - } - - /** - * Returns a scalar subblock at the given row and column. - * - * @param row The scalar subblock's row. - * @param col The scalar subblock's column. - */ - const Variable& operator()(int row, int col) const { - Assert(row >= 0 && row < Rows()); - Assert(col >= 0 && col < Cols()); - return (*m_mat)(m_rowSlice.start + row * m_rowSlice.step, - m_colSlice.start + col * m_colSlice.step); - } - - /** - * Returns a scalar subblock at the given row. - * - * @param row The scalar subblock's row. - */ - Variable& operator()(int row) - requires(!std::is_const_v) - { - Assert(row >= 0 && row < Rows() * Cols()); - return (*this)(row / Cols(), row % Cols()); - } - - /** - * Returns a scalar subblock at the given row. - * - * @param row The scalar subblock's row. - */ - const Variable& operator()(int row) const { - Assert(row >= 0 && row < Rows() * Cols()); - return (*this)(row / Cols(), row % Cols()); - } - - /** - * Returns a block of the variable matrix. - * - * @param rowOffset The row offset of the block selection. - * @param colOffset The column offset of the block selection. - * @param blockRows The number of rows in the block selection. - * @param blockCols The number of columns in the block selection. - */ - VariableBlock Block(int rowOffset, int colOffset, int blockRows, - int blockCols) { - Assert(rowOffset >= 0 && rowOffset <= Rows()); - Assert(colOffset >= 0 && colOffset <= Cols()); - Assert(blockRows >= 0 && blockRows <= Rows() - rowOffset); - Assert(blockCols >= 0 && blockCols <= Cols() - colOffset); - return (*this)({rowOffset, rowOffset + blockRows, 1}, - {colOffset, colOffset + blockCols, 1}); - } - - /** - * Returns a block slice of the variable matrix. - * - * @param rowOffset The row offset of the block selection. - * @param colOffset The column offset of the block selection. - * @param blockRows The number of rows in the block selection. - * @param blockCols The number of columns in the block selection. - */ - const VariableBlock Block(int rowOffset, int colOffset, - int blockRows, int blockCols) const { - Assert(rowOffset >= 0 && rowOffset <= Rows()); - Assert(colOffset >= 0 && colOffset <= Cols()); - Assert(blockRows >= 0 && blockRows <= Rows() - rowOffset); - Assert(blockCols >= 0 && blockCols <= Cols() - colOffset); - return (*this)({rowOffset, rowOffset + blockRows, 1}, - {colOffset, colOffset + blockCols, 1}); - } - - /** - * Returns a slice of the variable matrix. - * - * @param rowSlice The row slice. - * @param colSlice The column slice. - */ - VariableBlock operator()(Slice rowSlice, Slice colSlice) { - int rowSliceLength = rowSlice.Adjust(m_rowSliceLength); - int colSliceLength = colSlice.Adjust(m_colSliceLength); - return VariableBlock{ - *m_mat, - {m_rowSlice.start + rowSlice.start * m_rowSlice.step, - m_rowSlice.start + rowSlice.stop, m_rowSlice.step * rowSlice.step}, - rowSliceLength, - {m_colSlice.start + colSlice.start * m_colSlice.step, - m_colSlice.start + colSlice.stop, m_colSlice.step * colSlice.step}, - colSliceLength}; - } - - /** - * Returns a slice of the variable matrix. - * - * @param rowSlice The row slice. - * @param colSlice The column slice. - */ - const VariableBlock operator()(Slice rowSlice, - Slice colSlice) const { - int rowSliceLength = rowSlice.Adjust(m_rowSliceLength); - int colSliceLength = colSlice.Adjust(m_colSliceLength); - return VariableBlock{ - *m_mat, - {m_rowSlice.start + rowSlice.start * m_rowSlice.step, - m_rowSlice.start + rowSlice.stop, m_rowSlice.step * rowSlice.step}, - rowSliceLength, - {m_colSlice.start + colSlice.start * m_colSlice.step, - m_colSlice.start + colSlice.stop, m_colSlice.step * colSlice.step}, - colSliceLength}; - } - - /** - * Returns a slice of the variable matrix. - * - * The given slices aren't adjusted. This overload is for Python bindings - * only. - * - * @param rowSlice The row slice. - * @param rowSliceLength The row slice length. - * @param colSlice The column slice. - * @param colSliceLength The column slice length. - */ - VariableBlock operator()(Slice rowSlice, int rowSliceLength, - Slice colSlice, int colSliceLength) { - return VariableBlock{ - *m_mat, - {m_rowSlice.start + rowSlice.start * m_rowSlice.step, - m_rowSlice.start + rowSlice.stop, m_rowSlice.step * rowSlice.step}, - rowSliceLength, - {m_colSlice.start + colSlice.start * m_colSlice.step, - m_colSlice.start + colSlice.stop, m_colSlice.step * colSlice.step}, - colSliceLength}; - } - - /** - * Returns a slice of the variable matrix. - * - * The given slices aren't adjusted. This overload is for Python bindings - * only. - * - * @param rowSlice The row slice. - * @param rowSliceLength The row slice length. - * @param colSlice The column slice. - * @param colSliceLength The column slice length. - */ - const VariableBlock operator()(Slice rowSlice, int rowSliceLength, - Slice colSlice, - int colSliceLength) const { - return VariableBlock{ - *m_mat, - {m_rowSlice.start + rowSlice.start * m_rowSlice.step, - m_rowSlice.start + rowSlice.stop, m_rowSlice.step * rowSlice.step}, - rowSliceLength, - {m_colSlice.start + colSlice.start * m_colSlice.step, - m_colSlice.start + colSlice.stop, m_colSlice.step * colSlice.step}, - colSliceLength}; - } - - /** - * Returns a segment of the variable vector. - * - * @param offset The offset of the segment. - * @param length The length of the segment. - */ - VariableBlock Segment(int offset, int length) { - Assert(offset >= 0 && offset < Rows() * Cols()); - Assert(length >= 0 && length <= Rows() * Cols() - offset); - return Block(offset, 0, length, 1); - } - - /** - * Returns a segment of the variable vector. - * - * @param offset The offset of the segment. - * @param length The length of the segment. - */ - const VariableBlock Segment(int offset, int length) const { - Assert(offset >= 0 && offset < Rows() * Cols()); - Assert(length >= 0 && length <= Rows() * Cols() - offset); - return Block(offset, 0, length, 1); - } - - /** - * Returns a row slice of the variable matrix. - * - * @param row The row to slice. - */ - VariableBlock Row(int row) { - Assert(row >= 0 && row < Rows()); - return Block(row, 0, 1, Cols()); - } - - /** - * Returns a row slice of the variable matrix. - * - * @param row The row to slice. - */ - VariableBlock Row(int row) const { - Assert(row >= 0 && row < Rows()); - return Block(row, 0, 1, Cols()); - } - - /** - * Returns a column slice of the variable matrix. - * - * @param col The column to slice. - */ - VariableBlock Col(int col) { - Assert(col >= 0 && col < Cols()); - return Block(0, col, Rows(), 1); - } - - /** - * Returns a column slice of the variable matrix. - * - * @param col The column to slice. - */ - VariableBlock Col(int col) const { - Assert(col >= 0 && col < Cols()); - return Block(0, col, Rows(), 1); - } - - /** - * Compound matrix multiplication-assignment operator. - * - * @param rhs Variable to multiply. - */ - VariableBlock& operator*=(const VariableBlock& rhs) { - Assert(Cols() == rhs.Rows() && Cols() == rhs.Cols()); - - for (int i = 0; i < Rows(); ++i) { - for (int j = 0; j < rhs.Cols(); ++j) { - Variable sum; - for (int k = 0; k < Cols(); ++k) { - sum += (*this)(i, k) * rhs(k, j); - } - (*this)(i, j) = sum; - } - } - - return *this; - } - - /** - * Compound matrix multiplication-assignment operator (only enabled when lhs - * is a scalar). - * - * @param rhs Variable to multiply. - */ - VariableBlock& operator*=(double rhs) { - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) *= rhs; - } - } - - return *this; - } - - /** - * Compound matrix division-assignment operator (only enabled when rhs - * is a scalar). - * - * @param rhs Variable to divide. - */ - VariableBlock& operator/=(const VariableBlock& rhs) { - Assert(rhs.Rows() == 1 && rhs.Cols() == 1); - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) /= rhs(0, 0); - } - } - - return *this; - } - - /** - * Compound matrix division-assignment operator (only enabled when rhs - * is a scalar). - * - * @param rhs Variable to divide. - */ - VariableBlock& operator/=(double rhs) { - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) /= rhs; - } - } - - return *this; - } - - /** - * Compound addition-assignment operator. - * - * @param rhs Variable to add. - */ - VariableBlock& operator+=(const VariableBlock& rhs) { - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) += rhs(row, col); - } - } - - return *this; - } - - /** - * Compound subtraction-assignment operator. - * - * @param rhs Variable to subtract. - */ - VariableBlock& operator-=(const VariableBlock& rhs) { - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) -= rhs(row, col); - } - } - - return *this; - } - - /** - * Returns the transpose of the variable matrix. - */ - std::remove_cv_t T() const { - std::remove_cv_t result{Cols(), Rows()}; - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - result(col, row) = (*this)(row, col); - } - } - - return result; - } - - /** - * Returns number of rows in the matrix. - */ - int Rows() const { return m_rowSliceLength; } - - /** - * Returns number of columns in the matrix. - */ - int Cols() const { return m_colSliceLength; } - - /** - * Returns an element of the variable matrix. - * - * @param row The row of the element to return. - * @param col The column of the element to return. - */ - double Value(int row, int col) { - Assert(row >= 0 && row < Rows()); - Assert(col >= 0 && col < Cols()); - return (*m_mat)(m_rowSlice.start + row * m_rowSlice.step, - m_colSlice.start + col * m_colSlice.step) - .Value(); - } - - /** - * Returns a row of the variable column vector. - * - * @param index The index of the element to return. - */ - double Value(int index) { - Assert(index >= 0 && index < Rows() * Cols()); - return Value(index / Cols(), index % Cols()); - } - - /** - * Returns the contents of the variable matrix. - */ - Eigen::MatrixXd Value() { - Eigen::MatrixXd result{Rows(), Cols()}; - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - result(row, col) = Value(row, col); - } - } - - return result; - } - - /** - * Transforms the matrix coefficient-wise with an unary operator. - * - * @param unaryOp The unary operator to use for the transform operation. - */ - std::remove_cv_t CwiseTransform( - function_ref unaryOp) const { - std::remove_cv_t result{Rows(), Cols()}; - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - result(row, col) = unaryOp((*this)(row, col)); - } - } - - return result; - } - - class iterator { - public: - using iterator_category = std::forward_iterator_tag; - using value_type = Variable; - using difference_type = std::ptrdiff_t; - using pointer = Variable*; - using reference = Variable&; - - iterator(VariableBlock* mat, int row, int col) - : m_mat{mat}, m_row{row}, m_col{col} {} - - iterator& operator++() { - ++m_col; - if (m_col == m_mat->Cols()) { - m_col = 0; - ++m_row; - } - return *this; - } - iterator operator++(int) { - iterator retval = *this; - ++(*this); - return retval; - } - bool operator==(const iterator&) const = default; - reference operator*() { return (*m_mat)(m_row, m_col); } - - private: - VariableBlock* m_mat; - int m_row; - int m_col; - }; - - class const_iterator { - public: - using iterator_category = std::forward_iterator_tag; - using value_type = Variable; - using difference_type = std::ptrdiff_t; - using pointer = Variable*; - using const_reference = const Variable&; - - const_iterator(const VariableBlock* mat, int row, int col) - : m_mat{mat}, m_row{row}, m_col{col} {} - - const_iterator& operator++() { - ++m_col; - if (m_col == m_mat->Cols()) { - m_col = 0; - ++m_row; - } - return *this; - } - const_iterator operator++(int) { - const_iterator retval = *this; - ++(*this); - return retval; - } - bool operator==(const const_iterator&) const = default; - const_reference operator*() const { return (*m_mat)(m_row, m_col); } - - private: - const VariableBlock* m_mat; - int m_row; - int m_col; - }; - - /** - * Returns begin iterator. - */ - iterator begin() { return iterator(this, 0, 0); } - - /** - * Returns end iterator. - */ - iterator end() { return iterator(this, Rows(), 0); } - - /** - * Returns begin iterator. - */ - const_iterator begin() const { return const_iterator(this, 0, 0); } - - /** - * Returns end iterator. - */ - const_iterator end() const { return const_iterator(this, Rows(), 0); } - - /** - * Returns begin iterator. - */ - const_iterator cbegin() const { return const_iterator(this, 0, 0); } - - /** - * Returns end iterator. - */ - const_iterator cend() const { return const_iterator(this, Rows(), 0); } - - /** - * Returns number of elements in matrix. - */ - size_t size() const { return Rows() * Cols(); } - - private: - Mat* m_mat = nullptr; - - Slice m_rowSlice; - int m_rowSliceLength = 0; - - Slice m_colSlice; - int m_colSliceLength = 0; -}; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp deleted file mode 100644 index 57b09913d1..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp +++ /dev/null @@ -1,1096 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "sleipnir/autodiff/Slice.hpp" -#include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/autodiff/VariableBlock.hpp" -#include "sleipnir/util/Assert.hpp" -#include "sleipnir/util/FunctionRef.hpp" -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** - * A matrix of autodiff variables. - */ -class SLEIPNIR_DLLEXPORT VariableMatrix { - public: - /** - * Constructs an empty VariableMatrix. - */ - VariableMatrix() = default; - - /** - * Constructs a VariableMatrix column vector with the given rows. - * - * @param rows The number of matrix rows. - */ - explicit VariableMatrix(int rows) : m_rows{rows}, m_cols{1} { - for (int row = 0; row < rows; ++row) { - m_storage.emplace_back(); - } - } - - /** - * Constructs a VariableMatrix with the given dimensions. - * - * @param rows The number of matrix rows. - * @param cols The number of matrix columns. - */ - VariableMatrix(int rows, int cols) : m_rows{rows}, m_cols{cols} { - for (int row = 0; row < rows; ++row) { - for (int col = 0; col < cols; ++col) { - m_storage.emplace_back(); - } - } - } - - /** - * Constructs a scalar VariableMatrix from a nested list of Variables. - * - * @param list The nested list of Variables. - */ - VariableMatrix( // NOLINT - std::initializer_list> list) { - // Get row and column counts for destination matrix - m_rows = list.size(); - m_cols = 0; - if (list.size() > 0) { - m_cols = list.begin()->size(); - } - - // Assert the first and latest column counts are the same - for ([[maybe_unused]] - const auto& row : list) { - Assert(list.begin()->size() == row.size()); - } - - m_storage.reserve(Rows() * Cols()); - for (const auto& row : list) { - std::copy(row.begin(), row.end(), std::back_inserter(m_storage)); - } - } - - /** - * Constructs a scalar VariableMatrix from a nested list of doubles. - * - * This overload is for Python bindings only. - * - * @param list The nested list of Variables. - */ - VariableMatrix(const std::vector>& list) { // NOLINT - // Get row and column counts for destination matrix - m_rows = list.size(); - m_cols = 0; - if (list.size() > 0) { - m_cols = list.begin()->size(); - } - - // Assert the first and latest column counts are the same - for ([[maybe_unused]] - const auto& row : list) { - Assert(list.begin()->size() == row.size()); - } - - m_storage.reserve(Rows() * Cols()); - for (const auto& row : list) { - std::copy(row.begin(), row.end(), std::back_inserter(m_storage)); - } - } - - /** - * Constructs a scalar VariableMatrix from a nested list of Variables. - * - * This overload is for Python bindings only. - * - * @param list The nested list of Variables. - */ - VariableMatrix(const std::vector>& list) { // NOLINT - // Get row and column counts for destination matrix - m_rows = list.size(); - m_cols = 0; - if (list.size() > 0) { - m_cols = list.begin()->size(); - } - - // Assert the first and latest column counts are the same - for ([[maybe_unused]] - const auto& row : list) { - Assert(list.begin()->size() == row.size()); - } - - m_storage.reserve(Rows() * Cols()); - for (const auto& row : list) { - std::copy(row.begin(), row.end(), std::back_inserter(m_storage)); - } - } - - /** - * Constructs a VariableMatrix from an Eigen matrix. - * - * @param values Eigen matrix of values. - */ - template - VariableMatrix(const Eigen::MatrixBase& values) // NOLINT - : m_rows{static_cast(values.rows())}, - m_cols{static_cast(values.cols())} { - m_storage.reserve(values.rows() * values.cols()); - for (int row = 0; row < values.rows(); ++row) { - for (int col = 0; col < values.cols(); ++col) { - m_storage.emplace_back(values(row, col)); - } - } - } - - /** - * Constructs a VariableMatrix from an Eigen diagonal matrix. - * - * @param values Diagonal matrix of values. - */ - template - VariableMatrix(const Eigen::DiagonalBase& values) // NOLINT - : m_rows{static_cast(values.rows())}, - m_cols{static_cast(values.cols())} { - m_storage.reserve(values.rows() * values.cols()); - for (int row = 0; row < values.rows(); ++row) { - for (int col = 0; col < values.cols(); ++col) { - if (row == col) { - m_storage.emplace_back(values.diagonal()(row)); - } else { - m_storage.emplace_back(0.0); - } - } - } - } - - /** - * Assigns an Eigen matrix to a VariableMatrix. - * - * @param values Eigen matrix of values. - */ - template - VariableMatrix& operator=(const Eigen::MatrixBase& values) { - Assert(Rows() == values.rows()); - Assert(Cols() == values.cols()); - - for (int row = 0; row < values.rows(); ++row) { - for (int col = 0; col < values.cols(); ++col) { - (*this)(row, col) = values(row, col); - } - } - - return *this; - } - - /** - * Sets the VariableMatrix's internal values. - * - * @param values Eigen matrix of values. - */ - template - requires std::same_as - void SetValue(const Eigen::MatrixBase& values) { - Assert(Rows() == values.rows()); - Assert(Cols() == values.cols()); - - for (int row = 0; row < values.rows(); ++row) { - for (int col = 0; col < values.cols(); ++col) { - (*this)(row, col).SetValue(values(row, col)); - } - } - } - - /** - * Constructs a scalar VariableMatrix from a Variable. - * - * @param variable Variable. - */ - VariableMatrix(const Variable& variable) // NOLINT - : m_rows{1}, m_cols{1} { - m_storage.emplace_back(variable); - } - - /** - * Constructs a scalar VariableMatrix from a Variable. - * - * @param variable Variable. - */ - VariableMatrix(Variable&& variable) : m_rows{1}, m_cols{1} { // NOLINT - m_storage.emplace_back(std::move(variable)); - } - - /** - * Constructs a VariableMatrix from a VariableBlock. - * - * @param values VariableBlock of values. - */ - VariableMatrix(const VariableBlock& values) // NOLINT - : m_rows{values.Rows()}, m_cols{values.Cols()} { - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - m_storage.emplace_back(values(row, col)); - } - } - } - - /** - * Constructs a VariableMatrix from a VariableBlock. - * - * @param values VariableBlock of values. - */ - VariableMatrix(const VariableBlock& values) // NOLINT - : m_rows{values.Rows()}, m_cols{values.Cols()} { - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - m_storage.emplace_back(values(row, col)); - } - } - } - - /** - * Constructs a column vector wrapper around a Variable array. - * - * @param values Variable array to wrap. - */ - explicit VariableMatrix(std::span values) - : m_rows{static_cast(values.size())}, m_cols{1} { - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - m_storage.emplace_back(values[row * Cols() + col]); - } - } - } - - /** - * Constructs a matrix wrapper around a Variable array. - * - * @param values Variable array to wrap. - * @param rows The number of matrix rows. - * @param cols The number of matrix columns. - */ - VariableMatrix(std::span values, int rows, int cols) - : m_rows{rows}, m_cols{cols} { - Assert(static_cast(values.size()) == Rows() * Cols()); - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - m_storage.emplace_back(values[row * Cols() + col]); - } - } - } - - /** - * Returns a block pointing to the given row and column. - * - * @param row The block row. - * @param col The block column. - */ - Variable& operator()(int row, int col) { - Assert(row >= 0 && row < Rows()); - Assert(col >= 0 && col < Cols()); - return m_storage[row * Cols() + col]; - } - - /** - * Returns a block pointing to the given row and column. - * - * @param row The block row. - * @param col The block column. - */ - const Variable& operator()(int row, int col) const { - Assert(row >= 0 && row < Rows()); - Assert(col >= 0 && col < Cols()); - return m_storage[row * Cols() + col]; - } - - /** - * Returns a block pointing to the given row. - * - * @param row The block row. - */ - Variable& operator()(int row) { - Assert(row >= 0 && row < Rows() * Cols()); - return m_storage[row]; - } - - /** - * Returns a block pointing to the given row. - * - * @param row The block row. - */ - const Variable& operator()(int row) const { - Assert(row >= 0 && row < Rows() * Cols()); - return m_storage[row]; - } - - /** - * Returns a block of the variable matrix. - * - * @param rowOffset The row offset of the block selection. - * @param colOffset The column offset of the block selection. - * @param blockRows The number of rows in the block selection. - * @param blockCols The number of columns in the block selection. - */ - VariableBlock Block(int rowOffset, int colOffset, - int blockRows, int blockCols) { - Assert(rowOffset >= 0 && rowOffset <= Rows()); - Assert(colOffset >= 0 && colOffset <= Cols()); - Assert(blockRows >= 0 && blockRows <= Rows() - rowOffset); - Assert(blockCols >= 0 && blockCols <= Cols() - colOffset); - return VariableBlock{*this, rowOffset, colOffset, blockRows, blockCols}; - } - - /** - * Returns a block of the variable matrix. - * - * @param rowOffset The row offset of the block selection. - * @param colOffset The column offset of the block selection. - * @param blockRows The number of rows in the block selection. - * @param blockCols The number of columns in the block selection. - */ - const VariableBlock Block(int rowOffset, int colOffset, - int blockRows, - int blockCols) const { - Assert(rowOffset >= 0 && rowOffset <= Rows()); - Assert(colOffset >= 0 && colOffset <= Cols()); - Assert(blockRows >= 0 && blockRows <= Rows() - rowOffset); - Assert(blockCols >= 0 && blockCols <= Cols() - colOffset); - return VariableBlock{*this, rowOffset, colOffset, blockRows, blockCols}; - } - - /** - * Returns a slice of the variable matrix. - * - * @param rowSlice The row slice. - * @param colSlice The column slice. - */ - VariableBlock operator()(Slice rowSlice, Slice colSlice) { - int rowSliceLength = rowSlice.Adjust(Rows()); - int colSliceLength = colSlice.Adjust(Cols()); - return VariableBlock{*this, std::move(rowSlice), rowSliceLength, - std::move(colSlice), colSliceLength}; - } - - /** - * Returns a slice of the variable matrix. - * - * @param rowSlice The row slice. - * @param colSlice The column slice. - */ - const VariableBlock operator()(Slice rowSlice, - Slice colSlice) const { - int rowSliceLength = rowSlice.Adjust(Rows()); - int colSliceLength = colSlice.Adjust(Cols()); - return VariableBlock{*this, std::move(rowSlice), rowSliceLength, - std::move(colSlice), colSliceLength}; - } - - /** - * Returns a slice of the variable matrix. - * - * The given slices aren't adjusted. This overload is for Python bindings - * only. - * - * @param rowSlice The row slice. - * @param rowSliceLength The row slice length. - * @param colSlice The column slice. - * @param colSliceLength The column slice length. - * - */ - VariableBlock operator()(Slice rowSlice, int rowSliceLength, - Slice colSlice, int colSliceLength) { - return VariableBlock{*this, std::move(rowSlice), rowSliceLength, - std::move(colSlice), colSliceLength}; - } - - /** - * Returns a slice of the variable matrix. - * - * The given slices aren't adjusted. This overload is for Python bindings - * only. - * - * @param rowSlice The row slice. - * @param rowSliceLength The row slice length. - * @param colSlice The column slice. - * @param colSliceLength The column slice length. - */ - const VariableBlock operator()( - Slice rowSlice, int rowSliceLength, Slice colSlice, - int colSliceLength) const { - return VariableBlock{*this, std::move(rowSlice), rowSliceLength, - std::move(colSlice), colSliceLength}; - } - - /** - * Returns a segment of the variable vector. - * - * @param offset The offset of the segment. - * @param length The length of the segment. - */ - VariableBlock Segment(int offset, int length) { - Assert(offset >= 0 && offset < Rows() * Cols()); - Assert(length >= 0 && length <= Rows() * Cols() - offset); - return Block(offset, 0, length, 1); - } - - /** - * Returns a segment of the variable vector. - * - * @param offset The offset of the segment. - * @param length The length of the segment. - */ - const VariableBlock Segment(int offset, - int length) const { - Assert(offset >= 0 && offset < Rows() * Cols()); - Assert(length >= 0 && length <= Rows() * Cols() - offset); - return Block(offset, 0, length, 1); - } - - /** - * Returns a row slice of the variable matrix. - * - * @param row The row to slice. - */ - VariableBlock Row(int row) { - Assert(row >= 0 && row < Rows()); - return Block(row, 0, 1, Cols()); - } - - /** - * Returns a row slice of the variable matrix. - * - * @param row The row to slice. - */ - const VariableBlock Row(int row) const { - Assert(row >= 0 && row < Rows()); - return Block(row, 0, 1, Cols()); - } - - /** - * Returns a column slice of the variable matrix. - * - * @param col The column to slice. - */ - VariableBlock Col(int col) { - Assert(col >= 0 && col < Cols()); - return Block(0, col, Rows(), 1); - } - - /** - * Returns a column slice of the variable matrix. - * - * @param col The column to slice. - */ - const VariableBlock Col(int col) const { - Assert(col >= 0 && col < Cols()); - return Block(0, col, Rows(), 1); - } - - /** - * Matrix multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT VariableMatrix - operator*(const VariableMatrix& lhs, const VariableMatrix& rhs) { - Assert(lhs.Cols() == rhs.Rows()); - - VariableMatrix result{lhs.Rows(), rhs.Cols()}; - - for (int i = 0; i < lhs.Rows(); ++i) { - for (int j = 0; j < rhs.Cols(); ++j) { - Variable sum; - for (int k = 0; k < lhs.Cols(); ++k) { - sum += lhs(i, k) * rhs(k, j); - } - result(i, j) = sum; - } - } - - return result; - } - - /** - * Matrix-scalar multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT VariableMatrix operator*(const VariableMatrix& lhs, - const Variable& rhs) { - VariableMatrix result{lhs.Rows(), lhs.Cols()}; - - for (int row = 0; row < result.Rows(); ++row) { - for (int col = 0; col < result.Cols(); ++col) { - result(row, col) = lhs(row, col) * rhs; - } - } - - return result; - } - - /** - * Matrix-scalar multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT VariableMatrix operator*(const VariableMatrix& lhs, - double rhs) { - return lhs * Variable{rhs}; - } - - /** - * Scalar-matrix multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT VariableMatrix - operator*(const Variable& lhs, const VariableMatrix& rhs) { - VariableMatrix result{rhs.Rows(), rhs.Cols()}; - - for (int row = 0; row < result.Rows(); ++row) { - for (int col = 0; col < result.Cols(); ++col) { - result(row, col) = rhs(row, col) * lhs; - } - } - - return result; - } - - /** - * Scalar-matrix multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT VariableMatrix - operator*(double lhs, const VariableMatrix& rhs) { - return Variable{lhs} * rhs; - } - - /** - * Compound matrix multiplication-assignment operator. - * - * @param rhs Variable to multiply. - */ - VariableMatrix& operator*=(const VariableMatrix& rhs) { - Assert(Cols() == rhs.Rows() && Cols() == rhs.Cols()); - - for (int i = 0; i < Rows(); ++i) { - for (int j = 0; j < rhs.Cols(); ++j) { - Variable sum; - for (int k = 0; k < Cols(); ++k) { - sum += (*this)(i, k) * rhs(k, j); - } - (*this)(i, j) = sum; - } - } - - return *this; - } - - /** - * Binary division operator (only enabled when rhs is a scalar). - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT VariableMatrix operator/(const VariableMatrix& lhs, - const Variable& rhs) { - VariableMatrix result{lhs.Rows(), lhs.Cols()}; - - for (int row = 0; row < result.Rows(); ++row) { - for (int col = 0; col < result.Cols(); ++col) { - result(row, col) = lhs(row, col) / rhs; - } - } - - return result; - } - - /** - * Compound matrix division-assignment operator (only enabled when rhs - * is a scalar). - * - * @param rhs Variable to divide. - */ - VariableMatrix& operator/=(const Variable& rhs) { - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) /= rhs; - } - } - - return *this; - } - - /** - * Binary addition operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT VariableMatrix - operator+(const VariableMatrix& lhs, const VariableMatrix& rhs) { - VariableMatrix result{lhs.Rows(), lhs.Cols()}; - - for (int row = 0; row < result.Rows(); ++row) { - for (int col = 0; col < result.Cols(); ++col) { - result(row, col) = lhs(row, col) + rhs(row, col); - } - } - - return result; - } - - /** - * Compound addition-assignment operator. - * - * @param rhs Variable to add. - */ - VariableMatrix& operator+=(const VariableMatrix& rhs) { - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) += rhs(row, col); - } - } - - return *this; - } - - /** - * Binary subtraction operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT VariableMatrix - operator-(const VariableMatrix& lhs, const VariableMatrix& rhs) { - VariableMatrix result{lhs.Rows(), lhs.Cols()}; - - for (int row = 0; row < result.Rows(); ++row) { - for (int col = 0; col < result.Cols(); ++col) { - result(row, col) = lhs(row, col) - rhs(row, col); - } - } - - return result; - } - - /** - * Compound subtraction-assignment operator. - * - * @param rhs Variable to subtract. - */ - VariableMatrix& operator-=(const VariableMatrix& rhs) { - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) -= rhs(row, col); - } - } - - return *this; - } - - /** - * Unary minus operator. - * - * @param lhs Operand for unary minus. - */ - friend SLEIPNIR_DLLEXPORT VariableMatrix - operator-(const VariableMatrix& lhs) { - VariableMatrix result{lhs.Rows(), lhs.Cols()}; - - for (int row = 0; row < result.Rows(); ++row) { - for (int col = 0; col < result.Cols(); ++col) { - result(row, col) = -lhs(row, col); - } - } - - return result; - } - - /** - * Implicit conversion operator from 1x1 VariableMatrix to Variable. - */ - operator Variable() const { // NOLINT - Assert(Rows() == 1 && Cols() == 1); - return (*this)(0, 0); - } - - /** - * Returns the transpose of the variable matrix. - */ - VariableMatrix T() const { - VariableMatrix result{Cols(), Rows()}; - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - result(col, row) = (*this)(row, col); - } - } - - return result; - } - - /** - * Returns number of rows in the matrix. - */ - int Rows() const { return m_rows; } - - /** - * Returns number of columns in the matrix. - */ - int Cols() const { return m_cols; } - - /** - * Returns an element of the variable matrix. - * - * @param row The row of the element to return. - * @param col The column of the element to return. - */ - double Value(int row, int col) { - Assert(row >= 0 && row < Rows()); - Assert(col >= 0 && col < Cols()); - return m_storage[row * Cols() + col].Value(); - } - - /** - * Returns a row of the variable column vector. - * - * @param index The index of the element to return. - */ - double Value(int index) { - Assert(index >= 0 && index < Rows() * Cols()); - return m_storage[index].Value(); - } - - /** - * Returns the contents of the variable matrix. - */ - Eigen::MatrixXd Value() { - Eigen::MatrixXd result{Rows(), Cols()}; - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - result(row, col) = Value(row, col); - } - } - - return result; - } - - /** - * Transforms the matrix coefficient-wise with an unary operator. - * - * @param unaryOp The unary operator to use for the transform operation. - */ - VariableMatrix CwiseTransform( - function_ref unaryOp) const { - VariableMatrix result{Rows(), Cols()}; - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - result(row, col) = unaryOp((*this)(row, col)); - } - } - - return result; - } - - class iterator { - public: - using iterator_category = std::forward_iterator_tag; - using value_type = Variable; - using difference_type = std::ptrdiff_t; - using pointer = Variable*; - using reference = Variable&; - - iterator(VariableMatrix* mat, int row, int col) - : m_mat{mat}, m_row{row}, m_col{col} {} - - iterator& operator++() { - ++m_col; - if (m_col == m_mat->Cols()) { - m_col = 0; - ++m_row; - } - return *this; - } - iterator operator++(int) { - iterator retval = *this; - ++(*this); - return retval; - } - bool operator==(const iterator&) const = default; - reference operator*() { return (*m_mat)(m_row, m_col); } - - private: - VariableMatrix* m_mat; - int m_row; - int m_col; - }; - - class const_iterator { - public: - using iterator_category = std::forward_iterator_tag; - using value_type = Variable; - using difference_type = std::ptrdiff_t; - using pointer = Variable*; - using const_reference = const Variable&; - - const_iterator(const VariableMatrix* mat, int row, int col) - : m_mat{mat}, m_row{row}, m_col{col} {} - - const_iterator& operator++() { - ++m_col; - if (m_col == m_mat->Cols()) { - m_col = 0; - ++m_row; - } - return *this; - } - const_iterator operator++(int) { - const_iterator retval = *this; - ++(*this); - return retval; - } - bool operator==(const const_iterator&) const = default; - const_reference operator*() const { return (*m_mat)(m_row, m_col); } - - private: - const VariableMatrix* m_mat; - int m_row; - int m_col; - }; - - /** - * Returns begin iterator. - */ - iterator begin() { return iterator(this, 0, 0); } - - /** - * Returns end iterator. - */ - iterator end() { return iterator(this, Rows(), 0); } - - /** - * Returns begin iterator. - */ - const_iterator begin() const { return const_iterator(this, 0, 0); } - - /** - * Returns end iterator. - */ - const_iterator end() const { return const_iterator(this, Rows(), 0); } - - /** - * Returns begin iterator. - */ - const_iterator cbegin() const { return const_iterator(this, 0, 0); } - - /** - * Returns end iterator. - */ - const_iterator cend() const { return const_iterator(this, Rows(), 0); } - - /** - * Returns number of elements in matrix. - */ - size_t size() const { return m_rows * m_cols; } - - /** - * Returns a variable matrix filled with zeroes. - * - * @param rows The number of matrix rows. - * @param cols The number of matrix columns. - */ - static VariableMatrix Zero(int rows, int cols) { - VariableMatrix result{rows, cols}; - - for (auto& elem : result) { - elem = 0.0; - } - - return result; - } - - /** - * Returns a variable matrix filled with ones. - * - * @param rows The number of matrix rows. - * @param cols The number of matrix columns. - */ - static VariableMatrix Ones(int rows, int cols) { - VariableMatrix result{rows, cols}; - - for (auto& elem : result) { - elem = 1.0; - } - - return result; - } - - private: - wpi::SmallVector m_storage; - int m_rows = 0; - int m_cols = 0; -}; - -/** - * Applies a coefficient-wise reduce operation to two matrices. - * - * @param lhs The left-hand side of the binary operator. - * @param rhs The right-hand side of the binary operator. - * @param binaryOp The binary operator to use for the reduce operation. - */ -SLEIPNIR_DLLEXPORT inline VariableMatrix CwiseReduce( - const VariableMatrix& lhs, const VariableMatrix& rhs, - function_ref binaryOp) { - Assert(lhs.Rows() == rhs.Rows()); - Assert(lhs.Rows() == rhs.Rows()); - - VariableMatrix result{lhs.Rows(), lhs.Cols()}; - - for (int row = 0; row < lhs.Rows(); ++row) { - for (int col = 0; col < lhs.Cols(); ++col) { - result(row, col) = binaryOp(lhs(row, col), rhs(row, col)); - } - } - - return result; -} - -/** - * Assemble a VariableMatrix from a nested list of blocks. - * - * Each row's blocks must have the same height, and the assembled block rows - * must have the same width. For example, for the block matrix [[A, B], [C]] to - * be constructible, the number of rows in A and B must match, and the number of - * columns in [A, B] and [C] must match. - * - * @param list The nested list of blocks. - */ -SLEIPNIR_DLLEXPORT inline VariableMatrix Block( - std::initializer_list> list) { - // Get row and column counts for destination matrix - int rows = 0; - int cols = -1; - for (const auto& row : list) { - if (row.size() > 0) { - rows += row.begin()->Rows(); - } - - // Get number of columns in this row - int latestCols = 0; - for (const auto& elem : row) { - // Assert the first and latest row have the same height - Assert(row.begin()->Rows() == elem.Rows()); - - latestCols += elem.Cols(); - } - - // If this is the first row, record the column count. Otherwise, assert the - // first and latest column counts are the same. - if (cols == -1) { - cols = latestCols; - } else { - Assert(cols == latestCols); - } - } - - VariableMatrix result{rows, cols}; - - int rowOffset = 0; - for (const auto& row : list) { - int colOffset = 0; - for (const auto& elem : row) { - result.Block(rowOffset, colOffset, elem.Rows(), elem.Cols()) = elem; - colOffset += elem.Cols(); - } - rowOffset += row.begin()->Rows(); - } - - return result; -} - -/** - * Assemble a VariableMatrix from a nested list of blocks. - * - * Each row's blocks must have the same height, and the assembled block rows - * must have the same width. For example, for the block matrix [[A, B], [C]] to - * be constructible, the number of rows in A and B must match, and the number of - * columns in [A, B] and [C] must match. - * - * This overload is for Python bindings only. - * - * @param list The nested list of blocks. - */ -SLEIPNIR_DLLEXPORT inline VariableMatrix Block( - const std::vector>& list) { - // Get row and column counts for destination matrix - int rows = 0; - int cols = -1; - for (const auto& row : list) { - if (row.size() > 0) { - rows += row.begin()->Rows(); - } - - // Get number of columns in this row - int latestCols = 0; - for (const auto& elem : row) { - // Assert the first and latest row have the same height - Assert(row.begin()->Rows() == elem.Rows()); - - latestCols += elem.Cols(); - } - - // If this is the first row, record the column count. Otherwise, assert the - // first and latest column counts are the same. - if (cols == -1) { - cols = latestCols; - } else { - Assert(cols == latestCols); - } - } - - VariableMatrix result{rows, cols}; - - int rowOffset = 0; - for (const auto& row : list) { - int colOffset = 0; - for (const auto& elem : row) { - result.Block(rowOffset, colOffset, elem.Rows(), elem.Cols()) = elem; - colOffset += elem.Cols(); - } - rowOffset += row.begin()->Rows(); - } - - return result; -} - -/** - * Solves the VariableMatrix equation AX = B for X. - * - * @param A The left-hand side. - * @param B The right-hand side. - * @return The solution X. - */ -SLEIPNIR_DLLEXPORT VariableMatrix Solve(const VariableMatrix& A, - const VariableMatrix& B); - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/adjoint_expression_graph.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/adjoint_expression_graph.hpp new file mode 100644 index 0000000000..4576e19c96 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/adjoint_expression_graph.hpp @@ -0,0 +1,178 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include + +#include +#include + +#include "sleipnir/autodiff/expression_graph.hpp" +#include "sleipnir/autodiff/variable.hpp" +#include "sleipnir/autodiff/variable_matrix.hpp" + +namespace slp::detail { + +/** + * This class is an adaptor type that performs value updates of an expression's + * adjoint graph. + */ +class AdjointExpressionGraph { + public: + /** + * Generates the adjoint graph for the given expression. + * + * @param root The root node of the expression. + */ + explicit AdjointExpressionGraph(const Variable& root) + : m_top_list{topological_sort(root.expr)} { + for (const auto& node : m_top_list) { + m_col_list.emplace_back(node->col); + } + } + + /** + * Update the values of all nodes in this adjoint graph based on the values of + * their dependent nodes. + */ + void update_values() { detail::update_values(m_top_list); } + + /** + * Returns the variable's gradient tree. + * + * This function lazily allocates variables, so elements of the returned + * VariableMatrix will be empty if the corresponding element of wrt had no + * adjoint. Ensure Variable::expr isn't nullptr before calling member + * functions. + * + * @param wrt Variables with respect to which to compute the gradient. + * @return The variable's gradient tree. + */ + VariableMatrix generate_gradient_tree(const VariableMatrix& wrt) const { + // Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation + // for background on reverse accumulation automatic differentiation. + + if (m_top_list.empty()) { + return VariableMatrix{VariableMatrix::empty, wrt.rows(), 1}; + } + + // Set root node's adjoint to 1 since df/df is 1 + m_top_list[0]->adjoint_expr = make_expression_ptr(1.0); + + // df/dx = (df/dy)(dy/dx). The adjoint of x is equal to the adjoint of y + // multiplied by dy/dx. If there are multiple "paths" from the root node to + // variable; the variable's adjoint is the sum of each path's adjoint + // contribution. + for (auto& node : m_top_list) { + auto& lhs = node->args[0]; + auto& rhs = node->args[1]; + + if (lhs != nullptr) { + lhs->adjoint_expr += node->grad_expr_l(lhs, rhs, node->adjoint_expr); + if (rhs != nullptr) { + rhs->adjoint_expr += node->grad_expr_r(lhs, rhs, node->adjoint_expr); + } + } + } + + // Move gradient tree to return value + VariableMatrix grad{VariableMatrix::empty, wrt.rows(), 1}; + for (int row = 0; row < grad.rows(); ++row) { + grad[row] = Variable{std::move(wrt[row].expr->adjoint_expr)}; + } + + // Unlink adjoints to avoid circular references between them and their + // parent expressions. This ensures all expressions are returned to the free + // list. + for (auto& node : m_top_list) { + node->adjoint_expr = nullptr; + } + + return grad; + } + + /** + * Updates the adjoints in the expression graph (computes the gradient) then + * appends the adjoints of wrt to the sparse matrix triplets. + * + * @param triplets The sparse matrix triplets. + * @param row The row of wrt. + * @param wrt Vector of variables with respect to which to compute the + * Jacobian. + */ + void append_adjoint_triplets( + gch::small_vector>& triplets, int row, + const VariableMatrix& wrt) const { + // Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation + // for background on reverse accumulation automatic differentiation. + + // If wrt has fewer nodes than graph, zero wrt's adjoints + if (static_cast(wrt.rows()) < m_top_list.size()) { + for (const auto& elem : wrt) { + elem.expr->adjoint = 0.0; + } + } + + if (m_top_list.empty()) { + return; + } + + // Set root node's adjoint to 1 since df/df is 1 + m_top_list[0]->adjoint = 1.0; + + // Zero the rest of the adjoints + for (auto& node : m_top_list | std::views::drop(1)) { + node->adjoint = 0.0; + } + + // df/dx = (df/dy)(dy/dx). The adjoint of x is equal to the adjoint of y + // multiplied by dy/dx. If there are multiple "paths" from the root node to + // variable; the variable's adjoint is the sum of each path's adjoint + // contribution. + for (const auto& node : m_top_list) { + auto& lhs = node->args[0]; + auto& rhs = node->args[1]; + + if (lhs != nullptr) { + if (rhs != nullptr) { + lhs->adjoint += node->grad_l(lhs->val, rhs->val, node->adjoint); + rhs->adjoint += node->grad_r(lhs->val, rhs->val, node->adjoint); + } else { + lhs->adjoint += node->grad_l(lhs->val, 0.0, node->adjoint); + } + } + } + + // If wrt has fewer nodes than graph, iterate over wrt + if (static_cast(wrt.rows()) < m_top_list.size()) { + for (int col = 0; col < wrt.rows(); ++col) { + const auto& node = wrt[col].expr; + + // Append adjoints of wrt to sparse matrix triplets + if (node->adjoint != 0.0) { + triplets.emplace_back(row, col, node->adjoint); + } + } + } else { + for (size_t i = 0; i < m_top_list.size(); ++i) { + const auto& col = m_col_list[i]; + const auto& node = m_top_list[i]; + + // Append adjoints of wrt to sparse matrix triplets + if (col != -1 && node->adjoint != 0.0) { + triplets.emplace_back(row, col, node->adjoint); + } + } + } + } + + private: + // Topological sort of graph from parent to child + gch::small_vector m_top_list; + + // List that maps nodes to their respective column + gch::small_vector m_col_list; +}; + +} // namespace slp::detail diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp new file mode 100644 index 0000000000..1c5f84d22a --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp @@ -0,0 +1,1758 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include "sleipnir/autodiff/expression_type.hpp" +#include "sleipnir/util/intrusive_shared_ptr.hpp" +#include "sleipnir/util/pool.hpp" + +namespace slp::detail { + +// The global pool allocator uses a thread-local static pool resource, which +// isn't guaranteed to be initialized properly across DLL boundaries on Windows +#ifdef _WIN32 +inline constexpr bool USE_POOL_ALLOCATOR = false; +#else +inline constexpr bool USE_POOL_ALLOCATOR = true; +#endif + +struct Expression; + +inline constexpr void inc_ref_count(Expression* expr); +inline void dec_ref_count(Expression* expr); + +/** + * Typedef for intrusive shared pointer to Expression. + */ +using ExpressionPtr = IntrusiveSharedPtr; + +/** + * Creates an intrusive shared pointer to an expression from the global pool + * allocator. + * + * @tparam T The derived expression type. + * @param args Constructor arguments for Expression. + */ +template +static ExpressionPtr make_expression_ptr(Args&&... args) { + if constexpr (USE_POOL_ALLOCATOR) { + return allocate_intrusive_shared(global_pool_allocator(), + std::forward(args)...); + } else { + return make_intrusive_shared(std::forward(args)...); + } +} + +template +struct BinaryMinusExpression; + +template +struct BinaryPlusExpression; + +struct ConstExpression; + +template +struct DivExpression; + +template +struct MultExpression; + +template +struct UnaryMinusExpression; + +/** + * An autodiff expression node. + */ +struct Expression { + /// The value of the expression node. + double val = 0.0; + + /// The adjoint of the expression node used during autodiff. + double adjoint = 0.0; + + /// Counts incoming edges for this node. + uint32_t incoming_edges = 0; + + /// This expression's column in a Jacobian, or -1 otherwise. + int32_t col = -1; + + /// The adjoint of the expression node used during gradient expression tree + /// generation. + ExpressionPtr adjoint_expr; + + /// Reference count for intrusive shared pointer. + uint32_t ref_count = 0; + + /// Expression arguments. + std::array args{nullptr, nullptr}; + + /** + * Constructs a constant expression with a value of zero. + */ + constexpr Expression() = default; + + /** + * Constructs a nullary expression (an operator with no arguments). + * + * @param value The expression value. + */ + explicit constexpr Expression(double value) : val{value} {} + + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr Expression(ExpressionPtr lhs) + : args{std::move(lhs), nullptr} {} + + /** + * Constructs a binary expression (an operator with two arguments). + * + * @param lhs Binary operator's left operand. + * @param rhs Binary operator's right operand. + */ + constexpr Expression(ExpressionPtr lhs, ExpressionPtr rhs) + : args{std::move(lhs), std::move(rhs)} {} + + virtual ~Expression() = default; + + /** + * Returns true if the expression is the given constant. + * + * @param constant The constant. + * + * @return True if the expression is the given constant. + */ + constexpr bool is_constant(double constant) const { + return type() == ExpressionType::CONSTANT && val == constant; + } + + /** + * Expression-Expression multiplication operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + friend ExpressionPtr operator*(const ExpressionPtr& lhs, + const ExpressionPtr& rhs) { + using enum ExpressionType; + + // Prune expression + if (lhs->is_constant(0.0)) { + // Return zero + return lhs; + } else if (rhs->is_constant(0.0)) { + // Return zero + return rhs; + } else if (lhs->is_constant(1.0)) { + return rhs; + } else if (rhs->is_constant(1.0)) { + return lhs; + } + + // Evaluate constant + if (lhs->type() == CONSTANT && rhs->type() == CONSTANT) { + return make_expression_ptr(lhs->val * rhs->val); + } + + // Evaluate expression type + if (lhs->type() == CONSTANT) { + if (rhs->type() == LINEAR) { + return make_expression_ptr>(lhs, rhs); + } else if (rhs->type() == QUADRATIC) { + return make_expression_ptr>(lhs, rhs); + } else { + return make_expression_ptr>(lhs, rhs); + } + } else if (rhs->type() == CONSTANT) { + if (lhs->type() == LINEAR) { + return make_expression_ptr>(lhs, rhs); + } else if (lhs->type() == QUADRATIC) { + return make_expression_ptr>(lhs, rhs); + } else { + return make_expression_ptr>(lhs, rhs); + } + } else if (lhs->type() == LINEAR && rhs->type() == LINEAR) { + return make_expression_ptr>(lhs, rhs); + } else { + return make_expression_ptr>(lhs, rhs); + } + } + + /** + * Expression-Expression division operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + friend ExpressionPtr operator/(const ExpressionPtr& lhs, + const ExpressionPtr& rhs) { + using enum ExpressionType; + + // Prune expression + if (lhs->is_constant(0.0)) { + // Return zero + return lhs; + } else if (rhs->is_constant(1.0)) { + return lhs; + } + + // Evaluate constant + if (lhs->type() == CONSTANT && rhs->type() == CONSTANT) { + return make_expression_ptr(lhs->val / rhs->val); + } + + // Evaluate expression type + if (rhs->type() == CONSTANT) { + if (lhs->type() == LINEAR) { + return make_expression_ptr>(lhs, rhs); + } else if (lhs->type() == QUADRATIC) { + return make_expression_ptr>(lhs, rhs); + } else { + return make_expression_ptr>(lhs, rhs); + } + } else { + return make_expression_ptr>(lhs, rhs); + } + } + + /** + * Expression-Expression addition operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + friend ExpressionPtr operator+(const ExpressionPtr& lhs, + const ExpressionPtr& rhs) { + using enum ExpressionType; + + // Prune expression + if (lhs == nullptr || lhs->is_constant(0.0)) { + return rhs; + } else if (rhs == nullptr || rhs->is_constant(0.0)) { + return lhs; + } + + // Evaluate constant + if (lhs->type() == CONSTANT && rhs->type() == CONSTANT) { + return make_expression_ptr(lhs->val + rhs->val); + } + + auto type = std::max(lhs->type(), rhs->type()); + if (type == LINEAR) { + return make_expression_ptr>(lhs, rhs); + } else if (type == QUADRATIC) { + return make_expression_ptr>(lhs, rhs); + } else { + return make_expression_ptr>(lhs, rhs); + } + } + + /** + * Expression-Expression compound addition operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + friend ExpressionPtr operator+=(ExpressionPtr& lhs, + const ExpressionPtr& rhs) { + return lhs = lhs + rhs; + } + + /** + * Expression-Expression subtraction operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + friend ExpressionPtr operator-(const ExpressionPtr& lhs, + const ExpressionPtr& rhs) { + using enum ExpressionType; + + // Prune expression + if (lhs->is_constant(0.0)) { + if (rhs->is_constant(0.0)) { + // Return zero + return rhs; + } else { + return -rhs; + } + } else if (rhs->is_constant(0.0)) { + return lhs; + } + + // Evaluate constant + if (lhs->type() == CONSTANT && rhs->type() == CONSTANT) { + return make_expression_ptr(lhs->val - rhs->val); + } + + auto type = std::max(lhs->type(), rhs->type()); + if (type == LINEAR) { + return make_expression_ptr>(lhs, rhs); + } else if (type == QUADRATIC) { + return make_expression_ptr>(lhs, rhs); + } else { + return make_expression_ptr>(lhs, rhs); + } + } + + /** + * Unary minus operator. + * + * @param lhs Operand of unary minus. + */ + friend ExpressionPtr operator-(const ExpressionPtr& lhs) { + using enum ExpressionType; + + // Prune expression + if (lhs->is_constant(0.0)) { + // Return zero + return lhs; + } + + // Evaluate constant + if (lhs->type() == CONSTANT) { + return make_expression_ptr(-lhs->val); + } + + if (lhs->type() == LINEAR) { + return make_expression_ptr>(lhs); + } else if (lhs->type() == QUADRATIC) { + return make_expression_ptr>(lhs); + } else { + return make_expression_ptr>(lhs); + } + } + + /** + * Unary plus operator. + * + * @param lhs Operand of unary plus. + */ + friend ExpressionPtr operator+(const ExpressionPtr& lhs) { return lhs; } + + /** + * Either nullary operator with no arguments, unary operator with one + * argument, or binary operator with two arguments. This operator is used to + * update the node's value. + * + * @param lhs Left argument to binary operator. + * @param rhs Right argument to binary operator. + * @return The node's value. + */ + virtual double value([[maybe_unused]] double lhs, + [[maybe_unused]] double rhs) const = 0; + + /** + * Returns the type of this expression (constant, linear, quadratic, or + * nonlinear). + * + * @return The type of this expression. + */ + virtual ExpressionType type() const = 0; + + /** + * Returns double adjoint of the left child expression. + * + * @param lhs Left argument to binary operator. + * @param rhs Right argument to binary operator. + * @param parent_adjoint Adjoint of parent expression. + * @return The double adjoint of the left child expression. + */ + virtual double grad_l([[maybe_unused]] double lhs, + [[maybe_unused]] double rhs, + [[maybe_unused]] double parent_adjoint) const { + return 0.0; + } + + /** + * Returns double adjoint of the right child expression. + * + * @param lhs Left argument to binary operator. + * @param rhs Right argument to binary operator. + * @param parent_adjoint Adjoint of parent expression. + * @return The double adjoint of the right child expression. + */ + virtual double grad_r([[maybe_unused]] double lhs, + [[maybe_unused]] double rhs, + [[maybe_unused]] double parent_adjoint) const { + return 0.0; + } + + /** + * Returns Expression adjoint of the left child expression. + * + * @param lhs Left argument to binary operator. + * @param rhs Right argument to binary operator. + * @param parent_adjoint Adjoint of parent expression. + * @return The Expression adjoint of the left child expression. + */ + virtual ExpressionPtr grad_expr_l( + [[maybe_unused]] const ExpressionPtr& lhs, + [[maybe_unused]] const ExpressionPtr& rhs, + [[maybe_unused]] const ExpressionPtr& parent_adjoint) const { + return make_expression_ptr(); + } + + /** + * Returns Expression adjoint of the right child expression. + * + * @param lhs Left argument to binary operator. + * @param rhs Right argument to binary operator. + * @param parent_adjoint Adjoint of parent expression. + * @return The Expression adjoint of the right child expression. + */ + virtual ExpressionPtr grad_expr_r( + [[maybe_unused]] const ExpressionPtr& lhs, + [[maybe_unused]] const ExpressionPtr& rhs, + [[maybe_unused]] const ExpressionPtr& parent_adjoint) const { + return make_expression_ptr(); + } +}; + +/** + * Derived expression type for binary minus operator. + * + * @tparam T Expression type. + */ +template +struct BinaryMinusExpression final : Expression { + /** + * Constructs a binary expression (an operator with two arguments). + * + * @param lhs Binary operator's left operand. + * @param rhs Binary operator's right operand. + */ + constexpr BinaryMinusExpression(ExpressionPtr lhs, ExpressionPtr rhs) + : Expression{std::move(lhs), std::move(rhs)} {} + + double value(double lhs, double rhs) const override { return lhs - rhs; } + + ExpressionType type() const override { return T; } + + double grad_l(double, double, double parent_adjoint) const override { + return parent_adjoint; + } + + double grad_r(double, double, double parent_adjoint) const override { + return -parent_adjoint; + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr&, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint; + } + + ExpressionPtr grad_expr_r( + const ExpressionPtr&, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return -parent_adjoint; + } +}; + +/** + * Derived expression type for binary plus operator. + * + * @tparam T Expression type. + */ +template +struct BinaryPlusExpression final : Expression { + /** + * Constructs a binary expression (an operator with two arguments). + * + * @param lhs Binary operator's left operand. + * @param rhs Binary operator's right operand. + */ + constexpr BinaryPlusExpression(ExpressionPtr lhs, ExpressionPtr rhs) + : Expression{std::move(lhs), std::move(rhs)} {} + + double value(double lhs, double rhs) const override { return lhs + rhs; } + + ExpressionType type() const override { return T; } + + double grad_l(double, double, double parent_adjoint) const override { + return parent_adjoint; + } + + double grad_r(double, double, double parent_adjoint) const override { + return parent_adjoint; + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr&, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint; + } + + ExpressionPtr grad_expr_r( + const ExpressionPtr&, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint; + } +}; + +/** + * Derived expression type for constant. + */ +struct ConstExpression final : Expression { + /** + * Constructs a constant expression with a value of zero. + */ + constexpr ConstExpression() = default; + + /** + * Constructs a nullary expression (an operator with no arguments). + * + * @param value The expression value. + */ + explicit constexpr ConstExpression(double value) : Expression{value} {} + + double value(double, double) const override { return val; } + + ExpressionType type() const override { return ExpressionType::CONSTANT; } +}; + +/** + * Derived expression type for decision variable. + */ +struct DecisionVariableExpression final : Expression { + /** + * Constructs a decision variable expression with a value of zero. + */ + constexpr DecisionVariableExpression() = default; + + /** + * Constructs a nullary expression (an operator with no arguments). + * + * @param value The expression value. + */ + explicit constexpr DecisionVariableExpression(double value) + : Expression{value} {} + + double value(double, double) const override { return val; } + + ExpressionType type() const override { return ExpressionType::LINEAR; } +}; + +/** + * Derived expression type for binary division operator. + * + * @tparam T Expression type. + */ +template +struct DivExpression final : Expression { + /** + * Constructs a binary expression (an operator with two arguments). + * + * @param lhs Binary operator's left operand. + * @param rhs Binary operator's right operand. + */ + constexpr DivExpression(ExpressionPtr lhs, ExpressionPtr rhs) + : Expression{std::move(lhs), std::move(rhs)} {} + + double value(double lhs, double rhs) const override { return lhs / rhs; } + + ExpressionType type() const override { return T; } + + double grad_l(double, double rhs, double parent_adjoint) const override { + return parent_adjoint / rhs; + }; + + double grad_r(double lhs, double rhs, double parent_adjoint) const override { + return parent_adjoint * -lhs / (rhs * rhs); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr&, const ExpressionPtr& rhs, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint / rhs; + } + + ExpressionPtr grad_expr_r( + const ExpressionPtr& lhs, const ExpressionPtr& rhs, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * -lhs / (rhs * rhs); + } +}; + +/** + * Derived expression type for binary multiplication operator. + * + * @tparam T Expression type. + */ +template +struct MultExpression final : Expression { + /** + * Constructs a binary expression (an operator with two arguments). + * + * @param lhs Binary operator's left operand. + * @param rhs Binary operator's right operand. + */ + constexpr MultExpression(ExpressionPtr lhs, ExpressionPtr rhs) + : Expression{std::move(lhs), std::move(rhs)} {} + + double value(double lhs, double rhs) const override { return lhs * rhs; } + + ExpressionType type() const override { return T; } + + double grad_l([[maybe_unused]] double lhs, double rhs, + double parent_adjoint) const override { + return parent_adjoint * rhs; + } + + double grad_r(double lhs, [[maybe_unused]] double rhs, + double parent_adjoint) const override { + return parent_adjoint * lhs; + } + + ExpressionPtr grad_expr_l( + [[maybe_unused]] const ExpressionPtr& lhs, const ExpressionPtr& rhs, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * rhs; + } + + ExpressionPtr grad_expr_r( + const ExpressionPtr& lhs, [[maybe_unused]] const ExpressionPtr& rhs, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * lhs; + } +}; + +/** + * Derived expression type for unary minus operator. + * + * @tparam T Expression type. + */ +template +struct UnaryMinusExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr UnaryMinusExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double lhs, double) const override { return -lhs; } + + ExpressionType type() const override { return T; } + + double grad_l(double, double, double parent_adjoint) const override { + return -parent_adjoint; + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr&, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return -parent_adjoint; + } +}; + +inline ExpressionPtr exp(const ExpressionPtr& x); +inline ExpressionPtr sin(const ExpressionPtr& x); +inline ExpressionPtr sinh(const ExpressionPtr& x); +inline ExpressionPtr sqrt(const ExpressionPtr& x); + +/** + * Refcount increment for intrusive shared pointer. + * + * @param expr The shared pointer's managed object. + */ +inline constexpr void inc_ref_count(Expression* expr) { + ++expr->ref_count; +} + +/** + * Refcount decrement for intrusive shared pointer. + * + * @param expr The shared pointer's managed object. + */ +inline void dec_ref_count(Expression* expr) { + // If a deeply nested tree is being deallocated all at once, calling the + // Expression destructor when expr's refcount reaches zero can cause a stack + // overflow. Instead, we iterate over its children to decrement their + // refcounts and deallocate them. + gch::small_vector stack; + stack.emplace_back(expr); + + while (!stack.empty()) { + auto elem = stack.back(); + stack.pop_back(); + + // Decrement the current node's refcount. If it reaches zero, deallocate the + // node and enqueue its children so their refcounts are decremented too. + if (--elem->ref_count == 0) { + if (elem->adjoint_expr != nullptr) { + stack.emplace_back(elem->adjoint_expr.get()); + } + for (auto& arg : elem->args) { + if (arg != nullptr) { + stack.emplace_back(arg.get()); + } + } + + // Not calling the destructor here is safe because it only decrements + // refcounts, which was already done above. + if constexpr (USE_POOL_ALLOCATOR) { + auto alloc = global_pool_allocator(); + std::allocator_traits::deallocate(alloc, elem, + sizeof(Expression)); + } + } + } +} + +/** + * Derived expression type for std::abs(). + */ +struct AbsExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr AbsExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::abs(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + if (x < 0.0) { + return -parent_adjoint; + } else if (x > 0.0) { + return parent_adjoint; + } else { + return 0.0; + } + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + if (x->val < 0.0) { + return -parent_adjoint; + } else if (x->val > 0.0) { + return parent_adjoint; + } else { + // Return zero + return make_expression_ptr(); + } + } +}; + +/** + * std::abs() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr abs(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::abs(x->val)); + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::acos(). + */ +struct AcosExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr AcosExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::acos(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return -parent_adjoint / std::sqrt(1.0 - x * x); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return -parent_adjoint / + slp::detail::sqrt(make_expression_ptr(1.0) - x * x); + } +}; + +/** + * std::acos() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr acos(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + return make_expression_ptr(std::numbers::pi / 2.0); + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::acos(x->val)); + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::asin(). + */ +struct AsinExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr AsinExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::asin(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint / std::sqrt(1.0 - x * x); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint / + slp::detail::sqrt(make_expression_ptr(1.0) - x * x); + } +}; + +/** + * std::asin() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr asin(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::asin(x->val)); + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::atan(). + */ +struct AtanExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr AtanExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::atan(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint / (1.0 + x * x); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint / (make_expression_ptr(1.0) + x * x); + } +}; + +/** + * std::atan() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr atan(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::atan(x->val)); + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::atan2(). + */ +struct Atan2Expression final : Expression { + /** + * Constructs a binary expression (an operator with two arguments). + * + * @param lhs Binary operator's left operand. + * @param rhs Binary operator's right operand. + */ + constexpr Atan2Expression(ExpressionPtr lhs, ExpressionPtr rhs) + : Expression{std::move(lhs), std::move(rhs)} {} + + double value(double y, double x) const override { return std::atan2(y, x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double y, double x, double parent_adjoint) const override { + return parent_adjoint * x / (y * y + x * x); + } + + double grad_r(double y, double x, double parent_adjoint) const override { + return parent_adjoint * -y / (y * y + x * x); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& y, const ExpressionPtr& x, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * x / (y * y + x * x); + } + + ExpressionPtr grad_expr_r( + const ExpressionPtr& y, const ExpressionPtr& x, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * -y / (y * y + x * x); + } +}; + +/** + * std::atan2() for Expressions. + * + * @param y The y argument. + * @param x The x argument. + */ +inline ExpressionPtr atan2(const ExpressionPtr& y, const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (y->is_constant(0.0)) { + // Return zero + return y; + } else if (x->is_constant(0.0)) { + return make_expression_ptr(std::numbers::pi / 2.0); + } + + // Evaluate constant + if (y->type() == CONSTANT && x->type() == CONSTANT) { + return make_expression_ptr(std::atan2(y->val, x->val)); + } + + return make_expression_ptr(y, x); +} + +/** + * Derived expression type for std::cos(). + */ +struct CosExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr CosExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::cos(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return -parent_adjoint * std::sin(x); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * -slp::detail::sin(x); + } +}; + +/** + * std::cos() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr cos(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + return make_expression_ptr(1.0); + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::cos(x->val)); + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::cosh(). + */ +struct CoshExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr CoshExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::cosh(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint * std::sinh(x); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * slp::detail::sinh(x); + } +}; + +/** + * std::cosh() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr cosh(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + return make_expression_ptr(1.0); + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::cosh(x->val)); + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::erf(). + */ +struct ErfExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr ErfExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::erf(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint * 2.0 * std::numbers::inv_sqrtpi * std::exp(-x * x); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * + make_expression_ptr(2.0 * + std::numbers::inv_sqrtpi) * + slp::detail::exp(-x * x); + } +}; + +/** + * std::erf() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr erf(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::erf(x->val)); + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::exp(). + */ +struct ExpExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr ExpExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::exp(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint * std::exp(x); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * slp::detail::exp(x); + } +}; + +/** + * std::exp() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr exp(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + return make_expression_ptr(1.0); + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::exp(x->val)); + } + + return make_expression_ptr(x); +} + +inline ExpressionPtr hypot(const ExpressionPtr& x, const ExpressionPtr& y); + +/** + * Derived expression type for std::hypot(). + */ +struct HypotExpression final : Expression { + /** + * Constructs a binary expression (an operator with two arguments). + * + * @param lhs Binary operator's left operand. + * @param rhs Binary operator's right operand. + */ + constexpr HypotExpression(ExpressionPtr lhs, ExpressionPtr rhs) + : Expression{std::move(lhs), std::move(rhs)} {} + + double value(double x, double y) const override { return std::hypot(x, y); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double y, double parent_adjoint) const override { + return parent_adjoint * x / std::hypot(x, y); + } + + double grad_r(double x, double y, double parent_adjoint) const override { + return parent_adjoint * y / std::hypot(x, y); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr& y, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * x / slp::detail::hypot(x, y); + } + + ExpressionPtr grad_expr_r( + const ExpressionPtr& x, const ExpressionPtr& y, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * y / slp::detail::hypot(x, y); + } +}; + +/** + * std::hypot() for Expressions. + * + * @param x The x argument. + * @param y The y argument. + */ +inline ExpressionPtr hypot(const ExpressionPtr& x, const ExpressionPtr& y) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + return y; + } else if (y->is_constant(0.0)) { + return x; + } + + // Evaluate constant + if (x->type() == CONSTANT && y->type() == CONSTANT) { + return make_expression_ptr(std::hypot(x->val, y->val)); + } + + return make_expression_ptr(x, y); +} + +/** + * Derived expression type for std::log(). + */ +struct LogExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr LogExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::log(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint / x; + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint / x; + } +}; + +/** + * std::log() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr log(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::log(x->val)); + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::log10(). + */ +struct Log10Expression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr Log10Expression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::log10(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint / (std::numbers::ln10 * x); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint / + (make_expression_ptr(std::numbers::ln10) * x); + } +}; + +/** + * std::log10() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr log10(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::log10(x->val)); + } + + return make_expression_ptr(x); +} + +inline ExpressionPtr pow(const ExpressionPtr& base, const ExpressionPtr& power); + +/** + * Derived expression type for std::pow(). + * + * @tparam Expression type. + */ +template +struct PowExpression final : Expression { + /** + * Constructs a binary expression (an operator with two arguments). + * + * @param lhs Binary operator's left operand. + * @param rhs Binary operator's right operand. + */ + constexpr PowExpression(ExpressionPtr lhs, ExpressionPtr rhs) + : Expression{std::move(lhs), std::move(rhs)} {} + + double value(double base, double power) const override { + return std::pow(base, power); + } + + ExpressionType type() const override { return T; } + + double grad_l(double base, double power, + double parent_adjoint) const override { + return parent_adjoint * std::pow(base, power - 1) * power; + } + + double grad_r(double base, double power, + double parent_adjoint) const override { + // Since x * std::log(x) -> 0 as x -> 0 + if (base == 0.0) { + return 0.0; + } else { + return parent_adjoint * std::pow(base, power - 1) * base * std::log(base); + } + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& base, const ExpressionPtr& power, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * + slp::detail::pow(base, + power - make_expression_ptr(1.0)) * + power; + } + + ExpressionPtr grad_expr_r( + const ExpressionPtr& base, const ExpressionPtr& power, + const ExpressionPtr& parent_adjoint) const override { + // Since x * std::log(x) -> 0 as x -> 0 + if (base->val == 0.0) { + // Return zero + return base; + } else { + return parent_adjoint * + slp::detail::pow( + base, power - make_expression_ptr(1.0)) * + base * slp::detail::log(base); + } + } +}; + +/** + * std::pow() for Expressions. + * + * @param base The base. + * @param power The power. + */ +inline ExpressionPtr pow(const ExpressionPtr& base, + const ExpressionPtr& power) { + using enum ExpressionType; + + // Prune expression + if (base->is_constant(0.0)) { + // Return zero + return base; + } else if (base->is_constant(1.0)) { + // Return one + return base; + } + if (power->is_constant(0.0)) { + return make_expression_ptr(1.0); + } else if (power->is_constant(1.0)) { + return base; + } + + // Evaluate constant + if (base->type() == CONSTANT && power->type() == CONSTANT) { + return make_expression_ptr( + std::pow(base->val, power->val)); + } + + if (power->is_constant(2.0)) { + if (base->type() == LINEAR) { + return make_expression_ptr>(base, base); + } else { + return make_expression_ptr>(base, base); + } + } + + return make_expression_ptr>(base, power); +} + +/** + * Derived expression type for sign(). + */ +struct SignExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr SignExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { + if (x < 0.0) { + return -1.0; + } else if (x == 0.0) { + return 0.0; + } else { + return 1.0; + } + } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double, double, double) const override { return 0.0; } + + ExpressionPtr grad_expr_l(const ExpressionPtr&, const ExpressionPtr&, + const ExpressionPtr&) const override { + // Return zero + return make_expression_ptr(); + } +}; + +/** + * sign() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr sign(const ExpressionPtr& x) { + using enum ExpressionType; + + // Evaluate constant + if (x->type() == CONSTANT) { + if (x->val < 0.0) { + return make_expression_ptr(-1.0); + } else if (x->val == 0.0) { + // Return zero + return x; + } else { + return make_expression_ptr(1.0); + } + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::sin(). + */ +struct SinExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr SinExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::sin(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint * std::cos(x); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * slp::detail::cos(x); + } +}; + +/** + * std::sin() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr sin(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::sin(x->val)); + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::sinh(). + */ +struct SinhExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr SinhExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::sinh(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint * std::cosh(x); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * slp::detail::cosh(x); + } +}; + +/** + * std::sinh() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr sinh(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::sinh(x->val)); + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::sqrt(). + */ +struct SqrtExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr SqrtExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::sqrt(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint / (2.0 * std::sqrt(x)); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint / + (make_expression_ptr(2.0) * slp::detail::sqrt(x)); + } +}; + +/** + * std::sqrt() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr sqrt(const ExpressionPtr& x) { + using enum ExpressionType; + + // Evaluate constant + if (x->type() == CONSTANT) { + if (x->val == 0.0) { + // Return zero + return x; + } else if (x->val == 1.0) { + return x; + } else { + return make_expression_ptr(std::sqrt(x->val)); + } + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::tan(). + */ +struct TanExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr TanExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::tan(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint / (std::cos(x) * std::cos(x)); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint / (slp::detail::cos(x) * slp::detail::cos(x)); + } +}; + +/** + * std::tan() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr tan(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::tan(x->val)); + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::tanh(). + */ +struct TanhExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr TanhExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::tanh(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint / (std::cosh(x) * std::cosh(x)); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint / (slp::detail::cosh(x) * slp::detail::cosh(x)); + } +}; + +/** + * std::tanh() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr tanh(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::tanh(x->val)); + } + + return make_expression_ptr(x); +} + +} // namespace slp::detail diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp new file mode 100644 index 0000000000..a1d9c33539 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp @@ -0,0 +1,94 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include + +#include "sleipnir/autodiff/expression.hpp" + +namespace slp::detail { + +/** + * Generate a topological sort of an expression graph from parent to child. + * + * https://en.wikipedia.org/wiki/Topological_sorting + * + * @param root The root node of the expression. + */ +inline gch::small_vector topological_sort( + const ExpressionPtr& root) { + gch::small_vector list; + + // If the root type is a constant, Update() is a no-op, so there's no work + // to do + if (root == nullptr || root->type() == ExpressionType::CONSTANT) { + return list; + } + + // Stack of nodes to explore + gch::small_vector stack; + + // Enumerate incoming edges for each node via depth-first search + stack.emplace_back(root.get()); + while (!stack.empty()) { + auto node = stack.back(); + stack.pop_back(); + + for (auto& arg : node->args) { + // If the node hasn't been explored yet, add it to the stack + if (arg != nullptr && ++arg->incoming_edges == 1) { + stack.push_back(arg.get()); + } + } + } + + // Generate topological sort of graph from parent to child. + // + // A node is only added to the stack after all its incoming edges have been + // traversed. Expression::incoming_edges is a decrementing counter for + // tracking this. + // + // https://en.wikipedia.org/wiki/Topological_sorting + stack.emplace_back(root.get()); + while (!stack.empty()) { + auto node = stack.back(); + stack.pop_back(); + + list.emplace_back(node); + + for (auto& arg : node->args) { + // If we traversed all this node's incoming edges, add it to the stack + if (arg != nullptr && --arg->incoming_edges == 0) { + stack.push_back(arg.get()); + } + } + } + + return list; +} + +/** + * Update the values of all nodes in this graph based on the values of + * their dependent nodes. + * + * @param list Topological sort of graph from parent to child. + */ +inline void update_values(const gch::small_vector& list) { + // Traverse graph from child to parent and update values + for (auto& node : list | std::views::reverse) { + auto& lhs = node->args[0]; + auto& rhs = node->args[1]; + + if (lhs != nullptr) { + if (rhs != nullptr) { + node->val = node->value(lhs->val, rhs->val); + } else { + node->val = node->value(lhs->val, 0.0); + } + } + } +} + +} // namespace slp::detail diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_type.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_type.hpp new file mode 100644 index 0000000000..9e323d87f4 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_type.hpp @@ -0,0 +1,56 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include + +#include "sleipnir/util/symbol_exports.hpp" + +namespace slp { + +/** + * Expression type. + * + * Used for autodiff caching. + */ +enum class ExpressionType : uint8_t { + /// There is no expression. + NONE, + /// The expression is a constant. + CONSTANT, + /// The expression is composed of linear and lower-order operators. + LINEAR, + /// The expression is composed of quadratic and lower-order operators. + QUADRATIC, + /// The expression is composed of nonlinear and lower-order operators. + NONLINEAR +}; + +/** + * Returns user-readable message corresponding to the expression type. + * + * @param type Expression type. + */ +SLEIPNIR_DLLEXPORT constexpr std::string_view to_message( + const ExpressionType& type) { + using enum ExpressionType; + + switch (type) { + case NONE: + return "none"; + case CONSTANT: + return "constant"; + case LINEAR: + return "linear"; + case QUADRATIC: + return "quadratic"; + case NONLINEAR: + return "nonlinear"; + default: + return "unknown"; + } +} + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Gradient.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp similarity index 61% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Gradient.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp index cf6a4171e7..728259d3f5 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Gradient.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp @@ -6,13 +6,13 @@ #include -#include "sleipnir/autodiff/Jacobian.hpp" -#include "sleipnir/autodiff/Profiler.hpp" -#include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/autodiff/VariableMatrix.hpp" -#include "sleipnir/util/SymbolExports.hpp" +#include "sleipnir/autodiff/jacobian.hpp" +#include "sleipnir/autodiff/variable.hpp" +#include "sleipnir/autodiff/variable_matrix.hpp" +#include "sleipnir/util/concepts.hpp" +#include "sleipnir/util/symbol_exports.hpp" -namespace sleipnir { +namespace slp { /** * This class calculates the gradient of a a variable with respect to a vector @@ -30,7 +30,7 @@ class SLEIPNIR_DLLEXPORT Gradient { * @param wrt Variable with respect to which to compute the gradient. */ Gradient(Variable variable, Variable wrt) noexcept - : Gradient{std::move(variable), VariableMatrix{wrt}} {} + : m_jacobian{std::move(variable), std::move(wrt)} {} /** * Constructs a Gradient object. @@ -39,35 +39,34 @@ class SLEIPNIR_DLLEXPORT Gradient { * @param wrt Vector of variables with respect to which to compute the * gradient. */ - Gradient(Variable variable, const VariableMatrix& wrt) noexcept - : m_jacobian{variable, wrt} {} + Gradient(Variable variable, SleipnirMatrixLike auto wrt) noexcept + : m_jacobian{VariableMatrix{std::move(variable)}, std::move(wrt)} {} /** * Returns the gradient as a VariableMatrix. * * This is useful when constructing optimization problems with derivatives in * them. + * + * @return The gradient as a VariableMatrix. */ - VariableMatrix Get() const { return m_jacobian.Get().T(); } + VariableMatrix get() const { return m_jacobian.get().T(); } /** * Evaluates the gradient at wrt's value. + * + * @return The gradient at wrt's value. */ - const Eigen::SparseVector& Value() { - m_g = m_jacobian.Value(); + const Eigen::SparseVector& value() { + m_g = m_jacobian.value(); return m_g; } - /** - * Returns the profiler. - */ - Profiler& GetProfiler() { return m_jacobian.GetProfiler(); } - private: Eigen::SparseVector m_g; Jacobian m_jacobian; }; -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp new file mode 100644 index 0000000000..8b048ab3ba --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp @@ -0,0 +1,169 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include +#include + +#include "sleipnir/autodiff/adjoint_expression_graph.hpp" +#include "sleipnir/autodiff/variable.hpp" +#include "sleipnir/autodiff/variable_matrix.hpp" +#include "sleipnir/util/concepts.hpp" +#include "sleipnir/util/symbol_exports.hpp" + +namespace slp { + +/** + * This class calculates the Hessian of a variable with respect to a vector of + * variables. + * + * The gradient tree is cached so subsequent Hessian calculations are faster, + * and the Hessian is only recomputed if the variable expression is nonlinear. + * + * @tparam UpLo Which part of the Hessian to compute (Lower or Lower | Upper). + */ +template + requires(UpLo == Eigen::Lower) || (UpLo == (Eigen::Lower | Eigen::Upper)) +class SLEIPNIR_DLLEXPORT Hessian { + public: + /** + * Constructs a Hessian object. + * + * @param variable Variable of which to compute the Hessian. + * @param wrt Variable with respect to which to compute the Hessian. + */ + Hessian(Variable variable, Variable wrt) noexcept + : Hessian{std::move(variable), VariableMatrix{std::move(wrt)}} {} + + /** + * Constructs a Hessian object. + * + * @param variable Variable of which to compute the Hessian. + * @param wrt Vector of variables with respect to which to compute the + * Hessian. + */ + Hessian(Variable variable, SleipnirMatrixLike auto wrt) noexcept + : m_variables{detail::AdjointExpressionGraph{variable} + .generate_gradient_tree(wrt)}, + m_wrt{wrt} { + // Initialize column each expression's adjoint occupies in the Jacobian + for (size_t col = 0; col < m_wrt.size(); ++col) { + m_wrt[col].expr->col = col; + } + + for (auto& variable : m_variables) { + m_graphs.emplace_back(variable); + } + + // Reset col to -1 + for (auto& node : m_wrt) { + node.expr->col = -1; + } + + for (int row = 0; row < m_variables.rows(); ++row) { + if (m_variables[row].expr == nullptr) { + continue; + } + + if (m_variables[row].type() == ExpressionType::LINEAR) { + // If the row is linear, compute its gradient once here and cache its + // triplets. Constant rows are ignored because their gradients have no + // nonzero triplets. + m_graphs[row].append_adjoint_triplets(m_cached_triplets, row, m_wrt); + } else if (m_variables[row].type() > ExpressionType::LINEAR) { + // If the row is quadratic or nonlinear, add it to the list of nonlinear + // rows to be recomputed in Value(). + m_nonlinear_rows.emplace_back(row); + } + } + + if (m_nonlinear_rows.empty()) { + m_H.setFromTriplets(m_cached_triplets.begin(), m_cached_triplets.end()); + if constexpr (UpLo == Eigen::Lower) { + m_H = m_H.triangularView(); + } + } + } + + /** + * Returns the Hessian as a VariableMatrix. + * + * This is useful when constructing optimization problems with derivatives in + * them. + * + * @return The Hessian as a VariableMatrix. + */ + VariableMatrix get() const { + VariableMatrix result{VariableMatrix::empty, m_variables.rows(), + m_wrt.rows()}; + + for (int row = 0; row < m_variables.rows(); ++row) { + auto grad = m_graphs[row].generate_gradient_tree(m_wrt); + for (int col = 0; col < m_wrt.rows(); ++col) { + if (grad[col].expr != nullptr) { + result(row, col) = std::move(grad[col]); + } else { + result(row, col) = Variable{0.0}; + } + } + } + + return result; + } + + /** + * Evaluates the Hessian at wrt's value. + * + * @return The Hessian at wrt's value. + */ + const Eigen::SparseMatrix& value() { + if (m_nonlinear_rows.empty()) { + return m_H; + } + + for (auto& graph : m_graphs) { + graph.update_values(); + } + + // Copy the cached triplets so triplets added for the nonlinear rows are + // thrown away at the end of the function + auto triplets = m_cached_triplets; + + // Compute each nonlinear row of the Hessian + for (int row : m_nonlinear_rows) { + m_graphs[row].append_adjoint_triplets(triplets, row, m_wrt); + } + + if (!triplets.empty()) { + m_H.setFromTriplets(triplets.begin(), triplets.end()); + if constexpr (UpLo == Eigen::Lower) { + m_H = m_H.triangularView(); + } + } else { + // setFromTriplets() is a no-op on empty triplets, so explicitly zero out + // the storage + m_H.setZero(); + } + + return m_H; + } + + private: + VariableMatrix m_variables; + VariableMatrix m_wrt; + + gch::small_vector m_graphs; + + Eigen::SparseMatrix m_H{m_variables.rows(), m_wrt.rows()}; + + // Cached triplets for gradients of linear rows + gch::small_vector> m_cached_triplets; + + // List of row indices for nonlinear rows whose graients will be computed in + // Value() + gch::small_vector m_nonlinear_rows; +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp new file mode 100644 index 0000000000..7e7e1340d0 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp @@ -0,0 +1,158 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include +#include + +#include "sleipnir/autodiff/adjoint_expression_graph.hpp" +#include "sleipnir/autodiff/variable.hpp" +#include "sleipnir/autodiff/variable_matrix.hpp" +#include "sleipnir/util/concepts.hpp" +#include "sleipnir/util/symbol_exports.hpp" + +namespace slp { + +/** + * This class calculates the Jacobian of a vector of variables with respect to a + * vector of variables. + * + * The Jacobian is only recomputed if the variable expression is quadratic or + * higher order. + */ +class SLEIPNIR_DLLEXPORT Jacobian { + public: + /** + * Constructs a Jacobian object. + * + * @param variable Variable of which to compute the Jacobian. + * @param wrt Variable with respect to which to compute the Jacobian. + */ + Jacobian(Variable variable, Variable wrt) noexcept + : Jacobian{VariableMatrix{std::move(variable)}, + VariableMatrix{std::move(wrt)}} {} + + /** + * Constructs a Jacobian object. + * + * @param variables Vector of variables of which to compute the Jacobian. + * @param wrt Vector of variables with respect to which to compute the + * Jacobian. + */ + Jacobian(VariableMatrix variables, SleipnirMatrixLike auto wrt) noexcept + : m_variables{std::move(variables)}, m_wrt{std::move(wrt)} { + // Initialize column each expression's adjoint occupies in the Jacobian + for (size_t col = 0; col < m_wrt.size(); ++col) { + m_wrt[col].expr->col = col; + } + + for (auto& variable : m_variables) { + m_graphs.emplace_back(variable); + } + + // Reset col to -1 + for (auto& node : m_wrt) { + node.expr->col = -1; + } + + for (int row = 0; row < m_variables.rows(); ++row) { + if (m_variables[row].expr == nullptr) { + continue; + } + + if (m_variables[row].type() == ExpressionType::LINEAR) { + // If the row is linear, compute its gradient once here and cache its + // triplets. Constant rows are ignored because their gradients have no + // nonzero triplets. + m_graphs[row].append_adjoint_triplets(m_cached_triplets, row, m_wrt); + } else if (m_variables[row].type() > ExpressionType::LINEAR) { + // If the row is quadratic or nonlinear, add it to the list of nonlinear + // rows to be recomputed in Value(). + m_nonlinear_rows.emplace_back(row); + } + } + + if (m_nonlinear_rows.empty()) { + m_J.setFromTriplets(m_cached_triplets.begin(), m_cached_triplets.end()); + } + } + + /** + * Returns the Jacobian as a VariableMatrix. + * + * This is useful when constructing optimization problems with derivatives in + * them. + * + * @return The Jacobian as a VariableMatrix. + */ + VariableMatrix get() const { + VariableMatrix result{VariableMatrix::empty, m_variables.rows(), + m_wrt.rows()}; + + for (int row = 0; row < m_variables.rows(); ++row) { + auto grad = m_graphs[row].generate_gradient_tree(m_wrt); + for (int col = 0; col < m_wrt.rows(); ++col) { + if (grad[col].expr != nullptr) { + result(row, col) = std::move(grad[col]); + } else { + result(row, col) = Variable{0.0}; + } + } + } + + return result; + } + + /** + * Evaluates the Jacobian at wrt's value. + * + * @return The Jacobian at wrt's value. + */ + const Eigen::SparseMatrix& value() { + if (m_nonlinear_rows.empty()) { + return m_J; + } + + for (auto& graph : m_graphs) { + graph.update_values(); + } + + // Copy the cached triplets so triplets added for the nonlinear rows are + // thrown away at the end of the function + auto triplets = m_cached_triplets; + + // Compute each nonlinear row of the Jacobian + for (int row : m_nonlinear_rows) { + m_graphs[row].append_adjoint_triplets(triplets, row, m_wrt); + } + + if (!triplets.empty()) { + m_J.setFromTriplets(triplets.begin(), triplets.end()); + } else { + // setFromTriplets() is a no-op on empty triplets, so explicitly zero out + // the storage + m_J.setZero(); + } + + return m_J; + } + + private: + VariableMatrix m_variables; + VariableMatrix m_wrt; + + gch::small_vector m_graphs; + + Eigen::SparseMatrix m_J{m_variables.rows(), m_wrt.rows()}; + + // Cached triplets for gradients of linear rows + gch::small_vector> m_cached_triplets; + + // List of row indices for nonlinear rows whose graients will be computed in + // Value() + gch::small_vector m_nonlinear_rows; +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Slice.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/slice.hpp similarity index 79% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Slice.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/slice.hpp index 2f2b18993b..abb11f22f6 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Slice.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/slice.hpp @@ -6,18 +6,28 @@ #include #include -#include "sleipnir/util/Assert.hpp" -#include "sleipnir/util/SymbolExports.hpp" +#include "sleipnir/util/assert.hpp" +#include "sleipnir/util/symbol_exports.hpp" -namespace sleipnir { +namespace slp { namespace slicing { +/** + * Type tag used to designate an omitted argument of the slice. + */ struct none_t {}; + +/** + * Designates an omitted argument of the slice. + */ static inline constexpr none_t _; } // namespace slicing +/** + * Represents a sequence of elements in an iterable object. + */ class SLEIPNIR_DLLEXPORT Slice { public: /// Start index (inclusive). @@ -36,14 +46,20 @@ class SLEIPNIR_DLLEXPORT Slice { /** * Constructs a slice. - * - * @param stop Slice stop index (exclusive). */ - template - requires std::same_as || - std::convertible_to - constexpr Slice(Stop stop) // NOLINT - : Slice(slicing::_, std::move(stop), 1) {} + constexpr Slice(slicing::none_t) // NOLINT + : Slice(0, std::numeric_limits::max(), 1) {} + + /** + * Constructs a slice. + * + * @param start Slice start index (inclusive). + */ + constexpr Slice(int start) { // NOLINT + this->start = start; + this->stop = (start == -1) ? std::numeric_limits::max() : start + 1; + this->step = 1; + } /** * Constructs a slice. @@ -77,7 +93,7 @@ class SLEIPNIR_DLLEXPORT Slice { if constexpr (std::same_as) { this->step = 1; } else { - Assert(step != 0); + slp_assert(step != 0); this->step = step; } @@ -115,9 +131,9 @@ class SLEIPNIR_DLLEXPORT Slice { * @param length The sequence length. * @return The slice length. */ - constexpr int Adjust(int length) { - Assert(step != 0); - Assert(step >= -std::numeric_limits::max()); + constexpr int adjust(int length) { + slp_assert(step != 0); + slp_assert(step >= -std::numeric_limits::max()); if (start < 0) { start += length; @@ -155,4 +171,4 @@ class SLEIPNIR_DLLEXPORT Slice { } }; -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Variable.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp similarity index 67% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Variable.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp index f25c6d1533..03b929c778 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Variable.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp @@ -5,23 +5,32 @@ #include #include #include +#include #include #include #include #include -#include +#include -#include "sleipnir/autodiff/Expression.hpp" -#include "sleipnir/autodiff/ExpressionGraph.hpp" -#include "sleipnir/util/Assert.hpp" -#include "sleipnir/util/Concepts.hpp" -#include "sleipnir/util/Print.hpp" -#include "sleipnir/util/SymbolExports.hpp" +#include "sleipnir/autodiff/expression.hpp" +#include "sleipnir/autodiff/expression_graph.hpp" +#include "sleipnir/util/assert.hpp" +#include "sleipnir/util/concepts.hpp" +#include "sleipnir/util/symbol_exports.hpp" -namespace sleipnir { +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS +#include "sleipnir/util/print.hpp" +#endif + +namespace slp { // Forward declarations for friend declarations in Variable +namespace detail { +class AdjointExpressionGraph; +} // namespace detail +template + requires(UpLo == Eigen::Lower) || (UpLo == (Eigen::Lower | Eigen::Upper)) class SLEIPNIR_DLLEXPORT Hessian; class SLEIPNIR_DLLEXPORT Jacobian; @@ -36,11 +45,25 @@ class SLEIPNIR_DLLEXPORT Variable { Variable() = default; /** - * Constructs a Variable from a double. + * Constructs an empty Variable. + */ + explicit Variable(std::nullptr_t) : expr{nullptr} {} + + /** + * Constructs a Variable from a floating point type. * * @param value The value of the Variable. */ - Variable(double value) : expr{detail::MakeExpressionPtr(value)} {} // NOLINT + Variable(std::floating_point auto value) // NOLINT + : expr{detail::make_expression_ptr(value)} {} + + /** + * Constructs a Variable from an integral type. + * + * @param value The value of the Variable. + */ + Variable(std::integral auto value) // NOLINT + : expr{detail::make_expression_ptr(value)} {} /** * Constructs a Variable pointing to the specified expression. @@ -60,9 +83,10 @@ class SLEIPNIR_DLLEXPORT Variable { * Assignment operator for double. * * @param value The value of the Variable. + * @return This variable. */ Variable& operator=(double value) { - expr = detail::MakeExpressionPtr(value); + expr = detail::make_expression_ptr(value); return *this; } @@ -72,19 +96,22 @@ class SLEIPNIR_DLLEXPORT Variable { * * @param value The value of the Variable. */ - void SetValue(double value) { - if (expr->IsConstant(0.0)) { - expr = detail::MakeExpressionPtr(value); + void set_value(double value) { + if (expr->is_constant(0.0)) { + expr = detail::make_expression_ptr(value); } else { +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS // We only need to check the first argument since unary and binary // operators both use it - if (expr->args[0] != nullptr && !expr->args[0]->IsConstant(0.0)) { - sleipnir::println( + if (expr->args[0] != nullptr) { + auto location = std::source_location::current(); + slp::println( stderr, - "WARNING: {}:{}: Modified the value of a dependent variable", - __FILE__, __LINE__); + "WARNING: {}:{}: {}: Modified the value of a dependent variable", + location.file_name(), location.line(), location.function_name()); } - expr->value = value; +#endif + expr->val = value; } } @@ -93,6 +120,7 @@ class SLEIPNIR_DLLEXPORT Variable { * * @param lhs Operator left-hand side. * @param rhs Operator right-hand side. + * @return Result of multiplication. */ friend SLEIPNIR_DLLEXPORT Variable operator*(const Variable& lhs, const Variable& rhs) { @@ -103,6 +131,7 @@ class SLEIPNIR_DLLEXPORT Variable { * Variable-Variable compound multiplication operator. * * @param rhs Operator right-hand side. + * @return Result of multiplication. */ Variable& operator*=(const Variable& rhs) { *this = *this * rhs; @@ -114,6 +143,7 @@ class SLEIPNIR_DLLEXPORT Variable { * * @param lhs Operator left-hand side. * @param rhs Operator right-hand side. + * @return Result of division. */ friend SLEIPNIR_DLLEXPORT Variable operator/(const Variable& lhs, const Variable& rhs) { @@ -124,6 +154,7 @@ class SLEIPNIR_DLLEXPORT Variable { * Variable-Variable compound division operator. * * @param rhs Operator right-hand side. + * @return Result of division. */ Variable& operator/=(const Variable& rhs) { *this = *this / rhs; @@ -135,6 +166,7 @@ class SLEIPNIR_DLLEXPORT Variable { * * @param lhs Operator left-hand side. * @param rhs Operator right-hand side. + * @return Result of addition. */ friend SLEIPNIR_DLLEXPORT Variable operator+(const Variable& lhs, const Variable& rhs) { @@ -145,6 +177,7 @@ class SLEIPNIR_DLLEXPORT Variable { * Variable-Variable compound addition operator. * * @param rhs Operator right-hand side. + * @return Result of addition. */ Variable& operator+=(const Variable& rhs) { *this = *this + rhs; @@ -156,6 +189,7 @@ class SLEIPNIR_DLLEXPORT Variable { * * @param lhs Operator left-hand side. * @param rhs Operator right-hand side. + * @return Result of subtraction. */ friend SLEIPNIR_DLLEXPORT Variable operator-(const Variable& lhs, const Variable& rhs) { @@ -166,6 +200,7 @@ class SLEIPNIR_DLLEXPORT Variable { * Variable-Variable compound subtraction operator. * * @param rhs Operator right-hand side. + * @return Result of subtraction. */ Variable& operator-=(const Variable& rhs) { *this = *this - rhs; @@ -192,25 +227,38 @@ class SLEIPNIR_DLLEXPORT Variable { /** * Returns the value of this variable. + * + * @return The value of this variable. */ - double Value() { - // Updates the value of this variable based on the values of its dependent - // variables - detail::ExpressionGraph{expr}.Update(); + double value() { + if (!m_graph_initialized) { + m_graph = detail::topological_sort(expr); + m_graph_initialized = true; + } + detail::update_values(m_graph); - return expr->value; + return expr->val; } /** * Returns the type of this expression (constant, linear, quadratic, or * nonlinear). + * + * @return The type of this expression. */ - ExpressionType Type() const { return expr->type; } + ExpressionType type() const { return expr->type(); } private: - /// The expression node. + /// The expression node detail::ExpressionPtr expr = - detail::MakeExpressionPtr(0.0, ExpressionType::kLinear); + detail::make_expression_ptr(); + + /// Used to update the value of this variable based on the values of its + /// dependent variables + gch::small_vector m_graph; + + /// Used for lazy initialization of m_graph + bool m_graph_initialized = false; friend SLEIPNIR_DLLEXPORT Variable abs(const Variable& x); friend SLEIPNIR_DLLEXPORT Variable acos(const Variable& x); @@ -237,6 +285,9 @@ class SLEIPNIR_DLLEXPORT Variable { friend SLEIPNIR_DLLEXPORT Variable hypot(const Variable& x, const Variable& y, const Variable& z); + friend class detail::AdjointExpressionGraph; + template + requires(UpLo == Eigen::Lower) || (UpLo == (Eigen::Lower | Eigen::Upper)) friend class SLEIPNIR_DLLEXPORT Hessian; friend class SLEIPNIR_DLLEXPORT Jacobian; }; @@ -425,8 +476,7 @@ SLEIPNIR_DLLEXPORT inline Variable tanh(const Variable& x) { */ SLEIPNIR_DLLEXPORT inline Variable hypot(const Variable& x, const Variable& y, const Variable& z) { - return Variable{sleipnir::sqrt(sleipnir::pow(x, 2) + sleipnir::pow(y, 2) + - sleipnir::pow(z, 2))}; + return Variable{slp::sqrt(slp::pow(x, 2) + slp::pow(y, 2) + slp::pow(z, 2))}; } /** @@ -441,85 +491,38 @@ SLEIPNIR_DLLEXPORT inline Variable hypot(const Variable& x, const Variable& y, * @param rhs Right-hand side. */ template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) -wpi::SmallVector MakeConstraints(LHS&& lhs, RHS&& rhs) { - wpi::SmallVector constraints; + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (SleipnirType || SleipnirType) +gch::small_vector make_constraints(LHS&& lhs, RHS&& rhs) { + gch::small_vector constraints; - if constexpr (ScalarLike> && - ScalarLike>) { + if constexpr (ScalarLike && ScalarLike) { constraints.emplace_back(lhs - rhs); - } else if constexpr (ScalarLike> && - MatrixLike>) { - int rows; - int cols; - if constexpr (EigenMatrixLike>) { - rows = rhs.rows(); - cols = rhs.cols(); - } else { - rows = rhs.Rows(); - cols = rhs.Cols(); - } + } else if constexpr (ScalarLike && MatrixLike) { + constraints.reserve(rhs.rows() * rhs.cols()); - constraints.reserve(rows * cols); - - for (int row = 0; row < rows; ++row) { - for (int col = 0; col < cols; ++col) { + for (int row = 0; row < rhs.rows(); ++row) { + for (int col = 0; col < rhs.cols(); ++col) { // Make right-hand side zero constraints.emplace_back(lhs - rhs(row, col)); } } - } else if constexpr (MatrixLike> && - ScalarLike>) { - int rows; - int cols; - if constexpr (EigenMatrixLike>) { - rows = lhs.rows(); - cols = lhs.cols(); - } else { - rows = lhs.Rows(); - cols = lhs.Cols(); - } + } else if constexpr (MatrixLike && ScalarLike) { + constraints.reserve(lhs.rows() * lhs.cols()); - constraints.reserve(rows * cols); - - for (int row = 0; row < rows; ++row) { - for (int col = 0; col < cols; ++col) { + for (int row = 0; row < lhs.rows(); ++row) { + for (int col = 0; col < lhs.cols(); ++col) { // Make right-hand side zero constraints.emplace_back(lhs(row, col) - rhs); } } - } else if constexpr (MatrixLike> && - MatrixLike>) { - int lhsRows; - int lhsCols; - if constexpr (EigenMatrixLike>) { - lhsRows = lhs.rows(); - lhsCols = lhs.cols(); - } else { - lhsRows = lhs.Rows(); - lhsCols = lhs.Cols(); - } + } else if constexpr (MatrixLike && MatrixLike) { + slp_assert(lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols()); + constraints.reserve(lhs.rows() * lhs.cols()); - [[maybe_unused]] - int rhsRows; - [[maybe_unused]] - int rhsCols; - if constexpr (EigenMatrixLike>) { - rhsRows = rhs.rows(); - rhsCols = rhs.cols(); - } else { - rhsRows = rhs.Rows(); - rhsCols = rhs.Cols(); - } - - Assert(lhsRows == rhsRows && lhsCols == rhsCols); - constraints.reserve(lhsRows * lhsCols); - - for (int row = 0; row < lhsRows; ++row) { - for (int col = 0; col < lhsCols; ++col) { + for (int row = 0; row < lhs.rows(); ++row) { + for (int col = 0; col < lhs.cols(); ++col) { // Make right-hand side zero constraints.emplace_back(lhs(row, col) - rhs(row, col)); } @@ -534,16 +537,16 @@ wpi::SmallVector MakeConstraints(LHS&& lhs, RHS&& rhs) { */ struct SLEIPNIR_DLLEXPORT EqualityConstraints { /// A vector of scalar equality constraints. - wpi::SmallVector constraints; + gch::small_vector constraints; /** * Concatenates multiple equality constraints. * - * @param equalityConstraints The list of EqualityConstraints to concatenate. + * @param equality_constraints The list of EqualityConstraints to concatenate. */ - EqualityConstraints( // NOLINT - std::initializer_list equalityConstraints) { - for (const auto& elem : equalityConstraints) { + EqualityConstraints( + std::initializer_list equality_constraints) { + for (const auto& elem : equality_constraints) { constraints.insert(constraints.end(), elem.constraints.begin(), elem.constraints.end()); } @@ -554,11 +557,11 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints { * * This overload is for Python bindings only. * - * @param equalityConstraints The list of EqualityConstraints to concatenate. + * @param equality_constraints The list of EqualityConstraints to concatenate. */ explicit EqualityConstraints( - const std::vector& equalityConstraints) { - for (const auto& elem : equalityConstraints) { + const std::vector& equality_constraints) { + for (const auto& elem : equality_constraints) { constraints.insert(constraints.end(), elem.constraints.begin(), elem.constraints.end()); } @@ -574,20 +577,19 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints { * @param rhs Right-hand side. */ template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (SleipnirType || SleipnirType) EqualityConstraints(LHS&& lhs, RHS&& rhs) - : constraints{MakeConstraints(lhs, rhs)} {} + : constraints{make_constraints(lhs, rhs)} {} /** * Implicit conversion operator to bool. */ operator bool() { // NOLINT - return std::all_of( - constraints.begin(), constraints.end(), - [](auto& constraint) { return constraint.Value() == 0.0; }); + return std::ranges::all_of(constraints, [](auto& constraint) { + return constraint.value() == 0.0; + }); } }; @@ -596,17 +598,17 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints { */ struct SLEIPNIR_DLLEXPORT InequalityConstraints { /// A vector of scalar inequality constraints. - wpi::SmallVector constraints; + gch::small_vector constraints; /** * Concatenates multiple inequality constraints. * - * @param inequalityConstraints The list of InequalityConstraints to + * @param inequality_constraints The list of InequalityConstraints to * concatenate. */ InequalityConstraints( // NOLINT - std::initializer_list inequalityConstraints) { - for (const auto& elem : inequalityConstraints) { + std::initializer_list inequality_constraints) { + for (const auto& elem : inequality_constraints) { constraints.insert(constraints.end(), elem.constraints.begin(), elem.constraints.end()); } @@ -617,12 +619,12 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints { * * This overload is for Python bindings only. * - * @param inequalityConstraints The list of InequalityConstraints to + * @param inequality_constraints The list of InequalityConstraints to * concatenate. */ explicit InequalityConstraints( - const std::vector& inequalityConstraints) { - for (const auto& elem : inequalityConstraints) { + const std::vector& inequality_constraints) { + for (const auto& elem : inequality_constraints) { constraints.insert(constraints.end(), elem.constraints.begin(), elem.constraints.end()); } @@ -638,20 +640,19 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints { * @param rhs Right-hand side. */ template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (SleipnirType || SleipnirType) InequalityConstraints(LHS&& lhs, RHS&& rhs) - : constraints{MakeConstraints(lhs, rhs)} {} + : constraints{make_constraints(lhs, rhs)} {} /** * Implicit conversion operator to bool. */ operator bool() { // NOLINT - return std::all_of( - constraints.begin(), constraints.end(), - [](auto& constraint) { return constraint.Value() >= 0.0; }); + return std::ranges::all_of(constraints, [](auto& constraint) { + return constraint.value() >= 0.0; + }); } }; @@ -662,10 +663,9 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints { * @param rhs Left-hand side. */ template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (SleipnirType || SleipnirType) EqualityConstraints operator==(LHS&& lhs, RHS&& rhs) { return EqualityConstraints{lhs, rhs}; } @@ -678,10 +678,9 @@ EqualityConstraints operator==(LHS&& lhs, RHS&& rhs) { * @param rhs Left-hand side. */ template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (SleipnirType || SleipnirType) InequalityConstraints operator<(LHS&& lhs, RHS&& rhs) { return rhs >= lhs; } @@ -694,10 +693,9 @@ InequalityConstraints operator<(LHS&& lhs, RHS&& rhs) { * @param rhs Left-hand side. */ template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (SleipnirType || SleipnirType) InequalityConstraints operator<=(LHS&& lhs, RHS&& rhs) { return rhs >= lhs; } @@ -710,10 +708,9 @@ InequalityConstraints operator<=(LHS&& lhs, RHS&& rhs) { * @param rhs Left-hand side. */ template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (SleipnirType || SleipnirType) InequalityConstraints operator>(LHS&& lhs, RHS&& rhs) { return lhs >= rhs; } @@ -726,15 +723,14 @@ InequalityConstraints operator>(LHS&& lhs, RHS&& rhs) { * @param rhs Left-hand side. */ template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (SleipnirType || SleipnirType) InequalityConstraints operator>=(LHS&& lhs, RHS&& rhs) { return InequalityConstraints{lhs, rhs}; } -} // namespace sleipnir +} // namespace slp namespace Eigen { @@ -742,20 +738,28 @@ namespace Eigen { * NumTraits specialization that allows instantiating Eigen types with Variable. */ template <> -struct NumTraits : NumTraits { - using Real = sleipnir::Variable; - using NonInteger = sleipnir::Variable; - using Nested = sleipnir::Variable; +struct NumTraits : NumTraits { + /// Real type. + using Real = slp::Variable; + /// Non-integer type. + using NonInteger = slp::Variable; + /// Nested type. + using Nested = slp::Variable; - enum { - IsComplex = 0, - IsInteger = 0, - IsSigned = 1, - RequireInitialization = 1, - ReadCost = 1, - AddCost = 3, - MulCost = 3 - }; + /// Is complex. + static constexpr int IsComplex = 0; + /// Is integer. + static constexpr int IsInteger = 0; + /// Is signed. + static constexpr int IsSigned = 1; + /// Require initialization. + static constexpr int RequireInitialization = 1; + /// Read cost. + static constexpr int ReadCost = 1; + /// Add cost. + static constexpr int AddCost = 3; + /// Multiply cost. + static constexpr int MulCost = 3; }; } // namespace Eigen diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_block.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_block.hpp new file mode 100644 index 0000000000..632d44beb5 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_block.hpp @@ -0,0 +1,872 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include + +#include + +#include "sleipnir/autodiff/slice.hpp" +#include "sleipnir/autodiff/variable.hpp" +#include "sleipnir/util/assert.hpp" +#include "sleipnir/util/function_ref.hpp" + +namespace slp { + +/** + * A submatrix of autodiff variables with reference semantics. + * + * @tparam Mat The type of the matrix whose storage this class points to. + */ +template +class VariableBlock { + public: + /** + * Copy constructor. + */ + VariableBlock(const VariableBlock&) = default; + + /** + * Assigns a VariableBlock to the block. + * + * @param values VariableBlock of values. + * @return This VariableBlock. + */ + VariableBlock& operator=(const VariableBlock& values) { + if (this == &values) { + return *this; + } + + if (m_mat == nullptr) { + m_mat = values.m_mat; + m_row_slice = values.m_row_slice; + m_row_slice_length = values.m_row_slice_length; + m_col_slice = values.m_col_slice; + m_col_slice_length = values.m_col_slice_length; + } else { + slp_assert(rows() == values.rows() && cols() == values.cols()); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) = values(row, col); + } + } + } + + return *this; + } + + /** + * Move constructor. + */ + VariableBlock(VariableBlock&&) = default; + + /** + * Assigns a VariableBlock to the block. + * + * @param values VariableBlock of values. + * @return This VariableBlock. + */ + VariableBlock& operator=(VariableBlock&& values) { + if (this == &values) { + return *this; + } + + if (m_mat == nullptr) { + m_mat = values.m_mat; + m_row_slice = values.m_row_slice; + m_row_slice_length = values.m_row_slice_length; + m_col_slice = values.m_col_slice; + m_col_slice_length = values.m_col_slice_length; + } else { + slp_assert(rows() == values.rows() && cols() == values.cols()); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) = values(row, col); + } + } + } + + return *this; + } + + /** + * Constructs a Variable block pointing to all of the given matrix. + * + * @param mat The matrix to which to point. + */ + VariableBlock(Mat& mat) // NOLINT + : m_mat{&mat}, + m_row_slice{0, mat.rows(), 1}, + m_row_slice_length{m_row_slice.adjust(mat.rows())}, + m_col_slice{0, mat.cols(), 1}, + m_col_slice_length{m_col_slice.adjust(mat.cols())} {} + + /** + * Constructs a Variable block pointing to a subset of the given matrix. + * + * @param mat The matrix to which to point. + * @param row_offset The block's row offset. + * @param col_offset The block's column offset. + * @param block_rows The number of rows in the block. + * @param block_cols The number of columns in the block. + */ + VariableBlock(Mat& mat, int row_offset, int col_offset, int block_rows, + int block_cols) + : m_mat{&mat}, + m_row_slice{row_offset, row_offset + block_rows, 1}, + m_row_slice_length{m_row_slice.adjust(mat.rows())}, + m_col_slice{col_offset, col_offset + block_cols, 1}, + m_col_slice_length{m_col_slice.adjust(mat.cols())} {} + + /** + * Constructs a Variable block pointing to a subset of the given matrix. + * + * Note that the slices are taken as is rather than adjusted. + * + * @param mat The matrix to which to point. + * @param row_slice The block's row slice. + * @param row_slice_length The block's row length. + * @param col_slice The block's column slice. + * @param col_slice_length The block's column length. + */ + VariableBlock(Mat& mat, Slice row_slice, int row_slice_length, + Slice col_slice, int col_slice_length) + : m_mat{&mat}, + m_row_slice{std::move(row_slice)}, + m_row_slice_length{row_slice_length}, + m_col_slice{std::move(col_slice)}, + m_col_slice_length{col_slice_length} {} + + /** + * Assigns a double to the block. + * + * This only works for blocks with one row and one column. + * + * @param value Value to assign. + * @return This VariableBlock. + */ + VariableBlock& operator=(ScalarLike auto value) { + slp_assert(rows() == 1 && cols() == 1); + + (*this)(0, 0) = value; + + return *this; + } + + /** + * Assigns a double to the block. + * + * This only works for blocks with one row and one column. + * + * @param value Value to assign. + */ + void set_value(double value) { + slp_assert(rows() == 1 && cols() == 1); + + (*this)(0, 0).set_value(value); + } + + /** + * Assigns an Eigen matrix to the block. + * + * @param values Eigen matrix of values to assign. + * @return This VariableBlock. + */ + template + VariableBlock& operator=(const Eigen::MatrixBase& values) { + slp_assert(rows() == values.rows() && cols() == values.cols()); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) = values(row, col); + } + } + + return *this; + } + + /** + * Sets block's internal values. + * + * @param values Eigen matrix of values. + */ + template + requires std::same_as + void set_value(const Eigen::MatrixBase& values) { + slp_assert(rows() == values.rows() && cols() == values.cols()); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col).set_value(values(row, col)); + } + } + } + + /** + * Assigns a VariableMatrix to the block. + * + * @param values VariableMatrix of values. + * @return This VariableBlock. + */ + VariableBlock& operator=(const Mat& values) { + slp_assert(rows() == values.rows() && cols() == values.cols()); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) = values(row, col); + } + } + return *this; + } + + /** + * Assigns a VariableMatrix to the block. + * + * @param values VariableMatrix of values. + * @return This VariableBlock. + */ + VariableBlock& operator=(Mat&& values) { + slp_assert(rows() == values.rows() && cols() == values.cols()); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) = std::move(values(row, col)); + } + } + return *this; + } + + /** + * Returns a scalar subblock at the given row and column. + * + * @param row The scalar subblock's row. + * @param col The scalar subblock's column. + * @return A scalar subblock at the given row and column. + */ + Variable& operator()(int row, int col) + requires(!std::is_const_v) + { + slp_assert(row >= 0 && row < rows()); + slp_assert(col >= 0 && col < cols()); + return (*m_mat)(m_row_slice.start + row * m_row_slice.step, + m_col_slice.start + col * m_col_slice.step); + } + + /** + * Returns a scalar subblock at the given row and column. + * + * @param row The scalar subblock's row. + * @param col The scalar subblock's column. + * @return A scalar subblock at the given row and column. + */ + const Variable& operator()(int row, int col) const { + slp_assert(row >= 0 && row < rows()); + slp_assert(col >= 0 && col < cols()); + return (*m_mat)(m_row_slice.start + row * m_row_slice.step, + m_col_slice.start + col * m_col_slice.step); + } + + /** + * Returns a scalar subblock at the given row. + * + * @param row The scalar subblock's row. + * @return A scalar subblock at the given row. + */ + Variable& operator[](int row) + requires(!std::is_const_v) + { + slp_assert(row >= 0 && row < rows() * cols()); + return (*this)(row / cols(), row % cols()); + } + + /** + * Returns a scalar subblock at the given row. + * + * @param row The scalar subblock's row. + * @return A scalar subblock at the given row. + */ + const Variable& operator[](int row) const { + slp_assert(row >= 0 && row < rows() * cols()); + return (*this)(row / cols(), row % cols()); + } + + /** + * Returns a block of the variable matrix. + * + * @param row_offset The row offset of the block selection. + * @param col_offset The column offset of the block selection. + * @param block_rows The number of rows in the block selection. + * @param block_cols The number of columns in the block selection. + * @return A block of the variable matrix. + */ + VariableBlock block(int row_offset, int col_offset, int block_rows, + int block_cols) { + slp_assert(row_offset >= 0 && row_offset <= rows()); + slp_assert(col_offset >= 0 && col_offset <= cols()); + slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset); + slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset); + return (*this)({row_offset, row_offset + block_rows, 1}, + {col_offset, col_offset + block_cols, 1}); + } + + /** + * Returns a block slice of the variable matrix. + * + * @param row_offset The row offset of the block selection. + * @param col_offset The column offset of the block selection. + * @param block_rows The number of rows in the block selection. + * @param block_cols The number of columns in the block selection. + * @return A block slice of the variable matrix. + */ + const VariableBlock block(int row_offset, int col_offset, + int block_rows, int block_cols) const { + slp_assert(row_offset >= 0 && row_offset <= rows()); + slp_assert(col_offset >= 0 && col_offset <= cols()); + slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset); + slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset); + return (*this)({row_offset, row_offset + block_rows, 1}, + {col_offset, col_offset + block_cols, 1}); + } + + /** + * Returns a slice of the variable matrix. + * + * @param row_slice The row slice. + * @param col_slice The column slice. + * @return A slice of the variable matrix. + */ + VariableBlock operator()(Slice row_slice, Slice col_slice) { + int row_slice_length = row_slice.adjust(m_row_slice_length); + int col_slice_length = col_slice.adjust(m_col_slice_length); + return VariableBlock{ + *m_mat, + {m_row_slice.start + row_slice.start * m_row_slice.step, + m_row_slice.start + row_slice.stop, m_row_slice.step * row_slice.step}, + row_slice_length, + {m_col_slice.start + col_slice.start * m_col_slice.step, + m_col_slice.start + col_slice.stop, m_col_slice.step * col_slice.step}, + col_slice_length}; + } + + /** + * Returns a slice of the variable matrix. + * + * @param row_slice The row slice. + * @param col_slice The column slice. + * @return A slice of the variable matrix. + */ + const VariableBlock operator()(Slice row_slice, + Slice col_slice) const { + int row_slice_length = row_slice.adjust(m_row_slice_length); + int col_slice_length = col_slice.adjust(m_col_slice_length); + return VariableBlock{ + *m_mat, + {m_row_slice.start + row_slice.start * m_row_slice.step, + m_row_slice.start + row_slice.stop, m_row_slice.step * row_slice.step}, + row_slice_length, + {m_col_slice.start + col_slice.start * m_col_slice.step, + m_col_slice.start + col_slice.stop, m_col_slice.step * col_slice.step}, + col_slice_length}; + } + + /** + * Returns a slice of the variable matrix. + * + * The given slices aren't adjusted. This overload is for Python bindings + * only. + * + * @param row_slice The row slice. + * @param row_slice_length The row slice length. + * @param col_slice The column slice. + * @param col_slice_length The column slice length. + * @return A slice of the variable matrix. + */ + VariableBlock operator()(Slice row_slice, int row_slice_length, + Slice col_slice, int col_slice_length) { + return VariableBlock{ + *m_mat, + {m_row_slice.start + row_slice.start * m_row_slice.step, + m_row_slice.start + row_slice.stop, m_row_slice.step * row_slice.step}, + row_slice_length, + {m_col_slice.start + col_slice.start * m_col_slice.step, + m_col_slice.start + col_slice.stop, m_col_slice.step * col_slice.step}, + col_slice_length}; + } + + /** + * Returns a slice of the variable matrix. + * + * The given slices aren't adjusted. This overload is for Python bindings + * only. + * + * @param row_slice The row slice. + * @param row_slice_length The row slice length. + * @param col_slice The column slice. + * @param col_slice_length The column slice length. + * @return A slice of the variable matrix. + */ + const VariableBlock operator()(Slice row_slice, + int row_slice_length, + Slice col_slice, + int col_slice_length) const { + return VariableBlock{ + *m_mat, + {m_row_slice.start + row_slice.start * m_row_slice.step, + m_row_slice.start + row_slice.stop, m_row_slice.step * row_slice.step}, + row_slice_length, + {m_col_slice.start + col_slice.start * m_col_slice.step, + m_col_slice.start + col_slice.stop, m_col_slice.step * col_slice.step}, + col_slice_length}; + } + + /** + * Returns a segment of the variable vector. + * + * @param offset The offset of the segment. + * @param length The length of the segment. + * @return A segment of the variable vector. + */ + VariableBlock segment(int offset, int length) { + slp_assert(offset >= 0 && offset < rows() * cols()); + slp_assert(length >= 0 && length <= rows() * cols() - offset); + return block(offset, 0, length, 1); + } + + /** + * Returns a segment of the variable vector. + * + * @param offset The offset of the segment. + * @param length The length of the segment. + * @return A segment of the variable vector. + */ + const VariableBlock segment(int offset, int length) const { + slp_assert(offset >= 0 && offset < rows() * cols()); + slp_assert(length >= 0 && length <= rows() * cols() - offset); + return block(offset, 0, length, 1); + } + + /** + * Returns a row slice of the variable matrix. + * + * @param row The row to slice. + * @return A row slice of the variable matrix. + */ + VariableBlock row(int row) { + slp_assert(row >= 0 && row < rows()); + return block(row, 0, 1, cols()); + } + + /** + * Returns a row slice of the variable matrix. + * + * @param row The row to slice. + * @return A row slice of the variable matrix. + */ + VariableBlock row(int row) const { + slp_assert(row >= 0 && row < rows()); + return block(row, 0, 1, cols()); + } + + /** + * Returns a column slice of the variable matrix. + * + * @param col The column to slice. + * @return A column slice of the variable matrix. + */ + VariableBlock col(int col) { + slp_assert(col >= 0 && col < cols()); + return block(0, col, rows(), 1); + } + + /** + * Returns a column slice of the variable matrix. + * + * @param col The column to slice. + * @return A column slice of the variable matrix. + */ + VariableBlock col(int col) const { + slp_assert(col >= 0 && col < cols()); + return block(0, col, rows(), 1); + } + + /** + * Compound matrix multiplication-assignment operator. + * + * @param rhs Variable to multiply. + * @return Result of multiplication. + */ + VariableBlock& operator*=(const MatrixLike auto& rhs) { + slp_assert(cols() == rhs.rows() && cols() == rhs.cols()); + + for (int i = 0; i < rows(); ++i) { + for (int j = 0; j < rhs.cols(); ++j) { + Variable sum; + for (int k = 0; k < cols(); ++k) { + sum += (*this)(i, k) * rhs(k, j); + } + (*this)(i, j) = sum; + } + } + + return *this; + } + + /** + * Compound matrix multiplication-assignment operator. + * + * @param rhs Variable to multiply. + * @return Result of multiplication. + */ + VariableBlock& operator*=(const ScalarLike auto& rhs) { + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) *= rhs; + } + } + + return *this; + } + + /** + * Compound matrix division-assignment operator. + * + * @param rhs Variable to divide. + * @return Result of division. + */ + VariableBlock& operator/=(const MatrixLike auto& rhs) { + slp_assert(rhs.rows() == 1 && rhs.cols() == 1); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) /= rhs(0, 0); + } + } + + return *this; + } + + /** + * Compound matrix division-assignment operator. + * + * @param rhs Variable to divide. + * @return Result of division. + */ + VariableBlock& operator/=(const ScalarLike auto& rhs) { + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) /= rhs; + } + } + + return *this; + } + + /** + * Compound addition-assignment operator. + * + * @param rhs Variable to add. + * @return Result of addition. + */ + VariableBlock& operator+=(const MatrixLike auto& rhs) { + slp_assert(rows() == rhs.rows() && cols() == rhs.cols()); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) += rhs(row, col); + } + } + + return *this; + } + + /** + * Compound addition-assignment operator. + * + * @param rhs Variable to add. + * @return Result of addition. + */ + VariableBlock& operator+=(const ScalarLike auto& rhs) { + slp_assert(rows() == 1 && cols() == 1); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) += rhs; + } + } + + return *this; + } + + /** + * Compound subtraction-assignment operator. + * + * @param rhs Variable to subtract. + * @return Result of subtraction. + */ + VariableBlock& operator-=(const MatrixLike auto& rhs) { + slp_assert(rows() == rhs.rows() && cols() == rhs.cols()); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) -= rhs(row, col); + } + } + + return *this; + } + + /** + * Compound subtraction-assignment operator. + * + * @param rhs Variable to subtract. + * @return Result of subtraction. + */ + VariableBlock& operator-=(const ScalarLike auto& rhs) { + slp_assert(rows() == 1 && cols() == 1); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) -= rhs; + } + } + + return *this; + } + + /** + * Implicit conversion operator from 1x1 VariableBlock to Variable. + */ + operator Variable() const { // NOLINT + slp_assert(rows() == 1 && cols() == 1); + return (*this)(0, 0); + } + + /** + * Returns the transpose of the variable matrix. + * + * @return The transpose of the variable matrix. + */ + std::remove_cv_t T() const { + std::remove_cv_t result{Mat::empty, cols(), rows()}; + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + result(col, row) = (*this)(row, col); + } + } + + return result; + } + + /** + * Returns the number of rows in the matrix. + * + * @return The number of rows in the matrix. + */ + int rows() const { return m_row_slice_length; } + + /** + * Returns the number of columns in the matrix. + * + * @return The number of columns in the matrix. + */ + int cols() const { return m_col_slice_length; } + + /** + * Returns an element of the variable matrix. + * + * @param row The row of the element to return. + * @param col The column of the element to return. + * @return An element of the variable matrix. + */ + double value(int row, int col) { + slp_assert(row >= 0 && row < rows()); + slp_assert(col >= 0 && col < cols()); + return (*m_mat)(m_row_slice.start + row * m_row_slice.step, + m_col_slice.start + col * m_col_slice.step) + .value(); + } + + /** + * Returns a row of the variable column vector. + * + * @param index The index of the element to return. + * @return A row of the variable column vector. + */ + double value(int index) { + slp_assert(index >= 0 && index < rows() * cols()); + return value(index / cols(), index % cols()); + } + + /** + * Returns the contents of the variable matrix. + * + * @return The contents of the variable matrix. + */ + Eigen::MatrixXd value() { + Eigen::MatrixXd result{rows(), cols()}; + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + result(row, col) = value(row, col); + } + } + + return result; + } + + /** + * Transforms the matrix coefficient-wise with an unary operator. + * + * @param unary_op The unary operator to use for the transform operation. + * @return Result of the unary operator. + */ + std::remove_cv_t cwise_transform( + function_ref unary_op) const { + std::remove_cv_t result{Mat::empty, rows(), cols()}; + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + result(row, col) = unary_op((*this)(row, col)); + } + } + + return result; + } + +#ifndef DOXYGEN_SHOULD_SKIP_THIS + + class iterator { + public: + using iterator_category = std::forward_iterator_tag; + using value_type = Variable; + using difference_type = std::ptrdiff_t; + using pointer = Variable*; + using reference = Variable&; + + constexpr iterator() noexcept = default; + + constexpr iterator(VariableBlock* mat, int index) noexcept + : m_mat{mat}, m_index{index} {} + + constexpr iterator& operator++() noexcept { + ++m_index; + return *this; + } + + constexpr iterator operator++(int) noexcept { + iterator retval = *this; + ++(*this); + return retval; + } + + constexpr bool operator==(const iterator&) const noexcept = default; + + constexpr reference operator*() const noexcept { return (*m_mat)[m_index]; } + + private: + VariableBlock* m_mat = nullptr; + int m_index = 0; + }; + + class const_iterator { + public: + using iterator_category = std::forward_iterator_tag; + using value_type = Variable; + using difference_type = std::ptrdiff_t; + using pointer = Variable*; + using const_reference = const Variable&; + + constexpr const_iterator() noexcept = default; + + constexpr const_iterator(const VariableBlock* mat, int index) noexcept + : m_mat{mat}, m_index{index} {} + + constexpr const_iterator& operator++() noexcept { + ++m_index; + return *this; + } + + constexpr const_iterator operator++(int) noexcept { + const_iterator retval = *this; + ++(*this); + return retval; + } + + constexpr bool operator==(const const_iterator&) const noexcept = default; + + constexpr const_reference operator*() const noexcept { + return (*m_mat)[m_index]; + } + + private: + const VariableBlock* m_mat = nullptr; + int m_index = 0; + }; + +#endif // DOXYGEN_SHOULD_SKIP_THIS + + /** + * Returns begin iterator. + * + * @return Begin iterator. + */ + iterator begin() { return iterator(this, 0); } + + /** + * Returns end iterator. + * + * @return End iterator. + */ + iterator end() { return iterator(this, rows() * cols()); } + + /** + * Returns begin iterator. + * + * @return Begin iterator. + */ + const_iterator begin() const { return const_iterator(this, 0); } + + /** + * Returns end iterator. + * + * @return End iterator. + */ + const_iterator end() const { return const_iterator(this, rows() * cols()); } + + /** + * Returns begin iterator. + * + * @return Begin iterator. + */ + const_iterator cbegin() const { return const_iterator(this, 0); } + + /** + * Returns end iterator. + * + * @return End iterator. + */ + const_iterator cend() const { return const_iterator(this, rows() * cols()); } + + /** + * Returns number of elements in matrix. + * + * @return Number of elements in matrix. + */ + size_t size() const { return rows() * cols(); } + + private: + Mat* m_mat = nullptr; + + Slice m_row_slice; + int m_row_slice_length = 0; + + Slice m_col_slice; + int m_col_slice_length = 0; +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp new file mode 100644 index 0000000000..4dc2cea00c --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp @@ -0,0 +1,1283 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "sleipnir/autodiff/slice.hpp" +#include "sleipnir/autodiff/variable.hpp" +#include "sleipnir/autodiff/variable_block.hpp" +#include "sleipnir/util/assert.hpp" +#include "sleipnir/util/concepts.hpp" +#include "sleipnir/util/function_ref.hpp" +#include "sleipnir/util/symbol_exports.hpp" + +namespace slp { + +/** + * A matrix of autodiff variables. + */ +class SLEIPNIR_DLLEXPORT VariableMatrix { + public: + /** + * Type tag used to designate an uninitialized VariableMatrix. + */ + struct empty_t {}; + + /** + * Designates an uninitialized VariableMatrix. + */ + static constexpr empty_t empty{}; + + /** + * Constructs an empty VariableMatrix. + */ + VariableMatrix() = default; + + /** + * Constructs a VariableMatrix column vector with the given rows. + * + * @param rows The number of matrix rows. + */ + explicit VariableMatrix(int rows) : m_rows{rows}, m_cols{1} { + m_storage.reserve(rows); + for (int row = 0; row < rows; ++row) { + m_storage.emplace_back(); + } + } + + /** + * Constructs a zero-initialized VariableMatrix with the given dimensions. + * + * @param rows The number of matrix rows. + * @param cols The number of matrix columns. + */ + VariableMatrix(int rows, int cols) : m_rows{rows}, m_cols{cols} { + m_storage.reserve(rows * cols); + for (int index = 0; index < rows * cols; ++index) { + m_storage.emplace_back(); + } + } + + /** + * Constructs an empty VariableMatrix with the given dimensions. + * + * @param rows The number of matrix rows. + * @param cols The number of matrix columns. + */ + VariableMatrix(empty_t, int rows, int cols) : m_rows{rows}, m_cols{cols} { + m_storage.reserve(rows * cols); + for (int index = 0; index < rows * cols; ++index) { + m_storage.emplace_back(nullptr); + } + } + + /** + * Constructs a scalar VariableMatrix from a nested list of Variables. + * + * @param list The nested list of Variables. + */ + VariableMatrix( // NOLINT + std::initializer_list> list) { + // Get row and column counts for destination matrix + m_rows = list.size(); + m_cols = 0; + if (list.size() > 0) { + m_cols = list.begin()->size(); + } + + // Assert the first and latest column counts are the same + for ([[maybe_unused]] + const auto& row : list) { + slp_assert(list.begin()->size() == row.size()); + } + + m_storage.reserve(rows() * cols()); + for (const auto& row : list) { + std::ranges::copy(row, std::back_inserter(m_storage)); + } + } + + /** + * Constructs a scalar VariableMatrix from a nested list of doubles. + * + * This overload is for Python bindings only. + * + * @param list The nested list of Variables. + */ + VariableMatrix(const std::vector>& list) { // NOLINT + // Get row and column counts for destination matrix + m_rows = list.size(); + m_cols = 0; + if (list.size() > 0) { + m_cols = list.begin()->size(); + } + + // Assert the first and latest column counts are the same + for ([[maybe_unused]] + const auto& row : list) { + slp_assert(list.begin()->size() == row.size()); + } + + m_storage.reserve(rows() * cols()); + for (const auto& row : list) { + std::ranges::copy(row, std::back_inserter(m_storage)); + } + } + + /** + * Constructs a scalar VariableMatrix from a nested list of Variables. + * + * This overload is for Python bindings only. + * + * @param list The nested list of Variables. + */ + VariableMatrix(const std::vector>& list) { // NOLINT + // Get row and column counts for destination matrix + m_rows = list.size(); + m_cols = 0; + if (list.size() > 0) { + m_cols = list.begin()->size(); + } + + // Assert the first and latest column counts are the same + for ([[maybe_unused]] + const auto& row : list) { + slp_assert(list.begin()->size() == row.size()); + } + + m_storage.reserve(rows() * cols()); + for (const auto& row : list) { + std::ranges::copy(row, std::back_inserter(m_storage)); + } + } + + /** + * Constructs a VariableMatrix from an Eigen matrix. + * + * @param values Eigen matrix of values. + */ + template + VariableMatrix(const Eigen::MatrixBase& values) // NOLINT + : m_rows{static_cast(values.rows())}, + m_cols{static_cast(values.cols())} { + m_storage.reserve(values.rows() * values.cols()); + for (int row = 0; row < values.rows(); ++row) { + for (int col = 0; col < values.cols(); ++col) { + m_storage.emplace_back(values(row, col)); + } + } + } + + /** + * Constructs a VariableMatrix from an Eigen diagonal matrix. + * + * @param values Diagonal matrix of values. + */ + template + VariableMatrix(const Eigen::DiagonalBase& values) // NOLINT + : m_rows{static_cast(values.rows())}, + m_cols{static_cast(values.cols())} { + m_storage.reserve(values.rows() * values.cols()); + for (int row = 0; row < values.rows(); ++row) { + for (int col = 0; col < values.cols(); ++col) { + if (row == col) { + m_storage.emplace_back(values.diagonal()[row]); + } else { + m_storage.emplace_back(0.0); + } + } + } + } + + /** + * Assigns an Eigen matrix to a VariableMatrix. + * + * @param values Eigen matrix of values. + * @return This VariableMatrix. + */ + template + VariableMatrix& operator=(const Eigen::MatrixBase& values) { + slp_assert(rows() == values.rows() && cols() == values.cols()); + + for (int row = 0; row < values.rows(); ++row) { + for (int col = 0; col < values.cols(); ++col) { + (*this)(row, col) = values(row, col); + } + } + + return *this; + } + + /** + * Assigns a double to the matrix. + * + * This only works for matrices with one row and one column. + * + * @param value Value to assign. + * @return This VariableMatrix. + */ + VariableMatrix& operator=(ScalarLike auto value) { + slp_assert(rows() == 1 && cols() == 1); + + (*this)(0, 0) = value; + + return *this; + } + + /** + * Sets the VariableMatrix's internal values. + * + * @param values Eigen matrix of values. + */ + template + requires std::same_as + void set_value(const Eigen::MatrixBase& values) { + slp_assert(rows() == values.rows() && cols() == values.cols()); + + for (int row = 0; row < values.rows(); ++row) { + for (int col = 0; col < values.cols(); ++col) { + (*this)(row, col).set_value(values(row, col)); + } + } + } + + /** + * Constructs a scalar VariableMatrix from a Variable. + * + * @param variable Variable. + */ + VariableMatrix(const Variable& variable) // NOLINT + : m_rows{1}, m_cols{1} { + m_storage.emplace_back(variable); + } + + /** + * Constructs a scalar VariableMatrix from a Variable. + * + * @param variable Variable. + */ + VariableMatrix(Variable&& variable) : m_rows{1}, m_cols{1} { // NOLINT + m_storage.emplace_back(std::move(variable)); + } + + /** + * Constructs a VariableMatrix from a VariableBlock. + * + * @param values VariableBlock of values. + */ + VariableMatrix(const VariableBlock& values) // NOLINT + : m_rows{values.rows()}, m_cols{values.cols()} { + m_storage.reserve(rows() * cols()); + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + m_storage.emplace_back(values(row, col)); + } + } + } + + /** + * Constructs a VariableMatrix from a VariableBlock. + * + * @param values VariableBlock of values. + */ + VariableMatrix(const VariableBlock& values) // NOLINT + : m_rows{values.rows()}, m_cols{values.cols()} { + m_storage.reserve(rows() * cols()); + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + m_storage.emplace_back(values(row, col)); + } + } + } + + /** + * Constructs a column vector wrapper around a Variable array. + * + * @param values Variable array to wrap. + */ + explicit VariableMatrix(std::span values) + : m_rows{static_cast(values.size())}, m_cols{1} { + m_storage.reserve(rows() * cols()); + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + m_storage.emplace_back(values[row * cols() + col]); + } + } + } + + /** + * Constructs a matrix wrapper around a Variable array. + * + * @param values Variable array to wrap. + * @param rows The number of matrix rows. + * @param cols The number of matrix columns. + */ + VariableMatrix(std::span values, int rows, int cols) + : m_rows{rows}, m_cols{cols} { + slp_assert(static_cast(values.size()) == rows * cols); + m_storage.reserve(rows * cols); + for (int row = 0; row < rows; ++row) { + for (int col = 0; col < cols; ++col) { + m_storage.emplace_back(values[row * cols + col]); + } + } + } + + /** + * Returns a block pointing to the given row and column. + * + * @param row The block row. + * @param col The block column. + * @return A block pointing to the given row and column. + */ + Variable& operator()(int row, int col) { + slp_assert(row >= 0 && row < rows()); + slp_assert(col >= 0 && col < cols()); + return m_storage[row * cols() + col]; + } + + /** + * Returns a block pointing to the given row and column. + * + * @param row The block row. + * @param col The block column. + * @return A block pointing to the given row and column. + */ + const Variable& operator()(int row, int col) const { + slp_assert(row >= 0 && row < rows()); + slp_assert(col >= 0 && col < cols()); + return m_storage[row * cols() + col]; + } + + /** + * Returns a block pointing to the given row. + * + * @param row The block row. + * @return A block pointing to the given row. + */ + Variable& operator[](int row) { + slp_assert(row >= 0 && row < rows() * cols()); + return m_storage[row]; + } + + /** + * Returns a block pointing to the given row. + * + * @param row The block row. + * @return A block pointing to the given row. + */ + const Variable& operator[](int row) const { + slp_assert(row >= 0 && row < rows() * cols()); + return m_storage[row]; + } + + /** + * Returns a block of the variable matrix. + * + * @param row_offset The row offset of the block selection. + * @param col_offset The column offset of the block selection. + * @param block_rows The number of rows in the block selection. + * @param block_cols The number of columns in the block selection. + * @return A block of the variable matrix. + */ + VariableBlock block(int row_offset, int col_offset, + int block_rows, int block_cols) { + slp_assert(row_offset >= 0 && row_offset <= rows()); + slp_assert(col_offset >= 0 && col_offset <= cols()); + slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset); + slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset); + return VariableBlock{*this, row_offset, col_offset, block_rows, block_cols}; + } + + /** + * Returns a block of the variable matrix. + * + * @param row_offset The row offset of the block selection. + * @param col_offset The column offset of the block selection. + * @param block_rows The number of rows in the block selection. + * @param block_cols The number of columns in the block selection. + * @return A block of the variable matrix. + */ + const VariableBlock block(int row_offset, + int col_offset, + int block_rows, + int block_cols) const { + slp_assert(row_offset >= 0 && row_offset <= rows()); + slp_assert(col_offset >= 0 && col_offset <= cols()); + slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset); + slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset); + return VariableBlock{*this, row_offset, col_offset, block_rows, block_cols}; + } + + /** + * Returns a slice of the variable matrix. + * + * @param row_slice The row slice. + * @param col_slice The column slice. + * @return A slice of the variable matrix. + */ + VariableBlock operator()(Slice row_slice, Slice col_slice) { + int row_slice_length = row_slice.adjust(rows()); + int col_slice_length = col_slice.adjust(cols()); + return VariableBlock{*this, std::move(row_slice), row_slice_length, + std::move(col_slice), col_slice_length}; + } + + /** + * Returns a slice of the variable matrix. + * + * @param row_slice The row slice. + * @param col_slice The column slice. + * @return A slice of the variable matrix. + */ + const VariableBlock operator()(Slice row_slice, + Slice col_slice) const { + int row_slice_length = row_slice.adjust(rows()); + int col_slice_length = col_slice.adjust(cols()); + return VariableBlock{*this, std::move(row_slice), row_slice_length, + std::move(col_slice), col_slice_length}; + } + + /** + * Returns a slice of the variable matrix. + * + * The given slices aren't adjusted. This overload is for Python bindings + * only. + * + * @param row_slice The row slice. + * @param row_slice_length The row slice length. + * @param col_slice The column slice. + * @param col_slice_length The column slice length. + * @return A slice of the variable matrix. + * + */ + VariableBlock operator()(Slice row_slice, + int row_slice_length, + Slice col_slice, + int col_slice_length) { + return VariableBlock{*this, std::move(row_slice), row_slice_length, + std::move(col_slice), col_slice_length}; + } + + /** + * Returns a slice of the variable matrix. + * + * The given slices aren't adjusted. This overload is for Python bindings + * only. + * + * @param row_slice The row slice. + * @param row_slice_length The row slice length. + * @param col_slice The column slice. + * @param col_slice_length The column slice length. + * @return A slice of the variable matrix. + */ + const VariableBlock operator()( + Slice row_slice, int row_slice_length, Slice col_slice, + int col_slice_length) const { + return VariableBlock{*this, std::move(row_slice), row_slice_length, + std::move(col_slice), col_slice_length}; + } + + /** + * Returns a segment of the variable vector. + * + * @param offset The offset of the segment. + * @param length The length of the segment. + * @return A segment of the variable vector. + */ + VariableBlock segment(int offset, int length) { + slp_assert(offset >= 0 && offset < rows() * cols()); + slp_assert(length >= 0 && length <= rows() * cols() - offset); + return block(offset, 0, length, 1); + } + + /** + * Returns a segment of the variable vector. + * + * @param offset The offset of the segment. + * @param length The length of the segment. + * @return A segment of the variable vector. + */ + const VariableBlock segment(int offset, + int length) const { + slp_assert(offset >= 0 && offset < rows() * cols()); + slp_assert(length >= 0 && length <= rows() * cols() - offset); + return block(offset, 0, length, 1); + } + + /** + * Returns a row slice of the variable matrix. + * + * @param row The row to slice. + * @return A row slice of the variable matrix. + */ + VariableBlock row(int row) { + slp_assert(row >= 0 && row < rows()); + return block(row, 0, 1, cols()); + } + + /** + * Returns a row slice of the variable matrix. + * + * @param row The row to slice. + * @return A row slice of the variable matrix. + */ + const VariableBlock row(int row) const { + slp_assert(row >= 0 && row < rows()); + return block(row, 0, 1, cols()); + } + + /** + * Returns a column slice of the variable matrix. + * + * @param col The column to slice. + * @return A column slice of the variable matrix. + */ + VariableBlock col(int col) { + slp_assert(col >= 0 && col < cols()); + return block(0, col, rows(), 1); + } + + /** + * Returns a column slice of the variable matrix. + * + * @param col The column to slice. + * @return A column slice of the variable matrix. + */ + const VariableBlock col(int col) const { + slp_assert(col >= 0 && col < cols()); + return block(0, col, rows(), 1); + } + + /** + * Matrix multiplication operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + template + requires SleipnirMatrixLike || SleipnirMatrixLike + friend SLEIPNIR_DLLEXPORT VariableMatrix operator*(const LHS& lhs, + const RHS& rhs) { + slp_assert(lhs.cols() == rhs.rows()); + + VariableMatrix result(VariableMatrix::empty, lhs.rows(), rhs.cols()); + +#if __GNUC__ >= 12 +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wmaybe-uninitialized" +#endif + for (int i = 0; i < lhs.rows(); ++i) { + for (int j = 0; j < rhs.cols(); ++j) { + Variable sum; + for (int k = 0; k < lhs.cols(); ++k) { + sum += lhs(i, k) * rhs(k, j); + } + result(i, j) = sum; + } + } +#if __GNUC__ >= 12 +#pragma GCC diagnostic pop +#endif + + return result; + } + + /** + * Matrix-scalar multiplication operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + friend SLEIPNIR_DLLEXPORT VariableMatrix + operator*(const SleipnirMatrixLike auto& lhs, const ScalarLike auto& rhs) { + VariableMatrix result{VariableMatrix::empty, lhs.rows(), lhs.cols()}; + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { + result(row, col) = lhs(row, col) * rhs; + } + } + + return result; + } + + /** + * Matrix-scalar multiplication operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + friend SLEIPNIR_DLLEXPORT VariableMatrix operator*(const MatrixLike auto& lhs, + const Variable& rhs) { + VariableMatrix result(VariableMatrix::empty, lhs.rows(), lhs.cols()); + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { + result(row, col) = lhs(row, col) * rhs; + } + } + + return result; + } + + /** + * Scalar-matrix multiplication operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + friend SLEIPNIR_DLLEXPORT VariableMatrix + operator*(const ScalarLike auto& lhs, const SleipnirMatrixLike auto& rhs) { + VariableMatrix result{VariableMatrix::empty, rhs.rows(), rhs.cols()}; + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { + result(row, col) = rhs(row, col) * lhs; + } + } + + return result; + } + + /** + * Scalar-matrix multiplication operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + friend SLEIPNIR_DLLEXPORT VariableMatrix + operator*(const Variable& lhs, const MatrixLike auto& rhs) { + VariableMatrix result(VariableMatrix::empty, rhs.rows(), rhs.cols()); + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { + result(row, col) = rhs(row, col) * lhs; + } + } + + return result; + } + + /** + * Compound matrix multiplication-assignment operator. + * + * @param rhs Variable to multiply. + * @return Result of multiplication. + */ + VariableMatrix& operator*=(const MatrixLike auto& rhs) { + slp_assert(cols() == rhs.rows() && cols() == rhs.cols()); + + for (int i = 0; i < rows(); ++i) { + for (int j = 0; j < rhs.cols(); ++j) { + Variable sum; + for (int k = 0; k < cols(); ++k) { + sum += (*this)(i, k) * rhs(k, j); + } + (*this)(i, j) = sum; + } + } + + return *this; + } + + /** + * Compound matrix-scalar multiplication-assignment operator. + * + * @param rhs Variable to multiply. + * @return Result of multiplication. + */ + VariableMatrix& operator*=(const ScalarLike auto& rhs) { + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < rhs.cols(); ++col) { + (*this)(row, col) *= rhs; + } + } + + return *this; + } + + /** + * Binary division operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + * @return Result of division. + */ + friend SLEIPNIR_DLLEXPORT VariableMatrix + operator/(const MatrixLike auto& lhs, const ScalarLike auto& rhs) { + VariableMatrix result(VariableMatrix::empty, lhs.rows(), lhs.cols()); + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { + result(row, col) = lhs(row, col) / rhs; + } + } + + return result; + } + + /** + * Compound matrix division-assignment operator. + * + * @param rhs Variable to divide. + * @return Result of division. + */ + VariableMatrix& operator/=(const ScalarLike auto& rhs) { + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) /= rhs; + } + } + + return *this; + } + + /** + * Binary addition operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + * @return Result of addition. + */ + template + requires SleipnirMatrixLike || SleipnirMatrixLike + friend SLEIPNIR_DLLEXPORT VariableMatrix operator+(const LHS& lhs, + const RHS& rhs) { + slp_assert(lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols()); + + VariableMatrix result(VariableMatrix::empty, lhs.rows(), lhs.cols()); + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { + result(row, col) = lhs(row, col) + rhs(row, col); + } + } + + return result; + } + + /** + * Compound addition-assignment operator. + * + * @param rhs Variable to add. + * @return Result of addition. + */ + VariableMatrix& operator+=(const MatrixLike auto& rhs) { + slp_assert(rows() == rhs.rows() && cols() == rhs.cols()); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) += rhs(row, col); + } + } + + return *this; + } + + /** + * Compound addition-assignment operator. + * + * @param rhs Variable to add. + * @return Result of addition. + */ + VariableMatrix& operator+=(const ScalarLike auto& rhs) { + slp_assert(rows() == 1 && cols() == 1); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) += rhs; + } + } + + return *this; + } + + /** + * Binary subtraction operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + * @return Result of subtraction. + */ + template + requires SleipnirMatrixLike || SleipnirMatrixLike + friend SLEIPNIR_DLLEXPORT VariableMatrix operator-(const LHS& lhs, + const RHS& rhs) { + slp_assert(lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols()); + + VariableMatrix result(VariableMatrix::empty, lhs.rows(), lhs.cols()); + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { + result(row, col) = lhs(row, col) - rhs(row, col); + } + } + + return result; + } + + /** + * Compound subtraction-assignment operator. + * + * @param rhs Variable to subtract. + * @return Result of subtraction. + */ + VariableMatrix& operator-=(const MatrixLike auto& rhs) { + slp_assert(rows() == rhs.rows() && cols() == rhs.cols()); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) -= rhs(row, col); + } + } + + return *this; + } + + /** + * Compound subtraction-assignment operator. + * + * @param rhs Variable to subtract. + * @return Result of subtraction. + */ + VariableMatrix& operator-=(const ScalarLike auto& rhs) { + slp_assert(rows() == 1 && cols() == 1); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) -= rhs; + } + } + + return *this; + } + + /** + * Unary minus operator. + * + * @param lhs Operand for unary minus. + */ + friend SLEIPNIR_DLLEXPORT VariableMatrix + operator-(const SleipnirMatrixLike auto& lhs) { + VariableMatrix result{VariableMatrix::empty, lhs.rows(), lhs.cols()}; + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { + result(row, col) = -lhs(row, col); + } + } + + return result; + } + + /** + * Implicit conversion operator from 1x1 VariableMatrix to Variable. + */ + operator Variable() const { // NOLINT + slp_assert(rows() == 1 && cols() == 1); + return (*this)(0, 0); + } + + /** + * Returns the transpose of the variable matrix. + * + * @return The transpose of the variable matrix. + */ + VariableMatrix T() const { + VariableMatrix result{VariableMatrix::empty, cols(), rows()}; + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + result(col, row) = (*this)(row, col); + } + } + + return result; + } + + /** + * Returns the number of rows in the matrix. + * + * @return The number of rows in the matrix. + */ + int rows() const { return m_rows; } + + /** + * Returns the number of columns in the matrix. + * + * @return The number of columns in the matrix. + */ + int cols() const { return m_cols; } + + /** + * Returns an element of the variable matrix. + * + * @param row The row of the element to return. + * @param col The column of the element to return. + * @return An element of the variable matrix. + */ + double value(int row, int col) { + slp_assert(row >= 0 && row < rows()); + slp_assert(col >= 0 && col < cols()); + return m_storage[row * cols() + col].value(); + } + + /** + * Returns a row of the variable column vector. + * + * @param index The index of the element to return. + * @return A row of the variable column vector. + */ + double value(int index) { + slp_assert(index >= 0 && index < rows() * cols()); + return m_storage[index].value(); + } + + /** + * Returns the contents of the variable matrix. + * + * @return The contents of the variable matrix. + */ + Eigen::MatrixXd value() { + Eigen::MatrixXd result{rows(), cols()}; + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + result(row, col) = value(row, col); + } + } + + return result; + } + + /** + * Transforms the matrix coefficient-wise with an unary operator. + * + * @param unary_op The unary operator to use for the transform operation. + * @return Result of the unary operator. + */ + VariableMatrix cwise_transform( + function_ref unary_op) const { + VariableMatrix result{VariableMatrix::empty, rows(), cols()}; + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + result(row, col) = unary_op((*this)(row, col)); + } + } + + return result; + } + +#ifndef DOXYGEN_SHOULD_SKIP_THIS + + class iterator { + public: + using iterator_category = std::forward_iterator_tag; + using value_type = Variable; + using difference_type = std::ptrdiff_t; + using pointer = Variable*; + using reference = Variable&; + + constexpr iterator() noexcept = default; + + explicit constexpr iterator( + gch::small_vector::iterator it) noexcept + : m_it{it} {} + + constexpr iterator& operator++() noexcept { + ++m_it; + return *this; + } + + constexpr iterator operator++(int) noexcept { + iterator retval = *this; + ++(*this); + return retval; + } + + constexpr bool operator==(const iterator&) const noexcept = default; + + constexpr reference operator*() const noexcept { return *m_it; } + + private: + gch::small_vector::iterator m_it; + }; + + class const_iterator { + public: + using iterator_category = std::forward_iterator_tag; + using value_type = Variable; + using difference_type = std::ptrdiff_t; + using pointer = Variable*; + using const_reference = const Variable&; + + constexpr const_iterator() noexcept = default; + + explicit constexpr const_iterator( + gch::small_vector::const_iterator it) noexcept + : m_it{it} {} + + constexpr const_iterator& operator++() noexcept { + ++m_it; + return *this; + } + + constexpr const_iterator operator++(int) noexcept { + const_iterator retval = *this; + ++(*this); + return retval; + } + + constexpr bool operator==(const const_iterator&) const noexcept = default; + + constexpr const_reference operator*() const noexcept { return *m_it; } + + private: + gch::small_vector::const_iterator m_it; + }; + +#endif // DOXYGEN_SHOULD_SKIP_THIS + + /** + * Returns begin iterator. + * + * @return Begin iterator. + */ + iterator begin() { return iterator{m_storage.begin()}; } + + /** + * Returns end iterator. + * + * @return End iterator. + */ + iterator end() { return iterator{m_storage.end()}; } + + /** + * Returns begin iterator. + * + * @return Begin iterator. + */ + const_iterator begin() const { return const_iterator{m_storage.begin()}; } + + /** + * Returns end iterator. + * + * @return End iterator. + */ + const_iterator end() const { return const_iterator{m_storage.end()}; } + + /** + * Returns begin iterator. + * + * @return Begin iterator. + */ + const_iterator cbegin() const { return const_iterator{m_storage.begin()}; } + + /** + * Returns end iterator. + * + * @return End iterator. + */ + const_iterator cend() const { return const_iterator{m_storage.end()}; } + + /** + * Returns number of elements in matrix. + * + * @return Number of elements in matrix. + */ + size_t size() const { return m_storage.size(); } + + /** + * Returns a variable matrix filled with zeroes. + * + * @param rows The number of matrix rows. + * @param cols The number of matrix columns. + * @return A variable matrix filled with zeroes. + */ + static VariableMatrix zero(int rows, int cols) { + VariableMatrix result{VariableMatrix::empty, rows, cols}; + + for (auto& elem : result) { + elem = 0.0; + } + + return result; + } + + /** + * Returns a variable matrix filled with ones. + * + * @param rows The number of matrix rows. + * @param cols The number of matrix columns. + * @return A variable matrix filled with ones. + */ + static VariableMatrix ones(int rows, int cols) { + VariableMatrix result{VariableMatrix::empty, rows, cols}; + + for (auto& elem : result) { + elem = 1.0; + } + + return result; + } + + private: + gch::small_vector m_storage; + int m_rows = 0; + int m_cols = 0; +}; + +/** + * Applies a coefficient-wise reduce operation to two matrices. + * + * @param lhs The left-hand side of the binary operator. + * @param rhs The right-hand side of the binary operator. + * @param binary_op The binary operator to use for the reduce operation. + */ +SLEIPNIR_DLLEXPORT inline VariableMatrix cwise_reduce( + const VariableMatrix& lhs, const VariableMatrix& rhs, + function_ref binary_op) { + slp_assert(lhs.rows() == rhs.rows() && lhs.rows() == rhs.rows()); + + VariableMatrix result{VariableMatrix::empty, lhs.rows(), lhs.cols()}; + + for (int row = 0; row < lhs.rows(); ++row) { + for (int col = 0; col < lhs.cols(); ++col) { + result(row, col) = binary_op(lhs(row, col), rhs(row, col)); + } + } + + return result; +} + +/** + * Assemble a VariableMatrix from a nested list of blocks. + * + * Each row's blocks must have the same height, and the assembled block rows + * must have the same width. For example, for the block matrix [[A, B], [C]] to + * be constructible, the number of rows in A and B must match, and the number of + * columns in [A, B] and [C] must match. + * + * @param list The nested list of blocks. + */ +SLEIPNIR_DLLEXPORT inline VariableMatrix block( + std::initializer_list> list) { + // Get row and column counts for destination matrix + int rows = 0; + int cols = -1; + for (const auto& row : list) { + if (row.size() > 0) { + rows += row.begin()->rows(); + } + + // Get number of columns in this row + int latest_cols = 0; + for (const auto& elem : row) { + // Assert the first and latest row have the same height + slp_assert(row.begin()->rows() == elem.rows()); + + latest_cols += elem.cols(); + } + + // If this is the first row, record the column count. Otherwise, assert the + // first and latest column counts are the same. + if (cols == -1) { + cols = latest_cols; + } else { + slp_assert(cols == latest_cols); + } + } + + VariableMatrix result{VariableMatrix::empty, rows, cols}; + + int row_offset = 0; + for (const auto& row : list) { + int col_offset = 0; + for (const auto& elem : row) { + result.block(row_offset, col_offset, elem.rows(), elem.cols()) = elem; + col_offset += elem.cols(); + } + row_offset += row.begin()->rows(); + } + + return result; +} + +/** + * Assemble a VariableMatrix from a nested list of blocks. + * + * Each row's blocks must have the same height, and the assembled block rows + * must have the same width. For example, for the block matrix [[A, B], [C]] to + * be constructible, the number of rows in A and B must match, and the number of + * columns in [A, B] and [C] must match. + * + * This overload is for Python bindings only. + * + * @param list The nested list of blocks. + */ +SLEIPNIR_DLLEXPORT inline VariableMatrix block( + const std::vector>& list) { + // Get row and column counts for destination matrix + int rows = 0; + int cols = -1; + for (const auto& row : list) { + if (row.size() > 0) { + rows += row.begin()->rows(); + } + + // Get number of columns in this row + int latest_cols = 0; + for (const auto& elem : row) { + // Assert the first and latest row have the same height + slp_assert(row.begin()->rows() == elem.rows()); + + latest_cols += elem.cols(); + } + + // If this is the first row, record the column count. Otherwise, assert the + // first and latest column counts are the same. + if (cols == -1) { + cols = latest_cols; + } else { + slp_assert(cols == latest_cols); + } + } + + VariableMatrix result{VariableMatrix::empty, rows, cols}; + + int row_offset = 0; + for (const auto& row : list) { + int col_offset = 0; + for (const auto& elem : row) { + result.block(row_offset, col_offset, elem.rows(), elem.cols()) = elem; + col_offset += elem.cols(); + } + row_offset += row.begin()->rows(); + } + + return result; +} + +/** + * Solves the VariableMatrix equation AX = B for X. + * + * @param A The left-hand side. + * @param B The right-hand side. + * @return The solution X. + */ +SLEIPNIR_DLLEXPORT VariableMatrix solve(const VariableMatrix& A, + const VariableMatrix& B); + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/OCPSolver.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/ocp.hpp similarity index 50% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/OCPSolver.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/ocp.hpp index 663e81d9ba..d917442666 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/OCPSolver.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/ocp.hpp @@ -7,14 +7,14 @@ #include #include -#include "sleipnir/autodiff/VariableMatrix.hpp" -#include "sleipnir/optimization/OptimizationProblem.hpp" -#include "sleipnir/util/Assert.hpp" -#include "sleipnir/util/Concepts.hpp" -#include "sleipnir/util/FunctionRef.hpp" -#include "sleipnir/util/SymbolExports.hpp" +#include "sleipnir/autodiff/variable_matrix.hpp" +#include "sleipnir/optimization/problem.hpp" +#include "sleipnir/util/assert.hpp" +#include "sleipnir/util/concepts.hpp" +#include "sleipnir/util/function_ref.hpp" +#include "sleipnir/util/symbol_exports.hpp" -namespace sleipnir { +namespace slp { /** * Performs 4th order Runge-Kutta integration of dx/dt = f(t, x, u) for dt. @@ -26,7 +26,7 @@ namespace sleipnir { * @param dt The time over which to integrate. */ template -State RK4(F&& f, State x, Input u, Time t0, Time dt) { +State rk4(F&& f, State x, Input u, Time t0, Time dt) { auto halfdt = dt * 0.5; State k1 = f(t0, x, u, dt); State k2 = f(t0 + halfdt, x + k1 * halfdt, u, dt); @@ -42,13 +42,13 @@ State RK4(F&& f, State x, Input u, Time t0, Time dt) { enum class TranscriptionMethod : uint8_t { /// Each state is a decision variable constrained to the integrated dynamics /// of the previous state. - kDirectTranscription, + DIRECT_TRANSCRIPTION, /// The trajectory is modeled as a series of cubic polynomials where the /// centerpoint slope is constrained. - kDirectCollocation, + DIRECT_COLLOCATION, /// States depend explicitly as a function of all previous states and all /// previous inputs. - kSingleShooting + SINGLE_SHOOTING }; /** @@ -56,9 +56,9 @@ enum class TranscriptionMethod : uint8_t { */ enum class DynamicsType : uint8_t { /// The dynamics are a function in the form dx/dt = f(t, x, u). - kExplicitODE, + EXPLICIT_ODE, /// The dynamics are a function in the form xₖ₊₁ = f(t, xₖ, uₖ). - kDiscrete + DISCRETE }; /** @@ -66,12 +66,12 @@ enum class DynamicsType : uint8_t { */ enum class TimestepMethod : uint8_t { /// The timestep is a fixed constant. - kFixed, + FIXED, /// The timesteps are allowed to vary as independent decision variables. - kVariable, + VARIABLE, /// The timesteps are equal length but allowed to vary as a single decision /// variable. - kVariableSingle + VARIABLE_SINGLE }; /** @@ -100,179 +100,177 @@ enum class TimestepMethod : uint8_t { * https://underactuated.mit.edu/trajopt.html goes into more detail on each * transcription method. */ -class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem { +class SLEIPNIR_DLLEXPORT OCP : public Problem { public: /** * Build an optimization problem using a system evolution function (explicit * ODE or discrete state transition function). * - * @param numStates The number of system states. - * @param numInputs The number of system inputs. + * @param num_states The number of system states. + * @param num_inputs The number of system inputs. * @param dt The timestep for fixed-step integration. - * @param numSteps The number of control points. + * @param num_steps The number of control points. * @param dynamics Function representing an explicit or implicit ODE, or a * discrete state transition function. * - Explicit: dx/dt = f(x, u, *) * - Implicit: f([x dx/dt]', u, *) = 0 * - State transition: xₖ₊₁ = f(xₖ, uₖ) - * @param dynamicsType The type of system evolution function. - * @param timestepMethod The timestep method. + * @param dynamics_type The type of system evolution function. + * @param timestep_method The timestep method. * @param method The transcription method. */ - OCPSolver( - int numStates, int numInputs, std::chrono::duration dt, - int numSteps, + OCP(int num_states, int num_inputs, std::chrono::duration dt, + int num_steps, function_ref dynamics, - DynamicsType dynamicsType = DynamicsType::kExplicitODE, - TimestepMethod timestepMethod = TimestepMethod::kFixed, - TranscriptionMethod method = TranscriptionMethod::kDirectTranscription) - : OCPSolver{numStates, - numInputs, - dt, - numSteps, - [=]([[maybe_unused]] const VariableMatrix& t, - const VariableMatrix& x, const VariableMatrix& u, - [[maybe_unused]] - const VariableMatrix& dt) -> VariableMatrix { - return dynamics(x, u); - }, - dynamicsType, - timestepMethod, - method} {} + DynamicsType dynamics_type = DynamicsType::EXPLICIT_ODE, + TimestepMethod timestep_method = TimestepMethod::FIXED, + TranscriptionMethod method = TranscriptionMethod::DIRECT_TRANSCRIPTION) + : OCP{num_states, + num_inputs, + dt, + num_steps, + [=]([[maybe_unused]] const VariableMatrix& t, + const VariableMatrix& x, const VariableMatrix& u, + [[maybe_unused]] + const VariableMatrix& dt) -> VariableMatrix { + return dynamics(x, u); + }, + dynamics_type, + timestep_method, + method} {} /** * Build an optimization problem using a system evolution function (explicit * ODE or discrete state transition function). * - * @param numStates The number of system states. - * @param numInputs The number of system inputs. + * @param num_states The number of system states. + * @param num_inputs The number of system inputs. * @param dt The timestep for fixed-step integration. - * @param numSteps The number of control points. + * @param num_steps The number of control points. * @param dynamics Function representing an explicit or implicit ODE, or a * discrete state transition function. * - Explicit: dx/dt = f(t, x, u, *) * - Implicit: f(t, [x dx/dt]', u, *) = 0 * - State transition: xₖ₊₁ = f(t, xₖ, uₖ, dt) - * @param dynamicsType The type of system evolution function. - * @param timestepMethod The timestep method. + * @param dynamics_type The type of system evolution function. + * @param timestep_method The timestep method. * @param method The transcription method. */ - OCPSolver( - int numStates, int numInputs, std::chrono::duration dt, - int numSteps, + OCP(int num_states, int num_inputs, std::chrono::duration dt, + int num_steps, function_ref dynamics, - DynamicsType dynamicsType = DynamicsType::kExplicitODE, - TimestepMethod timestepMethod = TimestepMethod::kFixed, - TranscriptionMethod method = TranscriptionMethod::kDirectTranscription) - : m_numStates{numStates}, - m_numInputs{numInputs}, + DynamicsType dynamics_type = DynamicsType::EXPLICIT_ODE, + TimestepMethod timestep_method = TimestepMethod::FIXED, + TranscriptionMethod method = TranscriptionMethod::DIRECT_TRANSCRIPTION) + : m_num_states{num_states}, + m_num_inputs{num_inputs}, m_dt{dt}, - m_numSteps{numSteps}, - m_transcriptionMethod{method}, - m_dynamicsType{dynamicsType}, - m_dynamicsFunction{std::move(dynamics)}, - m_timestepMethod{timestepMethod} { - // u is numSteps + 1 so that the final constraintFunction evaluation works - m_U = DecisionVariable(m_numInputs, m_numSteps + 1); + m_num_steps{num_steps}, + m_transcription_method{method}, + m_dynamics_type{dynamics_type}, + m_dynamics_function{std::move(dynamics)}, + m_timestep_method{timestep_method} { + // u is num_steps + 1 so that the final constraint function evaluation works + m_U = decision_variable(m_num_inputs, m_num_steps + 1); - if (m_timestepMethod == TimestepMethod::kFixed) { - m_DT = VariableMatrix{1, m_numSteps + 1}; - for (int i = 0; i < numSteps + 1; ++i) { + if (m_timestep_method == TimestepMethod::FIXED) { + m_DT = VariableMatrix{1, m_num_steps + 1}; + for (int i = 0; i < num_steps + 1; ++i) { m_DT(0, i) = m_dt.count(); } - } else if (m_timestepMethod == TimestepMethod::kVariableSingle) { - Variable DT = DecisionVariable(); - DT.SetValue(m_dt.count()); + } else if (m_timestep_method == TimestepMethod::VARIABLE_SINGLE) { + Variable dt = decision_variable(); + dt.set_value(m_dt.count()); // Set the member variable matrix to track the decision variable - m_DT = VariableMatrix{1, m_numSteps + 1}; - for (int i = 0; i < numSteps + 1; ++i) { - m_DT(0, i) = DT; + m_DT = VariableMatrix{1, m_num_steps + 1}; + for (int i = 0; i < num_steps + 1; ++i) { + m_DT(0, i) = dt; } - } else if (m_timestepMethod == TimestepMethod::kVariable) { - m_DT = DecisionVariable(1, m_numSteps + 1); - for (int i = 0; i < numSteps + 1; ++i) { - m_DT(0, i).SetValue(m_dt.count()); + } else if (m_timestep_method == TimestepMethod::VARIABLE) { + m_DT = decision_variable(1, m_num_steps + 1); + for (int i = 0; i < num_steps + 1; ++i) { + m_DT(0, i).set_value(m_dt.count()); } } - if (m_transcriptionMethod == TranscriptionMethod::kDirectTranscription) { - m_X = DecisionVariable(m_numStates, m_numSteps + 1); - ConstrainDirectTranscription(); - } else if (m_transcriptionMethod == - TranscriptionMethod::kDirectCollocation) { - m_X = DecisionVariable(m_numStates, m_numSteps + 1); - ConstrainDirectCollocation(); - } else if (m_transcriptionMethod == TranscriptionMethod::kSingleShooting) { + if (m_transcription_method == TranscriptionMethod::DIRECT_TRANSCRIPTION) { + m_X = decision_variable(m_num_states, m_num_steps + 1); + constrain_direct_transcription(); + } else if (m_transcription_method == + TranscriptionMethod::DIRECT_COLLOCATION) { + m_X = decision_variable(m_num_states, m_num_steps + 1); + constrain_direct_collocation(); + } else if (m_transcription_method == TranscriptionMethod::SINGLE_SHOOTING) { // In single-shooting the states aren't decision variables, but instead // depend on the input and previous states - m_X = VariableMatrix{m_numStates, m_numSteps + 1}; - ConstrainSingleShooting(); + m_X = VariableMatrix{m_num_states, m_num_steps + 1}; + constrain_single_shooting(); } } /** * Utility function to constrain the initial state. * - * @param initialState the initial state to constrain to. + * @param initial_state the initial state to constrain to. */ template requires ScalarLike || MatrixLike - void ConstrainInitialState(const T& initialState) { - SubjectTo(InitialState() == initialState); + void constrain_initial_state(const T& initial_state) { + subject_to(this->initial_state() == initial_state); } /** * Utility function to constrain the final state. * - * @param finalState the final state to constrain to. + * @param final_state the final state to constrain to. */ template requires ScalarLike || MatrixLike - void ConstrainFinalState(const T& finalState) { - SubjectTo(FinalState() == finalState); + void constrain_final_state(const T& final_state) { + subject_to(this->final_state() == final_state); } /** * Set the constraint evaluation function. This function is called - * `numSteps+1` times, with the corresponding state and input + * `num_steps+1` times, with the corresponding state and input * VariableMatrices. * * @param callback The callback f(x, u) where x is the state and u is the * input vector. */ - void ForEachStep( + void for_each_step( const function_ref callback) { - for (int i = 0; i < m_numSteps + 1; ++i) { - auto x = X().Col(i); - auto u = U().Col(i); + for (int i = 0; i < m_num_steps + 1; ++i) { + auto x = X().col(i); + auto u = U().col(i); callback(x, u); } } /** * Set the constraint evaluation function. This function is called - * `numSteps+1` times, with the corresponding state and input + * `num_steps+1` times, with the corresponding state and input * VariableMatrices. * * @param callback The callback f(t, x, u, dt) where t is time, x is the state * vector, u is the input vector, and dt is the timestep duration. */ - void ForEachStep( + void for_each_step( const function_ref callback) { Variable time = 0.0; - for (int i = 0; i < m_numSteps + 1; ++i) { - auto x = X().Col(i); - auto u = U().Col(i); - auto dt = DT()(0, i); + for (int i = 0; i < m_num_steps + 1; ++i) { + auto x = X().col(i); + auto u = U().col(i); + auto dt = this->dt()(0, i); callback(time, x, u, dt); time += dt; @@ -282,115 +280,115 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem { /** * Convenience function to set a lower bound on the input. * - * @param lowerBound The lower bound that inputs must always be above. Must be - * shaped (numInputs)x1. + * @param lower_bound The lower bound that inputs must always be above. Must + * be shaped (num_inputs)x1. */ template requires ScalarLike || MatrixLike - void SetLowerInputBound(const T& lowerBound) { - for (int i = 0; i < m_numSteps + 1; ++i) { - SubjectTo(U().Col(i) >= lowerBound); + void set_lower_input_bound(const T& lower_bound) { + for (int i = 0; i < m_num_steps + 1; ++i) { + subject_to(U().col(i) >= lower_bound); } } /** * Convenience function to set an upper bound on the input. * - * @param upperBound The upper bound that inputs must always be below. Must be - * shaped (numInputs)x1. + * @param upper_bound The upper bound that inputs must always be below. Must + * be shaped (num_inputs)x1. */ template requires ScalarLike || MatrixLike - void SetUpperInputBound(const T& upperBound) { - for (int i = 0; i < m_numSteps + 1; ++i) { - SubjectTo(U().Col(i) <= upperBound); + void set_upper_input_bound(const T& upper_bound) { + for (int i = 0; i < m_num_steps + 1; ++i) { + subject_to(U().col(i) <= upper_bound); } } /** * Convenience function to set a lower bound on the timestep. * - * @param minTimestep The minimum timestep. + * @param min_timestep The minimum timestep. */ - void SetMinTimestep(std::chrono::duration minTimestep) { - SubjectTo(DT() >= minTimestep.count()); + void set_min_timestep(std::chrono::duration min_timestep) { + subject_to(dt() >= min_timestep.count()); } /** * Convenience function to set an upper bound on the timestep. * - * @param maxTimestep The maximum timestep. + * @param max_timestep The maximum timestep. */ - void SetMaxTimestep(std::chrono::duration maxTimestep) { - SubjectTo(DT() <= maxTimestep.count()); + void set_max_timestep(std::chrono::duration max_timestep) { + subject_to(dt() <= max_timestep.count()); } /** * Get the state variables. After the problem is solved, this will contain the * optimized trajectory. * - * Shaped (numStates)x(numSteps+1). + * Shaped (num_states)x(num_steps+1). * - * @returns The state variable matrix. + * @return The state variable matrix. */ - VariableMatrix& X() { return m_X; }; + VariableMatrix& X() { return m_X; } /** * Get the input variables. After the problem is solved, this will contain the * inputs corresponding to the optimized trajectory. * - * Shaped (numInputs)x(numSteps+1), although the last input step is unused in - * the trajectory. + * Shaped (num_inputs)x(num_steps+1), although the last input step is unused + * in the trajectory. * - * @returns The input variable matrix. + * @return The input variable matrix. */ - VariableMatrix& U() { return m_U; }; + VariableMatrix& U() { return m_U; } /** * Get the timestep variables. After the problem is solved, this will contain * the timesteps corresponding to the optimized trajectory. * - * Shaped 1x(numSteps+1), although the last timestep is unused in + * Shaped 1x(num_steps+1), although the last timestep is unused in * the trajectory. * - * @returns The timestep variable matrix. + * @return The timestep variable matrix. */ - VariableMatrix& DT() { return m_DT; }; + VariableMatrix& dt() { return m_DT; } /** * Convenience function to get the initial state in the trajectory. * - * @returns The initial state of the trajectory. + * @return The initial state of the trajectory. */ - VariableMatrix InitialState() { return m_X.Col(0); } + VariableMatrix initial_state() { return m_X.col(0); } /** * Convenience function to get the final state in the trajectory. * - * @returns The final state of the trajectory. + * @return The final state of the trajectory. */ - VariableMatrix FinalState() { return m_X.Col(m_numSteps); } + VariableMatrix final_state() { return m_X.col(m_num_steps); } private: - void ConstrainDirectCollocation() { - Assert(m_dynamicsType == DynamicsType::kExplicitODE); + void constrain_direct_collocation() { + slp_assert(m_dynamics_type == DynamicsType::EXPLICIT_ODE); Variable time = 0.0; // Derivation at https://mec560sbu.github.io/2016/09/30/direct_collocation/ - for (int i = 0; i < m_numSteps; ++i) { - Variable h = DT()(0, i); + for (int i = 0; i < m_num_steps; ++i) { + Variable h = dt()(0, i); - auto& f = m_dynamicsFunction; + auto& f = m_dynamics_function; auto t_begin = time; auto t_end = t_begin + h; - auto x_begin = X().Col(i); - auto x_end = X().Col(i + 1); + auto x_begin = X().col(i); + auto x_end = X().col(i + 1); - auto u_begin = U().Col(i); - auto u_end = U().Col(i + 1); + auto u_begin = U().col(i); + auto u_end = U().col(i + 1); auto xdot_begin = f(t_begin, x_begin, u_begin, h); auto xdot_end = f(t_end, x_end, u_end, h); @@ -401,71 +399,71 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem { auto x_c = 0.5 * (x_begin + x_end) + h / 8 * (xdot_begin - xdot_end); auto u_c = 0.5 * (u_begin + u_end); - SubjectTo(xdot_c == f(t_c, x_c, u_c, h)); + subject_to(xdot_c == f(t_c, x_c, u_c, h)); time += h; } } - void ConstrainDirectTranscription() { + void constrain_direct_transcription() { Variable time = 0.0; - for (int i = 0; i < m_numSteps; ++i) { - auto x_begin = X().Col(i); - auto x_end = X().Col(i + 1); - auto u = U().Col(i); - Variable dt = DT()(0, i); + for (int i = 0; i < m_num_steps; ++i) { + auto x_begin = X().col(i); + auto x_end = X().col(i + 1); + auto u = U().col(i); + Variable dt = this->dt()(0, i); - if (m_dynamicsType == DynamicsType::kExplicitODE) { - SubjectTo(x_end == RK4( - m_dynamicsFunction, x_begin, u, time, dt)); - } else if (m_dynamicsType == DynamicsType::kDiscrete) { - SubjectTo(x_end == m_dynamicsFunction(time, x_begin, u, dt)); + if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { + subject_to(x_end == rk4( + m_dynamics_function, x_begin, u, time, dt)); + } else if (m_dynamics_type == DynamicsType::DISCRETE) { + subject_to(x_end == m_dynamics_function(time, x_begin, u, dt)); } time += dt; } } - void ConstrainSingleShooting() { + void constrain_single_shooting() { Variable time = 0.0; - for (int i = 0; i < m_numSteps; ++i) { - auto x_begin = X().Col(i); - auto x_end = X().Col(i + 1); - auto u = U().Col(i); - Variable dt = DT()(0, i); + for (int i = 0; i < m_num_steps; ++i) { + auto x_begin = X().col(i); + auto x_end = X().col(i + 1); + auto u = U().col(i); + Variable dt = this->dt()(0, i); - if (m_dynamicsType == DynamicsType::kExplicitODE) { - x_end = RK4(m_dynamicsFunction, x_begin, u, + if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { + x_end = rk4(m_dynamics_function, x_begin, u, time, dt); - } else if (m_dynamicsType == DynamicsType::kDiscrete) { - x_end = m_dynamicsFunction(time, x_begin, u, dt); + } else if (m_dynamics_type == DynamicsType::DISCRETE) { + x_end = m_dynamics_function(time, x_begin, u, dt); } time += dt; } } - int m_numStates; - int m_numInputs; + int m_num_states; + int m_num_inputs; std::chrono::duration m_dt; - int m_numSteps; - TranscriptionMethod m_transcriptionMethod; + int m_num_steps; + TranscriptionMethod m_transcription_method; - DynamicsType m_dynamicsType; + DynamicsType m_dynamics_type; function_ref - m_dynamicsFunction; + m_dynamics_function; - TimestepMethod m_timestepMethod; + TimestepMethod m_timestep_method; VariableMatrix m_X; VariableMatrix m_U; VariableMatrix m_DT; }; -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp deleted file mode 100644 index 66883fed98..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp +++ /dev/null @@ -1,386 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/autodiff/VariableMatrix.hpp" -#include "sleipnir/optimization/SolverConfig.hpp" -#include "sleipnir/optimization/SolverExitCondition.hpp" -#include "sleipnir/optimization/SolverIterationInfo.hpp" -#include "sleipnir/optimization/SolverStatus.hpp" -#include "sleipnir/optimization/solver/InteriorPoint.hpp" -#include "sleipnir/optimization/solver/SQP.hpp" -#include "sleipnir/util/Print.hpp" -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** - * This class allows the user to pose a constrained nonlinear optimization - * problem in natural mathematical notation and solve it. - * - * This class supports problems of the form: -@verbatim - minₓ f(x) -subject to cₑ(x) = 0 - cᵢ(x) ≥ 0 -@endverbatim - * - * where f(x) is the scalar cost function, x is the vector of decision variables - * (variables the solver can tweak to minimize the cost function), cᵢ(x) are the - * inequality constraints, and cₑ(x) are the equality constraints. Constraints - * are equations or inequalities of the decision variables that constrain what - * values the solver is allowed to use when searching for an optimal solution. - * - * The nice thing about this class is users don't have to put their system in - * the form shown above manually; they can write it in natural mathematical form - * and it'll be converted for them. - */ -class SLEIPNIR_DLLEXPORT OptimizationProblem { - public: - /** - * Construct the optimization problem. - */ - OptimizationProblem() noexcept = default; - - /** - * Create a decision variable in the optimization problem. - */ - [[nodiscard]] - Variable DecisionVariable() { - m_decisionVariables.emplace_back(); - return m_decisionVariables.back(); - } - - /** - * Create a matrix of decision variables in the optimization problem. - * - * @param rows Number of matrix rows. - * @param cols Number of matrix columns. - */ - [[nodiscard]] - VariableMatrix DecisionVariable(int rows, int cols = 1) { - m_decisionVariables.reserve(m_decisionVariables.size() + rows * cols); - - VariableMatrix vars{rows, cols}; - - for (int row = 0; row < rows; ++row) { - for (int col = 0; col < cols; ++col) { - m_decisionVariables.emplace_back(); - vars(row, col) = m_decisionVariables.back(); - } - } - - return vars; - } - - /** - * Create a symmetric matrix of decision variables in the optimization - * problem. - * - * Variable instances are reused across the diagonal, which helps reduce - * problem dimensionality. - * - * @param rows Number of matrix rows. - */ - [[nodiscard]] - VariableMatrix SymmetricDecisionVariable(int rows) { - // We only need to store the lower triangle of an n x n symmetric matrix; - // the other elements are duplicates. The lower triangle has (n² + n)/2 - // elements. - // - // n - // Σ k = (n² + n)/2 - // k=1 - m_decisionVariables.reserve(m_decisionVariables.size() + - (rows * rows + rows) / 2); - - VariableMatrix vars{rows, rows}; - - for (int row = 0; row < rows; ++row) { - for (int col = 0; col <= row; ++col) { - m_decisionVariables.emplace_back(); - vars(row, col) = m_decisionVariables.back(); - vars(col, row) = m_decisionVariables.back(); - } - } - - return vars; - } - - /** - * Tells the solver to minimize the output of the given cost function. - * - * Note that this is optional. If only constraints are specified, the solver - * will find the closest solution to the initial conditions that's in the - * feasible set. - * - * @param cost The cost function to minimize. - */ - void Minimize(const Variable& cost) { - m_f = cost; - status.costFunctionType = m_f.value().Type(); - } - - /** - * Tells the solver to minimize the output of the given cost function. - * - * Note that this is optional. If only constraints are specified, the solver - * will find the closest solution to the initial conditions that's in the - * feasible set. - * - * @param cost The cost function to minimize. - */ - void Minimize(Variable&& cost) { - m_f = std::move(cost); - status.costFunctionType = m_f.value().Type(); - } - - /** - * Tells the solver to maximize the output of the given objective function. - * - * Note that this is optional. If only constraints are specified, the solver - * will find the closest solution to the initial conditions that's in the - * feasible set. - * - * @param objective The objective function to maximize. - */ - void Maximize(const Variable& objective) { - // Maximizing a cost function is the same as minimizing its negative - m_f = -objective; - status.costFunctionType = m_f.value().Type(); - } - - /** - * Tells the solver to maximize the output of the given objective function. - * - * Note that this is optional. If only constraints are specified, the solver - * will find the closest solution to the initial conditions that's in the - * feasible set. - * - * @param objective The objective function to maximize. - */ - void Maximize(Variable&& objective) { - // Maximizing a cost function is the same as minimizing its negative - m_f = -std::move(objective); - status.costFunctionType = m_f.value().Type(); - } - - /** - * Tells the solver to solve the problem while satisfying the given equality - * constraint. - * - * @param constraint The constraint to satisfy. - */ - void SubjectTo(const EqualityConstraints& constraint) { - // Get the highest order equality constraint expression type - for (const auto& c : constraint.constraints) { - status.equalityConstraintType = - std::max(status.equalityConstraintType, c.Type()); - } - - m_equalityConstraints.reserve(m_equalityConstraints.size() + - constraint.constraints.size()); - std::copy(constraint.constraints.begin(), constraint.constraints.end(), - std::back_inserter(m_equalityConstraints)); - } - - /** - * Tells the solver to solve the problem while satisfying the given equality - * constraint. - * - * @param constraint The constraint to satisfy. - */ - void SubjectTo(EqualityConstraints&& constraint) { - // Get the highest order equality constraint expression type - for (const auto& c : constraint.constraints) { - status.equalityConstraintType = - std::max(status.equalityConstraintType, c.Type()); - } - - m_equalityConstraints.reserve(m_equalityConstraints.size() + - constraint.constraints.size()); - std::copy(constraint.constraints.begin(), constraint.constraints.end(), - std::back_inserter(m_equalityConstraints)); - } - - /** - * Tells the solver to solve the problem while satisfying the given inequality - * constraint. - * - * @param constraint The constraint to satisfy. - */ - void SubjectTo(const InequalityConstraints& constraint) { - // Get the highest order inequality constraint expression type - for (const auto& c : constraint.constraints) { - status.inequalityConstraintType = - std::max(status.inequalityConstraintType, c.Type()); - } - - m_inequalityConstraints.reserve(m_inequalityConstraints.size() + - constraint.constraints.size()); - std::copy(constraint.constraints.begin(), constraint.constraints.end(), - std::back_inserter(m_inequalityConstraints)); - } - - /** - * Tells the solver to solve the problem while satisfying the given inequality - * constraint. - * - * @param constraint The constraint to satisfy. - */ - void SubjectTo(InequalityConstraints&& constraint) { - // Get the highest order inequality constraint expression type - for (const auto& c : constraint.constraints) { - status.inequalityConstraintType = - std::max(status.inequalityConstraintType, c.Type()); - } - - m_inequalityConstraints.reserve(m_inequalityConstraints.size() + - constraint.constraints.size()); - std::copy(constraint.constraints.begin(), constraint.constraints.end(), - std::back_inserter(m_inequalityConstraints)); - } - - /** - * Solve the optimization problem. The solution will be stored in the original - * variables used to construct the problem. - * - * @param config Configuration options for the solver. - */ - SolverStatus Solve(const SolverConfig& config = SolverConfig{}) { - // Create the initial value column vector - Eigen::VectorXd x{m_decisionVariables.size()}; - for (size_t i = 0; i < m_decisionVariables.size(); ++i) { - x(i) = m_decisionVariables[i].Value(); - } - - status.exitCondition = SolverExitCondition::kSuccess; - - // If there's no cost function, make it zero and continue - if (!m_f.has_value()) { - m_f = Variable(); - } - - if (config.diagnostics) { - constexpr std::array kExprTypeToName{"empty", "constant", "linear", - "quadratic", "nonlinear"}; - - // Print cost function and constraint expression types - sleipnir::println( - "The cost function is {}.", - kExprTypeToName[static_cast(status.costFunctionType)]); - sleipnir::println( - "The equality constraints are {}.", - kExprTypeToName[static_cast(status.equalityConstraintType)]); - sleipnir::println( - "The inequality constraints are {}.", - kExprTypeToName[static_cast(status.inequalityConstraintType)]); - sleipnir::println(""); - - // Print problem dimensionality - sleipnir::println("Number of decision variables: {}", - m_decisionVariables.size()); - sleipnir::println("Number of equality constraints: {}", - m_equalityConstraints.size()); - sleipnir::println("Number of inequality constraints: {}\n", - m_inequalityConstraints.size()); - } - - // If the problem is empty or constant, there's nothing to do - if (status.costFunctionType <= ExpressionType::kConstant && - status.equalityConstraintType <= ExpressionType::kConstant && - status.inequalityConstraintType <= ExpressionType::kConstant) { - return status; - } - - // Solve the optimization problem - if (m_inequalityConstraints.empty()) { - SQP(m_decisionVariables, m_equalityConstraints, m_f.value(), m_callback, - config, x, &status); - } else { - Eigen::VectorXd s = Eigen::VectorXd::Ones(m_inequalityConstraints.size()); - InteriorPoint(m_decisionVariables, m_equalityConstraints, - m_inequalityConstraints, m_f.value(), m_callback, config, - false, x, s, &status); - } - - if (config.diagnostics) { - sleipnir::println("Exit condition: {}", ToMessage(status.exitCondition)); - } - - // Assign the solution to the original Variable instances - VariableMatrix{m_decisionVariables}.SetValue(x); - - return status; - } - - /** - * Sets a callback to be called at each solver iteration. - * - * The callback for this overload should return void. - * - * @param callback The callback. - */ - template - requires requires(F callback, const SolverIterationInfo& info) { - { callback(info) } -> std::same_as; - } - void Callback(F&& callback) { - m_callback = [=, callback = std::forward(callback)]( - const SolverIterationInfo& info) { - callback(info); - return false; - }; - } - - /** - * Sets a callback to be called at each solver iteration. - * - * The callback for this overload should return bool. - * - * @param callback The callback. Returning true from the callback causes the - * solver to exit early with the solution it has so far. - */ - template - requires requires(F callback, const SolverIterationInfo& info) { - { callback(info) } -> std::same_as; - } - void Callback(F&& callback) { - m_callback = std::forward(callback); - } - - private: - // The list of decision variables, which are the root of the problem's - // expression tree - wpi::SmallVector m_decisionVariables; - - // The cost function: f(x) - std::optional m_f; - - // The list of equality constraints: cₑ(x) = 0 - wpi::SmallVector m_equalityConstraints; - - // The list of inequality constraints: cᵢ(x) ≥ 0 - wpi::SmallVector m_inequalityConstraints; - - // The user callback - std::function m_callback = - [](const SolverIterationInfo&) { return false; }; - - // The solver status - SolverStatus status; -}; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverConfig.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverConfig.hpp deleted file mode 100644 index f7323f7388..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverConfig.hpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include -#include - -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** - * Solver configuration. - */ -struct SLEIPNIR_DLLEXPORT SolverConfig { - /// The solver will stop once the error is below this tolerance. - double tolerance = 1e-8; - - /// The maximum number of solver iterations before returning a solution. - int maxIterations = 5000; - - /// The solver will stop once the error is below this tolerance for - /// `acceptableIterations` iterations. This is useful in cases where the - /// solver might not be able to achieve the desired level of accuracy due to - /// floating-point round-off. - double acceptableTolerance = 1e-6; - - /// The solver will stop once the error is below `acceptableTolerance` for - /// this many iterations. - int maxAcceptableIterations = 15; - - /// The maximum elapsed wall clock time before returning a solution. - std::chrono::duration timeout{ - std::numeric_limits::infinity()}; - - /// Enables the feasible interior-point method. When the inequality - /// constraints are all feasible, step sizes are reduced when necessary to - /// prevent them becoming infeasible again. This is useful when parts of the - /// problem are ill-conditioned in infeasible regions (e.g., square root of a - /// negative value). This can slow or prevent progress toward a solution - /// though, so only enable it if necessary. - bool feasibleIPM = false; - - /// Enables diagnostic prints. - bool diagnostics = false; - - /// Enables writing sparsity patterns of H, Aₑ, and Aᵢ to files named H.spy, - /// A_e.spy, and A_i.spy respectively during solve. - /// - /// Use tools/spy.py to plot them. - bool spy = false; -}; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp deleted file mode 100644 index 7d1445297e..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp +++ /dev/null @@ -1,80 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -#include - -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** - * Solver exit condition. - */ -enum class SolverExitCondition : int8_t { - /// Solved the problem to the desired tolerance. - kSuccess = 0, - /// Solved the problem to an acceptable tolerance, but not the desired one. - kSolvedToAcceptableTolerance = 1, - /// The solver returned its solution so far after the user requested a stop. - kCallbackRequestedStop = 2, - /// The solver determined the problem to be overconstrained and gave up. - kTooFewDOFs = -1, - /// The solver determined the problem to be locally infeasible and gave up. - kLocallyInfeasible = -2, - /// The solver failed to reach the desired tolerance, and feasibility - /// restoration failed to converge. - kFeasibilityRestorationFailed = -3, - /// The solver encountered nonfinite initial cost or constraints and gave up. - kNonfiniteInitialCostOrConstraints = -4, - /// The solver encountered diverging primal iterates xₖ and/or sₖ and gave up. - kDivergingIterates = -5, - /// The solver returned its solution so far after exceeding the maximum number - /// of iterations. - kMaxIterationsExceeded = -6, - /// The solver returned its solution so far after exceeding the maximum - /// elapsed wall clock time. - kTimeout = -7 -}; - -/** - * Returns user-readable message corresponding to the exit condition. - * - * @param exitCondition Solver exit condition. - */ -SLEIPNIR_DLLEXPORT constexpr std::string_view ToMessage( - const SolverExitCondition& exitCondition) { - using enum SolverExitCondition; - - switch (exitCondition) { - case kSuccess: - return "solved to desired tolerance"; - case kSolvedToAcceptableTolerance: - return "solved to acceptable tolerance"; - case kCallbackRequestedStop: - return "callback requested stop"; - case kTooFewDOFs: - return "problem has too few degrees of freedom"; - case kLocallyInfeasible: - return "problem is locally infeasible"; - case kFeasibilityRestorationFailed: - return "solver failed to reach the desired tolerance, and feasibility " - "restoration failed to converge"; - case kNonfiniteInitialCostOrConstraints: - return "solver encountered nonfinite initial cost or constraints and " - "gave up"; - case kDivergingIterates: - return "solver encountered diverging primal iterates xₖ and/or sₖ and " - "gave up"; - case kMaxIterationsExceeded: - return "solution returned after maximum iterations exceeded"; - case kTimeout: - return "solution returned after maximum wall clock time exceeded"; - default: - return "unknown"; - } -} - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverStatus.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverStatus.hpp deleted file mode 100644 index 122941cda7..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverStatus.hpp +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include "sleipnir/autodiff/ExpressionType.hpp" -#include "sleipnir/optimization/SolverExitCondition.hpp" -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** - * Return value of OptimizationProblem::Solve() containing the cost function and - * constraint types and solver's exit condition. - */ -struct SLEIPNIR_DLLEXPORT SolverStatus { - /// The cost function type detected by the solver. - ExpressionType costFunctionType = ExpressionType::kNone; - - /// The equality constraint type detected by the solver. - ExpressionType equalityConstraintType = ExpressionType::kNone; - - /// The inequality constraint type detected by the solver. - ExpressionType inequalityConstraintType = ExpressionType::kNone; - - /// The solver's exit condition. - SolverExitCondition exitCondition = SolverExitCondition::kSuccess; - - /// The solution's cost. - double cost = 0.0; -}; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Multistart.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/multistart.hpp similarity index 54% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Multistart.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/multistart.hpp index 09b1b2f3bf..ebe177ef21 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Multistart.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/multistart.hpp @@ -6,12 +6,12 @@ #include #include -#include +#include -#include "sleipnir/optimization/SolverStatus.hpp" -#include "sleipnir/util/FunctionRef.hpp" +#include "sleipnir/optimization/solver/exit_status.hpp" +#include "sleipnir/util/function_ref.hpp" -namespace sleipnir { +namespace slp { /** * The result of a multistart solve. @@ -21,7 +21,11 @@ namespace sleipnir { */ template struct MultistartResult { - SolverStatus status; + /// The solver exit status. + ExitStatus status; + /// The solution's cost. + double cost; + /// The decision variables. DecisionVariables variables; }; @@ -37,39 +41,37 @@ struct MultistartResult { * guess. * @param solve A user-provided function that takes a decision variable initial * guess and returns a MultistartResult. - * @param initialGuesses A list of decision variable initial guesses to try. + * @param initial_guesses A list of decision variable initial guesses to try. */ template MultistartResult Multistart( function_ref( - const DecisionVariables& initialGuess)> + const DecisionVariables& initial_guess)> solve, - std::span initialGuesses) { - wpi::SmallVector>> futures; - futures.reserve(initialGuesses.size()); + std::span initial_guesses) { + gch::small_vector>> futures; + futures.reserve(initial_guesses.size()); - for (const auto& initialGuess : initialGuesses) { - futures.emplace_back(std::async(std::launch::async, solve, initialGuess)); + for (const auto& initial_guess : initial_guesses) { + futures.emplace_back(std::async(std::launch::async, solve, initial_guess)); } - wpi::SmallVector> results; + gch::small_vector> results; results.reserve(futures.size()); for (auto& future : futures) { results.emplace_back(future.get()); } - return *std::min_element( - results.cbegin(), results.cend(), [](const auto& a, const auto& b) { - // Prioritize successful solve - if (a.status.exitCondition == SolverExitCondition::kSuccess && - b.status.exitCondition != SolverExitCondition::kSuccess) { - return true; - } + return *std::ranges::min_element(results, [](const auto& a, const auto& b) { + // Prioritize successful solve + if (a.status == ExitStatus::SUCCESS && b.status != ExitStatus::SUCCESS) { + return true; + } - // Otherwise prioritize solution with lower cost - return a.status.cost < b.status.cost; - }); + // Otherwise prioritize solution with lower cost + return a.cost < b.cost; + }); } -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp new file mode 100644 index 0000000000..b484ec08d6 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp @@ -0,0 +1,342 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "sleipnir/autodiff/expression_type.hpp" +#include "sleipnir/autodiff/variable.hpp" +#include "sleipnir/autodiff/variable_matrix.hpp" +#include "sleipnir/optimization/solver/exit_status.hpp" +#include "sleipnir/optimization/solver/iteration_info.hpp" +#include "sleipnir/optimization/solver/options.hpp" +#include "sleipnir/util/symbol_exports.hpp" + +namespace slp { + +/** + * This class allows the user to pose a constrained nonlinear optimization + * problem in natural mathematical notation and solve it. + * + * This class supports problems of the form: +@verbatim + minₓ f(x) +subject to cₑ(x) = 0 + cᵢ(x) ≥ 0 +@endverbatim + * + * where f(x) is the scalar cost function, x is the vector of decision variables + * (variables the solver can tweak to minimize the cost function), cᵢ(x) are the + * inequality constraints, and cₑ(x) are the equality constraints. Constraints + * are equations or inequalities of the decision variables that constrain what + * values the solver is allowed to use when searching for an optimal solution. + * + * The nice thing about this class is users don't have to put their system in + * the form shown above manually; they can write it in natural mathematical form + * and it'll be converted for them. + */ +class SLEIPNIR_DLLEXPORT Problem { + public: + /** + * Construct the optimization problem. + */ + Problem() noexcept = default; + + /** + * Create a decision variable in the optimization problem. + * + * @return A decision variable in the optimization problem. + */ + [[nodiscard]] + Variable decision_variable() { + m_decision_variables.emplace_back(); + return m_decision_variables.back(); + } + + /** + * Create a matrix of decision variables in the optimization problem. + * + * @param rows Number of matrix rows. + * @param cols Number of matrix columns. + * @return A matrix of decision variables in the optimization problem. + */ + [[nodiscard]] + VariableMatrix decision_variable(int rows, int cols = 1) { + m_decision_variables.reserve(m_decision_variables.size() + rows * cols); + + VariableMatrix vars{rows, cols}; + + for (int row = 0; row < rows; ++row) { + for (int col = 0; col < cols; ++col) { + m_decision_variables.emplace_back(); + vars(row, col) = m_decision_variables.back(); + } + } + + return vars; + } + + /** + * Create a symmetric matrix of decision variables in the optimization + * problem. + * + * Variable instances are reused across the diagonal, which helps reduce + * problem dimensionality. + * + * @param rows Number of matrix rows. + * @return A symmetric matrix of decision varaibles in the optimization + * problem. + */ + [[nodiscard]] + VariableMatrix symmetric_decision_variable(int rows) { + // We only need to store the lower triangle of an n x n symmetric matrix; + // the other elements are duplicates. The lower triangle has (n² + n)/2 + // elements. + // + // n + // Σ k = (n² + n)/2 + // k=1 + m_decision_variables.reserve(m_decision_variables.size() + + (rows * rows + rows) / 2); + + VariableMatrix vars{rows, rows}; + + for (int row = 0; row < rows; ++row) { + for (int col = 0; col <= row; ++col) { + m_decision_variables.emplace_back(); + vars(row, col) = m_decision_variables.back(); + vars(col, row) = m_decision_variables.back(); + } + } + + return vars; + } + + /** + * Tells the solver to minimize the output of the given cost function. + * + * Note that this is optional. If only constraints are specified, the solver + * will find the closest solution to the initial conditions that's in the + * feasible set. + * + * @param cost The cost function to minimize. + */ + void minimize(const Variable& cost) { m_f = cost; } + + /** + * Tells the solver to minimize the output of the given cost function. + * + * Note that this is optional. If only constraints are specified, the solver + * will find the closest solution to the initial conditions that's in the + * feasible set. + * + * @param cost The cost function to minimize. + */ + void minimize(Variable&& cost) { m_f = std::move(cost); } + + /** + * Tells the solver to maximize the output of the given objective function. + * + * Note that this is optional. If only constraints are specified, the solver + * will find the closest solution to the initial conditions that's in the + * feasible set. + * + * @param objective The objective function to maximize. + */ + void maximize(const Variable& objective) { + // Maximizing a cost function is the same as minimizing its negative + m_f = -objective; + } + + /** + * Tells the solver to maximize the output of the given objective function. + * + * Note that this is optional. If only constraints are specified, the solver + * will find the closest solution to the initial conditions that's in the + * feasible set. + * + * @param objective The objective function to maximize. + */ + void maximize(Variable&& objective) { + // Maximizing a cost function is the same as minimizing its negative + m_f = -std::move(objective); + } + + /** + * Tells the solver to solve the problem while satisfying the given equality + * constraint. + * + * @param constraint The constraint to satisfy. + */ + void subject_to(const EqualityConstraints& constraint) { + m_equality_constraints.reserve(m_equality_constraints.size() + + constraint.constraints.size()); + std::ranges::copy(constraint.constraints, + std::back_inserter(m_equality_constraints)); + } + + /** + * Tells the solver to solve the problem while satisfying the given equality + * constraint. + * + * @param constraint The constraint to satisfy. + */ + void subject_to(EqualityConstraints&& constraint) { + m_equality_constraints.reserve(m_equality_constraints.size() + + constraint.constraints.size()); + std::ranges::copy(constraint.constraints, + std::back_inserter(m_equality_constraints)); + } + + /** + * Tells the solver to solve the problem while satisfying the given inequality + * constraint. + * + * @param constraint The constraint to satisfy. + */ + void subject_to(const InequalityConstraints& constraint) { + m_inequality_constraints.reserve(m_inequality_constraints.size() + + constraint.constraints.size()); + std::ranges::copy(constraint.constraints, + std::back_inserter(m_inequality_constraints)); + } + + /** + * Tells the solver to solve the problem while satisfying the given inequality + * constraint. + * + * @param constraint The constraint to satisfy. + */ + void subject_to(InequalityConstraints&& constraint) { + m_inequality_constraints.reserve(m_inequality_constraints.size() + + constraint.constraints.size()); + std::ranges::copy(constraint.constraints, + std::back_inserter(m_inequality_constraints)); + } + + /** + * Returns the cost function's type. + * + * @return The cost function's type. + */ + ExpressionType cost_function_type() const { + if (m_f) { + return m_f.value().type(); + } else { + return ExpressionType::NONE; + } + } + + /** + * Returns the type of the highest order equality constraint. + * + * @return The type of the highest order equality constraint. + */ + ExpressionType equality_constraint_type() const { + if (!m_equality_constraints.empty()) { + return std::ranges::max(m_equality_constraints, {}, &Variable::type) + .type(); + } else { + return ExpressionType::NONE; + } + } + + /** + * Returns the type of the highest order inequality constraint. + * + * @return The type of the highest order inequality constraint. + */ + ExpressionType inequality_constraint_type() const { + if (!m_inequality_constraints.empty()) { + return std::ranges::max(m_inequality_constraints, {}, &Variable::type) + .type(); + } else { + return ExpressionType::NONE; + } + } + + /** + * Solve the optimization problem. The solution will be stored in the original + * variables used to construct the problem. + * + * @param options Solver options. + * @param spy Enables writing sparsity patterns of H, Aₑ, and Aᵢ to files + * named H.spy, A_e.spy, and A_i.spy respectively during solve. Use + * tools/spy.py to plot them. + * @return The solver status. + */ + ExitStatus solve(const Options& options = Options{}, + [[maybe_unused]] bool spy = false); + + /** + * Adds a callback to be called at the beginning of each solver iteration. + * + * The callback for this overload should return void. + * + * @param callback The callback. + */ + template + requires requires(F callback, const IterationInfo& info) { + { callback(info) } -> std::same_as; + } + void add_callback(F&& callback) { + m_iteration_callbacks.emplace_back( + [=, callback = std::forward(callback)](const IterationInfo& info) { + callback(info); + return false; + }); + } + + /** + * Adds a callback to be called at the beginning of each solver iteration. + * + * The callback for this overload should return bool. + * + * @param callback The callback. Returning true from the callback causes the + * solver to exit early with the solution it has so far. + */ + template + requires requires(F callback, const IterationInfo& info) { + { callback(info) } -> std::same_as; + } + void add_callback(F&& callback) { + m_iteration_callbacks.emplace_back(std::forward(callback)); + } + + /** + * Clears the registered callbacks. + */ + void clear_callbacks() { m_iteration_callbacks.clear(); } + + private: + // The list of decision variables, which are the root of the problem's + // expression tree + gch::small_vector m_decision_variables; + + // The cost function: f(x) + std::optional m_f; + + // The list of equality constraints: cₑ(x) = 0 + gch::small_vector m_equality_constraints; + + // The list of inequality constraints: cᵢ(x) ≥ 0 + gch::small_vector m_inequality_constraints; + + // The iteration callbacks + gch::small_vector> + m_iteration_callbacks; + + void print_exit_conditions([[maybe_unused]] const Options& options); + void print_problem_analysis(); +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/InteriorPoint.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/InteriorPoint.hpp deleted file mode 100644 index 51d8f97305..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/InteriorPoint.hpp +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -#include - -#include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/optimization/SolverConfig.hpp" -#include "sleipnir/optimization/SolverIterationInfo.hpp" -#include "sleipnir/optimization/SolverStatus.hpp" -#include "sleipnir/util/FunctionRef.hpp" -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** -Finds the optimal solution to a nonlinear program using the interior-point -method. - -A nonlinear program has the form: - -@verbatim - min_x f(x) -subject to cₑ(x) = 0 - cᵢ(x) ≥ 0 -@endverbatim - -where f(x) is the cost function, cₑ(x) are the equality constraints, and cᵢ(x) -are the inequality constraints. - -@param[in] decisionVariables The list of decision variables. -@param[in] equalityConstraints The list of equality constraints. -@param[in] inequalityConstraints The list of inequality constraints. -@param[in] f The cost function. -@param[in] callback The user callback. -@param[in] config Configuration options for the solver. -@param[in] feasibilityRestoration Whether to use feasibility restoration instead - of the normal algorithm. -@param[in,out] x The initial guess and output location for the decision - variables. -@param[in,out] s The initial guess and output location for the inequality - constraint slack variables. -@param[out] status The solver status. -*/ -SLEIPNIR_DLLEXPORT void InteriorPoint( - std::span decisionVariables, - std::span equalityConstraints, - std::span inequalityConstraints, Variable& f, - function_ref callback, - const SolverConfig& config, bool feasibilityRestoration, Eigen::VectorXd& x, - Eigen::VectorXd& s, SolverStatus* status); - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp deleted file mode 100644 index ed10ffedff..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -#include - -#include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/optimization/SolverConfig.hpp" -#include "sleipnir/optimization/SolverIterationInfo.hpp" -#include "sleipnir/optimization/SolverStatus.hpp" -#include "sleipnir/util/FunctionRef.hpp" -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** -Finds the optimal solution to a nonlinear program using Sequential Quadratic -Programming (SQP). - -A nonlinear program has the form: - -@verbatim - min_x f(x) -subject to cₑ(x) = 0 -@endverbatim - -where f(x) is the cost function and cₑ(x) are the equality constraints. - -@param[in] decisionVariables The list of decision variables. -@param[in] equalityConstraints The list of equality constraints. -@param[in] f The cost function. -@param[in] callback The user callback. -@param[in] config Configuration options for the solver. -@param[in,out] x The initial guess and output location for the decision - variables. -@param[out] status The solver status. -*/ -SLEIPNIR_DLLEXPORT void SQP( - std::span decisionVariables, - std::span equalityConstraints, Variable& f, - function_ref callback, - const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status); - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/exit_status.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/exit_status.hpp new file mode 100644 index 0000000000..f3acdcaf2d --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/exit_status.hpp @@ -0,0 +1,82 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include + +#include "sleipnir/util/symbol_exports.hpp" + +namespace slp { + +/** + * Solver exit status. Negative values indicate failure. + */ +enum class ExitStatus : int8_t { + /// Solved the problem to the desired tolerance. + SUCCESS = 0, + /// The solver returned its solution so far after the user requested a stop. + CALLBACK_REQUESTED_STOP = 1, + /// The solver determined the problem to be overconstrained and gave up. + TOO_FEW_DOFS = -1, + /// The solver determined the problem to be locally infeasible and gave up. + LOCALLY_INFEASIBLE = -2, + /// The problem setup frontend determined the problem to have an empty + /// feasible region. + GLOBALLY_INFEASIBLE = -3, + /// The linear system factorization failed. + FACTORIZATION_FAILED = -4, + /// The backtracking line search failed, and the problem isn't locally + /// infeasible. + LINE_SEARCH_FAILED = -5, + /// The solver encountered nonfinite initial cost or constraints and gave up. + NONFINITE_INITIAL_COST_OR_CONSTRAINTS = -6, + /// The solver encountered diverging primal iterates xₖ and/or sₖ and gave up. + DIVERGING_ITERATES = -7, + /// The solver returned its solution so far after exceeding the maximum number + /// of iterations. + MAX_ITERATIONS_EXCEEDED = -8, + /// The solver returned its solution so far after exceeding the maximum + /// elapsed wall clock time. + TIMEOUT = -9, +}; + +/** + * Returns user-readable message corresponding to the solver exit status. + * + * @param exit_status Solver exit status. + */ +SLEIPNIR_DLLEXPORT constexpr std::string_view to_message( + const ExitStatus& exit_status) { + using enum ExitStatus; + + switch (exit_status) { + case SUCCESS: + return "success"; + case CALLBACK_REQUESTED_STOP: + return "callback requested stop"; + case TOO_FEW_DOFS: + return "too few degrees of freedom"; + case LOCALLY_INFEASIBLE: + return "locally infeasible"; + case GLOBALLY_INFEASIBLE: + return "globally infeasible"; + case FACTORIZATION_FAILED: + return "factorization failed"; + case LINE_SEARCH_FAILED: + return "line search failed"; + case NONFINITE_INITIAL_COST_OR_CONSTRAINTS: + return "nonfinite initial cost or constraints"; + case DIVERGING_ITERATES: + return "diverging iterates"; + case MAX_ITERATIONS_EXCEEDED: + return "max iterations exceeded"; + case TIMEOUT: + return "timeout"; + default: + return "unknown"; + } +} + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp new file mode 100644 index 0000000000..2777f9e833 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp @@ -0,0 +1,232 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include + +#include +#include + +#include "sleipnir/optimization/solver/exit_status.hpp" +#include "sleipnir/optimization/solver/iteration_info.hpp" +#include "sleipnir/optimization/solver/options.hpp" +#include "sleipnir/util/symbol_exports.hpp" + +namespace slp { + +/** + * Matrix callbacks for the interior-point method solver. + */ +struct SLEIPNIR_DLLEXPORT InteriorPointMatrixCallbacks { + /// Cost function value f(x) getter. + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
f(x)11
+ std::function f; + + /// Cost function gradient ∇f(x) getter. + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
∇f(x)num_decision_variables1
+ std::function(const Eigen::VectorXd& x)> g; + + /// Lagrangian Hessian ∇ₓₓ²L(x, y, z) getter. + /// + /// L(xₖ, yₖ, zₖ) = f(xₖ) − yₖᵀcₑ(xₖ) − zₖᵀcᵢ(xₖ) + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
ynum_equality_constraints1
znum_inequality_constraints1
∇ₓₓ²L(x, y, z)num_decision_variablesnum_decision_variables
+ std::function(const Eigen::VectorXd& x, + const Eigen::VectorXd& y, + const Eigen::VectorXd& z)> + H; + + /// Equality constraint value cₑ(x) getter. + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
cₑ(x)num_equality_constraints1
+ std::function c_e; + + /// Equality constraint Jacobian ∂cₑ/∂x getter. + /// + /// @verbatim + /// [∇ᵀcₑ₁(xₖ)] + /// Aₑ(x) = [∇ᵀcₑ₂(xₖ)] + /// [ ⋮ ] + /// [∇ᵀcₑₘ(xₖ)] + /// @endverbatim + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
Aₑ(x)num_equality_constraintsnum_decision_variables
+ std::function(const Eigen::VectorXd& x)> A_e; + + /// Inequality constraint value cᵢ(x) getter. + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
cᵢ(x)num_inequality_constraints1
+ std::function c_i; + + /// Inequality constraint Jacobian ∂cᵢ/∂x getter. + /// + /// @verbatim + /// [∇ᵀcᵢ₁(xₖ)] + /// Aᵢ(x) = [∇ᵀcᵢ₂(xₖ)] + /// [ ⋮ ] + /// [∇ᵀcᵢₘ(xₖ)] + /// @endverbatim + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
Aᵢ(x)num_inequality_constraintsnum_decision_variables
+ std::function(const Eigen::VectorXd& x)> A_i; +}; + +/** +Finds the optimal solution to a nonlinear program using the interior-point +method. + +A nonlinear program has the form: + +@verbatim + min_x f(x) +subject to cₑ(x) = 0 + cᵢ(x) ≥ 0 +@endverbatim + +where f(x) is the cost function, cₑ(x) are the equality constraints, and cᵢ(x) +are the inequality constraints. + +@param[in] matrix_callbacks Matrix callbacks. +@param[in] iteration_callbacks The list of callbacks to call at the beginning of + each iteration. +@param[in] options Solver options. +@param[in,out] x The initial guess and output location for the decision + variables. +@return The exit status. +*/ +SLEIPNIR_DLLEXPORT ExitStatus +interior_point(const InteriorPointMatrixCallbacks& matrix_callbacks, + std::span> + iteration_callbacks, + const Options& options, +#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION + const Eigen::ArrayX& bound_constraint_mask, +#endif + Eigen::VectorXd& x); + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverIterationInfo.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/iteration_info.hpp similarity index 72% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverIterationInfo.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/iteration_info.hpp index bb915b856e..027f4e5a0c 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverIterationInfo.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/iteration_info.hpp @@ -5,21 +5,18 @@ #include #include -namespace sleipnir { +namespace slp { /** - * Solver iteration information exposed to a user callback. + * Solver iteration information exposed to an iteration callback. */ -struct SolverIterationInfo { +struct IterationInfo { /// The solver iteration. int iteration; /// The decision variables. const Eigen::VectorXd& x; - /// The inequality constraint slack variables. - const Eigen::VectorXd& s; - /// The gradient of the cost function. const Eigen::SparseVector& g; @@ -33,4 +30,4 @@ struct SolverIterationInfo { const Eigen::SparseMatrix& A_i; }; -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp new file mode 100644 index 0000000000..210ed87475 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp @@ -0,0 +1,113 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include + +#include +#include + +#include "sleipnir/optimization/solver/exit_status.hpp" +#include "sleipnir/optimization/solver/iteration_info.hpp" +#include "sleipnir/optimization/solver/options.hpp" +#include "sleipnir/util/symbol_exports.hpp" + +namespace slp { + +/** + * Matrix callbacks for the Newton's method solver. + */ +struct SLEIPNIR_DLLEXPORT NewtonMatrixCallbacks { + /// Cost function value f(x) getter. + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
f(x)11
+ std::function f; + + /// Cost function gradient ∇f(x) getter. + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
∇f(x)num_decision_variables1
+ std::function(const Eigen::VectorXd& x)> g; + + /// Lagrangian Hessian ∇ₓₓ²L(x) getter. + /// + /// L(xₖ) = f(xₖ) + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
∇ₓₓ²L(x)num_decision_variablesnum_decision_variables
+ std::function(const Eigen::VectorXd& x)> H; +}; + +/** +Finds the optimal solution to a nonlinear program using Newton's method. + +A nonlinear program has the form: + +@verbatim + min_x f(x) +@endverbatim + +where f(x) is the cost function. + +@param[in] matrix_callbacks Matrix callbacks. +@param[in] iteration_callbacks The list of callbacks to call at the beginning of + each iteration. +@param[in] options Solver options. +@param[in,out] x The initial guess and output location for the decision + variables. +@return The exit status. +*/ +SLEIPNIR_DLLEXPORT ExitStatus +newton(const NewtonMatrixCallbacks& matrix_callbacks, + std::span> + iteration_callbacks, + const Options& options, Eigen::VectorXd& x); + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/options.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/options.hpp new file mode 100644 index 0000000000..8b9cb93400 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/options.hpp @@ -0,0 +1,94 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include + +#include "sleipnir/util/symbol_exports.hpp" + +namespace slp { + +/** + * Solver options. + */ +struct SLEIPNIR_DLLEXPORT Options { + /// The solver will stop once the error is below this tolerance. + double tolerance = 1e-8; + + /// The maximum number of solver iterations before returning a solution. + int max_iterations = 5000; + + /// The maximum elapsed wall clock time before returning a solution. + std::chrono::duration timeout{ + std::numeric_limits::infinity()}; + + /// Enables the feasible interior-point method. When the inequality + /// constraints are all feasible, step sizes are reduced when necessary to + /// prevent them becoming infeasible again. This is useful when parts of the + /// problem are ill-conditioned in infeasible regions (e.g., square root of a + /// negative value). This can slow or prevent progress toward a solution + /// though, so only enable it if necessary. + bool feasible_ipm = false; + + /// Enables diagnostic prints. + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
HeadingDescription
iterIteration number
typeIteration type (normal, accepted second-order correction, rejected + /// second-order correction)
time (ms)Duration of iteration in milliseconds
errorError estimate
costCost function value at current iterate
infeas.Constraint infeasibility at current iterate
complement.Complementary slackness at current iterate (sᵀz)
μBarrier parameter
regIteration matrix regularization
primal αPrimal step size
dual αDual step size
↩Number of line search backtracks
+ bool diagnostics = false; +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp new file mode 100644 index 0000000000..c1dfb00bf1 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp @@ -0,0 +1,171 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include + +#include +#include + +#include "sleipnir/optimization/solver/exit_status.hpp" +#include "sleipnir/optimization/solver/iteration_info.hpp" +#include "sleipnir/optimization/solver/options.hpp" +#include "sleipnir/util/symbol_exports.hpp" + +namespace slp { + +/** + * Matrix callbacks for the Sequential Quadratic Programming (SQP) solver. + */ +struct SLEIPNIR_DLLEXPORT SQPMatrixCallbacks { + /// Cost function value f(x) getter. + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
f(x)11
+ std::function f; + + /// Cost function gradient ∇f(x) getter. + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
∇f(x)num_decision_variables1
+ std::function(const Eigen::VectorXd& x)> g; + + /// Lagrangian Hessian ∇ₓₓ²L(x, y) getter. + /// + /// L(xₖ, yₖ) = f(xₖ) − yₖᵀcₑ(xₖ) + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
ynum_equality_constraints1
∇ₓₓ²L(x, y)num_decision_variablesnum_decision_variables
+ std::function(const Eigen::VectorXd& x, + const Eigen::VectorXd& y)> + H; + + /// Equality constraint value cₑ(x) getter. + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
cₑ(x)num_equality_constraints1
+ std::function c_e; + + /// Equality constraint Jacobian ∂cₑ/∂x getter. + /// + /// @verbatim + /// [∇ᵀcₑ₁(xₖ)] + /// Aₑ(x) = [∇ᵀcₑ₂(xₖ)] + /// [ ⋮ ] + /// [∇ᵀcₑₘ(xₖ)] + /// @endverbatim + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
Aₑ(x)num_equality_constraintsnum_decision_variables
+ std::function(const Eigen::VectorXd& x)> A_e; +}; + +/** +Finds the optimal solution to a nonlinear program using Sequential Quadratic +Programming (SQP). + +A nonlinear program has the form: + +@verbatim + min_x f(x) +subject to cₑ(x) = 0 +@endverbatim + +where f(x) is the cost function and cₑ(x) are the equality constraints. + +@param[in] matrix_callbacks Matrix callbacks. +@param[in] iteration_callbacks The list of callbacks to call at the beginning of + each iteration. +@param[in] options Solver options. +@param[in,out] x The initial guess and output location for the decision + variables. +@return The exit status. +*/ +SLEIPNIR_DLLEXPORT ExitStatus +sqp(const SQPMatrixCallbacks& matrix_callbacks, + std::span> + iteration_callbacks, + const Options& options, Eigen::VectorXd& x); + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Assert.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Assert.hpp deleted file mode 100644 index ba381ef8f4..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Assert.hpp +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#ifdef JORMUNGANDR -#include -#include -/** - * Throw an exception in Python. - */ -#define Assert(condition) \ - do { \ - if (!(condition)) { \ - throw std::invalid_argument( \ - std::format("{}:{}: {}: Assertion `{}' failed.", __FILE__, __LINE__, \ - __func__, #condition)); \ - } \ - } while (0); -#else -#include -/** - * Abort in C++. - */ -#define Assert(condition) assert(condition) -#endif diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Concepts.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Concepts.hpp deleted file mode 100644 index 723284117e..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Concepts.hpp +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -#include - -namespace sleipnir { - -template -concept ScalarLike = requires(T t) { - t + 1.0; - t = 1.0; -}; - -template -concept SleipnirMatrixLike = requires(T t, int rows, int cols) { - t.Rows(); - t.Cols(); - t(rows, cols); -}; - -template -concept EigenMatrixLike = std::derived_from>; - -template -concept MatrixLike = SleipnirMatrixLike || EigenMatrixLike; - -template -concept EigenSolver = requires(T t) { t.solve(Eigen::VectorXd{}); }; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Spy.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Spy.hpp deleted file mode 100644 index 7f526a2d99..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Spy.hpp +++ /dev/null @@ -1,89 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include -#include -#include - -#include -#include - -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** - * Write the sparsity pattern of a sparse matrix to a file. - * - * Each character represents an element with '.' representing zero, '+' - * representing positive, and '-' representing negative. Here's an example for a - * 3x3 identity matrix. - * - * "+.." - * ".+." - * "..+" - * - * @param[out] file A file stream. - * @param[in] mat The sparse matrix. - */ -SLEIPNIR_DLLEXPORT inline void Spy(std::ostream& file, - const Eigen::SparseMatrix& mat) { - const int cells_width = mat.cols() + 1; - const int cells_height = mat.rows(); - - wpi::SmallVector cells; - - // Allocate space for matrix of characters plus trailing newlines - cells.reserve(cells_width * cells_height); - - // Initialize cell array - for (int row = 0; row < mat.rows(); ++row) { - for (int col = 0; col < mat.cols(); ++col) { - cells.emplace_back('.'); - } - cells.emplace_back('\n'); - } - - // Fill in non-sparse entries - for (int k = 0; k < mat.outerSize(); ++k) { - for (Eigen::SparseMatrix::InnerIterator it{mat, k}; it; ++it) { - if (it.value() < 0.0) { - cells[it.row() * cells_width + it.col()] = '-'; - } else if (it.value() > 0.0) { - cells[it.row() * cells_width + it.col()] = '+'; - } - } - } - - // Write cell array to file - for (const auto& c : cells) { - file << c; - } -} - -/** - * Write the sparsity pattern of a sparse matrix to a file. - * - * Each character represents an element with "." representing zero, "+" - * representing positive, and "-" representing negative. Here's an example for a - * 3x3 identity matrix. - * - * "+.." - * ".+." - * "..+" - * - * @param[in] filename The filename. - * @param[in] mat The sparse matrix. - */ -SLEIPNIR_DLLEXPORT inline void Spy(std::string_view filename, - const Eigen::SparseMatrix& mat) { - std::ofstream file{std::string{filename}}; - if (!file.is_open()) { - return; - } - - Spy(file, mat); -} - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp new file mode 100644 index 0000000000..75d8ffca32 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp @@ -0,0 +1,27 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#ifdef JORMUNGANDR +#include +#include +#include +/** + * Throw an exception in Python. + */ +#define slp_assert(condition) \ + do { \ + if (!(condition)) { \ + auto location = std::source_location::current(); \ + throw std::invalid_argument(std::format( \ + "{}:{}: {}: Assertion `{}' failed.", location.file_name(), \ + location.line(), location.function_name(), #condition)); \ + } \ + } while (0); +#else +#include +/** + * Abort in C++. + */ +#define slp_assert(condition) assert(condition) +#endif diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/concepts.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/concepts.hpp new file mode 100644 index 0000000000..38fac92dcd --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/concepts.hpp @@ -0,0 +1,42 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include + +#include + +namespace slp { + +template +concept ScalarLike = requires(std::decay_t t) { + t + 1.0; + t = 1.0; +}; + +template +concept SleipnirScalarLike = requires(std::decay_t t) { + t + 1.0; + t = 1.0; + { t.value() } -> std::same_as; +}; + +template +concept EigenMatrixLike = + std::derived_from, Eigen::MatrixBase>>; + +template +concept SleipnirMatrixLike = requires(std::decay_t t, int rows, int cols) { + t.rows(); + t.cols(); + { t.value() } -> std::same_as; +} && !EigenMatrixLike; + +template +concept SleipnirType = SleipnirScalarLike || SleipnirMatrixLike; + +template +concept MatrixLike = SleipnirMatrixLike || EigenMatrixLike; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/FunctionRef.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/function_ref.hpp similarity index 86% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/FunctionRef.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/function_ref.hpp index 14a46903dc..547dfc6ed8 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/FunctionRef.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/function_ref.hpp @@ -7,15 +7,15 @@ #include #include -namespace sleipnir { +namespace slp { + +template +class function_ref; /** * An implementation of std::function_ref, a lightweight non-owning reference to * a callable. */ -template -class function_ref; - template class function_ref { public: @@ -23,12 +23,17 @@ class function_ref { /** * Creates a `function_ref` which refers to the same callable as `rhs`. + * + * @param rhs Other `function_ref`. */ constexpr function_ref(const function_ref& rhs) noexcept = default; /** * Constructs a `function_ref` referring to `f`. + * + * @tparam F Callable type. + * @param f Callable to which to refer. */ template requires(!std::is_same_v, function_ref> && @@ -45,12 +50,18 @@ class function_ref { /** * Makes `*this` refer to the same callable as `rhs`. + * + * @param rhs Other `function_ref`. + * @return `*this` */ constexpr function_ref& operator=( const function_ref& rhs) noexcept = default; /** * Makes `*this` refer to `f`. + * + * @param f Callable to which to refer. + * @return `*this` */ template requires std::is_invocable_r_v @@ -67,6 +78,8 @@ class function_ref { /** * Swaps the referred callables of `*this` and `rhs`. + * + * @param rhs Other `function_ref`. */ constexpr void swap(function_ref& rhs) noexcept { std::swap(obj_, rhs.obj_); @@ -75,6 +88,9 @@ class function_ref { /** * Call the stored callable with the given arguments. + * + * @param args The arguments. + * @return The return value of the callable. */ R operator()(Args... args) const { return callback_(obj_, std::forward(args)...); @@ -97,4 +113,4 @@ constexpr void swap(function_ref& lhs, template function_ref(R (*)(Args...)) -> function_ref; -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/IntrusiveSharedPtr.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/intrusive_shared_ptr.hpp similarity index 60% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/IntrusiveSharedPtr.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/intrusive_shared_ptr.hpp index f1290e5837..b3f74e7bde 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/IntrusiveSharedPtr.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/intrusive_shared_ptr.hpp @@ -2,11 +2,12 @@ #pragma once +#include #include #include #include -namespace sleipnir { +namespace slp { /** * A custom intrusive shared pointer implementation without thread @@ -16,17 +17,20 @@ namespace sleipnir { * * 1. A zero-initialized public counter variable that serves as the shared * pointer's reference count. - * 2. A free function `void IntrusiveSharedPtrIncRefCount(T*)` that increments - * the reference count. - * 3. A free function `void IntrusiveSharedPtrDecRefCount(T*)` that decrements - * the reference count and deallocates the pointed to object if the reference - * count reaches zero. + * 2. A free function `void inc_ref_count(T*)` that increments the reference + * count. + * 3. A free function `void dec_ref_count(T*)` that decrements the reference + * count and deallocates the pointed to object if the reference count reaches + * zero. * * @tparam T The type of the object to be reference counted. */ template class IntrusiveSharedPtr { public: + template + friend class IntrusiveSharedPtr; + /** * Constructs an empty intrusive shared pointer. */ @@ -40,31 +44,53 @@ class IntrusiveSharedPtr { /** * Constructs an intrusive shared pointer from the given pointer and takes * ownership. + * + * @param ptr The pointer of which to take ownership. */ explicit constexpr IntrusiveSharedPtr(T* ptr) noexcept : m_ptr{ptr} { if (m_ptr != nullptr) { - IntrusiveSharedPtrIncRefCount(m_ptr); + inc_ref_count(m_ptr); } } constexpr ~IntrusiveSharedPtr() { if (m_ptr != nullptr) { - IntrusiveSharedPtrDecRefCount(m_ptr); + dec_ref_count(m_ptr); } } /** * Copy constructs from the given intrusive shared pointer. + * + * @param rhs The other intrusive shared pointer. */ constexpr IntrusiveSharedPtr(const IntrusiveSharedPtr& rhs) noexcept : m_ptr{rhs.m_ptr} { if (m_ptr != nullptr) { - IntrusiveSharedPtrIncRefCount(m_ptr); + inc_ref_count(m_ptr); + } + } + + /** + * Copy constructs from the given intrusive shared pointer. + * + * @param rhs The other intrusive shared pointer. + */ + template + requires(!std::same_as && std::convertible_to) + constexpr IntrusiveSharedPtr( // NOLINT + const IntrusiveSharedPtr& rhs) noexcept + : m_ptr{rhs.m_ptr} { + if (m_ptr != nullptr) { + inc_ref_count(m_ptr); } } /** * Makes a copy of the given intrusive shared pointer. + * + * @param rhs The other intrusive shared pointer. + * @return This intrusive shared pointer. */ constexpr IntrusiveSharedPtr& operator=( // NOLINT const IntrusiveSharedPtr& rhs) noexcept { @@ -73,13 +99,40 @@ class IntrusiveSharedPtr { } if (m_ptr != nullptr) { - IntrusiveSharedPtrDecRefCount(m_ptr); + dec_ref_count(m_ptr); } m_ptr = rhs.m_ptr; if (m_ptr != nullptr) { - IntrusiveSharedPtrIncRefCount(m_ptr); + inc_ref_count(m_ptr); + } + + return *this; + } + + /** + * Makes a copy of the given intrusive shared pointer. + * + * @param rhs The other intrusive shared pointer. + * @return This intrusive shared pointer. + */ + template + requires(!std::same_as && std::convertible_to) + constexpr IntrusiveSharedPtr& operator=( // NOLINT + const IntrusiveSharedPtr& rhs) noexcept { + if (m_ptr == rhs.m_ptr) { + return *this; + } + + if (m_ptr != nullptr) { + dec_ref_count(m_ptr); + } + + m_ptr = rhs.m_ptr; + + if (m_ptr != nullptr) { + inc_ref_count(m_ptr); } return *this; @@ -87,12 +140,28 @@ class IntrusiveSharedPtr { /** * Move constructs from the given intrusive shared pointer. + * + * @param rhs The other intrusive shared pointer. */ constexpr IntrusiveSharedPtr(IntrusiveSharedPtr&& rhs) noexcept : m_ptr{std::exchange(rhs.m_ptr, nullptr)} {} + /** + * Move constructs from the given intrusive shared pointer. + * + * @param rhs The other intrusive shared pointer. + */ + template + requires(!std::same_as && std::convertible_to) + constexpr IntrusiveSharedPtr( // NOLINT + IntrusiveSharedPtr&& rhs) noexcept + : m_ptr{std::exchange(rhs.m_ptr, nullptr)} {} + /** * Move assigns from the given intrusive shared pointer. + * + * @param rhs The other intrusive shared pointer. + * @return This intrusive shared pointer. */ constexpr IntrusiveSharedPtr& operator=( IntrusiveSharedPtr&& rhs) noexcept { @@ -106,28 +175,58 @@ class IntrusiveSharedPtr { } /** - * Returns the internal pointer. + * Move assigns from the given intrusive shared pointer. + * + * @param rhs The other intrusive shared pointer. + * @return This intrusive shared pointer. */ - constexpr T* Get() const noexcept { return m_ptr; } + template + requires(!std::same_as && std::convertible_to) + constexpr IntrusiveSharedPtr& operator=( + IntrusiveSharedPtr&& rhs) noexcept { + if (m_ptr == rhs.m_ptr) { + return *this; + } + + std::swap(m_ptr, rhs.m_ptr); + + return *this; + } + + /** + * Returns the internal pointer. + * + * @return The internal pointer. + */ + constexpr T* get() const noexcept { return m_ptr; } /** * Returns the object pointed to by the internal pointer. + * + * @return The object pointed to by the internal pointer. */ constexpr T& operator*() const noexcept { return *m_ptr; } /** * Returns the internal pointer. + * + * @return The internal pointer. */ constexpr T* operator->() const noexcept { return m_ptr; } /** * Returns true if the internal pointer isn't nullptr. + * + * @return True if the internal pointer isn't nullptr. */ explicit constexpr operator bool() const noexcept { return m_ptr != nullptr; } /** * Returns true if the given intrusive shared pointers point to the same * object. + * + * @param lhs The left-hand side. + * @param rhs The right-hand side. */ friend constexpr bool operator==(const IntrusiveSharedPtr& lhs, const IntrusiveSharedPtr& rhs) noexcept { @@ -137,6 +236,9 @@ class IntrusiveSharedPtr { /** * Returns true if the given intrusive shared pointers point to different * objects. + * + * @param lhs The left-hand side. + * @param rhs The right-hand side. */ friend constexpr bool operator!=(const IntrusiveSharedPtr& lhs, const IntrusiveSharedPtr& rhs) noexcept { @@ -145,6 +247,8 @@ class IntrusiveSharedPtr { /** * Returns true if the left-hand intrusive shared pointer points to nullptr. + * + * @param lhs The left-hand side. */ friend constexpr bool operator==(const IntrusiveSharedPtr& lhs, std::nullptr_t) noexcept { @@ -153,6 +257,8 @@ class IntrusiveSharedPtr { /** * Returns true if the right-hand intrusive shared pointer points to nullptr. + * + * @param rhs The right-hand side. */ friend constexpr bool operator==(std::nullptr_t, const IntrusiveSharedPtr& rhs) noexcept { @@ -162,6 +268,8 @@ class IntrusiveSharedPtr { /** * Returns true if the left-hand intrusive shared pointer doesn't point to * nullptr. + * + * @param lhs The left-hand side. */ friend constexpr bool operator!=(const IntrusiveSharedPtr& lhs, std::nullptr_t) noexcept { @@ -171,6 +279,8 @@ class IntrusiveSharedPtr { /** * Returns true if the right-hand intrusive shared pointer doesn't point to * nullptr. + * + * @param rhs The right-hand side. */ friend constexpr bool operator!=(std::nullptr_t, const IntrusiveSharedPtr& rhs) noexcept { @@ -190,7 +300,7 @@ class IntrusiveSharedPtr { * @param args Constructor arguments for T. */ template -IntrusiveSharedPtr MakeIntrusiveShared(Args&&... args) { +IntrusiveSharedPtr make_intrusive_shared(Args&&... args) { return IntrusiveSharedPtr{new T(std::forward(args)...)}; } @@ -206,11 +316,11 @@ IntrusiveSharedPtr MakeIntrusiveShared(Args&&... args) { * @param args Constructor arguments for T. */ template -IntrusiveSharedPtr AllocateIntrusiveShared(Alloc alloc, Args&&... args) { +IntrusiveSharedPtr allocate_intrusive_shared(Alloc alloc, Args&&... args) { auto ptr = std::allocator_traits::allocate(alloc, sizeof(T)); std::allocator_traits::construct(alloc, ptr, std::forward(args)...); return IntrusiveSharedPtr{ptr}; } -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Pool.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/pool.hpp similarity index 58% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Pool.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/pool.hpp index 1951bd1ee8..1386056b40 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Pool.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/pool.hpp @@ -5,11 +5,11 @@ #include #include -#include +#include -#include "sleipnir/util/SymbolExports.hpp" +#include "sleipnir/util/symbol_exports.hpp" -namespace sleipnir { +namespace slp { /** * This class implements a pool memory resource. @@ -23,14 +23,33 @@ class SLEIPNIR_DLLEXPORT PoolResource { /** * Constructs a default PoolResource. * - * @param blocksPerChunk Number of blocks per chunk of memory. + * @param blocks_per_chunk Number of blocks per chunk of memory. */ - explicit PoolResource(size_t blocksPerChunk) - : blocksPerChunk{blocksPerChunk} {} + explicit PoolResource(size_t blocks_per_chunk) + : blocks_per_chunk{blocks_per_chunk} {} + /** + * Copy constructor. + */ PoolResource(const PoolResource&) = delete; + + /** + * Copy assignment operator. + * + * @return This pool resource. + */ PoolResource& operator=(const PoolResource&) = delete; + + /** + * Move constructor. + */ PoolResource(PoolResource&&) = default; + + /** + * Move assignment operator. + * + * @return This pool resource. + */ PoolResource& operator=(PoolResource&&) = default; /** @@ -38,16 +57,17 @@ class SLEIPNIR_DLLEXPORT PoolResource { * * @param bytes Number of bytes in the block. * @param alignment Alignment of the block (unused). + * @return A block of memory from the pool. */ [[nodiscard]] void* allocate(size_t bytes, [[maybe_unused]] size_t alignment = alignof(std::max_align_t)) { - if (m_freeList.empty()) { - AddChunk(bytes); + if (m_free_list.empty()) { + add_chunk(bytes); } - auto ptr = m_freeList.back(); - m_freeList.pop_back(); + auto ptr = m_free_list.back(); + m_free_list.pop_back(); return ptr; } @@ -61,11 +81,14 @@ class SLEIPNIR_DLLEXPORT PoolResource { void deallocate( void* p, [[maybe_unused]] size_t bytes, [[maybe_unused]] size_t alignment = alignof(std::max_align_t)) { - m_freeList.emplace_back(p); + m_free_list.emplace_back(p); } /** * Returns true if this pool resource has the same backing storage as another. + * + * @param other The other pool resource. + * @return True if this pool resource has the same backing storage as another. */ bool is_equal(const PoolResource& other) const noexcept { return this == &other; @@ -73,26 +96,28 @@ class SLEIPNIR_DLLEXPORT PoolResource { /** * Returns the number of blocks from this pool resource that are in use. + * + * @return The number of blocks from this pool resource that are in use. */ size_t blocks_in_use() const noexcept { - return m_buffer.size() * blocksPerChunk - m_freeList.size(); + return m_buffer.size() * blocks_per_chunk - m_free_list.size(); } private: - wpi::SmallVector> m_buffer; - wpi::SmallVector m_freeList; - size_t blocksPerChunk; + gch::small_vector> m_buffer; + gch::small_vector m_free_list; + size_t blocks_per_chunk; /** * Adds a memory chunk to the pool, partitions it into blocks with the given * number of bytes, and appends pointers to them to the free list. * - * @param bytesPerBlock Number of bytes in the block. + * @param bytes_per_block Number of bytes in the block. */ - void AddChunk(size_t bytesPerBlock) { - m_buffer.emplace_back(new std::byte[bytesPerBlock * blocksPerChunk]); - for (int i = blocksPerChunk - 1; i >= 0; --i) { - m_freeList.emplace_back(m_buffer.back().get() + bytesPerBlock * i); + void add_chunk(size_t bytes_per_block) { + m_buffer.emplace_back(new std::byte[bytes_per_block * blocks_per_chunk]); + for (int i = blocks_per_chunk - 1; i >= 0; --i) { + m_free_list.emplace_back(m_buffer.back().get() + bytes_per_block * i); } } }; @@ -115,19 +140,29 @@ class PoolAllocator { * * @param r The pool resource. */ - explicit constexpr PoolAllocator(PoolResource* r) : m_memoryResource{r} {} + explicit constexpr PoolAllocator(PoolResource* r) : m_memory_resource{r} {} - constexpr PoolAllocator(const PoolAllocator& other) = default; + /** + * Copy constructor. + */ + constexpr PoolAllocator(const PoolAllocator&) = default; + + /** + * Copy assignment operator. + * + * @return This pool allocator. + */ constexpr PoolAllocator& operator=(const PoolAllocator&) = default; /** * Returns a block of memory from the pool. * * @param n Number of bytes in the block. + * @return A block of memory from the pool. */ [[nodiscard]] constexpr T* allocate(size_t n) { - return static_cast(m_memoryResource->allocate(n)); + return static_cast(m_memory_resource->allocate(n)); } /** @@ -137,17 +172,17 @@ class PoolAllocator { * @param n Number of bytes in the block. */ constexpr void deallocate(T* p, size_t n) { - m_memoryResource->deallocate(p, n); + m_memory_resource->deallocate(p, n); } private: - PoolResource* m_memoryResource; + PoolResource* m_memory_resource; }; /** * Returns a global pool memory resource. */ -SLEIPNIR_DLLEXPORT PoolResource& GlobalPoolResource(); +SLEIPNIR_DLLEXPORT PoolResource& global_pool_resource(); /** * Returns an allocator for a global pool memory resource. @@ -155,8 +190,8 @@ SLEIPNIR_DLLEXPORT PoolResource& GlobalPoolResource(); * @tparam T The type of object in the pool. */ template -PoolAllocator GlobalPoolAllocator() { - return PoolAllocator{&GlobalPoolResource()}; +PoolAllocator global_pool_allocator() { + return PoolAllocator{&global_pool_resource()}; } -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Print.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print.hpp similarity index 81% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Print.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print.hpp index c01fd4ac67..055d5c9fa2 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Print.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print.hpp @@ -2,6 +2,7 @@ #pragma once +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS #include #include #include @@ -12,7 +13,11 @@ #include #endif -namespace sleipnir { +#endif + +namespace slp { + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS /** * Wrapper around fmt::print() that squelches write failure exceptions. @@ -58,4 +63,14 @@ inline void println(std::FILE* f, fmt::format_string fmt, T&&... args) { } } -} // namespace sleipnir +#else + +template +inline void print([[maybe_unused]] Args&&... args) {} + +template +inline void println([[maybe_unused]] Args&&... args) {} + +#endif + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/spy.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/spy.hpp new file mode 100644 index 0000000000..8cd7d4353a --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/spy.hpp @@ -0,0 +1,127 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + +#include + +#include +#include +#include +#include + +#include +#include + +#include "sleipnir/util/symbol_exports.hpp" + +namespace slp { + +/** + * Writes the sparsity pattern of a sparse matrix to a file. + * + * Each file represents the sparsity pattern of one matrix over time. spy.py + * can display it as an animation. + * + * The file starts with the following header: + *
    + *
  1. Plot title (length as a little-endian int32, then characters)
    2. + *
  2. Row label (length as a little-endian int32, then characters)
    3. + *
  3. Column label (length as a little-endian int32, then characters)
    4. + *
+ * + * Then, each sparsity pattern starts with: + *
    + *
  1. Number of coordinates as a little-endian int32
    2. + *
+ * + * followed by that many coordinates in the following format: + *
    + *
  1. Row index as a little-endian int32
    2. + *
  2. Column index as a little-endian int32
    3. + *
  3. Sign as a character ('+' for positive, '-' for negative, or '0' for + * zero)
    4. + *
+ * + * @param[out] file A file stream. + * @param[in] mat The sparse matrix. + */ +class SLEIPNIR_DLLEXPORT Spy { + public: + /** + * Constructs a Spy instance. + * + * @param filename The filename. + * @param title Plot title. + * @param row_label Row label. + * @param col_label Column label. + * @param rows The sparse matrix's number of rows. + * @param cols The sparse matrix's number of columns. + */ + Spy(std::string_view filename, std::string_view title, + std::string_view row_label, std::string_view col_label, int rows, + int cols) + : m_file{std::string{filename}, std::ios::binary} { + // Write title + write32le(title.size()); + m_file.write(title.data(), title.size()); + + // Write row label + write32le(row_label.size()); + m_file.write(row_label.data(), row_label.size()); + + // Write column label + write32le(col_label.size()); + m_file.write(col_label.data(), col_label.size()); + + // Write row and column counts + write32le(rows); + write32le(cols); + } + + /** + * Adds a matrix to the file. + * + * @param mat The matrix. + */ + void add(const Eigen::SparseMatrix& mat) { + // Write number of coordinates + write32le(mat.nonZeros()); + + // Write coordinates + for (int k = 0; k < mat.outerSize(); ++k) { + for (Eigen::SparseMatrix::InnerIterator it{mat, k}; it; ++it) { + write32le(it.row()); + write32le(it.col()); + if (it.value() > 0.0) { + m_file << '+'; + } else if (it.value() < 0.0) { + m_file << '-'; + } else { + m_file << '0'; + } + } + } + } + + private: + std::ofstream m_file; + + /** + * Writes a 32-bit signed integer to the file as little-endian. + * + * @param num A 32-bit signed integer. + */ + void write32le(int32_t num) { + if constexpr (std::endian::native != std::endian::little) { + num = wpi::byteswap(num); + } + m_file.write(reinterpret_cast(&num), sizeof(num)); + } +}; + +} // namespace slp + +#endif diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/SymbolExports.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/symbol_exports.hpp similarity index 100% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/SymbolExports.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/symbol_exports.hpp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide b/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide index ad739cea6d..1b6652d3d5 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide @@ -8,5 +8,5 @@ cppSrcFileInclude { includeOtherLibs { ^Eigen/ - ^wpi/ + ^gch/ } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/VariableMatrix.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/VariableMatrix.cpp deleted file mode 100644 index 589ff8822a..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/VariableMatrix.cpp +++ /dev/null @@ -1,91 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#include "sleipnir/autodiff/VariableMatrix.hpp" - -#include - -namespace sleipnir { - -VariableMatrix Solve(const VariableMatrix& A, const VariableMatrix& B) { - // m x n * n x p = m x p - Assert(A.Rows() == B.Rows()); - - if (A.Rows() == 1 && A.Cols() == 1) { - // Compute optimal inverse instead of using Eigen's general solver - return B(0, 0) / A(0, 0); - } else if (A.Rows() == 2 && A.Cols() == 2) { - // Compute optimal inverse instead of using Eigen's general solver - // - // [a b]⁻¹ ___1___ [ d −b] - // [c d] = ad − bc [−c a] - - const auto& a = A(0, 0); - const auto& b = A(0, 1); - const auto& c = A(1, 0); - const auto& d = A(1, 1); - - sleipnir::VariableMatrix Ainv{{d, -b}, {-c, a}}; - auto detA = a * d - b * c; - Ainv /= detA; - - return Ainv * B; - } else if (A.Rows() == 3 && A.Cols() == 3) { - // Compute optimal inverse instead of using Eigen's general solver - // - // [a b c]⁻¹ - // [d e f] - // [g h i] - // 1 [ei − fh ch − bi bf − ce] - // = --------------------------------- [fg − di ai − cg cd − af] - // aei − afh − bdi + bfg + cdh − ceg [dh − eg bg − ah ae − bd] - - const auto& a = A(0, 0); - const auto& b = A(0, 1); - const auto& c = A(0, 2); - const auto& d = A(1, 0); - const auto& e = A(1, 1); - const auto& f = A(1, 2); - const auto& g = A(2, 0); - const auto& h = A(2, 1); - const auto& i = A(2, 2); - - sleipnir::VariableMatrix Ainv{ - {e * i - f * h, c * h - b * i, b * f - c * e}, - {f * g - d * i, a * i - c * g, c * d - a * f}, - {d * h - e * g, b * g - a * h, a * e - b * d}}; - auto detA = - a * e * i - a * f * h - b * d * i + b * f * g + c * d * h - c * e * g; - Ainv /= detA; - - return Ainv * B; - } else { - using MatrixXv = Eigen::Matrix; - - MatrixXv eigenA{A.Rows(), A.Cols()}; - for (int row = 0; row < A.Rows(); ++row) { - for (int col = 0; col < A.Cols(); ++col) { - eigenA(row, col) = A(row, col); - } - } - - MatrixXv eigenB{B.Rows(), B.Cols()}; - for (int row = 0; row < B.Rows(); ++row) { - for (int col = 0; col < B.Cols(); ++col) { - eigenB(row, col) = B(row, col); - } - } - - MatrixXv eigenX = eigenA.householderQr().solve(eigenB); - - VariableMatrix X{A.Cols(), B.Cols()}; - for (int row = 0; row < X.Rows(); ++row) { - for (int col = 0; col < X.Cols(); ++col) { - X(row, col) = eigenX(row, col); - } - } - - return X; - } -} - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/variable_matrix.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/variable_matrix.cpp new file mode 100644 index 0000000000..71f8153d34 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/variable_matrix.cpp @@ -0,0 +1,259 @@ +// Copyright (c) Sleipnir contributors + +#include "sleipnir/autodiff/variable_matrix.hpp" + +#include + +namespace slp { + +VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) { + // m x n * n x p = m x p + slp_assert(A.rows() == B.rows()); + + if (A.rows() == 1 && A.cols() == 1) { + // Compute optimal inverse instead of using Eigen's general solver + return B(0, 0) / A(0, 0); + } else if (A.rows() == 2 && A.cols() == 2) { + // Compute optimal inverse instead of using Eigen's general solver + // + // [a b]⁻¹ ___1___ [ d −b] + // [c d] = ad − bc [−c a] + + const auto& a = A(0, 0); + const auto& b = A(0, 1); + const auto& c = A(1, 0); + const auto& d = A(1, 1); + + slp::VariableMatrix adj_A{{d, -b}, {-c, a}}; + auto det_A = a * d - b * c; + return adj_A / det_A * B; + } else if (A.rows() == 3 && A.cols() == 3) { + // Compute optimal inverse instead of using Eigen's general solver + // + // [a b c]⁻¹ + // [d e f] + // [g h i] + // 1 [ei − fh ch − bi bf − ce] + // = ------------------------------------ [fg − di ai − cg cd − af] + // a(ei − fh) + b(fg − di) + c(dh − eg) [dh − eg bg − ah ae − bd] + // + // https://www.wolframalpha.com/input?i=inverse+%7B%7Ba%2C+b%2C+c%7D%2C+%7Bd%2C+e%2C+f%7D%2C+%7Bg%2C+h%2C+i%7D%7D + + const auto& a = A(0, 0); + const auto& b = A(0, 1); + const auto& c = A(0, 2); + const auto& d = A(1, 0); + const auto& e = A(1, 1); + const auto& f = A(1, 2); + const auto& g = A(2, 0); + const auto& h = A(2, 1); + const auto& i = A(2, 2); + + auto ae = a * e; + auto af = a * f; + auto ah = a * h; + auto ai = a * i; + auto bd = b * d; + auto bf = b * f; + auto bg = b * g; + auto bi = b * i; + auto cd = c * d; + auto ce = c * e; + auto cg = c * g; + auto ch = c * h; + auto dh = d * h; + auto di = d * i; + auto eg = e * g; + auto ei = e * i; + auto fg = f * g; + auto fh = f * h; + + auto adj_A00 = ei - fh; + auto adj_A10 = fg - di; + auto adj_A20 = dh - eg; + + slp::VariableMatrix adj_A{{adj_A00, ch - bi, bf - ce}, + {adj_A10, ai - cg, cd - af}, + {adj_A20, bg - ah, ae - bd}}; + auto det_A = a * adj_A00 + b * adj_A10 + c * adj_A20; + return adj_A / det_A * B; + } else if (A.rows() == 4 && A.cols() == 4) { + // Compute optimal inverse instead of using Eigen's general solver + // + // [a b c d]⁻¹ + // [e f g h] + // [i j k l] + // [m n o p] + // + // https://www.wolframalpha.com/input?i=inverse+%7B%7Ba%2C+b%2C+c%2C+d%7D%2C+%7Be%2C+f%2C+g%2C+h%7D%2C+%7Bi%2C+j%2C+k%2C+l%7D%2C+%7Bm%2C+n%2C+o%2C+p%7D%7D + + const auto& a = A(0, 0); + const auto& b = A(0, 1); + const auto& c = A(0, 2); + const auto& d = A(0, 3); + const auto& e = A(1, 0); + const auto& f = A(1, 1); + const auto& g = A(1, 2); + const auto& h = A(1, 3); + const auto& i = A(2, 0); + const auto& j = A(2, 1); + const auto& k = A(2, 2); + const auto& l = A(2, 3); + const auto& m = A(3, 0); + const auto& n = A(3, 1); + const auto& o = A(3, 2); + const auto& p = A(3, 3); + + auto afk = a * f * k; + auto afl = a * f * l; + auto afo = a * f * o; + auto afp = a * f * p; + auto agj = a * g * j; + auto agl = a * g * l; + auto agn = a * g * n; + auto agp = a * g * p; + auto ahj = a * h * j; + auto ahk = a * h * k; + auto ahn = a * h * n; + auto aho = a * h * o; + auto ajo = a * j * o; + auto ajp = a * j * p; + auto akn = a * k * n; + auto akp = a * k * p; + auto aln = a * l * n; + auto alo = a * l * o; + auto bek = b * e * k; + auto bel = b * e * l; + auto beo = b * e * o; + auto bep = b * e * p; + auto bgi = b * g * i; + auto bgl = b * g * l; + auto bgm = b * g * m; + auto bgp = b * g * p; + auto bhi = b * h * i; + auto bhk = b * h * k; + auto bhm = b * h * m; + auto bho = b * h * o; + auto bio = b * i * o; + auto bip = b * i * p; + auto bjp = b * j * p; + auto bkm = b * k * m; + auto bkp = b * k * p; + auto blm = b * l * m; + auto blo = b * l * o; + auto cej = c * e * j; + auto cel = c * e * l; + auto cen = c * e * n; + auto cep = c * e * p; + auto cfi = c * f * i; + auto cfl = c * f * l; + auto cfm = c * f * m; + auto cfp = c * f * p; + auto chi = c * h * i; + auto chj = c * h * j; + auto chm = c * h * m; + auto chn = c * h * n; + auto cin = c * i * n; + auto cip = c * i * p; + auto cjm = c * j * m; + auto cjp = c * j * p; + auto clm = c * l * m; + auto cln = c * l * n; + auto dej = d * e * j; + auto dek = d * e * k; + auto den = d * e * n; + auto deo = d * e * o; + auto dfi = d * f * i; + auto dfk = d * f * k; + auto dfm = d * f * m; + auto dfo = d * f * o; + auto dgi = d * g * i; + auto dgj = d * g * j; + auto dgm = d * g * m; + auto dgn = d * g * n; + auto din = d * i * n; + auto dio = d * i * o; + auto djm = d * j * m; + auto djo = d * j * o; + auto dkm = d * k * m; + auto dkn = d * k * n; + auto ejo = e * j * o; + auto ejp = e * j * p; + auto ekn = e * k * n; + auto ekp = e * k * p; + auto eln = e * l * n; + auto elo = e * l * o; + auto fio = f * i * o; + auto fip = f * i * p; + auto fkm = f * k * m; + auto fkp = f * k * p; + auto flm = f * l * m; + auto flo = f * l * o; + auto gin = g * i * n; + auto gip = g * i * p; + auto gjm = g * j * m; + auto gjp = g * j * p; + auto glm = g * l * m; + auto gln = g * l * n; + auto hin = h * i * n; + auto hio = h * i * o; + auto hjm = h * j * m; + auto hjo = h * j * o; + auto hkm = h * k * m; + auto hkn = h * k * n; + + auto adj_A00 = fkp - flo - gjp + gln + hjo - hkn; + auto adj_A01 = -bkp + blo + cjp - cln - djo + dkn; + auto adj_A02 = bgp - bho - cfp + chn + dfo - dgn; + auto adj_A03 = -bgl + bhk + cfl - chj - dfk + dgj; + auto adj_A10 = -ekp + elo + gip - glm - hio + hkm; + auto adj_A11 = akp - alo - cip + clm + dio - dkm; + auto adj_A12 = -agp + aho + cep - chm - deo + dgm; + auto adj_A13 = agl - ahk - cel + chi + dek - dgi; + auto adj_A20 = ejp - eln - fip + flm + hin - hjm; + auto adj_A21 = -ajp + aln + bip - blm - din + djm; + auto adj_A22 = afp - ahn - bep + bhm + den - dfm; + auto adj_A23 = -afl + ahj + bel - bhi - dej + dfi; + auto adj_A30 = -ejo + ekn + fio - fkm - gin + gjm; + // NOLINTNEXTLINE + auto adj_A31 = ajo - akn - bio + bkm + cin - cjm; + auto adj_A32 = -afo + agn + beo - bgm - cen + cfm; + auto adj_A33 = afk - agj - bek + bgi + cej - cfi; + + slp::VariableMatrix adj_A{{adj_A00, adj_A01, adj_A02, adj_A03}, + {adj_A10, adj_A11, adj_A12, adj_A13}, + {adj_A20, adj_A21, adj_A22, adj_A23}, + {adj_A30, adj_A31, adj_A32, adj_A33}}; + auto det_A = a * adj_A00 + b * adj_A10 + c * adj_A20 + d * adj_A30; + return adj_A / det_A * B; + } else { + using MatrixXv = Eigen::Matrix; + + MatrixXv eigen_A{A.rows(), A.cols()}; + for (int row = 0; row < A.rows(); ++row) { + for (int col = 0; col < A.cols(); ++col) { + eigen_A(row, col) = A(row, col); + } + } + + MatrixXv eigen_B{B.rows(), B.cols()}; + for (int row = 0; row < B.rows(); ++row) { + for (int col = 0; col < B.cols(); ++col) { + eigen_B(row, col) = B(row, col); + } + } + + MatrixXv eigen_X = eigen_A.householderQr().solve(eigen_B); + + VariableMatrix X{A.cols(), B.cols()}; + for (int row = 0; row < X.rows(); ++row) { + for (int col = 0; col < X.cols(); ++col) { + X(row, col) = eigen_X(row, col); + } + } + + return X; + } +} + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/Inertia.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/Inertia.hpp deleted file mode 100644 index 8ef2ebe0ce..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/Inertia.hpp +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -#include "sleipnir/util/Concepts.hpp" - -namespace sleipnir { - -/** - * Represents the inertia of a matrix (the number of positive, negative, and - * zero eigenvalues). - */ -class Inertia { - public: - size_t positive = 0; - size_t negative = 0; - size_t zero = 0; - - constexpr Inertia() = default; - - /** - * Constructs the Inertia type with the given number of positive, negative, - * and zero eigenvalues. - * - * @param positive The number of positive eigenvalues. - * @param negative The number of negative eigenvalues. - * @param zero The number of zero eigenvalues. - */ - constexpr Inertia(size_t positive, size_t negative, size_t zero) - : positive{positive}, negative{negative}, zero{zero} {} - - /** - * Constructs the Inertia type with the inertia of the given LDLT - * decomposition. - * - * @tparam Solver Eigen sparse linear system solver. - * @param solver The LDLT decomposition of which to compute the inertia. - */ - template - explicit Inertia(const Solver& solver) { - const auto& D = solver.vectorD(); - for (int row = 0; row < D.rows(); ++row) { - if (D(row) > 0.0) { - ++positive; - } else if (D(row) < 0.0) { - ++negative; - } else { - ++zero; - } - } - } - - bool operator==(const Inertia&) const = default; -}; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/RegularizedLDLT.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/RegularizedLDLT.hpp deleted file mode 100644 index d2488b7998..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/RegularizedLDLT.hpp +++ /dev/null @@ -1,179 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include -#include - -#include -#include -#include - -#include "optimization/Inertia.hpp" - -// See docs/algorithms.md#Works_cited for citation definitions - -namespace sleipnir { - -/** - * Solves systems of linear equations using a regularized LDLT factorization. - */ -class RegularizedLDLT { - public: - using Solver = Eigen::SimplicialLDLT, - Eigen::Lower, Eigen::AMDOrdering>; - - /** - * Constructs a RegularizedLDLT instance. - */ - RegularizedLDLT() = default; - - /** - * Reports whether previous computation was successful. - */ - Eigen::ComputationInfo Info() { return m_info; } - - /** - * Computes the regularized LDLT factorization of a matrix. - * - * @param lhs Left-hand side of the system. - * @param numEqualityConstraints The number of equality constraints in the - * system. - * @param μ The barrier parameter for the current interior-point iteration. - */ - void Compute(const Eigen::SparseMatrix& lhs, - size_t numEqualityConstraints, double μ) { - // The regularization procedure is based on algorithm B.1 of [1] - m_numDecisionVariables = lhs.rows() - numEqualityConstraints; - m_numEqualityConstraints = numEqualityConstraints; - - const Inertia idealInertia{m_numDecisionVariables, m_numEqualityConstraints, - 0}; - Inertia inertia; - - double δ = 0.0; - double γ = 0.0; - - AnalyzePattern(lhs); - m_solver.factorize(lhs); - - if (m_solver.info() == Eigen::Success) { - inertia = Inertia{m_solver}; - - // If the inertia is ideal, don't regularize the system - if (inertia == idealInertia) { - m_info = Eigen::Success; - return; - } - } - - // If the decomposition succeeded and the inertia has some zero eigenvalues, - // or the decomposition failed, regularize the equality constraints - if ((m_solver.info() == Eigen::Success && inertia.zero > 0) || - m_solver.info() != Eigen::Success) { - γ = 1e-8 * std::pow(μ, 0.25); - } - - // Also regularize the Hessian. If the Hessian wasn't regularized in a - // previous run of Compute(), start at a small value of δ. Otherwise, - // attempt a δ half as big as the previous run so δ can trend downwards over - // time. - if (m_δOld == 0.0) { - δ = 1e-4; - } else { - δ = m_δOld / 2.0; - } - - while (true) { - // Regularize lhs by adding a multiple of the identity matrix - // - // lhs = [H + AᵢᵀΣAᵢ + δI Aₑᵀ] - // [ Aₑ −γI ] - Eigen::SparseMatrix lhsReg = lhs + Regularization(δ, γ); - AnalyzePattern(lhsReg); - m_solver.factorize(lhsReg); - inertia = Inertia{m_solver}; - - // If the inertia is ideal, store that value of δ and return. - // Otherwise, increase δ by an order of magnitude and try again. - if (inertia == idealInertia) { - m_δOld = δ; - m_info = Eigen::Success; - return; - } else { - δ *= 10.0; - - // If the Hessian perturbation is too high, report failure. This can - // happen due to a rank-deficient equality constraint Jacobian with - // linearly dependent constraints. - if (δ > 1e20) { - m_info = Eigen::NumericalIssue; - return; - } - } - } - } - - /** - * Solve the system of equations using a regularized LDLT factorization. - * - * @param rhs Right-hand side of the system. - */ - template - auto Solve(const Eigen::MatrixBase& rhs) { - return m_solver.solve(rhs); - } - - private: - Solver m_solver; - - Eigen::ComputationInfo m_info = Eigen::Success; - - /// The number of decision variables in the system. - size_t m_numDecisionVariables = 0; - - /// The number of equality constraints in the system. - size_t m_numEqualityConstraints = 0; - - /// The value of δ from the previous run of Compute(). - double m_δOld = 0.0; - - // Number of non-zeros in LHS. - int m_nonZeros = -1; - - /** - * Reanalize LHS matrix's sparsity pattern if it changed. - * - * @param lhs Matrix to analyze. - */ - void AnalyzePattern(const Eigen::SparseMatrix& lhs) { - int nonZeros = lhs.nonZeros(); - if (m_nonZeros != nonZeros) { - m_solver.analyzePattern(lhs); - m_nonZeros = nonZeros; - } - } - - /** - * Returns regularization matrix. - * - * @param δ The Hessian regularization factor. - * @param γ The equality constraint Jacobian regularization factor. - */ - Eigen::SparseMatrix Regularization(double δ, double γ) { - Eigen::VectorXd vec{m_numDecisionVariables + m_numEqualityConstraints}; - size_t row = 0; - while (row < m_numDecisionVariables) { - vec(row) = δ; - ++row; - } - while (row < m_numDecisionVariables + m_numEqualityConstraints) { - vec(row) = -γ; - ++row; - } - - return Eigen::SparseMatrix{vec.asDiagonal()}; - } -}; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/bounds.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/bounds.hpp new file mode 100644 index 0000000000..9b68a72a8d --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/bounds.hpp @@ -0,0 +1,220 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +#include "sleipnir/autodiff/expression_type.hpp" +#include "sleipnir/autodiff/variable.hpp" +#include "sleipnir/util/assert.hpp" + +// See docs/algorithms.md#Works_cited for citation definitions + +namespace slp { + +struct Bounds { + /// Which constraints, if any, are bound constraints. + Eigen::ArrayX bound_constraint_mask; + + /// The tightest bounds on each decision variable. + gch::small_vector> bounds; + + /// Whether or not the constraints are feasible (given previously encountered + /// bounds). + gch::small_vector> + conflicting_bound_indices; +}; + +/** + * A "bound constraint" is any linear constraint in one scalar variable. + * + * Computes which constraints, if any, are bound constraints, the tightest + * bounds on each decision variable, and whether or not they're feasible (given + * previously encountered bounds), + * + * @param decision_variables Decision variables corresponding to each column of + * A_i. + * @param inequality_constraints Variables representing the left-hand side of + * cᵢ(decision_variables) ≥ 0. + * @param A_i The Jacobian of inequality_constraints wrt decision_variables, + * evaluated anywhere, in *row-major* storage; in practice, since we typically + * store Jacobians column-major, the user of this function must perform a + * transpose. + */ +inline Bounds get_bounds( + std::span decision_variables, + std::span inequality_constraints, + const Eigen::SparseMatrix& A_i) { + // TODO: A blocked, out-of-place transpose should be much faster than + // traversing row major on a column major matrix unless we have few linear + // constraints (using a heuristic to choose between this and staying column + // major based on the number of constraints would be an easy performance + // improvement.) + + // NB: Casting to long is unspecified if the size of decision_variable.size() + // is greater than the max long value, but then we wouldn't be able to fill + // A_i correctly anyway. + slp_assert(static_cast(decision_variables.size()) == + A_i.innerSize()); + slp_assert(static_cast(inequality_constraints.size()) == + A_i.outerSize()); + + // Maps each decision variable's index to the indices of its upper and lower + // bounds if they exist, or NO_BOUND if they do not; used only for bookkeeping + // to compute conflicting bounds + constexpr Eigen::Index NO_BOUND = -1; + gch::small_vector> + decision_var_indices_to_constraint_indices{decision_variables.size(), + {NO_BOUND, NO_BOUND}}; + // Lists pairs of indices of bound constraints in the inequality constraint + // list that conflict with each other + gch::small_vector> + conflicting_bound_indices; + + // Maps each decision variable's index to its upper and lower bounds + gch::small_vector> decision_var_indices_to_bounds{ + decision_variables.size(), + {-std::numeric_limits::infinity(), + std::numeric_limits::infinity()}}; + + // Vector corresponding to the inequality constraints where the i-th element + // is 1 if the i-th inequality constraint is a bound and 0 otherwise. + Eigen::ArrayX bound_constraint_mask{inequality_constraints.size()}; + bound_constraint_mask.fill(false); + + for (decltype(inequality_constraints)::size_type constraint_index = 0; + constraint_index < inequality_constraints.size(); ++constraint_index) { + // A constraint is a bound iff it is linear and its gradient has a + // single nonzero value. + if (inequality_constraints[constraint_index].type() != + ExpressionType::LINEAR) { + continue; + } + const Eigen::SparseVector& row_A_i = + A_i.innerVector(constraint_index); + const auto non_zeros = row_A_i.nonZeros(); + slp_assert(non_zeros != 0); + if (non_zeros > 1) { + // Constraint is in more than one variable and therefore not a bound. + continue; + } + + // Claim: The bound given by a bound constraint is the constraint evaluated + // at zero divided by the nonzero element of the constraint's gradient. + // + // Proof: If c(x) is a bound constraint, then by definition c is a linear + // function in one variable, hence there exist a, b ∈ ℝ s.t. c(x) = axᵢ + b + // and a ≠ 0. The gradient of c is then aeᵢ (where eᵢ denotes the i-th basis + // element), and c(0) = b. If c(x) ≥ 0, then since either a < 0 or a > 0, we + // have either x ≤ -b/a or x ≥ -b/a, respectively. ∎ + Eigen::SparseVector::InnerIterator row_iter(row_A_i); + const auto constraint_coefficient = + row_iter + .value(); // The nonzero value of the j-th constraint's gradient. + const auto decision_variable_index = row_iter.index(); + const auto decision_variable_value = + decision_variables[decision_variable_index].value(); + double constraint_constant; + // We need to evaluate this constraint *exactly* at zero. + if (decision_variable_value != 0.0) { + decision_variables[decision_variable_index].set_value(0.0); + constraint_constant = inequality_constraints[constraint_index].value(); + decision_variables[decision_variable_index].set_value( + decision_variable_value); + } else { + constraint_constant = inequality_constraints[constraint_index].value(); + } + + // Shouldn't happen since the constraint is supposed to be linear and not a + // constant. + slp_assert(constraint_coefficient != 0.0); + + // We should always get a finite value when evaluating the constraint at + // x = 0 since the constraint is linear. + slp_assert(std::isfinite(constraint_constant)); + + // This is possible if the user has provided a starting point at which their + // problem is ill-defined. + slp_assert(std::isfinite(constraint_coefficient)); + + // Update bounds; we assume constraints of the form c(x) ≥ 0. + auto& [lower_bound, upper_bound] = + decision_var_indices_to_bounds[decision_variable_index]; + auto& [lower_index, upper_index] = + decision_var_indices_to_constraint_indices[decision_variable_index]; + const auto detected_bound = -constraint_constant / constraint_coefficient; + if (constraint_coefficient < 0.0 && detected_bound < upper_bound) { + upper_bound = detected_bound; + upper_index = constraint_index; + } else if (constraint_coefficient > 0.0 && detected_bound > lower_bound) { + lower_bound = detected_bound; + lower_index = constraint_index; + } + + // Update conflicting bounds + if (lower_bound > upper_bound) { + conflicting_bound_indices.emplace_back(lower_index, upper_index); + } + + // Set the bound constraint mask appropriately. + bound_constraint_mask[constraint_index] = true; + } + return {bound_constraint_mask, decision_var_indices_to_bounds, + conflicting_bound_indices}; +} + +/** + * Projects the decision variables onto the given bounds, while ensuring some + * configurable distance from the boundary if possible. This is designed to + * match the algorithm given in section 3.6 of [2]. + * + * @param x A vector of decision variables. + * @param decision_variable_indices_to_bounds An array of bounds (stored [lower, + * upper]) for each decision variable in x. + * @param κ_1 A constant controlling distance from the lower or upper bound when + * the difference between the upper and lower bound is small. + * @param κ_2 A constant controlling distance from the lower or upper bound when + * the difference between the upper and lower bound is large (including when + * one of the bounds is ±∞). + */ +template + requires(static_cast(Eigen::DenseBase::IsVectorAtCompileTime)) +inline void project_onto_bounds( + Eigen::DenseBase& x, + std::span::Scalar, + typename Eigen::DenseBase::Scalar>> + decision_variable_indices_to_bounds, + const typename Eigen::DenseBase::Scalar κ_1 = 1e-2, + const typename Eigen::DenseBase::Scalar κ_2 = 1e-2) { + slp_assert(κ_1 > 0 && κ_2 > 0 && κ_2 < 0.5); + + Eigen::Index decision_variable_idx = 0; + for (const auto& [lower, upper] : decision_variable_indices_to_bounds) { + typename Eigen::DenseBase::Scalar& x_i = + x[decision_variable_idx++]; + + // We assume that bound infeasibility is handled elsewhere. + slp_assert(lower <= upper); + + // See B.2 in [5] and section 3.6 in [2] + if (std::isfinite(lower) && std::isfinite(upper)) { + auto p_L = + std::min(κ_1 * std::max(1.0, std::abs(lower)), κ_2 * (upper - lower)); + auto p_U = + std::min(κ_1 * std::max(1.0, std::abs(upper)), κ_2 * (upper - lower)); + x_i = std::min(std::max(lower + p_L, x_i), upper - p_U); + } else if (std::isfinite(lower)) { + x_i = std::max(x_i, lower + κ_1 * std::max(1.0, std::abs(lower))); + } else if (std::isfinite(upper)) { + x_i = std::min(x_i, upper - κ_1 * std::max(1.0, std::abs(upper))); + } + } +} +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/inertia.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/inertia.hpp new file mode 100644 index 0000000000..8666c3ed68 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/inertia.hpp @@ -0,0 +1,53 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +namespace slp { + +/** + * Represents the inertia of a matrix (the number of positive, negative, and + * zero eigenvalues). + */ +class Inertia { + public: + int positive = 0; + int negative = 0; + int zero = 0; + + constexpr Inertia() = default; + + /** + * Constructs Inertia with the given number of positive, negative, and zero + * eigenvalues. + * + * @param positive The number of positive eigenvalues. + * @param negative The number of negative eigenvalues. + * @param zero The number of zero eigenvalues. + */ + constexpr Inertia(int positive, int negative, int zero) + : positive{positive}, negative{negative}, zero{zero} {} + + /** + * Constructs Inertia from the D matrix of an LDLT decomposition + * (see https://en.wikipedia.org/wiki/Sylvester's_law_of_inertia). + * + * @param D The D matrix of an LDLT decomposition in vector form. + */ + explicit Inertia(const Eigen::VectorXd& D) { + for (const auto& elem : D) { + if (elem > 0.0) { + ++positive; + } else if (elem < 0.0) { + ++negative; + } else { + ++zero; + } + } + } + + bool operator==(const Inertia&) const = default; +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/problem.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/problem.cpp new file mode 100644 index 0000000000..5532b39624 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/problem.cpp @@ -0,0 +1,391 @@ +// Copyright (c) Sleipnir contributors + +#include "sleipnir/optimization/problem.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +#include "optimization/bounds.hpp" +#include "sleipnir/autodiff/expression_type.hpp" +#include "sleipnir/autodiff/gradient.hpp" +#include "sleipnir/autodiff/hessian.hpp" +#include "sleipnir/autodiff/jacobian.hpp" +#include "sleipnir/autodiff/variable.hpp" +#include "sleipnir/autodiff/variable_matrix.hpp" +#include "sleipnir/optimization/solver/exit_status.hpp" +#include "sleipnir/optimization/solver/interior_point.hpp" +#include "sleipnir/optimization/solver/newton.hpp" +#include "sleipnir/optimization/solver/options.hpp" +#include "sleipnir/optimization/solver/sqp.hpp" +#include "sleipnir/util/print.hpp" +#include "sleipnir/util/spy.hpp" +#include "util/print_diagnostics.hpp" +#include "util/setup_profiler.hpp" + +namespace slp { + +ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) { + // Create the initial value column vector + Eigen::VectorXd x{m_decision_variables.size()}; + for (size_t i = 0; i < m_decision_variables.size(); ++i) { + x[i] = m_decision_variables[i].value(); + } + + if (options.diagnostics) { + print_exit_conditions(options); + print_problem_analysis(); + } + + // Get the highest order constraint expression types + auto f_type = cost_function_type(); + auto c_e_type = equality_constraint_type(); + auto c_i_type = inequality_constraint_type(); + + // If the problem is empty or constant, there's nothing to do + if (f_type <= ExpressionType::CONSTANT && + c_e_type <= ExpressionType::CONSTANT && + c_i_type <= ExpressionType::CONSTANT) { +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + if (options.diagnostics) { + slp::println("\nInvoking no-op solver...\n"); + } +#endif + return ExitStatus::SUCCESS; + } + + gch::small_vector ad_setup_profilers; + ad_setup_profilers.emplace_back("setup").start(); + + VariableMatrix x_ad{m_decision_variables}; + + // Set up cost function + Variable f = m_f.value_or(0.0); + + // Set up gradient autodiff + ad_setup_profilers.emplace_back(" ↳ ∇f(x)").start(); + Gradient g{f, x_ad}; + ad_setup_profilers.back().stop(); + + [[maybe_unused]] + int num_decision_variables = m_decision_variables.size(); + [[maybe_unused]] + int num_equality_constraints = m_equality_constraints.size(); + [[maybe_unused]] + int num_inequality_constraints = m_inequality_constraints.size(); + + // Solve the optimization problem + ExitStatus status; + if (m_equality_constraints.empty() && m_inequality_constraints.empty()) { + if (options.diagnostics) { + slp::println("\nInvoking Newton solver...\n"); + } + + // Set up Lagrangian Hessian autodiff + ad_setup_profilers.emplace_back(" ↳ ∇²ₓₓL").start(); + Hessian H{f, x_ad}; + ad_setup_profilers.back().stop(); + + ad_setup_profilers[0].stop(); + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + // Sparsity pattern files written when spy flag is set + std::unique_ptr H_spy; + + if (spy) { + H_spy = std::make_unique( + "H.spy", "Hessian", "Decision variables", "Decision variables", + num_decision_variables, num_decision_variables); + m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool { + H_spy->add(info.H); + return false; + }); + } +#endif + + // Invoke Newton solver + status = newton( + NewtonMatrixCallbacks{ + [&](const Eigen::VectorXd& x) -> double { + x_ad.set_value(x); + return f.value(); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseVector { + x_ad.set_value(x); + return g.value(); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix { + x_ad.set_value(x); + return H.value(); + }}, + m_iteration_callbacks, options, x); + } else if (m_inequality_constraints.empty()) { + if (options.diagnostics) { + slp::println("\nInvoking SQP solver\n"); + } + + VariableMatrix c_e_ad{m_equality_constraints}; + + // Set up equality constraint Jacobian autodiff + ad_setup_profilers.emplace_back(" ↳ ∂cₑ/∂x").start(); + Jacobian A_e{c_e_ad, x_ad}; + ad_setup_profilers.back().stop(); + + // Set up Lagrangian + VariableMatrix y_ad(num_equality_constraints); + Variable L = f - (y_ad.T() * c_e_ad)[0]; + + // Set up Lagrangian Hessian autodiff + ad_setup_profilers.emplace_back(" ↳ ∇²ₓₓL").start(); + Hessian H{L, x_ad}; + ad_setup_profilers.back().stop(); + + ad_setup_profilers[0].stop(); + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + // Sparsity pattern files written when spy flag is set + std::unique_ptr H_spy; + std::unique_ptr A_e_spy; + + if (spy) { + H_spy = std::make_unique( + "H.spy", "Hessian", "Decision variables", "Decision variables", + num_decision_variables, num_decision_variables); + A_e_spy = std::make_unique("A_e.spy", "Equality constraint Jacobian", + "Constraints", "Decision variables", + num_equality_constraints, + num_decision_variables); + m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool { + H_spy->add(info.H); + A_e_spy->add(info.A_e); + return false; + }); + } +#endif + + // Invoke SQP solver + status = + sqp(SQPMatrixCallbacks{ + [&](const Eigen::VectorXd& x) -> double { + x_ad.set_value(x); + return f.value(); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseVector { + x_ad.set_value(x); + return g.value(); + }, + [&](const Eigen::VectorXd& x, + const Eigen::VectorXd& y) -> Eigen::SparseMatrix { + x_ad.set_value(x); + y_ad.set_value(y); + return H.value(); + }, + [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { + x_ad.set_value(x); + return c_e_ad.value(); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix { + x_ad.set_value(x); + return A_e.value(); + }}, + m_iteration_callbacks, options, x); + } else { + if (options.diagnostics) { + slp::println("\nInvoking IPM solver...\n"); + } + + VariableMatrix c_e_ad{m_equality_constraints}; + VariableMatrix c_i_ad{m_inequality_constraints}; + + // Set up equality constraint Jacobian autodiff + ad_setup_profilers.emplace_back(" ↳ ∂cₑ/∂x").start(); + Jacobian A_e{c_e_ad, x_ad}; + ad_setup_profilers.back().stop(); + + // Set up inequality constraint Jacobian autodiff + ad_setup_profilers.emplace_back(" ↳ ∂cᵢ/∂x").start(); + Jacobian A_i{c_i_ad, x_ad}; + ad_setup_profilers.back().stop(); + + // Set up Lagrangian + VariableMatrix y_ad(num_equality_constraints); + VariableMatrix z_ad(num_inequality_constraints); + Variable L = f - (y_ad.T() * c_e_ad)[0] - (z_ad.T() * c_i_ad)[0]; + + // Set up Lagrangian Hessian autodiff + ad_setup_profilers.emplace_back(" ↳ ∇²ₓₓL").start(); + Hessian H{L, x_ad}; + ad_setup_profilers.back().stop(); + + ad_setup_profilers[0].stop(); + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + // Sparsity pattern files written when spy flag is set + std::unique_ptr H_spy; + std::unique_ptr A_e_spy; + std::unique_ptr A_i_spy; + + if (spy) { + H_spy = std::make_unique( + "H.spy", "Hessian", "Decision variables", "Decision variables", + num_decision_variables, num_decision_variables); + A_e_spy = std::make_unique("A_e.spy", "Equality constraint Jacobian", + "Constraints", "Decision variables", + num_equality_constraints, + num_decision_variables); + A_i_spy = std::make_unique( + "A_i.spy", "Inequality constraint Jacobian", "Constraints", + "Decision variables", num_inequality_constraints, + num_decision_variables); + m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool { + H_spy->add(info.H); + A_e_spy->add(info.A_e); + A_i_spy->add(info.A_i); + return false; + }); + } +#endif + + const auto [bound_constraint_mask, bounds, conflicting_bound_indices] = + get_bounds(m_decision_variables, m_inequality_constraints, A_i.value()); + if (!conflicting_bound_indices.empty()) { + if (options.diagnostics) { + print_bound_constraint_global_infeasibility_error( + conflicting_bound_indices); + } + return ExitStatus::GLOBALLY_INFEASIBLE; + } + +#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION + project_onto_bounds(x, bounds); +#endif + // Invoke interior-point method solver + status = interior_point( + InteriorPointMatrixCallbacks{ + [&](const Eigen::VectorXd& x) -> double { + x_ad.set_value(x); + return f.value(); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseVector { + x_ad.set_value(x); + return g.value(); + }, + [&](const Eigen::VectorXd& x, const Eigen::VectorXd& y, + const Eigen::VectorXd& z) -> Eigen::SparseMatrix { + x_ad.set_value(x); + y_ad.set_value(y); + z_ad.set_value(z); + return H.value(); + }, + [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { + x_ad.set_value(x); + return c_e_ad.value(); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix { + x_ad.set_value(x); + return A_e.value(); + }, + [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { + x_ad.set_value(x); + return c_i_ad.value(); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix { + x_ad.set_value(x); + return A_i.value(); + }}, + m_iteration_callbacks, options, +#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION + bound_constraint_mask, +#endif + x); + } + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + if (spy) { + m_iteration_callbacks.pop_back(); + } +#endif + + if (options.diagnostics) { + print_autodiff_diagnostics(ad_setup_profilers); + slp::println("\nExit: {}", to_message(status)); + } + + // Assign the solution to the original Variable instances + VariableMatrix{m_decision_variables}.set_value(x); + + return status; +} + +void Problem::print_exit_conditions([[maybe_unused]] const Options& options) { + // Print possible exit conditions + slp::println("User-configured exit conditions:"); + slp::println(" ↳ error below {}", options.tolerance); + if (!m_iteration_callbacks.empty()) { + slp::println(" ↳ iteration callback requested stop"); + } + if (std::isfinite(options.max_iterations)) { + slp::println(" ↳ executed {} iterations", options.max_iterations); + } + if (std::isfinite(options.timeout.count())) { + slp::println(" ↳ {} elapsed", options.timeout.count()); + } +} + +void Problem::print_problem_analysis() { + constexpr std::array types{"no", "constant", "linear", "quadratic", + "nonlinear"}; + + // Print problem structure + slp::println("\nProblem structure:"); + slp::println(" ↳ {} cost function", + types[static_cast(cost_function_type())]); + slp::println(" ↳ {} equality constraints", + types[static_cast(equality_constraint_type())]); + slp::println(" ↳ {} inequality constraints", + types[static_cast(inequality_constraint_type())]); + + if (m_decision_variables.size() == 1) { + slp::print("\n1 decision variable\n"); + } else { + slp::print("\n{} decision variables\n", m_decision_variables.size()); + } + + auto print_constraint_types = + [](const gch::small_vector& constraints) { + std::array counts{}; + for (const auto& constraint : constraints) { + ++counts[static_cast(constraint.type())]; + } + for (size_t i = 0; i < counts.size(); ++i) { + constexpr std::array names{"empty", "constant", "linear", "quadratic", + "nonlinear"}; + const auto& count = counts[i]; + const auto& name = names[i]; + if (count > 0) { + slp::println(" ↳ {} {}", count, name); + } + } + }; + + // Print constraint types + if (m_equality_constraints.size() == 1) { + slp::println("1 equality constraint"); + } else { + slp::println("{} equality constraints", m_equality_constraints.size()); + } + print_constraint_types(m_equality_constraints); + if (m_inequality_constraints.size() == 1) { + slp::println("1 inequality constraint"); + } else { + slp::println("{} inequality constraints", m_inequality_constraints.size()); + } + print_constraint_types(m_inequality_constraints); +} + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/regularized_ldlt.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/regularized_ldlt.hpp new file mode 100644 index 0000000000..2a017e4f82 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/regularized_ldlt.hpp @@ -0,0 +1,227 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include +#include + +#include "optimization/inertia.hpp" + +// See docs/algorithms.md#Works_cited for citation definitions + +namespace slp { + +/** + * Solves systems of linear equations using a regularized LDLT factorization. + */ +class RegularizedLDLT { + public: + /** + * Constructs a RegularizedLDLT instance. + * + * @param num_decision_variables The number of decision variables in the + * system. + * @param num_equality_constraints The number of equality constraints in the + * system. + */ + RegularizedLDLT(int num_decision_variables, int num_equality_constraints) + : m_num_decision_variables{num_decision_variables}, + m_num_equality_constraints{num_equality_constraints} {} + + /** + * Reports whether previous computation was successful. + * + * @return Whether previous computation was successful. + */ + Eigen::ComputationInfo info() const { return m_info; } + + /** + * Computes the regularized LDLT factorization of a matrix. + * + * @param lhs Left-hand side of the system. + * @return The factorization. + */ + RegularizedLDLT& compute(const Eigen::SparseMatrix& lhs) { + // The regularization procedure is based on algorithm B.1 of [1] + + // Max density is 50% due to the caller only providing the lower triangle. + // We consider less than 25% to be sparse. + m_is_sparse = lhs.nonZeros() < 0.25 * lhs.size(); + + m_info = m_is_sparse ? compute_sparse(lhs).info() + : m_dense_solver.compute(lhs).info(); + + Inertia inertia; + + if (m_info == Eigen::Success) { + inertia = m_is_sparse ? Inertia{m_sparse_solver.vectorD()} + : Inertia{m_dense_solver.vectorD()}; + + // If the inertia is ideal, don't regularize the system + if (inertia == ideal_inertia) { + m_prev_δ = 0.0; + return *this; + } + } + + // Also regularize the Hessian. If the Hessian wasn't regularized in a + // previous run of compute(), start at small values of δ and γ. Otherwise, + // attempt a δ and γ half as big as the previous run so δ and γ can trend + // downwards over time. + double δ = m_prev_δ == 0.0 ? 1e-4 : m_prev_δ / 2.0; + double γ = 1e-10; + + while (true) { + // Regularize lhs by adding a multiple of the identity matrix + // + // lhs = [H + AᵢᵀΣAᵢ + δI Aₑᵀ] + // [ Aₑ −γI] + if (m_is_sparse) { + m_info = compute_sparse(lhs + regularization(δ, γ)).info(); + if (m_info == Eigen::Success) { + inertia = Inertia{m_sparse_solver.vectorD()}; + } + } else { + m_info = m_dense_solver.compute(lhs + regularization(δ, γ)).info(); + if (m_info == Eigen::Success) { + inertia = Inertia{m_dense_solver.vectorD()}; + } + } + + if (m_info == Eigen::Success) { + if (inertia == ideal_inertia) { + // If the inertia is ideal, store δ and return + m_prev_δ = δ; + return *this; + } else if (inertia.zero > 0) { + // If there's zero eigenvalues, increase δ and γ by an order of + // magnitude and try again + δ *= 10.0; + γ *= 10.0; + } else if (inertia.negative > ideal_inertia.negative) { + // If there's too many negative eigenvalues, increase δ by an order of + // magnitude and try again + δ *= 10.0; + } else if (inertia.positive > ideal_inertia.positive) { + // If there's too many positive eigenvalues, increase γ by an order of + // magnitude and try again + γ *= 10.0; + } + } else { + // If the decomposition failed, increase δ and γ by an order of + // magnitude and try again + δ *= 10.0; + γ *= 10.0; + } + + // If the Hessian perturbation is too high, report failure. This can be + // caused by ill-conditioning. + if (δ > 1e20 || γ > 1e20) { + m_info = Eigen::NumericalIssue; + return *this; + } + } + } + + /** + * Solves the system of equations using a regularized LDLT factorization. + * + * @param rhs Right-hand side of the system. + * @return The solution. + */ + template + Eigen::VectorXd solve(const Eigen::MatrixBase& rhs) { + if (m_is_sparse) { + return m_sparse_solver.solve(rhs); + } else { + return m_dense_solver.solve(rhs); + } + } + + /** + * Solves the system of equations using a regularized LDLT factorization. + * + * @param rhs Right-hand side of the system. + * @return The solution. + */ + template + Eigen::VectorXd solve(const Eigen::SparseMatrixBase& rhs) { + if (m_is_sparse) { + return m_sparse_solver.solve(rhs); + } else { + return m_dense_solver.solve(rhs.toDense()); + } + } + + /** + * Returns the Hessian regularization factor. + * + * @return Hessian regularization factor. + */ + double hessian_regularization() const { return m_prev_δ; } + + private: + using SparseSolver = Eigen::SimplicialLDLT>; + using DenseSolver = Eigen::LDLT; + + SparseSolver m_sparse_solver; + DenseSolver m_dense_solver; + bool m_is_sparse = true; + + Eigen::ComputationInfo m_info = Eigen::Success; + + /// The number of decision variables in the system. + int m_num_decision_variables = 0; + + /// The number of equality constraints in the system. + int m_num_equality_constraints = 0; + + /// The ideal system inertia. + Inertia ideal_inertia{m_num_decision_variables, m_num_equality_constraints, + 0}; + + /// The value of δ from the previous run of compute(). + double m_prev_δ = 0.0; + + // Number of non-zeros in LHS. + int m_non_zeros = -1; + + /** + * Computes factorization of a sparse matrix. + * + * @param lhs Matrix to factorize. + * @return The factorization. + */ + SparseSolver& compute_sparse(const Eigen::SparseMatrix& lhs) { + // Reanalize lhs's sparsity pattern if it changed + int non_zeros = lhs.nonZeros(); + if (m_non_zeros != non_zeros) { + m_sparse_solver.analyzePattern(lhs); + m_non_zeros = non_zeros; + } + + m_sparse_solver.factorize(lhs); + + return m_sparse_solver; + } + + /** + * Returns regularization matrix. + * + * @param δ The Hessian regularization factor. + * @param γ The equality constraint Jacobian regularization factor. + * @return Regularization matrix. + */ + Eigen::SparseMatrix regularization(double δ, double γ) { + Eigen::VectorXd vec{m_num_decision_variables + m_num_equality_constraints}; + vec.segment(0, m_num_decision_variables).setConstant(δ); + vec.segment(m_num_decision_variables, m_num_equality_constraints) + .setConstant(-γ); + + return Eigen::SparseMatrix{vec.asDiagonal()}; + } +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp deleted file mode 100644 index d3981c59d1..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp +++ /dev/null @@ -1,837 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#include "sleipnir/optimization/solver/InteriorPoint.hpp" - -#include -#include -#include -#include -#include - -#include -#include - -#include "optimization/RegularizedLDLT.hpp" -#include "optimization/solver/util/ErrorEstimate.hpp" -#include "optimization/solver/util/FeasibilityRestoration.hpp" -#include "optimization/solver/util/Filter.hpp" -#include "optimization/solver/util/FractionToTheBoundaryRule.hpp" -#include "optimization/solver/util/IsLocallyInfeasible.hpp" -#include "optimization/solver/util/KKTError.hpp" -#include "sleipnir/autodiff/Gradient.hpp" -#include "sleipnir/autodiff/Hessian.hpp" -#include "sleipnir/autodiff/Jacobian.hpp" -#include "sleipnir/optimization/SolverExitCondition.hpp" -#include "sleipnir/util/Print.hpp" -#include "sleipnir/util/Spy.hpp" -#include "util/ScopeExit.hpp" -#include "util/ToMilliseconds.hpp" - -// See docs/algorithms.md#Works_cited for citation definitions. -// -// See docs/algorithms.md#Interior-point_method for a derivation of the -// interior-point method formulation being used. - -namespace sleipnir { - -void InteriorPoint(std::span decisionVariables, - std::span equalityConstraints, - std::span inequalityConstraints, Variable& f, - function_ref callback, - const SolverConfig& config, bool feasibilityRestoration, - Eigen::VectorXd& x, Eigen::VectorXd& s, - SolverStatus* status) { - const auto solveStartTime = std::chrono::system_clock::now(); - - // Map decision variables and constraints to VariableMatrices for Lagrangian - VariableMatrix xAD{decisionVariables}; - xAD.SetValue(x); - VariableMatrix c_eAD{equalityConstraints}; - VariableMatrix c_iAD{inequalityConstraints}; - - // Create autodiff variables for s, y, and z for Lagrangian - VariableMatrix sAD(inequalityConstraints.size()); - sAD.SetValue(s); - VariableMatrix yAD(equalityConstraints.size()); - for (auto& y : yAD) { - y.SetValue(0.0); - } - VariableMatrix zAD(inequalityConstraints.size()); - for (auto& z : zAD) { - z.SetValue(1.0); - } - - // Lagrangian L - // - // L(xₖ, sₖ, yₖ, zₖ) = f(xₖ) − yₖᵀcₑ(xₖ) − zₖᵀ(cᵢ(xₖ) − sₖ) - auto L = f - (yAD.T() * c_eAD)(0) - (zAD.T() * (c_iAD - sAD))(0); - - // Equality constraint Jacobian Aₑ - // - // [∇ᵀcₑ₁(xₖ)] - // Aₑ(x) = [∇ᵀcₑ₂(xₖ)] - // [ ⋮ ] - // [∇ᵀcₑₘ(xₖ)] - Jacobian jacobianCe{c_eAD, xAD}; - Eigen::SparseMatrix A_e = jacobianCe.Value(); - - // Inequality constraint Jacobian Aᵢ - // - // [∇ᵀcᵢ₁(xₖ)] - // Aᵢ(x) = [∇ᵀcᵢ₂(xₖ)] - // [ ⋮ ] - // [∇ᵀcᵢₘ(xₖ)] - Jacobian jacobianCi{c_iAD, xAD}; - Eigen::SparseMatrix A_i = jacobianCi.Value(); - - // Gradient of f ∇f - Gradient gradientF{f, xAD}; - Eigen::SparseVector g = gradientF.Value(); - - // Hessian of the Lagrangian H - // - // Hₖ = ∇²ₓₓL(xₖ, sₖ, yₖ, zₖ) - Hessian hessianL{L, xAD}; - Eigen::SparseMatrix H = hessianL.Value(); - - Eigen::VectorXd y = yAD.Value(); - Eigen::VectorXd z = zAD.Value(); - Eigen::VectorXd c_e = c_eAD.Value(); - Eigen::VectorXd c_i = c_iAD.Value(); - - // Check for overconstrained problem - if (equalityConstraints.size() > decisionVariables.size()) { - if (config.diagnostics) { - sleipnir::println("The problem has too few degrees of freedom."); - sleipnir::println( - "Violated constraints (cₑ(x) = 0) in order of declaration:"); - for (int row = 0; row < c_e.rows(); ++row) { - if (c_e(row) < 0.0) { - sleipnir::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e(row)); - } - } - } - - status->exitCondition = SolverExitCondition::kTooFewDOFs; - return; - } - - // Check whether initial guess has finite f(xₖ), cₑ(xₖ), and cᵢ(xₖ) - if (!std::isfinite(f.Value()) || !c_e.allFinite() || !c_i.allFinite()) { - status->exitCondition = - SolverExitCondition::kNonfiniteInitialCostOrConstraints; - return; - } - - // Sparsity pattern files written when spy flag is set in SolverConfig - std::ofstream H_spy; - std::ofstream A_e_spy; - std::ofstream A_i_spy; - if (config.spy) { - A_e_spy.open("A_e.spy"); - A_i_spy.open("A_i.spy"); - H_spy.open("H.spy"); - } - - if (config.diagnostics && !feasibilityRestoration) { - sleipnir::println("Error tolerance: {}\n", config.tolerance); - } - - std::chrono::system_clock::time_point iterationsStartTime; - - int iterations = 0; - - // Prints final diagnostics when the solver exits - scope_exit exit{[&] { - status->cost = f.Value(); - - if (config.diagnostics && !feasibilityRestoration) { - auto solveEndTime = std::chrono::system_clock::now(); - - sleipnir::println("\nSolve time: {:.3f} ms", - ToMilliseconds(solveEndTime - solveStartTime)); - sleipnir::println(" ↳ {:.3f} ms (solver setup)", - ToMilliseconds(iterationsStartTime - solveStartTime)); - if (iterations > 0) { - sleipnir::println( - " ↳ {:.3f} ms ({} solver iterations; {:.3f} ms average)", - ToMilliseconds(solveEndTime - iterationsStartTime), iterations, - ToMilliseconds((solveEndTime - iterationsStartTime) / iterations)); - } - sleipnir::println(""); - - sleipnir::println("{:^8} {:^10} {:^14} {:^6}", "autodiff", - "setup (ms)", "avg solve (ms)", "solves"); - sleipnir::println("{:=^47}", ""); - constexpr auto format = "{:^8} {:10.3f} {:14.3f} {:6}"; - sleipnir::println(format, "∇f(x)", - gradientF.GetProfiler().SetupDuration(), - gradientF.GetProfiler().AverageSolveDuration(), - gradientF.GetProfiler().SolveMeasurements()); - sleipnir::println(format, "∇²ₓₓL", hessianL.GetProfiler().SetupDuration(), - hessianL.GetProfiler().AverageSolveDuration(), - hessianL.GetProfiler().SolveMeasurements()); - sleipnir::println(format, "∂cₑ/∂x", - jacobianCe.GetProfiler().SetupDuration(), - jacobianCe.GetProfiler().AverageSolveDuration(), - jacobianCe.GetProfiler().SolveMeasurements()); - sleipnir::println(format, "∂cᵢ/∂x", - jacobianCi.GetProfiler().SetupDuration(), - jacobianCi.GetProfiler().AverageSolveDuration(), - jacobianCi.GetProfiler().SolveMeasurements()); - sleipnir::println(""); - } - }}; - - // Barrier parameter minimum - const double μ_min = config.tolerance / 10.0; - - // Barrier parameter μ - double μ = 0.1; - - // Fraction-to-the-boundary rule scale factor minimum - constexpr double τ_min = 0.99; - - // Fraction-to-the-boundary rule scale factor τ - double τ = τ_min; - - Filter filter{f}; - - // This should be run when the error estimate is below a desired threshold for - // the current barrier parameter - auto UpdateBarrierParameterAndResetFilter = [&] { - // Barrier parameter linear decrease power in "κ_μ μ". Range of (0, 1). - constexpr double κ_μ = 0.2; - - // Barrier parameter superlinear decrease power in "μ^(θ_μ)". Range of (1, - // 2). - constexpr double θ_μ = 1.5; - - // Update the barrier parameter. - // - // μⱼ₊₁ = max(εₜₒₗ/10, min(κ_μ μⱼ, μⱼ^θ_μ)) - // - // See equation (7) of [2]. - μ = std::max(μ_min, std::min(κ_μ * μ, std::pow(μ, θ_μ))); - - // Update the fraction-to-the-boundary rule scaling factor. - // - // τⱼ = max(τₘᵢₙ, 1 − μⱼ) - // - // See equation (8) of [2]. - τ = std::max(τ_min, 1.0 - μ); - - // Reset the filter when the barrier parameter is updated - filter.Reset(); - }; - - // Kept outside the loop so its storage can be reused - wpi::SmallVector> triplets; - - RegularizedLDLT solver; - - // Variables for determining when a step is acceptable - constexpr double α_red_factor = 0.5; - int acceptableIterCounter = 0; - - int fullStepRejectedCounter = 0; - int stepTooSmallCounter = 0; - - // Error estimate - double E_0 = std::numeric_limits::infinity(); - - if (config.diagnostics) { - iterationsStartTime = std::chrono::system_clock::now(); - } - - while (E_0 > config.tolerance && - acceptableIterCounter < config.maxAcceptableIterations) { - std::chrono::system_clock::time_point innerIterStartTime; - if (config.diagnostics) { - innerIterStartTime = std::chrono::system_clock::now(); - } - - // Check for local equality constraint infeasibility - if (IsEqualityLocallyInfeasible(A_e, c_e)) { - if (config.diagnostics) { - sleipnir::println( - "The problem is locally infeasible due to violated equality " - "constraints."); - sleipnir::println( - "Violated constraints (cₑ(x) = 0) in order of declaration:"); - for (int row = 0; row < c_e.rows(); ++row) { - if (c_e(row) < 0.0) { - sleipnir::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e(row)); - } - } - } - - status->exitCondition = SolverExitCondition::kLocallyInfeasible; - return; - } - - // Check for local inequality constraint infeasibility - if (IsInequalityLocallyInfeasible(A_i, c_i)) { - if (config.diagnostics) { - sleipnir::println( - "The problem is infeasible due to violated inequality " - "constraints."); - sleipnir::println( - "Violated constraints (cᵢ(x) ≥ 0) in order of declaration:"); - for (int row = 0; row < c_i.rows(); ++row) { - if (c_i(row) < 0.0) { - sleipnir::println(" {}/{}: {} ≥ 0", row + 1, c_i.rows(), c_i(row)); - } - } - } - - status->exitCondition = SolverExitCondition::kLocallyInfeasible; - return; - } - - // Check for diverging iterates - if (x.lpNorm() > 1e20 || !x.allFinite() || - s.lpNorm() > 1e20 || !s.allFinite()) { - status->exitCondition = SolverExitCondition::kDivergingIterates; - return; - } - - // Write out spy file contents if that's enabled - if (config.spy) { - // Gap between sparsity patterns - if (iterations > 0) { - A_e_spy << "\n"; - A_i_spy << "\n"; - H_spy << "\n"; - } - - Spy(H_spy, H); - Spy(A_e_spy, A_e); - Spy(A_i_spy, A_i); - } - - // Call user callback - if (callback({iterations, x, s, g, H, A_e, A_i})) { - status->exitCondition = SolverExitCondition::kCallbackRequestedStop; - return; - } - - // [s₁ 0 ⋯ 0 ] - // S = [0 ⋱ ⋮ ] - // [⋮ ⋱ 0 ] - // [0 ⋯ 0 sₘ] - const auto S = s.asDiagonal(); - Eigen::SparseMatrix Sinv; - Sinv = s.cwiseInverse().asDiagonal(); - - // [z₁ 0 ⋯ 0 ] - // Z = [0 ⋱ ⋮ ] - // [⋮ ⋱ 0 ] - // [0 ⋯ 0 zₘ] - const auto Z = z.asDiagonal(); - Eigen::SparseMatrix Zinv; - Zinv = z.cwiseInverse().asDiagonal(); - - // Σ = S⁻¹Z - const Eigen::SparseMatrix Σ = Sinv * Z; - - // lhs = [H + AᵢᵀΣAᵢ Aₑᵀ] - // [ Aₑ 0 ] - // - // Don't assign upper triangle because solver only uses lower triangle. - const Eigen::SparseMatrix topLeft = - H.triangularView() + - (A_i.transpose() * Σ * A_i).triangularView(); - triplets.clear(); - triplets.reserve(topLeft.nonZeros() + A_e.nonZeros()); - for (int col = 0; col < H.cols(); ++col) { - // Append column of H + AᵢᵀΣAᵢ lower triangle in top-left quadrant - for (Eigen::SparseMatrix::InnerIterator it{topLeft, col}; it; - ++it) { - triplets.emplace_back(it.row(), it.col(), it.value()); - } - // Append column of Aₑ in bottom-left quadrant - for (Eigen::SparseMatrix::InnerIterator it{A_e, col}; it; ++it) { - triplets.emplace_back(H.rows() + it.row(), it.col(), it.value()); - } - } - Eigen::SparseMatrix lhs( - decisionVariables.size() + equalityConstraints.size(), - decisionVariables.size() + equalityConstraints.size()); - lhs.setFromSortedTriplets(triplets.begin(), triplets.end(), - [](const auto&, const auto& b) { return b; }); - - const Eigen::VectorXd e = Eigen::VectorXd::Ones(s.rows()); - - // rhs = −[∇f − Aₑᵀy + Aᵢᵀ(S⁻¹(Zcᵢ − μe) − z)] - // [ cₑ ] - Eigen::VectorXd rhs{x.rows() + y.rows()}; - rhs.segment(0, x.rows()) = - -(g - A_e.transpose() * y + - A_i.transpose() * (Sinv * (Z * c_i - μ * e) - z)); - rhs.segment(x.rows(), y.rows()) = -c_e; - - // Solve the Newton-KKT system - // - // [H + AᵢᵀΣAᵢ Aₑᵀ][ pₖˣ] = −[∇f − Aₑᵀy + Aᵢᵀ(S⁻¹(Zcᵢ − μe) − z)] - // [ Aₑ 0 ][−pₖʸ] [ cₑ ] - solver.Compute(lhs, equalityConstraints.size(), μ); - Eigen::VectorXd step{x.rows() + y.rows()}; - if (solver.Info() == Eigen::Success) { - step = solver.Solve(rhs); - } else { - // The regularization procedure failed due to a rank-deficient equality - // constraint Jacobian with linearly dependent constraints. Set the step - // length to zero and let second-order corrections attempt to restore - // feasibility. - step.setZero(); - } - - // step = [ pₖˣ] - // [−pₖʸ] - Eigen::VectorXd p_x = step.segment(0, x.rows()); - Eigen::VectorXd p_y = -step.segment(x.rows(), y.rows()); - - // pₖᶻ = −Σcᵢ + μS⁻¹e − ΣAᵢpₖˣ - Eigen::VectorXd p_z = -Σ * c_i + μ * Sinv * e - Σ * A_i * p_x; - - // pₖˢ = μZ⁻¹e − s − Z⁻¹Spₖᶻ - Eigen::VectorXd p_s = μ * Zinv * e - s - Zinv * S * p_z; - - // αᵐᵃˣ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ) - const double α_max = FractionToTheBoundaryRule(s, p_s, τ); - double α = α_max; - - // αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ) - double α_z = FractionToTheBoundaryRule(z, p_z, τ); - - // Loop until a step is accepted. If a step becomes acceptable, the loop - // will exit early. - while (1) { - Eigen::VectorXd trial_x = x + α * p_x; - Eigen::VectorXd trial_y = y + α_z * p_y; - Eigen::VectorXd trial_z = z + α_z * p_z; - - xAD.SetValue(trial_x); - - Eigen::VectorXd trial_c_e = c_eAD.Value(); - Eigen::VectorXd trial_c_i = c_iAD.Value(); - - // If f(xₖ + αpₖˣ), cₑ(xₖ + αpₖˣ), or cᵢ(xₖ + αpₖˣ) aren't finite, reduce - // step size immediately - if (!std::isfinite(f.Value()) || !trial_c_e.allFinite() || - !trial_c_i.allFinite()) { - // Reduce step size - α *= α_red_factor; - continue; - } - - Eigen::VectorXd trial_s; - if (config.feasibleIPM && c_i.cwiseGreater(0.0).all()) { - // If the inequality constraints are all feasible, prevent them from - // becoming infeasible again. - // - // See equation (19.30) in [1]. - trial_s = trial_c_i; - } else { - trial_s = s + α * p_s; - } - - // Check whether filter accepts trial iterate - auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ); - if (filter.TryAdd(entry)) { - // Accept step - break; - } - - double prevConstraintViolation = c_e.lpNorm<1>() + (c_i - s).lpNorm<1>(); - double nextConstraintViolation = - trial_c_e.lpNorm<1>() + (trial_c_i - trial_s).lpNorm<1>(); - - // Second-order corrections - // - // If first trial point was rejected and constraint violation stayed the - // same or went up, apply second-order corrections - if (nextConstraintViolation >= prevConstraintViolation) { - // Apply second-order corrections. See section 2.4 of [2]. - Eigen::VectorXd p_x_cor = p_x; - Eigen::VectorXd p_y_soc = p_y; - Eigen::VectorXd p_z_soc = p_z; - Eigen::VectorXd p_s_soc = p_s; - - double α_soc = α; - Eigen::VectorXd c_e_soc = c_e; - - bool stepAcceptable = false; - for (int soc_iteration = 0; soc_iteration < 5 && !stepAcceptable; - ++soc_iteration) { - // Rebuild Newton-KKT rhs with updated constraint values. - // - // rhs = −[∇f − Aₑᵀy + Aᵢᵀ(S⁻¹(Zcᵢ − μe) − z)] - // [ cₑˢᵒᶜ ] - // - // where cₑˢᵒᶜ = αc(xₖ) + c(xₖ + αpₖˣ) - c_e_soc = α_soc * c_e_soc + trial_c_e; - rhs.bottomRows(y.rows()) = -c_e_soc; - - // Solve the Newton-KKT system - step = solver.Solve(rhs); - - p_x_cor = step.segment(0, x.rows()); - p_y_soc = -step.segment(x.rows(), y.rows()); - - // pₖᶻ = −Σcᵢ + μS⁻¹e − ΣAᵢpₖˣ - p_z_soc = -Σ * c_i + μ * Sinv * e - Σ * A_i * p_x_cor; - - // pₖˢ = μZ⁻¹e − s − Z⁻¹Spₖᶻ - p_s_soc = μ * Zinv * e - s - Zinv * S * p_z_soc; - - // αˢᵒᶜ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ) - α_soc = FractionToTheBoundaryRule(s, p_s_soc, τ); - trial_x = x + α_soc * p_x_cor; - trial_s = s + α_soc * p_s_soc; - - // αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ) - double α_z_soc = FractionToTheBoundaryRule(z, p_z_soc, τ); - trial_y = y + α_z_soc * p_y_soc; - trial_z = z + α_z_soc * p_z_soc; - - xAD.SetValue(trial_x); - - trial_c_e = c_eAD.Value(); - trial_c_i = c_iAD.Value(); - - // Check whether filter accepts trial iterate - entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ); - if (filter.TryAdd(entry)) { - p_x = p_x_cor; - p_y = p_y_soc; - p_z = p_z_soc; - p_s = p_s_soc; - α = α_soc; - α_z = α_z_soc; - stepAcceptable = true; - } - } - - if (stepAcceptable) { - // Accept step - break; - } - } - - // If we got here and α is the full step, the full step was rejected. - // Increment the full-step rejected counter to keep track of how many full - // steps have been rejected in a row. - if (α == α_max) { - ++fullStepRejectedCounter; - } - - // If the full step was rejected enough times in a row, reset the filter - // because it may be impeding progress. - // - // See section 3.2 case I of [2]. - if (fullStepRejectedCounter >= 4 && - filter.maxConstraintViolation > entry.constraintViolation / 10.0) { - filter.maxConstraintViolation *= 0.1; - filter.Reset(); - continue; - } - - // Reduce step size - α *= α_red_factor; - - // Safety factor for the minimal step size - constexpr double α_min_frac = 0.05; - - // If step size hit a minimum, check if the KKT error was reduced. If it - // wasn't, invoke feasibility restoration. - if (α < α_min_frac * Filter::γConstraint) { - double currentKKTError = KKTError(g, A_e, c_e, A_i, c_i, s, y, z, μ); - - Eigen::VectorXd trial_x = x + α_max * p_x; - Eigen::VectorXd trial_s = s + α_max * p_s; - - Eigen::VectorXd trial_y = y + α_z * p_y; - Eigen::VectorXd trial_z = z + α_z * p_z; - - // Upate autodiff - xAD.SetValue(trial_x); - sAD.SetValue(trial_s); - yAD.SetValue(trial_y); - zAD.SetValue(trial_z); - - Eigen::VectorXd trial_c_e = c_eAD.Value(); - Eigen::VectorXd trial_c_i = c_iAD.Value(); - - double nextKKTError = KKTError(gradientF.Value(), jacobianCe.Value(), - trial_c_e, jacobianCi.Value(), trial_c_i, - trial_s, trial_y, trial_z, μ); - - // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway - if (nextKKTError <= 0.999 * currentKKTError) { - α = α_max; - - // Accept step - break; - } - - // If the step direction was bad and feasibility restoration is - // already running, running it again won't help - if (feasibilityRestoration) { - status->exitCondition = SolverExitCondition::kLocallyInfeasible; - return; - } - - auto initialEntry = filter.MakeEntry(s, c_e, c_i, μ); - - // Feasibility restoration phase - Eigen::VectorXd fr_x = x; - Eigen::VectorXd fr_s = s; - SolverStatus fr_status; - FeasibilityRestoration( - decisionVariables, equalityConstraints, inequalityConstraints, μ, - [&](const SolverIterationInfo& info) { - Eigen::VectorXd trial_x = - info.x.segment(0, decisionVariables.size()); - xAD.SetValue(trial_x); - - Eigen::VectorXd trial_s = - info.s.segment(0, inequalityConstraints.size()); - sAD.SetValue(trial_s); - - Eigen::VectorXd trial_c_e = c_eAD.Value(); - Eigen::VectorXd trial_c_i = c_iAD.Value(); - - // If current iterate is acceptable to normal filter and - // constraint violation has sufficiently reduced, stop - // feasibility restoration - auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ); - if (filter.IsAcceptable(entry) && - entry.constraintViolation < - 0.9 * initialEntry.constraintViolation) { - return true; - } - - return false; - }, - config, fr_x, fr_s, &fr_status); - - if (fr_status.exitCondition == - SolverExitCondition::kCallbackRequestedStop) { - p_x = fr_x - x; - p_s = fr_s - s; - - // Lagrange mutliplier estimates - // - // [y] = (ÂÂᵀ)⁻¹Â[ ∇f] - // [z] [−μe] - // - // where  = [Aₑ 0] - // [Aᵢ −S] - // - // See equation (19.37) of [1]. - { - xAD.SetValue(fr_x); - sAD.SetValue(c_iAD.Value()); - - A_e = jacobianCe.Value(); - A_i = jacobianCi.Value(); - g = gradientF.Value(); - - //  = [Aₑ 0] - // [Aᵢ −S] - triplets.clear(); - triplets.reserve(A_e.nonZeros() + A_i.nonZeros() + s.rows()); - for (int col = 0; col < A_e.cols(); ++col) { - // Append column of Aₑ in top-left quadrant - for (Eigen::SparseMatrix::InnerIterator it{A_e, col}; it; - ++it) { - triplets.emplace_back(it.row(), it.col(), it.value()); - } - // Append column of Aᵢ in bottom-left quadrant - for (Eigen::SparseMatrix::InnerIterator it{A_i, col}; it; - ++it) { - triplets.emplace_back(A_e.rows() + it.row(), it.col(), - it.value()); - } - } - // Append −S in bottom-right quadrant - for (int i = 0; i < s.rows(); ++i) { - triplets.emplace_back(A_e.rows() + i, A_e.cols() + i, -s(i)); - } - Eigen::SparseMatrix Ahat{A_e.rows() + A_i.rows(), - A_e.cols() + s.rows()}; - Ahat.setFromSortedTriplets( - triplets.begin(), triplets.end(), - [](const auto&, const auto& b) { return b; }); - - // lhs = ÂÂᵀ - Eigen::SparseMatrix lhs = Ahat * Ahat.transpose(); - - // rhs = Â[ ∇f] - // [−μe] - Eigen::VectorXd rhsTemp{g.rows() + e.rows()}; - rhsTemp.block(0, 0, g.rows(), 1) = g; - rhsTemp.block(g.rows(), 0, s.rows(), 1) = -μ * e; - Eigen::VectorXd rhs = Ahat * rhsTemp; - - Eigen::SimplicialLDLT> yzEstimator{lhs}; - Eigen::VectorXd sol = yzEstimator.solve(rhs); - - p_y = y - sol.block(0, 0, y.rows(), 1); - p_z = z - sol.block(y.rows(), 0, z.rows(), 1); - } - - α = 1.0; - α_z = 1.0; - - // Accept step - break; - } else if (fr_status.exitCondition == SolverExitCondition::kSuccess) { - status->exitCondition = SolverExitCondition::kLocallyInfeasible; - x = fr_x; - return; - } else { - status->exitCondition = - SolverExitCondition::kFeasibilityRestorationFailed; - x = fr_x; - return; - } - } - } - - // If full step was accepted, reset full-step rejected counter - if (α == α_max) { - fullStepRejectedCounter = 0; - } - - // Handle very small search directions by letting αₖ = αₖᵐᵃˣ when - // max(|pₖˣ(i)|/(1 + |xₖ(i)|)) < 10ε_mach. - // - // See section 3.9 of [2]. - double maxStepScaled = 0.0; - for (int row = 0; row < x.rows(); ++row) { - maxStepScaled = std::max(maxStepScaled, - std::abs(p_x(row)) / (1.0 + std::abs(x(row)))); - } - if (maxStepScaled < 10.0 * std::numeric_limits::epsilon()) { - α = α_max; - ++stepTooSmallCounter; - } else { - stepTooSmallCounter = 0; - } - - // xₖ₊₁ = xₖ + αₖpₖˣ - // sₖ₊₁ = sₖ + αₖpₖˢ - // yₖ₊₁ = yₖ + αₖᶻpₖʸ - // zₖ₊₁ = zₖ + αₖᶻpₖᶻ - x += α * p_x; - s += α * p_s; - y += α_z * p_y; - z += α_z * p_z; - - // A requirement for the convergence proof is that the "primal-dual barrier - // term Hessian" Σₖ does not deviate arbitrarily much from the "primal - // Hessian" μⱼSₖ⁻². We ensure this by resetting - // - // zₖ₊₁⁽ⁱ⁾ = max(min(zₖ₊₁⁽ⁱ⁾, κ_Σ μⱼ/sₖ₊₁⁽ⁱ⁾), μⱼ/(κ_Σ sₖ₊₁⁽ⁱ⁾)) - // - // for some fixed κ_Σ ≥ 1 after each step. See equation (16) of [2]. - { - // Barrier parameter scale factor for inequality constraint Lagrange - // multiplier safeguard - constexpr double κ_Σ = 1e10; - - for (int row = 0; row < z.rows(); ++row) { - z(row) = - std::max(std::min(z(row), κ_Σ * μ / s(row)), μ / (κ_Σ * s(row))); - } - } - - // Update autodiff for Jacobians and Hessian - xAD.SetValue(x); - sAD.SetValue(s); - yAD.SetValue(y); - zAD.SetValue(z); - A_e = jacobianCe.Value(); - A_i = jacobianCi.Value(); - g = gradientF.Value(); - H = hessianL.Value(); - - c_e = c_eAD.Value(); - c_i = c_iAD.Value(); - - // Update the error estimate - E_0 = ErrorEstimate(g, A_e, c_e, A_i, c_i, s, y, z, 0.0); - if (E_0 < config.acceptableTolerance) { - ++acceptableIterCounter; - } else { - acceptableIterCounter = 0; - } - - // Update the barrier parameter if necessary - if (E_0 > config.tolerance) { - // Barrier parameter scale factor for tolerance checks - constexpr double κ_ε = 10.0; - - // While the error estimate is below the desired threshold for this - // barrier parameter value, decrease the barrier parameter further - double E_μ = ErrorEstimate(g, A_e, c_e, A_i, c_i, s, y, z, μ); - while (μ > μ_min && E_μ <= κ_ε * μ) { - UpdateBarrierParameterAndResetFilter(); - E_μ = ErrorEstimate(g, A_e, c_e, A_i, c_i, s, y, z, μ); - } - } - - const auto innerIterEndTime = std::chrono::system_clock::now(); - - // Diagnostics for current iteration - if (config.diagnostics) { - if (iterations % 20 == 0) { - sleipnir::println("{:^4} {:^9} {:^13} {:^13} {:^13}", "iter", - "time (ms)", "error", "cost", "infeasibility"); - sleipnir::println("{:=^61}", ""); - } - - sleipnir::println("{:4}{} {:9.3f} {:13e} {:13e} {:13e}", iterations, - feasibilityRestoration ? "r" : " ", - ToMilliseconds(innerIterEndTime - innerIterStartTime), - E_0, f.Value(), - c_e.lpNorm<1>() + (c_i - s).lpNorm<1>()); - } - - ++iterations; - - // Check for max iterations - if (iterations >= config.maxIterations) { - status->exitCondition = SolverExitCondition::kMaxIterationsExceeded; - return; - } - - // Check for max wall clock time - if (innerIterEndTime - solveStartTime > config.timeout) { - status->exitCondition = SolverExitCondition::kTimeout; - return; - } - - // Check for solve to acceptable tolerance - if (E_0 > config.tolerance && - acceptableIterCounter == config.maxAcceptableIterations) { - status->exitCondition = SolverExitCondition::kSolvedToAcceptableTolerance; - return; - } - - // The search direction has been very small twice, so assume the problem has - // been solved as well as possible given finite precision and reduce the - // barrier parameter. - // - // See section 3.9 of [2]. - if (stepTooSmallCounter >= 2 && μ > μ_min) { - UpdateBarrierParameterAndResetFilter(); - continue; - } - } -} // NOLINT(readability/fn_size) - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp deleted file mode 100644 index 662abc2fb6..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp +++ /dev/null @@ -1,559 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#include "sleipnir/optimization/solver/SQP.hpp" - -#include -#include -#include -#include -#include - -#include -#include - -#include "optimization/RegularizedLDLT.hpp" -#include "optimization/solver/util/ErrorEstimate.hpp" -#include "optimization/solver/util/FeasibilityRestoration.hpp" -#include "optimization/solver/util/Filter.hpp" -#include "optimization/solver/util/IsLocallyInfeasible.hpp" -#include "optimization/solver/util/KKTError.hpp" -#include "sleipnir/autodiff/Gradient.hpp" -#include "sleipnir/autodiff/Hessian.hpp" -#include "sleipnir/autodiff/Jacobian.hpp" -#include "sleipnir/optimization/SolverExitCondition.hpp" -#include "sleipnir/util/Print.hpp" -#include "sleipnir/util/Spy.hpp" -#include "util/ScopeExit.hpp" -#include "util/ToMilliseconds.hpp" - -// See docs/algorithms.md#Works_cited for citation definitions. - -namespace sleipnir { - -void SQP(std::span decisionVariables, - std::span equalityConstraints, Variable& f, - function_ref callback, - const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status) { - const auto solveStartTime = std::chrono::system_clock::now(); - - // Map decision variables and constraints to VariableMatrices for Lagrangian - VariableMatrix xAD{decisionVariables}; - xAD.SetValue(x); - VariableMatrix c_eAD{equalityConstraints}; - - // Create autodiff variables for y for Lagrangian - VariableMatrix yAD(equalityConstraints.size()); - for (auto& y : yAD) { - y.SetValue(0.0); - } - - // Lagrangian L - // - // L(xₖ, yₖ) = f(xₖ) − yₖᵀcₑ(xₖ) - auto L = f - (yAD.T() * c_eAD)(0); - - // Equality constraint Jacobian Aₑ - // - // [∇ᵀcₑ₁(xₖ)] - // Aₑ(x) = [∇ᵀcₑ₂(xₖ)] - // [ ⋮ ] - // [∇ᵀcₑₘ(xₖ)] - Jacobian jacobianCe{c_eAD, xAD}; - Eigen::SparseMatrix A_e = jacobianCe.Value(); - - // Gradient of f ∇f - Gradient gradientF{f, xAD}; - Eigen::SparseVector g = gradientF.Value(); - - // Hessian of the Lagrangian H - // - // Hₖ = ∇²ₓₓL(xₖ, yₖ) - Hessian hessianL{L, xAD}; - Eigen::SparseMatrix H = hessianL.Value(); - - Eigen::VectorXd y = yAD.Value(); - Eigen::VectorXd c_e = c_eAD.Value(); - - // Check for overconstrained problem - if (equalityConstraints.size() > decisionVariables.size()) { - if (config.diagnostics) { - sleipnir::println("The problem has too few degrees of freedom."); - sleipnir::println( - "Violated constraints (cₑ(x) = 0) in order of declaration:"); - for (int row = 0; row < c_e.rows(); ++row) { - if (c_e(row) < 0.0) { - sleipnir::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e(row)); - } - } - } - - status->exitCondition = SolverExitCondition::kTooFewDOFs; - return; - } - - // Check whether initial guess has finite f(xₖ) and cₑ(xₖ) - if (!std::isfinite(f.Value()) || !c_e.allFinite()) { - status->exitCondition = - SolverExitCondition::kNonfiniteInitialCostOrConstraints; - return; - } - - // Sparsity pattern files written when spy flag is set in SolverConfig - std::ofstream H_spy; - std::ofstream A_e_spy; - if (config.spy) { - A_e_spy.open("A_e.spy"); - H_spy.open("H.spy"); - } - - if (config.diagnostics) { - sleipnir::println("Error tolerance: {}\n", config.tolerance); - } - - std::chrono::system_clock::time_point iterationsStartTime; - - int iterations = 0; - - // Prints final diagnostics when the solver exits - scope_exit exit{[&] { - status->cost = f.Value(); - - if (config.diagnostics) { - auto solveEndTime = std::chrono::system_clock::now(); - - sleipnir::println("\nSolve time: {:.3f} ms", - ToMilliseconds(solveEndTime - solveStartTime)); - sleipnir::println(" ↳ {:.3f} ms (solver setup)", - ToMilliseconds(iterationsStartTime - solveStartTime)); - if (iterations > 0) { - sleipnir::println( - " ↳ {:.3f} ms ({} solver iterations; {:.3f} ms average)", - ToMilliseconds(solveEndTime - iterationsStartTime), iterations, - ToMilliseconds((solveEndTime - iterationsStartTime) / iterations)); - } - sleipnir::println(""); - - sleipnir::println("{:^8} {:^10} {:^14} {:^6}", "autodiff", - "setup (ms)", "avg solve (ms)", "solves"); - sleipnir::println("{:=^47}", ""); - constexpr auto format = "{:^8} {:10.3f} {:14.3f} {:6}"; - sleipnir::println(format, "∇f(x)", - gradientF.GetProfiler().SetupDuration(), - gradientF.GetProfiler().AverageSolveDuration(), - gradientF.GetProfiler().SolveMeasurements()); - sleipnir::println(format, "∇²ₓₓL", hessianL.GetProfiler().SetupDuration(), - hessianL.GetProfiler().AverageSolveDuration(), - hessianL.GetProfiler().SolveMeasurements()); - sleipnir::println(format, "∂cₑ/∂x", - jacobianCe.GetProfiler().SetupDuration(), - jacobianCe.GetProfiler().AverageSolveDuration(), - jacobianCe.GetProfiler().SolveMeasurements()); - sleipnir::println(""); - } - }}; - - Filter filter{f}; - - // Kept outside the loop so its storage can be reused - wpi::SmallVector> triplets; - - RegularizedLDLT solver; - - // Variables for determining when a step is acceptable - constexpr double α_red_factor = 0.5; - int acceptableIterCounter = 0; - - int fullStepRejectedCounter = 0; - - // Error estimate - double E_0 = std::numeric_limits::infinity(); - - if (config.diagnostics) { - iterationsStartTime = std::chrono::system_clock::now(); - } - - while (E_0 > config.tolerance && - acceptableIterCounter < config.maxAcceptableIterations) { - std::chrono::system_clock::time_point innerIterStartTime; - if (config.diagnostics) { - innerIterStartTime = std::chrono::system_clock::now(); - } - - // Check for local equality constraint infeasibility - if (IsEqualityLocallyInfeasible(A_e, c_e)) { - if (config.diagnostics) { - sleipnir::println( - "The problem is locally infeasible due to violated equality " - "constraints."); - sleipnir::println( - "Violated constraints (cₑ(x) = 0) in order of declaration:"); - for (int row = 0; row < c_e.rows(); ++row) { - if (c_e(row) < 0.0) { - sleipnir::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e(row)); - } - } - } - - status->exitCondition = SolverExitCondition::kLocallyInfeasible; - return; - } - - // Check for diverging iterates - if (x.lpNorm() > 1e20 || !x.allFinite()) { - status->exitCondition = SolverExitCondition::kDivergingIterates; - return; - } - - // Write out spy file contents if that's enabled - if (config.spy) { - // Gap between sparsity patterns - if (iterations > 0) { - A_e_spy << "\n"; - H_spy << "\n"; - } - - Spy(H_spy, H); - Spy(A_e_spy, A_e); - } - - // Call user callback - if (callback({iterations, x, Eigen::VectorXd::Zero(0), g, H, A_e, - Eigen::SparseMatrix{}})) { - status->exitCondition = SolverExitCondition::kCallbackRequestedStop; - return; - } - - // lhs = [H Aₑᵀ] - // [Aₑ 0 ] - // - // Don't assign upper triangle because solver only uses lower triangle. - const Eigen::SparseMatrix topLeft = - H.triangularView(); - triplets.clear(); - triplets.reserve(topLeft.nonZeros() + A_e.nonZeros()); - for (int col = 0; col < H.cols(); ++col) { - // Append column of H + AᵢᵀΣAᵢ lower triangle in top-left quadrant - for (Eigen::SparseMatrix::InnerIterator it{topLeft, col}; it; - ++it) { - triplets.emplace_back(it.row(), it.col(), it.value()); - } - // Append column of Aₑ in bottom-left quadrant - for (Eigen::SparseMatrix::InnerIterator it{A_e, col}; it; ++it) { - triplets.emplace_back(H.rows() + it.row(), it.col(), it.value()); - } - } - Eigen::SparseMatrix lhs( - decisionVariables.size() + equalityConstraints.size(), - decisionVariables.size() + equalityConstraints.size()); - lhs.setFromSortedTriplets(triplets.begin(), triplets.end(), - [](const auto&, const auto& b) { return b; }); - - // rhs = −[∇f − Aₑᵀy] - // [ cₑ ] - Eigen::VectorXd rhs{x.rows() + y.rows()}; - rhs.segment(0, x.rows()) = -(g - A_e.transpose() * y); - rhs.segment(x.rows(), y.rows()) = -c_e; - - // Solve the Newton-KKT system - // - // [H Aₑᵀ][ pₖˣ] = −[∇f − Aₑᵀy] - // [Aₑ 0 ][−pₖʸ] [ cₑ ] - solver.Compute(lhs, equalityConstraints.size(), config.tolerance / 10.0); - Eigen::VectorXd step{x.rows() + y.rows()}; - if (solver.Info() == Eigen::Success) { - step = solver.Solve(rhs); - } else { - // The regularization procedure failed due to a rank-deficient equality - // constraint Jacobian with linearly dependent constraints - status->exitCondition = SolverExitCondition::kLocallyInfeasible; - return; - } - - // step = [ pₖˣ] - // [−pₖʸ] - Eigen::VectorXd p_x = step.segment(0, x.rows()); - Eigen::VectorXd p_y = -step.segment(x.rows(), y.rows()); - - constexpr double α_max = 1.0; - double α = α_max; - - // Loop until a step is accepted. If a step becomes acceptable, the loop - // will exit early. - while (1) { - Eigen::VectorXd trial_x = x + α * p_x; - Eigen::VectorXd trial_y = y + α * p_y; - - xAD.SetValue(trial_x); - - Eigen::VectorXd trial_c_e = c_eAD.Value(); - - // If f(xₖ + αpₖˣ) or cₑ(xₖ + αpₖˣ) aren't finite, reduce step size - // immediately - if (!std::isfinite(f.Value()) || !trial_c_e.allFinite()) { - // Reduce step size - α *= α_red_factor; - continue; - } - - // Check whether filter accepts trial iterate - auto entry = filter.MakeEntry(trial_c_e); - if (filter.TryAdd(entry)) { - // Accept step - break; - } - - double prevConstraintViolation = c_e.lpNorm<1>(); - double nextConstraintViolation = trial_c_e.lpNorm<1>(); - - // Second-order corrections - // - // If first trial point was rejected and constraint violation stayed the - // same or went up, apply second-order corrections - if (nextConstraintViolation >= prevConstraintViolation) { - // Apply second-order corrections. See section 2.4 of [2]. - Eigen::VectorXd p_x_cor = p_x; - Eigen::VectorXd p_y_soc = p_y; - - double α_soc = α; - Eigen::VectorXd c_e_soc = c_e; - - bool stepAcceptable = false; - for (int soc_iteration = 0; soc_iteration < 5 && !stepAcceptable; - ++soc_iteration) { - // Rebuild Newton-KKT rhs with updated constraint values. - // - // rhs = −[∇f − Aₑᵀy] - // [ cₑˢᵒᶜ ] - // - // where cₑˢᵒᶜ = αc(xₖ) + c(xₖ + αpₖˣ) - c_e_soc = α_soc * c_e_soc + trial_c_e; - rhs.bottomRows(y.rows()) = -c_e_soc; - - // Solve the Newton-KKT system - step = solver.Solve(rhs); - - p_x_cor = step.segment(0, x.rows()); - p_y_soc = -step.segment(x.rows(), y.rows()); - - trial_x = x + α_soc * p_x_cor; - trial_y = y + α_soc * p_y_soc; - - xAD.SetValue(trial_x); - - trial_c_e = c_eAD.Value(); - - // Check whether filter accepts trial iterate - entry = filter.MakeEntry(trial_c_e); - if (filter.TryAdd(entry)) { - p_x = p_x_cor; - p_y = p_y_soc; - α = α_soc; - stepAcceptable = true; - } - } - - if (stepAcceptable) { - // Accept step - break; - } - } - - // If we got here and α is the full step, the full step was rejected. - // Increment the full-step rejected counter to keep track of how many full - // steps have been rejected in a row. - if (α == α_max) { - ++fullStepRejectedCounter; - } - - // If the full step was rejected enough times in a row, reset the filter - // because it may be impeding progress. - // - // See section 3.2 case I of [2]. - if (fullStepRejectedCounter >= 4 && - filter.maxConstraintViolation > entry.constraintViolation / 10.0) { - filter.maxConstraintViolation *= 0.1; - filter.Reset(); - continue; - } - - // Reduce step size - α *= α_red_factor; - - // Safety factor for the minimal step size - constexpr double α_min_frac = 0.05; - - // If step size hit a minimum, check if the KKT error was reduced. If it - // wasn't, report infeasible. - if (α < α_min_frac * Filter::γConstraint) { - double currentKKTError = KKTError(g, A_e, c_e, y); - - Eigen::VectorXd trial_x = x + α_max * p_x; - Eigen::VectorXd trial_y = y + α_max * p_y; - - // Upate autodiff - xAD.SetValue(trial_x); - yAD.SetValue(trial_y); - - Eigen::VectorXd trial_c_e = c_eAD.Value(); - - double nextKKTError = - KKTError(gradientF.Value(), jacobianCe.Value(), trial_c_e, trial_y); - - // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway - if (nextKKTError <= 0.999 * currentKKTError) { - α = α_max; - - // Accept step - break; - } - - auto initialEntry = filter.MakeEntry(c_e); - - // Feasibility restoration phase - Eigen::VectorXd fr_x = x; - SolverStatus fr_status; - FeasibilityRestoration( - decisionVariables, equalityConstraints, - [&](const SolverIterationInfo& info) { - trial_x = info.x.segment(0, decisionVariables.size()); - xAD.SetValue(trial_x); - - trial_c_e = c_eAD.Value(); - - // If current iterate is acceptable to normal filter and - // constraint violation has sufficiently reduced, stop - // feasibility restoration - entry = filter.MakeEntry(trial_c_e); - if (filter.IsAcceptable(entry) && - entry.constraintViolation < - 0.9 * initialEntry.constraintViolation) { - return true; - } - - return false; - }, - config, fr_x, &fr_status); - - if (fr_status.exitCondition == - SolverExitCondition::kCallbackRequestedStop) { - p_x = fr_x - x; - - // Lagrange mutliplier estimates - // - // y = (AₑAₑᵀ)⁻¹Aₑ∇f - // - // See equation (19.37) of [1]. - { - xAD.SetValue(fr_x); - - A_e = jacobianCe.Value(); - g = gradientF.Value(); - - // lhs = AₑAₑᵀ - Eigen::SparseMatrix lhs = A_e * A_e.transpose(); - - // rhs = Aₑ∇f - Eigen::VectorXd rhs = A_e * g; - - Eigen::SimplicialLDLT> yEstimator{lhs}; - Eigen::VectorXd sol = yEstimator.solve(rhs); - - p_y = y - sol.block(0, 0, y.rows(), 1); - } - - α = 1.0; - - // Accept step - break; - } else if (fr_status.exitCondition == SolverExitCondition::kSuccess) { - status->exitCondition = SolverExitCondition::kLocallyInfeasible; - x = fr_x; - return; - } else { - status->exitCondition = - SolverExitCondition::kFeasibilityRestorationFailed; - x = fr_x; - return; - } - } - } - - // If full step was accepted, reset full-step rejected counter - if (α == α_max) { - fullStepRejectedCounter = 0; - } - - // Handle very small search directions by letting αₖ = αₖᵐᵃˣ when - // max(|pₖˣ(i)|/(1 + |xₖ(i)|)) < 10ε_mach. - // - // See section 3.9 of [2]. - double maxStepScaled = 0.0; - for (int row = 0; row < x.rows(); ++row) { - maxStepScaled = std::max(maxStepScaled, - std::abs(p_x(row)) / (1.0 + std::abs(x(row)))); - } - if (maxStepScaled < 10.0 * std::numeric_limits::epsilon()) { - α = α_max; - } - - // xₖ₊₁ = xₖ + αₖpₖˣ - // yₖ₊₁ = yₖ + αₖpₖʸ - x += α * p_x; - y += α * p_y; - - // Update autodiff for Jacobians and Hessian - xAD.SetValue(x); - yAD.SetValue(y); - A_e = jacobianCe.Value(); - g = gradientF.Value(); - H = hessianL.Value(); - - c_e = c_eAD.Value(); - - // Update the error estimate - E_0 = ErrorEstimate(g, A_e, c_e, y); - if (E_0 < config.acceptableTolerance) { - ++acceptableIterCounter; - } else { - acceptableIterCounter = 0; - } - - const auto innerIterEndTime = std::chrono::system_clock::now(); - - // Diagnostics for current iteration - if (config.diagnostics) { - if (iterations % 20 == 0) { - sleipnir::println("{:^4} {:^9} {:^13} {:^13} {:^13}", "iter", - "time (ms)", "error", "cost", "infeasibility"); - sleipnir::println("{:=^61}", ""); - } - - sleipnir::println("{:4} {:9.3f} {:13e} {:13e} {:13e}", iterations, - ToMilliseconds(innerIterEndTime - innerIterStartTime), - E_0, f.Value(), c_e.lpNorm<1>()); - } - - ++iterations; - - // Check for max iterations - if (iterations >= config.maxIterations) { - status->exitCondition = SolverExitCondition::kMaxIterationsExceeded; - return; - } - - // Check for max wall clock time - if (innerIterEndTime - solveStartTime > config.timeout) { - status->exitCondition = SolverExitCondition::kTimeout; - return; - } - - // Check for solve to acceptable tolerance - if (E_0 > config.tolerance && - acceptableIterCounter == config.maxAcceptableIterations) { - status->exitCondition = SolverExitCondition::kSolvedToAcceptableTolerance; - return; - } - } -} - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/interior_point.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/interior_point.cpp new file mode 100644 index 0000000000..b1421bf961 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/interior_point.cpp @@ -0,0 +1,664 @@ +// Copyright (c) Sleipnir contributors + +#include "sleipnir/optimization/solver/interior_point.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "optimization/regularized_ldlt.hpp" +#include "optimization/solver/util/error_estimate.hpp" +#include "optimization/solver/util/filter.hpp" +#include "optimization/solver/util/fraction_to_the_boundary_rule.hpp" +#include "optimization/solver/util/is_locally_infeasible.hpp" +#include "optimization/solver/util/kkt_error.hpp" +#include "sleipnir/optimization/solver/exit_status.hpp" +#include "sleipnir/optimization/solver/iteration_info.hpp" +#include "sleipnir/optimization/solver/options.hpp" +#include "sleipnir/util/assert.hpp" +#include "util/print_diagnostics.hpp" +#include "util/scope_exit.hpp" +#include "util/scoped_profiler.hpp" +#include "util/solve_profiler.hpp" + +// See docs/algorithms.md#Works_cited for citation definitions. +// +// See docs/algorithms.md#Interior-point_method for a derivation of the +// interior-point method formulation being used. + +namespace { + +/** + * Interior-point method step direction. + */ +struct Step { + /// Primal step. + Eigen::VectorXd p_x; + /// Equality constraint dual step. + Eigen::VectorXd p_y; + /// Inequality constraint slack variable step. + Eigen::VectorXd p_s; + /// Inequality constraint dual step. + Eigen::VectorXd p_z; +}; + +} // namespace + +namespace slp { + +ExitStatus interior_point( + const InteriorPointMatrixCallbacks& matrix_callbacks, + std::span> + iteration_callbacks, + const Options& options, +#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION + const Eigen::ArrayX& bound_constraint_mask, +#endif + Eigen::VectorXd& x) { + const auto solve_start_time = std::chrono::steady_clock::now(); + + gch::small_vector solve_profilers; + solve_profilers.emplace_back("solver"); + solve_profilers.emplace_back(" ↳ setup"); + solve_profilers.emplace_back(" ↳ iteration"); + solve_profilers.emplace_back(" ↳ feasibility ✓"); + solve_profilers.emplace_back(" ↳ iteration callbacks"); + solve_profilers.emplace_back(" ↳ iter matrix build"); + solve_profilers.emplace_back(" ↳ iter matrix compute"); + solve_profilers.emplace_back(" ↳ iter matrix solve"); + solve_profilers.emplace_back(" ↳ line search"); + solve_profilers.emplace_back(" ↳ SOC"); + solve_profilers.emplace_back(" ↳ next iter prep"); + solve_profilers.emplace_back(" ↳ f(x)"); + solve_profilers.emplace_back(" ↳ ∇f(x)"); + solve_profilers.emplace_back(" ↳ ∇²ₓₓL"); + solve_profilers.emplace_back(" ↳ cₑ(x)"); + solve_profilers.emplace_back(" ↳ ∂cₑ/∂x"); + solve_profilers.emplace_back(" ↳ cᵢ(x)"); + solve_profilers.emplace_back(" ↳ ∂cᵢ/∂x"); + + auto& solver_prof = solve_profilers[0]; + auto& setup_prof = solve_profilers[1]; + auto& inner_iter_prof = solve_profilers[2]; + auto& feasibility_check_prof = solve_profilers[3]; + auto& iteration_callbacks_prof = solve_profilers[4]; + auto& linear_system_build_prof = solve_profilers[5]; + auto& linear_system_compute_prof = solve_profilers[6]; + auto& linear_system_solve_prof = solve_profilers[7]; + auto& line_search_prof = solve_profilers[8]; + auto& soc_prof = solve_profilers[9]; + auto& next_iter_prep_prof = solve_profilers[10]; + + // Set up profiled matrix callbacks +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + auto& f_prof = solve_profilers[11]; + auto& g_prof = solve_profilers[12]; + auto& H_prof = solve_profilers[13]; + auto& c_e_prof = solve_profilers[14]; + auto& A_e_prof = solve_profilers[15]; + auto& c_i_prof = solve_profilers[16]; + auto& A_i_prof = solve_profilers[17]; + + InteriorPointMatrixCallbacks matrices{ + [&](const Eigen::VectorXd& x) -> double { + ScopedProfiler prof{f_prof}; + return matrix_callbacks.f(x); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseVector { + ScopedProfiler prof{g_prof}; + return matrix_callbacks.g(x); + }, + [&](const Eigen::VectorXd& x, const Eigen::VectorXd& y, + const Eigen::VectorXd& z) -> Eigen::SparseMatrix { + ScopedProfiler prof{H_prof}; + return matrix_callbacks.H(x, y, z); + }, + [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { + ScopedProfiler prof{c_e_prof}; + return matrix_callbacks.c_e(x); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix { + ScopedProfiler prof{A_e_prof}; + return matrix_callbacks.A_e(x); + }, + [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { + ScopedProfiler prof{c_i_prof}; + return matrix_callbacks.c_i(x); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix { + ScopedProfiler prof{A_i_prof}; + return matrix_callbacks.A_i(x); + }}; +#else + const auto& matrices = matrix_callbacks; +#endif + + solver_prof.start(); + setup_prof.start(); + + double f = matrices.f(x); + Eigen::VectorXd c_e = matrices.c_e(x); + Eigen::VectorXd c_i = matrices.c_i(x); + + int num_decision_variables = x.rows(); + int num_equality_constraints = c_e.rows(); + int num_inequality_constraints = c_i.rows(); + + // Check for overconstrained problem + if (num_equality_constraints > num_decision_variables) { + if (options.diagnostics) { + print_too_few_dofs_error(c_e); + } + + return ExitStatus::TOO_FEW_DOFS; + } + + Eigen::SparseVector g = matrices.g(x); + Eigen::SparseMatrix A_e = matrices.A_e(x); + Eigen::SparseMatrix A_i = matrices.A_i(x); + + Eigen::VectorXd s = Eigen::VectorXd::Ones(num_inequality_constraints); +#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION + // We set sʲ = cᵢʲ(x) for each bound inequality constraint index j + s = bound_constraint_mask.select(c_i, s); +#endif + Eigen::VectorXd y = Eigen::VectorXd::Zero(num_equality_constraints); + Eigen::VectorXd z = Eigen::VectorXd::Ones(num_inequality_constraints); + + Eigen::SparseMatrix H = matrices.H(x, y, z); + + // Ensure matrix callback dimensions are consistent + slp_assert(g.rows() == num_decision_variables); + slp_assert(A_e.rows() == num_equality_constraints); + slp_assert(A_e.cols() == num_decision_variables); + slp_assert(A_i.rows() == num_inequality_constraints); + slp_assert(A_i.cols() == num_decision_variables); + slp_assert(H.rows() == num_decision_variables); + slp_assert(H.cols() == num_decision_variables); + + // Check whether initial guess has finite f(xₖ), cₑ(xₖ), and cᵢ(xₖ) + if (!std::isfinite(f) || !c_e.allFinite() || !c_i.allFinite()) { + return ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS; + } + + int iterations = 0; + + // Barrier parameter minimum + const double μ_min = options.tolerance / 10.0; + + // Barrier parameter μ + double μ = 0.1; + + // Fraction-to-the-boundary rule scale factor minimum + constexpr double τ_min = 0.99; + + // Fraction-to-the-boundary rule scale factor τ + double τ = τ_min; + + Filter filter; + + // This should be run when the error estimate is below a desired threshold for + // the current barrier parameter + auto update_barrier_parameter_and_reset_filter = [&] { + // Barrier parameter linear decrease power in "κ_μ μ". Range of (0, 1). + constexpr double κ_μ = 0.2; + + // Barrier parameter superlinear decrease power in "μ^(θ_μ)". Range of (1, + // 2). + constexpr double θ_μ = 1.5; + + // Update the barrier parameter. + // + // μⱼ₊₁ = max(εₜₒₗ/10, min(κ_μ μⱼ, μⱼ^θ_μ)) + // + // See equation (7) of [2]. + μ = std::max(μ_min, std::min(κ_μ * μ, std::pow(μ, θ_μ))); + + // Update the fraction-to-the-boundary rule scaling factor. + // + // τⱼ = max(τₘᵢₙ, 1 − μⱼ) + // + // See equation (8) of [2]. + τ = std::max(τ_min, 1.0 - μ); + + // Reset the filter when the barrier parameter is updated + filter.reset(); + }; + + // Kept outside the loop so its storage can be reused + gch::small_vector> triplets; + + RegularizedLDLT solver{num_decision_variables, num_equality_constraints}; + + // Variables for determining when a step is acceptable + constexpr double α_reduction_factor = 0.5; + constexpr double α_min = 1e-7; + + int full_step_rejected_counter = 0; + + // Error estimate + double E_0 = std::numeric_limits::infinity(); + + setup_prof.stop(); + + // Prints final solver diagnostics when the solver exits + scope_exit exit{[&] { + if (options.diagnostics) { + solver_prof.stop(); + if (iterations > 0) { + print_bottom_iteration_diagnostics(); + } + print_solver_diagnostics(solve_profilers); + } + }}; + + while (E_0 > options.tolerance) { + ScopedProfiler inner_iter_profiler{inner_iter_prof}; + ScopedProfiler feasibility_check_profiler{feasibility_check_prof}; + + // Check for local equality constraint infeasibility + if (is_equality_locally_infeasible(A_e, c_e)) { + if (options.diagnostics) { + print_c_e_local_infeasibility_error(c_e); + } + + return ExitStatus::LOCALLY_INFEASIBLE; + } + + // Check for local inequality constraint infeasibility + if (is_inequality_locally_infeasible(A_i, c_i)) { + if (options.diagnostics) { + print_c_i_local_infeasibility_error(c_i); + } + + return ExitStatus::LOCALLY_INFEASIBLE; + } + + // Check for diverging iterates + if (x.lpNorm() > 1e10 || !x.allFinite() || + s.lpNorm() > 1e10 || !s.allFinite()) { + return ExitStatus::DIVERGING_ITERATES; + } + + feasibility_check_profiler.stop(); + ScopedProfiler iteration_callbacks_profiler{iteration_callbacks_prof}; + + // Call iteration callbacks + for (const auto& callback : iteration_callbacks) { + if (callback({iterations, x, g, H, A_e, A_i})) { + return ExitStatus::CALLBACK_REQUESTED_STOP; + } + } + + iteration_callbacks_profiler.stop(); + ScopedProfiler linear_system_build_profiler{linear_system_build_prof}; + + // S = diag(s) + // Z = diag(z) + // Σ = S⁻¹Z + const Eigen::SparseMatrix Σ{s.cwiseInverse().asDiagonal() * + z.asDiagonal()}; + + // lhs = [H + AᵢᵀΣAᵢ Aₑᵀ] + // [ Aₑ 0 ] + // + // Don't assign upper triangle because solver only uses lower triangle. + const Eigen::SparseMatrix top_left = + H + (A_i.transpose() * Σ * A_i).triangularView(); + triplets.clear(); + triplets.reserve(top_left.nonZeros() + A_e.nonZeros()); + for (int col = 0; col < H.cols(); ++col) { + // Append column of H + AᵢᵀΣAᵢ lower triangle in top-left quadrant + for (Eigen::SparseMatrix::InnerIterator it{top_left, col}; it; + ++it) { + triplets.emplace_back(it.row(), it.col(), it.value()); + } + // Append column of Aₑ in bottom-left quadrant + for (Eigen::SparseMatrix::InnerIterator it{A_e, col}; it; ++it) { + triplets.emplace_back(H.rows() + it.row(), it.col(), it.value()); + } + } + Eigen::SparseMatrix lhs( + num_decision_variables + num_equality_constraints, + num_decision_variables + num_equality_constraints); + lhs.setFromSortedTriplets(triplets.begin(), triplets.end(), + [](const auto&, const auto& b) { return b; }); + + // rhs = −[∇f − Aₑᵀy − Aᵢᵀ(−Σcᵢ + μS⁻¹e + z)] + // [ cₑ ] + Eigen::VectorXd rhs{x.rows() + y.rows()}; + rhs.segment(0, x.rows()) = + -g + A_e.transpose() * y + + A_i.transpose() * (-Σ * c_i + μ * s.cwiseInverse() + z); + rhs.segment(x.rows(), y.rows()) = -c_e; + + linear_system_build_profiler.stop(); + ScopedProfiler linear_system_compute_profiler{linear_system_compute_prof}; + + Step step; + double α_max = 1.0; + double α = 1.0; + double α_z = 1.0; + + // Solve the Newton-KKT system + // + // [H + AᵢᵀΣAᵢ Aₑᵀ][ pˣ] = −[∇f − Aₑᵀy − Aᵢᵀ(−Σcᵢ + μS⁻¹e + z)] + // [ Aₑ 0 ][−pʸ] [ cₑ ] + if (solver.compute(lhs).info() != Eigen::Success) [[unlikely]] { + return ExitStatus::FACTORIZATION_FAILED; + } + + linear_system_compute_profiler.stop(); + ScopedProfiler linear_system_solve_profiler{linear_system_solve_prof}; + + auto compute_step = [&](Step& step) { + // p = [ pˣ] + // [−pʸ] + Eigen::VectorXd p = solver.solve(rhs); + step.p_x = p.segment(0, x.rows()); + step.p_y = -p.segment(x.rows(), y.rows()); + + // pˢ = cᵢ − s + Aᵢpˣ + // pᶻ = −Σcᵢ + μS⁻¹e − ΣAᵢpˣ + step.p_s = c_i - s + A_i * step.p_x; + step.p_z = -Σ * c_i + μ * s.cwiseInverse() - Σ * A_i * step.p_x; + }; + compute_step(step); + + linear_system_solve_profiler.stop(); + ScopedProfiler line_search_profiler{line_search_prof}; + + // αᵐᵃˣ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ) + α_max = fraction_to_the_boundary_rule(s, step.p_s, τ); + α = α_max; + + // If maximum step size is below minimum, report line search failure + if (α < α_min) { + return ExitStatus::LINE_SEARCH_FAILED; + } + + // αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ) + α_z = fraction_to_the_boundary_rule(z, step.p_z, τ); + + // Loop until a step is accepted + while (1) { + Eigen::VectorXd trial_x = x + α * step.p_x; + Eigen::VectorXd trial_y = y + α_z * step.p_y; + Eigen::VectorXd trial_z = z + α_z * step.p_z; + + double trial_f = matrices.f(trial_x); + Eigen::VectorXd trial_c_e = matrices.c_e(trial_x); + Eigen::VectorXd trial_c_i = matrices.c_i(trial_x); + + // If f(xₖ + αpₖˣ), cₑ(xₖ + αpₖˣ), or cᵢ(xₖ + αpₖˣ) aren't finite, reduce + // step size immediately + if (!std::isfinite(trial_f) || !trial_c_e.allFinite() || + !trial_c_i.allFinite()) { + // Reduce step size + α *= α_reduction_factor; + + if (α < α_min) { + return ExitStatus::LINE_SEARCH_FAILED; + } + continue; + } + + Eigen::VectorXd trial_s; + if (options.feasible_ipm && c_i.cwiseGreater(0.0).all()) { + // If the inequality constraints are all feasible, prevent them from + // becoming infeasible again. + // + // See equation (19.30) in [1]. + trial_s = trial_c_i; + } else { + trial_s = s + α * step.p_s; + } + + // Check whether filter accepts trial iterate + if (filter.try_add(FilterEntry{trial_f, trial_s, trial_c_e, trial_c_i, μ}, + α)) { + // Accept step + break; + } + + double prev_constraint_violation = + c_e.lpNorm<1>() + (c_i - s).lpNorm<1>(); + double next_constraint_violation = + trial_c_e.lpNorm<1>() + (trial_c_i - trial_s).lpNorm<1>(); + + // Second-order corrections + // + // If first trial point was rejected and constraint violation stayed the + // same or went up, apply second-order corrections + if (α == α_max && + next_constraint_violation >= prev_constraint_violation) { + // Apply second-order corrections. See section 2.4 of [2]. + auto soc_step = step; + + double α_soc = α; + double α_z_soc = α_z; + Eigen::VectorXd c_e_soc = c_e; + + bool step_acceptable = false; + for (int soc_iteration = 0; soc_iteration < 5 && !step_acceptable; + ++soc_iteration) { + ScopedProfiler soc_profiler{soc_prof}; + + scope_exit soc_exit{[&] { + soc_profiler.stop(); + + if (options.diagnostics) { + print_iteration_diagnostics( + iterations, + step_acceptable ? IterationType::ACCEPTED_SOC + : IterationType::REJECTED_SOC, + soc_profiler.current_duration(), + error_estimate(g, A_e, trial_c_e, trial_y), trial_f, + trial_c_e.lpNorm<1>() + (trial_c_i - trial_s).lpNorm<1>(), + trial_s.dot(trial_z), μ, solver.hessian_regularization(), + α_soc, 1.0, α_reduction_factor, α_z_soc); + } + }}; + + // Rebuild Newton-KKT rhs with updated constraint values. + // + // rhs = −[∇f − Aₑᵀy − Aᵢᵀ(−Σcᵢ + μS⁻¹e + z)] + // [ cₑˢᵒᶜ ] + // + // where cₑˢᵒᶜ = αc(xₖ) + c(xₖ + αpₖˣ) + c_e_soc = α_soc * c_e_soc + trial_c_e; + rhs.bottomRows(y.rows()) = -c_e_soc; + + // Solve the Newton-KKT system + compute_step(soc_step); + + // αˢᵒᶜ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ) + α_soc = fraction_to_the_boundary_rule(s, soc_step.p_s, τ); + trial_x = x + α_soc * soc_step.p_x; + trial_s = s + α_soc * soc_step.p_s; + + // αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ) + α_z_soc = fraction_to_the_boundary_rule(z, soc_step.p_z, τ); + trial_y = y + α_z_soc * soc_step.p_y; + trial_z = z + α_z_soc * soc_step.p_z; + + trial_f = matrices.f(trial_x); + trial_c_e = matrices.c_e(trial_x); + trial_c_i = matrices.c_i(trial_x); + + // Constraint violation scale factor for second-order corrections + constexpr double κ_soc = 0.99; + + // If constraint violation hasn't been sufficiently reduced, stop + // making second-order corrections + next_constraint_violation = + trial_c_e.lpNorm<1>() + (trial_c_i - trial_s).lpNorm<1>(); + if (next_constraint_violation > κ_soc * prev_constraint_violation) { + break; + } + + // Check whether filter accepts trial iterate + if (filter.try_add( + FilterEntry{trial_f, trial_s, trial_c_e, trial_c_i, μ}, α)) { + step = soc_step; + α = α_soc; + α_z = α_z_soc; + step_acceptable = true; + } + } + + if (step_acceptable) { + // Accept step + break; + } + } + + // If we got here and α is the full step, the full step was rejected. + // Increment the full-step rejected counter to keep track of how many full + // steps have been rejected in a row. + if (α == α_max) { + ++full_step_rejected_counter; + } + + // If the full step was rejected enough times in a row, reset the filter + // because it may be impeding progress. + // + // See section 3.2 case I of [2]. + if (full_step_rejected_counter >= 4 && + filter.max_constraint_violation > + filter.back().constraint_violation / 10.0) { + filter.max_constraint_violation *= 0.1; + filter.reset(); + continue; + } + + // Reduce step size + α *= α_reduction_factor; + + // If step size hit a minimum, check if the KKT error was reduced. If it + // wasn't, report line search failure. + if (α < α_min) { + double current_kkt_error = kkt_error(g, A_e, c_e, A_i, c_i, s, y, z, μ); + + trial_x = x + α_max * step.p_x; + trial_s = s + α_max * step.p_s; + + trial_y = y + α_z * step.p_y; + trial_z = z + α_z * step.p_z; + + trial_c_e = matrices.c_e(trial_x); + trial_c_i = matrices.c_i(trial_x); + + double next_kkt_error = kkt_error( + matrices.g(trial_x), matrices.A_e(trial_x), matrices.c_e(trial_x), + matrices.A_i(trial_x), trial_c_i, trial_s, trial_y, trial_z, μ); + + // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway + if (next_kkt_error <= 0.999 * current_kkt_error) { + α = α_max; + + // Accept step + break; + } + + return ExitStatus::LINE_SEARCH_FAILED; + } + } + + line_search_profiler.stop(); + + // If full step was accepted, reset full-step rejected counter + if (α == α_max) { + full_step_rejected_counter = 0; + } + + // xₖ₊₁ = xₖ + αₖpₖˣ + // sₖ₊₁ = sₖ + αₖpₖˢ + // yₖ₊₁ = yₖ + αₖᶻpₖʸ + // zₖ₊₁ = zₖ + αₖᶻpₖᶻ + x += α * step.p_x; + s += α * step.p_s; + y += α_z * step.p_y; + z += α_z * step.p_z; + + // A requirement for the convergence proof is that the primal-dual barrier + // term Hessian Σₖ₊₁ does not deviate arbitrarily much from the primal + // barrier term Hessian μSₖ₊₁⁻². + // + // Σₖ₊₁ = μSₖ₊₁⁻² + // Sₖ₊₁⁻¹Zₖ₊₁ = μSₖ₊₁⁻² + // Zₖ₊₁ = μSₖ₊₁⁻¹ + // + // We ensure this by resetting + // + // zₖ₊₁ = clamp(zₖ₊₁, 1/κ_Σ μ/sₖ₊₁, κ_Σ μ/sₖ₊₁) + // + // for some fixed κ_Σ ≥ 1 after each step. See equation (16) of [2]. + for (int row = 0; row < z.rows(); ++row) { + constexpr double κ_Σ = 1e10; + z[row] = std::clamp(z[row], 1.0 / κ_Σ * μ / s[row], κ_Σ * μ / s[row]); + } + + // Update autodiff for Jacobians and Hessian + f = matrices.f(x); + A_e = matrices.A_e(x); + A_i = matrices.A_i(x); + g = matrices.g(x); + H = matrices.H(x, y, z); + + ScopedProfiler next_iter_prep_profiler{next_iter_prep_prof}; + + c_e = matrices.c_e(x); + c_i = matrices.c_i(x); + + // Update the error estimate + E_0 = error_estimate(g, A_e, c_e, A_i, c_i, s, y, z, 0.0); + + // Update the barrier parameter if necessary + if (E_0 > options.tolerance) { + // Barrier parameter scale factor for tolerance checks + constexpr double κ_ε = 10.0; + + // While the error estimate is below the desired threshold for this + // barrier parameter value, decrease the barrier parameter further + double E_μ = error_estimate(g, A_e, c_e, A_i, c_i, s, y, z, μ); + while (μ > μ_min && E_μ <= κ_ε * μ) { + update_barrier_parameter_and_reset_filter(); + E_μ = error_estimate(g, A_e, c_e, A_i, c_i, s, y, z, μ); + } + } + + next_iter_prep_profiler.stop(); + inner_iter_profiler.stop(); + + if (options.diagnostics) { + print_iteration_diagnostics(iterations, IterationType::NORMAL, + inner_iter_profiler.current_duration(), E_0, + f, c_e.lpNorm<1>() + (c_i - s).lpNorm<1>(), + s.dot(z), μ, solver.hessian_regularization(), + α, α_max, α_reduction_factor, α_z); + } + + ++iterations; + + // Check for max iterations + if (iterations >= options.max_iterations) { + return ExitStatus::MAX_ITERATIONS_EXCEEDED; + } + + // Check for max wall clock time + if (std::chrono::steady_clock::now() - solve_start_time > options.timeout) { + return ExitStatus::TIMEOUT; + } + } + + return ExitStatus::SUCCESS; +} + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/newton.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/newton.cpp new file mode 100644 index 0000000000..98ed6c941f --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/newton.cpp @@ -0,0 +1,259 @@ +// Copyright (c) Sleipnir contributors + +#include "sleipnir/optimization/solver/newton.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "optimization/regularized_ldlt.hpp" +#include "optimization/solver/util/error_estimate.hpp" +#include "optimization/solver/util/filter.hpp" +#include "optimization/solver/util/kkt_error.hpp" +#include "sleipnir/optimization/solver/exit_status.hpp" +#include "sleipnir/optimization/solver/iteration_info.hpp" +#include "sleipnir/optimization/solver/options.hpp" +#include "sleipnir/util/assert.hpp" +#include "util/print_diagnostics.hpp" +#include "util/scope_exit.hpp" +#include "util/scoped_profiler.hpp" +#include "util/solve_profiler.hpp" + +// See docs/algorithms.md#Works_cited for citation definitions. + +namespace slp { + +ExitStatus newton(const NewtonMatrixCallbacks& matrix_callbacks, + std::span> + iteration_callbacks, + const Options& options, Eigen::VectorXd& x) { + const auto solve_start_time = std::chrono::steady_clock::now(); + + gch::small_vector solve_profilers; + solve_profilers.emplace_back("solver"); + solve_profilers.emplace_back(" ↳ setup"); + solve_profilers.emplace_back(" ↳ iteration"); + solve_profilers.emplace_back(" ↳ feasibility ✓"); + solve_profilers.emplace_back(" ↳ iteration callbacks"); + solve_profilers.emplace_back(" ↳ iter matrix compute"); + solve_profilers.emplace_back(" ↳ iter matrix solve"); + solve_profilers.emplace_back(" ↳ line search"); + solve_profilers.emplace_back(" ↳ next iter prep"); + solve_profilers.emplace_back(" ↳ f(x)"); + solve_profilers.emplace_back(" ↳ ∇f(x)"); + solve_profilers.emplace_back(" ↳ ∇²ₓₓL"); + + auto& solver_prof = solve_profilers[0]; + auto& setup_prof = solve_profilers[1]; + auto& inner_iter_prof = solve_profilers[2]; + auto& feasibility_check_prof = solve_profilers[3]; + auto& iteration_callbacks_prof = solve_profilers[4]; + auto& linear_system_compute_prof = solve_profilers[5]; + auto& linear_system_solve_prof = solve_profilers[6]; + auto& line_search_prof = solve_profilers[7]; + auto& next_iter_prep_prof = solve_profilers[8]; + + // Set up profiled matrix callbacks +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + auto& f_prof = solve_profilers[9]; + auto& g_prof = solve_profilers[10]; + auto& H_prof = solve_profilers[11]; + + NewtonMatrixCallbacks matrices{ + [&](const Eigen::VectorXd& x) -> double { + ScopedProfiler prof{f_prof}; + return matrix_callbacks.f(x); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseVector { + ScopedProfiler prof{g_prof}; + return matrix_callbacks.g(x); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix { + ScopedProfiler prof{H_prof}; + return matrix_callbacks.H(x); + }}; +#else + const auto& matrices = matrix_callbacks; +#endif + + solver_prof.start(); + setup_prof.start(); + + double f = matrices.f(x); + + int num_decision_variables = x.rows(); + + Eigen::SparseVector g = matrices.g(x); + Eigen::SparseMatrix H = matrices.H(x); + + // Ensure matrix callback dimensions are consistent + slp_assert(g.rows() == num_decision_variables); + slp_assert(H.rows() == num_decision_variables); + slp_assert(H.cols() == num_decision_variables); + + // Check whether initial guess has finite f(xₖ) + if (!std::isfinite(f)) { + return ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS; + } + + int iterations = 0; + + Filter filter; + + RegularizedLDLT solver{num_decision_variables, 0}; + + // Variables for determining when a step is acceptable + constexpr double α_reduction_factor = 0.5; + constexpr double α_min = 1e-20; + + // Error estimate + double E_0 = std::numeric_limits::infinity(); + + setup_prof.stop(); + + // Prints final solver diagnostics when the solver exits + scope_exit exit{[&] { + if (options.diagnostics) { + solver_prof.stop(); + if (iterations > 0) { + print_bottom_iteration_diagnostics(); + } + print_solver_diagnostics(solve_profilers); + } + }}; + + while (E_0 > options.tolerance) { + ScopedProfiler inner_iter_profiler{inner_iter_prof}; + ScopedProfiler feasibility_check_profiler{feasibility_check_prof}; + + // Check for diverging iterates + if (x.lpNorm() > 1e10 || !x.allFinite()) { + return ExitStatus::DIVERGING_ITERATES; + } + + feasibility_check_profiler.stop(); + ScopedProfiler iteration_callbacks_profiler{iteration_callbacks_prof}; + + // Call iteration callbacks + for (const auto& callback : iteration_callbacks) { + if (callback({iterations, x, g, H, Eigen::SparseMatrix{}, + Eigen::SparseMatrix{}})) { + return ExitStatus::CALLBACK_REQUESTED_STOP; + } + } + + iteration_callbacks_profiler.stop(); + ScopedProfiler linear_system_compute_profiler{linear_system_compute_prof}; + + // Solve the Newton-KKT system + // + // Hpˣ = −∇f + solver.compute(H); + + linear_system_compute_profiler.stop(); + ScopedProfiler linear_system_solve_profiler{linear_system_solve_prof}; + + Eigen::VectorXd p_x = solver.solve(-g); + + linear_system_solve_profiler.stop(); + ScopedProfiler line_search_profiler{line_search_prof}; + + constexpr double α_max = 1.0; + double α = α_max; + + // Loop until a step is accepted. If a step becomes acceptable, the loop + // will exit early. + while (1) { + Eigen::VectorXd trial_x = x + α * p_x; + + double trial_f = matrices.f(trial_x); + + // If f(xₖ + αpₖˣ) isn't finite, reduce step size immediately + if (!std::isfinite(trial_f)) { + // Reduce step size + α *= α_reduction_factor; + + if (α < α_min) { + return ExitStatus::LINE_SEARCH_FAILED; + } + continue; + } + + // Check whether filter accepts trial iterate + if (filter.try_add(FilterEntry{trial_f}, α)) { + // Accept step + break; + } + + // Reduce step size + α *= α_reduction_factor; + + // If step size hit a minimum, check if the KKT error was reduced. If it + // wasn't, report bad line search. + if (α < α_min) { + double current_kkt_error = kkt_error(g); + + Eigen::VectorXd trial_x = x + α_max * p_x; + + double next_kkt_error = kkt_error(matrices.g(trial_x)); + + // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway + if (next_kkt_error <= 0.999 * current_kkt_error) { + α = α_max; + + // Accept step + break; + } + + return ExitStatus::LINE_SEARCH_FAILED; + } + } + + line_search_profiler.stop(); + + // xₖ₊₁ = xₖ + αₖpₖˣ + x += α * p_x; + + // Update autodiff for Hessian + f = matrices.f(x); + g = matrices.g(x); + H = matrices.H(x); + + ScopedProfiler next_iter_prep_profiler{next_iter_prep_prof}; + + // Update the error estimate + E_0 = error_estimate(g); + + next_iter_prep_profiler.stop(); + inner_iter_profiler.stop(); + + if (options.diagnostics) { + print_iteration_diagnostics( + iterations, IterationType::NORMAL, + inner_iter_profiler.current_duration(), E_0, f, 0.0, 0.0, 0.0, + solver.hessian_regularization(), α, α_max, α_reduction_factor, 1.0); + } + + ++iterations; + + // Check for max iterations + if (iterations >= options.max_iterations) { + return ExitStatus::MAX_ITERATIONS_EXCEEDED; + } + + // Check for max wall clock time + if (std::chrono::steady_clock::now() - solve_start_time > options.timeout) { + return ExitStatus::TIMEOUT; + } + } + + return ExitStatus::SUCCESS; +} + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/sqp.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/sqp.cpp new file mode 100644 index 0000000000..04447f8892 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/sqp.cpp @@ -0,0 +1,480 @@ +// Copyright (c) Sleipnir contributors + +#include "sleipnir/optimization/solver/sqp.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "optimization/regularized_ldlt.hpp" +#include "optimization/solver/util/error_estimate.hpp" +#include "optimization/solver/util/filter.hpp" +#include "optimization/solver/util/is_locally_infeasible.hpp" +#include "optimization/solver/util/kkt_error.hpp" +#include "sleipnir/optimization/solver/exit_status.hpp" +#include "sleipnir/optimization/solver/iteration_info.hpp" +#include "sleipnir/optimization/solver/options.hpp" +#include "sleipnir/util/assert.hpp" +#include "util/print_diagnostics.hpp" +#include "util/scope_exit.hpp" +#include "util/scoped_profiler.hpp" +#include "util/solve_profiler.hpp" + +// See docs/algorithms.md#Works_cited for citation definitions. + +namespace { + +/** + * SQP step direction. + */ +struct Step { + /// Primal step. + Eigen::VectorXd p_x; + /// Dual step. + Eigen::VectorXd p_y; +}; + +} // namespace + +namespace slp { + +ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, + std::span> + iteration_callbacks, + const Options& options, Eigen::VectorXd& x) { + const auto solve_start_time = std::chrono::steady_clock::now(); + + gch::small_vector solve_profilers; + solve_profilers.emplace_back("solver"); + solve_profilers.emplace_back(" ↳ setup"); + solve_profilers.emplace_back(" ↳ iteration"); + solve_profilers.emplace_back(" ↳ feasibility ✓"); + solve_profilers.emplace_back(" ↳ iteration callbacks"); + solve_profilers.emplace_back(" ↳ iter matrix build"); + solve_profilers.emplace_back(" ↳ iter matrix compute"); + solve_profilers.emplace_back(" ↳ iter matrix solve"); + solve_profilers.emplace_back(" ↳ line search"); + solve_profilers.emplace_back(" ↳ SOC"); + solve_profilers.emplace_back(" ↳ next iter prep"); + solve_profilers.emplace_back(" ↳ f(x)"); + solve_profilers.emplace_back(" ↳ ∇f(x)"); + solve_profilers.emplace_back(" ↳ ∇²ₓₓL"); + solve_profilers.emplace_back(" ↳ cₑ(x)"); + solve_profilers.emplace_back(" ↳ ∂cₑ/∂x"); + + auto& solver_prof = solve_profilers[0]; + auto& setup_prof = solve_profilers[1]; + auto& inner_iter_prof = solve_profilers[2]; + auto& feasibility_check_prof = solve_profilers[3]; + auto& iteration_callbacks_prof = solve_profilers[4]; + auto& linear_system_build_prof = solve_profilers[5]; + auto& linear_system_compute_prof = solve_profilers[6]; + auto& linear_system_solve_prof = solve_profilers[7]; + auto& line_search_prof = solve_profilers[8]; + auto& soc_prof = solve_profilers[9]; + auto& next_iter_prep_prof = solve_profilers[10]; + + // Set up profiled matrix callbacks +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + auto& f_prof = solve_profilers[11]; + auto& g_prof = solve_profilers[12]; + auto& H_prof = solve_profilers[13]; + auto& c_e_prof = solve_profilers[14]; + auto& A_e_prof = solve_profilers[15]; + + SQPMatrixCallbacks matrices{ + [&](const Eigen::VectorXd& x) -> double { + ScopedProfiler prof{f_prof}; + return matrix_callbacks.f(x); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseVector { + ScopedProfiler prof{g_prof}; + return matrix_callbacks.g(x); + }, + [&](const Eigen::VectorXd& x, + const Eigen::VectorXd& y) -> Eigen::SparseMatrix { + ScopedProfiler prof{H_prof}; + return matrix_callbacks.H(x, y); + }, + [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { + ScopedProfiler prof{c_e_prof}; + return matrix_callbacks.c_e(x); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix { + ScopedProfiler prof{A_e_prof}; + return matrix_callbacks.A_e(x); + }}; +#else + const auto& matrices = matrix_callbacks; +#endif + + solver_prof.start(); + setup_prof.start(); + + double f = matrices.f(x); + Eigen::VectorXd c_e = matrices.c_e(x); + + int num_decision_variables = x.rows(); + int num_equality_constraints = c_e.rows(); + + // Check for overconstrained problem + if (num_equality_constraints > num_decision_variables) { + if (options.diagnostics) { + print_too_few_dofs_error(c_e); + } + + return ExitStatus::TOO_FEW_DOFS; + } + + Eigen::SparseVector g = matrices.g(x); + Eigen::SparseMatrix A_e = matrices.A_e(x); + + Eigen::VectorXd y = Eigen::VectorXd::Zero(num_equality_constraints); + + Eigen::SparseMatrix H = matrices.H(x, y); + + // Ensure matrix callback dimensions are consistent + slp_assert(g.rows() == num_decision_variables); + slp_assert(A_e.rows() == num_equality_constraints); + slp_assert(A_e.cols() == num_decision_variables); + slp_assert(H.rows() == num_decision_variables); + slp_assert(H.cols() == num_decision_variables); + + // Check whether initial guess has finite f(xₖ) and cₑ(xₖ) + if (!std::isfinite(f) || !c_e.allFinite()) { + return ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS; + } + + int iterations = 0; + + Filter filter; + + // Kept outside the loop so its storage can be reused + gch::small_vector> triplets; + + RegularizedLDLT solver{num_decision_variables, num_equality_constraints}; + + // Variables for determining when a step is acceptable + constexpr double α_reduction_factor = 0.5; + constexpr double α_min = 1e-7; + + int full_step_rejected_counter = 0; + + // Error estimate + double E_0 = std::numeric_limits::infinity(); + + setup_prof.stop(); + + // Prints final solver diagnostics when the solver exits + scope_exit exit{[&] { + if (options.diagnostics) { + solver_prof.stop(); + if (iterations > 0) { + print_bottom_iteration_diagnostics(); + } + print_solver_diagnostics(solve_profilers); + } + }}; + + while (E_0 > options.tolerance) { + ScopedProfiler inner_iter_profiler{inner_iter_prof}; + ScopedProfiler feasibility_check_profiler{feasibility_check_prof}; + + // Check for local equality constraint infeasibility + if (is_equality_locally_infeasible(A_e, c_e)) { + if (options.diagnostics) { + print_c_e_local_infeasibility_error(c_e); + } + + return ExitStatus::LOCALLY_INFEASIBLE; + } + + // Check for diverging iterates + if (x.lpNorm() > 1e10 || !x.allFinite()) { + return ExitStatus::DIVERGING_ITERATES; + } + + feasibility_check_profiler.stop(); + ScopedProfiler iteration_callbacks_profiler{iteration_callbacks_prof}; + + // Call iteration callbacks + for (const auto& callback : iteration_callbacks) { + if (callback({iterations, x, g, H, A_e, Eigen::SparseMatrix{}})) { + return ExitStatus::CALLBACK_REQUESTED_STOP; + } + } + + iteration_callbacks_profiler.stop(); + ScopedProfiler linear_system_build_profiler{linear_system_build_prof}; + + // lhs = [H Aₑᵀ] + // [Aₑ 0 ] + // + // Don't assign upper triangle because solver only uses lower triangle. + triplets.clear(); + triplets.reserve(H.nonZeros() + A_e.nonZeros()); + for (int col = 0; col < H.cols(); ++col) { + // Append column of H lower triangle in top-left quadrant + for (Eigen::SparseMatrix::InnerIterator it{H, col}; it; ++it) { + triplets.emplace_back(it.row(), it.col(), it.value()); + } + // Append column of Aₑ in bottom-left quadrant + for (Eigen::SparseMatrix::InnerIterator it{A_e, col}; it; ++it) { + triplets.emplace_back(H.rows() + it.row(), it.col(), it.value()); + } + } + Eigen::SparseMatrix lhs( + num_decision_variables + num_equality_constraints, + num_decision_variables + num_equality_constraints); + lhs.setFromSortedTriplets(triplets.begin(), triplets.end(), + [](const auto&, const auto& b) { return b; }); + + // rhs = −[∇f − Aₑᵀy] + // [ cₑ ] + Eigen::VectorXd rhs{x.rows() + y.rows()}; + rhs.segment(0, x.rows()) = -g + A_e.transpose() * y; + rhs.segment(x.rows(), y.rows()) = -c_e; + + linear_system_build_profiler.stop(); + ScopedProfiler linear_system_compute_profiler{linear_system_compute_prof}; + + Step step; + constexpr double α_max = 1.0; + double α = 1.0; + + // Solve the Newton-KKT system + // + // [H Aₑᵀ][ pˣ] = −[∇f − Aₑᵀy] + // [Aₑ 0 ][−pʸ] [ cₑ ] + if (solver.compute(lhs).info() != Eigen::Success) [[unlikely]] { + return ExitStatus::FACTORIZATION_FAILED; + } + + linear_system_compute_profiler.stop(); + ScopedProfiler linear_system_solve_profiler{linear_system_solve_prof}; + + auto compute_step = [&](Step& step) { + // p = [ pˣ] + // [−pʸ] + Eigen::VectorXd p = solver.solve(rhs); + step.p_x = p.segment(0, x.rows()); + step.p_y = -p.segment(x.rows(), y.rows()); + }; + compute_step(step); + + linear_system_solve_profiler.stop(); + ScopedProfiler line_search_profiler{line_search_prof}; + + α = α_max; + + // Loop until a step is accepted + while (1) { + Eigen::VectorXd trial_x = x + α * step.p_x; + Eigen::VectorXd trial_y = y + α * step.p_y; + + double trial_f = matrices.f(trial_x); + Eigen::VectorXd trial_c_e = matrices.c_e(trial_x); + + // If f(xₖ + αpₖˣ) or cₑ(xₖ + αpₖˣ) aren't finite, reduce step size + // immediately + if (!std::isfinite(trial_f) || !trial_c_e.allFinite()) { + // Reduce step size + α *= α_reduction_factor; + + if (α < α_min) { + return ExitStatus::LINE_SEARCH_FAILED; + } + continue; + } + + // Check whether filter accepts trial iterate + if (filter.try_add(FilterEntry{trial_f, trial_c_e}, α)) { + // Accept step + break; + } + + double prev_constraint_violation = c_e.lpNorm<1>(); + double next_constraint_violation = trial_c_e.lpNorm<1>(); + + // Second-order corrections + // + // If first trial point was rejected and constraint violation stayed the + // same or went up, apply second-order corrections + if (α == α_max && + next_constraint_violation >= prev_constraint_violation) { + // Apply second-order corrections. See section 2.4 of [2]. + auto soc_step = step; + + double α_soc = α; + Eigen::VectorXd c_e_soc = c_e; + + bool step_acceptable = false; + for (int soc_iteration = 0; soc_iteration < 5 && !step_acceptable; + ++soc_iteration) { + ScopedProfiler soc_profiler{soc_prof}; + + scope_exit soc_exit{[&] { + soc_profiler.stop(); + + if (options.diagnostics) { + print_iteration_diagnostics( + iterations, + step_acceptable ? IterationType::ACCEPTED_SOC + : IterationType::REJECTED_SOC, + soc_profiler.current_duration(), + error_estimate(g, A_e, trial_c_e, trial_y), trial_f, + trial_c_e.lpNorm<1>(), 0.0, 0.0, + solver.hessian_regularization(), α_soc, 1.0, + α_reduction_factor, 1.0); + } + }}; + + // Rebuild Newton-KKT rhs with updated constraint values. + // + // rhs = −[∇f − Aₑᵀy] + // [ cₑˢᵒᶜ ] + // + // where cₑˢᵒᶜ = αc(xₖ) + c(xₖ + αpₖˣ) + c_e_soc = α_soc * c_e_soc + trial_c_e; + rhs.bottomRows(y.rows()) = -c_e_soc; + + // Solve the Newton-KKT system + compute_step(soc_step); + + trial_x = x + α_soc * soc_step.p_x; + trial_y = y + α_soc * soc_step.p_y; + + trial_f = matrices.f(trial_x); + trial_c_e = matrices.c_e(trial_x); + + // Constraint violation scale factor for second-order corrections + constexpr double κ_soc = 0.99; + + // If constraint violation hasn't been sufficiently reduced, stop + // making second-order corrections + next_constraint_violation = trial_c_e.lpNorm<1>(); + if (next_constraint_violation > κ_soc * prev_constraint_violation) { + break; + } + + // Check whether filter accepts trial iterate + if (filter.try_add(FilterEntry{trial_f, trial_c_e}, α)) { + step = soc_step; + α = α_soc; + step_acceptable = true; + } + } + + if (step_acceptable) { + // Accept step + break; + } + } + + // If we got here and α is the full step, the full step was rejected. + // Increment the full-step rejected counter to keep track of how many full + // steps have been rejected in a row. + if (α == α_max) { + ++full_step_rejected_counter; + } + + // If the full step was rejected enough times in a row, reset the filter + // because it may be impeding progress. + // + // See section 3.2 case I of [2]. + if (full_step_rejected_counter >= 4 && + filter.max_constraint_violation > + filter.back().constraint_violation / 10.0) { + filter.max_constraint_violation *= 0.1; + filter.reset(); + continue; + } + + // Reduce step size + α *= α_reduction_factor; + + // If step size hit a minimum, check if the KKT error was reduced. If it + // wasn't, report line search failure. + if (α < α_min) { + double current_kkt_error = kkt_error(g, A_e, c_e, y); + + trial_x = x + α_max * step.p_x; + trial_y = y + α_max * step.p_y; + + trial_f = matrices.f(trial_x); + trial_c_e = matrices.c_e(trial_x); + + double next_kkt_error = kkt_error( + matrices.g(trial_x), matrices.A_e(trial_x), trial_c_e, trial_y); + + // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway + if (next_kkt_error <= 0.999 * current_kkt_error) { + α = α_max; + + // Accept step + break; + } + + return ExitStatus::LINE_SEARCH_FAILED; + } + } + + line_search_profiler.stop(); + + // If full step was accepted, reset full-step rejected counter + if (α == α_max) { + full_step_rejected_counter = 0; + } + + // xₖ₊₁ = xₖ + αₖpₖˣ + // yₖ₊₁ = yₖ + αₖpₖʸ + x += α * step.p_x; + y += α * step.p_y; + + // Update autodiff for Jacobians and Hessian + f = matrices.f(x); + A_e = matrices.A_e(x); + g = matrices.g(x); + H = matrices.H(x, y); + + ScopedProfiler next_iter_prep_profiler{next_iter_prep_prof}; + + c_e = matrices.c_e(x); + + // Update the error estimate + E_0 = error_estimate(g, A_e, c_e, y); + + next_iter_prep_profiler.stop(); + inner_iter_profiler.stop(); + + if (options.diagnostics) { + print_iteration_diagnostics(iterations, IterationType::NORMAL, + inner_iter_profiler.current_duration(), E_0, + f, c_e.lpNorm<1>(), 0.0, 0.0, + solver.hessian_regularization(), α, α_max, + α_reduction_factor, α); + } + + ++iterations; + + // Check for max iterations + if (iterations >= options.max_iterations) { + return ExitStatus::MAX_ITERATIONS_EXCEEDED; + } + + // Check for max wall clock time + if (std::chrono::steady_clock::now() - solve_start_time > options.timeout) { + return ExitStatus::TIMEOUT; + } + } + + return ExitStatus::SUCCESS; +} + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp deleted file mode 100644 index 79b5d99ae2..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp +++ /dev/null @@ -1,400 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include -#include -#include -#include - -#include -#include - -#include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/autodiff/VariableMatrix.hpp" -#include "sleipnir/optimization/SolverConfig.hpp" -#include "sleipnir/optimization/SolverIterationInfo.hpp" -#include "sleipnir/optimization/SolverStatus.hpp" -#include "sleipnir/optimization/solver/InteriorPoint.hpp" -#include "sleipnir/util/FunctionRef.hpp" - -namespace sleipnir { - -/** - * Finds the iterate that minimizes the constraint violation while not deviating - * too far from the starting point. This is a fallback procedure when the normal - * Sequential Quadratic Programming method fails to converge to a feasible - * point. - * - * @param[in] decisionVariables The list of decision variables. - * @param[in] equalityConstraints The list of equality constraints. - * @param[in] callback The user callback. - * @param[in] config Configuration options for the solver. - * @param[in,out] x The current iterate from the normal solve. - * @param[out] status The solver status. - */ -inline void FeasibilityRestoration( - std::span decisionVariables, - std::span equalityConstraints, - function_ref callback, - const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status) { - // Feasibility restoration - // - // min ρ Σ (pₑ + nₑ) + ζ/2 (x - x_R)ᵀD_R(x - x_R) - // x - // pₑ,nₑ - // - // s.t. cₑ(x) - pₑ + nₑ = 0 - // pₑ ≥ 0 - // nₑ ≥ 0 - // - // where ρ = 1000, ζ = √μ where μ is the barrier parameter, x_R is original - // iterate before feasibility restoration, and D_R is a scaling matrix defined - // by - // - // D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾) - - constexpr double ρ = 1000.0; - double μ = config.tolerance / 10.0; - - wpi::SmallVector fr_decisionVariables; - fr_decisionVariables.reserve(decisionVariables.size() + - 2 * equalityConstraints.size()); - - // Assign x - fr_decisionVariables.assign(decisionVariables.begin(), - decisionVariables.end()); - - // Allocate pₑ and nₑ - for (size_t row = 0; row < 2 * equalityConstraints.size(); ++row) { - fr_decisionVariables.emplace_back(); - } - - auto it = fr_decisionVariables.begin(); - - VariableMatrix xAD{std::span{it, it + decisionVariables.size()}}; - it += decisionVariables.size(); - - VariableMatrix p_e{std::span{it, it + equalityConstraints.size()}}; - it += equalityConstraints.size(); - - VariableMatrix n_e{std::span{it, it + equalityConstraints.size()}}; - it += equalityConstraints.size(); - - // Set initial values for pₑ and nₑ. - // - // - // From equation (33) of [2]: - // ______________________ - // μ − ρ c(x) /(μ − ρ c(x))² μ c(x) - // n = −−−−−−−−−− + / (−−−−−−−−−−) + −−−−−− (1) - // 2ρ √ ( 2ρ ) 2ρ - // - // The quadratic formula: - // ________ - // -b + √b² - 4ac - // x = −−−−−−−−−−−−−− (2) - // 2a - // - // Rearrange (1) to fit the quadratic formula better: - // _________________________ - // μ - ρ c(x) + √(μ - ρ c(x))² + 2ρ μ c(x) - // n = −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− - // 2ρ - // - // Solve for coefficients: - // - // a = ρ (3) - // b = ρ c(x) - μ (4) - // - // -4ac = μ c(x) 2ρ - // -4(ρ)c = 2ρ μ c(x) - // -4c = 2μ c(x) - // c = -μ c(x)/2 (5) - // - // p = c(x) + n (6) - for (int row = 0; row < p_e.Rows(); ++row) { - double c_e = equalityConstraints[row].Value(); - - constexpr double a = 2 * ρ; - double b = ρ * c_e - μ; - double c = -μ * c_e / 2.0; - - double n = -b * std::sqrt(b * b - 4.0 * a * c) / (2.0 * a); - double p = c_e + n; - - p_e(row).SetValue(p); - n_e(row).SetValue(n); - } - - // cₑ(x) - pₑ + nₑ = 0 - wpi::SmallVector fr_equalityConstraints; - fr_equalityConstraints.assign(equalityConstraints.begin(), - equalityConstraints.end()); - for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) { - auto& constraint = fr_equalityConstraints[row]; - constraint = constraint - p_e(row) + n_e(row); - } - - // cᵢ(x) - s - pᵢ + nᵢ = 0 - wpi::SmallVector fr_inequalityConstraints; - - // pₑ ≥ 0 - std::copy(p_e.begin(), p_e.end(), - std::back_inserter(fr_inequalityConstraints)); - - // nₑ ≥ 0 - std::copy(n_e.begin(), n_e.end(), - std::back_inserter(fr_inequalityConstraints)); - - Variable J = 0.0; - - // J += ρ Σ (pₑ + nₑ) - for (auto& elem : p_e) { - J += elem; - } - for (auto& elem : n_e) { - J += elem; - } - J *= ρ; - - // D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾) - Eigen::VectorXd D_R{x.rows()}; - for (int row = 0; row < D_R.rows(); ++row) { - D_R(row) = std::min(1.0, 1.0 / std::abs(x(row))); - } - - // J += ζ/2 (x - x_R)ᵀD_R(x - x_R) - for (int row = 0; row < x.rows(); ++row) { - J += std::sqrt(μ) / 2.0 * D_R(row) * sleipnir::pow(xAD(row) - x(row), 2); - } - - Eigen::VectorXd fr_x = VariableMatrix{fr_decisionVariables}.Value(); - - // Set up initial value for inequality constraint slack variables - Eigen::VectorXd fr_s{fr_inequalityConstraints.size()}; - fr_s.setOnes(); - - InteriorPoint(fr_decisionVariables, fr_equalityConstraints, - fr_inequalityConstraints, J, callback, config, true, fr_x, fr_s, - status); - - x = fr_x.segment(0, decisionVariables.size()); -} - -/** - * Finds the iterate that minimizes the constraint violation while not deviating - * too far from the starting point. This is a fallback procedure when the normal - * interior-point method fails to converge to a feasible point. - * - * @param[in] decisionVariables The list of decision variables. - * @param[in] equalityConstraints The list of equality constraints. - * @param[in] inequalityConstraints The list of inequality constraints. - * @param[in] μ Barrier parameter. - * @param[in] callback The user callback. - * @param[in] config Configuration options for the solver. - * @param[in,out] x The current iterate from the normal solve. - * @param[in,out] s The current inequality constraint slack variables from the - * normal solve. - * @param[out] status The solver status. - */ -inline void FeasibilityRestoration( - std::span decisionVariables, - std::span equalityConstraints, - std::span inequalityConstraints, double μ, - function_ref callback, - const SolverConfig& config, Eigen::VectorXd& x, Eigen::VectorXd& s, - SolverStatus* status) { - // Feasibility restoration - // - // min ρ Σ (pₑ + nₑ + pᵢ + nᵢ) + ζ/2 (x - x_R)ᵀD_R(x - x_R) - // x - // pₑ,nₑ - // pᵢ,nᵢ - // - // s.t. cₑ(x) - pₑ + nₑ = 0 - // cᵢ(x) - s - pᵢ + nᵢ = 0 - // pₑ ≥ 0 - // nₑ ≥ 0 - // pᵢ ≥ 0 - // nᵢ ≥ 0 - // - // where ρ = 1000, ζ = √μ where μ is the barrier parameter, x_R is original - // iterate before feasibility restoration, and D_R is a scaling matrix defined - // by - // - // D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾) - - constexpr double ρ = 1000.0; - - wpi::SmallVector fr_decisionVariables; - fr_decisionVariables.reserve(decisionVariables.size() + - 2 * equalityConstraints.size() + - 2 * inequalityConstraints.size()); - - // Assign x - fr_decisionVariables.assign(decisionVariables.begin(), - decisionVariables.end()); - - // Allocate pₑ, nₑ, pᵢ, and nᵢ - for (size_t row = 0; - row < 2 * equalityConstraints.size() + 2 * inequalityConstraints.size(); - ++row) { - fr_decisionVariables.emplace_back(); - } - - auto it = fr_decisionVariables.begin(); - - VariableMatrix xAD{std::span{it, it + decisionVariables.size()}}; - it += decisionVariables.size(); - - VariableMatrix p_e{std::span{it, it + equalityConstraints.size()}}; - it += equalityConstraints.size(); - - VariableMatrix n_e{std::span{it, it + equalityConstraints.size()}}; - it += equalityConstraints.size(); - - VariableMatrix p_i{std::span{it, it + inequalityConstraints.size()}}; - it += inequalityConstraints.size(); - - VariableMatrix n_i{std::span{it, it + inequalityConstraints.size()}}; - - // Set initial values for pₑ, nₑ, pᵢ, and nᵢ. - // - // - // From equation (33) of [2]: - // ______________________ - // μ − ρ c(x) /(μ − ρ c(x))² μ c(x) - // n = −−−−−−−−−− + / (−−−−−−−−−−) + −−−−−− (1) - // 2ρ √ ( 2ρ ) 2ρ - // - // The quadratic formula: - // ________ - // -b + √b² - 4ac - // x = −−−−−−−−−−−−−− (2) - // 2a - // - // Rearrange (1) to fit the quadratic formula better: - // _________________________ - // μ - ρ c(x) + √(μ - ρ c(x))² + 2ρ μ c(x) - // n = −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− - // 2ρ - // - // Solve for coefficients: - // - // a = ρ (3) - // b = ρ c(x) - μ (4) - // - // -4ac = μ c(x) 2ρ - // -4(ρ)c = 2ρ μ c(x) - // -4c = 2μ c(x) - // c = -μ c(x)/2 (5) - // - // p = c(x) + n (6) - for (int row = 0; row < p_e.Rows(); ++row) { - double c_e = equalityConstraints[row].Value(); - - constexpr double a = 2 * ρ; - double b = ρ * c_e - μ; - double c = -μ * c_e / 2.0; - - double n = -b * std::sqrt(b * b - 4.0 * a * c) / (2.0 * a); - double p = c_e + n; - - p_e(row).SetValue(p); - n_e(row).SetValue(n); - } - for (int row = 0; row < p_i.Rows(); ++row) { - double c_i = inequalityConstraints[row].Value() - s(row); - - constexpr double a = 2 * ρ; - double b = ρ * c_i - μ; - double c = -μ * c_i / 2.0; - - double n = -b * std::sqrt(b * b - 4.0 * a * c) / (2.0 * a); - double p = c_i + n; - - p_i(row).SetValue(p); - n_i(row).SetValue(n); - } - - // cₑ(x) - pₑ + nₑ = 0 - wpi::SmallVector fr_equalityConstraints; - fr_equalityConstraints.assign(equalityConstraints.begin(), - equalityConstraints.end()); - for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) { - auto& constraint = fr_equalityConstraints[row]; - constraint = constraint - p_e(row) + n_e(row); - } - - // cᵢ(x) - s - pᵢ + nᵢ = 0 - wpi::SmallVector fr_inequalityConstraints; - fr_inequalityConstraints.assign(inequalityConstraints.begin(), - inequalityConstraints.end()); - for (size_t row = 0; row < fr_inequalityConstraints.size(); ++row) { - auto& constraint = fr_inequalityConstraints[row]; - constraint = constraint - s(row) - p_i(row) + n_i(row); - } - - // pₑ ≥ 0 - std::copy(p_e.begin(), p_e.end(), - std::back_inserter(fr_inequalityConstraints)); - - // pᵢ ≥ 0 - std::copy(p_i.begin(), p_i.end(), - std::back_inserter(fr_inequalityConstraints)); - - // nₑ ≥ 0 - std::copy(n_e.begin(), n_e.end(), - std::back_inserter(fr_inequalityConstraints)); - - // nᵢ ≥ 0 - std::copy(n_i.begin(), n_i.end(), - std::back_inserter(fr_inequalityConstraints)); - - Variable J = 0.0; - - // J += ρ Σ (pₑ + nₑ + pᵢ + nᵢ) - for (auto& elem : p_e) { - J += elem; - } - for (auto& elem : p_i) { - J += elem; - } - for (auto& elem : n_e) { - J += elem; - } - for (auto& elem : n_i) { - J += elem; - } - J *= ρ; - - // D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾) - Eigen::VectorXd D_R{x.rows()}; - for (int row = 0; row < D_R.rows(); ++row) { - D_R(row) = std::min(1.0, 1.0 / std::abs(x(row))); - } - - // J += ζ/2 (x - x_R)ᵀD_R(x - x_R) - for (int row = 0; row < x.rows(); ++row) { - J += std::sqrt(μ) / 2.0 * D_R(row) * sleipnir::pow(xAD(row) - x(row), 2); - } - - Eigen::VectorXd fr_x = VariableMatrix{fr_decisionVariables}.Value(); - - // Set up initial value for inequality constraint slack variables - Eigen::VectorXd fr_s{fr_inequalityConstraints.size()}; - fr_s.segment(0, inequalityConstraints.size()) = s; - fr_s.segment(inequalityConstraints.size(), - fr_s.size() - inequalityConstraints.size()) - .setOnes(); - - InteriorPoint(fr_decisionVariables, fr_equalityConstraints, - fr_inequalityConstraints, J, callback, config, true, fr_x, fr_s, - status); - - x = fr_x.segment(0, decisionVariables.size()); - s = fr_s.segment(0, inequalityConstraints.size()); -} - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp deleted file mode 100644 index 0c14efd7b8..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp +++ /dev/null @@ -1,183 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include -#include -#include -#include - -#include -#include - -#include "sleipnir/autodiff/Variable.hpp" - -// See docs/algorithms.md#Works_cited for citation definitions. - -namespace sleipnir { - -/** - * Filter entry consisting of cost and constraint violation. - */ -struct FilterEntry { - /// The cost function's value - double cost = 0.0; - - /// The constraint violation - double constraintViolation = 0.0; - - constexpr FilterEntry() = default; - - /** - * Constructs a FilterEntry. - * - * @param cost The cost function's value. - * @param constraintViolation The constraint violation. - */ - constexpr FilterEntry(double cost, double constraintViolation) - : cost{cost}, constraintViolation{constraintViolation} {} -}; - -/** - * Step filter. - * - * See the section on filters in chapter 15 of [1]. - */ -class Filter { - public: - static constexpr double γCost = 1e-8; - static constexpr double γConstraint = 1e-5; - - double maxConstraintViolation = 1e4; - - /** - * Construct an empty filter. - * - * @param f The cost function. - */ - explicit Filter(Variable& f) { - m_f = &f; - - // Initial filter entry rejects constraint violations above max - m_filter.emplace_back(std::numeric_limits::infinity(), - maxConstraintViolation); - } - - /** - * Reset the filter. - */ - void Reset() { - m_filter.clear(); - - // Initial filter entry rejects constraint violations above max - m_filter.emplace_back(std::numeric_limits::infinity(), - maxConstraintViolation); - } - - /** - * Creates a new Sequential Quadratic Programming filter entry. - * - * @param c_e The equality constraint values (nonzero means violation). - */ - FilterEntry MakeEntry(const Eigen::VectorXd& c_e) { - return FilterEntry{m_f->Value(), c_e.lpNorm<1>()}; - } - - /** - * Creates a new interior-point method filter entry. - * - * @param s The inequality constraint slack variables. - * @param c_e The equality constraint values (nonzero means violation). - * @param c_i The inequality constraint values (negative means violation). - * @param μ The barrier parameter. - */ - FilterEntry MakeEntry(Eigen::VectorXd& s, const Eigen::VectorXd& c_e, - const Eigen::VectorXd& c_i, double μ) { - return FilterEntry{m_f->Value() - μ * s.array().log().sum(), - c_e.lpNorm<1>() + (c_i - s).lpNorm<1>()}; - } - - /** - * Add a new entry to the filter. - * - * @param entry The entry to add to the filter. - */ - void Add(const FilterEntry& entry) { - // Remove dominated entries - erase_if(m_filter, [&](const auto& elem) { - return entry.cost <= elem.cost && - entry.constraintViolation <= elem.constraintViolation; - }); - - m_filter.push_back(entry); - } - - /** - * Add a new entry to the filter. - * - * @param entry The entry to add to the filter. - */ - void Add(FilterEntry&& entry) { - // Remove dominated entries - erase_if(m_filter, [&](const auto& elem) { - return entry.cost <= elem.cost && - entry.constraintViolation <= elem.constraintViolation; - }); - - m_filter.push_back(entry); - } - - /** - * Returns true if the given iterate is accepted by the filter. - * - * @param entry The entry to attempt adding to the filter. - */ - bool TryAdd(const FilterEntry& entry) { - if (IsAcceptable(entry)) { - Add(entry); - return true; - } else { - return false; - } - } - - /** - * Returns true if the given iterate is accepted by the filter. - * - * @param entry The entry to attempt adding to the filter. - */ - bool TryAdd(FilterEntry&& entry) { - if (IsAcceptable(entry)) { - Add(std::move(entry)); - return true; - } else { - return false; - } - } - - /** - * Returns true if the given entry is acceptable to the filter. - * - * @param entry The entry to check. - */ - bool IsAcceptable(const FilterEntry& entry) { - if (!std::isfinite(entry.cost) || - !std::isfinite(entry.constraintViolation)) { - return false; - } - - // If current filter entry is better than all prior ones in some respect, - // accept it - return std::all_of(m_filter.begin(), m_filter.end(), [&](const auto& elem) { - return entry.cost <= elem.cost - γCost * elem.constraintViolation || - entry.constraintViolation <= - (1.0 - γConstraint) * elem.constraintViolation; - }); - } - - private: - Variable* m_f = nullptr; - wpi::SmallVector m_filter; -}; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/error_estimate.hpp similarity index 66% rename from wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/error_estimate.hpp index f1490028dd..5a85732811 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/error_estimate.hpp @@ -9,7 +9,21 @@ // See docs/algorithms.md#Works_cited for citation definitions -namespace sleipnir { +namespace slp { + +/** + * Returns the error estimate using the KKT conditions for Newton's method. + * + * @param g Gradient of the cost function ∇f. + */ +inline double error_estimate(const Eigen::VectorXd& g) { + // Update the error estimate using the KKT conditions from equations (19.5a) + // through (19.5d) of [1]. + // + // ∇f = 0 + + return g.lpNorm(); +} /** * Returns the error estimate using the KKT conditions for SQP. @@ -21,12 +35,10 @@ namespace sleipnir { * iterate. * @param y Equality constraint dual variables. */ -inline double ErrorEstimate(const Eigen::VectorXd& g, - const Eigen::SparseMatrix& A_e, - const Eigen::VectorXd& c_e, - const Eigen::VectorXd& y) { - int numEqualityConstraints = A_e.rows(); - +inline double error_estimate(const Eigen::VectorXd& g, + const Eigen::SparseMatrix& A_e, + const Eigen::VectorXd& c_e, + const Eigen::VectorXd& y) { // Update the error estimate using the KKT conditions from equations (19.5a) // through (19.5d) of [1]. // @@ -41,7 +53,7 @@ inline double ErrorEstimate(const Eigen::VectorXd& g, // s_d = max(sₘₐₓ, ‖y‖₁ / m) / sₘₐₓ constexpr double s_max = 100.0; - double s_d = std::max(s_max, y.lpNorm<1>() / numEqualityConstraints) / s_max; + double s_d = std::max(s_max, y.lpNorm<1>() / y.rows()) / s_max; return std::max({(g - A_e.transpose() * y).lpNorm() / s_d, c_e.lpNorm()}); @@ -65,16 +77,13 @@ inline double ErrorEstimate(const Eigen::VectorXd& g, * @param z Inequality constraint dual variables. * @param μ Barrier parameter. */ -inline double ErrorEstimate(const Eigen::VectorXd& g, - const Eigen::SparseMatrix& A_e, - const Eigen::VectorXd& c_e, - const Eigen::SparseMatrix& A_i, - const Eigen::VectorXd& c_i, - const Eigen::VectorXd& s, const Eigen::VectorXd& y, - const Eigen::VectorXd& z, double μ) { - int numEqualityConstraints = A_e.rows(); - int numInequalityConstraints = A_i.rows(); - +inline double error_estimate(const Eigen::VectorXd& g, + const Eigen::SparseMatrix& A_e, + const Eigen::VectorXd& c_e, + const Eigen::SparseMatrix& A_i, + const Eigen::VectorXd& c_i, + const Eigen::VectorXd& s, const Eigen::VectorXd& y, + const Eigen::VectorXd& z, double μ) { // Update the error estimate using the KKT conditions from equations (19.5a) // through (19.5d) of [1]. // @@ -94,23 +103,21 @@ inline double ErrorEstimate(const Eigen::VectorXd& g, // s_d = max(sₘₐₓ, (‖y‖₁ + ‖z‖₁) / (m + n)) / sₘₐₓ constexpr double s_max = 100.0; double s_d = - std::max(s_max, (y.lpNorm<1>() + z.lpNorm<1>()) / - (numEqualityConstraints + numInequalityConstraints)) / + std::max(s_max, (y.lpNorm<1>() + z.lpNorm<1>()) / (y.rows() + z.rows())) / s_max; // s_c = max(sₘₐₓ, ‖z‖₁ / n) / sₘₐₓ - double s_c = - std::max(s_max, z.lpNorm<1>() / numInequalityConstraints) / s_max; + double s_c = std::max(s_max, z.lpNorm<1>() / z.rows()) / s_max; const auto S = s.asDiagonal(); - const Eigen::VectorXd e = Eigen::VectorXd::Ones(s.rows()); + const Eigen::VectorXd μe = Eigen::VectorXd::Constant(s.rows(), μ); return std::max({(g - A_e.transpose() * y - A_i.transpose() * z) .lpNorm() / s_d, - (S * z - μ * e).lpNorm() / s_c, + (S * z - μe).lpNorm() / s_c, c_e.lpNorm(), (c_i - s).lpNorm()}); } -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/filter.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/filter.hpp new file mode 100644 index 0000000000..093ce3674f --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/filter.hpp @@ -0,0 +1,197 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include +#include + +#include +#include + +// See docs/algorithms.md#Works_cited for citation definitions. + +namespace slp { + +/** + * Filter entry consisting of cost and constraint violation. + */ +struct FilterEntry { + /// The cost function's value + double cost = 0.0; + + /// The constraint violation + double constraint_violation = 0.0; + + constexpr FilterEntry() = default; + + /** + * Constructs a FilterEntry. + * + * @param cost The cost function's value. + * @param constraint_violation The constraint violation. + */ + constexpr FilterEntry(double cost, double constraint_violation) + : cost{cost}, constraint_violation{constraint_violation} {} + + /** + * Constructs a Newton's method filter entry. + * + * @param f The cost function value. + */ + explicit FilterEntry(double f) : FilterEntry{f, 0.0} {} + + /** + * Constructs a Sequential Quadratic Programming filter entry. + * + * @param f The cost function value. + * @param c_e The equality constraint values (nonzero means violation). + */ + FilterEntry(double f, const Eigen::VectorXd& c_e) + : FilterEntry{f, c_e.lpNorm<1>()} {} + + /** + * Constructs an interior-point method filter entry. + * + * @param f The cost function value. + * @param s The inequality constraint slack variables. + * @param c_e The equality constraint values (nonzero means violation). + * @param c_i The inequality constraint values (negative means violation). + * @param μ The barrier parameter. + */ + FilterEntry(double f, Eigen::VectorXd& s, const Eigen::VectorXd& c_e, + const Eigen::VectorXd& c_i, double μ) + : FilterEntry{f - μ * s.array().log().sum(), + c_e.lpNorm<1>() + (c_i - s).lpNorm<1>()} {} +}; + +/** + * Step filter. + * + * See the section on filters in chapter 15 of [1]. + */ +class Filter { + public: + double max_constraint_violation = 1e4; + + /** + * Constructs an empty filter. + */ + Filter() { + // Initial filter entry rejects constraint violations above max + m_filter.emplace_back(std::numeric_limits::infinity(), + max_constraint_violation); + } + + /** + * Resets the filter. + */ + void reset() { + m_filter.clear(); + + // Initial filter entry rejects constraint violations above max + m_filter.emplace_back(std::numeric_limits::infinity(), + max_constraint_violation); + } + + /** + * Adds a new entry to the filter. + * + * @param entry The entry to add to the filter. + */ + void add(const FilterEntry& entry) { + // Remove dominated entries + erase_if(m_filter, [&](const auto& elem) { + return entry.cost <= elem.cost && + entry.constraint_violation <= elem.constraint_violation; + }); + + m_filter.push_back(entry); + } + + /** + * Adds a new entry to the filter. + * + * @param entry The entry to add to the filter. + */ + void add(FilterEntry&& entry) { + // Remove dominated entries + erase_if(m_filter, [&](const auto& elem) { + return entry.cost <= elem.cost && + entry.constraint_violation <= elem.constraint_violation; + }); + + m_filter.push_back(entry); + } + + /** + * Returns true if the given iterate is accepted by the filter. + * + * @param entry The entry to attempt adding to the filter. + * @param α The step size (0, 1]. + */ + bool try_add(const FilterEntry& entry, double α) { + if (is_acceptable(entry, α)) { + add(entry); + return true; + } else { + return false; + } + } + + /** + * Returns true if the given iterate is accepted by the filter. + * + * @param entry The entry to attempt adding to the filter. + * @param α The step size (0, 1]. + */ + bool try_add(FilterEntry&& entry, double α) { + if (is_acceptable(entry, α)) { + add(std::move(entry)); + return true; + } else { + return false; + } + } + + /** + * Returns true if the given entry is acceptable to the filter. + * + * @param entry The entry to check. + * @param α The step size (0, 1]. + */ + bool is_acceptable(const FilterEntry& entry, double α) { + if (!std::isfinite(entry.cost) || + !std::isfinite(entry.constraint_violation)) { + return false; + } + + double ϕ = std::pow(α, 1.5); + + // If current filter entry is better than all prior ones in some respect, + // accept it. + // + // See equation (2.13) of [4]. + return std::ranges::all_of(m_filter, [&](const auto& elem) { + return entry.cost <= elem.cost - ϕ * γ_cost * elem.constraint_violation || + entry.constraint_violation <= + (1.0 - ϕ * γ_constraint) * elem.constraint_violation; + }); + } + + /** + * Returns the most recently added filter entry. + * + * @return The most recently added filter entry. + */ + const FilterEntry& back() const { return m_filter.back(); } + + private: + static constexpr double γ_cost = 1e-8; + static constexpr double γ_constraint = 1e-5; + + gch::small_vector m_filter; +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FractionToTheBoundaryRule.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/fraction_to_the_boundary_rule.hpp similarity index 92% rename from wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FractionToTheBoundaryRule.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/fraction_to_the_boundary_rule.hpp index 98a117f49a..595219ca37 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FractionToTheBoundaryRule.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/fraction_to_the_boundary_rule.hpp @@ -6,7 +6,7 @@ // See docs/algorithms.md#Works_cited for citation definitions -namespace sleipnir { +namespace slp { /** * Applies fraction-to-the-boundary rule to a variable and its iterate, then @@ -17,7 +17,7 @@ namespace sleipnir { * @param τ Fraction-to-the-boundary rule scaling factor within (0, 1]. * @return Fraction of the iterate step size within (0, 1]. */ -inline double FractionToTheBoundaryRule( +inline double fraction_to_the_boundary_rule( const Eigen::Ref& x, const Eigen::Ref& p, double τ) { // α = max(α ∈ (0, 1] : x + αp ≥ (1 − τ)x) @@ -42,4 +42,4 @@ inline double FractionToTheBoundaryRule( return α; } -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/IsLocallyInfeasible.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/is_locally_infeasible.hpp similarity index 87% rename from wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/IsLocallyInfeasible.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/is_locally_infeasible.hpp index dc3992ea12..11a798b0d8 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/IsLocallyInfeasible.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/is_locally_infeasible.hpp @@ -7,7 +7,7 @@ // See docs/algorithms.md#Works_cited for citation definitions -namespace sleipnir { +namespace slp { /** * Returns true if the problem's equality constraints are locally infeasible. @@ -17,8 +17,8 @@ namespace sleipnir { * @param c_e The problem's equality constraints cₑ(x) evaluated at the current * iterate. */ -inline bool IsEqualityLocallyInfeasible(const Eigen::SparseMatrix& A_e, - const Eigen::VectorXd& c_e) { +inline bool is_equality_locally_infeasible( + const Eigen::SparseMatrix& A_e, const Eigen::VectorXd& c_e) { // The equality constraints are locally infeasible if // // Aₑᵀcₑ → 0 @@ -37,7 +37,7 @@ inline bool IsEqualityLocallyInfeasible(const Eigen::SparseMatrix& A_e, * @param c_i The problem's inequality constraints cᵢ(x) evaluated at the * current iterate. */ -inline bool IsInequalityLocallyInfeasible( +inline bool is_inequality_locally_infeasible( const Eigen::SparseMatrix& A_i, const Eigen::VectorXd& c_i) { // The inequality constraints are locally infeasible if // @@ -60,4 +60,4 @@ inline bool IsInequalityLocallyInfeasible( return false; } -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/kkt_error.hpp similarity index 61% rename from wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/kkt_error.hpp index 3b603159d2..e01c064d2a 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/kkt_error.hpp @@ -7,7 +7,21 @@ // See docs/algorithms.md#Works_cited for citation definitions -namespace sleipnir { +namespace slp { + +/** + * Returns the KKT error for Newton's method. + * + * @param g Gradient of the cost function ∇f. + */ +inline double kkt_error(const Eigen::VectorXd& g) { + // Compute the KKT error as the 1-norm of the KKT conditions from equations + // (19.5a) through (19.5d) of [1]. + // + // ∇f = 0 + + return g.lpNorm<1>(); +} /** * Returns the KKT error for Sequential Quadratic Programming. @@ -19,9 +33,9 @@ namespace sleipnir { * iterate. * @param y Equality constraint dual variables. */ -inline double KKTError(const Eigen::VectorXd& g, - const Eigen::SparseMatrix& A_e, - const Eigen::VectorXd& c_e, const Eigen::VectorXd& y) { +inline double kkt_error(const Eigen::VectorXd& g, + const Eigen::SparseMatrix& A_e, + const Eigen::VectorXd& c_e, const Eigen::VectorXd& y) { // Compute the KKT error as the 1-norm of the KKT conditions from equations // (19.5a) through (19.5d) of [1]. // @@ -48,13 +62,13 @@ inline double KKTError(const Eigen::VectorXd& g, * @param z Inequality constraint dual variables. * @param μ Barrier parameter. */ -inline double KKTError(const Eigen::VectorXd& g, - const Eigen::SparseMatrix& A_e, - const Eigen::VectorXd& c_e, - const Eigen::SparseMatrix& A_i, - const Eigen::VectorXd& c_i, const Eigen::VectorXd& s, - const Eigen::VectorXd& y, const Eigen::VectorXd& z, - double μ) { +inline double kkt_error(const Eigen::VectorXd& g, + const Eigen::SparseMatrix& A_e, + const Eigen::VectorXd& c_e, + const Eigen::SparseMatrix& A_i, + const Eigen::VectorXd& c_i, const Eigen::VectorXd& s, + const Eigen::VectorXd& y, const Eigen::VectorXd& z, + double μ) { // Compute the KKT error as the 1-norm of the KKT conditions from equations // (19.5a) through (19.5d) of [1]. // @@ -64,10 +78,10 @@ inline double KKTError(const Eigen::VectorXd& g, // cᵢ − s = 0 const auto S = s.asDiagonal(); - const Eigen::VectorXd e = Eigen::VectorXd::Ones(s.rows()); + const Eigen::VectorXd μe = Eigen::VectorXd::Constant(s.rows(), μ); return (g - A_e.transpose() * y - A_i.transpose() * z).lpNorm<1>() + - (S * z - μ * e).lpNorm<1>() + c_e.lpNorm<1>() + (c_i - s).lpNorm<1>(); + (S * z - μe).lpNorm<1>() + c_e.lpNorm<1>() + (c_i - s).lpNorm<1>(); } -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/util/Pool.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/util/Pool.cpp deleted file mode 100644 index 33b137b961..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/util/Pool.cpp +++ /dev/null @@ -1,12 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#include "sleipnir/util/Pool.hpp" - -namespace sleipnir { - -PoolResource& GlobalPoolResource() { - thread_local PoolResource pool{16384}; - return pool; -} - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/util/ToMilliseconds.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/util/ToMilliseconds.hpp deleted file mode 100644 index 0e00418725..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/util/ToMilliseconds.hpp +++ /dev/null @@ -1,21 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -namespace sleipnir { - -/** - * Converts std::chrono::duration to a number of milliseconds rounded to three - * decimals. - */ -template > -constexpr double ToMilliseconds( - const std::chrono::duration& duration) { - using std::chrono::duration_cast; - using std::chrono::microseconds; - return duration_cast(duration).count() / 1e3; -} - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/util/pool.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/util/pool.cpp new file mode 100644 index 0000000000..a8b086c811 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/util/pool.cpp @@ -0,0 +1,12 @@ +// Copyright (c) Sleipnir contributors + +#include "sleipnir/util/pool.hpp" + +namespace slp { + +PoolResource& global_pool_resource() { + thread_local PoolResource pool{16384}; + return pool; +} + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/util/print_diagnostics.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/util/print_diagnostics.hpp new file mode 100644 index 0000000000..82e0e082b0 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/util/print_diagnostics.hpp @@ -0,0 +1,357 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "sleipnir/util/print.hpp" +#include "util/setup_profiler.hpp" +#include "util/solve_profiler.hpp" + +namespace slp { + +/** + * Iteration type. + */ +enum class IterationType : uint8_t { + /// Normal iteration. + NORMAL, + /// Accepted second-order correction iteration. + ACCEPTED_SOC, + /// Rejected second-order correction iteration. + REJECTED_SOC +}; + +/** + * Converts std::chrono::duration to a number of milliseconds rounded to three + * decimals. + */ +template > +constexpr double to_ms(const std::chrono::duration& duration) { + using std::chrono::duration_cast; + using std::chrono::microseconds; + return duration_cast(duration).count() / 1e3; +} + +/** + * Renders value as power of 10. + * + * @param value Value. + */ +inline std::string power_of_10(double value) { + if (value == 0.0) { + return " 0"; + } else { + int exponent = std::log10(value); + + if (exponent == 0) { + return " 1"; + } else if (exponent == 1) { + return "10"; + } else { + // Gather exponent digits + int n = std::abs(exponent); + gch::small_vector digits; + do { + digits.emplace_back(n % 10); + n /= 10; + } while (n > 0); + + std::string output = "10"; + + // Append exponent + if (exponent < 0) { + output += "⁻"; + } + constexpr std::array strs = {"⁰", "¹", "²", "³", "⁴", + "⁵", "⁶", "⁷", "⁸", "⁹"}; + for (const auto& digit : digits | std::views::reverse) { + output += strs[digit]; + } + + return output; + } + } +} + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS +/** + * Prints error for too few degrees of freedom. + * + * @param c_e The problem's equality constraints cₑ(x) evaluated at the current + * iterate. + */ +inline void print_too_few_dofs_error(const Eigen::VectorXd& c_e) { + slp::println("The problem has too few degrees of freedom."); + slp::println("Violated constraints (cₑ(x) = 0) in order of declaration:"); + for (int row = 0; row < c_e.rows(); ++row) { + if (c_e[row] < 0.0) { + slp::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e[row]); + } + } +} +#else +#define print_too_few_dofs_error(...) +#endif + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS +/** + * Prints equality constraint local infeasibility error. + * + * @param c_e The problem's equality constraints cₑ(x) evaluated at the current + * iterate. + */ +inline void print_c_e_local_infeasibility_error(const Eigen::VectorXd& c_e) { + slp::println( + "The problem is locally infeasible due to violated equality " + "constraints."); + slp::println("Violated constraints (cₑ(x) = 0) in order of declaration:"); + for (int row = 0; row < c_e.rows(); ++row) { + if (c_e[row] < 0.0) { + slp::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e[row]); + } + } +} +#else +#define print_c_e_local_infeasibility_error(...) +#endif + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS +/** + * Prints inequality constraint local infeasibility error. + * + * @param c_i The problem's inequality constraints cᵢ(x) evaluated at the + * current iterate. + */ +inline void print_c_i_local_infeasibility_error(const Eigen::VectorXd& c_i) { + slp::println( + "The problem is locally infeasible due to violated inequality " + "constraints."); + slp::println("Violated constraints (cᵢ(x) ≥ 0) in order of declaration:"); + for (int row = 0; row < c_i.rows(); ++row) { + if (c_i[row] < 0.0) { + slp::println(" {}/{}: {} ≥ 0", row + 1, c_i.rows(), c_i[row]); + } + } +} +#else +#define print_c_i_local_infeasibility_error(...) +#endif + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS +inline void print_bound_constraint_global_infeasibility_error( + const std::span> + conflicting_lower_upper_bound_indices) { + slp::println( + "The problem is globally infeasible due to conflicting bound " + "constraints:"); + for (const auto& [lower_bound_idx, upper_bound_idx] : + conflicting_lower_upper_bound_indices) { + slp::println( + " Inequality constraint {} gives a lower bound that is greater than " + "the upper bound given by inequality constraint {}", + lower_bound_idx, upper_bound_idx); + } +} +#else +#define print_bound_constraint_global_infeasibility_error(...) +#endif + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS +/** + * Prints diagnostics for the current iteration. + * + * @param iterations Number of iterations. + * @param type The iteration's type. + * @param time The iteration duration. + * @param error The error. + * @param cost The cost. + * @param infeasibility The infeasibility. + * @param complementarity The complementarity. + * @param μ The barrier parameter. + * @param δ The Hessian regularization factor. + * @param primal_α The primal step size. + * @param primal_α_max The max primal step size. + * @param α_reduction_factor Factor by which primal_α is reduced during + * backtracking. + * @param dual_α The dual step size. + */ +template > +void print_iteration_diagnostics(int iterations, IterationType type, + const std::chrono::duration& time, + double error, double cost, + double infeasibility, double complementarity, + double μ, double δ, double primal_α, + double primal_α_max, double α_reduction_factor, + double dual_α) { + if (iterations % 20 == 0) { + if (iterations == 0) { + slp::print("┏"); + } else { + slp::print("┢"); + } + slp::print( + "{:━^4}┯{:━^4}┯{:━^9}┯{:━^12}┯{:━^13}┯{:━^12}┯{:━^12}┯{:━^8}┯{:━^5}┯" + "{:━^8}┯{:━^8}┯{:━^2}", + "", "", "", "", "", "", "", "", "", "", "", ""); + if (iterations == 0) { + slp::println("┓"); + } else { + slp::println("┪"); + } + slp::println( + "┃{:^4}│{:^4}│{:^9}│{:^12}│{:^13}│{:^12}│{:^12}│{:^8}│{:^5}│{:^8}│{:^8}" + "│{:^2}┃", + "iter", "type", "time (ms)", "error", "cost", "infeas.", "complement.", + "μ", "reg", "primal α", "dual α", "↩"); + slp::println( + "┡{:━^4}┷{:━^4}┷{:━^9}┷{:━^12}┷{:━^13}┷{:━^12}┷{:━^12}┷{:━^8}┷{:━^5}┷" + "{:━^8}┷{:━^8}┷{:━^2}┩", + "", "", "", "", "", "", "", "", "", "", "", ""); + } + + // For the number of backtracks, we want x such that: + // + // αᵐᵃˣrˣ = α + // + // where r ∈ (0, 1) is the reduction factor. + // + // rˣ = α/αᵐᵃˣ + // ln(rˣ) = ln(α/αᵐᵃˣ) + // x ln(r) = ln(α/αᵐᵃˣ) + // x = ln(α/αᵐᵃˣ)/ln(r) + int backtracks = static_cast(std::log(primal_α / primal_α_max) / + std::log(α_reduction_factor)); + + constexpr std::array ITERATION_TYPES = {"norm", "✓SOC", "XSOC"}; + slp::println( + "│{:4} {:4} {:9.3f} {:12e} {:13e} {:12e} {:12e} {:.2e} {:<5} {:.2e} " + "{:.2e} {:2d}│", + iterations, ITERATION_TYPES[static_cast(type)], to_ms(time), + error, cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, + dual_α, backtracks); +} +#else +#define print_iteration_diagnostics(...) +#endif + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS +/** + * Prints bottom of iteration diagnostics table. + */ +inline void print_bottom_iteration_diagnostics() { + slp::println("└{:─^108}┘", ""); +} +#else +#define print_bottom_iteration_diagnostics(...) +#endif + +/** + * Renders histogram of the given normalized value. + * + * @tparam Width Width of the histogram in characters. + * @param value Normalized value from 0 to 1. + */ +template + requires(Width > 0) +std::string histogram(double value) { + value = std::clamp(value, 0.0, 1.0); + + double ipart; + int fpart = static_cast(std::modf(value * Width, &ipart) * 8); + + constexpr std::array strs = {" ", "▏", "▎", "▍", "▌", "▋", "▊", "▉", "█"}; + std::string hist; + + int index = 0; + while (index < ipart) { + hist += strs[8]; + ++index; + } + if (fpart > 0) { + hist += strs[fpart]; + ++index; + } + while (index < Width) { + hist += strs[0]; + ++index; + } + + return hist; +} + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS +/** + * Prints solver diagnostics. + * + * @param solve_profilers Solve profilers. + */ +inline void print_solver_diagnostics( + const gch::small_vector& solve_profilers) { + auto solve_duration = to_ms(solve_profilers[0].total_duration()); + + slp::println("┏{:━^25}┯{:━^18}┯{:━^10}┯{:━^9}┯{:━^4}┓", "", "", "", "", ""); + slp::println("┃{:^25}│{:^18}│{:^10}│{:^9}│{:^4}┃", "solver trace", "percent", + "total (ms)", "each (ms)", "runs"); + slp::println("┡{:━^25}┷{:━^18}┷{:━^10}┷{:━^9}┷{:━^4}┩", "", "", "", "", ""); + + for (auto& profiler : solve_profilers) { + double norm = solve_duration == 0.0 + ? (&profiler == &solve_profilers[0] ? 1.0 : 0.0) + : to_ms(profiler.total_duration()) / solve_duration; + slp::println("│{:<25} {:>6.2f}%▕{}▏ {:>10.3f} {:>9.3f} {:>4}│", + profiler.name(), norm * 100.0, histogram<9>(norm), + to_ms(profiler.total_duration()), + to_ms(profiler.average_duration()), profiler.num_solves()); + } + + slp::println("└{:─^70}┘", ""); +} +#else +#define print_solver_diagnostics(...) +#endif + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS +/** + * Prints autodiff diagnostics. + * + * @param setup_profilers Autodiff setup profilers. + */ +inline void print_autodiff_diagnostics( + const gch::small_vector& setup_profilers) { + auto setup_duration = to_ms(setup_profilers[0].duration()); + + // Print heading + slp::println("┏{:━^25}┯{:━^18}┯{:━^10}┯{:━^9}┯{:━^4}┓", "", "", "", "", ""); + slp::println("┃{:^25}│{:^18}│{:^10}│{:^9}│{:^4}┃", "autodiff trace", + "percent", "total (ms)", "each (ms)", "runs"); + slp::println("┡{:━^25}┷{:━^18}┷{:━^10}┷{:━^9}┷{:━^4}┩", "", "", "", "", ""); + + // Print setup profilers + for (auto& profiler : setup_profilers) { + double norm = setup_duration == 0.0 + ? (&profiler == &setup_profilers[0] ? 1.0 : 0.0) + : to_ms(profiler.duration()) / setup_duration; + slp::println("│{:<25} {:>6.2f}%▕{}▏ {:>10.3f} {:>9.3f} {:>4}│", + profiler.name(), norm * 100.0, histogram<9>(norm), + to_ms(profiler.duration()), to_ms(profiler.duration()), "1"); + } + + slp::println("└{:─^70}┘", ""); +} +#else +#define print_autodiff_diagnostics(...) +#endif + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/util/ScopeExit.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/util/scope_exit.hpp similarity index 92% rename from wpimath/src/main/native/thirdparty/sleipnir/src/util/ScopeExit.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/src/util/scope_exit.hpp index dedc7ae4d3..937121f1a8 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/util/ScopeExit.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/util/scope_exit.hpp @@ -4,7 +4,7 @@ #include -namespace sleipnir { +namespace slp { template class scope_exit { @@ -32,4 +32,4 @@ class scope_exit { bool m_active = true; }; -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/util/scoped_profiler.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/util/scoped_profiler.hpp new file mode 100644 index 0000000000..066a2b3dbe --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/util/scoped_profiler.hpp @@ -0,0 +1,78 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include + +#include "util/setup_profiler.hpp" +#include "util/solve_profiler.hpp" + +namespace slp { + +/** + * Starts a profiler in the constructor and stops it in the destructor. + */ +template + requires std::same_as || + std::same_as +class ScopedProfiler { + public: + /** + * Starts a profiler. + * + * @param profiler The profiler. + */ + explicit ScopedProfiler(Profiler& profiler) noexcept : m_profiler{&profiler} { + m_profiler->start(); + } + + /** + * Stops a profiler. + */ + ~ScopedProfiler() { + if (m_active) { + m_profiler->stop(); + } + } + + /** + * Move constructor. + * + * @param rhs The other ScopedProfiler. + */ + ScopedProfiler(ScopedProfiler&& rhs) noexcept + : m_profiler{std::move(rhs.m_profiler)}, m_active{rhs.m_active} { + rhs.m_active = false; + } + + ScopedProfiler(const ScopedProfiler&) = delete; + ScopedProfiler& operator=(const ScopedProfiler&) = delete; + + /** + * Stops the profiler. + * + * If this is called, the destructor is a no-op. + */ + void stop() { + if (m_active) { + m_profiler->stop(); + m_active = false; + } + } + + /** + * Returns the most recent solve duration in milliseconds as a double. + * + * @return The most recent solve duration in milliseconds as a double. + */ + const std::chrono::duration& current_duration() const { + return m_profiler->current_duration(); + } + + private: + Profiler* m_profiler; + bool m_active = true; +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/util/setup_profiler.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/util/setup_profiler.hpp new file mode 100644 index 0000000000..031f99d2e5 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/util/setup_profiler.hpp @@ -0,0 +1,68 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include + +namespace slp { + +/** + * Records the number of profiler measurements (start/stop pairs) and the + * average duration between each start and stop call. + */ +class SetupProfiler { + public: + /** + * Constructs a SetupProfiler. + * + * @param name Name of measurement to show in diagnostics. + */ + explicit SetupProfiler(std::string_view name) : m_name{name} {} + + /** + * Tell the profiler to start measuring setup time. + */ + void start() { +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + m_setup_start_time = std::chrono::steady_clock::now(); +#endif + } + + /** + * Tell the profiler to stop measuring setup time. + */ + void stop() { +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + m_setup_stop_time = std::chrono::steady_clock::now(); + m_setup_duration = m_setup_stop_time - m_setup_start_time; +#endif + } + + /** + * Returns name of measurement to show in diagnostics. + * + * @return Name of measurement to show in diagnostics. + */ + std::string_view name() const { return m_name; } + + /** + * Returns the setup duration in milliseconds as a double. + * + * @return The setup duration in milliseconds as a double. + */ + const std::chrono::duration& duration() const { + return m_setup_duration; + } + + private: + /// Name of measurement to show in diagnostics. + std::string m_name; + + std::chrono::steady_clock::time_point m_setup_start_time; + std::chrono::steady_clock::time_point m_setup_stop_time; + std::chrono::duration m_setup_duration{0.0}; +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/util/solve_profiler.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/util/solve_profiler.hpp new file mode 100644 index 0000000000..fb17f1dc39 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/util/solve_profiler.hpp @@ -0,0 +1,105 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include + +namespace slp { + +/** + * Records the number of profiler measurements (start/stop pairs) and the + * average duration between each start and stop call. + */ +class SolveProfiler { + public: + /** + * Constructs a SolveProfiler. + * + * @param name Name of measurement to show in diagnostics. + */ + explicit SolveProfiler(std::string_view name) : m_name{name} {} + + /** + * Tell the profiler to start measuring solve time. + */ + void start() { +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + m_current_solve_start_time = std::chrono::steady_clock::now(); +#endif + } + + /** + * Tell the profiler to stop measuring solve time, increment the number of + * averages, and incorporate the latest measurement into the average. + */ + void stop() { +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + m_current_solve_stop_time = std::chrono::steady_clock::now(); + m_current_solve_duration = + m_current_solve_stop_time - m_current_solve_start_time; + m_total_solve_duration += m_current_solve_duration; + + ++m_num_solves; + m_average_solve_duration = + (m_num_solves - 1.0) / m_num_solves * m_average_solve_duration + + 1.0 / m_num_solves * m_current_solve_duration; +#endif + } + + /** + * Returns name of measurement to show in diagnostics. + * + * @return Name of measurement to show in diagnostics. + */ + std::string_view name() const { return m_name; } + + /** + * Returns the number of solves. + * + * @return The number of solves. + */ + int num_solves() const { return m_num_solves; } + + /** + * Returns the most recent solve duration in seconds. + * + * @return The most recent solve duration in seconds. + */ + const std::chrono::duration& current_duration() const { + return m_current_solve_duration; + } + + /** + * Returns the average solve duration in seconds. + * + * @return The average solve duration in seconds. + */ + const std::chrono::duration& average_duration() const { + return m_average_solve_duration; + } + + /** + * Returns the sum of all solve durations in seconds. + * + * @return The sum of all solve durations in seconds. + */ + const std::chrono::duration& total_duration() const { + return m_total_solve_duration; + } + + private: + /// Name of measurement to show in diagnostics. + std::string m_name; + + std::chrono::steady_clock::time_point m_current_solve_start_time; + std::chrono::steady_clock::time_point m_current_solve_stop_time; + std::chrono::duration m_current_solve_duration{0.0}; + std::chrono::duration m_total_solve_duration{0.0}; + + int m_num_solves = 0; + std::chrono::duration m_average_solve_duration{0.0}; +}; + +} // namespace slp diff --git a/wpimath/src/test/native/cpp/SleipnirTest.cpp b/wpimath/src/test/native/cpp/SleipnirTest.cpp index 46cb437fd5..c0eb25d90e 100644 --- a/wpimath/src/test/native/cpp/SleipnirTest.cpp +++ b/wpimath/src/test/native/cpp/SleipnirTest.cpp @@ -3,24 +3,25 @@ // the WPILib BSD license file in the root directory of this project. #include -#include +#include TEST(SleipnirTest, Quartic) { - sleipnir::OptimizationProblem problem; + slp::Problem problem; - auto x = problem.DecisionVariable(); - x.SetValue(20.0); + auto x = problem.decision_variable(); + x.set_value(20.0); - problem.Minimize(sleipnir::pow(x, 4)); + problem.minimize(slp::pow(x, 4)); - problem.SubjectTo(x >= 1); + problem.subject_to(x >= 1); - auto status = problem.Solve({.diagnostics = true}); + EXPECT_EQ(problem.cost_function_type(), slp::ExpressionType::NONLINEAR); + EXPECT_EQ(problem.equality_constraint_type(), slp::ExpressionType::NONE); + EXPECT_EQ(problem.inequality_constraint_type(), slp::ExpressionType::LINEAR); - EXPECT_EQ(status.costFunctionType, sleipnir::ExpressionType::kNonlinear); - EXPECT_EQ(status.equalityConstraintType, sleipnir::ExpressionType::kNone); - EXPECT_EQ(status.inequalityConstraintType, sleipnir::ExpressionType::kLinear); - EXPECT_EQ(status.exitCondition, sleipnir::SolverExitCondition::kSuccess); + auto status = problem.solve({.diagnostics = true}); - EXPECT_NEAR(x.Value(), 1.0, 1e-6); + EXPECT_EQ(status, slp::ExitStatus::SUCCESS); + + EXPECT_NEAR(x.value(), 1.0, 1e-6); }