<commit_before>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * 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.
 *****************************************************************************/

/**
 * @file: active_set_qp_solver.cc
 **/
#include "modules/common/math/qp_solver/active_set_qp_solver.h"

#include <algorithm>
#include <climits>
#include <vector>

#include "modules/common/log.h"
#include "modules/common/math/qp_solver/qp_solver_gflags.h"

namespace apollo {
namespace common {
namespace math {

ActiveSetQpSolver::ActiveSetQpSolver(
    const Eigen::MatrixXd& kernel_matrix, const Eigen::MatrixXd& offset,
    const Eigen::MatrixXd& affine_inequality_matrix,
    const Eigen::MatrixXd& affine_inequality_boundary,
    const Eigen::MatrixXd& affine_equality_matrix,
    const Eigen::MatrixXd& affine_equality_boundary)
    : QpSolver(kernel_matrix, offset, affine_inequality_matrix,
               affine_inequality_boundary, affine_equality_matrix,
               affine_equality_boundary),
      num_constraint_(affine_equality_matrix_.rows() +
                      affine_inequality_matrix_.rows()),
      num_param_(kernel_matrix.rows()),
      qp_eps_num_(FLAGS_default_active_set_eps_num),
      qp_eps_den_(FLAGS_default_active_set_eps_den),
      qp_eps_iter_ref_(FLAGS_default_active_set_eps_iter_ref),
      debug_info_(FLAGS_default_enable_active_set_debug_info) {}

bool ActiveSetQpSolver::Solve() {
  ::qpOASES::QProblem qp_problem(num_param_, num_constraint_);
  ::qpOASES::Options my_options;
  my_options.epsNum = qp_eps_num_;
  my_options.epsDen = qp_eps_den_;
  my_options.epsIterRef = qp_eps_iter_ref_;
  qp_problem.setOptions(my_options);
  if (!debug_info_) {
    qp_problem.setPrintLevel(qpOASES::PL_NONE);
  }
  // definition of qpOASESproblem
  double h_matrix[kernel_matrix_.rows() * kernel_matrix_.cols()];  // NOLINT
  double g_matrix[offset_.rows()];                                 // NOLINT
  int index = 0;

  for (int r = 0; r < kernel_matrix_.rows(); ++r) {
    g_matrix[r] = offset_(r, 0);

    for (int c = 0; c < kernel_matrix_.cols(); ++c) {
      h_matrix[index++] = kernel_matrix_(r, c);
    }
  }

  // search space lower bound and uppper bound
  double lower_bound[num_param_];  // NOLINT
  double upper_bound[num_param_];  // NOLINT

  // TODO(fanhaoyang): Haoyang Fan change this to a configurable version
  for (int i = 0; i < num_param_; ++i) {
    lower_bound[i] = l_lower_bound_;
    upper_bound[i] = l_upper_bound_;
  }

  // constraint matrix construction
  double affine_constraint_matrix[num_param_ * num_constraint_];  // NOLINT
  double constraint_lower_bound[num_constraint_];                 // NOLINT
  double constraint_upper_bound[num_constraint_];                 // NOLINT
  index = 0;

  for (int r = 0; r < affine_equality_matrix_.rows(); ++r) {
    // TODO(fanhaoyang): change to a configurable version
    constraint_lower_bound[r] = affine_equality_boundary_(r, 0);
    constraint_upper_bound[r] = affine_equality_boundary_(r, 0);

    for (int c = 0; c < num_param_; ++c) {
      affine_constraint_matrix[index++] = affine_equality_matrix_(r, c);
    }
  }

  for (int r = 0; r < affine_inequality_matrix_.rows(); ++r) {
    constraint_lower_bound[r + affine_equality_boundary_.rows()] =
        affine_inequality_boundary_(r, 0);
    constraint_upper_bound[r + affine_equality_boundary_.rows()] =
        constraint_upper_bound_;

    // TODO(fanhaoyang): change to a configurable version
    for (int c = 0; c < num_param_; ++c) {
      affine_constraint_matrix[index++] = affine_inequality_matrix_(r, c);
    }
  }

  // initialize problem
  int max_iter = std::max(max_iteration_, num_constraint_);

  auto ret = qp_problem.init(h_matrix, g_matrix, affine_constraint_matrix,
                             lower_bound, upper_bound, constraint_lower_bound,
                             constraint_upper_bound, max_iter);
  if (ret != qpOASES::SUCCESSFUL_RETURN) {
    AERROR << "Fail to initialize qp problem";
    if (ret == qpOASES::RET_MAX_NWSR_REACHED) {
      AERROR << "Qp solver failed due to reached max iteration";
    } else {
      AERROR << "Qp failed due to infeasibility or other internal reasons";
    }
    return false;
  }

  double result[num_param_];  // NOLINT

  qp_problem.getPrimalSolution(result);

  params_ = Eigen::MatrixXd::Zero(num_param_, 1);

  if (qp_problem.isSolved() == qpOASES::BT_TRUE) {
    for (int i = 0; i < num_param_; ++i) {
      params_(i, 0) = result[i];
    }

    return true;
  }

  return false;
}

void ActiveSetQpSolver::set_qp_eps_num(const double eps) { qp_eps_num_ = eps; }

void ActiveSetQpSolver::set_qp_eps_den(const double eps) { qp_eps_den_ = eps; }

void ActiveSetQpSolver::set_qp_eps_iter_ref(const double eps) {<commit_msg>common::math : removed ambiguous error msg.<commit_after>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * 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.
 *****************************************************************************/

/**
 * @file: active_set_qp_solver.cc
 **/
#include "modules/common/math/qp_solver/active_set_qp_solver.h"

#include <algorithm>
#include <climits>
#include <vector>

#include "modules/common/log.h"
#include "modules/common/math/qp_solver/qp_solver_gflags.h"

namespace apollo {
namespace common {
namespace math {

ActiveSetQpSolver::ActiveSetQpSolver(
    const Eigen::MatrixXd& kernel_matrix, const Eigen::MatrixXd& offset,
    const Eigen::MatrixXd& affine_inequality_matrix,
    const Eigen::MatrixXd& affine_inequality_boundary,
    const Eigen::MatrixXd& affine_equality_matrix,
    const Eigen::MatrixXd& affine_equality_boundary)
    : QpSolver(kernel_matrix, offset, affine_inequality_matrix,
               affine_inequality_boundary, affine_equality_matrix,
               affine_equality_boundary),
      num_constraint_(affine_equality_matrix_.rows() +
                      affine_inequality_matrix_.rows()),
      num_param_(kernel_matrix.rows()),
      qp_eps_num_(FLAGS_default_active_set_eps_num),
      qp_eps_den_(FLAGS_default_active_set_eps_den),
      qp_eps_iter_ref_(FLAGS_default_active_set_eps_iter_ref),
      debug_info_(FLAGS_default_enable_active_set_debug_info) {}

bool ActiveSetQpSolver::Solve() {
  ::qpOASES::QProblem qp_problem(num_param_, num_constraint_);
  ::qpOASES::Options my_options;
  my_options.epsNum = qp_eps_num_;
  my_options.epsDen = qp_eps_den_;
  my_options.epsIterRef = qp_eps_iter_ref_;
  qp_problem.setOptions(my_options);
  if (!debug_info_) {
    qp_problem.setPrintLevel(qpOASES::PL_NONE);
  }
  // definition of qpOASESproblem
  double h_matrix[kernel_matrix_.rows() * kernel_matrix_.cols()];  // NOLINT
  double g_matrix[offset_.rows()];                                 // NOLINT
  int index = 0;

  for (int r = 0; r < kernel_matrix_.rows(); ++r) {
    g_matrix[r] = offset_(r, 0);

    for (int c = 0; c < kernel_matrix_.cols(); ++c) {
      h_matrix[index++] = kernel_matrix_(r, c);
    }
  }

  // search space lower bound and uppper bound
  double lower_bound[num_param_];  // NOLINT
  double upper_bound[num_param_];  // NOLINT

  // TODO(fanhaoyang): <NAME> change this to a configurable version
  for (int i = 0; i < num_param_; ++i) {
    lower_bound[i] = l_lower_bound_;
    upper_bound[i] = l_upper_bound_;
  }

  // constraint matrix construction
  double affine_constraint_matrix[num_param_ * num_constraint_];  // NOLINT
  double constraint_lower_bound[num_constraint_];                 // NOLINT
  double constraint_upper_bound[num_constraint_];                 // NOLINT
  index = 0;

  for (int r = 0; r < affine_equality_matrix_.rows(); ++r) {
    // TODO(fanhaoyang): change to a configurable version
    constraint_lower_bound[r] = affine_equality_boundary_(r, 0);
    constraint_upper_bound[r] = affine_equality_boundary_(r, 0);

    for (int c = 0; c < num_param_; ++c) {
      affine_constraint_matrix[index++] = affine_equality_matrix_(r, c);
    }
  }

  for (int r = 0; r < affine_inequality_matrix_.rows(); ++r) {
    constraint_lower_bound[r + affine_equality_boundary_.rows()] =
        affine_inequality_boundary_(r, 0);
    constraint_upper_bound[r + affine_equality_boundary_.rows()] =
        constraint_upper_bound_;

    // TODO(fanhaoyang): change to a configurable version
    for (int c = 0; c < num_param_; ++c) {
      affine_constraint_matrix[index++] = affine_inequality_matrix_(r, c);
    }
  }

  // initialize problem
  int max_iter = std::max(max_iteration_, num_constraint_);

  auto ret = qp_problem.init(h_matrix, g_matrix, affine_constraint_matrix,
                             lower_bound, upper_bound, constraint_lower_bound,
                             constraint_upper_bound, max_iter);
  if (ret != qpOASES::SUCCESSFUL_RETURN) {
    if (ret == qpOASES::RET_MAX_NWSR_REACHED) {
      AERROR << "qpOASES solver failed due to reached max iteration";
    } else {
      AERROR << "qpOASES solver failed due to infeasibility or other internal "
                "reasons";
    }
    return false;
  }

  double result[num_param_];  // NOLINT

  qp_problem.getPrimalSolution(result);

  params_ = Eigen::MatrixXd::Zero(num_param_, 1);

  if (qp_problem.isSolved() == qpOASES::BT_TRUE) {
    for (int i = 0; i < num_param_; ++i) {
      params_(i, 0) = result[i];
    }

    return true;
  }

  return false;
}

void ActiveSetQpSolver::set_qp_eps_num(const double eps) { qp_eps_num_ = eps; }

void ActiveSetQpSolver::set_qp_eps_den(const double eps) { qp_eps_den_ = eps; }

void ActiveSetQpSolver::set_qp_eps_iter_ref(const double eps) {
{'fraction_non_alphanumeric': 0.13116738037609385, 'fraction_numerical': 0.004468441630981195, 'mean_word_length': 3.8809631985461155, 'pattern_counts': {'":': 0, '<': 36, '<?xml version=': 0, '>': 10, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3627714', 'n_tokens_mistral': 3646, 'n_tokens_neox': 3443, 'n_words': 770}