diff --git a/dart/constraint/LCPUtils.cpp b/dart/constraint/LCPUtils.cpp index 5ec9601b6..88f5b20ba 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, @@ -17,66 +18,121 @@ bool LCPUtils::isLCPSolutionValid( const Eigen::VectorXs& mLo, const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices) +{ + std::vector solutionTypes = getLCPSolutionTypes( + mA, mX, mB, mHi, mLo, mFIndex, ignoreFrictionIndices); + + 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. +std::vector LCPUtils::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) { Eigen::VectorXs v = mA * mX - mB; + + std::vector solutionTypes; + 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); + solutionTypes.push_back(solType); + } + return solutionTypes; +} + +//============================================================================== +/// 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 = computeSlackFromLCPSolution(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 false; - 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 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 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 true; + 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; } //============================================================================== diff --git a/dart/constraint/LCPUtils.hpp b/dart/constraint/LCPUtils.hpp index 1cfe3ec97..1556993d4 100644 --- a/dart/constraint/LCPUtils.hpp +++ b/dart/constraint/LCPUtils.hpp @@ -10,9 +10,20 @@ namespace dart { namespace constraint { +enum LCPSolutionType +{ + SUCCESS, + FAILURE_IGNORE_FRICTION, + FAILURE_LOWER_BOUND, + FAILURE_UPPER_BOUND, + FAILURE_WITHIN_BOUNDS, + FAILURE_OUT_OF_BOUNDS +}; + 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 +33,33 @@ class LCPUtils const Eigen::VectorXi& mFIndex, bool ignoreFrictionIndices); + // This determines the solution types of an LCP problem. + static std::vector 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, + const Eigen::VectorXs& mHi, + const Eigen::VectorXs& mLo, + 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 new file mode 100644 index 000000000..c9d74be24 --- /dev/null +++ b/python/_nimblephysics/constraint/LCPUtils.cpp @@ -0,0 +1,104 @@ +/* + * 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 +#include +#include + +namespace py = pybind11; + +namespace dart { +namespace python { + +void LCPUtils(py::module& m) +{ + 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")); + 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"), + ::py::arg("mHi"), + ::py::arg("mLo"), + ::py::arg("mFIndex"), + ::py::arg("ignoreFrictionIndices")); + m.def( + "computeSlackFromLCPSolution", + &dart::constraint::LCPUtils::computeSlackFromLCPSolution, + ::py::arg("mA"), + ::py::arg("mX"), + ::py::arg("mB")); + ::py::enum_(m, "LCPSolutionType") + .value("SUCCESS", dart::constraint::LCPSolutionType::SUCCESS) + .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(); +} + +} // namespace python +} // namespace dart 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