Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[upstream_utils] Upgrade Sleipnir (#7973)

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

43
upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch
View File

@@ -1,30 +1,32 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Wed, 29 May 2024 16:29:55 -0700
Subject: [PATCH 1/3] Use fmtlib
Subject: [PATCH 1/8] Use fmtlib
---
include/.styleguide | 1 +
include/sleipnir/util/Print.hpp | 31 ++++++++++++++++++-------------
2 files changed, 19 insertions(+), 13 deletions(-)
include/sleipnir/util/print.hpp | 31 ++++++++++++++++++-------------
src/optimization/problem.cpp | 2 +-
3 files changed, 20 insertions(+), 14 deletions(-)
diff --git a/include/.styleguide b/include/.styleguide
index 8fb61fdf9cc5ceff633d3126f0579eef25a1326f..6a7f8ed28f9cb037c9746a7e0ef5e110481d9825 100644
index 1b6652d3d5886cf8c9eca0d855c21031775bad7c..4f4c76204071f90bf49eddb8c2aceb583b5e09ba 100644
--- a/include/.styleguide
+++ b/include/.styleguide
@@ -12,4 +12,5 @@ licenseUpdateExclude {
@@ -8,5 +8,6 @@ cppSrcFileInclude {
includeOtherLibs {
^Eigen/
+ ^fmt/
^gch/
}
diff --git a/include/sleipnir/util/Print.hpp b/include/sleipnir/util/Print.hpp
index a746cb77b70f095bb15f4c493295cb925bc74cd3..c01fd4ac679df854b885293d681ea1e0984626fa 100644
--- a/include/sleipnir/util/Print.hpp
+++ b/include/sleipnir/util/Print.hpp
@@ -3,52 +3,57 @@
#pragma once
diff --git a/include/sleipnir/util/print.hpp b/include/sleipnir/util/print.hpp
index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b1159aeb2a57 100644
--- a/include/sleipnir/util/print.hpp
+++ b/include/sleipnir/util/print.hpp
@@ -4,10 +4,15 @@
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
#include <cstdio>
-#include <print>
#include <system_error>
@@ -36,7 +38,11 @@ index a746cb77b70f095bb15f4c493295cb925bc74cd3..c01fd4ac679df854b885293d681ea1e0
+#include <fmt/core.h>
+#endif
+
namespace sleipnir {
#endif
namespace slp {
@@ -15,45 +20,45 @@ namespace slp {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
- * Wrapper around std::print() that squelches write failure exceptions.
@@ -93,3 +99,16 @@ index a746cb77b70f095bb15f4c493295cb925bc74cd3..c01fd4ac679df854b885293d681ea1e0
} catch (const std::system_error&) {
}
}
diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp
index 31115490867146ec166604bcc61731d7891a9f22..81863808d329a53d4162ce0624a3b8e8afc32dfc 100644
--- a/src/optimization/problem.cpp
+++ b/src/optimization/problem.cpp
@@ -335,7 +335,7 @@ void Problem::print_exit_conditions([[maybe_unused]] const Options& options) {
slp::println(" ↳ executed {} iterations", options.max_iterations);
}
if (std::isfinite(options.timeout.count())) {
- slp::println(" ↳ {} elapsed", options.timeout);
+ slp::println(" ↳ {} elapsed", options.timeout.count());
}
}

639
upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch
View File

@@ -1,610 +1,77 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sun, 16 Jun 2024 12:08:49 -0700
Subject: [PATCH 2/3] Use wpi::SmallVector
Subject: [PATCH 2/8] Use wpi::SmallVector
---
include/.styleguide | 1 +
include/sleipnir/autodiff/Expression.hpp | 13 +++++++------
include/sleipnir/autodiff/ExpressionGraph.hpp | 15 ++++++++-------
include/sleipnir/autodiff/Hessian.hpp | 4 ++--
include/sleipnir/autodiff/Jacobian.hpp | 10 +++++-----
include/sleipnir/autodiff/Variable.hpp | 10 +++++-----
include/sleipnir/autodiff/VariableMatrix.hpp | 4 ++--
include/sleipnir/optimization/Multistart.hpp | 7 ++++---
.../optimization/OptimizationProblem.hpp | 8 ++++----
include/sleipnir/util/Pool.hpp | 7 ++++---
include/sleipnir/util/Spy.hpp | 4 ++--
src/.styleguide | 1 +
src/optimization/solver/InteriorPoint.cpp | 4 ++--
src/optimization/solver/SQP.cpp | 4 ++--
.../solver/util/FeasibilityRestoration.hpp | 18 +++++++++---------
src/optimization/solver/util/Filter.hpp | 4 ++--
16 files changed, 60 insertions(+), 54 deletions(-)
include/sleipnir/autodiff/expression.hpp | 4 ++--
include/sleipnir/autodiff/variable.hpp | 5 ++---
include/sleipnir/autodiff/variable_matrix.hpp | 4 ++--
3 files changed, 6 insertions(+), 7 deletions(-)
diff --git a/include/.styleguide b/include/.styleguide
index 6a7f8ed28f9cb037c9746a7e0ef5e110481d9825..efa36cee1fb593ae810032340c64f1854fbbc523 100644
--- a/include/.styleguide
+++ b/include/.styleguide
@@ -13,4 +13,5 @@ licenseUpdateExclude {
includeOtherLibs {
^Eigen/
^fmt/
+ ^wpi/
}
diff --git a/include/sleipnir/autodiff/Expression.hpp b/include/sleipnir/autodiff/Expression.hpp
index dd53755ccae88e3975d1b5e6b13ac464bd4efcce..ef9a15bf69d8cae6b2196513b72ec4b359cc8752 100644
--- a/include/sleipnir/autodiff/Expression.hpp
+++ b/include/sleipnir/autodiff/Expression.hpp
@@ -11,11 +11,12 @@
#include <numbers>
#include <utility>
diff --git a/include/sleipnir/autodiff/expression.hpp b/include/sleipnir/autodiff/expression.hpp
index 873e1c27559d92eb1b3a217890ca41bdc65af122..1c5f84d22a0bed70869121acabd527825ba90adb 100644
--- a/include/sleipnir/autodiff/expression.hpp
+++ b/include/sleipnir/autodiff/expression.hpp
@@ -30,7 +30,7 @@ inline constexpr bool USE_POOL_ALLOCATOR = true;
struct Expression;
+#include <wpi/SmallVector.h>
+
#include "sleipnir/autodiff/ExpressionType.hpp"
#include "sleipnir/util/IntrusiveSharedPtr.hpp"
#include "sleipnir/util/Pool.hpp"
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir::detail {
@@ -29,8 +30,8 @@ inline constexpr bool kUsePoolAllocator = true;
struct SLEIPNIR_DLLEXPORT Expression;
-inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr);
-inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr);
+inline void IntrusiveSharedPtrIncRefCount(Expression* expr);
+inline void IntrusiveSharedPtrDecRefCount(Expression* expr);
inline constexpr void inc_ref_count(Expression* expr);
-inline constexpr void dec_ref_count(Expression* expr);
+inline void dec_ref_count(Expression* expr);
/**
* Typedef for intrusive shared pointer to Expression.
@@ -418,7 +419,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt(const ExpressionPtr& x);
@@ -680,7 +680,7 @@ inline constexpr void inc_ref_count(Expression* expr) {
*
* @param expr The shared pointer's managed object.
*/
-inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) {
+inline void IntrusiveSharedPtrIncRefCount(Expression* expr) {
++expr->refCount;
}
@@ -427,12 +428,12 @@ inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) {
*
* @param expr The shared pointer's managed object.
*/
-inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr) {
+inline void IntrusiveSharedPtrDecRefCount(Expression* expr) {
-inline constexpr void dec_ref_count(Expression* expr) {
+inline void dec_ref_count(Expression* expr) {
// If a deeply nested tree is being deallocated all at once, calling the
// Expression destructor when expr's refcount reaches zero can cause a stack
// overflow. Instead, we iterate over its children to decrement their
// refcounts and deallocate them.
- small_vector<Expression*> stack;
+ wpi::SmallVector<Expression*> stack;
stack.emplace_back(expr);
while (!stack.empty()) {
diff --git a/include/sleipnir/autodiff/ExpressionGraph.hpp b/include/sleipnir/autodiff/ExpressionGraph.hpp
index c614195d82ad022dfd0c3f6f8240b042c0056c2f..714bcbb82907e754138347334c7fca8a7ccf055d 100644
--- a/include/sleipnir/autodiff/ExpressionGraph.hpp
+++ b/include/sleipnir/autodiff/ExpressionGraph.hpp
@@ -4,10 +4,11 @@
#include <span>
+#include <wpi/SmallVector.h>
+
#include "sleipnir/autodiff/Expression.hpp"
#include "sleipnir/util/FunctionRef.hpp"
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir::detail {
@@ -36,7 +37,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph {
// https://en.wikipedia.org/wiki/Breadth-first_search
// BFS list sorted from parent to child.
- small_vector<Expression*> stack;
+ wpi::SmallVector<Expression*> stack;
stack.emplace_back(root.Get());
@@ -119,7 +120,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph {
*
* @param wrt Variables with respect to which to compute the gradient.
diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp
index 14eb1d3b95069e143699e1488f3081c4cd9de07c..9f79a82763213dc712cce4c2a322289d57645032 100644
--- a/include/sleipnir/autodiff/variable.hpp
+++ b/include/sleipnir/autodiff/variable.hpp
@@ -47,7 +47,7 @@ class SLEIPNIR_DLLEXPORT Variable {
/**
* Constructs an empty Variable.
*/
- small_vector<ExpressionPtr> GenerateGradientTree(
+ wpi::SmallVector<ExpressionPtr> GenerateGradientTree(
std::span<const ExpressionPtr> wrt) const {
// Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation
// for background on reverse accumulation automatic differentiation.
@@ -128,7 +129,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph {
wrt[row]->row = row;
}
- small_vector<ExpressionPtr> grad;
+ wpi::SmallVector<ExpressionPtr> grad;
grad.reserve(wrt.size());
for (size_t row = 0; row < wrt.size(); ++row) {
grad.emplace_back(MakeExpressionPtr());
@@ -231,13 +232,13 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph {
private:
// List that maps nodes to their respective row.
- small_vector<int> m_rowList;
+ wpi::SmallVector<int> m_rowList;
// List for updating adjoints
- small_vector<Expression*> m_adjointList;
+ wpi::SmallVector<Expression*> m_adjointList;
// List for updating values
- small_vector<Expression*> m_valueList;
+ wpi::SmallVector<Expression*> m_valueList;
};
} // namespace sleipnir::detail
diff --git a/include/sleipnir/autodiff/Hessian.hpp b/include/sleipnir/autodiff/Hessian.hpp
index 4563aa234bd7b0ec22e12d2fc0b6f4569bee7f39..2e60d89e95280bdac422b0d7dab955ba111b0059 100644
--- a/include/sleipnir/autodiff/Hessian.hpp
+++ b/include/sleipnir/autodiff/Hessian.hpp
@@ -6,6 +6,7 @@
#include <Eigen/Core>
#include <Eigen/SparseCore>
+#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/ExpressionGraph.hpp"
#include "sleipnir/autodiff/Jacobian.hpp"
@@ -13,7 +14,6 @@
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/autodiff/VariableMatrix.hpp"
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -36,7 +36,7 @@ class SLEIPNIR_DLLEXPORT Hessian {
Hessian(Variable variable, const VariableMatrix& wrt) noexcept
: m_jacobian{
[&] {
- small_vector<detail::ExpressionPtr> wrtVec;
+ wpi::SmallVector<detail::ExpressionPtr> wrtVec;
wrtVec.reserve(wrt.size());
for (auto& elem : wrt) {
wrtVec.emplace_back(elem.expr);
diff --git a/include/sleipnir/autodiff/Jacobian.hpp b/include/sleipnir/autodiff/Jacobian.hpp
index ac00c11ef8c947cbe95c58081bdbfb42bf901051..0c660156c80f94539383b8f0d01d7853e41e0297 100644
--- a/include/sleipnir/autodiff/Jacobian.hpp
+++ b/include/sleipnir/autodiff/Jacobian.hpp
@@ -5,13 +5,13 @@
#include <utility>
#include <Eigen/SparseCore>
+#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/ExpressionGraph.hpp"
#include "sleipnir/autodiff/Profiler.hpp"
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/autodiff/VariableMatrix.hpp"
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -81,7 +81,7 @@ class SLEIPNIR_DLLEXPORT Jacobian {
VariableMatrix Get() const {
VariableMatrix result{m_variables.Rows(), m_wrt.Rows()};
- small_vector<detail::ExpressionPtr> wrtVec;
+ wpi::SmallVector<detail::ExpressionPtr> wrtVec;
wrtVec.reserve(m_wrt.size());
for (auto& elem : m_wrt) {
wrtVec.emplace_back(elem.expr);
@@ -138,16 +138,16 @@ class SLEIPNIR_DLLEXPORT Jacobian {
VariableMatrix m_variables;
VariableMatrix m_wrt;
- small_vector<detail::ExpressionGraph> m_graphs;
+ wpi::SmallVector<detail::ExpressionGraph> m_graphs;
Eigen::SparseMatrix<double> m_J{m_variables.Rows(), m_wrt.Rows()};
// Cached triplets for gradients of linear rows
- small_vector<Eigen::Triplet<double>> m_cachedTriplets;
+ wpi::SmallVector<Eigen::Triplet<double>> m_cachedTriplets;
// List of row indices for nonlinear rows whose graients will be computed in
// Value()
- small_vector<int> m_nonlinearRows;
+ wpi::SmallVector<int> m_nonlinearRows;
Profiler m_profiler;
};
diff --git a/include/sleipnir/autodiff/Variable.hpp b/include/sleipnir/autodiff/Variable.hpp
index c04d629f482efe59497570ca1fd9b09a4af2ae1e..d192fb96e7984b7c0ca30262668c41e5e84ca34e 100644
--- a/include/sleipnir/autodiff/Variable.hpp
+++ b/include/sleipnir/autodiff/Variable.hpp
@@ -10,6 +10,7 @@
#include <vector>
#include <Eigen/Core>
+#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/Expression.hpp"
#include "sleipnir/autodiff/ExpressionGraph.hpp"
@@ -17,7 +18,6 @@
#include "sleipnir/util/Concepts.hpp"
#include "sleipnir/util/Print.hpp"
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -445,8 +445,8 @@ template <typename LHS, typename RHS>
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
-small_vector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
- small_vector<Variable> constraints;
+wpi::SmallVector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
+ wpi::SmallVector<Variable> constraints;
if constexpr (ScalarLike<std::decay_t<LHS>> &&
ScalarLike<std::decay_t<RHS>>) {
@@ -534,7 +534,7 @@ small_vector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
*/
struct SLEIPNIR_DLLEXPORT EqualityConstraints {
/// A vector of scalar equality constraints.
- small_vector<Variable> constraints;
+ wpi::SmallVector<Variable> constraints;
- explicit constexpr Variable(std::nullptr_t) : expr{nullptr} {}
+ explicit Variable(std::nullptr_t) : expr{nullptr} {}
/**
* Concatenates multiple equality constraints.
@@ -596,7 +596,7 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints {
*/
struct SLEIPNIR_DLLEXPORT InequalityConstraints {
/// A vector of scalar inequality constraints.
- small_vector<Variable> constraints;
+ wpi::SmallVector<Variable> constraints;
* Constructs a Variable from a floating point type.
@@ -77,8 +77,7 @@ class SLEIPNIR_DLLEXPORT Variable {
*
* @param expr The autodiff variable.
*/
- explicit constexpr Variable(detail::ExpressionPtr&& expr)
- : expr{std::move(expr)} {}
+ explicit Variable(detail::ExpressionPtr&& expr) : expr{std::move(expr)} {}
/**
* Concatenates multiple inequality constraints.
diff --git a/include/sleipnir/autodiff/VariableMatrix.hpp b/include/sleipnir/autodiff/VariableMatrix.hpp
index 47452b8988b3a1a96a78d28644200b1c4cdc89c9..57b09913d15e9590873ad7bf62e2baff9fbc5df9 100644
--- a/include/sleipnir/autodiff/VariableMatrix.hpp
+++ b/include/sleipnir/autodiff/VariableMatrix.hpp
@@ -11,6 +11,7 @@
#include <vector>
#include <Eigen/Core>
+#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/Slice.hpp"
#include "sleipnir/autodiff/Variable.hpp"
@@ -18,7 +19,6 @@
#include "sleipnir/util/Assert.hpp"
#include "sleipnir/util/FunctionRef.hpp"
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -946,7 +946,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
}
private:
- small_vector<Variable> m_storage;
+ wpi::SmallVector<Variable> m_storage;
int m_rows = 0;
int m_cols = 0;
};
diff --git a/include/sleipnir/optimization/Multistart.hpp b/include/sleipnir/optimization/Multistart.hpp
index 8055713a2492a9c8473f047a2fb9fe7ca57753c3..09b1b2f3bf33c35ae0aeddb9b5d47c0d80c68cec 100644
--- a/include/sleipnir/optimization/Multistart.hpp
+++ b/include/sleipnir/optimization/Multistart.hpp
@@ -6,9 +6,10 @@
#include <future>
#include <span>
+#include <wpi/SmallVector.h>
+
#include "sleipnir/optimization/SolverStatus.hpp"
#include "sleipnir/util/FunctionRef.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -44,14 +45,14 @@ MultistartResult<DecisionVariables> Multistart(
const DecisionVariables& initialGuess)>
solve,
std::span<const DecisionVariables> initialGuesses) {
- small_vector<std::future<MultistartResult<DecisionVariables>>> futures;
+ wpi::SmallVector<std::future<MultistartResult<DecisionVariables>>> futures;
futures.reserve(initialGuesses.size());
for (const auto& initialGuess : initialGuesses) {
futures.emplace_back(std::async(std::launch::async, solve, initialGuess));
}
- small_vector<MultistartResult<DecisionVariables>> results;
+ wpi::SmallVector<MultistartResult<DecisionVariables>> results;
results.reserve(futures.size());
for (auto& future : futures) {
diff --git a/include/sleipnir/optimization/OptimizationProblem.hpp b/include/sleipnir/optimization/OptimizationProblem.hpp
index 569dcdee507881ceb412585ca811927072552c15..66883fed98ad087010fb153bd91effce6e047928 100644
--- a/include/sleipnir/optimization/OptimizationProblem.hpp
+++ b/include/sleipnir/optimization/OptimizationProblem.hpp
@@ -11,6 +11,7 @@
#include <utility>
#include <Eigen/Core>
+#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/autodiff/VariableMatrix.hpp"
@@ -22,7 +23,6 @@
#include "sleipnir/optimization/solver/SQP.hpp"
#include "sleipnir/util/Print.hpp"
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -364,16 +364,16 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
private:
// The list of decision variables, which are the root of the problem's
// expression tree
- small_vector<Variable> m_decisionVariables;
+ wpi::SmallVector<Variable> m_decisionVariables;
// The cost function: f(x)
std::optional<Variable> m_f;
// The list of equality constraints: cₑ(x) = 0
- small_vector<Variable> m_equalityConstraints;
+ wpi::SmallVector<Variable> m_equalityConstraints;
// The list of inequality constraints: cᵢ(x) ≥ 0
- small_vector<Variable> m_inequalityConstraints;
+ wpi::SmallVector<Variable> m_inequalityConstraints;
// The user callback
std::function<bool(const SolverIterationInfo& info)> m_callback =
diff --git a/include/sleipnir/util/Pool.hpp b/include/sleipnir/util/Pool.hpp
index 441fa701d4972bc14973c6269d56d4a124deaee5..1951bd1ee8f3bee8d4a3c044c99354b0fd10031d 100644
--- a/include/sleipnir/util/Pool.hpp
+++ b/include/sleipnir/util/Pool.hpp
@@ -5,8 +5,9 @@
#include <cstddef>
#include <memory>
+#include <wpi/SmallVector.h>
+
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -78,8 +79,8 @@ class SLEIPNIR_DLLEXPORT PoolResource {
}
private:
- small_vector<std::unique_ptr<std::byte[]>> m_buffer;
- small_vector<void*> m_freeList;
+ wpi::SmallVector<std::unique_ptr<std::byte[]>> m_buffer;
+ wpi::SmallVector<void*> m_freeList;
size_t blocksPerChunk;
* Assignment operator for double.
diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
index 410f12873cfdf5d0d484653c6c3dac74ed96348a..1c6f9e8dade8bebce7aec18bbb9b5491acb1d977 100644
--- a/include/sleipnir/autodiff/variable_matrix.hpp
+++ b/include/sleipnir/autodiff/variable_matrix.hpp
@@ -1120,14 +1120,14 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
*
* @return Begin iterator.
*/
- const_iterator cbegin() const { return const_iterator{m_storage.cbegin()}; }
+ const_iterator cbegin() const { return const_iterator{m_storage.begin()}; }
/**
diff --git a/include/sleipnir/util/Spy.hpp b/include/sleipnir/util/Spy.hpp
index cb9b4e191545e96c2bade5f8f99b0bec376b656b..7f526a2d9968e76b385a0ddfb2edf5bab7274fb0 100644
--- a/include/sleipnir/util/Spy.hpp
+++ b/include/sleipnir/util/Spy.hpp
@@ -7,9 +7,9 @@
#include <string_view>
* Returns end iterator.
*
* @return End iterator.
*/
- const_iterator cend() const { return const_iterator{m_storage.cend()}; }
+ const_iterator cend() const { return const_iterator{m_storage.end()}; }
#include <Eigen/SparseCore>
+#include <wpi/SmallVector.h>
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -32,7 +32,7 @@ SLEIPNIR_DLLEXPORT inline void Spy(std::ostream& file,
const int cells_width = mat.cols() + 1;
const int cells_height = mat.rows();
- small_vector<uint8_t> cells;
+ wpi::SmallVector<uint8_t> cells;
// Allocate space for matrix of characters plus trailing newlines
cells.reserve(cells_width * cells_height);
diff --git a/src/.styleguide b/src/.styleguide
index f3b2f0cf9e60b3a86b9654ff2b381f9c48734ff6..ad739cea6dce6f6cb586f538d1d30b92503484c1 100644
--- a/src/.styleguide
+++ b/src/.styleguide
@@ -8,4 +8,5 @@ cppSrcFileInclude {
includeOtherLibs {
^Eigen/
+ ^wpi/
}
diff --git a/src/optimization/solver/InteriorPoint.cpp b/src/optimization/solver/InteriorPoint.cpp
index a09d9866d05731c8ce53122b3d1a850803883df4..d3981c59d163927e3e5ba602c3323f6e1429c475 100644
--- a/src/optimization/solver/InteriorPoint.cpp
+++ b/src/optimization/solver/InteriorPoint.cpp
@@ -9,6 +9,7 @@
#include <limits>
#include <Eigen/SparseCholesky>
+#include <wpi/SmallVector.h>
#include "optimization/RegularizedLDLT.hpp"
#include "optimization/solver/util/ErrorEstimate.hpp"
@@ -23,7 +24,6 @@
#include "sleipnir/optimization/SolverExitCondition.hpp"
#include "sleipnir/util/Print.hpp"
#include "sleipnir/util/Spy.hpp"
-#include "sleipnir/util/small_vector.hpp"
#include "util/ScopeExit.hpp"
#include "util/ToMilliseconds.hpp"
@@ -226,7 +226,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
};
// Kept outside the loop so its storage can be reused
- small_vector<Eigen::Triplet<double>> triplets;
+ wpi::SmallVector<Eigen::Triplet<double>> triplets;
RegularizedLDLT solver;
diff --git a/src/optimization/solver/SQP.cpp b/src/optimization/solver/SQP.cpp
index 77b9632e1da37361c995a8579d1d83a2756d6d88..662abc2fb6e311767b0fbb3a61121ce78549a3f6 100644
--- a/src/optimization/solver/SQP.cpp
+++ b/src/optimization/solver/SQP.cpp
@@ -9,6 +9,7 @@
#include <limits>
#include <Eigen/SparseCholesky>
+#include <wpi/SmallVector.h>
#include "optimization/RegularizedLDLT.hpp"
#include "optimization/solver/util/ErrorEstimate.hpp"
@@ -22,7 +23,6 @@
#include "sleipnir/optimization/SolverExitCondition.hpp"
#include "sleipnir/util/Print.hpp"
#include "sleipnir/util/Spy.hpp"
-#include "sleipnir/util/small_vector.hpp"
#include "util/ScopeExit.hpp"
#include "util/ToMilliseconds.hpp"
@@ -155,7 +155,7 @@ void SQP(std::span<Variable> decisionVariables,
Filter filter{f};
// Kept outside the loop so its storage can be reused
- small_vector<Eigen::Triplet<double>> triplets;
+ wpi::SmallVector<Eigen::Triplet<double>> triplets;
RegularizedLDLT solver;
diff --git a/src/optimization/solver/util/FeasibilityRestoration.hpp b/src/optimization/solver/util/FeasibilityRestoration.hpp
index feefe137adf0832b094a36d61201b15962138ded..79b5d99ae27de6049ba098888a965291e6b677fa 100644
--- a/src/optimization/solver/util/FeasibilityRestoration.hpp
+++ b/src/optimization/solver/util/FeasibilityRestoration.hpp
@@ -8,6 +8,7 @@
#include <span>
#include <Eigen/Core>
+#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/autodiff/VariableMatrix.hpp"
@@ -16,7 +17,6 @@
#include "sleipnir/optimization/SolverStatus.hpp"
#include "sleipnir/optimization/solver/InteriorPoint.hpp"
#include "sleipnir/util/FunctionRef.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -57,7 +57,7 @@ inline void FeasibilityRestoration(
constexpr double ρ = 1000.0;
double μ = config.tolerance / 10.0;
- small_vector<Variable> fr_decisionVariables;
+ wpi::SmallVector<Variable> fr_decisionVariables;
fr_decisionVariables.reserve(decisionVariables.size() +
2 * equalityConstraints.size());
@@ -70,7 +70,7 @@ inline void FeasibilityRestoration(
fr_decisionVariables.emplace_back();
}
- auto it = fr_decisionVariables.cbegin();
+ auto it = fr_decisionVariables.begin();
VariableMatrix xAD{std::span{it, it + decisionVariables.size()}};
it += decisionVariables.size();
@@ -128,7 +128,7 @@ inline void FeasibilityRestoration(
}
// cₑ(x) - pₑ + nₑ = 0
- small_vector<Variable> fr_equalityConstraints;
+ wpi::SmallVector<Variable> fr_equalityConstraints;
fr_equalityConstraints.assign(equalityConstraints.begin(),
equalityConstraints.end());
for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) {
@@ -137,7 +137,7 @@ inline void FeasibilityRestoration(
}
// cᵢ(x) - s - pᵢ + nᵢ = 0
- small_vector<Variable> fr_inequalityConstraints;
+ wpi::SmallVector<Variable> fr_inequalityConstraints;
// pₑ ≥ 0
std::copy(p_e.begin(), p_e.end(),
@@ -227,7 +227,7 @@ inline void FeasibilityRestoration(
constexpr double ρ = 1000.0;
- small_vector<Variable> fr_decisionVariables;
+ wpi::SmallVector<Variable> fr_decisionVariables;
fr_decisionVariables.reserve(decisionVariables.size() +
2 * equalityConstraints.size() +
2 * inequalityConstraints.size());
@@ -243,7 +243,7 @@ inline void FeasibilityRestoration(
fr_decisionVariables.emplace_back();
}
- auto it = fr_decisionVariables.cbegin();
+ auto it = fr_decisionVariables.begin();
VariableMatrix xAD{std::span{it, it + decisionVariables.size()}};
it += decisionVariables.size();
@@ -319,7 +319,7 @@ inline void FeasibilityRestoration(
}
// cₑ(x) - pₑ + nₑ = 0
- small_vector<Variable> fr_equalityConstraints;
+ wpi::SmallVector<Variable> fr_equalityConstraints;
fr_equalityConstraints.assign(equalityConstraints.begin(),
equalityConstraints.end());
for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) {
@@ -328,7 +328,7 @@ inline void FeasibilityRestoration(
}
// cᵢ(x) - s - pᵢ + nᵢ = 0
- small_vector<Variable> fr_inequalityConstraints;
+ wpi::SmallVector<Variable> fr_inequalityConstraints;
fr_inequalityConstraints.assign(inequalityConstraints.begin(),
inequalityConstraints.end());
for (size_t row = 0; row < fr_inequalityConstraints.size(); ++row) {
diff --git a/src/optimization/solver/util/Filter.hpp b/src/optimization/solver/util/Filter.hpp
index f19236928c59623bc0a3ce87b659f0756997f821..0c14efd7b8afa6cef398f5a7d383c54dbf64ec68 100644
--- a/src/optimization/solver/util/Filter.hpp
+++ b/src/optimization/solver/util/Filter.hpp
@@ -8,9 +8,9 @@
#include <utility>
#include <Eigen/Core>
+#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/Variable.hpp"
-#include "sleipnir/util/small_vector.hpp"
// See docs/algorithms.md#Works_cited for citation definitions.
@@ -177,7 +177,7 @@ class Filter {
private:
Variable* m_f = nullptr;
- small_vector<FilterEntry> m_filter;
+ wpi::SmallVector<FilterEntry> m_filter;
};
} // namespace sleipnir
/**
* Returns number of elements in matrix.

31
upstream_utils/sleipnir_patches/0003-Suppress-clang-tidy-false-positives.patch
View File

@@ -1,31 +0,0 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Wed, 26 Jun 2024 12:13:33 -0700
Subject: [PATCH 3/3] Suppress clang-tidy false positives
---
include/sleipnir/autodiff/Variable.hpp | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/include/sleipnir/autodiff/Variable.hpp b/include/sleipnir/autodiff/Variable.hpp
index d192fb96e7984b7c0ca30262668c41e5e84ca34e..f25c6d153310a01700ee2390ecf35ffa8af7df11 100644
--- a/include/sleipnir/autodiff/Variable.hpp
+++ b/include/sleipnir/autodiff/Variable.hpp
@@ -541,7 +541,7 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints {
*
* @param equalityConstraints The list of EqualityConstraints to concatenate.
*/
- EqualityConstraints(
+ EqualityConstraints( // NOLINT
std::initializer_list<EqualityConstraints> equalityConstraints) {
for (const auto& elem : equalityConstraints) {
constraints.insert(constraints.end(), elem.constraints.begin(),
@@ -604,7 +604,7 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints {
* @param inequalityConstraints The list of InequalityConstraints to
* concatenate.
*/
- InequalityConstraints(
+ InequalityConstraints( // NOLINT
std::initializer_list<InequalityConstraints> inequalityConstraints) {
for (const auto& elem : inequalityConstraints) {
constraints.insert(constraints.end(), elem.constraints.begin(),

41
upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch Normal file
View File

@@ -0,0 +1,41 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Tue, 28 Jan 2025 22:19:14 -0800
Subject: [PATCH 3/8] Use wpi::byteswap()
---
include/.styleguide | 1 +
include/sleipnir/util/spy.hpp | 3 ++-
2 files changed, 3 insertions(+), 1 deletion(-)
diff --git a/include/.styleguide b/include/.styleguide
index 4f4c76204071f90bf49eddb8c2aceb583b5e09ba..03938557c2600a7a1f72c6b93c935602f5acb2b2 100644
--- a/include/.styleguide
+++ b/include/.styleguide
@@ -10,4 +10,5 @@ includeOtherLibs {
^Eigen/
^fmt/
^gch/
+ ^wpi/
}
diff --git a/include/sleipnir/util/spy.hpp b/include/sleipnir/util/spy.hpp
index a2f94803e3744cee771669210d1af883160e9896..8cd7d4353aad20153af5cd7a818fa55889d35721 100644
--- a/include/sleipnir/util/spy.hpp
+++ b/include/sleipnir/util/spy.hpp
@@ -12,6 +12,7 @@
#include <string_view>
#include <Eigen/SparseCore>
+#include <wpi/bit.h>
#include "sleipnir/util/symbol_exports.hpp"
@@ -115,7 +116,7 @@ class SLEIPNIR_DLLEXPORT Spy {
*/
void write32le(int32_t num) {
if constexpr (std::endian::native != std::endian::little) {
- num = std::byteswap(num);
+ num = wpi::byteswap(num);
}
m_file.write(reinterpret_cast<char*>(&num), sizeof(num));
}

63
upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch Normal file
View File

@@ -0,0 +1,63 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Tue, 28 Jan 2025 22:19:31 -0800
Subject: [PATCH 4/8] Replace std::to_underlying()
---
src/optimization/problem.cpp | 9 ++++-----
src/util/print_diagnostics.hpp | 6 +++---
2 files changed, 7 insertions(+), 8 deletions(-)
diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp
index 81863808d329a53d4162ce0624a3b8e8afc32dfc..c3319fc0a927cf452871a2db08d5edff87ac8eea 100644
--- a/src/optimization/problem.cpp
+++ b/src/optimization/problem.cpp
@@ -7,7 +7,6 @@
#include <memory>
#include <optional>
#include <ranges>
-#include <utility>
#include <Eigen/Core>
#include <Eigen/SparseCore>
@@ -346,11 +345,11 @@ void Problem::print_problem_analysis() {
// Print problem structure
slp::println("\nProblem structure:");
slp::println(" ↳ {} cost function",
- types[std::to_underlying(cost_function_type())]);
+ types[static_cast<uint8_t>(cost_function_type())]);
slp::println(" ↳ {} equality constraints",
- types[std::to_underlying(equality_constraint_type())]);
+ types[static_cast<uint8_t>(equality_constraint_type())]);
slp::println(" ↳ {} inequality constraints",
- types[std::to_underlying(inequality_constraint_type())]);
+ types[static_cast<uint8_t>(inequality_constraint_type())]);
if (m_decision_variables.size() == 1) {
slp::print("\n1 decision variable\n");
@@ -362,7 +361,7 @@ void Problem::print_problem_analysis() {
[](const gch::small_vector<Variable>& constraints) {
std::array<size_t, 5> counts{};
for (const auto& constraint : constraints) {
- ++counts[std::to_underlying(constraint.type())];
+ ++counts[static_cast<uint8_t>(constraint.type())];
}
for (const auto& [count, name] :
std::views::zip(counts, std::array{"empty", "constant", "linear",
diff --git a/src/util/print_diagnostics.hpp b/src/util/print_diagnostics.hpp
index fde36957c0258f6e3cd435ef6224d60407012ff7..82e0e082b0e40153dcb2fcd2c655a412a8a9540a 100644
--- a/src/util/print_diagnostics.hpp
+++ b/src/util/print_diagnostics.hpp
@@ -238,9 +238,9 @@ void print_iteration_diagnostics(int iterations, IterationType type,
slp::println(
"│{:4} {:4} {:9.3f} {:12e} {:13e} {:12e} {:12e} {:.2e} {:<5} {:.2e} "
"{:.2e} {:2d}│",
- iterations, ITERATION_TYPES[std::to_underlying(type)], to_ms(time), error,
- cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, dual_α,
- backtracks);
+ iterations, ITERATION_TYPES[static_cast<uint8_t>(type)], to_ms(time),
+ error, cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α,
+ dual_α, backtracks);
}
#else
#define print_iteration_diagnostics(...)

53
upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch Normal file
View File

@@ -0,0 +1,53 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sat, 8 Feb 2025 13:42:36 -0800
Subject: [PATCH 5/8] Replace std::views::zip()
---
include/sleipnir/autodiff/adjoint_expression_graph.hpp | 5 ++++-
src/optimization/problem.cpp | 9 +++++----
2 files changed, 9 insertions(+), 5 deletions(-)
diff --git a/include/sleipnir/autodiff/adjoint_expression_graph.hpp b/include/sleipnir/autodiff/adjoint_expression_graph.hpp
index 4b4f3303faed766d3ac39829870514f50d9a582f..4576e19c9695caf4407fbbb592afe32d8252a0db 100644
--- a/include/sleipnir/autodiff/adjoint_expression_graph.hpp
+++ b/include/sleipnir/autodiff/adjoint_expression_graph.hpp
@@ -155,7 +155,10 @@ class AdjointExpressionGraph {
}
}
} else {
- for (const auto& [col, node] : std::views::zip(m_col_list, m_top_list)) {
+ for (size_t i = 0; i < m_top_list.size(); ++i) {
+ const auto& col = m_col_list[i];
+ const auto& node = m_top_list[i];
+
// Append adjoints of wrt to sparse matrix triplets
if (col != -1 && node->adjoint != 0.0) {
triplets.emplace_back(row, col, node->adjoint);
diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp
index c3319fc0a927cf452871a2db08d5edff87ac8eea..5532b3962409e2140132e79241da4fba0f36bc78 100644
--- a/src/optimization/problem.cpp
+++ b/src/optimization/problem.cpp
@@ -6,7 +6,6 @@
#include <cmath>
#include <memory>
#include <optional>
-#include <ranges>
#include <Eigen/Core>
#include <Eigen/SparseCore>
@@ -363,9 +362,11 @@ void Problem::print_problem_analysis() {
for (const auto& constraint : constraints) {
++counts[static_cast<uint8_t>(constraint.type())];
}
- for (const auto& [count, name] :
- std::views::zip(counts, std::array{"empty", "constant", "linear",
- "quadratic", "nonlinear"})) {
+ for (size_t i = 0; i < counts.size(); ++i) {
+ constexpr std::array names{"empty", "constant", "linear", "quadratic",
+ "nonlinear"};
+ const auto& count = counts[i];
+ const auto& name = names[i];
if (count > 0) {
slp::println(" ↳ {} {}", count, name);
}

22
upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch Normal file
View File

@@ -0,0 +1,22 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Mon, 10 Feb 2025 11:37:02 -0800
Subject: [PATCH 6/8] Suppress clang-tidy false positives
---
include/sleipnir/autodiff/variable.hpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp
index 9f79a82763213dc712cce4c2a322289d57645032..17e7eb7cc2c7c7599eaba97d8ec80972524c1599 100644
--- a/include/sleipnir/autodiff/variable.hpp
+++ b/include/sleipnir/autodiff/variable.hpp
@@ -626,7 +626,7 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints {
* @param inequality_constraints The list of InequalityConstraints to
* concatenate.
*/
- InequalityConstraints(
+ InequalityConstraints( // NOLINT
std::initializer_list<InequalityConstraints> inequality_constraints) {
for (const auto& elem : inequality_constraints) {
constraints.insert(constraints.end(), elem.constraints.begin(),

34
upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch Normal file
View File

@@ -0,0 +1,34 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Mon, 24 Feb 2025 15:12:03 -0800
Subject: [PATCH 7/8] Suppress GCC 12 warning false positive
---
include/sleipnir/autodiff/variable_matrix.hpp | 7 +++++++
1 file changed, 7 insertions(+)
diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
index 1c6f9e8dade8bebce7aec18bbb9b5491acb1d977..dee43f926d304e1f4900bd57b99cd613e808f58e 100644
--- a/include/sleipnir/autodiff/variable_matrix.hpp
+++ b/include/sleipnir/autodiff/variable_matrix.hpp
@@ -573,6 +573,10 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
VariableMatrix result(VariableMatrix::empty, lhs.rows(), rhs.cols());
+#if __GNUC__ >= 12
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
+#endif
for (int i = 0; i < lhs.rows(); ++i) {
for (int j = 0; j < rhs.cols(); ++j) {
Variable sum;
@@ -590,6 +594,9 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
result[i, j] = sum;
}
}
+#if __GNUC__ >= 12
+#pragma GCC diagnostic pop
+#endif
return result;
}

915
upstream_utils/sleipnir_patches/0008-Revert-Use-multidimensional-array-subscript-operator.patch Normal file
View File

@@ -0,0 +1,915 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sat, 12 Apr 2025 16:28:47 -0700
Subject: [PATCH 8/8] Revert "Use multidimensional array subscript operator
(#843)"
This reverts commit f9b2c450bbbf6f14b194b8b81708d032a6431ee0.
---
include/sleipnir/autodiff/hessian.hpp | 4 +-
include/sleipnir/autodiff/jacobian.hpp | 4 +-
include/sleipnir/autodiff/variable.hpp | 26 +----
include/sleipnir/autodiff/variable_block.hpp | 70 +++++------
include/sleipnir/autodiff/variable_matrix.hpp | 110 ++++++------------
include/sleipnir/control/ocp.hpp | 14 +--
include/sleipnir/optimization/problem.hpp | 6 +-
src/autodiff/variable_matrix.cpp | 66 +++++------
8 files changed, 118 insertions(+), 182 deletions(-)
diff --git a/include/sleipnir/autodiff/hessian.hpp b/include/sleipnir/autodiff/hessian.hpp
index 4ad097a8117dac47566a3c6896d281004147be70..8b048ab3ba0d671397cfdadcd137ac67bef1b441 100644
--- a/include/sleipnir/autodiff/hessian.hpp
+++ b/include/sleipnir/autodiff/hessian.hpp
@@ -103,9 +103,9 @@ class SLEIPNIR_DLLEXPORT Hessian {
auto grad = m_graphs[row].generate_gradient_tree(m_wrt);
for (int col = 0; col < m_wrt.rows(); ++col) {
if (grad[col].expr != nullptr) {
- result[row, col] = std::move(grad[col]);
+ result(row, col) = std::move(grad[col]);
} else {
- result[row, col] = Variable{0.0};
+ result(row, col) = Variable{0.0};
}
}
}
diff --git a/include/sleipnir/autodiff/jacobian.hpp b/include/sleipnir/autodiff/jacobian.hpp
index 787fca8ccd3fd6e46c5d31ab980704e6a5e99402..7e7e1340d065d35412f43b27fac7d8a719b7e5b5 100644
--- a/include/sleipnir/autodiff/jacobian.hpp
+++ b/include/sleipnir/autodiff/jacobian.hpp
@@ -95,9 +95,9 @@ class SLEIPNIR_DLLEXPORT Jacobian {
auto grad = m_graphs[row].generate_gradient_tree(m_wrt);
for (int col = 0; col < m_wrt.rows(); ++col) {
if (grad[col].expr != nullptr) {
- result[row, col] = std::move(grad[col]);
+ result(row, col) = std::move(grad[col]);
} else {
- result[row, col] = Variable{0.0};
+ result(row, col) = Variable{0.0};
}
}
}
diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp
index 17e7eb7cc2c7c7599eaba97d8ec80972524c1599..03b929c778c03186cc5b461a2e855da23034457a 100644
--- a/include/sleipnir/autodiff/variable.hpp
+++ b/include/sleipnir/autodiff/variable.hpp
@@ -505,11 +505,7 @@ gch::small_vector<Variable> make_constraints(LHS&& lhs, RHS&& rhs) {
for (int row = 0; row < rhs.rows(); ++row) {
for (int col = 0; col < rhs.cols(); ++col) {
// Make right-hand side zero
- if constexpr (EigenMatrixLike<std::decay_t<RHS>>) {
- constraints.emplace_back(lhs - rhs(row, col));
- } else {
- constraints.emplace_back(lhs - rhs[row, col]);
- }
+ constraints.emplace_back(lhs - rhs(row, col));
}
}
} else if constexpr (MatrixLike<LHS> && ScalarLike<RHS>) {
@@ -518,11 +514,7 @@ gch::small_vector<Variable> make_constraints(LHS&& lhs, RHS&& rhs) {
for (int row = 0; row < lhs.rows(); ++row) {
for (int col = 0; col < lhs.cols(); ++col) {
// Make right-hand side zero
- if constexpr (EigenMatrixLike<std::decay_t<LHS>>) {
- constraints.emplace_back(lhs(row, col) - rhs);
- } else {
- constraints.emplace_back(lhs[row, col] - rhs);
- }
+ constraints.emplace_back(lhs(row, col) - rhs);
}
}
} else if constexpr (MatrixLike<LHS> && MatrixLike<RHS>) {
@@ -532,19 +524,7 @@ gch::small_vector<Variable> make_constraints(LHS&& lhs, RHS&& rhs) {
for (int row = 0; row < lhs.rows(); ++row) {
for (int col = 0; col < lhs.cols(); ++col) {
// Make right-hand side zero
- if constexpr (EigenMatrixLike<std::decay_t<LHS>> &&
- EigenMatrixLike<std::decay_t<RHS>>) {
- constraints.emplace_back(lhs(row, col) - rhs(row, col));
- } else if constexpr (EigenMatrixLike<std::decay_t<LHS>> &&
- SleipnirMatrixLike<std::decay_t<RHS>>) {
- constraints.emplace_back(lhs(row, col) - rhs[row, col]);
- } else if constexpr (SleipnirMatrixLike<std::decay_t<LHS>> &&
- EigenMatrixLike<std::decay_t<RHS>>) {
- constraints.emplace_back(lhs[row, col] - rhs(row, col));
- } else if constexpr (SleipnirMatrixLike<std::decay_t<LHS>> &&
- SleipnirMatrixLike<std::decay_t<RHS>>) {
- constraints.emplace_back(lhs[row, col] - rhs[row, col]);
- }
+ constraints.emplace_back(lhs(row, col) - rhs(row, col));
}
}
}
diff --git a/include/sleipnir/autodiff/variable_block.hpp b/include/sleipnir/autodiff/variable_block.hpp
index f1c1ca0dc3fde663c3e74f6fca4b89b119cf377d..632d44beb5b3dae29b9829c52a6168fee39fe537 100644
--- a/include/sleipnir/autodiff/variable_block.hpp
+++ b/include/sleipnir/autodiff/variable_block.hpp
@@ -50,7 +50,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] = values[row, col];
+ (*this)(row, col) = values(row, col);
}
}
}
@@ -85,7 +85,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] = values[row, col];
+ (*this)(row, col) = values(row, col);
}
}
}
@@ -152,7 +152,7 @@ class VariableBlock {
VariableBlock<Mat>& operator=(ScalarLike auto value) {
slp_assert(rows() == 1 && cols() == 1);
- (*this)[0, 0] = value;
+ (*this)(0, 0) = value;
return *this;
}
@@ -167,7 +167,7 @@ class VariableBlock {
void set_value(double value) {
slp_assert(rows() == 1 && cols() == 1);
- (*this)[0, 0].set_value(value);
+ (*this)(0, 0).set_value(value);
}
/**
@@ -182,7 +182,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] = values(row, col);
+ (*this)(row, col) = values(row, col);
}
}
@@ -201,7 +201,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col].set_value(values(row, col));
+ (*this)(row, col).set_value(values(row, col));
}
}
}
@@ -217,7 +217,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] = values[row, col];
+ (*this)(row, col) = values(row, col);
}
}
return *this;
@@ -234,7 +234,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] = std::move(values[row, col]);
+ (*this)(row, col) = std::move(values(row, col));
}
}
return *this;
@@ -247,13 +247,13 @@ class VariableBlock {
* @param col The scalar subblock's column.
* @return A scalar subblock at the given row and column.
*/
- Variable& operator[](int row, int col)
+ Variable& operator()(int row, int col)
requires(!std::is_const_v<Mat>)
{
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
- return (*m_mat)[m_row_slice.start + row * m_row_slice.step,
- m_col_slice.start + col * m_col_slice.step];
+ return (*m_mat)(m_row_slice.start + row * m_row_slice.step,
+ m_col_slice.start + col * m_col_slice.step);
}
/**
@@ -263,11 +263,11 @@ class VariableBlock {
* @param col The scalar subblock's column.
* @return A scalar subblock at the given row and column.
*/
- const Variable& operator[](int row, int col) const {
+ const Variable& operator()(int row, int col) const {
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
- return (*m_mat)[m_row_slice.start + row * m_row_slice.step,
- m_col_slice.start + col * m_col_slice.step];
+ return (*m_mat)(m_row_slice.start + row * m_row_slice.step,
+ m_col_slice.start + col * m_col_slice.step);
}
/**
@@ -280,7 +280,7 @@ class VariableBlock {
requires(!std::is_const_v<Mat>)
{
slp_assert(row >= 0 && row < rows() * cols());
- return (*this)[row / cols(), row % cols()];
+ return (*this)(row / cols(), row % cols());
}
/**
@@ -291,7 +291,7 @@ class VariableBlock {
*/
const Variable& operator[](int row) const {
slp_assert(row >= 0 && row < rows() * cols());
- return (*this)[row / cols(), row % cols()];
+ return (*this)(row / cols(), row % cols());
}
/**
@@ -309,8 +309,8 @@ class VariableBlock {
slp_assert(col_offset >= 0 && col_offset <= cols());
slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset);
slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset);
- return (*this)[Slice{row_offset, row_offset + block_rows, 1},
- Slice{col_offset, col_offset + block_cols, 1}];
+ return (*this)({row_offset, row_offset + block_rows, 1},
+ {col_offset, col_offset + block_cols, 1});
}
/**
@@ -328,8 +328,8 @@ class VariableBlock {
slp_assert(col_offset >= 0 && col_offset <= cols());
slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset);
slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset);
- return (*this)[Slice{row_offset, row_offset + block_rows, 1},
- Slice{col_offset, col_offset + block_cols, 1}];
+ return (*this)({row_offset, row_offset + block_rows, 1},
+ {col_offset, col_offset + block_cols, 1});
}
/**
@@ -339,7 +339,7 @@ class VariableBlock {
* @param col_slice The column slice.
* @return A slice of the variable matrix.
*/
- VariableBlock<Mat> operator[](Slice row_slice, Slice col_slice) {
+ VariableBlock<Mat> operator()(Slice row_slice, Slice col_slice) {
int row_slice_length = row_slice.adjust(m_row_slice_length);
int col_slice_length = col_slice.adjust(m_col_slice_length);
return VariableBlock{
@@ -359,7 +359,7 @@ class VariableBlock {
* @param col_slice The column slice.
* @return A slice of the variable matrix.
*/
- const VariableBlock<const Mat> operator[](Slice row_slice,
+ const VariableBlock<const Mat> operator()(Slice row_slice,
Slice col_slice) const {
int row_slice_length = row_slice.adjust(m_row_slice_length);
int col_slice_length = col_slice.adjust(m_col_slice_length);
@@ -385,7 +385,7 @@ class VariableBlock {
* @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,
+ VariableBlock<Mat> operator()(Slice row_slice, int row_slice_length,
Slice col_slice, int col_slice_length) {
return VariableBlock{
*m_mat,
@@ -409,7 +409,7 @@ class VariableBlock {
* @param col_slice_length The column slice length.
* @return A slice of the variable matrix.
*/
- const VariableBlock<const Mat> operator[](Slice row_slice,
+ const VariableBlock<const Mat> operator()(Slice row_slice,
int row_slice_length,
Slice col_slice,
int col_slice_length) const {
@@ -524,7 +524,7 @@ class VariableBlock {
VariableBlock<Mat>& operator*=(const ScalarLike auto& rhs) {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] *= rhs;
+ (*this)(row, col) *= rhs;
}
}
@@ -542,7 +542,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] /= rhs[0, 0];
+ (*this)(row, col) /= rhs(0, 0);
}
}
@@ -558,7 +558,7 @@ class VariableBlock {
VariableBlock<Mat>& operator/=(const ScalarLike auto& rhs) {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] /= rhs;
+ (*this)(row, col) /= rhs;
}
}
@@ -576,7 +576,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] += rhs[row, col];
+ (*this)(row, col) += rhs(row, col);
}
}
@@ -594,7 +594,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] += rhs;
+ (*this)(row, col) += rhs;
}
}
@@ -612,7 +612,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] -= rhs[row, col];
+ (*this)(row, col) -= rhs(row, col);
}
}
@@ -630,7 +630,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] -= rhs;
+ (*this)(row, col) -= rhs;
}
}
@@ -655,7 +655,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- result[col, row] = (*this)[row, col];
+ result(col, row) = (*this)(row, col);
}
}
@@ -686,8 +686,8 @@ class VariableBlock {
double value(int row, int col) {
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
- return (*m_mat)[m_row_slice.start + row * m_row_slice.step,
- m_col_slice.start + col * m_col_slice.step]
+ return (*m_mat)(m_row_slice.start + row * m_row_slice.step,
+ m_col_slice.start + col * m_col_slice.step)
.value();
}
@@ -731,7 +731,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- result[row, col] = unary_op((*this)[row, col]);
+ result(row, col) = unary_op((*this)(row, col));
}
}
diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
index dee43f926d304e1f4900bd57b99cd613e808f58e..4dc2cea00cb9491035a9b4795be3562186991c7a 100644
--- a/include/sleipnir/autodiff/variable_matrix.hpp
+++ b/include/sleipnir/autodiff/variable_matrix.hpp
@@ -211,7 +211,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < values.rows(); ++row) {
for (int col = 0; col < values.cols(); ++col) {
- (*this)[row, col] = values(row, col);
+ (*this)(row, col) = values(row, col);
}
}
@@ -229,7 +229,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
VariableMatrix& operator=(ScalarLike auto value) {
slp_assert(rows() == 1 && cols() == 1);
- (*this)[0, 0] = value;
+ (*this)(0, 0) = value;
return *this;
}
@@ -246,7 +246,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < values.rows(); ++row) {
for (int col = 0; col < values.cols(); ++col) {
- (*this)[row, col].set_value(values(row, col));
+ (*this)(row, col).set_value(values(row, col));
}
}
}
@@ -280,7 +280,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
m_storage.reserve(rows() * cols());
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- m_storage.emplace_back(values[row, col]);
+ m_storage.emplace_back(values(row, col));
}
}
}
@@ -295,7 +295,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
m_storage.reserve(rows() * cols());
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- m_storage.emplace_back(values[row, col]);
+ m_storage.emplace_back(values(row, col));
}
}
}
@@ -340,7 +340,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
* @param col The block column.
* @return A block pointing to the given row and column.
*/
- Variable& operator[](int row, int col) {
+ Variable& operator()(int row, int col) {
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
return m_storage[row * cols() + col];
@@ -353,7 +353,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
* @param col The block column.
* @return A block pointing to the given row and column.
*/
- const Variable& operator[](int row, int col) const {
+ const Variable& operator()(int row, int col) const {
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
return m_storage[row * cols() + col];
@@ -426,7 +426,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
* @param col_slice The column slice.
* @return A slice of the variable matrix.
*/
- VariableBlock<VariableMatrix> operator[](Slice row_slice, Slice col_slice) {
+ VariableBlock<VariableMatrix> operator()(Slice row_slice, Slice col_slice) {
int row_slice_length = row_slice.adjust(rows());
int col_slice_length = col_slice.adjust(cols());
return VariableBlock{*this, std::move(row_slice), row_slice_length,
@@ -440,7 +440,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
* @param col_slice The column slice.
* @return A slice of the variable matrix.
*/
- const VariableBlock<const VariableMatrix> operator[](Slice row_slice,
+ const VariableBlock<const VariableMatrix> operator()(Slice row_slice,
Slice col_slice) const {
int row_slice_length = row_slice.adjust(rows());
int col_slice_length = col_slice.adjust(cols());
@@ -461,7 +461,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
* @return A slice of the variable matrix.
*
*/
- VariableBlock<VariableMatrix> operator[](Slice row_slice,
+ VariableBlock<VariableMatrix> operator()(Slice row_slice,
int row_slice_length,
Slice col_slice,
int col_slice_length) {
@@ -481,7 +481,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
* @param col_slice_length The column slice length.
* @return A slice of the variable matrix.
*/
- const VariableBlock<const VariableMatrix> operator[](
+ const VariableBlock<const VariableMatrix> operator()(
Slice row_slice, int row_slice_length, Slice col_slice,
int col_slice_length) const {
return VariableBlock{*this, std::move(row_slice), row_slice_length,
@@ -581,17 +581,9 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int j = 0; j < rhs.cols(); ++j) {
Variable sum;
for (int k = 0; k < lhs.cols(); ++k) {
- if constexpr (SleipnirMatrixLike<LHS> && SleipnirMatrixLike<RHS>) {
- sum += lhs[i, k] * rhs[k, j];
- } else if constexpr (SleipnirMatrixLike<LHS> &&
- EigenMatrixLike<RHS>) {
- sum += lhs[i, k] * rhs(k, j);
- } else if constexpr (EigenMatrixLike<LHS> &&
- SleipnirMatrixLike<RHS>) {
- sum += lhs(i, k) * rhs[k, j];
- }
+ sum += lhs(i, k) * rhs(k, j);
}
- result[i, j] = sum;
+ result(i, j) = sum;
}
}
#if __GNUC__ >= 12
@@ -613,7 +605,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
- result[row, col] = lhs[row, col] * rhs;
+ result(row, col) = lhs(row, col) * rhs;
}
}
@@ -632,11 +624,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
- if constexpr (SleipnirMatrixLike<decltype(lhs)>) {
- result[row, col] = lhs[row, col] * rhs;
- } else {
- result[row, col] = lhs(row, col) * rhs;
- }
+ result(row, col) = lhs(row, col) * rhs;
}
}
@@ -655,7 +643,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
- result[row, col] = rhs[row, col] * lhs;
+ result(row, col) = rhs(row, col) * lhs;
}
}
@@ -674,11 +662,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
- if constexpr (SleipnirMatrixLike<decltype(rhs)>) {
- result[row, col] = rhs[row, col] * lhs;
- } else {
- result[row, col] = rhs(row, col) * lhs;
- }
+ result(row, col) = rhs(row, col) * lhs;
}
}
@@ -698,13 +682,9 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int j = 0; j < rhs.cols(); ++j) {
Variable sum;
for (int k = 0; k < cols(); ++k) {
- if constexpr (SleipnirMatrixLike<decltype(rhs)>) {
- sum += (*this)[i, k] * rhs[k, j];
- } else {
- sum += (*this)[i, k] * rhs(k, j);
- }
+ sum += (*this)(i, k) * rhs(k, j);
}
- (*this)[i, j] = sum;
+ (*this)(i, j) = sum;
}
}
@@ -720,7 +700,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
VariableMatrix& operator*=(const ScalarLike auto& rhs) {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < rhs.cols(); ++col) {
- (*this)[row, col] *= rhs;
+ (*this)(row, col) *= rhs;
}
}
@@ -740,11 +720,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
- if constexpr (SleipnirMatrixLike<decltype(lhs)>) {
- result[row, col] = lhs[row, col] / rhs;
- } else {
- result[row, col] = lhs(row, col) / rhs;
- }
+ result(row, col) = lhs(row, col) / rhs;
}
}
@@ -760,7 +736,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
VariableMatrix& operator/=(const ScalarLike auto& rhs) {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] /= rhs;
+ (*this)(row, col) /= rhs;
}
}
@@ -784,13 +760,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
- if constexpr (SleipnirMatrixLike<LHS> && SleipnirMatrixLike<RHS>) {
- result[row, col] = lhs[row, col] + rhs[row, col];
- } else if constexpr (SleipnirMatrixLike<LHS> && EigenMatrixLike<RHS>) {
- result[row, col] = lhs[row, col] + rhs(row, col);
- } else if constexpr (EigenMatrixLike<LHS> && SleipnirMatrixLike<RHS>) {
- result[row, col] = lhs(row, col) + rhs[row, col];
- }
+ result(row, col) = lhs(row, col) + rhs(row, col);
}
}
@@ -808,11 +778,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- if constexpr (SleipnirMatrixLike<decltype(rhs)>) {
- (*this)[row, col] += rhs[row, col];
- } else {
- (*this)[row, col] += rhs(row, col);
- }
+ (*this)(row, col) += rhs(row, col);
}
}
@@ -830,7 +796,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] += rhs;
+ (*this)(row, col) += rhs;
}
}
@@ -854,13 +820,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
- if constexpr (SleipnirMatrixLike<LHS> && SleipnirMatrixLike<RHS>) {
- result[row, col] = lhs[row, col] - rhs[row, col];
- } else if constexpr (SleipnirMatrixLike<LHS> && EigenMatrixLike<RHS>) {
- result[row, col] = lhs[row, col] - rhs(row, col);
- } else if constexpr (EigenMatrixLike<LHS> && SleipnirMatrixLike<RHS>) {
- result[row, col] = lhs(row, col) - rhs[row, col];
- }
+ result(row, col) = lhs(row, col) - rhs(row, col);
}
}
@@ -878,11 +838,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- if constexpr (SleipnirMatrixLike<decltype(rhs)>) {
- (*this)[row, col] -= rhs[row, col];
- } else {
- (*this)[row, col] -= rhs(row, col);
- }
+ (*this)(row, col) -= rhs(row, col);
}
}
@@ -900,7 +856,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] -= rhs;
+ (*this)(row, col) -= rhs;
}
}
@@ -918,7 +874,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
- result[row, col] = -lhs[row, col];
+ result(row, col) = -lhs(row, col);
}
}
@@ -930,7 +886,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
*/
operator Variable() const { // NOLINT
slp_assert(rows() == 1 && cols() == 1);
- return (*this)[0, 0];
+ return (*this)(0, 0);
}
/**
@@ -943,7 +899,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- result[col, row] = (*this)[row, col];
+ result(col, row) = (*this)(row, col);
}
}
@@ -1017,7 +973,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- result[row, col] = unary_op((*this)[row, col]);
+ result(row, col) = unary_op((*this)(row, col));
}
}
@@ -1199,7 +1155,7 @@ SLEIPNIR_DLLEXPORT inline VariableMatrix cwise_reduce(
for (int row = 0; row < lhs.rows(); ++row) {
for (int col = 0; col < lhs.cols(); ++col) {
- result[row, col] = binary_op(lhs[row, col], rhs[row, col]);
+ result(row, col) = binary_op(lhs(row, col), rhs(row, col));
}
}
diff --git a/include/sleipnir/control/ocp.hpp b/include/sleipnir/control/ocp.hpp
index 282520fb852d8588b96846eb5b4952bf47d1309f..d9174426669281e68a5c09d298cfd5bcd3be3776 100644
--- a/include/sleipnir/control/ocp.hpp
+++ b/include/sleipnir/control/ocp.hpp
@@ -180,7 +180,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
if (m_timestep_method == TimestepMethod::FIXED) {
m_DT = VariableMatrix{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) {
- m_DT[0, i] = m_dt.count();
+ m_DT(0, i) = m_dt.count();
}
} else if (m_timestep_method == TimestepMethod::VARIABLE_SINGLE) {
Variable dt = decision_variable();
@@ -189,12 +189,12 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
// Set the member variable matrix to track the decision variable
m_DT = VariableMatrix{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) {
- m_DT[0, i] = dt;
+ m_DT(0, i) = dt;
}
} else if (m_timestep_method == TimestepMethod::VARIABLE) {
m_DT = decision_variable(1, m_num_steps + 1);
for (int i = 0; i < num_steps + 1; ++i) {
- m_DT[0, i].set_value(m_dt.count());
+ m_DT(0, i).set_value(m_dt.count());
}
}
@@ -270,7 +270,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
for (int i = 0; i < m_num_steps + 1; ++i) {
auto x = X().col(i);
auto u = U().col(i);
- auto dt = this->dt()[0, i];
+ auto dt = this->dt()(0, i);
callback(time, x, u, dt);
time += dt;
@@ -377,7 +377,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
// Derivation at https://mec560sbu.github.io/2016/09/30/direct_collocation/
for (int i = 0; i < m_num_steps; ++i) {
- Variable h = dt()[0, i];
+ Variable h = dt()(0, i);
auto& f = m_dynamics_function;
@@ -412,7 +412,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
auto x_begin = X().col(i);
auto x_end = X().col(i + 1);
auto u = U().col(i);
- Variable dt = this->dt()[0, i];
+ Variable dt = this->dt()(0, i);
if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
subject_to(x_end == rk4<const decltype(m_dynamics_function)&,
@@ -433,7 +433,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
auto x_begin = X().col(i);
auto x_end = X().col(i + 1);
auto u = U().col(i);
- Variable dt = this->dt()[0, i];
+ Variable dt = this->dt()(0, i);
if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
x_end = rk4<const decltype(m_dynamics_function)&, VariableMatrix,
diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
index b7a868657c704487049efaf6b3972b1f7b72bfb4..b484ec08d6c50bf42fbaa1d5b4c66a20cb11a922 100644
--- a/include/sleipnir/optimization/problem.hpp
+++ b/include/sleipnir/optimization/problem.hpp
@@ -78,7 +78,7 @@ class SLEIPNIR_DLLEXPORT Problem {
for (int row = 0; row < rows; ++row) {
for (int col = 0; col < cols; ++col) {
m_decision_variables.emplace_back();
- vars[row, col] = m_decision_variables.back();
+ vars(row, col) = m_decision_variables.back();
}
}
@@ -113,8 +113,8 @@ class SLEIPNIR_DLLEXPORT Problem {
for (int row = 0; row < rows; ++row) {
for (int col = 0; col <= row; ++col) {
m_decision_variables.emplace_back();
- vars[row, col] = m_decision_variables.back();
- vars[col, row] = m_decision_variables.back();
+ vars(row, col) = m_decision_variables.back();
+ vars(col, row) = m_decision_variables.back();
}
}
diff --git a/src/autodiff/variable_matrix.cpp b/src/autodiff/variable_matrix.cpp
index decdc70809189d309708774ec60603fe73c50ecc..71f8153d345750d79fa41cf7af14ac766fcad2a4 100644
--- a/src/autodiff/variable_matrix.cpp
+++ b/src/autodiff/variable_matrix.cpp
@@ -12,17 +12,17 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
if (A.rows() == 1 && A.cols() == 1) {
// Compute optimal inverse instead of using Eigen's general solver
- return B[0, 0] / A[0, 0];
+ return B(0, 0) / A(0, 0);
} else if (A.rows() == 2 && A.cols() == 2) {
// Compute optimal inverse instead of using Eigen's general solver
//
// [a b]⁻¹ ___1___ [ d −b]
// [c d] = ad − bc [−c a]
- const auto& a = A[0, 0];
- const auto& b = A[0, 1];
- const auto& c = A[1, 0];
- const auto& d = A[1, 1];
+ const auto& a = A(0, 0);
+ const auto& b = A(0, 1);
+ const auto& c = A(1, 0);
+ const auto& d = A(1, 1);
slp::VariableMatrix adj_A{{d, -b}, {-c, a}};
auto det_A = a * d - b * c;
@@ -39,15 +39,15 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
//
// https://www.wolframalpha.com/input?i=inverse+%7B%7Ba%2C+b%2C+c%7D%2C+%7Bd%2C+e%2C+f%7D%2C+%7Bg%2C+h%2C+i%7D%7D
- const auto& a = A[0, 0];
- const auto& b = A[0, 1];
- const auto& c = A[0, 2];
- const auto& d = A[1, 0];
- const auto& e = A[1, 1];
- const auto& f = A[1, 2];
- const auto& g = A[2, 0];
- const auto& h = A[2, 1];
- const auto& i = A[2, 2];
+ const auto& a = A(0, 0);
+ const auto& b = A(0, 1);
+ const auto& c = A(0, 2);
+ const auto& d = A(1, 0);
+ const auto& e = A(1, 1);
+ const auto& f = A(1, 2);
+ const auto& g = A(2, 0);
+ const auto& h = A(2, 1);
+ const auto& i = A(2, 2);
auto ae = a * e;
auto af = a * f;
@@ -87,22 +87,22 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
//
// https://www.wolframalpha.com/input?i=inverse+%7B%7Ba%2C+b%2C+c%2C+d%7D%2C+%7Be%2C+f%2C+g%2C+h%7D%2C+%7Bi%2C+j%2C+k%2C+l%7D%2C+%7Bm%2C+n%2C+o%2C+p%7D%7D
- const auto& a = A[0, 0];
- const auto& b = A[0, 1];
- const auto& c = A[0, 2];
- const auto& d = A[0, 3];
- const auto& e = A[1, 0];
- const auto& f = A[1, 1];
- const auto& g = A[1, 2];
- const auto& h = A[1, 3];
- const auto& i = A[2, 0];
- const auto& j = A[2, 1];
- const auto& k = A[2, 2];
- const auto& l = A[2, 3];
- const auto& m = A[3, 0];
- const auto& n = A[3, 1];
- const auto& o = A[3, 2];
- const auto& p = A[3, 3];
+ const auto& a = A(0, 0);
+ const auto& b = A(0, 1);
+ const auto& c = A(0, 2);
+ const auto& d = A(0, 3);
+ const auto& e = A(1, 0);
+ const auto& f = A(1, 1);
+ const auto& g = A(1, 2);
+ const auto& h = A(1, 3);
+ const auto& i = A(2, 0);
+ const auto& j = A(2, 1);
+ const auto& k = A(2, 2);
+ const auto& l = A(2, 3);
+ const auto& m = A(3, 0);
+ const auto& n = A(3, 1);
+ const auto& o = A(3, 2);
+ const auto& p = A(3, 3);
auto afk = a * f * k;
auto afl = a * f * l;
@@ -232,14 +232,14 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
MatrixXv eigen_A{A.rows(), A.cols()};
for (int row = 0; row < A.rows(); ++row) {
for (int col = 0; col < A.cols(); ++col) {
- eigen_A(row, col) = A[row, col];
+ eigen_A(row, col) = A(row, col);
}
}
MatrixXv eigen_B{B.rows(), B.cols()};
for (int row = 0; row < B.rows(); ++row) {
for (int col = 0; col < B.cols(); ++col) {
- eigen_B(row, col) = B[row, col];
+ eigen_B(row, col) = B(row, col);
}
}
@@ -248,7 +248,7 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
VariableMatrix X{A.cols(), B.cols()};
for (int row = 0; row < X.rows(); ++row) {
for (int col = 0; col < X.cols(); ++col) {
- X[row, col] = eigen_X(row, col);
+ X(row, col) = eigen_X(row, col);
}
}