osqp_interface.hpp

Go to the documentation of this file.

// Copyright 2021 The Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#ifndef OSQP_INTERFACE__OSQP_INTERFACE_HPP_
#define OSQP_INTERFACE__OSQP_INTERFACE_HPP_
 
#include <limits>
#include <memory>
#include <string>
#include <tuple>
#include <vector>
 
#include "common/types.hpp"
#include "eigen3/Eigen/Core"
#include "osqp/osqp.h"
#include "osqp_interface/visibility_control.hpp"
#include "osqp_interface/csc_matrix_conv.hpp"
 
 
namespace autoware
{
namespace common
{
namespace osqp
{
constexpr c_float INF = OSQP_INFTY;
using autoware::common::types::float64_t;
using autoware::common::types::bool8_t;
 
class OSQP_INTERFACE_PUBLIC OSQPInterface
{
private:
  std::unique_ptr<OSQPWorkspace, std::function<void(OSQPWorkspace *)>> m_work;
  std::unique_ptr<OSQPSettings> m_settings;
  std::unique_ptr<OSQPData> m_data;
  // store last work info since work is cleaned up at every execution to prevent memory leak.
  OSQPInfo m_latest_work_info;
  // Number of parameters to optimize
  int64_t m_param_n;
  // Flag to check if the current work exists
  bool8_t m_work_initialized = false;
  // Exitflag
  int64_t m_exitflag;
 
  // Runs the solver on the stored problem.
  std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int64_t> solve();
 
  static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept;
 
public:
  explicit OSQPInterface(
    const c_float eps_abs = std::numeric_limits<c_float>::epsilon(), const bool8_t polish = true);
  OSQPInterface(
    const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<float64_t> & q,
    const std::vector<float64_t> & l, const std::vector<float64_t> & u, const c_float eps_abs);
  OSQPInterface(
    const CSC_Matrix & P, const CSC_Matrix & A, const std::vector<float64_t> & q,
    const std::vector<float64_t> & l, const std::vector<float64_t> & u, const c_float eps_abs);
 
  /****************
   * OPTIMIZATION
   ****************/
  //
 
  std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int64_t> optimize();
 
  std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int64_t> optimize(
    const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<float64_t> & q,
    const std::vector<float64_t> & l, const std::vector<float64_t> & u);
 
  int64_t initializeProblem(
    const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<float64_t> & q,
    const std::vector<float64_t> & l, const std::vector<float64_t> & u);
  int64_t initializeProblem(
    CSC_Matrix P, CSC_Matrix A, const std::vector<float64_t> & q,
    const std::vector<float64_t> & l, const std::vector<float64_t> & u);
 
  // Updates problem parameters while keeping solution in memory.
  //
  // Args:
  //   P_new: (n,n) matrix defining relations between parameters.
  //   A_new: (m,n) matrix defining parameter constraints relative to the lower and upper bound.
  //   q_new: (n) vector defining the linear cost of the problem.
  //   l_new: (m) vector defining the lower bound problem constraint.
  //   u_new: (m) vector defining the upper bound problem constraint.
  void updateP(const Eigen::MatrixXd & P_new);
  void updateCscP(const CSC_Matrix & P_csc);
  void updateA(const Eigen::MatrixXd & A_new);
  void updateCscA(const CSC_Matrix & A_csc);
  void updateQ(const std::vector<double> & q_new);
  void updateL(const std::vector<double> & l_new);
  void updateU(const std::vector<double> & u_new);
  void updateBounds(const std::vector<double> & l_new, const std::vector<double> & u_new);
  void updateEpsAbs(const double eps_abs);
  void updateEpsRel(const double eps_rel);
  void updateMaxIter(const int iter);
  void updateVerbose(const bool verbose);
  void updateRhoInterval(const int rho_interval);
  void updateRho(const double rho);
  void updateAlpha(const double alpha);
 
  inline int64_t getTakenIter() const {return static_cast<int64_t>(m_latest_work_info.iter);}
  inline std::string getStatusMessage() const
  {
    return static_cast<std::string>(m_latest_work_info.status);
  }
  inline int64_t getStatus() const {return static_cast<int64_t>(m_latest_work_info.status_val);}
  inline int64_t getStatusPolish() const
  {
    return static_cast<int64_t>(m_latest_work_info.status_polish);
  }
  inline float64_t getRunTime() const {return m_latest_work_info.run_time;}
  inline float64_t getObjVal() const {return m_latest_work_info.obj_val;}
  inline int64_t getExitFlag() const {return m_exitflag;}
};
 
}  // namespace osqp
}  // namespace common
}  // namespace autoware
 
#endif  // OSQP_INTERFACE__OSQP_INTERFACE_HPP_