diff --git a/.github/workflows/ci_windows.yml b/.github/workflows/ci_windows.yml
index 3c92109aed376..83f8f31323013 100644
--- a/.github/workflows/ci_windows.yml
+++ b/.github/workflows/ci_windows.yml
@@ -28,7 +28,7 @@ jobs:
       fail-fast: false
       matrix:
         toolset: [""]
-        build_type: [Release]
+        build_type: [Release, Debug]
         build_shared_libs: [OFF] # TODO(JS): Add ON once shared lib build is resolved
 
     steps:
@@ -93,8 +93,12 @@ jobs:
             || exit /b
 
   build_on_pixi:
-    name: win-pixi
+    name: win-pixi${{ matrix.build_type }}
     runs-on: windows-latest
+    strategy:
+      fail-fast: false
+      matrix:
+        build_type: ["", "-dev"]
     steps:
       - name: Checkout
         uses: actions/checkout@v4
@@ -109,4 +113,4 @@ jobs:
 
       - name: Test DART and dartpy
         run: |
-          pixi run test # TODO: Change to test-all
+          pixi run test${{ matrix.build_type }} # TODO: Change to test-all
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7e2ef2722ec21..f225f09ac767f 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 <dart/config.hpp>
+
 // 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 <mutex>
-#endif
 #include <dart/common/FreeListAllocator.hpp>
 #include <dart/common/PoolAllocator.hpp>
 
@@ -151,11 +148,6 @@ class MemoryManager final
   template <typename T>
   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..dc521ec766206 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 <cassert>
-#if DART_BUILD_MODE_DEBUG
+#ifndef NDEBUG
   #include <iomanip>
   #include <iostream>
 #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 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
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<BoxedLcpSolver>
       bool earlyTermination = false)
       = 0;
 
-#if DART_BUILD_MODE_DEBUG
   virtual bool canSolve(int n, const double* A) = 0;
-#endif
 };
 
 } // namespace constraint
diff --git a/dart/constraint/ConstrainedGroup.cpp b/dart/constraint/ConstrainedGroup.cpp
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<ConstraintBasePtr> 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 <iomanip>
   #include <iostream>
 #endif
@@ -46,144 +46,11 @@
 namespace dart {
 namespace constraint {
 
-//==============================================================================
-DantzigLCPSolver::DantzigLCPSolver(double _timestep) : LCPSolver(_timestep)
-{
-  // Do nothing
-}
+namespace {
 
 //==============================================================================
-DantzigLCPSolver::~DantzigLCPSolver()
-{
-  // Do nothing
-}
-
-//==============================================================================
-void DantzigLCPSolver::solve(ConstrainedGroup* _group)
-{
-  // Build LCP terms by aggregating them from constraints
-  std::size_t numConstraints = _group->getNumConstraints();
-  std::size_t n = _group->getTotalDimension();
-
-  // If there is no constraint, then just return.
-  if (0u == n)
-    return;
-
-  int nSkip = dPAD(n);
-  double* A = new double[n * nSkip];
-  double* x = new double[n];
-  double* b = new double[n];
-  double* w = new double[n];
-  double* lo = new double[n];
-  double* hi = new double[n];
-  int* findex = new int[n];
-
-  // Set w to 0 and findex to -1
-#if DART_BUILD_MODE_DEBUG
-  std::memset(A, 0.0, n * nSkip * sizeof(double));
-#endif
-  std::memset(w, 0.0, n * sizeof(double));
-  std::memset(findex, -1, n * sizeof(int));
-
-  // Compute offset indices
-  std::size_t* offset = new std::size_t[n];
-  offset[0] = 0;
-  //  std::cout << "offset[" << 0 << "]: " << offset[0] << std::endl;
-  for (std::size_t i = 1; i < numConstraints; ++i) {
-    const ConstraintBasePtr& constraint = _group->getConstraint(i - 1);
-    assert(constraint->getDimension() > 0);
-    offset[i] = offset[i - 1] + constraint->getDimension();
-    //    std::cout << "offset[" << i << "]: " << offset[i] << std::endl;
-  }
-
-  // For each constraint
-  ConstraintInfo constInfo;
-  constInfo.invTimeStep = 1.0 / mTimeStep;
-  for (std::size_t i = 0; i < numConstraints; ++i) {
-    const ConstraintBasePtr& constraint = _group->getConstraint(i);
-
-    constInfo.x = x + offset[i];
-    constInfo.lo = lo + offset[i];
-    constInfo.hi = hi + offset[i];
-    constInfo.b = b + offset[i];
-    constInfo.findex = findex + offset[i];
-    constInfo.w = w + offset[i];
-
-    // Fill vectors: lo, hi, b, w
-    constraint->getInformation(&constInfo);
-
-    // Fill a matrix by impulse tests: A
-    constraint->excite();
-    for (std::size_t j = 0; j < constraint->getDimension(); ++j) {
-      // Adjust findex for global index
-      if (findex[offset[i] + j] >= 0)
-        findex[offset[i] + j] += offset[i];
-
-      // Apply impulse for impulse test
-      constraint->applyUnitImpulse(j);
-
-      // Fill upper triangle blocks of A matrix
-      int index = nSkip * (offset[i] + j) + offset[i];
-      constraint->getVelocityChange(A + index, true);
-      for (std::size_t k = i + 1; k < numConstraints; ++k) {
-        index = nSkip * (offset[i] + j) + offset[k];
-        _group->getConstraint(k)->getVelocityChange(A + index, false);
-      }
-
-      // Filling symmetric part of A matrix
-      for (std::size_t k = 0; k < i; ++k) {
-        for (std::size_t l = 0; l < _group->getConstraint(k)->getDimension();
-             ++l) {
-          int index1 = nSkip * (offset[i] + j) + offset[k] + l;
-          int index2 = nSkip * (offset[k] + l) + offset[i] + j;
-
-          A[index1] = A[index2];
-        }
-      }
-    }
-
-    assert(isSymmetric(
-        n, A, offset[i], offset[i] + constraint->getDimension() - 1));
-
-    constraint->unexcite();
-  }
-
-  assert(isSymmetric(n, A));
-
-  // Print LCP formulation
-  //  dtdbg << "Before solve:" << std::endl;
-  //  print(n, A, x, lo, hi, b, w, findex);
-  //  std::cout << std::endl;
-
-  // Solve LCP using ODE's Dantzig algorithm
-  external::ode::dSolveLCP(n, A, x, b, w, 0, lo, hi, findex);
-
-  // Print LCP formulation
-  //  dtdbg << "After solve:" << std::endl;
-  //  print(n, A, x, lo, hi, b, w, findex);
-  //  std::cout << std::endl;
-
-  // Apply constraint impulses
-  for (std::size_t i = 0; i < numConstraints; ++i) {
-    const ConstraintBasePtr& constraint = _group->getConstraint(i);
-    constraint->applyImpulse(x + offset[i]);
-    constraint->excite();
-  }
-
-  delete[] offset;
-
-  delete[] A;
-  delete[] x;
-  delete[] b;
-  delete[] w;
-  delete[] lo;
-  delete[] hi;
-  delete[] findex;
-}
-
-//==============================================================================
-#if DART_BUILD_MODE_DEBUG
-bool DantzigLCPSolver::isSymmetric(std::size_t _n, double* _A)
+#ifndef NDEBUG
+bool isSymmetric(std::size_t _n, double* _A)
 {
   std::size_t nSkip = dPAD(_n);
   for (std::size_t i = 0; i < _n; ++i) {
@@ -210,7 +77,7 @@ bool DantzigLCPSolver::isSymmetric(std::size_t _n, double* _A)
 }
 
 //==============================================================================
-bool DantzigLCPSolver::isSymmetric(
+bool isSymmetric(
     std::size_t _n, double* _A, std::size_t _begin, std::size_t _end)
 {
   std::size_t nSkip = dPAD(_n);
@@ -238,7 +105,7 @@ bool DantzigLCPSolver::isSymmetric(
 }
 
 //==============================================================================
-void DantzigLCPSolver::print(
+[[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 <iomanip>
   #include <iostream>
 #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 bcd8f56e3faa2..cd2907f94eb39 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<double, 6, 3> 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<double, 6, 3> 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<double, 6, 3> EulerJoint::getRelativeJacobianStatic(
 
   assert(!math::isNan(J));
 
-#if DART_BUILD_MODE_DEBUG
+#ifndef NDEBUG
   Eigen::MatrixXd JTJ = J.transpose() * J;
   Eigen::FullPivLU<Eigen::MatrixXd> luJTJ(JTJ);
   //    Eigen::FullPivLU<Eigen::MatrixXd> 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<BodyNode*>::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/pixi.toml b/pixi.toml
index a660b0f19fb16..6ad031c37c5b8 100644
--- a/pixi.toml
+++ b/pixi.toml
@@ -46,6 +46,7 @@ install-local = { cmd = "cmake --install build --prefix $CONDA_PREFIX", depends_
 ] }
 
 configure = { cmd = "cmake -G Ninja -S . -B build -DCMAKE_BUILD_TYPE=Release -DDART_VERBOSE=ON -DDART_USE_SYSTEM_IMGUI=ON" }
+configure-dev = { cmd = "cmake -G Ninja -S . -B build -DCMAKE_BUILD_TYPE=Debug -DDART_VERBOSE=ON -DDART_USE_SYSTEM_IMGUI=ON" }
 
 lint = { cmd = "cmake --build build --target format && black . && isort .", depends_on = [
     "configure",
@@ -57,9 +58,15 @@ check-lint = { cmd = "cmake --build build --target check-format && black . --che
 build = { cmd = "cmake --build build -j --target all", depends_on = [
     "configure",
 ] }
+build-dev = { cmd = "cmake --build build -j --target all", depends_on = [
+    "configure-dev",
+] }
 build-tests = { cmd = "cmake --build build -j --target tests", depends_on = [
     "configure",
 ] }
+build-tests-dev = { cmd = "cmake --build build -j --target tests", depends_on = [
+    "configure-dev",
+] }
 build-dartpy = { cmd = "cmake --build build -j --target dartpy", depends_on = [
     "configure",
 ] }
@@ -67,6 +74,9 @@ build-dartpy = { cmd = "cmake --build build -j --target dartpy", depends_on = [
 test = { cmd = "ctest --test-dir build --output-on-failure", depends_on = [
     "build-tests",
 ] }
+test-dev = { cmd = "ctest --test-dir build --output-on-failure", depends_on = [
+    "build-tests-dev",
+] }
 test-dartpy = { cmd = "cmake --build build -j --target pytest", depends_on = [
     "configure",
 ] }
@@ -151,15 +161,24 @@ check-lint = { cmd = "black . --check && isort . --check", depends_on = [
 build = { cmd = "cmake --build build --config Release -j", depends_on = [
     "configure",
 ] }
+build-dev = { cmd = "cmake --build build --config Debug -j", depends_on = [
+    "configure",
+] }
 build-tests = { cmd = "cmake --build build --config Release -j --target tests", depends_on = [
     "configure",
 ] }
+build-tests-dev = { cmd = "cmake --build build --config Debug -j --target tests", depends_on = [
+    "configure",
+] }
 build-dartpy = { cmd = "cmake --build build -j --target dartpy", depends_on = [
     "configure",
 ] }
 test = { cmd = "ctest --test-dir build --build-config Release --output-on-failure", depends_on = [
     "build-tests",
 ] }
+test-dev = { cmd = "ctest --test-dir build --build-config Debug --output-on-failure", depends_on = [
+    "build-tests-dev",
+] }
 test-dartpy = { cmd = "cmake --build build --config Release -j --target pytest", depends_on = [
     "configure",
 ] }
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 <cassert>
-#include <cmath>
+#include "pybind11/pybind11.h"
 
 #include <dart/common/common.hpp>
 
-#include "pybind11/pybind11.h"
+#include <cassert>
+#include <cmath>
 
 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<SkeletonPtr> 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<SkeletonPtr> 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<GenericNode>("node"), node);
     EXPECT_EQ(skel->getNode<GenericNode>("node"), nullptr);
 
@@ -711,7 +711,7 @@ TEST(Skeleton, NodePersistence)
     EXPECT_EQ(skel->getNode<GenericNode>(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<dart::dynamics::SphereShape>(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;