Add python bindings for LCPUtils

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

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<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,
+      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.

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 <dart/constraint/BoxedLcpSolver.hpp>
+#include <dart/constraint/LCPUtils.hpp>
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+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_<dart::constraint::LCPSolutionType>(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

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