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

[upstream_utils] Upgrade Sleipnir (#8235)

Browse Source
This commit is contained in:
Tyler Veness
2025-09-20 11:21:06 -07:00
committed by GitHub
parent ee3d55e848
commit 3dbdfa1839
28 changed files with 403 additions and 260 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/adjoint_expression_graph.hpp vendored
View File

@@ -11,6 +11,7 @@
#include "sleipnir/autodiff/expression_graph.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/assert.hpp"
namespace slp::detail {
@@ -50,6 +51,8 @@ class AdjointExpressionGraph {
* @return The variable's gradient tree.
*/
VariableMatrix generate_gradient_tree(const VariableMatrix& wrt) const {
slp_assert(wrt.cols() == 1);
// Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation
// for background on reverse accumulation automatic differentiation.

63
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp vendored
View File

@@ -422,6 +422,12 @@ struct Expression {
}
};
inline ExpressionPtr cbrt(const ExpressionPtr& x);
inline ExpressionPtr exp(const ExpressionPtr& x);
inline ExpressionPtr sin(const ExpressionPtr& x);
inline ExpressionPtr sinh(const ExpressionPtr& x);
inline ExpressionPtr sqrt(const ExpressionPtr& x);
/**
* Derived expression type for binary minus operator.
*
@@ -504,6 +510,58 @@ struct BinaryPlusExpression final : Expression {
}
};
/**
* Derived expression type for std::cbrt().
*/
struct CbrtExpression final : Expression {
/**
* Constructs an unary expression (an operator with one argument).
*
* @param lhs Unary operator's operand.
*/
explicit constexpr CbrtExpression(ExpressionPtr lhs)
: Expression{std::move(lhs)} {}
double value(double x, double) const override { return std::cbrt(x); }
ExpressionType type() const override { return ExpressionType::NONLINEAR; }
double grad_l(double x, double, double parent_adjoint) const override {
double c = std::cbrt(x);
return parent_adjoint / (3.0 * c * c);
}
ExpressionPtr grad_expr_l(
const ExpressionPtr& x, const ExpressionPtr&,
const ExpressionPtr& parent_adjoint) const override {
auto c = slp::detail::cbrt(x);
return parent_adjoint / (make_expression_ptr<ConstExpression>(3.0) * c * c);
}
};
/**
* std::cbrt() for Expressions.
*
* @param x The argument.
*/
inline ExpressionPtr cbrt(const ExpressionPtr& x) {
using enum ExpressionType;
// Evaluate constant
if (x->type() == CONSTANT) {
if (x->val == 0.0) {
// Return zero
return x;
} else if (x->val == -1.0 || x->val == 1.0) {
return x;
} else {
return make_expression_ptr<ConstExpression>(std::cbrt(x->val));
}
}
return make_expression_ptr<CbrtExpression>(x);
}
/**
* Derived expression type for constant.
*/
@@ -661,11 +719,6 @@ struct UnaryMinusExpression final : Expression {
}
};
inline ExpressionPtr exp(const ExpressionPtr& x);
inline ExpressionPtr sin(const ExpressionPtr& x);
inline ExpressionPtr sinh(const ExpressionPtr& x);
inline ExpressionPtr sqrt(const ExpressionPtr& x);
/**
* Refcount increment for intrusive shared pointer.
*

3
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp vendored
View File

@@ -21,8 +21,7 @@ inline gch::small_vector<Expression*> topological_sort(
const ExpressionPtr& root) {
gch::small_vector<Expression*> list;
// If the root type is a constant, Update() is a no-op, so there's no work
// to do
// If the root type is constant, updates are a no-op, so return an empty list
if (root == nullptr || root->type() == ExpressionType::CONSTANT) {
return list;
}

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

@@ -15,8 +15,8 @@
namespace slp {
/**
* This class calculates the gradient of a a variable with respect to a vector
* of variables.
* This class calculates the gradient of a variable with respect to a vector of
* variables.
*
* The gradient is only recomputed if the variable expression is quadratic or
* higher order.
@@ -29,7 +29,7 @@ class SLEIPNIR_DLLEXPORT Gradient {
* @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(Variable variable, Variable wrt)
: m_jacobian{std::move(variable), std::move(wrt)} {}
/**
@@ -39,7 +39,7 @@ class SLEIPNIR_DLLEXPORT Gradient {
* @param wrt Vector of variables with respect to which to compute the
* gradient.
*/
Gradient(Variable variable, SleipnirMatrixLike auto wrt) noexcept
Gradient(Variable variable, SleipnirMatrixLike auto wrt)
: m_jacobian{VariableMatrix{std::move(variable)}, std::move(wrt)} {}
/**
@@ -58,7 +58,7 @@ class SLEIPNIR_DLLEXPORT Gradient {
* @return The gradient at wrt's value.
*/
const Eigen::SparseVector<double>& value() {
m_g = m_jacobian.value();
m_g = m_jacobian.value().transpose();
return m_g;
}

19
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp vendored
View File

@@ -10,6 +10,7 @@
#include "sleipnir/autodiff/adjoint_expression_graph.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/concepts.hpp"
#include "sleipnir/util/symbol_exports.hpp"
@@ -34,7 +35,7 @@ class SLEIPNIR_DLLEXPORT Hessian {
* @param variable Variable of which to compute the Hessian.
* @param wrt Variable with respect to which to compute the Hessian.
*/
Hessian(Variable variable, Variable wrt) noexcept
Hessian(Variable variable, Variable wrt)
: Hessian{std::move(variable), VariableMatrix{std::move(wrt)}} {}
/**
@@ -44,10 +45,12 @@ class SLEIPNIR_DLLEXPORT Hessian {
* @param wrt Vector of variables with respect to which to compute the
* Hessian.
*/
Hessian(Variable variable, SleipnirMatrixLike auto wrt) noexcept
Hessian(Variable variable, SleipnirMatrixLike auto wrt)
: m_variables{detail::AdjointExpressionGraph{variable}
.generate_gradient_tree(wrt)},
m_wrt{wrt} {
slp_assert(m_wrt.cols() == 1);
// Initialize column each expression's adjoint occupies in the Jacobian
for (size_t col = 0; col < m_wrt.size(); ++col) {
m_wrt[col].expr->col = col;
@@ -136,15 +139,9 @@ class SLEIPNIR_DLLEXPORT Hessian {
m_graphs[row].append_adjoint_triplets(triplets, row, m_wrt);
}
if (!triplets.empty()) {
m_H.setFromTriplets(triplets.begin(), triplets.end());
if constexpr (UpLo == Eigen::Lower) {
m_H = m_H.triangularView<Eigen::Lower>();
}
} else {
// setFromTriplets() is a no-op on empty triplets, so explicitly zero out
// the storage
m_H.setZero();
m_H.setFromTriplets(triplets.begin(), triplets.end());
if constexpr (UpLo == Eigen::Lower) {
m_H = m_H.triangularView<Eigen::Lower>();
}
return m_H;

16
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp vendored
View File

@@ -10,6 +10,7 @@
#include "sleipnir/autodiff/adjoint_expression_graph.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/concepts.hpp"
#include "sleipnir/util/symbol_exports.hpp"
@@ -30,7 +31,7 @@ class SLEIPNIR_DLLEXPORT Jacobian {
* @param variable Variable of which to compute the Jacobian.
* @param wrt Variable with respect to which to compute the Jacobian.
*/
Jacobian(Variable variable, Variable wrt) noexcept
Jacobian(Variable variable, Variable wrt)
: Jacobian{VariableMatrix{std::move(variable)},
VariableMatrix{std::move(wrt)}} {}
@@ -41,8 +42,11 @@ class SLEIPNIR_DLLEXPORT Jacobian {
* @param wrt Vector of variables with respect to which to compute the
* Jacobian.
*/
Jacobian(VariableMatrix variables, SleipnirMatrixLike auto wrt) noexcept
Jacobian(VariableMatrix variables, SleipnirMatrixLike auto wrt)
: m_variables{std::move(variables)}, m_wrt{std::move(wrt)} {
slp_assert(m_variables.cols() == 1);
slp_assert(m_wrt.cols() == 1);
// Initialize column each expression's adjoint occupies in the Jacobian
for (size_t col = 0; col < m_wrt.size(); ++col) {
m_wrt[col].expr->col = col;
@@ -128,13 +132,7 @@ class SLEIPNIR_DLLEXPORT Jacobian {
m_graphs[row].append_adjoint_triplets(triplets, row, m_wrt);
}
if (!triplets.empty()) {
m_J.setFromTriplets(triplets.begin(), triplets.end());
} else {
// setFromTriplets() is a no-op on empty triplets, so explicitly zero out
// the storage
m_J.setZero();
}
m_J.setFromTriplets(triplets.begin(), triplets.end());
return m_J;
}

35
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp vendored
View File

@@ -87,6 +87,7 @@ class SLEIPNIR_DLLEXPORT Variable {
*/
Variable& operator=(double value) {
expr = detail::make_expression_ptr<detail::ConstExpression>(value);
m_graph_initialized = false;
return *this;
}
@@ -97,22 +98,18 @@ class SLEIPNIR_DLLEXPORT Variable {
* @param value The value of the Variable.
*/
void set_value(double value) {
if (expr->is_constant(0.0)) {
expr = detail::make_expression_ptr<detail::ConstExpression>(value);
} else {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
// We only need to check the first argument since unary and binary
// operators both use it
if (expr->args[0] != nullptr) {
auto location = std::source_location::current();
slp::println(
stderr,
"WARNING: {}:{}: {}: Modified the value of a dependent variable",
location.file_name(), location.line(), location.function_name());
}
#endif
expr->val = value;
// We only need to check the first argument since unary and binary operators
// both use it
if (expr->args[0] != nullptr) {
auto location = std::source_location::current();
slp::println(
stderr,
"WARNING: {}:{}: {}: Modified the value of a dependent variable",
location.file_name(), location.line(), location.function_name());
}
#endif
expr->val = value;
}
/**
@@ -266,6 +263,7 @@ class SLEIPNIR_DLLEXPORT Variable {
friend SLEIPNIR_DLLEXPORT Variable atan(const Variable& x);
friend SLEIPNIR_DLLEXPORT Variable atan2(const Variable& y,
const Variable& x);
friend SLEIPNIR_DLLEXPORT Variable cbrt(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);
@@ -338,6 +336,15 @@ SLEIPNIR_DLLEXPORT inline Variable atan2(const Variable& y, const Variable& x) {
return Variable{detail::atan2(y.expr, x.expr)};
}
/**
* std::cbrt() for Variables.
*
* @param x The argument.
*/
SLEIPNIR_DLLEXPORT inline Variable cbrt(const Variable& x) {
return Variable{detail::cbrt(x.expr)};
}
/**
* std::cos() for Variables.
*

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

@@ -1149,7 +1149,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
SLEIPNIR_DLLEXPORT inline VariableMatrix cwise_reduce(
const VariableMatrix& lhs, const VariableMatrix& rhs,
function_ref<Variable(const Variable& x, const Variable& y)> binary_op) {
slp_assert(lhs.rows() == rhs.rows() && lhs.rows() == rhs.rows());
slp_assert(lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols());
VariableMatrix result{VariableMatrix::empty, lhs.rows(), lhs.cols()};

189
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/ocp.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp.hpp vendored
View File

@@ -8,6 +8,9 @@
#include <utility>
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/optimization/ocp/dynamics_type.hpp"
#include "sleipnir/optimization/ocp/timestep_method.hpp"
#include "sleipnir/optimization/ocp/transcription_method.hpp"
#include "sleipnir/optimization/problem.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/concepts.hpp"
@@ -16,64 +19,6 @@
namespace slp {
/**
* 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.
DIRECT_TRANSCRIPTION,
/// The trajectory is modeled as a series of cubic polynomials where the
/// centerpoint slope is constrained.
DIRECT_COLLOCATION,
/// States depend explicitly as a function of all previous states and all
/// previous inputs.
SINGLE_SHOOTING
};
/**
* 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).
EXPLICIT_ODE,
/// The dynamics are a function in the form xₖ₊₁ = f(t, xₖ, uₖ).
DISCRETE
};
/**
* Enum describing the type of system timestep.
*/
enum class TimestepMethod : uint8_t {
/// The timestep is a fixed constant.
FIXED,
/// The timesteps are allowed to vary as independent decision variables.
VARIABLE,
/// The timesteps are equal length but allowed to vary as a single decision
/// variable.
VARIABLE_SINGLE
};
/**
* This class allows the user to pose and solve a constrained optimal control
* problem (OCP) in a variety of ways.
@@ -117,7 +62,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
* - State transition: xₖ₊₁ = f(xₖ, uₖ)
* @param dynamics_type The type of system evolution function.
* @param timestep_method The timestep method.
* @param method The transcription method.
* @param transcription_method The transcription method.
*/
OCP(int num_states, int num_inputs, std::chrono::duration<double> dt,
int num_steps,
@@ -126,7 +71,8 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
dynamics,
DynamicsType dynamics_type = DynamicsType::EXPLICIT_ODE,
TimestepMethod timestep_method = TimestepMethod::FIXED,
TranscriptionMethod method = TranscriptionMethod::DIRECT_TRANSCRIPTION)
TranscriptionMethod transcription_method =
TranscriptionMethod::DIRECT_TRANSCRIPTION)
: OCP{num_states,
num_inputs,
dt,
@@ -139,7 +85,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
},
dynamics_type,
timestep_method,
method} {}
transcription_method} {}
/**
* Build an optimization problem using a system evolution function (explicit
@@ -156,7 +102,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
* - State transition: xₖ₊₁ = f(t, xₖ, uₖ, dt)
* @param dynamics_type The type of system evolution function.
* @param timestep_method The timestep method.
* @param method The transcription method.
* @param transcription_method The transcription method.
*/
OCP(int num_states, int num_inputs, std::chrono::duration<double> dt,
int num_steps,
@@ -165,50 +111,46 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
dynamics,
DynamicsType dynamics_type = DynamicsType::EXPLICIT_ODE,
TimestepMethod timestep_method = TimestepMethod::FIXED,
TranscriptionMethod method = TranscriptionMethod::DIRECT_TRANSCRIPTION)
: m_num_states{num_states},
m_num_inputs{num_inputs},
m_dt{dt},
m_num_steps{num_steps},
m_transcription_method{method},
m_dynamics_type{dynamics_type},
m_dynamics_function{std::move(dynamics)},
m_timestep_method{timestep_method} {
TranscriptionMethod transcription_method =
TranscriptionMethod::DIRECT_TRANSCRIPTION)
: m_num_steps{num_steps},
m_dynamics{std::move(dynamics)},
m_dynamics_type{dynamics_type} {
// u is num_steps + 1 so that the final constraint function evaluation works
m_U = decision_variable(m_num_inputs, m_num_steps + 1);
m_U = decision_variable(num_inputs, m_num_steps + 1);
if (m_timestep_method == TimestepMethod::FIXED) {
if (timestep_method == TimestepMethod::FIXED) {
m_DT = VariableMatrix{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) {
m_DT(0, i) = m_dt.count();
m_DT(0, i) = dt.count();
}
} else if (m_timestep_method == TimestepMethod::VARIABLE_SINGLE) {
Variable dt = decision_variable();
dt.set_value(m_dt.count());
} else if (timestep_method == TimestepMethod::VARIABLE_SINGLE) {
Variable single_dt = decision_variable();
single_dt.set_value(dt.count());
// Set the member variable matrix to track the decision variable
m_DT = VariableMatrix{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) {
m_DT(0, i) = dt;
m_DT(0, i) = single_dt;
}
} else if (m_timestep_method == TimestepMethod::VARIABLE) {
} else if (timestep_method == TimestepMethod::VARIABLE) {
m_DT = decision_variable(1, m_num_steps + 1);
for (int i = 0; i < num_steps + 1; ++i) {
m_DT(0, i).set_value(m_dt.count());
m_DT(0, i).set_value(dt.count());
}
}
if (m_transcription_method == TranscriptionMethod::DIRECT_TRANSCRIPTION) {
m_X = decision_variable(m_num_states, m_num_steps + 1);
if (transcription_method == TranscriptionMethod::DIRECT_TRANSCRIPTION) {
m_X = decision_variable(num_states, m_num_steps + 1);
constrain_direct_transcription();
} else if (m_transcription_method ==
} else if (transcription_method ==
TranscriptionMethod::DIRECT_COLLOCATION) {
m_X = decision_variable(m_num_states, m_num_steps + 1);
m_X = decision_variable(num_states, m_num_steps + 1);
constrain_direct_collocation();
} else if (m_transcription_method == TranscriptionMethod::SINGLE_SHOOTING) {
} else if (transcription_method == TranscriptionMethod::SINGLE_SHOOTING) {
// In single-shooting the states aren't decision variables, but instead
// depend on the input and previous states
m_X = VariableMatrix{m_num_states, m_num_steps + 1};
m_X = VariableMatrix{num_states, m_num_steps + 1};
constrain_single_shooting();
}
}
@@ -370,6 +312,40 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
VariableMatrix final_state() { return m_X.col(m_num_steps); }
private:
int m_num_steps;
function_ref<VariableMatrix(const Variable& t, const VariableMatrix& x,
const VariableMatrix& u, const Variable& dt)>
m_dynamics;
DynamicsType m_dynamics_type;
VariableMatrix m_X;
VariableMatrix m_U;
VariableMatrix m_DT;
/**
* 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);
}
/**
* Apply direct collocation dynamics constraints.
*/
void constrain_direct_collocation() {
slp_assert(m_dynamics_type == DynamicsType::EXPLICIT_ODE);
@@ -379,7 +355,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
for (int i = 0; i < m_num_steps; ++i) {
Variable h = dt()(0, i);
auto& f = m_dynamics_function;
auto& f = m_dynamics;
auto t_begin = time;
auto t_end = t_begin + h;
@@ -405,6 +381,9 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
}
}
/**
* Apply direct transcription dynamics constraints.
*/
void constrain_direct_transcription() {
Variable time = 0.0;
@@ -415,17 +394,20 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
Variable dt = this->dt()(0, i);
if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
subject_to(x_end == rk4<const decltype(m_dynamics_function)&,
VariableMatrix, VariableMatrix, Variable>(
m_dynamics_function, x_begin, u, time, dt));
subject_to(x_end == rk4<const decltype(m_dynamics)&, VariableMatrix,
VariableMatrix, Variable>(m_dynamics, x_begin,
u, time, dt));
} else if (m_dynamics_type == DynamicsType::DISCRETE) {
subject_to(x_end == m_dynamics_function(time, x_begin, u, dt));
subject_to(x_end == m_dynamics(time, x_begin, u, dt));
}
time += dt;
}
}
/**
* Apply single shooting dynamics constraints.
*/
void constrain_single_shooting() {
Variable time = 0.0;
@@ -436,34 +418,15 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
Variable dt = this->dt()(0, i);
if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
x_end = rk4<const decltype(m_dynamics_function)&, VariableMatrix,
VariableMatrix, Variable>(m_dynamics_function, x_begin, u,
time, dt);
x_end = rk4<const decltype(m_dynamics)&, VariableMatrix, VariableMatrix,
Variable>(m_dynamics, x_begin, u, time, dt);
} else if (m_dynamics_type == DynamicsType::DISCRETE) {
x_end = m_dynamics_function(time, x_begin, u, dt);
x_end = m_dynamics(time, x_begin, u, dt);
}
time += dt;
}
}
int m_num_states;
int m_num_inputs;
std::chrono::duration<double> m_dt;
int m_num_steps;
TranscriptionMethod m_transcription_method;
DynamicsType m_dynamics_type;
function_ref<VariableMatrix(const Variable& t, const VariableMatrix& x,
const VariableMatrix& u, const Variable& dt)>
m_dynamics_function;
TimestepMethod m_timestep_method;
VariableMatrix m_X;
VariableMatrix m_U;
VariableMatrix m_DT;
};
} // namespace slp

19
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/dynamics_type.hpp vendored Normal file
View File

@@ -0,0 +1,19 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <stdint.h>
namespace slp {
/**
* 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).
EXPLICIT_ODE,
/// The dynamics are a function in the form xₖ₊₁ = f(t, xₖ, uₖ).
DISCRETE
};
} // namespace slp

22
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/timestep_method.hpp vendored Normal file
View File

@@ -0,0 +1,22 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <stdint.h>
namespace slp {
/**
* Enum describing the type of system timestep.
*/
enum class TimestepMethod : uint8_t {
/// The timestep is a fixed constant.
FIXED,
/// The timesteps are allowed to vary as independent decision variables.
VARIABLE,
/// The timesteps are equal length but allowed to vary as a single decision
/// variable.
VARIABLE_SINGLE
};
} // namespace slp

24
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/transcription_method.hpp vendored Normal file
View File

@@ -0,0 +1,24 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <stdint.h>
namespace slp {
/**
* 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.
DIRECT_TRANSCRIPTION,
/// The trajectory is modeled as a series of cubic polynomials where the
/// centerpoint slope is constrained.
DIRECT_COLLOCATION,
/// States depend explicitly as a function of all previous states and all
/// previous inputs.
SINGLE_SHOOTING
};
} // namespace slp

24
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp vendored
View File

@@ -73,7 +73,7 @@ class SLEIPNIR_DLLEXPORT Problem {
VariableMatrix decision_variable(int rows, int cols = 1) {
m_decision_variables.reserve(m_decision_variables.size() + rows * cols);
VariableMatrix vars{rows, cols};
VariableMatrix vars{VariableMatrix::empty, rows, cols};
for (int row = 0; row < rows; ++row) {
for (int col = 0; col < cols; ++col) {
@@ -108,7 +108,7 @@ class SLEIPNIR_DLLEXPORT Problem {
m_decision_variables.reserve(m_decision_variables.size() +
(rows * rows + rows) / 2);
VariableMatrix vars{rows, rows};
VariableMatrix vars{VariableMatrix::empty, rows, rows};
for (int row = 0; row < rows; ++row) {
for (int col = 0; col <= row; ++col) {
@@ -317,6 +317,24 @@ class SLEIPNIR_DLLEXPORT Problem {
*/
void clear_callbacks() { m_iteration_callbacks.clear(); }
/**
* Adds a callback to be called at the beginning of each solver iteration.
*
* Language bindings should call this in the Problem constructor to register
* callbacks that shouldn't be removed by clear_callbacks(). Persistent
* callbacks run after non-persistent callbacks.
*
* @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 requires(F callback, const IterationInfo& info) {
{ callback(info) } -> std::same_as<bool>;
}
void add_persistent_callback(F&& callback) {
m_persistent_iteration_callbacks.emplace_back(std::forward<F>(callback));
}
private:
// The list of decision variables, which are the root of the problem's
// expression tree
@@ -334,6 +352,8 @@ class SLEIPNIR_DLLEXPORT Problem {
// The iteration callbacks
gch::small_vector<std::function<bool(const IterationInfo& info)>>
m_iteration_callbacks;
gch::small_vector<std::function<bool(const IterationInfo& info)>>
m_persistent_iteration_callbacks;
void print_exit_conditions([[maybe_unused]] const Options& options);
void print_problem_analysis();

5
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp vendored
View File

@@ -3,9 +3,10 @@
#pragma once
#ifdef JORMUNGANDR
#include <format>
#include <source_location>
#include <stdexcept>
#include <fmt/format.h>
/**
* Throw an exception in Python.
*/
@@ -13,7 +14,7 @@
do { \
if (!(condition)) { \
auto location = std::source_location::current(); \
throw std::invalid_argument(std::format( \
throw std::invalid_argument(fmt::format( \
"{}:{}: {}: Assertion `{}' failed.", location.file_name(), \
location.line(), location.function_name(), #condition)); \
} \

1
wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide vendored
View File

@@ -8,5 +8,6 @@ cppSrcFileInclude {
includeOtherLibs {
^Eigen/
^fmt/
^gch/
}

18
wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/variable_matrix.cpp vendored
View File

@@ -24,7 +24,7 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
const auto& c = A(1, 0);
const auto& d = A(1, 1);
slp::VariableMatrix adj_A{{d, -b}, {-c, a}};
VariableMatrix adj_A{{d, -b}, {-c, a}};
auto det_A = a * d - b * c;
return adj_A / det_A * B;
} else if (A.rows() == 3 && A.cols() == 3) {
@@ -72,9 +72,9 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
auto adj_A10 = fg - di;
auto adj_A20 = dh - eg;
slp::VariableMatrix adj_A{{adj_A00, ch - bi, bf - ce},
{adj_A10, ai - cg, cd - af},
{adj_A20, bg - ah, ae - bd}};
VariableMatrix adj_A{{adj_A00, ch - bi, bf - ce},
{adj_A10, ai - cg, cd - af},
{adj_A20, bg - ah, ae - bd}};
auto det_A = a * adj_A00 + b * adj_A10 + c * adj_A20;
return adj_A / det_A * B;
} else if (A.rows() == 4 && A.cols() == 4) {
@@ -220,10 +220,10 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
auto adj_A32 = -afo + agn + beo - bgm - cen + cfm;
auto adj_A33 = afk - agj - bek + bgi + cej - cfi;
slp::VariableMatrix adj_A{{adj_A00, adj_A01, adj_A02, adj_A03},
{adj_A10, adj_A11, adj_A12, adj_A13},
{adj_A20, adj_A21, adj_A22, adj_A23},
{adj_A30, adj_A31, adj_A32, adj_A33}};
VariableMatrix adj_A{{adj_A00, adj_A01, adj_A02, adj_A03},
{adj_A10, adj_A11, adj_A12, adj_A13},
{adj_A20, adj_A21, adj_A22, adj_A23},
{adj_A30, adj_A31, adj_A32, adj_A33}};
auto det_A = a * adj_A00 + b * adj_A10 + c * adj_A20 + d * adj_A30;
return adj_A / det_A * B;
} else {
@@ -245,7 +245,7 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
MatrixXv eigen_X = eigen_A.householderQr().solve(eigen_B);
VariableMatrix X{A.cols(), B.cols()};
VariableMatrix X{VariableMatrix::empty, A.cols(), B.cols()};
for (int row = 0; row < X.rows(); ++row) {
for (int col = 0; col < X.cols(); ++col) {
X(row, col) = eigen_X(row, col);

32
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/problem.cpp vendored
View File

@@ -9,6 +9,7 @@
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <fmt/chrono.h>
#include <gch/small_vector.hpp>
#include "optimization/bounds.hpp"
@@ -79,6 +80,14 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
[[maybe_unused]]
int num_inequality_constraints = m_inequality_constraints.size();
gch::small_vector<std::function<bool(const IterationInfo& info)>> callbacks;
for (const auto& callback : m_iteration_callbacks) {
callbacks.emplace_back(callback);
}
for (const auto& callback : m_persistent_iteration_callbacks) {
callbacks.emplace_back(callback);
}
// Solve the optimization problem
ExitStatus status;
if (m_equality_constraints.empty() && m_inequality_constraints.empty()) {
@@ -101,7 +110,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
H_spy = std::make_unique<Spy>(
"H.spy", "Hessian", "Decision variables", "Decision variables",
num_decision_variables, num_decision_variables);
m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool {
callbacks.push_back([&](const IterationInfo& info) -> bool {
H_spy->add(info.H);
return false;
});
@@ -123,7 +132,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
x_ad.set_value(x);
return H.value();
}},
m_iteration_callbacks, options, x);
callbacks, options, x);
} else if (m_inequality_constraints.empty()) {
if (options.diagnostics) {
slp::println("\nInvoking SQP solver\n");
@@ -160,7 +169,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
"Constraints", "Decision variables",
num_equality_constraints,
num_decision_variables);
m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool {
callbacks.push_back([&](const IterationInfo& info) -> bool {
H_spy->add(info.H);
A_e_spy->add(info.A_e);
return false;
@@ -193,7 +202,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
x_ad.set_value(x);
return A_e.value();
}},
m_iteration_callbacks, options, x);
callbacks, options, x);
} else {
if (options.diagnostics) {
slp::println("\nInvoking IPM solver...\n");
@@ -242,7 +251,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
"A_i.spy", "Inequality constraint Jacobian", "Constraints",
"Decision variables", num_inequality_constraints,
num_decision_variables);
m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool {
callbacks.push_back([&](const IterationInfo& info) -> bool {
H_spy->add(info.H);
A_e_spy->add(info.A_e);
A_i_spy->add(info.A_i);
@@ -298,19 +307,13 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
x_ad.set_value(x);
return A_i.value();
}},
m_iteration_callbacks, options,
callbacks, options,
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
bound_constraint_mask,
#endif
x);
}
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
if (spy) {
m_iteration_callbacks.pop_back();
}
#endif
if (options.diagnostics) {
print_autodiff_diagnostics(ad_setup_profilers);
slp::println("\nExit: {}", to_message(status));
@@ -326,14 +329,15 @@ void Problem::print_exit_conditions([[maybe_unused]] const Options& options) {
// Print possible exit conditions
slp::println("User-configured exit conditions:");
slp::println(" ↳ error below {}", options.tolerance);
if (!m_iteration_callbacks.empty()) {
if (!m_iteration_callbacks.empty() ||
!m_persistent_iteration_callbacks.empty()) {
slp::println(" ↳ iteration callback requested stop");
}
if (std::isfinite(options.max_iterations)) {
slp::println(" ↳ executed {} iterations", options.max_iterations);
}
if (std::isfinite(options.timeout.count())) {
slp::println(" ↳ {} elapsed", options.timeout.count());
slp::println(" ↳ {} elapsed", options.timeout);
}
}

3
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/interior_point.cpp vendored
View File

@@ -327,8 +327,7 @@ ExitStatus interior_point(
Eigen::SparseMatrix<double> lhs(
num_decision_variables + num_equality_constraints,
num_decision_variables + num_equality_constraints);
lhs.setFromSortedTriplets(triplets.begin(), triplets.end(),
[](const auto&, const auto& b) { return b; });
lhs.setFromSortedTriplets(triplets.begin(), triplets.end());
// rhs = −[∇f − Aₑᵀy − Aᵢᵀ(−Σcᵢ + μS⁻¹e + z)]
// [ cₑ ]

1
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/newton.cpp vendored
View File

@@ -2,7 +2,6 @@
#include "sleipnir/optimization/solver/newton.hpp"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <functional>

5
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/sqp.cpp vendored
View File

@@ -2,7 +2,6 @@
#include "sleipnir/optimization/solver/sqp.hpp"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <functional>
@@ -232,8 +231,7 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks,
Eigen::SparseMatrix<double> lhs(
num_decision_variables + num_equality_constraints,
num_decision_variables + num_equality_constraints);
lhs.setFromSortedTriplets(triplets.begin(), triplets.end(),
[](const auto&, const auto& b) { return b; });
lhs.setFromSortedTriplets(triplets.begin(), triplets.end());
// rhs = −[∇f − Aₑᵀy]
// [ cₑ ]
@@ -407,7 +405,6 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks,
trial_x = x + α_max * step.p_x;
trial_y = y + α_max * step.p_y;
trial_f = matrices.f(trial_x);
trial_c_e = matrices.c_e(trial_x);
double next_kkt_error = kkt_error(