From df64a250c36070ee8b5b7d2ad93c7995b14798ab Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 5 Apr 2024 08:38:47 -0700 Subject: [PATCH] w --- dart/common/FreeListAllocator.cpp | 2 - dart/common/FreeListAllocator.hpp | 2 - dart/common/MemoryManager.cpp | 28 +- dart/common/MemoryManager.hpp | 16 - dart/config.hpp.in | 10 +- dart/constraint/BoxedLcpConstraintSolver.cpp | 293 +++++++++---------- dart/constraint/BoxedLcpConstraintSolver.hpp | 21 -- 7 files changed, 149 insertions(+), 223 deletions(-) diff --git a/dart/common/FreeListAllocator.cpp b/dart/common/FreeListAllocator.cpp index 6e56fc2303290..0208681e12502 100644 --- a/dart/common/FreeListAllocator.cpp +++ b/dart/common/FreeListAllocator.cpp @@ -372,7 +372,6 @@ void FreeListAllocator::MemoryBlockHeader::merge(MemoryBlockHeader* other) } //============================================================================== -#if DART_BUILD_MODE_DEBUG bool FreeListAllocator::MemoryBlockHeader::isValid() const { if (mPrev != nullptr && mPrev->mNext != this) { @@ -385,6 +384,5 @@ bool FreeListAllocator::MemoryBlockHeader::isValid() const return true; } -#endif } // namespace dart::common diff --git a/dart/common/FreeListAllocator.hpp b/dart/common/FreeListAllocator.hpp index d31911c4484cf..c6c3921cce70d 100644 --- a/dart/common/FreeListAllocator.hpp +++ b/dart/common/FreeListAllocator.hpp @@ -126,10 +126,8 @@ class FreeListAllocator : public MemoryAllocator /// Merges this memory block with the given memory block void merge(MemoryBlockHeader* other); -#if DART_BUILD_MODE_DEBUG /// [Debug only] Returns whether this memory block is valid bool isValid() const; -#endif }; /// Allocates a new memory block for \c sizeToAllocate bytes diff --git a/dart/common/MemoryManager.cpp b/dart/common/MemoryManager.cpp index 4e9972bd7045b..9d4a90289dfde 100644 --- a/dart/common/MemoryManager.cpp +++ b/dart/common/MemoryManager.cpp @@ -32,7 +32,7 @@ #include "dart/common/MemoryManager.hpp" -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG #include "dart/common/Logging.hpp" #endif @@ -49,11 +49,7 @@ MemoryManager& MemoryManager::GetDefault() MemoryManager::MemoryManager(MemoryAllocator& baseAllocator) : mBaseAllocator(baseAllocator), mFreeListAllocator(mBaseAllocator), -#if DART_BUILD_MODE_RELEASE mPoolAllocator(mFreeListAllocator) -#else - mPoolAllocator(mFreeListAllocator.getInternalAllocator()) -#endif { // Do nothing } @@ -73,21 +69,13 @@ MemoryAllocator& MemoryManager::getBaseAllocator() //============================================================================== FreeListAllocator& MemoryManager::getFreeListAllocator() { -#if DART_BUILD_MODE_RELEASE return mFreeListAllocator; -#else - return mFreeListAllocator.getInternalAllocator(); -#endif } //============================================================================== PoolAllocator& MemoryManager::getPoolAllocator() { -#if DART_BUILD_MODE_RELEASE return mPoolAllocator; -#else - return mPoolAllocator.getInternalAllocator(); -#endif } //============================================================================== @@ -144,20 +132,6 @@ void MemoryManager::deallocateUsingPool(void* pointer, size_t bytes) deallocate(Type::Pool, pointer, bytes); } -#if DART_BUILD_MODE_DEBUG -//============================================================================== -bool MemoryManager::hasAllocated(void* pointer, size_t size) const noexcept -{ - if (mFreeListAllocator.hasAllocated(pointer, size)) - return true; - - if (mPoolAllocator.hasAllocated(pointer, size)) - return true; - - return false; -} -#endif - //============================================================================== void MemoryManager::print(std::ostream& os, int indent) const { diff --git a/dart/common/MemoryManager.hpp b/dart/common/MemoryManager.hpp index d258b6ee576f4..bca52e3684ad0 100644 --- a/dart/common/MemoryManager.hpp +++ b/dart/common/MemoryManager.hpp @@ -33,9 +33,6 @@ #ifndef DART_COMMON_MEMORYMANAGER_HPP_ #define DART_COMMON_MEMORYMANAGER_HPP_ -#if DART_BUILD_MODE_DEBUG - #include -#endif #include #include @@ -151,11 +148,6 @@ class MemoryManager final template void destroyUsingPool(T* pointer) noexcept; -#if DART_BUILD_MODE_DEBUG - /// Returns true if a pointer is allocated by the internal allocator. - [[nodiscard]] bool hasAllocated(void* pointer, size_t size) const noexcept; -#endif - /// Prints state of the memory manager. void print(std::ostream& os = std::cout, int indent = 0) const; @@ -167,19 +159,11 @@ class MemoryManager final /// The base allocator to allocate memory chunk. MemoryAllocator& mBaseAllocator; -#if DART_BUILD_MODE_RELEASE /// The free list allocator. FreeListAllocator mFreeListAllocator; /// The pool allocator. PoolAllocator mPoolAllocator; -#else - /// The free list allocator. - FreeListAllocator::Debug mFreeListAllocator; - - /// The pool allocator. - PoolAllocator::Debug mPoolAllocator; -#endif }; } // namespace dart::common diff --git a/dart/config.hpp.in b/dart/config.hpp.in index 7ee2997fd7a80..2f8317d6aaf8d 100644 --- a/dart/config.hpp.in +++ b/dart/config.hpp.in @@ -34,8 +34,7 @@ || (DART_MINOR_VERSION <= y && DART_PATCH_VERSION <= z))) // Deprecated in 6.14 -#define DART_VERSION_AT_LEAST(x, y, z) \ - DART_VERSION_GE(x, y, z) +#define DART_VERSION_AT_LEAST(x, y, z) DART_VERSION_GE(x, y, z) // Deprecated in 6.14 #define DART_MAJOR_MINOR_VERSION_AT_LEAST(x, y) \ @@ -44,8 +43,7 @@ && (DART_MINOR_VERSION > y || (DART_MINOR_VERSION >= y)))) // Deprecated in 6.14 -#define DART_VERSION_AT_MOST(x, y, z) \ - DART_VERSION_LE(x, y, z) +#define DART_VERSION_AT_MOST(x, y, z) DART_VERSION_LE(x, y, z) // Deprecated in 6.14 #define DART_MAJOR_MINOR_VERSION_AT_MOST(x, y) \ @@ -62,10 +60,6 @@ #define DART_COMPILER_MSVC #endif -// Indicates the build mode used for compiling -#cmakedefine01 DART_BUILD_MODE_DEBUG -#cmakedefine01 DART_BUILD_MODE_RELEASE - #cmakedefine01 HAVE_NLOPT #cmakedefine01 HAVE_IPOPT #cmakedefine01 HAVE_PAGMO diff --git a/dart/constraint/BoxedLcpConstraintSolver.cpp b/dart/constraint/BoxedLcpConstraintSolver.cpp index c455b3416af62..fc8d1b48685ee 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.cpp +++ b/dart/constraint/BoxedLcpConstraintSolver.cpp @@ -33,7 +33,7 @@ #include "dart/constraint/BoxedLcpConstraintSolver.hpp" #include -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG #include #include #endif @@ -49,6 +49,151 @@ namespace dart { namespace constraint { +namespace { + +//============================================================================== +#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) { + for (std::size_t j = 0; j < n; ++j) { + if (std::abs(A[nSkip * i + j] - A[nSkip * j + i]) > 1e-6) { + std::cout << "A: " << std::endl; + for (std::size_t k = 0; k < n; ++k) { + for (std::size_t l = 0; l < nSkip; ++l) { + std::cout << std::setprecision(4) << A[k * nSkip + l] << " "; + } + std::cout << std::endl; + } + + std::cout << "A(" << i << ", " << j << "): " << A[nSkip * i + j] + << std::endl; + std::cout << "A(" << j << ", " << i << "): " << A[nSkip * j + i] + << std::endl; + return false; + } + } + } + + return true; +} + +//============================================================================== +bool isSymmetric(std::size_t n, double* A, std::size_t begin, std::size_t end) +{ + std::size_t nSkip = dPAD(n); + for (std::size_t i = begin; i <= end; ++i) { + for (std::size_t j = begin; j <= end; ++j) { + if (std::abs(A[nSkip * i + j] - A[nSkip * j + i]) > 1e-6) { + std::cout << "A: " << std::endl; + for (std::size_t k = 0; k < n; ++k) { + for (std::size_t l = 0; l < nSkip; ++l) { + std::cout << std::setprecision(4) << A[k * nSkip + l] << " "; + } + std::cout << std::endl; + } + + std::cout << "A(" << i << ", " << j << "): " << A[nSkip * i + j] + << std::endl; + std::cout << "A(" << j << ", " << i << "): " << A[nSkip * j + i] + << std::endl; + return false; + } + } + } + + return true; +} + +//============================================================================== +void print( + std::size_t n, + double* A, + double* x, + double* /*lo*/, + double* /*hi*/, + double* b, + double* w, + int* findex) +{ + std::size_t nSkip = dPAD(n); + std::cout << "A: " << std::endl; + for (std::size_t i = 0; i < n; ++i) { + for (std::size_t j = 0; j < nSkip; ++j) { + std::cout << std::setprecision(4) << A[i * nSkip + j] << " "; + } + std::cout << std::endl; + } + + std::cout << "b: "; + for (std::size_t i = 0; i < n; ++i) { + std::cout << std::setprecision(4) << b[i] << " "; + } + std::cout << std::endl; + + std::cout << "w: "; + for (std::size_t i = 0; i < n; ++i) { + std::cout << w[i] << " "; + } + std::cout << std::endl; + + std::cout << "x: "; + for (std::size_t i = 0; i < n; ++i) { + std::cout << x[i] << " "; + } + std::cout << std::endl; + + // std::cout << "lb: "; + // for (int i = 0; i < dim; ++i) + // { + // std::cout << lb[i] << " "; + // } + // std::cout << std::endl; + + // std::cout << "ub: "; + // for (int i = 0; i < dim; ++i) + // { + // std::cout << ub[i] << " "; + // } + // std::cout << std::endl; + + std::cout << "frictionIndex: "; + for (std::size_t i = 0; i < n; ++i) { + std::cout << findex[i] << " "; + } + std::cout << std::endl; + + double* Ax = new double[n]; + + for (std::size_t i = 0; i < n; ++i) { + Ax[i] = 0.0; + } + + for (std::size_t i = 0; i < n; ++i) { + for (std::size_t j = 0; j < n; ++j) { + Ax[i] += A[i * nSkip + j] * x[j]; + } + } + + std::cout << "Ax : "; + for (std::size_t i = 0; i < n; ++i) { + std::cout << Ax[i] << " "; + } + std::cout << std::endl; + + std::cout << "b + w: "; + for (std::size_t i = 0; i < n; ++i) { + std::cout << b[i] + w[i] << " "; + } + std::cout << std::endl; + + delete[] Ax; +} +#endif + +} // namespace + //============================================================================== BoxedLcpConstraintSolver::BoxedLcpConstraintSolver( double timeStep, @@ -152,11 +297,7 @@ void BoxedLcpConstraintSolver::solveConstrainedGroup(ConstrainedGroup& group) return; const int nSkip = dPAD(n); -#if DART_BUILD_MODE_RELEASE mA.resize(n, nSkip); -#else // debug - mA.setZero(n, nSkip); -#endif mX.resize(n); mB.resize(n); mW.setZero(n); // set w to 0 @@ -314,147 +455,5 @@ void BoxedLcpConstraintSolver::solveConstrainedGroup(ConstrainedGroup& group) } } -//============================================================================== -#if DART_BUILD_MODE_DEBUG -bool BoxedLcpConstraintSolver::isSymmetric(std::size_t n, double* A) -{ - std::size_t nSkip = dPAD(n); - for (std::size_t i = 0; i < n; ++i) { - for (std::size_t j = 0; j < n; ++j) { - if (std::abs(A[nSkip * i + j] - A[nSkip * j + i]) > 1e-6) { - std::cout << "A: " << std::endl; - for (std::size_t k = 0; k < n; ++k) { - for (std::size_t l = 0; l < nSkip; ++l) { - std::cout << std::setprecision(4) << A[k * nSkip + l] << " "; - } - std::cout << std::endl; - } - - std::cout << "A(" << i << ", " << j << "): " << A[nSkip * i + j] - << std::endl; - std::cout << "A(" << j << ", " << i << "): " << A[nSkip * j + i] - << std::endl; - return false; - } - } - } - - return true; -} - -//============================================================================== -bool BoxedLcpConstraintSolver::isSymmetric( - std::size_t n, double* A, std::size_t begin, std::size_t end) -{ - std::size_t nSkip = dPAD(n); - for (std::size_t i = begin; i <= end; ++i) { - for (std::size_t j = begin; j <= end; ++j) { - if (std::abs(A[nSkip * i + j] - A[nSkip * j + i]) > 1e-6) { - std::cout << "A: " << std::endl; - for (std::size_t k = 0; k < n; ++k) { - for (std::size_t l = 0; l < nSkip; ++l) { - std::cout << std::setprecision(4) << A[k * nSkip + l] << " "; - } - std::cout << std::endl; - } - - std::cout << "A(" << i << ", " << j << "): " << A[nSkip * i + j] - << std::endl; - std::cout << "A(" << j << ", " << i << "): " << A[nSkip * j + i] - << std::endl; - return false; - } - } - } - - return true; -} - -//============================================================================== -void BoxedLcpConstraintSolver::print( - std::size_t n, - double* A, - double* x, - double* /*lo*/, - double* /*hi*/, - double* b, - double* w, - int* findex) -{ - std::size_t nSkip = dPAD(n); - std::cout << "A: " << std::endl; - for (std::size_t i = 0; i < n; ++i) { - for (std::size_t j = 0; j < nSkip; ++j) { - std::cout << std::setprecision(4) << A[i * nSkip + j] << " "; - } - std::cout << std::endl; - } - - std::cout << "b: "; - for (std::size_t i = 0; i < n; ++i) { - std::cout << std::setprecision(4) << b[i] << " "; - } - std::cout << std::endl; - - std::cout << "w: "; - for (std::size_t i = 0; i < n; ++i) { - std::cout << w[i] << " "; - } - std::cout << std::endl; - - std::cout << "x: "; - for (std::size_t i = 0; i < n; ++i) { - std::cout << x[i] << " "; - } - std::cout << std::endl; - - // std::cout << "lb: "; - // for (int i = 0; i < dim; ++i) - // { - // std::cout << lb[i] << " "; - // } - // std::cout << std::endl; - - // std::cout << "ub: "; - // for (int i = 0; i < dim; ++i) - // { - // std::cout << ub[i] << " "; - // } - // std::cout << std::endl; - - std::cout << "frictionIndex: "; - for (std::size_t i = 0; i < n; ++i) { - std::cout << findex[i] << " "; - } - std::cout << std::endl; - - double* Ax = new double[n]; - - for (std::size_t i = 0; i < n; ++i) { - Ax[i] = 0.0; - } - - for (std::size_t i = 0; i < n; ++i) { - for (std::size_t j = 0; j < n; ++j) { - Ax[i] += A[i * nSkip + j] * x[j]; - } - } - - std::cout << "Ax : "; - for (std::size_t i = 0; i < n; ++i) { - std::cout << Ax[i] << " "; - } - std::cout << std::endl; - - std::cout << "b + w: "; - for (std::size_t i = 0; i < n; ++i) { - std::cout << b[i] + w[i] << " "; - } - std::cout << std::endl; - - delete[] Ax; -} -#endif - } // namespace constraint } // namespace dart diff --git a/dart/constraint/BoxedLcpConstraintSolver.hpp b/dart/constraint/BoxedLcpConstraintSolver.hpp index 3220c1876aaad..fe2db541aa2e2 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.hpp +++ b/dart/constraint/BoxedLcpConstraintSolver.hpp @@ -153,27 +153,6 @@ class BoxedLcpConstraintSolver : public ConstraintSolver /// Cache data for boxed LCP formulation Eigen::VectorXi mOffset; - -#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