Handle NaN fallback for Dantzig LCP (#2253)

Author: jslee02

dart/constraint/BoxedLcpConstraintSolver.cpp

@@ -37,6 +37,7 @@
 #include <fmt/ostream.h>
 
 #include <cassert>
+#include <cmath>
 
 #if DART_BUILD_MODE_DEBUG
   #include <iomanip>
@@ -149,6 +150,7 @@ void BoxedLcpConstraintSolver::solveConstrainedGroup(ConstrainedGroup& group)
   mA.setZero(n, nSkip);
 #endif
   mX.resize(n);
+  mX.setZero();
   mB.resize(n);
   mW.setZero(n); // set w to 0
   mLo.resize(n);
@@ -267,9 +269,11 @@ void BoxedLcpConstraintSolver::solveConstrainedGroup(ConstrainedGroup& group)
   if (success && mX.hasNaN())
     success = false;
 
+  bool fallbackSuccess = false;
+  bool fallbackRan = false;
   if (!success && mSecondaryBoxedLcpSolver) {
     DART_PROFILE_SCOPED_N("Secondary LCP");
-    mSecondaryBoxedLcpSolver->solve(
+    fallbackSuccess = mSecondaryBoxedLcpSolver->solve(
         n,
         mABackup.data(),
         mXBackup.data(),
@@ -280,15 +284,40 @@ void BoxedLcpConstraintSolver::solveConstrainedGroup(ConstrainedGroup& group)
         mFIndexBackup.data(),
         false);
     mX = mXBackup;
+    fallbackRan = true;
   }
 
-  if (mX.hasNaN()) {
-    DART_ERROR(
-        "[BoxedLcpConstraintSolver] The solution of LCP includes NAN values: "
-        "{}. We're setting it zero for safety. Consider using more robust "
-        "solver such as PGS as a secondary solver. If this happens even with "
-        "PGS solver, please report this as a bug.",
-        fmt::streamed(mX.transpose()));
+  const bool hasNaN = mX.hasNaN();
+  if (!success && fallbackRan && hasNaN) {
+    // If the fallback produced NaNs, zero just those entries but still allow
+    // non-NaN entries to propagate.
+    for (int i = 0; i < mX.size(); ++i) {
+      if (std::isnan(mX[i]))
+        mX[i] = 0.0;
+    }
+  }
+
+  // Treat a finite fallback solution as usable even if the solver reported
+  // failure to avoid discarding potentially valid impulses.
+  const bool finalSuccess
+      = success || fallbackSuccess || (fallbackRan && !mX.hasNaN());
+
+  if (!finalSuccess) {
+    if (hasNaN) {
+      DART_ERROR(
+          "[BoxedLcpConstraintSolver] The solution of LCP includes NAN values: "
+          "{}. We're setting it zero for safety. Consider using more robust "
+          "solver such as PGS as a secondary solver. If this happens even with "
+          "PGS solver, please report this as a bug.",
+          fmt::streamed(mX.transpose()));
+    } else {
+      DART_ERROR(
+          "[BoxedLcpConstraintSolver] Primary LCP solver failed to find a "
+          "solution. The constraint impulses are set to zero for safety. "
+          "Consider configuring a secondary solver (e.g., PGS) to provide a "
+          "fallback when Dantzig fails.");
+    }
+
     mX.setZero();
   }
 

dart/constraint/ConstrainedGroup.hpp

@@ -37,6 +37,8 @@
 
 #include <dart/constraint/Fwd.hpp>
 
+#include <dart/Export.hpp>
+
 #include <Eigen/Dense>
 
 #include <memory>
@@ -57,7 +59,7 @@ class ConstraintSolver;
 /// ConstrainedGroup is a group of skeletons that interact each other with
 /// constraints
 /// @sa class ConstraintSolver
-class ConstrainedGroup
+class DART_API ConstrainedGroup
 {
 public:
   //----------------------------------------------------------------------------