Skip to content

Commit

Permalink
Use multidimensional array subscript operator
Browse files Browse the repository at this point in the history
  • Loading branch information
@calcmogul
calcmogul committed Oct 6, 2024
1 parent 80f071a commit e45ff2d
Show file tree
Hide file tree
Showing 33 changed files with 459 additions and 437 deletions.
2 changes: 1 addition & 1 deletion README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -138,7 +138,7 @@ See the [examples](https://github.com/SleipnirGroup/Sleipnir/tree/main/examples)

### Dependencies

* C++23 compiler
* C++26 compiler
* On Windows 10 or greater, install [Visual Studio Community 2022](https://visualstudio.microsoft.com/vs/community/) and select the C++ programming language during installation
* On Ubuntu 24.04 or greater, install GCC 14 via `sudo apt install g++-14`
* On macOS 14 or greater, install the Xcode 15.3 command-line build tools via `xcode-select --install`
Expand Down
28 changes: 14 additions & 14 deletions benchmarks/scalability/CartPole/Sleipnir.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,30 +41,30 @@ sleipnir::VariableMatrix CartPoleDynamics(const sleipnir::VariableMatrix& x,

auto q = x.Segment(0, 2);
auto qdot = x.Segment(2, 2);
auto theta = q(1);
auto thetadot = qdot(1);
auto theta = q[1];
auto thetadot = qdot[1];

// [ m_c + m_p m_p l cosθ]
// M(q) = [m_p l cosθ m_p l² ]
sleipnir::VariableMatrix M{2, 2};
M(0, 0) = m_c + m_p;
M(0, 1) = m_p * l * cos(theta); // NOLINT
M(1, 0) = m_p * l * cos(theta); // NOLINT
M(1, 1) = m_p * std::pow(l, 2);
M[0, 0] = m_c + m_p;
M[0, 1] = m_p * l * cos(theta); // NOLINT
M[1, 0] = m_p * l * cos(theta); // NOLINT
M[1, 1] = m_p * std::pow(l, 2);

// [0 −m_p lθ̇ sinθ]
// C(q, q̇) = [0 0 ]
sleipnir::VariableMatrix C{2, 2};
C(0, 0) = 0;
C(0, 1) = -m_p * l * thetadot * sin(theta); // NOLINT
C(1, 0) = 0;
C(1, 1) = 0;
C[0, 0] = 0;
C[0, 1] = -m_p * l * thetadot * sin(theta); // NOLINT
C[1, 0] = 0;
C[1, 1] = 0;

// [ 0 ]
// τ_g(q) = [-m_p gl sinθ]
sleipnir::VariableMatrix tau_g{2, 1};
tau_g(0) = 0;
tau_g(1) = -m_p * g * l * sin(theta); // NOLINT
tau_g[0] = 0;
tau_g[1] = -m_p * g * l * sin(theta); // NOLINT

// [1]
// B = [0]
Expand Down Expand Up @@ -92,9 +92,9 @@ sleipnir::OptimizationProblem CartPoleSleipnir(std::chrono::duration<double> dt,

// Initial guess
for (int k = 0; k < N + 1; ++k) {
X(0, k).SetValue(
X[0, k].SetValue(
std::lerp(x_initial(0), x_final(0), static_cast<double>(k) / N));
X(1, k).SetValue(
X[1, k].SetValue(
std::lerp(x_initial(1), x_final(1), static_cast<double>(k) / N));
}

Expand Down
2 changes: 2 additions & 0 deletions cmake/modules/CompilerFlags.cmake
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,5 +14,7 @@ macro(compiler_flags target)
target_compile_features(${target} PUBLIC cxx_std_23)
if(MSVC)
target_compile_options(${target} PUBLIC /MP /utf-8 /bigobj)
elseif(APPLE)
target_compile_options(${target} PUBLIC -Wno-pre-c++2b-compat)
endif()
endmacro()
10 changes: 5 additions & 5 deletions examples/CurrentManager/src/CurrentManager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,21 +15,21 @@ CurrentManager::CurrentManager(std::span<const double> currentTolerances,
// Ensure m_desiredCurrents contains initialized Variables
for (int row = 0; row < m_desiredCurrents.Rows(); ++row) {
// Don't initialize to 0 or 1, because those will get folded by Sleipnir
m_desiredCurrents(row) = std::numeric_limits<double>::infinity();
m_desiredCurrents[row] = std::numeric_limits<double>::infinity();
}

sleipnir::Variable J = 0.0;
sleipnir::Variable currentSum = 0.0;
for (size_t i = 0; i < currentTolerances.size(); ++i) {
// The weight is 1/tolᵢ² where tolᵢ is the tolerance between the desired
// and allocated current for subsystem i
auto error = m_desiredCurrents(i) - m_allocatedCurrents(i);
auto error = m_desiredCurrents[i] - m_allocatedCurrents[i];
J += error * error / (currentTolerances[i] * currentTolerances[i]);

currentSum += m_allocatedCurrents(i);
currentSum += m_allocatedCurrents[i];

// Currents must be nonnegative
m_problem.SubjectTo(m_allocatedCurrents(i) >= 0.0);
m_problem.SubjectTo(m_allocatedCurrents[i] >= 0.0);
}
m_problem.Minimize(J);

Expand All @@ -46,7 +46,7 @@ std::vector<double> CurrentManager::Calculate(
}

for (size_t i = 0; i < desiredCurrents.size(); ++i) {
m_desiredCurrents(i).SetValue(desiredCurrents[i]);
m_desiredCurrents[i].SetValue(desiredCurrents[i]);
}

m_problem.Solve();
Expand Down
24 changes: 12 additions & 12 deletions examples/FRC2022Shooter/src/Main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,9 +40,9 @@ slp::VariableMatrix f(const slp::VariableMatrix& x) {
constexpr double m = 2.0; // kg
auto a_D = [](auto v) { return 0.5 * rho * v * v * C_D * A / m; };

auto v_x = x(3, 0);
auto v_y = x(4, 0);
auto v_z = x(5, 0);
auto v_x = x[3, 0];
auto v_y = x[4, 0];
auto v_z = x[5, 0];
return slp::VariableMatrix{{v_x}, {v_y}, {v_z},
{-a_D(v_x)}, {-a_D(v_y)}, {-g - a_D(v_z)}};
}
Expand Down Expand Up @@ -94,11 +94,11 @@ int main() {
// Position initial guess is linear interpolation between start and end
// position
for (int k = 0; k < N; ++k) {
p_x(k).SetValue(std::lerp(shooter_wrt_field(0), target_wrt_field(0),
p_x[k].SetValue(std::lerp(shooter_wrt_field[0], target_wrt_field[0],
static_cast<double>(k) / N));
p_y(k).SetValue(std::lerp(shooter_wrt_field(1), target_wrt_field(1),
p_y[k].SetValue(std::lerp(shooter_wrt_field[1], target_wrt_field[1],
static_cast<double>(k) / N));
p_z(k).SetValue(std::lerp(shooter_wrt_field(2), target_wrt_field(2),
p_z[k].SetValue(std::lerp(shooter_wrt_field[2], target_wrt_field[2],
static_cast<double>(k) / N));
}

Expand All @@ -118,9 +118,9 @@ int main() {
//
// √{v_x² + v_y² + v_z²) ≤ vₘₐₓ
// v_x² + v_y² + v_z² ≤ vₘₐₓ²
problem.SubjectTo(slp::pow(v_x(0) - robot_wrt_field(3), 2) +
slp::pow(v_y(0) - robot_wrt_field(4), 2) +
slp::pow(v_z(0) - robot_wrt_field(5), 2) <=
problem.SubjectTo(slp::pow(v_x[0] - robot_wrt_field[3], 2) +
slp::pow(v_y[0] - robot_wrt_field[4], 2) +
slp::pow(v_z[0] - robot_wrt_field[5], 2) <=
max_initial_velocity * max_initial_velocity);

// Dynamics constraints - RK4 integration
Expand All @@ -140,7 +140,7 @@ int main() {
problem.SubjectTo(p.Col(N - 1) == target_wrt_field.block(0, 0, 3, 1));

// Require the final velocity is down
problem.SubjectTo(v_z(N - 1) < 0.0);
problem.SubjectTo(v_z[N - 1] < 0.0);

// Minimize time-to-target
problem.Minimize(T);
Expand All @@ -154,10 +154,10 @@ int main() {
double velocity = v0.norm();
std::println("Velocity = {:.03} ms", velocity);

double pitch = std::atan2(v0(2), std::hypot(v0(0), v0(1)));
double pitch = std::atan2(v0[2], std::hypot(v0[0], v0[1]));
std::println("Pitch = {:.03}°", pitch * 180.0 / std::numbers::pi);

double yaw = std::atan2(v0(1), v0(0));
double yaw = std::atan2(v0[1], v0[0]);
std::println("Yaw = {:.03}°", yaw * 180.0 / std::numbers::pi);

std::println("Total time = {:.03} s", T.Value());
Expand Down
20 changes: 10 additions & 10 deletions examples/FRC2024Shooter/src/Main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,9 +50,9 @@ slp::VariableMatrix f(const slp::VariableMatrix& x) {
constexpr double m = 2.0; // kg
auto a_D = [](auto v) { return 0.5 * rho * v * v * C_D * A / m; };

auto v_x = x(3, 0);
auto v_y = x(4, 0);
auto v_z = x(5, 0);
auto v_x = x[3, 0];
auto v_y = x[4, 0];
auto v_z = x[5, 0];
return slp::VariableMatrix{{v_x}, {v_y}, {v_z},
{-a_D(v_x)}, {-a_D(v_y)}, {-g - a_D(v_z)}};
}
Expand Down Expand Up @@ -108,9 +108,9 @@ int main() {
//
// √{v_x² + v_y² + v_z²) ≤ vₘₐₓ
// v_x² + v_y² + v_z² ≤ vₘₐₓ²
problem.SubjectTo(slp::pow(x(3) - robot_wrt_field(3), 2) +
slp::pow(x(4) - robot_wrt_field(4), 2) +
slp::pow(x(5) - robot_wrt_field(5), 2) <=
problem.SubjectTo(slp::pow(x[3] - robot_wrt_field[3], 2) +
slp::pow(x[4] - robot_wrt_field[4], 2) +
slp::pow(x[5] - robot_wrt_field[5], 2) <=
max_initial_velocity * max_initial_velocity);

// Dynamics constraints - RK4 integration
Expand All @@ -128,10 +128,10 @@ int main() {
problem.SubjectTo(x_k.Segment(0, 3) == target_wrt_field.block(0, 0, 3, 1));

// Require the final velocity is up
problem.SubjectTo(x_k(5) > 0.0);
problem.SubjectTo(x_k[5] > 0.0);

// Minimize sensitivity of vertical position to velocity
auto sensitivity = slp::Gradient(x_k(3), x.Segment(3, 3)).Get();
auto sensitivity = slp::Gradient(x_k[3], x.Segment(3, 3)).Get();
problem.Minimize(sensitivity.T() * sensitivity);

problem.Solve({.diagnostics = true});
Expand All @@ -142,10 +142,10 @@ int main() {
double velocity = v0.norm();
std::println("Velocity = {:.03} ms", velocity);

double pitch = std::atan2(v0(2), std::hypot(v0(0), v0(1)));
double pitch = std::atan2(v0[2], std::hypot(v0[0], v0[1]));
std::println("Pitch = {:.03}°", pitch * 180.0 / std::numbers::pi);

double yaw = std::atan2(v0(1), v0(0));
double yaw = std::atan2(v0[1], v0[0]);
std::println("Yaw = {:.03}°", yaw * 180.0 / std::numbers::pi);

std::println("Total time = {:.03} s", T.Value());
Expand Down
2 changes: 1 addition & 1 deletion include/sleipnir/autodiff/Hessian.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ class SLEIPNIR_DLLEXPORT Hessian {

VariableMatrix ret{wrt.Rows()};
for (int row = 0; row < ret.Rows(); ++row) {
ret(row) = Variable{std::move(grad[row])};
ret[row] = Variable{std::move(grad[row])};
}
return ret;
}(),
Expand Down
10 changes: 5 additions & 5 deletions include/sleipnir/autodiff/Jacobian.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ class SLEIPNIR_DLLEXPORT Jacobian {
m_profiler.StartSetup();

for (int row = 0; row < m_wrt.Rows(); ++row) {
m_wrt(row).expr->row = row;
m_wrt[row].expr->row = row;
}

for (Variable variable : m_variables) {
Expand All @@ -47,22 +47,22 @@ class SLEIPNIR_DLLEXPORT Jacobian {
m_cachedTriplets.reserve(m_variables.Rows() * m_wrt.Rows() * 0.01);

for (int row = 0; row < m_variables.Rows(); ++row) {
if (m_variables(row).Type() == ExpressionType::kLinear) {
if (m_variables[row].Type() == ExpressionType::kLinear) {
// If the row is linear, compute its gradient once here and cache its
// triplets. Constant rows are ignored because their gradients have no
// nonzero triplets.
m_graphs[row].ComputeAdjoints([&](int col, double adjoint) {
m_cachedTriplets.emplace_back(row, col, adjoint);
});
} else if (m_variables(row).Type() > ExpressionType::kLinear) {
} else if (m_variables[row].Type() > ExpressionType::kLinear) {
// If the row is quadratic or nonlinear, add it to the list of nonlinear
// rows to be recomputed in Value().
m_nonlinearRows.emplace_back(row);
}
}

for (int row = 0; row < m_wrt.Rows(); ++row) {
m_wrt(row).expr->row = -1;
m_wrt[row].expr->row = -1;
}

if (m_nonlinearRows.empty()) {
Expand Down Expand Up @@ -90,7 +90,7 @@ class SLEIPNIR_DLLEXPORT Jacobian {
for (int row = 0; row < m_variables.Rows(); ++row) {
auto grad = m_graphs[row].GenerateGradientTree(wrtVec);
for (int col = 0; col < m_wrt.Rows(); ++col) {
result(row, col) = Variable{std::move(grad[col])};
result[row, col] = Variable{std::move(grad[col])};
}
}

Expand Down
26 changes: 23 additions & 3 deletions include/sleipnir/autodiff/Variable.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -468,7 +468,11 @@ small_vector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
for (int row = 0; row < rows; ++row) {
for (int col = 0; col < cols; ++col) {
// Make right-hand side zero
constraints.emplace_back(lhs - rhs(row, col));
if constexpr (EigenMatrixLike<std::decay_t<RHS>>) {
constraints.emplace_back(lhs - rhs(row, col));
} else {
constraints.emplace_back(lhs - rhs[row, col]);
}
}
}
} else if constexpr (MatrixLike<std::decay_t<LHS>> &&
Expand All @@ -488,7 +492,11 @@ small_vector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
for (int row = 0; row < rows; ++row) {
for (int col = 0; col < cols; ++col) {
// Make right-hand side zero
constraints.emplace_back(lhs(row, col) - rhs);
if constexpr (EigenMatrixLike<std::decay_t<LHS>>) {
constraints.emplace_back(lhs(row, col) - rhs);
} else {
constraints.emplace_back(lhs[row, col] - rhs);
}
}
}
} else if constexpr (MatrixLike<std::decay_t<LHS>> &&
Expand Down Expand Up @@ -521,7 +529,19 @@ small_vector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
for (int row = 0; row < lhsRows; ++row) {
for (int col = 0; col < lhsCols; ++col) {
// Make right-hand side zero
constraints.emplace_back(lhs(row, col) - rhs(row, col));
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]);
}
}
}
}
Expand Down
Loading

0 comments on commit e45ff2d

Please sign in to comment.