From cae33a3c4809be843343b70677d7ea418a7c09e7 Mon Sep 17 00:00:00 2001 From: yang-lab Date: Thu, 27 Oct 2016 20:13:15 -0400 Subject: [PATCH] Fix another Lemke bug: tmpMinRatio ==> tmpd Provide unittest instead of another apps example --- apps/Lemke_fix/CMakeLists.txt | 7 -- apps/Lemke_fix/main.cpp | 111 ------------------------ dart/lcpsolver/Lemke.cpp | 28 +++--- unittests/testLemke.cpp | 157 ++++++++++++++++++++++++++++++++++ 4 files changed, 171 insertions(+), 132 deletions(-) delete mode 100644 apps/Lemke_fix/CMakeLists.txt delete mode 100644 apps/Lemke_fix/main.cpp create mode 100644 unittests/testLemke.cpp diff --git a/apps/Lemke_fix/CMakeLists.txt b/apps/Lemke_fix/CMakeLists.txt deleted file mode 100644 index 9da3fb8017eb5..0000000000000 --- a/apps/Lemke_fix/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -############################################### -# apps/Lemke_Fix -file(GLOB Lemke_Fix_srcs "*.cpp") -file(GLOB Lemke_Fix_hdrs "*.h") -add_executable(Lemke_Fix ${Lemke_Fix_srcs} ${Lemke_Fix_hdrs}) -target_link_libraries(Lemke_Fix dart) -set_target_properties(Lemke_Fix PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin") diff --git a/apps/Lemke_fix/main.cpp b/apps/Lemke_fix/main.cpp deleted file mode 100644 index cfd8efc57401e..0000000000000 --- a/apps/Lemke_fix/main.cpp +++ /dev/null @@ -1,111 +0,0 @@ -#include - -#include "dart/dart.h" -#include "dart/lcpsolver/Lemke.h" - -int main(int argc, char* argv[]) -{ - Eigen::MatrixXd testA; - Eigen::VectorXd testb; - Eigen::VectorXd* f; - int err; - - f = new Eigen::VectorXd(1); - testA.resize(1,1); - testA << 1; - testb.resize(1); - testb<< -1.5; - std::cout<<"Matrix A is"< 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(); +}