From 78edb2aec92ef056a0c4d9d533223f28592c5a88 Mon Sep 17 00:00:00 2001 From: YangTian Date: Sun, 23 Oct 2016 21:19:55 -0400 Subject: [PATCH] Fix Lemke bug: add early stopping --- dart/lcpsolver/Lemke.cpp | 296 +++++++++++++++++++++++---------------- 1 file changed, 175 insertions(+), 121 deletions(-) diff --git a/dart/lcpsolver/Lemke.cpp b/dart/lcpsolver/Lemke.cpp index 78acdf7cfd719..7e1d2fbe43454 100644 --- a/dart/lcpsolver/Lemke.cpp +++ b/dart/lcpsolver/Lemke.cpp @@ -47,9 +47,13 @@ (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 @@ -57,12 +61,13 @@ static inline int isnan_ld (long double _x) { return _x != _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 { @@ -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 bas; - std::vector 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 bas; +std::vector 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 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 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 tmpJ; std::vector tmpMinRatio; 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]); + tmpMinRatio.push_back(minRatio[i]); + } } + // if (tmpJ.empty()) // { // LOG(WARNING) << "tmpJ should never be empty!!!"; @@ -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 = tmpMinRatio[0]; + lvindex = 0; + for (size_t i = 0; i < jSize; ++i) { + if (tmpMinRatio[i] - theta > piv_tol) { // Bubble sorting + theta = tmpMinRatio[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"; @@ -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