Skip to content

Commit

Permalink
Merge remote-tracking branch 'origin/release-5.1'
Browse files Browse the repository at this point in the history 
Conflicts:
	CHANGELOG.md
	dart/lcpsolver/Lemke.cpp
	debian/changelog
  • Loading branch information
@jslee02
jslee02 committed Jan 20, 2017
2 parents e2d5249 + 11437f5 commit 6ab8227
Show file tree
Hide file tree
Showing 3 changed files with 172 additions and 1 deletion.
8 changes: 7 additions & 1 deletion CHANGELOG.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -198,7 +198,13 @@

## DART 5

### Version 5.1.5 (201X-XX-XX)
### Version 5.1.5 (2017-01-20)

1. Fixed Lemke LCP solver for several failing cases
* [Pull request #808](https://github.com/dartsim/dart/pull/808)

1. Increase minimum required Ipopt version to 3.11.9
* [Pull request #800](https://github.com/dartsim/dart/pull/800)

1. Added support of urdfdom_headers 1.0 for DART 5.1 (backport of [#766](https://github.com/dartsim/dart/pull/766))
* [Pull request #799](https://github.com/dartsim/dart/pull/799)
Expand Down
8 changes: 8 additions & 0 deletions debian/changelog
View file
Original file line number Diff line number Diff line change
Expand Up @@ -121,6 +121,14 @@ dart6 (6.0.0) unstable; urgency=medium

-- Jeongseok Lee <jslee02@gmail.com> Tue, 10 May 2016 12:00:00 -0500

dart5 (5.1.5) unstable; urgency=medium

* Added support of urdfdom_headers 1.0 for DART 5.1 (backport of #766)
* Increase minimum required Ipopt version to 3.11.9
* Fixed Lemke LCP solver for several failing cases

-- Jeongseok Lee <jslee02@gmail.com> Fri, 20 Jan 2017 23:59:00 -0500

dart5 (5.1.4) unstable; urgency=medium

* Fixed inconsistent frame rate of GlutWindow
Expand Down
157 changes: 157 additions & 0 deletions unittests/testLemke.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,157 @@
#include <gtest/gtest.h>

#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();
}

0 comments on commit 6ab8227

Please sign in to comment.