diff --git a/CHANGELOG.md b/CHANGELOG.md
index e3bed2aacaf79..0d6b344a8c038 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)
+
 * Misc
 
   * Added CMake targets for code formatting using clang-format: [#811](https://github.com/dartsim/dart/pull/811)
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 d22b5b4a57129..d78a0d5fc34d1 100644
--- a/dart/lcpsolver/Lemke.cpp
+++ b/dart/lcpsolver/Lemke.cpp
@@ -29,35 +29,13 @@
  *   POSSIBILITY OF SUCH DAMAGE.
  */
 
+#include "dart/lcpsolver/Lemke.hpp"
+
 #include <cmath>
 #include <iostream>
 #include <vector>
 
 #include "dart/math/Helpers.hpp"
-#include "dart/lcpsolver/Lemke.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 {
@@ -78,8 +56,10 @@ namespace lcpsolver {
 //  return temp;
 // }
 
-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)
+{
   int n = _q.size();
 
   const double zer_tol = 1e-5;
@@ -87,48 +67,101 @@ int Lemke(const Eigen::MatrixXd& _M, const Eigen::VectorXd& _q,
   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;
+  int leaving = 0;
   Eigen::VectorXd Be = Eigen::VectorXd::Constant(n, 1);
   Eigen::VectorXd x = _q;
   std::vector<int> bas;
   std::vector<int> nonbas;
 
-  int t = 2 * n + 1;
+  int t = 2 * n;
   int entering = t;
 
   bas.clear();
   nonbas.clear();
 
-  for (int i = 0; i < n; ++i) {
-    bas.push_back(i);
+  // 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);
 
-  for (std::size_t i = 0; i < bas.size(); ++i) {
-    B.col(bas[i]) = _M.col(bas[i]);
+  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 < nonbas.size(); ++i)
+    {
+      B.col(bas.size() + i) = B_copy.col(nonbas[i]);
+    }
+    // TODO: check the condition number to return err = 3
+    Eigen::JacobiSVD<Eigen::MatrixXd> 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);
   }
 
-  x = -B.partialPivLu().solve(_q);
+  // 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);
+    }
+    (*_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)
+  {
+    bas.push_back(nonbas[i] + n);
+  }
   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) {
+  for (int i = 0; i < n; ++i)
+  {
     if (x[i] < 0)
       U[i] = 1;
   }
@@ -137,98 +170,122 @@ int Lemke(const Eigen::MatrixXd& _M, const Eigen::VectorXd& _q,
   x[lvindex] = tval;
   B.col(lvindex) = Be;
 
-  for (iter = 0; iter < maxiter; ++iter) {
-    if (leaving == t) {
+  for (iter = 0; iter < maxiter; ++iter)
+  {
+    if (leaving == t)
+    {
       break;
-    } else if (leaving < n) {
+    }
+    else if (leaving < n)
+    {
       entering = n + leaving;
       Be = Eigen::VectorXd::Zero(n);
       Be[leaving] = -1;
-    } else {
+    }
+    else
+    {
       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<int> j;
-    for (int i = 0; i < n; ++i) {
+    for (int i = 0; i < n; ++i)
+    {
       if (d[i] > piv_tol)
         j.push_back(i);
     }
-    if (j.empty()) {
-      // err = 2;
+    if (j.empty()) // no new pivots - ray termination
+    {
+      err = 2;
       break;
     }
 
     std::size_t jSize = j.size();
     Eigen::VectorXd minRatio(jSize);
-    for (std::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<int> tmpJ;
-    std::vector<double> tmpMinRatio;
-    for (std::size_t i = 0; i < jSize; ++i) {
-      if (x[j[i]] / d[j[i]] <= theta) {
+    std::vector<double> tmpd;
+    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]);
+        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) {
+    if (jSize == 0)
+    {
       err = 4;
       break;
     }
     lvindex = -1;
 
-    for (std::size_t i = 0; i < jSize; ++i) {
+    // Check if artificial among these
+    for (std::size_t i = 0; i < jSize; ++i)
+    {
       if (bas[j[i]] == t)
         lvindex = i;
     }
-    if (lvindex != -1) {
-      lvindex = j[lvindex];
-    } else {
-      theta = tmpMinRatio[0];
-      lvindex = 0;
 
-      for (std::size_t i = 0; i < jSize; ++i) {
-        if (tmpMinRatio[i]-theta > piv_tol) {
-          theta = tmpMinRatio[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];
+      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;
@@ -236,46 +293,59 @@ int Lemke(const Eigen::MatrixXd& _M, const Eigen::VectorXd& _q,
   }
 
   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()) {
+  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 realZ = _z->segment(0, n);
-    *_z = realZ;
+    Eigen::VectorXd __z = _z->head(n);
+    *_z = __z;
 
-    if (!validate(_M, *_z, _q)) {
+    if (!validate(_M, *_z, _q))
+    {
       // _z = VectorXd::Zero(n);
       err = 3;
     }
-  } else {
-    *_z = Eigen::VectorXd::Zero(n);  // solve failed, return a 0 vector
+  }
+  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.";
+  //  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) {
+//==============================================================================
+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) {
+  for (int i = 0; i < n; ++i)
+  {
     if (w(i) < -threshold || _z(i) < -threshold)
       return false;
     if (std::abs(w(i) * _z(i)) > threshold)
@@ -284,5 +354,5 @@ bool validate(const Eigen::MatrixXd& _M, const Eigen::VectorXd& _z,
   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..46b944570b93f 100644
--- a/dart/lcpsolver/ODELCPSolver.cpp
+++ b/dart/lcpsolver/ODELCPSolver.cpp
@@ -42,27 +42,39 @@
 namespace dart {
 namespace lcpsolver {
 
-ODELCPSolver::ODELCPSolver() {
+//==============================================================================
+ODELCPSolver::ODELCPSolver()
+{
+  // Do nothing
 }
 
-ODELCPSolver::~ODELCPSolver() {
+//==============================================================================
+ODELCPSolver::~ODELCPSolver()
+{
+  // Do nothing
 }
 
-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 +88,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 +116,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 +145,23 @@ 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 +170,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 +192,42 @@ 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 +236,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/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/unit/test_Lemke.cpp b/unittests/unit/test_Lemke.cpp
new file mode 100644
index 0000000000000..f39cc267916db
--- /dev/null
+++ b/unittests/unit/test_Lemke.cpp
@@ -0,0 +1,187 @@
+/*
+ * 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 <gtest/gtest.h>
+
+#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));
+}
+
+//==============================================================================
+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();
+}