Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Lemke fails several LCP problems with release-5.1 (tag 5.1.5) #808

Merged
merged 3 commits into from
Oct 29, 2016
+333 −122
Merged

Lemke fails several LCP problems with release-5.1 (tag 5.1.5) #808

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
78edb2a
Fix Lemke bug: add early stopping
Oct 24, 2016
fc04bba
add testing
Oct 24, 2016
cae33a3
Fix another Lemke bug: tmpMinRatio ==> tmpd
Oct 28, 2016
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
298 changes: 176 additions & 122 deletions dart/lcpsolver/Lemke.cpp
View file
Open in desktop
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