From 3b3a55cc071a01be923175da10c7089487d70d85 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Tue, 22 Mar 2022 12:40:07 -0700 Subject: [PATCH 01/22] Create LCPUtils.cpp under python/_nimblephysics/constraint. --- python/_nimblephysics/constraint/LCPUtils.cpp | 48 +++++++++++++++++++ 1 file changed, 48 insertions(+) create mode 100644 python/_nimblephysics/constraint/LCPUtils.cpp diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp new file mode 100644 index 000000000..49d7692a1 --- /dev/null +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -0,0 +1,48 @@ +/* + * Copyright (c) 2011-2019, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include + +namespace py = pybind11; + +namespace dart { +namespace python { + +void LCPUtils(py::module& m) +{ + ::py::class_(m, "LCPUtils"); +} + +} // namespace python +} // namespace dart From baa48e436616a77e22791fb946d43a316a2221fc Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Tue, 22 Mar 2022 12:44:14 -0700 Subject: [PATCH 02/22] fix typo --- python/_nimblephysics/constraint/LCPUtils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index 49d7692a1..4c9f477a6 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -41,7 +41,7 @@ namespace python { void LCPUtils(py::module& m) { - ::py::class_(m, "LCPUtils"); + ::py::class_(m, "LCPUtils"); } } // namespace python From 76062e44a2d15a6969ec1e81ca71bf38fecc4e21 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Tue, 22 Mar 2022 12:50:35 -0700 Subject: [PATCH 03/22] add lcp utils to module.cpp --- python/_nimblephysics/constraint/module.cpp | 14 ++------------ 1 file changed, 2 insertions(+), 12 deletions(-) diff --git a/python/_nimblephysics/constraint/module.cpp b/python/_nimblephysics/constraint/module.cpp index 825508bad..3b87d0f72 100644 --- a/python/_nimblephysics/constraint/module.cpp +++ b/python/_nimblephysics/constraint/module.cpp @@ -51,10 +51,7 @@ void BoxedLcpConstraintSolver(py::module& sm); void ConstrainedGroup(py::module& sm); void LcpInputs(py::module& sm); - -// void ConstraintBase(py::module& sm); -// void ConstraintBase(py::module& sm); -// void ConstraintBase(py::module& sm); +void LCPUtils(py::module& sm); void dart_constraint(py::module& m) { @@ -74,14 +71,7 @@ void dart_constraint(py::module& m) ConstrainedGroup(sm); LcpInputs(sm); - - // ConstraintBase(sm); - // ConstraintBase(sm); - // ConstraintBase(sm); - // ConstraintBase(sm); - // ConstraintBase(sm); - // ConstraintBase(sm); - // ConstraintBase(sm); + LCPUtils(sm); } } // namespace python From 64d7d0f0f408641b8c9c547485815f18558ae7e9 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Tue, 22 Mar 2022 12:59:02 -0700 Subject: [PATCH 04/22] import missing header file --- python/_nimblephysics/constraint/LCPUtils.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index 4c9f477a6..625ff9382 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -31,6 +31,7 @@ */ #include +#include #include #include @@ -41,7 +42,9 @@ namespace python { void LCPUtils(py::module& m) { - ::py::class_(m, "LCPUtils"); + ::py::class_< + dart::constraint::LCPUtils, + std::shared_ptr >(m, "LCPUtils"); } } // namespace python From cec39068ccc9981ee9f6b3cb7ca601ae7bc338fb Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Tue, 22 Mar 2022 13:15:14 -0700 Subject: [PATCH 05/22] add binding for isLcpSolutionValid --- python/_nimblephysics/constraint/LCPUtils.cpp | 22 ++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index 625ff9382..c6ffb38a7 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -44,7 +44,27 @@ void LCPUtils(py::module& m) { ::py::class_< dart::constraint::LCPUtils, - std::shared_ptr >(m, "LCPUtils"); + std::shared_ptr >(m, "LCPUtils") + .def( + "isLcpSolutionValid", + +[](dart::constraint::LCPUtils* self, + const Eigen::MatrixXs& mA, + const Eigen::VectorXs& mX, + const Eigen::VectorXs& mB, + const Eigen::VectorXs& mHi, + const Eigen::VectorXs& mLo, + const Eigen::VectorXi& mFIndex, + bool ignoreFrictionIndices) { + self->isLcpSolutionValid( + mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); + }, + ::py::arg("A"), + ::py::arg("x"), + ::py::arg("b"), + ::py::arg("hi"), + ::py::arg("lo"), + ::py::arg("fIndex"), + ::py::arg("ignoreFrictionIndices")); } } // namespace python From b2ef95a468982b8de67bd8d4da7d959d09f5b72d Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Tue, 22 Mar 2022 13:16:49 -0700 Subject: [PATCH 06/22] fix typo --- python/_nimblephysics/constraint/LCPUtils.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index c6ffb38a7..cb84026da 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -46,7 +46,7 @@ void LCPUtils(py::module& m) dart::constraint::LCPUtils, std::shared_ptr >(m, "LCPUtils") .def( - "isLcpSolutionValid", + "isLCPSolutionValid", +[](dart::constraint::LCPUtils* self, const Eigen::MatrixXs& mA, const Eigen::VectorXs& mX, @@ -55,7 +55,7 @@ void LCPUtils(py::module& m) const Eigen::VectorXs& mLo, const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices) { - self->isLcpSolutionValid( + self->isLCPSolutionValid( mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); }, ::py::arg("A"), From 27635b4a740eeac7d74f8c0e32eac08b5343e175 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Tue, 22 Mar 2022 16:46:57 -0700 Subject: [PATCH 07/22] add default args and return type --- python/_nimblephysics/constraint/LCPUtils.cpp | 30 +++++++++---------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index cb84026da..d808eaf6c 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -48,23 +48,23 @@ void LCPUtils(py::module& m) .def( "isLCPSolutionValid", +[](dart::constraint::LCPUtils* self, - const Eigen::MatrixXs& mA, - const Eigen::VectorXs& mX, - const Eigen::VectorXs& mB, - const Eigen::VectorXs& mHi, - const Eigen::VectorXs& mLo, - const Eigen::VectorXi& mFIndex, - bool ignoreFrictionIndices) { - self->isLCPSolutionValid( + Eigen::MatrixXs mA, + Eigen::VectorXs mX, + Eigen::VectorXs mB, + Eigen::VectorXs mHi, + Eigen::VectorXs mLo, + Eigen::VectorXi mFIndex, + bool ignoreFrictionIndices) -> bool { + return self->isLCPSolutionValid( mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); }, - ::py::arg("A"), - ::py::arg("x"), - ::py::arg("b"), - ::py::arg("hi"), - ::py::arg("lo"), - ::py::arg("fIndex"), - ::py::arg("ignoreFrictionIndices")); + ::py::arg("A") = Eigen::MatrixXs::Zero(3, 4), + ::py::arg("x") = Eigen::VectorXs::Zero(3), + ::py::arg("b") = Eigen::VectorXs::Zero(3), + ::py::arg("hi") = Eigen::VectorXs::Zero(3), + ::py::arg("lo") = Eigen::VectorXs::Zero(3), + ::py::arg("fIndex") = Eigen::VectorXi::Zero(3), + ::py::arg("ignoreFrictionIndices") = true); } } // namespace python From 73c97f1f40b42f4439839596111807ba548cb0be Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Tue, 22 Mar 2022 17:17:06 -0700 Subject: [PATCH 08/22] get isLcpSolutionValid() to run in python without errors --- python/_nimblephysics/constraint/LCPUtils.cpp | 33 ++++++------------- 1 file changed, 10 insertions(+), 23 deletions(-) diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index d808eaf6c..259674071 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -42,29 +42,16 @@ namespace python { void LCPUtils(py::module& m) { - ::py::class_< - dart::constraint::LCPUtils, - std::shared_ptr >(m, "LCPUtils") - .def( - "isLCPSolutionValid", - +[](dart::constraint::LCPUtils* self, - Eigen::MatrixXs mA, - Eigen::VectorXs mX, - Eigen::VectorXs mB, - Eigen::VectorXs mHi, - Eigen::VectorXs mLo, - Eigen::VectorXi mFIndex, - bool ignoreFrictionIndices) -> bool { - return self->isLCPSolutionValid( - mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); - }, - ::py::arg("A") = Eigen::MatrixXs::Zero(3, 4), - ::py::arg("x") = Eigen::VectorXs::Zero(3), - ::py::arg("b") = Eigen::VectorXs::Zero(3), - ::py::arg("hi") = Eigen::VectorXs::Zero(3), - ::py::arg("lo") = Eigen::VectorXs::Zero(3), - ::py::arg("fIndex") = Eigen::VectorXi::Zero(3), - ::py::arg("ignoreFrictionIndices") = true); + m.def( + "isLCPSolutionValid", + &dart::constraint::LCPUtils::isLCPSolutionValid, + ::py::arg("mA"), + ::py::arg("mX"), + ::py::arg("mB"), + ::py::arg("mHi"), + ::py::arg("mLo"), + ::py::arg("mFIndex"), + ::py::arg("ignoreFrictionIndices")); } } // namespace python From a3608e558046107a36e4af4d70b551401f8f48d4 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 23 Mar 2022 17:46:10 -0700 Subject: [PATCH 09/22] create and call empty getLCPSolutionType function --- dart/constraint/LCPUtils.cpp | 9 +++++++++ dart/constraint/LCPUtils.hpp | 4 ++++ 2 files changed, 13 insertions(+) diff --git a/dart/constraint/LCPUtils.cpp b/dart/constraint/LCPUtils.cpp index 5ec9601b6..0e9e269c9 100644 --- a/dart/constraint/LCPUtils.cpp +++ b/dart/constraint/LCPUtils.cpp @@ -9,6 +9,7 @@ namespace dart { namespace constraint { //============================================================================== +/// This checks whether a solution to an LCP problem is valid. bool LCPUtils::isLCPSolutionValid( const Eigen::MatrixXs& mA, const Eigen::VectorXs& mX, @@ -18,6 +19,7 @@ bool LCPUtils::isLCPSolutionValid( const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices) { + getLCPSolutionType(); Eigen::VectorXs v = mA * mX - mB; for (int i = 0; i < mX.size(); i++) { @@ -79,6 +81,13 @@ bool LCPUtils::isLCPSolutionValid( return true; } +//============================================================================== +/// This checks whether a solution to an LCP problem is valid. +void LCPUtils::getLCPSolutionType() +{ + // Doesn't do anything, yet. +} + //============================================================================== /// This applies a simple algorithm to guess the solution to the LCP problem. /// It's not guaranteed to be correct, but it often can be if there is no diff --git a/dart/constraint/LCPUtils.hpp b/dart/constraint/LCPUtils.hpp index 1cfe3ec97..c6015c4fa 100644 --- a/dart/constraint/LCPUtils.hpp +++ b/dart/constraint/LCPUtils.hpp @@ -13,6 +13,7 @@ namespace constraint { class LCPUtils { public: + // This checks whether a solution to an LCP problem is valid. static bool isLCPSolutionValid( const Eigen::MatrixXs& mA, const Eigen::VectorXs& mX, @@ -22,6 +23,9 @@ class LCPUtils const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices); + // This determines the type of a solution to an LCP problem. + static void getLCPSolutionType(); + /// This applies a simple algorithm to guess the solution to the LCP problem. /// It's not guaranteed to be correct, but it often can be if there is no /// sliding friction on this timestep. From 1eff5eec519362f0321eb60809521d3542bf7bc2 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 23 Mar 2022 18:06:17 -0700 Subject: [PATCH 10/22] add LcpSolutionType enum --- dart/constraint/LCPUtils.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/dart/constraint/LCPUtils.hpp b/dart/constraint/LCPUtils.hpp index c6015c4fa..990ec9219 100644 --- a/dart/constraint/LCPUtils.hpp +++ b/dart/constraint/LCPUtils.hpp @@ -10,6 +10,12 @@ namespace dart { namespace constraint { +enum LcpSolutionType +{ + SUCCESS, + FAILURE +}; + class LCPUtils { public: From 19e6daece7080f63dd5879b4203681aae49fdaa8 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 23 Mar 2022 18:17:08 -0700 Subject: [PATCH 11/22] make getLCPSolutionType return a LCPSolutionType --- dart/constraint/LCPUtils.cpp | 4 ++-- dart/constraint/LCPUtils.hpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/dart/constraint/LCPUtils.cpp b/dart/constraint/LCPUtils.cpp index 0e9e269c9..a60fe20f7 100644 --- a/dart/constraint/LCPUtils.cpp +++ b/dart/constraint/LCPUtils.cpp @@ -83,9 +83,9 @@ bool LCPUtils::isLCPSolutionValid( //============================================================================== /// This checks whether a solution to an LCP problem is valid. -void LCPUtils::getLCPSolutionType() +LCPSolutionType LCPUtils::getLCPSolutionType() { - // Doesn't do anything, yet. + return LCPSolutionType::FAILURE; } //============================================================================== diff --git a/dart/constraint/LCPUtils.hpp b/dart/constraint/LCPUtils.hpp index 990ec9219..b2f2c1517 100644 --- a/dart/constraint/LCPUtils.hpp +++ b/dart/constraint/LCPUtils.hpp @@ -10,7 +10,7 @@ namespace dart { namespace constraint { -enum LcpSolutionType +enum LCPSolutionType { SUCCESS, FAILURE @@ -30,7 +30,7 @@ class LCPUtils bool ignoreFrictionIndices); // This determines the type of a solution to an LCP problem. - static void getLCPSolutionType(); + static LCPSolutionType getLCPSolutionType(); /// This applies a simple algorithm to guess the solution to the LCP problem. /// It's not guaranteed to be correct, but it often can be if there is no From 77658e94f640a89052ec3d63f769f1239cb37ed7 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 23 Mar 2022 18:22:21 -0700 Subject: [PATCH 12/22] add python binding for getLCPSolutionType --- python/_nimblephysics/constraint/LCPUtils.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index 259674071..a74368fe5 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -52,6 +52,7 @@ void LCPUtils(py::module& m) ::py::arg("mLo"), ::py::arg("mFIndex"), ::py::arg("ignoreFrictionIndices")); + m.def("getLCPSolutionType", &dart::constraint::LCPUtils::getLCPSolutionType); } } // namespace python From 4bfca7253692cdc1a4b56febd0a8f219a752421b Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 23 Mar 2022 18:34:09 -0700 Subject: [PATCH 13/22] add python binding for LCPSolutionType enum --- python/_nimblephysics/constraint/LCPUtils.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index a74368fe5..ae9250315 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -53,6 +53,10 @@ void LCPUtils(py::module& m) ::py::arg("mFIndex"), ::py::arg("ignoreFrictionIndices")); m.def("getLCPSolutionType", &dart::constraint::LCPUtils::getLCPSolutionType); + ::py::enum_(m, "LCPSolutionType") + .value("SUCCESS", dart::constraint::LCPSolutionType::SUCCESS) + .value("FAILURE", dart::constraint::LCPSolutionType::FAILURE) + .export_values(); } } // namespace python From 3537a16e6565d0f882e7bb0e9e34b0f094a1adc9 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 23 Mar 2022 18:47:55 -0700 Subject: [PATCH 14/22] add arguments for getLCPSolutionType --- dart/constraint/LCPUtils.cpp | 18 ++++++++++++++++-- dart/constraint/LCPUtils.hpp | 9 ++++++++- python/_nimblephysics/constraint/LCPUtils.cpp | 11 ++++++++++- 3 files changed, 34 insertions(+), 4 deletions(-) diff --git a/dart/constraint/LCPUtils.cpp b/dart/constraint/LCPUtils.cpp index a60fe20f7..d8f788c2a 100644 --- a/dart/constraint/LCPUtils.cpp +++ b/dart/constraint/LCPUtils.cpp @@ -19,7 +19,7 @@ bool LCPUtils::isLCPSolutionValid( const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices) { - getLCPSolutionType(); + getLCPSolutionType(mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); Eigen::VectorXs v = mA * mX - mB; for (int i = 0; i < mX.size(); i++) { @@ -83,8 +83,22 @@ bool LCPUtils::isLCPSolutionValid( //============================================================================== /// This checks whether a solution to an LCP problem is valid. -LCPSolutionType LCPUtils::getLCPSolutionType() +LCPSolutionType LCPUtils::getLCPSolutionType( + const Eigen::MatrixXs& mA, + const Eigen::VectorXs& mX, + const Eigen::VectorXs& mB, + const Eigen::VectorXs& mHi, + const Eigen::VectorXs& mLo, + const Eigen::VectorXi& mFIndex, + bool ignoreFrictionIndices) { + Eigen::MatrixXs mANew = mA; + Eigen::VectorXs mXNew = mX; + Eigen::VectorXs mBNew = mB; + Eigen::VectorXs mHiNew = mHi; + Eigen::VectorXs mLoNew = mLo; + Eigen::VectorXi mFIndexNew = mFIndex; + ignoreFrictionIndices = !ignoreFrictionIndices; return LCPSolutionType::FAILURE; } diff --git a/dart/constraint/LCPUtils.hpp b/dart/constraint/LCPUtils.hpp index b2f2c1517..5927790fc 100644 --- a/dart/constraint/LCPUtils.hpp +++ b/dart/constraint/LCPUtils.hpp @@ -30,7 +30,14 @@ class LCPUtils bool ignoreFrictionIndices); // This determines the type of a solution to an LCP problem. - static LCPSolutionType getLCPSolutionType(); + static LCPSolutionType getLCPSolutionType( + const Eigen::MatrixXs& mA, + const Eigen::VectorXs& mX, + const Eigen::VectorXs& mB, + const Eigen::VectorXs& mHi, + const Eigen::VectorXs& mLo, + const Eigen::VectorXi& mFIndex, + bool ignoreFrictionIndices); /// This applies a simple algorithm to guess the solution to the LCP problem. /// It's not guaranteed to be correct, but it often can be if there is no diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index ae9250315..5c3505cdc 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -52,7 +52,16 @@ void LCPUtils(py::module& m) ::py::arg("mLo"), ::py::arg("mFIndex"), ::py::arg("ignoreFrictionIndices")); - m.def("getLCPSolutionType", &dart::constraint::LCPUtils::getLCPSolutionType); + m.def( + "getLCPSolutionType", + &dart::constraint::LCPUtils::getLCPSolutionType, + ::py::arg("mA"), + ::py::arg("mX"), + ::py::arg("mB"), + ::py::arg("mHi"), + ::py::arg("mLo"), + ::py::arg("mFIndex"), + ::py::arg("ignoreFrictionIndices")); ::py::enum_(m, "LCPSolutionType") .value("SUCCESS", dart::constraint::LCPSolutionType::SUCCESS) .value("FAILURE", dart::constraint::LCPSolutionType::FAILURE) From 7bb0e68a7b20dd75fed5d707f3bd334b152ef710 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 23 Mar 2022 18:56:20 -0700 Subject: [PATCH 15/22] copy function body, assign return value to a variable --- dart/constraint/LCPUtils.cpp | 71 +++++++++++++++++++++++++++++++----- 1 file changed, 62 insertions(+), 9 deletions(-) diff --git a/dart/constraint/LCPUtils.cpp b/dart/constraint/LCPUtils.cpp index d8f788c2a..90282868b 100644 --- a/dart/constraint/LCPUtils.cpp +++ b/dart/constraint/LCPUtils.cpp @@ -19,7 +19,9 @@ bool LCPUtils::isLCPSolutionValid( const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices) { - getLCPSolutionType(mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); + LCPSolutionType solutionType = getLCPSolutionType( + mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); + (void)solutionType; // temporarily suppress unused variable warnings Eigen::VectorXs v = mA * mX - mB; for (int i = 0; i < mX.size(); i++) { @@ -92,14 +94,65 @@ LCPSolutionType LCPUtils::getLCPSolutionType( const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices) { - Eigen::MatrixXs mANew = mA; - Eigen::VectorXs mXNew = mX; - Eigen::VectorXs mBNew = mB; - Eigen::VectorXs mHiNew = mHi; - Eigen::VectorXs mLoNew = mLo; - Eigen::VectorXi mFIndexNew = mFIndex; - ignoreFrictionIndices = !ignoreFrictionIndices; - return LCPSolutionType::FAILURE; + Eigen::VectorXs v = mA * mX - mB; + for (int i = 0; i < mX.size(); i++) + { + s_t upperLimit = mHi(i); + s_t lowerLimit = mLo(i); + if (mFIndex(i) != -1) + { + if (ignoreFrictionIndices) + { + if (mX(i) != 0) + return LCPSolutionType::FAILURE; + continue; + } + upperLimit *= mX(mFIndex(i)); + lowerLimit *= mX(mFIndex(i)); + } + + const s_t tol = 1e-5; + + /// Solves constriant impulses for a constrained group. The LCP formulation + /// setting that this function solve is A*x = b + w where each x[i], w[i] + /// satisfies one of + /// (1) x = lo, w >= 0 + /// (2) x = hi, w <= 0 + /// (3) lo < x < hi, w = 0 + + // If force has a zero bound, and we're at a zero bound (this is common with + // friction being upper-bounded by a near-zero normal force) then allow + // velocity in either direction. + if (abs(lowerLimit) < tol && abs(upperLimit) < tol && abs(mX(i)) < tol) + { + // This is always allowed + } + // If force is at the lower bound, velocity must be >= 0 + else if (abs(mX(i) - lowerLimit) < tol) + { + if (v(i) < -tol) + return LCPSolutionType::FAILURE; + } + // If force is at the upper bound, velocity must be <= 0 + else if (abs(mX(i) - upperLimit) < tol) + { + if (v(i) > tol) + return LCPSolutionType::FAILURE; + } + // If force is within bounds, then velocity must be zero + else if (mX(i) > lowerLimit && mX(i) < upperLimit) + { + if (abs(v(i)) > tol) + return LCPSolutionType::FAILURE; + } + // If force is out of bounds, we're always illegal + else + { + return LCPSolutionType::FAILURE; + } + } + // If we make it here, the solution is fine + return LCPSolutionType::SUCCESS; } //============================================================================== From 07032472cadb600d12752e02ae7586e298e66dde Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 23 Mar 2022 19:16:14 -0700 Subject: [PATCH 16/22] return boolean based on enum value --- dart/constraint/LCPUtils.cpp | 61 ++---------------------------------- 1 file changed, 2 insertions(+), 59 deletions(-) diff --git a/dart/constraint/LCPUtils.cpp b/dart/constraint/LCPUtils.cpp index 90282868b..60227c129 100644 --- a/dart/constraint/LCPUtils.cpp +++ b/dart/constraint/LCPUtils.cpp @@ -21,65 +21,8 @@ bool LCPUtils::isLCPSolutionValid( { LCPSolutionType solutionType = getLCPSolutionType( mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); - (void)solutionType; // temporarily suppress unused variable warnings - Eigen::VectorXs v = mA * mX - mB; - for (int i = 0; i < mX.size(); i++) - { - s_t upperLimit = mHi(i); - s_t lowerLimit = mLo(i); - if (mFIndex(i) != -1) - { - if (ignoreFrictionIndices) - { - if (mX(i) != 0) - return false; - continue; - } - upperLimit *= mX(mFIndex(i)); - lowerLimit *= mX(mFIndex(i)); - } - - const s_t tol = 1e-5; - - /// Solves constriant impulses for a constrained group. The LCP formulation - /// setting that this function solve is A*x = b + w where each x[i], w[i] - /// satisfies one of - /// (1) x = lo, w >= 0 - /// (2) x = hi, w <= 0 - /// (3) lo < x < hi, w = 0 - - // If force has a zero bound, and we're at a zero bound (this is common with - // friction being upper-bounded by a near-zero normal force) then allow - // velocity in either direction. - if (abs(lowerLimit) < tol && abs(upperLimit) < tol && abs(mX(i)) < tol) - { - // This is always allowed - } - // If force is at the lower bound, velocity must be >= 0 - else if (abs(mX(i) - lowerLimit) < tol) - { - if (v(i) < -tol) - return false; - } - // If force is at the upper bound, velocity must be <= 0 - else if (abs(mX(i) - upperLimit) < tol) - { - if (v(i) > tol) - return false; - } - // If force is within bounds, then velocity must be zero - else if (mX(i) > lowerLimit && mX(i) < upperLimit) - { - if (abs(v(i)) > tol) - return false; - } - // If force is out of bounds, we're always illegal - else - { - return false; - } - } - // If we make it here, the solution is fine + if (solutionType == LCPSolutionType::FAILURE) + return false; return true; } From e18a7fcccc5e0d28936e05d0b875490665c899fa Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 23 Mar 2022 19:29:41 -0700 Subject: [PATCH 17/22] update solution types with different failure codes --- dart/constraint/LCPUtils.cpp | 16 ++++++++-------- dart/constraint/LCPUtils.hpp | 6 +++++- python/_nimblephysics/constraint/LCPUtils.cpp | 16 +++++++++++++++- 3 files changed, 28 insertions(+), 10 deletions(-) diff --git a/dart/constraint/LCPUtils.cpp b/dart/constraint/LCPUtils.cpp index 60227c129..5e998c9af 100644 --- a/dart/constraint/LCPUtils.cpp +++ b/dart/constraint/LCPUtils.cpp @@ -21,9 +21,9 @@ bool LCPUtils::isLCPSolutionValid( { LCPSolutionType solutionType = getLCPSolutionType( mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); - if (solutionType == LCPSolutionType::FAILURE) - return false; - return true; + if (solutionType == LCPSolutionType::SUCCESS) + return true; + return false; } //============================================================================== @@ -47,7 +47,7 @@ LCPSolutionType LCPUtils::getLCPSolutionType( if (ignoreFrictionIndices) { if (mX(i) != 0) - return LCPSolutionType::FAILURE; + return LCPSolutionType::FAILURE_IGNORE_FRICTION; continue; } upperLimit *= mX(mFIndex(i)); @@ -74,24 +74,24 @@ LCPSolutionType LCPUtils::getLCPSolutionType( else if (abs(mX(i) - lowerLimit) < tol) { if (v(i) < -tol) - return LCPSolutionType::FAILURE; + return LCPSolutionType::FAILURE_LOWER_BOUND; } // If force is at the upper bound, velocity must be <= 0 else if (abs(mX(i) - upperLimit) < tol) { if (v(i) > tol) - return LCPSolutionType::FAILURE; + return LCPSolutionType::FAILURE_UPPER_BOUND; } // If force is within bounds, then velocity must be zero else if (mX(i) > lowerLimit && mX(i) < upperLimit) { if (abs(v(i)) > tol) - return LCPSolutionType::FAILURE; + return LCPSolutionType::FAILURE_WITHIN_BOUNDS; } // If force is out of bounds, we're always illegal else { - return LCPSolutionType::FAILURE; + return LCPSolutionType::FAILURE_OUT_OF_BOUNDS; } } // If we make it here, the solution is fine diff --git a/dart/constraint/LCPUtils.hpp b/dart/constraint/LCPUtils.hpp index 5927790fc..efeb2b821 100644 --- a/dart/constraint/LCPUtils.hpp +++ b/dart/constraint/LCPUtils.hpp @@ -13,7 +13,11 @@ namespace constraint { enum LCPSolutionType { SUCCESS, - FAILURE + FAILURE_IGNORE_FRICTION, + FAILURE_LOWER_BOUND, + FAILURE_UPPER_BOUND, + FAILURE_WITHIN_BOUNDS, + FAILURE_OUT_OF_BOUNDS }; class LCPUtils diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index 5c3505cdc..6a9d5fd6f 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -64,7 +64,21 @@ void LCPUtils(py::module& m) ::py::arg("ignoreFrictionIndices")); ::py::enum_(m, "LCPSolutionType") .value("SUCCESS", dart::constraint::LCPSolutionType::SUCCESS) - .value("FAILURE", dart::constraint::LCPSolutionType::FAILURE) + .value( + "FAILURE_IGNORE_FRICTION", + dart::constraint::LCPSolutionType::FAILURE_IGNORE_FRICTION) + .value( + "FAILURE_LOWER_BOUND", + dart::constraint::LCPSolutionType::FAILURE_LOWER_BOUND) + .value( + "FAILURE_UPPER_BOUND", + dart::constraint::LCPSolutionType::FAILURE_UPPER_BOUND) + .value( + "FAILURE_WITHIN_BOUNDS", + dart::constraint::LCPSolutionType::FAILURE_WITHIN_BOUNDS) + .value( + "FAILURE_OUT_OF_BOUNDS", + dart::constraint::LCPSolutionType::FAILURE_OUT_OF_BOUNDS) .export_values(); } From 7dc4e04dbfdd83a9dedaea295750b5b564d3a2a5 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Thu, 24 Mar 2022 22:43:36 -0700 Subject: [PATCH 18/22] refactor to check one constraint at a time --- dart/constraint/LCPUtils.cpp | 122 ++++++++++-------- dart/constraint/LCPUtils.hpp | 11 ++ python/_nimblephysics/constraint/LCPUtils.cpp | 11 ++ 3 files changed, 93 insertions(+), 51 deletions(-) diff --git a/dart/constraint/LCPUtils.cpp b/dart/constraint/LCPUtils.cpp index 5e998c9af..fffc86061 100644 --- a/dart/constraint/LCPUtils.cpp +++ b/dart/constraint/LCPUtils.cpp @@ -19,7 +19,7 @@ bool LCPUtils::isLCPSolutionValid( const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices) { - LCPSolutionType solutionType = getLCPSolutionType( + LCPSolutionType solutionType = getLCPSolutionTypes( mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); if (solutionType == LCPSolutionType::SUCCESS) return true; @@ -27,8 +27,8 @@ bool LCPUtils::isLCPSolutionValid( } //============================================================================== -/// This checks whether a solution to an LCP problem is valid. -LCPSolutionType LCPUtils::getLCPSolutionType( +/// This determines the solution types of an LCP problem. +LCPSolutionType LCPUtils::getLCPSolutionTypes( const Eigen::MatrixXs& mA, const Eigen::VectorXs& mX, const Eigen::VectorXs& mB, @@ -40,59 +40,79 @@ LCPSolutionType LCPUtils::getLCPSolutionType( Eigen::VectorXs v = mA * mX - mB; for (int i = 0; i < mX.size(); i++) { - s_t upperLimit = mHi(i); - s_t lowerLimit = mLo(i); - if (mFIndex(i) != -1) + LCPSolutionType solType = getLCPSolutionType( + i, mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); + if (solType != LCPSolutionType::SUCCESS) + return solType; + } + return LCPSolutionType::SUCCESS; +} + +//============================================================================== +/// This determines the type of a solution to an LCP problem. +LCPSolutionType LCPUtils::getLCPSolutionType( + int i, + const Eigen::MatrixXs& mA, + const Eigen::VectorXs& mX, + const Eigen::VectorXs& mB, + const Eigen::VectorXs& mHi, + const Eigen::VectorXs& mLo, + const Eigen::VectorXi& mFIndex, + bool ignoreFrictionIndices) +{ + Eigen::VectorXs v = mA * mX - mB; + s_t upperLimit = mHi(i); + s_t lowerLimit = mLo(i); + if (mFIndex(i) != -1) + { + if (ignoreFrictionIndices) { - if (ignoreFrictionIndices) - { - if (mX(i) != 0) - return LCPSolutionType::FAILURE_IGNORE_FRICTION; - continue; - } - upperLimit *= mX(mFIndex(i)); - lowerLimit *= mX(mFIndex(i)); + if (mX(i) != 0) + return LCPSolutionType::FAILURE_IGNORE_FRICTION; + return LCPSolutionType::SUCCESS; } + upperLimit *= mX(mFIndex(i)); + lowerLimit *= mX(mFIndex(i)); + } - const s_t tol = 1e-5; + const s_t tol = 1e-5; - /// Solves constriant impulses for a constrained group. The LCP formulation - /// setting that this function solve is A*x = b + w where each x[i], w[i] - /// satisfies one of - /// (1) x = lo, w >= 0 - /// (2) x = hi, w <= 0 - /// (3) lo < x < hi, w = 0 + /// Solves constriant impulses for a constrained group. The LCP formulation + /// setting that this function solve is A*x = b + w where each x[i], w[i] + /// satisfies one of + /// (1) x = lo, w >= 0 + /// (2) x = hi, w <= 0 + /// (3) lo < x < hi, w = 0 - // If force has a zero bound, and we're at a zero bound (this is common with - // friction being upper-bounded by a near-zero normal force) then allow - // velocity in either direction. - if (abs(lowerLimit) < tol && abs(upperLimit) < tol && abs(mX(i)) < tol) - { - // This is always allowed - } - // If force is at the lower bound, velocity must be >= 0 - else if (abs(mX(i) - lowerLimit) < tol) - { - if (v(i) < -tol) - return LCPSolutionType::FAILURE_LOWER_BOUND; - } - // If force is at the upper bound, velocity must be <= 0 - else if (abs(mX(i) - upperLimit) < tol) - { - if (v(i) > tol) - return LCPSolutionType::FAILURE_UPPER_BOUND; - } - // If force is within bounds, then velocity must be zero - else if (mX(i) > lowerLimit && mX(i) < upperLimit) - { - if (abs(v(i)) > tol) - return LCPSolutionType::FAILURE_WITHIN_BOUNDS; - } - // If force is out of bounds, we're always illegal - else - { - return LCPSolutionType::FAILURE_OUT_OF_BOUNDS; - } + // If force has a zero bound, and we're at a zero bound (this is common with + // friction being upper-bounded by a near-zero normal force) then allow + // velocity in either direction. + if (abs(lowerLimit) < tol && abs(upperLimit) < tol && abs(mX(i)) < tol) + { + // This is always allowed + } + // If force is at the lower bound, velocity must be >= 0 + else if (abs(mX(i) - lowerLimit) < tol) + { + if (v(i) < -tol) + return LCPSolutionType::FAILURE_LOWER_BOUND; + } + // If force is at the upper bound, velocity must be <= 0 + else if (abs(mX(i) - upperLimit) < tol) + { + if (v(i) > tol) + return LCPSolutionType::FAILURE_UPPER_BOUND; + } + // If force is within bounds, then velocity must be zero + else if (mX(i) > lowerLimit && mX(i) < upperLimit) + { + if (abs(v(i)) > tol) + return LCPSolutionType::FAILURE_WITHIN_BOUNDS; + } + // If force is out of bounds, we're always illegal + else + { + return LCPSolutionType::FAILURE_OUT_OF_BOUNDS; } // If we make it here, the solution is fine return LCPSolutionType::SUCCESS; diff --git a/dart/constraint/LCPUtils.hpp b/dart/constraint/LCPUtils.hpp index efeb2b821..5fd8934e6 100644 --- a/dart/constraint/LCPUtils.hpp +++ b/dart/constraint/LCPUtils.hpp @@ -33,8 +33,19 @@ class LCPUtils const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices); + // This determines the solution types of an LCP problem. + static LCPSolutionType getLCPSolutionTypes( + const Eigen::MatrixXs& mA, + const Eigen::VectorXs& mX, + const Eigen::VectorXs& mB, + const Eigen::VectorXs& mHi, + const Eigen::VectorXs& mLo, + const Eigen::VectorXi& mFIndex, + bool ignoreFrictionIndices); + // This determines the type of a solution to an LCP problem. static LCPSolutionType getLCPSolutionType( + int i, const Eigen::MatrixXs& mA, const Eigen::VectorXs& mX, const Eigen::VectorXs& mB, diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index 6a9d5fd6f..4ea4fe1d6 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -52,9 +52,20 @@ void LCPUtils(py::module& m) ::py::arg("mLo"), ::py::arg("mFIndex"), ::py::arg("ignoreFrictionIndices")); + m.def( + "getLCPSolutionTypes", + &dart::constraint::LCPUtils::getLCPSolutionTypes, + ::py::arg("mA"), + ::py::arg("mX"), + ::py::arg("mB"), + ::py::arg("mHi"), + ::py::arg("mLo"), + ::py::arg("mFIndex"), + ::py::arg("ignoreFrictionIndices")); m.def( "getLCPSolutionType", &dart::constraint::LCPUtils::getLCPSolutionType, + ::py::arg("i"), ::py::arg("mA"), ::py::arg("mX"), ::py::arg("mB"), From bbafff1cbce35d96f4d7aecfc386fcbb49a3dc44 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Thu, 24 Mar 2022 22:55:33 -0700 Subject: [PATCH 19/22] return vector of solution types --- dart/constraint/LCPUtils.cpp | 27 +++++++++++++++++++-------- dart/constraint/LCPUtils.hpp | 2 +- 2 files changed, 20 insertions(+), 9 deletions(-) diff --git a/dart/constraint/LCPUtils.cpp b/dart/constraint/LCPUtils.cpp index fffc86061..7da319c89 100644 --- a/dart/constraint/LCPUtils.cpp +++ b/dart/constraint/LCPUtils.cpp @@ -19,16 +19,21 @@ bool LCPUtils::isLCPSolutionValid( const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices) { - LCPSolutionType solutionType = getLCPSolutionTypes( + std::vector solutionTypes = getLCPSolutionTypes( mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); - if (solutionType == LCPSolutionType::SUCCESS) - return true; - return false; + + for (int i = 0; i < solutionTypes.size(); i++) + { + LCPSolutionType solutionType = solutionTypes[i]; + if (solutionType != LCPSolutionType::SUCCESS) + return false; + } + return true; } //============================================================================== /// This determines the solution types of an LCP problem. -LCPSolutionType LCPUtils::getLCPSolutionTypes( +std::vector LCPUtils::getLCPSolutionTypes( const Eigen::MatrixXs& mA, const Eigen::VectorXs& mX, const Eigen::VectorXs& mB, @@ -38,14 +43,20 @@ LCPSolutionType LCPUtils::getLCPSolutionTypes( bool ignoreFrictionIndices) { Eigen::VectorXs v = mA * mX - mB; + + std::vector solutionTypes; + for (int i = 0; i < mX.size(); i++) { LCPSolutionType solType = getLCPSolutionType( i, mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); - if (solType != LCPSolutionType::SUCCESS) - return solType; + solutionTypes.push_back(solType); + + // if (solType != LCPSolutionType::SUCCESS) + // return solType; } - return LCPSolutionType::SUCCESS; + return solutionTypes; + // return LCPSolutionType::SUCCESS; } //============================================================================== diff --git a/dart/constraint/LCPUtils.hpp b/dart/constraint/LCPUtils.hpp index 5fd8934e6..04da520ce 100644 --- a/dart/constraint/LCPUtils.hpp +++ b/dart/constraint/LCPUtils.hpp @@ -34,7 +34,7 @@ class LCPUtils bool ignoreFrictionIndices); // This determines the solution types of an LCP problem. - static LCPSolutionType getLCPSolutionTypes( + static std::vector getLCPSolutionTypes( const Eigen::MatrixXs& mA, const Eigen::VectorXs& mX, const Eigen::VectorXs& mB, From 8e14be22190fffba7c9a47c4f5ea882213c813ff Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Thu, 24 Mar 2022 23:03:56 -0700 Subject: [PATCH 20/22] include stlh --- python/_nimblephysics/constraint/LCPUtils.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index 4ea4fe1d6..07996ca89 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -34,6 +34,7 @@ #include #include #include +#include namespace py = pybind11; From 192284e03c0dbcc608a94bb3347a7f457bb02f2b Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Fri, 25 Mar 2022 07:38:47 -0700 Subject: [PATCH 21/22] add computeSlackFromLCPSolution() --- dart/constraint/LCPUtils.cpp | 16 +++++++++++----- dart/constraint/LCPUtils.hpp | 6 ++++++ python/_nimblephysics/constraint/LCPUtils.cpp | 6 ++++++ 3 files changed, 23 insertions(+), 5 deletions(-) diff --git a/dart/constraint/LCPUtils.cpp b/dart/constraint/LCPUtils.cpp index 7da319c89..88f5b20ba 100644 --- a/dart/constraint/LCPUtils.cpp +++ b/dart/constraint/LCPUtils.cpp @@ -51,12 +51,8 @@ std::vector LCPUtils::getLCPSolutionTypes( LCPSolutionType solType = getLCPSolutionType( i, mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); solutionTypes.push_back(solType); - - // if (solType != LCPSolutionType::SUCCESS) - // return solType; } return solutionTypes; - // return LCPSolutionType::SUCCESS; } //============================================================================== @@ -71,7 +67,7 @@ LCPSolutionType LCPUtils::getLCPSolutionType( const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices) { - Eigen::VectorXs v = mA * mX - mB; + Eigen::VectorXs v = computeSlackFromLCPSolution(mA, mX, mB); s_t upperLimit = mHi(i); s_t lowerLimit = mLo(i); if (mFIndex(i) != -1) @@ -129,6 +125,16 @@ LCPSolutionType LCPUtils::getLCPSolutionType( return LCPSolutionType::SUCCESS; } +//============================================================================== +/// This computes the slack variable from the LCP solution. +Eigen::VectorXs LCPUtils::computeSlackFromLCPSolution( + const Eigen::MatrixXs& mA, + const Eigen::VectorXs& mX, + const Eigen::VectorXs& mB) +{ + return mA * mX - mB; +} + //============================================================================== /// This applies a simple algorithm to guess the solution to the LCP problem. /// It's not guaranteed to be correct, but it often can be if there is no diff --git a/dart/constraint/LCPUtils.hpp b/dart/constraint/LCPUtils.hpp index 04da520ce..1556993d4 100644 --- a/dart/constraint/LCPUtils.hpp +++ b/dart/constraint/LCPUtils.hpp @@ -54,6 +54,12 @@ class LCPUtils const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices); + // This computes the slack variable from the LCP solution. + static Eigen::VectorXs computeSlackFromLCPSolution( + const Eigen::MatrixXs& mA, + const Eigen::VectorXs& mX, + const Eigen::VectorXs& mB); + /// This applies a simple algorithm to guess the solution to the LCP problem. /// It's not guaranteed to be correct, but it often can be if there is no /// sliding friction on this timestep. diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index 07996ca89..fc5556557 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -74,6 +74,12 @@ void LCPUtils(py::module& m) ::py::arg("mLo"), ::py::arg("mFIndex"), ::py::arg("ignoreFrictionIndices")); + m.def( + "computeSlackFromLCPSolution", + &dart::constraint::LCPUtils::getLCPSolutionType, + ::py::arg("mA"), + ::py::arg("mX"), + ::py::arg("mB")); ::py::enum_(m, "LCPSolutionType") .value("SUCCESS", dart::constraint::LCPSolutionType::SUCCESS) .value( From 16e9de38518718f84ce73a0b1d8d3db79464a559 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Fri, 25 Mar 2022 07:41:05 -0700 Subject: [PATCH 22/22] fix typo --- python/_nimblephysics/constraint/LCPUtils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/_nimblephysics/constraint/LCPUtils.cpp b/python/_nimblephysics/constraint/LCPUtils.cpp index fc5556557..c9d74be24 100644 --- a/python/_nimblephysics/constraint/LCPUtils.cpp +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -76,7 +76,7 @@ void LCPUtils(py::module& m) ::py::arg("ignoreFrictionIndices")); m.def( "computeSlackFromLCPSolution", - &dart::constraint::LCPUtils::getLCPSolutionType, + &dart::constraint::LCPUtils::computeSlackFromLCPSolution, ::py::arg("mA"), ::py::arg("mX"), ::py::arg("mB"));