Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F77587275
micpsolver.inl
No One
Temporary
Actions
Download File
Edit File
Delete File
View Transforms
Subscribe
Mute Notifications
Award Token
Subscribers
None
File Metadata
Details
File Info
Storage
Attached
Created
Thu, Aug 15, 08:21
Size
20 KB
Mime Type
text/x-c++
Expires
Sat, Aug 17, 08:21 (2 d)
Engine
blob
Format
Raw Data
Handle
19870022
Attached To
rSPECMICP SpecMiCP / ReactMiCP
micpsolver.inl
View Options
#include "micpsolver.hpp" // for syntaxic coloration only
#include "micpsolver/ncp_function.hpp"
#include "utils/log.hpp"
#include <iostream>
#include <unsupported/Eigen/SparseExtra>
namespace specmicp {
namespace reactmicp {
namespace micpsolver {
// ============================= //
// //
// Main algorithm //
// -------------- //
// //
// ============================= //
template <class program_t>
MiCPParabolicSolverReturnCode MiCPParabolicSolver<program_t>::solve_one_timestep(double dt,
ParabolicVariables& x)
{
int cnt = 0;
m_residuals.resize(get_neq());
MiCPParabolicSolverReturnCode retcode = MiCPParabolicSolverReturnCode::NotConvergedYet;
Eigen::VectorXd update(get_neq());
Eigen::VectorXd predictor;
m_program->set_predictor(x, predictor, get_options().alpha, dt);
setup_residuals(x);
m_norm_0 = m_phi_residuals.lpNorm<Eigen::Infinity>();
while (retcode == MiCPParabolicSolverReturnCode::NotConvergedYet)
{
//std::cout << "iteration : " << cnt << std::endl;
DEBUG << "Iteration : " << cnt;
// (S.0) hook
const bool may_have_converged = get_program()->hook_start_iteration(x, m_phi_residuals.norm());
// (S.1) compute the residuals
setup_residuals(x);
//std::cout << m_phi_residuals << std::endl;
// (S.2) check the convergence
retcode = check_convergence(cnt, update, x, m_phi_residuals, may_have_converged);
get_performance().return_code = retcode;
//std::cout << "Retcode : " << (int) retcode << std::endl;
if (retcode != MiCPParabolicSolverReturnCode::NotConvergedYet) break;
++cnt;
// (S.3) compute the jacobian
compute_search_direction(update, x, dt);
// (S.4) linesearch and update
int retcode = linesearch(update, x, predictor, dt);
if (retcode == 1)
{
ERROR << "Linesearch has failed - minimum step allowed taken";
}
//m_program->projection(x);
const int ndf = m_program->get_ndf();
for (int i=0; i<ndf; ++i)
{
for (int node=0; node<5; ++node)
{
std::cout << x.displacement(i+ndf*node) << "\t";
}
std::cout << std::endl;
}
}
get_performance().nb_iterations = cnt;
std::cout << "Nb iterations : " << cnt << std::endl;
return retcode;
}
template <class program_t>
MiCPParabolicSolverReturnCode MiCPParabolicSolver<program_t>::check_convergence(
int nb_iterations,
const Eigen::VectorXd& update,
const ParabolicVariables& solution,
const Eigen::VectorXd& residuals,
bool may_have_converged)
{
MiCPParabolicSolverReturnCode termcode = MiCPParabolicSolverReturnCode::NotConvergedYet;
const double norm_residuals = residuals.lpNorm<Eigen::Infinity>();
WARNING << "Residuals : " << nb_iterations << " - " << norm_residuals/m_norm_0;
if (norm_residuals/m_norm_0 < get_options().residuals_tolerance)
{
if (may_have_converged == true)
termcode = MiCPParabolicSolverReturnCode::ResidualMinimized;
}
else if (nb_iterations/m_norm_0 > 0 and update.lpNorm<Eigen::Infinity>() < get_options().step_tolerance)
{
if (norm_residuals > get_options().threshold_stationary_point)
{
ERROR << "Stationary point detected !";
termcode = MiCPParabolicSolverReturnCode::StationaryPoint;
}
if (may_have_converged == true)
{
WARNING << "Error is minimized - may indicate a stationnary point";
termcode = MiCPParabolicSolverReturnCode::ErrorMinimized;
}
}
else if (nb_iterations > get_options().maximum_iterations)
{
ERROR << "Maximum number of iteration reached (" << get_options().maximum_iterations << ")";
termcode = MiCPParabolicSolverReturnCode::MaxIterations;
}
else if (get_performance().maximum_step_taken)
{
++get_performance().nb_consecutive_maximum_step_taken;
++get_performance().nb_maximum_step_taken;
if (get_performance().nb_consecutive_maximum_step_taken == get_options().maximum_iterations_at_maximum_step) {
ERROR << "Divergence detected - Maximum step length taken two many times";
termcode = MiCPParabolicSolverReturnCode::MaxStepTakenTooManyTimes;
}
}
else
{
get_performance().nb_consecutive_maximum_step_taken = 0;
}
return termcode;
}
// ============================== //
// //
// Residuals and Jacobian //
// ---------------------- //
// //
// ============================== //
// Residuals
// =========
template <class program_t>
void MiCPParabolicSolver<program_t>::setup_residuals(const ParabolicVariables& x)
{
compute_residuals(x, m_residuals);
reformulate_residuals(x);
}
template <class program_t>
void MiCPParabolicSolver<program_t>::compute_residuals(const ParabolicVariables& x,
Eigen::VectorXd& residuals)
{
m_program->copy_cp_variables(x, m_cp_variables);
get_program()->get_residuals(x, residuals);
get_performance().nb_call_residuals += 1;
}
// reformulation
// -------------
template <class program_t>
void MiCPParabolicSolver<program_t>::reformulate_residuals(const ParabolicVariables &x)
{
const std::vector<bool>& is_cp = m_program->micp_index();
m_phi_residuals.resizeLike(m_residuals);
m_phi_residuals.setZero();
for (int i=0; i<m_program->get_neq(); ++i)
{
if (is_cp[i])
{
m_phi_residuals(i) = specmicp::micpsolver::penalized_fisher_burmeister(
m_cp_variables(i),
m_residuals(i),
get_options().penalization_factor
);
}
else
{
m_phi_residuals(i) = m_residuals(i);
}
}
}
// this is mainly used in the linesearch
template <class program_t>
void MiCPParabolicSolver<program_t>::reformulate_residuals_inplace(const ParabolicVariables& x,
Eigen::VectorXd& residual)
{
const std::vector<bool>& is_cp = m_program->micp_index();
for (int i=0; i<m_program->get_neq(); ++i)
{
if (is_cp[i])
{
residual.coeffRef(i) = specmicp::micpsolver::penalized_fisher_burmeister(
m_cp_variables(i),
residual(i),
get_options().penalization_factor
);
}
}
}
// Jacobian
// ========
template <class program_t>
void MiCPParabolicSolver<program_t>::setup_jacobian(
ParabolicVariables& x,
const double dt,
Eigen::SparseMatrix<double, Eigen::RowMajor>& jacobian)
{
compute_jacobian(x, dt, jacobian);
reformulate_jacobian(x, jacobian, dt);
jacobian.makeCompressed();
}
template <class program_t>
void MiCPParabolicSolver<program_t>::compute_jacobian(ParabolicVariables& x,
const double dt,
Eigen::SparseMatrix<double, Eigen::RowMajor> &jacobian)
{
get_program()->get_jacobian(x, m_residuals, jacobian, get_options().alpha*dt);
get_performance().nb_call_jacobian += 1;
}
// Reformulation
// -------------
template <class program_t>
void MiCPParabolicSolver<program_t>::reformulate_jacobian(const ParabolicVariables& x,
Eigen::SparseMatrix<double, Eigen::RowMajor> &jacobian,
double dt)
{
// see Facchinei and Pang for more details
// adapted to sparse matrix in ROW major
// i.e. the outer direction is a row (the algorithm proceed row by row)
// Iterating over the non zero element :
// see Eigen doc http://eigen.tuxfamily.org/dox/group__TutorialSparse.html
// and https://forum.kde.org/viewtopic.php?f=74&t=122606&sid=a5b017a2e6dad7bab6f09e90680275b5
jacobian.makeCompressed(); // should be the case, but just to be sure
//std::cout << "residuals" << std::endl;
//std::cout << m_phi_residuals << std::endl;
//std::cout << "Jacobian " << std::endl;
//std::cout << jacobian.toDense() << std::endl;
const std::vector<bool>& is_cp = m_program->micp_index();
const double lambda = get_options().penalization_factor;
for (int i=0; i<get_neq(); ++i)
{
if (not is_cp[i]) continue;
double c, d;
const double z = 1.0;
if (m_cp_variables(i) == 0.0 and m_residuals(i) == 0.0)
{
std::cout << "is this path taken ?" << std::endl;
ERROR << "This part of the code is not correct (reformulation jacobian with x_i=0=F_i)";
// correct that or we will run into trouble
double gpdotz = 0; // grad(Fi)^t z
// for (Eigen::SparseMatrix<double, Eigen::RowMajor>::InnerIterator it(jacobian,i); it; ++it)
// {
// if (is_cp[it.col()] and m_cp_variables(it.col()) == 0.0 and m_residuals(it.col()) == 0.0)
// {
// gpdotz += z*it.value();
// }
// }
double s = z + std::abs(gpdotz);
s = s * std::sqrt(z*z/(s*s) + gpdotz*gpdotz/(s*s));
c = lambda*(z/s - 1);
d = lambda*(gpdotz/s -1);
}
else
{
double s = std::abs(m_cp_variables(i)) + std::abs( m_residuals(i));
s = s * std::sqrt((m_cp_variables(i)*m_cp_variables(i))/(s*s) + (m_residuals(i)*m_residuals(i))/(s*s));
c = lambda*(m_cp_variables(i)/s - 1.0);
d = lambda*( m_residuals(i)/s - 1.0);
if ((lambda < 1.0) and ( m_residuals(i) > 0.0) and (m_cp_variables(i) > 0.0))
{
c -= (1.0-lambda) * m_residuals(i);
d -= (1.0-lambda) * m_cp_variables(i);
}
}
c *= get_options().alpha*dt;
// update the jacobian
for (Eigen::SparseMatrix<double, Eigen::RowMajor>::InnerIterator it(jacobian,i); it; ++it)
{
it.valueRef() *= d;
if (it.row() == it.col()) it.valueRef() += c;
}
}
}
// ================================= //
// //
// Solving the linear system //
// ------------------------- //
// //
// ================================= //
template <class program_t>
MiCPParabolicSolverReturnCode MiCPParabolicSolver<program_t>::compute_search_direction(Eigen::VectorXd& update,
ParabolicVariables& x,
double dt)
{
Eigen::SparseMatrix<double, Eigen::RowMajor> jacobian;
setup_jacobian(x, dt, jacobian);
Eigen::SparseMatrix<double, Eigen::ColMajor> new_jacobian(jacobian);
Eigen::SparseQR<Eigen::SparseMatrix<double, Eigen::ColMajor>,
Eigen::COLAMDOrdering<int>> solver(new_jacobian);
//std::cout << new_jacobian.toDense() << std::endl;
m_gradient_step_taken = false;
if (solver.info() != Eigen::Success)
{
ERROR << "Failed to factorize the jacobian !";
return MiCPParabolicSolverReturnCode::FailedDecomposition;
}
m_gradient = jacobian.transpose()*m_phi_residuals;
update = solver.solve(-m_phi_residuals);
//std::cout << update << std::endl;
if (solver.info() != Eigen::Success)
{
ERROR << "Failed to solve the system !";
return MiCPParabolicSolverReturnCode::FailedDecomposition;
}
WARNING << "Linear system error : " << (jacobian*update + m_phi_residuals).norm();
//const double condition = - (get_options().factor_descent_direction *
// std::pow(update.norm(), get_options().power_descent_direction));
//WARNING << "Gradient step condition : " << m_gradient.dot(update) << " compared to : " << condition;
//std::cout << update << std::endl;
return MiCPParabolicSolverReturnCode::NotConvergedYet;
}
template <class program_t>
double MiCPParabolicSolver<program_t>::is_step_too_long(Eigen::VectorXd& update)
{
double steplength = update.norm();
//std::cout << "update : " << steplength << std::endl;
if (steplength > get_options().maximum_step_length)
{
get_performance().maximum_step_taken = true;
update = get_options().maximum_step_length / steplength * update;
steplength = get_options().maximum_step_length;
}
//0std::cout << "update : " << steplength << std::endl;
return steplength;
}
// ================== //
// //
// Linesearch //
// ---------- //
// //
// ================== //
template <class program_t>
double MiCPParabolicSolver<program_t>::compute_residuals_linesearch(
const Eigen::VectorXd& update,
ParabolicVariables& vars,
const Eigen::VectorXd &predictor,
double dt,
double lambda
)
{
Eigen::VectorXd residuals(get_neq());
get_program()->update_variables(vars, predictor, update, lambda, get_options().alpha*dt);
compute_residuals(vars, residuals);
reformulate_residuals_inplace(vars, residuals);
return 0.5*residuals.squaredNorm();
}
template <class program_t>
int MiCPParabolicSolver<program_t>::linesearch(Eigen::VectorXd& update,
ParabolicVariables& x,
const Eigen::VectorXd &predictor,
double dt)
{
// References
// ----------
// - Algo A6.3.1 : Dennis and Schnabel (1983)
// - Munson et al. (2001)
// - Facchinei (2003)
// - Nocedal & Wrigth (2006)
DEBUG << "Linesearch";
ParabolicVariables xp(x); // copy
double fcp;
WARNING << "Norm update : " << update.norm();
get_performance().maximum_step_taken = false;
int retcode = 2;
const double alpha = 1e-4;
double newtlen = is_step_too_long(update);
double init_slope = m_gradient.dot(update);
//std::cout << "slope : " << init_slope << std::endl;
double rellength = update.cwiseAbs().maxCoeff();
const double minlambda = get_options().step_tolerance / rellength;
//WARNING << "Minimum lambda : " << minlambda;
const double alphadt = get_options().alpha*dt;
double lambda = 1.0;
//double lambda = get_program()->maximum_lambda(x, update, alphadt);
double lambda_prev = lambda;
//std::cout << "Max lambda : " << lambda << std::endl;
// non monotone linesearch
// =======================
double merit_value = 0.5*m_phi_residuals.squaredNorm();
std::cout << "Beginning linesearch : " << merit_value << std::endl;
// new residual
fcp = compute_residuals_linesearch(update, xp, predictor, dt, lambda);
//xp.velocity = x.velocity;
// Skip linesearch if enough progress is done
// ------------------------------------------
if (fcp < get_options().coeff_accept_newton_step *merit_value)
{
WARNING << "Skip linesearch ";
if (m_max_merits.size() > 0) m_max_merits[m_max_merits.size()-1] = merit_value;
else m_max_merits.push_back(merit_value);
get_program()->update_variables(x, predictor, update, lambda, alphadt);
return 0;
}
// Gradient step
// -------------
const double condition = - (get_options().factor_descent_direction *
std::pow(update.norm(), get_options().power_descent_direction));
WARNING << "Gradient step condition : " << m_gradient.dot(update) << " compared to : " << condition;
// if (m_gradient.dot(update) > condition)
// {
// WARNING << "Gradient step taken !";
// get_performance().gradient_step_taken = true;
// update = - m_gradient;
// fcp = compute_residuals_linesearch(update, xp, predictor, dt, lambda, new_res);
// }
// Select the merit value of reference
// -----------------------------------
// 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) < 1e-3*get_options().residuals_tolerance)
//// {
//// WARNING << "Cycling has been detected by the linesearch - Taking the full Newton step";
//// x = xp;
//// update = lambda*update;
//// 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 (get_performance().gradient_step_taken)
//{
// merit_value *= 100;
//}
// The linesearch
// --------------
double fc = merit_value;
double fcp_prev;
int cnt = 0;
do
{
WARNING << "cnt : " <<cnt << " - lambda : " << lambda << " # " << fc << " # " << fcp << " # " << init_slope;
if (fcp <= fc + alpha*lambda*init_slope) //pg760 Fachinei2003
{
retcode = 0;
if (lambda ==1 and (newtlen > 0.99 * get_options().maximum_step_length)) {
get_performance().maximum_step_taken = true;
}
break;
}
else if (lambda < minlambda)
{
retcode = 1;
break;
}
else
{
//WARNING << "denom : " << fcp - fc -init_slope;
// 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;
}
//WARNING << "lambdatmp : " << lambdatmp;
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 : " << lambda;
const int ndf = 13;
for (int i=0; i<ndf; ++i)
{
for (int node=0; node<5; ++node)
{
std::cout << xp.displacement(i+ndf*node) << "\t";
}
std::cout << std::endl;
}
throw std::runtime_error("Lambda is nonfinite : "+std::to_string(lambda));
}
xp = x;
fcp = compute_residuals_linesearch(update, xp, predictor, dt, lambda);
//xp.velocity = x.velocity;
++cnt;
} while(retcode == 2 and cnt < 100);
//WARNING << "Lambda : " << lambda << " - iterations : " << cnt;
if (cnt == 100)
{
ERROR << "Too much linesearch iterations ! We stop";
}
get_program()->update_variables(x, predictor, update, lambda, alphadt);
return retcode;
}
} // end namespace micpsolver
} // end namespace reactmicp
} // end namespace specmicp
Event Timeline
Log In to Comment