Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +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);
}