diff --git a/dart/lcpsolver/Lemke.cpp b/dart/lcpsolver/Lemke.cpp index 78acdf7cfd719..e9bee0743f81b 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; + std::vector 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!!!"; @@ -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"; @@ -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 diff --git a/unittests/testLemke.cpp b/unittests/testLemke.cpp new file mode 100644 index 0000000000000..0a181ba7d6ed7 --- /dev/null +++ b/unittests/testLemke.cpp @@ -0,0 +1,157 @@ +#include + +#include "dart/lcpsolver/Lemke.h" +#include "TestHelpers.h" + + +//============================================================================== +TEST(Lemke, Lemke_1D) +{ + Eigen::MatrixXd A; + Eigen::VectorXd b; + Eigen::VectorXd* f; + int err; + + f = new Eigen::VectorXd(1); + A.resize(1,1); + A << 1; + b.resize(1); + b << -1.5; + err = dart::lcpsolver::Lemke(A,b,f); + + EXPECT_EQ(err, 0); + EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b)); +} + +//============================================================================== +TEST(Lemke, Lemke_2D) +{ + Eigen::MatrixXd A; + Eigen::VectorXd b; + Eigen::VectorXd* f; + int err; + + f = new Eigen::VectorXd(2); + A.resize(2,2); + A << 3.12, 0.1877, 0.1877, 3.254; + b.resize(2); + b << -0.00662, -0.006711; + err = dart::lcpsolver::Lemke(A,b,f); + + EXPECT_EQ(err, 0); + EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b)); +} + +//============================================================================== +TEST(Lemke, Lemke_4D) +{ + Eigen::MatrixXd A; + Eigen::VectorXd b; + Eigen::VectorXd* f; + int err; + + f = new Eigen::VectorXd(4); + A.resize(4,4); + A << + 3.999,0.9985, 1.001, -2, + 0.9985, 3.998, -2,0.9995, + 1.001, -2, 4.002, 1.001, + -2,0.9995, 1.001, 4.001; + + b.resize(4); + b << + -0.01008, + -0.009494, + -0.07234, + -0.07177; + + err = dart::lcpsolver::Lemke(A,b,f); + + EXPECT_EQ(err, 0); + EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b)); +} + +//============================================================================== +TEST(Lemke, Lemke_6D) +{ + Eigen::MatrixXd A; + Eigen::VectorXd b; + Eigen::VectorXd* f; + int err; + + f = new Eigen::VectorXd(6); + A.resize(6,6); + A << + 3.1360, -2.0370, 0.9723, 0.1096, -2.0370, 0.9723, + -2.0370, 3.7820, 0.8302, -0.0257, 2.4730, 0.0105, + 0.9723, 0.8302, 5.1250, -2.2390, -1.9120, 3.4080, + 0.1096, -0.0257, -2.2390, 3.1010, -0.0257, -2.2390, + -2.0370, 2.4730, -1.9120, -0.0257, 5.4870, -0.0242, + 0.9723, 0.0105, 3.4080, -2.2390, -0.0242, 3.3860; + + b.resize(6); + b << + 0.1649, + -0.0025, + -0.0904, + -0.0093, + -0.0000, + -0.0889; + + err = dart::lcpsolver::Lemke(A,b,f); + + EXPECT_EQ(err, 0); + EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b)); +} + +//============================================================================== +TEST(Lemke, Lemke_12D) +{ + Eigen::MatrixXd A; + Eigen::VectorXd b; + Eigen::VectorXd* f; + int err; + + f = new Eigen::VectorXd(12); + A.resize(12,12); + A << + 4.03, -1.014, -1.898, 1.03, -1.014, -1.898, 1, -1.014, -1.898, -2, -1.014, -1.898, + -1.014, 4.885, -1.259, 1.888, 3.81, 2.345, -1.879, 1.281, -2.334, 1.022, 0.206, 1.27, + -1.898, -1.259, 3.2, -1.032,-0.6849, 1.275, 1.003, 0.6657, 3.774, 1.869, 1.24, 1.85, + 1.03, 1.888, -1.032, 4.03, 1.888, -1.032, -2, 1.888, -1.032, 1, 1.888, -1.032, + -1.014, 3.81,-0.6849, 1.888, 3.225, 1.275, -1.879, 1.85, -1.27, 1.022, 1.265, 0.6907, + -1.898, 2.345, 1.275, -1.032, 1.275, 4.86, 1.003, -1.24, 0.2059, 1.869, -2.309, 3.791, + 1, -1.879, 1.003, -2, -1.879, 1.003, 3.97, -1.879, 1.003, 0.9703, -1.879, 1.003, + -1.014, 1.281, 0.6657, 1.888, 1.85, -1.24, -1.879, 3.187, 1.234, 1.022, 3.755,-0.6714, + -1.898, -2.334, 3.774, -1.032, -1.27, 0.2059, 1.003, 1.234, 4.839, 1.869, 2.299, 1.27, + -2, 1.022, 1.869, 1, 1.022, 1.869, 0.9703, 1.022, 1.869, 3.97, 1.022, 1.869, + -1.014, 0.206, 1.24, 1.888, 1.265, -2.309, -1.879, 3.755, 2.299, 1.022, 4.814, -1.25, + -1.898, 1.27, 1.85, -1.032, 0.6907, 3.791, 1.003,-0.6714, 1.27, 1.869, -1.25, 3.212; + + b.resize(12); + b << + -0.00981, + -1.458e-10, + 5.357e-10, + -0.0098, + -1.44e-10, + 5.298e-10, + -0.009807, + -1.399e-10, + 5.375e-10, + -0.009807, + -1.381e-10, + 5.316e-10; + + err = dart::lcpsolver::Lemke(A,b,f); + + EXPECT_EQ(err, 0); + EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b)); +} + +//============================================================================== +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc,argv); + return RUN_ALL_TESTS(); +}