Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add Sleipnir (#6541)

Browse Source 
This is useful for solving quadratic programs.
This commit is contained in:
Tyler Veness
2024-04-27 22:42:42 -07:00
committed by GitHub
parent 1e4a647918
commit fd363fdf5a
53 changed files with 9289 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
.github/workflows/upstream-utils.yml vendored
View File

@@ -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

16
ThirdPartyNotices.txt
View File

@@ -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.

4
shared/jni/setupBuild.gradle
View File

@@ -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'
}

638
upstream_utils/sleipnir_patches/0001-Remove-using-enum-declarations.patch Normal file
View File

@@ -0,0 +1,638 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
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";

67
upstream_utils/update_sleipnir.py Executable file
View File

@@ -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()

20
wpimath/CMakeLists.txt
View File

@@ -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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/sleipnir/include>
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/sleipnir/src>
)
install(DIRECTORY src/main/native/thirdparty/gcem/include/ DESTINATION "${include_dest}/wpimath")
target_include_directories(
wpimath

28
wpimath/build.gradle
View File

@@ -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 {

249
wpimath/src/main/native/thirdparty/sleipnir/.clang-format vendored Normal file
View File

@@ -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: '^<ext/.*\.h>'
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
...

74
wpimath/src/main/native/thirdparty/sleipnir/.clang-tidy vendored Normal file
View File

@@ -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

23
wpimath/src/main/native/thirdparty/sleipnir/.styleguide vendored Normal file
View File

@@ -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/
}

1
wpimath/src/main/native/thirdparty/sleipnir/.styleguide-license vendored Normal file
View File

@@ -0,0 +1 @@
// Copyright (c) Sleipnir contributors

12
wpimath/src/main/native/thirdparty/sleipnir/include/.styleguide vendored Normal file
View File

@@ -0,0 +1,12 @@
cppHeaderFileInclude {
\.hpp$
}
cppSrcFileInclude {
\.cpp$
}
includeOtherLibs {
^Eigen/
^fmt/
}

1105
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

243
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionGraph.hpp vendored Normal file
View File

@@ -0,0 +1,243 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <span>
#include <vector>
#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<Expression*> 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<ExpressionPtr> GenerateGradientTree(
std::span<const ExpressionPtr> 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<ExpressionPtr> 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<void(int, double)> 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<int> m_rowList;
// List for updating adjoints
std::vector<Expression*> m_adjointList;
// List for updating values
std::vector<Expression*> m_valueList;
};
} // namespace sleipnir::detail

27
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionType.hpp vendored Normal file
View File

@@ -0,0 +1,27 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <stdint.h>
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

78
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Gradient.hpp vendored Normal file
View File

@@ -0,0 +1,78 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <utility>
#include <Eigen/SparseCore>
#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<double>& 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<double> m_g;
Jacobian m_jacobian;
};
} // namespace sleipnir

84
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Hessian.hpp vendored Normal file
View File

@@ -0,0 +1,84 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <utility>
#include <vector>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#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<detail::ExpressionPtr> 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<double>& 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

162
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Jacobian.hpp vendored Normal file
View File

@@ -0,0 +1,162 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <utility>
#include <vector>
#include <Eigen/SparseCore>
#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<detail::ExpressionPtr> 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<double>& 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<detail::ExpressionGraph> m_graphs;
Eigen::SparseMatrix<double> m_J{m_variables.Rows(), m_wrt.Rows()};
// Cached triplets for gradients of linear rows
std::vector<Eigen::Triplet<double>> m_cachedTriplets;
// List of row indices for nonlinear rows whose graients will be computed in
// Value()
std::vector<int> m_nonlinearRows;
Profiler m_profiler;
};
} // namespace sleipnir

79
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Profiler.hpp vendored Normal file
View File

@@ -0,0 +1,79 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <chrono>
#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<nanoseconds>(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<nanoseconds>(m_averageSolveDuration).count() / 1e6;
}
private:
std::chrono::system_clock::time_point m_setupStartTime;
std::chrono::duration<double> m_setupDuration{0.0};
int m_solveMeasurements = 0;
std::chrono::duration<double> m_averageSolveDuration{0.0};
std::chrono::system_clock::time_point m_solveStartTime;
};
} // namespace sleipnir

467
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Variable.hpp vendored Normal file
View File

@@ -0,0 +1,467 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <utility>
#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

617
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp vendored Normal file
View File

@@ -0,0 +1,617 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <concepts>
#include <type_traits>
#include <utility>
#include <Eigen/Core>
#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 <typename Mat>
class VariableBlock {
public:
VariableBlock(const VariableBlock<Mat>& values) = default;
/**
* Assigns a VariableBlock to the block.
*/
VariableBlock<Mat>& operator=(const VariableBlock<Mat>& 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<Mat>&&) = default;
/**
* Assigns a VariableBlock to the block.
*/
VariableBlock<Mat>& operator=(VariableBlock<Mat>&& 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<Mat>& 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<Mat>& SetValue(double value) {
Assert(Rows() == 1 && Cols() == 1);
(*this)(0, 0).SetValue(value);
return *this;
}
/**
* Assigns an Eigen matrix to the block.
*/
template <typename Derived>
VariableBlock<Mat>& operator=(const Eigen::MatrixBase<Derived>& 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 <typename Derived>
requires std::same_as<typename Derived::Scalar, double>
VariableBlock<Mat>& SetValue(const Eigen::MatrixBase<Derived>& 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<Mat>& 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<Mat>& 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 <typename Mat2 = Mat>
requires(!std::is_const_v<Mat2>)
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 <typename Mat2 = Mat>
requires(!std::is_const_v<Mat2>)
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<Mat> 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<const Mat> 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<Mat> 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<const Mat> 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<Mat> 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<const Mat> 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<Mat>& operator*=(const VariableBlock<Mat>& 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<Mat>& operator/=(const VariableBlock<Mat>& 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<Mat>& 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<Mat>& operator+=(const VariableBlock<Mat>& 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<Mat>& operator-=(const VariableBlock<Mat>& 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<Mat> T() const {
std::remove_cv_t<Mat> 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<Mat> CwiseTransform(
function_ref<Variable(const Variable&)> unaryOp) const {
std::remove_cv_t<Mat> 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>* 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<Mat>* 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>* 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<Mat>* 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

1007
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

408
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/OCPSolver.hpp vendored Normal file
View File

@@ -0,0 +1,408 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <stdint.h>
#include <chrono>
#include <utility>
#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<VariableMatrix(const Variable&, const VariableMatrix&,
const VariableMatrix&, const Variable&)>;
/**
* 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 <typename F, typename State, typename Input, typename Time>
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<double> 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 <typename T>
requires ScalarLike<T> || MatrixLike<T>
void ConstrainInitialState(const T& initialState) {
SubjectTo(InitialState() == initialState);
}
/**
* Utility function to constrain the final state.
*
* @param finalState the final state to constrain to.
*/
template <typename T>
requires ScalarLike<T> || MatrixLike<T>
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<void(const Variable&, const VariableMatrix&,
const VariableMatrix&, const Variable&)>
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 <typename T>
requires ScalarLike<T> || MatrixLike<T>
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 <typename T>
requires ScalarLike<T> || MatrixLike<T>
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<double> 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<double> 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<const DynamicsFunction&, VariableMatrix, VariableMatrix,
Variable>(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<const DynamicsFunction&, VariableMatrix, VariableMatrix,
Variable>(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<double> 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

251
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Constraints.hpp vendored Normal file
View File

@@ -0,0 +1,251 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <algorithm>
#include <concepts>
#include <vector>
#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 <typename LHS, typename RHS>
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(!std::same_as<LHS, double> || !std::same_as<RHS, double>)
std::vector<Variable> MakeConstraints(const LHS& lhs, const RHS& rhs) {
std::vector<Variable> constraints;
if constexpr (ScalarLike<LHS> && ScalarLike<RHS>) {
constraints.emplace_back(lhs - rhs);
} else if constexpr (ScalarLike<LHS> && MatrixLike<RHS>) {
int rows;
int cols;
if constexpr (EigenMatrixLike<RHS>) {
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<LHS> && ScalarLike<RHS>) {
int rows;
int cols;
if constexpr (EigenMatrixLike<LHS>) {
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<LHS> && MatrixLike<RHS>) {
int lhsRows;
int lhsCols;
if constexpr (EigenMatrixLike<LHS>) {
lhsRows = lhs.rows();
lhsCols = lhs.cols();
} else {
lhsRows = lhs.Rows();
lhsCols = lhs.Cols();
}
[[maybe_unused]]
int rhsRows;
[[maybe_unused]]
int rhsCols;
if constexpr (EigenMatrixLike<RHS>) {
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<Variable> 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 <typename LHS, typename RHS>
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(!std::same_as<LHS, double> || !std::same_as<RHS, double>)
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<Variable> 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 <typename LHS, typename RHS>
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(!std::same_as<LHS, double> || !std::same_as<RHS, double>)
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 <typename LHS, typename RHS>
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(!std::same_as<LHS, double> || !std::same_as<RHS, double>)
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 <typename LHS, typename RHS>
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(!std::same_as<LHS, double> || !std::same_as<RHS, double>)
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 <typename LHS, typename RHS>
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(!std::same_as<LHS, double> || !std::same_as<RHS, double>)
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 <typename LHS, typename RHS>
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(!std::same_as<LHS, double> || !std::same_as<RHS, double>)
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 <typename LHS, typename RHS>
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(!std::same_as<LHS, double> || !std::same_as<RHS, double>)
InequalityConstraints operator>=(const LHS& lhs, const RHS& rhs) {
return InequalityConstraints{lhs, rhs};
}
} // namespace sleipnir

69
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Multistart.hpp vendored Normal file
View File

@@ -0,0 +1,69 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <algorithm>
#include <future>
#include <span>
#include <vector>
#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 <typename DecisionVariables>
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 <typename DecisionVariables>
MultistartResult<DecisionVariables> Multistart(
function_ref<MultistartResult<DecisionVariables>(const DecisionVariables&)>
solve,
std::span<const DecisionVariables> initialGuesses) {
std::vector<std::future<MultistartResult<DecisionVariables>>> futures;
for (const auto& initialGuess : initialGuesses) {
futures.emplace_back(std::async(std::launch::async, solve, initialGuess));
}
std::vector<MultistartResult<DecisionVariables>> 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

553
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp vendored Normal file
View File

@@ -0,0 +1,553 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <algorithm>
#include <array>
#include <concepts>
#include <functional>
#include <iterator>
#include <optional>
#include <type_traits>
#include <utility>
#include <vector>
#include <Eigen/Core>
#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 <Eigen/Core>
* #include <sleipnir/optimization/OptimizationProblem.hpp>
*
* 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<double>(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<double, 2, 1>{{0.0}, {0.0}});
* problem.SubjectTo(
* X.Col(N + 1) == Eigen::Matrix<double, 2, 1>{{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:
*
* <ul>
* <li>none (i.e., there is no cost function or are no constraints)</li>
* <li>constant</li>
* <li>linear</li>
* <li>quadratic</li>
* <li>nonlinear</li>
* </ul>
*
* 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:
*
* <ul>
* <li>the Hessian of a quadratic or lower cost function</li>
* <li>the Jacobian of linear or lower constraints</li>
* </ul>
*
* A problem is constant if:
*
* <ul>
* <li>the cost function is constant or lower</li>
* <li>the equality constraints are constant or lower</li>
* <li>the inequality constraints are constant or lower</li>
* </ul>
*
* A problem is linear if:
*
* <ul>
* <li>the cost function is linear</li>
* <li>the equality constraints are linear or lower</li>
* <li>the inequality constraints are linear or lower</li>
* </ul>
*
* A problem is quadratic if:
*
* <ul>
* <li>the cost function is quadratic</li>
* <li>the equality constraints are linear or lower</li>
* <li>the inequality constraints are linear or lower</li>
* </ul>
*
* 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<int>(status.costFunctionType)]);
sleipnir::println(
"The equality constraints are {}.",
kExprTypeToName[static_cast<int>(status.equalityConstraintType)]);
sleipnir::println(
"The inequality constraints are {}.",
kExprTypeToName[static_cast<int>(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 <typename F>
requires std::invocable<F, const SolverIterationInfo&> &&
std::same_as<std::invoke_result_t<F, const SolverIterationInfo&>,
void>
void Callback(F&& callback) {
m_callback = [=, callback = std::forward<F>(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 <typename F>
requires std::invocable<F, const SolverIterationInfo&> &&
std::same_as<std::invoke_result_t<F, const SolverIterationInfo&>,
bool>
void Callback(F&& callback) {
m_callback = std::forward<F>(callback);
}
private:
// The list of decision variables, which are the root of the problem's
// expression tree
std::vector<Variable> m_decisionVariables;
// The cost function: f(x)
std::optional<Variable> m_f;
// The list of equality constraints: cₑ(x) = 0
std::vector<Variable> m_equalityConstraints;
// The list of inequality constraints: cᵢ(x) ≥ 0
std::vector<Variable> m_inequalityConstraints;
// The user callback
std::function<bool(const SolverIterationInfo&)> m_callback =
[](const SolverIterationInfo&) { return false; };
// The solver status
SolverStatus status;
};
} // namespace sleipnir

54
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverConfig.hpp vendored Normal file
View File

@@ -0,0 +1,54 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <chrono>
#include <limits>
#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<double> timeout{
std::numeric_limits<double>::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

78
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp vendored Normal file
View File

@@ -0,0 +1,78 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <stdint.h>
#include <string_view>
#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

36
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverIterationInfo.hpp vendored Normal file
View File

@@ -0,0 +1,36 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <Eigen/Core>
#include <Eigen/SparseCore>
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<double>& g;
/// The Hessian of the Lagrangian.
const Eigen::SparseMatrix<double>& H;
/// The equality constraint Jacobian.
const Eigen::SparseMatrix<double>& A_e;
/// The inequality constraint Jacobian.
const Eigen::SparseMatrix<double>& A_i;
};
} // namespace sleipnir

32
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverStatus.hpp vendored Normal file
View File

@@ -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

55
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/InteriorPoint.hpp vendored Normal file
View File

@@ -0,0 +1,55 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <span>
#include <Eigen/Core>
#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<Variable> decisionVariables,
std::span<Variable> equalityConstraints,
std::span<Variable> inequalityConstraints, Variable& f,
function_ref<bool(const SolverIterationInfo&)> callback,
const SolverConfig& config, bool feasibilityRestoration, Eigen::VectorXd& x,
Eigen::VectorXd& s, SolverStatus* status);
} // namespace sleipnir

26
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Assert.hpp vendored Normal file
View File

@@ -0,0 +1,26 @@
// Copyright (c) Sleipnir contributors
#pragma once
#ifdef JORMUNGANDR
#include <stdexcept>
#include <fmt/format.h>
/**
* 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 <cassert>
/**
* Abort in C++.
*/
#define Assert(condition) assert(condition)
#endif

30
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Concepts.hpp vendored Normal file
View File

@@ -0,0 +1,30 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <concepts>
#include <Eigen/Core>
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/autodiff/VariableMatrix.hpp"
namespace sleipnir {
template <typename T>
concept ScalarLike = std::same_as<T, double> || std::same_as<T, int> ||
std::same_as<T, Variable>;
template <typename Derived>
concept EigenMatrixLike =
std::derived_from<Derived, Eigen::MatrixBase<Derived>>;
template <typename T>
concept EigenSolver = requires(T t) { t.solve(Eigen::VectorXd{}); };
template <typename T>
concept MatrixLike =
std::same_as<T, VariableMatrix> ||
std::same_as<T, VariableBlock<VariableMatrix>> || EigenMatrixLike<T>;
} // namespace sleipnir

100
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/FunctionRef.hpp vendored Normal file
View File

@@ -0,0 +1,100 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <functional>
#include <memory>
#include <type_traits>
#include <utility>
namespace sleipnir {
/**
* An implementation of std::function_ref, a lightweight non-owning reference to
* a callable.
*/
template <class F>
class function_ref;
template <class R, class... Args>
class function_ref<R(Args...)> {
public:
constexpr function_ref() noexcept = delete;
/**
* Creates a `function_ref` which refers to the same callable as `rhs`.
*/
constexpr function_ref(const function_ref<R(Args...)>& rhs) noexcept =
default;
/**
* Constructs a `function_ref` referring to `f`.
*/
template <typename F>
requires(!std::is_same_v<std::decay_t<F>, function_ref> &&
std::is_invocable_r_v<R, F &&, Args...>)
constexpr function_ref(F&& f) noexcept // NOLINT(google-explicit-constructor)
: obj_(const_cast<void*>(
reinterpret_cast<const void*>(std::addressof(f)))) {
callback_ = [](void* obj, Args... args) -> R {
return std::invoke(
*reinterpret_cast<typename std::add_pointer<F>::type>(obj),
std::forward<Args>(args)...);
};
}
/**
* Makes `*this` refer to the same callable as `rhs`.
*/
constexpr function_ref<R(Args...)>& operator=(
const function_ref<R(Args...)>& rhs) noexcept = default;
/**
* Makes `*this` refer to `f`.
*/
template <typename F>
requires std::is_invocable_r_v<R, F&&, Args...>
constexpr function_ref<R(Args...)>& operator=(F&& f) noexcept {
obj_ = reinterpret_cast<void*>(std::addressof(f));
callback_ = [](void* obj, Args... args) {
return std::invoke(
*reinterpret_cast<typename std::add_pointer<F>::type>(obj),
std::forward<Args>(args)...);
};
return *this;
}
/**
* Swaps the referred callables of `*this` and `rhs`.
*/
constexpr void swap(function_ref<R(Args...)>& 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>(args)...);
}
private:
void* obj_ = nullptr;
R (*callback_)(void*, Args...) = nullptr;
};
/**
* Swaps the referred callables of `lhs` and `rhs`.
*/
template <typename R, typename... Args>
constexpr void swap(function_ref<R(Args...)>& lhs,
function_ref<R(Args...)>& rhs) noexcept {
lhs.swap(rhs);
}
template <typename R, typename... Args>
function_ref(R (*)(Args...)) -> function_ref<R(Args...)>;
} // namespace sleipnir

216
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/IntrusiveSharedPtr.hpp vendored Normal file
View File

@@ -0,0 +1,216 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <cstddef>
#include <memory>
#include <utility>
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 <typename T>
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<T>& 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<T>& operator=( // NOLINT
const IntrusiveSharedPtr<T>& 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<T>&& rhs) noexcept
: m_ptr{std::exchange(rhs.m_ptr, nullptr)} {}
/**
* Move assigns from the given intrusive shared pointer.
*/
constexpr IntrusiveSharedPtr<T>& operator=(
IntrusiveSharedPtr<T>&& 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<T>& lhs,
const IntrusiveSharedPtr<T>& 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<T>& lhs,
const IntrusiveSharedPtr<T>& 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<T>& 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<T>& 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<T>& 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<T>& 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 <typename T, typename... Args>
IntrusiveSharedPtr<T> MakeIntrusiveShared(Args&&... args) {
return IntrusiveSharedPtr<T>{new T(std::forward<Args>(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 <typename T, typename Alloc, typename... Args>
IntrusiveSharedPtr<T> AllocateIntrusiveShared(Alloc alloc, Args&&... args) {
auto ptr = std::allocator_traits<Alloc>::allocate(alloc, sizeof(T));
std::allocator_traits<Alloc>::construct(alloc, ptr,
std::forward<Args>(args)...);
return IntrusiveSharedPtr<T>{ptr};
}
} // namespace sleipnir

159
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Pool.hpp vendored Normal file
View File

@@ -0,0 +1,159 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <cstddef>
#include <memory>
#include <vector>
#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<std::unique_ptr<std::byte[]>> m_buffer;
std::vector<void*> 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 <typename T>
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<T>& other) = default;
constexpr PoolAllocator<T>& operator=(const PoolAllocator<T>&) = 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<T*>(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 <typename T>
PoolAllocator<T> GlobalPoolAllocator() {
return PoolAllocator<T>{&GlobalPoolResource()};
}
} // namespace sleipnir

56
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Print.hpp vendored Normal file
View File

@@ -0,0 +1,56 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <system_error>
#include <utility>
#include <fmt/core.h>
namespace sleipnir {
/**
* Wrapper around fmt::print() that squelches write failure exceptions.
*/
template <typename... T>
inline void print(fmt::format_string<T...> fmt, T&&... args) {
try {
fmt::print(fmt, std::forward<T>(args)...);
} catch (const std::system_error&) {
}
}
/**
* Wrapper around fmt::print() that squelches write failure exceptions.
*/
template <typename... T>
inline void print(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
try {
fmt::print(f, fmt, std::forward<T>(args)...);
} catch (const std::system_error&) {
}
}
/**
* Wrapper around fmt::println() that squelches write failure exceptions.
*/
template <typename... T>
inline void println(fmt::format_string<T...> fmt, T&&... args) {
try {
fmt::println(fmt, std::forward<T>(args)...);
} catch (const std::system_error&) {
}
}
/**
* Wrapper around fmt::println() that squelches write failure exceptions.
*/
template <typename... T>
inline void println(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
try {
fmt::println(f, fmt, std::forward<T>(args)...);
} catch (const std::system_error&) {
}
}
} // namespace sleipnir

89
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Spy.hpp vendored Normal file
View File

@@ -0,0 +1,89 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <fstream>
#include <string>
#include <string_view>
#include <vector>
#include <Eigen/SparseCore>
#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<double>& mat) {
const int cells_width = mat.cols() + 1;
const int cells_height = mat.rows();
std::vector<uint8_t> 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<double>::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<double>& mat) {
std::ofstream file{std::string{filename}};
if (!file.is_open()) {
return;
}
Spy(file, mat);
}
} // namespace sleipnir

192
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/SymbolExports.hpp vendored Normal file
View File

@@ -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<bar>;
//
// In a source file, write:
//
// template class EXPORT_TEMPLATE_DEFINE(FOO_EXPORT) foo<bar>;
// 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

12
wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide vendored Normal file
View File

@@ -0,0 +1,12 @@
cppHeaderFileInclude {
\.hpp$
}
cppSrcFileInclude {
\.cpp$
}
includeOtherLibs {
^Eigen/
^fmt/
}

58
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/Inertia.hpp vendored Normal file
View File

@@ -0,0 +1,58 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <cstddef>
#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 <EigenSolver Solver>
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

179
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/RegularizedLDLT.hpp vendored Normal file
View File

@@ -0,0 +1,179 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <cmath>
#include <cstddef>
#include <Eigen/Core>
#include <Eigen/SparseCholesky>
#include <Eigen/SparseCore>
#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::SparseMatrix<double>,
Eigen::Lower, Eigen::AMDOrdering<int>>;
/**
* 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<double>& 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<double> 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 <typename Rhs>
auto Solve(const Eigen::MatrixBase<Rhs>& 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<double>& 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<double> 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<double>{vec.asDiagonal()};
}
};
} // namespace sleipnir

876
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp vendored Normal file
View File

@@ -0,0 +1,876 @@
// Copyright (c) Sleipnir contributors
#include "sleipnir/optimization/solver/InteriorPoint.hpp"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <fstream>
#include <limits>
#include <vector>
#include <Eigen/SparseCholesky>
#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<Variable> decisionVariables,
std::span<Variable> equalityConstraints,
std::span<Variable> inequalityConstraints, Variable& f,
function_ref<bool(const SolverIterationInfo&)> 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<double> A_e = jacobianCe.Value();
// Inequality constraint Jacobian Aᵢ
//
// [∇ᵀcᵢ₁(xₖ)]
// Aᵢ(x) = [∇ᵀcᵢ₂(xₖ)]
// [ ⋮ ]
// [∇ᵀcᵢₘ(xₖ)]
Jacobian jacobianCi{c_iAD, xAD};
Eigen::SparseMatrix<double> A_i = jacobianCi.Value();
// Gradient of f ∇f
Gradient gradientF{f, xAD};
Eigen::SparseVector<double> g = gradientF.Value();
// Hessian of the Lagrangian H
//
// Hₖ = ∇²ₓₓL(xₖ, sₖ, yₖ, zₖ)
Hessian hessianL{L, xAD};
Eigen::SparseMatrix<double> 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<Eigen::Triplet<double>> 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<double>::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<Eigen::Infinity>() > 1e20 || !x.allFinite() ||
s.lpNorm<Eigen::Infinity>() > 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<double> Sinv;
Sinv = s.cwiseInverse().asDiagonal();
// [z₁ 0 ⋯ 0 ]
// Z = [0 ⋱ ⋮ ]
// [⋮ ⋱ 0 ]
// [0 ⋯ 0 zₘ]
const auto Z = z.asDiagonal();
Eigen::SparseMatrix<double> Zinv;
Zinv = z.cwiseInverse().asDiagonal();
// Σ = S⁻¹Z
const Eigen::SparseMatrix<double> Σ = Sinv * Z;
// lhs = [H + AᵢᵀΣAᵢ Aₑᵀ]
// [ Aₑ 0 ]
//
// Don't assign upper triangle because solver only uses lower triangle.
const Eigen::SparseMatrix<double> topLeft =
H.triangularView<Eigen::Lower>() +
(A_i.transpose() * Σ * A_i).triangularView<Eigen::Lower>();
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<double>::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<double>::InnerIterator it{A_e, col}; it; ++it) {
triplets.emplace_back(H.rows() + it.row(), it.col(), it.value());
}
}
Eigen::SparseMatrix<double> 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<double>::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<double>::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<double> 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<double> 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<Eigen::SparseMatrix<double>> 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<double>::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

80
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp vendored Normal file
View File

@@ -0,0 +1,80 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/SparseCore>
// 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<double>& A_e,
const Eigen::VectorXd& c_e,
const Eigen::SparseMatrix<double>& 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<Eigen::Infinity>() /
s_d,
(S * z - μ * e).lpNorm<Eigen::Infinity>() / s_c,
c_e.lpNorm<Eigen::Infinity>(),
(c_i - s).lpNorm<Eigen::Infinity>()});
}
} // namespace sleipnir

239
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp vendored Normal file
View File

@@ -0,0 +1,239 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <algorithm>
#include <cmath>
#include <iterator>
#include <span>
#include <vector>
#include <Eigen/Core>
#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<Variable> decisionVariables,
std::span<Variable> equalityConstraints,
std::span<Variable> inequalityConstraints, Variable& f, double μ,
function_ref<bool(const SolverIterationInfo&)> 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<Variable> 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<Variable> 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<Variable> 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

188
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp vendored Normal file
View File

@@ -0,0 +1,188 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <algorithm>
#include <cmath>
#include <limits>
#include <utility>
#include <vector>
#include <Eigen/Core>
#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<double>::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<double>::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<FilterEntry> m_filter;
};
} // namespace sleipnir

45
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FractionToTheBoundaryRule.hpp vendored Normal file
View File

@@ -0,0 +1,45 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <Eigen/Core>
// 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<const Eigen::VectorXd>& x,
const Eigen::Ref<const Eigen::VectorXd>& 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

63
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/IsLocallyInfeasible.hpp vendored Normal file
View File

@@ -0,0 +1,63 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <Eigen/Core>
#include <Eigen/SparseCore>
// 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<double>& 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<double>& 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

51
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp vendored Normal file
View File

@@ -0,0 +1,51 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <Eigen/Core>
#include <Eigen/SparseCore>
// 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<double>& A_e,
const Eigen::VectorXd& c_e,
const Eigen::SparseMatrix<double>& 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

12
wpimath/src/main/native/thirdparty/sleipnir/src/util/Pool.cpp vendored Normal file
View File

@@ -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

35
wpimath/src/main/native/thirdparty/sleipnir/src/util/ScopeExit.hpp vendored Normal file
View File

@@ -0,0 +1,35 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <utility>
namespace sleipnir {
template <typename F>
class scope_exit {
public:
explicit scope_exit(F&& f) noexcept : m_f{std::forward<F>(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

21
wpimath/src/main/native/thirdparty/sleipnir/src/util/ToMilliseconds.hpp vendored Normal file
View File

@@ -0,0 +1,21 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <chrono>
namespace sleipnir {
/**
* Converts std::chrono::duration to a number of milliseconds rounded to three
* decimals.
*/
template <typename Rep, typename Period = std::ratio<1>>
constexpr double ToMilliseconds(
const std::chrono::duration<Rep, Period>& duration) {
using std::chrono::duration_cast;
using std::chrono::microseconds;
return duration_cast<microseconds>(duration).count() / 1e3;
}
} // namespace sleipnir

26
wpimath/src/test/native/cpp/SleipnirTest.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#include <sleipnir/optimization/OptimizationProblem.hpp>
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);
}