Allow BoxedLcpConstraintSolver to have secondary LCP solver (#1265)

### Problem
Dantzig LCP solver sometimes returns inaccurate solution or even NAN values when the LCP doesn't have a solution. More importantly, `BoxedLCPConstraintSolver` uses the solution without any sanity checks, which easily leads to unstable simulation. This is reported in #892.

### Solution
Let `BoxedLCPConstraintSolver` to use more robust LCP solver such as PGS when it fails to solve with the primary LCP solver. In addition to that, as a final resort, it discards NAN value solution when the secondary LCP solver failed as well.

### Testing
Added the regression test provided by @pchorak. Thanks!

***

**Before creating a pull request**

- [x] Document new methods and classes
- [x] Format new code files using `clang-format`

**Before merging a pull request**

- [x] Set version target by selecting a milestone on the right side
- [x] Summarize this change in `CHANGELOG.md`
- [x] Add unit test(s) for this change

CHANGELOG.md

@@ -7,6 +7,7 @@
 * Dynamics
 
   * Fixed incorrect transpose check in Inertia::verifySpatialTensor(): [#1258](https://github.com/dartsim/dart/pull/1258)
+  * Allowed BoxedLcpConstraintSolver to have a secondary LCP solver: [#1265](https://github.com/dartsim/dart/pull/1265)
 
 * Simulation
 

dart/collision/CollisionDetector.hpp

@@ -76,7 +76,7 @@ class CollisionDetector : public std::enable_shared_from_this<CollisionDetector>
 
   /// \brief Create a clone of this CollisionDetector. All the properties will
   /// be copied over, but not collision objects.
-  virtual std::shared_ptr<CollisionDetector> cloneWithoutCollisionObjects() = 0;
+  virtual std::shared_ptr<CollisionDetector> cloneWithoutCollisionObjects() const = 0;
 
   /// Return collision detection engine type as a std::string
   virtual const std::string& getType() const = 0;

dart/collision/bullet/BulletCollisionDetector.cpp

@@ -109,7 +109,7 @@ BulletCollisionDetector::~BulletCollisionDetector()
 
 //==============================================================================
 std::shared_ptr<CollisionDetector>
-BulletCollisionDetector::cloneWithoutCollisionObjects()
+BulletCollisionDetector::cloneWithoutCollisionObjects() const
 {
   return BulletCollisionDetector::create();
 }

dart/collision/bullet/BulletCollisionDetector.hpp

@@ -61,7 +61,8 @@ class BulletCollisionDetector : public CollisionDetector
   virtual ~BulletCollisionDetector();
 
   // Documentation inherited
-  std::shared_ptr<CollisionDetector> cloneWithoutCollisionObjects() override;
+  std::shared_ptr<CollisionDetector> cloneWithoutCollisionObjects() const
+  override;
 
   // Documentation inherited
   const std::string& getType() const override;

dart/collision/dart/DARTCollisionDetector.cpp

@@ -74,7 +74,7 @@ std::shared_ptr<DARTCollisionDetector> DARTCollisionDetector::create()
 
 //==============================================================================
 std::shared_ptr<CollisionDetector>
-DARTCollisionDetector::cloneWithoutCollisionObjects()
+DARTCollisionDetector::cloneWithoutCollisionObjects() const
 {
   return DARTCollisionDetector::create();
 }

dart/collision/dart/DARTCollisionDetector.hpp

@@ -49,7 +49,8 @@ class DARTCollisionDetector : public CollisionDetector
   static std::shared_ptr<DARTCollisionDetector> create();
 
   // Documentation inherited
-  std::shared_ptr<CollisionDetector> cloneWithoutCollisionObjects() override;
+  std::shared_ptr<CollisionDetector> cloneWithoutCollisionObjects() const
+  override;
 
   // Documentation inherited
   const std::string& getType() const override;

dart/collision/fcl/FCLCollisionDetector.cpp

@@ -667,7 +667,7 @@ FCLCollisionDetector::~FCLCollisionDetector()
 
 //==============================================================================
 std::shared_ptr<CollisionDetector>
-FCLCollisionDetector::cloneWithoutCollisionObjects()
+FCLCollisionDetector::cloneWithoutCollisionObjects() const
 {
   return FCLCollisionDetector::create();
 }

dart/collision/fcl/FCLCollisionDetector.hpp

@@ -84,7 +84,8 @@ class FCLCollisionDetector : public CollisionDetector
   virtual ~FCLCollisionDetector();
 
   // Documentation inherited
-  std::shared_ptr<CollisionDetector> cloneWithoutCollisionObjects() override;
+  std::shared_ptr<CollisionDetector> cloneWithoutCollisionObjects() const
+  override;
 
   // Documentation inherited
   const std::string& getType() const override;

dart/collision/ode/OdeCollisionDetector.cpp

@@ -127,7 +127,7 @@ OdeCollisionDetector::~OdeCollisionDetector()
 
 //==============================================================================
 std::shared_ptr<CollisionDetector>
-OdeCollisionDetector::cloneWithoutCollisionObjects()
+OdeCollisionDetector::cloneWithoutCollisionObjects() const
 {
   return OdeCollisionDetector::create();
 }

dart/collision/ode/OdeCollisionDetector.hpp

@@ -62,7 +62,8 @@ class OdeCollisionDetector : public CollisionDetector
   virtual ~OdeCollisionDetector();
 
   // Documentation inherited
-  std::shared_ptr<CollisionDetector> cloneWithoutCollisionObjects() override;
+  std::shared_ptr<CollisionDetector> cloneWithoutCollisionObjects() const
+  override;
 
   // Documentation inherited
   const std::string& getType() const override;

dart/constraint/BoxedLcpConstraintSolver.cpp

@@ -41,21 +41,59 @@
 #include "dart/external/odelcpsolver/lcp.h"
 
 #include "dart/common/Console.hpp"
-#include "dart/constraint/ConstrainedGroup.hpp"
 #include "dart/constraint/ConstraintBase.hpp"
 #include "dart/constraint/DantzigBoxedLcpSolver.hpp"
+#include "dart/constraint/PgsBoxedLcpSolver.hpp"
 #include "dart/lcpsolver/Lemke.hpp"
 
 namespace dart {
 namespace constraint {
 
 //==============================================================================
 BoxedLcpConstraintSolver::BoxedLcpConstraintSolver(
-    double timeStep, BoxedLcpSolverPtr boxedLcpSolver)
-  : ConstraintSolver(timeStep), mBoxedLcpSolver(std::move(boxedLcpSolver))
+    double timeStep,
+    BoxedLcpSolverPtr boxedLcpSolver,
+    BoxedLcpSolverPtr secondaryBoxedLcpSolver)
+  : BoxedLcpConstraintSolver(
+        std::move(boxedLcpSolver), std::move(secondaryBoxedLcpSolver))
 {
-  if (!mBoxedLcpSolver)
-    mBoxedLcpSolver = std::make_shared<DantzigBoxedLcpSolver>();
+  setTimeStep(timeStep);
+}
+
+//==============================================================================
+BoxedLcpConstraintSolver::BoxedLcpConstraintSolver()
+  : BoxedLcpConstraintSolver(std::make_shared<DantzigBoxedLcpSolver>())
+{
+  // Do nothing
+}
+
+//==============================================================================
+BoxedLcpConstraintSolver::BoxedLcpConstraintSolver(
+    BoxedLcpSolverPtr boxedLcpSolver)
+  : BoxedLcpConstraintSolver(
+        std::move(boxedLcpSolver), std::make_shared<PgsBoxedLcpSolver>())
+{
+  // Do nothing
+}
+
+//==============================================================================
+BoxedLcpConstraintSolver::BoxedLcpConstraintSolver(
+    BoxedLcpSolverPtr boxedLcpSolver, BoxedLcpSolverPtr secondaryBoxedLcpSolver)
+  : ConstraintSolver()
+{
+  if (boxedLcpSolver)
+  {
+    setBoxedLcpSolver(std::move(boxedLcpSolver));
+  }
+  else
+  {
+    dtwarn << "[BoxedLcpConstraintSolver] Attempting to construct with nullptr "
+           << "LCP solver, which is not allowed. Using Dantzig solver "
+           << "instead.\n";
+    setBoxedLcpSolver(std::make_shared<DantzigBoxedLcpSolver>());
+  }
+
+  setSecondaryBoxedLcpSolver(std::move(secondaryBoxedLcpSolver));
 }
 
 //==============================================================================
@@ -68,6 +106,13 @@ void BoxedLcpConstraintSolver::setBoxedLcpSolver(BoxedLcpSolverPtr lcpSolver)
     return;
   }
 
+  if (lcpSolver == mSecondaryBoxedLcpSolver)
+  {
+    dtwarn << "[BoxedLcpConstraintSolver::setBoxedLcpSolver] Attempting to set "
+           << "a primary LCP solver that is the same with the secondary LCP "
+           << "solver, which is discouraged. Ignoring this request.\n";
+  }
+
   mBoxedLcpSolver = std::move(lcpSolver);
 }
 
@@ -77,6 +122,28 @@ ConstBoxedLcpSolverPtr BoxedLcpConstraintSolver::getBoxedLcpSolver() const
   return mBoxedLcpSolver;
 }
 
+//==============================================================================
+void BoxedLcpConstraintSolver::setSecondaryBoxedLcpSolver(
+    BoxedLcpSolverPtr lcpSolver)
+{
+  if (lcpSolver == mBoxedLcpSolver)
+  {
+    dtwarn << "[BoxedLcpConstraintSolver::setBoxedLcpSolver] Attempting to set "
+           << "the secondary LCP solver that is identical to the primary LCP "
+           << "solver, which is redundant. Please use different solvers or set "
+           << "the secondary LCP solver to nullptr.\n";
+  }
+
+  mSecondaryBoxedLcpSolver = std::move(lcpSolver);
+}
+
+//==============================================================================
+ConstBoxedLcpSolverPtr BoxedLcpConstraintSolver::getSecondaryBoxedLcpSolver()
+    const
+{
+  return mSecondaryBoxedLcpSolver;
+}
+
 //==============================================================================
 void BoxedLcpConstraintSolver::solveConstrainedGroup(ConstrainedGroup& group)
 {
@@ -173,17 +240,61 @@ void BoxedLcpConstraintSolver::solveConstrainedGroup(ConstrainedGroup& group)
   //  print(n, A, x, lo, hi, b, w, findex);
   //  std::cout << std::endl;
 
-  // Solve LCP using ODE's Dantzig algorithm
+  // Solve LCP using the primary solver and fallback to secondary solver when
+  // the parimary solver failed.
+  if (mSecondaryBoxedLcpSolver)
+  {
+    // Make backups for the secondary LCP solver because the primary solver
+    // modifies the original terms.
+    mABackup = mA;
+    mXBackup = mX;
+    mBBackup = mB;
+    mLoBackup = mLo;
+    mHiBackup = mHi;
+    mFIndexBackup = mFIndex;
+  }
+  const bool earlyTermination = (mSecondaryBoxedLcpSolver != nullptr);
   assert(mBoxedLcpSolver);
-  mBoxedLcpSolver->solve(
+  bool success = mBoxedLcpSolver->solve(
       n,
       mA.data(),
       mX.data(),
       mB.data(),
       0,
       mLo.data(),
       mHi.data(),
-      mFIndex.data());
+      mFIndex.data(),
+      earlyTermination);
+
+  // Sanity check. LCP solvers should not report success with nan values, but
+  // it could happen. So we set the sucees to false for nan values.
+  if (success && mX.hasNaN())
+    success = false;
+
+  if (!success && mSecondaryBoxedLcpSolver)
+  {
+    mSecondaryBoxedLcpSolver->solve(
+        n,
+        mABackup.data(),
+        mXBackup.data(),
+        mBBackup.data(),
+        0,
+        mLoBackup.data(),
+        mHiBackup.data(),
+        mFIndexBackup.data(),
+        false);
+    mX = mXBackup;
+  }
+
+  if (mX.hasNaN())
+  {
+    dterr << "[BoxedLcpConstraintSolver] The solution of LCP includes NAN "
+          << "values: " << mX.transpose() << ". 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.\n";
+    mX.setZero();
+  }
 
   // Print LCP formulation
   //  dtdbg << "After solve:" << std::endl;

dart/constraint/BoxedLcpConstraintSolver.hpp

@@ -43,43 +43,114 @@ class BoxedLcpConstraintSolver : public ConstraintSolver
 {
 public:
   /// Constructor
+  ///
+  /// \param[in] timeStep Simulation time step
+  /// \param[in] boxedLcpSolver The primary boxed LCP solver. When nullptr is
+  /// passed, Dantzig solver will be used.
+  /// \param[in] secondaryBoxedLcpSolver The secondary boxed-LCP solver. When
+  /// nullptr is passed, PGS solver will be used. This is to make the default
+  /// solver setting to be Dantzig + PGS. In order to disable use of secondary
+  /// solver, call setSecondaryBoxedLcpSolver(nullptr) explicitly.
+  ///
+  /// \deprecated Deprecated in DART 6.8. Please use other constructors that
+  /// doesn't take timespte. Timestep should be set by the owner of this solver
+  /// such as dart::simulation::World when the solver added.
+  DART_DEPRECATED(6.8)
   BoxedLcpConstraintSolver(
-      double timeStep, BoxedLcpSolverPtr boxedLcpSolver = nullptr);
+      double timeStep,
+      BoxedLcpSolverPtr boxedLcpSolver,
+      BoxedLcpSolverPtr secondaryBoxedLcpSolver);
+
+  /// Constructos with default primary and secondary LCP solvers, which are
+  /// Dantzig and PGS, respectively.
+  BoxedLcpConstraintSolver();
+
+  /// Constructors with specific primary LCP solver.
+  ///
+  /// \param[in] boxedLcpSolver The primary boxed LCP solver. When nullptr is
+  /// passed, which is discouraged, Dantzig solver will be used.
+  BoxedLcpConstraintSolver(BoxedLcpSolverPtr boxedLcpSolver);
+
+  /// Constructs with specific primary and secondary LCP solvers.
+  ///
+  /// \param[in] boxedLcpSolver The primary boxed LCP solver. When nullptr is
+  /// passed, which is discouraged, Dantzig solver will be used.
+  /// \param[in] secondaryBoxedLcpSolver The secondary boxed-LCP solver. Pass
+  /// nullptr to disable using secondary LCP solver.
+  BoxedLcpConstraintSolver(
+      BoxedLcpSolverPtr boxedLcpSolver,
+      BoxedLcpSolverPtr secondaryBoxedLcpSolver);
 
   /// Sets boxed LCP (BLCP) solver
+  ///
+  /// \param[in] boxedLcpSolver The primary boxed LCP solver. When nullptr is
+  /// passed, Dantzig solver will be used.
   void setBoxedLcpSolver(BoxedLcpSolverPtr lcpSolver);
 
   /// Returns boxed LCP (BLCP) solver
   ConstBoxedLcpSolverPtr getBoxedLcpSolver() const;
 
+  /// Sets boxed LCP (BLCP) solver that is used when the primary solver failed
+  void setSecondaryBoxedLcpSolver(BoxedLcpSolverPtr lcpSolver);
+
+  /// Returns boxed LCP (BLCP) solver that is used when the primary solver
+  /// failed
+  ConstBoxedLcpSolverPtr getSecondaryBoxedLcpSolver() const;
+
 protected:
   // Documentation inherited.
   void solveConstrainedGroup(ConstrainedGroup& group) override;
 
   /// Boxed LCP solver
   BoxedLcpSolverPtr mBoxedLcpSolver;
+  // TODO(JS): Hold as unique_ptr because there is no reason to share. Make this
+  // change in DART 7 because it's API breaking change.
+
+  /// Boxed LCP solver to be used when the primary solver failed
+  BoxedLcpSolverPtr mSecondaryBoxedLcpSolver;
+  // TODO(JS): Hold as unique_ptr because there is no reason to share. Make this
+  // change in DART 7 because it's API breaking change.
 
   /// Cache data for boxed LCP formulation
   Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> mA;
 
+  /// Cache data for boxed LCP formulation
+  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
+      mABackup;
+
   /// Cache data for boxed LCP formulation
   Eigen::VectorXd mX;
 
+  /// Cache data for boxed LCP formulation
+  Eigen::VectorXd mXBackup;
+
   /// Cache data for boxed LCP formulation
   Eigen::VectorXd mB;
 
+  /// Cache data for boxed LCP formulation
+  Eigen::VectorXd mBBackup;
+
   /// Cache data for boxed LCP formulation
   Eigen::VectorXd mW;
 
   /// Cache data for boxed LCP formulation
   Eigen::VectorXd mLo;
 
+  /// Cache data for boxed LCP formulation
+  Eigen::VectorXd mLoBackup;
+
   /// Cache data for boxed LCP formulation
   Eigen::VectorXd mHi;
 
+  /// Cache data for boxed LCP formulation
+  Eigen::VectorXd mHiBackup;
+
   /// Cache data for boxed LCP formulation
   Eigen::VectorXi mFIndex;
 
+  /// Cache data for boxed LCP formulation
+  Eigen::VectorXi mFIndexBackup;
+
   /// Cache data for boxed LCP formulation
   Eigen::VectorXi mOffset;