Triball benchmark

CMakeLists.txt

@@ -3,10 +3,13 @@ project(benchmark_gazebo)
 enable_testing()
 
 include(FindBoost)
+
 find_package(PkgConfig REQUIRED)
 pkg_check_modules(zstd REQUIRED IMPORTED_TARGET libzstd)
 pkg_check_modules(lz4 REQUIRED IMPORTED_TARGET liblz4)
+
 find_package(protobuf CONFIG)
+
 if(NOT Protobuf_FOUND)
   find_package(Protobuf REQUIRED)
 endif()
@@ -23,27 +26,44 @@ if (NOT GAZEBO_FOUND)
   message (STATUS "Looking for gazebo - not found")
   message (FATAL_ERROR "Missing: Gazebo version 11.")
 endif()
+
 include_directories(${GAZEBO_INCLUDE_DIRS}
   ${PROJECT_SOURCE_DIR}/gtest/include
   ${PROJECT_SOURCE_DIR}/gtest
 )
 link_directories(${GAZEBO_LIBRARY_DIRS})
+
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
 
 # Build gtest
 add_library(gtest STATIC gtest/src/gtest-all.cc)
 add_library(gtest_main STATIC gtest/src/gtest_main.cc)
 target_link_libraries(gtest_main gtest tbb)
-set(PROTO_FILES proto/boxes_msg.proto)
-include(${PROJECT_SOURCE_DIR}/tools/TestMacro.cmake)
-set(MCAP_DEPENDENCIES PkgConfig::lz4 PkgConfig::zstd)
+
+set(TEST_TYPE "BENCHMARK")
 set(WORLDS_DIR_PATH "${PROJECT_SOURCE_DIR}/worlds")
 set(TEST_RESULT_DIR "${PROJECT_SOURCE_DIR}/test_results")
-set(TEST_TYPE "BENCHMARK")
+
+include(${PROJECT_SOURCE_DIR}/tools/TestMacro.cmake)
+
+set(MCAP_DEPENDENCIES PkgConfig::lz4 PkgConfig::zstd)
+
+set(PROTO_FILES proto/boxes_msg.proto proto/triball_msg.proto
+    proto/Accel.proto proto/Pose.proto proto/Quaternion.proto
+    proto/Twist.proto proto/Vector3.proto proto/Wrench.proto)
+
 include_directories(${Protobuf_INCLUDE_DIRS})
+
 protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS ${PROTO_FILES})
+
 configure_file(${PROJECT_SOURCE_DIR}/PathConfig.h.in PathConfig.h)
 
+set(PROTOBUF_DESCRIPTION_DIR ${CMAKE_SOURCE_DIR}/mcap/cpp/examples/protobuf/)
+set(PROTOBUF_DESCRIPTION_SRC 
+    ${PROTOBUF_DESCRIPTION_DIR}/BuildFileDescriptorSet.cpp
+)
+
+set(TEST_NAME boxes)
 # Boxes tests
 set(BOXES_TEST_FILES
   boxes_dt.cc
@@ -52,20 +72,22 @@ set(BOXES_TEST_FILES
 set(GZ_BUILD_TESTS_EXTRA_EXE_SRCS
   boxes.cc
 )
-set(PROTOBUF_DESCRIPTION_DIR ${CMAKE_SOURCE_DIR}/mcap/cpp/examples/protobuf/)
-set(PROTOBUF_DESCRIPTION_SRC 
-    ${PROTOBUF_DESCRIPTION_DIR}/BuildFileDescriptorSet.cpp
-)
+
 gz_build_tests(${BOXES_TEST_FILES})
 
 set_tests_properties(BENCHMARK_boxes_dt PROPERTIES TIMEOUT 500)
 set_tests_properties(BENCHMARK_boxes_model_count PROPERTIES TIMEOUT 3000)
 
-# # Collide sphere tests
-# set(COLLIDE_SPHERES_TEST_FILES
-#   collide_spheres_dt.cc
-# )
-# set(GZ_BUILD_TESTS_EXTRA_EXE_SRCS
-#   collide_spheres.cc
-# )
-# gz_build_tests(${COLLIDE_SPHERES_TEST_FILES})
+set(TEST_NAME triball)
+set(TRIBALL_TEST_FILES
+  triball_configuration.cc
+  triball_sliding.cc
+)
+
+set(GZ_BUILD_TESTS_EXTRA_EXE_SRCS
+  triball.cc
+)
+
+gz_build_tests(${TRIBALL_TEST_FILES})
+
+set_tests_properties(BENCHMARK_triball_configuration PROPERTIES TIMEOUT 500)

README.md

@@ -11,6 +11,8 @@ or on your own machine following the instructions given below:
 ![boxes benchmark animation: simple trajectory](img/boxes_5_simple.gif)
 ![boxes benchmark animation: complex trajectory](img/boxes_5_complex.gif)
 
+* [Triball: rigid bodies in contact](triball_description.ipynb)
+
 These benchmarks can be run for the physics engines supported by gazebo:
 
 * [Open Dynamics Engine (ODE)](http://ode.org), (source code on [bitbucket](https://bitbucket.org/odedevs/ode))
@@ -37,7 +39,8 @@ and then clone and build the benchmarks:
 
 ~~~
 git clone https://github.com/scpeters/benchmark
-pip install lz4 protobuf zstandard
+pip install lz4 protobuf zstandard pandas
+sudo apt-get install python3-gz-math7
 cd benchmark
 git submodule update --init --recursive
 mkdir build

boxes.cc

@@ -173,13 +173,13 @@ void BoxesTest::Boxes(const std::string &_physicsEngine, double _dt,
 
       auto model = models[model_no];
       link = model->GetLink();
-      // linear velocity 
+      // linear velocity in world frame
       ignition::math::Vector3d v = link->WorldCoGLinearVel();
-      // angular velocity
+      // angular velocity in world frame
       ignition::math::Vector3d a = link->WorldAngularVel();
       log.recordTwist(model_no, v, a);
-  
-      // linear position 
+
+      // linear position in world frame
       ignition::math::Pose3d pose = link->WorldInertialPose();
       log.recordPose(model_no, pose);
     }

boxes.hh

@@ -36,6 +36,7 @@ namespace gazebo
                             , bool
                             , bool
                             > char1double1int1bool2;
+
     class BoxesTest : public ServerFixture,
                       public testing::WithParamInterface<char1double1int1bool2>
     {