Add BFGS

CHANGELOG.md

@@ -7,6 +7,11 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ## [Unreleased]
 
+### Added
+
+- `prox-solver.hpp`: Add more alternatives to the `HessianApprox` enum  ([#92](https://github.com/Simple-Robotics/proxsuite-nlp/pull/92))
+- Add `BfgsStrategy` to estimate (inverse) Hessian and use it in Euclidian example ([#92](https://github.com/Simple-Robotics/proxsuite-nlp/pull/92))
+
 ## [0.10.1] - 2025-01-24
 
 ### Changed

examples/ur5-ik.cpp

@@ -11,6 +11,7 @@
 #include <proxsuite-nlp/cost-sum.hpp>
 #include <proxsuite-nlp/prox-solver.hpp>
 #include <proxsuite-nlp/modelling/constraints.hpp>
+#include <proxsuite-nlp/fmt-hessian-approx.hpp>
 
 #include <boost/filesystem/path.hpp>
 
@@ -70,6 +71,11 @@ struct FramePosition : C2FunctionTpl<Scalar> {
 
 int main() {
 
+  // solve with different hessian approximations
+  std::vector<HessianApprox> hess_approximations = {
+      HessianApprox::EXACT, HessianApprox::GAUSS_NEWTON, HessianApprox::BFGS,
+      HessianApprox::IDENTITY};
+
   const std::string ee_link_name = "tool0";
   Model model = loadModel();
   Space space{model};
@@ -106,19 +112,23 @@ int main() {
     problem.addConstraint(cstrobj);
   }
 
-  ProxNLPSolverTpl<Scalar> solver(problem, 1e-4, 0.01, 0.0,
-                                  proxsuite::nlp::VERBOSE);
-  solver.setup();
-  solver.solve(q0);
-
-  ResultsTpl<Scalar> const &results = *solver.results_;
-  WorkspaceTpl<Scalar> const &ws = *solver.workspace_;
-  std::cout << results << std::endl;
-
-  fmt::print("Optimal cost: {}\n", ws.objective_value);
-  fmt::print("Optimal cost grad: {}\n", ws.objective_gradient.transpose());
-  auto opt_frame_pos = (*fn)(results.x_opt);
-  fmt::print("Optimal frame pos: {}\n", opt_frame_pos.transpose());
-
+  for (auto hess_approx : hess_approximations) {
+    fmt::print("Solving with hessian approximation: {}\n", hess_approx);
+    ProxNLPSolverTpl<Scalar> solver(problem, 1e-4, 0.01, 0.0,
+                                    proxsuite::nlp::QUIET);
+    solver.hess_approx = hess_approx;
+    solver.max_iters = 500;
+    solver.setup();
+    solver.solve(q0);
+
+    ResultsTpl<Scalar> const &results = *solver.results_;
+    WorkspaceTpl<Scalar> const &ws = *solver.workspace_;
+    std::cout << results << std::endl;
+
+    fmt::print("Optimal cost: {}\n", ws.objective_value);
+    fmt::print("Optimal cost grad: {}\n", ws.objective_gradient.transpose());
+    auto opt_frame_pos = (*fn)(results.x_opt);
+    fmt::print("Optimal frame pos: {}\n\n", opt_frame_pos.transpose());
+  }
   return 0;
 }

include/proxsuite-nlp/bfgs-strategy.hpp

@@ -0,0 +1,103 @@
+/// @file
+/// @copyright Copyright (C) 2024-2025 INRIA
+#pragma once
+#include "proxsuite-nlp/fwd.hpp"
+
+namespace proxsuite {
+namespace nlp {
+
+enum BFGSType { Hessian, InverseHessian };
+
+template <typename Scalar, BFGSType BFGS_TYPE = BFGSType::InverseHessian>
+class BFGSStrategy {
+  PROXSUITE_NLP_DYNAMIC_TYPEDEFS(Scalar);
+
+public:
+  BFGSStrategy()
+      : is_init(false), is_psd(false), x_prev(), g_prev(), s(), y(),
+        xx_transpose(), xy_transpose(), V(), VMinv(), VMinvVt(),
+        is_valid(false) {}
+
+  explicit BFGSStrategy(const int num_vars)
+      : is_init(false), is_psd(true), x_prev(VectorXs::Zero(num_vars)),
+        g_prev(VectorXs::Zero(num_vars)), s(VectorXs::Zero(num_vars)),
+        y(VectorXs::Zero(num_vars)),
+        xx_transpose(MatrixXs::Zero(num_vars, num_vars)),
+        xy_transpose(MatrixXs::Zero(num_vars, num_vars)),
+        V(MatrixXs::Zero(num_vars, num_vars)),
+        VMinv(MatrixXs::Zero(num_vars, num_vars)),
+        VMinvVt(MatrixXs::Zero(num_vars, num_vars)), is_valid(true) {
+    x_prev = VectorXs::Zero(num_vars);
+    g_prev = VectorXs::Zero(num_vars);
+  }
+
+  void init(const ConstVectorRef &x0, const ConstVectorRef &g0) {
+    if (!is_valid) {
+      throw std::runtime_error("Cannot initialize an invalid BFGSStrategy. Use "
+                               "the constructor with num_vars first.");
+    }
+
+    x_prev = x0;
+    g_prev = g0;
+    is_init = true;
+  }
+
+  void update(const ConstVectorRef &x, const ConstVectorRef &g,
+              MatrixXs &hessian) {
+    if (!is_init) {
+      init(x, g);
+      return;
+    }
+    PROXSUITE_NLP_NOMALLOC_BEGIN;
+    s = x - x_prev;
+    y = g - g_prev;
+    const Scalar sy = s.dot(y);
+
+    if (sy > 0) {
+      if constexpr (BFGS_TYPE == BFGSType::InverseHessian) {
+        // Nocedal and Wright, Numerical Optimization, 2nd ed., p. 140, eqn 6.17
+        // (BFGS update)
+        xx_transpose.noalias() = s * s.transpose();
+        xy_transpose.noalias() = s * y.transpose();
+      } else if constexpr (BFGS_TYPE == BFGSType::Hessian) {
+        // Nocedal and Wright, Numerical Optimization, 2nd ed., p. 139, eqn 6.13
+        // (DFP update)
+        xx_transpose.noalias() = y * y.transpose();
+        xy_transpose.noalias() = y * s.transpose();
+      }
+
+      V.noalias() = MatrixXs::Identity(s.size(), s.size()) - xy_transpose / sy;
+      VMinv.noalias() = V * hessian;
+      VMinvVt.noalias() = VMinv * V.transpose();
+      hessian.noalias() = VMinvVt + xx_transpose / sy;
+      is_psd = true;
+    } else {
+      is_psd = false;
+      PROXSUITE_NLP_WARN("Skipping BFGS update as s^Ty <= 0");
+    }
+    x_prev = x;
+    g_prev = g;
+    PROXSUITE_NLP_NOMALLOC_END;
+  }
+  bool isValid() const { return is_valid; }
+
+public:
+  bool is_init;
+  bool is_psd;
+
+private:
+  VectorXs x_prev; // previous iterate
+  VectorXs g_prev; // previous gradient
+  VectorXs s;      // delta iterate
+  VectorXs y;      // delta gradient
+  // temporary variables to avoid dynamic memory allocation
+  MatrixXs xx_transpose;
+  MatrixXs xy_transpose;
+  MatrixXs V;
+  MatrixXs VMinv;
+  MatrixXs VMinvVt;
+  bool is_valid;
+};
+
+} // namespace nlp
+} // namespace proxsuite

include/proxsuite-nlp/fmt-hessian-approx.hpp

@@ -0,0 +1,35 @@
+/// @file
+/// @copyright Copyright (C) 2024-2025 INRIA
+#pragma once
+
+#include <fmt/ostream.h>
+#include <fmt/ranges.h>
+
+namespace fmt {
+template <> struct formatter<proxsuite::nlp::HessianApprox> {
+  template <typename ParseContext> constexpr auto parse(ParseContext &ctx) {
+    return ctx.begin();
+  }
+
+  template <typename FormatContext>
+  auto format(const proxsuite::nlp::HessianApprox &hessian_approx,
+              FormatContext &ctx) const {
+    std::string name;
+    switch (hessian_approx) {
+    case proxsuite::nlp::HessianApprox::EXACT:
+      name = "EXACT";
+      break;
+    case proxsuite::nlp::HessianApprox::GAUSS_NEWTON:
+      name = "GAUSS_NEWTON";
+      break;
+    case proxsuite::nlp::HessianApprox::BFGS:
+      name = "BFGS";
+      break;
+    case proxsuite::nlp::HessianApprox::IDENTITY:
+      name = "IDENTITY";
+      break;
+    }
+    return format_to(ctx.out(), "{}", name);
+  }
+};
+} // namespace fmt

include/proxsuite-nlp/prox-solver.hpp

@@ -1,5 +1,6 @@
 /// @file
 /// @copyright Copyright (C) 2022 LAAS-CNRS, INRIA
+/// @copyright Copyright (C) 2024-2025 INRIA
 #pragma once
 
 #include "proxsuite-nlp/fwd.hpp"
@@ -10,6 +11,7 @@
 #include "proxsuite-nlp/helpers-base.hpp"
 #include "proxsuite-nlp/logger.hpp"
 #include "proxsuite-nlp/bcl-params.hpp"
+#include "proxsuite-nlp/bfgs-strategy.hpp"
 
 #include <boost/mpl/bool.hpp>
 
@@ -27,6 +29,10 @@ enum class HessianApprox {
   EXACT,
   /// Gauss-Newton (or rather SCQP) approximation
   GAUSS_NEWTON,
+  /// BFGS approximation
+  BFGS,
+  /// Identity
+  IDENTITY
 };
 
 enum InertiaFlag { INERTIA_OK = 0, INERTIA_BAD = 1, INERTIA_HAS_ZEROS = 2 };
@@ -48,6 +54,7 @@ template <typename _Scalar> class ProxNLPSolverTpl {
   using CallbackPtr = shared_ptr<helpers::base_callback<Scalar>>;
   using ConstraintSet = ConstraintSetTpl<Scalar>;
   using ConstraintObject = ConstraintObjectTpl<Scalar>;
+  using BFGS = BFGSStrategy<Scalar, BFGSType::Hessian>;
 
 protected:
   /// General nonlinear program to solve.
@@ -67,6 +74,8 @@ template <typename _Scalar> class ProxNLPSolverTpl {
   LinesearchStrategy ls_strat = LinesearchStrategy::ARMIJO;
   MultiplierUpdateMode mul_update_mode = MultiplierUpdateMode::NEWTON;
 
+  mutable BFGS bfgs_strategy_;
+
   /// linear algebra opts
   std::size_t max_refinement_steps_ = 5;
   Scalar kkt_tolerance_ = 1e-13;
@@ -138,6 +147,19 @@ template <typename _Scalar> class ProxNLPSolverTpl {
   void setup() {
     workspace_ = std::make_unique<Workspace>(*problem_, ldlt_choice_);
     results_ = std::make_unique<Results>(*problem_);
+
+    if (hess_approx == HessianApprox::BFGS) {
+      // TODO: work on implementation of BFGS on manifolds
+      if (problem_->ndx() == problem_->nx()) {
+        BFGS valid_bfgs_strategy_ = BFGS(problem_->ndx());
+        bfgs_strategy_ = std::move(valid_bfgs_strategy_);
+        //  set workspace hessian to identity matrix (init to zero in workspace)
+        workspace_->objective_hessian.setIdentity();
+      } else {
+        throw std::runtime_error("BFGS for hessian approximation currently "
+                                 "only works for Euclidean manifolds.");
+      }
+    }
   }
 
   /**

include/proxsuite-nlp/prox-solver.hxx

@@ -1,5 +1,6 @@
 /// @file
 /// @copyright Copyright (C) 2022 LAAS-CNRS, INRIA
+/// @copyright Copyright (C) 2024-2025 INRIA
 /// @brief     Implementations for the prox solver.
 #pragma once
 
@@ -228,7 +229,22 @@ template <typename Scalar>
 void ProxNLPSolverTpl<Scalar>::computeProblemDerivatives(
     const ConstVectorRef &x, Workspace &workspace, boost::mpl::true_) const {
   this->computeProblemDerivatives(x, workspace, boost::mpl::false_());
-  problem_->computeHessians(x, workspace, hess_approx == HessianApprox::EXACT);
+
+  switch (hess_approx) {
+  case HessianApprox::BFGS:
+    bfgs_strategy_.update(x, workspace.objective_gradient,
+                          workspace.objective_hessian);
+    break;
+
+  case HessianApprox::IDENTITY:
+    workspace.objective_hessian.setIdentity();
+    break;
+
+  default: // handle both EXACT and GAUSS_NEWTON
+    problem_->computeHessians(x, workspace,
+                              hess_approx == HessianApprox::EXACT);
+    break;
+  }
 }
 
 template <typename Scalar>

tests/CMakeLists.txt

@@ -153,6 +153,7 @@ add_proxsuite_nlp_test(finite-diff)
 add_proxsuite_nlp_test(math)
 add_proxsuite_nlp_test(functions)
 add_proxsuite_nlp_test(linesearch)
+add_proxsuite_nlp_test(bfgs-strategy)
 add_proxsuite_nlp_test(manifolds)
 add_proxsuite_nlp_test(solver)