From 4ff442a0eee41ba79d050a45b6c24b60c4aa956e Mon Sep 17 00:00:00 2001 From: Chang-Hong Chen Date: Thu, 13 Mar 2025 16:58:44 +0900 Subject: [PATCH 1/8] Add hessian api for ipopt adapter --- ifopt_core/include/ifopt/problem.h | 9 ++++++++ ifopt_core/src/problem.cc | 4 ++++ ifopt_ipopt/include/ifopt/ipopt_adapter.h | 9 ++++++++ ifopt_ipopt/src/ipopt_adapter.cc | 25 +++++++++++++++++++++++ 4 files changed, 47 insertions(+) diff --git a/ifopt_core/include/ifopt/problem.h b/ifopt_core/include/ifopt/problem.h index 4a3321f..1f825d2 100644 --- a/ifopt_core/include/ifopt/problem.h +++ b/ifopt_core/include/ifopt/problem.h @@ -215,6 +215,15 @@ class Problem { */ Jacobian GetJacobianOfCosts() const; + /** + * @brief Extracts those entries from total hessian (combination of hessian from cost and constraints) that are not zero. + * @param [in] x The current values of the optimization variables. + * @param [in] obj_factor The scaling multiplier for the hessian of the objective function(cost). + * @param [in] lambda Lagrange Multipliers of constraints. + * @param [out] values The nonzero derivatives ordered by Eigen::RowMajor. + */ + void EvalNonzerosOfHessian(const double* x, double obj_factor, const double* lambda, double* values); + /** * @brief Saves the current values of the optimization variables in x_prev. * diff --git a/ifopt_core/src/problem.cc b/ifopt_core/src/problem.cc index d472563..7b2be01 100644 --- a/ifopt_core/src/problem.cc +++ b/ifopt_core/src/problem.cc @@ -153,6 +153,10 @@ Problem::Jacobian Problem::GetJacobianOfCosts() const return costs_.GetJacobian(); } +void Problem::EvalNonzerosOfHessian(const double* x, double obj_factor, const double* lambda, double* values) +{ +} + void Problem::SaveCurrent() { x_prev.push_back(variables_->GetValues()); diff --git a/ifopt_ipopt/include/ifopt/ipopt_adapter.h b/ifopt_ipopt/include/ifopt/ipopt_adapter.h index 44e6935..cf313fe 100644 --- a/ifopt_ipopt/include/ifopt/ipopt_adapter.h +++ b/ifopt_ipopt/include/ifopt/ipopt_adapter.h @@ -102,6 +102,15 @@ class IpoptAdapter : public TNLP { Index nele_jac, Index* iRow, Index* jCol, double* values); + /** Method to return: + * 1) The structure of the Hessian of the Lagrangian (if "values" is NULL) + * 2) The values of the Hessian of the Lagrangian (if "values" is not NULL) + */ + virtual bool eval_h(Index n, const double* x, bool new_x, double obj_factor, + Index m, const double* lambda, bool new_lambda, + Index nele_hess, Index* iRow, Index* jCol, + double* values); + /** This is called after every iteration and is used to save intermediate * solutions in the nlp */ virtual bool intermediate_callback(AlgorithmMode mode, Index iter, diff --git a/ifopt_ipopt/src/ipopt_adapter.cc b/ifopt_ipopt/src/ipopt_adapter.cc index 7ddda8a..5ab9f6c 100644 --- a/ifopt_ipopt/src/ipopt_adapter.cc +++ b/ifopt_ipopt/src/ipopt_adapter.cc @@ -147,6 +147,31 @@ bool IpoptAdapter::eval_jac_g(Index n, const double* x, bool new_x, Index m, return true; } +bool IpoptAdapter::eval_h(Index n, const double* x, bool new_x, double obj_factor, + Index m, const double* lambda, bool new_lambda, + Index nele_hess, Index* iRow, Index* jCol, + double* values) +{ + // defines the positions of the nonzero elements of the hessian + if (values == NULL) { + Index nele = 0; + for (Index row = 0; row < n; row++) { + for (Index col = row; col < n; col++) { // only need upper triangular part because hessian is symmetry matrix + iRow[nele] = row; + jCol[nele] = col; + nele++; + } + } + + assert(nele == + nele_hess); // initial sparsity structure is never allowed to change + } else { + // only gets used if "hessian_approximation" option is "exact" + nlp_->EvalNonzerosOfHessian(x, obj_factor, lambda, values); + } + return true; +} + bool IpoptAdapter::intermediate_callback( AlgorithmMode mode, Index iter, double obj_value, double inf_pr, double inf_du, double mu, double d_norm, double regularization_size, From a5da3df900586867c65fd37b0f57d7652106a406 Mon Sep 17 00:00:00 2001 From: Chang-Hong Chen Date: Fri, 14 Mar 2025 14:05:56 +0900 Subject: [PATCH 2/8] Implement GetHessians for components --- ifopt_core/include/ifopt/composite.h | 5 +++++ ifopt_core/include/ifopt/constraint_set.h | 7 +++++++ ifopt_core/include/ifopt/problem.h | 6 +++++- ifopt_core/include/ifopt/variable_set.h | 4 ++++ ifopt_core/src/composite.cc | 19 ++++++++++++++++++ ifopt_core/src/leaves.cc | 24 +++++++++++++++++++++++ ifopt_core/src/problem.cc | 11 +++++++++++ ifopt_core/test/composite_test.cc | 4 ++++ 8 files changed, 79 insertions(+), 1 deletion(-) diff --git a/ifopt_core/include/ifopt/composite.h b/ifopt_core/include/ifopt/composite.h index 7f7a5a5..03f2b2c 100644 --- a/ifopt_core/include/ifopt/composite.h +++ b/ifopt_core/include/ifopt/composite.h @@ -67,6 +67,8 @@ class Component { using Jacobian = Eigen::SparseMatrix; using VectorXd = Eigen::VectorXd; using VecBound = std::vector; + using Hessian = Eigen::SparseMatrix; + using HessianTriplet = std::vector>; /** * @brief Creates a component. @@ -116,6 +118,8 @@ class Component { */ virtual Jacobian GetJacobian() const = 0; + virtual std::vector GetHessians() const = 0; + /** * @brief Returns the number of rows of this component. */ @@ -176,6 +180,7 @@ class Composite : public Component { // see Component for documentation VectorXd GetValues() const override; Jacobian GetJacobian() const override; + std::vector GetHessians() const override; VecBound GetBounds() const override; void SetVariables(const VectorXd& x) override; void PrintAll() const; diff --git a/ifopt_core/include/ifopt/constraint_set.h b/ifopt_core/include/ifopt/constraint_set.h index da258f7..5276cc4 100644 --- a/ifopt_core/include/ifopt/constraint_set.h +++ b/ifopt_core/include/ifopt/constraint_set.h @@ -107,6 +107,13 @@ class ConstraintSet : public Component { virtual void FillJacobianBlock(std::string var_set, Jacobian& jac_block) const = 0; + + std::vector GetHessians() const final; + virtual void FillHessianTriplets(std::vector var_set_list, + std::vector& triplets_list) const + { + } + protected: /** * @brief Read access to the value of the optimization variables. diff --git a/ifopt_core/include/ifopt/problem.h b/ifopt_core/include/ifopt/problem.h index 1f825d2..67bbf35 100644 --- a/ifopt_core/include/ifopt/problem.h +++ b/ifopt_core/include/ifopt/problem.h @@ -98,6 +98,7 @@ class Problem { using VecBound = Component::VecBound; using Jacobian = Component::Jacobian; using VectorXd = Component::VectorXd; + using Hessian = Component::Hessian; /** * @brief Creates a optimization problem with no variables, costs or constraints. @@ -219,11 +220,14 @@ class Problem { * @brief Extracts those entries from total hessian (combination of hessian from cost and constraints) that are not zero. * @param [in] x The current values of the optimization variables. * @param [in] obj_factor The scaling multiplier for the hessian of the objective function(cost). - * @param [in] lambda Lagrange Multipliers of constraints. + * @param [in] lambda Lagrange Multipliers of the constraints. * @param [out] values The nonzero derivatives ordered by Eigen::RowMajor. */ void EvalNonzerosOfHessian(const double* x, double obj_factor, const double* lambda, double* values); + std::vector GetHessianOfConstraints() const; + std::vector GetHessianOfCosts() const; + /** * @brief Saves the current values of the optimization variables in x_prev. * diff --git a/ifopt_core/include/ifopt/variable_set.h b/ifopt_core/include/ifopt/variable_set.h index 9fc9ece..330199d 100644 --- a/ifopt_core/include/ifopt/variable_set.h +++ b/ifopt_core/include/ifopt/variable_set.h @@ -58,6 +58,10 @@ class VariableSet : public Component { { throw std::runtime_error("not implemented for variables"); }; + std::vector GetHessians() const final + { + throw std::runtime_error("not implemented for variables"); + } }; } // namespace ifopt diff --git a/ifopt_core/src/composite.cc b/ifopt_core/src/composite.cc index 831aeba..900715b 100644 --- a/ifopt_core/src/composite.cc +++ b/ifopt_core/src/composite.cc @@ -175,6 +175,25 @@ Composite::Jacobian Composite::GetJacobian() const return jacobian; } +std::vector Composite::GetHessians() const +{ + if (n_var == -1) + n_var = components_.empty() ? 0 : components_.front()->GetHessians().front().cols(); + + std::vector hessians; + hessians.reserve(GetRows()); // reserve space in the vector to avoid reallocations + + if (n_var == 0) + return hessians; + + for (const auto& c : components_) { + const std::vector& hess = c->GetHessians(); + hessians.insert(hessians.end(), hess.begin(), hess.end()); + } + + return hessians; +} + Composite::VecBound Composite::GetBounds() const { VecBound bounds_; diff --git a/ifopt_core/src/leaves.cc b/ifopt_core/src/leaves.cc index 6e03800..0993407 100644 --- a/ifopt_core/src/leaves.cc +++ b/ifopt_core/src/leaves.cc @@ -73,6 +73,30 @@ ConstraintSet::Jacobian ConstraintSet::GetJacobian() const return jacobian; } +std::vector ConstraintSet::GetHessians() const +{ + std::vector triplets_list; + + int nrows; + std::vector variable_names; + for (const auto& vars : variables_->GetComponents()) { + nrows += vars->GetRows(); + variable_names.push_back(vars->GetName()); + } + + FillHessianTriplets(variable_names, triplets_list); + + std::vector hessians; + hessians.reserve(GetRows()); // reserve space in the vector to avoid reallocations + + for (const auto& triplets : triplets_list) { + Eigen::SparseMatrix H(nrows, nrows); + H.setFromTriplets(triplets.begin(), triplets.end()); // efficiently construct sparse matrix + hessians.push_back(H); + } + return hessians; +} + void ConstraintSet::LinkWithVariables(const VariablesPtr& x) { variables_ = x; diff --git a/ifopt_core/src/problem.cc b/ifopt_core/src/problem.cc index 7b2be01..646b78a 100644 --- a/ifopt_core/src/problem.cc +++ b/ifopt_core/src/problem.cc @@ -157,6 +157,17 @@ void Problem::EvalNonzerosOfHessian(const double* x, double obj_factor, const do { } +std::vector Problem::GetHessianOfConstraints() const +{ + return constraints_.GetHessians(); +} + +std::vector Problem::GetHessianOfCosts() const +{ + return costs_.GetHessians(); +} + + void Problem::SaveCurrent() { x_prev.push_back(variables_->GetValues()); diff --git a/ifopt_core/test/composite_test.cc b/ifopt_core/test/composite_test.cc index 77d2cb3..6d665dd 100644 --- a/ifopt_core/test/composite_test.cc +++ b/ifopt_core/test/composite_test.cc @@ -48,6 +48,10 @@ class ExComponent : public Component { { throw std::runtime_error("not implemented"); }; + virtual std::vector GetHessians() const + { + throw std::runtime_error("not implemented"); + }; virtual void SetVariables(const VectorXd& x){}; }; From 2a81879589bcdb3dc1dcc3c393a7a269202008eb Mon Sep 17 00:00:00 2001 From: Chang-Hong Chen Date: Fri, 14 Mar 2025 15:11:57 +0900 Subject: [PATCH 3/8] Extract hessian sparse matrix to ipopt lib format --- ifopt_core/src/problem.cc | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/ifopt_core/src/problem.cc b/ifopt_core/src/problem.cc index 646b78a..2fafd6f 100644 --- a/ifopt_core/src/problem.cc +++ b/ifopt_core/src/problem.cc @@ -30,6 +30,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include namespace ifopt { @@ -155,6 +156,35 @@ Problem::Jacobian Problem::GetJacobianOfCosts() const void Problem::EvalNonzerosOfHessian(const double* x, double obj_factor, const double* lambda, double* values) { + SetVariables(x); + std::vector hessian_of_costs = GetHessianOfCosts(); + std::vector hessian_of_constraints = GetHessianOfConstraints(); + + int dim = GetNumberOfOptimizationVariables(); + + // dimension of hessian matrix should be n by n where n is the number of optimization variables + assert(hessian_of_costs.empty() | hessian_of_costs[0].rows() == hessian_of_costs[0].cols() == dim); + assert(hessian_of_constraints.empty() | hessian_of_constraints[0].rows() == hessian_of_constraints[0].cols() == dim); + + if (hessian_of_costs.empty() && hessian_of_constraints.empty()) + return; + + // hessian from costs + Hessian total_hessian = obj_factor * std::accumulate(hessian_of_costs.begin(), hessian_of_costs.end(), Hessian(dim, dim)); + // hessian from constraints + for (int i = 0; i < (int)hessian_of_constraints.size(); ++i) { + total_hessian += lambda[i] * hessian_of_constraints[i]; + } + + // only need upper triangular values because hessian matrix is a symmetry matrix + for (int k = 0; k < total_hessian.outerSize(); ++k) { + for (Hessian::InnerIterator it(total_hessian, k); it; ++it) { + if (it.row() <= it.col()) { // check if it is upper triangular + int flat_index = it.row() * dim + it.col(); + values[flat_index] = it.value(); + } + } + } } std::vector Problem::GetHessianOfConstraints() const From 3de514206842582564f539cfb78283869fa89cb6 Mon Sep 17 00:00:00 2001 From: Chang-Hong Chen Date: Mon, 17 Mar 2025 18:34:28 +0900 Subject: [PATCH 4/8] Connect ifopt lib to ipopt for hessian function --- ifopt_core/include/ifopt/problem.h | 1 + ifopt_core/src/composite.cc | 4 +++- ifopt_core/src/leaves.cc | 7 +++++-- ifopt_core/src/problem.cc | 31 +++++++++++++++++++++++++----- ifopt_ipopt/src/ipopt_adapter.cc | 18 ++++++++++++----- 5 files changed, 48 insertions(+), 13 deletions(-) diff --git a/ifopt_core/include/ifopt/problem.h b/ifopt_core/include/ifopt/problem.h index 67bbf35..2f3a103 100644 --- a/ifopt_core/include/ifopt/problem.h +++ b/ifopt_core/include/ifopt/problem.h @@ -225,6 +225,7 @@ class Problem { */ void EvalNonzerosOfHessian(const double* x, double obj_factor, const double* lambda, double* values); + Hessian GetTotalHessian() const; std::vector GetHessianOfConstraints() const; std::vector GetHessianOfCosts() const; diff --git a/ifopt_core/src/composite.cc b/ifopt_core/src/composite.cc index 900715b..472c2ab 100644 --- a/ifopt_core/src/composite.cc +++ b/ifopt_core/src/composite.cc @@ -188,7 +188,9 @@ std::vector Composite::GetHessians() const for (const auto& c : components_) { const std::vector& hess = c->GetHessians(); - hessians.insert(hessians.end(), hess.begin(), hess.end()); + if (!hess.empty()) { + hessians.insert(hessians.end(), hess.begin(), hess.end()); + } } return hessians; diff --git a/ifopt_core/src/leaves.cc b/ifopt_core/src/leaves.cc index 0993407..d0e2a8e 100644 --- a/ifopt_core/src/leaves.cc +++ b/ifopt_core/src/leaves.cc @@ -76,19 +76,22 @@ ConstraintSet::Jacobian ConstraintSet::GetJacobian() const std::vector ConstraintSet::GetHessians() const { std::vector triplets_list; + triplets_list.reserve(GetRows()); - int nrows; std::vector variable_names; for (const auto& vars : variables_->GetComponents()) { - nrows += vars->GetRows(); variable_names.push_back(vars->GetName()); } FillHessianTriplets(variable_names, triplets_list); std::vector hessians; + if (triplets_list.empty()) { + return hessians; + } hessians.reserve(GetRows()); // reserve space in the vector to avoid reallocations + int nrows = variables_->GetRows(); for (const auto& triplets : triplets_list) { Eigen::SparseMatrix H(nrows, nrows); H.setFromTriplets(triplets.begin(), triplets.end()); // efficiently construct sparse matrix diff --git a/ifopt_core/src/problem.cc b/ifopt_core/src/problem.cc index 2fafd6f..c9c3150 100644 --- a/ifopt_core/src/problem.cc +++ b/ifopt_core/src/problem.cc @@ -163,8 +163,8 @@ void Problem::EvalNonzerosOfHessian(const double* x, double obj_factor, const do int dim = GetNumberOfOptimizationVariables(); // dimension of hessian matrix should be n by n where n is the number of optimization variables - assert(hessian_of_costs.empty() | hessian_of_costs[0].rows() == hessian_of_costs[0].cols() == dim); - assert(hessian_of_constraints.empty() | hessian_of_constraints[0].rows() == hessian_of_constraints[0].cols() == dim); + assert(hessian_of_costs.empty() || (hessian_of_costs[0].rows() == dim && hessian_of_costs[0].cols() == dim)); + assert(hessian_of_constraints.empty() || (hessian_of_constraints[0].rows() == dim && hessian_of_constraints[0].cols() == dim)); if (hessian_of_costs.empty() && hessian_of_constraints.empty()) return; @@ -177,16 +177,37 @@ void Problem::EvalNonzerosOfHessian(const double* x, double obj_factor, const do } // only need upper triangular values because hessian matrix is a symmetry matrix + int index = 0; for (int k = 0; k < total_hessian.outerSize(); ++k) { for (Hessian::InnerIterator it(total_hessian, k); it; ++it) { - if (it.row() <= it.col()) { // check if it is upper triangular - int flat_index = it.row() * dim + it.col(); - values[flat_index] = it.value(); + if (it.col() < it.row()) { + continue; } + values[index] = it.value(); + index++; } } } +Problem::Hessian Problem::GetTotalHessian() const +{ + std::vector hessian_of_costs = GetHessianOfCosts(); + std::vector hessian_of_constraints = GetHessianOfConstraints(); + + int dim = GetNumberOfOptimizationVariables(); + + // dimension of hessian matrix should be n by n where n is the number of optimization variables + assert(hessian_of_costs.empty() || (hessian_of_costs[0].rows() == dim && hessian_of_costs[0].cols() == dim)); + assert(hessian_of_constraints.empty() || (hessian_of_constraints[0].rows() == dim && hessian_of_constraints[0].cols() == dim)); + + // hessian from costs + Hessian total_hessian = std::accumulate(hessian_of_costs.begin(), hessian_of_costs.end(), Hessian(dim, dim)); + // hessian from constraints + total_hessian = std::accumulate(hessian_of_constraints.begin(), hessian_of_constraints.end(), total_hessian); + + return total_hessian; +} + std::vector Problem::GetHessianOfConstraints() const { return constraints_.GetHessians(); diff --git a/ifopt_ipopt/src/ipopt_adapter.cc b/ifopt_ipopt/src/ipopt_adapter.cc index 5ab9f6c..ee9be44 100644 --- a/ifopt_ipopt/src/ipopt_adapter.cc +++ b/ifopt_ipopt/src/ipopt_adapter.cc @@ -45,7 +45,10 @@ bool IpoptAdapter::get_nlp_info(Index& n, Index& m, Index& nnz_jac_g, else nnz_jac_g = nlp_->GetJacobianOfConstraints().nonZeros(); - nnz_h_lag = n * n; + if (finite_diff_) + nnz_h_lag = n * n; + else + nnz_h_lag = (nlp_->GetTotalHessian().nonZeros() - n) / 2 + n; // only need the upper triangular values because hessian is a symmetry matrix // start index at 0 for row/col entries index_style = C_STYLE; @@ -155,10 +158,15 @@ bool IpoptAdapter::eval_h(Index n, const double* x, bool new_x, double obj_facto // defines the positions of the nonzero elements of the hessian if (values == NULL) { Index nele = 0; - for (Index row = 0; row < n; row++) { - for (Index col = row; col < n; col++) { // only need upper triangular part because hessian is symmetry matrix - iRow[nele] = row; - jCol[nele] = col; + auto hess = nlp_->GetTotalHessian(); + for (int k = 0; k < hess.outerSize(); ++k) { + for (Jacobian::InnerIterator it(hess, k); it; ++it) { + if (it.col() < it.row()) { + // only need the upper triangular values because hessian is a symmetry matrix + continue; + } + iRow[nele] = it.row(); + jCol[nele] = it.col(); nele++; } } From 1f38839ece813c80a128105cc0162b4ac70fc394 Mon Sep 17 00:00:00 2001 From: Chang-Hong Chen Date: Tue, 18 Mar 2025 09:07:15 +0900 Subject: [PATCH 5/8] Fix hessian diagonal contain zero value --- ifopt_ipopt/src/ipopt_adapter.cc | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/ifopt_ipopt/src/ipopt_adapter.cc b/ifopt_ipopt/src/ipopt_adapter.cc index ee9be44..25aa486 100644 --- a/ifopt_ipopt/src/ipopt_adapter.cc +++ b/ifopt_ipopt/src/ipopt_adapter.cc @@ -45,10 +45,24 @@ bool IpoptAdapter::get_nlp_info(Index& n, Index& m, Index& nnz_jac_g, else nnz_jac_g = nlp_->GetJacobianOfConstraints().nonZeros(); - if (finite_diff_) + auto hess = nlp_->GetTotalHessian(); + if (hess.nonZeros() == 0) { + // not using custom hessian nnz_h_lag = n * n; - else - nnz_h_lag = (nlp_->GetTotalHessian().nonZeros() - n) / 2 + n; // only need the upper triangular values because hessian is a symmetry matrix + } + else { + Index nele = 0; + for (int k = 0; k < hess.outerSize(); ++k) { + for (Jacobian::InnerIterator it(hess, k); it; ++it) { + if (it.col() < it.row()) { + // only need the upper triangular values because hessian is a symmetry matrix + continue; + } + nele++; + } + } + nnz_h_lag = nele; + } // start index at 0 for row/col entries index_style = C_STYLE; From be85d603221dac28bf82d6c51ab0e588b045e68c Mon Sep 17 00:00:00 2001 From: Chang-Hong Chen Date: Tue, 18 Mar 2025 14:43:16 +0900 Subject: [PATCH 6/8] Make defining hessian for multiple constraints more flexible Computing hessian requires large memory and long duration when there are many constraints and variables. Allow the user to skip constraint that has 0 hessian value can improve performance. --- ifopt_core/include/ifopt/composite.h | 5 ++- ifopt_core/include/ifopt/constraint_set.h | 3 +- ifopt_core/include/ifopt/problem.h | 5 ++- ifopt_core/include/ifopt/variable_set.h | 2 +- ifopt_core/src/composite.cc | 42 +++++++++++++------ ifopt_core/src/leaves.cc | 31 ++++++++------ ifopt_core/src/problem.cc | 20 +++++---- ifopt_core/test/composite_test.cc | 2 +- ifopt_core/test/ifopt/test_vars_constr_cost.h | 33 +++++++++++++++ ifopt_ipopt/test/ex_test_ipopt.cc | 1 + 10 files changed, 104 insertions(+), 40 deletions(-) diff --git a/ifopt_core/include/ifopt/composite.h b/ifopt_core/include/ifopt/composite.h index 03f2b2c..8953127 100644 --- a/ifopt_core/include/ifopt/composite.h +++ b/ifopt_core/include/ifopt/composite.h @@ -69,6 +69,7 @@ class Component { using VecBound = std::vector; using Hessian = Eigen::SparseMatrix; using HessianTriplet = std::vector>; + using RowIndicesHessiansPair = std::pair, std::vector>; /** * @brief Creates a component. @@ -118,7 +119,7 @@ class Component { */ virtual Jacobian GetJacobian() const = 0; - virtual std::vector GetHessians() const = 0; + virtual RowIndicesHessiansPair GetHessians() const = 0; /** * @brief Returns the number of rows of this component. @@ -180,7 +181,7 @@ class Composite : public Component { // see Component for documentation VectorXd GetValues() const override; Jacobian GetJacobian() const override; - std::vector GetHessians() const override; + RowIndicesHessiansPair GetHessians() const override; VecBound GetBounds() const override; void SetVariables(const VectorXd& x) override; void PrintAll() const; diff --git a/ifopt_core/include/ifopt/constraint_set.h b/ifopt_core/include/ifopt/constraint_set.h index 5276cc4..f40dbed 100644 --- a/ifopt_core/include/ifopt/constraint_set.h +++ b/ifopt_core/include/ifopt/constraint_set.h @@ -108,8 +108,9 @@ class ConstraintSet : public Component { Jacobian& jac_block) const = 0; - std::vector GetHessians() const final; + RowIndicesHessiansPair GetHessians() const final; virtual void FillHessianTriplets(std::vector var_set_list, + std::vector& hessian_row_index_list, std::vector& triplets_list) const { } diff --git a/ifopt_core/include/ifopt/problem.h b/ifopt_core/include/ifopt/problem.h index 2f3a103..205222f 100644 --- a/ifopt_core/include/ifopt/problem.h +++ b/ifopt_core/include/ifopt/problem.h @@ -99,6 +99,7 @@ class Problem { using Jacobian = Component::Jacobian; using VectorXd = Component::VectorXd; using Hessian = Component::Hessian; + using RowIndicesHessiansPair = Component::RowIndicesHessiansPair; /** * @brief Creates a optimization problem with no variables, costs or constraints. @@ -226,8 +227,8 @@ class Problem { void EvalNonzerosOfHessian(const double* x, double obj_factor, const double* lambda, double* values); Hessian GetTotalHessian() const; - std::vector GetHessianOfConstraints() const; - std::vector GetHessianOfCosts() const; + RowIndicesHessiansPair GetHessianOfConstraints() const; + RowIndicesHessiansPair GetHessianOfCosts() const; /** * @brief Saves the current values of the optimization variables in x_prev. diff --git a/ifopt_core/include/ifopt/variable_set.h b/ifopt_core/include/ifopt/variable_set.h index 330199d..f091df9 100644 --- a/ifopt_core/include/ifopt/variable_set.h +++ b/ifopt_core/include/ifopt/variable_set.h @@ -58,7 +58,7 @@ class VariableSet : public Component { { throw std::runtime_error("not implemented for variables"); }; - std::vector GetHessians() const final + RowIndicesHessiansPair GetHessians() const final { throw std::runtime_error("not implemented for variables"); } diff --git a/ifopt_core/src/composite.cc b/ifopt_core/src/composite.cc index 472c2ab..aa395a2 100644 --- a/ifopt_core/src/composite.cc +++ b/ifopt_core/src/composite.cc @@ -175,25 +175,41 @@ Composite::Jacobian Composite::GetJacobian() const return jacobian; } -std::vector Composite::GetHessians() const +Composite::RowIndicesHessiansPair Composite::GetHessians() const { - if (n_var == -1) - n_var = components_.empty() ? 0 : components_.front()->GetHessians().front().cols(); - - std::vector hessians; - hessians.reserve(GetRows()); // reserve space in the vector to avoid reallocations + if (n_var == -1 && components_.empty()) + n_var = 0; + RowIndicesHessiansPair row_indices_hessians_pair; if (n_var == 0) - return hessians; + return row_indices_hessians_pair; + + std::vector& hessian_row_indices = row_indices_hessians_pair.first; + hessian_row_indices.reserve(GetRows()); + std::vector& hessians = row_indices_hessians_pair.second; + hessians.reserve(GetRows()); // reserve space in the vector to avoid reallocations + + int offset; // offset for row indices in different component for (const auto& c : components_) { - const std::vector& hess = c->GetHessians(); - if (!hess.empty()) { - hessians.insert(hessians.end(), hess.begin(), hess.end()); - } + const RowIndicesHessiansPair& local_row_indices_hessians = c->GetHessians(); + const std::vector& row_indices = local_row_indices_hessians.first; + const std::vector& hess = local_row_indices_hessians.second; + assert(row_indices.size() == hess.size()); + + for(int i = 0; i < row_indices.size(); ++i) { + hessian_row_indices.push_back(row_indices[i] + offset); + } + if (!hess.empty()) { + if (n_var == -1) + n_var = hess.front().cols(); + + hessians.insert(hessians.end(), hess.begin(), hess.end()); + } + + offset += c->GetRows(); } - - return hessians; + return row_indices_hessians_pair; } Composite::VecBound Composite::GetBounds() const diff --git a/ifopt_core/src/leaves.cc b/ifopt_core/src/leaves.cc index d0e2a8e..ac07fd4 100644 --- a/ifopt_core/src/leaves.cc +++ b/ifopt_core/src/leaves.cc @@ -73,31 +73,38 @@ ConstraintSet::Jacobian ConstraintSet::GetJacobian() const return jacobian; } -std::vector ConstraintSet::GetHessians() const +ConstraintSet::RowIndicesHessiansPair ConstraintSet::GetHessians() const { std::vector triplets_list; triplets_list.reserve(GetRows()); + // hessians and their corresponding row index + RowIndicesHessiansPair row_indices_hessians_pair; + std::vector& hessian_row_indices = row_indices_hessians_pair.first; + hessian_row_indices.reserve(GetRows()); + std::vector variable_names; for (const auto& vars : variables_->GetComponents()) { variable_names.push_back(vars->GetName()); } - FillHessianTriplets(variable_names, triplets_list); + FillHessianTriplets(variable_names, hessian_row_indices, triplets_list); + assert(hessian_row_indices.size() == triplets_list.size()); - std::vector hessians; - if (triplets_list.empty()) { - return hessians; - } - hessians.reserve(GetRows()); // reserve space in the vector to avoid reallocations + if (triplets_list.empty()) + return {}; + + std::vector& hessians = row_indices_hessians_pair.second; + hessians.reserve(hessian_row_indices.size()); int nrows = variables_->GetRows(); - for (const auto& triplets : triplets_list) { - Eigen::SparseMatrix H(nrows, nrows); - H.setFromTriplets(triplets.begin(), triplets.end()); // efficiently construct sparse matrix - hessians.push_back(H); + for (int i = 0; i < hessian_row_indices.size(); ++i) { + Eigen::SparseMatrix H(nrows, nrows); + H.setFromTriplets(triplets_list[i].begin(), triplets_list[i].end()); // efficiently construct sparse matrix + hessians.push_back(std::move(H)); } - return hessians; + + return row_indices_hessians_pair; } void ConstraintSet::LinkWithVariables(const VariablesPtr& x) diff --git a/ifopt_core/src/problem.cc b/ifopt_core/src/problem.cc index c9c3150..8b5f964 100644 --- a/ifopt_core/src/problem.cc +++ b/ifopt_core/src/problem.cc @@ -157,8 +157,10 @@ Problem::Jacobian Problem::GetJacobianOfCosts() const void Problem::EvalNonzerosOfHessian(const double* x, double obj_factor, const double* lambda, double* values) { SetVariables(x); - std::vector hessian_of_costs = GetHessianOfCosts(); - std::vector hessian_of_constraints = GetHessianOfConstraints(); + const std::vector& hessian_of_costs = GetHessianOfCosts().second; + const RowIndicesHessiansPair& row_indices_hessians_pair_of_constraints = GetHessianOfConstraints(); + const std::vector& row_indices_of_constraints = row_indices_hessians_pair_of_constraints.first; + const std::vector& hessian_of_constraints = row_indices_hessians_pair_of_constraints.second; int dim = GetNumberOfOptimizationVariables(); @@ -172,8 +174,10 @@ void Problem::EvalNonzerosOfHessian(const double* x, double obj_factor, const do // hessian from costs Hessian total_hessian = obj_factor * std::accumulate(hessian_of_costs.begin(), hessian_of_costs.end(), Hessian(dim, dim)); // hessian from constraints - for (int i = 0; i < (int)hessian_of_constraints.size(); ++i) { - total_hessian += lambda[i] * hessian_of_constraints[i]; + for (int i = 0; i < (int)row_indices_of_constraints.size(); ++i) { + int row_index = row_indices_of_constraints[i]; + const Hessian& hess = hessian_of_constraints[i]; + total_hessian += lambda[row_index] * hess; } // only need upper triangular values because hessian matrix is a symmetry matrix @@ -191,8 +195,8 @@ void Problem::EvalNonzerosOfHessian(const double* x, double obj_factor, const do Problem::Hessian Problem::GetTotalHessian() const { - std::vector hessian_of_costs = GetHessianOfCosts(); - std::vector hessian_of_constraints = GetHessianOfConstraints(); + const std::vector& hessian_of_costs = GetHessianOfCosts().second; + const std::vector& hessian_of_constraints = GetHessianOfConstraints().second; int dim = GetNumberOfOptimizationVariables(); @@ -208,12 +212,12 @@ Problem::Hessian Problem::GetTotalHessian() const return total_hessian; } -std::vector Problem::GetHessianOfConstraints() const +Problem::RowIndicesHessiansPair Problem::GetHessianOfConstraints() const { return constraints_.GetHessians(); } -std::vector Problem::GetHessianOfCosts() const +Problem::RowIndicesHessiansPair Problem::GetHessianOfCosts() const { return costs_.GetHessians(); } diff --git a/ifopt_core/test/composite_test.cc b/ifopt_core/test/composite_test.cc index 6d665dd..758c343 100644 --- a/ifopt_core/test/composite_test.cc +++ b/ifopt_core/test/composite_test.cc @@ -48,7 +48,7 @@ class ExComponent : public Component { { throw std::runtime_error("not implemented"); }; - virtual std::vector GetHessians() const + virtual RowIndicesHessiansPair GetHessians() const { throw std::runtime_error("not implemented"); }; diff --git a/ifopt_core/test/ifopt/test_vars_constr_cost.h b/ifopt_core/test/ifopt/test_vars_constr_cost.h index cbe9e8a..c72c3c5 100644 --- a/ifopt_core/test/ifopt/test_vars_constr_cost.h +++ b/ifopt_core/test/ifopt/test_vars_constr_cost.h @@ -144,6 +144,23 @@ class ExConstraint : public ConstraintSet { 1.0; // derivative of first constraint w.r.t x1 } } + + void FillHessianTriplets(std::vector var_set_list, + std::vector& hessian_row_index_list, + std::vector& triplets_list) const override + { + // only 1 constraint in this problem + HessianTriplet constraint1_triplets; + for (int i = 0; i < (int)var_set_list.size(); ++i) { + const std::string& var_set = var_set_list[i]; + if (var_set == "var_set1") { + constraint1_triplets.emplace_back(i, i, 2.0); // H(0, 0) = 2.0, dx0^2 + } + } + // hessian info for the first constraint + hessian_row_index_list.push_back(0); + triplets_list.push_back(std::move(constraint1_triplets)); + } }; class ExCost : public CostTerm { @@ -166,6 +183,22 @@ class ExCost : public CostTerm { jac.coeffRef(0, 1) = -2.0 * (x(1) - 2.0); // derivative of cost w.r.t x1 } } + + void FillHessianTriplets(std::vector var_set_list, + std::vector& hessian_row_index_list, + std::vector& triplets_list) const override + { + HessianTriplet cost_triplets; + for (int i = 0; i < (int)var_set_list.size(); ++i) { + const std::string& var_set = var_set_list[i]; + if (var_set == "var_set1") { + cost_triplets.emplace_back(i, i, -2.0); // H(0, 0) = -2.0, dx0^2 + } + } + // hessian info for the only 1 cost + hessian_row_index_list.push_back(0); + triplets_list.push_back(std::move(cost_triplets)); + } }; } // namespace ifopt diff --git a/ifopt_ipopt/test/ex_test_ipopt.cc b/ifopt_ipopt/test/ex_test_ipopt.cc index 3229695..1dc70ae 100644 --- a/ifopt_ipopt/test/ex_test_ipopt.cc +++ b/ifopt_ipopt/test/ex_test_ipopt.cc @@ -45,6 +45,7 @@ int main() IpoptSolver ipopt; ipopt.SetOption("linear_solver", "mumps"); ipopt.SetOption("jacobian_approximation", "exact"); + ipopt.SetOption("hessian_approximation", "exact"); // 3 . solve ipopt.Solve(nlp); From 79119dc253aeaeab94e3a234a503047d28c707b9 Mon Sep 17 00:00:00 2001 From: Chang-Hong Chen Date: Thu, 20 Mar 2025 10:19:04 +0900 Subject: [PATCH 7/8] Add docstring and comments for hessian related function --- ifopt_core/include/ifopt/constraint_set.h | 37 ++++++++++++- ifopt_core/include/ifopt/problem.h | 53 +++++++++++++++++-- ifopt_core/test/ifopt/test_vars_constr_cost.h | 5 ++ 3 files changed, 90 insertions(+), 5 deletions(-) diff --git a/ifopt_core/include/ifopt/constraint_set.h b/ifopt_core/include/ifopt/constraint_set.h index f40dbed..830399d 100644 --- a/ifopt_core/include/ifopt/constraint_set.h +++ b/ifopt_core/include/ifopt/constraint_set.h @@ -107,8 +107,43 @@ class ConstraintSet : public Component { virtual void FillJacobianBlock(std::string var_set, Jacobian& jac_block) const = 0; - + /** + * @brief The second-order derivatives (Hessians) for these constraints and variables. + * + * This function returns a pair containing two vectors. The first vector holds + * the indices of the corresponding constraints or costs, while the second vector + * contains the Hessian matrices as sparse representations. + * + * Assuming @c n constraints or costs and @c m variables, each Hessian matrix + * in the second vector has dimensions m x m, representing the second-order + * derivatives of a specific constraint or cost function with respect to the + * optimization variables. + * + * This function only combines the user-defined Hessians from + * FillHessianTriplets(). + */ RowIndicesHessiansPair GetHessians() const final; + + /** + * @brief Set individual Hessians corresponding to each constraint or cost function. + * @param var_set_list The full set of variables involved in Hessian computation. + * @param hessian_row_index_list Indices of the corresponding constraints or costs. + * @param triplets_list Nonzero elements of each Hessian, stored as triplets. + * + * This function provides the second-order derivatives (Hessians) of constraints + * or costs with respect to the optimization variables. Unlike the Jacobian, Hessians + * involve cross-derivatives between different variable sets, so the full variable + * set is provided. + * + * The Hessians are stored efficiently in triplet form to minimize memory usage, + * as Hessian matrices are typically much sparser than Jacobians. Each entry in + * @c triplets_list corresponds to a specific constraint or cost function, with + * its nonzero elements stored as Eigen triplets. + * + * If a constraint or cost does not require a Hessian, it should simply be omitted + * from @c hessian_row_index_list and @c triplets_list. The missing entries will + * be treated as empty Hessians. + */ virtual void FillHessianTriplets(std::vector var_set_list, std::vector& hessian_row_index_list, std::vector& triplets_list) const diff --git a/ifopt_core/include/ifopt/problem.h b/ifopt_core/include/ifopt/problem.h index 205222f..ef5699a 100644 --- a/ifopt_core/include/ifopt/problem.h +++ b/ifopt_core/include/ifopt/problem.h @@ -218,16 +218,61 @@ class Problem { Jacobian GetJacobianOfCosts() const; /** - * @brief Extracts those entries from total hessian (combination of hessian from cost and constraints) that are not zero. + * @brief Computes the nonzero entries of the total Hessian after applying scaling factors. + * + * This function extracts the nonzero elements from the total Hessian, which is + * the combination of the Hessians from both cost functions and constraints, and + * applies the appropriate scaling factors. + * + * The Hessian of the cost function is multiplied by @c obj_factor, while the + * Hessians of the constraints are weighted by their corresponding Lagrange + * multipliers in @c lambda. + * * @param [in] x The current values of the optimization variables. - * @param [in] obj_factor The scaling multiplier for the hessian of the objective function(cost). - * @param [in] lambda Lagrange Multipliers of the constraints. - * @param [out] values The nonzero derivatives ordered by Eigen::RowMajor. + * @param [in] obj_factor The scaling multiplier for the Hessian of the objective function (cost). + * @param [in] lambda The Lagrange multipliers for the constraints. + * @param [out] values The nonzero second-order derivatives, ordered in @c Eigen::RowMajor format. */ void EvalNonzerosOfHessian(const double* x, double obj_factor, const double* lambda, double* values); + /** + * @brief Computes the full Hessian by summing the Hessians of costs and constraints. + * + * This function returns the total Hessian, which combines the second-order + * derivatives of both cost functions and constraints with respect to the + * optimization variables. + * + * The primary purpose of this function is to determine the structure of the + * overall Hessian, including the number and positions of nonzero elements. + * The resulting matrix is stored in sparse format for efficiency. + */ Hessian GetTotalHessian() const; + + /** + * @brief The sparse-matrix representation of the Hessians of the constraints. + * + * This function returns a pair of vectors, where the first vector contains + * the indices of the corresponding constraints, and the second vector holds + * the Hessian matrices in sparse format. + * + * Each Hessian matrix represents the second-order derivatives of a constraint + * with respect to the optimization variables. The Hessians are stored sparsely + * to optimize memory usage, as they are typically very sparse. + */ RowIndicesHessiansPair GetHessianOfConstraints() const; + + + /** + * @brief The sparse-matrix representation of the Hessians of the cost functions. + * + * This function returns a pair of vectors, where the first vector contains + * the indices of the corresponding cost functions, and the second vector holds + * the Hessian matrices in sparse format. + * + * Each Hessian matrix represents the second-order derivatives of a cost function + * with respect to the optimization variables. The Hessians are stored sparsely + * to optimize memory usage, as they are typically very sparse. + */ RowIndicesHessiansPair GetHessianOfCosts() const; /** diff --git a/ifopt_core/test/ifopt/test_vars_constr_cost.h b/ifopt_core/test/ifopt/test_vars_constr_cost.h index c72c3c5..dc91590 100644 --- a/ifopt_core/test/ifopt/test_vars_constr_cost.h +++ b/ifopt_core/test/ifopt/test_vars_constr_cost.h @@ -149,6 +149,9 @@ class ExConstraint : public ConstraintSet { std::vector& hessian_row_index_list, std::vector& triplets_list) const override { + // only the upper triangular elements need to be provided, as the Hessian + // is symmetric and internally only these values are used. + // only 1 constraint in this problem HessianTriplet constraint1_triplets; for (int i = 0; i < (int)var_set_list.size(); ++i) { @@ -188,6 +191,8 @@ class ExCost : public CostTerm { std::vector& hessian_row_index_list, std::vector& triplets_list) const override { + // only the upper triangular elements need to be provided, as the Hessian + // is symmetric and internally only these values are used. HessianTriplet cost_triplets; for (int i = 0; i < (int)var_set_list.size(); ++i) { const std::string& var_set = var_set_list[i]; From fbfa0fcfe4813c371b28dbbf0edc3cbdbc36c0ed Mon Sep 17 00:00:00 2001 From: Chang-Hong Chen Date: Sat, 22 Mar 2025 00:09:06 +0900 Subject: [PATCH 8/8] Fix uninitialize variable --- ifopt_core/src/composite.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ifopt_core/src/composite.cc b/ifopt_core/src/composite.cc index aa395a2..1e34131 100644 --- a/ifopt_core/src/composite.cc +++ b/ifopt_core/src/composite.cc @@ -190,7 +190,7 @@ Composite::RowIndicesHessiansPair Composite::GetHessians() const std::vector& hessians = row_indices_hessians_pair.second; hessians.reserve(GetRows()); // reserve space in the vector to avoid reallocations - int offset; // offset for row indices in different component + int offset = 0; // offset for row indices in different component for (const auto& c : components_) { const RowIndicesHessiansPair& local_row_indices_hessians = c->GetHessians(); const std::vector& row_indices = local_row_indices_hessians.first;