[benchmark] Add boxes benchmark

(cherry picked from commit 8bcca71)

pixi.toml

@@ -6,13 +6,48 @@ authors = ["Jeongseok Lee <[email protected]>"]
 channels = ["conda-forge"]
 platforms = ["linux-64", "osx-64", "osx-arm64", "win-64"]
 
+[build-dependencies]
+cmake = "3.22.*"
+ninja = ">=1.11.1,<1.12"
+pkg-config = ">=0.29.2,<0.30"
+pytest = ">=8.1.1,<8.2"
+doxygen = ">=1.10.0,<1.11"
+setuptools = ">=69.1.1,<69.2"
+black = ">=24.2.0,<24.3"
+isort = ">=5.13.2,<5.14"
+pip = ">=24.0,<25"
+pipx = ">=1.4.3,<1.5"
+
+[dependencies]
+assimp = ">=5.3.1,<5.4"
+bullet-cpp = ">=3.25,<4"
+console_bridge = ">=1.0.2,<1.1"
+eigen = ">=3.4.0,<3.5"
+fcl = ">=0.7.0,<0.8"
+fmt = ">=10.2.1,<10.3"
+libode = ">=0.16.2,<0.17"
+octomap = ">=1.9.8,<1.10"
+openscenegraph = ">=3.6.5,<3.7"
+spdlog = ">=1.12.0,<1.13"
+tinyxml2 = ">=10.0.0,<10.1"
+urdfdom = ">=4.0.0,<4.1"
+ipopt = ">=3.14.14,<3.15"
+nlopt = ">=2.7.1,<2.8"
+pagmo = ">=2.19.0,<2.20"
+numpy = ">=1.26.4,<1.27"
+
 [tasks]
 clean = "rm -rf build"
+
 configure-local = { cmd = "cmake -G Ninja -S . -B build -DDART_VERBOSE=ON -DCMAKE_INSTALL_PREFIX=$CONDA_PREFIX" }
 install-local = { cmd = "cmake --install build --prefix $CONDA_PREFIX", depends_on = [
     "configure_local",
     "build",
 ] }
+
+configure-unix = { cmd = "cmake -G Ninja -S . -B build -DCMAKE_BUILD_TYPE=Release -DDART_VERBOSE=ON -DDART_USE_SYSTEM_IMGUI=ON" }
+configure-win = { cmd = "cmake -S . -B build -G 'Visual Studio 17 2022' -DDART_VERBOSE=ON -DDART_MSVC_DEFAULT_OPTIONS=ON -DBUILD_SHARED_LIBS=OFF -DDART_USE_SYSTEM_IMGUI=OFF" }
+
 example-hello-world = { cmd = "cmake --build build --target hello_world --parallel && ./build/bin/hello_world", depends_on = [
     "configure",
 ] }
@@ -22,6 +57,10 @@ example-atlas-puppet = { cmd = "cmake --build build --target atlas_puppet --para
 example-atlas-simbicon = { cmd = "cmake --build build --target atlas_simbicon --parallel && ./build/bin/atlas_simbicon", depends_on = [
     "configure",
 ] }
+
+bm-boxes = { cmd = "cmake --build build --target BM_INTEGRATION_boxes --parallel && ./build/bin/BM_INTEGRATION_boxes", depends_on = [
+    "configure",
+] }
 bm-empty = { cmd = "cmake --build build --target BM_INTEGRATION_empty --parallel && ./build/bin/BM_INTEGRATION_empty", depends_on = [
     "configure",
 ] }
@@ -34,42 +73,19 @@ remove-deps-dir = { cmd = "rm -rf .deps" }
 
 tracy-linux = { cmd = "docker run --rm -d -e DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix:rw --net=host tracy-profiler" }
 
-[dependencies]
-assimp = ">=5.3.1,<5.4"
-bullet-cpp = ">=3.25,<4"
-console_bridge = ">=1.0.2,<1.1"
-eigen = ">=3.4.0,<3.5"
-fcl = ">=0.7.0,<0.8"
-fmt = ">=10.2.1,<10.3"
-libode = ">=0.16.2,<0.17"
-octomap = ">=1.9.8,<1.10"
-openscenegraph = ">=3.6.5,<3.7"
-spdlog = ">=1.12.0,<1.13"
-tinyxml2 = ">=10.0.0,<10.1"
-urdfdom = ">=4.0.0,<4.1"
-ipopt = ">=3.14.14,<3.15"
-nlopt = ">=2.7.1,<2.8"
-pagmo = ">=2.19.0,<2.20"
-numpy = ">=1.26.4,<1.27"
+################################################################################
+# linux-64
+################################################################################
 
-[build-dependencies]
-cmake = "3.22.*"
-ninja = ">=1.11.1,<1.12"
-pkg-config = ">=0.29.2,<0.30"
-pytest = ">=8.1.1,<8.2"
-doxygen = ">=1.10.0,<1.11"
-setuptools = ">=69.1.1,<69.2"
-black = ">=24.2.0,<24.3"
-isort = ">=5.13.2,<5.14"
-pip = ">=24.0,<25"
-pipx = ">=1.4.3,<1.5"
+[target.linux-64.build-dependencies]
+clang-format-14 = ">=14.0.6,<14.1"
 
 [target.linux-64.dependencies]
 freeglut = ">=3.2.2,<3.3"
 imgui = ">=1.90.4,<1.91"
 
 [target.linux-64.tasks]
-configure = "cmake -G Ninja -S . -B build -DDART_VERBOSE=ON -DDART_USE_SYSTEM_IMGUI=ON"
+configure = { depends_on = ["configure-unix"] }
 lint = { cmd = "cmake --build build --target format && black . && isort .", depends_on = [
     "configure",
 ] }
@@ -97,11 +113,18 @@ test-all = { cmd = "cmake --build build -j --target ALL", depends_on = [
 
 tracy = { depends_on = ["tracy-linux"] }
 
-[target.linux-64.build-dependencies]
+################################################################################
+# osx-64
+################################################################################
+
+[target.osx-64.build-dependencies]
 clang-format-14 = ">=14.0.6,<14.1"
 
+[target.osx-64.dependencies]
+imgui = ">=1.90.4,<1.91"
+
 [target.osx-64.tasks]
-configure = "cmake -G Ninja -S . -B build -DDART_VERBOSE=ON -DDART_USE_SYSTEM_IMGUI=ON"
+configure = { depends_on = ["configure-unix"] }
 lint = { cmd = "cmake --build build --target format && black . && isort .", depends_on = [
     "configure",
 ] }
@@ -127,14 +150,18 @@ test-all = { cmd = "cmake --build build -j --target ALL", depends_on = [
     "configure",
 ] }
 
-[target.osx-64.build-dependencies]
+################################################################################
+# osx-arm64
+################################################################################
+
+[target.osx-arm64.build-dependencies]
 clang-format-14 = ">=14.0.6,<14.1"
 
-[target.osx-64.dependencies]
+[target.osx-arm64.dependencies]
 imgui = ">=1.90.4,<1.91"
 
 [target.osx-arm64.tasks]
-configure = "cmake -G Ninja -S . -B build -DDART_VERBOSE=ON -DDART_USE_SYSTEM_IMGUI=ON"
+configure = { depends_on = ["configure-unix"] }
 lint = { cmd = "cmake --build build --target format && black . && isort .", depends_on = [
     "configure",
 ] }
@@ -158,17 +185,15 @@ test-all = { cmd = "cmake --build build -j --target ALL", depends_on = [
     "configure",
 ] }
 
-[target.osx-arm64.build-dependencies]
-clang-format-14 = ">=14.0.6,<14.1"
-
-[target.osx-arm64.dependencies]
-imgui = ">=1.90.4,<1.91"
+################################################################################
+# win-64
+################################################################################
 
 [target.win-64.dependencies]
 freeglut = ">=3.2.2,<3.3"
 
 [target.win-64.tasks]
-configure = "cmake -S . -B build -G 'Visual Studio 17 2022' -DCMAKE_BUILD_TYPE=Release -DDART_MSVC_DEFAULT_OPTIONS=ON -DBUILD_SHARED_LIBS=OFF -DDART_VERBOSE=ON -DDART_USE_SYSTEM_IMGUI=OFF"
+configure = { depends_on = ["configure-win"] }
 lint = { cmd = "black . && isort .", depends_on = ["configure"] }
 check-lint = { cmd = "black . --check && isort . --check", depends_on = [
     "configure",
@@ -191,6 +216,3 @@ test-dartpy = { cmd = "cmake --build build --config Release -j --target pytest",
 test-all = { cmd = "cmake --build build --config Release -j --target ALL", depends_on = [
     "configure",
 ] }
-
-[target.win-64.build-dependencies]
-freeglut = ">=3.2.2,<3.3"

tests/benchmark/integration/CMakeLists.txt

@@ -34,6 +34,14 @@ dart_benchmarks(
     bm_empty.cpp
 )
 
+dart_benchmarks(
+  TYPE BM_INTEGRATION
+  SOURCES
+    bm_boxes.cpp
+  LINK_LIBRARIES
+    dart
+)
+
 if(TARGET dart-utils)
 
 dart_benchmarks(

tests/benchmark/integration/bm_boxes.cpp

@@ -0,0 +1,128 @@
+/*
+ * Copyright (c) 2011-2024, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/main/LICENSE
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <dart/simulation/simulation.hpp>
+
+#include <dart/dynamics/dynamics.hpp>
+
+#include <dart/math/math.hpp>
+
+#include <benchmark/benchmark.h>
+
+using namespace dart;
+
+namespace {
+
+[[nodiscard]] dynamics::SkeletonPtr createBox(
+    const Eigen::Vector3d& position,
+    const Eigen::Vector3d& size = Eigen::Vector3d(1, 1, 1),
+    const Eigen::Vector3d& color
+    = dart::math::Random::uniform<Eigen::Vector3d>(0.0, 1.0))
+{
+  static size_t index = 0;
+  auto boxSkel = dynamics::Skeleton::create("box" + std::to_string(index++));
+
+  // Give the floor a body
+  auto boxBody
+      = boxSkel->createJointAndBodyNodePair<dynamics::FreeJoint>(nullptr)
+            .second;
+
+  // Give the body a shape
+  auto boxShape = std::make_shared<dynamics::BoxShape>(size);
+  dynamics::ShapeNode* shapeNode = boxBody->createShapeNodeWith<
+      dynamics::VisualAspect,
+      dynamics::CollisionAspect,
+      dynamics::DynamicsAspect>(boxShape);
+  shapeNode->getVisualAspect()->setColor(color);
+
+  // Put the body into position
+  Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
+  tf.translation() = position;
+  boxBody->getParentJoint()->setTransformFromParentBodyNode(tf);
+
+  return boxSkel;
+}
+
+[[nodiscard]] simulation::WorldPtr createWorld(size_t dim)
+{
+  // Create an empty world
+  auto world = simulation::World::create();
+
+  // Create dim x dim x dim boxes
+  for (auto i = 0u; i < dim; ++i) {
+    for (auto j = 0u; j < dim; ++j) {
+      for (auto k = 0u; k < dim; ++k) {
+        auto x = i - dim / 2;
+        auto y = j - dim / 2;
+        auto z = k + 1;
+        auto position = Eigen::Vector3d(x, y, z);
+        auto size = Eigen::Vector3d(0.9, 0.9, 0.9);
+        auto color = Eigen::Vector3d(
+            static_cast<double>(i) / dim,
+            static_cast<double>(j) / dim,
+            static_cast<double>(k) / dim);
+        auto box = createBox(position, size, color);
+        world->addSkeleton(box);
+      }
+    }
+  }
+
+  // Create ground
+  auto ground = dynamics::Skeleton::create("ground");
+  auto groundBody
+      = ground->createJointAndBodyNodePair<dynamics::WeldJoint>().second;
+  auto groundShapeNode = groundBody->createShapeNodeWith<
+      dynamics::VisualAspect,
+      dynamics::CollisionAspect,
+      dynamics::DynamicsAspect>(
+      std::make_shared<dynamics::BoxShape>(Eigen::Vector3d(10.0, 10.0, 0.1)));
+  groundShapeNode->getVisualAspect()->setColor(dart::Color::LightGray());
+  world->addSkeleton(ground);
+
+  return world;
+}
+
+} // namespace
+
+static void BM_RunBoxes(benchmark::State& state)
+{
+  const auto steps = 1000u;
+  auto world = createWorld(state.range(0));
+
+  for (auto _ : state) {
+    for (size_t i = 0; i < steps; ++i) {
+      world->step();
+    }
+  }
+}
+
+BENCHMARK(BM_RunBoxes)->Arg(2)->Arg(5)->Unit(benchmark::kMillisecond);

tests/integration/test_Collision.cpp

@@ -857,9 +857,9 @@ TEST_F(Collision, testConeCone)
 
 #if HAVE_ODE
   {
-    // SCOPED_TRACE("OdeCollisionDetector");
-    // auto ode = OdeCollisionDetector::create();
-    // testConeCone(ode);
+      // SCOPED_TRACE("OdeCollisionDetector");
+      // auto ode = OdeCollisionDetector::create();
+      // testConeCone(ode);
   }
 #endif