From be69a6973890658c088bb41056ff3c27f56f2444 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 5 Apr 2024 08:38:47 -0700 Subject: [PATCH] w --- CMakeLists.txt | 64 ++-- dart/common/FreeListAllocator.cpp | 2 - dart/common/FreeListAllocator.hpp | 2 - dart/common/Logging.hpp | 7 +- dart/common/MemoryManager.cpp | 28 +- dart/common/MemoryManager.hpp | 16 - dart/config.hpp.in | 12 +- dart/constraint/BoxedLcpConstraintSolver.cpp | 273 ++++++++--------- dart/constraint/BoxedLcpConstraintSolver.hpp | 21 -- 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 - dart/dynamics/BodyNode.cpp | 2 +- dart/dynamics/EulerJoint.cpp | 6 +- dart/dynamics/Node.cpp | 4 +- dart/dynamics/Skeleton.cpp | 12 +- dart/math/Geometry.cpp | 2 +- examples/wam_ikfast/Helpers.cpp | 2 +- python/dartpy/eigen_geometry_pybind.cpp | 8 +- tests/integration/test_Constraint.cpp | 4 +- tests/integration/test_Dynamics.cpp | 46 +-- tests/integration/test_IkFast.cpp | 2 +- tests/integration/test_Joints.cpp | 12 +- tests/integration/test_MetaSkeleton.cpp | 2 +- .../test_MultiObjectiveOptimization.cpp | 8 +- tests/integration/test_Skeleton.cpp | 10 +- tests/integration/test_SoftDynamics.cpp | 4 +- tests/integration/test_World.cpp | 2 +- tests/regression/test_Issue1184.cpp | 2 +- tests/regression/test_Issue1596.cpp | 2 +- tests/unit/common/test_MemoryManager.cpp | 4 +- tests/unit/math/test_Math.cpp | 2 +- 41 files changed, 514 insertions(+), 659 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index fc5249444a5ce..abcef2f1cd2dd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -162,24 +162,11 @@ endif() #=============================================================================== # Build type settings #=============================================================================== -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the type of build, options are: Debug | Release | RelWithDebInfo | MinSizeRel" FORCE) -endif() -string(TOUPPER ${CMAKE_BUILD_TYPE} CMAKE_BUILD_TYPE_UPPERCASE) - -set(DART_BUILD_MODE_DEBUG FALSE) -set(DART_BUILD_MODE_RELEASE FALSE) - -if("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "DEBUG") - set(DART_BUILD_MODE_DEBUG TRUE) -elseif("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "RELEASE") - set(DART_BUILD_MODE_RELEASE TRUE) -elseif("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "RELWITHDEBINFO") - set(DART_BUILD_MODE_RELEASE TRUE) -elseif("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "MINSIZEREL") - set(DART_BUILD_MODE_RELEASE TRUE) -else() - message(WARNING "CMAKE_BUILD_TYPE ${CMAKE_BUILD_TYPE} unknown. Valid options are: Debug | Release | RelWithDebInfo | MinSizeRel") +if(NOT CMAKE_CONFIGURATION_TYPES) + if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the type of build." FORCE) + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo") + endif() endif() # Active log level: @@ -191,14 +178,33 @@ endif() # - ERROR: To enable log with DART_ERROR() and below # - FATAL: To enable log with DART_FATAL() # - OFF: To turn off all the logs -if(DART_BUILD_MODE_DEBUG) - set(DART_ACTIVE_LOG_LEVEL "DEBUG" CACHE STRING "Compile time active log level to enable") +if(NOT DART_ACTIVE_LOG_LEVEL) + if(NOT CMAKE_CONFIGURATION_TYPES AND CMAKE_BUILD_TYPE STREQUAL "Debug") + set(DART_ACTIVE_LOG_LEVEL "DEBUG" CACHE STRING "Compile time active log level to enable") + else() + set(DART_ACTIVE_LOG_LEVEL "INFO" CACHE STRING "Compile time active log level to enable") + endif() + set_property(CACHE DART_ACTIVE_LOG_LEVEL PROPERTY STRINGS TRACE DEBUG INFO WARN ERROR FATAL OFF) +endif() +if(DART_ACTIVE_LOG_LEVEL STREQUAL "TRACE") + set(DART_ACTIVE_LOG_LEVEL_VALUE 0) +elseif(DART_ACTIVE_LOG_LEVEL STREQUAL "DEBUG") + set(DART_ACTIVE_LOG_LEVEL_VALUE 1) +elseif(DART_ACTIVE_LOG_LEVEL STREQUAL "INFO") + set(DART_ACTIVE_LOG_LEVEL_VALUE 2) +elseif(DART_ACTIVE_LOG_LEVEL STREQUAL "WARN") + set(DART_ACTIVE_LOG_LEVEL_VALUE 3) +elseif(DART_ACTIVE_LOG_LEVEL STREQUAL "ERROR") + set(DART_ACTIVE_LOG_LEVEL_VALUE 4) +elseif(DART_ACTIVE_LOG_LEVEL STREQUAL "FATAL") + set(DART_ACTIVE_LOG_LEVEL_VALUE 5) +elseif(DART_ACTIVE_LOG_LEVEL STREQUAL "OFF") + set(DART_ACTIVE_LOG_LEVEL_VALUE 6) else() - set(DART_ACTIVE_LOG_LEVEL "INFO" CACHE STRING "Compile time active log level to enable") + message(FATAL_ERROR "Unknown DART_ACTIVE_LOG_LEVEL: ${DART_ACTIVE_LOG_LEVEL}") endif() -set_property(CACHE DART_ACTIVE_LOG_LEVEL PROPERTY STRINGS TRACE DEBUG INFO WARN ERROR FATAL OFF) -if(DART_BUILD_MODE_DEBUG) +if(NOT CMAKE_CONFIGURATION_TYPES AND CMAKE_BUILD_TYPE STREQUAL "Debug") option(DART_TREAT_WARNINGS_AS_ERRORS "Treat warnings as errors" OFF) else() option(DART_TREAT_WARNINGS_AS_ERRORS "Treat warnings as errors" ON) @@ -355,14 +361,12 @@ if(DART_VERBOSE) message(STATUS "Build gui::osg : ${DART_BUILD_GUI_OSG}") message(STATUS "Install path : ${CMAKE_INSTALL_PREFIX}") message(STATUS "CXX_FLAGS : ${CMAKE_CXX_FLAGS}") - if(${CMAKE_BUILD_TYPE_UPPERCASE} STREQUAL "RELEASE") - message(STATUS "CXX_FLAGS_RELEASE: ${CMAKE_CXX_FLAGS_RELEASE}") - elseif(${CMAKE_BUILD_TYPE_UPPERCASE} STREQUAL "DEBUG") - message(STATUS "CXX_FLAGS_DEBUG : ${CMAKE_CXX_FLAGS_DEBUG}") - elseif(${CMAKE_BUILD_TYPE_UPPERCASE} STREQUAL "RELWITHDEBINFO") + if(NOT CMAKE_CONFIGURATION_TYPES) + message(STATUS "CXX_FLAGS_RELEASE : ${CMAKE_CXX_FLAGS_RELEASE}") + message(STATUS "CXX_FLAGS_DEBUG : ${CMAKE_CXX_FLAGS_DEBUG}") + message(STATUS "CXX_FLAGS_MINSIZEREL : ${CXX_FLAGS_MINSIZEREL}") message(STATUS "CXX_FLAGS_RELWITHDEBINFO: ${CMAKE_CXX_FLAGS_RELWITHDEBINFO}") - elseif(${CMAKE_BUILD_TYPE_UPPERCASE} STREQUAL "PROFILE") - message(STATUS "CXX_FLAGS_PROFILE: ${CMAKE_CXX_FLAGS_PROFILE}") + message(STATUS "CXX_FLAGS_PROFILE : ${CMAKE_CXX_FLAGS_PROFILE}") endif() message(STATUS "DART_SOURCE_DIR : ${DART_SOURCE_DIR}") message(STATUS "DART_BINARY_DIR : ${DART_BINARY_DIR}") 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/Logging.hpp b/dart/common/Logging.hpp index 2ec2251c149e8..4812a0efa4f09 100644 --- a/dart/common/Logging.hpp +++ b/dart/common/Logging.hpp @@ -33,6 +33,8 @@ #ifndef DART_COMMON_LOGGING_HPP_ #define DART_COMMON_LOGGING_HPP_ +#include + // clang-format off #define DART_LOG_LEVEL_TRACE 0 #define DART_LOG_LEVEL_DEBUG 1 @@ -43,11 +45,6 @@ #define DART_LOG_LEVEL_OFF 6 // clang-format on -// Default active log level -#if !defined(DART_ACTIVE_LOG_LEVEL) - #define DART_ACTIVE_LOG_LEVEL DART_LOG_LEVEL_INFO -#endif - #if DART_ACTIVE_LOG_LEVEL <= DART_LOG_LEVEL_TRACE #define DART_TRACE(...) ::dart::common::trace(__VA_ARGS__) #else 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 fc0e84852417b..230491ab7c5d2 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 @@ -98,3 +92,5 @@ #cmakedefine01 DART_USE_SYSTEM_IMGUI #cmakedefine01 DART_BUILD_PROFILE + +#define DART_ACTIVE_LOG_LEVEL @DART_ACTIVE_LOG_LEVEL_VALUE@ diff --git a/dart/constraint/BoxedLcpConstraintSolver.cpp b/dart/constraint/BoxedLcpConstraintSolver.cpp index c455b3416af62..53260cb5f5166 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,131 @@ namespace dart { namespace constraint { +namespace { + +#ifndef NDEBUG + +//============================================================================== +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; +} + +//============================================================================== +bool isSymmetric(std::size_t n, double* A) +{ + return isSymmetric(n, A, 0, n - 1); +} + +//============================================================================== +[[maybe_unused]] 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 +277,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 +435,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 7d9ff613c9134..fa3fa97e5b5b1 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 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..b2d01375fa062 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( +[[maybe_unused]] 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..f0f14a2830ada 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( +[[maybe_unused]] 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); diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index 31e64a5d59a47..edba35cbaeb81 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -1383,7 +1383,7 @@ void BodyNode::init(const SkeletonPtr& _skeleton) mConstDependentDofs.push_back(_skeleton->getDof(index)); } -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG // Check whether there is duplicated indices. std::size_t nDepGenCoordIndices = mDependentGenCoordIndices.size(); for (std::size_t i = 0; i < nDepGenCoordIndices; ++i) { diff --git a/dart/dynamics/EulerJoint.cpp b/dart/dynamics/EulerJoint.cpp index 326b21c5ee4a0..18c66d37ee74a 100644 --- a/dart/dynamics/EulerJoint.cpp +++ b/dart/dynamics/EulerJoint.cpp @@ -209,7 +209,7 @@ Eigen::Matrix EulerJoint::getRelativeJacobianStatic( J1 << s2, c2, 0.0, 0.0, 0.0, 0.0; J2 << 0.0, 0.0, 1.0, 0.0, 0.0, 0.0; -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG if (std::abs(getPositionsStatic()[1]) == math::constantsd::pi() * 0.5) std::cout << "Singular configuration in ZYX-euler joint [" << Joint::mAspectProperties.mName << "]. (" << _positions[0] @@ -232,7 +232,7 @@ Eigen::Matrix EulerJoint::getRelativeJacobianStatic( J1 << 0.0, c2, -s2, 0.0, 0.0, 0.0; J2 << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0; -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG if (std::abs(_positions[1]) == math::constantsd::pi() * 0.5) std::cout << "Singular configuration in ZYX-euler joint [" << Joint::mAspectProperties.mName << "]. (" << _positions[0] @@ -254,7 +254,7 @@ Eigen::Matrix EulerJoint::getRelativeJacobianStatic( assert(!math::isNan(J)); -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG Eigen::MatrixXd JTJ = J.transpose() * J; Eigen::FullPivLU luJTJ(JTJ); // Eigen::FullPivLU luS(mS); diff --git a/dart/dynamics/Node.cpp b/dart/dynamics/Node.cpp index ac26b873b5028..b3eba85dd9c1b 100644 --- a/dart/dynamics/Node.cpp +++ b/dart/dynamics/Node.cpp @@ -189,7 +189,7 @@ void Node::attach() // If we are in release mode, and the Node believes it is attached, then we // can shortcut this procedure -#if DART_BUILD_MODE_RELEASE +#ifdef NDEBUG if (mAmAttached) return; #endif @@ -241,7 +241,7 @@ void Node::stageForRemoval() // If we are in release mode, and the Node believes it is detached, then we // can shortcut this procedure. -#if DART_BUILD_MODE_RELEASE +#ifdef NDEBUG if (!mAmAttached) return; #endif diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 16a76c4616f0b..746bcada3de00 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -2106,7 +2106,7 @@ void Skeleton::constructNewTree() //============================================================================== void Skeleton::registerBodyNode(BodyNode* _newBodyNode) { -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG std::vector::iterator repeat = std::find( mSkelCache.mBodyNodes.begin(), mSkelCache.mBodyNodes.end(), _newBodyNode); if (repeat != mSkelCache.mBodyNodes.end()) { @@ -2155,7 +2155,7 @@ void Skeleton::registerBodyNode(BodyNode* _newBodyNode) updateTotalMass(); updateCacheDimensions(_newBodyNode->mTreeIndex); -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG for (std::size_t i = 0; i < mSkelCache.mBodyNodes.size(); ++i) { if (mSkelCache.mBodyNodes[i]->mIndexInSkeleton != i) { dterr << "[Skeleton::registerBodyNode] BodyNode named [" @@ -3656,7 +3656,7 @@ void Skeleton::updateBiasImpulse(BodyNode* _bodyNode) // This skeleton should contain _bodyNode assert(_bodyNode->getSkeleton().get() == this); -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG // All the constraint impulse should be zero for (std::size_t i = 0; i < mSkelCache.mBodyNodes.size(); ++i) assert( @@ -3687,7 +3687,7 @@ void Skeleton::updateBiasImpulse( // This skeleton should contain _bodyNode assert(_bodyNode->getSkeleton().get() == this); -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG // All the constraint impulse should be zero for (std::size_t i = 0; i < mSkelCache.mBodyNodes.size(); ++i) assert( @@ -3734,7 +3734,7 @@ void Skeleton::updateBiasImpulse( assert(_bodyNode1->getSkeleton().get() == this); assert(_bodyNode2->getSkeleton().get() == this); -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG // All the constraint impulse should be zero for (std::size_t i = 0; i < mSkelCache.mBodyNodes.size(); ++i) assert( @@ -3775,7 +3775,7 @@ void Skeleton::updateBiasImpulse( std::find(mSoftBodyNodes.begin(), mSoftBodyNodes.end(), _softBodyNode) != mSoftBodyNodes.end()); -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG // All the constraint impulse should be zero for (std::size_t i = 0; i < mSkelCache.mBodyNodes.size(); ++i) assert( diff --git a/dart/math/Geometry.cpp b/dart/math/Geometry.cpp index ac77033cd722a..2944cb35c4c23 100644 --- a/dart/math/Geometry.cpp +++ b/dart/math/Geometry.cpp @@ -1478,7 +1478,7 @@ bool verifyTransform(const Eigen::Isometry3d& _T) Eigen::Vector3d fromSkewSymmetric(const Eigen::Matrix3d& _m) { -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG if (std::abs(_m(0, 0)) > DART_EPSILON || std::abs(_m(1, 1)) > DART_EPSILON || std::abs(_m(2, 2)) > DART_EPSILON) { dtwarn << "[math::fromSkewSymmetric] Not skew a symmetric matrix:\n" diff --git a/examples/wam_ikfast/Helpers.cpp b/examples/wam_ikfast/Helpers.cpp index 7f9a81d40956f..1a1d2c21819da 100644 --- a/examples/wam_ikfast/Helpers.cpp +++ b/examples/wam_ikfast/Helpers.cpp @@ -109,7 +109,7 @@ void setupEndEffectors(const dart::dynamics::SkeletonPtr& wam) std::stringstream ss; ss << DART_SHARED_LIB_PREFIX << "wamIk"; -#if (DART_OS_LINUX || DART_OS_MACOS) && DART_BUILD_MODE_DEBUG +#if (DART_OS_LINUX || DART_OS_MACOS) && !defined(NDEBUG) ss << "d"; #endif ss << "." << DART_SHARED_LIB_EXTENSION; diff --git a/python/dartpy/eigen_geometry_pybind.cpp b/python/dartpy/eigen_geometry_pybind.cpp index c6a04b0ea97ef..5bbaeb61a4b79 100644 --- a/python/dartpy/eigen_geometry_pybind.cpp +++ b/python/dartpy/eigen_geometry_pybind.cpp @@ -34,12 +34,12 @@ #include "eigen_geometry_pybind.h" -#include -#include +#include "pybind11/pybind11.h" #include -#include "pybind11/pybind11.h" +#include +#include using std::fabs; @@ -60,7 +60,7 @@ namespace { // N.B. Use a loose tolerance, so that we don't have to be super strict with // C++. -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG const double kCheckTolerance = 1e-5; #endif diff --git a/tests/integration/test_Constraint.cpp b/tests/integration/test_Constraint.cpp index e1c0c7c124f76..0da2177130e0e 100644 --- a/tests/integration/test_Constraint.cpp +++ b/tests/integration/test_Constraint.cpp @@ -112,7 +112,7 @@ void ConstraintTest::SingleContactTest(const std::string& /*_fileName*/) // Settings //---------------------------------------------------------------------------- // Number of random state tests for each skeletons -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG // std::size_t testCount = 1; #else // std::size_t testCount = 1; @@ -222,7 +222,7 @@ TEST_F(ConstraintTest, SingleContactTest) { // for (int i = 0; i < getList().size(); ++i) // { - // #if DART_BUILD_MODE_DEBUG + // #ifndef NDEBUG // dtdbg << getList()[i] << std::endl; // #endif // SingleContactTest(getList()[i]); diff --git a/tests/integration/test_Dynamics.cpp b/tests/integration/test_Dynamics.cpp index 09f9755505e56..d5f6a04913bb5 100644 --- a/tests/integration/test_Dynamics.cpp +++ b/tests/integration/test_Dynamics.cpp @@ -862,7 +862,7 @@ void DynamicsTest::testJacobians(const common::Uri& uri) //----------------------------- Settings ------------------------------------- const double TOLERANCE = 1.0e-6; -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG int nTestItr = 2; #else int nTestItr = 5; @@ -958,7 +958,7 @@ void DynamicsTest::testJacobians(const common::Uri& uri) compareBodyNodeFkToJacobianRelative(bn, bn, Frame::World(), TOLERANCE); -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG if (skeleton->getNumBodyNodes() == 0u) continue; @@ -1034,7 +1034,7 @@ void DynamicsTest::testFiniteDifferenceGeneralizedCoordinates( using namespace utils; //----------------------------- Settings ------------------------------------- -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG int nRandomItr = 2; #else int nRandomItr = 10; @@ -1121,7 +1121,7 @@ void DynamicsTest::testFiniteDifferenceBodyNodeVelocity(const common::Uri& uri) using namespace utils; //----------------------------- Settings ------------------------------------- -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG int nRandomItr = 2; std::size_t numSteps = 1e+1; #else @@ -1211,7 +1211,7 @@ void DynamicsTest::testFiniteDifferenceBodyNodeAcceleration( //----------------------------- Settings ------------------------------------- const double TOLERANCE = 1.0e-2; -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG int nRandomItr = 2; #else int nRandomItr = 10; @@ -1342,7 +1342,7 @@ void DynamicsTest::testFiniteDifferenceBodyNodeAcceleration( //============================================================================== void testForwardKinematicsSkeleton(const dynamics::SkeletonPtr& skel) { -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG std::size_t nRandomItr = 1e+1; std::size_t numSteps = 1e+1; #else @@ -1471,7 +1471,7 @@ void DynamicsTest::testInverseDynamics(const common::Uri& uri) { //---------------------------- Settings -------------------------------------- // Number of random state tests for each skeleton -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG const std::size_t nRandomItr = 2; #else const std::size_t nRandomItr = 100; @@ -1554,7 +1554,7 @@ void DynamicsTest::compareEquationsOfMotion(const common::Uri& uri) //---------------------------- Settings -------------------------------------- // Number of random state tests for each skeletons -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG std::size_t nRandomItr = 2; #else std::size_t nRandomItr = 100; @@ -1831,7 +1831,7 @@ void DynamicsTest::testCenterOfMass(const common::Uri& uri) //---------------------------- Settings -------------------------------------- // Number of random state tests for each skeletons -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG std::size_t nRandomItr = 2; #else std::size_t nRandomItr = 100; @@ -1984,7 +1984,7 @@ void DynamicsTest::testCenterOfMassFreeFall(const common::Uri& uri) //---------------------------- Settings -------------------------------------- // Number of random state tests for each skeletons -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG std::size_t nRandomItr = 2; #else std::size_t nRandomItr = 10; @@ -2021,7 +2021,7 @@ void DynamicsTest::testCenterOfMassFreeFall(const common::Uri& uri) auto dof = skel->getNumDofs(); if (nullptr == rootFreeJoint || !skel->isMobile() || 0 == dof) { -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG dtmsg << "Skipping COM free fall test for Skeleton [" << skel->getName() << "] since the Skeleton doesn't have FreeJoint at the root body " << " or immobile." << endl; @@ -2090,7 +2090,7 @@ void DynamicsTest::testConstraintImpulse(const common::Uri& uri) //---------------------------- Settings -------------------------------------- // Number of random state tests for each skeletons -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG std::size_t nRandomItr = 1; #else std::size_t nRandomItr = 1; @@ -2177,7 +2177,7 @@ void DynamicsTest::testImpulseBasedDynamics(const common::Uri& uri) //---------------------------- Settings -------------------------------------- // Number of random state tests for each skeletons -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG std::size_t nRandomItr = 1; #else std::size_t nRandomItr = 100; @@ -2260,7 +2260,7 @@ void DynamicsTest::testImpulseBasedDynamics(const common::Uri& uri) TEST_F(DynamicsTest, testJacobians) { for (std::size_t i = 0; i < getList().size(); ++i) { -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG dtdbg << getList()[i].toString() << std::endl; #endif testJacobians(getList()[i]); @@ -2271,7 +2271,7 @@ TEST_F(DynamicsTest, testJacobians) TEST_F(DynamicsTest, testFiniteDifference) { for (std::size_t i = 0; i < getList().size(); ++i) { -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG dtdbg << getList()[i].toString() << std::endl; #endif testFiniteDifferenceGeneralizedCoordinates(getList()[i]); @@ -2284,7 +2284,7 @@ TEST_F(DynamicsTest, testFiniteDifference) TEST_F(DynamicsTest, testForwardKinematics) { for (std::size_t i = 0; i < getList().size(); ++i) { -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG dtdbg << getList()[i].toString() << std::endl; #endif testForwardKinematics(getList()[i]); @@ -2295,7 +2295,7 @@ TEST_F(DynamicsTest, testForwardKinematics) TEST_F(DynamicsTest, testInverseDynamics) { for (std::size_t i = 0; i < getList().size(); ++i) { -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG dtdbg << getList()[i].toString() << std::endl; #endif testInverseDynamics(getList()[i]); @@ -2321,7 +2321,7 @@ TEST_F(DynamicsTest, compareEquationsOfMotion) } //////////////////////////////////////////////////////////////////////////// -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG dtdbg << getList()[i].toString() << std::endl; #endif compareEquationsOfMotion(getList()[i]); @@ -2332,7 +2332,7 @@ TEST_F(DynamicsTest, compareEquationsOfMotion) TEST_F(DynamicsTest, testCenterOfMass) { for (std::size_t i = 0; i < getList().size(); ++i) { -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG dtdbg << getList()[i].toString() << std::endl; #endif testCenterOfMass(getList()[i]); @@ -2343,7 +2343,7 @@ TEST_F(DynamicsTest, testCenterOfMass) TEST_F(DynamicsTest, testCenterOfMassFreeFall) { for (std::size_t i = 0; i < getList().size(); ++i) { -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG dtdbg << getList()[i].toString() << std::endl; #endif testCenterOfMassFreeFall(getList()[i]); @@ -2354,7 +2354,7 @@ TEST_F(DynamicsTest, testCenterOfMassFreeFall) TEST_F(DynamicsTest, testConstraintImpulse) { for (std::size_t i = 0; i < getList().size(); ++i) { -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG dtdbg << getList()[i].toString() << std::endl; #endif testConstraintImpulse(getList()[i]); @@ -2365,7 +2365,7 @@ TEST_F(DynamicsTest, testConstraintImpulse) TEST_F(DynamicsTest, testImpulseBasedDynamics) { for (std::size_t i = 0; i < getList().size(); ++i) { -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG dtdbg << getList()[i].toString() << std::endl; #endif testImpulseBasedDynamics(getList()[i]); @@ -2377,7 +2377,7 @@ TEST_F(DynamicsTest, HybridDynamics) { const double tol = 1e-8; const double timeStep = 1e-3; -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG const std::size_t numFrames = 50; // 0.05 secs #else const std::size_t numFrames = 5e+3; // 5 secs diff --git a/tests/integration/test_IkFast.cpp b/tests/integration/test_IkFast.cpp index 6876aa59d90ad..f608eac09d7df 100644 --- a/tests/integration/test_IkFast.cpp +++ b/tests/integration/test_IkFast.cpp @@ -134,7 +134,7 @@ TEST(IkFast, LoadWamArmIk) ik->setHierarchyLevel(1); std::stringstream ss; ss << DART_SHARED_LIB_PREFIX << "GeneratedWamIkFast"; -#if (DART_OS_LINUX || DART_OS_MACOS) && DART_BUILD_MODE_DEBUG +#if (DART_OS_LINUX || DART_OS_MACOS) && !defined(NDEBUG) ss << "d"; #endif ss << "." << DART_SHARED_LIB_EXTENSION; diff --git a/tests/integration/test_Joints.cpp b/tests/integration/test_Joints.cpp index 05fecf636b3b4..189410e55be9c 100644 --- a/tests/integration/test_Joints.cpp +++ b/tests/integration/test_Joints.cpp @@ -455,7 +455,7 @@ TEST_F(JOINTS, POSITION_LIMIT) joint1->setPositionLowerLimit(0, -limit1); joint1->setPositionUpperLimit(0, limit1); -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG double simTime = 0.2; #else double simTime = 2.0; @@ -535,7 +535,7 @@ TEST_F(JOINTS, POSITION_AND_VELOCITY_LIMIT) joint1->setVelocityLowerLimit(0, -velLimit1); joint1->setVelocityUpperLimit(0, velLimit1); -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG double simTime = 0.2; #else double simTime = 2.0; @@ -694,7 +694,7 @@ void testJointCoulombFrictionForce(double _timeStep) EXPECT_EQ(joint0->getCoulombFriction(0), frictionForce); EXPECT_EQ(joint1->getCoulombFriction(0), frictionForce); -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG double simTime = 0.2; #else double simTime = 2.0; @@ -950,7 +950,7 @@ void testServoMotor() for (auto pendulum : pendulums) world->addSkeleton(pendulum); -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG double simTime = 0.2; #else double simTime = 2.0; @@ -1073,7 +1073,7 @@ void testMimicJoint() world->addSkeleton(pendulum); -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG double simTime = 0.2; #else double simTime = 2.0; @@ -1159,7 +1159,7 @@ TEST_F(JOINTS, JOINT_COULOMB_FRICTION_AND_POSITION_LIMIT) EXPECT_EQ(joint0->getCoulombFriction(0), frictionForce); EXPECT_EQ(joint1->getCoulombFriction(0), frictionForce); -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG double simTime = 0.2; #else double simTime = 2.0; diff --git a/tests/integration/test_MetaSkeleton.cpp b/tests/integration/test_MetaSkeleton.cpp index 801a71543892c..32df4bb840161 100644 --- a/tests/integration/test_MetaSkeleton.cpp +++ b/tests/integration/test_MetaSkeleton.cpp @@ -105,7 +105,7 @@ TEST(MetaSkeleton, Referential) { std::vector skeletons = getSkeletons(); -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG std::size_t numIterations = 1; #else // Release mode std::size_t numIterations = 20; diff --git a/tests/integration/test_MultiObjectiveOptimization.cpp b/tests/integration/test_MultiObjectiveOptimization.cpp index 75e5ff6eebd64..3c51090ac467f 100644 --- a/tests/integration/test_MultiObjectiveOptimization.cpp +++ b/tests/integration/test_MultiObjectiveOptimization.cpp @@ -101,12 +101,12 @@ class Func2 : public Function //============================================================================== void testZDT1(MultiObjectiveSolver& solver) { -#if DART_BUILD_MODE_RELEASE +#ifdef NDEBUG std::size_t numSolutions = 50; #else std::size_t numSolutions = 10; #endif -#if DART_BUILD_MODE_RELEASE +#ifdef NDEBUG std::size_t iterationNum = 1000; #else std::size_t iterationNum = 200; @@ -137,12 +137,12 @@ void testZDT1Generic(MultiObjectiveSolver& solver) pFuncs.push_back(pFunc1); pFuncs.push_back(pFunc2); -#if DART_BUILD_MODE_RELEASE +#ifdef NDEBUG std::size_t numSolutions = 50; #else std::size_t numSolutions = 10; #endif -#if DART_BUILD_MODE_RELEASE +#ifdef NDEBUG std::size_t iterationNum = 1000; #else std::size_t iterationNum = 200; diff --git a/tests/integration/test_Skeleton.cpp b/tests/integration/test_Skeleton.cpp index df991d1940171..76d4124ff3116 100644 --- a/tests/integration/test_Skeleton.cpp +++ b/tests/integration/test_Skeleton.cpp @@ -109,7 +109,7 @@ TEST(Skeleton, Restructuring) { std::vector skeletons = getSkeletons(); -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG std::size_t numIterations = 10; #else std::size_t numIterations = 2 * skeletons.size(); @@ -631,7 +631,7 @@ TEST(Skeleton, NodePersistence) // The Node has been removed, so no reference to it will exist in the // Skeleton -#if DART_BUILD_MODE_RELEASE +#ifdef NDEBUG EXPECT_NE(skel->getEndEffector("manip"), manip); EXPECT_EQ(skel->getEndEffector("manip"), nullptr); @@ -639,7 +639,7 @@ TEST(Skeleton, NodePersistence) EXPECT_EQ(skel->getEndEffector(0), nullptr); #endif // Release Mode -#if DART_BUILD_MODE_RELEASE +#ifdef NDEBUG // But it will not remain in the BodyNode's indexing. // Note: We should only run this test in release mode, because otherwise it // will trigger an assertion. @@ -703,7 +703,7 @@ TEST(Skeleton, NodePersistence) // The Node has been removed, so no reference to it will exist in the // Skeleton -#if DART_BUILD_MODE_RELEASE +#ifdef NDEBUG EXPECT_NE(skel->getNode("node"), node); EXPECT_EQ(skel->getNode("node"), nullptr); @@ -711,7 +711,7 @@ TEST(Skeleton, NodePersistence) EXPECT_EQ(skel->getNode(0), nullptr); #endif // Release Mode -#if DART_BUILD_MODE_RELEASE +#ifdef NDEBUG // But it will not remain in the BodyNode's indexing. // Note: We should only run this test in release mode, because otherwise it // will trigger an assertion. diff --git a/tests/integration/test_SoftDynamics.cpp b/tests/integration/test_SoftDynamics.cpp index 2ea4dda4bb594..52df635cd5d7b 100644 --- a/tests/integration/test_SoftDynamics.cpp +++ b/tests/integration/test_SoftDynamics.cpp @@ -236,7 +236,7 @@ void SoftDynamicsTest::compareEquationsOfMotion(const std::string& _fileName) //---------------------------- Settings -------------------------------------- // Number of random state tests for each skeletons -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG std::size_t nRandomItr = 1; #else std::size_t nRandomItr = 1; @@ -430,7 +430,7 @@ TEST_F(SoftDynamicsTest, compareEquationsOfMotion) // for (int i = 0; i < getList().size(); ++i) // { - // #if DART_BUILD_MODE_DEBUG + // #ifndef NDEBUG // dtdbg << getList()[i] << std::endl; // #endif // compareEquationsOfMotion(getList()[i]); diff --git a/tests/integration/test_World.cpp b/tests/integration/test_World.cpp index 74d0949b1d1e2..201106d4602db 100644 --- a/tests/integration/test_World.cpp +++ b/tests/integration/test_World.cpp @@ -223,7 +223,7 @@ TEST(World, Cloning) for (std::size_t j = 1; j < 5; ++j) clones.push_back(clones[j - 1]->clone()); -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG std::size_t numIterations = 3; #else std::size_t numIterations = 500; diff --git a/tests/regression/test_Issue1184.cpp b/tests/regression/test_Issue1184.cpp index 2cd8447cd2ef9..5290f3544a4b2 100644 --- a/tests/regression/test_Issue1184.cpp +++ b/tests/regression/test_Issue1184.cpp @@ -79,7 +79,7 @@ TEST(Issue1184, Accuracy) return std::make_shared(s); }; -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG const auto groundInfoFunctions = {makePlaneGround}; const auto objectShapeFunctions = {makeSphereObject}; const auto halfsizes = {10.0}; diff --git a/tests/regression/test_Issue1596.cpp b/tests/regression/test_Issue1596.cpp index a594a4ec36983..52a57e758f4fc 100644 --- a/tests/regression/test_Issue1596.cpp +++ b/tests/regression/test_Issue1596.cpp @@ -40,7 +40,7 @@ //======================================================================================== TEST(Issue1596, ServoJointWithPositionLimits) { -#if DART_BUILD_MODE_RELEASE +#ifdef NDEBUG const auto num_steps = 50000; #else const auto num_steps = 1000; diff --git a/tests/unit/common/test_MemoryManager.cpp b/tests/unit/common/test_MemoryManager.cpp index 2e1e57586b6a1..237331049e512 100644 --- a/tests/unit/common/test_MemoryManager.cpp +++ b/tests/unit/common/test_MemoryManager.cpp @@ -63,7 +63,7 @@ TEST(MemoryManagerTest, Allocate) // Allocate 1 byte using FreeListAllocator auto ptr1 = mm.allocateUsingFree(1); EXPECT_NE(ptr1, nullptr); -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG EXPECT_TRUE(mm.hasAllocated(ptr1, 1)); EXPECT_FALSE(mm.hasAllocated(nullptr, 1)); EXPECT_FALSE(mm.hasAllocated(ptr1, 1 * 2)); @@ -72,7 +72,7 @@ TEST(MemoryManagerTest, Allocate) // Allocate 1 byte using PoolAllocator auto ptr2 = mm.allocateUsingPool(1); EXPECT_NE(ptr2, nullptr); -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG EXPECT_TRUE(mm.hasAllocated(ptr2, 1)); EXPECT_FALSE(mm.hasAllocated(nullptr, 1)); EXPECT_FALSE(mm.hasAllocated(ptr2, 1 * 2)); diff --git a/tests/unit/math/test_Math.cpp b/tests/unit/math/test_Math.cpp index 8d3b5fd1b0ad9..ecb7bd37721e7 100644 --- a/tests/unit/math/test_Math.cpp +++ b/tests/unit/math/test_Math.cpp @@ -1335,7 +1335,7 @@ typename Derived::PlainObject AdTJac3( //============================================================================== TEST(MATH, PerformanceComparisonOfAdTJac) { -#if DART_BUILD_MODE_DEBUG +#ifndef NDEBUG int testCount = 1e+2; #else int testCount = 1e+6;