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
Files
50fd29e6974fc23e5d323dc4e4df02318d97ca48
allwpilib/upstream_utils/sleipnir_patches/0010-Use-operator-instead-of-multidimensional-array-subsc.patch
Tyler Veness f1d95ee5f8 [upstream_utils] Upgrade to Sleipnir 0.6.0 (#8923)
2026-05-27 21:26:42 -07:00

985 lines
36 KiB
Diff
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
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 10/10] Use operator() instead of multidimensional array
subscript operator
---
include/sleipnir/autodiff/hessian.hpp | 4 +-
include/sleipnir/autodiff/jacobian.hpp | 4 +-
include/sleipnir/autodiff/variable.hpp | 8 +-
include/sleipnir/autodiff/variable_block.hpp | 76 ++++-----
include/sleipnir/autodiff/variable_matrix.hpp | 158 +++++++++---------
include/sleipnir/optimization/ocp.hpp | 14 +-
include/sleipnir/optimization/problem.hpp | 6 +-
7 files changed, 135 insertions(+), 135 deletions(-)
diff --git a/include/sleipnir/autodiff/hessian.hpp b/include/sleipnir/autodiff/hessian.hpp
index 2a8087eda23f49f8c01be35d5289568644a9742d..c48e01c7b89d80db8032f6d2e492a0ec1bfa9f42 100644
--- a/include/sleipnir/autodiff/hessian.hpp
+++ b/include/sleipnir/autodiff/hessian.hpp
@@ -100,9 +100,9 @@ class Hessian {
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]);
+ result(row, col) = std::move(grad[col]);
} else {
- result[row, col] = Variable{Scalar(0)};
+ result(row, col) = Variable{Scalar(0)};
}
}
}
diff --git a/include/sleipnir/autodiff/jacobian.hpp b/include/sleipnir/autodiff/jacobian.hpp
index 7a5c3798dab50f14db35bee430f1d5e8e32196ff..1a5b01de99a7c011ebf8c5ed9fc64942675fcb4b 100644
--- a/include/sleipnir/autodiff/jacobian.hpp
+++ b/include/sleipnir/autodiff/jacobian.hpp
@@ -104,9 +104,9 @@ class Jacobian {
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]);
+ result(row, col) = std::move(grad[col]);
} else {
- result[row, col] = Variable{Scalar(0)};
+ result(row, col) = Variable{Scalar(0)};
}
}
}
diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp
index 3d3b355f9fef7d5b6b57521317f67041e0fb1ad6..130a4f057ea08ab0565b54f8a3556bd81fbc6825 100644
--- a/include/sleipnir/autodiff/variable.hpp
+++ b/include/sleipnir/autodiff/variable.hpp
@@ -68,7 +68,7 @@ class Variable : public SleipnirBase {
///
/// @param value The value of the Variable.
// NOLINTNEXTLINE (google-explicit-constructor)
- Variable(SleipnirMatrixLike<Scalar> auto value) : expr{value[0, 0].expr} {
+ Variable(SleipnirMatrixLike<Scalar> auto value) : expr{value(0, 0).expr} {
slp_assert(value.rows() == 1 && value.cols() == 1);
}
@@ -725,7 +725,7 @@ auto 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
- constraints.emplace_back(lhs - rhs[row, col]);
+ constraints.emplace_back(lhs - rhs(row, col));
}
}
@@ -741,7 +741,7 @@ auto 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
- constraints.emplace_back(lhs[row, col] - rhs);
+ constraints.emplace_back(lhs(row, col) - rhs);
}
}
@@ -759,7 +759,7 @@ auto 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
- 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 4018606df45941b578c861caf934495f8c9e368e..cf554832b82adb17b4b1d7b56842a77d7bf629dc 100644
--- a/include/sleipnir/autodiff/variable_block.hpp
+++ b/include/sleipnir/autodiff/variable_block.hpp
@@ -49,7 +49,7 @@ class VariableBlock : public SleipnirBase {
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);
}
}
}
@@ -80,7 +80,7 @@ class VariableBlock : public SleipnirBase {
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);
}
}
}
@@ -135,7 +135,7 @@ class VariableBlock : public SleipnirBase {
VariableBlock<Mat>& operator=(ScalarLike auto value) {
slp_assert(rows() == 1 && cols() == 1);
- (*this)[0, 0] = value;
+ (*this)(0, 0) = value;
return *this;
}
@@ -148,7 +148,7 @@ class VariableBlock : public SleipnirBase {
void set_value(Scalar value) {
slp_assert(rows() == 1 && cols() == 1);
- (*this)[0, 0].set_value(value);
+ (*this)(0, 0).set_value(value);
}
/// Assigns an Eigen matrix to the block.
@@ -161,7 +161,7 @@ class VariableBlock : public SleipnirBase {
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);
}
}
@@ -178,7 +178,7 @@ class VariableBlock : public SleipnirBase {
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));
}
}
}
@@ -192,7 +192,7 @@ class VariableBlock : public SleipnirBase {
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;
@@ -207,7 +207,7 @@ class VariableBlock : public SleipnirBase {
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;
@@ -218,13 +218,13 @@ class VariableBlock : public SleipnirBase {
/// @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)
+ Variable<Scalar>& 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);
}
/// Returns a scalar subblock at the given row and column.
@@ -232,11 +232,11 @@ class VariableBlock : public SleipnirBase {
/// @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 {
+ const Variable<Scalar>& 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);
}
/// Returns a scalar subblock at the given index.
@@ -247,7 +247,7 @@ class VariableBlock : public SleipnirBase {
requires(!std::is_const_v<Mat>)
{
slp_assert(index >= 0 && index < rows() * cols());
- return (*this)[index / cols(), index % cols()];
+ return (*this)(index / cols(), index % cols());
}
/// Returns a scalar subblock at the given index.
@@ -256,7 +256,7 @@ class VariableBlock : public SleipnirBase {
/// @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()];
+ return (*this)(index / cols(), index % cols());
}
/// Returns a block of the variable matrix.
@@ -272,8 +272,8 @@ class VariableBlock : public SleipnirBase {
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});
}
/// Returns a block slice of the variable matrix.
@@ -289,8 +289,8 @@ class VariableBlock : public SleipnirBase {
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});
}
/// Returns a slice of the variable matrix.
@@ -298,10 +298,10 @@ class VariableBlock : public SleipnirBase {
/// @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) {
+ 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];
+ return (*this)(row_slice, row_slice_length, col_slice, col_slice_length);
}
/// Returns a slice of the variable matrix.
@@ -309,11 +309,11 @@ class VariableBlock : public SleipnirBase {
/// @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,
+ const VariableBlock<const Mat> operator()(Slice row_slice,
Slice col_slice) const {
int row_slice_length = row_slice.adjust(m_row_slice_length);
int col_slice_length = col_slice.adjust(m_col_slice_length);
- return (*this)[row_slice, row_slice_length, col_slice, col_slice_length];
+ return (*this)(row_slice, row_slice_length, col_slice, col_slice_length);
}
/// Returns a slice of the variable matrix.
@@ -326,7 +326,7 @@ class VariableBlock : public SleipnirBase {
/// @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,
+ VariableBlock<Mat> operator()(Slice row_slice, int row_slice_length,
Slice col_slice, int col_slice_length) {
return VariableBlock{
*m_mat,
@@ -350,7 +350,7 @@ class VariableBlock : public SleipnirBase {
/// @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,
+ const VariableBlock<const Mat> operator()(Slice row_slice,
int row_slice_length,
Slice col_slice,
int col_slice_length) const {
@@ -453,7 +453,7 @@ class VariableBlock : public SleipnirBase {
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;
}
}
@@ -469,7 +469,7 @@ class VariableBlock : public SleipnirBase {
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);
}
}
@@ -483,7 +483,7 @@ class VariableBlock : public SleipnirBase {
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;
}
}
@@ -499,7 +499,7 @@ class VariableBlock : public SleipnirBase {
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);
}
}
@@ -515,7 +515,7 @@ class VariableBlock : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] += rhs;
+ (*this)(row, col) += rhs;
}
}
@@ -531,7 +531,7 @@ class VariableBlock : public SleipnirBase {
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);
}
}
@@ -547,7 +547,7 @@ class VariableBlock : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] -= rhs;
+ (*this)(row, col) -= rhs;
}
}
@@ -558,7 +558,7 @@ class VariableBlock : public SleipnirBase {
// NOLINTNEXTLINE (google-explicit-constructor)
operator Variable<Scalar>() const {
slp_assert(rows() == 1 && cols() == 1);
- return (*this)[0, 0];
+ return (*this)(0, 0);
}
/// Returns the transpose of the variable matrix.
@@ -569,7 +569,7 @@ class VariableBlock : public SleipnirBase {
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);
}
}
@@ -591,7 +591,7 @@ class VariableBlock : public SleipnirBase {
/// @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(); }
+ Scalar value(int row, int col) { return (*this)(row, col).value(); }
/// Returns an element of the variable block.
///
@@ -611,7 +611,7 @@ class VariableBlock : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- result[row, col] = value(row, col);
+ result(row, col) = value(row, col);
}
}
@@ -629,7 +629,7 @@ class VariableBlock : public SleipnirBase {
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 b5d15edc5b294a2ea50dca15a7612c61e5f2680b..02bccb660474d9380444f5e20124fb687e372982 100644
--- a/include/sleipnir/autodiff/variable_matrix.hpp
+++ b/include/sleipnir/autodiff/variable_matrix.hpp
@@ -154,7 +154,7 @@ class VariableMatrix : public SleipnirBase {
m_storage.reserve(values.rows() * values.cols());
for (int row = 0; row < values.rows(); ++row) {
for (int col = 0; col < values.cols(); ++col) {
- m_storage.emplace_back(values[row, col]);
+ m_storage.emplace_back(values(row, col));
}
}
}
@@ -204,7 +204,7 @@ class VariableMatrix : public SleipnirBase {
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));
}
}
}
@@ -218,7 +218,7 @@ class VariableMatrix : public SleipnirBase {
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));
}
}
}
@@ -262,7 +262,7 @@ class VariableMatrix : public SleipnirBase {
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);
}
}
@@ -278,7 +278,7 @@ class VariableMatrix : public SleipnirBase {
VariableMatrix& operator=(ScalarLike auto value) {
slp_assert(rows() == 1 && cols() == 1);
- (*this)[0, 0] = value;
+ (*this)(0, 0) = value;
return *this;
}
@@ -293,7 +293,7 @@ class VariableMatrix : public SleipnirBase {
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));
}
}
}
@@ -303,7 +303,7 @@ class VariableMatrix : public SleipnirBase {
/// @param row The row.
/// @param col The column.
/// @return The element at the given row and column.
- Variable<Scalar>& operator[](int row, int col) {
+ Variable<Scalar>& operator()(int row, int col) {
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
return m_storage[row * cols() + col];
@@ -314,7 +314,7 @@ class VariableMatrix : public SleipnirBase {
/// @param row The row.
/// @param col The column.
/// @return The element at the given row and column.
- const Variable<Scalar>& operator[](int row, int col) const {
+ const Variable<Scalar>& operator()(int row, int col) const {
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
return m_storage[row * cols() + col];
@@ -377,7 +377,7 @@ class VariableMatrix : public SleipnirBase {
/// @param row_slice The row slice.
/// @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,
@@ -389,7 +389,7 @@ class VariableMatrix : public SleipnirBase {
/// @param row_slice The row slice.
/// @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());
@@ -407,7 +407,7 @@ class VariableMatrix : public SleipnirBase {
/// @param col_slice The column slice.
/// @param col_slice_length The column slice length.
/// @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) {
@@ -425,7 +425,7 @@ class VariableMatrix : public SleipnirBase {
/// @param col_slice The column slice.
/// @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,
@@ -511,9 +511,9 @@ class VariableMatrix : public SleipnirBase {
for (int j = 0; j < rhs.cols(); ++j) {
Variable sum{Scalar(0)};
for (int k = 0; k < lhs.cols(); ++k) {
- sum += lhs(i, k) * rhs[k, j];
+ sum += lhs(i, k) * rhs(k, j);
}
- result[i, j] = sum;
+ result(i, j) = sum;
}
}
@@ -534,9 +534,9 @@ class VariableMatrix : public SleipnirBase {
for (int j = 0; j < rhs.cols(); ++j) {
Variable sum{Scalar(0)};
for (int k = 0; k < lhs.cols(); ++k) {
- sum += lhs[i, k] * rhs(k, j);
+ sum += lhs(i, k) * rhs(k, j);
}
- result[i, j] = sum;
+ result(i, j) = sum;
}
}
@@ -557,9 +557,9 @@ class VariableMatrix : public SleipnirBase {
for (int j = 0; j < rhs.cols(); ++j) {
Variable sum{Scalar(0)};
for (int k = 0; k < lhs.cols(); ++k) {
- 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
@@ -580,7 +580,7 @@ class VariableMatrix : public SleipnirBase {
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;
}
}
@@ -597,7 +597,7 @@ class VariableMatrix : public SleipnirBase {
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;
}
}
@@ -615,7 +615,7 @@ class VariableMatrix : public SleipnirBase {
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;
}
}
@@ -632,7 +632,7 @@ class VariableMatrix : public SleipnirBase {
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;
}
}
@@ -650,9 +650,9 @@ class VariableMatrix : public SleipnirBase {
for (int j = 0; j < rhs.cols(); ++j) {
Variable sum{Scalar(0)};
for (int k = 0; k < cols(); ++k) {
- sum += (*this)[i, k] * rhs[k, j];
+ sum += (*this)(i, k) * rhs(k, j);
}
- (*this)[i, j] = sum;
+ (*this)(i, j) = sum;
}
}
@@ -666,7 +666,7 @@ class VariableMatrix : public SleipnirBase {
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;
}
}
@@ -685,7 +685,7 @@ class VariableMatrix : public SleipnirBase {
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;
}
}
@@ -704,7 +704,7 @@ class VariableMatrix : public SleipnirBase {
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;
}
}
@@ -723,7 +723,7 @@ class VariableMatrix : public SleipnirBase {
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;
}
}
@@ -737,7 +737,7 @@ class VariableMatrix : public SleipnirBase {
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;
}
}
@@ -757,7 +757,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
- result[row, col] = lhs[row, col] + rhs[row, col];
+ result(row, col) = lhs(row, col) + rhs(row, col);
}
}
@@ -777,7 +777,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
- result[row, col] = lhs[row, col] + rhs[row, col];
+ result(row, col) = lhs(row, col) + rhs(row, col);
}
}
@@ -797,7 +797,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
- result[row, col] = lhs[row, col] + rhs[row, col];
+ result(row, col) = lhs(row, col) + rhs(row, col);
}
}
@@ -813,7 +813,7 @@ class VariableMatrix : public SleipnirBase {
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);
}
}
@@ -829,7 +829,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] += rhs;
+ (*this)(row, col) += rhs;
}
}
@@ -849,7 +849,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
- result[row, col] = lhs[row, col] - rhs[row, col];
+ result(row, col) = lhs(row, col) - rhs(row, col);
}
}
@@ -869,7 +869,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
- result[row, col] = lhs[row, col] - rhs[row, col];
+ result(row, col) = lhs(row, col) - rhs(row, col);
}
}
@@ -889,7 +889,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
- result[row, col] = lhs[row, col] - rhs[row, col];
+ result(row, col) = lhs(row, col) - rhs(row, col);
}
}
@@ -905,7 +905,7 @@ class VariableMatrix : public SleipnirBase {
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);
}
}
@@ -921,7 +921,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] -= rhs;
+ (*this)(row, col) -= rhs;
}
}
@@ -937,7 +937,7 @@ class VariableMatrix : public SleipnirBase {
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);
}
}
@@ -948,7 +948,7 @@ class VariableMatrix : public SleipnirBase {
// NOLINTNEXTLINE (google-explicit-constructor)
operator Variable<Scalar>() const {
slp_assert(rows() == 1 && cols() == 1);
- return (*this)[0, 0];
+ return (*this)(0, 0);
}
/// Returns the transpose of the variable matrix.
@@ -959,7 +959,7 @@ class VariableMatrix : public SleipnirBase {
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);
}
}
@@ -981,7 +981,7 @@ class VariableMatrix : public SleipnirBase {
/// @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(); }
+ Scalar value(int row, int col) { return (*this)(row, col).value(); }
/// Returns an element of the variable matrix.
///
@@ -998,7 +998,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- result[row, col] = value(row, col);
+ result(row, col) = value(row, col);
}
}
@@ -1016,7 +1016,7 @@ class VariableMatrix : public SleipnirBase {
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));
}
}
@@ -1269,7 +1269,7 @@ VariableMatrix<Scalar> 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));
}
}
@@ -1402,17 +1402,17 @@ VariableMatrix<Scalar> solve(const VariableMatrix<Scalar>& A,
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);
VariableMatrix adj_A{{d, -b}, {-c, a}};
auto det_A = a * d - b * c;
@@ -1429,15 +1429,15 @@ VariableMatrix<Scalar> solve(const VariableMatrix<Scalar>& A,
//
// 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;
@@ -1477,22 +1477,22 @@ VariableMatrix<Scalar> solve(const VariableMatrix<Scalar>& A,
//
// 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;
@@ -1623,14 +1623,14 @@ VariableMatrix<Scalar> solve(const VariableMatrix<Scalar>& A,
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);
}
}
@@ -1639,7 +1639,7 @@ VariableMatrix<Scalar> solve(const VariableMatrix<Scalar>& A,
VariableMatrix<Scalar> X{detail::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];
+ X(row, col) = eigen_X(row, col);
}
}
diff --git a/include/sleipnir/optimization/ocp.hpp b/include/sleipnir/optimization/ocp.hpp
index 5e20b9b8debc13611d5d719b589d8fd896c35a90..e5163f9c2dbbfa0a580d029e61c741e6c9d00fd9 100644
--- a/include/sleipnir/optimization/ocp.hpp
+++ b/include/sleipnir/optimization/ocp.hpp
@@ -123,7 +123,7 @@ class OCP : public Problem<Scalar> {
if (timestep_method == TimestepMethod::FIXED) {
m_DT = VariableMatrix<Scalar>{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) {
- m_DT[0, i] = dt.count();
+ m_DT(0, i) = dt.count();
}
} else if (timestep_method == TimestepMethod::VARIABLE_SINGLE) {
Variable single_dt = this->decision_variable();
@@ -132,12 +132,12 @@ class OCP : public Problem<Scalar> {
// Set the member variable matrix to track the decision variable
m_DT = VariableMatrix<Scalar>{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) {
- m_DT[0, i] = single_dt;
+ m_DT(0, i) = single_dt;
}
} else if (timestep_method == TimestepMethod::VARIABLE) {
m_DT = this->decision_variable(1, m_num_steps + 1);
for (int i = 0; i < num_steps + 1; ++i) {
- m_DT[0, i].set_value(dt.count());
+ m_DT(0, i).set_value(dt.count());
}
}
@@ -206,7 +206,7 @@ class OCP : public Problem<Scalar> {
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;
@@ -326,7 +326,7 @@ class OCP : public Problem<Scalar> {
// 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;
@@ -363,7 +363,7 @@ class OCP : public Problem<Scalar> {
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) {
this->subject_to(
@@ -386,7 +386,7 @@ class OCP : public Problem<Scalar> {
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)&, VariableMatrix<Scalar>,
diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
index d3feadd577bd53147a8b07da76910e046ccbf95d..18afc996590de98050bfd3ddc629952bf0c8c2e2 100644
--- a/include/sleipnir/optimization/problem.hpp
+++ b/include/sleipnir/optimization/problem.hpp
@@ -98,7 +98,7 @@ class 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();
}
}
@@ -133,8 +133,8 @@ class 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();
}
}
Reference in New Issue View Git Blame Copy Permalink