diff --git a/src/specmicp_common/micpsolver/CMakeLists.txt b/src/specmicp_common/micpsolver/CMakeLists.txt index bf3049e..0f57394 100644 --- a/src/specmicp_common/micpsolver/CMakeLists.txt +++ b/src/specmicp_common/micpsolver/CMakeLists.txt @@ -1,24 +1,23 @@ set(micpsolver_headers micpsolver_base.hpp micpsolver_base.inl micpsolver.hpp micpsolver.inl micpsolver_structs.hpp micpprog.hpp boxviprog.hpp - boxvisolver.hpp - boxvisolver.inl + reformulations.inl ncp_function.hpp estimate_cond_number.hpp micpsolver_min.hpp # to remove at some point micpsolver_min.inl # to remove at some point micpsolverold.hpp # to remove at some point micpsolverold.inl # to remove at some point ) add_to_main_headers_list(micpsolver_headers) install(FILES ${micpsolver_headers} DESTINATION ${INCLUDE_INSTALL_DIR}/specmicp_common/micpsolver ) diff --git a/src/specmicp_common/micpsolver/boxvisolver.hpp b/src/specmicp_common/micpsolver/boxvisolver.hpp deleted file mode 100644 index cb6b1b9..0000000 --- a/src/specmicp_common/micpsolver/boxvisolver.hpp +++ /dev/null @@ -1,241 +0,0 @@ -/* ============================================================================= - - Copyright (c) 2014 - 2016 - F. Georget Princeton University - All rights reserved. - - -Redistribution and use in source and binary forms, with or without modification, -are permitted provided that the following conditions are met: - - 1. Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - 2. Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - 3. Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from this - software without specific prior written permission. - -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON -ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * - -============================================================================= */ - -#ifndef SPECMIC_MICPSOLVER_BOXVISOLVER_HPP -#define SPECMIC_MICPSOLVER_BOXVISOLVER_HPP - -#include "specmicp_common/types.hpp" -#include "micpsolver_base.hpp" -#include "ncp_function.hpp" - -//! \file boxvisolver.hpp -//! \brief The Box constrained VI solver - -namespace specmicp { -namespace micpsolver { - -//! \class BoxVISolver -//! \brief The Box constrained VI Solver -//! -//! ### Mathematics -//! -//! References : -//! - Munson et al (2001) \cite Munson2001 -//! - Facchinei and Pang (2003) \cite Facchinei2003 -//! -//! ### Usage -//! -//! The options to customize the solver are contained in the struct -//! specmicp::micpsolver::MiCPSolverOptions . -//! -//! \tparam program_t a subclass of MiCPProg -//! -//! \sa specmicp::micpsolver::MiCPProgram -template -class BoxVISolver: - public MiCPSolverBaseProgram -{ -public: - using base = MiCPSolverBaseProgram; - using base::get_options; - using base::get_program; - using base::get_perfs; - using base::get_neq; - using base::get_neq_free; - - //! \brief Constructor - //! - //! \param prog smart pointer toward an instance of a program_t to solve - BoxVISolver(std::shared_ptr prog): - MiCPSolverBaseProgram(prog), - m_residuals(prog->total_variables()), - m_phi_residuals(Vector::Zero(prog->total_variables())), - m_jacobian(prog->total_variables(),prog ->total_variables()) - {} - - //! \brief Compute the residuals, use internal storage - //! - //! \param[in] x the variables - void compute_residuals(const Vector& x) - { - base::compute_residuals(x, m_residuals); - } - //! \brief Compute the jacobian - //! - //! Assumes that the residual have been computed before - //! - //! \param[in] x the variables - void compute_jacobian(Vector& x) - { - base::compute_jacobian(x, m_jacobian); - } - - //! \brief Reformulation of the residuals - //! - //! Reformulate the problem - assumes that r, the residual, has been computed before - //! - //! \param[in] x the variables - //! \param[in] r the residuals - //! \param[out] r_phi a vector of size neq, which will contain the reformulated residuals - void reformulate_residuals(const Vector& x, - const Vector& r, - Vector& r_phi); - //! \brief Reformulation of the residuals *inplace* - //! - //! \param[in] x the variables - //! \param[in,out] r the residual, will contain the reformulated residuals - void reformulate_residuals_inplace(const Vector &x, - Vector &r); - //! \brief Reformulation of the jacobian - //! - //! r is the original vector of residuals (not reformulated) - void reformulate_jacobian(const Vector& x); - - - // Algorithm - // ######### - - //! \brief Solver the program using x as initial guess - //! - //! \param[in,out] x the initial guess, as output contains the solution (from the last iteration) - MiCPSolverReturnCode solve(Vector& x); - - //! \brief Setup the residuals - //! - //! \param[in] x the current solution - void setup_residuals(const Eigen::VectorXd& x) { - compute_residuals(x); - reformulate_residuals(x, m_residuals, m_phi_residuals); - } - //! \brief Setup the jacobian - //! - //! \param[in] x the current solution - void setup_jacobian(Eigen::VectorXd& x) { - compute_jacobian(x); - reformulate_jacobian(x); - m_grad_phi = m_jacobian.transpose() * m_phi_residuals; - } - //! \brief Solve the Newton system - //! - //! The system will be scaled before being solved, may be expensive and not necessary. - //! Do disable the scaling, disable the corresponding options in specmicp::MiCPSolver::MiCPSolverOptions. - //! It assumes that the Newton system has been formed - //! - //! \param[out] update the update to apply to the solution - //! - //! \sa search_direction_calculation_no_scaling. - MiCPSolverReturnCode search_direction_calculation(Vector& update); - - //! \brief Solve the Newton system - does not scale the jacobian - //! - //! The system will not be scaled. - //! It assumes that the Newton system has been formed - //! - //! \param[out] update the update to apply to the solution - //! - //! \sa search_direction_scaling - MiCPSolverReturnCode search_direction_calculation_no_scaling(Vector &update); - - //! \brief Linesearch - //! - //! It is a backtracking linesearch. - //! If the correct option is set, this is a nonmonotone linesearch. - //! - //! \param[in,out] update the update of the timestep - //! \param[in,out] x the current solution - //! - //! References : - //! - \cite Dennis1983 - //! - \cite Munson2001 - int linesearch(Eigen::VectorXd& update, Eigen::VectorXd& x); - - //! \brief Crashing - //! - //! This function improves, if possible, the initial guess - //! - //! \param[in] x the current solution - //! - //! Reference : - //! - \cite Munson2001 - void crashing(Vector &x); - -protected: - Vector& get_residuals() { - return m_residuals; - } - Vector get_reformulated_residuals() { - return m_phi_residuals; - } - Matrix& get_jacobian() { - return m_jacobian; - } -private: - - // Residuals and jacobian - Eigen::VectorXd m_residuals; //!< The residuals - Eigen::VectorXd m_phi_residuals; //!< The reformulated residuals - Eigen::VectorXd m_grad_phi; //!< The gradient of the reformulated residuals - Eigen::MatrixXd m_jacobian; //!< The jacobian - - std::vector m_max_merits; //!< Contains the m best value of the merit function - - bool m_gradient_step_taken {false}; //!< True if the update was computed using the gradient - - - void derivative_box_constrained_vi_b_function( - index_t i, - scalar_t x, - scalar_t r, - scalar_t v, - const Vector& z - ); - - void deritative_cck( - index_t i, - scalar_t x, - scalar_t r, - scalar_t t, - const Vector& z - ); -}; - -} // end namespace micpsolver -} // end namespace specmicp - -// ###############// -// Implementation // -// ###############// -#include "boxvisolver.inl" - -#endif // SPECMIC_MICPSOLVER_BOXVISOLVER_HPP diff --git a/src/specmicp_common/micpsolver/boxvisolver.inl b/src/specmicp_common/micpsolver/boxvisolver.inl deleted file mode 100644 index 2e48073..0000000 --- a/src/specmicp_common/micpsolver/boxvisolver.inl +++ /dev/null @@ -1,635 +0,0 @@ -/* ============================================================================= - - Copyright (c) 2014 - 2016 - F. Georget Princeton University - All rights reserved. - - -Redistribution and use in source and binary forms, with or without modification, -are permitted provided that the following conditions are met: - - 1. Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - 2. Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - 3. Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from this - software without specific prior written permission. - -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON -ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * - -============================================================================= */ - -#ifndef SPECMIC_MICPSOLVER_BOXVISOLVER_HPP -#include "boxvisolver.hpp" // for syntaxic coloration... -#endif // SPECMIC_MICPSOLVER_BOXVISOLVER_HPP - -#include -#include "estimate_cond_number.hpp" - -#include "specmicp_common/log.hpp" - - -//! \file boxvisolver.inl implementation of the box VI solver - -namespace specmicp { -namespace micpsolver { - - -template -void BoxVISolver::reformulate_residuals( - const Vector &x, - const Vector &r, - Vector &r_phi - ) -{ - scalar_t upper_bound; - // reformulation with copy from r to r_phi - r_phi.resizeLike(r); - r_phi.block(0, 0, get_neq_free(), 1) = r.block(0, 0, get_neq_free(), 1); - for (index_t i = get_neq_free(); iis_box_vi(i, upper_bound)) - { - r_phi(i) = penalized_fisher_burmeister( - x(i), r(i), get_options().penalization_factor); - } - else - { - if (upper_bound == 0.0) - r_phi(i) = 0; - else - r_phi(i) = box_constrained_vi_b_function( - x(i), r(i), 0.0, upper_bound); - } - } -} - -template -void BoxVISolver::reformulate_residuals_inplace( - const Vector& x, - Vector& r - ) -{ - scalar_t upper_bound; - for (index_t i = get_neq_free(); iis_box_vi(i, upper_bound)) - { - r(i) = penalized_fisher_burmeister( - x(i), r(i), get_options().penalization_factor); - } - else - { - if (upper_bound == 0.0) - r(i) = 0.0; - else - r(i) = box_constrained_vi_b_function( - x(i), r(i), 0.0, upper_bound); - } - } -} - -template -void BoxVISolver::reformulate_jacobian( - const Vector& x - ) -{ - - scalar_t upper_bound; - Vector& r = get_residuals(); - - Eigen::VectorXd z(Eigen::VectorXd::Zero(get_neq())); - for (index_t i=get_neq_free(); iis_box_vi(i, upper_bound) ) - { - is_degenerate = (x(i) == upper_bound and r(i) == 0.0); - } - if (is_degenerate) - z(i) = 1.0; - } - - for (index_t i = get_neq_free(); iis_box_vi(i, upper_bound)) - { - derivative_box_constrained_vi_b_function( - i, x(i), r(i), upper_bound, z); - } - else - { - deritative_cck(i, x(i), r(i), get_options().penalization_factor, z); - } - } -} - - -template -void BoxVISolver::derivative_box_constrained_vi_b_function( - index_t i, - scalar_t x, - scalar_t r, - scalar_t v, - const Vector& z - ) -{ - bool is_degenerate = false; - - const scalar_t u = 0.0; - scalar_t Da = 0.0; - scalar_t Db = 0.0; - auto& jacobian = get_jacobian(); - - if (v == 0.0) - { - jacobian.row(i).setZero(); - jacobian(i,i) = 1; - } - - - if (x <= u and r >= 0) - { - if (x == u and r ==0) {is_degenerate = true;} - Da = 1; - Db = 0; - } - else if (x >= v and r <= 0) - { - if (x == v and r ==0) {is_degenerate = true;} - Da = 1; - Db = 0; - } - else if (x > u and x <= v and r >= 0) - { - if (x == v and r == 0) {is_degenerate = true;} - const scalar_t hsqm = sqrt_square_sum((x-u), r); - Da = 1 - (x - u)/hsqm; - Db = 1 - r / hsqm; - } - else if ( x >= u and r < v and r <= 0) - { - if (x == u and r == 0) {is_degenerate = true;} - const scalar_t hsqp = sqrt_square_sum((x-v), r); - Da = 1 + (x - v)/hsqp; - Db = 1 + r / hsqp; - } - else if ( (x > v and r > 0) - or (x < u and r < 0)) - { - const scalar_t hsqm = sqrt_square_sum((x-u), r); - const scalar_t hsqp = sqrt_square_sum((x-v), r); - Da = 1 - (x - u)/hsqm + (x - v)/hsqp; - Db = - r/hsqm + r/hsqp; - } - - if (is_degenerate) { - if (x==u) - { - const scalar_t gpdotz = jacobian.row(i).dot(z); - const scalar_t s = sqrt_square_sum(z(i), gpdotz); - Da = (z(i)/s - 1); - Db = (gpdotz/s -1); - - } else - { - const scalar_t gpdotz = jacobian.row(i).dot(z); - const scalar_t s = sqrt_square_sum(z(i), gpdotz); - Da = (z(i)/s - 1); - Db = (gpdotz/s -1); - } - - } - - // update jacobian - jacobian.row(i) *= Db; - jacobian(i, i) += Da; - - - return; -} - - -template -void BoxVISolver::deritative_cck( - index_t i, - scalar_t x, - scalar_t r, - scalar_t t, - const Vector& z - ) -{ - - scalar_t Da; - scalar_t Db; - - auto& jacobian = get_jacobian(); - - if (z(i) != 0) - { - const scalar_t gpdotz = jacobian.row(i).dot(z); - const scalar_t s = sqrt_square_sum(z(i), gpdotz); - Da = t*(z(i)/s - 1); - Db = t*(gpdotz/s -1); - } - else - { - const scalar_t s = sqrt_square_sum(x, r); - Da = t*(x/s - 1); - Db = t*(r/s - 1); - if ((t <1) and (r > 0) and (x >0)) - { - Da -= (1-t)*r; - Db -= (1-t)*x; - } - } - - jacobian.row(i) *= Db; - jacobian(i, i) += Da; -} - - -template -MiCPSolverReturnCode BoxVISolver::solve(Vector &x) -{ - int cnt = 0; - if (get_options().use_crashing) crashing(x); - else setup_residuals(x); - MiCPSolverReturnCode retcode = MiCPSolverReturnCode::NotConvergedYet; - Eigen::VectorXd update(get_neq()); - while (retcode == MiCPSolverReturnCode::NotConvergedYet) - { - DEBUG << "Iteration : " << cnt; - // (S.0) Initialization of the iteration - // this is a hook for simultaneous fixed-point iterations - // implemented to solve the non-ideality model - bool may_have_converged = get_program()->hook_start_iteration(x, m_phi_residuals.norm()); - // (S.1) Check the convergence - setup_residuals(x); - get_perfs().current_residual = m_phi_residuals.norm(); - DEBUG << "Norm residuals : " << get_perfs().current_residual; - retcode = base::check_convergence(cnt, update, x, m_phi_residuals, may_have_converged); - get_perfs().return_code = retcode; - if (retcode != MiCPSolverReturnCode::NotConvergedYet) break; - ++cnt; - // (S.2) Compute the jacobian and solve the linear problem - setup_jacobian(x); - if(get_options().use_scaling) - search_direction_calculation(update); - else - search_direction_calculation_no_scaling(update); - // (S.3) Linesearch - int termcode = linesearch(update, x); - if (termcode != 0) retcode = MiCPSolverReturnCode::LinesearchFailure; - get_perfs().return_code = retcode; - get_perfs().current_update = update.norm(); - DEBUG << "Return LineSearch : " << termcode; - base::projection(x); // project x to the positive quadrant - get_perfs().nb_iterations = cnt; - } - return retcode; -} - -template -MiCPSolverReturnCode BoxVISolver::search_direction_calculation(Eigen::VectorXd& update) -{ - using SolverT = Eigen::ColPivHouseholderQR; - Vector rscaler(Vector::Ones(m_jacobian.cols())); - Vector cscaler(Vector::Ones(m_jacobian.rows())); - base::scaling_jacobian(m_jacobian, m_phi_residuals, rscaler, cscaler); - m_jacobian = rscaler.asDiagonal() * (m_jacobian) * cscaler.asDiagonal(); - SolverT solver(m_jacobian.rows(), m_jacobian.cols()); // it needs to be correctly initialized - m_gradient_step_taken = false; - int m; - for (m=0; m 0) - { - scalar_t cond = estimate_condition_number(solver.matrixR()); - if (cond > get_options().condition_limit) - { - m_gradient_step_taken = true; - m_jacobian.diagonal() += lambda*rscaler.cwiseProduct(cscaler); - continue; - } - } - update = solver.solve(-rscaler.cwiseProduct(m_phi_residuals + m*lambda*m_grad_phi)); - update = cscaler.cwiseProduct(update); - if (get_options().factor_descent_condition < 0 ) break; // we have a solution - // else we compute the descent condition - scalar_t descent_cond = m_grad_phi.dot(update); - scalar_t norm_grad = m_grad_phi.norm(); - scalar_t norm_update = update.norm(); - - if ((descent_cond <= -get_options().factor_descent_condition*std::min( - std::pow(norm_update,2),std::pow(norm_update,3))) - and (descent_cond <= -get_options().factor_descent_condition*std::min( - std::pow(norm_grad,2),std::pow(norm_grad,3))) - ) - break; // we have a solution ! - m_gradient_step_taken = true; - - m_jacobian.diagonal() += lambda*rscaler.cwiseProduct(cscaler); - } - DEBUG << "Gradient step : m = " << m; - if (m == get_options().max_factorization_step) { - INFO << "Full gradient step taken !"; - update = -m_grad_phi; - } - return MiCPSolverReturnCode::NotConvergedYet; -} - -template -MiCPSolverReturnCode BoxVISolver::search_direction_calculation_no_scaling( - Vector& update - ) -{ - using SolverT = Eigen::ColPivHouseholderQR; - SolverT solver(m_jacobian.rows(), m_jacobian.cols()); // it needs to be correctly initialized - // if everything is ok, this is the only decomposition - m_gradient_step_taken = false; - int m; // this is an index that increase the contribution of the gradient in the solution - // the pure Newton solution is when m=0 - for (m=0; m 0) - { - scalar_t cond = estimate_condition_number(solver.matrixR()); - - if (cond > get_options().condition_limit) - { - // add the perturbation - m_gradient_step_taken = true; - m_jacobian.diagonal() += Eigen::VectorXd::Constant(m_jacobian.rows(), lambda); - } - } - update = solver.solve(-(m_phi_residuals + m*lambda*m_grad_phi)); - - if (get_options().factor_descent_condition < 0 ) break; // we have a solution - - scalar_t descent_cond = m_grad_phi.dot(update); - scalar_t norm_grad = m_grad_phi.norm(); - scalar_t norm_update = update.norm(); - - if ( (descent_cond <= -get_options().factor_descent_condition*std::min( - std::pow(norm_update,2),std::pow(norm_update,3))) - and (descent_cond <= -get_options().factor_descent_condition*std::min( - std::pow(norm_grad,2),std::pow(norm_grad,3))) - ) - break; // we have a solution ! - - // add the perturbation - m_gradient_step_taken = true; - m_jacobian.diagonal() += Eigen::VectorXd::Constant(m_jacobian.rows(), lambda); - } - DEBUG << "Gradient step : m = " << m; - if (m_gradient_step_taken && m == get_options().max_factorization_step) { - INFO << "Full gradient step taken !"; - update = -m_grad_phi; - } - return MiCPSolverReturnCode::NotConvergedYet; -} - -template -void BoxVISolver::crashing(Vector& x) -{ - // steepest descent direction algorithm - DEBUG << "Crashing "; - const scalar_t beta = 0.5; - const scalar_t sigma = 1e-5; - int cnt = 0; - while (cnt < 10) - { - setup_residuals(x); - setup_jacobian(x); - m_grad_phi = m_jacobian.transpose()*m_phi_residuals; - Vector xp(get_neq()); - int l=0; - const int maxl = 10; - while (l -int BoxVISolver::linesearch(Eigen::VectorXd& p, Eigen::VectorXd& x) -{ - // References - // ---------- - // - Algo A6.3.1 : Dennis and Schnabel (1983) - // - Munson et al. (2001) - // - Facchinei (2003) - // - Nocedal & Wrigth (2006) - DEBUG << "Linesearch"; - Eigen::VectorXd xp(get_neq()); - Eigen::VectorXd new_res(get_neq()); - double fcp; - - get_perfs().max_taken = false; - int retcode = 2; - const double alpha = 1e-4; - double newtlen = base::is_step_too_long(p); - double init_slope = m_grad_phi.dot(p); - double rellength = std::abs(p(0)); - for (int i=1; imax_lambda(x, p); - double lambda_prev = lambda; - - // non monotone linesearch - // ======================= - - double merit_value = 0.5*m_phi_residuals.squaredNorm(); - // new residual - xp = x + lambda*p; - base::compute_residuals(xp, new_res); - reformulate_residuals_inplace(xp, new_res); - fcp = 0.5*new_res.squaredNorm(); - - // Skip linesearch if enough progress is done - // ------------------------------------------ - if (fcp < get_options().coeff_accept_newton_step *merit_value) - { - if (m_max_merits.size() > 0) m_max_merits[m_max_merits.size()-1] = merit_value; - else m_max_merits.push_back(merit_value); - x = xp; - return 0; - } - // Select the merit value of reference - // ----------------------------------- - if (get_options().non_monotone_linesearch) - { - double mmax = merit_value; - if (m_max_merits.size() > 0) - { - mmax = m_max_merits[m_max_merits.size()-1]; - // check for cycling - // - - - - - - - - - - if ( m_max_merits.size() ==4 - && std::fabs(mmax - merit_value) < get_options().threshold_cycling_linesearch*get_options().fvectol) - { - //std::cout << merit_value << std::endl; - WARNING << "Cycling has been detected by the linesearch - Taking the full Newton step"; - x = xp; - p = lambda*p; - return 3; - } - } - if (m_max_merits.size() < 4) - { - m_max_merits.push_back(merit_value); - if (merit_value < mmax) merit_value = (3*merit_value + mmax)/4; - } - else if (merit_value < mmax) - { - m_max_merits[3] = merit_value; - merit_value = mmax; - } - if (m_gradient_step_taken) - { - merit_value *= 100; - } - } - // The linesearch - // -------------- - double fc = merit_value; - double fcp_prev; - int cnt = 0; - do - { - fcp = 0.5*new_res.squaredNorm(); - if (fcp <= fc - std::min(-alpha*lambda*init_slope,(1-alpha)*fc)) //pg760 Fachinei2003 - { - retcode = 0; - if (lambda ==1 and (newtlen > 0.99 * get_options().maxstep)) { - get_perfs().max_taken = true; - } - break; - } - else if (lambda < minlambda) - { - ERROR << "Linesearch cannot find a solution bigger than the minimal step"; - retcode = 1; - break; - } - else - { - // Select a new step length - // - - - - - - - - - - - - - double lambdatmp; - if (cnt == 0) { // only a quadratic at the first - lambdatmp = - init_slope / (2*(fcp - fc -init_slope)); - } - else - { - const double factor = 1 /(lambda - lambda_prev); - const double x1 = fcp - fc - lambda*init_slope; - const double x2 = fcp_prev - fc - lambda_prev*init_slope; - - const double a = factor * ( x1/(lambda*lambda) - x2/(lambda_prev*lambda_prev)); - const double b = factor * ( -x1*lambda_prev/(lambda*lambda) + x2*lambda/(lambda_prev*lambda_prev)); - - if (a == 0) - { // cubic interpolation is in fact a quadratic interpolation - lambdatmp = - init_slope/(2*b); - } - else - { - const double disc = b*b-3*a*init_slope; - lambdatmp = (-b+std::sqrt(disc))/(3*a); - } - if (lambdatmp > 0.5*lambda ) lambdatmp = 0.5*lambda; - } - lambda_prev = lambda; - fcp_prev = fcp; - if (lambdatmp < 0.1*lambda) { - lambda = 0.1 * lambda; - } else { - lambda = lambdatmp; - } - if (not std::isfinite(lambda)) - { - ERROR << "Lambda is non finite in micpsolver linesearch - we stop"; - return 3; - } - } - xp = x + lambda*p; - base::compute_residuals(xp, new_res); - reformulate_residuals_inplace(xp, new_res); - ++cnt; - } while(retcode == 2 and cnt < 100); - DEBUG << "Lambda : " << lambda; - if (cnt == 100) - { - ERROR << "Too much linesearch iterations ! We stop"; - } - x = xp; - p = lambda*p; - return retcode; -} - -} // end namespace micpsolver -} // end namespace specmicp diff --git a/src/specmicp_common/micpsolver/micpsolver.hpp b/src/specmicp_common/micpsolver/micpsolver.hpp index 894358b..5c1d598 100644 --- a/src/specmicp_common/micpsolver/micpsolver.hpp +++ b/src/specmicp_common/micpsolver/micpsolver.hpp @@ -1,258 +1,311 @@ /* ============================================================================= Copyright (c) 2014 - 2016 F. Georget Princeton University All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * ============================================================================= */ #ifndef SPECMIC_MICPSOLVER_MICPSOLVER_HPP #define SPECMIC_MICPSOLVER_MICPSOLVER_HPP #include "specmicp_common/types.hpp" #include "micpsolver_base.hpp" -#include "ncp_function.hpp" //! \file micpsolver.hpp //! \brief The MiCP solver namespace specmicp { //! \namespace specmicp::micpsolver //! \brief the MiCP Solver(s) and related classes namespace micpsolver { +enum class ReformulationF +{ + CCK, + BoxVI +}; + //! \class MiCPSolver //! \brief The MiCP Solver //! //! ### Mathematics //! //! Solve //! - \f$ G(u, v) = 0\f$ //! - \f$ 0 \leq v \perp H(u,v) \geq 0 \f$ //! //! Using the penalized Fisher-Burmeister C-function. //! This is a semismooth Newton solver with many features including : //! //! - Non-monotone linesearch //! - Scaling of the jacobian //! - Check of the condition number of the jacobian //! - Check of the descent directions //! - Crashing //! - ... //! //! References : //! - Munson et al (2001) \cite Munson2001 //! - Facchinei and Pang (2003) \cite Facchinei2003 //! //! ### Usage //! //! The options to customize the solver are contained in the struct //! specmicp::micpsolver::MiCPSolverOptions . //! //! \tparam program_t a subclass of MiCPProg //! //! To separate the program and the solver, the program is passed as a shared_ptr. //! The MiCPSolver is implemented using the CRTP pattern. //! The advantage is that we have compile time polymorphism instead of //! the runtime polymorphism when using the virtual functions. //! I am not sure of the effectiveness of this method (I have not run any benchmarks) //! but since the solver is destined to be called many times any small gain is good to have. //! Also, more compile-time optimization is possible (especially inlining). //! Anyway, that was fun to code... //! //! \sa specmicp::micpsolver::MiCPProgram -template +template class MiCPSolver: public MiCPSolverBaseProgram { public: using base = MiCPSolverBaseProgram; using base::get_options; using base::get_program; using base::get_perfs; using base::get_neq; using base::get_neq_free; //! \brief Constructor //! //! \param prog smart pointer toward an instance of a program_t to solve MiCPSolver(std::shared_ptr prog): MiCPSolverBaseProgram(prog), m_residuals(prog->total_variables()), m_phi_residuals(Vector::Zero(prog->total_variables())), m_jacobian(prog->total_variables(),prog ->total_variables()) {} // Merit function // ############## // Residuals and jacobian // ---------------------- //! \brief Compute the residuals, use internal storage //! //! \param[in] x the variables void compute_residuals(const Vector& x) { base::compute_residuals(x, m_residuals); } //! \brief Compute the jacobian //! //! Assumes that the residual have been computed before //! //! \param[in] x the variables void compute_jacobian(Vector& x) { base::compute_jacobian(x, m_jacobian); } - //! \brief Reformulation of the residuals - //! - //! Reformulate the problem - assumes that r, the residual, has been computed before - //! - //! \param[in] x the variables - //! \param[in] r the residuals - //! \param[out] r_phi a vector of size neq, which will contain the reformulated residuals - void reformulate_residuals(const Vector& x, - const Vector& r, - Vector& r_phi); - //! \brief Reformulation of the residuals *inplace* - //! - //! \param[in] x the variables - //! \param[in,out] r the residual, will contain the reformulated residuals - void reformulate_residuals_inplace(const Vector &x, - Vector &r); - //! \brief Reformulation of the jacobian - //! - //! r is the original vector of residuals (not reformulated) - void reformulate_jacobian(const Vector& x) { - base::reformulate_jacobian_cck(x, m_residuals, m_jacobian); - } + + // Algorithm // ######### //! \brief Solver the program using x as initial guess //! //! \param[in,out] x the initial guess, as output contains the solution (from the last iteration) MiCPSolverReturnCode solve(Vector& x); //! \brief Setup the residuals //! //! \param[in] x the current solution - void setup_residuals(const Eigen::VectorXd& x) { - compute_residuals(x); - reformulate_residuals(x, m_residuals, m_phi_residuals); - } + void setup_residuals(const Eigen::VectorXd& x); //! \brief Setup the jacobian //! //! \param[in] x the current solution - void setup_jacobian(Eigen::VectorXd& x) { - compute_jacobian(x); - reformulate_jacobian(x); - m_grad_phi = m_jacobian.transpose() * m_phi_residuals; - } + void setup_jacobian(Eigen::VectorXd& x); + //! \brief Solve the Newton system //! //! The system will be scaled before being solved, may be expensive and not necessary. //! Do disable the scaling, disable the corresponding options in specmicp::MiCPSolver::MiCPSolverOptions. //! It assumes that the Newton system has been formed //! //! \param[out] update the update to apply to the solution //! //! \sa search_direction_calculation_no_scaling. MiCPSolverReturnCode search_direction_calculation(Vector& update); //! \brief Solve the Newton system - does not scale the jacobian //! //! The system will not be scaled. //! It assumes that the Newton system has been formed //! //! \param[out] update the update to apply to the solution //! //! \sa search_direction_scaling MiCPSolverReturnCode search_direction_calculation_no_scaling(Vector &update); //! \brief Linesearch //! //! It is a backtracking linesearch. //! If the correct option is set, this is a nonmonotone linesearch. //! //! \param[in,out] update the update of the timestep //! \param[in,out] x the current solution //! //! References : //! - \cite Dennis1983 //! - \cite Munson2001 int linesearch(Eigen::VectorXd& update, Eigen::VectorXd& x); //! \brief Crashing //! //! This function improves, if possible, the initial guess //! //! \param[in] x the current solution //! //! Reference : //! - \cite Munson2001 void crashing(Vector &x); + //! \brief Reformulation of the residuals + //! + //! Reformulate the problem - assumes that r, the residual, has been computed before + //! + //! \param[in] x the variables + //! \param[in] r the residuals + //! \param[out] r_phi a vector of size neq, which will contain the reformulated residuals + void reformulate_residuals( + const Vector& x, + const Vector& r, + Vector& r_phi + ) { + Reformulation::reformulate_residuals(this, x, r, r_phi); + } + //! \brief Reformulation of the residuals *inplace* + //! + //! \param[in] x the variables + //! \param[in,out] r the residual, will contain the reformulated residuals + void reformulate_residuals_inplace( + const Vector &x, + Vector &r + ) { + Reformulation::reformulate_residuals_inplace(this, x, r); + } + + //! \brief Reformulation of the jacobian + //! + //! r is the original vector of residuals (not reformulated) + void reformulate_jacobian( + const Vector& x + ) { + Reformulation::reformulate_jacobian(this, x); + } + protected: - Eigen::VectorXd& get_residuals() { + + template + struct Reformulation{ + //! \brief Reformulation of the residuals + //! + //! Reformulate the problem - assumes that r, the residual, has been computed before + //! + //! \param[in] x the variables + //! \param[in] r the residuals + //! \param[out] r_phi a vector of size neq, which will contain the reformulated residuals + static void reformulate_residuals( + MiCPSolver* const parent, + const Vector& x, + const Vector& r, + Vector& r_phi + ); + //! \brief Reformulation of the residuals *inplace* + //! + //! \param[in] x the variables + //! \param[in,out] r the residual, will contain the reformulated residuals + static void reformulate_residuals_inplace( + MiCPSolver* const parent, + const Vector &x, + Vector &r + ); + //! \brief Reformulation of the jacobian + //! + //! r is the original vector of residuals (not reformulated) + static void reformulate_jacobian( + MiCPSolver* const parent, + const Vector& x + ); + }; + + //! \brief Return the residuals (as givent by the system) + Vector& get_residuals() { return m_residuals; } - Eigen::VectorXd& get_reformulated_residuals() { + //! \brief Return the reformulated residuals + Vector& get_reformulated_residuals() { return m_phi_residuals; } - Eigen::VectorXd& get_jacobian() { + //! \brief Return the jacobian + //! + //! The reformulation is in-place so the meaning is dependent on the call + Matrix& get_jacobian() { return m_jacobian; } private: // Residuals and jacobian - Eigen::VectorXd m_residuals; //!< The residuals - Eigen::VectorXd m_phi_residuals; //!< The reformulated residuals - Eigen::VectorXd m_grad_phi; //!< The gradient of the reformulated residuals - Eigen::MatrixXd m_jacobian; //!< The jacobian + Vector m_residuals; //!< The residuals + Vector m_phi_residuals; //!< The reformulated residuals + Vector m_grad_phi; //!< The gradient of the reformulated residuals + Matrix m_jacobian; //!< The jacobian std::vector m_max_merits; //!< Contains the m best value of the merit function bool m_gradient_step_taken {false}; //!< True if the update was computed using the gradient }; } // end namespace micpsolver } // end namespace specmicp // ###############// // Implementation // // ###############// -#include "micpsolver.inl" +#include "reformulations.inl" // definitions of the reformulations +#include "micpsolver.inl" // implementations of the solver #endif // SPECMIC_MICPSOLVER_MICPSOLVER_HPP diff --git a/src/specmicp_common/micpsolver/micpsolver.inl b/src/specmicp_common/micpsolver/micpsolver.inl index d04d65a..2a145cf 100644 --- a/src/specmicp_common/micpsolver/micpsolver.inl +++ b/src/specmicp_common/micpsolver/micpsolver.inl @@ -1,453 +1,444 @@ /* ============================================================================= Copyright (c) 2014 - 2016 F. Georget Princeton University All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * ============================================================================= */ #ifndef SPECMIC_MICPSOLVER_MICPSOLVER_HPP #include "micpsolver.hpp" // for syntaxic coloration... #endif // SPECMIC_MICPSOLVER_MICPSOLVER_HPP #include #include "estimate_cond_number.hpp" #include "specmicp_common/log.hpp" //! \file micpsolver.inl implementation of the MiCP solver namespace specmicp { namespace micpsolver { // Main algorithm // ############## -template -MiCPSolverReturnCode MiCPSolver::solve(Vector &x) +template +inline void MiCPSolver::setup_residuals( + const Eigen::VectorXd& x) { + compute_residuals(x); + Reformulation::reformulate_residuals(this, x, m_residuals, m_phi_residuals); +} + +template +inline void MiCPSolver::setup_jacobian( + Eigen::VectorXd& x) { + compute_jacobian(x); + Reformulation::reformulate_jacobian(this, x); + m_grad_phi = m_jacobian.transpose() * m_phi_residuals; +} + +template +MiCPSolverReturnCode MiCPSolver::solve(Vector &x) { int cnt = 0; if (get_options().use_crashing) crashing(x); else setup_residuals(x); MiCPSolverReturnCode retcode = MiCPSolverReturnCode::NotConvergedYet; Eigen::VectorXd update(get_neq()); while (retcode == MiCPSolverReturnCode::NotConvergedYet) { DEBUG << "Iteration : " << cnt; // (S.0) Initialization of the iteration // this is a hook for simultaneous fixed-point iterations // implemented to solve the non-ideality model bool may_have_converged = get_program()->hook_start_iteration(x, m_phi_residuals.norm()); // (S.1) Check the convergence setup_residuals(x); get_perfs().current_residual = m_phi_residuals.norm(); DEBUG << "Norm residuals : " << get_perfs().current_residual; retcode = base::check_convergence(cnt, update, x, m_phi_residuals, may_have_converged); get_perfs().return_code = retcode; if (retcode != MiCPSolverReturnCode::NotConvergedYet) break; ++cnt; // (S.2) Compute the jacobian and solve the linear problem setup_jacobian(x); if(get_options().use_scaling) search_direction_calculation(update); else search_direction_calculation_no_scaling(update); // (S.3) Linesearch int termcode = linesearch(update, x); if (termcode != 0) retcode = MiCPSolverReturnCode::LinesearchFailure; get_perfs().return_code = retcode; get_perfs().current_update = update.norm(); DEBUG << "Return LineSearch : " << termcode; base::projection(x); // project x to the positive quadrant get_perfs().nb_iterations = cnt; } return retcode; } -template -MiCPSolverReturnCode MiCPSolver::search_direction_calculation(Eigen::VectorXd& update) +template +MiCPSolverReturnCode MiCPSolver::search_direction_calculation(Eigen::VectorXd& update) { using SolverT = Eigen::ColPivHouseholderQR; Vector rscaler(Vector::Ones(m_jacobian.cols())); Vector cscaler(Vector::Ones(m_jacobian.rows())); base::scaling_jacobian(m_jacobian, m_phi_residuals, rscaler, cscaler); m_jacobian = rscaler.asDiagonal() * (m_jacobian) * cscaler.asDiagonal(); SolverT solver(m_jacobian.rows(), m_jacobian.cols()); // it needs to be correctly initialized m_gradient_step_taken = false; int m; for (m=0; m 0) { scalar_t cond = estimate_condition_number(solver.matrixR()); if (cond > get_options().condition_limit) { m_gradient_step_taken = true; m_jacobian.diagonal() += lambda*rscaler.cwiseProduct(cscaler); continue; } } update = solver.solve(-rscaler.cwiseProduct(m_phi_residuals + m*lambda*m_grad_phi)); update = cscaler.cwiseProduct(update); if (get_options().factor_descent_condition < 0 ) break; // we have a solution // else we compute the descent condition scalar_t descent_cond = m_grad_phi.dot(update); scalar_t norm_grad = m_grad_phi.norm(); scalar_t norm_update = update.norm(); if ((descent_cond <= -get_options().factor_descent_condition*std::min( std::pow(norm_update,2),std::pow(norm_update,3))) and (descent_cond <= -get_options().factor_descent_condition*std::min( std::pow(norm_grad,2),std::pow(norm_grad,3))) ) break; // we have a solution ! m_gradient_step_taken = true; m_jacobian.diagonal() += lambda*rscaler.cwiseProduct(cscaler); } DEBUG << "Gradient step : m = " << m; if (m == get_options().max_factorization_step) { INFO << "Full gradient step taken !"; update = -m_grad_phi; } return MiCPSolverReturnCode::NotConvergedYet; } -template -MiCPSolverReturnCode MiCPSolver::search_direction_calculation_no_scaling( +template +MiCPSolverReturnCode MiCPSolver::search_direction_calculation_no_scaling( Vector& update ) { using SolverT = Eigen::ColPivHouseholderQR; SolverT solver(m_jacobian.rows(), m_jacobian.cols()); // it needs to be correctly initialized // if everything is ok, this is the only decomposition m_gradient_step_taken = false; int m; // this is an index that increase the contribution of the gradient in the solution // the pure Newton solution is when m=0 for (m=0; m 0) { scalar_t cond = estimate_condition_number(solver.matrixR()); if (cond > get_options().condition_limit) { // add the perturbation m_gradient_step_taken = true; m_jacobian.diagonal() += Eigen::VectorXd::Constant(m_jacobian.rows(), lambda); } } update = solver.solve(-(m_phi_residuals + m*lambda*m_grad_phi)); if (get_options().factor_descent_condition < 0 ) break; // we have a solution scalar_t descent_cond = m_grad_phi.dot(update); scalar_t norm_grad = m_grad_phi.norm(); scalar_t norm_update = update.norm(); if ( (descent_cond <= -get_options().factor_descent_condition*std::min( std::pow(norm_update,2),std::pow(norm_update,3))) and (descent_cond <= -get_options().factor_descent_condition*std::min( std::pow(norm_grad,2),std::pow(norm_grad,3))) ) break; // we have a solution ! // add the perturbation m_gradient_step_taken = true; m_jacobian.diagonal() += Eigen::VectorXd::Constant(m_jacobian.rows(), lambda); } DEBUG << "Gradient step : m = " << m; if (m_gradient_step_taken && m == get_options().max_factorization_step) { INFO << "Full gradient step taken !"; update = -m_grad_phi; } return MiCPSolverReturnCode::NotConvergedYet; } -template -void MiCPSolver::crashing(Vector& x) +template +void MiCPSolver::crashing(Vector& x) { // steepest descent direction algorithm DEBUG << "Crashing "; const scalar_t beta = 0.5; const scalar_t sigma = 1e-5; int cnt = 0; while (cnt < 10) { setup_residuals(x); setup_jacobian(x); m_grad_phi = m_jacobian.transpose()*m_phi_residuals; Vector xp(get_neq()); int l=0; const int maxl = 10; while (l::reformulate_residuals_inplace(this, x, new_res); + scalar_t test = 0.5*(new_res.squaredNorm() + - m_phi_residuals.squaredNorm() + ) + sigma*m_grad_phi.dot(x - xp); if (test <= 0) break; ++l; } if (l == maxl) break; x = xp; ++cnt; } get_perfs().nb_crashing_iterations = cnt; DEBUG << "Crashing iterations : " << cnt; m_max_merits.reserve(4); SPAM << "Solution after crashing \n ------ \n " << x << "\n ----- \n"; } // Others // ###### -template -void MiCPSolver::reformulate_residuals( - const Vector &x, - const Vector &r, - Vector &r_phi +template +int MiCPSolver::linesearch( + Eigen::VectorXd& p, Eigen::VectorXd& x ) -{ - // reformulation with copy from r to r_phi - r_phi.resizeLike(r); - r_phi.block(0, 0, get_neq_free(), 1) = r.block(0, 0, get_neq_free(), 1); - for (index_t i = get_neq_free(); i -void MiCPSolver::reformulate_residuals_inplace( - const Vector& x, - Vector& r - ) -{ - for (index_t i = get_neq_free(); i -int MiCPSolver::linesearch(Eigen::VectorXd& p, Eigen::VectorXd& x) { // References // ---------- // - Algo A6.3.1 : Dennis and Schnabel (1983) // - Munson et al. (2001) // - Facchinei (2003) // - Nocedal & Wrigth (2006) DEBUG << "Linesearch"; Eigen::VectorXd xp(get_neq()); Eigen::VectorXd new_res(get_neq()); double fcp; get_perfs().max_taken = false; int retcode = 2; const double alpha = 1e-4; double newtlen = base::is_step_too_long(p); double init_slope = m_grad_phi.dot(p); double rellength = std::abs(p(0)); for (int i=1; imax_lambda(x, p); double lambda_prev = lambda; // non monotone linesearch // ======================= double merit_value = 0.5*m_phi_residuals.squaredNorm(); // new residual xp = x + lambda*p; base::compute_residuals(xp, new_res); - reformulate_residuals_inplace(xp, new_res); + Reformulation::reformulate_residuals_inplace(this, xp, new_res); fcp = 0.5*new_res.squaredNorm(); // Skip linesearch if enough progress is done // ------------------------------------------ if (fcp < get_options().coeff_accept_newton_step *merit_value) { if (m_max_merits.size() > 0) m_max_merits[m_max_merits.size()-1] = merit_value; else m_max_merits.push_back(merit_value); x = xp; return 0; } // Select the merit value of reference // ----------------------------------- if (get_options().non_monotone_linesearch) { double mmax = merit_value; if (m_max_merits.size() > 0) { mmax = m_max_merits[m_max_merits.size()-1]; // check for cycling // - - - - - - - - - if ( m_max_merits.size() ==4 && std::fabs(mmax - merit_value) < get_options().threshold_cycling_linesearch*get_options().fvectol) { //std::cout << merit_value << std::endl; WARNING << "Cycling has been detected by the linesearch - Taking the full Newton step"; x = xp; p = lambda*p; return 3; } } if (m_max_merits.size() < 4) { m_max_merits.push_back(merit_value); if (merit_value < mmax) merit_value = (3*merit_value + mmax)/4; } else if (merit_value < mmax) { m_max_merits[3] = merit_value; merit_value = mmax; } if (m_gradient_step_taken) { merit_value *= 100; } } // The linesearch // -------------- double fc = merit_value; double fcp_prev; int cnt = 0; do { fcp = 0.5*new_res.squaredNorm(); if (fcp <= fc - std::min(-alpha*lambda*init_slope,(1-alpha)*fc)) //pg760 Fachinei2003 { retcode = 0; if (lambda ==1 and (newtlen > 0.99 * get_options().maxstep)) { get_perfs().max_taken = true; } break; } else if (lambda < minlambda) { ERROR << "Linesearch cannot find a solution bigger than the minimal step"; retcode = 1; break; } else { // Select a new step length // - - - - - - - - - - - - double lambdatmp; if (cnt == 0) { // only a quadratic at the first lambdatmp = - init_slope / (2*(fcp - fc -init_slope)); } else { const double factor = 1 /(lambda - lambda_prev); const double x1 = fcp - fc - lambda*init_slope; const double x2 = fcp_prev - fc - lambda_prev*init_slope; const double a = factor * ( x1/(lambda*lambda) - x2/(lambda_prev*lambda_prev)); const double b = factor * ( -x1*lambda_prev/(lambda*lambda) + x2*lambda/(lambda_prev*lambda_prev)); if (a == 0) { // cubic interpolation is in fact a quadratic interpolation lambdatmp = - init_slope/(2*b); } else { const double disc = b*b-3*a*init_slope; lambdatmp = (-b+std::sqrt(disc))/(3*a); } if (lambdatmp > 0.5*lambda ) lambdatmp = 0.5*lambda; } lambda_prev = lambda; fcp_prev = fcp; if (lambdatmp < 0.1*lambda) { lambda = 0.1 * lambda; } else { lambda = lambdatmp; } if (not std::isfinite(lambda)) { ERROR << "Lambda is non finite in micpsolver linesearch - we stop"; return 3; } } xp = x + lambda*p; base::compute_residuals(xp, new_res); - reformulate_residuals_inplace(xp, new_res); + Reformulation::reformulate_residuals_inplace(this, xp, new_res); ++cnt; } while(retcode == 2 and cnt < 100); DEBUG << "Lambda : " << lambda; if (cnt == 100) { ERROR << "Too much linesearch iterations ! We stop"; } x = xp; p = lambda*p; return retcode; } } // end namespace micpsolver } // end namespace specmicp diff --git a/src/specmicp_common/micpsolver/reformulations.inl b/src/specmicp_common/micpsolver/reformulations.inl new file mode 100644 index 0000000..562651a --- /dev/null +++ b/src/specmicp_common/micpsolver/reformulations.inl @@ -0,0 +1,387 @@ +/* ============================================================================= + + Copyright (c) 2014 - 2016 + F. Georget Princeton University + All rights reserved. + + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + 3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from this + software without specific prior written permission. + +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * + +============================================================================= */ + +//! \file reformulations.inl +//! \brief Reformulations used in the MiCPsolver + +#ifndef SPECMIC_MICPSOLVER_MICPSOLVER_HPP +#include "micpsolver.hpp" +#endif + +#include "ncp_function.hpp" + +namespace specmicp { +namespace micpsolver { + +// ========================================== // +// // +// Penalized Fisher-Burmeister reformulation // +// // +// ========================================== // + +template +template +struct MiCPSolver::Reformulation +{ + static void reformulate_residuals( + MiCPSolver* const parent, + const Vector &x, + const Vector &r, + Vector &r_phi + ) + { + const auto neq = parent->get_neq(); + const auto neq_free = parent->get_neq_free(); + const auto t_cck = parent->get_options().penalization_factor; + // reformulation with copy from r to r_phi + r_phi.resizeLike(r); + r_phi.block(0, 0, neq_free, 1) = r.block(0, 0, neq_free, 1); + for (index_t i = neq_free; i* const parent, + const Vector& x, + Vector& r + ) + { + const auto neq = parent->get_neq(); + const auto neq_free = parent->get_neq_free(); + const auto t_cck = parent->get_options().penalization_factor; + for (index_t i = neq_free; i* const parent, + const Vector& x) { + + const auto neq = parent->get_neq(); + const auto neq_free = parent->get_neq_free(); + const auto t_cck = parent->get_options().penalization_factor; + + auto& r = parent->get_residuals(); + auto& jacobian = parent->get_jacobian(); + + // set the z vector : contains 1 for degenerate points + Eigen::VectorXd z(Eigen::VectorXd::Zero(neq)); + for (index_t i=neq_free; i 0) and (x(i) >0)) + { + c -= (1-t_cck)*r(i); + d -= (1-t_cck)*x(i); + } + jacobian.row(i) *= d; + jacobian(i, i) += c; + } + } + } +}; + + + + +// ========================================== // +// // +// Box Constrained VI reformulation // +// // +// ========================================== // + + +template +template +struct MiCPSolver::Reformulation +{ + static void reformulate_residuals( + MiCPSolver* const parent, + const Vector &x, + const Vector &r, + Vector &r_phi + ) + { + const auto neq = parent->get_neq(); + const auto neq_free = parent->get_neq_free(); + const auto t_cck = parent->get_options().penalization_factor; + + scalar_t upper_bound; + // reformulation with copy from r to r_phi + r_phi.resizeLike(r); + r_phi.block(0, 0, neq_free, 1) = r.block(0, 0, neq_free, 1); + for (index_t i = neq_free; iget_program()->is_box_vi(i, upper_bound)) + { + r_phi(i) = penalized_fisher_burmeister( + x(i), r(i), t_cck); + } + else + { + if (upper_bound == 0.0) + r_phi(i) = 0; + else + r_phi(i) = box_constrained_vi_b_function( + x(i), r(i), 0.0, upper_bound); + } + } + } + + + static void reformulate_residuals_inplace( + MiCPSolver* const parent, + const Vector& x, + Vector& r + ) + { + const auto neq = parent->get_neq(); + const auto neq_free = parent->get_neq_free(); + const auto t_cck = parent->get_options().penalization_factor; + + scalar_t upper_bound; + for (index_t i = neq_free; iget_program()->is_box_vi(i, upper_bound)) + { + r(i) = penalized_fisher_burmeister(x(i), r(i), t_cck); + } + else + { + if (upper_bound == 0.0) + r(i) = 0.0; + else + r(i) = box_constrained_vi_b_function( + x(i), r(i), 0.0, upper_bound); + } + } + } + + + static void reformulate_jacobian( + MiCPSolver* const parent, + const Vector& x + ) + { + + const auto neq = parent->get_neq(); + const auto neq_free = parent->get_neq_free(); + const auto t_cck = parent->get_options().penalization_factor; + + scalar_t upper_bound; + Vector& r = parent->get_residuals(); + Matrix& jacobian = parent->get_jacobian(); + + Eigen::VectorXd z(Eigen::VectorXd::Zero(neq)); + for (index_t i=neq_free; iget_program()->is_box_vi(i, upper_bound) ) + { + is_degenerate = (x(i) == upper_bound and r(i) == 0.0); + } + if (is_degenerate) + z(i) = 1.0; + } + + for (index_t i = neq_free; iget_program()->is_box_vi(i, upper_bound)) + { + derivative_box_constrained_vi_b_function( + i, x(i), r(i), upper_bound, z, jacobian); + } + else + { + deritative_cck(i, x(i), r(i), t_cck, z, jacobian); + } + } + } + + // Box VI jacobian reformulation + static void derivative_box_constrained_vi_b_function( + index_t i, + scalar_t x, + scalar_t r, + scalar_t v, + const Vector& z, + Matrix& jacobian + ) + { + bool is_degenerate = false; + + const scalar_t u = 0.0; + scalar_t Da = 0.0; + scalar_t Db = 0.0; + + if (v == 0.0) + { + jacobian.row(i).setZero(); + jacobian(i,i) = 1; + } + + + if (x <= u and r >= 0) + { + if (x == u and r ==0) {is_degenerate = true;} + Da = 1; + Db = 0; + } + else if (x >= v and r <= 0) + { + if (x == v and r ==0) {is_degenerate = true;} + Da = 1; + Db = 0; + } + else if (x > u and x <= v and r >= 0) + { + if (x == v and r == 0) {is_degenerate = true;} + const scalar_t hsqm = sqrt_square_sum((x-u), r); + Da = 1 - (x - u)/hsqm; + Db = 1 - r / hsqm; + } + else if ( x >= u and r < v and r <= 0) + { + if (x == u and r == 0) {is_degenerate = true;} + const scalar_t hsqp = sqrt_square_sum((x-v), r); + Da = 1 + (x - v)/hsqp; + Db = 1 + r / hsqp; + } + else if ( (x > v and r > 0) + or (x < u and r < 0)) + { + const scalar_t hsqm = sqrt_square_sum((x-u), r); + const scalar_t hsqp = sqrt_square_sum((x-v), r); + Da = 1 - (x - u)/hsqm + (x - v)/hsqp; + Db = - r/hsqm + r/hsqp; + } + + if (is_degenerate) { + if (x==u) + { + const scalar_t gpdotz = jacobian.row(i).dot(z); + const scalar_t s = sqrt_square_sum(z(i), gpdotz); + Da = (z(i)/s - 1); + Db = (gpdotz/s -1); + + } else + { + const scalar_t gpdotz = jacobian.row(i).dot(z); + const scalar_t s = sqrt_square_sum(z(i), gpdotz); + Da = (z(i)/s - 1); + Db = (gpdotz/s -1); + } + + } + + // update jacobian + jacobian.row(i) *= Db; + jacobian(i, i) += Da; + + + return; + } + + // CCK jacobian reformulation + static void deritative_cck( + index_t i, + scalar_t x, + scalar_t r, + scalar_t t, + const Vector& z, + Matrix& jacobian + ) + { + + scalar_t Da; + scalar_t Db; + + if (z(i) != 0) + { + const scalar_t gpdotz = jacobian.row(i).dot(z); + const scalar_t s = sqrt_square_sum(z(i), gpdotz); + Da = t*(z(i)/s - 1); + Db = t*(gpdotz/s -1); + } + else + { + const scalar_t s = sqrt_square_sum(x, r); + Da = t*(x/s - 1); + Db = t*(r/s - 1); + if ((t <1) and (r > 0) and (x >0)) + { + Da -= (1-t)*r; + Db -= (1-t)*x; + } + } + + jacobian.row(i) *= Db; + jacobian(i, i) += Da; + } + +}; + + + + + + + + +} // end namespace micpsolver +} // end namespace specmicp diff --git a/tests/specmicp_common/micpsolver.cpp b/tests/specmicp_common/micpsolver.cpp index 565429e..21c9d3a 100644 --- a/tests/specmicp_common/micpsolver.cpp +++ b/tests/specmicp_common/micpsolver.cpp @@ -1,498 +1,497 @@ /*------------------------------------------------------- - Module : tests/micpsolver - File : test_micpsolver.hpp - Author : Fabien Georget Copyright (c) 2014, Fabien Georget , Princeton University All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. * Neither the name of the Princeton University nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ---------------------------------------------------------*/ #include "catch.hpp" #include "specmicp_common/micpsolver/micpsolver.hpp" #include "specmicp_common/micpsolver/micpprog.hpp" #include "specmicp_common/log.hpp" #include "specmicp_common/micpsolver/boxviprog.hpp" -#include "specmicp_common/micpsolver/boxvisolver.hpp" using namespace specmicp; using namespace specmicp::micpsolver; #include "specmicp_common/micpsolver/ncp_function.hpp" using specmicp::micpsolver::fisher_burmeister; using specmicp::micpsolver::penalized_fisher_burmeister; TEST_CASE("NCP functions") { SECTION("fisher Burmeister") { CHECK(fisher_burmeister(0.0, 0.0) == 0.0); CHECK(fisher_burmeister(0.0, 1.0) == 0.0); CHECK(fisher_burmeister(0.0, 3.4) == 0.0); CHECK(std::abs(fisher_burmeister(1.0, 1.0)-std::sqrt(2)+2) == Approx(0.0)); CHECK(std::abs(fisher_burmeister(2.0, 1.0)-std::sqrt(5)+3) == Approx(0.0)); } SECTION("Penalized Fischer Burmeister") { CHECK(penalized_fisher_burmeister(0.0, 0.0, 0.5) == 0.0); CHECK(penalized_fisher_burmeister(0.0, 1.0, 0.3) == 0.0); CHECK(penalized_fisher_burmeister(0.0, 3.4, 0.5) == 0.0); CHECK(penalized_fisher_burmeister(8.4, 0.0, 0.2) == 0.0); CHECK(std::abs(fisher_burmeister(1.0, 1.0)-penalized_fisher_burmeister(1.0, 1.0, 1.0)) == Approx(0.0)); CHECK(std::abs(penalized_fisher_burmeister(2.0, 1.0, 0.5)-0.5*(std::sqrt(5)-3)+0.5*2.0) == Approx(0.0)); } } class TestLinearProgram: public MiCPProg { public: //! Return the number of variables int total_variables() {return 3;} int nb_free_variables() {return 2;} int nb_complementarity_variables() {return 1;} //! Return the residual (R(X)) void get_residuals(const Vector& x, Vector& residual) { assert(residual.rows() == 3); residual << 1 + x(0) + x(2), 1 + 1*x(1) + x(2), -x(0) - x(1) +x(2); } //! Return the jacobian (J(x)) void get_jacobian(const Vector& x, Matrix& jacobian) { assert(jacobian.cols() == 3); assert(jacobian.rows() == 3); jacobian << 1, 0, 1, 0, 1, 1, -1, -1, 1; } }; class TestNonLinearProgram: public MiCPProg { public: //! Return the number of variables int total_variables() {return 3;} int nb_free_variables() {return 2;} int nb_complementarity_variables() {return 1;} //! Return the residual (R(X)) void get_residuals(const Vector& x, Vector& residual) { assert(residual.rows() == 3); residual << -1 + x(0)*x(0) + x(2), 1 + 1*x(1) + x(2)*x(2), -x(0) - x(1) +x(2); } //! Return the jacobian (J(x)) void get_jacobian(const Vector& x, Matrix& jacobian) { assert(jacobian.cols() == 3); assert(jacobian.rows() == 3); jacobian << 2*x(0), 0, 1, 0, 1, 2*x(2), -1, -1, 1; } }; /* * S. P. Dirkse and M. C. Ferris. * MCPLIB: a collection of nonlinear mixed complementarity problems. * Optimization Methods and Software, 5(4):319-345, 1995. * */ class TestKojimaProgram: public MiCPProg { public: //! Return the number of variables int total_variables() {return 4;} int nb_free_variables() {return 0;} int nb_complementarity_variables() {return 4;} //! Return the residual (R(X)) void get_residuals(const Vector& x, Vector& residual) { assert(residual.rows() == 4); residual << 3*x(0)*x(0)+2*x(0)*x(1)+2*x(1)*x(1)+x(2)+3*x(3)-6, 2*x(0)*x(0)+x(1)*x(1)+x(0)+10*x(2) +2*x(3)-2, 3*x(0)+x(0)*x(1)+2*x(1)*x(1)+2*x(2)+9*x(3)-9, x(0)*x(0)+3*x(1)*x(1)+2*x(2)+3*x(3)-3 ; } //! Return the jacobian (J(x)) void get_jacobian(Vector& x, Matrix& jacobian) { assert(jacobian.cols() == 4); assert(jacobian.rows() == 4); const int neq = total_variables(); Vector res(total_variables()); Vector perturbed_res(total_variables()); get_residuals(x, res); for (int j=0; j ptrprog = std::make_shared(); MiCPSolver solver(ptrprog); Eigen::VectorXd x(3); x << -1.5, -2 , 0; MiCPSolverReturnCode ret = solver.solve(x); CHECK(ret == MiCPSolverReturnCode::ResidualMinimized); CHECK(std::abs(x(0)+1) == Approx(0.0).epsilon(1e-8)); CHECK(std::abs(x(1)+1) == Approx(0.0).epsilon(1e-8)); CHECK(std::abs(x(2)) == Approx(0.0).epsilon(1e-8)); } SECTION("linear program 10") { std::shared_ptr ptrprog = std::make_shared(); MiCPSolver solver(ptrprog); Eigen::VectorXd x(3); x << -1.5, -2 , 10; MiCPSolverReturnCode ret = solver.solve(x); CHECK(ret == MiCPSolverReturnCode::ResidualMinimized); CHECK(std::abs(x(0)+1) == Approx(0.0).epsilon(1e-8)); CHECK(std::abs(x(1)+1) == Approx(0.0).epsilon(1e-8)); CHECK(std::abs(x(2)) == Approx(0.0).epsilon(1e-8)); } SECTION("non linear program 0") { std::shared_ptr ptrprog = std::make_shared(); MiCPSolver solver(ptrprog); Eigen::VectorXd x(3); x << -1.5, -2 , 0; MiCPSolverReturnCode ret = solver.solve(x); CHECK(ret == MiCPSolverReturnCode::ResidualMinimized); CHECK(std::abs(x(0)+1) == Approx(0.0).epsilon(1e-8)); CHECK(std::abs(x(1)+1) == Approx(0.0).epsilon(1e-8)); CHECK(std::abs(x(2)) == Approx(0.0).epsilon(1e-8)); } SECTION("non linear program 5") { std::shared_ptr ptrprog = std::make_shared(); MiCPSolver solver(ptrprog); Eigen::VectorXd x(3); x << -1.5, -2 , 5; MiCPSolverReturnCode ret = solver.solve(x); CHECK(ret == MiCPSolverReturnCode::ResidualMinimized); CHECK(std::abs(x(0)+1) == Approx(0.0).epsilon(1e-8)); CHECK(std::abs(x(1)+1) == Approx(0.0).epsilon(1e-8)); CHECK(std::abs(x(2)) == Approx(0.0).epsilon(1e-8)); } SECTION("non linear program : Kojima") { std::shared_ptr ptrprog = std::make_shared(); MiCPSolver solver(ptrprog); Eigen::VectorXd x(4); x << 0.9, 0.1 , 2.9, 0.1; MiCPSolverReturnCode ret = solver.solve(x); CHECK(ret == MiCPSolverReturnCode::ResidualMinimized); CHECK(std::abs(x(0)-1) == Approx(0.0).epsilon(1e-8)); CHECK(std::abs(x(1)) == Approx(0.0).epsilon(1e-8)); CHECK(std::abs(x(2)-3) == Approx(0.0).epsilon(1e-8)); CHECK(std::abs(x(3)) == Approx(0.0).epsilon(1e-8)); } // void test_kojima_program_min() // { // std::shared_ptr ptrprog = std::make_shared(); // MiCPSolver solver(ptrprog); // Eigen::VectorXd x(4); // x << 0.9, 0.1 , 2.9, 0.1; // MiCPSolverReturnCode ret = solver.solve(x); // std::cout << x << std::endl; // TS_ASSERT_EQUALS(ret, MiCPSolverReturnCode::ResidualMinimized); // //TS_ASSERT_LESS_THAN_EQUALS(std::abs(x(0)+1), 1e-8); // //TS_ASSERT_LESS_THAN_EQUALS(std::abs(x(1)+1), 1e-8); // //TS_ASSERT_LESS_THAN_EQUALS(std::abs(x(2)), 1e-8); // } } // Box constrained VI problem class Test1LinearVI: public BoxVIProg { public: //! Return the number of variables int total_variables() {return 3;} int nb_free_variables() {return 1;} int nb_complementarity_variables() {return 2;} //! Return the residual (R(X)) void get_residuals(const Vector& x, Vector& residual) { assert(residual.rows() == 3); residual << x(0) - 3.0*x(1) - 2.0*x(2) - 2.0, 3.0*x(0) - 3.0*x(1) - x(2) + 2.0, -x(0) + 2.0*x(1) + 3.0*x(2) + 1.0 ; } //! Return the jacobian (J(x)) void get_jacobian(const Vector& x, Matrix& jacobian) { assert(jacobian.cols() == 3); assert(jacobian.rows() == 3); jacobian << 1.0, -3.0, -2.0, 3.0, -3.0, -1.0, -1.0, +2.0, +3.0; } bool is_box_vi(index_t ideq, scalar_t& upper_bound) { if (ideq == 2) { upper_bound = 10.0; return true; } else { return false; } } }; class Test2LinearVI: public BoxVIProg { public: //! Return the number of variables int total_variables() {return 3;} int nb_free_variables() {return 1;} int nb_complementarity_variables() {return 2;} //! Return the residual (R(X)) void get_residuals(const Vector& x, Vector& residual) { assert(residual.rows() == 3); residual << x(0) + 2.0*x(1) + x(2) - 5.0, 3.0*x(0) + x(1) - 3.0*x(2) - 3.0, x(0) + 2.0*x(1) + x(2) + 5.0; ; } //! Return the jacobian (J(x)) void get_jacobian(const Vector& x, Matrix& jacobian) { assert(jacobian.cols() == 3); assert(jacobian.rows() == 3); jacobian << 1.0, +2.0, 1.0, 3.0, +1.0, -3.0, 1.0, +2.0, +1.0; } bool is_box_vi(index_t ideq, scalar_t& upper_bound) { if (ideq == 2) { upper_bound = 10.0; return true; } else { return false; } } }; class Test3LinearVI: public BoxVIProg { public: //! Return the number of variables int total_variables() {return 3;} int nb_free_variables() {return 1;} int nb_complementarity_variables() {return 2;} //! Return the residual (R(X)) void get_residuals(const Vector& x, Vector& residual) { assert(residual.rows() == 3); residual << 3.0*x(0) + 2.0*x(1) + x(2) - 5.0, x(0) + 3.0*x(1) - 2.0*x(2) + 6.0, 2.0*x(0) - 3.0*x(1) + x(2) - 5.0; ; } //! Return the jacobian (J(x)) void get_jacobian(const Vector& x, Matrix& jacobian) { assert(jacobian.cols() == 3); assert(jacobian.rows() == 3); jacobian << 3.0, +2.0, 1.0, 1.0, +3.0, -2.0, 2.0, -3.0, +1.0; } bool is_box_vi(index_t ideq, scalar_t& upper_bound) { if (ideq == 2) { upper_bound = 4.0; return true; } else { return false; } } }; class Test4NLVI: public BoxVIProg { public: //! Return the number of variables int total_variables() {return 3;} int nb_free_variables() {return 1;} int nb_complementarity_variables() {return 2;} //! Return the residual (R(X)) void get_residuals(const Vector& x, Vector& residual) { assert(residual.rows() == 3); residual << -x(0)*x(2)+3*x(1)*x(1)+x(2)+2, 2*x(0)*x(0)+3*x(0)*x(1)*x(1)+2*x(0)*x(2), -4+x(0)*x(0)+x(1)*x(1)-3*x(2)*x(2)+3*x(0)*x(2); } //! Return the jacobian (J(x)) void get_jacobian(const Vector& x, Matrix& jacobian) { assert(jacobian.cols() == 3); assert(jacobian.rows() == 3); jacobian << -x(2), +6.0*x(1), 1.0-x(0), 4*x(0)+3*x(1)*x(1)+2*x(2), 6*x(0)*x(1), 2.0*x(0), 2*x(0)+3*x(2), 2*x(1), -6.0*x(2)+3*x(2); } bool is_box_vi(index_t ideq, scalar_t& upper_bound) { if (ideq == 2) { upper_bound = 10.0; return true; } else { return false; } } }; TEST_CASE("BOX VI solver") { SECTION("test 1 VI") { std::shared_ptr ptrprog = std::make_shared(); - BoxVISolver solver(ptrprog); + MiCPSolver solver(ptrprog); Eigen::VectorXd x(3); x << 3, 2, 0; MiCPSolverReturnCode ret = solver.solve(x); CHECK(ret == MiCPSolverReturnCode::ResidualMinimized); CHECK(x(0) == Approx(4.0).epsilon(1e-8)); CHECK(x(1) == Approx(0.0).epsilon(1e-8)); CHECK(x(2) == Approx(1.0).epsilon(1e-8)); } SECTION("test 2 VI") { std::shared_ptr ptrprog = std::make_shared(); - BoxVISolver solver(ptrprog); + MiCPSolver solver(ptrprog); Eigen::VectorXd x(3); x << 3, 2, 2; MiCPSolverReturnCode ret = solver.solve(x); CHECK(ret == MiCPSolverReturnCode::ResidualMinimized); CHECK(x(0) == Approx(1.0/5.0).epsilon(1e-8)); CHECK(x(1) == Approx(12.0/5.0).epsilon(1e-8)); CHECK(x(2) == Approx(0.0).epsilon(1e-8)); } SECTION("test 3 VI") { std::shared_ptr ptrprog = std::make_shared(); - BoxVISolver solver(ptrprog); + MiCPSolver solver(ptrprog); Eigen::VectorXd x(3); x << 0.0, 0.0, 0.0; MiCPSolverReturnCode ret = solver.solve(x); CHECK(ret == MiCPSolverReturnCode::ResidualMinimized); CHECK(x(0) == Approx(-1.0/7.0).epsilon(1e-8)); CHECK(x(1) == Approx(5.0/7.0).epsilon(1e-8)); CHECK(x(2) == Approx(4.0).epsilon(1e-8)); } SECTION("test 4 NL VI") { std::shared_ptr ptrprog = std::make_shared(); - BoxVISolver solver(ptrprog); + MiCPSolver solver(ptrprog); Eigen::VectorXd x(3); x << 1.0, 0.0, 2.0; MiCPSolverReturnCode ret = solver.solve(x); CHECK(ret == MiCPSolverReturnCode::ResidualMinimized); CHECK(x(0) == Approx(2.0).epsilon(1e-8)); CHECK(x(1) == Approx(0.0).epsilon(1e-8)); CHECK(x(2) == Approx(2.0).epsilon(1e-8)); } }