From d6919e9fbbfa128de4eb9c74441061f072548151 Mon Sep 17 00:00:00 2001 From: YangTian Date: Sun, 23 Oct 2016 21:19:55 -0400 Subject: [PATCH 1/7] Fix Lemke bug: add early stopping (cherry picked from commit 78edb2aec92ef056a0c4d9d533223f28592c5a88) # Conflicts: # dart/lcpsolver/Lemke.cpp --- dart/lcpsolver/Lemke.cpp | 322 +++++++++++++++++++++++---------------- 1 file changed, 191 insertions(+), 131 deletions(-) diff --git a/dart/lcpsolver/Lemke.cpp b/dart/lcpsolver/Lemke.cpp index d22b5b4a57129..7e1d2fbe43454 100644 --- a/dart/lcpsolver/Lemke.cpp +++ b/dart/lcpsolver/Lemke.cpp @@ -1,9 +1,15 @@ /* - * Copyright (c) 2011-2016, Graphics Lab, Georgia Tech Research Corporation - * Copyright (c) 2011-2016, Humanoid Lab, Georgia Tech Research Corporation - * Copyright (c) 2016, Personal Robotics Lab, Carnegie Mellon University + * Copyright (c) 2011-2015, Georgia Tech Research Corporation * All rights reserved. * + * Author(s): Jie (Jay) Tan , + * Yunfei Bai + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * * This file is provided under the following "BSD-style" License: * Redistribution and use in source and binary forms, with or * without modification, are permitted provided that the following @@ -33,17 +39,21 @@ #include #include -#include "dart/math/Helpers.hpp" -#include "dart/lcpsolver/Lemke.hpp" +#include "dart/math/Helpers.h" +#include "dart/lcpsolver/Lemke.h" #ifndef isnan # define isnan(x) \ (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 @@ -51,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 { @@ -78,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 (std::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; } - std::size_t jSize = j.size(); + size_t jSize = j.size(); Eigen::VectorXd minRatio(jSize); - for (std::size_t i = 0; i < jSize; ++i) { - minRatio[i] = (x[j[i]] + zer_tol) / d[j[i]]; + for (size_t i = 0; i < jSize; ++i) { + minRatio[i] = (x[j[i]] + zer_tol) / d[j[i]]; } double theta = minRatio.minCoeff(); std::vector tmpJ; std::vector tmpMinRatio; - for (std::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]); - } + 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 (tmpJ.empty()) // { // LOG(WARNING) << "tmpJ should never be empty!!!"; @@ -190,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; - for (std::size_t i = 0; i < jSize; ++i) { - if (bas[j[i]] == t) - lvindex = i; + // Check if artificial among these + for (size_t i = 0; i < jSize; ++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 (std::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) { - for (std::size_t i = 0; i < bas.size(); ++i) { - if (bas[i] < _z->size()) { - (*_z)[bas[i]] = x[i]; - } +if (err == 0) { + for (size_t i = 0; i < bas.size(); ++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"; @@ -266,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 From 5ff4d6e784d124f63a943f91e6cd87af2bae1de7 Mon Sep 17 00:00:00 2001 From: yang-lab Date: Thu, 27 Oct 2016 20:13:15 -0400 Subject: [PATCH 2/7] Fix another Lemke bug: tmpMinRatio ==> tmpd Provide unittest instead of another apps example (cherry picked from commit cae33a3c4809be843343b70677d7ea418a7c09e7) --- dart/lcpsolver/Lemke.cpp | 28 +++---- unittests/testLemke.cpp | 157 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 171 insertions(+), 14 deletions(-) create mode 100644 unittests/testLemke.cpp diff --git a/dart/lcpsolver/Lemke.cpp b/dart/lcpsolver/Lemke.cpp index 7e1d2fbe43454..e9bee0743f81b 100644 --- a/dart/lcpsolver/Lemke.cpp +++ b/dart/lcpsolver/Lemke.cpp @@ -224,11 +224,11 @@ for (iter = 0; iter < maxiter; ++iter) { 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]); + tmpd.push_back(d[j[i]]); } } @@ -260,11 +260,11 @@ for (iter = 0; iter < maxiter; ++iter) { if (lvindex != -1) { lvindex = j[lvindex]; // Always use artificial if possible } else { - theta = tmpMinRatio[0]; + theta = tmpd[0]; lvindex = 0; for (size_t i = 0; i < jSize; ++i) { - if (tmpMinRatio[i] - theta > piv_tol) { // Bubble sorting - theta = tmpMinRatio[i]; + if (tmpd[i] - theta > piv_tol) { // Bubble sorting + theta = tmpd[i]; lvindex = i; } } @@ -275,17 +275,17 @@ for (iter = 0; iter < maxiter; ++iter) { ratio = x[lvindex] / d[lvindex]; -// bool bDiverged = false; -// for (int i = 0; i < n; ++i) { -// if (isnan(x[i]) || isinf(x[i])) { -// bDiverged = true; +// 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; // } -// } -// if (bDiverged) { -// err = 4; -// break; -// } // Perform pivot x = x - ratio * d; 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(); +} From c989138a86bc10b211827428e5045e657ada9360 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 3 Nov 2016 02:45:14 -0400 Subject: [PATCH 3/7] Update Lemke fix to be compatible with DART 6 --- dart/lcpsolver/Lemke.cpp | 24 ++++++------- unittests/unit/CMakeLists.txt | 1 + .../{testLemke.cpp => unit/test_Lemke.cpp} | 36 +++++++++++++++++-- 3 files changed, 46 insertions(+), 15 deletions(-) rename unittests/{testLemke.cpp => unit/test_Lemke.cpp} (73%) diff --git a/dart/lcpsolver/Lemke.cpp b/dart/lcpsolver/Lemke.cpp index e9bee0743f81b..45ac86ea4c6e3 100644 --- a/dart/lcpsolver/Lemke.cpp +++ b/dart/lcpsolver/Lemke.cpp @@ -39,8 +39,8 @@ #include #include -#include "dart/math/Helpers.h" -#include "dart/lcpsolver/Lemke.h" +#include "dart/math/Helpers.hpp" +#include "dart/lcpsolver/Lemke.hpp" #ifndef isnan # define isnan(x) \ @@ -141,10 +141,10 @@ 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) { + for (std::size_t i = 0; i < bas.size(); ++i) { B.col(i) = _M.col(bas[i]); } - for (size_t i = 0; i < nonbas.size(); ++i) { + for (std::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 @@ -162,7 +162,7 @@ if (!bas.empty()) { // 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) { + for (std::size_t i = 0; i < bas.size(); ++i) { (__z).row(bas[i]) = x.row(i); } (*_z) = __z.head(n); @@ -173,7 +173,7 @@ if (x.minCoeff() >=0 ) { Eigen::VectorXd minuxX = -x; int lvindex; double tval = minuxX.maxCoeff(&lvindex); -for (size_t i = 0; i < nonbas.size(); ++i) { +for (std::size_t i = 0; i < nonbas.size(); ++i) { bas.push_back(nonbas[i] + n); } leaving = bas[lvindex]; @@ -216,16 +216,16 @@ for (iter = 0; iter < maxiter; ++iter) { break; } - size_t jSize = j.size(); + std::size_t jSize = j.size(); Eigen::VectorXd minRatio(jSize); - for (size_t i = 0; i < jSize; ++i) { + for (std::size_t i = 0; i < jSize; ++i) { minRatio[i] = (x[j[i]] + zer_tol) / d[j[i]]; } double theta = minRatio.minCoeff(); std::vector tmpJ; std::vector tmpd; - for (size_t i = 0; i < jSize; ++i) { + for (std::size_t i = 0; i < jSize; ++i) { if (x[j[i]] / d[j[i]] <= theta) { tmpJ.push_back(j[i]); tmpd.push_back(d[j[i]]); @@ -252,7 +252,7 @@ for (iter = 0; iter < maxiter; ++iter) { lvindex = -1; // Check if artificial among these - for (size_t i = 0; i < jSize; ++i) { + for (std::size_t i = 0; i < jSize; ++i) { if (bas[j[i]] == t) lvindex = i; } @@ -262,7 +262,7 @@ for (iter = 0; iter < maxiter; ++iter) { } else { theta = tmpd[0]; lvindex = 0; - for (size_t i = 0; i < jSize; ++i) { + for (std::size_t i = 0; i < jSize; ++i) { if (tmpd[i] - theta > piv_tol) { // Bubble sorting theta = tmpd[i]; lvindex = i; @@ -299,7 +299,7 @@ if (iter >= maxiter && leaving != t) { } if (err == 0) { - for (size_t i = 0; i < bas.size(); ++i) { + for (std::size_t i = 0; i < bas.size(); ++i) { if (bas[i] < _z->size()) { (*_z)[bas[i]] = x[i]; } diff --git a/unittests/unit/CMakeLists.txt b/unittests/unit/CMakeLists.txt index d5929d4ee9dc7..d41d67b637813 100644 --- a/unittests/unit/CMakeLists.txt +++ b/unittests/unit/CMakeLists.txt @@ -2,6 +2,7 @@ dart_add_test("unit" test_Aspect) dart_add_test("unit" test_ContactConstraint) dart_add_test("unit" test_GenericJoints) dart_add_test("unit" test_Geometry) +dart_add_test("unit" test_Lemke) dart_add_test("unit" test_LocalResourceRetriever) dart_add_test("unit" test_Math) dart_add_test("unit" test_Optimizer) diff --git a/unittests/testLemke.cpp b/unittests/unit/test_Lemke.cpp similarity index 73% rename from unittests/testLemke.cpp rename to unittests/unit/test_Lemke.cpp index 0a181ba7d6ed7..357748f30e2a4 100644 --- a/unittests/testLemke.cpp +++ b/unittests/unit/test_Lemke.cpp @@ -1,8 +1,38 @@ -#include +/* + * Copyright (c) 2016, Graphics Lab, Georgia Tech Research Corporation + * Copyright (c) 2016, Humanoid Lab, Georgia Tech Research Corporation + * Copyright (c) 2016, Personal Robotics Lab, Carnegie Mellon University + * All rights reserved. + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ -#include "dart/lcpsolver/Lemke.h" -#include "TestHelpers.h" +#include +#include "dart/lcpsolver/Lemke.hpp" +#include "TestHelpers.hpp" //============================================================================== TEST(Lemke, Lemke_1D) From d52945687ec303e978c47e80c10989546cb79111 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 3 Nov 2016 02:48:56 -0400 Subject: [PATCH 4/7] Fix code style of dart/lcpsolver --- dart/lcpsolver/CMakeLists.txt | 6 +- dart/lcpsolver/Lemke.cpp | 445 ++++++++++++++++++-------------- dart/lcpsolver/Lemke.hpp | 16 +- dart/lcpsolver/ODELCPSolver.cpp | 127 +++++---- dart/lcpsolver/ODELCPSolver.hpp | 53 ++-- unittests/unit/test_Lemke.cpp | 246 +++++++++--------- 6 files changed, 492 insertions(+), 401 deletions(-) diff --git a/dart/lcpsolver/CMakeLists.txt b/dart/lcpsolver/CMakeLists.txt index 705aa17a81948..1ade679327612 100644 --- a/dart/lcpsolver/CMakeLists.txt +++ b/dart/lcpsolver/CMakeLists.txt @@ -1,8 +1,8 @@ # Search all header and source files file(GLOB hdrs "*.hpp") file(GLOB srcs "*.cpp") -dart_add_core_headers(${hdrs} ${detail_hdrs}) -dart_add_core_sources(${srcs} ${detail_srcs}) +dart_add_core_headers(${hdrs}) +dart_add_core_sources(${srcs}) # Generate header for this namespace dart_get_filename_components(header_names "lcpsolver headers" ${hdrs}) @@ -23,3 +23,5 @@ install( DESTINATION include/dart/lcpsolver COMPONENT headers ) + +dart_format_add(${hdrs} ${srcs}) diff --git a/dart/lcpsolver/Lemke.cpp b/dart/lcpsolver/Lemke.cpp index 45ac86ea4c6e3..54e754a9cc98d 100644 --- a/dart/lcpsolver/Lemke.cpp +++ b/dart/lcpsolver/Lemke.cpp @@ -39,34 +39,52 @@ #include #include -#include "dart/math/Helpers.hpp" #include "dart/lcpsolver/Lemke.hpp" +#include "dart/math/Helpers.hpp" #ifndef isnan -# define isnan(x) \ - (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; } +#define isnan(x) \ + (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_d(double _x) +{ + return _x != _x; +} -static inline int isnan_ld(long 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); } +#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_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_ld(long double _x) +{ + return !isnan(_x) && isnan(_x - _x); +} #endif @@ -89,260 +107,301 @@ 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; -} - -// 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) { + } + + // 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); -} + } -Eigen::MatrixXd B = -Eigen::MatrixXd::Identity(n, n); + Eigen::MatrixXd B = -Eigen::MatrixXd::Identity(n, n); -if (!bas.empty()) { + if (!bas.empty()) + { Eigen::MatrixXd B_copy = B; - for (std::size_t i = 0; i < bas.size(); ++i) { - B.col(i) = _M.col(bas[i]); + for (std::size_t i = 0; i < bas.size(); ++i) + { + B.col(i) = _M.col(bas[i]); } - for (std::size_t i = 0; i < nonbas.size(); ++i) { - B.col(bas.size() + i) = B_copy.col(nonbas[i]); + for (std::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; + 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); -} + } -// Check if initial basis provides solution -if (x.minCoeff() >=0 ) { + // Check if initial basis provides solution + if (x.minCoeff() >= 0) + { Eigen::VectorXd __z = Eigen::VectorXd::Zero(2 * n); - for (std::size_t i = 0; i < bas.size(); ++i) { - (__z).row(bas[i]) = x.row(i); + for (std::size_t i = 0; i < bas.size(); ++i) + { + (__z).row(bas[i]) = x.row(i); } (*_z) = __z.head(n); return err; -} - -// Determine initial leaving variable -Eigen::VectorXd minuxX = -x; -int lvindex; -double tval = minuxX.maxCoeff(&lvindex); -for (std::size_t i = 0; i < nonbas.size(); ++i) { + } + + // Determine initial leaving variable + Eigen::VectorXd minuxX = -x; + int lvindex; + double tval = minuxX.maxCoeff(&lvindex); + for (std::size_t i = 0; i < nonbas.size(); ++i) + { bas.push_back(nonbas[i] + n); -} -leaving = bas[lvindex]; + } + leaving = bas[lvindex]; -bas[lvindex] = t; // pivoting in the artificial variable + 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) { - if (leaving == t) { - break; - } else if (leaving < n) { - entering = n + leaving; - Be = Eigen::VectorXd::Zero(n); - Be[leaving] = -1; - } else { - entering = leaving - n; - Be = _M.col(entering); + 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; + } + else if (leaving < n) + { + entering = n + leaving; + Be = Eigen::VectorXd::Zero(n); + Be[leaving] = -1; + } + else + { + entering = leaving - n; + Be = _M.col(entering); } 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); + for (int i = 0; i < n; ++i) + { + if (d[i] > piv_tol) + j.push_back(i); } if (j.empty()) // no new pivots - ray termination { - err = 2; - break; + err = 2; + break; } std::size_t jSize = j.size(); Eigen::VectorXd minRatio(jSize); - for (std::size_t i = 0; i < jSize; ++i) { - minRatio[i] = (x[j[i]] + zer_tol) / d[j[i]]; + for (std::size_t i = 0; i < jSize; ++i) + { + minRatio[i] = (x[j[i]] + zer_tol) / d[j[i]]; } double theta = minRatio.minCoeff(); std::vector tmpJ; std::vector tmpd; - for (std::size_t i = 0; i < jSize; ++i) { - if (x[j[i]] / d[j[i]] <= theta) { - tmpJ.push_back(j[i]); - tmpd.push_back(d[j[i]]); - } + for (std::size_t i = 0; i < jSize; ++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!!!"; -// LOG(WARNING) << "dumping data:"; -// LOG(WARNING) << "theta:" << theta; -// for (int i = 0; i < jSize; ++i) -// { -// LOG(WARNING) << "x(" << j[i] << "): " << x[j[i]] << "d: " << d[j[i]]; -// } -// } + // if (tmpJ.empty()) + // { + // LOG(WARNING) << "tmpJ should never be empty!!!"; + // LOG(WARNING) << "dumping data:"; + // LOG(WARNING) << "theta:" << theta; + // for (int i = 0; i < jSize; ++i) + // { + // LOG(WARNING) << "x(" << j[i] << "): " << x[j[i]] << "d: " << + // d[j[i]]; + // } + // } j = tmpJ; jSize = j.size(); - if (jSize == 0) { - err = 4; - break; + if (jSize == 0) + { + err = 4; + break; } lvindex = -1; // Check if artificial among these - for (std::size_t i = 0; i < jSize; ++i) { - if (bas[j[i]] == t) - lvindex = i; + for (std::size_t i = 0; i < jSize; ++i) + { + if (bas[j[i]] == t) + lvindex = i; } - if (lvindex != -1) { - lvindex = j[lvindex]; // Always use artificial if possible - } else { - theta = tmpd[0]; - lvindex = 0; - for (std::size_t i = 0; i < jSize; ++i) { - if (tmpd[i] - theta > piv_tol) { // Bubble sorting - theta = tmpd[i]; - lvindex = i; - } + if (lvindex != -1) + { + lvindex = j[lvindex]; // Always use artificial if possible + } + else + { + theta = tmpd[0]; + lvindex = 0; + for (std::size_t i = 0; i < jSize; ++i) + { + if (tmpd[i] - theta > piv_tol) + { // Bubble sorting + theta = tmpd[i]; + lvindex = i; } - lvindex = j[lvindex]; // choose the first if there are multiple + } + 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) { - for (std::size_t i = 0; i < bas.size(); ++i) { - if (bas[i] < _z->size()) { - (*_z)[bas[i]] = x[i]; - } + if (err == 0) + { + for (std::size_t i = 0; i < bas.size(); ++i) + { + if (bas[i] < _z->size()) + { + (*_z)[bas[i]] = x[i]; + } } Eigen::VectorXd __z = _z->head(n); *_z = __z; - if (!validate(_M, *_z, _q)) { - // _z = VectorXd::Zero(n); - err = 3; + if (!validate(_M, *_z, _q)) + { + // _z = VectorXd::Zero(n); + err = 3; } -} else { - *_z = Eigen::VectorXd::Zero(n); // solve failed, return a 0 vector -} - -// if (err == 1) -// LOG(ERROR) << "LCP Solver: Iterations exceeded limit"; -// else if (err == 2) -// LOG(ERROR) << "LCP Solver: Unbounded ray"; -// else if (err == 3) -// LOG(ERROR) << "LCP Solver: Solver converged with numerical issues. " -// << "Validation failed."; -// else if (err == 4) -// LOG(ERROR) << "LCP Solver: Iteration diverged."; - -return err; + } + else + { + *_z = Eigen::VectorXd::Zero(n); // solve failed, return a 0 vector + } + + // if (err == 1) + // LOG(ERROR) << "LCP Solver: Iterations exceeded limit"; + // else if (err == 2) + // LOG(ERROR) << "LCP Solver: Unbounded ray"; + // else if (err == 3) + // LOG(ERROR) << "LCP Solver: Solver converged with numerical issues. " + // << "Validation failed."; + // else if (err == 4) + // LOG(ERROR) << "LCP Solver: Iteration diverged."; + + 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(); - -Eigen::VectorXd w = _M * _z + _q; -for (int i = 0; i < n; ++i) { +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) + { 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 -} // namespace dart +} // namespace lcpsolver +} // namespace dart diff --git a/dart/lcpsolver/Lemke.hpp b/dart/lcpsolver/Lemke.hpp index 33a52230e8b44..f03bf9fe07685 100644 --- a/dart/lcpsolver/Lemke.hpp +++ b/dart/lcpsolver/Lemke.hpp @@ -38,14 +38,16 @@ namespace dart { namespace lcpsolver { /// \brief -int Lemke(const Eigen::MatrixXd& _M, const Eigen::VectorXd& _q, - Eigen::VectorXd* _z); +int Lemke( + const Eigen::MatrixXd& _M, const Eigen::VectorXd& _q, Eigen::VectorXd* _z); /// \brief -bool validate(const Eigen::MatrixXd& _M, const Eigen::VectorXd& _z, - const Eigen::VectorXd& _q); +bool validate( + const Eigen::MatrixXd& _M, + const Eigen::VectorXd& _z, + const Eigen::VectorXd& _q); -} // namespace lcpsolver -} // namespace dart +} // namespace lcpsolver +} // namespace dart -#endif // DART_LCPSOLVER_LEMKE_HPP_ +#endif // DART_LCPSOLVER_LEMKE_HPP_ diff --git a/dart/lcpsolver/ODELCPSolver.cpp b/dart/lcpsolver/ODELCPSolver.cpp index 6c2e15301beec..516a54ecdf561 100644 --- a/dart/lcpsolver/ODELCPSolver.cpp +++ b/dart/lcpsolver/ODELCPSolver.cpp @@ -42,27 +42,34 @@ namespace dart { namespace lcpsolver { -ODELCPSolver::ODELCPSolver() { +ODELCPSolver::ODELCPSolver() +{ } -ODELCPSolver::~ODELCPSolver() { +ODELCPSolver::~ODELCPSolver() +{ } -bool ODELCPSolver::Solve(const Eigen::MatrixXd& _A, - const Eigen::VectorXd& _b, - Eigen::VectorXd* _x, - int _numContacts, - double _mu, - int _numDir, - bool _bUseODESolver) { - if (!_bUseODESolver) { +bool ODELCPSolver::Solve( + const Eigen::MatrixXd& _A, + const Eigen::VectorXd& _b, + Eigen::VectorXd* _x, + int _numContacts, + double _mu, + int _numDir, + bool _bUseODESolver) +{ + if (!_bUseODESolver) + { int err = Lemke(_A, _b, _x); return (err == 0); - } else { + } + else + { assert(_numDir >= 4); DART_UNUSED(_numDir); - double* A, *b, *x, *w, *lo, *hi; + double *A, *b, *x, *w, *lo, *hi; int n = _A.rows(); int nSkip = dPAD(n); @@ -76,18 +83,22 @@ bool ODELCPSolver::Solve(const Eigen::MatrixXd& _A, int* findex = new int[n]; memset(A, 0, n * nSkip * sizeof(double)); - for (int i = 0; i < n; ++i) { - for (int j = 0; j < n; ++j) { + for (int i = 0; i < n; ++i) + { + for (int j = 0; j < n; ++j) + { A[i * nSkip + j] = _A(i, j); } } - for (int i = 0; i < n; ++i) { + for (int i = 0; i < n; ++i) + { b[i] = -_b[i]; x[i] = w[i] = lo[i] = 0; hi[i] = dInfinity; findex[i] = -1; } - for (int i = 0; i < _numContacts; ++i) { + for (int i = 0; i < _numContacts; ++i) + { findex[_numContacts + i * 2 + 0] = i; findex[_numContacts + i * 2 + 1] = i; @@ -100,18 +111,19 @@ bool ODELCPSolver::Solve(const Eigen::MatrixXd& _A, // dClearUpperTriangle (A,n); dSolveLCP(n, A, x, b, w, 0, lo, hi, findex); -// for (int i = 0; i < n; i++) { -// if (w[i] < 0.0 && abs(x[i] - hi[i]) > 0.000001) -// cout << "w[" << i << "] is negative, but x is " << x[i] << endl; -// else if (w[i] > 0.0 && abs(x[i] - lo[i]) > 0.000001) -// cout << "w[" << i << "] is positive, but x is " << x[i] -// << " lo is " << lo[i] << endl; -// else if (abs(w[i]) < 0.000001 && (x[i] > hi[i] || x[i] < lo[i])) -// cout << "w[i] " << i << " is zero, but x is " << x[i] << endl; -// } + // for (int i = 0; i < n; i++) { + // if (w[i] < 0.0 && abs(x[i] - hi[i]) > 0.000001) + // cout << "w[" << i << "] is negative, but x is " << x[i] << endl; + // else if (w[i] > 0.0 && abs(x[i] - lo[i]) > 0.000001) + // cout << "w[" << i << "] is positive, but x is " << x[i] + // << " lo is " << lo[i] << endl; + // else if (abs(w[i]) < 0.000001 && (x[i] > hi[i] || x[i] < lo[i])) + // cout << "w[i] " << i << " is zero, but x is " << x[i] << endl; + // } *_x = Eigen::VectorXd(n); - for (int i = 0; i < n; ++i) { + for (int i = 0; i < n; ++i) + { (*_x)[i] = x[i]; } @@ -128,19 +140,22 @@ bool ODELCPSolver::Solve(const Eigen::MatrixXd& _A, } } -void ODELCPSolver::transferToODEFormulation(const Eigen::MatrixXd& _A, - const Eigen::VectorXd& _b, - Eigen::MatrixXd* _AOut, - Eigen::VectorXd* _bOut, - int _numDir, - int _numContacts) { +void ODELCPSolver::transferToODEFormulation( + const Eigen::MatrixXd& _A, + const Eigen::VectorXd& _b, + Eigen::MatrixXd* _AOut, + Eigen::VectorXd* _bOut, + int _numDir, + int _numContacts) +{ int numOtherConstrs = _A.rows() - _numContacts * (2 + _numDir); int n = _numContacts * 3 + numOtherConstrs; Eigen::MatrixXd AIntermediate = Eigen::MatrixXd::Zero(n, _A.cols()); *_AOut = Eigen::MatrixXd::Zero(n, n); *_bOut = Eigen::VectorXd::Zero(n); int offset = _numDir / 4; - for (int i = 0; i < _numContacts; ++i) { + for (int i = 0; i < _numContacts; ++i) + { AIntermediate.row(i) = _A.row(i); (*_bOut)[i] = _b[i]; @@ -149,15 +164,17 @@ void ODELCPSolver::transferToODEFormulation(const Eigen::MatrixXd& _A, AIntermediate.row(_numContacts + i * 2 + 1) = _A.row(_numContacts + i * _numDir + offset); (*_bOut)[_numContacts + i * 2 + 0] = _b[_numContacts + i * _numDir + 0]; - (*_bOut)[_numContacts + i * 2 + 1] = _b[_numContacts + i * _numDir - + offset]; + (*_bOut)[_numContacts + i * 2 + 1] = + _b[_numContacts + i * _numDir + offset]; } - for (int i = 0; i < numOtherConstrs; i++) { + for (int i = 0; i < numOtherConstrs; i++) + { AIntermediate.row(_numContacts * 3 + i) = _A.row(_numContacts * (_numDir + 2) + i); (*_bOut)[_numContacts * 3 + i] = _b[_numContacts * (_numDir + 2) + i]; } - for (int i = 0; i < _numContacts; ++i) { + for (int i = 0; i < _numContacts; ++i) + { _AOut->col(i) = AIntermediate.col(i); _AOut->col(_numContacts + i * 2 + 0) = AIntermediate.col(_numContacts + i * _numDir + 0); @@ -169,34 +186,40 @@ void ODELCPSolver::transferToODEFormulation(const Eigen::MatrixXd& _A, AIntermediate.col(_numContacts * (_numDir + 2) + i); } -void ODELCPSolver::transferSolFromODEFormulation(const Eigen::VectorXd& _x, - Eigen::VectorXd* _xOut, - int _numDir, - int _numContacts) { +void ODELCPSolver::transferSolFromODEFormulation( + const Eigen::VectorXd& _x, + Eigen::VectorXd* _xOut, + int _numDir, + int _numContacts) +{ int numOtherConstrs = _x.size() - _numContacts * 3; - *_xOut = Eigen::VectorXd::Zero(_numContacts * (2 + _numDir) - + numOtherConstrs); + *_xOut = + Eigen::VectorXd::Zero(_numContacts * (2 + _numDir) + numOtherConstrs); _xOut->head(_numContacts) = _x.head(_numContacts); int offset = _numDir / 4; - for (int i = 0; i < _numContacts; ++i) { + for (int i = 0; i < _numContacts; ++i) + { (*_xOut)[_numContacts + i * _numDir + 0] = _x[_numContacts + i * 2 + 0]; - (*_xOut)[_numContacts + i * _numDir + offset] = _x[_numContacts + i * 2 - + 1]; + (*_xOut)[_numContacts + i * _numDir + offset] = + _x[_numContacts + i * 2 + 1]; } for (int i = 0; i < numOtherConstrs; i++) (*_xOut)[_numContacts * (2 + _numDir) + i] = _x[_numContacts * 3 + i]; } -bool ODELCPSolver::checkIfSolution(const Eigen::MatrixXd& _A, - const Eigen::VectorXd& _b, - const Eigen::VectorXd& _x) { +bool ODELCPSolver::checkIfSolution( + const Eigen::MatrixXd& _A, + const Eigen::VectorXd& _b, + const Eigen::VectorXd& _x) +{ const double threshold = 1e-4; int n = _x.size(); Eigen::VectorXd w = _A * _x + _b; - for (int i = 0; i < n; ++i) { + for (int i = 0; i < n; ++i) + { if (w(i) < -threshold || _x(i) < -threshold) return false; if (std::abs(w(i) * _x(i)) > threshold) @@ -205,5 +228,5 @@ bool ODELCPSolver::checkIfSolution(const Eigen::MatrixXd& _A, return true; } -} // namespace lcpsolver -} // namespace dart +} // namespace lcpsolver +} // namespace dart diff --git a/dart/lcpsolver/ODELCPSolver.hpp b/dart/lcpsolver/ODELCPSolver.hpp index cf941102aff87..430c1a1d1fa18 100644 --- a/dart/lcpsolver/ODELCPSolver.hpp +++ b/dart/lcpsolver/ODELCPSolver.hpp @@ -38,7 +38,8 @@ namespace dart { namespace lcpsolver { /// \brief -class ODELCPSolver { +class ODELCPSolver +{ public: /// \brief ODELCPSolver(); @@ -47,36 +48,40 @@ class ODELCPSolver { virtual ~ODELCPSolver(); /// \brief - bool Solve(const Eigen::MatrixXd& _A, - const Eigen::VectorXd& _b, - Eigen::VectorXd* _x, - int numContacts, - double mu = 0, - int numDir = 0, - bool bUseODESolver = false); + bool Solve( + const Eigen::MatrixXd& _A, + const Eigen::VectorXd& _b, + Eigen::VectorXd* _x, + int numContacts, + double mu = 0, + int numDir = 0, + bool bUseODESolver = false); private: /// \brief - void transferToODEFormulation(const Eigen::MatrixXd& _A, - const Eigen::VectorXd& _b, - Eigen::MatrixXd* _AOut, - Eigen::VectorXd* _bOut, - int _numDir, - int _numContacts); + void transferToODEFormulation( + const Eigen::MatrixXd& _A, + const Eigen::VectorXd& _b, + Eigen::MatrixXd* _AOut, + Eigen::VectorXd* _bOut, + int _numDir, + int _numContacts); /// \brief - void transferSolFromODEFormulation(const Eigen::VectorXd& _x, - Eigen::VectorXd* _xOut, - int _numDir, - int _numContacts); + void transferSolFromODEFormulation( + const Eigen::VectorXd& _x, + Eigen::VectorXd* _xOut, + int _numDir, + int _numContacts); /// \brief - bool checkIfSolution(const Eigen::MatrixXd& _A, - const Eigen::VectorXd& _b, - const Eigen::VectorXd& _x); + bool checkIfSolution( + const Eigen::MatrixXd& _A, + const Eigen::VectorXd& _b, + const Eigen::VectorXd& _x); }; -} // namespace lcpsolver -} // namespace dart +} // namespace lcpsolver +} // namespace dart -#endif // DART_LCPSOLVER_ODELCPSOLVER_HPP_ +#endif // DART_LCPSOLVER_ODELCPSOLVER_HPP_ diff --git a/unittests/unit/test_Lemke.cpp b/unittests/unit/test_Lemke.cpp index 357748f30e2a4..f39cc267916db 100644 --- a/unittests/unit/test_Lemke.cpp +++ b/unittests/unit/test_Lemke.cpp @@ -33,155 +33,155 @@ #include "dart/lcpsolver/Lemke.hpp" #include "TestHelpers.hpp" - + //============================================================================== 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)); + 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)); + 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)); + 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)); + 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)); + 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(); + ::testing::InitGoogleTest(&argc,argv); + return RUN_ALL_TESTS(); } From 34ef972a4e28464d50e260597708e95a281b097a Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 3 Nov 2016 02:52:58 -0400 Subject: [PATCH 5/7] Update license block and fix code style --- dart/lcpsolver/Lemke.cpp | 17 +++++++---------- dart/lcpsolver/ODELCPSolver.cpp | 8 ++++++++ 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/dart/lcpsolver/Lemke.cpp b/dart/lcpsolver/Lemke.cpp index 54e754a9cc98d..6590a575ef620 100644 --- a/dart/lcpsolver/Lemke.cpp +++ b/dart/lcpsolver/Lemke.cpp @@ -1,15 +1,9 @@ /* - * Copyright (c) 2011-2015, Georgia Tech Research Corporation + * Copyright (c) 2011-2016, Graphics Lab, Georgia Tech Research Corporation + * Copyright (c) 2011-2016, Humanoid Lab, Georgia Tech Research Corporation + * Copyright (c) 2016, Personal Robotics Lab, Carnegie Mellon University * All rights reserved. * - * Author(s): Jie (Jay) Tan , - * Yunfei Bai - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * * This file is provided under the following "BSD-style" License: * Redistribution and use in source and binary forms, with or * without modification, are permitted provided that the following @@ -35,11 +29,12 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include "dart/lcpsolver/Lemke.hpp" + #include #include #include -#include "dart/lcpsolver/Lemke.hpp" #include "dart/math/Helpers.hpp" #ifndef isnan @@ -107,6 +102,7 @@ namespace lcpsolver { // return temp; // } +//============================================================================== int Lemke( const Eigen::MatrixXd& _M, const Eigen::VectorXd& _q, Eigen::VectorXd* _z) { @@ -384,6 +380,7 @@ int Lemke( return err; } +//============================================================================== bool validate( const Eigen::MatrixXd& _M, const Eigen::VectorXd& _z, diff --git a/dart/lcpsolver/ODELCPSolver.cpp b/dart/lcpsolver/ODELCPSolver.cpp index 516a54ecdf561..46b944570b93f 100644 --- a/dart/lcpsolver/ODELCPSolver.cpp +++ b/dart/lcpsolver/ODELCPSolver.cpp @@ -42,14 +42,19 @@ namespace dart { namespace lcpsolver { +//============================================================================== ODELCPSolver::ODELCPSolver() { + // Do nothing } +//============================================================================== ODELCPSolver::~ODELCPSolver() { + // Do nothing } +//============================================================================== bool ODELCPSolver::Solve( const Eigen::MatrixXd& _A, const Eigen::VectorXd& _b, @@ -140,6 +145,7 @@ bool ODELCPSolver::Solve( } } +//============================================================================== void ODELCPSolver::transferToODEFormulation( const Eigen::MatrixXd& _A, const Eigen::VectorXd& _b, @@ -186,6 +192,7 @@ void ODELCPSolver::transferToODEFormulation( AIntermediate.col(_numContacts * (_numDir + 2) + i); } +//============================================================================== void ODELCPSolver::transferSolFromODEFormulation( const Eigen::VectorXd& _x, Eigen::VectorXd* _xOut, @@ -209,6 +216,7 @@ void ODELCPSolver::transferSolFromODEFormulation( (*_xOut)[_numContacts * (2 + _numDir) + i] = _x[_numContacts * 3 + i]; } +//============================================================================== bool ODELCPSolver::checkIfSolution( const Eigen::MatrixXd& _A, const Eigen::VectorXd& _b, From 1ffc330257a3cd06471b779c1dda4062e6a230e8 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 3 Nov 2016 02:54:53 -0400 Subject: [PATCH 6/7] Remove unused code in Lemke.cpp --- dart/lcpsolver/Lemke.cpp | 46 ---------------------------------------- 1 file changed, 46 deletions(-) diff --git a/dart/lcpsolver/Lemke.cpp b/dart/lcpsolver/Lemke.cpp index 6590a575ef620..d78a0d5fc34d1 100644 --- a/dart/lcpsolver/Lemke.cpp +++ b/dart/lcpsolver/Lemke.cpp @@ -37,52 +37,6 @@ #include "dart/math/Helpers.hpp" -#ifndef isnan -#define isnan(x) \ - (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; -} - -#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); -} - -#endif - namespace dart { namespace lcpsolver { From 87f9420f65ef7a3b50965315261ec929d54bc390 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 3 Nov 2016 03:01:32 -0400 Subject: [PATCH 7/7] Update changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index eacec3d983850..8f5b00ac9464e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,10 @@ ### DART 6.2.0 (201X-XX-XX) +* Math + + * Fixed Lemke LCP solver (#808 for DART 6): [#812](https://github.com/dartsim/dart/pull/812) + ### DART 6.1.2 (2016-XX-XX) * Dynamics