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

[upstream_utils] Upgrade to Sleipnir 0.3.3 (#8463)

Browse Source
This commit is contained in:
Tyler Veness
2025-12-12 19:40:43 -08:00
committed by GitHub
parent d830c41063
commit cca035787c
58 changed files with 3141 additions and 3990 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

File diff suppressed because it is too large Load Diff

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

@@ -10,14 +10,12 @@
namespace slp::detail {
/**
* Generate a topological sort of an expression graph from parent to child.
*
* https://en.wikipedia.org/wiki/Topological_sorting
*
* @tparam Scalar Scalar type.
* @param root The root node of the expression.
*/
/// Generate a topological sort of an expression graph from parent to child.
///
/// https://en.wikipedia.org/wiki/Topological_sorting
///
/// @tparam Scalar Scalar type.
/// @param root The root node of the expression.
template <typename Scalar>
gch::small_vector<Expression<Scalar>*> topological_sort(
const ExpressionPtr<Scalar>& root) {
@@ -70,13 +68,11 @@ gch::small_vector<Expression<Scalar>*> topological_sort(
return list;
}
/**
* Update the values of all nodes in this graph based on the values of
* their dependent nodes.
*
* @tparam Scalar Scalar type.
* @param list Topological sort of graph from parent to child.
*/
/// Update the values of all nodes in this graph based on the values of
/// their dependent nodes.
///
/// @tparam Scalar Scalar type.
/// @param list Topological sort of graph from parent to child.
template <typename Scalar>
void update_values(const gch::small_vector<Expression<Scalar>*>& list) {
// Traverse graph from child to parent and update values

78
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_type.hpp vendored
View File

@@ -4,17 +4,15 @@
#include <stdint.h>
#include <string_view>
#include <fmt/base.h>
#include "sleipnir/util/symbol_exports.hpp"
#include "sleipnir/util/unreachable.hpp"
namespace slp {
/**
* Expression type.
*
* Used for autodiff caching.
*/
/// Expression type.
///
/// Used for autodiff caching.
enum class ExpressionType : uint8_t {
/// There is no expression.
NONE,
@@ -28,29 +26,45 @@ enum class ExpressionType : uint8_t {
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
/// Formatter for ExpressionType.
template <>
struct fmt::formatter<slp::ExpressionType> {
/// Parse format string.
///
/// @param ctx Format parse context.
/// @return Format parse context iterator.
constexpr auto parse(fmt::format_parse_context& ctx) {
return m_underlying.parse(ctx);
}
/// Format ExpressionType.
///
/// @tparam FmtContext Format context type.
/// @param type Expression type.
/// @param ctx Format context.
/// @return Format context iterator.
template <typename FmtContext>
auto format(const slp::ExpressionType& type, FmtContext& ctx) const {
using enum slp::ExpressionType;
switch (type) {
case NONE:
return m_underlying.format("none", ctx);
case CONSTANT:
return m_underlying.format("constant", ctx);
case LINEAR:
return m_underlying.format("linear", ctx);
case QUADRATIC:
return m_underlying.format("quadratic", ctx);
case NONLINEAR:
return m_underlying.format("nonlinear", ctx);
default:
slp::unreachable();
}
}
private:
fmt::formatter<const char*> m_underlying;
};

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

@@ -14,52 +14,42 @@
namespace slp {
/**
* This class calculates the gradient of a variable with respect to a vector of
* variables.
*
* The gradient is only recomputed if the variable expression is quadratic or
* higher order.
*
* @tparam Scalar Scalar type.
*/
/// This class calculates the gradient of a variable with respect to a vector of
/// variables.
///
/// The gradient is only recomputed if the variable expression is quadratic or
/// higher order.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
class Gradient {
public:
/**
* Constructs a Gradient object.
*
* @param variable Variable of which to compute the gradient.
* @param wrt Variable with respect to which to compute the gradient.
*/
/// Constructs a Gradient object.
///
/// @param variable Variable of which to compute the gradient.
/// @param wrt Variable with respect to which to compute the gradient.
Gradient(Variable<Scalar> variable, Variable<Scalar> wrt)
: m_jacobian{std::move(variable), std::move(wrt)} {}
/**
* Constructs a Gradient object.
*
* @param variable Variable of which to compute the gradient.
* @param wrt Vector of variables with respect to which to compute the
* gradient.
*/
/// Constructs a Gradient object.
///
/// @param variable Variable of which to compute the gradient.
/// @param wrt Vector of variables with respect to which to compute the
/// gradient.
Gradient(Variable<Scalar> variable, SleipnirMatrixLike<Scalar> auto wrt)
: 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.
*/
/// Returns the gradient as a VariableMatrix.
///
/// This is useful when constructing optimization problems with derivatives in
/// them.
///
/// @return The gradient as a VariableMatrix.
VariableMatrix<Scalar> get() const { return m_jacobian.get().T(); }
/**
* Evaluates the gradient at wrt's value.
*
* @return The gradient at wrt's value.
*/
/// Evaluates the gradient at wrt's value.
///
/// @return The gradient at wrt's value.
const Eigen::SparseVector<Scalar>& value() {
m_g = m_jacobian.value().transpose();

72
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/adjoint_expression_graph.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient_expression_graph.hpp vendored
View File

@@ -16,45 +16,38 @@
namespace slp::detail {
/**
* This class is an adaptor type that performs value updates of an expression's
* adjoint graph.
*
* @tparam Scalar Scalar type.
*/
/// This class is an adapter type that performs value updates of an expression
/// graph, generates a gradient tree, or appends gradient triplets for creating
/// a sparse matrix of gradients.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
class AdjointExpressionGraph {
class GradientExpressionGraph {
public:
/**
* Generates the adjoint graph for the given expression.
*
* @param root The root node of the expression.
*/
explicit AdjointExpressionGraph(const Variable<Scalar>& root)
/// Generates the gradient graph for the given expression.
///
/// @param root The root node of the expression.
explicit GradientExpressionGraph(const Variable<Scalar>& 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.
*/
/// Update the values of all nodes in this 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<Scalar> generate_gradient_tree(
/// 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<Scalar> generate_tree(
const VariableMatrix<Scalar>& wrt) const {
slp_assert(wrt.cols() == 1);
@@ -104,18 +97,15 @@ class AdjointExpressionGraph {
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_gradient_triplets(
gch::small_vector<Eigen::Triplet<Scalar>>& triplets, int row,
const VariableMatrix<Scalar>& wrt) const {
/// 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_triplets(gch::small_vector<Eigen::Triplet<Scalar>>& triplets,
int row, const VariableMatrix<Scalar>& wrt) const {
slp_assert(wrt.cols() == 1);
// Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation

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

@@ -7,7 +7,7 @@
#include <Eigen/SparseCore>
#include <gch/small_vector.hpp>
#include "sleipnir/autodiff/adjoint_expression_graph.hpp"
#include "sleipnir/autodiff/gradient_expression_graph.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/assert.hpp"
@@ -15,39 +15,33 @@
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 Scalar Scalar type.
* @tparam UpLo Which part of the Hessian to compute (Lower or Lower | Upper).
*/
/// 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 Scalar Scalar type.
/// @tparam UpLo Which part of the Hessian to compute (Lower or Lower | Upper).
template <typename Scalar, int UpLo>
requires(UpLo == Eigen::Lower) || (UpLo == (Eigen::Lower | Eigen::Upper))
class 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.
*/
/// 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<Scalar> variable, Variable<Scalar> wrt)
: Hessian{std::move(variable), VariableMatrix<Scalar>{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.
*/
/// 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<Scalar> variable, SleipnirMatrixLike<Scalar> auto wrt)
: m_variables{detail::AdjointExpressionGraph<Scalar>{variable}
.generate_gradient_tree(wrt)},
: m_variables{detail::GradientExpressionGraph<Scalar>{variable}
.generate_tree(wrt)},
m_wrt{wrt} {
slp_assert(m_wrt.cols() == 1);
@@ -74,7 +68,7 @@ class Hessian {
// 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_gradient_triplets(m_cached_triplets, row, m_wrt);
m_graphs[row].append_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().
@@ -90,20 +84,18 @@ class Hessian {
}
}
/**
* Returns the Hessian as a VariableMatrix.
*
* This is useful when constructing optimization problems with derivatives in
* them.
*
* @return The Hessian as a VariableMatrix.
*/
/// Returns the Hessian as a VariableMatrix.
///
/// This is useful when constructing optimization problems with derivatives in
/// them.
///
/// @return The Hessian as a VariableMatrix.
VariableMatrix<Scalar> get() const {
VariableMatrix<Scalar> result{detail::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);
auto grad = m_graphs[row].generate_tree(m_wrt);
for (int col = 0; col < m_wrt.rows(); ++col) {
if (grad[col].expr != nullptr) {
result(row, col) = std::move(grad[col]);
@@ -116,11 +108,9 @@ class Hessian {
return result;
}
/**
* Evaluates the Hessian at wrt's value.
*
* @return The Hessian at wrt's value.
*/
/// Evaluates the Hessian at wrt's value.
///
/// @return The Hessian at wrt's value.
const Eigen::SparseMatrix<Scalar>& value() {
if (m_nonlinear_rows.empty()) {
return m_H;
@@ -136,7 +126,7 @@ class Hessian {
// Compute each nonlinear row of the Hessian
for (int row : m_nonlinear_rows) {
m_graphs[row].append_gradient_triplets(triplets, row, m_wrt);
m_graphs[row].append_triplets(triplets, row, m_wrt);
}
m_H.setFromTriplets(triplets.begin(), triplets.end());
@@ -151,7 +141,7 @@ class Hessian {
VariableMatrix<Scalar> m_variables;
VariableMatrix<Scalar> m_wrt;
gch::small_vector<detail::AdjointExpressionGraph<Scalar>> m_graphs;
gch::small_vector<detail::GradientExpressionGraph<Scalar>> m_graphs;
Eigen::SparseMatrix<Scalar> m_H{m_variables.rows(), m_wrt.rows()};

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

@@ -7,7 +7,7 @@
#include <Eigen/SparseCore>
#include <gch/small_vector.hpp>
#include "sleipnir/autodiff/adjoint_expression_graph.hpp"
#include "sleipnir/autodiff/gradient_expression_graph.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/assert.hpp"
@@ -17,45 +17,37 @@
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.
*
* @tparam Scalar Scalar type.
*/
/// 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.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
class 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.
*/
/// 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<Scalar> variable, Variable<Scalar> wrt)
: Jacobian{VariableMatrix<Scalar>{std::move(variable)},
VariableMatrix<Scalar>{std::move(wrt)}} {}
/**
* Constructs a Jacobian object.
*
* @param variable Variable of which to compute the Jacobian.
* @param wrt Vector of variables with respect to which to compute the
* Jacobian.
*/
/// Constructs a Jacobian object.
///
/// @param variable Variable of which to compute the Jacobian.
/// @param wrt Vector of variables with respect to which to compute the
/// Jacobian.
Jacobian(Variable<Scalar> variable, SleipnirMatrixLike<Scalar> auto wrt)
: Jacobian{VariableMatrix<Scalar>{std::move(variable)}, 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.
*/
/// 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<Scalar> variables,
SleipnirMatrixLike<Scalar> auto wrt)
: m_variables{std::move(variables)}, m_wrt{std::move(wrt)} {
@@ -85,7 +77,7 @@ class Jacobian {
// 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_gradient_triplets(m_cached_triplets, row, m_wrt);
m_graphs[row].append_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().
@@ -98,20 +90,18 @@ class Jacobian {
}
}
/**
* Returns the Jacobian as a VariableMatrix.
*
* This is useful when constructing optimization problems with derivatives in
* them.
*
* @return The Jacobian as a VariableMatrix.
*/
/// Returns the Jacobian as a VariableMatrix.
///
/// This is useful when constructing optimization problems with derivatives in
/// them.
///
/// @return The Jacobian as a VariableMatrix.
VariableMatrix<Scalar> get() const {
VariableMatrix<Scalar> result{detail::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);
auto grad = m_graphs[row].generate_tree(m_wrt);
for (int col = 0; col < m_wrt.rows(); ++col) {
if (grad[col].expr != nullptr) {
result(row, col) = std::move(grad[col]);
@@ -124,11 +114,9 @@ class Jacobian {
return result;
}
/**
* Evaluates the Jacobian at wrt's value.
*
* @return The Jacobian at wrt's value.
*/
/// Evaluates the Jacobian at wrt's value.
///
/// @return The Jacobian at wrt's value.
const Eigen::SparseMatrix<Scalar>& value() {
if (m_nonlinear_rows.empty()) {
return m_J;
@@ -144,7 +132,7 @@ class Jacobian {
// Compute each nonlinear row of the Jacobian
for (int row : m_nonlinear_rows) {
m_graphs[row].append_gradient_triplets(triplets, row, m_wrt);
m_graphs[row].append_triplets(triplets, row, m_wrt);
}
m_J.setFromTriplets(triplets.begin(), triplets.end());
@@ -156,7 +144,7 @@ class Jacobian {
VariableMatrix<Scalar> m_variables;
VariableMatrix<Scalar> m_wrt;
gch::small_vector<detail::AdjointExpressionGraph<Scalar>> m_graphs;
gch::small_vector<detail::GradientExpressionGraph<Scalar>> m_graphs;
Eigen::SparseMatrix<Scalar> m_J{m_variables.rows(), m_wrt.rows()};

6
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/sleipnir_base.hpp vendored
View File

@@ -4,10 +4,8 @@
namespace slp {
/**
* Marker interface for concepts to determine whether a given scalar or matrix
* type belongs to Sleipnir.
*/
/// Marker interface for concepts to determine whether a given scalar or matrix
/// type belongs to Sleipnir.
class SleipnirBase {};
} // namespace slp

62
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/slice.hpp vendored
View File

@@ -13,21 +13,15 @@ namespace slp {
namespace slicing {
/**
* Type tag used to designate an omitted argument of the slice.
*/
/// Type tag used to designate an omitted argument of the slice.
struct none_t {};
/**
* Designates an omitted argument of the slice.
*/
/// Designates an omitted argument of the slice.
static inline constexpr none_t _;
} // namespace slicing
/**
* Represents a sequence of elements in an iterable object.
*/
/// Represents a sequence of elements in an iterable object.
class SLEIPNIR_DLLEXPORT Slice {
public:
/// Start index (inclusive).
@@ -39,23 +33,17 @@ class SLEIPNIR_DLLEXPORT Slice {
/// Step.
int step = 1;
/**
* Constructs a Slice.
*/
/// Constructs a Slice.
constexpr Slice() = default;
/**
* Constructs a slice.
*/
/// Constructs a slice.
// NOLINTNEXTLINE (google-explicit-constructor)
constexpr Slice(slicing::none_t)
: Slice(0, std::numeric_limits<int>::max(), 1) {}
/**
* Constructs a slice.
*
* @param start Slice start index (inclusive).
*/
/// Constructs a slice.
///
/// @param start Slice start index (inclusive).
// NOLINTNEXTLINE (google-explicit-constructor)
constexpr Slice(int start) {
this->start = start;
@@ -63,12 +51,10 @@ class SLEIPNIR_DLLEXPORT Slice {
this->step = 1;
}
/**
* Constructs a slice.
*
* @param start Slice start index (inclusive).
* @param stop Slice stop index (exclusive).
*/
/// Constructs a slice.
///
/// @param start Slice start index (inclusive).
/// @param stop Slice stop index (exclusive).
template <typename Start, typename Stop>
requires(std::same_as<Start, slicing::none_t> ||
std::convertible_to<Start, int>) &&
@@ -77,13 +63,11 @@ class SLEIPNIR_DLLEXPORT Slice {
constexpr Slice(Start start, Stop stop)
: Slice(std::move(start), std::move(stop), 1) {}
/**
* Constructs a slice.
*
* @param start Slice start index (inclusive).
* @param stop Slice stop index (exclusive).
* @param step Slice step.
*/
/// Constructs a slice.
///
/// @param start Slice start index (inclusive).
/// @param stop Slice stop index (exclusive).
/// @param step Slice step.
template <typename Start, typename Stop, typename Step>
requires(std::same_as<Start, slicing::none_t> ||
std::convertible_to<Start, int>) &&
@@ -126,13 +110,11 @@ class SLEIPNIR_DLLEXPORT Slice {
}
}
/**
* Adjusts start and end slice indices assuming a sequence of the specified
* length.
*
* @param length The sequence length.
* @return The slice length.
*/
/// Adjusts start and end slice indices assuming a sequence of the specified
/// length.
///
/// @param length The sequence length.
/// @return The slice length.
constexpr int adjust(int length) {
slp_assert(step != 0);
slp_assert(step >= -std::numeric_limits<int>::max());

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

File diff suppressed because it is too large Load Diff

608
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_block.hpp vendored
View File

@@ -17,30 +17,22 @@
namespace slp {
/**
* A submatrix of autodiff variables with reference semantics.
*
* @tparam Mat The type of the matrix whose storage this class points to.
*/
/// 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 SleipnirBase {
public:
/**
* Scalar type alias.
*/
/// Scalar type alias.
using Scalar = typename Mat::Scalar;
/**
* Copy constructor.
*/
/// Copy constructor.
VariableBlock(const VariableBlock<Mat>&) = default;
/**
* Assigns a VariableBlock to the block.
*
* @param values VariableBlock of values.
* @return This VariableBlock.
*/
/// 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;
@@ -65,17 +57,13 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Move constructor.
*/
/// Move constructor.
VariableBlock(VariableBlock<Mat>&&) = default;
/**
* Assigns a VariableBlock to the block.
*
* @param values VariableBlock of values.
* @return This VariableBlock.
*/
/// 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;
@@ -100,23 +88,19 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Constructs a Variable block pointing to all of the given matrix.
*
* @param mat The matrix to which to point.
*/
/// Constructs a Variable block pointing to all of the given matrix.
///
/// @param mat The matrix to which to point.
// NOLINTNEXTLINE (google-explicit-constructor)
VariableBlock(Mat& mat) : VariableBlock{mat, 0, 0, mat.rows(), 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.
*/
/// 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},
@@ -125,17 +109,15 @@ class VariableBlock : public SleipnirBase {
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.
*/
/// 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},
@@ -144,14 +126,12 @@ class VariableBlock : public SleipnirBase {
m_col_slice{std::move(col_slice)},
m_col_slice_length{col_slice_length} {}
/**
* Assigns a scalar to the block.
*
* This only works for blocks with one row and one column.
*
* @param value Value to assign.
* @return This VariableBlock.
*/
/// Assigns a scalar 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);
@@ -160,25 +140,21 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Assigns a scalar to the block.
*
* This only works for blocks with one row and one column.
*
* @param value Value to assign.
*/
/// Assigns a scalar to the block.
///
/// This only works for blocks with one row and one column.
///
/// @param value Value to assign.
void set_value(Scalar 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.
*/
/// 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());
@@ -192,11 +168,9 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Sets block's internal values.
*
* @param values Eigen matrix of values.
*/
/// Sets block's internal values.
///
/// @param values Eigen matrix of values.
template <typename Derived>
requires std::same_as<typename Derived::Scalar, Scalar>
void set_value(const Eigen::MatrixBase<Derived>& values) {
@@ -209,12 +183,10 @@ class VariableBlock : public SleipnirBase {
}
}
/**
* Assigns a VariableMatrix to the block.
*
* @param values VariableMatrix of values.
* @return This VariableBlock.
*/
/// 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());
@@ -226,12 +198,10 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Assigns a VariableMatrix to the block.
*
* @param values VariableMatrix of values.
* @return This VariableBlock.
*/
/// 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());
@@ -243,13 +213,11 @@ class VariableBlock : public SleipnirBase {
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.
*/
/// 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<Scalar>& operator()(int row, int col)
requires(!std::is_const_v<Mat>)
{
@@ -259,13 +227,11 @@ class VariableBlock : public SleipnirBase {
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.
*/
/// 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<Scalar>& operator()(int row, int col) const {
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
@@ -273,12 +239,10 @@ class VariableBlock : public SleipnirBase {
m_col_slice.start + col * m_col_slice.step);
}
/**
* Returns a scalar subblock at the given index.
*
* @param index The scalar subblock's index.
* @return A scalar subblock at the given index.
*/
/// Returns a scalar subblock at the given index.
///
/// @param index The scalar subblock's index.
/// @return A scalar subblock at the given index.
Variable<Scalar>& operator[](int index)
requires(!std::is_const_v<Mat>)
{
@@ -286,26 +250,22 @@ class VariableBlock : public SleipnirBase {
return (*this)(index / cols(), index % cols());
}
/**
* Returns a scalar subblock at the given index.
*
* @param index The scalar subblock's index.
* @return A scalar subblock at the given index.
*/
/// Returns a scalar subblock at the given index.
///
/// @param index The scalar subblock's index.
/// @return A scalar subblock at the given index.
const Variable<Scalar>& operator[](int index) const {
slp_assert(index >= 0 && index < rows() * cols());
return (*this)(index / cols(), index % 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.
*/
/// 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());
@@ -316,15 +276,13 @@ class VariableBlock : public SleipnirBase {
{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.
*/
/// 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());
@@ -335,26 +293,22 @@ class VariableBlock : public SleipnirBase {
{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.
*/
/// 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 (*this)(row_slice, row_slice_length, col_slice, 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.
*/
/// 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);
@@ -362,18 +316,16 @@ class VariableBlock : public SleipnirBase {
return (*this)(row_slice, row_slice_length, col_slice, 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.
*/
/// 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{
@@ -388,18 +340,16 @@ class VariableBlock : public SleipnirBase {
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.
*/
/// 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,
@@ -416,13 +366,11 @@ class VariableBlock : public SleipnirBase {
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.
*/
/// 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(cols() == 1);
slp_assert(offset >= 0 && offset < rows());
@@ -430,13 +378,11 @@ class VariableBlock : public SleipnirBase {
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.
*/
/// 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(cols() == 1);
slp_assert(offset >= 0 && offset < rows());
@@ -444,56 +390,46 @@ class VariableBlock : public SleipnirBase {
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.
*/
/// 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.
*/
/// 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.
*/
/// 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.
*/
/// 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.
*/
/// 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());
@@ -510,12 +446,10 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Compound matrix multiplication-assignment operator.
*
* @param rhs Variable to multiply.
* @return Result of multiplication.
*/
/// 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) {
@@ -526,12 +460,10 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Compound matrix division-assignment operator.
*
* @param rhs Variable to divide.
* @return Result of division.
*/
/// 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);
@@ -544,12 +476,10 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Compound matrix division-assignment operator.
*
* @param rhs Variable to divide.
* @return Result of division.
*/
/// 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) {
@@ -560,12 +490,10 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Compound addition-assignment operator.
*
* @param rhs Variable to add.
* @return Result of addition.
*/
/// 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());
@@ -578,12 +506,10 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Compound addition-assignment operator.
*
* @param rhs Variable to add.
* @return Result of addition.
*/
/// 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);
@@ -596,12 +522,10 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Compound subtraction-assignment operator.
*
* @param rhs Variable to subtract.
* @return Result of subtraction.
*/
/// 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());
@@ -614,12 +538,10 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Compound subtraction-assignment operator.
*
* @param rhs Variable to subtract.
* @return Result of subtraction.
*/
/// 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);
@@ -632,20 +554,16 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Implicit conversion operator from 1x1 VariableBlock to Variable.
*/
/// Implicit conversion operator from 1x1 VariableBlock to Variable.
// NOLINTNEXTLINE (google-explicit-constructor)
operator Variable<Scalar>() const {
slp_assert(rows() == 1 && cols() == 1);
return (*this)(0, 0);
}
/**
* Returns the transpose of the variable matrix.
*
* @return The transpose of the variable matrix.
*/
/// 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{detail::empty, cols(), rows()};
@@ -658,45 +576,35 @@ class VariableBlock : public SleipnirBase {
return result;
}
/**
* Returns the number of rows in the matrix.
*
* @return The number of rows in the matrix.
*/
/// 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.
*/
/// 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.
*/
/// 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.
Scalar value(int row, int col) { return (*this)(row, col).value(); }
/**
* Returns an element of the variable block.
*
* @param index The index of the element to return.
* @return An element of the variable block.
*/
/// Returns an element of the variable block.
///
/// @param index The index of the element to return.
/// @return An element of the variable block.
Scalar 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.
*/
/// Returns the contents of the variable matrix.
///
/// @return The contents of the variable matrix.
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> value() {
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> result{rows(),
cols()};
@@ -710,12 +618,10 @@ class VariableBlock : public SleipnirBase {
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.
*/
/// 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<Scalar>(const Variable<Scalar>& x)> unary_op)
const {
@@ -827,103 +733,77 @@ class VariableBlock : public SleipnirBase {
#endif // DOXYGEN_SHOULD_SKIP_THIS
/**
* Returns begin iterator.
*
* @return Begin iterator.
*/
/// Returns begin iterator.
///
/// @return Begin iterator.
iterator begin() { return iterator(this, 0); }
/**
* Returns end iterator.
*
* @return End iterator.
*/
/// Returns end iterator.
///
/// @return End iterator.
iterator end() { return iterator(this, rows() * cols()); }
/**
* Returns begin iterator.
*
* @return Begin iterator.
*/
/// Returns begin iterator.
///
/// @return Begin iterator.
const_iterator begin() const { return const_iterator(this, 0); }
/**
* Returns end iterator.
*
* @return End iterator.
*/
/// Returns end iterator.
///
/// @return End iterator.
const_iterator end() const { return const_iterator(this, rows() * cols()); }
/**
* Returns const begin iterator.
*
* @return Const begin iterator.
*/
/// Returns const begin iterator.
///
/// @return Const begin iterator.
const_iterator cbegin() const { return const_iterator(this, 0); }
/**
* Returns const end iterator.
*
* @return Const end iterator.
*/
/// Returns const end iterator.
///
/// @return Const end iterator.
const_iterator cend() const { return const_iterator(this, rows() * cols()); }
/**
* Returns reverse begin iterator.
*
* @return Reverse begin iterator.
*/
/// Returns reverse begin iterator.
///
/// @return Reverse begin iterator.
reverse_iterator rbegin() { return reverse_iterator{end()}; }
/**
* Returns reverse end iterator.
*
* @return Reverse end iterator.
*/
/// Returns reverse end iterator.
///
/// @return Reverse end iterator.
reverse_iterator rend() { return reverse_iterator{begin()}; }
/**
* Returns const reverse begin iterator.
*
* @return Const reverse begin iterator.
*/
/// Returns const reverse begin iterator.
///
/// @return Const reverse begin iterator.
const_reverse_iterator rbegin() const {
return const_reverse_iterator{end()};
}
/**
* Returns const reverse end iterator.
*
* @return Const reverse end iterator.
*/
/// Returns const reverse end iterator.
///
/// @return Const reverse end iterator.
const_reverse_iterator rend() const {
return const_reverse_iterator{begin()};
}
/**
* Returns const reverse begin iterator.
*
* @return Const reverse begin iterator.
*/
/// Returns const reverse begin iterator.
///
/// @return Const reverse begin iterator.
const_reverse_iterator crbegin() const {
return const_reverse_iterator{cend()};
}
/**
* Returns const reverse end iterator.
*
* @return Const reverse end iterator.
*/
/// Returns const reverse end iterator.
///
/// @return Const reverse end iterator.
const_reverse_iterator crend() const {
return const_reverse_iterator{cbegin()};
}
/**
* Returns number of elements in matrix.
*
* @return Number of elements in matrix.
*/
/// Returns number of elements in matrix.
///
/// @return Number of elements in matrix.
size_t size() const { return rows() * cols(); }
private:

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

File diff suppressed because it is too large Load Diff

42
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/multistart.hpp vendored
View File

@@ -13,13 +13,11 @@
namespace slp {
/**
* The result of a multistart solve.
*
* @tparam Scalar Scalar type.
* @tparam DecisionVariables The type containing the decision variable initial
* guess.
*/
/// The result of a multistart solve.
///
/// @tparam Scalar Scalar type.
/// @tparam DecisionVariables The type containing the decision variable initial
/// guess.
template <typename Scalar, typename DecisionVariables>
struct MultistartResult {
/// The solver exit status.
@@ -30,23 +28,21 @@ struct MultistartResult {
DecisionVariables variables;
};
/**
* Solves an optimization problem from different starting points in parallel,
* then returns the solution with the lowest cost.
*
* Each solve is performed on a separate thread. Solutions from successful
* solves are always preferred over solutions from unsuccessful solves, and cost
* (lower is better) is the tiebreaker between successful solves.
*
* @tparam Scalar Scalar type.
* @tparam DecisionVariables The type containing the decision variable initial
* guess.
* @param solve A user-provided function that takes a decision variable initial
* guess and returns a MultistartResult.
* @param initial_guesses A list of decision variable initial guesses to try.
*/
/// Solves an optimization problem from different starting points in parallel,
/// then returns the solution with the lowest cost.
///
/// Each solve is performed on a separate thread. Solutions from successful
/// solves are always preferred over solutions from unsuccessful solves, and
/// cost (lower is better) is the tiebreaker between successful solves.
///
/// @tparam Scalar Scalar type.
/// @tparam DecisionVariables The type containing the decision variable initial
/// guess.
/// @param solve A user-provided function that takes a decision variable initial
/// guess and returns a MultistartResult.
/// @param initial_guesses A list of decision variable initial guesses to try.
template <typename Scalar, typename DecisionVariables>
MultistartResult<Scalar, DecisionVariables> Multistart(
MultistartResult<Scalar, DecisionVariables> multistart(
function_ref<MultistartResult<Scalar, DecisionVariables>(
const DecisionVariables& initial_guess)>
solve,

292
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp.hpp vendored
View File

@@ -19,54 +19,52 @@
namespace slp {
/**
* This class allows the user to pose and solve a constrained optimal control
* problem (OCP) in a variety of ways.
*
* The system is transcripted by one of three methods (direct transcription,
* direct collocation, or single-shooting) and additional constraints can be
* added.
*
* In direct transcription, each state is a decision variable constrained to the
* integrated dynamics of the previous state. In direct collocation, the
* trajectory is modeled as a series of cubic polynomials where the centerpoint
* slope is constrained. In single-shooting, states depend explicitly as a
* function of all previous states and all previous inputs.
*
* Explicit ODEs are integrated using RK4.
*
* For explicit ODEs, the function must be in the form dx/dt = f(t, x, u).
* For discrete state transition functions, the function must be in the form
* xₖ₊₁ = f(t, xₖ, uₖ).
*
* Direct collocation requires an explicit ODE. Direct transcription and
* single-shooting can use either an ODE or state transition function.
*
* https://underactuated.mit.edu/trajopt.html goes into more detail on each
* transcription method.
*
* @tparam Scalar Scalar type.
*/
/// This class allows the user to pose and solve a constrained optimal control
/// problem (OCP) in a variety of ways.
///
/// The system is transcripted by one of three methods (direct transcription,
/// direct collocation, or single-shooting) and additional constraints can be
/// added.
///
/// In direct transcription, each state is a decision variable constrained to
/// the integrated dynamics of the previous state. In direct collocation, the
/// trajectory is modeled as a series of cubic polynomials where the centerpoint
/// slope is constrained. In single-shooting, states depend explicitly as a
/// function of all previous states and all previous inputs.
///
/// Explicit ODEs are integrated using RK4.
///
/// For explicit ODEs, the function must be in the form dx/dt = f(t, x, u).
/// For discrete state transition functions, the function must be in the form
/// xₖ₊₁ = f(t, xₖ, uₖ).
///
/// Direct collocation requires an explicit ODE. Direct transcription and
/// single-shooting can use either an ODE or state transition function.
///
/// https://underactuated.mit.edu/trajopt.html goes into more detail on each
/// transcription method.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
class OCP : public Problem<Scalar> {
public:
/**
* Build an optimization problem using a system evolution function (explicit
* ODE or discrete state transition function).
*
* @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 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 dynamics_type The type of system evolution function.
* @param timestep_method The timestep method.
* @param transcription_method The transcription method.
*/
/// Build an optimization problem using a system evolution function (explicit
/// ODE or discrete state transition function).
///
/// @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 num_steps The number of control points.
/// @param dynamics Function representing an explicit or implicit ODE, or a
/// discrete state transition function.
/// <ul>
/// <li>Explicit: dx/dt = f(x, u, *)</li>
/// <li>Implicit: f([x dx/dt]', u, *) = 0</li>
/// <li>State transition: xₖ₊₁ = f(xₖ, uₖ)</li>
/// </ul>
/// @param dynamics_type The type of system evolution function.
/// @param timestep_method The timestep method.
/// @param transcription_method The transcription method.
OCP(int num_states, int num_inputs, std::chrono::duration<Scalar> dt,
int num_steps,
function_ref<VariableMatrix<Scalar>(const VariableMatrix<Scalar>& x,
@@ -89,23 +87,23 @@ class OCP : public Problem<Scalar> {
timestep_method,
transcription_method} {}
/**
* Build an optimization problem using a system evolution function (explicit
* ODE or discrete state transition function).
*
* @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 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 dynamics_type The type of system evolution function.
* @param timestep_method The timestep method.
* @param transcription_method The transcription method.
*/
/// Build an optimization problem using a system evolution function (explicit
/// ODE or discrete state transition function).
///
/// @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 num_steps The number of control points.
/// @param dynamics Function representing an explicit or implicit ODE, or a
/// discrete state transition function.
/// <ul>
/// <li>Explicit: dx/dt = f(t, x, u, *)</li>
/// <li>Implicit: f(t, [x dx/dt]', u, *) = 0</li>
/// <li>State transition: xₖ₊₁ = f(t, xₖ, uₖ, dt)</li>
/// </ul>
/// @param dynamics_type The type of system evolution function.
/// @param timestep_method The timestep method.
/// @param transcription_method The transcription method.
OCP(int num_states, int num_inputs, std::chrono::duration<Scalar> dt,
int num_steps,
function_ref<VariableMatrix<Scalar>(
@@ -158,36 +156,30 @@ class OCP : public Problem<Scalar> {
}
}
/**
* Utility function to constrain the initial state.
*
* @param initial_state the initial state to constrain to.
*/
/// Utility function to constrain the initial state.
///
/// @param initial_state the initial state to constrain to.
template <typename T>
requires ScalarLike<T> || MatrixLike<T>
void constrain_initial_state(const T& initial_state) {
this->subject_to(this->initial_state() == initial_state);
}
/**
* Utility function to constrain the final state.
*
* @param final_state the final state to constrain to.
*/
/// Utility function to constrain the final state.
///
/// @param final_state the final state to constrain to.
template <typename T>
requires ScalarLike<T> || MatrixLike<T>
void constrain_final_state(const T& final_state) {
this->subject_to(this->final_state() == final_state);
}
/**
* Set the constraint evaluation function. This function is called
* `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.
*/
/// Set the constraint evaluation function. This function is called
/// `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 for_each_step(const function_ref<void(const VariableMatrix<Scalar>& x,
const VariableMatrix<Scalar>& u)>
callback) {
@@ -198,14 +190,12 @@ class OCP : public Problem<Scalar> {
}
}
/**
* Set the constraint evaluation function. This function is called
* `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.
*/
/// Set the constraint evaluation function. This function is called
/// `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 for_each_step(
const function_ref<
void(const Variable<Scalar>& t, const VariableMatrix<Scalar>& x,
@@ -223,12 +213,10 @@ class OCP : public Problem<Scalar> {
}
}
/**
* Convenience function to set a lower bound on the input.
*
* @param lower_bound The lower bound that inputs must always be above. Must
* be shaped (num_inputs)x1.
*/
/// Convenience function to set a lower bound on the input.
///
/// @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 set_lower_input_bound(const T& lower_bound) {
@@ -237,12 +225,10 @@ class OCP : public Problem<Scalar> {
}
}
/**
* Convenience function to set an upper bound on the input.
*
* @param upper_bound The upper bound that inputs must always be below. Must
* be shaped (num_inputs)x1.
*/
/// Convenience function to set an upper bound on the input.
///
/// @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 set_upper_input_bound(const T& upper_bound) {
@@ -251,68 +237,54 @@ class OCP : public Problem<Scalar> {
}
}
/**
* Convenience function to set a lower bound on the timestep.
*
* @param min_timestep The minimum timestep.
*/
/// Convenience function to set a lower bound on the timestep.
///
/// @param min_timestep The minimum timestep.
void set_min_timestep(std::chrono::duration<Scalar> min_timestep) {
this->subject_to(dt() >= min_timestep.count());
}
/**
* Convenience function to set an upper bound on the timestep.
*
* @param max_timestep The maximum timestep.
*/
/// Convenience function to set an upper bound on the timestep.
///
/// @param max_timestep The maximum timestep.
void set_max_timestep(std::chrono::duration<Scalar> max_timestep) {
this->subject_to(dt() <= max_timestep.count());
}
/**
* Get the state variables. After the problem is solved, this will contain the
* optimized trajectory.
*
* Shaped (num_states)x(num_steps+1).
*
* @return The state variable matrix.
*/
/// Get the state variables. After the problem is solved, this will contain
/// the optimized trajectory.
///
/// Shaped (num_states)x(num_steps+1).
///
/// @return The state variable matrix.
VariableMatrix<Scalar>& X() { return m_X; }
/**
* Get the input variables. After the problem is solved, this will contain the
* inputs corresponding to the optimized trajectory.
*
* Shaped (num_inputs)x(num_steps+1), although the last input step is unused
* in the trajectory.
*
* @return The input variable matrix.
*/
/// Get the input variables. After the problem is solved, this will contain
/// the inputs corresponding to the optimized trajectory.
///
/// Shaped (num_inputs)x(num_steps+1), although the last input step is unused
/// in the trajectory.
///
/// @return The input variable matrix.
VariableMatrix<Scalar>& 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(num_steps+1), although the last timestep is unused in
* the trajectory.
*
* @return The timestep variable matrix.
*/
/// Get the timestep variables. After the problem is solved, this will contain
/// the timesteps corresponding to the optimized trajectory.
///
/// Shaped 1x(num_steps+1), although the last timestep is unused in the
/// trajectory.
///
/// @return The timestep variable matrix.
VariableMatrix<Scalar>& dt() { return m_DT; }
/**
* Convenience function to get the initial state in the trajectory.
*
* @return The initial state of the trajectory.
*/
/// Convenience function to get the initial state in the trajectory.
///
/// @return The initial state of the trajectory.
VariableMatrix<Scalar> initial_state() { return m_X.col(0); }
/**
* Convenience function to get the final state in the trajectory.
*
* @return The final state of the trajectory.
*/
/// Convenience function to get the final state in the trajectory.
///
/// @return The final state of the trajectory.
VariableMatrix<Scalar> final_state() { return m_X.col(m_num_steps); }
private:
@@ -328,15 +300,13 @@ class OCP : public Problem<Scalar> {
VariableMatrix<Scalar> m_U;
VariableMatrix<Scalar> m_DT;
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(t, x, u) for dt.
*
* @param f The function to integrate. It must take two arguments x and u.
* @param x The initial value of x.
* @param u The value u held constant over the integration period.
* @param t0 The initial time.
* @param dt The time over which to integrate.
*/
/// Performs 4th order Runge-Kutta integration of dx/dt = f(t, x, u) for dt.
///
/// @param f The function to integrate. It must take two arguments x and u.
/// @param x The initial value of x.
/// @param u The value u held constant over the integration period.
/// @param t0 The initial time.
/// @param dt The time over which to integrate.
template <typename F, typename State, typename Input, typename Time>
State rk4(F&& f, State x, Input u, Time t0, Time dt) {
auto halfdt = dt * Scalar(0.5);
@@ -348,9 +318,7 @@ class OCP : public Problem<Scalar> {
return x + (k1 + k2 * Scalar(2) + k3 * Scalar(2) + k4) * (dt / Scalar(6));
}
/**
* Apply direct collocation dynamics constraints.
*/
/// Apply direct collocation dynamics constraints.
void constrain_direct_collocation() {
slp_assert(m_dynamics_type == DynamicsType::EXPLICIT_ODE);
@@ -387,9 +355,7 @@ class OCP : public Problem<Scalar> {
}
}
/**
* Apply direct transcription dynamics constraints.
*/
/// Apply direct transcription dynamics constraints.
void constrain_direct_transcription() {
Variable<Scalar> time{0};
@@ -412,9 +378,7 @@ class OCP : public Problem<Scalar> {
}
}
/**
* Apply single shooting dynamics constraints.
*/
/// Apply single shooting dynamics constraints.
void constrain_single_shooting() {
Variable<Scalar> time{0};

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

@@ -6,9 +6,7 @@
namespace slp {
/**
* Enum describing a type of system dynamics constraints.
*/
/// Enum describing a type of system dynamics constraints.
enum class DynamicsType : uint8_t {
/// The dynamics are a function in the form dx/dt = f(t, x, u).
EXPLICIT_ODE,

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

@@ -6,9 +6,7 @@
namespace slp {
/**
* Enum describing the type of system timestep.
*/
/// Enum describing the type of system timestep.
enum class TimestepMethod : uint8_t {
/// The timestep is a fixed constant.
FIXED,

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

@@ -6,9 +6,7 @@
namespace slp {
/**
* Enum describing an OCP transcription method.
*/
/// Enum describing an OCP transcription method.
enum class TranscriptionMethod : uint8_t {
/// Each state is a decision variable constrained to the integrated dynamics
/// of the previous state.

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

@@ -40,55 +40,49 @@
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.
*
* @tparam Scalar Scalar type.
*/
/// 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:
///
/// ```
/// minₓ f(x)
/// subject to cₑ(x) = 0
/// cᵢ(x) ≥ 0
/// ```
///
/// 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.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
class Problem {
public:
/**
* Construct the optimization problem.
*/
/// Construct the optimization problem.
Problem() noexcept = default;
/**
* Create a decision variable in the optimization problem.
*
* @return A decision variable in the optimization problem.
*/
/// Create a decision variable in the optimization problem.
///
/// @return A decision variable in the optimization problem.
[[nodiscard]]
Variable<Scalar> 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.
*/
/// 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<Scalar> decision_variable(int rows, int cols = 1) {
m_decision_variables.reserve(m_decision_variables.size() + rows * cols);
@@ -105,17 +99,15 @@ class Problem {
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.
*/
/// 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<Scalar> symmetric_decision_variable(int rows) {
// We only need to store the lower triangle of an n x n symmetric matrix;
@@ -141,62 +133,52 @@ class Problem {
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.
*/
/// 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<Scalar>& 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.
*/
/// 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<Scalar>&& 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.
*/
/// 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<Scalar>& 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.
*/
/// 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<Scalar>&& 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.
*/
/// Tells the solver to solve the problem while satisfying the given equality
/// constraint.
///
/// @param constraint The constraint to satisfy.
void subject_to(const EqualityConstraints<Scalar>& constraint) {
m_equality_constraints.reserve(m_equality_constraints.size() +
constraint.constraints.size());
@@ -204,12 +186,10 @@ class Problem {
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.
*/
/// Tells the solver to solve the problem while satisfying the given equality
/// constraint.
///
/// @param constraint The constraint to satisfy.
void subject_to(EqualityConstraints<Scalar>&& constraint) {
m_equality_constraints.reserve(m_equality_constraints.size() +
constraint.constraints.size());
@@ -217,12 +197,10 @@ class Problem {
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.
*/
/// Tells the solver to solve the problem while satisfying the given
/// inequality constraint.
///
/// @param constraint The constraint to satisfy.
void subject_to(const InequalityConstraints<Scalar>& constraint) {
m_inequality_constraints.reserve(m_inequality_constraints.size() +
constraint.constraints.size());
@@ -230,12 +208,10 @@ class Problem {
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.
*/
/// Tells the solver to solve the problem while satisfying the given
/// inequality constraint.
///
/// @param constraint The constraint to satisfy.
void subject_to(InequalityConstraints<Scalar>&& constraint) {
m_inequality_constraints.reserve(m_inequality_constraints.size() +
constraint.constraints.size());
@@ -243,11 +219,9 @@ class Problem {
std::back_inserter(m_inequality_constraints));
}
/**
* Returns the cost function's type.
*
* @return The cost function's type.
*/
/// 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();
@@ -256,11 +230,9 @@ class Problem {
}
}
/**
* Returns the type of the highest order equality constraint.
*
* @return The type of the highest order equality constraint.
*/
/// 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, {},
@@ -271,11 +243,9 @@ class Problem {
}
}
/**
* Returns the type of the highest order inequality constraint.
*
* @return The type of the highest order inequality constraint.
*/
/// 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, {},
@@ -286,20 +256,22 @@ class Problem {
}
}
/**
* 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.
*/
/// 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) {
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
using SparseMatrix = Eigen::SparseMatrix<Scalar>;
using SparseVector = Eigen::SparseVector<Scalar>;
// Create the initial value column vector
Eigen::Vector<Scalar, Eigen::Dynamic> x{m_decision_variables.size()};
DenseVector x{m_decision_variables.size()};
for (size_t i = 0; i < m_decision_variables.size(); ++i) {
x[i] = m_decision_variables[i].value();
}
@@ -385,23 +357,20 @@ class Problem {
#endif
// Invoke Newton solver
status = newton<Scalar>(
NewtonMatrixCallbacks<Scalar>{
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x) -> Scalar {
x_ad.set_value(x);
return f.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseVector<Scalar> {
x_ad.set_value(x);
return g.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
x_ad.set_value(x);
return H.value();
}},
callbacks, options, x);
status = newton<Scalar>(NewtonMatrixCallbacks<Scalar>{
[&](const DenseVector& x) -> Scalar {
x_ad.set_value(x);
return f.value();
},
[&](const DenseVector& x) -> SparseVector {
x_ad.set_value(x);
return g.value();
},
[&](const DenseVector& x) -> SparseMatrix {
x_ad.set_value(x);
return H.value();
}},
callbacks, options, x);
} else if (m_inequality_constraints.empty()) {
if (options.diagnostics) {
slp::println("\nInvoking SQP solver\n");
@@ -449,29 +418,24 @@ class Problem {
// Invoke SQP solver
status = sqp<Scalar>(
SQPMatrixCallbacks<Scalar>{
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x) -> Scalar {
[&](const DenseVector& x) -> Scalar {
x_ad.set_value(x);
return f.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseVector<Scalar> {
[&](const DenseVector& x) -> SparseVector {
x_ad.set_value(x);
return g.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x,
const Eigen::Vector<Scalar, Eigen::Dynamic>& y)
-> Eigen::SparseMatrix<Scalar> {
[&](const DenseVector& x, const DenseVector& y) -> SparseMatrix {
x_ad.set_value(x);
y_ad.set_value(y);
return H.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::Vector<Scalar, Eigen::Dynamic> {
[&](const DenseVector& x) -> DenseVector {
x_ad.set_value(x);
return c_e_ad.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
[&](const DenseVector& x) -> SparseMatrix {
x_ad.set_value(x);
return A_e.value();
}},
@@ -550,41 +514,34 @@ class Problem {
// Invoke interior-point method solver
status = interior_point<Scalar>(
InteriorPointMatrixCallbacks<Scalar>{
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x) -> Scalar {
[&](const DenseVector& x) -> Scalar {
x_ad.set_value(x);
return f.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseVector<Scalar> {
[&](const DenseVector& x) -> SparseVector {
x_ad.set_value(x);
return g.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x,
const Eigen::Vector<Scalar, Eigen::Dynamic>& y,
const Eigen::Vector<Scalar, Eigen::Dynamic>& z)
-> Eigen::SparseMatrix<Scalar> {
[&](const DenseVector& x, const DenseVector& y,
const DenseVector& z) -> SparseMatrix {
x_ad.set_value(x);
y_ad.set_value(y);
z_ad.set_value(z);
return H.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::Vector<Scalar, Eigen::Dynamic> {
[&](const DenseVector& x) -> DenseVector {
x_ad.set_value(x);
return c_e_ad.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
[&](const DenseVector& x) -> SparseMatrix {
x_ad.set_value(x);
return A_e.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::Vector<Scalar, Eigen::Dynamic> {
[&](const DenseVector& x) -> DenseVector {
x_ad.set_value(x);
return c_i_ad.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
[&](const DenseVector& x) -> SparseMatrix {
x_ad.set_value(x);
return A_i.value();
}},
@@ -597,7 +554,7 @@ class Problem {
if (options.diagnostics) {
print_autodiff_diagnostics(ad_setup_profilers);
slp::println("\nExit: {}", to_message(status));
slp::println("\nExit: {}", status);
}
// Assign the solution to the original Variable instances
@@ -606,13 +563,11 @@ class Problem {
return status;
}
/**
* 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.
*/
/// 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<Scalar>& info) {
{ callback(info) } -> std::same_as<void>;
@@ -626,14 +581,12 @@ class Problem {
});
}
/**
* 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.
*/
/// 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<Scalar>& info) {
{ callback(info) } -> std::same_as<bool>;
@@ -642,21 +595,17 @@ class Problem {
m_iteration_callbacks.emplace_back(std::forward<F>(callback));
}
/**
* Clears the registered callbacks.
*/
/// Clears the registered callbacks.
void clear_callbacks() { m_iteration_callbacks.clear(); }
/**
* Adds a callback to be called at the beginning of each solver iteration.
*
* Language bindings should call this in the Problem constructor to register
* callbacks that shouldn't be removed by clear_callbacks(). Persistent
* callbacks run after non-persistent callbacks.
*
* @param callback The callback. Returning true from the callback causes the
* solver to exit early with the solution it has so far.
*/
/// Adds a callback to be called at the beginning of each solver iteration.
///
/// Language bindings should call this in the Problem constructor to register
/// callbacks that shouldn't be removed by clear_callbacks(). Persistent
/// callbacks run after non-persistent callbacks.
///
/// @param callback The callback. Returning true from the callback causes the
/// solver to exit early with the solution it has so far.
template <typename F>
requires requires(F callback, const IterationInfo<Scalar>& info) {
{ callback(info) } -> std::same_as<bool>;

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

@@ -4,15 +4,13 @@
#include <stdint.h>
#include <string_view>
#include <fmt/base.h>
#include "sleipnir/util/symbol_exports.hpp"
#include "sleipnir/util/unreachable.hpp"
namespace slp {
/**
* Solver exit status. Negative values indicate failure.
*/
/// Solver exit status. Negative values indicate failure.
enum class ExitStatus : int8_t {
/// Solved the problem to the desired tolerance.
SUCCESS = 0,
@@ -42,41 +40,58 @@ enum class ExitStatus : int8_t {
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
/// Formatter for ExitStatus.
template <>
struct fmt::formatter<slp::ExitStatus> {
/// Parse format string.
///
/// @param ctx Format parse context.
/// @return Format parse context iterator.
constexpr auto parse(fmt::format_parse_context& ctx) {
return m_underlying.parse(ctx);
}
/// Format ExitStatus.
///
/// @tparam FmtContext Format context type.
/// @param exit_status Exit status.
/// @param ctx Format context.
/// @return Format context iterator.
template <typename FmtContext>
auto format(const slp::ExitStatus& exit_status, FmtContext& ctx) const {
using enum slp::ExitStatus;
switch (exit_status) {
case SUCCESS:
return m_underlying.format("success", ctx);
case CALLBACK_REQUESTED_STOP:
return m_underlying.format("callback requested stop", ctx);
case TOO_FEW_DOFS:
return m_underlying.format("too few degrees of freedom", ctx);
case LOCALLY_INFEASIBLE:
return m_underlying.format("locally infeasible", ctx);
case GLOBALLY_INFEASIBLE:
return m_underlying.format("globally infeasible", ctx);
case FACTORIZATION_FAILED:
return m_underlying.format("factorization failed", ctx);
case LINE_SEARCH_FAILED:
return m_underlying.format("line search failed", ctx);
case NONFINITE_INITIAL_COST_OR_CONSTRAINTS:
return m_underlying.format("nonfinite initial cost or constraints",
ctx);
case DIVERGING_ITERATES:
return m_underlying.format("diverging iterates", ctx);
case MAX_ITERATIONS_EXCEEDED:
return m_underlying.format("max iterations exceeded", ctx);
case TIMEOUT:
return m_underlying.format("timeout", ctx);
default:
slp::unreachable();
}
}
private:
fmt::formatter<const char*> m_underlying;
};

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

@@ -37,30 +37,28 @@
namespace slp {
/**
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.
@tparam Scalar Scalar type.
@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.
*/
/// Finds the optimal solution to a nonlinear program using the interior-point
/// method.
///
/// A nonlinear program has the form:
///
/// ```
/// min_x f(x)
/// subject to cₑ(x) = 0
/// cᵢ(x) ≥ 0
/// ```
///
/// where f(x) is the cost function, cₑ(x) are the equality constraints, and
/// cᵢ(x) are the inequality constraints.
///
/// @tparam Scalar Scalar type.
/// @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.
template <typename Scalar>
ExitStatus interior_point(
const InteriorPointMatrixCallbacks<Scalar>& matrix_callbacks,
@@ -71,18 +69,20 @@ ExitStatus interior_point(
const Eigen::ArrayX<bool>& bound_constraint_mask,
#endif
Eigen::Vector<Scalar, Eigen::Dynamic>& x) {
/**
* Interior-point method step direction.
*/
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
using SparseMatrix = Eigen::SparseMatrix<Scalar>;
using SparseVector = Eigen::SparseVector<Scalar>;
/// Interior-point method step direction.
struct Step {
/// Primal step.
Eigen::Vector<Scalar, Eigen::Dynamic> p_x;
DenseVector p_x;
/// Equality constraint dual step.
Eigen::Vector<Scalar, Eigen::Dynamic> p_y;
DenseVector p_y;
/// Inequality constraint slack variable step.
Eigen::Vector<Scalar, Eigen::Dynamic> p_s;
DenseVector p_s;
/// Inequality constraint dual step.
Eigen::Vector<Scalar, Eigen::Dynamic> p_z;
DenseVector p_z;
};
using std::isfinite;
@@ -132,39 +132,32 @@ ExitStatus interior_point(
auto& A_i_prof = solve_profilers[17];
InteriorPointMatrixCallbacks<Scalar> matrices{
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x) -> Scalar {
[&](const DenseVector& x) -> Scalar {
ScopedProfiler prof{f_prof};
return matrix_callbacks.f(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseVector<Scalar> {
[&](const DenseVector& x) -> SparseVector {
ScopedProfiler prof{g_prof};
return matrix_callbacks.g(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x,
const Eigen::Vector<Scalar, Eigen::Dynamic>& y,
const Eigen::Vector<Scalar, Eigen::Dynamic>& z)
-> Eigen::SparseMatrix<Scalar> {
[&](const DenseVector& x, const DenseVector& y,
const DenseVector& z) -> SparseMatrix {
ScopedProfiler prof{H_prof};
return matrix_callbacks.H(x, y, z);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::Vector<Scalar, Eigen::Dynamic> {
[&](const DenseVector& x) -> DenseVector {
ScopedProfiler prof{c_e_prof};
return matrix_callbacks.c_e(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
[&](const DenseVector& x) -> SparseMatrix {
ScopedProfiler prof{A_e_prof};
return matrix_callbacks.A_e(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::Vector<Scalar, Eigen::Dynamic> {
[&](const DenseVector& x) -> DenseVector {
ScopedProfiler prof{c_i_prof};
return matrix_callbacks.c_i(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
[&](const DenseVector& x) -> SparseMatrix {
ScopedProfiler prof{A_i_prof};
return matrix_callbacks.A_i(x);
}};
@@ -176,8 +169,8 @@ ExitStatus interior_point(
setup_prof.start();
Scalar f = matrices.f(x);
Eigen::Vector<Scalar, Eigen::Dynamic> c_e = matrices.c_e(x);
Eigen::Vector<Scalar, Eigen::Dynamic> c_i = matrices.c_i(x);
DenseVector c_e = matrices.c_e(x);
DenseVector c_i = matrices.c_i(x);
int num_decision_variables = x.rows();
int num_equality_constraints = c_e.rows();
@@ -192,22 +185,19 @@ ExitStatus interior_point(
return ExitStatus::TOO_FEW_DOFS;
}
Eigen::SparseVector<Scalar> g = matrices.g(x);
Eigen::SparseMatrix<Scalar> A_e = matrices.A_e(x);
Eigen::SparseMatrix<Scalar> A_i = matrices.A_i(x);
SparseVector g = matrices.g(x);
SparseMatrix A_e = matrices.A_e(x);
SparseMatrix A_i = matrices.A_i(x);
Eigen::Vector<Scalar, Eigen::Dynamic> s =
Eigen::Vector<Scalar, Eigen::Dynamic>::Ones(num_inequality_constraints);
DenseVector s = DenseVector::Ones(num_inequality_constraints);
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
// We set sʲ = cᵢʲ(x) for each bound inequality constraint index j
s = bound_constraint_mask.select(c_i, s);
#endif
Eigen::Vector<Scalar, Eigen::Dynamic> y =
Eigen::Vector<Scalar, Eigen::Dynamic>::Zero(num_equality_constraints);
Eigen::Vector<Scalar, Eigen::Dynamic> z =
Eigen::Vector<Scalar, Eigen::Dynamic>::Ones(num_inequality_constraints);
DenseVector y = DenseVector::Zero(num_equality_constraints);
DenseVector z = DenseVector::Ones(num_inequality_constraints);
Eigen::SparseMatrix<Scalar> H = matrices.H(x, y, z);
SparseMatrix H = matrices.H(x, y, z);
// Ensure matrix callback dimensions are consistent
slp_assert(g.rows() == num_decision_variables);
@@ -340,38 +330,33 @@ ExitStatus interior_point(
// S = diag(s)
// Z = diag(z)
// Σ = S⁻¹Z
const Eigen::SparseMatrix<Scalar> Σ{s.cwiseInverse().asDiagonal() *
z.asDiagonal()};
const SparseMatrix Σ{s.cwiseInverse().asDiagonal() * z.asDiagonal()};
// lhs = [H + AᵢᵀΣAᵢ Aₑᵀ]
// [ Aₑ 0 ]
//
// Don't assign upper triangle because solver only uses lower triangle.
const Eigen::SparseMatrix<Scalar> top_left =
const SparseMatrix top_left =
H + (A_i.transpose() * Σ * A_i).template triangularView<Eigen::Lower>();
triplets.clear();
triplets.reserve(top_left.nonZeros() + A_e.nonZeros());
for (int col = 0; col < H.cols(); ++col) {
// Append column of H + AᵢᵀΣAᵢ lower triangle in top-left quadrant
for (typename Eigen::SparseMatrix<Scalar>::InnerIterator it{top_left,
col};
it; ++it) {
for (typename SparseMatrix::InnerIterator it{top_left, col}; it; ++it) {
triplets.emplace_back(it.row(), it.col(), it.value());
}
// Append column of Aₑ in bottom-left quadrant
for (typename Eigen::SparseMatrix<Scalar>::InnerIterator it{A_e, col}; it;
++it) {
for (typename SparseMatrix::InnerIterator it{A_e, col}; it; ++it) {
triplets.emplace_back(H.rows() + it.row(), it.col(), it.value());
}
}
Eigen::SparseMatrix<Scalar> lhs(
num_decision_variables + num_equality_constraints,
num_decision_variables + num_equality_constraints);
SparseMatrix lhs(num_decision_variables + num_equality_constraints,
num_decision_variables + num_equality_constraints);
lhs.setFromSortedTriplets(triplets.begin(), triplets.end());
// rhs = −[∇f − Aₑᵀy − Aᵢᵀ(−Σcᵢ + μS⁻¹e + z)]
// [ cₑ ]
Eigen::Vector<Scalar, Eigen::Dynamic> rhs{x.rows() + y.rows()};
DenseVector rhs{x.rows() + y.rows()};
rhs.segment(0, x.rows()) =
-g + A_e.transpose() * y +
A_i.transpose() * (-Σ * c_i + μ * s.cwiseInverse() + z);
@@ -399,7 +384,7 @@ ExitStatus interior_point(
auto compute_step = [&](Step& step) {
// p = [ pˣ]
// [−pʸ]
Eigen::Vector<Scalar, Eigen::Dynamic> p = solver.solve(rhs);
DenseVector p = solver.solve(rhs);
step.p_x = p.segment(0, x.rows());
step.p_y = -p.segment(x.rows(), y.rows());
@@ -427,13 +412,13 @@ ExitStatus interior_point(
// Loop until a step is accepted
while (1) {
Eigen::Vector<Scalar, Eigen::Dynamic> trial_x = x + α * step.p_x;
Eigen::Vector<Scalar, Eigen::Dynamic> trial_y = y + α_z * step.p_y;
Eigen::Vector<Scalar, Eigen::Dynamic> trial_z = z + α_z * step.p_z;
DenseVector trial_x = x + α * step.p_x;
DenseVector trial_y = y + α_z * step.p_y;
DenseVector trial_z = z + α_z * step.p_z;
Scalar trial_f = matrices.f(trial_x);
Eigen::Vector<Scalar, Eigen::Dynamic> trial_c_e = matrices.c_e(trial_x);
Eigen::Vector<Scalar, Eigen::Dynamic> trial_c_i = matrices.c_i(trial_x);
DenseVector trial_c_e = matrices.c_e(trial_x);
DenseVector trial_c_i = matrices.c_i(trial_x);
// If f(xₖ + αpₖˣ), cₑ(xₖ + αpₖˣ), or cᵢ(xₖ + αpₖˣ) aren't finite, reduce
// step size immediately
@@ -448,7 +433,7 @@ ExitStatus interior_point(
continue;
}
Eigen::Vector<Scalar, Eigen::Dynamic> trial_s;
DenseVector trial_s;
if (options.feasible_ipm && c_i.cwiseGreater(Scalar(0)).all()) {
// If the inequality constraints are all feasible, prevent them from
// becoming infeasible again.
@@ -483,7 +468,7 @@ ExitStatus interior_point(
Scalar α_soc = α;
Scalar α_z_soc = α_z;
Eigen::Vector<Scalar, Eigen::Dynamic> c_e_soc = c_e;
DenseVector c_e_soc = c_e;
bool step_acceptable = false;
for (int soc_iteration = 0; soc_iteration < 5 && !step_acceptable;

51
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point_matrix_callbacks.hpp vendored
View File

@@ -9,13 +9,18 @@
namespace slp {
/**
* Matrix callbacks for the interior-point method solver.
*
* @tparam Scalar Scalar type.
*/
/// Matrix callbacks for the interior-point method solver.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
struct InteriorPointMatrixCallbacks {
/// Type alias for dense vector.
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
/// Type alias for sparse matrix.
using SparseMatrix = Eigen::SparseMatrix<Scalar>;
/// Type alias for sparse vector.
using SparseVector = Eigen::SparseVector<Scalar>;
/// Cost function value f(x) getter.
///
/// <table>
@@ -35,7 +40,7 @@ struct InteriorPointMatrixCallbacks {
/// <td>1</td>
/// </tr>
/// </table>
std::function<Scalar(const Eigen::Vector<Scalar, Eigen::Dynamic>& x)> f;
std::function<Scalar(const DenseVector& x)> f;
/// Cost function gradient ∇f(x) getter.
///
@@ -56,9 +61,7 @@ struct InteriorPointMatrixCallbacks {
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::SparseVector<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
g;
std::function<SparseVector(const DenseVector& x)> g;
/// Lagrangian Hessian ∇ₓₓ²L(x, y, z) getter.
///
@@ -91,10 +94,8 @@ struct InteriorPointMatrixCallbacks {
/// <td>num_decision_variables</td>
/// </tr>
/// </table>
std::function<Eigen::SparseMatrix<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x,
const Eigen::Vector<Scalar, Eigen::Dynamic>& y,
const Eigen::Vector<Scalar, Eigen::Dynamic>& z)>
std::function<SparseMatrix(const DenseVector& x, const DenseVector& y,
const DenseVector& z)>
H;
/// Equality constraint value cₑ(x) getter.
@@ -116,18 +117,16 @@ struct InteriorPointMatrixCallbacks {
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::Vector<Scalar, Eigen::Dynamic>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
c_e;
std::function<DenseVector(const DenseVector& x)> c_e;
/// Equality constraint Jacobian ∂cₑ/∂x getter.
///
/// @verbatim
/// ```
/// [∇ᵀcₑ₁(xₖ)]
/// Aₑ(x) = [∇ᵀcₑ₂(xₖ)]
/// [ ⋮ ]
/// [∇ᵀcₑₘ(xₖ)]
/// @endverbatim
/// ```
///
/// <table>
/// <tr>
@@ -146,9 +145,7 @@ struct InteriorPointMatrixCallbacks {
/// <td>num_decision_variables</td>
/// </tr>
/// </table>
std::function<Eigen::SparseMatrix<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
A_e;
std::function<SparseMatrix(const DenseVector& x)> A_e;
/// Inequality constraint value cᵢ(x) getter.
///
@@ -169,18 +166,16 @@ struct InteriorPointMatrixCallbacks {
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::Vector<Scalar, Eigen::Dynamic>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
c_i;
std::function<DenseVector(const DenseVector& x)> c_i;
/// Inequality constraint Jacobian ∂cᵢ/∂x getter.
///
/// @verbatim
/// ```
/// [∇ᵀcᵢ₁(xₖ)]
/// Aᵢ(x) = [∇ᵀcᵢ₂(xₖ)]
/// [ ⋮ ]
/// [∇ᵀcᵢₘ(xₖ)]
/// @endverbatim
/// ```
///
/// <table>
/// <tr>
@@ -199,9 +194,7 @@ struct InteriorPointMatrixCallbacks {
/// <td>num_decision_variables</td>
/// </tr>
/// </table>
std::function<Eigen::SparseMatrix<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
A_i;
std::function<SparseMatrix(const DenseVector& x)> A_i;
};
} // namespace slp

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

@@ -7,11 +7,9 @@
namespace slp {
/**
* Solver iteration information exposed to an iteration callback.
*
* @tparam Scalar Scalar type.
*/
/// Solver iteration information exposed to an iteration callback.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
struct IterationInfo {
/// The solver iteration.

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

@@ -31,32 +31,34 @@
namespace slp {
/**
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.
@tparam Scalar Scalar type.
@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.
*/
/// Finds the optimal solution to a nonlinear program using Newton's method.
///
/// A nonlinear program has the form:
///
/// ```
/// min_x f(x)
/// ```
///
/// where f(x) is the cost function.
///
/// @tparam Scalar Scalar type.
/// @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.
template <typename Scalar>
ExitStatus newton(
const NewtonMatrixCallbacks<Scalar>& matrix_callbacks,
std::span<std::function<bool(const IterationInfo<Scalar>& info)>>
iteration_callbacks,
const Options& options, Eigen::Vector<Scalar, Eigen::Dynamic>& x) {
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
using SparseMatrix = Eigen::SparseMatrix<Scalar>;
using SparseVector = Eigen::SparseVector<Scalar>;
using std::isfinite;
const auto solve_start_time = std::chrono::steady_clock::now();
@@ -92,17 +94,15 @@ ExitStatus newton(
auto& H_prof = solve_profilers[11];
NewtonMatrixCallbacks<Scalar> matrices{
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x) -> Scalar {
[&](const DenseVector& x) -> Scalar {
ScopedProfiler prof{f_prof};
return matrix_callbacks.f(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseVector<Scalar> {
[&](const DenseVector& x) -> SparseVector {
ScopedProfiler prof{g_prof};
return matrix_callbacks.g(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
[&](const DenseVector& x) -> SparseMatrix {
ScopedProfiler prof{H_prof};
return matrix_callbacks.H(x);
}};
@@ -117,8 +117,8 @@ ExitStatus newton(
int num_decision_variables = x.rows();
Eigen::SparseVector<Scalar> g = matrices.g(x);
Eigen::SparseMatrix<Scalar> H = matrices.H(x);
SparseVector g = matrices.g(x);
SparseMatrix H = matrices.H(x);
// Ensure matrix callback dimensions are consistent
slp_assert(g.rows() == num_decision_variables);
@@ -170,8 +170,7 @@ ExitStatus newton(
// Call iteration callbacks
for (const auto& callback : iteration_callbacks) {
if (callback({iterations, x, g, H, Eigen::SparseMatrix<Scalar>{},
Eigen::SparseMatrix<Scalar>{}})) {
if (callback({iterations, x, g, H, SparseMatrix{}, SparseMatrix{}})) {
return ExitStatus::CALLBACK_REQUESTED_STOP;
}
}
@@ -187,7 +186,7 @@ ExitStatus newton(
kkt_matrix_decomp_profiler.stop();
ScopedProfiler kkt_system_solve_profiler{kkt_system_solve_prof};
Eigen::Vector<Scalar, Eigen::Dynamic> p_x = solver.solve(-g);
DenseVector p_x = solver.solve(-g);
kkt_system_solve_profiler.stop();
ScopedProfiler line_search_profiler{line_search_prof};
@@ -198,7 +197,7 @@ ExitStatus newton(
// Loop until a step is accepted. If a step becomes acceptable, the loop
// will exit early.
while (1) {
Eigen::Vector<Scalar, Eigen::Dynamic> trial_x = x + α * p_x;
DenseVector trial_x = x + α * p_x;
Scalar trial_f = matrices.f(trial_x);
@@ -227,7 +226,7 @@ ExitStatus newton(
if (α < α_min) {
Scalar current_kkt_error = kkt_error<Scalar>(g);
Eigen::Vector<Scalar, Eigen::Dynamic> trial_x = x + α_max * p_x;
DenseVector trial_x = x + α_max * p_x;
Scalar next_kkt_error = kkt_error<Scalar>(matrices.g(trial_x));

25
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton_matrix_callbacks.hpp vendored
View File

@@ -9,13 +9,18 @@
namespace slp {
/**
* Matrix callbacks for the Newton's method solver.
*
* @tparam Scalar Scalar type.
*/
/// Matrix callbacks for the Newton's method solver.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
struct NewtonMatrixCallbacks {
/// Type alias for dense vector.
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
/// Type alias for sparse matrix.
using SparseMatrix = Eigen::SparseMatrix<Scalar>;
/// Type alias for sparse vector.
using SparseVector = Eigen::SparseVector<Scalar>;
/// Cost function value f(x) getter.
///
/// <table>
@@ -35,7 +40,7 @@ struct NewtonMatrixCallbacks {
/// <td>1</td>
/// </tr>
/// </table>
std::function<Scalar(const Eigen::Vector<Scalar, Eigen::Dynamic>& x)> f;
std::function<Scalar(const DenseVector& x)> f;
/// Cost function gradient ∇f(x) getter.
///
@@ -56,9 +61,7 @@ struct NewtonMatrixCallbacks {
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::SparseVector<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
g;
std::function<SparseVector(const DenseVector& x)> g;
/// Lagrangian Hessian ∇ₓₓ²L(x) getter.
///
@@ -81,9 +84,7 @@ struct NewtonMatrixCallbacks {
/// <td>num_decision_variables</td>
/// </tr>
/// </table>
std::function<Eigen::SparseMatrix<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
H;
std::function<SparseMatrix(const DenseVector& x)> H;
};
} // namespace slp

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

@@ -9,9 +9,7 @@
namespace slp {
/**
* Solver options.
*/
/// Solver options.
struct SLEIPNIR_DLLEXPORT Options {
/// The solver will stop once the error is below this tolerance.
double tolerance = 1e-8;

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

@@ -32,42 +32,42 @@
namespace slp {
/**
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.
@tparam Scalar Scalar type.
@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.
*/
/// Finds the optimal solution to a nonlinear program using Sequential Quadratic
/// Programming (SQP).
///
/// A nonlinear program has the form:
///
/// ```
/// min_x f(x)
/// subject to cₑ(x) = 0
/// ```
///
/// where f(x) is the cost function and cₑ(x) are the equality constraints.
///
/// @tparam Scalar Scalar type.
/// @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.
template <typename Scalar>
ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
std::span<std::function<bool(const IterationInfo<Scalar>& info)>>
iteration_callbacks,
const Options& options,
Eigen::Vector<Scalar, Eigen::Dynamic>& x) {
/**
* SQP step direction.
*/
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
using SparseMatrix = Eigen::SparseMatrix<Scalar>;
using SparseVector = Eigen::SparseVector<Scalar>;
/// SQP step direction.
struct Step {
/// Primal step.
Eigen::Vector<Scalar, Eigen::Dynamic> p_x;
DenseVector p_x;
/// Dual step.
Eigen::Vector<Scalar, Eigen::Dynamic> p_y;
DenseVector p_y;
};
using std::isfinite;
@@ -113,28 +113,23 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
auto& A_e_prof = solve_profilers[15];
SQPMatrixCallbacks<Scalar> matrices{
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x) -> Scalar {
[&](const DenseVector& x) -> Scalar {
ScopedProfiler prof{f_prof};
return matrix_callbacks.f(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseVector<Scalar> {
[&](const DenseVector& x) -> SparseVector {
ScopedProfiler prof{g_prof};
return matrix_callbacks.g(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x,
const Eigen::Vector<Scalar, Eigen::Dynamic>& y)
-> Eigen::SparseMatrix<Scalar> {
[&](const DenseVector& x, const DenseVector& y) -> SparseMatrix {
ScopedProfiler prof{H_prof};
return matrix_callbacks.H(x, y);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::Vector<Scalar, Eigen::Dynamic> {
[&](const DenseVector& x) -> DenseVector {
ScopedProfiler prof{c_e_prof};
return matrix_callbacks.c_e(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
[&](const DenseVector& x) -> SparseMatrix {
ScopedProfiler prof{A_e_prof};
return matrix_callbacks.A_e(x);
}};
@@ -146,7 +141,7 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
setup_prof.start();
Scalar f = matrices.f(x);
Eigen::Vector<Scalar, Eigen::Dynamic> c_e = matrices.c_e(x);
DenseVector c_e = matrices.c_e(x);
int num_decision_variables = x.rows();
int num_equality_constraints = c_e.rows();
@@ -160,13 +155,12 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
return ExitStatus::TOO_FEW_DOFS;
}
Eigen::SparseVector<Scalar> g = matrices.g(x);
Eigen::SparseMatrix<Scalar> A_e = matrices.A_e(x);
SparseVector g = matrices.g(x);
SparseMatrix A_e = matrices.A_e(x);
Eigen::Vector<Scalar, Eigen::Dynamic> y =
Eigen::Vector<Scalar, Eigen::Dynamic>::Zero(num_equality_constraints);
DenseVector y = DenseVector::Zero(num_equality_constraints);
Eigen::SparseMatrix<Scalar> H = matrices.H(x, y);
SparseMatrix H = matrices.H(x, y);
// Ensure matrix callback dimensions are consistent
slp_assert(g.rows() == num_decision_variables);
@@ -235,7 +229,7 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
// Call iteration callbacks
for (const auto& callback : iteration_callbacks) {
if (callback({iterations, x, g, H, A_e, Eigen::SparseMatrix<Scalar>{}})) {
if (callback({iterations, x, g, H, A_e, SparseMatrix{}})) {
return ExitStatus::CALLBACK_REQUESTED_STOP;
}
}
@@ -251,24 +245,21 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
triplets.reserve(H.nonZeros() + A_e.nonZeros());
for (int col = 0; col < H.cols(); ++col) {
// Append column of H lower triangle in top-left quadrant
for (typename Eigen::SparseMatrix<Scalar>::InnerIterator it{H, col}; it;
++it) {
for (typename SparseMatrix::InnerIterator it{H, col}; it; ++it) {
triplets.emplace_back(it.row(), it.col(), it.value());
}
// Append column of Aₑ in bottom-left quadrant
for (typename Eigen::SparseMatrix<Scalar>::InnerIterator it{A_e, col}; it;
++it) {
for (typename SparseMatrix::InnerIterator it{A_e, col}; it; ++it) {
triplets.emplace_back(H.rows() + it.row(), it.col(), it.value());
}
}
Eigen::SparseMatrix<Scalar> lhs(
num_decision_variables + num_equality_constraints,
num_decision_variables + num_equality_constraints);
SparseMatrix lhs(num_decision_variables + num_equality_constraints,
num_decision_variables + num_equality_constraints);
lhs.setFromSortedTriplets(triplets.begin(), triplets.end());
// rhs = −[∇f − Aₑᵀy]
// [ cₑ ]
Eigen::Vector<Scalar, Eigen::Dynamic> rhs{x.rows() + y.rows()};
DenseVector rhs{x.rows() + y.rows()};
rhs.segment(0, x.rows()) = -g + A_e.transpose() * y;
rhs.segment(x.rows(), y.rows()) = -c_e;
@@ -293,7 +284,7 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
auto compute_step = [&](Step& step) {
// p = [ pˣ]
// [−pʸ]
Eigen::Vector<Scalar, Eigen::Dynamic> p = solver.solve(rhs);
DenseVector p = solver.solve(rhs);
step.p_x = p.segment(0, x.rows());
step.p_y = -p.segment(x.rows(), y.rows());
};
@@ -306,11 +297,11 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
// Loop until a step is accepted
while (1) {
Eigen::Vector<Scalar, Eigen::Dynamic> trial_x = x + α * step.p_x;
Eigen::Vector<Scalar, Eigen::Dynamic> trial_y = y + α * step.p_y;
DenseVector trial_x = x + α * step.p_x;
DenseVector trial_y = y + α * step.p_y;
Scalar trial_f = matrices.f(trial_x);
Eigen::Vector<Scalar, Eigen::Dynamic> trial_c_e = matrices.c_e(trial_x);
DenseVector trial_c_e = matrices.c_e(trial_x);
// If f(xₖ + αpₖˣ) or cₑ(xₖ + αpₖˣ) aren't finite, reduce step size
// immediately
@@ -343,7 +334,7 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
auto soc_step = step;
Scalar α_soc = α;
Eigen::Vector<Scalar, Eigen::Dynamic> c_e_soc = c_e;
DenseVector c_e_soc = c_e;
bool step_acceptable = false;
for (int soc_iteration = 0; soc_iteration < 5 && !step_acceptable;

38
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp_matrix_callbacks.hpp vendored
View File

@@ -9,13 +9,18 @@
namespace slp {
/**
* Matrix callbacks for the Sequential Quadratic Programming (SQP) solver.
*
* @tparam Scalar Scalar type.
*/
/// Matrix callbacks for the Sequential Quadratic Programming (SQP) solver.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
struct SQPMatrixCallbacks {
/// Type alias for dense vector.
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
/// Type alias for sparse matrix.
using SparseMatrix = Eigen::SparseMatrix<Scalar>;
/// Type alias for sparse vector.
using SparseVector = Eigen::SparseVector<Scalar>;
/// Cost function value f(x) getter.
///
/// <table>
@@ -35,7 +40,7 @@ struct SQPMatrixCallbacks {
/// <td>1</td>
/// </tr>
/// </table>
std::function<Scalar(const Eigen::Vector<Scalar, Eigen::Dynamic>& x)> f;
std::function<Scalar(const DenseVector& x)> f;
/// Cost function gradient ∇f(x) getter.
///
@@ -56,9 +61,7 @@ struct SQPMatrixCallbacks {
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::SparseVector<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
g;
std::function<SparseVector(const DenseVector& x)> g;
/// Lagrangian Hessian ∇ₓₓ²L(x, y) getter.
///
@@ -86,10 +89,7 @@ struct SQPMatrixCallbacks {
/// <td>num_decision_variables</td>
/// </tr>
/// </table>
std::function<Eigen::SparseMatrix<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x,
const Eigen::Vector<Scalar, Eigen::Dynamic>& y)>
H;
std::function<SparseMatrix(const DenseVector& x, const DenseVector& y)> H;
/// Equality constraint value cₑ(x) getter.
///
@@ -110,18 +110,16 @@ struct SQPMatrixCallbacks {
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::Vector<Scalar, Eigen::Dynamic>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
c_e;
std::function<DenseVector(const DenseVector& x)> c_e;
/// Equality constraint Jacobian ∂cₑ/∂x getter.
///
/// @verbatim
/// ```
/// [∇ᵀcₑ₁(xₖ)]
/// Aₑ(x) = [∇ᵀcₑ₂(xₖ)]
/// [ ⋮ ]
/// [∇ᵀcₑₘ(xₖ)]
/// @endverbatim
/// ```
///
/// <table>
/// <tr>
@@ -140,9 +138,7 @@ struct SQPMatrixCallbacks {
/// <td>num_decision_variables</td>
/// </tr>
/// </table>
std::function<Eigen::SparseMatrix<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
A_e;
std::function<SparseMatrix(const DenseVector& x)> A_e;
};
} // namespace slp

66
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/bounds.hpp vendored
View File

@@ -19,11 +19,9 @@
namespace slp {
/**
* Bound constraint metadata.
*
* @tparam Scalar Scalar type.
*/
/// Bound constraint metadata.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
struct Bounds {
/// Which constraints, if any, are bound constraints.
@@ -38,23 +36,21 @@ struct Bounds {
conflicting_bound_indices;
};
/**
* A "bound constraint" is any linear constraint in one scalar variable.
*
* Computes which constraints, if any, are bound constraints, the tightest
* bounds on each decision variable, and whether or not they're feasible (given
* previously encountered bounds),
*
* @tparam Scalar Scalar type.
* @param decision_variables Decision variables corresponding to each column of
* A_i.
* @param inequality_constraints Variables representing the left-hand side of
* cᵢ(decision_variables) ≥ 0.
* @param A_i The Jacobian of inequality_constraints wrt decision_variables,
* evaluated anywhere, in *row-major* storage; in practice, since we typically
* store Jacobians column-major, the user of this function must perform a
* transpose.
*/
/// A "bound constraint" is any linear constraint in one scalar variable.
///
/// Computes which constraints, if any, are bound constraints, the tightest
/// bounds on each decision variable, and whether or not they're feasible (given
/// previously encountered bounds),
///
/// @tparam Scalar Scalar type.
/// @param decision_variables Decision variables corresponding to each column of
/// A_i.
/// @param inequality_constraints Variables representing the left-hand side of
/// cᵢ(decision_variables) ≥ 0.
/// @param A_i The Jacobian of inequality_constraints wrt decision_variables,
/// evaluated anywhere, in *row-major* storage; in practice, since we
/// typically store Jacobians column-major, the user of this function must
/// perform a transpose.
template <typename Scalar>
Bounds<Scalar> get_bounds(
std::span<Variable<Scalar>> decision_variables,
@@ -182,20 +178,18 @@ Bounds<Scalar> get_bounds(
conflicting_bound_indices};
}
/**
* Projects the decision variables onto the given bounds, while ensuring some
* configurable distance from the boundary if possible. This is designed to
* match the algorithm given in section 3.6 of [2].
*
* @param x A vector of decision variables.
* @param decision_variable_indices_to_bounds An array of bounds (stored [lower,
* upper]) for each decision variable in x.
* @param κ_1 A constant controlling distance from the lower or upper bound when
* the difference between the upper and lower bound is small.
* @param κ_2 A constant controlling distance from the lower or upper bound when
* the difference between the upper and lower bound is large (including when
* one of the bounds is ±∞).
*/
/// Projects the decision variables onto the given bounds, while ensuring some
/// configurable distance from the boundary if possible. This is designed to
/// match the algorithm given in section 3.6 of [2].
///
/// @param x A vector of decision variables.
/// @param decision_variable_indices_to_bounds An array of bounds (stored
/// [lower, upper]) for each decision variable in x.
/// @param κ_1 A constant controlling distance from the lower or upper bound
/// when the difference between the upper and lower bound is small.
/// @param κ_2 A constant controlling distance from the lower or upper bound
/// when the difference between the upper and lower bound is large
/// (including when one of the bounds is ±∞).
template <typename Derived>
requires(static_cast<bool>(Eigen::DenseBase<Derived>::IsVectorAtCompileTime))
void project_onto_bounds(

66
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/error_estimate.hpp vendored
View File

@@ -11,12 +11,10 @@
namespace slp {
/**
* Returns the error estimate using the KKT conditions for Newton's method.
*
* @tparam Scalar Scalar type.
* @param g Gradient of the cost function ∇f.
*/
/// Returns the error estimate using the KKT conditions for Newton's method.
///
/// @tparam Scalar Scalar type.
/// @param g Gradient of the cost function ∇f.
template <typename Scalar>
Scalar error_estimate(const Eigen::Vector<Scalar, Eigen::Dynamic>& g) {
// Update the error estimate using the KKT conditions from equations (19.5a)
@@ -27,17 +25,15 @@ Scalar error_estimate(const Eigen::Vector<Scalar, Eigen::Dynamic>& g) {
return g.template lpNorm<Eigen::Infinity>();
}
/**
* Returns the error estimate using the KKT conditions for SQP.
*
* @tparam Scalar Scalar type.
* @param g Gradient of the cost function ∇f.
* @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
* current iterate.
* @param c_e The problem's equality constraints cₑ(x) evaluated at the current
* iterate.
* @param y Equality constraint dual variables.
*/
/// Returns the error estimate using the KKT conditions for SQP.
///
/// @tparam Scalar Scalar type.
/// @param g Gradient of the cost function ∇f.
/// @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
/// current iterate.
/// @param c_e The problem's equality constraints cₑ(x) evaluated at the current
/// iterate.
/// @param y Equality constraint dual variables.
template <typename Scalar>
Scalar error_estimate(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
const Eigen::SparseMatrix<Scalar>& A_e,
@@ -65,25 +61,23 @@ Scalar error_estimate(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
c_e.template lpNorm<Eigen::Infinity>()});
}
/**
* Returns the error estimate using the KKT conditions for the interior-point
* method.
*
* @tparam Scalar Scalar type.
* @param g Gradient of the cost function ∇f.
* @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
* current iterate.
* @param c_e The problem's equality constraints cₑ(x) evaluated at the current
* iterate.
* @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
* the current iterate.
* @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
* current iterate.
* @param s Inequality constraint slack variables.
* @param y Equality constraint dual variables.
* @param z Inequality constraint dual variables.
* @param μ Barrier parameter.
*/
/// Returns the error estimate using the KKT conditions for the interior-point
/// method.
///
/// @tparam Scalar Scalar type.
/// @param g Gradient of the cost function ∇f.
/// @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
/// current iterate.
/// @param c_e The problem's equality constraints cₑ(x) evaluated at the current
/// iterate.
/// @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
/// the current iterate.
/// @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
/// current iterate.
/// @param s Inequality constraint slack variables.
/// @param y Equality constraint dual variables.
/// @param z Inequality constraint dual variables.
/// @param μ Barrier parameter.
template <typename Scalar>
Scalar error_estimate(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
const Eigen::SparseMatrix<Scalar>& A_e,

142
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/filter.hpp vendored
View File

@@ -14,13 +14,14 @@
namespace slp {
/**
* Filter entry consisting of cost and constraint violation.
*
* @tparam Scalar Scalar type.
*/
/// Filter entry consisting of cost and constraint violation.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
struct FilterEntry {
/// Type alias for dense vector.
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
/// The cost function's value
Scalar cost{0};
@@ -29,73 +30,58 @@ struct FilterEntry {
constexpr FilterEntry() = default;
/**
* Constructs a FilterEntry.
*
* @param cost The cost function's value.
* @param constraint_violation The constraint violation.
*/
/// Constructs a FilterEntry.
///
/// @param cost The cost function's value.
/// @param constraint_violation The constraint violation.
constexpr FilterEntry(Scalar cost, Scalar constraint_violation)
: cost{cost}, constraint_violation{constraint_violation} {}
/**
* Constructs a Newton's method filter entry.
*
* @param f The cost function value.
*/
/// Constructs a Newton's method filter entry.
///
/// @param f The cost function value.
explicit FilterEntry(Scalar f) : FilterEntry{f, Scalar(0)} {}
/**
* Constructs a Sequential Quadratic Programming filter entry.
*
* @param f The cost function value.
* @param c_e The equality constraint values (nonzero means violation).
*/
FilterEntry(Scalar f, const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e)
/// Constructs a Sequential Quadratic Programming filter entry.
///
/// @param f The cost function value.
/// @param c_e The equality constraint values (nonzero means violation).
FilterEntry(Scalar f, const DenseVector& c_e)
: FilterEntry{f, c_e.template lpNorm<1>()} {}
/**
* Constructs an interior-point method filter entry.
*
* @param f The cost function value.
* @param s The inequality constraint slack variables.
* @param c_e The equality constraint values (nonzero means violation).
* @param c_i The inequality constraint values (negative means violation).
* @param μ The barrier parameter.
*/
FilterEntry(Scalar f, Eigen::Vector<Scalar, Eigen::Dynamic>& s,
const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e,
const Eigen::Vector<Scalar, Eigen::Dynamic>& c_i, Scalar μ)
/// Constructs an interior-point method filter entry.
///
/// @param f The cost function value.
/// @param s The inequality constraint slack variables.
/// @param c_e The equality constraint values (nonzero means violation).
/// @param c_i The inequality constraint values (negative means violation).
/// @param μ The barrier parameter.
FilterEntry(Scalar f, DenseVector& s, const DenseVector& c_e,
const DenseVector& c_i, Scalar μ)
: FilterEntry{f - μ * s.array().log().sum(),
c_e.template lpNorm<1>() + (c_i - s).template lpNorm<1>()} {
}
};
/**
* Step filter.
*
* See the section on filters in chapter 15 of [1].
*
* @tparam Scalar Scalar type.
*/
/// Step filter.
///
/// See the section on filters in chapter 15 of [1].
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
class Filter {
public:
/// The maximum constraint violation
Scalar max_constraint_violation{1e4};
/**
* Constructs an empty filter.
*/
/// Constructs an empty filter.
Filter() {
// Initial filter entry rejects constraint violations above max
m_filter.emplace_back(std::numeric_limits<Scalar>::infinity(),
max_constraint_violation);
}
/**
* Resets the filter.
*/
/// Resets the filter.
void reset() {
m_filter.clear();
@@ -104,11 +90,9 @@ class Filter {
max_constraint_violation);
}
/**
* Adds a new entry to the filter.
*
* @param entry The entry to add to the filter.
*/
/// Adds a new entry to the filter.
///
/// @param entry The entry to add to the filter.
void add(const FilterEntry<Scalar>& entry) {
// Remove dominated entries
erase_if(m_filter, [&](const auto& elem) {
@@ -119,11 +103,9 @@ class Filter {
m_filter.push_back(entry);
}
/**
* Adds a new entry to the filter.
*
* @param entry The entry to add to the filter.
*/
/// Adds a new entry to the filter.
///
/// @param entry The entry to add to the filter.
void add(FilterEntry<Scalar>&& entry) {
// Remove dominated entries
erase_if(m_filter, [&](const auto& elem) {
@@ -134,13 +116,11 @@ class Filter {
m_filter.push_back(entry);
}
/**
* Returns true if the given iterate is accepted by the filter.
*
* @param entry The entry to attempt adding to the filter.
* @param α The step size (0, 1].
* @return True if the given iterate is accepted by the filter.
*/
/// Returns true if the given iterate is accepted by the filter.
///
/// @param entry The entry to attempt adding to the filter.
/// @param α The step size (0, 1].
/// @return True if the given iterate is accepted by the filter.
bool try_add(const FilterEntry<Scalar>& entry, Scalar α) {
if (is_acceptable(entry, α)) {
add(entry);
@@ -150,13 +130,11 @@ class Filter {
}
}
/**
* Returns true if the given iterate is accepted by the filter.
*
* @param entry The entry to attempt adding to the filter.
* @param α The step size (0, 1].
* @return True if the given iterate is accepted by the filter.
*/
/// Returns true if the given iterate is accepted by the filter.
///
/// @param entry The entry to attempt adding to the filter.
/// @param α The step size (0, 1].
/// @return True if the given iterate is accepted by the filter.
bool try_add(FilterEntry<Scalar>&& entry, Scalar α) {
if (is_acceptable(entry, α)) {
add(std::move(entry));
@@ -166,13 +144,11 @@ class Filter {
}
}
/**
* Returns true if the given entry is acceptable to the filter.
*
* @param entry The entry to check.
* @param α The step size (0, 1].
* @return True if the given entry is acceptable to the filter.
*/
/// Returns true if the given entry is acceptable to the filter.
///
/// @param entry The entry to check.
/// @param α The step size (0, 1].
/// @return True if the given entry is acceptable to the filter.
bool is_acceptable(const FilterEntry<Scalar>& entry, Scalar α) {
using std::isfinite;
using std::pow;
@@ -194,11 +170,9 @@ class Filter {
});
}
/**
* Returns the most recently added filter entry.
*
* @return The most recently added filter entry.
*/
/// Returns the most recently added filter entry.
///
/// @return The most recently added filter entry.
const FilterEntry<Scalar>& back() const { return m_filter.back(); }
private:

23
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/fraction_to_the_boundary_rule.hpp vendored
View File

@@ -8,21 +8,18 @@
namespace slp {
/**
* Applies fraction-to-the-boundary rule to a variable and its iterate, then
* returns a fraction of the iterate step size within (0, 1].
*
* @tparam Scalar Scalar type.
* @param x The variable.
* @param p The iterate on the variable.
* @param τ Fraction-to-the-boundary rule scaling factor within (0, 1].
* @return Fraction of the iterate step size within (0, 1].
*/
/// Applies fraction-to-the-boundary rule to a variable and its iterate, then
/// returns a fraction of the iterate step size within (0, 1].
///
/// @tparam Scalar Scalar type.
/// @param x The variable.
/// @param p The iterate on the variable.
/// @param τ Fraction-to-the-boundary rule scaling factor within (0, 1].
/// @return Fraction of the iterate step size within (0, 1].
template <typename Scalar>
Scalar fraction_to_the_boundary_rule(
const Eigen::Ref<const Eigen::Vector<Scalar, Eigen::Dynamic>>& x,
const Eigen::Ref<const Eigen::Vector<Scalar, Eigen::Dynamic>>& p,
Scalar τ) {
const Eigen::Vector<Scalar, Eigen::Dynamic>& x,
const Eigen::Vector<Scalar, Eigen::Dynamic>& p, Scalar τ) {
// α = max(α ∈ (0, 1] : x + αp ≥ (1 − τ)x)
//
// where x and τ are positive.

52
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/inertia.hpp vendored
View File

@@ -6,10 +6,8 @@
namespace slp {
/**
* Represents the inertia of a matrix (the number of positive, negative, and
* zero eigenvalues).
*/
/// Represents the inertia of a matrix (the number of positive, negative, and
/// zero eigenvalues).
class Inertia {
public:
/// The number of positive eigenvalues.
@@ -21,24 +19,20 @@ class Inertia {
constexpr Inertia() = default;
/**
* Constructs Inertia with the given number of positive, negative, and zero
* eigenvalues.
*
* @param positive The number of positive eigenvalues.
* @param negative The number of negative eigenvalues.
* @param zero The number of zero eigenvalues.
*/
/// Constructs Inertia with the given number of positive, negative, and zero
/// eigenvalues.
///
/// @param positive The number of positive eigenvalues.
/// @param negative The number of negative eigenvalues.
/// @param zero The number of zero eigenvalues.
constexpr Inertia(int positive, int negative, int zero)
: positive{positive}, negative{negative}, zero{zero} {}
/**
* Constructs Inertia from the D matrix of an LDLT decomposition
* (see https://en.wikipedia.org/wiki/Sylvester's_law_of_inertia).
*
* @tparam Scalar Scalar type.
* @param D The D matrix of an LDLT decomposition in vector form.
*/
/// Constructs Inertia from the D matrix of an LDLT decomposition
/// (see https://en.wikipedia.org/wiki/Sylvester's_law_of_inertia).
///
/// @tparam Scalar Scalar type.
/// @param D The D matrix of an LDLT decomposition in vector form.
template <typename Scalar>
explicit Inertia(const Eigen::Vector<Scalar, Eigen::Dynamic>& D) {
for (const auto& elem : D) {
@@ -52,13 +46,11 @@ class Inertia {
}
}
/**
* Constructs Inertia from the D matrix of an LDLT decomposition
* (see https://en.wikipedia.org/wiki/Sylvester's_law_of_inertia).
*
* @tparam Scalar Scalar type.
* @param D The D matrix of an LDLT decomposition in vector form.
*/
/// Constructs Inertia from the D matrix of an LDLT decomposition
/// (see https://en.wikipedia.org/wiki/Sylvester's_law_of_inertia).
///
/// @tparam Scalar Scalar type.
/// @param D The D matrix of an LDLT decomposition in vector form.
template <typename Scalar>
explicit Inertia(
const Eigen::Diagonal<
@@ -74,11 +66,9 @@ class Inertia {
}
}
/**
* Inertia equality operator.
*
* @return True if Inertia is equal.
*/
/// Inertia equality operator.
///
/// @return True if Inertia is equal.
bool operator==(const Inertia&) const = default;
};

32
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/is_locally_infeasible.hpp vendored
View File

@@ -9,15 +9,13 @@
namespace slp {
/**
* Returns true if the problem's equality constraints are locally infeasible.
*
* @tparam Scalar Scalar type.
* @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
* current iterate.
* @param c_e The problem's equality constraints cₑ(x) evaluated at the current
* iterate.
*/
/// Returns true if the problem's equality constraints are locally infeasible.
///
/// @tparam Scalar Scalar type.
/// @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
/// current iterate.
/// @param c_e The problem's equality constraints cₑ(x) evaluated at the current
/// iterate.
template <typename Scalar>
bool is_equality_locally_infeasible(
const Eigen::SparseMatrix<Scalar>& A_e,
@@ -32,15 +30,13 @@ bool is_equality_locally_infeasible(
c_e.norm() > Scalar(1e-2);
}
/**
* Returns true if the problem's inequality constraints are locally infeasible.
*
* @tparam Scalar Scalar type.
* @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
* the current iterate.
* @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
* current iterate.
*/
/// Returns true if the problem's inequality constraints are locally infeasible.
///
/// @tparam Scalar Scalar type.
/// @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
/// the current iterate.
/// @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
/// current iterate.
template <typename Scalar>
bool is_inequality_locally_infeasible(
const Eigen::SparseMatrix<Scalar>& A_i,

64
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/kkt_error.hpp vendored
View File

@@ -9,12 +9,10 @@
namespace slp {
/**
* Returns the KKT error for Newton's method.
*
* @tparam Scalar Scalar type.
* @param g Gradient of the cost function ∇f.
*/
/// Returns the KKT error for Newton's method.
///
/// @tparam Scalar Scalar type.
/// @param g Gradient of the cost function ∇f.
template <typename Scalar>
Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g) {
// Compute the KKT error as the 1-norm of the KKT conditions from equations
@@ -25,17 +23,15 @@ Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g) {
return g.template lpNorm<1>();
}
/**
* Returns the KKT error for Sequential Quadratic Programming.
*
* @tparam Scalar Scalar type.
* @param g Gradient of the cost function ∇f.
* @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
* current iterate.
* @param c_e The problem's equality constraints cₑ(x) evaluated at the current
* iterate.
* @param y Equality constraint dual variables.
*/
/// Returns the KKT error for Sequential Quadratic Programming.
///
/// @tparam Scalar Scalar type.
/// @param g Gradient of the cost function ∇f.
/// @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
/// current iterate.
/// @param c_e The problem's equality constraints cₑ(x) evaluated at the current
/// iterate.
/// @param y Equality constraint dual variables.
template <typename Scalar>
Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
const Eigen::SparseMatrix<Scalar>& A_e,
@@ -51,24 +47,22 @@ Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
c_e.template lpNorm<1>();
}
/**
* Returns the KKT error for the interior-point method.
*
* @tparam Scalar Scalar type.
* @param g Gradient of the cost function ∇f.
* @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
* current iterate.
* @param c_e The problem's equality constraints cₑ(x) evaluated at the current
* iterate.
* @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
* the current iterate.
* @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
* current iterate.
* @param s Inequality constraint slack variables.
* @param y Equality constraint dual variables.
* @param z Inequality constraint dual variables.
* @param μ Barrier parameter.
*/
/// Returns the KKT error for the interior-point method.
///
/// @tparam Scalar Scalar type.
/// @param g Gradient of the cost function ∇f.
/// @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
/// current iterate.
/// @param c_e The problem's equality constraints cₑ(x) evaluated at the current
/// iterate.
/// @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
/// the current iterate.
/// @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
/// current iterate.
/// @param s Inequality constraint slack variables.
/// @param y Equality constraint dual variables.
/// @param z Inequality constraint dual variables.
/// @param μ Barrier parameter.
template <typename Scalar>
Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
const Eigen::SparseMatrix<Scalar>& A_e,

119
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/regularized_ldlt.hpp vendored
View File

@@ -13,40 +13,39 @@
namespace slp {
/**
* Solves systems of linear equations using a regularized LDLT factorization.
*
* @tparam Scalar Scalar type.
*/
/// Solves systems of linear equations using a regularized LDLT factorization.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
class RegularizedLDLT {
public:
/**
* Constructs a RegularizedLDLT instance.
*
* @param num_decision_variables The number of decision variables in the
* system.
* @param num_equality_constraints The number of equality constraints in the
* system.
*/
/// Type alias for dense matrix.
using DenseMatrix = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
/// Type alias for dense vector.
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
/// Type alias for sparse matrix.
using SparseMatrix = Eigen::SparseMatrix<Scalar>;
/// Constructs a RegularizedLDLT instance.
///
/// @param num_decision_variables The number of decision variables in the
/// system.
/// @param num_equality_constraints The number of equality constraints in the
/// system.
RegularizedLDLT(int num_decision_variables, int num_equality_constraints)
: m_num_decision_variables{num_decision_variables},
m_num_equality_constraints{num_equality_constraints} {}
/**
* Reports whether previous computation was successful.
*
* @return Whether previous computation was successful.
*/
/// Reports whether previous computation was successful.
///
/// @return Whether previous computation was successful.
Eigen::ComputationInfo info() const { return m_info; }
/**
* Computes the regularized LDLT factorization of a matrix.
*
* @param lhs Left-hand side of the system.
* @return The factorization.
*/
RegularizedLDLT& compute(const Eigen::SparseMatrix<Scalar>& lhs) {
/// Computes the regularized LDLT factorization of a matrix.
///
/// @param lhs Left-hand side of the system.
/// @return The factorization.
RegularizedLDLT& compute(const SparseMatrix& lhs) {
// The regularization procedure is based on algorithm B.1 of [1]
// Max density is 50% due to the caller only providing the lower triangle.
@@ -128,15 +127,12 @@ class RegularizedLDLT {
}
}
/**
* Solves the system of equations using a regularized LDLT factorization.
*
* @param rhs Right-hand side of the system.
* @return The solution.
*/
/// Solves the system of equations using a regularized LDLT factorization.
///
/// @param rhs Right-hand side of the system.
/// @return The solution.
template <typename Rhs>
Eigen::Vector<Scalar, Eigen::Dynamic> solve(
const Eigen::MatrixBase<Rhs>& rhs) {
DenseVector solve(const Eigen::MatrixBase<Rhs>& rhs) {
if (m_is_sparse) {
return m_sparse_solver.solve(rhs);
} else {
@@ -144,15 +140,12 @@ class RegularizedLDLT {
}
}
/**
* Solves the system of equations using a regularized LDLT factorization.
*
* @param rhs Right-hand side of the system.
* @return The solution.
*/
/// Solves the system of equations using a regularized LDLT factorization.
///
/// @param rhs Right-hand side of the system.
/// @return The solution.
template <typename Rhs>
Eigen::Vector<Scalar, Eigen::Dynamic> solve(
const Eigen::SparseMatrixBase<Rhs>& rhs) {
DenseVector solve(const Eigen::SparseMatrixBase<Rhs>& rhs) {
if (m_is_sparse) {
return m_sparse_solver.solve(rhs);
} else {
@@ -160,17 +153,14 @@ class RegularizedLDLT {
}
}
/**
* Returns the Hessian regularization factor.
*
* @return Hessian regularization factor.
*/
/// Returns the Hessian regularization factor.
///
/// @return Hessian regularization factor.
Scalar hessian_regularization() const { return m_prev_δ; }
private:
using SparseSolver = Eigen::SimplicialLDLT<Eigen::SparseMatrix<Scalar>>;
using DenseSolver =
Eigen::LDLT<Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>>;
using SparseSolver = Eigen::SimplicialLDLT<SparseMatrix>;
using DenseSolver = Eigen::LDLT<DenseMatrix>;
SparseSolver m_sparse_solver;
DenseSolver m_dense_solver;
@@ -194,13 +184,11 @@ class RegularizedLDLT {
// Number of non-zeros in LHS.
int m_non_zeros = -1;
/**
* Computes factorization of a sparse matrix.
*
* @param lhs Matrix to factorize.
* @return The factorization.
*/
SparseSolver& compute_sparse(const Eigen::SparseMatrix<Scalar>& lhs) {
/// Computes factorization of a sparse matrix.
///
/// @param lhs Matrix to factorize.
/// @return The factorization.
SparseSolver& compute_sparse(const SparseMatrix& lhs) {
// Reanalize lhs's sparsity pattern if it changed
int non_zeros = lhs.nonZeros();
if (m_non_zeros != non_zeros) {
@@ -213,21 +201,18 @@ class RegularizedLDLT {
return m_sparse_solver;
}
/**
* Returns regularization matrix.
*
* @param δ The Hessian regularization factor.
* @param γ The equality constraint Jacobian regularization factor.
* @return Regularization matrix.
*/
Eigen::SparseMatrix<Scalar> regularization(Scalar δ, Scalar γ) {
Eigen::Vector<Scalar, Eigen::Dynamic> vec{m_num_decision_variables +
m_num_equality_constraints};
/// Returns regularization matrix.
///
/// @param δ The Hessian regularization factor.
/// @param γ The equality constraint Jacobian regularization factor.
/// @return Regularization matrix.
SparseMatrix regularization(Scalar δ, Scalar γ) {
DenseVector vec{m_num_decision_variables + m_num_equality_constraints};
vec.segment(0, m_num_decision_variables).setConstant(δ);
vec.segment(m_num_decision_variables, m_num_equality_constraints)
.setConstant(-γ);
return Eigen::SparseMatrix<Scalar>{vec.asDiagonal()};
return SparseMatrix{vec.asDiagonal()};
}
};

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

@@ -3,15 +3,12 @@
#pragma once
#ifdef SLEIPNIR_PYTHON
#include <source_location>
#include <stdexcept>
#include <fmt/format.h>
/**
* Throw an exception in Python.
*/
/// Throw an exception in Python.
#define slp_assert(condition) \
do { \
if (!(condition)) { \
@@ -21,14 +18,9 @@
location.line(), location.function_name(), #condition)); \
} \
} while (0)
#else
#include <cassert>
/**
* Abort in C++.
*/
/// Abort in C++.
#define slp_assert(condition) assert(condition)
#endif

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

@@ -4,14 +4,10 @@
namespace slp::detail {
/**
* Type tag used to designate an uninitialized VariableMatrix.
*/
/// Type tag used to designate an uninitialized VariableMatrix.
struct empty_t {};
/**
* Designates an uninitialized VariableMatrix.
*/
/// Designates an uninitialized VariableMatrix.
static constexpr empty_t empty{};
} // namespace slp::detail

66
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/function_ref.hpp vendored
View File

@@ -12,29 +12,23 @@ namespace slp {
template <class F>
class function_ref;
/**
* An implementation of std::function_ref, a lightweight non-owning reference to
* a callable.
*/
/// An implementation of std::function_ref, a lightweight non-owning reference
/// to a callable.
template <class R, class... Args>
class function_ref<R(Args...)> {
public:
constexpr function_ref() noexcept = delete;
/**
* Creates a `function_ref` which refers to the same callable as `rhs`.
*
* @param rhs Other `function_ref`.
*/
/// 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.
*/
/// 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> &&
std::is_invocable_r_v<R, F &&, Args...>)
@@ -49,21 +43,17 @@ class function_ref<R(Args...)> {
};
}
/**
* Makes `*this` refer to the same callable as `rhs`.
*
* @param rhs Other `function_ref`.
* @return `*this`
*/
/// 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`
*/
/// 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...>
constexpr function_ref<R(Args...)>& operator=(F&& f) noexcept {
@@ -77,22 +67,18 @@ class function_ref<R(Args...)> {
return *this;
}
/**
* Swaps the referred callables of `*this` and `rhs`.
*
* @param rhs Other `function_ref`.
*/
/// 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_);
std::swap(callback_, rhs.callback_);
}
/**
* Call the stored callable with the given arguments.
*
* @param args The arguments.
* @return The return value of the callable.
*/
/// 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)...);
}
@@ -102,9 +88,7 @@ class function_ref<R(Args...)> {
R (*callback_)(void*, Args...) = nullptr;
};
/**
* Swaps the referred callables of `lhs` and `rhs`.
*/
/// Swaps the referred callables of `lhs` and `rhs`.
template <typename R, typename... Args>
constexpr void swap(function_ref<R(Args...)>& lhs,
function_ref<R(Args...)>& rhs) noexcept {

246
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/intrusive_shared_ptr.hpp vendored
View File

@@ -9,45 +9,37 @@
namespace slp {
/**
* A custom intrusive shared pointer implementation without thread
* synchronization overhead.
*
* Types used with this class should have three things:
*
* 1. A zero-initialized public counter variable that serves as the shared
* pointer's reference count.
* 2. A free function `void 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.
*/
/// A custom intrusive shared pointer implementation without thread
/// synchronization overhead.
///
/// Types used with this class should have three things:
///
/// 1. A zero-initialized public counter variable that serves as the shared
/// pointer's reference count.
/// 2. A free function `void 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.
*/
/// Constructs an empty intrusive shared pointer.
constexpr IntrusiveSharedPtr() noexcept = default;
/**
* Constructs an empty intrusive shared pointer.
*/
/// Constructs an empty intrusive shared pointer.
// NOLINTNEXTLINE (google-explicit-constructor)
constexpr IntrusiveSharedPtr(std::nullptr_t) noexcept {}
/**
* Constructs an intrusive shared pointer from the given pointer and takes
* ownership.
*
* @param ptr The pointer of which to take ownership.
*/
/// 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) {
inc_ref_count(m_ptr);
@@ -60,11 +52,9 @@ class IntrusiveSharedPtr {
}
}
/**
* Copy constructs from the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
*/
/// 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) {
@@ -72,11 +62,9 @@ class IntrusiveSharedPtr {
}
}
/**
* Copy constructs from the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
*/
/// 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*>)
// NOLINTNEXTLINE (google-explicit-constructor)
@@ -87,12 +75,10 @@ class IntrusiveSharedPtr {
}
}
/**
* Makes a copy of the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
* @return This intrusive shared pointer.
*/
/// Makes a copy of the given intrusive shared pointer.
///
/// @param rhs The other intrusive shared pointer.
/// @return This intrusive shared pointer.
// NOLINTNEXTLINE (google-explicit-constructor)
constexpr IntrusiveSharedPtr<T>& operator=(
const IntrusiveSharedPtr<T>& rhs) noexcept {
@@ -113,12 +99,10 @@ class IntrusiveSharedPtr {
return *this;
}
/**
* Makes a copy of the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
* @return This intrusive shared pointer.
*/
/// 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*>)
// NOLINTNEXTLINE (google-explicit-constructor)
@@ -141,31 +125,25 @@ class IntrusiveSharedPtr {
return *this;
}
/**
* Move constructs from the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
*/
/// 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.
*/
/// 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*>)
// NOLINTNEXTLINE (google-explicit-constructor)
constexpr IntrusiveSharedPtr(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.
*/
/// 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 {
if (m_ptr == rhs.m_ptr) {
@@ -177,12 +155,10 @@ class IntrusiveSharedPtr {
return *this;
}
/**
* Move assigns from the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
* @return This intrusive shared pointer.
*/
/// Move assigns from 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=(
@@ -196,95 +172,75 @@ class IntrusiveSharedPtr {
return *this;
}
/**
* Returns the internal pointer.
*
* @return The internal pointer.
*/
/// 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.
*/
/// 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.
*/
/// 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.
*/
/// 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.
*/
/// 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 {
return lhs.m_ptr == rhs.m_ptr;
}
/**
* Returns true if the given intrusive shared pointers point to different
* objects.
*
* @param lhs The left-hand side.
* @param rhs The right-hand side.
*/
/// 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 {
return lhs.m_ptr != rhs.m_ptr;
}
/**
* Returns true if the left-hand intrusive shared pointer points to nullptr.
*
* @param lhs The left-hand side.
*/
/// 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 {
return lhs.m_ptr == nullptr;
}
/**
* Returns true if the right-hand intrusive shared pointer points to nullptr.
*
* @param rhs The right-hand side.
*/
/// 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 {
return nullptr == rhs.m_ptr;
}
/**
* Returns true if the left-hand intrusive shared pointer doesn't point to
* nullptr.
*
* @param lhs The left-hand side.
*/
/// 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 {
return lhs.m_ptr != nullptr;
}
/**
* Returns true if the right-hand intrusive shared pointer doesn't point to
* nullptr.
*
* @param rhs The right-hand side.
*/
/// 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 {
return nullptr != rhs.m_ptr;
@@ -294,30 +250,26 @@ class IntrusiveSharedPtr {
T* m_ptr = nullptr;
};
/**
* Constructs an object of type T and wraps it in an intrusive shared pointer
* using args as the parameter list for the constructor of T.
*
* @tparam T Type of object for intrusive shared pointer.
* @tparam Args Types of constructor arguments.
* @param args Constructor arguments for T.
*/
/// Constructs an object of type T and wraps it in an intrusive shared pointer
/// using args as the parameter list for the constructor of T.
///
/// @tparam T Type of object for intrusive shared pointer.
/// @tparam Args Types of constructor arguments.
/// @param args Constructor arguments for T.
template <typename T, typename... Args>
IntrusiveSharedPtr<T> make_intrusive_shared(Args&&... args) {
return IntrusiveSharedPtr<T>{new T(std::forward<Args>(args)...)};
}
/**
* Constructs an object of type T and wraps it in an intrusive shared pointer
* using alloc as the storage allocator of T and args as the parameter list for
* the constructor of T.
*
* @tparam T Type of object for intrusive shared pointer.
* @tparam Alloc Type of allocator for T.
* @tparam Args Types of constructor arguments.
* @param alloc The allocator for T.
* @param args Constructor arguments for T.
*/
/// Constructs an object of type T and wraps it in an intrusive shared pointer
/// using alloc as the storage allocator of T and args as the parameter list for
/// the constructor of T.
///
/// @tparam T Type of object for intrusive shared pointer.
/// @tparam Alloc Type of allocator for T.
/// @tparam Args Types of constructor arguments.
/// @param alloc The allocator for T.
/// @param args Constructor arguments for T.
template <typename T, typename Alloc, typename... Args>
IntrusiveSharedPtr<T> allocate_intrusive_shared(Alloc alloc, Args&&... args) {
auto ptr = std::allocator_traits<Alloc>::allocate(alloc, sizeof(T));

162
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/pool.hpp vendored
View File

@@ -11,54 +11,40 @@
namespace slp {
/**
* This class implements a pool memory resource.
*
* The pool allocates chunks of memory and splits them into blocks managed by a
* free list. Allocations return pointers from the free list, and deallocations
* return pointers to the free list.
*/
/// This class implements a pool memory resource.
///
/// The pool allocates chunks of memory and splits them into blocks managed by a
/// free list. Allocations return pointers from the free list, and deallocations
/// return pointers to the free list.
class SLEIPNIR_DLLEXPORT PoolResource {
public:
/**
* Constructs a default PoolResource.
*
* @param blocks_per_chunk Number of blocks per chunk of memory.
*/
/// Constructs a default PoolResource.
///
/// @param blocks_per_chunk Number of blocks per chunk of memory.
explicit PoolResource(size_t blocks_per_chunk)
: blocks_per_chunk{blocks_per_chunk} {}
/**
* Copy constructor.
*/
/// Copy constructor.
PoolResource(const PoolResource&) = delete;
/**
* Copy assignment operator.
*
* @return This pool resource.
*/
/// Copy assignment operator.
///
/// @return This pool resource.
PoolResource& operator=(const PoolResource&) = delete;
/**
* Move constructor.
*/
/// Move constructor.
PoolResource(PoolResource&&) = default;
/**
* Move assignment operator.
*
* @return This pool resource.
*/
/// Move assignment operator.
///
/// @return This pool resource.
PoolResource& operator=(PoolResource&&) = default;
/**
* Returns a block of memory from the pool.
*
* @param bytes Number of bytes in the block.
* @param alignment Alignment of the block (unused).
* @return A block of memory from the pool.
*/
/// Returns a block of memory from the pool.
///
/// @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)) {
@@ -71,34 +57,30 @@ class SLEIPNIR_DLLEXPORT PoolResource {
return ptr;
}
/**
* Gives a block of memory back to the pool.
*
* @param p A pointer to the block of memory.
* @param bytes Number of bytes in the block (unused).
* @param alignment Alignment of the block (unused).
*/
/// Gives a block of memory back to the pool.
///
/// @param p A pointer to the block of memory.
/// @param bytes Number of bytes in the block (unused).
/// @param alignment Alignment of the block (unused).
void deallocate(
void* p, [[maybe_unused]] size_t bytes,
[[maybe_unused]] size_t alignment = alignof(std::max_align_t)) {
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.
*/
/// 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;
}
/**
* 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.
*/
/// 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() * blocks_per_chunk - m_free_list.size();
}
@@ -108,12 +90,10 @@ class SLEIPNIR_DLLEXPORT PoolResource {
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 bytes_per_block Number of bytes in the block.
*/
/// 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 bytes_per_block Number of bytes in the block.
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) {
@@ -122,55 +102,41 @@ class SLEIPNIR_DLLEXPORT PoolResource {
}
};
/**
* This class is an allocator for the pool resource.
*
* @tparam T The type of object in the pool.
*/
/// This class is an allocator for the pool resource.
///
/// @tparam T The type of object in the pool.
template <typename T>
class PoolAllocator {
public:
/**
* The type of object in the pool.
*/
/// The type of object in the pool.
using value_type = T;
/**
* Constructs a pool allocator with the given pool memory resource.
*
* @param r The pool resource.
*/
/// Constructs a pool allocator with the given pool memory resource.
///
/// @param r The pool resource.
explicit constexpr PoolAllocator(PoolResource* r) : m_memory_resource{r} {}
/**
* Copy constructor.
*/
/// Copy constructor.
constexpr PoolAllocator(const PoolAllocator<T>&) = default;
/**
* Copy assignment operator.
*
* @return This pool allocator.
*/
/// 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.
*/
/// 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_memory_resource->allocate(n));
}
/**
* Gives a block of memory back to the pool.
*
* @param p A pointer to the block of memory.
* @param n Number of bytes in the block.
*/
/// Gives a block of memory back to the pool.
///
/// @param p A pointer to the block of memory.
/// @param n Number of bytes in the block.
constexpr void deallocate(T* p, size_t n) {
m_memory_resource->deallocate(p, n);
}
@@ -179,16 +145,12 @@ class PoolAllocator {
PoolResource* m_memory_resource;
};
/**
* Returns a global pool memory resource.
*/
/// Returns a global pool memory resource.
SLEIPNIR_DLLEXPORT PoolResource& global_pool_resource();
/**
* Returns an allocator for a global pool memory resource.
*
* @tparam T The type of object in the pool.
*/
/// Returns an allocator for a global pool memory resource.
///
/// @tparam T The type of object in the pool.
template <typename T>
PoolAllocator<T> global_pool_allocator() {
return PoolAllocator<T>{&global_pool_resource()};

21
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print.hpp vendored
View File

@@ -7,21 +7,14 @@
#include <system_error>
#include <utility>
#if __has_include(<fmt/base.h>)
#include <fmt/base.h>
#else
#include <fmt/core.h>
#endif
#endif
namespace slp {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* Wrapper around fmt::print() that squelches write failure exceptions.
*/
/// Wrapper around fmt::print() that squelches write failure exceptions.
template <typename... T>
void print(fmt::format_string<T...> fmt, T&&... args) {
try {
@@ -30,9 +23,7 @@ void print(fmt::format_string<T...> fmt, T&&... args) {
}
}
/**
* Wrapper around fmt::print() that squelches write failure exceptions.
*/
/// Wrapper around fmt::print() that squelches write failure exceptions.
template <typename... T>
void print(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
try {
@@ -41,9 +32,7 @@ void print(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
}
}
/**
* Wrapper around fmt::println() that squelches write failure exceptions.
*/
/// Wrapper around fmt::println() that squelches write failure exceptions.
template <typename... T>
void println(fmt::format_string<T...> fmt, T&&... args) {
try {
@@ -52,9 +41,7 @@ void println(fmt::format_string<T...> fmt, T&&... args) {
}
}
/**
* Wrapper around fmt::println() that squelches write failure exceptions.
*/
/// Wrapper around fmt::println() that squelches write failure exceptions.
template <typename... T>
void println(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
try {

122
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print_diagnostics.hpp vendored
View File

@@ -21,9 +21,7 @@
namespace slp {
/**
* Iteration type.
*/
/// Iteration type.
enum class IterationType : uint8_t {
/// Normal iteration.
NORMAL,
@@ -33,10 +31,8 @@ enum class IterationType : uint8_t {
REJECTED_SOC
};
/**
* Converts std::chrono::duration to a number of milliseconds rounded to three
* decimals.
*/
/// Converts std::chrono::duration to a number of milliseconds rounded to three
/// decimals.
template <typename Rep, typename Period = std::ratio<1>>
constexpr double to_ms(const std::chrono::duration<Rep, Period>& duration) {
using std::chrono::duration_cast;
@@ -44,12 +40,10 @@ constexpr double to_ms(const std::chrono::duration<Rep, Period>& duration) {
return duration_cast<microseconds>(duration).count() / 1e3;
}
/**
* Renders value as power of 10.
*
* @tparam Scalar Scalar type.
* @param value Value.
*/
/// Renders value as power of 10.
///
/// @tparam Scalar Scalar type.
/// @param value Value.
template <typename Scalar>
std::string power_of_10(Scalar value) {
if (value == Scalar(0)) {
@@ -89,13 +83,11 @@ std::string power_of_10(Scalar value) {
}
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* Prints error for too few degrees of freedom.
*
* @tparam Scalar Scalar type.
* @param c_e The problem's equality constraints cₑ(x) evaluated at the current
* iterate.
*/
/// Prints error for too few degrees of freedom.
///
/// @tparam Scalar Scalar type.
/// @param c_e The problem's equality constraints cₑ(x) evaluated at the current
/// iterate.
template <typename Scalar>
void print_too_few_dofs_error(
const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e) {
@@ -112,13 +104,11 @@ void print_too_few_dofs_error(
#endif
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* Prints equality constraint local infeasibility error.
*
* @tparam Scalar Scalar type.
* @param c_e The problem's equality constraints cₑ(x) evaluated at the current
* iterate.
*/
/// Prints equality constraint local infeasibility error.
///
/// @tparam Scalar Scalar type.
/// @param c_e The problem's equality constraints cₑ(x) evaluated at the current
/// iterate.
template <typename Scalar>
void print_c_e_local_infeasibility_error(
const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e) {
@@ -137,13 +127,11 @@ void print_c_e_local_infeasibility_error(
#endif
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* Prints inequality constraint local infeasibility error.
*
* @tparam Scalar Scalar type.
* @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
* current iterate.
*/
/// Prints inequality constraint local infeasibility error.
///
/// @tparam Scalar Scalar type.
/// @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
/// current iterate.
template <typename Scalar>
void print_c_i_local_infeasibility_error(
const Eigen::Vector<Scalar, Eigen::Dynamic>& c_i) {
@@ -181,25 +169,23 @@ inline void print_bound_constraint_global_infeasibility_error(
#endif
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* Prints diagnostics for the current iteration.
*
* @tparam Scalar Scalar type.
* @param iterations Number of iterations.
* @param type The iteration's type.
* @param time The iteration duration.
* @param error The error.
* @param cost The cost.
* @param infeasibility The infeasibility.
* @param complementarity The complementarity.
* @param μ The barrier parameter.
* @param δ The Hessian regularization factor.
* @param primal_α The primal step size.
* @param primal_α_max The max primal step size.
* @param α_reduction_factor Factor by which primal_α is reduced during
* backtracking.
* @param dual_α The dual step size.
*/
/// Prints diagnostics for the current iteration.
///
/// @tparam Scalar Scalar type.
/// @param iterations Number of iterations.
/// @param type The iteration's type.
/// @param time The iteration duration.
/// @param error The error.
/// @param cost The cost.
/// @param infeasibility The infeasibility.
/// @param complementarity The complementarity.
/// @param μ The barrier parameter.
/// @param δ The Hessian regularization factor.
/// @param primal_α The primal step size.
/// @param primal_α_max The max primal step size.
/// @param α_reduction_factor Factor by which primal_α is reduced during
/// backtracking.
/// @param dual_α The dual step size.
template <typename Scalar, typename Rep, typename Period = std::ratio<1>>
void print_iteration_diagnostics(int iterations, IterationType type,
const std::chrono::duration<Rep, Period>& time,
@@ -261,9 +247,7 @@ void print_iteration_diagnostics(int iterations, IterationType type,
#endif
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* Prints bottom of iteration diagnostics table.
*/
/// Prints bottom of iteration diagnostics table.
inline void print_bottom_iteration_diagnostics() {
slp::println("└{:─^108}┘", "");
}
@@ -271,12 +255,10 @@ inline void print_bottom_iteration_diagnostics() {
#define print_bottom_iteration_diagnostics(...)
#endif
/**
* Renders histogram of the given normalized value.
*
* @tparam Width Width of the histogram in characters.
* @param value Normalized value from 0 to 1.
*/
/// Renders histogram of the given normalized value.
///
/// @tparam Width Width of the histogram in characters.
/// @param value Normalized value from 0 to 1.
template <int Width>
requires(Width > 0)
std::string histogram(double value) {
@@ -306,11 +288,9 @@ std::string histogram(double value) {
}
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* Prints solver diagnostics.
*
* @param solve_profilers Solve profilers.
*/
/// Prints solver diagnostics.
///
/// @param solve_profilers Solve profilers.
inline void print_solver_diagnostics(
const gch::small_vector<SolveProfiler>& solve_profilers) {
auto solve_duration = to_ms(solve_profilers[0].total_duration());
@@ -337,11 +317,9 @@ inline void print_solver_diagnostics(
#endif
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* Prints autodiff diagnostics.
*
* @param setup_profilers Autodiff setup profilers.
*/
/// Prints autodiff diagnostics.
///
/// @param setup_profilers Autodiff setup profilers.
inline void print_autodiff_diagnostics(
const gch::small_vector<SetupProfiler>& setup_profilers) {
auto setup_duration = to_ms(setup_profilers[0].duration());

30
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/scope_exit.hpp vendored
View File

@@ -6,20 +6,16 @@
namespace slp {
/**
* scope_exit is a general-purpose scope guard intended to call its exit
* function when a scope is exited.
*
* @tparam F Function type.
*/
/// scope_exit is a general-purpose scope guard intended to call its exit
/// function when a scope is exited.
///
/// @tparam F Function type.
template <typename F>
class scope_exit {
public:
/**
* Constructs a scope_exit.
*
* @param f Function to call on scope exit.
*/
/// Constructs a scope_exit.
///
/// @param f Function to call on scope exit.
explicit scope_exit(F&& f) noexcept : m_f{std::forward<F>(f)} {}
~scope_exit() {
@@ -28,11 +24,9 @@ class scope_exit {
}
}
/**
* Move constructor.
*
* @param rhs scope_exit from which to move.
*/
/// Move constructor.
///
/// @param rhs scope_exit from which to move.
scope_exit(scope_exit&& rhs) noexcept
: m_f{std::move(rhs.m_f)}, m_active{rhs.m_active} {
rhs.release();
@@ -41,9 +35,7 @@ class scope_exit {
scope_exit(const scope_exit&) = delete;
scope_exit& operator=(const scope_exit&) = delete;
/**
* Makes the scope_exit inactive.
*/
/// Makes the scope_exit inactive.
void release() noexcept { m_active = false; }
private:

40
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/scoped_profiler.hpp vendored
View File

@@ -10,37 +10,29 @@
namespace slp {
/**
* Starts a profiler in the constructor and stops it in the destructor.
*/
/// Starts a profiler in the constructor and stops it in the destructor.
template <typename Profiler>
requires std::same_as<Profiler, SetupProfiler> ||
std::same_as<Profiler, SolveProfiler>
class ScopedProfiler {
public:
/**
* Starts a profiler.
*
* @param profiler The profiler.
*/
/// Starts a profiler.
///
/// @param profiler The profiler.
explicit ScopedProfiler(Profiler& profiler) noexcept : m_profiler{&profiler} {
m_profiler->start();
}
/**
* Stops a profiler.
*/
/// Stops a profiler.
~ScopedProfiler() {
if (m_active) {
m_profiler->stop();
}
}
/**
* Move constructor.
*
* @param rhs The other ScopedProfiler.
*/
/// Move constructor.
///
/// @param rhs The other ScopedProfiler.
ScopedProfiler(ScopedProfiler&& rhs) noexcept
: m_profiler{std::move(rhs.m_profiler)}, m_active{rhs.m_active} {
rhs.m_active = false;
@@ -49,11 +41,9 @@ class ScopedProfiler {
ScopedProfiler(const ScopedProfiler&) = delete;
ScopedProfiler& operator=(const ScopedProfiler&) = delete;
/**
* Stops the profiler.
*
* If this is called, the destructor is a no-op.
*/
/// Stops the profiler.
///
/// If this is called, the destructor is a no-op.
void stop() {
if (m_active) {
m_profiler->stop();
@@ -61,11 +51,9 @@ class ScopedProfiler {
}
}
/**
* Returns the most recent solve duration in milliseconds as a double.
*
* @return The most recent solve duration in milliseconds as a double.
*/
/// Returns the most recent solve duration in milliseconds as a double.
///
/// @return The most recent solve duration in milliseconds as a double.
const std::chrono::duration<double>& current_duration() const {
return m_profiler->current_duration();
}

38
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/setup_profiler.hpp vendored
View File

@@ -8,31 +8,23 @@
namespace slp {
/**
* Records the number of profiler measurements (start/stop pairs) and the
* average duration between each start and stop call.
*/
/// Records the number of profiler measurements (start/stop pairs) and the
/// average duration between each start and stop call.
class SetupProfiler {
public:
/**
* Constructs a SetupProfiler.
*
* @param name Name of measurement to show in diagnostics.
*/
/// Constructs a SetupProfiler.
///
/// @param name Name of measurement to show in diagnostics.
explicit SetupProfiler(std::string_view name) : m_name{name} {}
/**
* Tell the profiler to start measuring setup time.
*/
/// Tell the profiler to start measuring setup time.
void start() {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
m_setup_start_time = std::chrono::steady_clock::now();
#endif
}
/**
* Tell the profiler to stop measuring setup time.
*/
/// Tell the profiler to stop measuring setup time.
void stop() {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
m_setup_stop_time = std::chrono::steady_clock::now();
@@ -40,18 +32,14 @@ class SetupProfiler {
#endif
}
/**
* Returns name of measurement to show in diagnostics.
*
* @return Name of measurement to show in diagnostics.
*/
/// Returns name of measurement to show in diagnostics.
///
/// @return Name of measurement to show in diagnostics.
std::string_view name() const { return m_name; }
/**
* Returns the setup duration in milliseconds as a double.
*
* @return The setup duration in milliseconds as a double.
*/
/// Returns the setup duration in milliseconds as a double.
///
/// @return The setup duration in milliseconds as a double.
const std::chrono::duration<double>& duration() const {
return m_setup_duration;
}

64
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/solve_profiler.hpp vendored
View File

@@ -8,32 +8,24 @@
namespace slp {
/**
* Records the number of profiler measurements (start/stop pairs) and the
* average duration between each start and stop call.
*/
/// Records the number of profiler measurements (start/stop pairs) and the
/// average duration between each start and stop call.
class SolveProfiler {
public:
/**
* Constructs a SolveProfiler.
*
* @param name Name of measurement to show in diagnostics.
*/
/// Constructs a SolveProfiler.
///
/// @param name Name of measurement to show in diagnostics.
explicit SolveProfiler(std::string_view name) : m_name{name} {}
/**
* Tell the profiler to start measuring solve time.
*/
/// Tell the profiler to start measuring solve time.
void start() {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
m_current_solve_start_time = std::chrono::steady_clock::now();
#endif
}
/**
* Tell the profiler to stop measuring solve time, increment the number of
* averages, and incorporate the latest measurement into the average.
*/
/// Tell the profiler to stop measuring solve time, increment the number of
/// averages, and incorporate the latest measurement into the average.
void stop() {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
m_current_solve_stop_time = std::chrono::steady_clock::now();
@@ -48,43 +40,33 @@ class SolveProfiler {
#endif
}
/**
* Returns name of measurement to show in diagnostics.
*
* @return Name of measurement to show in diagnostics.
*/
/// Returns name of measurement to show in diagnostics.
///
/// @return Name of measurement to show in diagnostics.
std::string_view name() const { return m_name; }
/**
* Returns the number of solves.
*
* @return The number of solves.
*/
/// Returns the number of solves.
///
/// @return The number of solves.
int num_solves() const { return m_num_solves; }
/**
* Returns the most recent solve duration in seconds.
*
* @return The most recent solve duration in seconds.
*/
/// Returns the most recent solve duration in seconds.
///
/// @return The most recent solve duration in seconds.
const std::chrono::duration<double>& current_duration() const {
return m_current_solve_duration;
}
/**
* Returns the average solve duration in seconds.
*
* @return The average solve duration in seconds.
*/
/// Returns the average solve duration in seconds.
///
/// @return The average solve duration in seconds.
const std::chrono::duration<double>& average_duration() const {
return m_average_solve_duration;
}
/**
* Returns the sum of all solve durations in seconds.
*
* @return The sum of all solve durations in seconds.
*/
/// Returns the sum of all solve durations in seconds.
///
/// @return The sum of all solve durations in seconds.
const std::chrono::duration<double>& total_duration() const {
return m_total_solve_duration;
}

90
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/spy.hpp vendored
View File

@@ -16,48 +16,44 @@
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>
*
* @tparam Scalar Scalar type.
*/
/// 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>
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
class 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.
*/
/// 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)
@@ -79,11 +75,9 @@ class Spy {
write32le(cols);
}
/**
* Adds a matrix to the file.
*
* @param mat The matrix.
*/
/// Adds a matrix to the file.
///
/// @param mat The matrix.
void add(const Eigen::SparseMatrix<Scalar>& mat) {
// Write number of coordinates
write32le(mat.nonZeros());
@@ -108,11 +102,9 @@ class Spy {
private:
std::ofstream m_file;
/**
* Writes a 32-bit signed integer to the file as little-endian.
*
* @param num A 32-bit signed integer.
*/
/// 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::util::byteswap(num);

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

@@ -0,0 +1,16 @@
// Copyright (c) Sleipnir contributors
#pragma once
namespace slp {
[[noreturn]]
inline void unreachable() {
#if defined(_MSC_VER) && !defined(__clang__)
__assume(false);
#else
__builtin_unreachable();
#endif
}
} // namespace slp