diff --git a/include/ungar/optimization/soft_sqp.hpp b/include/ungar/optimization/soft_sqp.hpp index 0629f08..dd826bb 100644 --- a/include/ungar/optimization/soft_sqp.hpp +++ b/include/ungar/optimization/soft_sqp.hpp @@ -107,6 +107,26 @@ class SoftSQPOptimizer { return _cache.xp.head(nlpProblem.objective.IndependentVariableSize()); } + Autodiff::Function MakeSoftInequalityConstraintFunction( + const Concepts::NLPProblem auto& nlpProblem) const { + auto Zsoft = [&](const VectorXad& variables, VectorXad& Zsoft) -> void { + Zsoft.resize(1_idx); + Zsoft << SoftInequalityConstraint{0.0, _stiffness, _epsilon}.Evaluate( + -variables); + }; + + std::string softIneqModelName = Utils::ToSnakeCase( + "soft_sqp_soft_ineq_sz_"s + + std::to_string(nlpProblem.inequalityConstraints.DependentVariableSize()) + "_k_" + + std::to_string(_stiffness) + "_eps_" + std::to_string(_epsilon)); + return Autodiff::MakeFunction({Zsoft, + nlpProblem.inequalityConstraints.DependentVariableSize(), + 0_idx, + softIneqModelName, + EnabledDerivatives::ALL}, + false); + } + private: template // clang-format off requires std::same_as @@ -203,26 +223,6 @@ class SoftSQPOptimizer { } } - Autodiff::Function MakeSoftInequalityConstraintFunction( - const Concepts::NLPProblem auto& nlpProblem) const { - auto Zsoft = [&](const VectorXad& variables, VectorXad& Zsoft) -> void { - Zsoft.resize(1_idx); - Zsoft << SoftInequalityConstraint{0.0, _stiffness, _epsilon}.Evaluate( - -variables); - }; - - std::string softIneqModelName = Utils::ToSnakeCase( - "soft_sqp_soft_ineq_sz_"s + - std::to_string(nlpProblem.inequalityConstraints.DependentVariableSize()) + "_k_" + - std::to_string(_stiffness) + "_eps_" + std::to_string(_epsilon)); - return Autodiff::MakeFunction({Zsoft, - nlpProblem.inequalityConstraints.DependentVariableSize(), - 0_idx, - softIneqModelName, - EnabledDerivatives::ALL}, - false); - } - template // clang-format off requires std::same_as real_t EvaluateSoftInequalityConstraints(const Concepts::NLPProblem auto& nlpProblem,