Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-27 02:01:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[upstream_utils] Upgrade Sleipnir to fix unused parameter warnings (#6803)

Browse Source
This commit is contained in:
Tyler Veness
2024-07-07 06:43:44 -07:00
committed by GitHub
parent 32ed9c5f0b
commit 6478ba6e3f
20 changed files with 342 additions and 212 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp vendored
View File

@@ -226,15 +226,15 @@ struct SLEIPNIR_DLLEXPORT Expression {
return MakeExpressionPtr(
type, [](double lhs, double rhs) { return lhs * rhs; },
[](double lhs, double rhs, double parentAdjoint) {
[](double, double rhs, double parentAdjoint) {
return parentAdjoint * rhs;
},
[](double lhs, double rhs, double parentAdjoint) {
[](double lhs, double, double parentAdjoint) {
return parentAdjoint * lhs;
},
[](const ExpressionPtr& lhs, const ExpressionPtr& rhs,
[](const ExpressionPtr&, const ExpressionPtr& rhs,
const ExpressionPtr& parentAdjoint) { return parentAdjoint * rhs; },
[](const ExpressionPtr& lhs, const ExpressionPtr& rhs,
[](const ExpressionPtr& lhs, const ExpressionPtr&,
const ExpressionPtr& parentAdjoint) { return parentAdjoint * lhs; },
lhs, rhs);
}
@@ -271,13 +271,13 @@ struct SLEIPNIR_DLLEXPORT Expression {
return MakeExpressionPtr(
type, [](double lhs, double rhs) { return lhs / rhs; },
[](double lhs, double rhs, double parentAdjoint) {
[](double, double rhs, double parentAdjoint) {
return parentAdjoint / rhs;
},
[](double lhs, double rhs, double parentAdjoint) {
return parentAdjoint * -lhs / (rhs * rhs);
},
[](const ExpressionPtr& lhs, const ExpressionPtr& rhs,
[](const ExpressionPtr&, const ExpressionPtr& rhs,
const ExpressionPtr& parentAdjoint) { return parentAdjoint / rhs; },
[](const ExpressionPtr& lhs, const ExpressionPtr& rhs,
const ExpressionPtr& parentAdjoint) {
@@ -310,15 +310,11 @@ struct SLEIPNIR_DLLEXPORT Expression {
return MakeExpressionPtr(
std::max(lhs->type, rhs->type),
[](double lhs, double rhs) { return lhs + rhs; },
[](double lhs, double rhs, double parentAdjoint) {
return parentAdjoint;
},
[](double lhs, double rhs, double parentAdjoint) {
return parentAdjoint;
},
[](const ExpressionPtr& lhs, const ExpressionPtr& rhs,
[](double, double, double parentAdjoint) { return parentAdjoint; },
[](double, double, double parentAdjoint) { return parentAdjoint; },
[](const ExpressionPtr&, const ExpressionPtr&,
const ExpressionPtr& parentAdjoint) { return parentAdjoint; },
[](const ExpressionPtr& lhs, const ExpressionPtr& rhs,
[](const ExpressionPtr&, const ExpressionPtr&,
const ExpressionPtr& parentAdjoint) { return parentAdjoint; },
lhs, rhs);
}
@@ -352,15 +348,11 @@ struct SLEIPNIR_DLLEXPORT Expression {
return MakeExpressionPtr(
std::max(lhs->type, rhs->type),
[](double lhs, double rhs) { return lhs - rhs; },
[](double lhs, double rhs, double parentAdjoint) {
return parentAdjoint;
},
[](double lhs, double rhs, double parentAdjoint) {
return -parentAdjoint;
},
[](const ExpressionPtr& lhs, const ExpressionPtr& rhs,
[](double, double, double parentAdjoint) { return parentAdjoint; },
[](double, double, double parentAdjoint) { return -parentAdjoint; },
[](const ExpressionPtr&, const ExpressionPtr&,
const ExpressionPtr& parentAdjoint) { return parentAdjoint; },
[](const ExpressionPtr& lhs, const ExpressionPtr& rhs,
[](const ExpressionPtr&, const ExpressionPtr&,
const ExpressionPtr& parentAdjoint) { return -parentAdjoint; },
lhs, rhs);
}
@@ -384,8 +376,8 @@ struct SLEIPNIR_DLLEXPORT Expression {
return MakeExpressionPtr(
lhs->type, [](double lhs, double) { return -lhs; },
[](double lhs, double, double parentAdjoint) { return -parentAdjoint; },
[](const ExpressionPtr& lhs, const ExpressionPtr& rhs,
[](double, double, double parentAdjoint) { return -parentAdjoint; },
[](const ExpressionPtr&, const ExpressionPtr&,
const ExpressionPtr& parentAdjoint) { return -parentAdjoint; },
lhs);
}
@@ -940,9 +932,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) {
return 1.0;
}
},
[](double x, double, double parentAdjoint) { return 0.0; },
[](const ExpressionPtr& x, const ExpressionPtr&,
const ExpressionPtr& parentAdjoint) {
[](double, double, double) { return 0.0; },
[](const ExpressionPtr&, const ExpressionPtr&, const ExpressionPtr&) {
// Return zero
return MakeExpressionPtr();
},

2
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionGraph.hpp vendored
View File

@@ -193,7 +193,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph {
* @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) {
void ComputeAdjoints(function_ref<void(int row, double adjoint)> 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) {

43
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Variable.hpp vendored
View File

@@ -32,13 +32,6 @@ class SLEIPNIR_DLLEXPORT 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.
*
@@ -64,23 +57,12 @@ class SLEIPNIR_DLLEXPORT Variable {
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) {
void SetValue(double value) {
if (expr->IsConstant(0.0)) {
expr = detail::MakeExpressionPtr(value);
} else {
@@ -94,29 +76,6 @@ class SLEIPNIR_DLLEXPORT Variable {
}
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;
}
/**

8
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp vendored
View File

@@ -131,12 +131,10 @@ class VariableBlock {
*
* @param value Value to assign.
*/
VariableBlock<Mat>& SetValue(double value) {
void SetValue(double value) {
Assert(Rows() == 1 && Cols() == 1);
(*this)(0, 0).SetValue(value);
return *this;
}
/**
@@ -165,7 +163,7 @@ class VariableBlock {
*/
template <typename Derived>
requires std::same_as<typename Derived::Scalar, double>
VariableBlock<Mat>& SetValue(const Eigen::MatrixBase<Derived>& values) {
void SetValue(const Eigen::MatrixBase<Derived>& values) {
Assert(Rows() == values.rows());
Assert(Cols() == values.cols());
@@ -174,8 +172,6 @@ class VariableBlock {
(*this)(row, col).SetValue(values(row, col));
}
}
return *this;
}
/**

10
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp vendored
View File

@@ -89,7 +89,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
*
* @param list The nested list of Variables.
*/
VariableMatrix(std::vector<std::vector<double>> list) { // NOLINT
VariableMatrix(const std::vector<std::vector<double>>& list) { // NOLINT
// Get row and column counts for destination matrix
m_rows = list.size();
m_cols = 0;
@@ -116,7 +116,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
*
* @param list The nested list of Variables.
*/
VariableMatrix(std::vector<std::vector<Variable>> list) { // NOLINT
VariableMatrix(const std::vector<std::vector<Variable>>& list) { // NOLINT
// Get row and column counts for destination matrix
m_rows = list.size();
m_cols = 0;
@@ -200,7 +200,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
*/
template <typename Derived>
requires std::same_as<typename Derived::Scalar, double>
VariableMatrix& SetValue(const Eigen::MatrixBase<Derived>& values) {
void SetValue(const Eigen::MatrixBase<Derived>& values) {
Assert(Rows() == values.rows());
Assert(Cols() == values.cols());
@@ -209,8 +209,6 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
(*this)(row, col).SetValue(values(row, col));
}
}
return *this;
}
/**
@@ -979,7 +977,7 @@ SLEIPNIR_DLLEXPORT inline VariableMatrix Block(
* @param list The nested list of blocks.
*/
SLEIPNIR_DLLEXPORT inline VariableMatrix Block(
std::vector<std::vector<VariableMatrix>> list) {
const std::vector<std::vector<VariableMatrix>>& list) {
// Get row and column counts for destination matrix
int rows = 0;
int cols = -1;

102
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/OCPSolver.hpp vendored
View File

@@ -16,18 +16,6 @@
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.
*
@@ -122,15 +110,61 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem {
* @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 dynamics Function representing an explicit or implicit ODE, or a
* discrete state transition function.
* - Explicit: dx/dt = f(x, u, *)
* - Implicit: f([x dx/dt]', u, *) = 0
* - State transition: xₖ₊₁ = f(xₖ, uₖ)
* @param dynamicsType The type of system evolution function.
* @param timestepMethod The timestep method.
* @param method The transcription method.
*/
OCPSolver(
int numStates, int numInputs, std::chrono::duration<double> dt,
int numSteps, DynamicsFunction dynamics,
int numSteps,
function_ref<VariableMatrix(const VariableMatrix& x,
const VariableMatrix& u)>
dynamics,
DynamicsType dynamicsType = DynamicsType::kExplicitODE,
TimestepMethod timestepMethod = TimestepMethod::kFixed,
TranscriptionMethod method = TranscriptionMethod::kDirectTranscription)
: OCPSolver{numStates,
numInputs,
dt,
numSteps,
[=]([[maybe_unused]] const VariableMatrix& t,
const VariableMatrix& x, const VariableMatrix& u,
[[maybe_unused]]
const VariableMatrix& dt) -> VariableMatrix {
return dynamics(x, u);
},
dynamicsType,
timestepMethod,
method} {}
/**
* 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 Function representing an explicit or implicit ODE, or a
* discrete state transition function.
* - Explicit: dx/dt = f(t, x, u, *)
* - Implicit: f(t, [x dx/dt]', u, *) = 0
* - State transition: xₖ₊₁ = f(t, xₖ, uₖ, dt)
* @param dynamicsType The type of system evolution function.
* @param timestepMethod The timestep method.
* @param method The transcription method.
*/
OCPSolver(
int numStates, int numInputs, std::chrono::duration<double> dt,
int numSteps,
function_ref<VariableMatrix(const Variable& t, const VariableMatrix& x,
const VariableMatrix& u, const Variable& dt)>
dynamics,
DynamicsType dynamicsType = DynamicsType::kExplicitODE,
TimestepMethod timestepMethod = TimestepMethod::kFixed,
TranscriptionMethod method = TranscriptionMethod::kDirectTranscription)
@@ -203,6 +237,24 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem {
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(x, u) where x is the state and u is the
* input vector.
*/
void ForEachStep(
const function_ref<void(const VariableMatrix& x, const VariableMatrix& u)>
callback) {
for (int i = 0; i < m_numSteps + 1; ++i) {
auto x = X().Col(i);
auto u = U().Col(i);
callback(x, u);
}
}
/**
* Set the constraint evaluation function. This function is called
* `numSteps+1` times, with the corresponding state and input
@@ -212,8 +264,8 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem {
* 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&)>
const function_ref<void(const Variable& t, const VariableMatrix& x,
const VariableMatrix& u, const Variable& dt)>
callback) {
Variable time = 0.0;
@@ -365,9 +417,9 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem {
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));
SubjectTo(x_end == RK4<const decltype(m_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));
}
@@ -386,8 +438,9 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem {
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);
x_end = RK4<const decltype(m_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);
}
@@ -403,7 +456,10 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem {
TranscriptionMethod m_transcriptionMethod;
DynamicsType m_dynamicsType;
DynamicsFunction m_dynamicsFunction;
function_ref<VariableMatrix(const Variable& t, const VariableMatrix& x,
const VariableMatrix& u, const Variable& dt)>
m_dynamicsFunction;
TimestepMethod m_timestepMethod;

60
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Constraints.hpp vendored
View File

@@ -4,7 +4,9 @@
#include <algorithm>
#include <concepts>
#include <initializer_list>
#include <type_traits>
#include <vector>
#include <wpi/SmallVector.h>
@@ -122,6 +124,34 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints {
/// A vector of scalar equality constraints.
wpi::SmallVector<Variable> constraints;
/**
* Concatenates multiple equality constraints.
*
* @param equalityConstraints The list of EqualityConstraints to concatenate.
*/
EqualityConstraints( // NOLINT
std::initializer_list<EqualityConstraints> equalityConstraints) {
for (const auto& elem : equalityConstraints) {
constraints.insert(constraints.end(), elem.constraints.begin(),
elem.constraints.end());
}
}
/**
* Concatenates multiple equality constraints.
*
* This overload is for Python bindings only.
*
* @param equalityConstraints The list of EqualityConstraints to concatenate.
*/
explicit EqualityConstraints(
const std::vector<EqualityConstraints>& equalityConstraints) {
for (const auto& elem : equalityConstraints) {
constraints.insert(constraints.end(), elem.constraints.begin(),
elem.constraints.end());
}
}
/**
* Constructs an equality constraint from a left and right side.
*
@@ -156,6 +186,36 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints {
/// A vector of scalar inequality constraints.
wpi::SmallVector<Variable> constraints;
/**
* Concatenates multiple inequality constraints.
*
* @param inequalityConstraints The list of InequalityConstraints to
* concatenate.
*/
InequalityConstraints( // NOLINT
std::initializer_list<InequalityConstraints> inequalityConstraints) {
for (const auto& elem : inequalityConstraints) {
constraints.insert(constraints.end(), elem.constraints.begin(),
elem.constraints.end());
}
}
/**
* Concatenates multiple inequality constraints.
*
* This overload is for Python bindings only.
*
* @param inequalityConstraints The list of InequalityConstraints to
* concatenate.
*/
explicit InequalityConstraints(
const std::vector<InequalityConstraints>& inequalityConstraints) {
for (const auto& elem : inequalityConstraints) {
constraints.insert(constraints.end(), elem.constraints.begin(),
elem.constraints.end());
}
}
/**
* Constructs an inequality constraint from a left and right side.
*

3
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Multistart.hpp vendored
View File

@@ -41,7 +41,8 @@ struct MultistartResult {
*/
template <typename DecisionVariables>
MultistartResult<DecisionVariables> Multistart(
function_ref<MultistartResult<DecisionVariables>(const DecisionVariables&)>
function_ref<MultistartResult<DecisionVariables>(
const DecisionVariables& initialGuess)>
solve,
std::span<const DecisionVariables> initialGuesses) {
wpi::SmallVector<std::future<MultistartResult<DecisionVariables>>> futures;

15
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp vendored
View File

@@ -8,7 +8,6 @@
#include <functional>
#include <iterator>
#include <optional>
#include <type_traits>
#include <utility>
#include <Eigen/Core>
@@ -330,9 +329,9 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
* @param callback The callback.
*/
template <typename F>
requires std::invocable<F, const SolverIterationInfo&> &&
std::same_as<std::invoke_result_t<F, const SolverIterationInfo&>,
void>
requires requires(F callback, const SolverIterationInfo& info) {
{ callback(info) } -> std::same_as<void>;
}
void Callback(F&& callback) {
m_callback = [=, callback = std::forward<F>(callback)](
const SolverIterationInfo& info) {
@@ -350,9 +349,9 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
* 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>
requires requires(F callback, const SolverIterationInfo& info) {
{ callback(info) } -> std::same_as<bool>;
}
void Callback(F&& callback) {
m_callback = std::forward<F>(callback);
}
@@ -372,7 +371,7 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
wpi::SmallVector<Variable> m_inequalityConstraints;
// The user callback
std::function<bool(const SolverIterationInfo&)> m_callback =
std::function<bool(const SolverIterationInfo& info)> m_callback =
[](const SolverIterationInfo&) { return false; };
// The solver status

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

@@ -48,7 +48,7 @@ SLEIPNIR_DLLEXPORT void InteriorPoint(
std::span<Variable> decisionVariables,
std::span<Variable> equalityConstraints,
std::span<Variable> inequalityConstraints, Variable& f,
function_ref<bool(const SolverIterationInfo&)> callback,
function_ref<bool(const SolverIterationInfo& info)> callback,
const SolverConfig& config, bool feasibilityRestoration, Eigen::VectorXd& x,
Eigen::VectorXd& s, SolverStatus* status);

8
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Pool.hpp vendored
View File

@@ -40,7 +40,8 @@ class SLEIPNIR_DLLEXPORT PoolResource {
* @param alignment Alignment of the block (unused).
*/
[[nodiscard]]
void* allocate(size_t bytes, size_t alignment = alignof(std::max_align_t)) {
void* allocate(size_t bytes, [[maybe_unused]] size_t alignment =
alignof(std::max_align_t)) {
if (m_freeList.empty()) {
AddChunk(bytes);
}
@@ -57,8 +58,9 @@ class SLEIPNIR_DLLEXPORT PoolResource {
* @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)) {
void deallocate(
void* p, [[maybe_unused]] size_t bytes,
[[maybe_unused]] size_t alignment = alignof(std::max_align_t)) {
m_freeList.emplace_back(p);
}

59
wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/VariableMatrix.cpp vendored
View File

@@ -34,35 +34,72 @@ VariableMatrix Solve(const VariableMatrix& A, const VariableMatrix& B) {
// m x n * n x p = m x p
Assert(A.Rows() == B.Rows());
if (A.Rows() == 2 && A.Cols() == 2) {
if (A.Rows() == 1 && A.Cols() == 1) {
// Compute optimal inverse instead of using Eigen's general solver
sleipnir::VariableMatrix Ainv{2, 2};
Ainv(0, 0) = A(1, 1);
Ainv(0, 1) = -A(0, 1);
Ainv(1, 0) = -A(1, 0);
Ainv(1, 1) = A(0, 0);
auto detA = A(0, 0) * A(1, 1) - A(0, 1) * A(1, 0);
return B(0, 0) / A(0, 0);
} else if (A.Rows() == 2 && A.Cols() == 2) {
// Compute optimal inverse instead of using Eigen's general solver
//
// [a b]⁻¹ ___1___ [ d −b]
// [c d] = ad − bc [−c a]
const auto& a = A(0, 0);
const auto& b = A(0, 1);
const auto& c = A(1, 0);
const auto& d = A(1, 1);
sleipnir::VariableMatrix Ainv{{d, -b}, {-c, a}};
auto detA = a * d - b * c;
Ainv /= detA;
return Ainv * B;
} else if (A.Rows() == 3 && A.Cols() == 3) {
// Compute optimal inverse instead of using Eigen's general solver
//
// [a b c]⁻¹
// [d e f]
// [g h i]
// 1 [ei − fh ch − bi bf − ce]
// = --------------------------------- [fg − di ai − cg cd − af]
// aei − afh − bdi + bfg + cdh − ceg [dh − eg bg − ah ae − bd]
const auto& a = A(0, 0);
const auto& b = A(0, 1);
const auto& c = A(0, 2);
const auto& d = A(1, 0);
const auto& e = A(1, 1);
const auto& f = A(1, 2);
const auto& g = A(2, 0);
const auto& h = A(2, 1);
const auto& i = A(2, 2);
sleipnir::VariableMatrix Ainv{
{e * i - f * h, c * h - b * i, b * f - c * e},
{f * g - d * i, a * i - c * g, c * d - a * f},
{d * h - e * g, b * g - a * h, a * e - b * d}};
auto detA =
a * e * i - a * f * h - b * d * i + b * f * g + c * d * h - c * e * g;
Ainv /= detA;
return Ainv * B;
} else {
using MatrixXvar = Eigen::Matrix<Variable, Eigen::Dynamic, Eigen::Dynamic>;
using MatrixXv = Eigen::Matrix<Variable, Eigen::Dynamic, Eigen::Dynamic>;
MatrixXvar eigenA{A.Rows(), A.Cols()};
MatrixXv eigenA{A.Rows(), A.Cols()};
for (int row = 0; row < A.Rows(); ++row) {
for (int col = 0; col < A.Cols(); ++col) {
eigenA(row, col) = A(row, col);
}
}
MatrixXvar eigenB{B.Rows(), B.Cols()};
MatrixXv eigenB{B.Rows(), B.Cols()};
for (int row = 0; row < B.Rows(); ++row) {
for (int col = 0; col < B.Cols(); ++col) {
eigenB(row, col) = B(row, col);
}
}
MatrixXvar eigenX = eigenA.householderQr().solve(eigenB);
MatrixXv eigenX = eigenA.householderQr().solve(eigenB);
VariableMatrix X{A.Cols(), B.Cols()};
for (int row = 0; row < X.Rows(); ++row) {

8
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp vendored
View File

@@ -37,7 +37,7 @@ namespace sleipnir {
void InteriorPoint(std::span<Variable> decisionVariables,
std::span<Variable> equalityConstraints,
std::span<Variable> inequalityConstraints, Variable& f,
function_ref<bool(const SolverIterationInfo&)> callback,
function_ref<bool(const SolverIterationInfo& info)> callback,
const SolverConfig& config, bool feasibilityRestoration,
Eigen::VectorXd& x, Eigen::VectorXd& s,
SolverStatus* status) {
@@ -354,7 +354,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
decisionVariables.size() + equalityConstraints.size(),
decisionVariables.size() + equalityConstraints.size());
lhs.setFromSortedTriplets(triplets.begin(), triplets.end(),
[](const auto& a, const auto& b) { return b; });
[](const auto&, const auto& b) { return b; });
const Eigen::VectorXd e = Eigen::VectorXd::Ones(s.rows());
@@ -582,7 +582,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
Eigen::VectorXd fr_s = s;
SolverStatus fr_status;
FeasibilityRestoration(
decisionVariables, equalityConstraints, inequalityConstraints, f, μ,
decisionVariables, equalityConstraints, inequalityConstraints, μ,
[&](const SolverIterationInfo& info) {
Eigen::VectorXd trial_x =
info.x.segment(0, decisionVariables.size());
@@ -656,7 +656,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
A_e.cols() + s.rows()};
Ahat.setFromSortedTriplets(
triplets.begin(), triplets.end(),
[](const auto& a, const auto& b) { return b; });
[](const auto&, const auto& b) { return b; });
// lhs = ÂÂᵀ
Eigen::SparseMatrix<double> lhs = Ahat * Ahat.transpose();

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

@@ -28,7 +28,6 @@ namespace sleipnir {
* @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.
@@ -40,8 +39,8 @@ namespace sleipnir {
inline void FeasibilityRestoration(
std::span<Variable> decisionVariables,
std::span<Variable> equalityConstraints,
std::span<Variable> inequalityConstraints, Variable& f, double μ,
function_ref<bool(const SolverIterationInfo&)> callback,
std::span<Variable> inequalityConstraints, double μ,
function_ref<bool(const SolverIterationInfo& info)> callback,
const SolverConfig& config, Eigen::VectorXd& x, Eigen::VectorXd& s,
SolverStatus* status) {
// Feasibility restoration