Skip to content

Commit

Permalink
Merge pull request #808 from tianyang9310/Lemke_fix
Browse files Browse the repository at this point in the history 
Lemke fails several LCP problems with release-5.1 (tag 5.1.5)
  • Loading branch information
@jslee02
jslee02 authored Oct 29, 2016
2 parents 73eb001 + cae33a3 commit bf0549f
Show file tree
Hide file tree
Showing 2 changed files with 333 additions and 122 deletions.
298 changes: 176 additions & 122 deletions dart/lcpsolver/Lemke.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,22 +47,27 @@
(sizeof (x) == sizeof (long double) ? isnan_ld (x) \
: sizeof (x) == sizeof (double) ? isnan_d (x) \
: isnan_f(x))
static inline int isnan_f (float _x) { return _x != _x; }
static inline int isnan_d (double _x) { return _x != _x; }
static inline int isnan_ld (long double _x) { return _x != _x; }

static inline int isnan_f(float _x) { return _x != _x; }

static inline int isnan_d(double _x) { return _x != _x; }

static inline int isnan_ld(long double _x) { return _x != _x; }

#endif

#ifndef isinf
# define isinf(x) \
(sizeof (x) == sizeof (long double) ? isinf_ld (x) \
: sizeof (x) == sizeof (double) ? isinf_d (x) \
: isinf_f(x))
static inline int isinf_f(float _x)
{ return !isnan (_x) && isnan (_x - _x); }
static inline int isinf_d(double _x)
{ return !isnan (_x) && isnan (_x - _x); }
static inline int isinf_ld(long double _x)
{ return !isnan (_x) && isnan (_x - _x); }

static inline int isinf_f(float _x) { return !isnan (_x) && isnan (_x - _x); }

static inline int isinf_d(double _x) { return !isnan (_x) && isnan (_x - _x); }

static inline int isinf_ld(long double _x) { return !isnan (_x) && isnan (_x - _x); }

#endif

namespace dart {
Expand All @@ -84,104 +89,149 @@ namespace lcpsolver {
// return temp;
// }

int Lemke(const Eigen::MatrixXd& _M, const Eigen::VectorXd& _q,
Eigen::VectorXd* _z) {
int n = _q.size();
int Lemke(const Eigen::MatrixXd &_M, const Eigen::VectorXd &_q,
Eigen::VectorXd *_z) {
int n = _q.size();

const double zer_tol = 1e-5;
const double piv_tol = 1e-8;
int maxiter = 1000;
int err = 0;
const double zer_tol = 1e-5;
const double piv_tol = 1e-8;
int maxiter = 1000;
int err = 0;

if (_q.minCoeff() > 0) {
if (_q.minCoeff() >= 0) {
// LOG(INFO) << "Trivial solution exists.";
*_z = Eigen::VectorXd::Zero(n);
return err;
}

*_z = Eigen::VectorXd::Zero(2 * n);
int iter = 0;
// double theta = 0;
double ratio = 0;
int leaving = 0;
Eigen::VectorXd Be = Eigen::VectorXd::Constant(n, 1);
Eigen::VectorXd x = _q;
std::vector<int> bas;
std::vector<int> nonbas;

int t = 2 * n + 1;
int entering = t;
}

bas.clear();
nonbas.clear();
// solve trivial case for n=1
// if (n==1){
// if (_M(0)>0){
// *_z = (- _q(0)/_M(0) )*Eigen::VectorXd::Ones(n);
// return err;
// } else {
// *_z = Eigen::VectorXd::Zero(n);
// err = 4; // no solution
// return err;
// }
// }

*_z = Eigen::VectorXd::Zero(2 * n);
int iter = 0;
// double theta = 0;
double ratio = 0;
int leaving = 0;
Eigen::VectorXd Be = Eigen::VectorXd::Constant(n, 1);
Eigen::VectorXd x = _q;
std::vector<int> bas;
std::vector<int> nonbas;

int t = 2 * n;
int entering = t;

bas.clear();
nonbas.clear();

// TODO: here suppose initial guess z0 is [0,0,0,...], this contradicts to ODE's w always initilized as 0
for (int i = 0; i < n; ++i) {
nonbas.push_back(i);
}

for (int i = 0; i < n; ++i) {
bas.push_back(i);
}
Eigen::MatrixXd B = -Eigen::MatrixXd::Identity(n, n);

Eigen::MatrixXd B = -Eigen::MatrixXd::Identity(n, n);
if (!bas.empty()) {
Eigen::MatrixXd B_copy = B;
for (size_t i = 0; i < bas.size(); ++i) {
B.col(i) = _M.col(bas[i]);
}
for (size_t i = 0; i < nonbas.size(); ++i) {
B.col(bas.size() + i) = B_copy.col(nonbas[i]);
}
// TODO: check the condition number to return err = 3
Eigen::JacobiSVD<Eigen::MatrixXd> svd(B);
double cond = svd.singularValues()(0)
/ svd.singularValues()(svd.singularValues().size() - 1);
if (cond > 1e16) {
(*_z) = Eigen::VectorXd::Zero(n);
err = 3;
return err;
}
x = -B.householderQr().solve(_q);
}

for (size_t i = 0; i < bas.size(); ++i) {
B.col(bas[i]) = _M.col(bas[i]);
}
// Check if initial basis provides solution
if (x.minCoeff() >=0 ) {
Eigen::VectorXd __z = Eigen::VectorXd::Zero(2 * n);
for (size_t i = 0; i < bas.size(); ++i) {
(__z).row(bas[i]) = x.row(i);
}
(*_z) = __z.head(n);
return err;
}

x = -B.partialPivLu().solve(_q);
// Determine initial leaving variable
Eigen::VectorXd minuxX = -x;
int lvindex;
double tval = minuxX.maxCoeff(&lvindex);
for (size_t i = 0; i < nonbas.size(); ++i) {
bas.push_back(nonbas[i] + n);
}
leaving = bas[lvindex];

Eigen::VectorXd minuxX = -x;
int lvindex;
double tval = minuxX.maxCoeff(&lvindex);
leaving = bas[lvindex];
bas[lvindex] = t;
bas[lvindex] = t; // pivoting in the artificial variable

Eigen::VectorXd U = Eigen::VectorXd::Zero(n);
for (int i = 0; i < n; ++i) {
Eigen::VectorXd U = Eigen::VectorXd::Zero(n);
for (int i = 0; i < n; ++i) {
if (x[i] < 0)
U[i] = 1;
}
Be = -(B * U);
x += tval * U;
x[lvindex] = tval;
B.col(lvindex) = Be;

for (iter = 0; iter < maxiter; ++iter) {
U[i] = 1;
}
Be = -(B * U);
x += tval * U;
x[lvindex] = tval;
B.col(lvindex) = Be;

for (iter = 0; iter < maxiter; ++iter) {
if (leaving == t) {
break;
break;
} else if (leaving < n) {
entering = n + leaving;
Be = Eigen::VectorXd::Zero(n);
Be[leaving] = -1;
entering = n + leaving;
Be = Eigen::VectorXd::Zero(n);
Be[leaving] = -1;
} else {
entering = leaving - n;
Be = _M.col(entering);
entering = leaving - n;
Be = _M.col(entering);
}

Eigen::VectorXd d = B.partialPivLu().solve(Be);
Eigen::VectorXd d = B.householderQr().solve(Be);

// Find new leaving variable
std::vector<int> j;
for (int i = 0; i < n; ++i) {
if (d[i] > piv_tol)
j.push_back(i);
if (d[i] > piv_tol)
j.push_back(i);
}
if (j.empty()) {
// err = 2;
break;
if (j.empty()) // no new pivots - ray termination
{
err = 2;
break;
}

size_t jSize = j.size();
Eigen::VectorXd minRatio(jSize);
for (size_t i = 0; i < jSize; ++i) {
minRatio[i] = (x[j[i]] + zer_tol) / d[j[i]];
minRatio[i] = (x[j[i]] + zer_tol) / d[j[i]];
}
double theta = minRatio.minCoeff();

std::vector<int> tmpJ;
std::vector<double> tmpMinRatio;
std::vector<double> tmpd;
for (size_t i = 0; i < jSize; ++i) {
if (x[j[i]] / d[j[i]] <= theta) {
tmpJ.push_back(j[i]);
tmpMinRatio.push_back(minRatio[i]);
}
if (x[j[i]] / d[j[i]] <= theta) {
tmpJ.push_back(j[i]);
tmpd.push_back(d[j[i]]);
}
}

// if (tmpJ.empty())
// {
// LOG(WARNING) << "tmpJ should never be empty!!!";
Expand All @@ -196,71 +246,75 @@ int Lemke(const Eigen::MatrixXd& _M, const Eigen::VectorXd& _q,
j = tmpJ;
jSize = j.size();
if (jSize == 0) {
err = 4;
break;
err = 4;
break;
}
lvindex = -1;

// Check if artificial among these
for (size_t i = 0; i < jSize; ++i) {
if (bas[j[i]] == t)
lvindex = i;
if (bas[j[i]] == t)
lvindex = i;
}

if (lvindex != -1) {
lvindex = j[lvindex];
lvindex = j[lvindex]; // Always use artificial if possible
} else {
theta = tmpMinRatio[0];
lvindex = 0;

for (size_t i = 0; i < jSize; ++i) {
if (tmpMinRatio[i]-theta > piv_tol) {
theta = tmpMinRatio[i];
lvindex = i;
theta = tmpd[0];
lvindex = 0;
for (size_t i = 0; i < jSize; ++i) {
if (tmpd[i] - theta > piv_tol) { // Bubble sorting
theta = tmpd[i];
lvindex = i;
}
}
}
lvindex = j[lvindex];
lvindex = j[lvindex]; // choose the first if there are multiple
}

leaving = bas[lvindex];

ratio = x[lvindex] / d[lvindex];

bool bDiverged = false;
for (int i = 0; i < n; ++i) {
if (isnan(x[i]) || isinf(x[i])) {
bDiverged = true;
break;
}
}
if (bDiverged) {
err = 4;
break;
}
// bool bDiverged = false;
// for (int i = 0; i < n; ++i) {
// if (isnan(x[i]) || isinf(x[i])) {
// bDiverged = true;
// break;
// }
// }
// if (bDiverged) {
// err = 4;
// break;
// }

// Perform pivot
x = x - ratio * d;
x[lvindex] = ratio;
B.col(lvindex) = Be;
bas[lvindex] = entering;
}
}

if (iter >= maxiter && leaving != t)
if (iter >= maxiter && leaving != t) {
err = 1;
}

if (err == 0) {
if (err == 0) {
for (size_t i = 0; i < bas.size(); ++i) {
if (bas[i] < _z->size()) {
(*_z)[bas[i]] = x[i];
}
if (bas[i] < _z->size()) {
(*_z)[bas[i]] = x[i];
}
}

Eigen::VectorXd realZ = _z->segment(0, n);
*_z = realZ;
Eigen::VectorXd __z = _z->head(n);
*_z = __z;

if (!validate(_M, *_z, _q)) {
// _z = VectorXd::Zero(n);
err = 3;
// _z = VectorXd::Zero(n);
err = 3;
}
} else {
} else {
*_z = Eigen::VectorXd::Zero(n); // solve failed, return a 0 vector
}
}

// if (err == 1)
// LOG(ERROR) << "LCP Solver: Iterations exceeded limit";
Expand All @@ -272,22 +326,22 @@ int Lemke(const Eigen::MatrixXd& _M, const Eigen::VectorXd& _q,
// else if (err == 4)
// LOG(ERROR) << "LCP Solver: Iteration diverged.";

return err;
return err;
}

bool validate(const Eigen::MatrixXd& _M, const Eigen::VectorXd& _z,
const Eigen::VectorXd& _q) {
const double threshold = 1e-4;
int n = _z.size();
bool validate(const Eigen::MatrixXd &_M, const Eigen::VectorXd &_z,
const Eigen::VectorXd &_q) {
const double threshold = 1e-4;
int n = _z.size();

Eigen::VectorXd w = _M * _z + _q;
for (int i = 0; i < n; ++i) {
Eigen::VectorXd w = _M * _z + _q;
for (int i = 0; i < n; ++i) {
if (w(i) < -threshold || _z(i) < -threshold)
return false;
return false;
if (std::abs(w(i) * _z(i)) > threshold)
return false;
}
return true;
return false;
}
return true;
}

} // namespace lcpsolver
Expand Down
Loading

0 comments on commit bf0549f

Please sign in to comment.