Homec4science
Diffusion SpecMiCP / ReactMiCP (yaml/safe) Browse

specmicp/src/micpsolver/micpsolverold.inl
54f50e62ff4byaml/safe

Last Change
Raw File

micpsolverold.inl
View Options

/*-------------------------------------------------------
- Module : micpsolver
- File : micpsolver.inl
- Author : Fabien Georget
Copyright (c) 2014, Fabien Georget, Princeton University
---------------------------------------------------------*/
#include "micpsolverold.hpp" // for syntaxic coloration...
#include "estimate_cond_number.hpp"
#include "utils/log.hpp"
#include <iostream>
//! \file micpsolver.inl implementation of the MiCP solver
namespace specmicp {
namespace micpsolver {
// Main algorithm
// ##############
template <class Program, NCPfunction ncp_t>
MiCPSolverReturnCode MiCPSolverOLD<Program, ncp_t>::solve(Eigen::VectorXd &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;
m_program->hook_start_iteration(x, m_phi_residuals.norm());
setup_residuals(x);
SPAM << "Residuals : \n ----- \n" << m_phi_residuals << "\n ----- \n";
retcode = check_convergence(cnt, update, x);
get_perf().return_code = retcode;
if (retcode != MiCPSolverReturnCode::NotConvergedYet) break;
++cnt;
m_max_taken = false;
setup_jacobian(x);
if(get_options().use_scaling)
search_direction_calculation(update);
else
search_direction_calculation_no_scaling(update);
reformulate_result<ncp_t>(get_neq(), get_neq_free(),
x, m_residuals,
m_grad_phi, update);
int termcode = linesearch(update, x);
DEBUG << "Return LineSearch : " << termcode;
projection(x);
get_perf().nb_iterations = cnt;
}
return retcode;
}
template <class Program, NCPfunction ncp_t>
MiCPSolverReturnCode MiCPSolverOLD<Program, ncp_t>::check_convergence(int nb_iterations,
Eigen::VectorXd& update,
Eigen::VectorXd& solution)
{
MiCPSolverReturnCode termcode = MiCPSolverReturnCode::NotConvergedYet;
const double norm_residuals = m_phi_residuals.lpNorm<Eigen::Infinity>();
if (norm_residuals < get_options().fvectol)
{
termcode = MiCPSolverReturnCode::ResidualMinimized;
}
else if (nb_iterations >0 and norm_update<Eigen::Infinity>(update, solution) < get_options().steptol)
{
if (norm_residuals > get_options().threshold_stationary_point)
{
ERROR << "Stationary point detected !";
termcode = MiCPSolverReturnCode::StationaryPoint;
}
WARNING << "Error is minimized - may indicate a stationnary point";
termcode = MiCPSolverReturnCode::ErrorMinimized;
}
else if (nb_iterations > get_options().max_iter)
{
ERROR << "Maximum number of iteration reached (" << get_options().max_iter << ")";
termcode = MiCPSolverReturnCode::MaxIterations;
}
else if (m_max_taken)
{
++m_consec_max;
if (m_consec_max == get_options().maxiter_maxstep) {
ERROR << "Divergence detected - Maximum step length taken two many times";
termcode = MiCPSolverReturnCode::MaxStepTakenTooManyTimes;
}
}
else
{
m_consec_max = 0;
}
return termcode;
}
template <class Program, NCPfunction ncp_t>
MiCPSolverReturnCode MiCPSolverOLD<Program, ncp_t>::search_direction_calculation(Eigen::VectorXd& update)
{
Eigen::VectorXd rscaler(Eigen::VectorXd::Ones(m_jacobian.cols()));
Eigen::VectorXd cscaler(Eigen::VectorXd::Ones(m_jacobian.rows()));
scaling_jacobian(m_jacobian, m_phi_residuals, rscaler, cscaler);
m_jacobian = rscaler.asDiagonal() * (m_jacobian) * cscaler.asDiagonal();
Eigen::ColPivHouseholderQR<Eigen::MatrixXd> solver;
m_gradient_step_taken = false;
int m;
for (m=0; m<get_options().max_factorization_step; ++m)
{
const double lambda = get_options().factor_gradient_search_direction;
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;
}
double 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);
double descent_cond = m_grad_phi.dot(update);
double norm_grad = m_grad_phi.norm();
double 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, NCPfunction ncp_t>
MiCPSolverReturnCode MiCPSolverOLD<Program, ncp_t>::search_direction_calculation_no_scaling(Eigen::VectorXd& update)
{
DEBUG << "Solving linear system";
Eigen::ColPivHouseholderQR<Eigen::MatrixXd> solver;
m_gradient_step_taken = false;
int m;
for (m=0; m<get_options().max_factorization_step; ++m)
{
const double lambda = get_options().factor_gradient_search_direction;
solver.compute(m_jacobian);
get_perf().nb_factorization += 1;
if (solver.info() != Eigen::Success or not solver.isInvertible()) continue;
double cond = estimate_condition_number(solver.matrixR().triangularView<Eigen::Upper>());
if (cond > get_options().condition_limit)
{
continue;
}
update = solver.solve(-(m_phi_residuals + m*lambda*m_grad_phi));
double descent_cond = m_grad_phi.dot(update);
double norm_grad = m_grad_phi.norm();
double 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 += lambda*Eigen::MatrixXd::Identity(m_jacobian.rows(),m_jacobian.cols());
}
DEBUG << "Gradient step : m = " << m;
if (m ==4) {
INFO << "Full gradient step taken !";
update = -m_grad_phi;
}
return MiCPSolverReturnCode::NotConvergedYet;
}
template <class Program, NCPfunction ncp_t>
void MiCPSolverOLD<Program, ncp_t>::crashing(Eigen::VectorXd &x)
{
DEBUG << "Crashing ";
const double beta = 0.5;
const double sigma = 1e-5;
int cnt = 0;
while (cnt < 10)
{
setup_residuals(x);
setup_jacobian(x);
m_grad_phi = m_jacobian.transpose()*m_phi_residuals;
Eigen::VectorXd 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;
}
Eigen::VectorXd new_res(get_neq());
compute_residuals(x, new_res);
reformulate_residuals_inplace(x, new_res);
double 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, NCPfunction ncp_t>
void MiCPSolverOLD<Program, ncp_t>::reformulate_residuals(const Eigen::VectorXd& x,
const Eigen::VectorXd& r,
Eigen::VectorXd& r_phi)
{
r_phi.resizeLike(r);
r_phi.block(0, 0, get_neq_free(), 1) = r.block(0, 0, get_neq_free(), 1);
for (int i = get_neq_free(); i<get_neq(); ++i)
{
r_phi(i) = phi_lower_bounded(x(i), r(i), 0);
}
}
template <class Program, NCPfunction ncp_t>
void MiCPSolverOLD<Program, ncp_t>::reformulate_residuals_inplace(const Eigen::VectorXd& x,
Eigen::VectorXd& r)
{
for (int i = get_neq_free(); i<get_neq(); ++i)
{
r(i) = phi_lower_bounded(x(i), r(i), 0);
}
}
// ref : Munson et al. (2001)
template <class Program, NCPfunction ncp_t>
void MiCPSolverOLD<Program, ncp_t>:: scaling_jacobian(
const Eigen::MatrixXd& jacobian,
const Eigen::VectorXd& r_phi,
Eigen::VectorXd& rscaler,
Eigen::VectorXd& cscaler)
{
for (int i=0; i<jacobian.cols(); ++i)
{
const double sumhsq = jacobian.row(i).array().square().sum();
double s = std::sqrt(r_phi(i)*r_phi(i) + sumhsq);
rscaler(i) = 1.0/std::max(s, 1e-10);
}
for (int i=0; i<jacobian.cols(); ++i)
{
const double sumhsq = (rscaler.asDiagonal()*jacobian).col(i).array().square().sum();
double s = std::sqrt(sumhsq);
cscaler(i) = 1.0/std::max(s, 1e-10);
}
}
template <class Program, NCPfunction ncp_t>
int MiCPSolverOLD<Program, ncp_t>::linesearch(Eigen::VectorXd& p, Eigen::VectorXd& x)
{
// Reference Algo A6.3.1 : Dennis and Schnabel (1983)
DEBUG << "Linesearch";
Eigen::VectorXd xp(get_neq());
Eigen::VectorXd new_res(get_neq());
double fcp;
m_max_taken = false;
int retcode = 2;
const double alpha = 1e-6;
double newtlen = 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 = m_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;
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;
}
//std::cout << "Merit value : " << merit_value << std::endl;
double mmax = merit_value;
if (m_max_merits.size() > 0)
{
mmax = m_max_merits[m_max_merits.size()-1];
}
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;
}
//std::cout << "Merit value used : " << merit_value << std::endl;
double fc = merit_value;
double fcp_prev;
int cnt = 0;
do
{
fcp = 0.5*new_res.squaredNorm();
//std::cout << "fcp : " << fcp << "\n fc+alin : " << fc+alpha*lambda*init_slope << " # fc : " << fc << std::endl;
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)) {
m_max_taken = true;
}
break;
}
else if (lambda < minlambda)
{
retcode = 1;
break;
}
else
{
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;
}
}
xp = x + lambda*p;
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;
}
// Projection of the variables onto the feasible set
template <class Program, NCPfunction ncp_t>
void MiCPSolverOLD<Program, ncp_t>::projection(Eigen::VectorXd &x)
{
for (int i=0; i<m_program->nb_complementarity_variables(); ++i)
{
if (x(i+m_program->nb_free_variables()) < get_options().projection_min_variable)
{
x(i+m_program->nb_free_variables()) = 0;
}
}
}
template <class Program, NCPfunction ncp_t>
double MiCPSolverOLD<Program, ncp_t>::is_step_too_long(Eigen::VectorXd& update)
{
double steplength = update.norm();
if (steplength > get_options().maxstep)
{
m_max_taken = true;
update = get_options().maxstep / steplength * update;
steplength = get_options().maxstep;
}
return steplength;
}
// ================================================= //
// //
// NCP functions and reformulation //
// //
// ================================================= //
template <>
inline double ncp_function<NCPfunction::penalizedFB>(double a, double b, double t)
{
return penalized_fisher_burmeister(a, b, t);
}
template <>
inline double ncp_function<NCPfunction::min>(double a, double b, double _)
{
return std::min(a, b);
}
template <>
inline void reformulate_jacobian_helper<NCPfunction::penalizedFB>(
int neq,
int neq_free,
const Eigen::VectorXd& x,
const Eigen::VectorXd& r,
Eigen::MatrixXd& jacobian,
Eigen::VectorXd& _,
double t
)
{
// set the z vector : contains 1 for degenerate points
Eigen::VectorXd z(Eigen::VectorXd::Zero(neq));
for (int i=neq_free; i<neq; ++i)
{
if (x(i) == 0 and r(i) == 0)
z(i) = 1.0;
}
// modify the jacobian
const double lambda = t;
for (int i=neq_free; i<neq; ++i)
{
Eigen::VectorXd grad_fi = jacobian.block(i, 0, 1, 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(r(i));
s = s * std::sqrt(x(i)*x(i)/(s*s) + r(i)*r(i)/(s*s));
double c = lambda*(x(i)/s - 1);
double d = lambda*(r(i)/s - 1);
if ((lambda <1) and (r(i) > 0) and (x(i) >0))
{
c -= (1-lambda)*r(i);
d -= (1-lambda)*x(i);
}
grad_fi = d*grad_fi;
grad_fi(i) += c;
}
jacobian.block(i, 0, 1, neq) = grad_fi.transpose();
}
}
template <>
inline void reformulate_jacobian_helper<NCPfunction::min>(
int neq,
int neq_free,
const Eigen::VectorXd& x,
const Eigen::VectorXd& r,
Eigen::MatrixXd& jacobian,
Eigen::VectorXd& r_phi,
double _
)
{
std::vector<int> to_keep;
to_keep.reserve(10);
Eigen::VectorXd to_remove(neq-neq_free);
for (int i=neq_free; i<neq; ++i)
{
if (x(i) >= r(i))
{
to_remove(i-neq_free) = 0;
to_keep.push_back(i);
}
else
to_remove(i-neq_free) = x(i);
}
r_phi.block(0, 0, neq_free, 1) -= jacobian.block(0, neq_free, neq_free, neq-neq_free)*to_remove;
int new_i = neq_free;
for (auto it=to_keep.begin(); it!=to_keep.end(); ++it)
{
//r_phi.block(0, 0, neq_free, 1) += x(*it)*jacobian.block(0, *it, neq_free, 1);
jacobian.block(new_i, 0, 1, neq_free) = jacobian.block(*it, 0, 1, neq_free); // the bottom right corner is 0
jacobian.block(0, new_i, neq_free, 1) = jacobian.block(0, *it, neq_free, 1);
r_phi(new_i) = r_phi(*it);
++new_i;
}
r_phi.conservativeResize(new_i);
jacobian.conservativeResize(new_i, new_i);
DEBUG << jacobian;
}
template <>
inline void reformulate_result<NCPfunction::penalizedFB>(
int neq,
int neq_free,
Eigen::VectorXd& x,
const Eigen::VectorXd& orig_r,
Eigen::VectorXd& grad_phi,
Eigen::VectorXd& update
)
{}
template <>
inline void reformulate_result<NCPfunction::min>(
int neq,
int neq_free,
Eigen::VectorXd& x,
const Eigen::VectorXd& orig_r,
Eigen::VectorXd& grad_phi,
Eigen::VectorXd& update
)
{
//std::cout << " Update \n ------- \n " << update << std::endl;
int tot_to_keep = 0;
for (int i=neq_free; i<neq; ++i)
{
if (x(i) >= orig_r(i))
++tot_to_keep;
}
//std::cout << " update \n ------ \n" << update.block(neq_free, 0, tot_to_keep, 1) << std::endl;
update.conservativeResize(neq);
grad_phi.conservativeResize(neq);
int kept_i = 1;
for (int i=neq-1; i>=neq_free; --i)
{ // we go backwards to avoid extra copies
//std::cout << i << " # " << x(i) << " - " << orig_r(i) << std::endl;
if (x(i) >= orig_r(i))
{
//std::cout << i << std::endl;
update(i) = update(neq_free+(tot_to_keep-kept_i));
//std::cout << update(i) << std::endl;
grad_phi(i) = grad_phi(neq_free+(tot_to_keep-kept_i));
++kept_i;
}
else
{
//x(i) = 0.0;
//update(i) = 0.0;
update(i) = -x(i);
grad_phi(i) = x(i);
}
}
}
} // end namespace micpsolver
} // end namespace specmicp
c4science · Help