Merge pull request #9 from machines-in-motion/skleff/testing

 Solvers C++ unittests (unconstrained)

CMakeLists.txt

@@ -17,6 +17,7 @@ add_compile_options(--warn-no-conversion)
 # Project options
 option(BUILD_PYTHON_INTERFACE "Build the python binding" ON)
 option(SUFFIX_SO_VERSION "Suffix library name with its version" ON)
+option(BUILD_WITH_PROXQP "Build the ProxQP-based SQP solver" ON)
 
 # Project configuration
 set(PROJECT_USE_CMAKE_EXPORT TRUE)
@@ -51,7 +52,15 @@ find_package(eigenpy REQUIRED)
 
 add_project_dependency(crocoddyl 2.0.0 REQUIRED)
 add_project_dependency(eigenpy 2.7.10 REQUIRED)
-# find_package(proxsuite REQUIRED)
+add_project_dependency(example-robot-data 4.0.7 REQUIRED PKG_CONFIG_REQUIRES
+"example-robot-data >= 4.0.7")
+
+if(BUILD_WITH_PROXQP)
+  find_package(proxsuite REQUIRED)
+  add_project_dependency(proxsuite REQUIRED)
+  add_definitions(-DMIM_SOLVERS_WITH_PROXQP)
+endif()
+
 find_package(OpenMP REQUIRED COMPONENTS CXX)
 
 if(BUILD_PYTHON_INTERFACE)
@@ -63,6 +72,7 @@ endif()
 
 # Main Library
 set(${PROJECT_NAME}_HEADERS
+  include/${PROJECT_NAME}/kkt.hpp
   include/${PROJECT_NAME}/ddp.hpp
   include/${PROJECT_NAME}/fddp.hpp
   include/${PROJECT_NAME}/sqp.hpp
@@ -71,6 +81,7 @@ set(${PROJECT_NAME}_HEADERS
   )
 
 set(${PROJECT_NAME}_SOURCES
+  src/kkt.cpp
   src/ddp.cpp
   src/fddp.cpp
   src/sqp.cpp
@@ -87,6 +98,9 @@ endif()
 
 # Main Executable
 TARGET_LINK_LIBRARIES(${PROJECT_NAME} crocoddyl::crocoddyl)
+if(BUILD_WITH_PROXQP)
+  TARGET_LINK_LIBRARIES(${PROJECT_NAME} proxsuite::proxsuite)
+endif()
 set_target_properties(${PROJECT_NAME} PROPERTIES INSTALL_RPATH "\$ORIGIN/../lib")
 
 # Python Bindings

include/mim_solvers/csqp_proxqp.hpp

@@ -6,6 +6,8 @@
 // All rights reserved.
 ///////////////////////////////////////////////////////////////////////////////
 
+// #ifdef MIM_SOLVERS_WITH_PROXQP
+
 #ifndef MIM_SOLVERS_CSQP_PROXQP_HPP_
 #define MIM_SOLVERS_CSQP_PROXQP_HPP_
 
@@ -237,4 +239,5 @@ class SolverPROXQP : public SolverDDP {
 
 }  // namespace mim_solvers
 
-#endif  // MIM_SOLVERS_CSQP_PROXQP_HPP_
+#endif  // MIM_SOLVERS_CSQP_PROXQP_HPP_
+// #endif  // MIM_SOLVERS_WITH_PROXQP

include/mim_solvers/kkt.hpp

@@ -0,0 +1,105 @@
+///////////////////////////////////////////////////////////////////////////////
+// 
+// This file is a modified version of SolverKKT from the Crocoddyl library
+// This modified version is used for testing purposes only
+// Original file : https://github.com/loco-3d/crocoddyl/blob/devel/include/crocoddyl/core/solvers/kkt.hpp
+// 
+// BSD 3-Clause License
+// Copyright (C) 2023, New York University
+//
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef MIM_SOLVERS_KKT_HPP_
+#define MIM_SOLVERS_KKT_HPP_
+
+#include <vector>
+#include <Eigen/Cholesky>
+#include <crocoddyl/core/solver-base.hpp>
+#include <crocoddyl/core/mathbase.hpp>
+#include <crocoddyl/core/utils/deprecate.hpp>
+
+namespace mim_solvers {
+
+class SolverKKT : public crocoddyl::SolverAbstract {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  explicit SolverKKT(boost::shared_ptr<crocoddyl::ShootingProblem> problem);
+  virtual ~SolverKKT();
+
+  virtual bool solve(
+      const std::vector<Eigen::VectorXd>& init_xs = crocoddyl::DEFAULT_VECTOR,
+      const std::vector<Eigen::VectorXd>& init_us = crocoddyl::DEFAULT_VECTOR,
+      const std::size_t maxiter = 100, const bool is_feasible = false,
+      const double regInit = 1e-9);
+  virtual void computeDirection(const bool recalc = true);
+  virtual double tryStep(const double steplength = 1);
+  virtual double stoppingCriteria();
+  virtual const Eigen::Vector2d& expectedImprovement();
+
+  const Eigen::MatrixXd& get_kkt() const;
+  const Eigen::VectorXd& get_kktref() const;
+  const Eigen::VectorXd& get_primaldual() const;
+  const std::vector<Eigen::VectorXd>& get_dxs() const;
+  const std::vector<Eigen::VectorXd>& get_dus() const;
+  const std::vector<Eigen::VectorXd>& get_lambdas() const;
+  std::size_t get_nx() const;
+  std::size_t get_ndx() const;
+  std::size_t get_nu() const;
+
+  /**
+   * @brief Compute the KKT conditions residual
+   */
+  virtual void checkKKTConditions();
+  void set_termination_tolerance(double tol) { termination_tol_ = tol; };
+  void set_use_kkt_criteria(bool inBool) { use_kkt_criteria_ = inBool; };
+  double get_termination_tolerance() const { return termination_tol_; };
+  bool get_use_kkt_criteria() const { return use_kkt_criteria_; }
+  double get_KKT() const { return KKT_; };
+
+ protected:
+  double reg_incfactor_;
+  double reg_decfactor_;
+  double reg_min_;
+  double reg_max_;
+  double cost_try_;
+  std::vector<Eigen::VectorXd> xs_try_;
+  std::vector<Eigen::VectorXd> us_try_;
+
+ private:
+  double calcDiff();
+  void computePrimalDual();
+  void increaseRegularization();
+  void decreaseRegularization();
+  void allocateData();
+
+  std::size_t nx_;
+  std::size_t ndx_;
+  std::size_t nu_;
+  std::vector<Eigen::VectorXd> dxs_;
+  std::vector<Eigen::VectorXd> dus_;
+  std::vector<Eigen::VectorXd> lambdas_;
+
+  // allocate data
+  Eigen::MatrixXd kkt_;
+  Eigen::VectorXd kktref_;
+  Eigen::VectorXd primaldual_;
+  Eigen::VectorXd primal_;
+  Eigen::VectorXd dual_;
+  std::vector<double> alphas_;
+  double th_grad_;
+  bool was_feasible_;
+  Eigen::VectorXd kkt_primal_;
+  Eigen::VectorXd dF;
+  double KKT_ = std::numeric_limits<double>::infinity();   //!< KKT conditions residual
+  bool use_kkt_criteria_ = true;                           //!< Use KKT conditions as termination criteria 
+  Eigen::VectorXd fs_flat_;                                //!< Gaps/defects between shooting nodes (1D array)
+  double termination_tol_ = 1e-6;                          //!< Termination tolerance
+
+};
+
+}  // namespace mim_solvers
+
+#endif  // MIM_SOLVERS_KKT_HPP_

package.xml

@@ -20,6 +20,7 @@
 
   <depend>crocoddyl</depend>
   <depend>eigenpy</depend>
+  <depend>example-robot-data</depend>
 
   <buildtool_depend>cmake</buildtool_depend>
   <export>

src/csqp.cpp

@@ -235,8 +235,9 @@ bool SolverCSQP::solve(const std::vector<Eigen::VectorXd>& init_xs, const std::v
   if (problem_->is_updated()) {
     resizeData();
   }
-  xs_try_[0] = problem_->get_x0();  // it is needed in case that init_xs[0] is infeasible
   setCandidate(init_xs, init_us, false);
+  xs_[0] = problem_->get_x0();      // Otherwise xs[0] is overwritten by init_xs inside setCandidate()
+  xs_try_[0] = problem_->get_x0();  // it is needed in case that init_xs[0] is infeasible
 
   // REMOVED REGULARIZATION : 
   // if (std::isnan(reginit)) {

src/csqp_proxqp.cpp

@@ -6,6 +6,7 @@
 // All rights reserved.
 ///////////////////////////////////////////////////////////////////////////////
 
+// #ifdef MIM_SOLVERS_WITH_PROXQP
 
 #ifdef CROCODDYL_WITH_MULTITHREADING
 #include <omp.h>
@@ -44,17 +45,25 @@ SolverPROXQP::SolverPROXQP(boost::shared_ptr<crocoddyl::ShootingProblem> problem
       xs_try_.resize(T+1); us_try_.resize(T);
 
       const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
-      const std::size_t nx = models[0]->get_state()->get_nx();
-      const std::size_t nu = models[0]->get_nu();
-      // NOTE : Assuming nx and nu don't change with time
-      n_vars = T*(nx + nu);
+      // We allow nx and nu to change with time
+      n_vars = 0;
+      n_eq = 0;
+      for (std::size_t t = 0; t < T; ++t){
+        if(t < T-1){
+          n_vars += models[t+1]->get_state()->get_nx();
+          n_eq += models[t+1]->get_state()->get_nx();
+        }
+        n_vars += models[t]->get_nu();
+      }
+      n_vars += problem_->get_terminalModel()->get_state()->get_nx();
+      n_eq += problem_->get_terminalModel()->get_state()->get_nx();
       P_.resize(n_vars, n_vars); P_.setZero();
       q_.resize(n_vars), q_.setZero();
-      A_.resize(T*nx, n_vars); A_.setZero();
-      b_.resize(T*nx); b_.setZero();
+      A_.resize(n_eq, n_vars); A_.setZero();
+      b_.resize(n_eq); b_.setZero();
 
       Psp_.resize(n_vars, n_vars);
-      Asp_.resize(T*nx, n_vars);
+      Asp_.resize(n_eq, n_vars);
 
       for (std::size_t t = 0; t < T; ++t) {
         const boost::shared_ptr<ActionModelAbstract>& model = models[t];
@@ -86,13 +95,13 @@ SolverPROXQP::SolverPROXQP(boost::shared_ptr<crocoddyl::ShootingProblem> problem
       int nc = problem_->get_terminalModel()->get_ng();
       y_.back().resize(nc); y_.back().setZero();
       n_in += nc;
-      n_eq = T* nx;
+      // n_eq = T* nx;
 
-      C_.resize(n_in, T*(nx + nu)); C_.setZero();
+      C_.resize(n_in, n_vars); C_.setZero();
       l_.resize(n_in); l_.setZero();
       u_.resize(n_in); u_.setZero();
 
-      Csp_.resize(n_in, T*(nx + nu));
+      Csp_.resize(n_in, n_vars);
 
       const std::size_t n_alphas = 10;
       alphas_.resize(n_alphas);
@@ -118,8 +127,10 @@ bool SolverPROXQP::solve(const std::vector<Eigen::VectorXd>& init_xs, const std:
   if (problem_->is_updated()) {
     resizeData();
   }
-  xs_try_[0] = problem_->get_x0();  // it is needed in case that init_xs[0] is infeasible
   setCandidate(init_xs, init_us, false);
+  xs_[0] = problem_->get_x0();      // Otherwise xs[0] is overwritten by init_xs inside setCandidate()
+  xs_try_[0] = problem_->get_x0();  // it is needed in case that init_xs[0] is infeasible
+
 
   if (std::isnan(reginit)) {
     xreg_ = reg_min_;
@@ -230,6 +241,7 @@ void SolverPROXQP::calc(const bool recalc){
   const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
   const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas = problem_->get_runningDatas();
 
+  double index_u = n_eq;
   for (std::size_t t = 0; t < T; ++t) {
 
     const boost::shared_ptr<ActionModelAbstract>& m = models[t];
@@ -247,8 +259,7 @@ void SolverPROXQP::calc(const bool recalc){
     constraint_norm_ += (lb - d->g).cwiseMax(Eigen::VectorXd::Zero(nc)).lpNorm<1>();
     constraint_norm_ += (d->g - ub).cwiseMax(Eigen::VectorXd::Zero(nc)).lpNorm<1>();
 
-    // creating matrices
-    const double index_u = T * nx + t*nu;
+    // creating matrices    
     if(t>0){
       const double index_x = (t-1)*nx;
       P_.middleCols(index_x, nx).middleRows(index_x, nx) = d->Lxx;
@@ -277,6 +288,8 @@ void SolverPROXQP::calc(const bool recalc){
     // make faster
     A_.middleCols(t*nx,nx).middleRows(t*nx,nx) = Eigen::MatrixXd::Identity(nx, nx);
     b_.segment(t*nx, nx) = fs_[t+1];
+
+    index_u += nu;
 
   }
 
@@ -339,14 +352,15 @@ void SolverPROXQP::computeDirection(const bool recalcDiff){
   const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
   x_grad_norm_ = 0; u_grad_norm_ = 0;
   double nin_count = 0;
+  double index_u = n_eq;
   for (std::size_t t = 0; t < T; ++t){
     const boost::shared_ptr<ActionModelAbstract>& m = models[t];
     const int nx = m->get_state()->get_nx();
     const int nu = m->get_nu();
     int nc = m->get_ng();
 
     dx_[t+1] = res.segment(t * nx, nx);
-    double index_u = T *nx + t * nu;
+    // double index_u = T *nx + t * nu;
     du_[t] = res.segment(index_u, nu);
     x_grad_norm_ += dx_[t+1].lpNorm<1>(); // assuming that there is no gap in the initial state
     u_grad_norm_ += du_[t].lpNorm<1>(); // assuming that there is no gap in the initial state
@@ -356,6 +370,7 @@ void SolverPROXQP::computeDirection(const bool recalcDiff){
     lag_mul_[t+1] = -qp.results.y.segment(t* nx, nx);
     y_[t] = qp.results.z.segment(nin_count, nc);
     nin_count += nc;
+    index_u += nu;
   }
   x_grad_norm_ = x_grad_norm_/(T+1);
   u_grad_norm_ = u_grad_norm_/T; 
@@ -498,3 +513,5 @@ bool SolverPROXQP::getCallbacks(){
 }
 
 }  // namespace mim_solvers
+
+// #endif // MIM_SOLVERS_WITH_PROXQP