From 7b900795b1b008ef1fa16b6fa1ff3f7f5d09c9ba Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 5 Apr 2024 08:45:32 -0700 Subject: [PATCH] w2 --- dart/constraint/BoxedLcpSolver.hpp | 2 - dart/constraint/ConstrainedGroup.cpp | 2 - dart/constraint/ConstrainedGroup.hpp | 2 - dart/constraint/DantzigBoxedLcpSolver.cpp | 2 - dart/constraint/DantzigBoxedLcpSolver.hpp | 2 - dart/constraint/DantzigLCPSolver.cpp | 279 +++++++++++----------- dart/constraint/DantzigLCPSolver.hpp | 21 -- dart/constraint/JointConstraint.cpp | 4 +- dart/constraint/PGSLCPSolver.cpp | 273 ++++++++++----------- dart/constraint/PGSLCPSolver.hpp | 21 -- dart/constraint/PgsBoxedLcpSolver.cpp | 2 - dart/constraint/PgsBoxedLcpSolver.hpp | 2 - 12 files changed, 279 insertions(+), 333 deletions(-) diff --git a/dart/constraint/BoxedLcpSolver.hpp b/dart/constraint/BoxedLcpSolver.hpp index 866cce291172e..b003fad4019fa 100644 --- a/dart/constraint/BoxedLcpSolver.hpp +++ b/dart/constraint/BoxedLcpSolver.hpp @@ -90,9 +90,7 @@ class BoxedLcpSolver : public common::Castable 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 index 72293b5c4b79d..cc68dd0a40154 100644 --- 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 index aa79ae559b28d..7fc193b1d94cf 100644 --- 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 mConstraints; diff --git a/dart/constraint/DantzigBoxedLcpSolver.cpp b/dart/constraint/DantzigBoxedLcpSolver.cpp index 635a671182d50..48dbd69b88a86 100644 --- 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 index 790759dea602a..d4af7cd1c214d 100644 --- 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 index fde0dd166b278..45834f9854f43 100644 --- 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 #include #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 diff --git a/dart/constraint/DantzigLCPSolver.hpp b/dart/constraint/DantzigLCPSolver.hpp index 2937657977759..7bc0ceed34510 100644 --- a/dart/constraint/DantzigLCPSolver.hpp +++ b/dart/constraint/DantzigLCPSolver.hpp @@ -58,27 +58,6 @@ class DantzigLCPSolver : public LCPSolver // Documentation inherited void solve(ConstrainedGroup* _group) override; - -#if DART_BUILD_MODE_DEBUG -private: - /// Return true if the matrix is symmetric - bool isSymmetric(std::size_t _n, double* _A); - - /// Return true if the diagonal block of matrix is symmetric - bool isSymmetric( - std::size_t _n, double* _A, std::size_t _begin, std::size_t _end); - - /// Print debug information - void print( - std::size_t _n, - double* _A, - double* _x, - double* _lo, - double* _hi, - double* _b, - double* w, - int* _findex); -#endif }; } // namespace constraint diff --git a/dart/constraint/JointConstraint.cpp b/dart/constraint/JointConstraint.cpp index 32ebec57fdfc1..374d9c1edf98c 100644 --- a/dart/constraint/JointConstraint.cpp +++ b/dart/constraint/JointConstraint.cpp @@ -373,7 +373,7 @@ void JointConstraint::getInformation(ConstraintInfo* lcp) if (!mActive[i]) continue; -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG if (std::abs(lcp->w[index]) > 1e-6) { dterr << "Invalid " << index << "-th slack variable. Expected: 0.0. Actual: " << lcp->w[index] @@ -387,7 +387,7 @@ void JointConstraint::getInformation(ConstraintInfo* lcp) lcp->lo[index] = mImpulseLowerBound[i]; lcp->hi[index] = mImpulseUpperBound[i]; -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG if (lcp->findex[index] != -1) { dterr << "Invalid " << index << "-th friction index. Expected: -1. Actual: " diff --git a/dart/constraint/PGSLCPSolver.cpp b/dart/constraint/PGSLCPSolver.cpp index 1a5a446f39cb5..e4eff78553a54 100644 --- a/dart/constraint/PGSLCPSolver.cpp +++ b/dart/constraint/PGSLCPSolver.cpp @@ -32,7 +32,7 @@ #include "dart/constraint/PGSLCPSolver.hpp" -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG #include #include #endif @@ -47,141 +47,11 @@ namespace dart { namespace constraint { -//============================================================================== -PGSLCPSolver::PGSLCPSolver(double _timestep) : LCPSolver(_timestep) {} - -//============================================================================== -PGSLCPSolver::~PGSLCPSolver() {} - -//============================================================================== -void PGSLCPSolver::solve(ConstrainedGroup* _group) -{ - DART_PROFILE_SCOPED; - // If there is no constraint, then just return true. - std::size_t numConstraints = _group->getNumConstraints(); - if (numConstraints == 0) - return; - - // Build LCP terms by aggregating them from constraints - std::size_t n = _group->getTotalDimension(); - 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 - // dSolveLCP(n, A, x, b, w, 0, lo, hi, findex); - PGSOption option; - option.setDefault(); - solvePGS(n, nSkip, 0, A, x, b, lo, hi, findex, &option); - - // 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 { //============================================================================== -#if DART_BUILD_MODE_DEBUG -bool PGSLCPSolver::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) { @@ -208,7 +78,7 @@ bool PGSLCPSolver::isSymmetric(std::size_t _n, double* _A) } //============================================================================== -bool PGSLCPSolver::isSymmetric( +bool isSymmetric( std::size_t _n, double* _A, std::size_t _begin, std::size_t _end) { std::size_t nSkip = dPAD(_n); @@ -236,7 +106,7 @@ bool PGSLCPSolver::isSymmetric( } //============================================================================== -void PGSLCPSolver::print( +void print( std::size_t _n, double* _A, double* _x, @@ -321,6 +191,137 @@ void PGSLCPSolver::print( } #endif +} // namespace + +//============================================================================== +PGSLCPSolver::PGSLCPSolver(double _timestep) : LCPSolver(_timestep) {} + +//============================================================================== +PGSLCPSolver::~PGSLCPSolver() {} + +//============================================================================== +void PGSLCPSolver::solve(ConstrainedGroup* _group) +{ + DART_PROFILE_SCOPED; + // If there is no constraint, then just return true. + std::size_t numConstraints = _group->getNumConstraints(); + if (numConstraints == 0) + return; + + // Build LCP terms by aggregating them from constraints + std::size_t n = _group->getTotalDimension(); + 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 + // dSolveLCP(n, A, x, b, w, 0, lo, hi, findex); + PGSOption option; + option.setDefault(); + solvePGS(n, nSkip, 0, A, x, b, lo, hi, findex, &option); + + // 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; +} + bool solvePGS( int n, int nskip, diff --git a/dart/constraint/PGSLCPSolver.hpp b/dart/constraint/PGSLCPSolver.hpp index 7d72b1b22eb88..7683054b18db9 100644 --- a/dart/constraint/PGSLCPSolver.hpp +++ b/dart/constraint/PGSLCPSolver.hpp @@ -57,27 +57,6 @@ class PGSLCPSolver : public LCPSolver // Documentation inherited void solve(ConstrainedGroup* _group) override; - -#if DART_BUILD_MODE_DEBUG -private: - /// Return true if the matrix is symmetric - bool isSymmetric(std::size_t _n, double* _A); - - /// Return true if the diagonal block of matrix is symmetric - bool isSymmetric( - std::size_t _n, double* _A, std::size_t _begin, std::size_t _end); - - /// Print debug information - void print( - std::size_t _n, - double* _A, - double* _x, - double* _lo, - double* _hi, - double* _b, - double* w, - int* _findex); -#endif }; struct PGSOption diff --git a/dart/constraint/PgsBoxedLcpSolver.cpp b/dart/constraint/PgsBoxedLcpSolver.cpp index 41eae5d00ce29..1bfdf07c93c8f 100644 --- a/dart/constraint/PgsBoxedLcpSolver.cpp +++ b/dart/constraint/PgsBoxedLcpSolver.cpp @@ -228,7 +228,6 @@ bool PgsBoxedLcpSolver::solve( return possibleToTerminate; } -#if DART_BUILD_MODE_DEBUG //============================================================================== bool PgsBoxedLcpSolver::canSolve(int n, const double* A) { @@ -247,7 +246,6 @@ bool PgsBoxedLcpSolver::canSolve(int n, const double* A) return true; } -#endif //============================================================================== void PgsBoxedLcpSolver::setOption(const PgsBoxedLcpSolver::Option& option) diff --git a/dart/constraint/PgsBoxedLcpSolver.hpp b/dart/constraint/PgsBoxedLcpSolver.hpp index 2bd77936a032d..782450c9e6588 100644 --- a/dart/constraint/PgsBoxedLcpSolver.hpp +++ b/dart/constraint/PgsBoxedLcpSolver.hpp @@ -78,10 +78,8 @@ class PgsBoxedLcpSolver : public BoxedLcpSolver int* findex, bool earlyTermination) override; -#if DART_BUILD_MODE_DEBUG // Documentation inherited. bool canSolve(int n, const double* A) override; -#endif /// Sets options void setOption(const Option& option);