return LcpResult in solveLcp

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())
@@ -785,15 +790,20 @@ std::vector<s_t*> BoxedLcpConstraintSolver::solveLcp(
     }
     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(
     ConstrainedGroup& group)
 {
   LcpInputs lcpInputs = buildLcpInputs(group);
-  return solveLcp(lcpInputs, group);
+  LcpResult result = solveLcp(lcpInputs, group);
+  return result.impulses;
 }
 
 //==============================================================================

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:
@@ -120,7 +133,10 @@ class BoxedLcpConstraintSolver : public ConstraintSolver
   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

python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp

@@ -88,9 +88,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