From 342b0c9b84165ccf8eb5edbf432e77641ad4a83f Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Thu, 24 Mar 2022 08:48:35 -0700 Subject: [PATCH 1/5] return LcpResult in solveLcp --- dart/constraint/BoxedLcpConstraintSolver.cpp | 9 ++++++--- dart/constraint/BoxedLcpConstraintSolver.hpp | 8 +++++++- .../constraint/BoxedLcpConstraintSolver.cpp | 6 +++++- 3 files changed, 18 insertions(+), 5 deletions(-) diff --git a/dart/constraint/BoxedLcpConstraintSolver.cpp b/dart/constraint/BoxedLcpConstraintSolver.cpp index 6cd898a44..7a4ad989d 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.cpp +++ b/dart/constraint/BoxedLcpConstraintSolver.cpp @@ -349,7 +349,7 @@ LcpInputs BoxedLcpConstraintSolver::buildLcpInputs(ConstrainedGroup& group) } //============================================================================== -std::vector BoxedLcpConstraintSolver::solveLcp( +LcpResult BoxedLcpConstraintSolver::solveLcp( LcpInputs lcpInputs, ConstrainedGroup& group) { const std::size_t numConstraints = group.getNumConstraints(); @@ -785,7 +785,9 @@ std::vector BoxedLcpConstraintSolver::solveLcp( } constraintImpulses.push_back(mX.data() + mOffset[i]); } - return constraintImpulses; + LcpResult result; + result.impulses = constraintImpulses; + return result; } //============================================================================== @@ -793,7 +795,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..d1e67ff4b 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.hpp +++ b/dart/constraint/BoxedLcpConstraintSolver.hpp @@ -40,6 +40,12 @@ namespace dart { namespace constraint { +struct LcpResult +{ + // This is the solution to the LCP. + std::vector impulses; +}; + class BoxedLcpConstraintSolver : public ConstraintSolver { public: @@ -120,7 +126,7 @@ 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); protected: /// Boxed LCP solver diff --git a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp index 97b332266..42fc222bd 100644 --- a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp +++ b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp @@ -88,9 +88,13 @@ void BoxedLcpConstraintSolver(py::module& m) "solveLcp", +[](dart::constraint::BoxedLcpConstraintSolver* self, dart::constraint::LcpInputs lcpInputs, - dart::constraint::ConstrainedGroup& group) -> std::vector { + dart::constraint::ConstrainedGroup& group) + -> dart::constraint::LcpResult { return self->solveLcp(lcpInputs, group); }); + ::py::class_(m, "LcpResult") + .def(::py::init<>()) + .def_readwrite("impulses", &constraint::LcpResult::impulses); } } // namespace python From 0512d802343df2bb1e12f43662587769a0f2c287 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Thu, 24 Mar 2022 10:00:28 -0700 Subject: [PATCH 2/5] change impulses type to matrix --- dart/constraint/BoxedLcpConstraintSolver.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/dart/constraint/BoxedLcpConstraintSolver.hpp b/dart/constraint/BoxedLcpConstraintSolver.hpp index d1e67ff4b..7221d5717 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.hpp +++ b/dart/constraint/BoxedLcpConstraintSolver.hpp @@ -42,8 +42,9 @@ namespace constraint { struct LcpResult { - // This is the solution to the LCP. - std::vector impulses; + // This is the solution to the LCP, a matrix of impulses with shape + // (numContacts, 3). + Eigen::MatrixXs impulses; }; class BoxedLcpConstraintSolver : public ConstraintSolver From ec9ba25387a7f33797af82f91293d1d60dfe9ce8 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Fri, 25 Mar 2022 20:57:58 -0700 Subject: [PATCH 3/5] add hadToIgnoreFrictionToSolve flag --- dart/constraint/BoxedLcpConstraintSolver.cpp | 1 + dart/constraint/BoxedLcpConstraintSolver.hpp | 3 +++ .../_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp | 5 ++++- 3 files changed, 8 insertions(+), 1 deletion(-) diff --git a/dart/constraint/BoxedLcpConstraintSolver.cpp b/dart/constraint/BoxedLcpConstraintSolver.cpp index 7a4ad989d..b2e4c014b 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.cpp +++ b/dart/constraint/BoxedLcpConstraintSolver.cpp @@ -787,6 +787,7 @@ LcpResult BoxedLcpConstraintSolver::solveLcp( } LcpResult result; result.impulses = constraintImpulses; + result.hadToIgnoreFrictionToSolve = hadToIgnoreFrictionToSolve; return result; } diff --git a/dart/constraint/BoxedLcpConstraintSolver.hpp b/dart/constraint/BoxedLcpConstraintSolver.hpp index 7221d5717..f604853c2 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.hpp +++ b/dart/constraint/BoxedLcpConstraintSolver.hpp @@ -45,6 +45,9 @@ struct LcpResult // This is the solution to the LCP, a matrix of impulses with shape // (numContacts, 3). Eigen::MatrixXs impulses; + + // Whether we fell back to solving a frictionless LCP. + bool hadToIgnoreFrictionToSolve; }; class BoxedLcpConstraintSolver : public ConstraintSolver diff --git a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp index 42fc222bd..c25c27589 100644 --- a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp +++ b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp @@ -94,7 +94,10 @@ void BoxedLcpConstraintSolver(py::module& m) }); ::py::class_(m, "LcpResult") .def(::py::init<>()) - .def_readwrite("impulses", &constraint::LcpResult::impulses); + .def_readwrite("impulses", &constraint::LcpResult::impulses) + .def_readwrite( + "hadToIgnoreFrictionToSolve", + &constraint::LcpResult::hadToIgnoreFrictionToSolve); } } // namespace python From 16aedc21ece46f7176afdd7c1fe3e216853d06c3 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Fri, 25 Mar 2022 21:29:32 -0700 Subject: [PATCH 4/5] add disableFrictionlessFallback flag --- dart/constraint/BoxedLcpConstraintSolver.cpp | 141 +++++++++--------- dart/constraint/BoxedLcpConstraintSolver.hpp | 5 +- .../constraint/BoxedLcpConstraintSolver.cpp | 12 +- 3 files changed, 85 insertions(+), 73 deletions(-) diff --git a/dart/constraint/BoxedLcpConstraintSolver.cpp b/dart/constraint/BoxedLcpConstraintSolver.cpp index b2e4c014b..1650d91dd 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.cpp +++ b/dart/constraint/BoxedLcpConstraintSolver.cpp @@ -350,7 +350,9 @@ LcpInputs BoxedLcpConstraintSolver::buildLcpInputs(ConstrainedGroup& group) //============================================================================== LcpResult BoxedLcpConstraintSolver::solveLcp( - LcpInputs lcpInputs, ConstrainedGroup& group) + LcpInputs lcpInputs, + ConstrainedGroup& group, + bool disableFrictionlessFallback) { const std::size_t numConstraints = group.getNumConstraints(); const std::size_t n = group.getTotalDimension(); @@ -603,77 +605,80 @@ LcpResult 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()) diff --git a/dart/constraint/BoxedLcpConstraintSolver.hpp b/dart/constraint/BoxedLcpConstraintSolver.hpp index f604853c2..09a741a04 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.hpp +++ b/dart/constraint/BoxedLcpConstraintSolver.hpp @@ -130,7 +130,10 @@ class BoxedLcpConstraintSolver : public ConstraintSolver LcpInputs buildLcpInputs(ConstrainedGroup& group); /// Setup and solve an LCP to enforce the constraints on the ConstrainedGroup. - LcpResult 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 c25c27589..5bb82a674 100644 --- a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp +++ b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp @@ -88,10 +88,14 @@ void BoxedLcpConstraintSolver(py::module& m) "solveLcp", +[](dart::constraint::BoxedLcpConstraintSolver* self, dart::constraint::LcpInputs lcpInputs, - dart::constraint::ConstrainedGroup& group) - -> dart::constraint::LcpResult { - 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) From 1ef9ce57092a64c8c73435ffc6b53f4e6aa0d456 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Sat, 26 Mar 2022 10:12:43 -0700 Subject: [PATCH 5/5] add success attribute --- dart/constraint/BoxedLcpConstraintSolver.cpp | 1 + dart/constraint/BoxedLcpConstraintSolver.hpp | 3 +++ python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp | 1 + 3 files changed, 5 insertions(+) diff --git a/dart/constraint/BoxedLcpConstraintSolver.cpp b/dart/constraint/BoxedLcpConstraintSolver.cpp index 1650d91dd..3074dfd67 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.cpp +++ b/dart/constraint/BoxedLcpConstraintSolver.cpp @@ -791,6 +791,7 @@ LcpResult BoxedLcpConstraintSolver::solveLcp( constraintImpulses.push_back(mX.data() + mOffset[i]); } LcpResult result; + result.success = success; result.impulses = constraintImpulses; result.hadToIgnoreFrictionToSolve = hadToIgnoreFrictionToSolve; return result; diff --git a/dart/constraint/BoxedLcpConstraintSolver.hpp b/dart/constraint/BoxedLcpConstraintSolver.hpp index 09a741a04..82887ab91 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.hpp +++ b/dart/constraint/BoxedLcpConstraintSolver.hpp @@ -46,6 +46,9 @@ struct LcpResult // (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; }; diff --git a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp index 5bb82a674..9c5a89672 100644 --- a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp +++ b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp @@ -99,6 +99,7 @@ void BoxedLcpConstraintSolver(py::module& m) ::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);