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 (#7973)

Browse Source
This commit is contained in:
Tyler Veness
2025-05-27 07:24:15 -07:00
committed by GitHub
parent 5368e8c6ed
commit de718f7ae5
90 changed files with 11188 additions and 7917 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -6,12 +6,9 @@ cppSrcFileInclude {
\.cpp$
}
licenseUpdateExclude {
include/sleipnir/util/small_vector\.hpp$
}
includeOtherLibs {
^Eigen/
^fmt/
^gch/
^wpi/
}

12
wpimath/src/main/native/thirdparty/sleipnir/include/gch/small_vector.hpp vendored Normal file
View File

@@ -0,0 +1,12 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <wpi/SmallVector.h>
namespace gch {
template <typename T>
using small_vector = wpi::SmallVector<T>;
} // namespace gch

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

File diff suppressed because it is too large Load Diff

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

@@ -1,244 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <span>
#include <wpi/SmallVector.h>
#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.
wpi::SmallVector<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.
*/
wpi::SmallVector<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;
}
wpi::SmallVector<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 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) {
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.
wpi::SmallVector<int> m_rowList;
// List for updating adjoints
wpi::SmallVector<Expression*> m_adjointList;
// List for updating values
wpi::SmallVector<Expression*> m_valueList;
};
} // namespace sleipnir::detail

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

@@ -1,27 +0,0 @@
// 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

79
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Hessian.hpp vendored
View File

@@ -1,79 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <utility>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <wpi/SmallVector.h>
#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{
[&] {
wpi::SmallVector<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(); }
/**
* Returns the profiler.
*/
Profiler& GetProfiler() { return m_jacobian.GetProfiler(); }
private:
Jacobian m_jacobian;
};
} // namespace sleipnir

155
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Jacobian.hpp vendored
View File

@@ -1,155 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <utility>
#include <Eigen/SparseCore>
#include <wpi/SmallVector.h>
#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()};
wpi::SmallVector<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();
for (auto& graph : m_graphs) {
graph.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;
}
/**
* Returns the profiler.
*/
Profiler& GetProfiler() { return m_profiler; }
private:
VariableMatrix m_variables;
VariableMatrix m_wrt;
wpi::SmallVector<detail::ExpressionGraph> m_graphs;
Eigen::SparseMatrix<double> m_J{m_variables.Rows(), m_wrt.Rows()};
// Cached triplets for gradients of linear rows
wpi::SmallVector<Eigen::Triplet<double>> m_cachedTriplets;
// List of row indices for nonlinear rows whose graients will be computed in
// Value()
wpi::SmallVector<int> m_nonlinearRows;
Profiler m_profiler;
};
} // namespace sleipnir

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

@@ -1,79 +0,0 @@
// 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

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

@@ -1,765 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <concepts>
#include <type_traits>
#include <utility>
#include <Eigen/Core>
#include "sleipnir/autodiff/Slice.hpp"
#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.
*
* @param values VariableBlock of values.
*/
VariableBlock<Mat>& operator=(const VariableBlock<Mat>& values) {
if (this == &values) {
return *this;
}
if (m_mat == nullptr) {
m_mat = values.m_mat;
m_rowSlice = values.m_rowSlice;
m_rowSliceLength = values.m_rowSliceLength;
m_colSlice = values.m_colSlice;
m_colSliceLength = values.m_colSliceLength;
} 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.
*
* @param values VariableBlock of values.
*/
VariableBlock<Mat>& operator=(VariableBlock<Mat>&& values) {
if (this == &values) {
return *this;
}
if (m_mat == nullptr) {
m_mat = values.m_mat;
m_rowSlice = values.m_rowSlice;
m_rowSliceLength = values.m_rowSliceLength;
m_colSlice = values.m_colSlice;
m_colSliceLength = values.m_colSliceLength;
} 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_rowSlice{0, mat.Rows(), 1},
m_rowSliceLength{m_rowSlice.Adjust(mat.Rows())},
m_colSlice{0, mat.Cols(), 1},
m_colSliceLength{m_colSlice.Adjust(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_rowSlice{rowOffset, rowOffset + blockRows, 1},
m_rowSliceLength{m_rowSlice.Adjust(mat.Rows())},
m_colSlice{colOffset, colOffset + blockCols, 1},
m_colSliceLength{m_colSlice.Adjust(mat.Cols())} {}
/**
* Constructs a Variable block pointing to a subset of the given matrix.
*
* Note that the slices are taken as is rather than adjusted.
*
* @param mat The matrix to which to point.
* @param rowSlice The block's row slice.
* @param rowSliceLength The block's row length.
* @param colSlice The block's column slice.
* @param colSliceLength The block's column length.
*/
VariableBlock(Mat& mat, Slice rowSlice, int rowSliceLength, Slice colSlice,
int colSliceLength)
: m_mat{&mat},
m_rowSlice{std::move(rowSlice)},
m_rowSliceLength{rowSliceLength},
m_colSlice{std::move(colSlice)},
m_colSliceLength{colSliceLength} {}
/**
* 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.
*
* @param value Value to assign.
*/
void SetValue(double value) {
Assert(Rows() == 1 && Cols() == 1);
(*this)(0, 0).SetValue(value);
}
/**
* Assigns an Eigen matrix to the block.
*
* @param values Eigen matrix of values to assign.
*/
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.
*
* @param values Eigen matrix of values.
*/
template <typename Derived>
requires std::same_as<typename Derived::Scalar, double>
void 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));
}
}
}
/**
* Assigns a VariableMatrix to the block.
*
* @param values VariableMatrix of values.
*/
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.
*
* @param values VariableMatrix of values.
*/
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.
*/
Variable& operator()(int row, int col)
requires(!std::is_const_v<Mat>)
{
Assert(row >= 0 && row < Rows());
Assert(col >= 0 && col < Cols());
return (*m_mat)(m_rowSlice.start + row * m_rowSlice.step,
m_colSlice.start + col * m_colSlice.step);
}
/**
* 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_rowSlice.start + row * m_rowSlice.step,
m_colSlice.start + col * m_colSlice.step);
}
/**
* Returns a scalar subblock at the given row.
*
* @param row The scalar subblock's row.
*/
Variable& operator()(int row)
requires(!std::is_const_v<Mat>)
{
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 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 (*this)({rowOffset, rowOffset + blockRows, 1},
{colOffset, colOffset + blockCols, 1});
}
/**
* 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 (*this)({rowOffset, rowOffset + blockRows, 1},
{colOffset, colOffset + blockCols, 1});
}
/**
* Returns a slice of the variable matrix.
*
* @param rowSlice The row slice.
* @param colSlice The column slice.
*/
VariableBlock<Mat> operator()(Slice rowSlice, Slice colSlice) {
int rowSliceLength = rowSlice.Adjust(m_rowSliceLength);
int colSliceLength = colSlice.Adjust(m_colSliceLength);
return VariableBlock{
*m_mat,
{m_rowSlice.start + rowSlice.start * m_rowSlice.step,
m_rowSlice.start + rowSlice.stop, m_rowSlice.step * rowSlice.step},
rowSliceLength,
{m_colSlice.start + colSlice.start * m_colSlice.step,
m_colSlice.start + colSlice.stop, m_colSlice.step * colSlice.step},
colSliceLength};
}
/**
* Returns a slice of the variable matrix.
*
* @param rowSlice The row slice.
* @param colSlice The column slice.
*/
const VariableBlock<const Mat> operator()(Slice rowSlice,
Slice colSlice) const {
int rowSliceLength = rowSlice.Adjust(m_rowSliceLength);
int colSliceLength = colSlice.Adjust(m_colSliceLength);
return VariableBlock{
*m_mat,
{m_rowSlice.start + rowSlice.start * m_rowSlice.step,
m_rowSlice.start + rowSlice.stop, m_rowSlice.step * rowSlice.step},
rowSliceLength,
{m_colSlice.start + colSlice.start * m_colSlice.step,
m_colSlice.start + colSlice.stop, m_colSlice.step * colSlice.step},
colSliceLength};
}
/**
* Returns a slice of the variable matrix.
*
* The given slices aren't adjusted. This overload is for Python bindings
* only.
*
* @param rowSlice The row slice.
* @param rowSliceLength The row slice length.
* @param colSlice The column slice.
* @param colSliceLength The column slice length.
*/
VariableBlock<Mat> operator()(Slice rowSlice, int rowSliceLength,
Slice colSlice, int colSliceLength) {
return VariableBlock{
*m_mat,
{m_rowSlice.start + rowSlice.start * m_rowSlice.step,
m_rowSlice.start + rowSlice.stop, m_rowSlice.step * rowSlice.step},
rowSliceLength,
{m_colSlice.start + colSlice.start * m_colSlice.step,
m_colSlice.start + colSlice.stop, m_colSlice.step * colSlice.step},
colSliceLength};
}
/**
* Returns a slice of the variable matrix.
*
* The given slices aren't adjusted. This overload is for Python bindings
* only.
*
* @param rowSlice The row slice.
* @param rowSliceLength The row slice length.
* @param colSlice The column slice.
* @param colSliceLength The column slice length.
*/
const VariableBlock<const Mat> operator()(Slice rowSlice, int rowSliceLength,
Slice colSlice,
int colSliceLength) const {
return VariableBlock{
*m_mat,
{m_rowSlice.start + rowSlice.start * m_rowSlice.step,
m_rowSlice.start + rowSlice.stop, m_rowSlice.step * rowSlice.step},
rowSliceLength,
{m_colSlice.start + colSlice.start * m_colSlice.step,
m_colSlice.start + colSlice.stop, m_colSlice.step * colSlice.step},
colSliceLength};
}
/**
* Returns a segment of the variable vector.
*
* @param offset The offset of the segment.
* @param length The length of the segment.
*/
VariableBlock<Mat> Segment(int offset, int length) {
Assert(offset >= 0 && offset < Rows() * Cols());
Assert(length >= 0 && length <= Rows() * Cols() - offset);
return Block(offset, 0, length, 1);
}
/**
* Returns a segment of the variable vector.
*
* @param offset The offset of the segment.
* @param length The length of the segment.
*/
const VariableBlock<Mat> Segment(int offset, int length) const {
Assert(offset >= 0 && offset < Rows() * Cols());
Assert(length >= 0 && length <= Rows() * Cols() - offset);
return Block(offset, 0, length, 1);
}
/**
* Returns a row slice of the variable matrix.
*
* @param row The row to slice.
*/
VariableBlock<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_rowSliceLength; }
/**
* Returns number of columns in the matrix.
*/
int Cols() const { return m_colSliceLength; }
/**
* 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) {
Assert(row >= 0 && row < Rows());
Assert(col >= 0 && col < Cols());
return (*m_mat)(m_rowSlice.start + row * m_rowSlice.step,
m_colSlice.start + col * m_colSlice.step)
.Value();
}
/**
* Returns a row of the variable column vector.
*
* @param index The index of the element to return.
*/
double Value(int index) {
Assert(index >= 0 && index < Rows() * Cols());
return Value(index / Cols(), index % Cols());
}
/**
* Returns the contents of the variable matrix.
*/
Eigen::MatrixXd Value() {
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& x)> 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 Rows() * Cols(); }
private:
Mat* m_mat = nullptr;
Slice m_rowSlice;
int m_rowSliceLength = 0;
Slice m_colSlice;
int m_colSliceLength = 0;
};
} // namespace sleipnir

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

File diff suppressed because it is too large Load Diff

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

@@ -0,0 +1,178 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <ranges>
#include <utility>
#include <Eigen/SparseCore>
#include <gch/small_vector.hpp>
#include "sleipnir/autodiff/expression_graph.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
namespace slp::detail {
/**
* This class is an adaptor type that performs value updates of an expression's
* adjoint graph.
*/
class AdjointExpressionGraph {
public:
/**
* Generates the adjoint graph for the given expression.
*
* @param root The root node of the expression.
*/
explicit AdjointExpressionGraph(const Variable& root)
: m_top_list{topological_sort(root.expr)} {
for (const auto& node : m_top_list) {
m_col_list.emplace_back(node->col);
}
}
/**
* Update the values of all nodes in this adjoint graph based on the values of
* their dependent nodes.
*/
void update_values() { detail::update_values(m_top_list); }
/**
* Returns the variable's gradient tree.
*
* This function lazily allocates variables, so elements of the returned
* VariableMatrix will be empty if the corresponding element of wrt had no
* adjoint. Ensure Variable::expr isn't nullptr before calling member
* functions.
*
* @param wrt Variables with respect to which to compute the gradient.
* @return The variable's gradient tree.
*/
VariableMatrix generate_gradient_tree(const VariableMatrix& wrt) const {
// Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation
// for background on reverse accumulation automatic differentiation.
if (m_top_list.empty()) {
return VariableMatrix{VariableMatrix::empty, wrt.rows(), 1};
}
// Set root node's adjoint to 1 since df/df is 1
m_top_list[0]->adjoint_expr = make_expression_ptr<ConstExpression>(1.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 (auto& node : m_top_list) {
auto& lhs = node->args[0];
auto& rhs = node->args[1];
if (lhs != nullptr) {
lhs->adjoint_expr += node->grad_expr_l(lhs, rhs, node->adjoint_expr);
if (rhs != nullptr) {
rhs->adjoint_expr += node->grad_expr_r(lhs, rhs, node->adjoint_expr);
}
}
}
// Move gradient tree to return value
VariableMatrix grad{VariableMatrix::empty, wrt.rows(), 1};
for (int row = 0; row < grad.rows(); ++row) {
grad[row] = Variable{std::move(wrt[row].expr->adjoint_expr)};
}
// 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_top_list) {
node->adjoint_expr = nullptr;
}
return grad;
}
/**
* Updates the adjoints in the expression graph (computes the gradient) then
* appends the adjoints of wrt to the sparse matrix triplets.
*
* @param triplets The sparse matrix triplets.
* @param row The row of wrt.
* @param wrt Vector of variables with respect to which to compute the
* Jacobian.
*/
void append_adjoint_triplets(
gch::small_vector<Eigen::Triplet<double>>& triplets, int row,
const VariableMatrix& wrt) const {
// Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation
// for background on reverse accumulation automatic differentiation.
// If wrt has fewer nodes than graph, zero wrt's adjoints
if (static_cast<size_t>(wrt.rows()) < m_top_list.size()) {
for (const auto& elem : wrt) {
elem.expr->adjoint = 0.0;
}
}
if (m_top_list.empty()) {
return;
}
// Set root node's adjoint to 1 since df/df is 1
m_top_list[0]->adjoint = 1.0;
// Zero the rest of the adjoints
for (auto& node : m_top_list | std::views::drop(1)) {
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 (const auto& node : m_top_list) {
auto& lhs = node->args[0];
auto& rhs = node->args[1];
if (lhs != nullptr) {
if (rhs != nullptr) {
lhs->adjoint += node->grad_l(lhs->val, rhs->val, node->adjoint);
rhs->adjoint += node->grad_r(lhs->val, rhs->val, node->adjoint);
} else {
lhs->adjoint += node->grad_l(lhs->val, 0.0, node->adjoint);
}
}
}
// If wrt has fewer nodes than graph, iterate over wrt
if (static_cast<size_t>(wrt.rows()) < m_top_list.size()) {
for (int col = 0; col < wrt.rows(); ++col) {
const auto& node = wrt[col].expr;
// Append adjoints of wrt to sparse matrix triplets
if (node->adjoint != 0.0) {
triplets.emplace_back(row, col, node->adjoint);
}
}
} else {
for (size_t i = 0; i < m_top_list.size(); ++i) {
const auto& col = m_col_list[i];
const auto& node = m_top_list[i];
// Append adjoints of wrt to sparse matrix triplets
if (col != -1 && node->adjoint != 0.0) {
triplets.emplace_back(row, col, node->adjoint);
}
}
}
}
private:
// Topological sort of graph from parent to child
gch::small_vector<Expression*> m_top_list;
// List that maps nodes to their respective column
gch::small_vector<int> m_col_list;
};
} // namespace slp::detail

1758
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

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

@@ -0,0 +1,94 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <ranges>
#include <gch/small_vector.hpp>
#include "sleipnir/autodiff/expression.hpp"
namespace slp::detail {
/**
* Generate a topological sort of an expression graph from parent to child.
*
* https://en.wikipedia.org/wiki/Topological_sorting
*
* @param root The root node of the expression.
*/
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 (root == nullptr || root->type() == ExpressionType::CONSTANT) {
return list;
}
// Stack of nodes to explore
gch::small_vector<Expression*> stack;
// Enumerate incoming edges for each node via depth-first search
stack.emplace_back(root.get());
while (!stack.empty()) {
auto node = stack.back();
stack.pop_back();
for (auto& arg : node->args) {
// If the node hasn't been explored yet, add it to the stack
if (arg != nullptr && ++arg->incoming_edges == 1) {
stack.push_back(arg.get());
}
}
}
// Generate topological sort of graph from parent to child.
//
// A node is only added to the stack after all its incoming edges have been
// traversed. Expression::incoming_edges is a decrementing counter for
// tracking this.
//
// https://en.wikipedia.org/wiki/Topological_sorting
stack.emplace_back(root.get());
while (!stack.empty()) {
auto node = stack.back();
stack.pop_back();
list.emplace_back(node);
for (auto& arg : node->args) {
// If we traversed all this node's incoming edges, add it to the stack
if (arg != nullptr && --arg->incoming_edges == 0) {
stack.push_back(arg.get());
}
}
}
return list;
}
/**
* Update the values of all nodes in this graph based on the values of
* their dependent nodes.
*
* @param list Topological sort of graph from parent to child.
*/
inline void update_values(const gch::small_vector<Expression*>& list) {
// Traverse graph from child to parent and update values
for (auto& node : list | std::views::reverse) {
auto& lhs = node->args[0];
auto& rhs = node->args[1];
if (lhs != nullptr) {
if (rhs != nullptr) {
node->val = node->value(lhs->val, rhs->val);
} else {
node->val = node->value(lhs->val, 0.0);
}
}
}
}
} // namespace slp::detail

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

@@ -0,0 +1,56 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <stdint.h>
#include <string_view>
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
/**
* Expression type.
*
* Used for autodiff caching.
*/
enum class ExpressionType : uint8_t {
/// There is no expression.
NONE,
/// The expression is a constant.
CONSTANT,
/// The expression is composed of linear and lower-order operators.
LINEAR,
/// The expression is composed of quadratic and lower-order operators.
QUADRATIC,
/// The expression is composed of nonlinear and lower-order operators.
NONLINEAR
};
/**
* Returns user-readable message corresponding to the expression type.
*
* @param type Expression type.
*/
SLEIPNIR_DLLEXPORT constexpr std::string_view to_message(
const ExpressionType& type) {
using enum ExpressionType;
switch (type) {
case NONE:
return "none";
case CONSTANT:
return "constant";
case LINEAR:
return "linear";
case QUADRATIC:
return "quadratic";
case NONLINEAR:
return "nonlinear";
default:
return "unknown";
}
}
} // namespace slp

35
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Gradient.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp vendored
View File

@@ -6,13 +6,13 @@
#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"
#include "sleipnir/autodiff/jacobian.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/concepts.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace sleipnir {
namespace slp {
/**
* This class calculates the gradient of a a variable with respect to a vector
@@ -30,7 +30,7 @@ class SLEIPNIR_DLLEXPORT Gradient {
* @param wrt Variable with respect to which to compute the gradient.
*/
Gradient(Variable variable, Variable wrt) noexcept
: Gradient{std::move(variable), VariableMatrix{wrt}} {}
: m_jacobian{std::move(variable), std::move(wrt)} {}
/**
* Constructs a Gradient object.
@@ -39,35 +39,34 @@ class SLEIPNIR_DLLEXPORT 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} {}
Gradient(Variable variable, SleipnirMatrixLike auto wrt) noexcept
: m_jacobian{VariableMatrix{std::move(variable)}, std::move(wrt)} {}
/**
* Returns the gradient as a VariableMatrix.
*
* This is useful when constructing optimization problems with derivatives in
* them.
*
* @return The gradient as a VariableMatrix.
*/
VariableMatrix Get() const { return m_jacobian.Get().T(); }
VariableMatrix get() const { return m_jacobian.get().T(); }
/**
* Evaluates the gradient at wrt's value.
*
* @return The gradient at wrt's value.
*/
const Eigen::SparseVector<double>& Value() {
m_g = m_jacobian.Value();
const Eigen::SparseVector<double>& value() {
m_g = m_jacobian.value();
return m_g;
}
/**
* Returns the profiler.
*/
Profiler& GetProfiler() { return m_jacobian.GetProfiler(); }
private:
Eigen::SparseVector<double> m_g;
Jacobian m_jacobian;
};
} // namespace sleipnir
} // namespace slp

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

@@ -0,0 +1,169 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <utility>
#include <Eigen/SparseCore>
#include <gch/small_vector.hpp>
#include "sleipnir/autodiff/adjoint_expression_graph.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/concepts.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
/**
* 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.
*
* @tparam UpLo Which part of the Hessian to compute (Lower or Lower | Upper).
*/
template <int UpLo>
requires(UpLo == Eigen::Lower) || (UpLo == (Eigen::Lower | Eigen::Upper))
class SLEIPNIR_DLLEXPORT Hessian {
public:
/**
* Constructs a Hessian object.
*
* @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{std::move(variable), VariableMatrix{std::move(wrt)}} {}
/**
* 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, SleipnirMatrixLike auto wrt) noexcept
: m_variables{detail::AdjointExpressionGraph{variable}
.generate_gradient_tree(wrt)},
m_wrt{wrt} {
// 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;
}
for (auto& variable : m_variables) {
m_graphs.emplace_back(variable);
}
// Reset col to -1
for (auto& node : m_wrt) {
node.expr->col = -1;
}
for (int row = 0; row < m_variables.rows(); ++row) {
if (m_variables[row].expr == nullptr) {
continue;
}
if (m_variables[row].type() == ExpressionType::LINEAR) {
// 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].append_adjoint_triplets(m_cached_triplets, row, m_wrt);
} else if (m_variables[row].type() > ExpressionType::LINEAR) {
// If the row is quadratic or nonlinear, add it to the list of nonlinear
// rows to be recomputed in Value().
m_nonlinear_rows.emplace_back(row);
}
}
if (m_nonlinear_rows.empty()) {
m_H.setFromTriplets(m_cached_triplets.begin(), m_cached_triplets.end());
if constexpr (UpLo == Eigen::Lower) {
m_H = m_H.triangularView<Eigen::Lower>();
}
}
}
/**
* Returns the Hessian as a VariableMatrix.
*
* This is useful when constructing optimization problems with derivatives in
* them.
*
* @return The Hessian as a VariableMatrix.
*/
VariableMatrix get() const {
VariableMatrix result{VariableMatrix::empty, m_variables.rows(),
m_wrt.rows()};
for (int row = 0; row < m_variables.rows(); ++row) {
auto grad = m_graphs[row].generate_gradient_tree(m_wrt);
for (int col = 0; col < m_wrt.rows(); ++col) {
if (grad[col].expr != nullptr) {
result(row, col) = std::move(grad[col]);
} else {
result(row, col) = Variable{0.0};
}
}
}
return result;
}
/**
* Evaluates the Hessian at wrt's value.
*
* @return The Hessian at wrt's value.
*/
const Eigen::SparseMatrix<double>& value() {
if (m_nonlinear_rows.empty()) {
return m_H;
}
for (auto& graph : m_graphs) {
graph.update_values();
}
// Copy the cached triplets so triplets added for the nonlinear rows are
// thrown away at the end of the function
auto triplets = m_cached_triplets;
// Compute each nonlinear row of the Hessian
for (int row : m_nonlinear_rows) {
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();
}
return m_H;
}
private:
VariableMatrix m_variables;
VariableMatrix m_wrt;
gch::small_vector<detail::AdjointExpressionGraph> m_graphs;
Eigen::SparseMatrix<double> m_H{m_variables.rows(), m_wrt.rows()};
// Cached triplets for gradients of linear rows
gch::small_vector<Eigen::Triplet<double>> m_cached_triplets;
// List of row indices for nonlinear rows whose graients will be computed in
// Value()
gch::small_vector<int> m_nonlinear_rows;
};
} // namespace slp

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

@@ -0,0 +1,158 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <utility>
#include <Eigen/SparseCore>
#include <gch/small_vector.hpp>
#include "sleipnir/autodiff/adjoint_expression_graph.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/concepts.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
/**
* 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 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{VariableMatrix{std::move(variable)},
VariableMatrix{std::move(wrt)}} {}
/**
* 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(VariableMatrix variables, SleipnirMatrixLike auto wrt) noexcept
: m_variables{std::move(variables)}, m_wrt{std::move(wrt)} {
// 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;
}
for (auto& variable : m_variables) {
m_graphs.emplace_back(variable);
}
// Reset col to -1
for (auto& node : m_wrt) {
node.expr->col = -1;
}
for (int row = 0; row < m_variables.rows(); ++row) {
if (m_variables[row].expr == nullptr) {
continue;
}
if (m_variables[row].type() == ExpressionType::LINEAR) {
// 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].append_adjoint_triplets(m_cached_triplets, row, m_wrt);
} else if (m_variables[row].type() > ExpressionType::LINEAR) {
// If the row is quadratic or nonlinear, add it to the list of nonlinear
// rows to be recomputed in Value().
m_nonlinear_rows.emplace_back(row);
}
}
if (m_nonlinear_rows.empty()) {
m_J.setFromTriplets(m_cached_triplets.begin(), m_cached_triplets.end());
}
}
/**
* Returns the Jacobian as a VariableMatrix.
*
* This is useful when constructing optimization problems with derivatives in
* them.
*
* @return The Jacobian as a VariableMatrix.
*/
VariableMatrix get() const {
VariableMatrix result{VariableMatrix::empty, m_variables.rows(),
m_wrt.rows()};
for (int row = 0; row < m_variables.rows(); ++row) {
auto grad = m_graphs[row].generate_gradient_tree(m_wrt);
for (int col = 0; col < m_wrt.rows(); ++col) {
if (grad[col].expr != nullptr) {
result(row, col) = std::move(grad[col]);
} else {
result(row, col) = Variable{0.0};
}
}
}
return result;
}
/**
* Evaluates the Jacobian at wrt's value.
*
* @return The Jacobian at wrt's value.
*/
const Eigen::SparseMatrix<double>& value() {
if (m_nonlinear_rows.empty()) {
return m_J;
}
for (auto& graph : m_graphs) {
graph.update_values();
}
// Copy the cached triplets so triplets added for the nonlinear rows are
// thrown away at the end of the function
auto triplets = m_cached_triplets;
// Compute each nonlinear row of the Jacobian
for (int row : m_nonlinear_rows) {
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();
}
return m_J;
}
private:
VariableMatrix m_variables;
VariableMatrix m_wrt;
gch::small_vector<detail::AdjointExpressionGraph> m_graphs;
Eigen::SparseMatrix<double> m_J{m_variables.rows(), m_wrt.rows()};
// Cached triplets for gradients of linear rows
gch::small_vector<Eigen::Triplet<double>> m_cached_triplets;
// List of row indices for nonlinear rows whose graients will be computed in
// Value()
gch::small_vector<int> m_nonlinear_rows;
};
} // namespace slp

46
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Slice.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/slice.hpp vendored
View File

@@ -6,18 +6,28 @@
#include <limits>
#include <utility>
#include "sleipnir/util/Assert.hpp"
#include "sleipnir/util/SymbolExports.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace sleipnir {
namespace slp {
namespace slicing {
/**
* Type tag used to designate an omitted argument of the slice.
*/
struct none_t {};
/**
* Designates an omitted argument of the slice.
*/
static inline constexpr none_t _;
} // namespace slicing
/**
* Represents a sequence of elements in an iterable object.
*/
class SLEIPNIR_DLLEXPORT Slice {
public:
/// Start index (inclusive).
@@ -36,14 +46,20 @@ class SLEIPNIR_DLLEXPORT Slice {
/**
* Constructs a slice.
*
* @param stop Slice stop index (exclusive).
*/
template <typename Stop>
requires std::same_as<Stop, slicing::none_t> ||
std::convertible_to<Stop, int>
constexpr Slice(Stop stop) // NOLINT
: Slice(slicing::_, std::move(stop), 1) {}
constexpr Slice(slicing::none_t) // NOLINT
: Slice(0, std::numeric_limits<int>::max(), 1) {}
/**
* Constructs a slice.
*
* @param start Slice start index (inclusive).
*/
constexpr Slice(int start) { // NOLINT
this->start = start;
this->stop = (start == -1) ? std::numeric_limits<int>::max() : start + 1;
this->step = 1;
}
/**
* Constructs a slice.
@@ -77,7 +93,7 @@ class SLEIPNIR_DLLEXPORT Slice {
if constexpr (std::same_as<Step, slicing::none_t>) {
this->step = 1;
} else {
Assert(step != 0);
slp_assert(step != 0);
this->step = step;
}
@@ -115,9 +131,9 @@ class SLEIPNIR_DLLEXPORT Slice {
* @param length The sequence length.
* @return The slice length.
*/
constexpr int Adjust(int length) {
Assert(step != 0);
Assert(step >= -std::numeric_limits<int>::max());
constexpr int adjust(int length) {
slp_assert(step != 0);
slp_assert(step >= -std::numeric_limits<int>::max());
if (start < 0) {
start += length;
@@ -155,4 +171,4 @@ class SLEIPNIR_DLLEXPORT Slice {
}
};
} // namespace sleipnir
} // namespace slp

324
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Variable.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp vendored
View File

@@ -5,23 +5,32 @@
#include <algorithm>
#include <concepts>
#include <initializer_list>
#include <source_location>
#include <type_traits>
#include <utility>
#include <vector>
#include <Eigen/Core>
#include <wpi/SmallVector.h>
#include <gch/small_vector.hpp>
#include "sleipnir/autodiff/Expression.hpp"
#include "sleipnir/autodiff/ExpressionGraph.hpp"
#include "sleipnir/util/Assert.hpp"
#include "sleipnir/util/Concepts.hpp"
#include "sleipnir/util/Print.hpp"
#include "sleipnir/util/SymbolExports.hpp"
#include "sleipnir/autodiff/expression.hpp"
#include "sleipnir/autodiff/expression_graph.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/concepts.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace sleipnir {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
#include "sleipnir/util/print.hpp"
#endif
namespace slp {
// Forward declarations for friend declarations in Variable
namespace detail {
class AdjointExpressionGraph;
} // namespace detail
template <int UpLo = Eigen::Lower | Eigen::Upper>
requires(UpLo == Eigen::Lower) || (UpLo == (Eigen::Lower | Eigen::Upper))
class SLEIPNIR_DLLEXPORT Hessian;
class SLEIPNIR_DLLEXPORT Jacobian;
@@ -36,11 +45,25 @@ class SLEIPNIR_DLLEXPORT Variable {
Variable() = default;
/**
* Constructs a Variable from a double.
* Constructs an empty Variable.
*/
explicit Variable(std::nullptr_t) : expr{nullptr} {}
/**
* Constructs a Variable from a floating point type.
*
* @param value The value of the Variable.
*/
Variable(double value) : expr{detail::MakeExpressionPtr(value)} {} // NOLINT
Variable(std::floating_point auto value) // NOLINT
: expr{detail::make_expression_ptr<detail::ConstExpression>(value)} {}
/**
* Constructs a Variable from an integral type.
*
* @param value The value of the Variable.
*/
Variable(std::integral auto value) // NOLINT
: expr{detail::make_expression_ptr<detail::ConstExpression>(value)} {}
/**
* Constructs a Variable pointing to the specified expression.
@@ -60,9 +83,10 @@ class SLEIPNIR_DLLEXPORT Variable {
* Assignment operator for double.
*
* @param value The value of the Variable.
* @return This variable.
*/
Variable& operator=(double value) {
expr = detail::MakeExpressionPtr(value);
expr = detail::make_expression_ptr<detail::ConstExpression>(value);
return *this;
}
@@ -72,19 +96,22 @@ class SLEIPNIR_DLLEXPORT Variable {
*
* @param value The value of the Variable.
*/
void SetValue(double value) {
if (expr->IsConstant(0.0)) {
expr = detail::MakeExpressionPtr(value);
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 && !expr->args[0]->IsConstant(0.0)) {
sleipnir::println(
if (expr->args[0] != nullptr) {
auto location = std::source_location::current();
slp::println(
stderr,
"WARNING: {}:{}: Modified the value of a dependent variable",
__FILE__, __LINE__);
"WARNING: {}:{}: {}: Modified the value of a dependent variable",
location.file_name(), location.line(), location.function_name());
}
expr->value = value;
#endif
expr->val = value;
}
}
@@ -93,6 +120,7 @@ class SLEIPNIR_DLLEXPORT Variable {
*
* @param lhs Operator left-hand side.
* @param rhs Operator right-hand side.
* @return Result of multiplication.
*/
friend SLEIPNIR_DLLEXPORT Variable operator*(const Variable& lhs,
const Variable& rhs) {
@@ -103,6 +131,7 @@ class SLEIPNIR_DLLEXPORT Variable {
* Variable-Variable compound multiplication operator.
*
* @param rhs Operator right-hand side.
* @return Result of multiplication.
*/
Variable& operator*=(const Variable& rhs) {
*this = *this * rhs;
@@ -114,6 +143,7 @@ class SLEIPNIR_DLLEXPORT Variable {
*
* @param lhs Operator left-hand side.
* @param rhs Operator right-hand side.
* @return Result of division.
*/
friend SLEIPNIR_DLLEXPORT Variable operator/(const Variable& lhs,
const Variable& rhs) {
@@ -124,6 +154,7 @@ class SLEIPNIR_DLLEXPORT Variable {
* Variable-Variable compound division operator.
*
* @param rhs Operator right-hand side.
* @return Result of division.
*/
Variable& operator/=(const Variable& rhs) {
*this = *this / rhs;
@@ -135,6 +166,7 @@ class SLEIPNIR_DLLEXPORT Variable {
*
* @param lhs Operator left-hand side.
* @param rhs Operator right-hand side.
* @return Result of addition.
*/
friend SLEIPNIR_DLLEXPORT Variable operator+(const Variable& lhs,
const Variable& rhs) {
@@ -145,6 +177,7 @@ class SLEIPNIR_DLLEXPORT Variable {
* Variable-Variable compound addition operator.
*
* @param rhs Operator right-hand side.
* @return Result of addition.
*/
Variable& operator+=(const Variable& rhs) {
*this = *this + rhs;
@@ -156,6 +189,7 @@ class SLEIPNIR_DLLEXPORT Variable {
*
* @param lhs Operator left-hand side.
* @param rhs Operator right-hand side.
* @return Result of subtraction.
*/
friend SLEIPNIR_DLLEXPORT Variable operator-(const Variable& lhs,
const Variable& rhs) {
@@ -166,6 +200,7 @@ class SLEIPNIR_DLLEXPORT Variable {
* Variable-Variable compound subtraction operator.
*
* @param rhs Operator right-hand side.
* @return Result of subtraction.
*/
Variable& operator-=(const Variable& rhs) {
*this = *this - rhs;
@@ -192,25 +227,38 @@ class SLEIPNIR_DLLEXPORT Variable {
/**
* Returns the value of this variable.
*
* @return The value of this variable.
*/
double Value() {
// Updates the value of this variable based on the values of its dependent
// variables
detail::ExpressionGraph{expr}.Update();
double value() {
if (!m_graph_initialized) {
m_graph = detail::topological_sort(expr);
m_graph_initialized = true;
}
detail::update_values(m_graph);
return expr->value;
return expr->val;
}
/**
* Returns the type of this expression (constant, linear, quadratic, or
* nonlinear).
*
* @return The type of this expression.
*/
ExpressionType Type() const { return expr->type; }
ExpressionType type() const { return expr->type(); }
private:
/// The expression node.
/// The expression node
detail::ExpressionPtr expr =
detail::MakeExpressionPtr(0.0, ExpressionType::kLinear);
detail::make_expression_ptr<detail::DecisionVariableExpression>();
/// Used to update the value of this variable based on the values of its
/// dependent variables
gch::small_vector<detail::Expression*> m_graph;
/// Used for lazy initialization of m_graph
bool m_graph_initialized = false;
friend SLEIPNIR_DLLEXPORT Variable abs(const Variable& x);
friend SLEIPNIR_DLLEXPORT Variable acos(const Variable& x);
@@ -237,6 +285,9 @@ class SLEIPNIR_DLLEXPORT Variable {
friend SLEIPNIR_DLLEXPORT Variable hypot(const Variable& x, const Variable& y,
const Variable& z);
friend class detail::AdjointExpressionGraph;
template <int UpLo>
requires(UpLo == Eigen::Lower) || (UpLo == (Eigen::Lower | Eigen::Upper))
friend class SLEIPNIR_DLLEXPORT Hessian;
friend class SLEIPNIR_DLLEXPORT Jacobian;
};
@@ -425,8 +476,7 @@ SLEIPNIR_DLLEXPORT inline Variable tanh(const Variable& x) {
*/
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))};
return Variable{slp::sqrt(slp::pow(x, 2) + slp::pow(y, 2) + slp::pow(z, 2))};
}
/**
@@ -441,85 +491,38 @@ SLEIPNIR_DLLEXPORT inline Variable hypot(const Variable& x, const Variable& y,
* @param rhs Right-hand side.
*/
template <typename LHS, typename RHS>
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
wpi::SmallVector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
wpi::SmallVector<Variable> constraints;
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(SleipnirType<LHS> || SleipnirType<RHS>)
gch::small_vector<Variable> make_constraints(LHS&& lhs, RHS&& rhs) {
gch::small_vector<Variable> constraints;
if constexpr (ScalarLike<std::decay_t<LHS>> &&
ScalarLike<std::decay_t<RHS>>) {
if constexpr (ScalarLike<LHS> && ScalarLike<RHS>) {
constraints.emplace_back(lhs - rhs);
} else if constexpr (ScalarLike<std::decay_t<LHS>> &&
MatrixLike<std::decay_t<RHS>>) {
int rows;
int cols;
if constexpr (EigenMatrixLike<std::decay_t<RHS>>) {
rows = rhs.rows();
cols = rhs.cols();
} else {
rows = rhs.Rows();
cols = rhs.Cols();
}
} else if constexpr (ScalarLike<LHS> && MatrixLike<RHS>) {
constraints.reserve(rhs.rows() * rhs.cols());
constraints.reserve(rows * cols);
for (int row = 0; row < rows; ++row) {
for (int col = 0; col < cols; ++col) {
for (int row = 0; row < rhs.rows(); ++row) {
for (int col = 0; col < rhs.cols(); ++col) {
// Make right-hand side zero
constraints.emplace_back(lhs - rhs(row, col));
}
}
} else if constexpr (MatrixLike<std::decay_t<LHS>> &&
ScalarLike<std::decay_t<RHS>>) {
int rows;
int cols;
if constexpr (EigenMatrixLike<std::decay_t<LHS>>) {
rows = lhs.rows();
cols = lhs.cols();
} else {
rows = lhs.Rows();
cols = lhs.Cols();
}
} else if constexpr (MatrixLike<LHS> && ScalarLike<RHS>) {
constraints.reserve(lhs.rows() * lhs.cols());
constraints.reserve(rows * cols);
for (int row = 0; row < rows; ++row) {
for (int col = 0; col < cols; ++col) {
for (int row = 0; row < lhs.rows(); ++row) {
for (int col = 0; col < lhs.cols(); ++col) {
// Make right-hand side zero
constraints.emplace_back(lhs(row, col) - rhs);
}
}
} else if constexpr (MatrixLike<std::decay_t<LHS>> &&
MatrixLike<std::decay_t<RHS>>) {
int lhsRows;
int lhsCols;
if constexpr (EigenMatrixLike<std::decay_t<LHS>>) {
lhsRows = lhs.rows();
lhsCols = lhs.cols();
} else {
lhsRows = lhs.Rows();
lhsCols = lhs.Cols();
}
} else if constexpr (MatrixLike<LHS> && MatrixLike<RHS>) {
slp_assert(lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols());
constraints.reserve(lhs.rows() * lhs.cols());
[[maybe_unused]]
int rhsRows;
[[maybe_unused]]
int rhsCols;
if constexpr (EigenMatrixLike<std::decay_t<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) {
for (int row = 0; row < lhs.rows(); ++row) {
for (int col = 0; col < lhs.cols(); ++col) {
// Make right-hand side zero
constraints.emplace_back(lhs(row, col) - rhs(row, col));
}
@@ -534,16 +537,16 @@ wpi::SmallVector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
*/
struct SLEIPNIR_DLLEXPORT EqualityConstraints {
/// A vector of scalar equality constraints.
wpi::SmallVector<Variable> constraints;
gch::small_vector<Variable> constraints;
/**
* Concatenates multiple equality constraints.
*
* @param equalityConstraints The list of EqualityConstraints to concatenate.
* @param equality_constraints The list of EqualityConstraints to concatenate.
*/
EqualityConstraints( // NOLINT
std::initializer_list<EqualityConstraints> equalityConstraints) {
for (const auto& elem : equalityConstraints) {
EqualityConstraints(
std::initializer_list<EqualityConstraints> equality_constraints) {
for (const auto& elem : equality_constraints) {
constraints.insert(constraints.end(), elem.constraints.begin(),
elem.constraints.end());
}
@@ -554,11 +557,11 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints {
*
* This overload is for Python bindings only.
*
* @param equalityConstraints The list of EqualityConstraints to concatenate.
* @param equality_constraints The list of EqualityConstraints to concatenate.
*/
explicit EqualityConstraints(
const std::vector<EqualityConstraints>& equalityConstraints) {
for (const auto& elem : equalityConstraints) {
const std::vector<EqualityConstraints>& equality_constraints) {
for (const auto& elem : equality_constraints) {
constraints.insert(constraints.end(), elem.constraints.begin(),
elem.constraints.end());
}
@@ -574,20 +577,19 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints {
* @param rhs Right-hand side.
*/
template <typename LHS, typename RHS>
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(SleipnirType<LHS> || SleipnirType<RHS>)
EqualityConstraints(LHS&& lhs, RHS&& rhs)
: constraints{MakeConstraints(lhs, rhs)} {}
: constraints{make_constraints(lhs, rhs)} {}
/**
* Implicit conversion operator to bool.
*/
operator bool() { // NOLINT
return std::all_of(
constraints.begin(), constraints.end(),
[](auto& constraint) { return constraint.Value() == 0.0; });
return std::ranges::all_of(constraints, [](auto& constraint) {
return constraint.value() == 0.0;
});
}
};
@@ -596,17 +598,17 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints {
*/
struct SLEIPNIR_DLLEXPORT InequalityConstraints {
/// A vector of scalar inequality constraints.
wpi::SmallVector<Variable> constraints;
gch::small_vector<Variable> constraints;
/**
* Concatenates multiple inequality constraints.
*
* @param inequalityConstraints The list of InequalityConstraints to
* @param inequality_constraints The list of InequalityConstraints to
* concatenate.
*/
InequalityConstraints( // NOLINT
std::initializer_list<InequalityConstraints> inequalityConstraints) {
for (const auto& elem : inequalityConstraints) {
std::initializer_list<InequalityConstraints> inequality_constraints) {
for (const auto& elem : inequality_constraints) {
constraints.insert(constraints.end(), elem.constraints.begin(),
elem.constraints.end());
}
@@ -617,12 +619,12 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints {
*
* This overload is for Python bindings only.
*
* @param inequalityConstraints The list of InequalityConstraints to
* @param inequality_constraints The list of InequalityConstraints to
* concatenate.
*/
explicit InequalityConstraints(
const std::vector<InequalityConstraints>& inequalityConstraints) {
for (const auto& elem : inequalityConstraints) {
const std::vector<InequalityConstraints>& inequality_constraints) {
for (const auto& elem : inequality_constraints) {
constraints.insert(constraints.end(), elem.constraints.begin(),
elem.constraints.end());
}
@@ -638,20 +640,19 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints {
* @param rhs Right-hand side.
*/
template <typename LHS, typename RHS>
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(SleipnirType<LHS> || SleipnirType<RHS>)
InequalityConstraints(LHS&& lhs, RHS&& rhs)
: constraints{MakeConstraints(lhs, rhs)} {}
: constraints{make_constraints(lhs, rhs)} {}
/**
* Implicit conversion operator to bool.
*/
operator bool() { // NOLINT
return std::all_of(
constraints.begin(), constraints.end(),
[](auto& constraint) { return constraint.Value() >= 0.0; });
return std::ranges::all_of(constraints, [](auto& constraint) {
return constraint.value() >= 0.0;
});
}
};
@@ -662,10 +663,9 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints {
* @param rhs Left-hand side.
*/
template <typename LHS, typename RHS>
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(SleipnirType<LHS> || SleipnirType<RHS>)
EqualityConstraints operator==(LHS&& lhs, RHS&& rhs) {
return EqualityConstraints{lhs, rhs};
}
@@ -678,10 +678,9 @@ EqualityConstraints operator==(LHS&& lhs, RHS&& rhs) {
* @param rhs Left-hand side.
*/
template <typename LHS, typename RHS>
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(SleipnirType<LHS> || SleipnirType<RHS>)
InequalityConstraints operator<(LHS&& lhs, RHS&& rhs) {
return rhs >= lhs;
}
@@ -694,10 +693,9 @@ InequalityConstraints operator<(LHS&& lhs, RHS&& rhs) {
* @param rhs Left-hand side.
*/
template <typename LHS, typename RHS>
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(SleipnirType<LHS> || SleipnirType<RHS>)
InequalityConstraints operator<=(LHS&& lhs, RHS&& rhs) {
return rhs >= lhs;
}
@@ -710,10 +708,9 @@ InequalityConstraints operator<=(LHS&& lhs, RHS&& rhs) {
* @param rhs Left-hand side.
*/
template <typename LHS, typename RHS>
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(SleipnirType<LHS> || SleipnirType<RHS>)
InequalityConstraints operator>(LHS&& lhs, RHS&& rhs) {
return lhs >= rhs;
}
@@ -726,15 +723,14 @@ InequalityConstraints operator>(LHS&& lhs, RHS&& rhs) {
* @param rhs Left-hand side.
*/
template <typename LHS, typename RHS>
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(SleipnirType<LHS> || SleipnirType<RHS>)
InequalityConstraints operator>=(LHS&& lhs, RHS&& rhs) {
return InequalityConstraints{lhs, rhs};
}
} // namespace sleipnir
} // namespace slp
namespace Eigen {
@@ -742,20 +738,28 @@ namespace Eigen {
* NumTraits specialization that allows instantiating Eigen types with Variable.
*/
template <>
struct NumTraits<sleipnir::Variable> : NumTraits<double> {
using Real = sleipnir::Variable;
using NonInteger = sleipnir::Variable;
using Nested = sleipnir::Variable;
struct NumTraits<slp::Variable> : NumTraits<double> {
/// Real type.
using Real = slp::Variable;
/// Non-integer type.
using NonInteger = slp::Variable;
/// Nested type.
using Nested = slp::Variable;
enum {
IsComplex = 0,
IsInteger = 0,
IsSigned = 1,
RequireInitialization = 1,
ReadCost = 1,
AddCost = 3,
MulCost = 3
};
/// Is complex.
static constexpr int IsComplex = 0;
/// Is integer.
static constexpr int IsInteger = 0;
/// Is signed.
static constexpr int IsSigned = 1;
/// Require initialization.
static constexpr int RequireInitialization = 1;
/// Read cost.
static constexpr int ReadCost = 1;
/// Add cost.
static constexpr int AddCost = 3;
/// Multiply cost.
static constexpr int MulCost = 3;
};
} // namespace Eigen

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

@@ -0,0 +1,872 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <concepts>
#include <type_traits>
#include <utility>
#include <Eigen/Core>
#include "sleipnir/autodiff/slice.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/function_ref.hpp"
namespace slp {
/**
* 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:
/**
* Copy constructor.
*/
VariableBlock(const VariableBlock<Mat>&) = default;
/**
* Assigns a VariableBlock to the block.
*
* @param values VariableBlock of values.
* @return This VariableBlock.
*/
VariableBlock<Mat>& operator=(const VariableBlock<Mat>& values) {
if (this == &values) {
return *this;
}
if (m_mat == nullptr) {
m_mat = values.m_mat;
m_row_slice = values.m_row_slice;
m_row_slice_length = values.m_row_slice_length;
m_col_slice = values.m_col_slice;
m_col_slice_length = values.m_col_slice_length;
} else {
slp_assert(rows() == values.rows() && 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;
}
/**
* Move constructor.
*/
VariableBlock(VariableBlock<Mat>&&) = default;
/**
* Assigns a VariableBlock to the block.
*
* @param values VariableBlock of values.
* @return This VariableBlock.
*/
VariableBlock<Mat>& operator=(VariableBlock<Mat>&& values) {
if (this == &values) {
return *this;
}
if (m_mat == nullptr) {
m_mat = values.m_mat;
m_row_slice = values.m_row_slice;
m_row_slice_length = values.m_row_slice_length;
m_col_slice = values.m_col_slice;
m_col_slice_length = values.m_col_slice_length;
} else {
slp_assert(rows() == values.rows() && 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_row_slice{0, mat.rows(), 1},
m_row_slice_length{m_row_slice.adjust(mat.rows())},
m_col_slice{0, mat.cols(), 1},
m_col_slice_length{m_col_slice.adjust(mat.cols())} {}
/**
* Constructs a Variable block pointing to a subset of the given matrix.
*
* @param mat The matrix to which to point.
* @param row_offset The block's row offset.
* @param col_offset The block's column offset.
* @param block_rows The number of rows in the block.
* @param block_cols The number of columns in the block.
*/
VariableBlock(Mat& mat, int row_offset, int col_offset, int block_rows,
int block_cols)
: m_mat{&mat},
m_row_slice{row_offset, row_offset + block_rows, 1},
m_row_slice_length{m_row_slice.adjust(mat.rows())},
m_col_slice{col_offset, col_offset + block_cols, 1},
m_col_slice_length{m_col_slice.adjust(mat.cols())} {}
/**
* Constructs a Variable block pointing to a subset of the given matrix.
*
* Note that the slices are taken as is rather than adjusted.
*
* @param mat The matrix to which to point.
* @param row_slice The block's row slice.
* @param row_slice_length The block's row length.
* @param col_slice The block's column slice.
* @param col_slice_length The block's column length.
*/
VariableBlock(Mat& mat, Slice row_slice, int row_slice_length,
Slice col_slice, int col_slice_length)
: m_mat{&mat},
m_row_slice{std::move(row_slice)},
m_row_slice_length{row_slice_length},
m_col_slice{std::move(col_slice)},
m_col_slice_length{col_slice_length} {}
/**
* Assigns a double to the block.
*
* This only works for blocks with one row and one column.
*
* @param value Value to assign.
* @return This VariableBlock.
*/
VariableBlock<Mat>& operator=(ScalarLike auto value) {
slp_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.
*
* @param value Value to assign.
*/
void set_value(double value) {
slp_assert(rows() == 1 && cols() == 1);
(*this)(0, 0).set_value(value);
}
/**
* Assigns an Eigen matrix to the block.
*
* @param values Eigen matrix of values to assign.
* @return This VariableBlock.
*/
template <typename Derived>
VariableBlock<Mat>& operator=(const Eigen::MatrixBase<Derived>& values) {
slp_assert(rows() == values.rows() && 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.
*
* @param values Eigen matrix of values.
*/
template <typename Derived>
requires std::same_as<typename Derived::Scalar, double>
void set_value(const Eigen::MatrixBase<Derived>& values) {
slp_assert(rows() == values.rows() && cols() == values.cols());
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
(*this)(row, col).set_value(values(row, col));
}
}
}
/**
* Assigns a VariableMatrix to the block.
*
* @param values VariableMatrix of values.
* @return This VariableBlock.
*/
VariableBlock<Mat>& operator=(const Mat& values) {
slp_assert(rows() == values.rows() && 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.
*
* @param values VariableMatrix of values.
* @return This VariableBlock.
*/
VariableBlock<Mat>& operator=(Mat&& values) {
slp_assert(rows() == values.rows() && 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.
* @return A scalar subblock at the given row and column.
*/
Variable& operator()(int row, int col)
requires(!std::is_const_v<Mat>)
{
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
return (*m_mat)(m_row_slice.start + row * m_row_slice.step,
m_col_slice.start + col * m_col_slice.step);
}
/**
* Returns a scalar subblock at the given row and column.
*
* @param row The scalar subblock's row.
* @param col The scalar subblock's column.
* @return A scalar subblock at the given row and column.
*/
const Variable& operator()(int row, int col) const {
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
return (*m_mat)(m_row_slice.start + row * m_row_slice.step,
m_col_slice.start + col * m_col_slice.step);
}
/**
* Returns a scalar subblock at the given row.
*
* @param row The scalar subblock's row.
* @return A scalar subblock at the given row.
*/
Variable& operator[](int row)
requires(!std::is_const_v<Mat>)
{
slp_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.
* @return A scalar subblock at the given row.
*/
const Variable& operator[](int row) const {
slp_assert(row >= 0 && row < rows() * cols());
return (*this)(row / cols(), row % cols());
}
/**
* Returns a block of the variable matrix.
*
* @param row_offset The row offset of the block selection.
* @param col_offset The column offset of the block selection.
* @param block_rows The number of rows in the block selection.
* @param block_cols The number of columns in the block selection.
* @return A block of the variable matrix.
*/
VariableBlock<Mat> block(int row_offset, int col_offset, int block_rows,
int block_cols) {
slp_assert(row_offset >= 0 && row_offset <= rows());
slp_assert(col_offset >= 0 && col_offset <= cols());
slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset);
slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset);
return (*this)({row_offset, row_offset + block_rows, 1},
{col_offset, col_offset + block_cols, 1});
}
/**
* Returns a block slice of the variable matrix.
*
* @param row_offset The row offset of the block selection.
* @param col_offset The column offset of the block selection.
* @param block_rows The number of rows in the block selection.
* @param block_cols The number of columns in the block selection.
* @return A block slice of the variable matrix.
*/
const VariableBlock<const Mat> block(int row_offset, int col_offset,
int block_rows, int block_cols) const {
slp_assert(row_offset >= 0 && row_offset <= rows());
slp_assert(col_offset >= 0 && col_offset <= cols());
slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset);
slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset);
return (*this)({row_offset, row_offset + block_rows, 1},
{col_offset, col_offset + block_cols, 1});
}
/**
* Returns a slice of the variable matrix.
*
* @param row_slice The row slice.
* @param col_slice The column slice.
* @return A slice of the variable matrix.
*/
VariableBlock<Mat> operator()(Slice row_slice, Slice col_slice) {
int row_slice_length = row_slice.adjust(m_row_slice_length);
int col_slice_length = col_slice.adjust(m_col_slice_length);
return VariableBlock{
*m_mat,
{m_row_slice.start + row_slice.start * m_row_slice.step,
m_row_slice.start + row_slice.stop, m_row_slice.step * row_slice.step},
row_slice_length,
{m_col_slice.start + col_slice.start * m_col_slice.step,
m_col_slice.start + col_slice.stop, m_col_slice.step * col_slice.step},
col_slice_length};
}
/**
* Returns a slice of the variable matrix.
*
* @param row_slice The row slice.
* @param col_slice The column slice.
* @return A slice of the variable matrix.
*/
const VariableBlock<const Mat> operator()(Slice row_slice,
Slice col_slice) const {
int row_slice_length = row_slice.adjust(m_row_slice_length);
int col_slice_length = col_slice.adjust(m_col_slice_length);
return VariableBlock{
*m_mat,
{m_row_slice.start + row_slice.start * m_row_slice.step,
m_row_slice.start + row_slice.stop, m_row_slice.step * row_slice.step},
row_slice_length,
{m_col_slice.start + col_slice.start * m_col_slice.step,
m_col_slice.start + col_slice.stop, m_col_slice.step * col_slice.step},
col_slice_length};
}
/**
* Returns a slice of the variable matrix.
*
* The given slices aren't adjusted. This overload is for Python bindings
* only.
*
* @param row_slice The row slice.
* @param row_slice_length The row slice length.
* @param col_slice The column slice.
* @param col_slice_length The column slice length.
* @return A slice of the variable matrix.
*/
VariableBlock<Mat> operator()(Slice row_slice, int row_slice_length,
Slice col_slice, int col_slice_length) {
return VariableBlock{
*m_mat,
{m_row_slice.start + row_slice.start * m_row_slice.step,
m_row_slice.start + row_slice.stop, m_row_slice.step * row_slice.step},
row_slice_length,
{m_col_slice.start + col_slice.start * m_col_slice.step,
m_col_slice.start + col_slice.stop, m_col_slice.step * col_slice.step},
col_slice_length};
}
/**
* Returns a slice of the variable matrix.
*
* The given slices aren't adjusted. This overload is for Python bindings
* only.
*
* @param row_slice The row slice.
* @param row_slice_length The row slice length.
* @param col_slice The column slice.
* @param col_slice_length The column slice length.
* @return A slice of the variable matrix.
*/
const VariableBlock<const Mat> operator()(Slice row_slice,
int row_slice_length,
Slice col_slice,
int col_slice_length) const {
return VariableBlock{
*m_mat,
{m_row_slice.start + row_slice.start * m_row_slice.step,
m_row_slice.start + row_slice.stop, m_row_slice.step * row_slice.step},
row_slice_length,
{m_col_slice.start + col_slice.start * m_col_slice.step,
m_col_slice.start + col_slice.stop, m_col_slice.step * col_slice.step},
col_slice_length};
}
/**
* Returns a segment of the variable vector.
*
* @param offset The offset of the segment.
* @param length The length of the segment.
* @return A segment of the variable vector.
*/
VariableBlock<Mat> segment(int offset, int length) {
slp_assert(offset >= 0 && offset < rows() * cols());
slp_assert(length >= 0 && length <= rows() * cols() - offset);
return block(offset, 0, length, 1);
}
/**
* Returns a segment of the variable vector.
*
* @param offset The offset of the segment.
* @param length The length of the segment.
* @return A segment of the variable vector.
*/
const VariableBlock<Mat> segment(int offset, int length) const {
slp_assert(offset >= 0 && offset < rows() * cols());
slp_assert(length >= 0 && length <= rows() * cols() - offset);
return block(offset, 0, length, 1);
}
/**
* Returns a row slice of the variable matrix.
*
* @param row The row to slice.
* @return A row slice of the variable matrix.
*/
VariableBlock<Mat> row(int row) {
slp_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.
* @return A row slice of the variable matrix.
*/
VariableBlock<const Mat> row(int row) const {
slp_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.
* @return A column slice of the variable matrix.
*/
VariableBlock<Mat> col(int col) {
slp_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.
* @return A column slice of the variable matrix.
*/
VariableBlock<const Mat> col(int col) const {
slp_assert(col >= 0 && col < cols());
return block(0, col, rows(), 1);
}
/**
* Compound matrix multiplication-assignment operator.
*
* @param rhs Variable to multiply.
* @return Result of multiplication.
*/
VariableBlock<Mat>& operator*=(const MatrixLike auto& rhs) {
slp_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.
*
* @param rhs Variable to multiply.
* @return Result of multiplication.
*/
VariableBlock<Mat>& operator*=(const ScalarLike auto& 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.
*
* @param rhs Variable to divide.
* @return Result of division.
*/
VariableBlock<Mat>& operator/=(const MatrixLike auto& rhs) {
slp_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.
*
* @param rhs Variable to divide.
* @return Result of division.
*/
VariableBlock<Mat>& operator/=(const ScalarLike auto& 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.
* @return Result of addition.
*/
VariableBlock<Mat>& operator+=(const MatrixLike auto& rhs) {
slp_assert(rows() == rhs.rows() && cols() == rhs.cols());
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
(*this)(row, col) += rhs(row, col);
}
}
return *this;
}
/**
* Compound addition-assignment operator.
*
* @param rhs Variable to add.
* @return Result of addition.
*/
VariableBlock<Mat>& operator+=(const ScalarLike auto& rhs) {
slp_assert(rows() == 1 && cols() == 1);
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
(*this)(row, col) += rhs;
}
}
return *this;
}
/**
* Compound subtraction-assignment operator.
*
* @param rhs Variable to subtract.
* @return Result of subtraction.
*/
VariableBlock<Mat>& operator-=(const MatrixLike auto& rhs) {
slp_assert(rows() == rhs.rows() && cols() == rhs.cols());
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.
* @return Result of subtraction.
*/
VariableBlock<Mat>& operator-=(const ScalarLike auto& rhs) {
slp_assert(rows() == 1 && cols() == 1);
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
(*this)(row, col) -= rhs;
}
}
return *this;
}
/**
* Implicit conversion operator from 1x1 VariableBlock to Variable.
*/
operator Variable() const { // NOLINT
slp_assert(rows() == 1 && cols() == 1);
return (*this)(0, 0);
}
/**
* Returns the transpose of the variable matrix.
*
* @return The transpose of the variable matrix.
*/
std::remove_cv_t<Mat> T() const {
std::remove_cv_t<Mat> result{Mat::empty, 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 the number of rows in the matrix.
*
* @return The number of rows in the matrix.
*/
int rows() const { return m_row_slice_length; }
/**
* Returns the number of columns in the matrix.
*
* @return The number of columns in the matrix.
*/
int cols() const { return m_col_slice_length; }
/**
* 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.
* @return An element of the variable matrix.
*/
double value(int row, int col) {
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
return (*m_mat)(m_row_slice.start + row * m_row_slice.step,
m_col_slice.start + col * m_col_slice.step)
.value();
}
/**
* Returns a row of the variable column vector.
*
* @param index The index of the element to return.
* @return A row of the variable column vector.
*/
double value(int index) {
slp_assert(index >= 0 && index < rows() * cols());
return value(index / cols(), index % cols());
}
/**
* Returns the contents of the variable matrix.
*
* @return The contents of the variable matrix.
*/
Eigen::MatrixXd value() {
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 unary_op The unary operator to use for the transform operation.
* @return Result of the unary operator.
*/
std::remove_cv_t<Mat> cwise_transform(
function_ref<Variable(const Variable& x)> unary_op) const {
std::remove_cv_t<Mat> result{Mat::empty, rows(), cols()};
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
result(row, col) = unary_op((*this)(row, col));
}
}
return result;
}
#ifndef DOXYGEN_SHOULD_SKIP_THIS
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&;
constexpr iterator() noexcept = default;
constexpr iterator(VariableBlock<Mat>* mat, int index) noexcept
: m_mat{mat}, m_index{index} {}
constexpr iterator& operator++() noexcept {
++m_index;
return *this;
}
constexpr iterator operator++(int) noexcept {
iterator retval = *this;
++(*this);
return retval;
}
constexpr bool operator==(const iterator&) const noexcept = default;
constexpr reference operator*() const noexcept { return (*m_mat)[m_index]; }
private:
VariableBlock<Mat>* m_mat = nullptr;
int m_index = 0;
};
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&;
constexpr const_iterator() noexcept = default;
constexpr const_iterator(const VariableBlock<Mat>* mat, int index) noexcept
: m_mat{mat}, m_index{index} {}
constexpr const_iterator& operator++() noexcept {
++m_index;
return *this;
}
constexpr const_iterator operator++(int) noexcept {
const_iterator retval = *this;
++(*this);
return retval;
}
constexpr bool operator==(const const_iterator&) const noexcept = default;
constexpr const_reference operator*() const noexcept {
return (*m_mat)[m_index];
}
private:
const VariableBlock<Mat>* m_mat = nullptr;
int m_index = 0;
};
#endif // DOXYGEN_SHOULD_SKIP_THIS
/**
* Returns begin iterator.
*
* @return Begin iterator.
*/
iterator begin() { return iterator(this, 0); }
/**
* Returns end iterator.
*
* @return End iterator.
*/
iterator end() { return iterator(this, rows() * cols()); }
/**
* Returns begin iterator.
*
* @return Begin iterator.
*/
const_iterator begin() const { return const_iterator(this, 0); }
/**
* Returns end iterator.
*
* @return End iterator.
*/
const_iterator end() const { return const_iterator(this, rows() * cols()); }
/**
* Returns begin iterator.
*
* @return Begin iterator.
*/
const_iterator cbegin() const { return const_iterator(this, 0); }
/**
* Returns end iterator.
*
* @return End iterator.
*/
const_iterator cend() const { return const_iterator(this, rows() * cols()); }
/**
* Returns number of elements in matrix.
*
* @return Number of elements in matrix.
*/
size_t size() const { return rows() * cols(); }
private:
Mat* m_mat = nullptr;
Slice m_row_slice;
int m_row_slice_length = 0;
Slice m_col_slice;
int m_col_slice_length = 0;
};
} // namespace slp

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

File diff suppressed because it is too large Load Diff

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

@@ -7,14 +7,14 @@
#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"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/optimization/problem.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/concepts.hpp"
#include "sleipnir/util/function_ref.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace sleipnir {
namespace slp {
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(t, x, u) for dt.
@@ -26,7 +26,7 @@ namespace sleipnir {
* @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) {
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);
@@ -42,13 +42,13 @@ State RK4(F&& f, State x, Input u, Time t0, Time dt) {
enum class TranscriptionMethod : uint8_t {
/// Each state is a decision variable constrained to the integrated dynamics
/// of the previous state.
kDirectTranscription,
DIRECT_TRANSCRIPTION,
/// The trajectory is modeled as a series of cubic polynomials where the
/// centerpoint slope is constrained.
kDirectCollocation,
DIRECT_COLLOCATION,
/// States depend explicitly as a function of all previous states and all
/// previous inputs.
kSingleShooting
SINGLE_SHOOTING
};
/**
@@ -56,9 +56,9 @@ enum class TranscriptionMethod : uint8_t {
*/
enum class DynamicsType : uint8_t {
/// The dynamics are a function in the form dx/dt = f(t, x, u).
kExplicitODE,
EXPLICIT_ODE,
/// The dynamics are a function in the form xₖ₊₁ = f(t, xₖ, uₖ).
kDiscrete
DISCRETE
};
/**
@@ -66,12 +66,12 @@ enum class DynamicsType : uint8_t {
*/
enum class TimestepMethod : uint8_t {
/// The timestep is a fixed constant.
kFixed,
FIXED,
/// The timesteps are allowed to vary as independent decision variables.
kVariable,
VARIABLE,
/// The timesteps are equal length but allowed to vary as a single decision
/// variable.
kVariableSingle
VARIABLE_SINGLE
};
/**
@@ -100,179 +100,177 @@ enum class TimestepMethod : uint8_t {
* https://underactuated.mit.edu/trajopt.html goes into more detail on each
* transcription method.
*/
class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem {
class SLEIPNIR_DLLEXPORT OCP : public Problem {
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 num_states The number of system states.
* @param num_inputs The number of system inputs.
* @param dt The timestep for fixed-step integration.
* @param numSteps The number of control points.
* @param num_steps The number of control points.
* @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 dynamics_type The type of system evolution function.
* @param timestep_method The timestep method.
* @param method The transcription method.
*/
OCPSolver(
int numStates, int numInputs, std::chrono::duration<double> dt,
int numSteps,
OCP(int num_states, int num_inputs, std::chrono::duration<double> dt,
int num_steps,
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} {}
DynamicsType dynamics_type = DynamicsType::EXPLICIT_ODE,
TimestepMethod timestep_method = TimestepMethod::FIXED,
TranscriptionMethod method = TranscriptionMethod::DIRECT_TRANSCRIPTION)
: OCP{num_states,
num_inputs,
dt,
num_steps,
[=]([[maybe_unused]] const VariableMatrix& t,
const VariableMatrix& x, const VariableMatrix& u,
[[maybe_unused]]
const VariableMatrix& dt) -> VariableMatrix {
return dynamics(x, u);
},
dynamics_type,
timestep_method,
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 num_states The number of system states.
* @param num_inputs The number of system inputs.
* @param dt The timestep for fixed-step integration.
* @param numSteps The number of control points.
* @param num_steps 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 dynamics_type The type of system evolution function.
* @param timestep_method The timestep method.
* @param method The transcription method.
*/
OCPSolver(
int numStates, int numInputs, std::chrono::duration<double> dt,
int numSteps,
OCP(int num_states, int num_inputs, std::chrono::duration<double> dt,
int num_steps,
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)
: m_numStates{numStates},
m_numInputs{numInputs},
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_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);
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} {
// u is num_steps + 1 so that the final constraint function evaluation works
m_U = decision_variable(m_num_inputs, m_num_steps + 1);
if (m_timestepMethod == TimestepMethod::kFixed) {
m_DT = VariableMatrix{1, m_numSteps + 1};
for (int i = 0; i < numSteps + 1; ++i) {
if (m_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();
}
} else if (m_timestepMethod == TimestepMethod::kVariableSingle) {
Variable DT = DecisionVariable();
DT.SetValue(m_dt.count());
} else if (m_timestep_method == TimestepMethod::VARIABLE_SINGLE) {
Variable dt = decision_variable();
dt.set_value(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;
m_DT = VariableMatrix{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 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());
} else if (m_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());
}
}
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) {
if (m_transcription_method == TranscriptionMethod::DIRECT_TRANSCRIPTION) {
m_X = decision_variable(m_num_states, m_num_steps + 1);
constrain_direct_transcription();
} else if (m_transcription_method ==
TranscriptionMethod::DIRECT_COLLOCATION) {
m_X = decision_variable(m_num_states, m_num_steps + 1);
constrain_direct_collocation();
} else if (m_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_numStates, m_numSteps + 1};
ConstrainSingleShooting();
m_X = VariableMatrix{m_num_states, m_num_steps + 1};
constrain_single_shooting();
}
}
/**
* Utility function to constrain the initial state.
*
* @param initialState the initial state to constrain to.
* @param initial_state the initial state to constrain to.
*/
template <typename T>
requires ScalarLike<T> || MatrixLike<T>
void ConstrainInitialState(const T& initialState) {
SubjectTo(InitialState() == initialState);
void constrain_initial_state(const T& initial_state) {
subject_to(this->initial_state() == initial_state);
}
/**
* Utility function to constrain the final state.
*
* @param finalState the final state to constrain to.
* @param final_state the final state to constrain to.
*/
template <typename T>
requires ScalarLike<T> || MatrixLike<T>
void ConstrainFinalState(const T& finalState) {
SubjectTo(FinalState() == finalState);
void constrain_final_state(const T& final_state) {
subject_to(this->final_state() == final_state);
}
/**
* Set the constraint evaluation function. This function is called
* `numSteps+1` times, with the corresponding state and input
* `num_steps+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(
void for_each_step(
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);
for (int i = 0; i < m_num_steps + 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
* `num_steps+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(
void for_each_step(
const function_ref<void(const Variable& t, const VariableMatrix& x,
const VariableMatrix& u, const Variable& dt)>
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);
for (int i = 0; i < m_num_steps + 1; ++i) {
auto x = X().col(i);
auto u = U().col(i);
auto dt = this->dt()(0, i);
callback(time, x, u, dt);
time += dt;
@@ -282,115 +280,115 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem {
/**
* 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.
* @param lower_bound The lower bound that inputs must always be above. Must
* be shaped (num_inputs)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);
void set_lower_input_bound(const T& lower_bound) {
for (int i = 0; i < m_num_steps + 1; ++i) {
subject_to(U().col(i) >= lower_bound);
}
}
/**
* 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.
* @param upper_bound The upper bound that inputs must always be below. Must
* be shaped (num_inputs)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);
void set_upper_input_bound(const T& upper_bound) {
for (int i = 0; i < m_num_steps + 1; ++i) {
subject_to(U().col(i) <= upper_bound);
}
}
/**
* Convenience function to set a lower bound on the timestep.
*
* @param minTimestep The minimum timestep.
* @param min_timestep The minimum timestep.
*/
void SetMinTimestep(std::chrono::duration<double> minTimestep) {
SubjectTo(DT() >= minTimestep.count());
void set_min_timestep(std::chrono::duration<double> min_timestep) {
subject_to(dt() >= min_timestep.count());
}
/**
* Convenience function to set an upper bound on the timestep.
*
* @param maxTimestep The maximum timestep.
* @param max_timestep The maximum timestep.
*/
void SetMaxTimestep(std::chrono::duration<double> maxTimestep) {
SubjectTo(DT() <= maxTimestep.count());
void set_max_timestep(std::chrono::duration<double> max_timestep) {
subject_to(dt() <= max_timestep.count());
}
/**
* Get the state variables. After the problem is solved, this will contain the
* optimized trajectory.
*
* Shaped (numStates)x(numSteps+1).
* Shaped (num_states)x(num_steps+1).
*
* @returns The state variable matrix.
* @return The state variable matrix.
*/
VariableMatrix& X() { return m_X; };
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.
* Shaped (num_inputs)x(num_steps+1), although the last input step is unused
* in the trajectory.
*
* @returns The input variable matrix.
* @return The input variable matrix.
*/
VariableMatrix& U() { return m_U; };
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
* Shaped 1x(num_steps+1), although the last timestep is unused in
* the trajectory.
*
* @returns The timestep variable matrix.
* @return The timestep variable matrix.
*/
VariableMatrix& DT() { return m_DT; };
VariableMatrix& dt() { return m_DT; }
/**
* Convenience function to get the initial state in the trajectory.
*
* @returns The initial state of the trajectory.
* @return The initial state of the trajectory.
*/
VariableMatrix InitialState() { return m_X.Col(0); }
VariableMatrix initial_state() { return m_X.col(0); }
/**
* Convenience function to get the final state in the trajectory.
*
* @returns The final state of the trajectory.
* @return The final state of the trajectory.
*/
VariableMatrix FinalState() { return m_X.Col(m_numSteps); }
VariableMatrix final_state() { return m_X.col(m_num_steps); }
private:
void ConstrainDirectCollocation() {
Assert(m_dynamicsType == DynamicsType::kExplicitODE);
void constrain_direct_collocation() {
slp_assert(m_dynamics_type == DynamicsType::EXPLICIT_ODE);
Variable time = 0.0;
// Derivation at https://mec560sbu.github.io/2016/09/30/direct_collocation/
for (int i = 0; i < m_numSteps; ++i) {
Variable h = DT()(0, i);
for (int i = 0; i < m_num_steps; ++i) {
Variable h = dt()(0, i);
auto& f = m_dynamicsFunction;
auto& f = m_dynamics_function;
auto t_begin = time;
auto t_end = t_begin + h;
auto x_begin = X().Col(i);
auto x_end = X().Col(i + 1);
auto x_begin = X().col(i);
auto x_end = X().col(i + 1);
auto u_begin = U().Col(i);
auto u_end = U().Col(i + 1);
auto u_begin = U().col(i);
auto u_end = U().col(i + 1);
auto xdot_begin = f(t_begin, x_begin, u_begin, h);
auto xdot_end = f(t_end, x_end, u_end, h);
@@ -401,71 +399,71 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem {
auto x_c = 0.5 * (x_begin + x_end) + h / 8 * (xdot_begin - xdot_end);
auto u_c = 0.5 * (u_begin + u_end);
SubjectTo(xdot_c == f(t_c, x_c, u_c, h));
subject_to(xdot_c == f(t_c, x_c, u_c, h));
time += h;
}
}
void ConstrainDirectTranscription() {
void constrain_direct_transcription() {
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);
for (int i = 0; i < m_num_steps; ++i) {
auto x_begin = X().col(i);
auto x_end = X().col(i + 1);
auto u = U().col(i);
Variable dt = this->dt()(0, i);
if (m_dynamicsType == DynamicsType::kExplicitODE) {
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));
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));
} else if (m_dynamics_type == DynamicsType::DISCRETE) {
subject_to(x_end == m_dynamics_function(time, x_begin, u, dt));
}
time += dt;
}
}
void ConstrainSingleShooting() {
void constrain_single_shooting() {
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);
for (int i = 0; i < m_num_steps; ++i) {
auto x_begin = X().col(i);
auto x_end = X().col(i + 1);
auto u = U().col(i);
Variable dt = this->dt()(0, i);
if (m_dynamicsType == DynamicsType::kExplicitODE) {
x_end = RK4<const decltype(m_dynamicsFunction)&, VariableMatrix,
VariableMatrix, Variable>(m_dynamicsFunction, x_begin, u,
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);
} else if (m_dynamicsType == DynamicsType::kDiscrete) {
x_end = m_dynamicsFunction(time, x_begin, u, dt);
} else if (m_dynamics_type == DynamicsType::DISCRETE) {
x_end = m_dynamics_function(time, x_begin, u, dt);
}
time += dt;
}
}
int m_numStates;
int m_numInputs;
int m_num_states;
int m_num_inputs;
std::chrono::duration<double> m_dt;
int m_numSteps;
TranscriptionMethod m_transcriptionMethod;
int m_num_steps;
TranscriptionMethod m_transcription_method;
DynamicsType m_dynamicsType;
DynamicsType m_dynamics_type;
function_ref<VariableMatrix(const Variable& t, const VariableMatrix& x,
const VariableMatrix& u, const Variable& dt)>
m_dynamicsFunction;
m_dynamics_function;
TimestepMethod m_timestepMethod;
TimestepMethod m_timestep_method;
VariableMatrix m_X;
VariableMatrix m_U;
VariableMatrix m_DT;
};
} // namespace sleipnir
} // namespace slp

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

@@ -1,386 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <algorithm>
#include <array>
#include <concepts>
#include <functional>
#include <iterator>
#include <optional>
#include <utility>
#include <Eigen/Core>
#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/autodiff/VariableMatrix.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/optimization/solver/SQP.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.
*/
class SLEIPNIR_DLLEXPORT OptimizationProblem {
public:
/**
* Construct the optimization problem.
*/
OptimizationProblem() noexcept = default;
/**
* 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
if (m_inequalityConstraints.empty()) {
SQP(m_decisionVariables, m_equalityConstraints, m_f.value(), m_callback,
config, x, &status);
} else {
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 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) {
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 requires(F callback, const SolverIterationInfo& info) {
{ callback(info) } -> std::same_as<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
wpi::SmallVector<Variable> m_decisionVariables;
// The cost function: f(x)
std::optional<Variable> m_f;
// The list of equality constraints: cₑ(x) = 0
wpi::SmallVector<Variable> m_equalityConstraints;
// The list of inequality constraints: cᵢ(x) ≥ 0
wpi::SmallVector<Variable> m_inequalityConstraints;
// The user callback
std::function<bool(const SolverIterationInfo& info)> 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
View File

@@ -1,54 +0,0 @@
// 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

80
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp vendored
View File

@@ -1,80 +0,0 @@
// 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) {
using enum SolverExitCondition;
switch (exitCondition) {
case kSuccess:
return "solved to desired tolerance";
case kSolvedToAcceptableTolerance:
return "solved to acceptable tolerance";
case kCallbackRequestedStop:
return "callback requested stop";
case kTooFewDOFs:
return "problem has too few degrees of freedom";
case kLocallyInfeasible:
return "problem is locally infeasible";
case kFeasibilityRestorationFailed:
return "solver failed to reach the desired tolerance, and feasibility "
"restoration failed to converge";
case kNonfiniteInitialCostOrConstraints:
return "solver encountered nonfinite initial cost or constraints and "
"gave up";
case kDivergingIterates:
return "solver encountered diverging primal iterates xₖ and/or sₖ and "
"gave up";
case kMaxIterationsExceeded:
return "solution returned after maximum iterations exceeded";
case kTimeout:
return "solution returned after maximum wall clock time exceeded";
default:
return "unknown";
}
}
} // namespace sleipnir

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

@@ -1,32 +0,0 @@
// 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

50
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Multistart.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/multistart.hpp vendored
View File

@@ -6,12 +6,12 @@
#include <future>
#include <span>
#include <wpi/SmallVector.h>
#include <gch/small_vector.hpp>
#include "sleipnir/optimization/SolverStatus.hpp"
#include "sleipnir/util/FunctionRef.hpp"
#include "sleipnir/optimization/solver/exit_status.hpp"
#include "sleipnir/util/function_ref.hpp"
namespace sleipnir {
namespace slp {
/**
* The result of a multistart solve.
@@ -21,7 +21,11 @@ namespace sleipnir {
*/
template <typename DecisionVariables>
struct MultistartResult {
SolverStatus status;
/// The solver exit status.
ExitStatus status;
/// The solution's cost.
double cost;
/// The decision variables.
DecisionVariables variables;
};
@@ -37,39 +41,37 @@ struct MultistartResult {
* 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.
* @param initial_guesses A list of decision variable initial guesses to try.
*/
template <typename DecisionVariables>
MultistartResult<DecisionVariables> Multistart(
function_ref<MultistartResult<DecisionVariables>(
const DecisionVariables& initialGuess)>
const DecisionVariables& initial_guess)>
solve,
std::span<const DecisionVariables> initialGuesses) {
wpi::SmallVector<std::future<MultistartResult<DecisionVariables>>> futures;
futures.reserve(initialGuesses.size());
std::span<const DecisionVariables> initial_guesses) {
gch::small_vector<std::future<MultistartResult<DecisionVariables>>> futures;
futures.reserve(initial_guesses.size());
for (const auto& initialGuess : initialGuesses) {
futures.emplace_back(std::async(std::launch::async, solve, initialGuess));
for (const auto& initial_guess : initial_guesses) {
futures.emplace_back(std::async(std::launch::async, solve, initial_guess));
}
wpi::SmallVector<MultistartResult<DecisionVariables>> results;
gch::small_vector<MultistartResult<DecisionVariables>> results;
results.reserve(futures.size());
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;
}
return *std::ranges::min_element(results, [](const auto& a, const auto& b) {
// Prioritize successful solve
if (a.status == ExitStatus::SUCCESS && b.status != ExitStatus::SUCCESS) {
return true;
}
// Otherwise prioritize solution with lower cost
return a.status.cost < b.status.cost;
});
// Otherwise prioritize solution with lower cost
return a.cost < b.cost;
});
}
} // namespace sleipnir
} // namespace slp

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

@@ -0,0 +1,342 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <algorithm>
#include <concepts>
#include <functional>
#include <iterator>
#include <optional>
#include <ranges>
#include <utility>
#include <Eigen/Core>
#include <gch/small_vector.hpp>
#include "sleipnir/autodiff/expression_type.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/optimization/solver/exit_status.hpp"
#include "sleipnir/optimization/solver/iteration_info.hpp"
#include "sleipnir/optimization/solver/options.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
/**
* 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.
*/
class SLEIPNIR_DLLEXPORT Problem {
public:
/**
* Construct the optimization problem.
*/
Problem() noexcept = default;
/**
* Create a decision variable in the optimization problem.
*
* @return A decision variable in the optimization problem.
*/
[[nodiscard]]
Variable decision_variable() {
m_decision_variables.emplace_back();
return m_decision_variables.back();
}
/**
* Create a matrix of decision variables in the optimization problem.
*
* @param rows Number of matrix rows.
* @param cols Number of matrix columns.
* @return A matrix of decision variables in the optimization problem.
*/
[[nodiscard]]
VariableMatrix decision_variable(int rows, int cols = 1) {
m_decision_variables.reserve(m_decision_variables.size() + rows * cols);
VariableMatrix vars{rows, cols};
for (int row = 0; row < rows; ++row) {
for (int col = 0; col < cols; ++col) {
m_decision_variables.emplace_back();
vars(row, col) = m_decision_variables.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.
* @return A symmetric matrix of decision varaibles in the optimization
* problem.
*/
[[nodiscard]]
VariableMatrix symmetric_decision_variable(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_decision_variables.reserve(m_decision_variables.size() +
(rows * rows + rows) / 2);
VariableMatrix vars{rows, rows};
for (int row = 0; row < rows; ++row) {
for (int col = 0; col <= row; ++col) {
m_decision_variables.emplace_back();
vars(row, col) = m_decision_variables.back();
vars(col, row) = m_decision_variables.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; }
/**
* 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); }
/**
* 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;
}
/**
* 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);
}
/**
* Tells the solver to solve the problem while satisfying the given equality
* constraint.
*
* @param constraint The constraint to satisfy.
*/
void subject_to(const EqualityConstraints& constraint) {
m_equality_constraints.reserve(m_equality_constraints.size() +
constraint.constraints.size());
std::ranges::copy(constraint.constraints,
std::back_inserter(m_equality_constraints));
}
/**
* Tells the solver to solve the problem while satisfying the given equality
* constraint.
*
* @param constraint The constraint to satisfy.
*/
void subject_to(EqualityConstraints&& constraint) {
m_equality_constraints.reserve(m_equality_constraints.size() +
constraint.constraints.size());
std::ranges::copy(constraint.constraints,
std::back_inserter(m_equality_constraints));
}
/**
* Tells the solver to solve the problem while satisfying the given inequality
* constraint.
*
* @param constraint The constraint to satisfy.
*/
void subject_to(const InequalityConstraints& constraint) {
m_inequality_constraints.reserve(m_inequality_constraints.size() +
constraint.constraints.size());
std::ranges::copy(constraint.constraints,
std::back_inserter(m_inequality_constraints));
}
/**
* Tells the solver to solve the problem while satisfying the given inequality
* constraint.
*
* @param constraint The constraint to satisfy.
*/
void subject_to(InequalityConstraints&& constraint) {
m_inequality_constraints.reserve(m_inequality_constraints.size() +
constraint.constraints.size());
std::ranges::copy(constraint.constraints,
std::back_inserter(m_inequality_constraints));
}
/**
* Returns the cost function's type.
*
* @return The cost function's type.
*/
ExpressionType cost_function_type() const {
if (m_f) {
return m_f.value().type();
} else {
return ExpressionType::NONE;
}
}
/**
* Returns the type of the highest order equality constraint.
*
* @return The type of the highest order equality constraint.
*/
ExpressionType equality_constraint_type() const {
if (!m_equality_constraints.empty()) {
return std::ranges::max(m_equality_constraints, {}, &Variable::type)
.type();
} else {
return ExpressionType::NONE;
}
}
/**
* Returns the type of the highest order inequality constraint.
*
* @return The type of the highest order inequality constraint.
*/
ExpressionType inequality_constraint_type() const {
if (!m_inequality_constraints.empty()) {
return std::ranges::max(m_inequality_constraints, {}, &Variable::type)
.type();
} else {
return ExpressionType::NONE;
}
}
/**
* Solve the optimization problem. The solution will be stored in the original
* variables used to construct the problem.
*
* @param options Solver options.
* @param spy 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.
* @return The solver status.
*/
ExitStatus solve(const Options& options = Options{},
[[maybe_unused]] bool spy = false);
/**
* Adds a callback to be called at the beginning of each solver iteration.
*
* The callback for this overload should return void.
*
* @param callback The callback.
*/
template <typename F>
requires requires(F callback, const IterationInfo& info) {
{ callback(info) } -> std::same_as<void>;
}
void add_callback(F&& callback) {
m_iteration_callbacks.emplace_back(
[=, callback = std::forward<F>(callback)](const IterationInfo& info) {
callback(info);
return false;
});
}
/**
* Adds a callback to be called at the beginning of 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 requires(F callback, const IterationInfo& info) {
{ callback(info) } -> std::same_as<bool>;
}
void add_callback(F&& callback) {
m_iteration_callbacks.emplace_back(std::forward<F>(callback));
}
/**
* Clears the registered callbacks.
*/
void clear_callbacks() { m_iteration_callbacks.clear(); }
private:
// The list of decision variables, which are the root of the problem's
// expression tree
gch::small_vector<Variable> m_decision_variables;
// The cost function: f(x)
std::optional<Variable> m_f;
// The list of equality constraints: cₑ(x) = 0
gch::small_vector<Variable> m_equality_constraints;
// The list of inequality constraints: cᵢ(x) ≥ 0
gch::small_vector<Variable> m_inequality_constraints;
// The iteration callbacks
gch::small_vector<std::function<bool(const IterationInfo& info)>>
m_iteration_callbacks;
void print_exit_conditions([[maybe_unused]] const Options& options);
void print_problem_analysis();
};
} // namespace slp

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

@@ -1,55 +0,0 @@
// 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& info)> callback,
const SolverConfig& config, bool feasibilityRestoration, Eigen::VectorXd& x,
Eigen::VectorXd& s, SolverStatus* status);
} // namespace sleipnir

46
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp vendored
View File

@@ -1,46 +0,0 @@
// 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 Sequential Quadratic
Programming (SQP).
A nonlinear program has the form:
@verbatim
min_x f(x)
subject to cₑ(x) = 0
@endverbatim
where f(x) is the cost function and cₑ(x) are the equality constraints.
@param[in] decisionVariables The list of decision variables.
@param[in] equalityConstraints The list of equality constraints.
@param[in] f The cost function.
@param[in] callback The user callback.
@param[in] config Configuration options for the solver.
@param[in,out] x The initial guess and output location for the decision
variables.
@param[out] status The solver status.
*/
SLEIPNIR_DLLEXPORT void SQP(
std::span<Variable> decisionVariables,
std::span<Variable> equalityConstraints, Variable& f,
function_ref<bool(const SolverIterationInfo& info)> callback,
const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status);
} // namespace sleipnir

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

@@ -0,0 +1,82 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <stdint.h>
#include <string_view>
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
/**
* Solver exit status. Negative values indicate failure.
*/
enum class ExitStatus : int8_t {
/// Solved the problem to the desired tolerance.
SUCCESS = 0,
/// The solver returned its solution so far after the user requested a stop.
CALLBACK_REQUESTED_STOP = 1,
/// The solver determined the problem to be overconstrained and gave up.
TOO_FEW_DOFS = -1,
/// The solver determined the problem to be locally infeasible and gave up.
LOCALLY_INFEASIBLE = -2,
/// The problem setup frontend determined the problem to have an empty
/// feasible region.
GLOBALLY_INFEASIBLE = -3,
/// The linear system factorization failed.
FACTORIZATION_FAILED = -4,
/// The backtracking line search failed, and the problem isn't locally
/// infeasible.
LINE_SEARCH_FAILED = -5,
/// The solver encountered nonfinite initial cost or constraints and gave up.
NONFINITE_INITIAL_COST_OR_CONSTRAINTS = -6,
/// The solver encountered diverging primal iterates xₖ and/or sₖ and gave up.
DIVERGING_ITERATES = -7,
/// The solver returned its solution so far after exceeding the maximum number
/// of iterations.
MAX_ITERATIONS_EXCEEDED = -8,
/// The solver returned its solution so far after exceeding the maximum
/// elapsed wall clock time.
TIMEOUT = -9,
};
/**
* Returns user-readable message corresponding to the solver exit status.
*
* @param exit_status Solver exit status.
*/
SLEIPNIR_DLLEXPORT constexpr std::string_view to_message(
const ExitStatus& exit_status) {
using enum ExitStatus;
switch (exit_status) {
case SUCCESS:
return "success";
case CALLBACK_REQUESTED_STOP:
return "callback requested stop";
case TOO_FEW_DOFS:
return "too few degrees of freedom";
case LOCALLY_INFEASIBLE:
return "locally infeasible";
case GLOBALLY_INFEASIBLE:
return "globally infeasible";
case FACTORIZATION_FAILED:
return "factorization failed";
case LINE_SEARCH_FAILED:
return "line search failed";
case NONFINITE_INITIAL_COST_OR_CONSTRAINTS:
return "nonfinite initial cost or constraints";
case DIVERGING_ITERATES:
return "diverging iterates";
case MAX_ITERATIONS_EXCEEDED:
return "max iterations exceeded";
case TIMEOUT:
return "timeout";
default:
return "unknown";
}
}
} // namespace slp

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

@@ -0,0 +1,232 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <functional>
#include <span>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include "sleipnir/optimization/solver/exit_status.hpp"
#include "sleipnir/optimization/solver/iteration_info.hpp"
#include "sleipnir/optimization/solver/options.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
/**
* Matrix callbacks for the interior-point method solver.
*/
struct SLEIPNIR_DLLEXPORT InteriorPointMatrixCallbacks {
/// Cost function value f(x) getter.
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>f(x)</td>
/// <td>1</td>
/// <td>1</td>
/// </tr>
/// </table>
std::function<double(const Eigen::VectorXd& x)> f;
/// Cost function gradient ∇f(x) getter.
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>∇f(x)</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::SparseVector<double>(const Eigen::VectorXd& x)> g;
/// Lagrangian Hessian ∇ₓₓ²L(x, y, z) getter.
///
/// L(xₖ, yₖ, zₖ) = f(xₖ) − yₖᵀcₑ(xₖ) − zₖᵀcᵢ(xₖ)
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>y</td>
/// <td>num_equality_constraints</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>z</td>
/// <td>num_inequality_constraints</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>∇ₓₓ²L(x, y, z)</td>
/// <td>num_decision_variables</td>
/// <td>num_decision_variables</td>
/// </tr>
/// </table>
std::function<Eigen::SparseMatrix<double>(const Eigen::VectorXd& x,
const Eigen::VectorXd& y,
const Eigen::VectorXd& z)>
H;
/// Equality constraint value cₑ(x) getter.
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>cₑ(x)</td>
/// <td>num_equality_constraints</td>
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::VectorXd(const Eigen::VectorXd& x)> c_e;
/// Equality constraint Jacobian ∂cₑ/∂x getter.
///
/// @verbatim
/// [∇ᵀcₑ₁(xₖ)]
/// Aₑ(x) = [∇ᵀcₑ₂(xₖ)]
/// [ ⋮ ]
/// [∇ᵀcₑₘ(xₖ)]
/// @endverbatim
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>Aₑ(x)</td>
/// <td>num_equality_constraints</td>
/// <td>num_decision_variables</td>
/// </tr>
/// </table>
std::function<Eigen::SparseMatrix<double>(const Eigen::VectorXd& x)> A_e;
/// Inequality constraint value cᵢ(x) getter.
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>cᵢ(x)</td>
/// <td>num_inequality_constraints</td>
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::VectorXd(const Eigen::VectorXd& x)> c_i;
/// Inequality constraint Jacobian ∂cᵢ/∂x getter.
///
/// @verbatim
/// [∇ᵀcᵢ₁(xₖ)]
/// Aᵢ(x) = [∇ᵀcᵢ₂(xₖ)]
/// [ ⋮ ]
/// [∇ᵀcᵢₘ(xₖ)]
/// @endverbatim
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>Aᵢ(x)</td>
/// <td>num_inequality_constraints</td>
/// <td>num_decision_variables</td>
/// </tr>
/// </table>
std::function<Eigen::SparseMatrix<double>(const Eigen::VectorXd& x)> A_i;
};
/**
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] matrix_callbacks Matrix callbacks.
@param[in] iteration_callbacks The list of callbacks to call at the beginning of
each iteration.
@param[in] options Solver options.
@param[in,out] x The initial guess and output location for the decision
variables.
@return The exit status.
*/
SLEIPNIR_DLLEXPORT ExitStatus
interior_point(const InteriorPointMatrixCallbacks& matrix_callbacks,
std::span<std::function<bool(const IterationInfo& info)>>
iteration_callbacks,
const Options& options,
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
const Eigen::ArrayX<bool>& bound_constraint_mask,
#endif
Eigen::VectorXd& x);
} // namespace slp

11
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverIterationInfo.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/iteration_info.hpp vendored
View File

@@ -5,21 +5,18 @@
#include <Eigen/Core>
#include <Eigen/SparseCore>
namespace sleipnir {
namespace slp {
/**
* Solver iteration information exposed to a user callback.
* Solver iteration information exposed to an iteration callback.
*/
struct SolverIterationInfo {
struct IterationInfo {
/// 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;
@@ -33,4 +30,4 @@ struct SolverIterationInfo {
const Eigen::SparseMatrix<double>& A_i;
};
} // namespace sleipnir
} // namespace slp

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

@@ -0,0 +1,113 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <functional>
#include <span>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include "sleipnir/optimization/solver/exit_status.hpp"
#include "sleipnir/optimization/solver/iteration_info.hpp"
#include "sleipnir/optimization/solver/options.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
/**
* Matrix callbacks for the Newton's method solver.
*/
struct SLEIPNIR_DLLEXPORT NewtonMatrixCallbacks {
/// Cost function value f(x) getter.
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>f(x)</td>
/// <td>1</td>
/// <td>1</td>
/// </tr>
/// </table>
std::function<double(const Eigen::VectorXd& x)> f;
/// Cost function gradient ∇f(x) getter.
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>∇f(x)</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::SparseVector<double>(const Eigen::VectorXd& x)> g;
/// Lagrangian Hessian ∇ₓₓ²L(x) getter.
///
/// L(xₖ) = f(xₖ)
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>∇ₓₓ²L(x)</td>
/// <td>num_decision_variables</td>
/// <td>num_decision_variables</td>
/// </tr>
/// </table>
std::function<Eigen::SparseMatrix<double>(const Eigen::VectorXd& x)> H;
};
/**
Finds the optimal solution to a nonlinear program using Newton's method.
A nonlinear program has the form:
@verbatim
min_x f(x)
@endverbatim
where f(x) is the cost function.
@param[in] matrix_callbacks Matrix callbacks.
@param[in] iteration_callbacks The list of callbacks to call at the beginning of
each iteration.
@param[in] options Solver options.
@param[in,out] x The initial guess and output location for the decision
variables.
@return The exit status.
*/
SLEIPNIR_DLLEXPORT ExitStatus
newton(const NewtonMatrixCallbacks& matrix_callbacks,
std::span<std::function<bool(const IterationInfo& info)>>
iteration_callbacks,
const Options& options, Eigen::VectorXd& x);
} // namespace slp

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

@@ -0,0 +1,94 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <chrono>
#include <limits>
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
/**
* Solver options.
*/
struct SLEIPNIR_DLLEXPORT Options {
/// 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 max_iterations = 5000;
/// 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 feasible_ipm = false;
/// Enables diagnostic prints.
///
/// <table>
/// <tr>
/// <th>Heading</th>
/// <th>Description</th>
/// </tr>
/// <tr>
/// <td>iter</td>
/// <td>Iteration number</td>
/// </tr>
/// <tr>
/// <td>type</td>
/// <td>Iteration type (normal, accepted second-order correction, rejected
/// second-order correction)</td>
/// </tr>
/// <tr>
/// <td>time (ms)</td>
/// <td>Duration of iteration in milliseconds</td>
/// </tr>
/// <tr>
/// <td>error</td>
/// <td>Error estimate</td>
/// </tr>
/// <tr>
/// <td>cost</td>
/// <td>Cost function value at current iterate</td>
/// </tr>
/// <tr>
/// <td>infeas.</td>
/// <td>Constraint infeasibility at current iterate</td>
/// </tr>
/// <tr>
/// <td>complement.</td>
/// <td>Complementary slackness at current iterate (sᵀz)</td>
/// </tr>
/// <tr>
/// <td>μ</td>
/// <td>Barrier parameter</td>
/// </tr>
/// <tr>
/// <td>reg</td>
/// <td>Iteration matrix regularization</td>
/// </tr>
/// <tr>
/// <td>primal α</td>
/// <td>Primal step size</td>
/// </tr>
/// <tr>
/// <td>dual α</td>
/// <td>Dual step size</td>
/// </tr>
/// <tr>
/// <td>↩</td>
/// <td>Number of line search backtracks</td>
/// </tr>
/// </table>
bool diagnostics = false;
};
} // namespace slp

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

@@ -0,0 +1,171 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <functional>
#include <span>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include "sleipnir/optimization/solver/exit_status.hpp"
#include "sleipnir/optimization/solver/iteration_info.hpp"
#include "sleipnir/optimization/solver/options.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
/**
* Matrix callbacks for the Sequential Quadratic Programming (SQP) solver.
*/
struct SLEIPNIR_DLLEXPORT SQPMatrixCallbacks {
/// Cost function value f(x) getter.
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>f(x)</td>
/// <td>1</td>
/// <td>1</td>
/// </tr>
/// </table>
std::function<double(const Eigen::VectorXd& x)> f;
/// Cost function gradient ∇f(x) getter.
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>∇f(x)</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::SparseVector<double>(const Eigen::VectorXd& x)> g;
/// Lagrangian Hessian ∇ₓₓ²L(x, y) getter.
///
/// L(xₖ, yₖ) = f(xₖ) − yₖᵀcₑ(xₖ)
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>y</td>
/// <td>num_equality_constraints</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>∇ₓₓ²L(x, y)</td>
/// <td>num_decision_variables</td>
/// <td>num_decision_variables</td>
/// </tr>
/// </table>
std::function<Eigen::SparseMatrix<double>(const Eigen::VectorXd& x,
const Eigen::VectorXd& y)>
H;
/// Equality constraint value cₑ(x) getter.
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>cₑ(x)</td>
/// <td>num_equality_constraints</td>
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::VectorXd(const Eigen::VectorXd& x)> c_e;
/// Equality constraint Jacobian ∂cₑ/∂x getter.
///
/// @verbatim
/// [∇ᵀcₑ₁(xₖ)]
/// Aₑ(x) = [∇ᵀcₑ₂(xₖ)]
/// [ ⋮ ]
/// [∇ᵀcₑₘ(xₖ)]
/// @endverbatim
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>Aₑ(x)</td>
/// <td>num_equality_constraints</td>
/// <td>num_decision_variables</td>
/// </tr>
/// </table>
std::function<Eigen::SparseMatrix<double>(const Eigen::VectorXd& x)> A_e;
};
/**
Finds the optimal solution to a nonlinear program using Sequential Quadratic
Programming (SQP).
A nonlinear program has the form:
@verbatim
min_x f(x)
subject to cₑ(x) = 0
@endverbatim
where f(x) is the cost function and cₑ(x) are the equality constraints.
@param[in] matrix_callbacks Matrix callbacks.
@param[in] iteration_callbacks The list of callbacks to call at the beginning of
each iteration.
@param[in] options Solver options.
@param[in,out] x The initial guess and output location for the decision
variables.
@return The exit status.
*/
SLEIPNIR_DLLEXPORT ExitStatus
sqp(const SQPMatrixCallbacks& matrix_callbacks,
std::span<std::function<bool(const IterationInfo& info)>>
iteration_callbacks,
const Options& options, Eigen::VectorXd& x);
} // namespace slp

25
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Assert.hpp vendored
View File

@@ -1,25 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#ifdef JORMUNGANDR
#include <format>
#include <stdexcept>
/**
* Throw an exception in Python.
*/
#define Assert(condition) \
do { \
if (!(condition)) { \
throw std::invalid_argument( \
std::format("{}:{}: {}: Assertion `{}' failed.", __FILE__, __LINE__, \
__func__, #condition)); \
} \
} while (0);
#else
#include <cassert>
/**
* Abort in C++.
*/
#define Assert(condition) assert(condition)
#endif

33
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Concepts.hpp vendored
View File

@@ -1,33 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <concepts>
#include <Eigen/Core>
namespace sleipnir {
template <typename T>
concept ScalarLike = requires(T t) {
t + 1.0;
t = 1.0;
};
template <typename T>
concept SleipnirMatrixLike = requires(T t, int rows, int cols) {
t.Rows();
t.Cols();
t(rows, cols);
};
template <typename T>
concept EigenMatrixLike = std::derived_from<T, Eigen::MatrixBase<T>>;
template <typename T>
concept MatrixLike = SleipnirMatrixLike<T> || EigenMatrixLike<T>;
template <typename T>
concept EigenSolver = requires(T t) { t.solve(Eigen::VectorXd{}); };
} // namespace sleipnir

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

@@ -1,89 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <fstream>
#include <string>
#include <string_view>
#include <Eigen/SparseCore>
#include <wpi/SmallVector.h>
#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();
wpi::SmallVector<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

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

@@ -0,0 +1,27 @@
// Copyright (c) Sleipnir contributors
#pragma once
#ifdef JORMUNGANDR
#include <format>
#include <source_location>
#include <stdexcept>
/**
* Throw an exception in Python.
*/
#define slp_assert(condition) \
do { \
if (!(condition)) { \
auto location = std::source_location::current(); \
throw std::invalid_argument(std::format( \
"{}:{}: {}: Assertion `{}' failed.", location.file_name(), \
location.line(), location.function_name(), #condition)); \
} \
} while (0);
#else
#include <cassert>
/**
* Abort in C++.
*/
#define slp_assert(condition) assert(condition)
#endif

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

@@ -0,0 +1,42 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <concepts>
#include <type_traits>
#include <Eigen/Core>
namespace slp {
template <typename T>
concept ScalarLike = requires(std::decay_t<T> t) {
t + 1.0;
t = 1.0;
};
template <typename T>
concept SleipnirScalarLike = requires(std::decay_t<T> t) {
t + 1.0;
t = 1.0;
{ t.value() } -> std::same_as<double>;
};
template <typename T>
concept EigenMatrixLike =
std::derived_from<std::decay_t<T>, Eigen::MatrixBase<std::decay_t<T>>>;
template <typename T>
concept SleipnirMatrixLike = requires(std::decay_t<T> t, int rows, int cols) {
t.rows();
t.cols();
{ t.value() } -> std::same_as<Eigen::MatrixXd>;
} && !EigenMatrixLike<T>;
template <typename T>
concept SleipnirType = SleipnirScalarLike<T> || SleipnirMatrixLike<T>;
template <typename T>
concept MatrixLike = SleipnirMatrixLike<T> || EigenMatrixLike<T>;
} // namespace slp

26
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/FunctionRef.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/function_ref.hpp vendored
View File

@@ -7,15 +7,15 @@
#include <type_traits>
#include <utility>
namespace sleipnir {
namespace slp {
template <class F>
class function_ref;
/**
* 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:
@@ -23,12 +23,17 @@ class function_ref<R(Args...)> {
/**
* Creates a `function_ref` which refers to the same callable as `rhs`.
*
* @param rhs Other `function_ref`.
*/
constexpr function_ref(const function_ref<R(Args...)>& rhs) noexcept =
default;
/**
* Constructs a `function_ref` referring to `f`.
*
* @tparam F Callable type.
* @param f Callable to which to refer.
*/
template <typename F>
requires(!std::is_same_v<std::decay_t<F>, function_ref> &&
@@ -45,12 +50,18 @@ class function_ref<R(Args...)> {
/**
* Makes `*this` refer to the same callable as `rhs`.
*
* @param rhs Other `function_ref`.
* @return `*this`
*/
constexpr function_ref<R(Args...)>& operator=(
const function_ref<R(Args...)>& rhs) noexcept = default;
/**
* Makes `*this` refer to `f`.
*
* @param f Callable to which to refer.
* @return `*this`
*/
template <typename F>
requires std::is_invocable_r_v<R, F&&, Args...>
@@ -67,6 +78,8 @@ class function_ref<R(Args...)> {
/**
* Swaps the referred callables of `*this` and `rhs`.
*
* @param rhs Other `function_ref`.
*/
constexpr void swap(function_ref<R(Args...)>& rhs) noexcept {
std::swap(obj_, rhs.obj_);
@@ -75,6 +88,9 @@ class function_ref<R(Args...)> {
/**
* Call the stored callable with the given arguments.
*
* @param args The arguments.
* @return The return value of the callable.
*/
R operator()(Args... args) const {
return callback_(obj_, std::forward<Args>(args)...);
@@ -97,4 +113,4 @@ constexpr void swap(function_ref<R(Args...)>& lhs,
template <typename R, typename... Args>
function_ref(R (*)(Args...)) -> function_ref<R(Args...)>;
} // namespace sleipnir
} // namespace slp

142
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/IntrusiveSharedPtr.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/intrusive_shared_ptr.hpp vendored
View File

@@ -2,11 +2,12 @@
#pragma once
#include <concepts>
#include <cstddef>
#include <memory>
#include <utility>
namespace sleipnir {
namespace slp {
/**
* A custom intrusive shared pointer implementation without thread
@@ -16,17 +17,20 @@ namespace sleipnir {
*
* 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.
* 2. A free function `void inc_ref_count(T*)` that increments the reference
* count.
* 3. A free function `void dec_ref_count(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:
template <typename>
friend class IntrusiveSharedPtr;
/**
* Constructs an empty intrusive shared pointer.
*/
@@ -40,31 +44,53 @@ class IntrusiveSharedPtr {
/**
* Constructs an intrusive shared pointer from the given pointer and takes
* ownership.
*
* @param ptr The pointer of which to take ownership.
*/
explicit constexpr IntrusiveSharedPtr(T* ptr) noexcept : m_ptr{ptr} {
if (m_ptr != nullptr) {
IntrusiveSharedPtrIncRefCount(m_ptr);
inc_ref_count(m_ptr);
}
}
constexpr ~IntrusiveSharedPtr() {
if (m_ptr != nullptr) {
IntrusiveSharedPtrDecRefCount(m_ptr);
dec_ref_count(m_ptr);
}
}
/**
* Copy constructs from the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
*/
constexpr IntrusiveSharedPtr(const IntrusiveSharedPtr<T>& rhs) noexcept
: m_ptr{rhs.m_ptr} {
if (m_ptr != nullptr) {
IntrusiveSharedPtrIncRefCount(m_ptr);
inc_ref_count(m_ptr);
}
}
/**
* Copy constructs from the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
*/
template <typename U>
requires(!std::same_as<T, U> && std::convertible_to<U*, T*>)
constexpr IntrusiveSharedPtr( // NOLINT
const IntrusiveSharedPtr<U>& rhs) noexcept
: m_ptr{rhs.m_ptr} {
if (m_ptr != nullptr) {
inc_ref_count(m_ptr);
}
}
/**
* Makes a copy of the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
* @return This intrusive shared pointer.
*/
constexpr IntrusiveSharedPtr<T>& operator=( // NOLINT
const IntrusiveSharedPtr<T>& rhs) noexcept {
@@ -73,13 +99,40 @@ class IntrusiveSharedPtr {
}
if (m_ptr != nullptr) {
IntrusiveSharedPtrDecRefCount(m_ptr);
dec_ref_count(m_ptr);
}
m_ptr = rhs.m_ptr;
if (m_ptr != nullptr) {
IntrusiveSharedPtrIncRefCount(m_ptr);
inc_ref_count(m_ptr);
}
return *this;
}
/**
* Makes a copy of the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
* @return This intrusive shared pointer.
*/
template <typename U>
requires(!std::same_as<T, U> && std::convertible_to<U*, T*>)
constexpr IntrusiveSharedPtr<T>& operator=( // NOLINT
const IntrusiveSharedPtr<U>& rhs) noexcept {
if (m_ptr == rhs.m_ptr) {
return *this;
}
if (m_ptr != nullptr) {
dec_ref_count(m_ptr);
}
m_ptr = rhs.m_ptr;
if (m_ptr != nullptr) {
inc_ref_count(m_ptr);
}
return *this;
@@ -87,12 +140,28 @@ class IntrusiveSharedPtr {
/**
* Move constructs from the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
*/
constexpr IntrusiveSharedPtr(IntrusiveSharedPtr<T>&& rhs) noexcept
: m_ptr{std::exchange(rhs.m_ptr, nullptr)} {}
/**
* Move constructs from the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
*/
template <typename U>
requires(!std::same_as<T, U> && std::convertible_to<U*, T*>)
constexpr IntrusiveSharedPtr( // NOLINT
IntrusiveSharedPtr<U>&& rhs) noexcept
: m_ptr{std::exchange(rhs.m_ptr, nullptr)} {}
/**
* Move assigns from the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
* @return This intrusive shared pointer.
*/
constexpr IntrusiveSharedPtr<T>& operator=(
IntrusiveSharedPtr<T>&& rhs) noexcept {
@@ -106,28 +175,58 @@ class IntrusiveSharedPtr {
}
/**
* Returns the internal pointer.
* Move assigns from the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
* @return This intrusive shared pointer.
*/
constexpr T* Get() const noexcept { return m_ptr; }
template <typename U>
requires(!std::same_as<T, U> && std::convertible_to<U*, T*>)
constexpr IntrusiveSharedPtr<T>& operator=(
IntrusiveSharedPtr<U>&& rhs) noexcept {
if (m_ptr == rhs.m_ptr) {
return *this;
}
std::swap(m_ptr, rhs.m_ptr);
return *this;
}
/**
* Returns the internal pointer.
*
* @return The internal pointer.
*/
constexpr T* get() const noexcept { return m_ptr; }
/**
* Returns the object pointed to by the internal pointer.
*
* @return The object pointed to by the internal pointer.
*/
constexpr T& operator*() const noexcept { return *m_ptr; }
/**
* Returns the internal pointer.
*
* @return The internal pointer.
*/
constexpr T* operator->() const noexcept { return m_ptr; }
/**
* Returns true if the internal pointer isn't nullptr.
*
* @return 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.
*
* @param lhs The left-hand side.
* @param rhs The right-hand side.
*/
friend constexpr bool operator==(const IntrusiveSharedPtr<T>& lhs,
const IntrusiveSharedPtr<T>& rhs) noexcept {
@@ -137,6 +236,9 @@ class IntrusiveSharedPtr {
/**
* Returns true if the given intrusive shared pointers point to different
* objects.
*
* @param lhs The left-hand side.
* @param rhs The right-hand side.
*/
friend constexpr bool operator!=(const IntrusiveSharedPtr<T>& lhs,
const IntrusiveSharedPtr<T>& rhs) noexcept {
@@ -145,6 +247,8 @@ class IntrusiveSharedPtr {
/**
* Returns true if the left-hand intrusive shared pointer points to nullptr.
*
* @param lhs The left-hand side.
*/
friend constexpr bool operator==(const IntrusiveSharedPtr<T>& lhs,
std::nullptr_t) noexcept {
@@ -153,6 +257,8 @@ class IntrusiveSharedPtr {
/**
* Returns true if the right-hand intrusive shared pointer points to nullptr.
*
* @param rhs The right-hand side.
*/
friend constexpr bool operator==(std::nullptr_t,
const IntrusiveSharedPtr<T>& rhs) noexcept {
@@ -162,6 +268,8 @@ class IntrusiveSharedPtr {
/**
* Returns true if the left-hand intrusive shared pointer doesn't point to
* nullptr.
*
* @param lhs The left-hand side.
*/
friend constexpr bool operator!=(const IntrusiveSharedPtr<T>& lhs,
std::nullptr_t) noexcept {
@@ -171,6 +279,8 @@ class IntrusiveSharedPtr {
/**
* Returns true if the right-hand intrusive shared pointer doesn't point to
* nullptr.
*
* @param rhs The right-hand side.
*/
friend constexpr bool operator!=(std::nullptr_t,
const IntrusiveSharedPtr<T>& rhs) noexcept {
@@ -190,7 +300,7 @@ class IntrusiveSharedPtr {
* @param args Constructor arguments for T.
*/
template <typename T, typename... Args>
IntrusiveSharedPtr<T> MakeIntrusiveShared(Args&&... args) {
IntrusiveSharedPtr<T> make_intrusive_shared(Args&&... args) {
return IntrusiveSharedPtr<T>{new T(std::forward<Args>(args)...)};
}
@@ -206,11 +316,11 @@ IntrusiveSharedPtr<T> MakeIntrusiveShared(Args&&... args) {
* @param args Constructor arguments for T.
*/
template <typename T, typename Alloc, typename... Args>
IntrusiveSharedPtr<T> AllocateIntrusiveShared(Alloc alloc, Args&&... args) {
IntrusiveSharedPtr<T> allocate_intrusive_shared(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
} // namespace slp

93
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Pool.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/pool.hpp vendored
View File

@@ -5,11 +5,11 @@
#include <cstddef>
#include <memory>
#include <wpi/SmallVector.h>
#include <gch/small_vector.hpp>
#include "sleipnir/util/SymbolExports.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace sleipnir {
namespace slp {
/**
* This class implements a pool memory resource.
@@ -23,14 +23,33 @@ class SLEIPNIR_DLLEXPORT PoolResource {
/**
* Constructs a default PoolResource.
*
* @param blocksPerChunk Number of blocks per chunk of memory.
* @param blocks_per_chunk Number of blocks per chunk of memory.
*/
explicit PoolResource(size_t blocksPerChunk)
: blocksPerChunk{blocksPerChunk} {}
explicit PoolResource(size_t blocks_per_chunk)
: blocks_per_chunk{blocks_per_chunk} {}
/**
* Copy constructor.
*/
PoolResource(const PoolResource&) = delete;
/**
* Copy assignment operator.
*
* @return This pool resource.
*/
PoolResource& operator=(const PoolResource&) = delete;
/**
* Move constructor.
*/
PoolResource(PoolResource&&) = default;
/**
* Move assignment operator.
*
* @return This pool resource.
*/
PoolResource& operator=(PoolResource&&) = default;
/**
@@ -38,16 +57,17 @@ class SLEIPNIR_DLLEXPORT PoolResource {
*
* @param bytes Number of bytes in the block.
* @param alignment Alignment of the block (unused).
* @return A block of memory from the pool.
*/
[[nodiscard]]
void* allocate(size_t bytes, [[maybe_unused]] size_t alignment =
alignof(std::max_align_t)) {
if (m_freeList.empty()) {
AddChunk(bytes);
if (m_free_list.empty()) {
add_chunk(bytes);
}
auto ptr = m_freeList.back();
m_freeList.pop_back();
auto ptr = m_free_list.back();
m_free_list.pop_back();
return ptr;
}
@@ -61,11 +81,14 @@ class SLEIPNIR_DLLEXPORT PoolResource {
void deallocate(
void* p, [[maybe_unused]] size_t bytes,
[[maybe_unused]] size_t alignment = alignof(std::max_align_t)) {
m_freeList.emplace_back(p);
m_free_list.emplace_back(p);
}
/**
* Returns true if this pool resource has the same backing storage as another.
*
* @param other The other pool resource.
* @return True if this pool resource has the same backing storage as another.
*/
bool is_equal(const PoolResource& other) const noexcept {
return this == &other;
@@ -73,26 +96,28 @@ class SLEIPNIR_DLLEXPORT PoolResource {
/**
* Returns the number of blocks from this pool resource that are in use.
*
* @return 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();
return m_buffer.size() * blocks_per_chunk - m_free_list.size();
}
private:
wpi::SmallVector<std::unique_ptr<std::byte[]>> m_buffer;
wpi::SmallVector<void*> m_freeList;
size_t blocksPerChunk;
gch::small_vector<std::unique_ptr<std::byte[]>> m_buffer;
gch::small_vector<void*> m_free_list;
size_t blocks_per_chunk;
/**
* 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.
* @param bytes_per_block 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);
void add_chunk(size_t bytes_per_block) {
m_buffer.emplace_back(new std::byte[bytes_per_block * blocks_per_chunk]);
for (int i = blocks_per_chunk - 1; i >= 0; --i) {
m_free_list.emplace_back(m_buffer.back().get() + bytes_per_block * i);
}
}
};
@@ -115,19 +140,29 @@ class PoolAllocator {
*
* @param r The pool resource.
*/
explicit constexpr PoolAllocator(PoolResource* r) : m_memoryResource{r} {}
explicit constexpr PoolAllocator(PoolResource* r) : m_memory_resource{r} {}
constexpr PoolAllocator(const PoolAllocator<T>& other) = default;
/**
* Copy constructor.
*/
constexpr PoolAllocator(const PoolAllocator<T>&) = default;
/**
* Copy assignment operator.
*
* @return This pool allocator.
*/
constexpr PoolAllocator<T>& operator=(const PoolAllocator<T>&) = default;
/**
* Returns a block of memory from the pool.
*
* @param n Number of bytes in the block.
* @return A block of memory from the pool.
*/
[[nodiscard]]
constexpr T* allocate(size_t n) {
return static_cast<T*>(m_memoryResource->allocate(n));
return static_cast<T*>(m_memory_resource->allocate(n));
}
/**
@@ -137,17 +172,17 @@ class PoolAllocator {
* @param n Number of bytes in the block.
*/
constexpr void deallocate(T* p, size_t n) {
m_memoryResource->deallocate(p, n);
m_memory_resource->deallocate(p, n);
}
private:
PoolResource* m_memoryResource;
PoolResource* m_memory_resource;
};
/**
* Returns a global pool memory resource.
*/
SLEIPNIR_DLLEXPORT PoolResource& GlobalPoolResource();
SLEIPNIR_DLLEXPORT PoolResource& global_pool_resource();
/**
* Returns an allocator for a global pool memory resource.
@@ -155,8 +190,8 @@ SLEIPNIR_DLLEXPORT PoolResource& GlobalPoolResource();
* @tparam T The type of object in the pool.
*/
template <typename T>
PoolAllocator<T> GlobalPoolAllocator() {
return PoolAllocator<T>{&GlobalPoolResource()};
PoolAllocator<T> global_pool_allocator() {
return PoolAllocator<T>{&global_pool_resource()};
}
} // namespace sleipnir
} // namespace slp

19
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Print.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print.hpp vendored
View File

@@ -2,6 +2,7 @@
#pragma once
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
#include <cstdio>
#include <system_error>
#include <utility>
@@ -12,7 +13,11 @@
#include <fmt/core.h>
#endif
namespace sleipnir {
#endif
namespace slp {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* Wrapper around fmt::print() that squelches write failure exceptions.
@@ -58,4 +63,14 @@ inline void println(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
}
}
} // namespace sleipnir
#else
template <typename... Args>
inline void print([[maybe_unused]] Args&&... args) {}
template <typename... Args>
inline void println([[maybe_unused]] Args&&... args) {}
#endif
} // namespace slp

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

@@ -0,0 +1,127 @@
// Copyright (c) Sleipnir contributors
#pragma once
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
#include <stdint.h>
#include <bit>
#include <fstream>
#include <string>
#include <string_view>
#include <Eigen/SparseCore>
#include <wpi/bit.h>
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
/**
* Writes the sparsity pattern of a sparse matrix to a file.
*
* Each file represents the sparsity pattern of one matrix over time. <a
* href="https://github.com/SleipnirGroup/Sleipnir/blob/main/tools/spy.py">spy.py</a>
* can display it as an animation.
*
* The file starts with the following header:
* <ol>
* <li>Plot title (length as a little-endian int32, then characters)</li>
* <li>Row label (length as a little-endian int32, then characters)</li>
* <li>Column label (length as a little-endian int32, then characters)</li>
* </ol>
*
* Then, each sparsity pattern starts with:
* <ol>
* <li>Number of coordinates as a little-endian int32</li>
* </ol>
*
* followed by that many coordinates in the following format:
* <ol>
* <li>Row index as a little-endian int32</li>
* <li>Column index as a little-endian int32</li>
* <li>Sign as a character ('+' for positive, '-' for negative, or '0' for
* zero)</li>
* </ol>
*
* @param[out] file A file stream.
* @param[in] mat The sparse matrix.
*/
class SLEIPNIR_DLLEXPORT Spy {
public:
/**
* Constructs a Spy instance.
*
* @param filename The filename.
* @param title Plot title.
* @param row_label Row label.
* @param col_label Column label.
* @param rows The sparse matrix's number of rows.
* @param cols The sparse matrix's number of columns.
*/
Spy(std::string_view filename, std::string_view title,
std::string_view row_label, std::string_view col_label, int rows,
int cols)
: m_file{std::string{filename}, std::ios::binary} {
// Write title
write32le(title.size());
m_file.write(title.data(), title.size());
// Write row label
write32le(row_label.size());
m_file.write(row_label.data(), row_label.size());
// Write column label
write32le(col_label.size());
m_file.write(col_label.data(), col_label.size());
// Write row and column counts
write32le(rows);
write32le(cols);
}
/**
* Adds a matrix to the file.
*
* @param mat The matrix.
*/
void add(const Eigen::SparseMatrix<double>& mat) {
// Write number of coordinates
write32le(mat.nonZeros());
// Write coordinates
for (int k = 0; k < mat.outerSize(); ++k) {
for (Eigen::SparseMatrix<double>::InnerIterator it{mat, k}; it; ++it) {
write32le(it.row());
write32le(it.col());
if (it.value() > 0.0) {
m_file << '+';
} else if (it.value() < 0.0) {
m_file << '-';
} else {
m_file << '0';
}
}
}
}
private:
std::ofstream m_file;
/**
* Writes a 32-bit signed integer to the file as little-endian.
*
* @param num A 32-bit signed integer.
*/
void write32le(int32_t num) {
if constexpr (std::endian::native != std::endian::little) {
num = wpi::byteswap(num);
}
m_file.write(reinterpret_cast<char*>(&num), sizeof(num));
}
};
} // namespace slp
#endif

0
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/SymbolExports.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/symbol_exports.hpp vendored
View File