diff --git a/src/micpsolver/micpprog.hpp b/src/micpsolver/micpprog.hpp
index 3f46db2..d5e0651 100644
--- a/src/micpsolver/micpprog.hpp
+++ b/src/micpsolver/micpprog.hpp
@@ -1,56 +1,58 @@
/*-------------------------------------------------------
- Module : micpsolver
- File : micpprog.hpp
- Author : Fabien Georget
Copyright (c) 2014, Fabien Georget, Princeton University
---------------------------------------------------------*/
#ifndef SPECMICP_MICPSOLVER_MICPPROG_HPP
#define SPECMICP_MICPSOLVER_MICPPROG_HPP
//! \file micpprog.hpp The static interface of a MiCP solver
namespace specmicp {
namespace micpsolver {
struct traits_MiCPProg
{
using scalar_t = double;
using vector_t = Eigen::Matrix<double, Eigen::Dynamic, 1>;
using matrix_t = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
};
//! A MiCP program
template <class Derived>
class MiCPProg
{
public:
using types = traits_MiCPProg;
static double infinity() {return HUGE_VAL;}
Derived& derived() {return static_cast<Derived*>(*this);}
//! Return the number of variables
int total_variables();
int nb_free_variables();
int nb_complementarity_variables() {return total_variables() - nb_free_variables();}
//! Return the residual (R(X))
void get_residuals(const types::vector_t& x,
types::vector_t& residual);
//! Return the jacobian (J(x))
void get_jacobian(const types::vector_t& x,
types::matrix_t& jacobian);
+
+ void hook_start_iteration(const types::vector_t& x) {}
};
} // end namespace micpsolver
} // end namespace specmicp
#endif // SPECMICP_MICPSOLVER_MICPPROG_HPP
diff --git a/src/micpsolver/micpsolver.inl b/src/micpsolver/micpsolver.inl
index 254b426..a866a72 100644
--- a/src/micpsolver/micpsolver.inl
+++ b/src/micpsolver/micpsolver.inl
@@ -1,514 +1,515 @@
/*-------------------------------------------------------
- Module : micpsolver
- File : micpsolver.inl
- Author : Fabien Georget
Copyright (c) 2014, Fabien Georget, Princeton University
---------------------------------------------------------*/
#include "micpsolver.hpp" // for syntaxic coloration...
#include "estimate_cond_number.hpp"
namespace specmicp {
namespace micpsolver {
// Main algorithm
// ##############
template <class Program>
MiCPSolverReturnCode MiCPSolver<Program>::solve(Eigen::VectorXd &x)
{
int cnt = 0;
crashing(x);
MiCPSolverReturnCode retcode = MiCPSolverReturnCode::NotConvergedYet;
Eigen::VectorXd update(get_neq());
while (retcode == MiCPSolverReturnCode::NotConvergedYet)
{
std::cout << "Iteration : " << cnt << std::endl;
+ m_program->hook_start_iteration(x);
setup_residuals(x);
//std::cout << "Residuals \n ------- \n " << m_residuals << "\n ---- \n" << m_phi_residuals << std::endl;
retcode = check_convergence(cnt, update, x);
m_max_taken = false;
if (retcode != MiCPSolverReturnCode::NotConvergedYet) break;
setup_jacobian(x);
std::cout << "Solve system " << std::endl;
search_direction_calculation(update);
//x += update;
int retcode = linesearch(update, x);
std::cout << " Return linesearch : " << retcode << std::endl;
std::cout << "Solution : \n ------ \n" << x << std::endl;
++cnt;
}
return retcode;
}
template <class Program>
MiCPSolverReturnCode MiCPSolver<Program>::check_convergence(int nb_iterations,
Eigen::VectorXd& update,
Eigen::VectorXd& solution)
{
MiCPSolverReturnCode termcode = MiCPSolverReturnCode::NotConvergedYet;
std::cout << "Residuals : " << m_phi_residuals.lpNorm<Eigen::Infinity>() << std::endl;
//<< " -------- \n" << m_phi_residuals << std::endl;
if (m_phi_residuals.lpNorm<Eigen::Infinity>() < get_options().fvectol)
{
termcode = MiCPSolverReturnCode::ResidualMinimized;
}
else if (nb_iterations >0 and norm_update<Eigen::Infinity>(update, solution) < get_options().steptol)
{
termcode = MiCPSolverReturnCode::ErrorMinimized;
}
else if (nb_iterations > get_options().max_iter)
{
termcode = MiCPSolverReturnCode::MaxIterations;
}
else if (m_max_taken)
{
++m_consec_max;
if (m_consec_max == get_options().maxiter_maxstep) {
termcode = MiCPSolverReturnCode::MaxStepTakenTooManyTimes;
}
}
else
{
m_consec_max = 0;
}
return termcode;
}
template <class Program>
MiCPSolverReturnCode MiCPSolver<Program>::search_direction_calculation(Eigen::VectorXd& update)
{
Eigen::VectorXd rscaler(Eigen::VectorXd::Ones(get_neq()));
Eigen::VectorXd cscaler(Eigen::VectorXd::Ones(get_neq()));
scaling_jacobian(m_jacobian, m_phi_residuals, rscaler, cscaler);
//std::cout << m_jacobian << std::endl;
m_jacobian = rscaler.asDiagonal() * m_jacobian * cscaler.asDiagonal();
//m_jacobian += 1e-5*Eigen::MatrixXd::Identity(get_neq(), get_neq());
//std::cout << m_jacobian << std::endl;
//m_phi_residuals = - rscaler.cwiseProduct(m_phi_residuals);
double descent_cond = 0;
Eigen::ColPivHouseholderQR<Eigen::MatrixXd> solver = m_jacobian.colPivHouseholderQr();
if (solver.info() != Eigen::Success)
{
throw std::runtime_error("Error when computing decomposition");
}
//solver.compute(m_jacobian);
if (solver.isInvertible())
{
double cond = estimate_condition_number(solver.matrixR().triangularView<Eigen::Upper>());
int cnt = 0;
while (cond > get_options().condition_limit and cnt < 10)
{
std::cout << "Bad conditionning : " << cond << std::endl;
//std::cout << m_jacobian << std::endl;
//throw std::runtime_error("bummer");
double delta = m_phi_residuals.norm();
m_jacobian += delta*Eigen::MatrixXd::Identity(get_neq(), get_neq());
solver.compute(m_jacobian);
cond = estimate_condition_number(solver.matrixR().triangularView<Eigen::Upper>());
++cnt;
}
update = solver.solve(-rscaler.cwiseProduct(m_phi_residuals));
update = cscaler.cwiseProduct(update);
- std::cout << "update : \n" << update << std::endl;
+ //std::cout << "update : \n" << update << std::endl;
descent_cond = m_grad_phi.dot(update)
+ get_options().factor_descent_condition * std::pow(update.norm(), get_options().power_descent_condition);
}
std::cout << "Descent condition : " << descent_cond << std::endl;
if ( (not solver.isInvertible())
or (solver.info() != Eigen::Success)
or (descent_cond > 0)
)
{
std::cout << "Gradient step " << std::endl;
update = -m_grad_phi;
}
return MiCPSolverReturnCode::NotConvergedYet;
}
//template <class Program>
//int MiCPSolver<Program>::linesearch(Eigen::VectorXd& update, Eigen::VectorXd& x)
//{
// double beta = 0.5;
// double sigma = 1e-4;
// double prev_test = HUGE_VAL;
// Eigen::VectorXd xp(get_neq());
// int l = 0;
// // non monotone linesearch
// double merit_value = 0.5*m_phi_residuals.squaredNorm();
// 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;
// }
// std::cout << "Merit value used : " << merit_value << std::endl;
// std::sort(m_max_merits.begin(), m_max_merits.end());
// const double gphidupdate = m_grad_phi.dot(update);
// while (l<20)
// {
// xp = x + std::pow(beta, l)*update;
// // projection
//// for (int i=0; i<m_program->nb_complementarity_variables(); ++i)
//// {
//// if (xp(i+m_program->nb_free_variables()) < 0)
//// {
//// xp(i+m_program->nb_free_variables()) = 0;
//// }
//// }
// Eigen::VectorXd new_res(get_neq());
// m_program->get_residuals(xp, new_res);
// reformulate_residuals_inplace(xp, new_res);
// double test = 0.5*new_res.squaredNorm() - merit_value - sigma*std::pow(beta, l)*gphidupdate;
// if (test > prev_test) {
// xp = x + std::pow(beta, l-1)*update;
// std::cout << "Failed Global Method ! -- too short" << std::endl;
// // projection
//// for (int i=0; i<m_program->nb_complementarity_variables(); ++i)
//// {
//// if (xp(i+m_program->nb_free_variables()) < 0)
//// {
//// xp(i+m_program->nb_free_variables()) = 0;
//// }
//// }
// break;
// }
// if (test <= 0)
// {
// break;
// }
// prev_test = test;
// ++l;
// }
// x = xp;
// if (l == 20) {
// std::cout << "Failed Global Method !" << std::endl;
// //x -= m_grad_phi / (10*m_grad_phi.norm());
// }
// return 0;
//}
template <class Program>
void MiCPSolver<Program>::crashing(Eigen::VectorXd &x)
{
std::cout << "Crashing " << std::endl;
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;
std::cout << m_grad_phi << std::endl;
Eigen::VectorXd xp(get_neq());
int l=0;
while (l<20)
{
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 == 20) break;
x = xp;
++cnt;
}
std::cout << "Crashing iterations : " << cnt << std::endl;
std::cout << "Merit function after crashing " << 0.5*m_grad_phi.squaredNorm() << std::endl;
m_max_merits.reserve(4);
std::cout << "Solution after crashing \n ------ \n " << x << std::endl;
//m_max_merits.push_back(0.5*m_grad_phi.squaredNorm());
}
// Others
// ######
template <class Program>
void MiCPSolver<Program>::reformulate_residuals(const Eigen::VectorXd& x,
const Eigen::VectorXd& r,
Eigen::VectorXd& r_phi)
{
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);
}
- std::cout << "Residuals \n-----\n" << r << "\n -----\n Reformulation \n ---- \n" << r_phi << "\n ---- \n";
+ //std::cout << "Residuals \n-----\n" << r << "\n -----\n Reformulation \n ---- \n" << r_phi << "\n ---- \n";
}
template <class Program>
void MiCPSolver<Program>::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);
}
}
double dz(double z) {
if (z > 0)
{
return 1.0;
}
else if (z < 0)
{
return 0;
}
else // z ==0
return 0.0;
}
// page 808 Facchinei and Pang(2003) (vol 2, chap 9)
template <class Program>
void MiCPSolver<Program>::reformulate_jacobian(const Eigen::VectorXd& x,
const Eigen::VectorXd& r,
Eigen::MatrixXd& jacobian
)
{
// 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 r(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)
{
const auto ei = unit_vector(get_neq(), i);
Eigen::VectorXd grad_fi = 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 = c*ei + d*grad_fi;
}
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 s = std::sqrt(x(i)*x(i) + r(i)*r(i));
//const double c = (x(i)/s -1);
//const double d = (r(i)/s -1);
- const double c = lambda*(x(i)/s - 1) - (1-lambda)*(std::min(0.0, r(i)*dz(x(i))));
- const double d = lambda*(r(i)/s - 1) - (1-lambda)*(std::min(0.0, x(i))*dz(r(i)));
+ const double c = lambda*(x(i)/s - 1) - (1-lambda)*(std::max(0.0, r(i)*dz(x(i))));
+ const double d = lambda*(r(i)/s - 1) - (1-lambda)*(std::max(0.0, x(i))*dz(r(i)));
grad_fi = c*ei + d*grad_fi;
}
jacobian.block(i, 0, 1, get_neq()) = grad_fi.transpose();
}
}
// ref : Munson et al. (2001)
template <class Program>
void MiCPSolver<Program>:: scaling_jacobian(
const Eigen::MatrixXd& jacobian,
const Eigen::VectorXd& r_phi,
Eigen::VectorXd& rscaler,
Eigen::VectorXd& cscaler)
{
//rscaler = Eigen::VectorXd::Ones(get_neq());
//cscaler = Eigen::VectorXd::Ones(get_neq());
for (int i=0; i<get_neq(); ++i)
{
const double sumhsq = jacobian.block(i, 0, 1, get_neq()).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<get_neq(); ++i)
{
const double sumhsq = (rscaler.asDiagonal()*jacobian).block(0, i, get_neq(), 1).array().square().sum();
double s = std::sqrt(sumhsq);
cscaler(i) = 1.0/std::max(s, 1e-10);
}
}
template <class Program>
int MiCPSolver<Program>::linesearch(Eigen::VectorXd& p, Eigen::VectorXd& x)
{
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);
std::cout << "Init slope : " << init_slope << std::endl;
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 = 1.0;
double lambda_prev = 1.0;
// non monotone linesearch
double merit_value = 0.5*m_phi_residuals.squaredNorm();
xp = x + lambda*p;
projection(xp);
//std::cout << xp << std::endl;
compute_residuals(xp, new_res);
reformulate_residuals_inplace(xp, new_res);
fcp = 0.5*new_res.squaredNorm();
if (fcp < 0.95 *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;
}
std::cout << "Merit value used : " << merit_value << std::endl;
double fc = merit_value;
double fcp_prev;
do
{
fcp = 0.5*new_res.squaredNorm();
std::cout << "lambda : " << lambda << std::endl;
//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 (lambda == 1.0) {
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 is quadratic
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 ) 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;
projection(xp);
//std::cout << xp << std::endl;
compute_residuals(xp, new_res);
reformulate_residuals_inplace(xp, new_res);
} while(retcode == 2);
//projection(xp);
x = xp;
p = lambda*p;
return retcode;
}
// Projection of the variables onto the feasible set
template <class Program>
void MiCPSolver<Program>::projection(Eigen::VectorXd &x)
{
for (int i=0; i<m_program->nb_complementarity_variables(); ++i)
{
if (x(i+m_program->nb_free_variables()) < 0)
{
x(i+m_program->nb_free_variables()) = 0;
}
}
}
template <class Program>
double MiCPSolver<Program>::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;
}
} // end namespace micpsolver
} // end namespace specmicp
diff --git a/tests/micpsolver/test_micpsolver.hpp b/tests/micpsolver/test_micpsolver.hpp
index d44e12a..74277ee 100644
--- a/tests/micpsolver/test_micpsolver.hpp
+++ b/tests/micpsolver/test_micpsolver.hpp
@@ -1,202 +1,208 @@
/*-------------------------------------------------------
- Module : tests/micpsolver
- File : test_micpsolver.hpp
- Author : Fabien Georget
Copyright (c) 2014, Fabien Georget, Princeton University
---------------------------------------------------------*/
#include "cxxtest/TestSuite.h"
#include "micpsolver/micpsolver.hpp"
#include "micpsolver/micpprog.hpp"
using namespace specmicp::micpsolver;
class TestLinearProgram: public MiCPProg<TestLinearProgram>
{
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 types::vector_t& x,
types::vector_t& 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 types::vector_t& x,
types::matrix_t& jacobian)
{
assert(jacobian.cols() == 3);
assert(jacobian.rows() == 3);
jacobian << 1, 0, 1, 0, 1, 1, -1, -1, 1;
}
};
class TestNonLinearProgram: public MiCPProg<TestNonLinearProgram>
{
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 types::vector_t& x,
types::vector_t& 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 types::vector_t& x,
types::matrix_t& 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<TestKojimaProgram>
{
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 types::vector_t& x,
types::vector_t& 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(types::vector_t& x,
types::matrix_t& jacobian)
{
assert(jacobian.cols() == 4);
assert(jacobian.rows() == 4);
const int neq = total_variables();
Eigen::VectorXd res(total_variables());
Eigen::VectorXd perturbed_res(total_variables());
get_residuals(x, res);
for (int j=0; j<neq; ++j)
{
double h = 1e-8*std::abs(x(j));
//h = std::copysign(h, x(j));
if (h==0) h = 1e-8;
double tmp = x(j);
x(j) += h;
h = x(j) - tmp;
get_residuals(x, perturbed_res);
for (int i=0; i<neq; ++i)
{
jacobian(i, j) = (perturbed_res(i) - res(i))/h;
}
x(j) = tmp;
}
return;
}
};
class Test_Micp : public CxxTest::TestSuite
{
public:
void test_compiletest()
{
}
void test_linear_program_0()
{
std::shared_ptr<TestLinearProgram> ptrprog = std::make_shared<TestLinearProgram>();
MiCPSolver<TestLinearProgram> solver(ptrprog);
Eigen::VectorXd x(3);
x << -1.5, -2 , 0;
MiCPSolverReturnCode ret = solver.solve(x);
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);
}
void test_linear_program_10()
{
std::shared_ptr<TestLinearProgram> ptrprog = std::make_shared<TestLinearProgram>();
MiCPSolver<TestLinearProgram> solver(ptrprog);
Eigen::VectorXd x(3);
x << -1.5, -2 , 10;
MiCPSolverReturnCode ret = solver.solve(x);
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);
}
void test_nonlinear_program_0()
{
std::shared_ptr<TestNonLinearProgram> ptrprog = std::make_shared<TestNonLinearProgram>();
MiCPSolver<TestNonLinearProgram> solver(ptrprog);
Eigen::VectorXd x(3);
x << -1.5, -2 , 0;
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);
}
void test_nonlinear_program_5()
{
std::shared_ptr<TestNonLinearProgram> ptrprog = std::make_shared<TestNonLinearProgram>();
MiCPSolver<TestNonLinearProgram> solver(ptrprog);
Eigen::VectorXd x(3);
x << -1.5, -2 , 5;
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);
}
void test_kojima_program()
{
std::shared_ptr<TestKojimaProgram> ptrprog = std::make_shared<TestKojimaProgram>();
MiCPSolver<TestKojimaProgram> 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);
}
};