Homec4science
Diffusion SpecMiCP / ReactMiCP (reactmicp/unsaturated) Browse

specmicp/src/micpsolver/micpsolver.inl
dd32bf844e0creactmicp/unsaturated

Last Change
Raw File

micpsolver.inl
View Options

/*-------------------------------------------------------
- Module : micpsolver
- File : micpsolver.inl
- Author : Fabien Georget
Copyright (c) 2014, Fabien Georget <fabieng@princeton.edu>, 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 "micpsolver.hpp" // for syntaxic coloration...
#include "estimate_cond_number.hpp"
#include "utils/log.hpp"
//! \file micpsolver.inl implementation of the MiCP solver
namespace specmicp {
namespace micpsolver {
// Main algorithm
// ##############
template <class program_t>
MiCPSolverReturnCode MiCPSolver<program_t>::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;
SPAM << "Solution : \n" << x;
// (S.0) this is a hook for simultaneous fixed-point iterations
bool may_have_converged = get_program()->hook_start_iteration(x, m_phi_residuals.norm());
// (S.1) Check the convergence
setup_residuals(x);
get_perf().current_residual = m_phi_residuals.norm();
SPAM << "Residuals : \n ----- \n" << m_phi_residuals << "\n ----- \n";
retcode = base::check_convergence(cnt, update, x, m_phi_residuals, may_have_converged);
get_perf().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(x, update);
// (S.3) Linesearch
int termcode = linesearch(update, x);
if (termcode != 0) retcode = MiCPSolverReturnCode::LinesearchFailure;
get_perf().current_update = update.norm();
DEBUG << "Return LineSearch : " << termcode;
base::projection(x); // project x over the positive quadrant
get_perf().nb_iterations = cnt;
}
return retcode;
}
template <class program_t>
MiCPSolverReturnCode MiCPSolver<program_t>::search_direction_calculation(Eigen::VectorXd& update)
{
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();
Eigen::ColPivHouseholderQR<Matrix> solver(m_jacobian); // it needs to be correctly initialized
m_gradient_step_taken = false;
int m;
for (m=0; m<get_options().max_factorization_step; ++m)
{
const scalar_t lambda = get_options().factor_gradient_search_direction;
if (m != 0) solver.compute(m_jacobian);
get_perf().nb_factorization += 1;
if (solver.info() != Eigen::Success or not solver.isInvertible())
{
DEBUG << "Solver.info : " << solver.info() << " - is invertible : " << solver.isInvertible();
ERROR << "System cannot be solved, we try a perturbation";
m_gradient_step_taken = true;
m_jacobian += rscaler.asDiagonal() * (
lambda*Eigen::MatrixXd::Identity(m_jacobian.rows(),m_jacobian.cols())) * cscaler.asDiagonal();
continue;
}
scalar_t cond = estimate_condition_number(solver.matrixR().triangularView<Eigen::Upper>());
if (cond > get_options().condition_limit)
{
m_gradient_step_taken = true;
m_jacobian += rscaler.asDiagonal() * (
lambda*Eigen::MatrixXd::Identity(m_jacobian.rows(),m_jacobian.cols())) * cscaler.asDiagonal();
continue;
}
update = solver.solve(-rscaler.cwiseProduct(m_phi_residuals + m*lambda*m_grad_phi));
update = cscaler.cwiseProduct(update);
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 += rscaler.asDiagonal() * ( lambda*
Eigen::MatrixXd::Identity(m_jacobian.rows(),m_jacobian.cols())
) * cscaler.asDiagonal();
}
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 <class program_t>
MiCPSolverReturnCode MiCPSolver<program_t>::search_direction_calculation_no_scaling(
Vector& x,
Vector& update
)
{
DEBUG << "Solving linear system";
Eigen::ColPivHouseholderQR<Eigen::MatrixXd> solver(m_jacobian); // 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<get_options().max_factorization_step; ++m)
{
const scalar_t lambda = get_options().factor_gradient_search_direction;
if (m != 0) solver.compute(m_jacobian); // we need to recompute it (?)
get_perf().nb_factorization += 1;
DEBUG << "condition of Jacobian : " << solver.info() << " - " << Eigen::Success;
// Check that the decomposition was ok
if (solver.info() != Eigen::Success or not solver.isInvertible())
{
DEBUG << "Solver.info : " << solver.info() << " - is invertible : " << solver.isInvertible();
ERROR << "System cannot be solved, we try a perturbation";
m_gradient_step_taken = true;
m_jacobian += lambda*Eigen::MatrixXd::Identity(m_jacobian.rows(),m_jacobian.cols());
continue;
}
// Estimate the condition number
scalar_t cond = estimate_condition_number(solver.matrixR().triangularView<Eigen::Upper>());
if (cond < get_options().condition_limit) // if ok
{
update = solver.solve(-(m_phi_residuals + m*lambda*m_grad_phi));
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 += lambda*Eigen::MatrixXd::Identity(m_jacobian.rows(),m_jacobian.cols());
}
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 <class program_t>
void MiCPSolver<program_t>::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<maxl)
{
xp = x - std::pow(beta, l)*m_grad_phi;
for (int i=get_neq_free(); i<get_neq(); ++i)
{
if (xp(i) < 0) xp(i) = 0;
}
Vector new_res(get_neq());
base::compute_residuals(x, new_res);
reformulate_residuals_inplace(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_perf().nb_crashing_iterations = cnt;
DEBUG << "Crashing iterations : " << cnt;
m_max_merits.reserve(4);
SPAM << "Solution after crashing \n ------ \n " << x << "\n ----- \n";
}
// Others
// ######
template <class program_t>
void MiCPSolver<program_t>::reformulate_residuals(
const Vector &x,
const Vector &r,
Vector &r_phi
)
{
// 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<get_neq(); ++i)
{
r_phi(i) = penalized_fisher_burmeister(x(i), r(i), get_options().penalization_factor);
}
}
template <class program_t>
void MiCPSolver<program_t>::reformulate_residuals_inplace(
const Vector& x,
Vector& r
)
{
for (index_t i = get_neq_free(); i<get_neq(); ++i)
{
r(i) = penalized_fisher_burmeister(x(i), r(i), get_options().penalization_factor);
}
}
template <class program_t>
int MiCPSolver<program_t>::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_perf().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; i<get_neq(); ++i)
{
rellength = std::max(rellength, std::abs(p(i)));
}
double minlambda = get_options().steptol / rellength;
double lambda = get_program()->max_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_perf().max_taken = true;
}
break;
}
else if (lambda < minlambda)
{
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;
}
template <class program_t>
void MiCPSolver<program_t>::reformulate_jacobian(const Vector &x)
{
base::reformulate_jacobian_cck(x, m_residuals, m_jacobian);
// // see Facchinei and Pang for more details
// // set the z vector : contains 1 for degenerate points
// Eigen::VectorXd z(Eigen::VectorXd::Zero(get_neq()));
// for (int i=get_neq_free(); i<get_neq(); ++i)
// {
// if (x(i) == 0 and m_residuals(i) == 0)
// z(i) = 1.0;
// }
// // modify the jacobian
// const double lambda = get_options().penalization_factor;
// for (int i=get_neq_free(); i<get_neq(); ++i)
// {
// Eigen::VectorXd grad_fi = m_jacobian.block(i, 0, 1, get_neq()).transpose();
// if (z(i) != 0)
// {
// double gpdotz = grad_fi.dot(z);
// double s = std::abs(z(i)) + std::abs(gpdotz);
// s = s * std::sqrt(z(i)*z(i)/(s*s) + gpdotz*gpdotz/(s*s));
// const double c = lambda*(z(i)/s - 1);
// const double d = lambda*(gpdotz/s -1);
// grad_fi = d*grad_fi;
// grad_fi(i) += c;
// }
// else
// {
// double s = std::abs(x(i)) + std::abs( m_residuals(i));
// s = s * std::sqrt(x(i)*x(i)/(s*s) + m_residuals(i)* m_residuals(i)/(s*s));
// double c = lambda*(x(i)/s - 1);
// double d = lambda*( m_residuals(i)/s - 1);
// if ((lambda <1) and ( m_residuals(i) > 0) and (x(i) >0))
// {
// c -= (1-lambda)* m_residuals(i);
// d -= (1-lambda)*x(i);
// }
// grad_fi = d*grad_fi;
// grad_fi(i) += c;
// }
// m_jacobian.block(i, 0, 1, get_neq()) = grad_fi.transpose();
// }
}
} // end namespace micpsolver
} // end namespace specmicp
c4science · Help