diff --git a/dart/constraint/BoxedLcpConstraintSolver.cpp b/dart/constraint/BoxedLcpConstraintSolver.cpp index 6cd898a44..3074dfd67 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.cpp +++ b/dart/constraint/BoxedLcpConstraintSolver.cpp @@ -349,8 +349,10 @@ LcpInputs BoxedLcpConstraintSolver::buildLcpInputs(ConstrainedGroup& group) } //============================================================================== -std::vector 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 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 - 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 + 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,7 +790,11 @@ std::vector BoxedLcpConstraintSolver::solveLcp( } constraintImpulses.push_back(mX.data() + mOffset[i]); } - return constraintImpulses; + LcpResult result; + result.success = success; + result.impulses = constraintImpulses; + result.hadToIgnoreFrictionToSolve = hadToIgnoreFrictionToSolve; + return result; } //============================================================================== @@ -793,7 +802,8 @@ std::vector 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..82887ab91 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: @@ -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 solveLcp(LcpInputs lcpInputs, ConstrainedGroup& group); + LcpResult solveLcp( + LcpInputs lcpInputs, + ConstrainedGroup& group, + bool disableFrictionlessFallback = false); protected: /// Boxed LCP solver diff --git a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp index 97b332266..9c5a89672 100644 --- a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp +++ b/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 { - 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_(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