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

[upstream_utils] Upgrade Sleipnir (#8235)

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

4
upstream_utils/sleipnir.py
View File

@@ -52,8 +52,8 @@ using small_vector = wpi::SmallVector<T>;
def main():
name = "sleipnir"
url = "https://github.com/SleipnirGroup/Sleipnir"
# main on 2025-05-18
tag = "2cc18ff6d25ee0a9bd0f9993a0a41a61a28bda3e"
# main on 2025-09-19
tag = "7f89d5547702a09e3617bc31fe5bafe6add04fab"
sleipnir = Lib(name, url, tag, copy_upstream_src)
sleipnir.main()

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

@@ -4,10 +4,12 @@ Date: Wed, 29 May 2024 16:29:55 -0700
Subject: [PATCH 1/8] Use fmtlib
---
include/.styleguide | 1 +
include/sleipnir/util/print.hpp | 31 ++++++++++++++++++-------------
src/optimization/problem.cpp | 2 +-
3 files changed, 20 insertions(+), 14 deletions(-)
include/.styleguide | 1 +
include/sleipnir/util/assert.hpp | 5 +++--
include/sleipnir/util/print.hpp | 31 ++++++++++++++++++-------------
src/.styleguide | 1 +
src/optimization/problem.cpp | 1 +
5 files changed, 24 insertions(+), 15 deletions(-)
diff --git a/include/.styleguide b/include/.styleguide
index 1b6652d3d5886cf8c9eca0d855c21031775bad7c..4f4c76204071f90bf49eddb8c2aceb583b5e09ba 100644
@@ -20,6 +22,31 @@ index 1b6652d3d5886cf8c9eca0d855c21031775bad7c..4f4c76204071f90bf49eddb8c2aceb58
+ ^fmt/
^gch/
}
diff --git a/include/sleipnir/util/assert.hpp b/include/sleipnir/util/assert.hpp
index 75d8ffca32accbf66ffce30f073de1db2f42469b..53de01928b929793fa77885ec4a6d1a928bdc5a9 100644
--- a/include/sleipnir/util/assert.hpp
+++ b/include/sleipnir/util/assert.hpp
@@ -3,9 +3,10 @@
#pragma once
#ifdef JORMUNGANDR
-#include <format>
#include <source_location>
#include <stdexcept>
+
+#include <fmt/format.h>
/**
* Throw an exception in Python.
*/
@@ -13,7 +14,7 @@
do { \
if (!(condition)) { \
auto location = std::source_location::current(); \
- throw std::invalid_argument(std::format( \
+ throw std::invalid_argument(fmt::format( \
"{}:{}: {}: Assertion `{}' failed.", location.file_name(), \
location.line(), location.function_name(), #condition)); \
} \
diff --git a/include/sleipnir/util/print.hpp b/include/sleipnir/util/print.hpp
index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b1159aeb2a57 100644
--- a/include/sleipnir/util/print.hpp
@@ -99,16 +126,26 @@ index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b115
} catch (const std::system_error&) {
}
}
diff --git a/src/.styleguide b/src/.styleguide
index 1b6652d3d5886cf8c9eca0d855c21031775bad7c..4f4c76204071f90bf49eddb8c2aceb583b5e09ba 100644
--- a/src/.styleguide
+++ b/src/.styleguide
@@ -8,5 +8,6 @@ cppSrcFileInclude {
includeOtherLibs {
^Eigen/
+ ^fmt/
^gch/
}
diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp
index 31115490867146ec166604bcc61731d7891a9f22..81863808d329a53d4162ce0624a3b8e8afc32dfc 100644
index c3331197e2365934273f57422b79fa18c2b78a5b..09828cdb6d7cddff692b9d17603dc0c11cd5a3ec 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());
}
}
@@ -11,6 +11,7 @@
#include <Eigen/Core>
#include <Eigen/SparseCore>
+#include <fmt/chrono.h>
#include <gch/small_vector.hpp>
#include "optimization/bounds.hpp"

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

@@ -10,7 +10,7 @@ Subject: [PATCH 2/8] Use wpi::SmallVector
3 files changed, 6 insertions(+), 7 deletions(-)
diff --git a/include/sleipnir/autodiff/expression.hpp b/include/sleipnir/autodiff/expression.hpp
index 873e1c27559d92eb1b3a217890ca41bdc65af122..1c5f84d22a0bed70869121acabd527825ba90adb 100644
index bb4d8c5641a5b3d633d372674e0a35f857889cd4..53a5f6d68d3153537840c4ff45fe5e5d8b0076b7 100644
--- a/include/sleipnir/autodiff/expression.hpp
+++ b/include/sleipnir/autodiff/expression.hpp
@@ -30,7 +30,7 @@ inline constexpr bool USE_POOL_ALLOCATOR = true;
@@ -22,7 +22,7 @@ index 873e1c27559d92eb1b3a217890ca41bdc65af122..1c5f84d22a0bed70869121acabd52782
/**
* Typedef for intrusive shared pointer to Expression.
@@ -680,7 +680,7 @@ inline constexpr void inc_ref_count(Expression* expr) {
@@ -733,7 +733,7 @@ inline constexpr void inc_ref_count(Expression* expr) {
*
* @param expr The shared pointer's managed object.
*/
@@ -32,7 +32,7 @@ index 873e1c27559d92eb1b3a217890ca41bdc65af122..1c5f84d22a0bed70869121acabd52782
// Expression destructor when expr's refcount reaches zero can cause a stack
// overflow. Instead, we iterate over its children to decrement their
diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp
index 14eb1d3b95069e143699e1488f3081c4cd9de07c..9f79a82763213dc712cce4c2a322289d57645032 100644
index f60236811eba45c67a9638e90d5101d877ecc2d0..264f0950f293c67d6e6c7e729887090c050e40e2 100644
--- a/include/sleipnir/autodiff/variable.hpp
+++ b/include/sleipnir/autodiff/variable.hpp
@@ -47,7 +47,7 @@ class SLEIPNIR_DLLEXPORT Variable {
@@ -55,7 +55,7 @@ index 14eb1d3b95069e143699e1488f3081c4cd9de07c..9f79a82763213dc712cce4c2a322289d
/**
* Assignment operator for double.
diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
index 410f12873cfdf5d0d484653c6c3dac74ed96348a..1c6f9e8dade8bebce7aec18bbb9b5491acb1d977 100644
index e1a419ca5356660b3c1c27230d1cb2a86977fb65..349a1550235516f9853609b61feded834ef2894b 100644
--- a/include/sleipnir/autodiff/variable_matrix.hpp
+++ b/include/sleipnir/autodiff/variable_matrix.hpp
@@ -1120,14 +1120,14 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {

6
upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch
View File

@@ -9,7 +9,7 @@ Subject: [PATCH 4/8] Replace std::to_underlying()
2 files changed, 7 insertions(+), 8 deletions(-)
diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp
index 81863808d329a53d4162ce0624a3b8e8afc32dfc..c3319fc0a927cf452871a2db08d5edff87ac8eea 100644
index 09828cdb6d7cddff692b9d17603dc0c11cd5a3ec..886de24cc0532d31f1e186150da79e925f212556 100644
--- a/src/optimization/problem.cpp
+++ b/src/optimization/problem.cpp
@@ -7,7 +7,6 @@
@@ -20,7 +20,7 @@ index 81863808d329a53d4162ce0624a3b8e8afc32dfc..c3319fc0a927cf452871a2db08d5edff
#include <Eigen/Core>
#include <Eigen/SparseCore>
@@ -346,11 +345,11 @@ void Problem::print_problem_analysis() {
@@ -350,11 +349,11 @@ void Problem::print_problem_analysis() {
// Print problem structure
slp::println("\nProblem structure:");
slp::println(" ↳ {} cost function",
@@ -35,7 +35,7 @@ index 81863808d329a53d4162ce0624a3b8e8afc32dfc..c3319fc0a927cf452871a2db08d5edff
if (m_decision_variables.size() == 1) {
slp::print("\n1 decision variable\n");
@@ -362,7 +361,7 @@ void Problem::print_problem_analysis() {
@@ -366,7 +365,7 @@ void Problem::print_problem_analysis() {
[](const gch::small_vector<Variable>& constraints) {
std::array<size_t, 5> counts{};
for (const auto& constraint : constraints) {

8
upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch
View File

@@ -9,10 +9,10 @@ Subject: [PATCH 5/8] Replace std::views::zip()
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
index 33b6eee615141a1d6472f116842d62052ef54dd9..b333aebd3e59fa23eed6046c13d736c3d2eccac7 100644
--- a/include/sleipnir/autodiff/adjoint_expression_graph.hpp
+++ b/include/sleipnir/autodiff/adjoint_expression_graph.hpp
@@ -155,7 +155,10 @@ class AdjointExpressionGraph {
@@ -158,7 +158,10 @@ class AdjointExpressionGraph {
}
}
} else {
@@ -25,7 +25,7 @@ index 4b4f3303faed766d3ac39829870514f50d9a582f..4576e19c9695caf4407fbbb592afe32d
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
index 886de24cc0532d31f1e186150da79e925f212556..e32481e9314c9ef472843adb5bedbd993627d5d9 100644
--- a/src/optimization/problem.cpp
+++ b/src/optimization/problem.cpp
@@ -6,7 +6,6 @@
@@ -36,7 +36,7 @@ index c3319fc0a927cf452871a2db08d5edff87ac8eea..5532b3962409e2140132e79241da4fba
#include <Eigen/Core>
#include <Eigen/SparseCore>
@@ -363,9 +362,11 @@ void Problem::print_problem_analysis() {
@@ -367,9 +366,11 @@ void Problem::print_problem_analysis() {
for (const auto& constraint : constraints) {
++counts[static_cast<uint8_t>(constraint.type())];
}

4
upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch
View File

@@ -8,10 +8,10 @@ Subject: [PATCH 6/8] Suppress clang-tidy false positives
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
index 264f0950f293c67d6e6c7e729887090c050e40e2..62135a5539308ae69f6b45a64d9337c4c3e96d7b 100644
--- a/include/sleipnir/autodiff/variable.hpp
+++ b/include/sleipnir/autodiff/variable.hpp
@@ -626,7 +626,7 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints {
@@ -633,7 +633,7 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints {
* @param inequality_constraints The list of InequalityConstraints to
* concatenate.
*/

2
upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch
View File

@@ -8,7 +8,7 @@ Subject: [PATCH 7/8] Suppress GCC 12 warning false positive
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
index 349a1550235516f9853609b61feded834ef2894b..70bccf4fc078a49e22b6699db1228c765430a121 100644
--- a/include/sleipnir/autodiff/variable_matrix.hpp
+++ b/include/sleipnir/autodiff/variable_matrix.hpp
@@ -573,6 +573,10 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {

74
upstream_utils/sleipnir_patches/0008-Revert-Use-multidimensional-array-subscript-operator.patch
View File

@@ -11,16 +11,16 @@ This reverts commit f9b2c450bbbf6f14b194b8b81708d032a6431ee0.
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/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
index fa6d8af0843eca8b674744f02551584dd8d79c21..4f093b7b39ea84e56c4a12ae1b6f645c4f84a1f0 100644
--- a/include/sleipnir/autodiff/hessian.hpp
+++ b/include/sleipnir/autodiff/hessian.hpp
@@ -103,9 +103,9 @@ class SLEIPNIR_DLLEXPORT Hessian {
@@ -106,9 +106,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) {
@@ -33,10 +33,10 @@ index 4ad097a8117dac47566a3c6896d281004147be70..8b048ab3ba0d671397cfdadcd137ac67
}
}
diff --git a/include/sleipnir/autodiff/jacobian.hpp b/include/sleipnir/autodiff/jacobian.hpp
index 787fca8ccd3fd6e46c5d31ab980704e6a5e99402..7e7e1340d065d35412f43b27fac7d8a719b7e5b5 100644
index 4515076cde12a2112e1b5711acc3092bd807e250..3662b5e49b93f63b5ccac0e732149bd9178f1aae 100644
--- a/include/sleipnir/autodiff/jacobian.hpp
+++ b/include/sleipnir/autodiff/jacobian.hpp
@@ -95,9 +95,9 @@ class SLEIPNIR_DLLEXPORT Jacobian {
@@ -99,9 +99,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) {
@@ -49,10 +49,10 @@ index 787fca8ccd3fd6e46c5d31ab980704e6a5e99402..7e7e1340d065d35412f43b27fac7d8a7
}
}
diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp
index 17e7eb7cc2c7c7599eaba97d8ec80972524c1599..03b929c778c03186cc5b461a2e855da23034457a 100644
index 62135a5539308ae69f6b45a64d9337c4c3e96d7b..2fc2119d2dedaa5b4c941ce449b7fb113c641635 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) {
@@ -512,11 +512,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
@@ -65,7 +65,7 @@ index 17e7eb7cc2c7c7599eaba97d8ec80972524c1599..03b929c778c03186cc5b461a2e855da2
}
}
} else if constexpr (MatrixLike<LHS> && ScalarLike<RHS>) {
@@ -518,11 +514,7 @@ gch::small_vector<Variable> make_constraints(LHS&& lhs, RHS&& rhs) {
@@ -525,11 +521,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
@@ -78,7 +78,7 @@ index 17e7eb7cc2c7c7599eaba97d8ec80972524c1599..03b929c778c03186cc5b461a2e855da2
}
}
} else if constexpr (MatrixLike<LHS> && MatrixLike<RHS>) {
@@ -532,19 +524,7 @@ gch::small_vector<Variable> make_constraints(LHS&& lhs, RHS&& rhs) {
@@ -539,19 +531,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
@@ -376,7 +376,7 @@ index f1c1ca0dc3fde663c3e74f6fca4b89b119cf377d..632d44beb5b3dae29b9829c52a6168fe
}
diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
index dee43f926d304e1f4900bd57b99cd613e808f58e..4dc2cea00cb9491035a9b4795be3562186991c7a 100644
index 70bccf4fc078a49e22b6699db1228c765430a121..2ed997819e70c584ce413f639826b6da506e382b 100644
--- a/include/sleipnir/autodiff/variable_matrix.hpp
+++ b/include/sleipnir/autodiff/variable_matrix.hpp
@@ -211,7 +211,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
@@ -708,35 +708,35 @@ index dee43f926d304e1f4900bd57b99cd613e808f58e..4dc2cea00cb9491035a9b4795be35621
}
}
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) {
diff --git a/include/sleipnir/optimization/ocp.hpp b/include/sleipnir/optimization/ocp.hpp
index 124224cf5ba6e54c141086e3a21389530198449f..74492a0d756a9d587df6158c7e2ef8548ae22be4 100644
--- a/include/sleipnir/optimization/ocp.hpp
+++ b/include/sleipnir/optimization/ocp.hpp
@@ -122,7 +122,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
if (timestep_method == TimestepMethod::FIXED) {
m_DT = VariableMatrix{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) {
- m_DT[0, i] = m_dt.count();
+ m_DT(0, i) = m_dt.count();
- m_DT[0, i] = dt.count();
+ m_DT(0, i) = dt.count();
}
} else if (m_timestep_method == TimestepMethod::VARIABLE_SINGLE) {
Variable dt = decision_variable();
@@ -189,12 +189,12 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
} else if (timestep_method == TimestepMethod::VARIABLE_SINGLE) {
Variable single_dt = decision_variable();
@@ -131,12 +131,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;
- m_DT[0, i] = single_dt;
+ m_DT(0, i) = single_dt;
}
} else if (m_timestep_method == TimestepMethod::VARIABLE) {
} else if (timestep_method == TimestepMethod::VARIABLE) {
m_DT = decision_variable(1, m_num_steps + 1);
for (int i = 0; i < num_steps + 1; ++i) {
- m_DT[0, i].set_value(m_dt.count());
+ m_DT(0, i).set_value(m_dt.count());
- m_DT[0, i].set_value(dt.count());
+ m_DT(0, i).set_value(dt.count());
}
}
@@ -270,7 +270,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
@@ -212,7 +212,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);
@@ -745,16 +745,16 @@ index 282520fb852d8588b96846eb5b4952bf47d1309f..d9174426669281e68a5c09d298cfd5bc
callback(time, x, u, dt);
time += dt;
@@ -377,7 +377,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
@@ -353,7 +353,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;
auto& f = m_dynamics;
@@ -412,7 +412,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
@@ -391,7 +391,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);
@@ -762,8 +762,8 @@ index 282520fb852d8588b96846eb5b4952bf47d1309f..d9174426669281e68a5c09d298cfd5bc
+ 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 {
subject_to(x_end == rk4<const decltype(m_dynamics)&, VariableMatrix,
@@ -415,7 +415,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);
@@ -771,9 +771,9 @@ index 282520fb852d8588b96846eb5b4952bf47d1309f..d9174426669281e68a5c09d298cfd5bc
+ Variable dt = this->dt()(0, i);
if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
x_end = rk4<const decltype(m_dynamics_function)&, VariableMatrix,
x_end = rk4<const decltype(m_dynamics)&, VariableMatrix, VariableMatrix,
diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
index b7a868657c704487049efaf6b3972b1f7b72bfb4..b484ec08d6c50bf42fbaa1d5b4c66a20cb11a922 100644
index efde2006397fb7d8ca24651e9a84b47fc879ee15..c996b372311f708153f8c89ef15fa35a097a6171 100644
--- a/include/sleipnir/optimization/problem.hpp
+++ b/include/sleipnir/optimization/problem.hpp
@@ -78,7 +78,7 @@ class SLEIPNIR_DLLEXPORT Problem {
@@ -797,7 +797,7 @@ index b7a868657c704487049efaf6b3972b1f7b72bfb4..b484ec08d6c50bf42fbaa1d5b4c66a20
}
diff --git a/src/autodiff/variable_matrix.cpp b/src/autodiff/variable_matrix.cpp
index decdc70809189d309708774ec60603fe73c50ecc..71f8153d345750d79fa41cf7af14ac766fcad2a4 100644
index 6c3a040e08bdc5009885e762402a8b44434024c3..d9619a39d583e1a29c46602ba61e881531f57e09 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) {
@@ -821,7 +821,7 @@ index decdc70809189d309708774ec60603fe73c50ecc..71f8153d345750d79fa41cf7af14ac76
+ const auto& c = A(1, 0);
+ const auto& d = A(1, 1);
slp::VariableMatrix adj_A{{d, -b}, {-c, a}};
VariableMatrix adj_A{{d, -b}, {-c, a}};
auto det_A = a * d - b * c;
@@ -39,15 +39,15 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
//
@@ -905,7 +905,7 @@ index decdc70809189d309708774ec60603fe73c50ecc..71f8153d345750d79fa41cf7af14ac76
}
@@ -248,7 +248,7 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
VariableMatrix X{A.cols(), B.cols()};
VariableMatrix X{VariableMatrix::empty, A.cols(), B.cols()};
for (int row = 0; row < X.rows(); ++row) {
for (int col = 0; col < X.cols(); ++col) {
- X[row, col] = eigen_X(row, col);