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
09be21c319744383942edf7ce05ab9bca1bcf805
allwpilib/upstream_utils/sleipnir_patches/0009-Use-operator-instead-of-multidimensional-array-subsc.patch
Tyler Veness cca035787c [upstream_utils] Upgrade to Sleipnir 0.3.3 (#8463)
2025-12-12 19:40:43 -08: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 9/9] 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 5c8d18b41c4025f4492e7216f94299785a513552..b52371b182b755f87f4d038dbeb1795704ab730e 100644
--- a/include/sleipnir/autodiff/hessian.hpp
+++ b/include/sleipnir/autodiff/hessian.hpp
@@ -98,9 +98,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 bb3a0319880bd240497e1e27ca26f0105b50b803..082a6cf6782f376cbb6ce05809e62e8021feb321 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 2091f339744f8f8980ccb1e44b0b54b06eb32418..7616c02b456c755a56f68de86a755d29cd55d91d 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);
}
@@ -635,7 +635,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));
}
}
@@ -651,7 +651,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);
}
}
@@ -669,7 +669,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 10b3a4cc5ab22e93fe4e0b44d4664adce9228867..d90645910d1db545fb34a726aa801e08739318cc 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));
}
}
@@ -1253,7 +1253,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));
}
}
@@ -1386,17 +1386,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;
@@ -1413,15 +1413,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;
@@ -1461,22 +1461,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;
@@ -1607,14 +1607,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);
}
}
@@ -1623,7 +1623,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 091d985234ac8be5c177b9269c7608036f0daddb..e906c194e93a9e26b9cb5c20b720b87828463e80 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 70955fdc7148e5af737d3094a5602024df790b3d..4243d13c9d04eeee948f8e0e7a21d72b6ef0eaa6 100644
--- a/include/sleipnir/optimization/problem.hpp
+++ b/include/sleipnir/optimization/problem.hpp
@@ -92,7 +92,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();
}
}
@@ -125,8 +125,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