mguo/my-master-20220324

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];

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_

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;
 }
 
 //==============================================================================

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

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;

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();
   }
 }

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;

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

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;