From 8d0444bd44eba1abb91aebaec6e776cc1a5b7811 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Fri, 21 Jun 2024 21:16:35 -0700 Subject: [PATCH] Fix unused parameter warnings --- benchmarks/scalability/Util.hpp | 5 +- cmake/modules/CompilerFlags.cmake | 2 +- examples/FlywheelOCP/src/Main.cpp | 4 +- include/sleipnir/autodiff/Expression.hpp | 45 ++++---- include/sleipnir/autodiff/ExpressionGraph.hpp | 2 +- include/sleipnir/control/OCPSolver.hpp | 102 ++++++++++++++---- include/sleipnir/optimization/Multistart.hpp | 3 +- .../optimization/OptimizationProblem.hpp | 2 +- .../optimization/solver/InteriorPoint.hpp | 2 +- include/sleipnir/util/Pool.hpp | 8 +- jormungandr/cpp/Docstrings.hpp | 46 +++++++- .../cpp/autodiff/BindVariableBlock.cpp | 2 +- .../cpp/autodiff/BindVariableMatrix.cpp | 2 +- src/optimization/solver/InteriorPoint.cpp | 8 +- .../solver/util/FeasibilityRestoration.hpp | 5 +- test/src/control/OCPSolverTest_CartPole.cpp | 20 ++-- .../OCPSolverTest_DifferentialDrive.cpp | 57 +++++----- test/src/control/OCPSolverTest_Flywheel.cpp | 14 +-- 18 files changed, 211 insertions(+), 118 deletions(-) diff --git a/benchmarks/scalability/Util.hpp b/benchmarks/scalability/Util.hpp index 7cb7a3a8..a539e41a 100644 --- a/benchmarks/scalability/Util.hpp +++ b/benchmarks/scalability/Util.hpp @@ -40,7 +40,7 @@ constexpr double ToMilliseconds( template void RunBenchmark(std::ofstream& results, sleipnir::function_ref setup, - sleipnir::function_ref solve) { + sleipnir::function_ref solve) { // Record setup time auto setupStartTime = std::chrono::system_clock::now(); auto problem = setup(); @@ -86,7 +86,8 @@ template int RunBenchmarksAndLog( std::string_view filename, bool diagnostics, std::chrono::duration T, std::span sampleSizesToTest, - sleipnir::function_ref, int)> setup) { + sleipnir::function_ref dt, int N)> + setup) { std::ofstream results{std::string{filename}}; if (!results.is_open()) { return 1; diff --git a/cmake/modules/CompilerFlags.cmake b/cmake/modules/CompilerFlags.cmake index 9f3fad1c..a69f72af 100644 --- a/cmake/modules/CompilerFlags.cmake +++ b/cmake/modules/CompilerFlags.cmake @@ -2,7 +2,7 @@ macro(compiler_flags target) if(NOT MSVC) target_compile_options( ${target} - PRIVATE -Wall -pedantic -Wextra -Werror -Wno-unused-parameter + PRIVATE -Wall -pedantic -Wextra -Werror ) else() # Suppress the following warnings: diff --git a/examples/FlywheelOCP/src/Main.cpp b/examples/FlywheelOCP/src/Main.cpp index e6f60842..f6fc2bc2 100644 --- a/examples/FlywheelOCP/src/Main.cpp +++ b/examples/FlywheelOCP/src/Main.cpp @@ -25,8 +25,8 @@ int main() { Eigen::Matrix A_discrete{std::exp(A(0) * dt.count())}; Eigen::Matrix B_discrete{(1.0 - A_discrete(0)) * B(0)}; - auto f_discrete = [=](sleipnir::Variable t, sleipnir::VariableMatrix x, - sleipnir::VariableMatrix u, sleipnir::Variable dt) { + auto f_discrete = [=](sleipnir::VariableMatrix x, + sleipnir::VariableMatrix u) { return A_discrete * x + B_discrete * u; }; diff --git a/include/sleipnir/autodiff/Expression.hpp b/include/sleipnir/autodiff/Expression.hpp index 99997da5..6c4ae626 100644 --- a/include/sleipnir/autodiff/Expression.hpp +++ b/include/sleipnir/autodiff/Expression.hpp @@ -225,15 +225,15 @@ struct SLEIPNIR_DLLEXPORT Expression { return MakeExpressionPtr( type, [](double lhs, double rhs) { return lhs * rhs; }, - [](double lhs, double rhs, double parentAdjoint) { + [](double, double rhs, double parentAdjoint) { return parentAdjoint * rhs; }, - [](double lhs, double rhs, double parentAdjoint) { + [](double lhs, double, double parentAdjoint) { return parentAdjoint * lhs; }, - [](const ExpressionPtr& lhs, const ExpressionPtr& rhs, + [](const ExpressionPtr&, const ExpressionPtr& rhs, const ExpressionPtr& parentAdjoint) { return parentAdjoint * rhs; }, - [](const ExpressionPtr& lhs, const ExpressionPtr& rhs, + [](const ExpressionPtr& lhs, const ExpressionPtr&, const ExpressionPtr& parentAdjoint) { return parentAdjoint * lhs; }, lhs, rhs); } @@ -271,13 +271,13 @@ struct SLEIPNIR_DLLEXPORT Expression { return MakeExpressionPtr( type, [](double lhs, double rhs) { return lhs / rhs; }, - [](double lhs, double rhs, double parentAdjoint) { + [](double, double rhs, double parentAdjoint) { return parentAdjoint / rhs; }, [](double lhs, double rhs, double parentAdjoint) { return parentAdjoint * -lhs / (rhs * rhs); }, - [](const ExpressionPtr& lhs, const ExpressionPtr& rhs, + [](const ExpressionPtr&, const ExpressionPtr& rhs, const ExpressionPtr& parentAdjoint) { return parentAdjoint / rhs; }, [](const ExpressionPtr& lhs, const ExpressionPtr& rhs, const ExpressionPtr& parentAdjoint) { @@ -311,15 +311,11 @@ struct SLEIPNIR_DLLEXPORT Expression { return MakeExpressionPtr( std::max(lhs->type, rhs->type), [](double lhs, double rhs) { return lhs + rhs; }, - [](double lhs, double rhs, double parentAdjoint) { - return parentAdjoint; - }, - [](double lhs, double rhs, double parentAdjoint) { - return parentAdjoint; - }, - [](const ExpressionPtr& lhs, const ExpressionPtr& rhs, + [](double, double, double parentAdjoint) { return parentAdjoint; }, + [](double, double, double parentAdjoint) { return parentAdjoint; }, + [](const ExpressionPtr&, const ExpressionPtr&, const ExpressionPtr& parentAdjoint) { return parentAdjoint; }, - [](const ExpressionPtr& lhs, const ExpressionPtr& rhs, + [](const ExpressionPtr&, const ExpressionPtr&, const ExpressionPtr& parentAdjoint) { return parentAdjoint; }, lhs, rhs); } @@ -354,15 +350,11 @@ struct SLEIPNIR_DLLEXPORT Expression { return MakeExpressionPtr( std::max(lhs->type, rhs->type), [](double lhs, double rhs) { return lhs - rhs; }, - [](double lhs, double rhs, double parentAdjoint) { - return parentAdjoint; - }, - [](double lhs, double rhs, double parentAdjoint) { - return -parentAdjoint; - }, - [](const ExpressionPtr& lhs, const ExpressionPtr& rhs, + [](double, double, double parentAdjoint) { return parentAdjoint; }, + [](double, double, double parentAdjoint) { return -parentAdjoint; }, + [](const ExpressionPtr&, const ExpressionPtr&, const ExpressionPtr& parentAdjoint) { return parentAdjoint; }, - [](const ExpressionPtr& lhs, const ExpressionPtr& rhs, + [](const ExpressionPtr&, const ExpressionPtr&, const ExpressionPtr& parentAdjoint) { return -parentAdjoint; }, lhs, rhs); } @@ -388,8 +380,8 @@ struct SLEIPNIR_DLLEXPORT Expression { return MakeExpressionPtr( lhs->type, [](double lhs, double) { return -lhs; }, - [](double lhs, double, double parentAdjoint) { return -parentAdjoint; }, - [](const ExpressionPtr& lhs, const ExpressionPtr& rhs, + [](double, double, double parentAdjoint) { return -parentAdjoint; }, + [](const ExpressionPtr&, const ExpressionPtr&, const ExpressionPtr& parentAdjoint) { return -parentAdjoint; }, lhs); } @@ -964,9 +956,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { return 1.0; } }, - [](double x, double, double parentAdjoint) { return 0.0; }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { + [](double, double, double) { return 0.0; }, + [](const ExpressionPtr&, const ExpressionPtr&, const ExpressionPtr&) { // Return zero return MakeExpressionPtr(); }, diff --git a/include/sleipnir/autodiff/ExpressionGraph.hpp b/include/sleipnir/autodiff/ExpressionGraph.hpp index 56fe6bd5..c614195d 100644 --- a/include/sleipnir/autodiff/ExpressionGraph.hpp +++ b/include/sleipnir/autodiff/ExpressionGraph.hpp @@ -192,7 +192,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph { * @param func A function that takes two arguments: an int for the gradient * row, and a double for the adjoint (gradient value). */ - void ComputeAdjoints(function_ref func) { + void ComputeAdjoints(function_ref func) { // Zero adjoints. The root node's adjoint is 1.0 as df/df is always 1. m_adjointList[0]->adjoint = 1.0; for (auto it = m_adjointList.begin() + 1; it != m_adjointList.end(); ++it) { diff --git a/include/sleipnir/control/OCPSolver.hpp b/include/sleipnir/control/OCPSolver.hpp index a3a83341..adfca449 100644 --- a/include/sleipnir/control/OCPSolver.hpp +++ b/include/sleipnir/control/OCPSolver.hpp @@ -16,18 +16,6 @@ namespace sleipnir { -/** - * Function representing an explicit or implicit ODE, or a discrete state - * transition function. - * - * - Explicit: dx/dt = f(t, x, u, *) - * - Implicit: f(t, [x dx/dt]', u, *) = 0 - * - State transition: xₖ₊₁ = f(t, xₖ, uₖ, dt) - */ -using DynamicsFunction = - function_ref; - /** * Performs 4th order Runge-Kutta integration of dx/dt = f(t, x, u) for dt. * @@ -122,15 +110,61 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem { * @param numInputs The number of system inputs. * @param dt The timestep for fixed-step integration. * @param numSteps The number of control points. - * @param dynamics The system evolution function, either an explicit ODE or a - * discrete state transition function. + * @param dynamics Function representing an explicit or implicit ODE, or a + * discrete state transition function. + * - Explicit: dx/dt = f(x, u, *) + * - Implicit: f([x dx/dt]', u, *) = 0 + * - State transition: xₖ₊₁ = f(xₖ, uₖ) * @param dynamicsType The type of system evolution function. * @param timestepMethod The timestep method. * @param method The transcription method. */ OCPSolver( int numStates, int numInputs, std::chrono::duration dt, - int numSteps, DynamicsFunction dynamics, + int numSteps, + function_ref + dynamics, + DynamicsType dynamicsType = DynamicsType::kExplicitODE, + TimestepMethod timestepMethod = TimestepMethod::kFixed, + TranscriptionMethod method = TranscriptionMethod::kDirectTranscription) + : OCPSolver{numStates, + numInputs, + dt, + numSteps, + [=]([[maybe_unused]] const VariableMatrix& t, + const VariableMatrix& x, const VariableMatrix& u, + [[maybe_unused]] + const VariableMatrix& dt) -> VariableMatrix { + return dynamics(x, u); + }, + dynamicsType, + timestepMethod, + method} {} + + /** + * Build an optimization problem using a system evolution function (explicit + * ODE or discrete state transition function). + * + * @param numStates The number of system states. + * @param numInputs The number of system inputs. + * @param dt The timestep for fixed-step integration. + * @param numSteps The number of control points. + * @param dynamics Function representing an explicit or implicit ODE, or a + * discrete state transition function. + * - Explicit: dx/dt = f(t, x, u, *) + * - Implicit: f(t, [x dx/dt]', u, *) = 0 + * - State transition: xₖ₊₁ = f(t, xₖ, uₖ, dt) + * @param dynamicsType The type of system evolution function. + * @param timestepMethod The timestep method. + * @param method The transcription method. + */ + OCPSolver( + int numStates, int numInputs, std::chrono::duration dt, + int numSteps, + function_ref + dynamics, DynamicsType dynamicsType = DynamicsType::kExplicitODE, TimestepMethod timestepMethod = TimestepMethod::kFixed, TranscriptionMethod method = TranscriptionMethod::kDirectTranscription) @@ -203,6 +237,24 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem { SubjectTo(FinalState() == finalState); } + /** + * Set the constraint evaluation function. This function is called + * `numSteps+1` times, with the corresponding state and input + * VariableMatrices. + * + * @param callback The callback f(x, u) where x is the state and u is the + * input vector. + */ + void ForEachStep( + const function_ref + callback) { + for (int i = 0; i < m_numSteps + 1; ++i) { + auto x = X().Col(i); + auto u = U().Col(i); + callback(x, u); + } + } + /** * Set the constraint evaluation function. This function is called * `numSteps+1` times, with the corresponding state and input @@ -212,8 +264,8 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem { * vector, u is the input vector, and dt is the timestep duration. */ void ForEachStep( - const function_ref + const function_ref callback) { Variable time = 0.0; @@ -365,9 +417,9 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem { Variable dt = DT()(0, i); if (m_dynamicsType == DynamicsType::kExplicitODE) { - SubjectTo(x_end == - RK4(m_dynamicsFunction, x_begin, u, time, dt)); + SubjectTo(x_end == RK4( + m_dynamicsFunction, x_begin, u, time, dt)); } else if (m_dynamicsType == DynamicsType::kDiscrete) { SubjectTo(x_end == m_dynamicsFunction(time, x_begin, u, dt)); } @@ -386,8 +438,9 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem { Variable dt = DT()(0, i); if (m_dynamicsType == DynamicsType::kExplicitODE) { - x_end = RK4(m_dynamicsFunction, x_begin, u, time, dt); + x_end = RK4(m_dynamicsFunction, x_begin, u, + time, dt); } else if (m_dynamicsType == DynamicsType::kDiscrete) { x_end = m_dynamicsFunction(time, x_begin, u, dt); } @@ -403,7 +456,10 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem { TranscriptionMethod m_transcriptionMethod; DynamicsType m_dynamicsType; - DynamicsFunction m_dynamicsFunction; + + function_ref + m_dynamicsFunction; TimestepMethod m_timestepMethod; diff --git a/include/sleipnir/optimization/Multistart.hpp b/include/sleipnir/optimization/Multistart.hpp index d905cafd..8055713a 100644 --- a/include/sleipnir/optimization/Multistart.hpp +++ b/include/sleipnir/optimization/Multistart.hpp @@ -40,7 +40,8 @@ struct MultistartResult { */ template MultistartResult Multistart( - function_ref(const DecisionVariables&)> + function_ref( + const DecisionVariables& initialGuess)> solve, std::span initialGuesses) { small_vector>> futures; diff --git a/include/sleipnir/optimization/OptimizationProblem.hpp b/include/sleipnir/optimization/OptimizationProblem.hpp index 32ef0bf6..9df25605 100644 --- a/include/sleipnir/optimization/OptimizationProblem.hpp +++ b/include/sleipnir/optimization/OptimizationProblem.hpp @@ -372,7 +372,7 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem { small_vector m_inequalityConstraints; // The user callback - std::function m_callback = + std::function m_callback = [](const SolverIterationInfo&) { return false; }; // The solver status diff --git a/include/sleipnir/optimization/solver/InteriorPoint.hpp b/include/sleipnir/optimization/solver/InteriorPoint.hpp index abc9f778..51d8f973 100644 --- a/include/sleipnir/optimization/solver/InteriorPoint.hpp +++ b/include/sleipnir/optimization/solver/InteriorPoint.hpp @@ -48,7 +48,7 @@ SLEIPNIR_DLLEXPORT void InteriorPoint( std::span decisionVariables, std::span equalityConstraints, std::span inequalityConstraints, Variable& f, - function_ref callback, + function_ref callback, const SolverConfig& config, bool feasibilityRestoration, Eigen::VectorXd& x, Eigen::VectorXd& s, SolverStatus* status); diff --git a/include/sleipnir/util/Pool.hpp b/include/sleipnir/util/Pool.hpp index 02d8e190..441fa701 100644 --- a/include/sleipnir/util/Pool.hpp +++ b/include/sleipnir/util/Pool.hpp @@ -39,7 +39,8 @@ class SLEIPNIR_DLLEXPORT PoolResource { * @param alignment Alignment of the block (unused). */ [[nodiscard]] - void* allocate(size_t bytes, size_t alignment = alignof(std::max_align_t)) { + void* allocate(size_t bytes, [[maybe_unused]] size_t alignment = + alignof(std::max_align_t)) { if (m_freeList.empty()) { AddChunk(bytes); } @@ -56,8 +57,9 @@ class SLEIPNIR_DLLEXPORT PoolResource { * @param bytes Number of bytes in the block (unused). * @param alignment Alignment of the block (unused). */ - void deallocate(void* p, size_t bytes, - size_t alignment = alignof(std::max_align_t)) { + void deallocate( + void* p, [[maybe_unused]] size_t bytes, + [[maybe_unused]] size_t alignment = alignof(std::max_align_t)) { m_freeList.emplace_back(p); } diff --git a/jormungandr/cpp/Docstrings.hpp b/jormungandr/cpp/Docstrings.hpp index 1103678b..d58505e9 100644 --- a/jormungandr/cpp/Docstrings.hpp +++ b/jormungandr/cpp/Docstrings.hpp @@ -382,6 +382,15 @@ R"doc(Set the constraint evaluation function. This function is called `numSteps+1` times, with the corresponding state and input VariableMatrices. +Parameter ``callback``: + The callback f(x, u) where x is the state and u is the input + vector.)doc"; + +static const char *__doc_sleipnir_OCPSolver_ForEachStep_2 = +R"doc(Set the constraint evaluation function. This function is called +`numSteps+1` times, with the corresponding state and input +VariableMatrices. + Parameter ``callback``: The callback f(t, x, u, dt) where t is time, x is the state vector, u is the input vector, and dt is the timestep duration.)doc"; @@ -409,8 +418,41 @@ Parameter ``numSteps``: The number of control points. Parameter ``dynamics``: - The system evolution function, either an explicit ODE or a - discrete state transition function. + Function representing an explicit or implicit ODE, or a discrete + state transition function. - Explicit: dx/dt = f(x, u, *) - + Implicit: f([x dx/dt]', u, *) = 0 - State transition: xₖ₊₁ = f(xₖ, + uₖ) + +Parameter ``dynamicsType``: + The type of system evolution function. + +Parameter ``timestepMethod``: + The timestep method. + +Parameter ``method``: + The transcription method.)doc"; + +static const char *__doc_sleipnir_OCPSolver_OCPSolver_2 = +R"doc(Build an optimization problem using a system evolution function +(explicit ODE or discrete state transition function). + +Parameter ``numStates``: + The number of system states. + +Parameter ``numInputs``: + The number of system inputs. + +Parameter ``dt``: + The timestep for fixed-step integration. + +Parameter ``numSteps``: + The number of control points. + +Parameter ``dynamics``: + Function representing an explicit or implicit ODE, or a discrete + state transition function. - Explicit: dx/dt = f(t, x, u, *) - + Implicit: f(t, [x dx/dt]', u, *) = 0 - State transition: xₖ₊₁ = + f(t, xₖ, uₖ, dt) Parameter ``dynamicsType``: The type of system evolution function. diff --git a/jormungandr/cpp/autodiff/BindVariableBlock.cpp b/jormungandr/cpp/autodiff/BindVariableBlock.cpp index 0c786490..860ac0dc 100644 --- a/jormungandr/cpp/autodiff/BindVariableBlock.cpp +++ b/jormungandr/cpp/autodiff/BindVariableBlock.cpp @@ -241,7 +241,7 @@ void BindVariableBlock(py::module_& autodiff, cls.def( "__array_ufunc__", [](VariableBlock& self, py::object ufunc, py::str method, - py::args inputs, const py::kwargs& kwargs) -> py::object { + py::args inputs, const py::kwargs&) -> py::object { std::string method_name = method; std::string ufunc_name = ufunc.attr("__repr__")().cast(); diff --git a/jormungandr/cpp/autodiff/BindVariableMatrix.cpp b/jormungandr/cpp/autodiff/BindVariableMatrix.cpp index 20967c69..b56b7894 100644 --- a/jormungandr/cpp/autodiff/BindVariableMatrix.cpp +++ b/jormungandr/cpp/autodiff/BindVariableMatrix.cpp @@ -235,7 +235,7 @@ void BindVariableMatrix(py::module_& autodiff, cls.def( "__array_ufunc__", [](VariableMatrix& self, py::object ufunc, py::str method, - py::args inputs, const py::kwargs& kwargs) -> py::object { + py::args inputs, const py::kwargs&) -> py::object { std::string method_name = method; std::string ufunc_name = ufunc.attr("__repr__")().cast(); diff --git a/src/optimization/solver/InteriorPoint.cpp b/src/optimization/solver/InteriorPoint.cpp index 0d3c8fa2..817e3d1d 100644 --- a/src/optimization/solver/InteriorPoint.cpp +++ b/src/optimization/solver/InteriorPoint.cpp @@ -37,7 +37,7 @@ namespace sleipnir { void InteriorPoint(std::span decisionVariables, std::span equalityConstraints, std::span inequalityConstraints, Variable& f, - function_ref callback, + function_ref callback, const SolverConfig& config, bool feasibilityRestoration, Eigen::VectorXd& x, Eigen::VectorXd& s, SolverStatus* status) { @@ -354,7 +354,7 @@ void InteriorPoint(std::span decisionVariables, decisionVariables.size() + equalityConstraints.size(), decisionVariables.size() + equalityConstraints.size()); lhs.setFromSortedTriplets(triplets.begin(), triplets.end(), - [](const auto& a, const auto& b) { return b; }); + [](const auto&, const auto& b) { return b; }); const Eigen::VectorXd e = Eigen::VectorXd::Ones(s.rows()); @@ -582,7 +582,7 @@ void InteriorPoint(std::span decisionVariables, Eigen::VectorXd fr_s = s; SolverStatus fr_status; FeasibilityRestoration( - decisionVariables, equalityConstraints, inequalityConstraints, f, μ, + decisionVariables, equalityConstraints, inequalityConstraints, μ, [&](const SolverIterationInfo& info) { Eigen::VectorXd trial_x = info.x.segment(0, decisionVariables.size()); @@ -656,7 +656,7 @@ void InteriorPoint(std::span decisionVariables, A_e.cols() + s.rows()}; Ahat.setFromSortedTriplets( triplets.begin(), triplets.end(), - [](const auto& a, const auto& b) { return b; }); + [](const auto&, const auto& b) { return b; }); // lhs = ÂÂᵀ Eigen::SparseMatrix lhs = Ahat * Ahat.transpose(); diff --git a/src/optimization/solver/util/FeasibilityRestoration.hpp b/src/optimization/solver/util/FeasibilityRestoration.hpp index 37adf64e..c324ef09 100644 --- a/src/optimization/solver/util/FeasibilityRestoration.hpp +++ b/src/optimization/solver/util/FeasibilityRestoration.hpp @@ -28,7 +28,6 @@ namespace sleipnir { * @param[in] decisionVariables The list of decision variables. * @param[in] equalityConstraints The list of equality constraints. * @param[in] inequalityConstraints The list of inequality constraints. - * @param[in] f The cost function. * @param[in] μ Barrier parameter. * @param[in] callback The user callback. * @param[in] config Configuration options for the solver. @@ -40,8 +39,8 @@ namespace sleipnir { inline void FeasibilityRestoration( std::span decisionVariables, std::span equalityConstraints, - std::span inequalityConstraints, Variable& f, double μ, - function_ref callback, + std::span inequalityConstraints, double μ, + function_ref callback, const SolverConfig& config, Eigen::VectorXd& x, Eigen::VectorXd& s, SolverStatus* status) { // Feasibility restoration diff --git a/test/src/control/OCPSolverTest_CartPole.cpp b/test/src/control/OCPSolverTest_CartPole.cpp index 75d84e52..59011e8c 100644 --- a/test/src/control/OCPSolverTest_CartPole.cpp +++ b/test/src/control/OCPSolverTest_CartPole.cpp @@ -32,10 +32,10 @@ TEST_CASE("OCPSolver - Cart-pole", "[OCPSolver]") { constexpr Eigen::Vector x_initial{{0.0, 0.0, 0.0, 0.0}}; constexpr Eigen::Vector x_final{{1.0, std::numbers::pi, 0.0, 0.0}}; - auto dynamicsFunction = - [=](const sleipnir::Variable& t, const sleipnir::VariableMatrix& x, - const sleipnir::VariableMatrix& u, - const sleipnir::Variable& dt) { return CartPoleDynamics(x, u); }; + auto dynamicsFunction = [=](const sleipnir::VariableMatrix& x, + const sleipnir::VariableMatrix& u) { + return CartPoleDynamics(x, u); + }; sleipnir::OCPSolver problem( 4, 1, dt, N, dynamicsFunction, sleipnir::DynamicsType::kExplicitODE, @@ -63,12 +63,12 @@ TEST_CASE("OCPSolver - Cart-pole", "[OCPSolver]") { problem.ConstrainFinalState(x_final); // Cart position constraints - problem.ForEachStep( - [&](const sleipnir::Variable& t, const sleipnir::VariableMatrix& x, - const sleipnir::VariableMatrix& u, const sleipnir::Variable& dt) { - problem.SubjectTo(x(0) >= 0.0); - problem.SubjectTo(x(0) <= d_max); - }); + problem.ForEachStep([&](const sleipnir::VariableMatrix& x, + [[maybe_unused]] + const sleipnir::VariableMatrix& u) { + problem.SubjectTo(x(0) >= 0.0); + problem.SubjectTo(x(0) <= d_max); + }); // Input constraints problem.SetLowerInputBound(-u_max); diff --git a/test/src/control/OCPSolverTest_DifferentialDrive.cpp b/test/src/control/OCPSolverTest_DifferentialDrive.cpp index 589f6c88..9c16a4bb 100644 --- a/test/src/control/OCPSolverTest_DifferentialDrive.cpp +++ b/test/src/control/OCPSolverTest_DifferentialDrive.cpp @@ -22,35 +22,34 @@ TEST_CASE("OCPSolver - Differential drive", "[OCPSolver]") { constexpr int N = 50; - auto dynamics = - [=](const sleipnir::Variable& t, const sleipnir::VariableMatrix& x, - const sleipnir::VariableMatrix& u, const sleipnir::Variable& dt) { - // x = [x, y, heading, left velocity, right velocity]ᵀ - // u = [left voltage, right voltage]ᵀ - constexpr double trackwidth = 0.699; // m - constexpr double Kv_linear = 3.02; // V/(m/s) - constexpr double Ka_linear = 0.642; // V/(m/s²) - constexpr double Kv_angular = 1.382; // V/(m/s) - constexpr double Ka_angular = 0.08495; // V/(m/s²) - - auto v = (x(3) + x(4)) / 2.0; - - constexpr double A1 = - -(Kv_linear / Ka_linear + Kv_angular / Ka_angular) / 2.0; - constexpr double A2 = - -(Kv_linear / Ka_linear - Kv_angular / Ka_angular) / 2.0; - constexpr double B1 = 0.5 / Ka_linear + 0.5 / Ka_angular; - constexpr double B2 = 0.5 / Ka_linear - 0.5 / Ka_angular; - Eigen::Matrix A{{A1, A2}, {A2, A1}}; - Eigen::Matrix B{{B1, B2}, {B2, B1}}; - - sleipnir::VariableMatrix xdot{5}; - xdot(0) = v * sleipnir::cos(x(2)); - xdot(1) = v * sleipnir::sin(x(2)); - xdot(2) = (x(4) - x(3)) / trackwidth; - xdot.Segment(3, 2) = A * x.Segment(3, 2) + B * u; - return xdot; - }; + auto dynamics = [=](const sleipnir::VariableMatrix& x, + const sleipnir::VariableMatrix& u) { + // x = [x, y, heading, left velocity, right velocity]ᵀ + // u = [left voltage, right voltage]ᵀ + constexpr double trackwidth = 0.699; // m + constexpr double Kv_linear = 3.02; // V/(m/s) + constexpr double Ka_linear = 0.642; // V/(m/s²) + constexpr double Kv_angular = 1.382; // V/(m/s) + constexpr double Ka_angular = 0.08495; // V/(m/s²) + + auto v = (x(3) + x(4)) / 2.0; + + constexpr double A1 = + -(Kv_linear / Ka_linear + Kv_angular / Ka_angular) / 2.0; + constexpr double A2 = + -(Kv_linear / Ka_linear - Kv_angular / Ka_angular) / 2.0; + constexpr double B1 = 0.5 / Ka_linear + 0.5 / Ka_angular; + constexpr double B2 = 0.5 / Ka_linear - 0.5 / Ka_angular; + Eigen::Matrix A{{A1, A2}, {A2, A1}}; + Eigen::Matrix B{{B1, B2}, {B2, B1}}; + + sleipnir::VariableMatrix xdot{5}; + xdot(0) = v * sleipnir::cos(x(2)); + xdot(1) = v * sleipnir::sin(x(2)); + xdot(2) = (x(4) - x(3)) / trackwidth; + xdot.Segment(3, 2) = A * x.Segment(3, 2) + B * u; + return xdot; + }; constexpr std::chrono::duration minTimestep = 50ms; constexpr Eigen::Vector x_initial{{0.0, 0.0, 0.0, 0.0, 0.0}}; diff --git a/test/src/control/OCPSolverTest_Flywheel.cpp b/test/src/control/OCPSolverTest_Flywheel.cpp index b92f7054..23245075 100644 --- a/test/src/control/OCPSolverTest_Flywheel.cpp +++ b/test/src/control/OCPSolverTest_Flywheel.cpp @@ -23,7 +23,9 @@ bool Near(double expected, double actual, double tolerance) { } // namespace void TestFlywheel(std::string testName, double A, double B, - const sleipnir::DynamicsFunction& F, + const sleipnir::function_ref& F, sleipnir::DynamicsType dynamicsType, sleipnir::TranscriptionMethod method) { sleipnir::scope_exit exit{ @@ -146,9 +148,9 @@ TEST_CASE("OCPSolver - Flywheel (explicit)", "[OCPSolver]") { constexpr double A = -1.0; constexpr double B = 1.0; - auto f_ode = [=](sleipnir::Variable t, sleipnir::VariableMatrix x, - sleipnir::VariableMatrix u, - sleipnir::Variable dt) { return A * x + B * u; }; + auto f_ode = [=](sleipnir::VariableMatrix x, sleipnir::VariableMatrix u) { + return A * x + B * u; + }; TestFlywheel("OCPSolver Flywheel Explicit Collocation", A, B, f_ode, sleipnir::DynamicsType::kExplicitODE, @@ -169,8 +171,8 @@ TEST_CASE("OCPSolver - Flywheel (discrete)", "[OCPSolver]") { double A_discrete = std::exp(A * dt.count()); double B_discrete = (1.0 - A_discrete) * B; - auto f_discrete = [=](sleipnir::Variable t, sleipnir::VariableMatrix x, - sleipnir::VariableMatrix u, sleipnir::Variable dt) { + auto f_discrete = [=](sleipnir::VariableMatrix x, + sleipnir::VariableMatrix u) { return A_discrete * x + B_discrete * u; };