w2

dartsim · Apr 5, 2024 · 7b90079 · 7b90079
1 parent df64a25
commit 7b90079
Show file tree

Hide file tree

Showing 12 changed files with 279 additions and 333 deletions.
diff --git a/dart/constraint/BoxedLcpSolver.hpp b/dart/constraint/BoxedLcpSolver.hpp
@@ -90,9 +90,7 @@ class BoxedLcpSolver : public common::Castable<BoxedLcpSolver>
       bool earlyTermination = false)
       = 0;
 
-#if DART_BUILD_MODE_DEBUG
   virtual bool canSolve(int n, const double* A) = 0;
-#endif
 };
 
 } // namespace constraint

diff --git a/dart/constraint/ConstrainedGroup.cpp b/dart/constraint/ConstrainedGroup.cpp
@@ -101,14 +101,12 @@ void ConstrainedGroup::removeAllConstraints()
 }
 
 //==============================================================================
-#if DART_BUILD_MODE_DEBUG
 bool ConstrainedGroup::containConstraint(
     const ConstConstraintBasePtr& _constraint) const
 {
   return std::find(mConstraints.begin(), mConstraints.end(), _constraint)
          != mConstraints.end();
 }
-#endif
 
 //==============================================================================
 std::size_t ConstrainedGroup::getTotalDimension() const

diff --git a/dart/constraint/ConstrainedGroup.hpp b/dart/constraint/ConstrainedGroup.hpp
@@ -102,10 +102,8 @@ class ConstrainedGroup
   friend class ConstraintSolver;
 
 private:
-#if DART_BUILD_MODE_DEBUG
   /// Return true if _constraint is contained
   bool containConstraint(const ConstConstraintBasePtr& _constraint) const;
-#endif
 
   /// List of constraints
   std::vector<ConstraintBasePtr> mConstraints;

diff --git a/dart/constraint/DantzigBoxedLcpSolver.cpp b/dart/constraint/DantzigBoxedLcpSolver.cpp
@@ -68,14 +68,12 @@ bool DantzigBoxedLcpSolver::solve(
       n, A, x, b, nullptr, 0, lo, hi, findex, earlyTermination);
 }
 
-#if DART_BUILD_MODE_DEBUG
 //==============================================================================
 bool DantzigBoxedLcpSolver::canSolve(int /*n*/, const double* /*A*/)
 {
   // TODO(JS): Not implemented.
   return true;
 }
-#endif
 
 } // namespace constraint
 } // namespace dart
diff --git a/dart/constraint/DantzigBoxedLcpSolver.hpp b/dart/constraint/DantzigBoxedLcpSolver.hpp
@@ -59,10 +59,8 @@ class DantzigBoxedLcpSolver : public BoxedLcpSolver
       int* findex,
       bool earlyTermination) override;
 
-#if DART_BUILD_MODE_DEBUG
   // Documentation inherited.
   bool canSolve(int n, const double* A) override;
-#endif
 };
 
 } // namespace constraint

diff --git a/dart/constraint/DantzigLCPSolver.cpp b/dart/constraint/DantzigLCPSolver.cpp
@@ -32,7 +32,7 @@
 
 #include "dart/constraint/DantzigLCPSolver.hpp"
 
-#if DART_BUILD_MODE_DEBUG
+#ifndef NDEBUG
   #include <iomanip>
   #include <iostream>
 #endif
@@ -46,144 +46,11 @@
 namespace dart {
 namespace constraint {
 
-//==============================================================================
-DantzigLCPSolver::DantzigLCPSolver(double _timestep) : LCPSolver(_timestep)
-{
-  // Do nothing
-}
+namespace {
 
 //==============================================================================
-DantzigLCPSolver::~DantzigLCPSolver()
-{
-  // Do nothing
-}
-
-//==============================================================================
-void DantzigLCPSolver::solve(ConstrainedGroup* _group)
-{
-  // Build LCP terms by aggregating them from constraints
-  std::size_t numConstraints = _group->getNumConstraints();
-  std::size_t n = _group->getTotalDimension();
-
-  // If there is no constraint, then just return.
-  if (0u == n)
-    return;
-
-  int nSkip = dPAD(n);
-  double* A = new double[n * nSkip];
-  double* x = new double[n];
-  double* b = new double[n];
-  double* w = new double[n];
-  double* lo = new double[n];
-  double* hi = new double[n];
-  int* findex = new int[n];
-
-  // Set w to 0 and findex to -1
-#if DART_BUILD_MODE_DEBUG
-  std::memset(A, 0.0, n * nSkip * sizeof(double));
-#endif
-  std::memset(w, 0.0, n * sizeof(double));
-  std::memset(findex, -1, n * sizeof(int));
-
-  // Compute offset indices
-  std::size_t* offset = new std::size_t[n];
-  offset[0] = 0;
-  //  std::cout << "offset[" << 0 << "]: " << offset[0] << std::endl;
-  for (std::size_t i = 1; i < numConstraints; ++i) {
-    const ConstraintBasePtr& constraint = _group->getConstraint(i - 1);
-    assert(constraint->getDimension() > 0);
-    offset[i] = offset[i - 1] + constraint->getDimension();
-    //    std::cout << "offset[" << i << "]: " << offset[i] << std::endl;
-  }
-
-  // For each constraint
-  ConstraintInfo constInfo;
-  constInfo.invTimeStep = 1.0 / mTimeStep;
-  for (std::size_t i = 0; i < numConstraints; ++i) {
-    const ConstraintBasePtr& constraint = _group->getConstraint(i);
-
-    constInfo.x = x + offset[i];
-    constInfo.lo = lo + offset[i];
-    constInfo.hi = hi + offset[i];
-    constInfo.b = b + offset[i];
-    constInfo.findex = findex + offset[i];
-    constInfo.w = w + offset[i];
-
-    // Fill vectors: lo, hi, b, w
-    constraint->getInformation(&constInfo);
-
-    // Fill a matrix by impulse tests: A
-    constraint->excite();
-    for (std::size_t j = 0; j < constraint->getDimension(); ++j) {
-      // Adjust findex for global index
-      if (findex[offset[i] + j] >= 0)
-        findex[offset[i] + j] += offset[i];
-
-      // Apply impulse for impulse test
-      constraint->applyUnitImpulse(j);
-
-      // Fill upper triangle blocks of A matrix
-      int index = nSkip * (offset[i] + j) + offset[i];
-      constraint->getVelocityChange(A + index, true);
-      for (std::size_t k = i + 1; k < numConstraints; ++k) {
-        index = nSkip * (offset[i] + j) + offset[k];
-        _group->getConstraint(k)->getVelocityChange(A + index, false);
-      }
-
-      // Filling symmetric part of A matrix
-      for (std::size_t k = 0; k < i; ++k) {
-        for (std::size_t l = 0; l < _group->getConstraint(k)->getDimension();
-             ++l) {
-          int index1 = nSkip * (offset[i] + j) + offset[k] + l;
-          int index2 = nSkip * (offset[k] + l) + offset[i] + j;
-
-          A[index1] = A[index2];
-        }
-      }
-    }
-
-    assert(isSymmetric(
-        n, A, offset[i], offset[i] + constraint->getDimension() - 1));
-
-    constraint->unexcite();
-  }
-
-  assert(isSymmetric(n, A));
-
-  // Print LCP formulation
-  //  dtdbg << "Before solve:" << std::endl;
-  //  print(n, A, x, lo, hi, b, w, findex);
-  //  std::cout << std::endl;
-
-  // Solve LCP using ODE's Dantzig algorithm
-  external::ode::dSolveLCP(n, A, x, b, w, 0, lo, hi, findex);
-
-  // Print LCP formulation
-  //  dtdbg << "After solve:" << std::endl;
-  //  print(n, A, x, lo, hi, b, w, findex);
-  //  std::cout << std::endl;
-
-  // Apply constraint impulses
-  for (std::size_t i = 0; i < numConstraints; ++i) {
-    const ConstraintBasePtr& constraint = _group->getConstraint(i);
-    constraint->applyImpulse(x + offset[i]);
-    constraint->excite();
-  }
-
-  delete[] offset;
-
-  delete[] A;
-  delete[] x;
-  delete[] b;
-  delete[] w;
-  delete[] lo;
-  delete[] hi;
-  delete[] findex;
-}
-
-//==============================================================================
-#if DART_BUILD_MODE_DEBUG
-bool DantzigLCPSolver::isSymmetric(std::size_t _n, double* _A)
+#ifndef NDEBUG
+bool isSymmetric(std::size_t _n, double* _A)
 {
   std::size_t nSkip = dPAD(_n);
   for (std::size_t i = 0; i < _n; ++i) {
@@ -210,7 +77,7 @@ bool DantzigLCPSolver::isSymmetric(std::size_t _n, double* _A)
 }
 
 //==============================================================================
-bool DantzigLCPSolver::isSymmetric(
+bool isSymmetric(
     std::size_t _n, double* _A, std::size_t _begin, std::size_t _end)
 {
   std::size_t nSkip = dPAD(_n);
@@ -238,7 +105,7 @@ bool DantzigLCPSolver::isSymmetric(
 }
 
 //==============================================================================
-void DantzigLCPSolver::print(
+void print(
     std::size_t _n,
     double* _A,
     double* _x,
@@ -323,5 +190,139 @@ void DantzigLCPSolver::print(
 }
 #endif
 
+} // namespace
+
+//==============================================================================
+DantzigLCPSolver::DantzigLCPSolver(double _timestep) : LCPSolver(_timestep)
+{
+  // Do nothing
+}
+
+//==============================================================================
+DantzigLCPSolver::~DantzigLCPSolver()
+{
+  // Do nothing
+}
+
+//==============================================================================
+void DantzigLCPSolver::solve(ConstrainedGroup* _group)
+{
+  // Build LCP terms by aggregating them from constraints
+  std::size_t numConstraints = _group->getNumConstraints();
+  std::size_t n = _group->getTotalDimension();
+
+  // If there is no constraint, then just return.
+  if (0u == n)
+    return;
+
+  int nSkip = dPAD(n);
+  double* A = new double[n * nSkip];
+  double* x = new double[n];
+  double* b = new double[n];
+  double* w = new double[n];
+  double* lo = new double[n];
+  double* hi = new double[n];
+  int* findex = new int[n];
+
+  // Set w to 0 and findex to -1
+  std::memset(w, 0.0, n * sizeof(double));
+  std::memset(findex, -1, n * sizeof(int));
+
+  // Compute offset indices
+  std::size_t* offset = new std::size_t[n];
+  offset[0] = 0;
+  //  std::cout << "offset[" << 0 << "]: " << offset[0] << std::endl;
+  for (std::size_t i = 1; i < numConstraints; ++i) {
+    const ConstraintBasePtr& constraint = _group->getConstraint(i - 1);
+    assert(constraint->getDimension() > 0);
+    offset[i] = offset[i - 1] + constraint->getDimension();
+    //    std::cout << "offset[" << i << "]: " << offset[i] << std::endl;
+  }
+
+  // For each constraint
+  ConstraintInfo constInfo;
+  constInfo.invTimeStep = 1.0 / mTimeStep;
+  for (std::size_t i = 0; i < numConstraints; ++i) {
+    const ConstraintBasePtr& constraint = _group->getConstraint(i);
+
+    constInfo.x = x + offset[i];
+    constInfo.lo = lo + offset[i];
+    constInfo.hi = hi + offset[i];
+    constInfo.b = b + offset[i];
+    constInfo.findex = findex + offset[i];
+    constInfo.w = w + offset[i];
+
+    // Fill vectors: lo, hi, b, w
+    constraint->getInformation(&constInfo);
+
+    // Fill a matrix by impulse tests: A
+    constraint->excite();
+    for (std::size_t j = 0; j < constraint->getDimension(); ++j) {
+      // Adjust findex for global index
+      if (findex[offset[i] + j] >= 0)
+        findex[offset[i] + j] += offset[i];
+
+      // Apply impulse for impulse test
+      constraint->applyUnitImpulse(j);
+
+      // Fill upper triangle blocks of A matrix
+      int index = nSkip * (offset[i] + j) + offset[i];
+      constraint->getVelocityChange(A + index, true);
+      for (std::size_t k = i + 1; k < numConstraints; ++k) {
+        index = nSkip * (offset[i] + j) + offset[k];
+        _group->getConstraint(k)->getVelocityChange(A + index, false);
+      }
+
+      // Filling symmetric part of A matrix
+      for (std::size_t k = 0; k < i; ++k) {
+        for (std::size_t l = 0; l < _group->getConstraint(k)->getDimension();
+             ++l) {
+          int index1 = nSkip * (offset[i] + j) + offset[k] + l;
+          int index2 = nSkip * (offset[k] + l) + offset[i] + j;
+
+          A[index1] = A[index2];
+        }
+      }
+    }
+
+    assert(isSymmetric(
+        n, A, offset[i], offset[i] + constraint->getDimension() - 1));
+
+    constraint->unexcite();
+  }
+
+  assert(isSymmetric(n, A));
+
+  // Print LCP formulation
+  //  dtdbg << "Before solve:" << std::endl;
+  //  print(n, A, x, lo, hi, b, w, findex);
+  //  std::cout << std::endl;
+
+  // Solve LCP using ODE's Dantzig algorithm
+  external::ode::dSolveLCP(n, A, x, b, w, 0, lo, hi, findex);
+
+  // Print LCP formulation
+  //  dtdbg << "After solve:" << std::endl;
+  //  print(n, A, x, lo, hi, b, w, findex);
+  //  std::cout << std::endl;
+
+  // Apply constraint impulses
+  for (std::size_t i = 0; i < numConstraints; ++i) {
+    const ConstraintBasePtr& constraint = _group->getConstraint(i);
+    constraint->applyImpulse(x + offset[i]);
+    constraint->excite();
+  }
+
+  delete[] offset;
+
+  delete[] A;
+  delete[] x;
+  delete[] b;
+  delete[] w;
+  delete[] lo;
+  delete[] hi;
+  delete[] findex;
+}
+
 } // namespace constraint
 } // namespace dart