diff --git a/dart/constraint/BallJointConstraint.cpp b/dart/constraint/BallJointConstraint.cpp
index 5b607645e..a864e0e44 100644
--- a/dart/constraint/BallJointConstraint.cpp
+++ b/dart/constraint/BallJointConstraint.cpp
@@ -302,7 +302,7 @@ void BallJointConstraint::unexcite()
 }
 
 //==============================================================================
-void BallJointConstraint::applyImpulse(s_t* _lambda)
+void BallJointConstraint::applyImpulse(Eigen::VectorXs _lambda)
 {
   mOldX[0] = _lambda[0];
   mOldX[1] = _lambda[1];
diff --git a/dart/constraint/BallJointConstraint.hpp b/dart/constraint/BallJointConstraint.hpp
index 09e6a439e..33d752212 100644
--- a/dart/constraint/BallJointConstraint.hpp
+++ b/dart/constraint/BallJointConstraint.hpp
@@ -35,8 +35,8 @@
 
 #include <Eigen/Dense>
 
-#include "dart/math/MathTypes.hpp"
 #include "dart/constraint/JointConstraint.hpp"
+#include "dart/math/MathTypes.hpp"
 
 namespace dart {
 namespace constraint {
@@ -49,17 +49,18 @@ class BallJointConstraint : public JointConstraint
   /// Constructor that takes one body and the joint position in the world frame
   /// \param[in] _body
   /// \param[in] _jointPos Joint position expressed in world frame
-  BallJointConstraint(dynamics::BodyNode* _body,
-                      const Eigen::Vector3s& _jointPos);
-
+  BallJointConstraint(
+      dynamics::BodyNode* _body, const Eigen::Vector3s& _jointPos);
 
   /// Constructor that takes two bodies and the joint position in the frame of
   /// _body1
   /// \param[in] _body1
   /// \param[in] _body2
   /// \param[in] _jointPos Joint position expressed in world frame
-  BallJointConstraint(dynamics::BodyNode* _body1, dynamics::BodyNode* _body2,
-                      const Eigen::Vector3s& _jointPos);
+  BallJointConstraint(
+      dynamics::BodyNode* _body1,
+      dynamics::BodyNode* _body2,
+      const Eigen::Vector3s& _jointPos);
 
   /// Destructor
   virtual ~BallJointConstraint();
@@ -88,7 +89,7 @@ class BallJointConstraint : public JointConstraint
   void unexcite() override;
 
   // Documentation inherited
-  void applyImpulse(s_t* _lambda) override;
+  void applyImpulse(Eigen::VectorXs _lambda) override;
 
   // Documentation inherited
   bool isActive() const override;
@@ -128,8 +129,7 @@ class BallJointConstraint : public JointConstraint
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
-}  // namespace constraint
-}  // namespace dart
-
-#endif  // DART_CONSTRAINT_BALLJOINTCONSTRAINT_HPP_
+} // namespace constraint
+} // namespace dart
 
+#endif // DART_CONSTRAINT_BALLJOINTCONSTRAINT_HPP_
diff --git a/dart/constraint/BoxedLcpConstraintSolver.cpp b/dart/constraint/BoxedLcpConstraintSolver.cpp
index 6cd898a44..53e6d7db0 100644
--- a/dart/constraint/BoxedLcpConstraintSolver.cpp
+++ b/dart/constraint/BoxedLcpConstraintSolver.cpp
@@ -349,8 +349,10 @@ LcpInputs BoxedLcpConstraintSolver::buildLcpInputs(ConstrainedGroup& group)
 }
 
 //==============================================================================
-std::vector<s_t*> BoxedLcpConstraintSolver::solveLcp(
-    LcpInputs lcpInputs, ConstrainedGroup& group)
+LcpResult BoxedLcpConstraintSolver::solveLcp(
+    LcpInputs lcpInputs,
+    ConstrainedGroup& group,
+    bool disableFrictionlessFallback)
 {
   const std::size_t numConstraints = group.getNumConstraints();
   const std::size_t n = group.getTotalDimension();
@@ -603,77 +605,80 @@ std::vector<s_t*> BoxedLcpConstraintSolver::solveLcp(
   // like the best we can do, given the limitations of boxed LCP solvers. In the
   // long run, we should probably reformulate the LCP problem to guarantee
   // solvability.
-  if (!success)
+  if (!disableFrictionlessFallback)
   {
-    hadToIgnoreFrictionToSolve = true;
-
-    Eigen::MatrixXs mAReduced = mABackup.block(0, 0, n, n);
-    Eigen::VectorXs mXReduced = mXBackup;
-    Eigen::VectorXs mBReduced = mBBackup;
-    Eigen::VectorXs mHiReduced = mHiBackup;
-    Eigen::VectorXs mLoReduced = mLoBackup;
-    Eigen::VectorXi mFIndexReduced = mFIndexBackup;
-    Eigen::MatrixXs mapOut = LCPUtils::removeFriction(
-        mAReduced,
-        mXReduced,
-        mBReduced,
-        mHiReduced,
-        mLoReduced,
-        mFIndexReduced);
-
-    mXReduced.setZero();
-
-    int reducedN = mXReduced.size();
-    Eigen::Matrix<s_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
-        reducedAPadded = Eigen::MatrixXs::Zero(reducedN, dPAD(reducedN));
-    reducedAPadded.block(0, 0, reducedN, reducedN) = mAReduced;
-    // Prefer using PGS to Dantzig at this point, if it's available
-    if (mSecondaryBoxedLcpSolver)
-    {
-      success = mSecondaryBoxedLcpSolver->solve(
-          reducedN,
-          reducedAPadded.data(),
-          mXReduced.data(),
-          mBReduced.data(),
-          0,
-          mLoReduced.data(),
-          mHiReduced.data(),
-          mFIndexReduced.data(),
-          false);
-    }
-    else
-    {
-      success = mBoxedLcpSolver->solve(
-          reducedN,
-          reducedAPadded.data(),
-          mXReduced.data(),
-          mBReduced.data(),
-          0,
-          mLoReduced.data(),
-          mHiReduced.data(),
-          mFIndexReduced.data(),
-          true);
-    }
-    mX = mapOut * mXReduced;
-    // Don't bother checking validity at this point, because we know the
-    // solution is invalid with friction constraints, and that's ok.
-
-    /*
-#ifndef NDEBUG
-    // If we still haven't succeeded, let's debug
     if (!success)
     {
-      std::cout << "Failed to solve LCP, even after disabling friction!"
-                << std::endl;
-      std::cout << "mAReduced: " << std::endl << mAReduced << std::endl;
-      std::cout << "mBReduced: " << std::endl << mBReduced << std::endl;
-      std::cout << "mFIndexReduced: " << std::endl
-                << mFIndexReduced << std::endl;
-      std::cout << "eigenvalues: " << std::endl
-                << mAReduced.eigenvalues() << std::endl;
+      hadToIgnoreFrictionToSolve = true;
+
+      Eigen::MatrixXs mAReduced = mABackup.block(0, 0, n, n);
+      Eigen::VectorXs mXReduced = mXBackup;
+      Eigen::VectorXs mBReduced = mBBackup;
+      Eigen::VectorXs mHiReduced = mHiBackup;
+      Eigen::VectorXs mLoReduced = mLoBackup;
+      Eigen::VectorXi mFIndexReduced = mFIndexBackup;
+      Eigen::MatrixXs mapOut = LCPUtils::removeFriction(
+          mAReduced,
+          mXReduced,
+          mBReduced,
+          mHiReduced,
+          mLoReduced,
+          mFIndexReduced);
+
+      mXReduced.setZero();
+
+      int reducedN = mXReduced.size();
+      Eigen::Matrix<s_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
+          reducedAPadded = Eigen::MatrixXs::Zero(reducedN, dPAD(reducedN));
+      reducedAPadded.block(0, 0, reducedN, reducedN) = mAReduced;
+      // Prefer using PGS to Dantzig at this point, if it's available
+      if (mSecondaryBoxedLcpSolver)
+      {
+        success = mSecondaryBoxedLcpSolver->solve(
+            reducedN,
+            reducedAPadded.data(),
+            mXReduced.data(),
+            mBReduced.data(),
+            0,
+            mLoReduced.data(),
+            mHiReduced.data(),
+            mFIndexReduced.data(),
+            false);
+      }
+      else
+      {
+        success = mBoxedLcpSolver->solve(
+            reducedN,
+            reducedAPadded.data(),
+            mXReduced.data(),
+            mBReduced.data(),
+            0,
+            mLoReduced.data(),
+            mHiReduced.data(),
+            mFIndexReduced.data(),
+            true);
+      }
+      mX = mapOut * mXReduced;
+      // Don't bother checking validity at this point, because we know the
+      // solution is invalid with friction constraints, and that's ok.
+
+      /*
+  #ifndef NDEBUG
+      // If we still haven't succeeded, let's debug
+      if (!success)
+      {
+        std::cout << "Failed to solve LCP, even after disabling friction!"
+                  << std::endl;
+        std::cout << "mAReduced: " << std::endl << mAReduced << std::endl;
+        std::cout << "mBReduced: " << std::endl << mBReduced << std::endl;
+        std::cout << "mFIndexReduced: " << std::endl
+                  << mFIndexReduced << std::endl;
+        std::cout << "eigenvalues: " << std::endl
+                  << mAReduced.eigenvalues() << std::endl;
+      }
+  #endif
+      */
     }
-#endif
-    */
   }
 
   if (mX.hasNaN())
@@ -736,9 +741,7 @@ std::vector<s_t*> BoxedLcpConstraintSolver::solveLcp(
   }
 
   // Initialize the vector of constraint impulses we will eventually return.
-  // Each ith element of the vector will contain a pointer to the constraint
-  // impulse to be applied for the ith constraint.
-  std::vector<s_t*> constraintImpulses;
+  Eigen::MatrixXs constraintImpulses = Eigen::MatrixXs::Zero(numConstraints, 3);
 
   // Collect the final solved constraint impulses to apply per constraint.
   // TODO(mguo): Make impulse magnitudes all have 3 elements regardless of
@@ -748,6 +751,8 @@ std::vector<s_t*> BoxedLcpConstraintSolver::solveLcp(
     const ConstraintBasePtr& constraint = group.getConstraint(i);
     if (constraint->isContactConstraint())
     {
+      s_t* lambda = mX.data() + mOffset[i];
+      constraintImpulses(i, 0) = lambda[0];
       std::shared_ptr<ContactConstraint> contactConstraint
           = std::static_pointer_cast<ContactConstraint>(constraint);
       // getContact() returns a const, which is generally what we want, but in
@@ -766,12 +771,14 @@ std::vector<s_t*> BoxedLcpConstraintSolver::solveLcp(
           = contactConstraint->isFrictionOn();
       if (contactConstraint->isFrictionOn())
       {
+        constraintImpulses(i, 1) = lambda[1];
+        constraintImpulses(i, 2) = lambda[2];
         const_cast<collision::Contact*>(&contactConstraint->getContact())
             ->lcpResultTangent1
-            = mX(mOffset[i] + 1);
+            = lambda[1];
         const_cast<collision::Contact*>(&contactConstraint->getContact())
             ->lcpResultTangent2
-            = mX(mOffset[i] + 2);
+            = lambda[2];
         const ContactConstraint::TangentBasisMatrix D
             = contactConstraint->getTangentBasisMatrixODE(
                 contactConstraint->getContact().normal);
@@ -783,17 +790,21 @@ std::vector<s_t*> BoxedLcpConstraintSolver::solveLcp(
             = D.col(1);
       }
     }
-    constraintImpulses.push_back(mX.data() + mOffset[i]);
   }
-  return constraintImpulses;
+  LcpResult result;
+  result.success = success;
+  result.impulses = constraintImpulses;
+  result.hadToIgnoreFrictionToSolve = hadToIgnoreFrictionToSolve;
+  return result;
 }
 
 //==============================================================================
-std::vector<s_t*> BoxedLcpConstraintSolver::solveConstrainedGroup(
+Eigen::MatrixXs BoxedLcpConstraintSolver::solveConstrainedGroup(
     ConstrainedGroup& group)
 {
   LcpInputs lcpInputs = buildLcpInputs(group);
-  return solveLcp(lcpInputs, group);
+  LcpResult result = solveLcp(lcpInputs, group);
+  return result.impulses;
 }
 
 //==============================================================================
diff --git a/dart/constraint/BoxedLcpConstraintSolver.hpp b/dart/constraint/BoxedLcpConstraintSolver.hpp
index 71d520d88..485b1f522 100644
--- a/dart/constraint/BoxedLcpConstraintSolver.hpp
+++ b/dart/constraint/BoxedLcpConstraintSolver.hpp
@@ -40,6 +40,19 @@
 namespace dart {
 namespace constraint {
 
+struct LcpResult
+{
+  // This is the solution to the LCP, a matrix of impulses with shape
+  // (numContacts, 3).
+  Eigen::MatrixXs impulses;
+
+  // Whether Nimble says that it succeeded at solving the LCP.
+  bool success;
+
+  // Whether we fell back to solving a frictionless LCP.
+  bool hadToIgnoreFrictionToSolve;
+};
+
 class BoxedLcpConstraintSolver : public ConstraintSolver
 {
 public:
@@ -114,13 +127,16 @@ class BoxedLcpConstraintSolver : public ConstraintSolver
   virtual void setCachedLCPSolution(Eigen::VectorXs X) override;
 
   // Documentation inherited.
-  std::vector<s_t*> solveConstrainedGroup(ConstrainedGroup& group) override;
+  Eigen::MatrixXs solveConstrainedGroup(ConstrainedGroup& group) override;
 
   /// Build the inputs to the LCP from the constraint group.
   LcpInputs buildLcpInputs(ConstrainedGroup& group);
 
   /// Setup and solve an LCP to enforce the constraints on the ConstrainedGroup.
-  std::vector<s_t*> solveLcp(LcpInputs lcpInputs, ConstrainedGroup& group);
+  LcpResult solveLcp(
+      LcpInputs lcpInputs,
+      ConstrainedGroup& group,
+      bool disableFrictionlessFallback = false);
 
 protected:
   /// Boxed LCP solver
diff --git a/dart/constraint/ConstraintBase.hpp b/dart/constraint/ConstraintBase.hpp
index e4fe35ff2..219e8c602 100644
--- a/dart/constraint/ConstraintBase.hpp
+++ b/dart/constraint/ConstraintBase.hpp
@@ -98,7 +98,7 @@ class ConstraintBase
   virtual void unexcite() = 0;
 
   /// Apply computed constraint impulse to constrained skeletons
-  virtual void applyImpulse(s_t* lambda) = 0;
+  virtual void applyImpulse(Eigen::VectorXs lambda) = 0;
 
   /// Return true if this constraint is active
   virtual bool isActive() const = 0;
diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp
index 2af247094..cf8e153d8 100644
--- a/dart/constraint/ConstraintSolver.cpp
+++ b/dart/constraint/ConstraintSolver.cpp
@@ -804,20 +804,20 @@ void ConstraintSolver::solveConstrainedGroups()
     if (0u == n)
       continue;
 
-    std::vector<s_t*> impulses = solveConstrainedGroup(constraintGroup);
+    Eigen::MatrixXs impulses = solveConstrainedGroup(constraintGroup);
     applyConstraintImpulses(constraintGroup.getConstraints(), impulses);
   }
 }
 
 //==============================================================================
 void ConstraintSolver::applyConstraintImpulses(
-    std::vector<ConstraintBasePtr> constraints, std::vector<s_t*> impulses)
+    std::vector<ConstraintBasePtr> constraints, Eigen::MatrixXs impulses)
 {
   const std::size_t numConstraints = constraints.size();
   for (std::size_t i = 0; i < numConstraints; ++i)
   {
     const ConstraintBasePtr& constraint = constraints[i];
-    constraint->applyImpulse(impulses[i]);
+    constraint->applyImpulse(impulses.row(i));
     constraint->excite();
   }
 }
diff --git a/dart/constraint/ConstraintSolver.hpp b/dart/constraint/ConstraintSolver.hpp
index 92b48171f..4f7f0efc1 100644
--- a/dart/constraint/ConstraintSolver.hpp
+++ b/dart/constraint/ConstraintSolver.hpp
@@ -206,11 +206,11 @@ class ConstraintSolver
   void solveConstrainedGroups();
 
   // Solve for constraint impulses to apply to each constraint in group.
-  virtual std::vector<s_t*> solveConstrainedGroup(ConstrainedGroup& group) = 0;
+  virtual Eigen::MatrixXs solveConstrainedGroup(ConstrainedGroup& group) = 0;
 
   /// Apply constraint impulses to each constraint.
   void applyConstraintImpulses(
-      std::vector<ConstraintBasePtr> constraints, std::vector<s_t*> impulses);
+      std::vector<ConstraintBasePtr> constraints, Eigen::MatrixXs impulses);
 
   /// Get constrained groups.
   const std::vector<ConstrainedGroup>& getConstrainedGroups() const;
diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp
index 8a571473e..1545a2a8d 100644
--- a/dart/constraint/ContactConstraint.cpp
+++ b/dart/constraint/ContactConstraint.cpp
@@ -627,7 +627,7 @@ void ContactConstraint::unexcite()
 }
 
 //==============================================================================
-void ContactConstraint::applyImpulse(s_t* lambda)
+void ContactConstraint::applyImpulse(Eigen::VectorXs lambda)
 {
   //----------------------------------------------------------------------------
   // Friction case
diff --git a/dart/constraint/ContactConstraint.hpp b/dart/constraint/ContactConstraint.hpp
index 860bd374c..e6ae20a83 100644
--- a/dart/constraint/ContactConstraint.hpp
+++ b/dart/constraint/ContactConstraint.hpp
@@ -142,7 +142,7 @@ class ContactConstraint : public ConstraintBase
   void unexcite() override;
 
   // Documentation inherited
-  void applyImpulse(s_t* lambda) override;
+  void applyImpulse(Eigen::VectorXs lambda) override;
 
   // Documentation inherited
   dynamics::SkeletonPtr getRootSkeleton() const override;
diff --git a/dart/constraint/DantzigLCPSolver.cpp b/dart/constraint/DantzigLCPSolver.cpp
index 99f658d81..ff7ef0f34 100644
--- a/dart/constraint/DantzigLCPSolver.cpp
+++ b/dart/constraint/DantzigLCPSolver.cpp
@@ -218,8 +218,8 @@ void DantzigLCPSolver::solve(ConstrainedGroup* _group)
   for (std::size_t i = 0; i < numConstraints; ++i)
   {
     const ConstraintBasePtr& constraint = _group->getConstraint(i);
-    constraint->applyImpulse(x + offset[i]);
-    constraint->excite();
+    // constraint->applyImpulse(x + offset[i]);
+    // constraint->excite();
   }
 
   delete[] offset;
diff --git a/dart/constraint/JointCoulombFrictionConstraint.cpp b/dart/constraint/JointCoulombFrictionConstraint.cpp
index 86842cee6..0ab11e658 100644
--- a/dart/constraint/JointCoulombFrictionConstraint.cpp
+++ b/dart/constraint/JointCoulombFrictionConstraint.cpp
@@ -39,7 +39,7 @@
 #include "dart/dynamics/Joint.hpp"
 #include "dart/dynamics/Skeleton.hpp"
 
-#define DART_CFM     1e-9
+#define DART_CFM 1e-9
 
 namespace dart {
 namespace constraint {
@@ -84,13 +84,15 @@ void JointCoulombFrictionConstraint::setConstraintForceMixing(s_t _cfm)
   if (_cfm < 1e-9)
   {
     dtwarn << "Constraint force mixing parameter[" << _cfm
-           << "] is lower than 1e-9. " << "It is set to 1e-9." << std::endl;
+           << "] is lower than 1e-9. "
+           << "It is set to 1e-9." << std::endl;
     mConstraintForceMixing = 1e-9;
   }
   if (_cfm > 1.0)
   {
     dtwarn << "Constraint force mixing parameter[" << _cfm
-           << "] is greater than 1.0. " << "It is set to 1.0." << std::endl;
+           << "] is greater than 1.0. "
+           << "It is set to 1.0." << std::endl;
     mConstraintForceMixing = 1.0;
   }
 
@@ -126,7 +128,7 @@ void JointCoulombFrictionConstraint::update()
       // redundancy.
 
       // Note: Coulomb friction is force not impulse
-      mUpperBound[i] =  mJoint->getCoulombFriction(i) * timeStep;
+      mUpperBound[i] = mJoint->getCoulombFriction(i) * timeStep;
       mLowerBound[i] = -mUpperBound[i];
 
       if (mActive[i])
@@ -205,14 +207,14 @@ void JointCoulombFrictionConstraint::applyUnitImpulse(std::size_t _index)
 }
 
 //==============================================================================
-void JointCoulombFrictionConstraint::getVelocityChange(s_t* _delVel,
-                                                       bool _withCfm)
+void JointCoulombFrictionConstraint::getVelocityChange(
+    s_t* _delVel, bool _withCfm)
 {
   assert(_delVel != nullptr && "Null pointer is not allowed.");
 
   std::size_t localIndex = 0;
   std::size_t dof = mJoint->getNumDofs();
-  for (std::size_t i = 0; i < dof ; ++i)
+  for (std::size_t i = 0; i < dof; ++i)
   {
     if (mActive[i] == false)
       continue;
@@ -229,8 +231,8 @@ void JointCoulombFrictionConstraint::getVelocityChange(s_t* _delVel,
   // varaible in ODE
   if (_withCfm)
   {
-    _delVel[mAppliedImpulseIndex] += _delVel[mAppliedImpulseIndex]
-                                     * mConstraintForceMixing;
+    _delVel[mAppliedImpulseIndex]
+        += _delVel[mAppliedImpulseIndex] * mConstraintForceMixing;
   }
 
   assert(localIndex == mDim);
@@ -249,17 +251,17 @@ void JointCoulombFrictionConstraint::unexcite()
 }
 
 //==============================================================================
-void JointCoulombFrictionConstraint::applyImpulse(s_t* _lambda)
+void JointCoulombFrictionConstraint::applyImpulse(Eigen::VectorXs _lambda)
 {
   std::size_t localIndex = 0;
   std::size_t dof = mJoint->getNumDofs();
-  for (std::size_t i = 0; i < dof ; ++i)
+  for (std::size_t i = 0; i < dof; ++i)
   {
     if (mActive[i] == false)
       continue;
 
     mJoint->setConstraintImpulse(
-          i, mJoint->getConstraintImpulse(i) + _lambda[localIndex]);
+        i, mJoint->getConstraintImpulse(i) + _lambda[localIndex]);
 
     mOldX[i] = _lambda[localIndex];
 
diff --git a/dart/constraint/JointCoulombFrictionConstraint.hpp b/dart/constraint/JointCoulombFrictionConstraint.hpp
index 85a2a40c7..067f9250a 100644
--- a/dart/constraint/JointCoulombFrictionConstraint.hpp
+++ b/dart/constraint/JointCoulombFrictionConstraint.hpp
@@ -40,7 +40,7 @@ namespace dart {
 namespace dynamics {
 class BodyNode;
 class Joint;
-}  // namespace dynamics
+} // namespace dynamics
 
 namespace constraint {
 
@@ -95,7 +95,7 @@ class JointCoulombFrictionConstraint : public ConstraintBase
   void unexcite() override;
 
   // Documentation inherited
-  void applyImpulse(s_t* _lambda) override;
+  void applyImpulse(Eigen::VectorXs _lambda) override;
 
   // Documentation inherited
   dynamics::SkeletonPtr getRootSkeleton() const override;
@@ -137,8 +137,7 @@ class JointCoulombFrictionConstraint : public ConstraintBase
   static s_t mConstraintForceMixing;
 };
 
-}  // namespace constraint
-}  // namespace dart
-
-#endif  // DART_CONSTRAINT_JOINTCOULOMBFRICTIONCONSTRAINT_HPP_
+} // namespace constraint
+} // namespace dart
 
+#endif // DART_CONSTRAINT_JOINTCOULOMBFRICTIONCONSTRAINT_HPP_
diff --git a/dart/constraint/JointLimitConstraint.cpp b/dart/constraint/JointLimitConstraint.cpp
index bc785a12e..59da747f2 100644
--- a/dart/constraint/JointLimitConstraint.cpp
+++ b/dart/constraint/JointLimitConstraint.cpp
@@ -360,7 +360,7 @@ void JointLimitConstraint::unexcite()
 }
 
 //==============================================================================
-void JointLimitConstraint::applyImpulse(s_t* _lambda)
+void JointLimitConstraint::applyImpulse(Eigen::VectorXs _lambda)
 {
   std::size_t localIndex = 0;
   std::size_t dof = mJoint->getNumDofs();
diff --git a/dart/constraint/JointLimitConstraint.hpp b/dart/constraint/JointLimitConstraint.hpp
index 3afac7e42..6e230865e 100644
--- a/dart/constraint/JointLimitConstraint.hpp
+++ b/dart/constraint/JointLimitConstraint.hpp
@@ -40,7 +40,7 @@ namespace dart {
 namespace dynamics {
 class BodyNode;
 class Joint;
-}  // namespace dynamics
+} // namespace dynamics
 
 namespace constraint {
 
@@ -114,7 +114,7 @@ class JointLimitConstraint : public ConstraintBase
   void unexcite() override;
 
   // Documentation inherited
-  void applyImpulse(s_t* _lambda) override;
+  void applyImpulse(Eigen::VectorXs _lambda) override;
 
   // Documentation inherited
   dynamics::SkeletonPtr getRootSkeleton() const override;
@@ -169,8 +169,7 @@ class JointLimitConstraint : public ConstraintBase
   static s_t mConstraintForceMixing;
 };
 
-}  // namespace constraint
-}  // namespace dart
-
-#endif  // DART_CONSTRAINT_JOINTLIMITCONSTRAINT_HPP_
+} // namespace constraint
+} // namespace dart
 
+#endif // DART_CONSTRAINT_JOINTLIMITCONSTRAINT_HPP_
diff --git a/dart/constraint/LCPUtils.cpp b/dart/constraint/LCPUtils.cpp
index 5ec9601b6..88f5b20ba 100644
--- a/dart/constraint/LCPUtils.cpp
+++ b/dart/constraint/LCPUtils.cpp
@@ -9,6 +9,7 @@ namespace dart {
 namespace constraint {
 
 //==============================================================================
+/// This checks whether a solution to an LCP problem is valid.
 bool LCPUtils::isLCPSolutionValid(
     const Eigen::MatrixXs& mA,
     const Eigen::VectorXs& mX,
@@ -17,66 +18,121 @@ bool LCPUtils::isLCPSolutionValid(
     const Eigen::VectorXs& mLo,
     const Eigen::VectorXi& mFIndex,
     bool ignoreFrictionIndices)
+{
+  std::vector<LCPSolutionType> solutionTypes = getLCPSolutionTypes(
+      mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices);
+
+  for (int i = 0; i < solutionTypes.size(); i++)
+  {
+    LCPSolutionType solutionType = solutionTypes[i];
+    if (solutionType != LCPSolutionType::SUCCESS)
+      return false;
+  }
+  return true;
+}
+
+//==============================================================================
+/// This determines the solution types of an LCP problem.
+std::vector<LCPSolutionType> LCPUtils::getLCPSolutionTypes(
+    const Eigen::MatrixXs& mA,
+    const Eigen::VectorXs& mX,
+    const Eigen::VectorXs& mB,
+    const Eigen::VectorXs& mHi,
+    const Eigen::VectorXs& mLo,
+    const Eigen::VectorXi& mFIndex,
+    bool ignoreFrictionIndices)
 {
   Eigen::VectorXs v = mA * mX - mB;
+
+  std::vector<LCPSolutionType> solutionTypes;
+
   for (int i = 0; i < mX.size(); i++)
   {
-    s_t upperLimit = mHi(i);
-    s_t lowerLimit = mLo(i);
-    if (mFIndex(i) != -1)
+    LCPSolutionType solType = getLCPSolutionType(
+        i, mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices);
+    solutionTypes.push_back(solType);
+  }
+  return solutionTypes;
+}
+
+//==============================================================================
+/// This determines the type of a solution to an LCP problem.
+LCPSolutionType LCPUtils::getLCPSolutionType(
+    int i,
+    const Eigen::MatrixXs& mA,
+    const Eigen::VectorXs& mX,
+    const Eigen::VectorXs& mB,
+    const Eigen::VectorXs& mHi,
+    const Eigen::VectorXs& mLo,
+    const Eigen::VectorXi& mFIndex,
+    bool ignoreFrictionIndices)
+{
+  Eigen::VectorXs v = computeSlackFromLCPSolution(mA, mX, mB);
+  s_t upperLimit = mHi(i);
+  s_t lowerLimit = mLo(i);
+  if (mFIndex(i) != -1)
+  {
+    if (ignoreFrictionIndices)
     {
-      if (ignoreFrictionIndices)
-      {
-        if (mX(i) != 0)
-          return false;
-        continue;
-      }
-      upperLimit *= mX(mFIndex(i));
-      lowerLimit *= mX(mFIndex(i));
+      if (mX(i) != 0)
+        return LCPSolutionType::FAILURE_IGNORE_FRICTION;
+      return LCPSolutionType::SUCCESS;
     }
+    upperLimit *= mX(mFIndex(i));
+    lowerLimit *= mX(mFIndex(i));
+  }
 
-    const s_t tol = 1e-5;
+  const s_t tol = 1e-5;
 
-    /// Solves constriant impulses for a constrained group. The LCP formulation
-    /// setting that this function solve is A*x = b + w where each x[i], w[i]
-    /// satisfies one of
-    ///   (1) x = lo, w >= 0
-    ///   (2) x = hi, w <= 0
-    ///   (3) lo < x < hi, w = 0
+  /// Solves constriant impulses for a constrained group. The LCP formulation
+  /// setting that this function solve is A*x = b + w where each x[i], w[i]
+  /// satisfies one of
+  ///   (1) x = lo, w >= 0
+  ///   (2) x = hi, w <= 0
+  ///   (3) lo < x < hi, w = 0
 
-    // If force has a zero bound, and we're at a zero bound (this is common with
-    // friction being upper-bounded by a near-zero normal force) then allow
-    // velocity in either direction.
-    if (abs(lowerLimit) < tol && abs(upperLimit) < tol && abs(mX(i)) < tol)
-    {
-      // This is always allowed
-    }
-    // If force is at the lower bound, velocity must be >= 0
-    else if (abs(mX(i) - lowerLimit) < tol)
-    {
-      if (v(i) < -tol)
-        return false;
-    }
-    // If force is at the upper bound, velocity must be <= 0
-    else if (abs(mX(i) - upperLimit) < tol)
-    {
-      if (v(i) > tol)
-        return false;
-    }
-    // If force is within bounds, then velocity must be zero
-    else if (mX(i) > lowerLimit && mX(i) < upperLimit)
-    {
-      if (abs(v(i)) > tol)
-        return false;
-    }
-    // If force is out of bounds, we're always illegal
-    else
-    {
-      return false;
-    }
+  // If force has a zero bound, and we're at a zero bound (this is common with
+  // friction being upper-bounded by a near-zero normal force) then allow
+  // velocity in either direction.
+  if (abs(lowerLimit) < tol && abs(upperLimit) < tol && abs(mX(i)) < tol)
+  {
+    // This is always allowed
+  }
+  // If force is at the lower bound, velocity must be >= 0
+  else if (abs(mX(i) - lowerLimit) < tol)
+  {
+    if (v(i) < -tol)
+      return LCPSolutionType::FAILURE_LOWER_BOUND;
+  }
+  // If force is at the upper bound, velocity must be <= 0
+  else if (abs(mX(i) - upperLimit) < tol)
+  {
+    if (v(i) > tol)
+      return LCPSolutionType::FAILURE_UPPER_BOUND;
+  }
+  // If force is within bounds, then velocity must be zero
+  else if (mX(i) > lowerLimit && mX(i) < upperLimit)
+  {
+    if (abs(v(i)) > tol)
+      return LCPSolutionType::FAILURE_WITHIN_BOUNDS;
+  }
+  // If force is out of bounds, we're always illegal
+  else
+  {
+    return LCPSolutionType::FAILURE_OUT_OF_BOUNDS;
   }
   // If we make it here, the solution is fine
-  return true;
+  return LCPSolutionType::SUCCESS;
+}
+
+//==============================================================================
+/// This computes the slack variable from the LCP solution.
+Eigen::VectorXs LCPUtils::computeSlackFromLCPSolution(
+    const Eigen::MatrixXs& mA,
+    const Eigen::VectorXs& mX,
+    const Eigen::VectorXs& mB)
+{
+  return mA * mX - mB;
 }
 
 //==============================================================================
diff --git a/dart/constraint/LCPUtils.hpp b/dart/constraint/LCPUtils.hpp
index 1cfe3ec97..1556993d4 100644
--- a/dart/constraint/LCPUtils.hpp
+++ b/dart/constraint/LCPUtils.hpp
@@ -10,9 +10,20 @@
 namespace dart {
 namespace constraint {
 
+enum LCPSolutionType
+{
+  SUCCESS,
+  FAILURE_IGNORE_FRICTION,
+  FAILURE_LOWER_BOUND,
+  FAILURE_UPPER_BOUND,
+  FAILURE_WITHIN_BOUNDS,
+  FAILURE_OUT_OF_BOUNDS
+};
+
 class LCPUtils
 {
 public:
+  // This checks whether a solution to an LCP problem is valid.
   static bool isLCPSolutionValid(
       const Eigen::MatrixXs& mA,
       const Eigen::VectorXs& mX,
@@ -22,6 +33,33 @@ class LCPUtils
       const Eigen::VectorXi& mFIndex,
       bool ignoreFrictionIndices);
 
+  // This determines the solution types of an LCP problem.
+  static std::vector<LCPSolutionType> getLCPSolutionTypes(
+      const Eigen::MatrixXs& mA,
+      const Eigen::VectorXs& mX,
+      const Eigen::VectorXs& mB,
+      const Eigen::VectorXs& mHi,
+      const Eigen::VectorXs& mLo,
+      const Eigen::VectorXi& mFIndex,
+      bool ignoreFrictionIndices);
+
+  // This determines the type of a solution to an LCP problem.
+  static LCPSolutionType getLCPSolutionType(
+      int i,
+      const Eigen::MatrixXs& mA,
+      const Eigen::VectorXs& mX,
+      const Eigen::VectorXs& mB,
+      const Eigen::VectorXs& mHi,
+      const Eigen::VectorXs& mLo,
+      const Eigen::VectorXi& mFIndex,
+      bool ignoreFrictionIndices);
+
+  // This computes the slack variable from the LCP solution.
+  static Eigen::VectorXs computeSlackFromLCPSolution(
+      const Eigen::MatrixXs& mA,
+      const Eigen::VectorXs& mX,
+      const Eigen::VectorXs& mB);
+
   /// This applies a simple algorithm to guess the solution to the LCP problem.
   /// It's not guaranteed to be correct, but it often can be if there is no
   /// sliding friction on this timestep.
diff --git a/dart/constraint/MimicMotorConstraint.cpp b/dart/constraint/MimicMotorConstraint.cpp
index f99793390..16a5ad446 100644
--- a/dart/constraint/MimicMotorConstraint.cpp
+++ b/dart/constraint/MimicMotorConstraint.cpp
@@ -34,12 +34,11 @@
 
 #include <iostream>
 
-#include "dart/external/odelcpsolver/lcp.h"
-
 #include "dart/common/Console.hpp"
 #include "dart/dynamics/BodyNode.hpp"
 #include "dart/dynamics/Joint.hpp"
 #include "dart/dynamics/Skeleton.hpp"
+#include "dart/external/odelcpsolver/lcp.h"
 
 #define DART_CFM 1e-9
 
@@ -129,7 +128,7 @@ void MimicMotorConstraint::update()
   {
     s_t timeStep = mJoint->getSkeleton()->getTimeStep();
     s_t qError = mMimicJoint->getPosition(i) * mMultiplier + mOffset
-                    - mJoint->getPosition(i);
+                 - mJoint->getPosition(i);
     s_t desiredVelocity = math::clip(
         qError / timeStep,
         mJoint->getVelocityLowerLimit(i),
@@ -262,7 +261,7 @@ void MimicMotorConstraint::unexcite()
 }
 
 //==============================================================================
-void MimicMotorConstraint::applyImpulse(s_t* lambda)
+void MimicMotorConstraint::applyImpulse(Eigen::VectorXs lambda)
 {
   std::size_t localIndex = 0;
   std::size_t dof = mJoint->getNumDofs();
diff --git a/dart/constraint/MimicMotorConstraint.hpp b/dart/constraint/MimicMotorConstraint.hpp
index 05fd60782..75bf1ebe2 100644
--- a/dart/constraint/MimicMotorConstraint.hpp
+++ b/dart/constraint/MimicMotorConstraint.hpp
@@ -99,7 +99,7 @@ class MimicMotorConstraint : public ConstraintBase
   void unexcite() override;
 
   // Documentation inherited
-  void applyImpulse(s_t* lambda) override;
+  void applyImpulse(Eigen::VectorXs lambda) override;
 
   // Documentation inherited
   dynamics::SkeletonPtr getRootSkeleton() const override;
diff --git a/dart/constraint/PGSLCPSolver.cpp b/dart/constraint/PGSLCPSolver.cpp
index ef93d639c..52c18d1ac 100644
--- a/dart/constraint/PGSLCPSolver.cpp
+++ b/dart/constraint/PGSLCPSolver.cpp
@@ -185,8 +185,8 @@ void PGSLCPSolver::solve(ConstrainedGroup* _group)
   for (std::size_t i = 0; i < numConstraints; ++i)
   {
     const ConstraintBasePtr& constraint = _group->getConstraint(i);
-    constraint->applyImpulse(x + offset[i]);
-    constraint->excite();
+    // constraint->applyImpulse(x + offset[i]);
+    // constraint->excite();
   }
 
   delete[] offset;
diff --git a/dart/constraint/ServoMotorConstraint.cpp b/dart/constraint/ServoMotorConstraint.cpp
index 41ab1bc6d..090bbf356 100644
--- a/dart/constraint/ServoMotorConstraint.cpp
+++ b/dart/constraint/ServoMotorConstraint.cpp
@@ -34,14 +34,13 @@
 
 #include <iostream>
 
-#include "dart/external/odelcpsolver/lcp.h"
-
 #include "dart/common/Console.hpp"
 #include "dart/dynamics/BodyNode.hpp"
 #include "dart/dynamics/Joint.hpp"
 #include "dart/dynamics/Skeleton.hpp"
+#include "dart/external/odelcpsolver/lcp.h"
 
-#define DART_CFM     1e-9
+#define DART_CFM 1e-9
 
 namespace dart {
 namespace constraint {
@@ -86,14 +85,16 @@ void ServoMotorConstraint::setConstraintForceMixing(s_t cfm)
   {
     dtwarn << "[ServoMotorConstraint::setConstraintForceMixing] "
            << "Constraint force mixing parameter[" << cfm
-           << "] is lower than 1e-9. " << "It is set to 1e-9." << std::endl;
+           << "] is lower than 1e-9. "
+           << "It is set to 1e-9." << std::endl;
     mConstraintForceMixing = 1e-9;
   }
   if (cfm > 1.0)
   {
     dtwarn << "[ServoMotorConstraint::setConstraintForceMixing] "
            << "Constraint force mixing parameter[" << cfm
-           << "] is greater than 1.0. " << "It is set to 1.0." << std::endl;
+           << "] is greater than 1.0. "
+           << "It is set to 1.0." << std::endl;
     mConstraintForceMixing = 1.0;
   }
 
@@ -214,7 +215,7 @@ void ServoMotorConstraint::getVelocityChange(s_t* delVel, bool withCfm)
 
   std::size_t localIndex = 0;
   std::size_t dof = mJoint->getNumDofs();
-  for (std::size_t i = 0; i < dof ; ++i)
+  for (std::size_t i = 0; i < dof; ++i)
   {
     if (mActive[i] == false)
       continue;
@@ -231,8 +232,8 @@ void ServoMotorConstraint::getVelocityChange(s_t* delVel, bool withCfm)
   // varaible in ODE
   if (withCfm)
   {
-    delVel[mAppliedImpulseIndex] += delVel[mAppliedImpulseIndex]
-                                     * mConstraintForceMixing;
+    delVel[mAppliedImpulseIndex]
+        += delVel[mAppliedImpulseIndex] * mConstraintForceMixing;
   }
 
   assert(localIndex == mDim);
@@ -251,17 +252,17 @@ void ServoMotorConstraint::unexcite()
 }
 
 //==============================================================================
-void ServoMotorConstraint::applyImpulse(s_t* lambda)
+void ServoMotorConstraint::applyImpulse(Eigen::VectorXs lambda)
 {
   std::size_t localIndex = 0;
   std::size_t dof = mJoint->getNumDofs();
-  for (std::size_t i = 0; i < dof ; ++i)
+  for (std::size_t i = 0; i < dof; ++i)
   {
     if (mActive[i] == false)
       continue;
 
     mJoint->setConstraintImpulse(
-          i, mJoint->getConstraintImpulse(i) + lambda[localIndex]);
+        i, mJoint->getConstraintImpulse(i) + lambda[localIndex]);
     // TODO(JS): consider to add Joint::addConstraintImpulse()
 
     mOldX[i] = lambda[localIndex];
diff --git a/dart/constraint/ServoMotorConstraint.hpp b/dart/constraint/ServoMotorConstraint.hpp
index f5de7450e..1932affca 100644
--- a/dart/constraint/ServoMotorConstraint.hpp
+++ b/dart/constraint/ServoMotorConstraint.hpp
@@ -40,7 +40,7 @@ namespace dart {
 namespace dynamics {
 class BodyNode;
 class Joint;
-}  // namespace dynamics
+} // namespace dynamics
 
 namespace constraint {
 
@@ -95,7 +95,7 @@ class ServoMotorConstraint : public ConstraintBase
   void unexcite() override;
 
   // Documentation inherited
-  void applyImpulse(s_t* lambda) override;
+  void applyImpulse(Eigen::VectorXs lambda) override;
 
   // Documentation inherited
   dynamics::SkeletonPtr getRootSkeleton() const override;
@@ -139,8 +139,7 @@ class ServoMotorConstraint : public ConstraintBase
   static s_t mConstraintForceMixing;
 };
 
-}  // namespace constraint
-}  // namespace dart
-
-#endif  // DART_CONSTRAINT_SERVOMOTORCONSTRAINT_HPP_
+} // namespace constraint
+} // namespace dart
 
+#endif // DART_CONSTRAINT_SERVOMOTORCONSTRAINT_HPP_
diff --git a/dart/constraint/SoftContactConstraint.cpp b/dart/constraint/SoftContactConstraint.cpp
index b407c378e..cd00d9f61 100644
--- a/dart/constraint/SoftContactConstraint.cpp
+++ b/dart/constraint/SoftContactConstraint.cpp
@@ -733,7 +733,7 @@ void SoftContactConstraint::unexcite()
 }
 
 //==============================================================================
-void SoftContactConstraint::applyImpulse(s_t* _lambda)
+void SoftContactConstraint::applyImpulse(Eigen::VectorXs _lambda)
 {
   // TODO(JS): Need to optimize code
 
diff --git a/dart/constraint/SoftContactConstraint.hpp b/dart/constraint/SoftContactConstraint.hpp
index 0a9d4f188..f7d5f8dc4 100644
--- a/dart/constraint/SoftContactConstraint.hpp
+++ b/dart/constraint/SoftContactConstraint.hpp
@@ -33,23 +33,22 @@
 #ifndef DART_CONSTRAINT_SOFTCONTACTCONSTRAINT_HPP_
 #define DART_CONSTRAINT_SOFTCONTACTCONSTRAINT_HPP_
 
+#include "dart/collision/CollisionDetector.hpp"
 #include "dart/constraint/ConstraintBase.hpp"
-
 #include "dart/math/MathTypes.hpp"
-#include "dart/collision/CollisionDetector.hpp"
 
 namespace dart {
 
-namespace collision{
+namespace collision {
 class SoftCollisionInfo;
-}  // namespace collision
+} // namespace collision
 
 namespace dynamics {
 class BodyNode;
 class SoftBodyNode;
 class PointMass;
 class Skeleton;
-}  // namespace dynamics
+} // namespace dynamics
 
 namespace constraint {
 
@@ -128,7 +127,7 @@ class SoftContactConstraint : public ConstraintBase
   void unexcite() override;
 
   // Documentation inherited
-  void applyImpulse(s_t* _lambda) override;
+  void applyImpulse(Eigen::VectorXs _lambda) override;
 
   // Documentation inherited
   dynamics::SkeletonPtr getRootSkeleton() const override;
@@ -247,5 +246,4 @@ class SoftContactConstraint : public ConstraintBase
 } // namespace constraint
 } // namespace dart
 
-#endif  // DART_CONSTRAINT_SOFTCONTACTCONSTRAINT_HPP_
-
+#endif // DART_CONSTRAINT_SOFTCONTACTCONSTRAINT_HPP_
diff --git a/dart/constraint/WeldJointConstraint.cpp b/dart/constraint/WeldJointConstraint.cpp
index f7aa1fe5e..c6365523a 100644
--- a/dart/constraint/WeldJointConstraint.cpp
+++ b/dart/constraint/WeldJointConstraint.cpp
@@ -32,10 +32,9 @@
 
 #include "dart/constraint/WeldJointConstraint.hpp"
 
-#include "dart/external/odelcpsolver/lcp.h"
-
 #include "dart/dynamics/BodyNode.hpp"
 #include "dart/dynamics/Skeleton.hpp"
+#include "dart/external/odelcpsolver/lcp.h"
 
 namespace dart {
 namespace constraint {
@@ -59,11 +58,11 @@ WeldJointConstraint::WeldJointConstraint(dynamics::BodyNode* _body)
 }
 
 //==============================================================================
-WeldJointConstraint::WeldJointConstraint(dynamics::BodyNode* _body1,
-                                         dynamics::BodyNode* _body2)
+WeldJointConstraint::WeldJointConstraint(
+    dynamics::BodyNode* _body1, dynamics::BodyNode* _body2)
   : JointConstraint(_body1, _body2),
-    mRelativeTransform(_body2->getTransform().inverse()
-                       * _body1->getTransform()),
+    mRelativeTransform(
+        _body2->getTransform().inverse() * _body1->getTransform()),
     mViolation(Eigen::Vector6s::Zero()),
     mJacobian1(Eigen::Matrix6s::Identity()),
     mAppliedImpulseIndex(0)
@@ -108,18 +107,17 @@ void WeldJointConstraint::update()
   // Update Jacobian for body2
   if (mBodyNode2)
   {
-    Eigen::Isometry3s T12 = mBodyNode1->getTransform().inverse()
-                            * mBodyNode2->getTransform();
+    Eigen::Isometry3s T12
+        = mBodyNode1->getTransform().inverse() * mBodyNode2->getTransform();
     mJacobian2 = math::AdTJac(T12, mJacobian1);
   }
 
   // Update position constraint error
   if (mBodyNode2)
   {
-    const Eigen::Isometry3s& violationT
-        = mRelativeTransform.inverse()
-          * mBodyNode2->getTransform().inverse()
-          * mBodyNode1->getTransform();
+    const Eigen::Isometry3s& violationT = mRelativeTransform.inverse()
+                                          * mBodyNode2->getTransform().inverse()
+                                          * mBodyNode1->getTransform();
 
     mViolation = math::logMap(violationT);
   }
@@ -206,13 +204,15 @@ void WeldJointConstraint::applyUnitImpulse(std::size_t _index)
         if (mBodyNode2->isReactive())
         {
           mBodyNode1->getSkeleton()->updateBiasImpulse(
-                mBodyNode1, mJacobian1.row(_index),
-                mBodyNode2, -mJacobian2.row(_index));
+              mBodyNode1,
+              mJacobian1.row(_index),
+              mBodyNode2,
+              -mJacobian2.row(_index));
         }
         else
         {
           mBodyNode1->getSkeleton()->updateBiasImpulse(
-                mBodyNode1, mJacobian1.row(_index));
+              mBodyNode1, mJacobian1.row(_index));
         }
       }
       else
@@ -220,7 +220,7 @@ void WeldJointConstraint::applyUnitImpulse(std::size_t _index)
         if (mBodyNode2->isReactive())
         {
           mBodyNode2->getSkeleton()->updateBiasImpulse(
-                mBodyNode2, -mJacobian2.row(_index));
+              mBodyNode2, -mJacobian2.row(_index));
         }
         else
         {
@@ -237,7 +237,7 @@ void WeldJointConstraint::applyUnitImpulse(std::size_t _index)
       {
         mBodyNode1->getSkeleton()->clearConstraintImpulses();
         mBodyNode1->getSkeleton()->updateBiasImpulse(
-              mBodyNode1, mJacobian1.row(_index));
+            mBodyNode1, mJacobian1.row(_index));
         mBodyNode1->getSkeleton()->updateVelocityChange();
       }
 
@@ -245,7 +245,7 @@ void WeldJointConstraint::applyUnitImpulse(std::size_t _index)
       {
         mBodyNode2->getSkeleton()->clearConstraintImpulses();
         mBodyNode2->getSkeleton()->updateBiasImpulse(
-              mBodyNode2, -mJacobian2.row(_index));
+            mBodyNode2, -mJacobian2.row(_index));
         mBodyNode2->getSkeleton()->updateVelocityChange();
       }
     }
@@ -256,7 +256,7 @@ void WeldJointConstraint::applyUnitImpulse(std::size_t _index)
 
     mBodyNode1->getSkeleton()->clearConstraintImpulses();
     mBodyNode1->getSkeleton()->updateBiasImpulse(
-          mBodyNode1, mJacobian1.row(_index));
+        mBodyNode1, mJacobian1.row(_index));
     mBodyNode1->getSkeleton()->updateVelocityChange();
   }
 
@@ -270,14 +270,12 @@ void WeldJointConstraint::getVelocityChange(s_t* _vel, bool _withCfm)
   assert(isActive());
 
   Eigen::Vector6s velChange = Eigen::Vector6s::Zero();
-  if (mBodyNode1->getSkeleton()->isImpulseApplied()
-      && mBodyNode1->isReactive())
+  if (mBodyNode1->getSkeleton()->isImpulseApplied() && mBodyNode1->isReactive())
   {
     velChange += mBodyNode1->getBodyVelocityChange();
   }
 
-  if (mBodyNode2
-      && mBodyNode2->getSkeleton()->isImpulseApplied()
+  if (mBodyNode2 && mBodyNode2->getSkeleton()->isImpulseApplied()
       && mBodyNode2->isReactive())
   {
     velChange -= mJacobian2 * mBodyNode2->getBodyVelocityChange();
@@ -290,8 +288,8 @@ void WeldJointConstraint::getVelocityChange(s_t* _vel, bool _withCfm)
   // varaible in ODE
   if (_withCfm)
   {
-    _vel[mAppliedImpulseIndex] += _vel[mAppliedImpulseIndex]
-                                  * mConstraintForceMixing;
+    _vel[mAppliedImpulseIndex]
+        += _vel[mAppliedImpulseIndex] * mConstraintForceMixing;
   }
 }
 
@@ -322,7 +320,7 @@ void WeldJointConstraint::unexcite()
 }
 
 //==============================================================================
-void WeldJointConstraint::applyImpulse(s_t* _lambda)
+void WeldJointConstraint::applyImpulse(Eigen::VectorXs _lambda)
 {
   mOldX[0] = _lambda[0];
   mOldX[1] = _lambda[1];
@@ -332,12 +330,7 @@ void WeldJointConstraint::applyImpulse(s_t* _lambda)
   mOldX[5] = _lambda[5];
 
   Eigen::Vector6s imp;
-  imp << _lambda[0],
-         _lambda[1],
-         _lambda[2],
-         _lambda[3],
-         _lambda[4],
-         _lambda[5];
+  imp << _lambda[0], _lambda[1], _lambda[2], _lambda[3], _lambda[4], _lambda[5];
 
   mBodyNode1->addConstraintImpulse(imp);
 
diff --git a/dart/constraint/WeldJointConstraint.hpp b/dart/constraint/WeldJointConstraint.hpp
index 77ea6f178..5cfd7ebff 100644
--- a/dart/constraint/WeldJointConstraint.hpp
+++ b/dart/constraint/WeldJointConstraint.hpp
@@ -35,8 +35,8 @@
 
 #include <Eigen/Dense>
 
-#include "dart/math/MathTypes.hpp"
 #include "dart/constraint/JointConstraint.hpp"
+#include "dart/math/MathTypes.hpp"
 
 namespace dart {
 namespace constraint {
@@ -85,7 +85,7 @@ class WeldJointConstraint : public JointConstraint
   void unexcite() override;
 
   // Documentation inherited
-  void applyImpulse(s_t* _lambda) override;
+  void applyImpulse(Eigen::VectorXs _lambda) override;
 
   // Documentation inherited
   bool isActive() const override;
@@ -120,8 +120,7 @@ class WeldJointConstraint : public JointConstraint
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
-}  // namespace constraint
-}  // namespace dart
-
-#endif  // DART_CONSTRAINT_WELDJOINTCONSTRAINT_HPP_
+} // namespace constraint
+} // namespace dart
 
+#endif // DART_CONSTRAINT_WELDJOINTCONSTRAINT_HPP_
diff --git a/python/_nimblephysics/collision/CollisionGroup.cpp b/python/_nimblephysics/collision/CollisionGroup.cpp
index c64536420..a00b0b288 100644
--- a/python/_nimblephysics/collision/CollisionGroup.cpp
+++ b/python/_nimblephysics/collision/CollisionGroup.cpp
@@ -74,6 +74,12 @@ void CollisionGroup(py::module& m)
           +[](dart::collision::CollisionGroup* self) {
             self->addShapeFramesOf();
           })
+      .def(
+          "addShapeFramesOf",
+          +[](dart::collision::CollisionGroup* self,
+              dart::dynamics::BodyNode* bodyNode) {
+            self->addShapeFramesOf(bodyNode);
+          })
       .def(
           "subscribeTo",
           +[](dart::collision::CollisionGroup* self) { self->subscribeTo(); })
@@ -95,6 +101,12 @@ void CollisionGroup(py::module& m)
           +[](dart::collision::CollisionGroup* self) {
             self->removeShapeFramesOf();
           })
+      .def(
+          "removeShapeFramesOf",
+          +[](dart::collision::CollisionGroup* self,
+              dart::dynamics::BodyNode* bodyNode) {
+            self->removeShapeFramesOf(bodyNode);
+          })
       .def(
           "removeAllShapeFrames",
           +[](dart::collision::CollisionGroup* self) {
diff --git a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp
index 97b332266..13fe3fdfa 100644
--- a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp
+++ b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp
@@ -33,6 +33,7 @@
 #include <dart/constraint/BoxedLcpConstraintSolver.hpp>
 #include <dart/constraint/BoxedLcpSolver.hpp>
 #include <dart/constraint/ConstraintSolver.hpp>
+#include <pybind11/eigen.h>
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
 
@@ -88,9 +89,21 @@ void BoxedLcpConstraintSolver(py::module& m)
           "solveLcp",
           +[](dart::constraint::BoxedLcpConstraintSolver* self,
               dart::constraint::LcpInputs lcpInputs,
-              dart::constraint::ConstrainedGroup& group) -> std::vector<s_t*> {
-            return self->solveLcp(lcpInputs, group);
-          });
+              dart::constraint::ConstrainedGroup& group,
+              bool disableFrictionlessFallback) -> dart::constraint::LcpResult {
+            return self->solveLcp(
+                lcpInputs, group, disableFrictionlessFallback);
+          },
+          ::py::arg("lcpInputs"),
+          ::py::arg("group"),
+          ::py::arg("disableFrictionlessFallback") = false);
+  ::py::class_<dart::constraint::LcpResult>(m, "LcpResult")
+      .def(::py::init<>())
+      .def_readwrite("impulses", &constraint::LcpResult::impulses)
+      .def_readwrite("success", &constraint::LcpResult::success)
+      .def_readwrite(
+          "hadToIgnoreFrictionToSolve",
+          &constraint::LcpResult::hadToIgnoreFrictionToSolve);
 }
 
 } // namespace python
diff --git a/python/_nimblephysics/constraint/ConstraintBase.cpp b/python/_nimblephysics/constraint/ConstraintBase.cpp
index 7408ebe12..23d0f2bde 100644
--- a/python/_nimblephysics/constraint/ConstraintBase.cpp
+++ b/python/_nimblephysics/constraint/ConstraintBase.cpp
@@ -81,7 +81,7 @@ void ConstraintBase(py::module& m)
           +[](dart::constraint::ConstraintBase* self) { self->unexcite(); })
       .def(
           "applyImpulse",
-          +[](dart::constraint::ConstraintBase* self, s_t* lambda) {
+          +[](dart::constraint::ConstraintBase* self, Eigen::VectorXs lambda) {
             self->applyImpulse(lambda);
           },
           ::py::arg("lambda"))
diff --git a/python/_nimblephysics/constraint/ConstraintSolver.cpp b/python/_nimblephysics/constraint/ConstraintSolver.cpp
index 757c0fa09..22d7b5133 100644
--- a/python/_nimblephysics/constraint/ConstraintSolver.cpp
+++ b/python/_nimblephysics/constraint/ConstraintSolver.cpp
@@ -35,6 +35,7 @@
 #include <dart/constraint/ConstrainedGroup.hpp>
 #include <dart/constraint/ConstraintSolver.hpp>
 #include <dart/dynamics/Skeleton.hpp>
+#include <pybind11/eigen.h>
 #include <pybind11/functional.h>
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
@@ -201,7 +202,7 @@ void ConstraintSolver(py::module& m)
           "applyConstraintImpulses",
           +[](dart::constraint::ConstraintSolver* self,
               std::vector<dart::constraint::ConstraintBasePtr> constraints,
-              std::vector<s_t*> impulses) {
+              Eigen::MatrixXs impulses) {
             self->applyConstraintImpulses(constraints, impulses);
           })
       .def(
diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp
new file mode 100644
index 000000000..c9d74be24
--- /dev/null
+++ b/python/_nimblephysics/constraint/LCPUtils.cpp
@@ -0,0 +1,104 @@
+/*
+ * Copyright (c) 2011-2019, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/master/LICENSE
+ *
+ * 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/constraint/BoxedLcpSolver.hpp>
+#include <dart/constraint/LCPUtils.hpp>
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+namespace py = pybind11;
+
+namespace dart {
+namespace python {
+
+void LCPUtils(py::module& m)
+{
+  m.def(
+      "isLCPSolutionValid",
+      &dart::constraint::LCPUtils::isLCPSolutionValid,
+      ::py::arg("mA"),
+      ::py::arg("mX"),
+      ::py::arg("mB"),
+      ::py::arg("mHi"),
+      ::py::arg("mLo"),
+      ::py::arg("mFIndex"),
+      ::py::arg("ignoreFrictionIndices"));
+  m.def(
+      "getLCPSolutionTypes",
+      &dart::constraint::LCPUtils::getLCPSolutionTypes,
+      ::py::arg("mA"),
+      ::py::arg("mX"),
+      ::py::arg("mB"),
+      ::py::arg("mHi"),
+      ::py::arg("mLo"),
+      ::py::arg("mFIndex"),
+      ::py::arg("ignoreFrictionIndices"));
+  m.def(
+      "getLCPSolutionType",
+      &dart::constraint::LCPUtils::getLCPSolutionType,
+      ::py::arg("i"),
+      ::py::arg("mA"),
+      ::py::arg("mX"),
+      ::py::arg("mB"),
+      ::py::arg("mHi"),
+      ::py::arg("mLo"),
+      ::py::arg("mFIndex"),
+      ::py::arg("ignoreFrictionIndices"));
+  m.def(
+      "computeSlackFromLCPSolution",
+      &dart::constraint::LCPUtils::computeSlackFromLCPSolution,
+      ::py::arg("mA"),
+      ::py::arg("mX"),
+      ::py::arg("mB"));
+  ::py::enum_<dart::constraint::LCPSolutionType>(m, "LCPSolutionType")
+      .value("SUCCESS", dart::constraint::LCPSolutionType::SUCCESS)
+      .value(
+          "FAILURE_IGNORE_FRICTION",
+          dart::constraint::LCPSolutionType::FAILURE_IGNORE_FRICTION)
+      .value(
+          "FAILURE_LOWER_BOUND",
+          dart::constraint::LCPSolutionType::FAILURE_LOWER_BOUND)
+      .value(
+          "FAILURE_UPPER_BOUND",
+          dart::constraint::LCPSolutionType::FAILURE_UPPER_BOUND)
+      .value(
+          "FAILURE_WITHIN_BOUNDS",
+          dart::constraint::LCPSolutionType::FAILURE_WITHIN_BOUNDS)
+      .value(
+          "FAILURE_OUT_OF_BOUNDS",
+          dart::constraint::LCPSolutionType::FAILURE_OUT_OF_BOUNDS)
+      .export_values();
+}
+
+} // namespace python
+} // namespace dart
diff --git a/python/_nimblephysics/constraint/module.cpp b/python/_nimblephysics/constraint/module.cpp
index 825508bad..3b87d0f72 100644
--- a/python/_nimblephysics/constraint/module.cpp
+++ b/python/_nimblephysics/constraint/module.cpp
@@ -51,10 +51,7 @@ void BoxedLcpConstraintSolver(py::module& sm);
 void ConstrainedGroup(py::module& sm);
 
 void LcpInputs(py::module& sm);
-
-// void ConstraintBase(py::module& sm);
-// void ConstraintBase(py::module& sm);
-// void ConstraintBase(py::module& sm);
+void LCPUtils(py::module& sm);
 
 void dart_constraint(py::module& m)
 {
@@ -74,14 +71,7 @@ void dart_constraint(py::module& m)
   ConstrainedGroup(sm);
 
   LcpInputs(sm);
-
-  // ConstraintBase(sm);
-  // ConstraintBase(sm);
-  // ConstraintBase(sm);
-  // ConstraintBase(sm);
-  // ConstraintBase(sm);
-  // ConstraintBase(sm);
-  // ConstraintBase(sm);
+  LCPUtils(sm);
 }
 
 } // namespace python
diff --git a/python/_nimblephysics/dynamics/BodyNode.cpp b/python/_nimblephysics/dynamics/BodyNode.cpp
index 64c9f1383..c6d322ab3 100644
--- a/python/_nimblephysics/dynamics/BodyNode.cpp
+++ b/python/_nimblephysics/dynamics/BodyNode.cpp
@@ -171,6 +171,27 @@ void BodyNode(py::module& m)
           ::py::init<const dart::dynamics::detail::BodyNodeAspectProperties&>(),
           ::py::arg("aspectProperties"));
 
+  ::py::class_<dart::dynamics::BodyNode::MovingVertex>(m, "MovingVertex")
+      .def(
+          ::py::init<
+              Eigen::Vector3s,
+              Eigen::Vector3s,
+              Eigen::Vector3s,
+              dart::dynamics::BodyNode*,
+              int>(),
+          ::py::arg("pos"),
+          ::py::arg("vel"),
+          ::py::arg("accel"),
+          ::py::arg("bodyNode"),
+          ::py::arg("timestep"))
+      .def_readwrite("pos", &dart::dynamics::BodyNode::MovingVertex::pos)
+      .def_readwrite("vel", &dart::dynamics::BodyNode::MovingVertex::vel)
+      .def_readwrite("accel", &dart::dynamics::BodyNode::MovingVertex::accel)
+      .def_readwrite(
+          "bodyNode", &dart::dynamics::BodyNode::MovingVertex::bodyNode)
+      .def_readwrite(
+          "timestep", &dart::dynamics::BodyNode::MovingVertex::timestep);
+
   ::py::class_<
       dart::dynamics::TemplatedJacobianNode<dart::dynamics::BodyNode>,
       dart::dynamics::JacobianNode,
@@ -1007,6 +1028,19 @@ void BodyNode(py::module& m)
       .def(
           "removeAllShapeNodes",
           +[](dart::dynamics::BodyNode* self) { self->removeAllShapeNodes(); })
+      .def(
+          "getLocalVertices",
+          +[](const dart::dynamics::BodyNode* self)
+              -> const std::vector<Eigen::Vector3s> {
+            return self->getLocalVertices();
+          })
+      .def(
+          "getMovingVerticesInWorldSpace",
+          +[](const dart::dynamics::BodyNode* self, int timestep)
+              -> std::vector<dart::dynamics::BodyNode::MovingVertex> {
+            return self->getMovingVerticesInWorldSpace(timestep);
+          },
+          ::py::arg("timestep") = -1)
       .def(
           "getNumEndEffectors",
           +[](const dart::dynamics::BodyNode* self) -> std::size_t {
diff --git a/unittests/unit/test_ConstraintSolver.cpp b/unittests/unit/test_ConstraintSolver.cpp
index ab0cd3e52..b17d2c405 100644
--- a/unittests/unit/test_ConstraintSolver.cpp
+++ b/unittests/unit/test_ConstraintSolver.cpp
@@ -70,7 +70,7 @@ TEST(ConstraintSolver, SIMPLE)
   // Solve constraint impulses.
   for (auto constraintGroup : solver->getConstrainedGroups())
   {
-    std::vector<s_t*> impulses = solver->solveConstrainedGroup(constraintGroup);
+    Eigen::MatrixXs impulses = solver->solveConstrainedGroup(constraintGroup);
     EXPECT_TRUE(impulses.size() > 0);
     solver->applyConstraintImpulses(constraintGroup.getConstraints(), impulses);
   }