BoxQP.h

Go to the documentation of this file.

/* Author: Masaki Murooka */
 
#pragma once
 
#include <vector>
 
#include <Eigen/Dense>
 
namespace nmpc_ddp
{
template<int VarDim>
class BoxQP
{
public:
  using VarDimVector = Eigen::Matrix<double, VarDim, 1>;
 
  using VarVarDimMatrix = Eigen::Matrix<double, VarDim, VarDim>;
 
  using VarDimArray = Eigen::Array<bool, VarDim, 1>;
 
public:
  struct Configuration
  {
    int print_level = 1;
 
    int max_iter = 500;
 
    double grad_thre = 1e-8;
 
    double rel_improve_thre = 1e-8;
 
    double step_factor = 0.6;
 
    double min_step = 1e-22;
 
    double armijo_param = 0.1;
  };
 
  struct TraceData
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    int iter = 0;
 
    VarDimVector x;
 
    double obj = 0;
 
    VarDimVector search_dir;
 
    VarDimArray clamped_flag;
 
    int factorization_num = 0;
 
    int step_num = 0;
 
    TraceData(int var_dim)
    {
      x.setZero(var_dim);
      search_dir.setZero(var_dim);
      clamped_flag.setZero(var_dim);
    }
  };
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  BoxQP(int var_dim = VarDim) : var_dim_(var_dim)
  {
    // Check dimension is positive
    if(var_dim_ <= 0)
    {
      throw std::runtime_error("var_dim must be positive: " + std::to_string(var_dim_) + " <= 0");
    }
 
    // Check dimension consistency
    if constexpr(VarDim != Eigen::Dynamic)
    {
      if(var_dim_ != VarDim)
      {
        throw std::runtime_error("var_dim is inconsistent with template parameter: " + std::to_string(var_dim_)
                                 + " != " + std::to_string(VarDim));
      }
    }
  }
 
  inline VarDimVector solve(const VarVarDimMatrix & H,
                            const VarDimVector & g,
                            const VarDimVector & lower,
                            const VarDimVector & upper)
  {
    return solve(H, g, lower, upper, VarDimVector::Zero(var_dim_));
  }
 
  inline VarDimVector solve(const VarVarDimMatrix & H,
                            const VarDimVector & g,
                            const VarDimVector & lower,
                            const VarDimVector & upper,
                            const VarDimVector & initial_x)
  {
    // Initialize objective value
    VarDimVector x = initial_x.cwiseMin(upper).cwiseMax(lower);
    double obj = x.dot(g) + 0.5 * x.dot(H * x);
    double old_obj = obj;
 
    // Initialize trace data
    trace_data_list_.clear();
    TraceData initial_trace_data(var_dim_);
    initial_trace_data.iter = 0;
    initial_trace_data.x = x;
    initial_trace_data.obj = obj;
    trace_data_list_.push_back(initial_trace_data);
 
    // Main loop
    retval_ = 0;
    int factorization_num = 0;
    VarDimVector grad = VarDimVector::Zero(var_dim_);
    VarDimArray clamped_flag = VarDimArray::Zero(var_dim_);
    VarDimArray old_clamped_flag = clamped_flag;
    int iter = 1;
    free_idxs_.clear();
    for(;; iter++)
    {
      // Append trace data
      trace_data_list_.push_back(TraceData(var_dim_));
      auto & trace_data = trace_data_list_.back();
      trace_data.iter = iter;
 
      // Check relative improvement
      if(iter > 1 && (old_obj - obj) < config_.rel_improve_thre * std::abs(old_obj))
      {
        retval_ = 4;
        break;
      }
      old_obj = obj;
 
      // Calculate gradient
      grad = g + H * x;
 
      // Find clamped dimensions
      old_clamped_flag = clamped_flag;
      clamped_flag.setConstant(false);
      clamped_flag =
          ((x.array() == lower.array() && grad.array() > 0) || (x.array() == upper.array() && grad.array() < 0))
              .select(true, clamped_flag);
 
      // Set clamped and free indices
      std::vector<int> clamped_idxs;
      free_idxs_.clear();
      for(int i = 0; i < clamped_flag.size(); i++)
      {
        if(clamped_flag[i])
        {
          clamped_idxs.push_back(i);
        }
        else
        {
          free_idxs_.push_back(i);
        }
      }
 
      // Check for all clamped
      if(clamped_flag.all())
      {
        retval_ = 6;
        break;
      }
 
      // Factorize if clamped has changed
      if(iter == 1 || (clamped_flag != old_clamped_flag).any())
      {
        // Set H_free
        Eigen::MatrixXd H_free(free_idxs_.size(), free_idxs_.size());
        for(size_t i = 0; i < free_idxs_.size(); i++)
        {
          for(size_t j = 0; j < free_idxs_.size(); j++)
          {
            H_free(i, j) = H(free_idxs_[i], free_idxs_[j]);
          }
        }
 
        // Cholesky decomposition
        llt_free_ = std::make_unique<Eigen::LLT<Eigen::MatrixXd>>(H_free);
        if(llt_free_->info() == Eigen::NumericalIssue)
        {
          if(config_.print_level >= 1)
          {
            std::cout << "[BoxQP] H_free is not positive definite in Cholesky decomposition (LLT)." << std::endl;
          }
          retval_ = -1;
          break;
        }
 
        factorization_num++;
      }
 
      // Check gradient norm
      double grad_norm = 0;
      for(size_t i = 0; i < free_idxs_.size(); i++)
      {
        grad_norm += std::pow(grad[free_idxs_[i]], 2);
      }
      if(grad_norm < std::pow(config_.grad_thre, 2))
      {
        retval_ = 5;
        break;
      }
 
      // Calculate search direction
      Eigen::VectorXd x_clamped(clamped_idxs.size());
      Eigen::VectorXd x_free(free_idxs_.size());
      Eigen::VectorXd g_free(free_idxs_.size());
      Eigen::MatrixXd H_free_clamped(free_idxs_.size(), clamped_idxs.size());
      for(size_t i = 0; i < clamped_idxs.size(); i++)
      {
        x_clamped[i] = x[clamped_idxs[i]];
      }
      for(size_t i = 0; i < free_idxs_.size(); i++)
      {
        x_free[i] = x[free_idxs_[i]];
        g_free[i] = g[free_idxs_[i]];
        for(size_t j = 0; j < clamped_idxs.size(); j++)
        {
          H_free_clamped(i, j) = H(free_idxs_[i], clamped_idxs[j]);
        }
      }
      Eigen::VectorXd grad_free_clamped = g_free + H_free_clamped * x_clamped;
      Eigen::VectorXd search_dir_free = -1 * llt_free_->solve(grad_free_clamped) - x_free;
      VarDimVector search_dir = VarDimVector::Zero(var_dim_);
      for(size_t i = 0; i < free_idxs_.size(); i++)
      {
        search_dir[free_idxs_[i]] = search_dir_free[i];
      }
 
      // Check for descent direction
      double search_dir_grad = search_dir.dot(grad);
      if(search_dir_grad > 1e-10) // This should not happen
      {
        if(config_.print_level >= 1)
        {
          std::cout << "[BoxQP] search_dir_grad is positive: " << search_dir_grad << std::endl;
        }
        retval_ = -2;
        break;
      }
 
      // Armijo linesearch
      double step = 1;
      int step_num = 0;
      VarDimVector x_candidate = (x + step * search_dir).cwiseMin(upper).cwiseMax(lower);
      double obj_candidate = x_candidate.dot(g) + 0.5 * x_candidate.dot(H * x_candidate);
      while((obj_candidate - old_obj) / (step * search_dir_grad) < config_.armijo_param)
      {
        step = step * config_.step_factor;
        step_num++;
        x_candidate = (x + step * search_dir).cwiseMin(upper).cwiseMax(lower);
        obj_candidate = x_candidate.dot(g) + 0.5 * x_candidate.dot(H * x_candidate);
        if(step < config_.min_step)
        {
          retval_ = 2;
          break;
        }
      }
 
      // Print
      if(config_.print_level >= 3)
      {
        std::cout << "[BoxQP] iter: " << iter << ", obj: " << obj << ", grad_norm: " << grad_norm
                  << ", obj_update, : " << old_obj - obj_candidate << ", step: " << step
                  << ", clamped_flag_num: " << clamped_idxs.size() << std::endl;
      }
 
      // Set trace data
      trace_data.x = x;
      trace_data.obj = obj;
      trace_data.search_dir = search_dir;
      trace_data.clamped_flag = clamped_flag;
      trace_data.factorization_num = factorization_num;
      trace_data.step_num = step_num;
 
      // Accept candidate
      x = x_candidate;
      obj = obj_candidate;
 
      // Check loop termination
      if(iter == config_.max_iter)
      {
        retval_ = 1;
        break;
      }
    }
 
    // Print
    if(config_.print_level >= 2)
    {
      std::cout << "[BoxQP] result: " << retval_ << " (" << retstr_.at(retval_) << "), iter: " << iter
                << ", obj: " << obj << ", factorization_num: " << factorization_num << std::endl;
    }
 
    return x;
  }
 
  inline Configuration & config()
  {
    return config_;
  }
 
  inline const Configuration & config() const
  {
    return config_;
  }
 
  inline const std::vector<TraceData> & traceDataList() const
  {
    return trace_data_list_;
  }
 
public:
  const int var_dim_ = 0;
 
  int retval_ = 0;
 
  const std::unordered_map<int, std::string> retstr_ = {{-2, "Gradient of search direction is positive"},
                                                        {-1, "Hessian is not positive definite"},
                                                        {0, "Computation is not finished"},
                                                        {1, "Maximum main iterations exceeded"},
                                                        {2, "Maximum line-search iterations exceeded"},
                                                        {3, "No bounds, returning Newton point"},
                                                        {4, "Improvement smaller than tolerance"},
                                                        {5, "Gradient norm smaller than tolerance"},
                                                        {6, "All dimensions are clamped"}};
 
  std::unique_ptr<Eigen::LLT<Eigen::MatrixXd>> llt_free_;
 
  std::vector<int> free_idxs_;
 
protected:
  Configuration config_;
 
  std::vector<TraceData> trace_data_list_;
};
} // namespace nmpc_ddp