diff --git a/.github/workflows/upstream-utils.yml b/.github/workflows/upstream-utils.yml index 5d96b40d06..102aaf621b 100644 --- a/.github/workflows/upstream-utils.yml +++ b/.github/workflows/upstream-utils.yml @@ -70,6 +70,10 @@ jobs: run: | cd upstream_utils ./update_protobuf.py + - name: Run update_sleipnir.py + run: | + cd upstream_utils + ./update_sleipnir.py - name: Add untracked files to index so they count as changes run: git add -A - name: Check output diff --git a/ThirdPartyNotices.txt b/ThirdPartyNotices.txt index 3fc55565ad..c561c8a322 100644 --- a/ThirdPartyNotices.txt +++ b/ThirdPartyNotices.txt @@ -43,6 +43,7 @@ Team 254 Library wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineP Portable File Dialogs wpigui/src/main/native/include/portable-file-dialogs.h V8 export-template wpiutil/src/main/native/include/wpi/SymbolExports.h GCEM wpimath/src/main/native/thirdparty/gcem/include/ +Sleipnir wpimath/src/main/native/thirdparty/sleipnir ============================================================================== Google Test License @@ -1209,3 +1210,18 @@ limitations under the License. 2024 Field Image ================ 2024 Field Image from MikLast: https://www.chiefdelphi.com/t/2024-crescendo-top-down-field-renders/447764 + +================ +Sleipnir License +================ +Copyright (c) Sleipnir contributors + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/shared/jni/setupBuild.gradle b/shared/jni/setupBuild.gradle index de7c50f49b..fda467366e 100644 --- a/shared/jni/setupBuild.gradle +++ b/shared/jni/setupBuild.gradle @@ -62,8 +62,8 @@ model { it.buildable = false return } - it.cppCompiler.define 'WPILIB_EXPORTS' - it.cCompiler.define 'WPILIB_EXPORTS' + it.cppCompiler.define 'WPILIB_EXPORTS', 'SLEIPNIR_EXPORTS' + it.cCompiler.define 'WPILIB_EXPORTS', 'SLEIPNIR_EXPORTS' if (!project.hasProperty('noWpiutil')) { lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' } diff --git a/upstream_utils/sleipnir_patches/0001-Remove-using-enum-declarations.patch b/upstream_utils/sleipnir_patches/0001-Remove-using-enum-declarations.patch new file mode 100644 index 0000000000..29c083bcca --- /dev/null +++ b/upstream_utils/sleipnir_patches/0001-Remove-using-enum-declarations.patch @@ -0,0 +1,638 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +Date: Wed, 24 Apr 2024 15:56:06 -0700 +Subject: [PATCH] Remove "using enum" declarations + +--- + include/sleipnir/autodiff/Expression.hpp | 161 +++++++----------- + .../optimization/SolverExitCondition.hpp | 22 ++- + 2 files changed, 73 insertions(+), 110 deletions(-) + +diff --git a/include/sleipnir/autodiff/Expression.hpp b/include/sleipnir/autodiff/Expression.hpp +index ff904cec61315b244e3302ccc8ba68e2a6f6a3a3..460be3791c084cf30c979fe8ca53d27c72ac71ee 100644 +--- a/include/sleipnir/autodiff/Expression.hpp ++++ b/include/sleipnir/autodiff/Expression.hpp +@@ -192,8 +192,6 @@ struct SLEIPNIR_DLLEXPORT Expression { + */ + friend SLEIPNIR_DLLEXPORT ExpressionPtr operator*(const ExpressionPtr& lhs, + const ExpressionPtr& rhs) { +- using enum ExpressionType; +- + // Prune expression + if (lhs->IsConstant(0.0)) { + // Return zero +@@ -208,20 +206,22 @@ struct SLEIPNIR_DLLEXPORT Expression { + } + + // Evaluate constant +- if (lhs->type == kConstant && rhs->type == kConstant) { ++ if (lhs->type == ExpressionType::kConstant && ++ rhs->type == ExpressionType::kConstant) { + return MakeExpressionPtr(lhs->value * rhs->value); + } + + // Evaluate expression type + ExpressionType type; +- if (lhs->type == kConstant) { ++ if (lhs->type == ExpressionType::kConstant) { + type = rhs->type; +- } else if (rhs->type == kConstant) { ++ } else if (rhs->type == ExpressionType::kConstant) { + type = lhs->type; +- } else if (lhs->type == kLinear && rhs->type == kLinear) { +- type = kQuadratic; ++ } else if (lhs->type == ExpressionType::kLinear && ++ rhs->type == ExpressionType::kLinear) { ++ type = ExpressionType::kQuadratic; + } else { +- type = kNonlinear; ++ type = ExpressionType::kNonlinear; + } + + return MakeExpressionPtr( +@@ -247,8 +247,6 @@ struct SLEIPNIR_DLLEXPORT Expression { + */ + friend SLEIPNIR_DLLEXPORT ExpressionPtr operator/(const ExpressionPtr& lhs, + const ExpressionPtr& rhs) { +- using enum ExpressionType; +- + // Prune expression + if (lhs->IsConstant(0.0)) { + // Return zero +@@ -258,16 +256,17 @@ struct SLEIPNIR_DLLEXPORT Expression { + } + + // Evaluate constant +- if (lhs->type == kConstant && rhs->type == kConstant) { ++ if (lhs->type == ExpressionType::kConstant && ++ rhs->type == ExpressionType::kConstant) { + return MakeExpressionPtr(lhs->value / rhs->value); + } + + // Evaluate expression type + ExpressionType type; +- if (rhs->type == kConstant) { ++ if (rhs->type == ExpressionType::kConstant) { + type = lhs->type; + } else { +- type = kNonlinear; ++ type = ExpressionType::kNonlinear; + } + + return MakeExpressionPtr( +@@ -295,8 +294,6 @@ struct SLEIPNIR_DLLEXPORT Expression { + */ + 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; +@@ -305,7 +302,8 @@ struct SLEIPNIR_DLLEXPORT Expression { + } + + // Evaluate constant +- if (lhs->type == kConstant && rhs->type == kConstant) { ++ if (lhs->type == ExpressionType::kConstant && ++ rhs->type == ExpressionType::kConstant) { + return MakeExpressionPtr(lhs->value + rhs->value); + } + +@@ -333,8 +331,6 @@ struct SLEIPNIR_DLLEXPORT Expression { + */ + 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)) { +@@ -348,7 +344,8 @@ struct SLEIPNIR_DLLEXPORT Expression { + } + + // Evaluate constant +- if (lhs->type == kConstant && rhs->type == kConstant) { ++ if (lhs->type == ExpressionType::kConstant && ++ rhs->type == ExpressionType::kConstant) { + return MakeExpressionPtr(lhs->value - rhs->value); + } + +@@ -374,8 +371,6 @@ struct SLEIPNIR_DLLEXPORT Expression { + * @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 +@@ -383,7 +378,7 @@ struct SLEIPNIR_DLLEXPORT Expression { + } + + // Evaluate constant +- if (lhs->type == kConstant) { ++ if (lhs->type == ExpressionType::kConstant) { + return MakeExpressionPtr(-lhs->value); + } + +@@ -465,8 +460,6 @@ inline void IntrusiveSharedPtrDecRefCount(Expression* expr) { + */ + SLEIPNIR_DLLEXPORT inline ExpressionPtr abs( // NOLINT + const ExpressionPtr& x) { +- using enum ExpressionType; +- + // Prune expression + if (x->IsConstant(0.0)) { + // Return zero +@@ -474,12 +467,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr abs( // NOLINT + } + + // Evaluate constant +- if (x->type == kConstant) { ++ if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::abs(x->value)); + } + + return MakeExpressionPtr( +- kNonlinear, [](double x, double) { return std::abs(x); }, ++ ExpressionType::kNonlinear, [](double x, double) { return std::abs(x); }, + [](double x, double, double parentAdjoint) { + if (x < 0.0) { + return -parentAdjoint; +@@ -510,20 +503,18 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr abs( // NOLINT + */ + 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) { ++ if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::acos(x->value)); + } + + return MakeExpressionPtr( +- kNonlinear, [](double x, double) { return std::acos(x); }, ++ ExpressionType::kNonlinear, [](double x, double) { return std::acos(x); }, + [](double x, double, double parentAdjoint) { + return -parentAdjoint / std::sqrt(1.0 - x * x); + }, +@@ -542,8 +533,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr acos( // NOLINT + */ + SLEIPNIR_DLLEXPORT inline ExpressionPtr asin( // NOLINT + const ExpressionPtr& x) { +- using enum ExpressionType; +- + // Prune expression + if (x->IsConstant(0.0)) { + // Return zero +@@ -551,12 +540,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr asin( // NOLINT + } + + // Evaluate constant +- if (x->type == kConstant) { ++ if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::asin(x->value)); + } + + return MakeExpressionPtr( +- kNonlinear, [](double x, double) { return std::asin(x); }, ++ ExpressionType::kNonlinear, [](double x, double) { return std::asin(x); }, + [](double x, double, double parentAdjoint) { + return parentAdjoint / std::sqrt(1.0 - x * x); + }, +@@ -575,8 +564,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr asin( // NOLINT + */ + SLEIPNIR_DLLEXPORT inline ExpressionPtr atan( // NOLINT + const ExpressionPtr& x) { +- using enum ExpressionType; +- + // Prune expression + if (x->IsConstant(0.0)) { + // Return zero +@@ -584,12 +571,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan( // NOLINT + } + + // Evaluate constant +- if (x->type == kConstant) { ++ if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::atan(x->value)); + } + + return MakeExpressionPtr( +- kNonlinear, [](double x, double) { return std::atan(x); }, ++ ExpressionType::kNonlinear, [](double x, double) { return std::atan(x); }, + [](double x, double, double parentAdjoint) { + return parentAdjoint / (1.0 + x * x); + }, +@@ -608,8 +595,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan( // NOLINT + */ + SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2( // NOLINT + const ExpressionPtr& y, const ExpressionPtr& x) { +- using enum ExpressionType; +- + // Prune expression + if (y->IsConstant(0.0)) { + // Return zero +@@ -619,12 +604,14 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2( // NOLINT + } + + // Evaluate constant +- if (y->type == kConstant && x->type == kConstant) { ++ if (y->type == ExpressionType::kConstant && ++ x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::atan2(y->value, x->value)); + } + + return MakeExpressionPtr( +- kNonlinear, [](double y, double x) { return std::atan2(y, x); }, ++ ExpressionType::kNonlinear, ++ [](double y, double x) { return std::atan2(y, x); }, + [](double y, double x, double parentAdjoint) { + return parentAdjoint * x / (y * y + x * x); + }, +@@ -649,20 +636,18 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2( // NOLINT + */ + 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) { ++ if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::cos(x->value)); + } + + return MakeExpressionPtr( +- kNonlinear, [](double x, double) { return std::cos(x); }, ++ ExpressionType::kNonlinear, [](double x, double) { return std::cos(x); }, + [](double x, double, double parentAdjoint) { + return -parentAdjoint * std::sin(x); + }, +@@ -680,20 +665,18 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr cos( // NOLINT + */ + 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) { ++ if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::cosh(x->value)); + } + + return MakeExpressionPtr( +- kNonlinear, [](double x, double) { return std::cosh(x); }, ++ ExpressionType::kNonlinear, [](double x, double) { return std::cosh(x); }, + [](double x, double, double parentAdjoint) { + return parentAdjoint * std::sinh(x); + }, +@@ -711,8 +694,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr cosh( // NOLINT + */ + SLEIPNIR_DLLEXPORT inline ExpressionPtr erf( // NOLINT + const ExpressionPtr& x) { +- using enum ExpressionType; +- + // Prune expression + if (x->IsConstant(0.0)) { + // Return zero +@@ -720,12 +701,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr erf( // NOLINT + } + + // Evaluate constant +- if (x->type == kConstant) { ++ if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::erf(x->value)); + } + + return MakeExpressionPtr( +- kNonlinear, [](double x, double) { return std::erf(x); }, ++ ExpressionType::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); +@@ -746,20 +727,18 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr erf( // NOLINT + */ + 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) { ++ if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::exp(x->value)); + } + + return MakeExpressionPtr( +- kNonlinear, [](double x, double) { return std::exp(x); }, ++ ExpressionType::kNonlinear, [](double x, double) { return std::exp(x); }, + [](double x, double, double parentAdjoint) { + return parentAdjoint * std::exp(x); + }, +@@ -778,8 +757,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr exp( // NOLINT + */ + SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot( // NOLINT + const ExpressionPtr& x, const ExpressionPtr& y) { +- using enum ExpressionType; +- + // Prune expression + if (x->IsConstant(0.0)) { + return y; +@@ -788,12 +765,14 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot( // NOLINT + } + + // Evaluate constant +- if (x->type == kConstant && y->type == kConstant) { ++ if (x->type == ExpressionType::kConstant && ++ y->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::hypot(x->value, y->value)); + } + + return MakeExpressionPtr( +- kNonlinear, [](double x, double y) { return std::hypot(x, y); }, ++ ExpressionType::kNonlinear, ++ [](double x, double y) { return std::hypot(x, y); }, + [](double x, double y, double parentAdjoint) { + return parentAdjoint * x / std::hypot(x, y); + }, +@@ -818,8 +797,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot( // NOLINT + */ + SLEIPNIR_DLLEXPORT inline ExpressionPtr log( // NOLINT + const ExpressionPtr& x) { +- using enum ExpressionType; +- + // Prune expression + if (x->IsConstant(0.0)) { + // Return zero +@@ -827,12 +804,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log( // NOLINT + } + + // Evaluate constant +- if (x->type == kConstant) { ++ if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::log(x->value)); + } + + return MakeExpressionPtr( +- kNonlinear, [](double x, double) { return std::log(x); }, ++ ExpressionType::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; }, +@@ -846,8 +823,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log( // NOLINT + */ + SLEIPNIR_DLLEXPORT inline ExpressionPtr log10( // NOLINT + const ExpressionPtr& x) { +- using enum ExpressionType; +- + // Prune expression + if (x->IsConstant(0.0)) { + // Return zero +@@ -855,12 +830,13 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log10( // NOLINT + } + + // Evaluate constant +- if (x->type == kConstant) { ++ if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::log10(x->value)); + } + + return MakeExpressionPtr( +- kNonlinear, [](double x, double) { return std::log10(x); }, ++ ExpressionType::kNonlinear, ++ [](double x, double) { return std::log10(x); }, + [](double x, double, double parentAdjoint) { + return parentAdjoint / (std::numbers::ln10 * x); + }, +@@ -879,8 +855,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log10( // NOLINT + */ + SLEIPNIR_DLLEXPORT inline ExpressionPtr pow( // NOLINT + const ExpressionPtr& base, const ExpressionPtr& power) { +- using enum ExpressionType; +- + // Prune expression + if (base->IsConstant(0.0)) { + // Return zero +@@ -895,12 +869,15 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr pow( // NOLINT + } + + // Evaluate constant +- if (base->type == kConstant && power->type == kConstant) { ++ if (base->type == ExpressionType::kConstant && ++ power->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::pow(base->value, power->value)); + } + + return MakeExpressionPtr( +- base->type == kLinear && power->IsConstant(2.0) ? kQuadratic : kNonlinear, ++ base->type == ExpressionType::kLinear && power->IsConstant(2.0) ++ ? ExpressionType::kQuadratic ++ : ExpressionType::kNonlinear, + [](double base, double power) { return std::pow(base, power); }, + [](double base, double power, double parentAdjoint) { + return parentAdjoint * std::pow(base, power - 1) * power; +@@ -941,10 +918,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr pow( // NOLINT + * @param x The argument. + */ + SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { +- using enum ExpressionType; +- + // Evaluate constant +- if (x->type == kConstant) { ++ if (x->type == ExpressionType::kConstant) { + if (x->value < 0.0) { + return MakeExpressionPtr(-1.0); + } else if (x->value == 0.0) { +@@ -956,7 +931,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { + } + + return MakeExpressionPtr( +- kNonlinear, ++ ExpressionType::kNonlinear, + [](double x, double) { + if (x < 0.0) { + return -1.0; +@@ -982,8 +957,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { + */ + SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT + const ExpressionPtr& x) { +- using enum ExpressionType; +- + // Prune expression + if (x->IsConstant(0.0)) { + // Return zero +@@ -991,12 +964,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT + } + + // Evaluate constant +- if (x->type == kConstant) { ++ if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::sin(x->value)); + } + + return MakeExpressionPtr( +- kNonlinear, [](double x, double) { return std::sin(x); }, ++ ExpressionType::kNonlinear, [](double x, double) { return std::sin(x); }, + [](double x, double, double parentAdjoint) { + return parentAdjoint * std::cos(x); + }, +@@ -1013,8 +986,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT + * @param x The argument. + */ + SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) { +- using enum ExpressionType; +- + // Prune expression + if (x->IsConstant(0.0)) { + // Return zero +@@ -1022,12 +993,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) { + } + + // Evaluate constant +- if (x->type == kConstant) { ++ if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::sinh(x->value)); + } + + return MakeExpressionPtr( +- kNonlinear, [](double x, double) { return std::sinh(x); }, ++ ExpressionType::kNonlinear, [](double x, double) { return std::sinh(x); }, + [](double x, double, double parentAdjoint) { + return parentAdjoint * std::cosh(x); + }, +@@ -1045,10 +1016,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) { + */ + SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt( // NOLINT + const ExpressionPtr& x) { +- using enum ExpressionType; +- + // Evaluate constant +- if (x->type == kConstant) { ++ if (x->type == ExpressionType::kConstant) { + if (x->value == 0.0) { + // Return zero + return x; +@@ -1060,7 +1029,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt( // NOLINT + } + + return MakeExpressionPtr( +- kNonlinear, [](double x, double) { return std::sqrt(x); }, ++ ExpressionType::kNonlinear, [](double x, double) { return std::sqrt(x); }, + [](double x, double, double parentAdjoint) { + return parentAdjoint / (2.0 * std::sqrt(x)); + }, +@@ -1079,8 +1048,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt( // NOLINT + */ + SLEIPNIR_DLLEXPORT inline ExpressionPtr tan( // NOLINT + const ExpressionPtr& x) { +- using enum ExpressionType; +- + // Prune expression + if (x->IsConstant(0.0)) { + // Return zero +@@ -1088,12 +1055,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tan( // NOLINT + } + + // Evaluate constant +- if (x->type == kConstant) { ++ if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::tan(x->value)); + } + + return MakeExpressionPtr( +- kNonlinear, [](double x, double) { return std::tan(x); }, ++ ExpressionType::kNonlinear, [](double x, double) { return std::tan(x); }, + [](double x, double, double parentAdjoint) { + return parentAdjoint / (std::cos(x) * std::cos(x)); + }, +@@ -1111,8 +1078,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tan( // NOLINT + * @param x The argument. + */ + SLEIPNIR_DLLEXPORT inline ExpressionPtr tanh(const ExpressionPtr& x) { +- using enum ExpressionType; +- + // Prune expression + if (x->IsConstant(0.0)) { + // Return zero +@@ -1120,12 +1085,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tanh(const ExpressionPtr& x) { + } + + // Evaluate constant +- if (x->type == kConstant) { ++ if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::tanh(x->value)); + } + + return MakeExpressionPtr( +- kNonlinear, [](double x, double) { return std::tanh(x); }, ++ ExpressionType::kNonlinear, [](double x, double) { return std::tanh(x); }, + [](double x, double, double parentAdjoint) { + return parentAdjoint / (std::cosh(x) * std::cosh(x)); + }, +diff --git a/include/sleipnir/optimization/SolverExitCondition.hpp b/include/sleipnir/optimization/SolverExitCondition.hpp +index 7d1445297e33e3c62bcdf9d03eebeaad20af9a1c..734cd3d127327e8ce01e1a42fe74ccc81fea1f90 100644 +--- a/include/sleipnir/optimization/SolverExitCondition.hpp ++++ b/include/sleipnir/optimization/SolverExitCondition.hpp +@@ -46,31 +46,29 @@ enum class SolverExitCondition : int8_t { + */ + SLEIPNIR_DLLEXPORT constexpr std::string_view ToMessage( + const SolverExitCondition& exitCondition) { +- using enum SolverExitCondition; +- + switch (exitCondition) { +- case kSuccess: ++ case SolverExitCondition::kSuccess: + return "solved to desired tolerance"; +- case kSolvedToAcceptableTolerance: ++ case SolverExitCondition::kSolvedToAcceptableTolerance: + return "solved to acceptable tolerance"; +- case kCallbackRequestedStop: ++ case SolverExitCondition::kCallbackRequestedStop: + return "callback requested stop"; +- case kTooFewDOFs: ++ case SolverExitCondition::kTooFewDOFs: + return "problem has too few degrees of freedom"; +- case kLocallyInfeasible: ++ case SolverExitCondition::kLocallyInfeasible: + return "problem is locally infeasible"; +- case kFeasibilityRestorationFailed: ++ case SolverExitCondition::kFeasibilityRestorationFailed: + return "solver failed to reach the desired tolerance, and feasibility " + "restoration failed to converge"; +- case kNonfiniteInitialCostOrConstraints: ++ case SolverExitCondition::kNonfiniteInitialCostOrConstraints: + return "solver encountered nonfinite initial cost or constraints and " + "gave up"; +- case kDivergingIterates: ++ case SolverExitCondition::kDivergingIterates: + return "solver encountered diverging primal iterates xₖ and/or sₖ and " + "gave up"; +- case kMaxIterationsExceeded: ++ case SolverExitCondition::kMaxIterationsExceeded: + return "solution returned after maximum iterations exceeded"; +- case kTimeout: ++ case SolverExitCondition::kTimeout: + return "solution returned after maximum wall clock time exceeded"; + default: + return "unknown"; diff --git a/upstream_utils/update_sleipnir.py b/upstream_utils/update_sleipnir.py new file mode 100755 index 0000000000..e9407990c5 --- /dev/null +++ b/upstream_utils/update_sleipnir.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python3 + +import os +import shutil + +from upstream_utils import ( + get_repo_root, + clone_repo, + copy_to, + walk_cwd_and_copy_if, + git_am, +) + + +def main(): + upstream_root = clone_repo( + "https://github.com/SleipnirGroup/Sleipnir", + # main on 2024-04-25 + "fd2df40330df1f39b04f831f41fbcc22bab07a70", + shallow=False, + ) + wpilib_root = get_repo_root() + wpimath = os.path.join(wpilib_root, "wpimath") + + # Apply patches to upstream Git repo + os.chdir(upstream_root) + for f in [ + "0001-Remove-using-enum-declarations.patch", + ]: + git_am(os.path.join(wpilib_root, "upstream_utils/sleipnir_patches", f)) + + # Delete old install + for d in [ + "src/main/native/thirdparty/sleipnir/src", + "src/main/native/thirdparty/sleipnir/include", + ]: + shutil.rmtree(os.path.join(wpimath, d), ignore_errors=True) + + # Copy Sleipnir source files into allwpilib + os.chdir(upstream_root) + src_files = [os.path.join(dp, f) for dp, dn, fn in os.walk("src") for f in fn] + src_files = copy_to( + src_files, os.path.join(wpimath, "src/main/native/thirdparty/sleipnir") + ) + + # 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 + ] + include_files = copy_to( + include_files, os.path.join(wpimath, "src/main/native/thirdparty/sleipnir") + ) + + for filename in [ + ".clang-format", + ".clang-tidy", + ".styleguide", + ".styleguide-license", + ]: + shutil.copyfile( + os.path.join(upstream_root, filename), + os.path.join(wpimath, "src/main/native/thirdparty/sleipnir", filename), + ) + + +if __name__ == "__main__": + main() diff --git a/wpimath/CMakeLists.txt b/wpimath/CMakeLists.txt index 3c9c7dcc9d..0d11252619 100644 --- a/wpimath/CMakeLists.txt +++ b/wpimath/CMakeLists.txt @@ -149,7 +149,11 @@ if(WITH_JAVA_SOURCE) set_property(TARGET wpimath_src_jar PROPERTY FOLDER "java") endif() -file(GLOB_RECURSE wpimath_native_src src/main/native/cpp/*.cpp) +file( + GLOB_RECURSE wpimath_native_src + src/main/native/cpp/*.cpp + src/main/native/thirdparty/sleipnir/src/*.cpp +) list(REMOVE_ITEM wpimath_native_src ${wpimath_jni_src}) set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS FALSE) @@ -158,7 +162,7 @@ set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS TRUE) set_target_properties(wpimath PROPERTIES DEBUG_POSTFIX "d") set_property(TARGET wpimath PROPERTY FOLDER "libraries") -target_compile_definitions(wpimath PRIVATE WPILIB_EXPORTS) +target_compile_definitions(wpimath PRIVATE WPILIB_EXPORTS SLEIPNIR_EXPORTS) target_compile_features(wpimath PUBLIC cxx_std_20) if(MSVC) @@ -184,6 +188,18 @@ else() target_link_libraries(wpimath Eigen3::Eigen) endif() +install( + DIRECTORY src/main/native/thirdparty/sleipnir/include/ + DESTINATION "${include_dest}/wpimath" +) +target_include_directories( + wpimath + SYSTEM + PUBLIC + $ + $ +) + install(DIRECTORY src/main/native/thirdparty/gcem/include/ DESTINATION "${include_dest}/wpimath") target_include_directories( wpimath diff --git a/wpimath/build.gradle b/wpimath/build.gradle index e16e5e7202..06d4e4668d 100644 --- a/wpimath/build.gradle +++ b/wpimath/build.gradle @@ -9,6 +9,23 @@ ext { nativeName = 'wpimath' devMain = 'edu.wpi.first.math.DevMain' + + splitSetup = { + it.sources { + sleipnirCpp(CppSourceSet) { + source { + srcDirs 'src/main/native/thirdparty/sleipnir/src' + include '**/*.cpp' + } + exportedHeaders { + srcDirs 'src/main/native/thirdparty/eigen/include', + 'src/main/native/thirdparty/fmt/include', + 'src/main/native/thirdparty/sleipnir/include', + 'src/main/native/thirdparty/sleipnir/src' + } + } + } + } } apply from: "${rootDir}/shared/jni/setupBuild.gradle" @@ -24,6 +41,9 @@ cppHeadersZip { into '/wpimath/protobuf' include '*.h' } + from('src/main/native/thirdparty/sleipnir/include') { + into '/' + } } cppHeadersZip.dependsOn generateProto @@ -35,11 +55,17 @@ model { it.exportedHeaders { srcDirs 'src/main/native/include', 'src/main/native/thirdparty/eigen/include', - 'src/main/native/thirdparty/gcem/include' + 'src/main/native/thirdparty/gcem/include', + 'src/main/native/thirdparty/sleipnir/include' } } } } + binaries { + all { + it.cppCompiler.define 'SLEIPNIR_EXPORTS' + } + } } nativeUtils.exportsConfigs { diff --git a/wpimath/src/main/native/thirdparty/sleipnir/.clang-format b/wpimath/src/main/native/thirdparty/sleipnir/.clang-format new file mode 100644 index 0000000000..5e5e1773c3 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/.clang-format @@ -0,0 +1,249 @@ +--- +Language: Cpp +BasedOnStyle: Google +AccessModifierOffset: -1 +AlignAfterOpenBracket: Align +AlignArrayOfStructures: None +AlignConsecutiveAssignments: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + PadOperators: true +AlignConsecutiveBitFields: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + PadOperators: false +AlignConsecutiveDeclarations: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + PadOperators: false +AlignConsecutiveMacros: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + PadOperators: false +AlignConsecutiveShortCaseStatements: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCaseColons: false +AlignEscapedNewlines: Left +AlignOperands: Align +AlignTrailingComments: + Kind: Always + OverEmptyLines: 0 +AllowAllArgumentsOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: Never +AllowShortCaseLabelsOnASingleLine: false +AllowShortEnumsOnASingleLine: true +AllowShortFunctionsOnASingleLine: Inline +AllowShortIfStatementsOnASingleLine: Never +AllowShortLambdasOnASingleLine: All +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AlwaysBreakTemplateDeclarations: Yes +BinPackArguments: true +BinPackParameters: true +BitFieldColonSpacing: Both +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: Never + AfterEnum: false + AfterExternBlock: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + BeforeCatch: false + BeforeElse: false + BeforeLambdaBody: false + BeforeWhile: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakAfterAttributes: Always +BreakAfterJavaFieldAnnotations: false +BreakArrays: true +BreakBeforeBinaryOperators: None +BreakBeforeConceptDeclarations: Always +BreakBeforeBraces: Attach +BreakBeforeInlineASMColon: OnlyMultiline +BreakBeforeTernaryOperators: true +BreakConstructorInitializers: BeforeColon +BreakInheritanceList: BeforeColon +BreakStringLiterals: true +ColumnLimit: 80 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +DisableFormat: false +EmptyLineAfterAccessModifier: Never +EmptyLineBeforeAccessModifier: LogicalBlock +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +IncludeBlocks: Regroup +IncludeCategories: + - Regex: '^' + Priority: 2 + SortPriority: 0 + CaseSensitive: false + - Regex: '^<.*\.h>' + Priority: 1 + SortPriority: 0 + CaseSensitive: false + - Regex: '^<.*' + Priority: 2 + SortPriority: 0 + CaseSensitive: false + - Regex: '.*' + Priority: 3 + SortPriority: 0 + CaseSensitive: false +IncludeIsMainRegex: '([-_](test|unittest))?$' +IncludeIsMainSourceRegex: '' +IndentAccessModifiers: false +IndentCaseBlocks: false +IndentCaseLabels: true +IndentExternBlock: AfterExternBlock +IndentGotoLabels: true +IndentPPDirectives: None +IndentRequiresClause: true +IndentWidth: 2 +IndentWrappedFunctionNames: false +InsertBraces: false +InsertNewlineAtEOF: false +InsertTrailingCommas: None +IntegerLiteralSeparator: + Binary: 0 + BinaryMinDigits: 0 + Decimal: 0 + DecimalMinDigits: 0 + Hex: 0 + HexMinDigits: 0 +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: false +KeepEmptyLinesAtEOF: false +LambdaBodyIndentation: Signature +LineEnding: DeriveLF +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBinPackProtocolList: Never +ObjCBlockIndentWidth: 2 +ObjCBreakBeforeNestedBlockParam: true +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: true +PackConstructorInitializers: NextLine +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakOpenParenthesis: 0 +PenaltyBreakString: 1000 +PenaltyBreakTemplateDeclaration: 10 +PenaltyExcessCharacter: 1000000 +PenaltyIndentedWhitespace: 0 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Left +PPIndentWidth: -1 +QualifierAlignment: Leave +RawStringFormats: + - Language: Cpp + Delimiters: + - cc + - CC + - cpp + - Cpp + - CPP + - 'c++' + - 'C++' + CanonicalDelimiter: '' + BasedOnStyle: google + - Language: TextProto + Delimiters: + - pb + - PB + - proto + - PROTO + EnclosingFunctions: + - EqualsProto + - EquivToProto + - PARSE_PARTIAL_TEXT_PROTO + - PARSE_TEST_PROTO + - PARSE_TEXT_PROTO + - ParseTextOrDie + - ParseTextProtoOrDie + - ParseTestProto + - ParsePartialTestProto + CanonicalDelimiter: pb + BasedOnStyle: google +ReferenceAlignment: Pointer +ReflowComments: true +RemoveBracesLLVM: false +RemoveParentheses: Leave +RemoveSemicolon: false +RequiresClausePosition: OwnLine +RequiresExpressionIndentation: OuterScope +SeparateDefinitionBlocks: Leave +ShortNamespaceLines: 1 +SortIncludes: false +SortJavaStaticImport: Before +SortUsingDeclarations: LexicographicNumeric +SpaceAfterCStyleCast: false +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: true +SpaceAroundPointerQualifiers: Default +SpaceBeforeAssignmentOperators: true +SpaceBeforeCaseColon: false +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeJsonColon: false +SpaceBeforeParens: ControlStatements +SpaceBeforeParensOptions: + AfterControlStatements: true + AfterForeachMacros: true + AfterFunctionDefinitionName: false + AfterFunctionDeclarationName: false + AfterIfMacros: true + AfterOverloadedOperator: false + AfterRequiresInClause: false + AfterRequiresInExpression: false + BeforeNonEmptyParentheses: false +SpaceBeforeRangeBasedForLoopColon: true +SpaceBeforeSquareBrackets: false +SpaceInEmptyBlock: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: Never +SpacesInContainerLiterals: true +SpacesInLineCommentPrefix: + Minimum: 1 + Maximum: -1 +SpacesInParens: Never +SpacesInParensOptions: + InCStyleCasts: false + InConditionalStatements: false + InEmptyParentheses: false + Other: false +SpacesInSquareBrackets: false +Standard: c++20 +TabWidth: 8 +UseTab: Never +... diff --git a/wpimath/src/main/native/thirdparty/sleipnir/.clang-tidy b/wpimath/src/main/native/thirdparty/sleipnir/.clang-tidy new file mode 100644 index 0000000000..cfb2b12f2a --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/.clang-tidy @@ -0,0 +1,74 @@ +Checks: + 'bugprone-assert-side-effect, + bugprone-bool-pointer-implicit-conversion, + bugprone-copy-constructor-init, + bugprone-dangling-handle, + bugprone-dynamic-static-initializers, + bugprone-forward-declaration-namespace, + bugprone-forwarding-reference-overload, + bugprone-inaccurate-erase, + bugprone-incorrect-roundings, + bugprone-integer-division, + bugprone-lambda-function-name, + bugprone-misplaced-operator-in-strlen-in-alloc, + bugprone-misplaced-widening-cast, + bugprone-move-forwarding-reference, + bugprone-multiple-statement-macro, + bugprone-parent-virtual-call, + bugprone-posix-return, + bugprone-sizeof-container, + bugprone-sizeof-expression, + bugprone-spuriously-wake-up-functions, + bugprone-string-constructor, + bugprone-string-integer-assignment, + bugprone-string-literal-with-embedded-nul, + bugprone-suspicious-enum-usage, + bugprone-suspicious-include, + bugprone-suspicious-memset-usage, + bugprone-suspicious-missing-comma, + bugprone-suspicious-semicolon, + bugprone-suspicious-string-compare, + bugprone-throw-keyword-missing, + bugprone-too-small-loop-variable, + bugprone-undefined-memory-manipulation, + bugprone-undelegated-constructor, + bugprone-unhandled-self-assignment, + bugprone-unused-raii, + bugprone-virtual-near-miss, + cert-err52-cpp, + cert-err60-cpp, + cert-mem57-cpp, + cert-oop57-cpp, + cert-oop58-cpp, + clang-diagnostic-*, + -clang-diagnostic-deprecated-declarations, + -clang-diagnostic-#warnings, + -clang-diagnostic-pedantic, + clang-analyzer-*, + -clang-analyzer-core.uninitialized.UndefReturn, + -clang-analyzer-optin.cplusplus.UninitializedObject, + -clang-analyzer-optin.portability.UnixAPI, + -clang-analyzer-unix.Malloc, + -cppcoreguidelines-slicing, + google-build-namespaces, + google-explicit-constructor, + google-global-names-in-headers, + google-readability-avoid-underscore-in-googletest-name, + google-readability-casting, + google-runtime-operator, + misc-definitions-in-headers, + misc-misplaced-const, + misc-new-delete-overloads, + misc-non-copyable-objects, + modernize-avoid-bind, + modernize-concat-nested-namespaces, + modernize-make-shared, + modernize-make-unique, + modernize-pass-by-value, + modernize-use-default-member-init, + modernize-use-noexcept, + modernize-use-nullptr, + modernize-use-override, + modernize-use-using, + readability-braces-around-statements' +FormatStyle: file diff --git a/wpimath/src/main/native/thirdparty/sleipnir/.styleguide b/wpimath/src/main/native/thirdparty/sleipnir/.styleguide new file mode 100644 index 0000000000..2476200a87 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/.styleguide @@ -0,0 +1,23 @@ +cppHeaderFileInclude { + \.hpp$ +} + +cppSrcFileInclude { + \.cpp$ +} + +modifiableFileExclude { + \.patch$ + \.png$ + \.svg$ + jormungandr/cpp/Docstrings\.hpp$ + jormungandr/py\.typed$ +} + +includeOtherLibs { + ^Eigen/ + ^catch2/ + ^fmt/ + ^pybind11/ + ^sleipnir/ +} diff --git a/wpimath/src/main/native/thirdparty/sleipnir/.styleguide-license b/wpimath/src/main/native/thirdparty/sleipnir/.styleguide-license new file mode 100644 index 0000000000..d664772a55 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/.styleguide-license @@ -0,0 +1 @@ +// Copyright (c) Sleipnir contributors diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/.styleguide b/wpimath/src/main/native/thirdparty/sleipnir/include/.styleguide new file mode 100644 index 0000000000..8251a49067 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/.styleguide @@ -0,0 +1,12 @@ +cppHeaderFileInclude { + \.hpp$ +} + +cppSrcFileInclude { + \.cpp$ +} + +includeOtherLibs { + ^Eigen/ + ^fmt/ +} 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..460be3791c --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp @@ -0,0 +1,1105 @@ +// 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 { + +struct SLEIPNIR_DLLEXPORT Expression; + +inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr); +// FIXME: Make constexpr after upgrading to GCC 12+ +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) { + return AllocateIntrusiveShared(GlobalPoolAllocator(), + 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) { + // 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 == ExpressionType::kConstant && + rhs->type == ExpressionType::kConstant) { + return MakeExpressionPtr(lhs->value * rhs->value); + } + + // Evaluate expression type + ExpressionType type; + if (lhs->type == ExpressionType::kConstant) { + type = rhs->type; + } else if (rhs->type == ExpressionType::kConstant) { + type = lhs->type; + } else if (lhs->type == ExpressionType::kLinear && + rhs->type == ExpressionType::kLinear) { + type = ExpressionType::kQuadratic; + } else { + type = ExpressionType::kNonlinear; + } + + return MakeExpressionPtr( + type, [](double lhs, double rhs) { return lhs * rhs; }, + [](double lhs, double rhs, double parentAdjoint) { + return parentAdjoint * rhs; + }, + [](double lhs, double rhs, double parentAdjoint) { + return parentAdjoint * lhs; + }, + [](const ExpressionPtr& lhs, const ExpressionPtr& rhs, + const ExpressionPtr& parentAdjoint) { return parentAdjoint * rhs; }, + [](const ExpressionPtr& lhs, const ExpressionPtr& rhs, + 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) { + // Prune expression + if (lhs->IsConstant(0.0)) { + // Return zero + return lhs; + } else if (rhs->IsConstant(1.0)) { + return lhs; + } + + // Evaluate constant + if (lhs->type == ExpressionType::kConstant && + rhs->type == ExpressionType::kConstant) { + return MakeExpressionPtr(lhs->value / rhs->value); + } + + // Evaluate expression type + ExpressionType type; + if (rhs->type == ExpressionType::kConstant) { + type = lhs->type; + } else { + type = ExpressionType::kNonlinear; + } + + return MakeExpressionPtr( + type, [](double lhs, double rhs) { return lhs / rhs; }, + [](double lhs, double rhs, double parentAdjoint) { + return parentAdjoint / rhs; + }, + [](double lhs, double rhs, double parentAdjoint) { + return parentAdjoint * -lhs / (rhs * rhs); + }, + [](const ExpressionPtr& lhs, 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) { + // 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 == ExpressionType::kConstant && + rhs->type == ExpressionType::kConstant) { + return MakeExpressionPtr(lhs->value + rhs->value); + } + + return MakeExpressionPtr( + std::max(lhs->type, rhs->type), + [](double lhs, double rhs) { return lhs + rhs; }, + [](double lhs, double rhs, double parentAdjoint) { + return parentAdjoint; + }, + [](double lhs, double rhs, double parentAdjoint) { + return parentAdjoint; + }, + [](const ExpressionPtr& lhs, const ExpressionPtr& rhs, + const ExpressionPtr& parentAdjoint) { return parentAdjoint; }, + [](const ExpressionPtr& lhs, const ExpressionPtr& rhs, + 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) { + // 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 == ExpressionType::kConstant && + rhs->type == ExpressionType::kConstant) { + return MakeExpressionPtr(lhs->value - rhs->value); + } + + return MakeExpressionPtr( + std::max(lhs->type, rhs->type), + [](double lhs, double rhs) { return lhs - rhs; }, + [](double lhs, double rhs, double parentAdjoint) { + return parentAdjoint; + }, + [](double lhs, double rhs, double parentAdjoint) { + return -parentAdjoint; + }, + [](const ExpressionPtr& lhs, const ExpressionPtr& rhs, + const ExpressionPtr& parentAdjoint) { return parentAdjoint; }, + [](const ExpressionPtr& lhs, const ExpressionPtr& rhs, + const ExpressionPtr& parentAdjoint) { return -parentAdjoint; }, + lhs, rhs); + } + + /** + * Unary minus operator. + * + * @param lhs Operand of unary minus. + */ + friend SLEIPNIR_DLLEXPORT ExpressionPtr operator-(const ExpressionPtr& lhs) { + // Prune expression + if (lhs->IsConstant(0.0)) { + // Return zero + return lhs; + } + + // Evaluate constant + if (lhs->type == ExpressionType::kConstant) { + return MakeExpressionPtr(-lhs->value); + } + + return MakeExpressionPtr( + lhs->type, [](double lhs, double) { return -lhs; }, + [](double lhs, double, double parentAdjoint) { return -parentAdjoint; }, + [](const ExpressionPtr& lhs, const ExpressionPtr& rhs, + 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 constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) { + ++expr->refCount; +} + +/** + * Refcount decrement for intrusive shared pointer. + * + * @param expr The shared pointer's managed object. + */ +// FIXME: Make constexpr after upgrading to GCC 12+ +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. + std::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->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. + 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) { + // Prune expression + if (x->IsConstant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::abs(x->value)); + } + + return MakeExpressionPtr( + ExpressionType::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) { + // Prune expression + if (x->IsConstant(0.0)) { + return MakeExpressionPtr(std::numbers::pi / 2.0); + } + + // Evaluate constant + if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::acos(x->value)); + } + + return MakeExpressionPtr( + ExpressionType::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) { + // Prune expression + if (x->IsConstant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::asin(x->value)); + } + + return MakeExpressionPtr( + ExpressionType::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) { + // Prune expression + if (x->IsConstant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::atan(x->value)); + } + + return MakeExpressionPtr( + ExpressionType::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) { + // 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 == ExpressionType::kConstant && + x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::atan2(y->value, x->value)); + } + + return MakeExpressionPtr( + ExpressionType::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) { + // Prune expression + if (x->IsConstant(0.0)) { + return MakeExpressionPtr(1.0); + } + + // Evaluate constant + if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::cos(x->value)); + } + + return MakeExpressionPtr( + ExpressionType::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) { + // Prune expression + if (x->IsConstant(0.0)) { + return MakeExpressionPtr(1.0); + } + + // Evaluate constant + if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::cosh(x->value)); + } + + return MakeExpressionPtr( + ExpressionType::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) { + // Prune expression + if (x->IsConstant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::erf(x->value)); + } + + return MakeExpressionPtr( + ExpressionType::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) { + // Prune expression + if (x->IsConstant(0.0)) { + return MakeExpressionPtr(1.0); + } + + // Evaluate constant + if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::exp(x->value)); + } + + return MakeExpressionPtr( + ExpressionType::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) { + // Prune expression + if (x->IsConstant(0.0)) { + return y; + } else if (y->IsConstant(0.0)) { + return x; + } + + // Evaluate constant + if (x->type == ExpressionType::kConstant && + y->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::hypot(x->value, y->value)); + } + + return MakeExpressionPtr( + ExpressionType::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) { + // Prune expression + if (x->IsConstant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::log(x->value)); + } + + return MakeExpressionPtr( + ExpressionType::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) { + // Prune expression + if (x->IsConstant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::log10(x->value)); + } + + return MakeExpressionPtr( + ExpressionType::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) { + // 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 == ExpressionType::kConstant && + power->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::pow(base->value, power->value)); + } + + return MakeExpressionPtr( + base->type == ExpressionType::kLinear && power->IsConstant(2.0) + ? ExpressionType::kQuadratic + : ExpressionType::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) { + // Evaluate constant + if (x->type == ExpressionType::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( + ExpressionType::kNonlinear, + [](double x, double) { + if (x < 0.0) { + return -1.0; + } else if (x == 0.0) { + return 0.0; + } else { + return 1.0; + } + }, + [](double x, double, double parentAdjoint) { return 0.0; }, + [](const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parentAdjoint) { + // Return zero + return MakeExpressionPtr(); + }, + x); +} + +/** + * std::sin() for Expressions. + * + * @param x The argument. + */ +SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT + const ExpressionPtr& x) { + // Prune expression + if (x->IsConstant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::sin(x->value)); + } + + return MakeExpressionPtr( + ExpressionType::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) { + // Prune expression + if (x->IsConstant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::sinh(x->value)); + } + + return MakeExpressionPtr( + ExpressionType::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) { + // Evaluate constant + if (x->type == ExpressionType::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( + ExpressionType::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) { + // Prune expression + if (x->IsConstant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::tan(x->value)); + } + + return MakeExpressionPtr( + ExpressionType::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) { + // Prune expression + if (x->IsConstant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type == ExpressionType::kConstant) { + return MakeExpressionPtr(std::tanh(x->value)); + } + + return MakeExpressionPtr( + ExpressionType::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 new file mode 100644 index 0000000000..337fc378f3 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionGraph.hpp @@ -0,0 +1,243 @@ +// 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. + std::vector 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. + */ + std::vector 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; + } + + std::vector 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. + std::vector m_rowList; + + // List for updating adjoints + std::vector m_adjointList; + + // List for updating values + std::vector 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 new file mode 100644 index 0000000000..37825e3b2e --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionType.hpp @@ -0,0 +1,27 @@ +// 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/Gradient.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Gradient.hpp new file mode 100644 index 0000000000..6436726af1 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Gradient.hpp @@ -0,0 +1,78 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#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" + +namespace sleipnir { + +/** + * This class calculates the gradient of a a variable with respect to a vector + * of variables. + * + * The gradient is only recomputed if the variable expression is quadratic or + * higher order. + */ +class SLEIPNIR_DLLEXPORT Gradient { + public: + /** + * Constructs a Gradient object. + * + * @param variable Variable of which to compute the gradient. + * @param wrt Variable with respect to which to compute the gradient. + */ + Gradient(Variable variable, Variable wrt) noexcept + : Gradient{std::move(variable), VariableMatrix{wrt}} {} + + /** + * Constructs a Gradient object. + * + * @param variable Variable of which to compute the 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} {} + + /** + * Returns the gradient as a VariableMatrix. + * + * This is useful when constructing optimization problems with derivatives in + * them. + */ + VariableMatrix Get() const { return m_jacobian.Get().T(); } + + /** + * Evaluates the gradient at wrt's value. + */ + const Eigen::SparseVector& Value() { + m_g = m_jacobian.Value(); + + return m_g; + } + + /** + * Updates the value of the variable. + */ + void Update() { m_jacobian.Update(); } + + /** + * Returns the profiler. + */ + Profiler& GetProfiler() { return m_jacobian.GetProfiler(); } + + private: + Eigen::SparseVector m_g; + + Jacobian m_jacobian; +}; + +} // 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 new file mode 100644 index 0000000000..8fb671c106 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Hessian.hpp @@ -0,0 +1,84 @@ +// 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{ + [&] { + std::vector 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(); } + + /** + * Updates the values of the gradient tree. + */ + void Update() { m_jacobian.Update(); } + + /** + * 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 new file mode 100644 index 0000000000..b415da0dd8 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Jacobian.hpp @@ -0,0 +1,162 @@ +// 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()}; + + std::vector 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(); + + 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; + } + + /** + * Updates the values of the variables. + */ + void Update() { + for (auto& graph : m_graphs) { + graph.Update(); + } + } + + /** + * Returns the profiler. + */ + Profiler& GetProfiler() { return m_profiler; } + + private: + VariableMatrix m_variables; + VariableMatrix m_wrt; + + std::vector m_graphs; + + Eigen::SparseMatrix m_J{m_variables.Rows(), m_wrt.Rows()}; + + // Cached triplets for gradients of linear rows + std::vector> m_cachedTriplets; + + // List of row indices for nonlinear rows whose graients will be computed in + // Value() + std::vector 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 new file mode 100644 index 0000000000..5c22d145b3 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Profiler.hpp @@ -0,0 +1,79 @@ +// 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/Variable.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Variable.hpp new file mode 100644 index 0000000000..6226556d32 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Variable.hpp @@ -0,0 +1,467 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include "sleipnir/autodiff/Expression.hpp" +#include "sleipnir/autodiff/ExpressionGraph.hpp" +#include "sleipnir/util/Print.hpp" +#include "sleipnir/util/SymbolExports.hpp" + +namespace sleipnir { + +// Forward declarations for friend declarations in Variable +class SLEIPNIR_DLLEXPORT Hessian; +class SLEIPNIR_DLLEXPORT Jacobian; + +/** + * An autodiff variable pointing to an expression node. + */ +class SLEIPNIR_DLLEXPORT Variable { + public: + /** + * Constructs a linear Variable with a value of zero. + */ + Variable() = default; + + /** + * Constructs a Variable from a double. + * + * @param value The value of the Variable. + */ + Variable(double value) : expr{detail::MakeExpressionPtr(value)} {} // NOLINT + + /** + * Constructs a Variable from an int. + * + * @param value The value of the Variable. + */ + Variable(int value) : expr{detail::MakeExpressionPtr(value)} {} // NOLINT + + /** + * Constructs a Variable pointing to the specified expression. + * + * @param expr The autodiff variable. + */ + explicit Variable(const detail::ExpressionPtr& expr) : expr{expr} {} + + /** + * Constructs a Variable pointing to the specified expression. + * + * @param expr The autodiff variable. + */ + explicit Variable(detail::ExpressionPtr&& expr) : expr{std::move(expr)} {} + + /** + * Assignment operator for double. + * + * @param value The value of the Variable. + */ + Variable& operator=(double value) { + expr = detail::MakeExpressionPtr(value); + + return *this; + } + + /** + * Assignment operator for int. + * + * @param value The value of the Variable. + */ + Variable& operator=(int value) { + expr = detail::MakeExpressionPtr(value); + + return *this; + } + + /** + * Sets Variable's internal value. + * + * @param value The value of the Variable. + */ + Variable& SetValue(double value) { + if (expr->IsConstant(0.0)) { + expr = detail::MakeExpressionPtr(value); + } else { + // 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( + stderr, + "WARNING: {}:{}: Modified the value of a dependent variable", + __FILE__, __LINE__); + } + expr->value = value; + } + return *this; + } + + /** + * Sets Variable's internal value. + * + * @param value The value of the Variable. + */ + Variable& SetValue(int value) { + if (expr->IsConstant(0.0)) { + expr = detail::MakeExpressionPtr(value); + } else { + // 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( + stderr, + "WARNING: {}:{}: Modified the value of a dependent variable", + __FILE__, __LINE__); + } + expr->value = value; + } + return *this; + } + + /** + * Variable-Variable multiplication operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + friend SLEIPNIR_DLLEXPORT Variable operator*(const Variable& lhs, + const Variable& rhs) { + return Variable{lhs.expr * rhs.expr}; + } + + /** + * Variable-Variable compound multiplication operator. + * + * @param rhs Operator right-hand side. + */ + Variable& operator*=(const Variable& rhs) { + *this = *this * rhs; + return *this; + } + + /** + * Variable-Variable division operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + friend SLEIPNIR_DLLEXPORT Variable operator/(const Variable& lhs, + const Variable& rhs) { + return Variable{lhs.expr / rhs.expr}; + } + + /** + * Variable-Variable compound division operator. + * + * @param rhs Operator right-hand side. + */ + Variable& operator/=(const Variable& rhs) { + *this = *this / rhs; + return *this; + } + + /** + * Variable-Variable addition operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + friend SLEIPNIR_DLLEXPORT Variable operator+(const Variable& lhs, + const Variable& rhs) { + return Variable{lhs.expr + rhs.expr}; + } + + /** + * Variable-Variable compound addition operator. + * + * @param rhs Operator right-hand side. + */ + Variable& operator+=(const Variable& rhs) { + *this = *this + rhs; + return *this; + } + + /** + * Variable-Variable subtraction operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + friend SLEIPNIR_DLLEXPORT Variable operator-(const Variable& lhs, + const Variable& rhs) { + return Variable{lhs.expr - rhs.expr}; + } + + /** + * Variable-Variable compound subtraction operator. + * + * @param rhs Operator right-hand side. + */ + Variable& operator-=(const Variable& rhs) { + *this = *this - rhs; + return *this; + } + + /** + * Unary minus operator. + * + * @param lhs Operand for unary minus. + */ + friend SLEIPNIR_DLLEXPORT Variable operator-(const Variable& lhs) { + return Variable{-lhs.expr}; + } + + /** + * Unary plus operator. + * + * @param lhs Operand for unary plus. + */ + friend SLEIPNIR_DLLEXPORT Variable operator+(const Variable& lhs) { + return Variable{+lhs.expr}; + } + + /** + * Returns the value of this variable. + */ + double Value() const { return expr->value; } + + /** + * Returns the type of this expression (constant, linear, quadratic, or + * nonlinear). + */ + ExpressionType Type() const { return expr->type; } + + /** + * Updates the value of this variable based on the values of its dependent + * variables. + */ + void Update() { + if (!expr->IsConstant(0.0)) { + detail::ExpressionGraph{expr}.Update(); + } + } + + private: + /// The expression node. + detail::ExpressionPtr expr = + detail::MakeExpressionPtr(0.0, ExpressionType::kLinear); + + friend SLEIPNIR_DLLEXPORT Variable abs(const Variable& x); + friend SLEIPNIR_DLLEXPORT Variable acos(const Variable& x); + friend SLEIPNIR_DLLEXPORT Variable asin(const Variable& x); + friend SLEIPNIR_DLLEXPORT Variable atan(const Variable& x); + friend SLEIPNIR_DLLEXPORT Variable atan2(const Variable& y, + const Variable& x); + friend SLEIPNIR_DLLEXPORT Variable cos(const Variable& x); + friend SLEIPNIR_DLLEXPORT Variable cosh(const Variable& x); + friend SLEIPNIR_DLLEXPORT Variable erf(const Variable& x); + friend SLEIPNIR_DLLEXPORT Variable exp(const Variable& x); + friend SLEIPNIR_DLLEXPORT Variable hypot(const Variable& x, + const Variable& y); + friend SLEIPNIR_DLLEXPORT Variable log(const Variable& x); + friend SLEIPNIR_DLLEXPORT Variable log10(const Variable& x); + friend SLEIPNIR_DLLEXPORT Variable pow(const Variable& base, + const Variable& power); + friend SLEIPNIR_DLLEXPORT Variable sign(const Variable& x); + friend SLEIPNIR_DLLEXPORT Variable sin(const Variable& x); + friend SLEIPNIR_DLLEXPORT Variable sinh(const Variable& x); + friend SLEIPNIR_DLLEXPORT Variable sqrt(const Variable& x); + friend SLEIPNIR_DLLEXPORT Variable tan(const Variable& x); + friend SLEIPNIR_DLLEXPORT Variable tanh(const Variable& x); + friend SLEIPNIR_DLLEXPORT Variable hypot(const Variable& x, const Variable& y, + const Variable& z); + + friend class SLEIPNIR_DLLEXPORT Hessian; + friend class SLEIPNIR_DLLEXPORT Jacobian; +}; + +/** + * std::abs() for Variables. + * + * @param x The argument. + */ +SLEIPNIR_DLLEXPORT inline Variable abs(const Variable& x) { + return Variable{detail::abs(x.expr)}; +} + +/** + * std::acos() for Variables. + * + * @param x The argument. + */ +SLEIPNIR_DLLEXPORT inline Variable acos(const Variable& x) { + return Variable{detail::acos(x.expr)}; +} + +/** + * std::asin() for Variables. + * + * @param x The argument. + */ +SLEIPNIR_DLLEXPORT inline Variable asin(const Variable& x) { + return Variable{detail::asin(x.expr)}; +} + +/** + * std::atan() for Variables. + * + * @param x The argument. + */ +SLEIPNIR_DLLEXPORT inline Variable atan(const Variable& x) { + return Variable{detail::atan(x.expr)}; +} + +/** + * std::atan2() for Variables. + * + * @param y The y argument. + * @param x The x argument. + */ +SLEIPNIR_DLLEXPORT inline Variable atan2(const Variable& y, const Variable& x) { + return Variable{detail::atan2(y.expr, x.expr)}; +} + +/** + * std::cos() for Variables. + * + * @param x The argument. + */ +SLEIPNIR_DLLEXPORT inline Variable cos(const Variable& x) { + return Variable{detail::cos(x.expr)}; +} + +/** + * std::cosh() for Variables. + * + * @param x The argument. + */ +SLEIPNIR_DLLEXPORT inline Variable cosh(const Variable& x) { + return Variable{detail::cosh(x.expr)}; +} + +/** + * std::erf() for Variables. + * + * @param x The argument. + */ +SLEIPNIR_DLLEXPORT inline Variable erf(const Variable& x) { + return Variable{detail::erf(x.expr)}; +} + +/** + * std::exp() for Variables. + * + * @param x The argument. + */ +SLEIPNIR_DLLEXPORT inline Variable exp(const Variable& x) { + return Variable{detail::exp(x.expr)}; +} + +/** + * std::hypot() for Variables. + * + * @param x The x argument. + * @param y The y argument. + */ +SLEIPNIR_DLLEXPORT inline Variable hypot(const Variable& x, const Variable& y) { + return Variable{detail::hypot(x.expr, y.expr)}; +} + +/** + * std::pow() for Variables. + * + * @param base The base. + * @param power The power. + */ +SLEIPNIR_DLLEXPORT inline Variable pow(const Variable& base, + const Variable& power) { + return Variable{detail::pow(base.expr, power.expr)}; +} + +/** + * std::log() for Variables. + * + * @param x The argument. + */ +SLEIPNIR_DLLEXPORT inline Variable log(const Variable& x) { + return Variable{detail::log(x.expr)}; +} + +/** + * std::log10() for Variables. + * + * @param x The argument. + */ +SLEIPNIR_DLLEXPORT inline Variable log10(const Variable& x) { + return Variable{detail::log10(x.expr)}; +} + +/** + * sign() for Variables. + * + * @param x The argument. + */ +SLEIPNIR_DLLEXPORT inline Variable sign(const Variable& x) { + return Variable{detail::sign(x.expr)}; +} + +/** + * std::sin() for Variables. + * + * @param x The argument. + */ +SLEIPNIR_DLLEXPORT inline Variable sin(const Variable& x) { + return Variable{detail::sin(x.expr)}; +} + +/** + * std::sinh() for Variables. + * + * @param x The argument. + */ +SLEIPNIR_DLLEXPORT inline Variable sinh(const Variable& x) { + return Variable{detail::sinh(x.expr)}; +} + +/** + * std::sqrt() for Variables. + * + * @param x The argument. + */ +SLEIPNIR_DLLEXPORT inline Variable sqrt(const Variable& x) { + return Variable{detail::sqrt(x.expr)}; +} + +/** + * std::tan() for Variables. + * + * @param x The argument. + */ +SLEIPNIR_DLLEXPORT inline Variable tan(const Variable& x) { + return Variable{detail::tan(x.expr)}; +} + +/** + * std::tanh() for Variables. + * + * @param x The argument. + */ +SLEIPNIR_DLLEXPORT inline Variable tanh(const Variable& x) { + return Variable{detail::tanh(x.expr)}; +} + +/** + * std::hypot() for Variables. + * + * @param x The x argument. + * @param y The y argument. + * @param z The z argument. + */ +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))}; +} + +} // 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 new file mode 100644 index 0000000000..44c48fb6ae --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp @@ -0,0 +1,617 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include + +#include + +#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. + */ + VariableBlock& operator=(const VariableBlock& values) { + if (this == &values) { + return *this; + } + + if (m_mat == nullptr) { + m_mat = values.m_mat; + m_rowOffset = values.m_rowOffset; + m_colOffset = values.m_colOffset; + m_blockRows = values.m_blockRows; + m_blockCols = values.m_blockCols; + } 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. + */ + VariableBlock& operator=(VariableBlock&& values) { + if (this == &values) { + return *this; + } + + if (m_mat == nullptr) { + m_mat = values.m_mat; + m_rowOffset = values.m_rowOffset; + m_colOffset = values.m_colOffset; + m_blockRows = values.m_blockRows; + m_blockCols = values.m_blockCols; + } 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_blockRows{mat.Rows()}, m_blockCols{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_rowOffset{rowOffset}, + m_colOffset{colOffset}, + m_blockRows{blockRows}, + m_blockCols{blockCols} {} + + /** + * 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. + */ + VariableBlock& SetValue(double value) { + Assert(Rows() == 1 && Cols() == 1); + + (*this)(0, 0).SetValue(value); + + return *this; + } + + /** + * Assigns an Eigen matrix to the block. + */ + 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. + */ + template + requires std::same_as + VariableBlock& 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)); + } + } + + return *this; + } + + /** + * Assigns a VariableMatrix to the block. + */ + 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. + */ + 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. + */ + template + requires(!std::is_const_v) + Variable& operator()(int row, int col) { + Assert(row >= 0 && row < Rows()); + Assert(col >= 0 && col < Cols()); + return (*m_mat)(m_rowOffset + row, m_colOffset + col); + } + + /** + * 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_rowOffset + row, m_colOffset + col); + } + + /** + * Returns a scalar subblock at the given row. + * + * @param row The scalar subblock's row. + */ + template + requires(!std::is_const_v) + Variable& operator()(int row) { + 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 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. + */ + 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{*m_mat, m_rowOffset + rowOffset, + m_colOffset + colOffset, blockRows, blockCols}; + } + + /** + * 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 VariableBlock{*m_mat, m_rowOffset + rowOffset, + m_colOffset + colOffset, blockRows, blockCols}; + } + + /** + * 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_blockRows; } + + /** + * Returns number of columns in the matrix. + */ + int Cols() const { return m_blockCols; } + + /** + * 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) const { + Assert(row >= 0 && row < Rows()); + Assert(col >= 0 && col < Cols()); + return (*m_mat)(m_rowOffset + row, m_colOffset + col).Value(); + } + + /** + * Returns a row of the variable column vector. + * + * @param index The index of the element to return. + */ + double Value(int index) const { + Assert(index >= 0 && index < Rows() * Cols()); + return (*m_mat)(m_rowOffset + index / m_blockCols, + m_colOffset + index % m_blockCols) + .Value(); + } + + /** + * Returns the contents of the variable matrix. + */ + Eigen::MatrixXd Value() const { + 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 m_blockRows * m_blockCols; } + + private: + Mat* m_mat = nullptr; + int m_rowOffset = 0; + int m_colOffset = 0; + int m_blockRows = 0; + int m_blockCols = 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 new file mode 100644 index 0000000000..68c71a5a37 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp @@ -0,0 +1,1007 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include + +#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(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(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. + */ + 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. + */ + 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. + */ + 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. + */ + template + requires std::same_as + VariableMatrix& 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)); + } + } + + return *this; + } + + /** + * Constructs a scalar VariableMatrix from a Variable. + */ + VariableMatrix(const Variable& variable) // NOLINT + : m_rows{1}, m_cols{1} { + m_storage.emplace_back(variable); + } + + /** + * Constructs a scalar VariableMatrix from a Variable. + */ + VariableMatrix(Variable&& variable) : m_rows{1}, m_cols{1} { // NOLINT + m_storage.emplace_back(std::move(variable)); + } + + /** + * Constructs a VariableMatrix from a VariableBlock. + */ + 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. + */ + 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 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. + */ + 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 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 VariableBlock{*this, rowOffset, colOffset, blockRows, blockCols}; + } + + /** + * 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) const { + 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) const { + Assert(index >= 0 && index < Rows() * Cols()); + return m_storage[index].Value(); + } + + /** + * Returns the contents of the variable matrix. + */ + Eigen::MatrixXd Value() const { + 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: + std::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 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( + 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; +} + +} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/OCPSolver.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/OCPSolver.hpp new file mode 100644 index 0000000000..ef0243f46e --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/OCPSolver.hpp @@ -0,0 +1,408 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#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" + +namespace sleipnir { + +/** + * 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) + */ +using DynamicsFunction = + function_ref; + +/** + * Performs 4th order Runge-Kutta integration of dx/dt = f(t, x, u) for dt. + * + * @param f The function to integrate. It must take two arguments x and u. + * @param x The initial value of x. + * @param u The value u held constant over the integration period. + * @param t0 The initial time. + * @param dt The time over which to integrate. + */ +template +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); + State k3 = f(t0 + halfdt, x + k2 * halfdt, u, dt); + State k4 = f(t0 + dt, x + k3 * dt, u, dt); + + return x + (k1 + k2 * 2.0 + k3 * 2.0 + k4) * (dt / 6.0); +} + +/** + * Enum describing an OCP transcription method. + */ +enum class TranscriptionMethod : uint8_t { + /// Each state is a decision variable constrained to the integrated dynamics + /// of the previous state. + kDirectTranscription, + /// The trajectory is modeled as a series of cubic polynomials where the + /// centerpoint slope is constrained. + kDirectCollocation, + /// States depend explicitly as a function of all previous states and all + /// previous inputs. + kSingleShooting +}; + +/** + * Enum describing a type of system dynamics constraints. + */ +enum class DynamicsType : uint8_t { + /// The dynamics are a function in the form dx/dt = f(t, x, u). + kExplicitODE, + /// The dynamics are a function in the form xₖ₊₁ = f(t, xₖ, uₖ). + kDiscrete +}; + +/** + * Enum describing the type of system timestep. + */ +enum class TimestepMethod : uint8_t { + /// The timestep is a fixed constant. + kFixed, + /// The timesteps are allowed to vary as independent decision variables. + kVariable, + /// The timesteps are equal length but allowed to vary as a single decision + /// variable. + kVariableSingle +}; + +/** + * This class allows the user to pose and solve a constrained optimal control + * problem (OCP) in a variety of ways. + * + * The system is transcripted by one of three methods (direct transcription, + * direct collocation, or single-shooting) and additional constraints can be + * added. + * + * In direct transcription, each state is a decision variable constrained to the + * integrated dynamics of the previous state. In direct collocation, the + * trajectory is modeled as a series of cubic polynomials where the centerpoint + * slope is constrained. In single-shooting, states depend explicitly as a + * function of all previous states and all previous inputs. + * + * Explicit ODEs are integrated using RK4. + * + * For explicit ODEs, the function must be in the form dx/dt = f(t, x, u). + * For discrete state transition functions, the function must be in the form + * xₖ₊₁ = f(t, xₖ, uₖ). + * + * Direct collocation requires an explicit ODE. Direct transcription and + * single-shooting can use either an ODE or state transition function. + * + * https://underactuated.mit.edu/trajopt.html goes into more detail on each + * transcription method. + */ +class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem { + 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 dt The timestep for fixed-step integration. + * @param numSteps The number of control points. + * @param dynamics The system evolution function, either an explicit ODE or a + * discrete state transition function. + * @param dynamicsType The type of system evolution function. + * @param timestepMethod The timestep method. + * @param method The transcription method. + */ + OCPSolver( + int numStates, int numInputs, std::chrono::duration dt, + int numSteps, DynamicsFunction dynamics, + DynamicsType dynamicsType = DynamicsType::kExplicitODE, + TimestepMethod timestepMethod = TimestepMethod::kFixed, + TranscriptionMethod method = TranscriptionMethod::kDirectTranscription) + : m_numStates{numStates}, + m_numInputs{numInputs}, + 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); + + if (m_timestepMethod == TimestepMethod::kFixed) { + m_DT = VariableMatrix{1, m_numSteps + 1}; + for (int i = 0; i < numSteps + 1; ++i) { + m_DT(0, i) = m_dt.count(); + } + } else if (m_timestepMethod == TimestepMethod::kVariableSingle) { + Variable DT = DecisionVariable(); + DT.SetValue(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; + } + } 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()); + } + } + + 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) { + // 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(); + } + } + + /** + * Utility function to constrain the initial state. + * + * @param initialState the initial state to constrain to. + */ + template + requires ScalarLike || MatrixLike + void ConstrainInitialState(const T& initialState) { + SubjectTo(InitialState() == initialState); + } + + /** + * Utility function to constrain the final state. + * + * @param finalState the final state to constrain to. + */ + template + requires ScalarLike || MatrixLike + void ConstrainFinalState(const T& finalState) { + SubjectTo(FinalState() == finalState); + } + + /** + * Set the constraint evaluation function. This function is called + * `numSteps+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( + 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); + callback(time, x, u, dt); + + time += dt; + } + } + + /** + * 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. + */ + template + requires ScalarLike || MatrixLike + void SetLowerInputBound(const T& lowerBound) { + for (int i = 0; i < m_numSteps + 1; ++i) { + SubjectTo(U().Col(i) >= lowerBound); + } + } + + /** + * 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. + */ + template + requires ScalarLike || MatrixLike + void SetUpperInputBound(const T& upperBound) { + for (int i = 0; i < m_numSteps + 1; ++i) { + SubjectTo(U().Col(i) <= upperBound); + } + } + + /** + * Convenience function to set an upper bound on the timestep. + * + * @param maxTimestep The maximum timestep. + */ + void SetMaxTimestep(std::chrono::duration maxTimestep) { + SubjectTo(DT() <= maxTimestep.count()); + } + + /** + * Convenience function to set a lower bound on the timestep. + * + * @param minTimestep The minimum timestep. + */ + void SetMinTimestep(std::chrono::duration minTimestep) { + SubjectTo(DT() >= minTimestep.count()); + } + + /** + * Get the state variables. After the problem is solved, this will contain the + * optimized trajectory. + * + * Shaped (numStates)x(numSteps+1). + * + * @returns The state variable matrix. + */ + 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. + * + * @returns The input variable matrix. + */ + 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 + * the trajectory. + * + * @returns The timestep variable matrix. + */ + VariableMatrix& DT() { return m_DT; }; + + /** + * Convenience function to get the initial state in the trajectory. + * + * @returns The initial state of the trajectory. + */ + VariableMatrix InitialState() { return m_X.Col(0); } + + /** + * Convenience function to get the final state in the trajectory. + * + * @returns The final state of the trajectory. + */ + VariableMatrix FinalState() { return m_X.Col(m_numSteps); } + + private: + void ConstrainDirectCollocation() { + Assert(m_dynamicsType == DynamicsType::kExplicitODE); + + 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_begin = U().Col(i); + Variable dt = DT()(0, i); + auto t_begin = time; + auto t_end = time + dt; + auto t_c = t_begin + dt / 2.0; + + time += dt; + + // Use u_begin on the end point as well because we are approaching a + // discontinuity from the left + auto f_begin = m_dynamicsFunction(t_begin, x_begin, u_begin, dt); + auto f_end = m_dynamicsFunction(t_end, x_end, u_begin, dt); + auto x_c = (x_begin + x_end) / 2.0 + (f_begin - f_end) * (dt / 8.0); + auto xprime_c = + (x_begin - x_end) * (-3.0 / (2.0 * dt)) - (f_begin + f_end) / 4.0; + auto f_c = m_dynamicsFunction(t_c, x_c, u_begin, dt); + SubjectTo(f_c == xprime_c); + } + } + + void ConstrainDirectTranscription() { + 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); + + 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)); + } + + time += dt; + } + } + + void ConstrainSingleShooting() { + 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); + + if (m_dynamicsType == DynamicsType::kExplicitODE) { + x_end = RK4(m_dynamicsFunction, x_begin, u, time, dt); + } else if (m_dynamicsType == DynamicsType::kDiscrete) { + x_end = m_dynamicsFunction(time, x_begin, u, dt); + } + + time += dt; + } + } + + int m_numStates; + int m_numInputs; + std::chrono::duration m_dt; + int m_numSteps; + TranscriptionMethod m_transcriptionMethod; + + DynamicsType m_dynamicsType; + DynamicsFunction m_dynamicsFunction; + + TimestepMethod m_timestepMethod; + + VariableMatrix m_X; + VariableMatrix m_U; + VariableMatrix m_DT; +}; + +} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Constraints.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Constraints.hpp new file mode 100644 index 0000000000..820430bd3b --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Constraints.hpp @@ -0,0 +1,251 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include + +#include "sleipnir/autodiff/Variable.hpp" +#include "sleipnir/util/Assert.hpp" +#include "sleipnir/util/Concepts.hpp" +#include "sleipnir/util/SymbolExports.hpp" + +namespace sleipnir { + +/** + * Make a list of constraints. + * + * The standard form for equality constraints is c(x) = 0, and the standard form + * for inequality constraints is c(x) ≥ 0. This function takes constraints of + * the form lhs = rhs or lhs ≥ rhs and converts them to lhs - rhs = 0 or + * lhs - rhs ≥ 0. + * + * @param lhs Left-hand side. + * @param rhs Right-hand side. + */ +template + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (!std::same_as || !std::same_as) +std::vector MakeConstraints(const LHS& lhs, const RHS& rhs) { + std::vector constraints; + + 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(); + } + + constraints.reserve(rows * cols); + + for (int row = 0; row < rows; ++row) { + for (int col = 0; col < 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(); + } + + constraints.reserve(rows * cols); + + for (int row = 0; row < rows; ++row) { + for (int col = 0; col < 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(); + } + + [[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) { + // Make right-hand side zero + constraints.emplace_back(lhs(row, col) - rhs(row, col)); + } + } + } + + return constraints; +} + +/** + * A vector of equality constraints of the form cₑ(x) = 0. + */ +struct SLEIPNIR_DLLEXPORT EqualityConstraints { + /// A vector of scalar equality constraints. + std::vector constraints; + + /** + * Constructs an equality constraint from a left and right side. + * + * The standard form for equality constraints is c(x) = 0. This function takes + * a constraint of the form lhs = rhs and converts it to lhs - rhs = 0. + * + * @param lhs Left-hand side. + * @param rhs Right-hand side. + */ + template + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (!std::same_as || !std::same_as) + EqualityConstraints(const LHS& lhs, const RHS& rhs) + : constraints{MakeConstraints(lhs, rhs)} {} + + /** + * Implicit conversion operator to bool. + */ + operator bool() const { // NOLINT + return std::all_of( + constraints.begin(), constraints.end(), + [](const auto& constraint) { return constraint.Value() == 0.0; }); + } +}; + +/** + * A vector of inequality constraints of the form cᵢ(x) ≥ 0. + */ +struct SLEIPNIR_DLLEXPORT InequalityConstraints { + /// A vector of scalar inequality constraints. + std::vector constraints; + + /** + * Constructs an inequality constraint from a left and right side. + * + * The standard form for inequality constraints is c(x) ≥ 0. This function + * takes a constraints of the form lhs ≥ rhs and converts it to lhs - rhs ≥ 0. + * + * @param lhs Left-hand side. + * @param rhs Right-hand side. + */ + template + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (!std::same_as || !std::same_as) + InequalityConstraints(const LHS& lhs, const RHS& rhs) + : constraints{MakeConstraints(lhs, rhs)} {} + + /** + * Implicit conversion operator to bool. + */ + operator bool() const { // NOLINT + return std::all_of( + constraints.begin(), constraints.end(), + [](const auto& constraint) { return constraint.Value() >= 0.0; }); + } +}; + +/** + * Equality operator that returns an equality constraint for two Variables. + * + * @param lhs Left-hand side. + * @param rhs Left-hand side. + */ +template + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (!std::same_as || !std::same_as) +EqualityConstraints operator==(const LHS& lhs, const RHS& rhs) { + return EqualityConstraints{lhs, rhs}; +} + +/** + * Less-than comparison operator that returns an inequality constraint for two + * Variables. + * + * @param lhs Left-hand side. + * @param rhs Left-hand side. + */ +template + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (!std::same_as || !std::same_as) +InequalityConstraints operator<(const LHS& lhs, const RHS& rhs) { + return rhs >= lhs; +} + +/** + * Less-than-or-equal-to comparison operator that returns an inequality + * constraint for two Variables. + * + * @param lhs Left-hand side. + * @param rhs Left-hand side. + */ +template + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (!std::same_as || !std::same_as) +InequalityConstraints operator<=(const LHS& lhs, const RHS& rhs) { + return rhs >= lhs; +} + +/** + * Greater-than comparison operator that returns an inequality constraint for + * two Variables. + * + * @param lhs Left-hand side. + * @param rhs Left-hand side. + */ +template + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (!std::same_as || !std::same_as) +InequalityConstraints operator>(const LHS& lhs, const RHS& rhs) { + return lhs >= rhs; +} + +/** + * Greater-than-or-equal-to comparison operator that returns an inequality + * constraint for two Variables. + * + * @param lhs Left-hand side. + * @param rhs Left-hand side. + */ +template + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (!std::same_as || !std::same_as) +InequalityConstraints operator>=(const LHS& lhs, const RHS& rhs) { + return InequalityConstraints{lhs, rhs}; +} + +} // 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 new file mode 100644 index 0000000000..e25ad1ea74 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Multistart.hpp @@ -0,0 +1,69 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include +#include + +#include "sleipnir/optimization/SolverStatus.hpp" +#include "sleipnir/util/FunctionRef.hpp" + +namespace sleipnir { + +/** + * The result of a multistart solve. + * + * @tparam DecisionVariables The type containing the decision variable initial + * guess. + */ +template +struct MultistartResult { + SolverStatus status; + DecisionVariables variables; +}; + +/** + * Solves an optimization problem from different starting points in parallel, + * then returns the solution with the lowest cost. + * + * Each solve is performed on a separate thread. Solutions from successful + * solves are always preferred over solutions from unsuccessful solves, and cost + * (lower is better) is the tiebreaker between successful solves. + * + * @tparam DecisionVariables The type containing the decision variable initial + * 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. + */ +template +MultistartResult Multistart( + function_ref(const DecisionVariables&)> + solve, + std::span initialGuesses) { + std::vector>> futures; + for (const auto& initialGuess : initialGuesses) { + futures.emplace_back(std::async(std::launch::async, solve, initialGuess)); + } + + std::vector> results; + 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; + } + + // Otherwise prioritize solution with lower cost + return a.status.cost < b.status.cost; + }); +} + +} // namespace sleipnir 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 new file mode 100644 index 0000000000..224618720d --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp @@ -0,0 +1,553 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "sleipnir/autodiff/Variable.hpp" +#include "sleipnir/autodiff/VariableMatrix.hpp" +#include "sleipnir/optimization/Constraints.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/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. We'll cover some examples next. + * + * ## Double integrator minimum time + * + * A system with position and velocity states and an acceleration input is an + * example of a double integrator. We want to go from 0 m at rest to 10 m at + * rest in the minimum time while obeying the velocity limit (-1, 1) and the + * acceleration limit (-1, 1). + * + * The model for our double integrator is ẍ=u where x is the vector [position; + * velocity] and u is the acceleration. The velocity constraints are -1 ≤ x(1) + * ≤ 1 and the acceleration constraints are -1 ≤ u ≤ 1. + * + * ### Initializing a problem instance + * + * First, we need to make a problem instance. + * @code{.cpp} + * #include + * #include + * + * int main() { + * constexpr auto T = 5s; + * constexpr auto dt = 5ms; + * constexpr int N = T / dt; + * + * sleipnir::OptimizationProblem problem; + * @endcode + * + * ### Creating decision variables + * + * First, we need to make decision variables for our state and input. + * @code{.cpp} + * // 2x1 state vector with N + 1 timesteps (includes last state) + * auto X = problem.DecisionVariable(2, N + 1); + * + * // 1x1 input vector with N timesteps (input at last state doesn't matter) + * auto U = problem.DecisionVariable(1, N); + * @endcode + * By convention, we use capital letters for the variables to designate + * matrices. + * + * ### Applying constraints + * + * Now, we need to apply dynamics constraints between timesteps. + * @code{.cpp} + * // Kinematics constraint assuming constant acceleration between timesteps + * for (int k = 0; k < N; ++k) { + * constexpr double t = std::chrono::duration(dt).count(); + * auto p_k1 = X(0, k + 1); + * auto v_k1 = X(1, k + 1); + * auto p_k = X(0, k); + * auto v_k = X(1, k); + * auto a_k = U(0, k); + * + * // pₖ₊₁ = pₖ + vₖt + * problem.SubjectTo(p_k1 == p_k + v_k * t); + * + * // vₖ₊₁ = vₖ + aₖt + * problem.SubjectTo(v_k1 == v_k + a_k * t); + * } + * @endcode + * + * Next, we'll apply the state and input constraints. + * @code{.cpp} + * // Start and end at rest + * problem.SubjectTo(X.Col(0) == Eigen::Matrix{{0.0}, {0.0}}); + * problem.SubjectTo( + * X.Col(N + 1) == Eigen::Matrix{{10.0}, {0.0}}); + * + * // Limit velocity + * problem.SubjectTo(-1 <= X.Row(1)); + * problem.SubjectTo(X.Row(1) <= 1); + * + * // Limit acceleration + * problem.SubjectTo(-1 <= U); + * problem.SubjectTo(U <= 1); + * @endcode + * + * ### Specifying a cost function + * + * Next, we'll create a cost function for minimizing position error. + * @code{.cpp} + * // Cost function - minimize position error + * sleipnir::Variable J = 0.0; + * for (int k = 0; k < N + 1; ++k) { + * J += sleipnir::pow(10.0 - X(0, k), 2); + * } + * problem.Minimize(J); + * @endcode + * The cost function passed to Minimize() should produce a scalar output. + * + * ### Solving the problem + * + * Now we can solve the problem. + * @code{.cpp} + * problem.Solve(); + * @endcode + * + * The solver will find the decision variable values that minimize the cost + * function while satisfying the constraints. + * + * ### Accessing the solution + * + * You can obtain the solution by querying the values of the variables like so. + * @code{.cpp} + * double position = X.Value(0, 0); + * double velocity = X.Value(1, 0); + * double acceleration = U.Value(0); + * @endcode + * + * ### Other applications + * + * In retrospect, the solution here seems obvious: if you want to reach the + * desired position in the minimum time, you just apply positive max input to + * accelerate to the max speed, coast for a while, then apply negative max input + * to decelerate to a stop at the desired position. Optimization problems can + * get more complex than this though. In fact, we can use this same framework to + * design optimal trajectories for a drivetrain while satisfying dynamics + * constraints, avoiding obstacles, and driving through points of interest. + * + * ## Optimizing the problem formulation + * + * Cost functions and constraints can have the following orders: + * + *
    + *
  • none (i.e., there is no cost function or are no constraints)
    • + *
  • constant
    • + *
  • linear
    • + *
  • quadratic
    • + *
  • nonlinear
    • + *
+ * + * For nonlinear problems, the solver calculates the Hessian of the cost + * function and the Jacobians of the constraints at each iteration. However, + * problems with lower order cost functions and constraints can be solved + * faster. For example, the following only need to be computed once because + * they're constant: + * + *
    + *
  • the Hessian of a quadratic or lower cost function
    • + *
  • the Jacobian of linear or lower constraints
    • + *
+ * + * A problem is constant if: + * + *
    + *
  • the cost function is constant or lower
    • + *
  • the equality constraints are constant or lower
    • + *
  • the inequality constraints are constant or lower
    • + *
+ * + * A problem is linear if: + * + *
    + *
  • the cost function is linear
    • + *
  • the equality constraints are linear or lower
    • + *
  • the inequality constraints are linear or lower
    • + *
+ * + * A problem is quadratic if: + * + *
    + *
  • the cost function is quadratic
    • + *
  • the equality constraints are linear or lower
    • + *
  • the inequality constraints are linear or lower
    • + *
+ * + * All other problems are nonlinear. + */ +class SLEIPNIR_DLLEXPORT OptimizationProblem { + public: + /** + * Construct the optimization problem. + */ + OptimizationProblem() noexcept { + m_decisionVariables.reserve(1024); + m_equalityConstraints.reserve(1024); + m_inequalityConstraints.reserve(1024); + } + + /** + * 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 + 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 std::invocable && + std::same_as, + void> + 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 std::invocable && + std::same_as, + bool> + 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 + std::vector m_decisionVariables; + + // The cost function: f(x) + std::optional m_f; + + // The list of equality constraints: cₑ(x) = 0 + std::vector m_equalityConstraints; + + // The list of inequality constraints: cᵢ(x) ≥ 0 + std::vector 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 new file mode 100644 index 0000000000..f7323f7388 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverConfig.hpp @@ -0,0 +1,54 @@ +// 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 new file mode 100644 index 0000000000..734cd3d127 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp @@ -0,0 +1,78 @@ +// 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) { + switch (exitCondition) { + case SolverExitCondition::kSuccess: + return "solved to desired tolerance"; + case SolverExitCondition::kSolvedToAcceptableTolerance: + return "solved to acceptable tolerance"; + case SolverExitCondition::kCallbackRequestedStop: + return "callback requested stop"; + case SolverExitCondition::kTooFewDOFs: + return "problem has too few degrees of freedom"; + case SolverExitCondition::kLocallyInfeasible: + return "problem is locally infeasible"; + case SolverExitCondition::kFeasibilityRestorationFailed: + return "solver failed to reach the desired tolerance, and feasibility " + "restoration failed to converge"; + case SolverExitCondition::kNonfiniteInitialCostOrConstraints: + return "solver encountered nonfinite initial cost or constraints and " + "gave up"; + case SolverExitCondition::kDivergingIterates: + return "solver encountered diverging primal iterates xₖ and/or sₖ and " + "gave up"; + case SolverExitCondition::kMaxIterationsExceeded: + return "solution returned after maximum iterations exceeded"; + case SolverExitCondition::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/SolverIterationInfo.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverIterationInfo.hpp new file mode 100644 index 0000000000..bb915b856e --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverIterationInfo.hpp @@ -0,0 +1,36 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include + +namespace sleipnir { + +/** + * Solver iteration information exposed to a user callback. + */ +struct SolverIterationInfo { + /// 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; + + /// The Hessian of the Lagrangian. + const Eigen::SparseMatrix& H; + + /// The equality constraint Jacobian. + const Eigen::SparseMatrix& A_e; + + /// The inequality constraint Jacobian. + const Eigen::SparseMatrix& A_i; +}; + +} // 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 new file mode 100644 index 0000000000..122941cda7 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverStatus.hpp @@ -0,0 +1,32 @@ +// 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/solver/InteriorPoint.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/InteriorPoint.hpp new file mode 100644 index 0000000000..abc9f778be --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/InteriorPoint.hpp @@ -0,0 +1,55 @@ +// 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/util/Assert.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Assert.hpp new file mode 100644 index 0000000000..9999544278 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Assert.hpp @@ -0,0 +1,26 @@ +// 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( \ + fmt::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 new file mode 100644 index 0000000000..3f4f725239 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Concepts.hpp @@ -0,0 +1,30 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include + +#include "sleipnir/autodiff/Variable.hpp" +#include "sleipnir/autodiff/VariableMatrix.hpp" + +namespace sleipnir { + +template +concept ScalarLike = std::same_as || std::same_as || + std::same_as; + +template +concept EigenMatrixLike = + std::derived_from>; + +template +concept EigenSolver = requires(T t) { t.solve(Eigen::VectorXd{}); }; + +template +concept MatrixLike = + std::same_as || + std::same_as> || EigenMatrixLike; + +} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/FunctionRef.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/FunctionRef.hpp new file mode 100644 index 0000000000..14a46903dc --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/FunctionRef.hpp @@ -0,0 +1,100 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include +#include + +namespace sleipnir { + +/** + * An implementation of std::function_ref, a lightweight non-owning reference to + * a callable. + */ +template +class function_ref; + +template +class function_ref { + public: + constexpr function_ref() noexcept = delete; + + /** + * Creates a `function_ref` which refers to the same callable as `rhs`. + */ + constexpr function_ref(const function_ref& rhs) noexcept = + default; + + /** + * Constructs a `function_ref` referring to `f`. + */ + template + requires(!std::is_same_v, function_ref> && + std::is_invocable_r_v) + constexpr function_ref(F&& f) noexcept // NOLINT(google-explicit-constructor) + : obj_(const_cast( + reinterpret_cast(std::addressof(f)))) { + callback_ = [](void* obj, Args... args) -> R { + return std::invoke( + *reinterpret_cast::type>(obj), + std::forward(args)...); + }; + } + + /** + * Makes `*this` refer to the same callable as `rhs`. + */ + constexpr function_ref& operator=( + const function_ref& rhs) noexcept = default; + + /** + * Makes `*this` refer to `f`. + */ + template + requires std::is_invocable_r_v + constexpr function_ref& operator=(F&& f) noexcept { + obj_ = reinterpret_cast(std::addressof(f)); + callback_ = [](void* obj, Args... args) { + return std::invoke( + *reinterpret_cast::type>(obj), + std::forward(args)...); + }; + + return *this; + } + + /** + * Swaps the referred callables of `*this` and `rhs`. + */ + constexpr void swap(function_ref& rhs) noexcept { + std::swap(obj_, rhs.obj_); + std::swap(callback_, rhs.callback_); + } + + /** + * Call the stored callable with the given arguments. + */ + R operator()(Args... args) const { + return callback_(obj_, std::forward(args)...); + } + + private: + void* obj_ = nullptr; + R (*callback_)(void*, Args...) = nullptr; +}; + +/** + * Swaps the referred callables of `lhs` and `rhs`. + */ +template +constexpr void swap(function_ref& lhs, + function_ref& rhs) noexcept { + lhs.swap(rhs); +} + +template +function_ref(R (*)(Args...)) -> function_ref; + +} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/IntrusiveSharedPtr.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/IntrusiveSharedPtr.hpp new file mode 100644 index 0000000000..f1290e5837 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/IntrusiveSharedPtr.hpp @@ -0,0 +1,216 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include + +namespace sleipnir { + +/** + * A custom intrusive shared pointer implementation without thread + * synchronization overhead. + * + * Types used with this class should have three things: + * + * 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. + * + * @tparam T The type of the object to be reference counted. + */ +template +class IntrusiveSharedPtr { + public: + /** + * Constructs an empty intrusive shared pointer. + */ + constexpr IntrusiveSharedPtr() noexcept = default; + + /** + * Constructs an empty intrusive shared pointer. + */ + constexpr IntrusiveSharedPtr(std::nullptr_t) noexcept {} // NOLINT + + /** + * Constructs an intrusive shared pointer from the given pointer and takes + * ownership. + */ + explicit constexpr IntrusiveSharedPtr(T* ptr) noexcept : m_ptr{ptr} { + if (m_ptr != nullptr) { + IntrusiveSharedPtrIncRefCount(m_ptr); + } + } + + constexpr ~IntrusiveSharedPtr() { + if (m_ptr != nullptr) { + IntrusiveSharedPtrDecRefCount(m_ptr); + } + } + + /** + * Copy constructs from the given intrusive shared pointer. + */ + constexpr IntrusiveSharedPtr(const IntrusiveSharedPtr& rhs) noexcept + : m_ptr{rhs.m_ptr} { + if (m_ptr != nullptr) { + IntrusiveSharedPtrIncRefCount(m_ptr); + } + } + + /** + * Makes a copy of the given intrusive shared pointer. + */ + constexpr IntrusiveSharedPtr& operator=( // NOLINT + const IntrusiveSharedPtr& rhs) noexcept { + if (m_ptr == rhs.m_ptr) { + return *this; + } + + if (m_ptr != nullptr) { + IntrusiveSharedPtrDecRefCount(m_ptr); + } + + m_ptr = rhs.m_ptr; + + if (m_ptr != nullptr) { + IntrusiveSharedPtrIncRefCount(m_ptr); + } + + return *this; + } + + /** + * Move constructs from the given intrusive shared pointer. + */ + constexpr IntrusiveSharedPtr(IntrusiveSharedPtr&& rhs) noexcept + : m_ptr{std::exchange(rhs.m_ptr, nullptr)} {} + + /** + * Move assigns from the given intrusive shared pointer. + */ + 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. + */ + constexpr T* Get() const noexcept { return m_ptr; } + + /** + * Returns the object pointed to by the internal pointer. + */ + constexpr T& operator*() const noexcept { return *m_ptr; } + + /** + * Returns the internal pointer. + */ + constexpr T* operator->() const noexcept { return m_ptr; } + + /** + * Returns 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. + */ + friend constexpr bool operator==(const IntrusiveSharedPtr& lhs, + const IntrusiveSharedPtr& rhs) noexcept { + return lhs.m_ptr == rhs.m_ptr; + } + + /** + * Returns true if the given intrusive shared pointers point to different + * objects. + */ + friend constexpr bool operator!=(const IntrusiveSharedPtr& lhs, + const IntrusiveSharedPtr& rhs) noexcept { + return lhs.m_ptr != rhs.m_ptr; + } + + /** + * Returns true if the left-hand intrusive shared pointer points to nullptr. + */ + friend constexpr bool operator==(const IntrusiveSharedPtr& lhs, + std::nullptr_t) noexcept { + return lhs.m_ptr == nullptr; + } + + /** + * Returns true if the right-hand intrusive shared pointer points to nullptr. + */ + friend constexpr bool operator==(std::nullptr_t, + const IntrusiveSharedPtr& rhs) noexcept { + return nullptr == rhs.m_ptr; + } + + /** + * Returns true if the left-hand intrusive shared pointer doesn't point to + * nullptr. + */ + friend constexpr bool operator!=(const IntrusiveSharedPtr& lhs, + std::nullptr_t) noexcept { + return lhs.m_ptr != nullptr; + } + + /** + * Returns true if the right-hand intrusive shared pointer doesn't point to + * nullptr. + */ + friend constexpr bool operator!=(std::nullptr_t, + const IntrusiveSharedPtr& rhs) noexcept { + return nullptr != rhs.m_ptr; + } + + private: + T* m_ptr = nullptr; +}; + +/** + * Constructs an object of type T and wraps it in an intrusive shared pointer + * using args as the parameter list for the constructor of T. + * + * @tparam T Type of object for intrusive shared pointer. + * @tparam Args Types of constructor arguments. + * @param args Constructor arguments for T. + */ +template +IntrusiveSharedPtr MakeIntrusiveShared(Args&&... args) { + return IntrusiveSharedPtr{new T(std::forward(args)...)}; +} + +/** + * Constructs an object of type T and wraps it in an intrusive shared pointer + * using alloc as the storage allocator of T and args as the parameter list for + * the constructor of T. + * + * @tparam T Type of object for intrusive shared pointer. + * @tparam Alloc Type of allocator for T. + * @tparam Args Types of constructor arguments. + * @param alloc The allocator for T. + * @param args Constructor arguments for T. + */ +template +IntrusiveSharedPtr AllocateIntrusiveShared(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 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 new file mode 100644 index 0000000000..23b80d03c0 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Pool.hpp @@ -0,0 +1,159 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include + +#include "sleipnir/util/SymbolExports.hpp" + +namespace sleipnir { + +/** + * This class implements a pool memory resource. + * + * The pool allocates chunks of memory and splits them into blocks managed by a + * free list. Allocations return pointers from the free list, and deallocations + * return pointers to the free list. + */ +class SLEIPNIR_DLLEXPORT PoolResource { + public: + /** + * Constructs a default PoolResource. + * + * @param blocksPerChunk Number of blocks per chunk of memory. + */ + explicit PoolResource(size_t blocksPerChunk) + : blocksPerChunk{blocksPerChunk} {} + + PoolResource(const PoolResource&) = delete; + PoolResource& operator=(const PoolResource&) = delete; + PoolResource(PoolResource&&) = default; + PoolResource& operator=(PoolResource&&) = default; + + /** + * Returns a block of memory from the pool. + * + * @param bytes Number of bytes in the block. + * @param alignment Alignment of the block (unused). + */ + [[nodiscard]] + void* allocate(size_t bytes, size_t alignment = alignof(std::max_align_t)) { + if (m_freeList.empty()) { + AddChunk(bytes); + } + + auto ptr = m_freeList.back(); + m_freeList.pop_back(); + return ptr; + } + + /** + * Gives a block of memory back to the pool. + * + * @param p A pointer to the block of memory. + * @param bytes Number of bytes in the block (unused). + * @param alignment Alignment of the block (unused). + */ + void deallocate(void* p, size_t bytes, + size_t alignment = alignof(std::max_align_t)) { + m_freeList.emplace_back(p); + } + + /** + * Returns true if this pool resource has the same backing storage as another. + */ + bool is_equal(const PoolResource& other) const noexcept { + return this == &other; + } + + /** + * Returns 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(); + } + + private: + std::vector> m_buffer; + std::vector m_freeList; + size_t blocksPerChunk; + + /** + * 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. + */ + 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); + } + } +}; + +/** + * This class is an allocator for the pool resource. + * + * @tparam T The type of object in the pool. + */ +template +class PoolAllocator { + public: + /** + * The type of object in the pool. + */ + using value_type = T; + + /** + * Constructs a pool allocator with the given pool memory resource. + * + * @param r The pool resource. + */ + explicit constexpr PoolAllocator(PoolResource* r) : m_memoryResource{r} {} + + constexpr PoolAllocator(const PoolAllocator& other) = default; + constexpr PoolAllocator& operator=(const PoolAllocator&) = default; + + /** + * Returns a block of memory from the pool. + * + * @param n Number of bytes in the block. + */ + [[nodiscard]] + constexpr T* allocate(size_t n) { + return static_cast(m_memoryResource->allocate(n)); + } + + /** + * Gives a block of memory back to the pool. + * + * @param p A pointer to the block of memory. + * @param n Number of bytes in the block. + */ + constexpr void deallocate(T* p, size_t n) { + m_memoryResource->deallocate(p, n); + } + + private: + PoolResource* m_memoryResource; +}; + +/** + * Returns a global pool memory resource. + */ +SLEIPNIR_DLLEXPORT PoolResource& GlobalPoolResource(); + +/** + * Returns an allocator for a global pool memory resource. + * + * @tparam T The type of object in the pool. + */ +template +PoolAllocator GlobalPoolAllocator() { + return PoolAllocator{&GlobalPoolResource()}; +} + +} // namespace sleipnir 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 new file mode 100644 index 0000000000..fcf2e69bfb --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Print.hpp @@ -0,0 +1,56 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include + +#include + +namespace sleipnir { + +/** + * Wrapper around fmt::print() that squelches write failure exceptions. + */ +template +inline void print(fmt::format_string fmt, T&&... args) { + try { + fmt::print(fmt, std::forward(args)...); + } catch (const std::system_error&) { + } +} + +/** + * Wrapper around fmt::print() that squelches write failure exceptions. + */ +template +inline void print(std::FILE* f, fmt::format_string fmt, T&&... args) { + try { + fmt::print(f, fmt, std::forward(args)...); + } catch (const std::system_error&) { + } +} + +/** + * Wrapper around fmt::println() that squelches write failure exceptions. + */ +template +inline void println(fmt::format_string fmt, T&&... args) { + try { + fmt::println(fmt, std::forward(args)...); + } catch (const std::system_error&) { + } +} + +/** + * Wrapper around fmt::println() that squelches write failure exceptions. + */ +template +inline void println(std::FILE* f, fmt::format_string fmt, T&&... args) { + try { + fmt::println(f, fmt, std::forward(args)...); + } catch (const std::system_error&) { + } +} + +} // 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 new file mode 100644 index 0000000000..d7276db65a --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Spy.hpp @@ -0,0 +1,89 @@ +// 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(); + + std::vector 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/SymbolExports.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/SymbolExports.hpp new file mode 100644 index 0000000000..f8266636da --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/SymbolExports.hpp @@ -0,0 +1,192 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#ifdef _WIN32 +#ifdef _MSC_VER +#pragma warning(disable : 4251) +#endif + +#ifdef SLEIPNIR_EXPORTS +#ifdef __GNUC__ +#define SLEIPNIR_DLLEXPORT __attribute__((dllexport)) +#else +#define SLEIPNIR_DLLEXPORT __declspec(dllexport) +#endif + +#elif defined(SLEIPNIR_IMPORTS) + +#ifdef __GNUC__ +#define SLEIPNIR_DLLEXPORT __attribute__((dllimport)) +#else +#define SLEIPNIR_DLLEXPORT __declspec(dllimport) +#endif + +#else +#define SLEIPNIR_DLLEXPORT +#endif + +#else // _WIN32 + +#ifdef SLEIPNIR_EXPORTS +#define SLEIPNIR_DLLEXPORT __attribute__((visibility("default"))) +#else +#define SLEIPNIR_DLLEXPORT +#endif + +#endif // _WIN32 + +// Synopsis +// +// This header provides macros for using FOO_EXPORT macros with explicit +// template instantiation declarations and definitions. +// Generally, the FOO_EXPORT macros are used at declarations, +// and GCC requires them to be used at explicit instantiation declarations, +// but MSVC requires __declspec(dllexport) to be used at the explicit +// instantiation definitions instead. + +// Usage +// +// In a header file, write: +// +// extern template class EXPORT_TEMPLATE_DECLARE(FOO_EXPORT) foo; +// +// In a source file, write: +// +// template class EXPORT_TEMPLATE_DEFINE(FOO_EXPORT) foo; + +// Implementation notes +// +// The implementation of this header uses some subtle macro semantics to +// detect what the provided FOO_EXPORT value was defined as and then +// to dispatch to appropriate macro definitions. Unfortunately, +// MSVC's C preprocessor is rather non-compliant and requires special +// care to make it work. +// +// Issue 1. +// +// #define F(x) +// F() +// +// MSVC emits warning C4003 ("not enough actual parameters for macro +// 'F'), even though it's a valid macro invocation. This affects the +// macros below that take just an "export" parameter, because export +// may be empty. +// +// As a workaround, we can add a dummy parameter and arguments: +// +// #define F(x,_) +// F(,) +// +// Issue 2. +// +// #define F(x) G##x +// #define Gj() ok +// F(j()) +// +// The correct replacement for "F(j())" is "ok", but MSVC replaces it +// with "Gj()". As a workaround, we can pass the result to an +// identity macro to force MSVC to look for replacements again. (This +// is why EXPORT_TEMPLATE_STYLE_3 exists.) + +#define EXPORT_TEMPLATE_DECLARE(export) \ + EXPORT_TEMPLATE_INVOKE(DECLARE, EXPORT_TEMPLATE_STYLE(export, ), export) +#define EXPORT_TEMPLATE_DEFINE(export) \ + EXPORT_TEMPLATE_INVOKE(DEFINE, EXPORT_TEMPLATE_STYLE(export, ), export) + +// INVOKE is an internal helper macro to perform parameter replacements +// and token pasting to chain invoke another macro. E.g., +// EXPORT_TEMPLATE_INVOKE(DECLARE, DEFAULT, FOO_EXPORT) +// will export to call +// EXPORT_TEMPLATE_DECLARE_DEFAULT(FOO_EXPORT, ) +// (but with FOO_EXPORT expanded too). +#define EXPORT_TEMPLATE_INVOKE(which, style, export) \ + EXPORT_TEMPLATE_INVOKE_2(which, style, export) +#define EXPORT_TEMPLATE_INVOKE_2(which, style, export) \ + EXPORT_TEMPLATE_##which##_##style(export, ) + +// Default style is to apply the FOO_EXPORT macro at declaration sites. +#define EXPORT_TEMPLATE_DECLARE_DEFAULT(export, _) export +#define EXPORT_TEMPLATE_DEFINE_DEFAULT(export, _) + +// The "MSVC hack" style is used when FOO_EXPORT is defined +// as __declspec(dllexport), which MSVC requires to be used at +// definition sites instead. +#define EXPORT_TEMPLATE_DECLARE_MSVC_HACK(export, _) +#define EXPORT_TEMPLATE_DEFINE_MSVC_HACK(export, _) export + +// EXPORT_TEMPLATE_STYLE is an internal helper macro that identifies which +// export style needs to be used for the provided FOO_EXPORT macro definition. +// "", "__attribute__(...)", and "__declspec(dllimport)" are mapped +// to "DEFAULT"; while "__declspec(dllexport)" is mapped to "MSVC_HACK". +// +// It's implemented with token pasting to transform the __attribute__ and +// __declspec annotations into macro invocations. E.g., if FOO_EXPORT is +// defined as "__declspec(dllimport)", it undergoes the following sequence of +// macro substitutions: +// EXPORT_TEMPLATE_STYLE(FOO_EXPORT, ) +// EXPORT_TEMPLATE_STYLE_2(__declspec(dllimport), ) +// EXPORT_TEMPLATE_STYLE_3(EXPORT_TEMPLATE_STYLE_MATCH__declspec(dllimport)) +// EXPORT_TEMPLATE_STYLE_MATCH__declspec(dllimport) +// EXPORT_TEMPLATE_STYLE_MATCH_DECLSPEC_dllimport +// DEFAULT +#define EXPORT_TEMPLATE_STYLE(export, _) EXPORT_TEMPLATE_STYLE_2(export, ) +#define EXPORT_TEMPLATE_STYLE_2(export, _) \ + EXPORT_TEMPLATE_STYLE_3( \ + EXPORT_TEMPLATE_STYLE_MATCH_foj3FJo5StF0OvIzl7oMxA##export) +#define EXPORT_TEMPLATE_STYLE_3(style) style + +// Internal helper macros for EXPORT_TEMPLATE_STYLE. +// +// XXX: C++ reserves all identifiers containing "__" for the implementation, +// but "__attribute__" and "__declspec" already contain "__" and the token-paste +// operator can only add characters; not remove them. To minimize the risk of +// conflict with implementations, we include "foj3FJo5StF0OvIzl7oMxA" (a random +// 128-bit string, encoded in Base64) in the macro name. +#define EXPORT_TEMPLATE_STYLE_MATCH_foj3FJo5StF0OvIzl7oMxA DEFAULT +#define EXPORT_TEMPLATE_STYLE_MATCH_foj3FJo5StF0OvIzl7oMxA__attribute__(...) \ + DEFAULT +#define EXPORT_TEMPLATE_STYLE_MATCH_foj3FJo5StF0OvIzl7oMxA__declspec(arg) \ + EXPORT_TEMPLATE_STYLE_MATCH_DECLSPEC_##arg + +// Internal helper macros for EXPORT_TEMPLATE_STYLE. +#define EXPORT_TEMPLATE_STYLE_MATCH_DECLSPEC_dllexport MSVC_HACK +#define EXPORT_TEMPLATE_STYLE_MATCH_DECLSPEC_dllimport DEFAULT + +// Sanity checks. +// +// EXPORT_TEMPLATE_TEST uses the same macro invocation pattern as +// EXPORT_TEMPLATE_DECLARE and EXPORT_TEMPLATE_DEFINE do to check that they're +// working correctly. When they're working correctly, the sequence of macro +// replacements should go something like: +// +// EXPORT_TEMPLATE_TEST(DEFAULT, __declspec(dllimport)); +// +// static_assert(EXPORT_TEMPLATE_INVOKE(TEST_DEFAULT, +// EXPORT_TEMPLATE_STYLE(__declspec(dllimport), ), +// __declspec(dllimport)), "__declspec(dllimport)"); +// +// static_assert(EXPORT_TEMPLATE_INVOKE(TEST_DEFAULT, +// DEFAULT, __declspec(dllimport)), "__declspec(dllimport)"); +// +// static_assert(EXPORT_TEMPLATE_TEST_DEFAULT_DEFAULT( +// __declspec(dllimport)), "__declspec(dllimport)"); +// +// static_assert(true, "__declspec(dllimport)"); +// +// When they're not working correctly, a syntax error should occur instead. +#define EXPORT_TEMPLATE_TEST(want, export) \ + static_assert(EXPORT_TEMPLATE_INVOKE( \ + TEST_##want, EXPORT_TEMPLATE_STYLE(export, ), export), \ + #export) +#define EXPORT_TEMPLATE_TEST_DEFAULT_DEFAULT(...) true +#define EXPORT_TEMPLATE_TEST_MSVC_HACK_MSVC_HACK(...) true + +EXPORT_TEMPLATE_TEST(DEFAULT, ); +EXPORT_TEMPLATE_TEST(DEFAULT, __attribute__((visibility("default")))); +EXPORT_TEMPLATE_TEST(MSVC_HACK, __declspec(dllexport)); +EXPORT_TEMPLATE_TEST(DEFAULT, __declspec(dllimport)); + +#undef EXPORT_TEMPLATE_TEST +#undef EXPORT_TEMPLATE_TEST_DEFAULT_DEFAULT +#undef EXPORT_TEMPLATE_TEST_MSVC_HACK_MSVC_HACK diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide b/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide new file mode 100644 index 0000000000..8251a49067 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide @@ -0,0 +1,12 @@ +cppHeaderFileInclude { + \.hpp$ +} + +cppSrcFileInclude { + \.cpp$ +} + +includeOtherLibs { + ^Eigen/ + ^fmt/ +} 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..8ef2ebe0ce --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/Inertia.hpp @@ -0,0 +1,58 @@ +// 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 new file mode 100644 index 0000000000..d2488b7998 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/RegularizedLDLT.hpp @@ -0,0 +1,179 @@ +// 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/solver/InteriorPoint.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp new file mode 100644 index 0000000000..596988bf45 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp @@ -0,0 +1,876 @@ +// 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 + std::vector> 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(); + + iterationsStartTime = std::chrono::system_clock::now(); + + while (E_0 > config.tolerance && + acceptableIterCounter < config.maxAcceptableIterations) { + auto 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& a, 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 + 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); + + f.Update(); + + for (int row = 0; row < c_e.rows(); ++row) { + c_eAD(row).Update(); + } + Eigen::VectorXd trial_c_e = c_eAD.Value(); + + for (int row = 0; row < c_i.rows(); ++row) { + c_iAD(row).Update(); + } + 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); + + f.Update(); + + for (int row = 0; row < c_e.rows(); ++row) { + c_eAD(row).Update(); + } + trial_c_e = c_eAD.Value(); + + for (int row = 0; row < c_i.rows(); ++row) { + c_iAD(row).Update(); + } + 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); + + for (int row = 0; row < c_e.rows(); ++row) { + c_eAD(row).Update(); + } + Eigen::VectorXd trial_c_e = c_eAD.Value(); + + for (int row = 0; row < c_i.rows(); ++row) { + c_iAD(row).Update(); + } + 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, f, μ, + [&](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); + + for (int row = 0; row < c_e.rows(); ++row) { + c_eAD(row).Update(); + } + Eigen::VectorXd trial_c_e = c_eAD.Value(); + + for (int row = 0; row < c_i.rows(); ++row) { + c_iAD(row).Update(); + } + Eigen::VectorXd trial_c_i = c_iAD.Value(); + + for (int row = 0; row < c_i.rows(); ++row) { + c_iAD(row).Update(); + } + + f.Update(); + + // 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& a, 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(); + + // Update cₑ + for (int row = 0; row < c_e.rows(); ++row) { + c_eAD(row).Update(); + } + c_e = c_eAD.Value(); + + // Update cᵢ + for (int row = 0; row < c_i.rows(); ++row) { + c_iAD(row).Update(); + } + 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/util/ErrorEstimate.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp new file mode 100644 index 0000000000..6b757df3e6 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp @@ -0,0 +1,80 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include +#include + +// See docs/algorithms.md#Works_cited for citation definitions + +namespace sleipnir { + +/** + * Returns the error estimate using the KKT conditions for the interior-point + * method. + * + * @param g Gradient of the cost function ∇f. + * @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the + * current iterate. + * @param c_e The problem's equality constraints cₑ(x) evaluated at the current + * iterate. + * @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at + * the current iterate. + * @param c_i The problem's inequality constraints cᵢ(x) evaluated at the + * current iterate. + * @param s Inequality constraint slack variables. + * @param y Equality constraint dual variables. + * @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(); + + // Update the error estimate using the KKT conditions from equations (19.5a) + // through (19.5d) of [1]. + // + // ∇f − Aₑᵀy − Aᵢᵀz = 0 + // Sz − μe = 0 + // cₑ = 0 + // cᵢ − s = 0 + // + // The error tolerance is the max of the following infinity norms scaled by + // s_d and s_c (see equation (5) of [2]). + // + // ‖∇f − Aₑᵀy − Aᵢᵀz‖_∞ / s_d + // ‖Sz − μe‖_∞ / s_c + // ‖cₑ‖_∞ + // ‖cᵢ − s‖_∞ + + // 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)) / + s_max; + + // s_c = max(sₘₐₓ, ‖z‖₁ / n) / sₘₐₓ + double s_c = + std::max(s_max, z.lpNorm<1>() / numInequalityConstraints) / s_max; + + const auto S = s.asDiagonal(); + const Eigen::VectorXd e = Eigen::VectorXd::Ones(s.rows()); + + return std::max({(g - A_e.transpose() * y - A_i.transpose() * z) + .lpNorm() / + s_d, + (S * z - μ * e).lpNorm() / s_c, + c_e.lpNorm(), + (c_i - s).lpNorm()}); +} + +} // namespace sleipnir 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 new file mode 100644 index 0000000000..15f0088112 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp @@ -0,0 +1,239 @@ +// 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 + * 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] f The cost function. + * @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, Variable& f, 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; + + std::vector 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.cbegin(); + + 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 + std::vector 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 + std::vector 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 new file mode 100644 index 0000000000..01d43dbee6 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp @@ -0,0 +1,188 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include +#include +#include + +#include + +#include "sleipnir/autodiff/Variable.hpp" + +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. + */ + FilterEntry(double cost, double constraintViolation) + : cost{cost}, constraintViolation{constraintViolation} {} + + /** + * Constructs a FilterEntry. + * + * @param f The cost function. + * @param μ The barrier parameter. + * @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). + */ + FilterEntry(const Variable& f, double μ, const Eigen::VectorXd& s, + const Eigen::VectorXd& c_e, const Eigen::VectorXd& c_i) + : cost{f.Value() - μ * s.array().log().sum()}, + constraintViolation{c_e.lpNorm<1>() + (c_i - s).lpNorm<1>()} {} +}; + +/** + * Interior-point step filter. + */ +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. + * @param μ The barrier parameter. + */ + explicit Filter(Variable& f, double μ) { + m_f = &f; + m_μ = μ; + + // Initial filter entry rejects constraint violations above max + m_filter.emplace_back(std::numeric_limits::infinity(), + maxConstraintViolation); + } + + /** + * Reset the filter. + * + * @param μ The new barrier parameter. + */ + void Reset(double μ) { + m_μ = μ; + m_filter.clear(); + + // Initial filter entry rejects constraint violations above max + m_filter.emplace_back(std::numeric_limits::infinity(), + maxConstraintViolation); + } + + /** + * Creates a new 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). + */ + FilterEntry MakeEntry(Eigen::VectorXd& s, const Eigen::VectorXd& c_e, + const Eigen::VectorXd& c_i) { + return FilterEntry{*m_f, m_μ, s, c_e, c_i}; + } + + /** + * Add a new entry to the filter. + * + * @param entry The entry to add to the filter. + */ + void Add(const FilterEntry& entry) { + // Remove dominated entries + std::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 + std::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; + double m_μ = 0.0; + std::vector m_filter; +}; + +} // namespace sleipnir 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/FractionToTheBoundaryRule.hpp new file mode 100644 index 0000000000..98a117f49a --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FractionToTheBoundaryRule.hpp @@ -0,0 +1,45 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +// See docs/algorithms.md#Works_cited for citation definitions + +namespace sleipnir { + +/** + * Applies fraction-to-the-boundary rule to a variable and its iterate, then + * returns a fraction of the iterate step size within (0, 1]. + * + * @param x The variable. + * @param p The iterate on the variable. + * @param τ Fraction-to-the-boundary rule scaling factor within (0, 1]. + * @return Fraction of the iterate step size within (0, 1]. + */ +inline double FractionToTheBoundaryRule( + const Eigen::Ref& x, + const Eigen::Ref& p, double τ) { + // α = max(α ∈ (0, 1] : x + αp ≥ (1 − τ)x) + // + // where x and τ are positive. + // + // x + αp ≥ (1 − τ)x + // x + αp ≥ x − τx + // αp ≥ −τx + // + // If the inequality is false, p < 0 and α is too big. Find the largest value + // of α that makes the inequality true. + // + // α = −τ/p x + double α = 1.0; + for (int i = 0; i < x.rows(); ++i) { + if (α * p(i) < -τ * x(i)) { + α = -τ / p(i) * x(i); + } + } + + return α; +} + +} // namespace sleipnir 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/IsLocallyInfeasible.hpp new file mode 100644 index 0000000000..dc3992ea12 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/IsLocallyInfeasible.hpp @@ -0,0 +1,63 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include + +// See docs/algorithms.md#Works_cited for citation definitions + +namespace sleipnir { + +/** + * Returns true if the problem's equality constraints are locally infeasible. + * + * @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the + * current iterate. + * @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) { + // The equality constraints are locally infeasible if + // + // Aₑᵀcₑ → 0 + // ‖cₑ‖ > ε + // + // See "Infeasibility detection" in section 6 of [3]. + return A_e.rows() > 0 && (A_e.transpose() * c_e).norm() < 1e-6 && + c_e.norm() > 1e-2; +} + +/** + * Returns true if the problem's inequality constraints are locally infeasible. + * + * @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at + * the current iterate. + * @param c_i The problem's inequality constraints cᵢ(x) evaluated at the + * current iterate. + */ +inline bool IsInequalityLocallyInfeasible( + const Eigen::SparseMatrix& A_i, const Eigen::VectorXd& c_i) { + // The inequality constraints are locally infeasible if + // + // Aᵢᵀcᵢ⁺ → 0 + // ‖cᵢ⁺‖ > ε + // + // where cᵢ⁺ = min(cᵢ, 0). + // + // See "Infeasibility detection" in section 6 of [3]. + // + // cᵢ⁺ is used instead of cᵢ⁻ from the paper to follow the convention that + // feasible inequality constraints are ≥ 0. + if (A_i.rows() > 0) { + Eigen::VectorXd c_i_plus = c_i.cwiseMin(0.0); + if ((A_i.transpose() * c_i_plus).norm() < 1e-6 && c_i_plus.norm() > 1e-6) { + return true; + } + } + + return false; +} + +} // namespace sleipnir 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/KKTError.hpp new file mode 100644 index 0000000000..03fa112b6a --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp @@ -0,0 +1,51 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include + +// See docs/algorithms.md#Works_cited for citation definitions + +namespace sleipnir { + +/** + * Returns the KKT error for the interior-point method. + * + * @param g Gradient of the cost function ∇f. + * @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the + * current iterate. + * @param c_e The problem's equality constraints cₑ(x) evaluated at the current + * iterate. + * @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at + * the current iterate. + * @param c_i The problem's inequality constraints cᵢ(x) evaluated at the + * current iterate. + * @param s Inequality constraint slack variables. + * @param y Equality constraint dual variables. + * @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 μ) { + // Compute the KKT error as the 1-norm of the KKT conditions from equations + // (19.5a) through (19.5d) of [1]. + // + // ∇f − Aₑᵀy − Aᵢᵀz = 0 + // Sz − μe = 0 + // cₑ = 0 + // cᵢ − s = 0 + + const auto S = s.asDiagonal(); + const Eigen::VectorXd e = Eigen::VectorXd::Ones(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>(); +} + +} // 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..33b137b961 --- /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 sleipnir { + +PoolResource& GlobalPoolResource() { + thread_local PoolResource pool{16384}; + return pool; +} + +} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/util/ScopeExit.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/util/ScopeExit.hpp new file mode 100644 index 0000000000..dedc7ae4d3 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/util/ScopeExit.hpp @@ -0,0 +1,35 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +namespace sleipnir { + +template +class scope_exit { + public: + explicit scope_exit(F&& f) noexcept : m_f{std::forward(f)} {} + + ~scope_exit() { + if (m_active) { + m_f(); + } + } + + scope_exit(scope_exit&& rhs) noexcept + : m_f{std::move(rhs.m_f)}, m_active{rhs.m_active} { + rhs.release(); + } + + scope_exit(const scope_exit&) = delete; + scope_exit& operator=(const scope_exit&) = delete; + + void release() noexcept { m_active = false; } + + private: + F m_f; + bool m_active = true; +}; + +} // 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 new file mode 100644 index 0000000000..0e00418725 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/util/ToMilliseconds.hpp @@ -0,0 +1,21 @@ +// 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/test/native/cpp/SleipnirTest.cpp b/wpimath/src/test/native/cpp/SleipnirTest.cpp new file mode 100644 index 0000000000..46cb437fd5 --- /dev/null +++ b/wpimath/src/test/native/cpp/SleipnirTest.cpp @@ -0,0 +1,26 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include + +TEST(SleipnirTest, Quartic) { + sleipnir::OptimizationProblem problem; + + auto x = problem.DecisionVariable(); + x.SetValue(20.0); + + problem.Minimize(sleipnir::pow(x, 4)); + + problem.SubjectTo(x >= 1); + + auto status = problem.Solve({.diagnostics = true}); + + 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); + + EXPECT_NEAR(x.Value(), 1.0, 1e-6); +}