From 10165398348111c0ac55e1af592c06fade1980a7 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 31 Jan 2024 08:37:21 -0600 Subject: [PATCH 01/18] bazel: updates for garden (#513) Signed-off-by: Michael Carroll --- BUILD.bazel | 130 +++++++++++ bullet-featherstone/BUILD.bazel | 45 ++++ dartsim/BUILD.bazel | 55 +++++ include/gz/physics/config.hh.in | 1 + src/InstallationDirectories.cc | 24 +- test/BUILD.bazel | 269 ++++++++++++++++++++++ test/common_test/CMakeLists.txt | 2 - test/include/test/TestLibLoader.hh | 28 ++- tpe/plugin/src/SimulationFeatures_TEST.cc | 1 - 9 files changed, 537 insertions(+), 18 deletions(-) create mode 100644 BUILD.bazel create mode 100644 bullet-featherstone/BUILD.bazel create mode 100644 dartsim/BUILD.bazel create mode 100644 test/BUILD.bazel diff --git a/BUILD.bazel b/BUILD.bazel new file mode 100644 index 000000000..a4296ba48 --- /dev/null +++ b/BUILD.bazel @@ -0,0 +1,130 @@ +load( + "@gz//bazel/skylark:build_defs.bzl", + "GZ_FEATURES", + "GZ_ROOT", + "GZ_VISIBILITY", + "gz_configure_header", + "gz_export_header", + "gz_include_header", +) + +package( + default_visibility = GZ_VISIBILITY, + features = GZ_FEATURES, +) + +licenses(["notice"]) # Apache-2.0 + +exports_files(["LICENSE"]) + +gz_configure_header( + name = "physics_config_hh", + src = "include/gz/physics/config.hh.in", + cmakelists = ["CMakeLists.txt"], + defines = { + # These definitions are unused, + # this is merely to suppress generator warnings + "GZ_PHYSICS_ENGINE_INSTALL_DIR": "unused", + }, + package = "physics", +) + +gz_export_header( + name = "include/gz/physics/Export.hh", + export_base = "GZ_PHYSICS", + lib_name = "gz-physics", + visibility = ["//visibility:private"], +) + +public_headers_no_gen = glob([ + "include/gz/physics/*.hh", + "include/gz/physics/detail/*.hh", +]) + +sources = glob( + ["src/*.cc"], + exclude = [ + "src/*_TEST.cc", + ], +) + +gz_include_header( + name = "physics_hh_genrule", + out = "include/gz/physics.hh", + hdrs = public_headers_no_gen + [ + "include/gz/physics/config.hh", + "include/gz/physics/Export.hh", + ], +) + +public_headers = public_headers_no_gen + [ + "include/gz/physics/config.hh", + "include/gz/physics/Export.hh", + "include/gz/physics.hh", +] + +cc_library( + name = "physics", + srcs = sources, + hdrs = public_headers, + defines = [ + 'GZ_PHYSICS_ENGINE_RELATIVE_INSTALL_DIR=\\"unused\\"', + "GZ_PHYSICS_BAZEL_BUILD=true", + ], + includes = ["include"], + deps = [ + GZ_ROOT + "math", + GZ_ROOT + "math/eigen3", + GZ_ROOT + "plugin:core", + GZ_ROOT + "plugin:loader", + GZ_ROOT + "utils", + ], +) + +cc_library( + name = "heightmap", + hdrs = [ + "heightmap/include/gz/physics/heightmap/HeightmapShape.hh", + "heightmap/include/gz/physics/heightmap/detail/HeightmapShape.hh", + ], + includes = ["heightmap/include"], +) + +cc_library( + name = "mesh", + hdrs = [ + "mesh/include/gz/physics/mesh/MeshShape.hh", + "mesh/include/gz/physics/mesh/detail/MeshShape.hh", + ], + includes = ["mesh/include"], +) + +cc_library( + name = "sdf", + hdrs = glob([ + "sdf/include/gz/physics/sdf/*.hh", + ]), + includes = ["sdf/include"], + deps = [ + GZ_ROOT + "sdformat", + ], +) + +test_sources = glob( + include = ["src/*_TEST.cc"], +) + +[cc_test( + name = src.replace("/", "_").replace(".cc", "").replace("src_", ""), + srcs = [src], + env = { + "GZ_BAZEL": "1", + "GZ_BAZEL_PATH": "physics", + }, + deps = [ + ":physics", + GZ_ROOT + "physics/test:test_headers", + "@gtest", + "@gtest//:gtest_main", + ], +) for src in test_sources] diff --git a/bullet-featherstone/BUILD.bazel b/bullet-featherstone/BUILD.bazel new file mode 100644 index 000000000..1296d4658 --- /dev/null +++ b/bullet-featherstone/BUILD.bazel @@ -0,0 +1,45 @@ +load( + "@gz//bazel/skylark:build_defs.bzl", + "GZ_FEATURES", + "GZ_ROOT", + "GZ_VISIBILITY", +) + +private_headers = glob(["src/*.hh"]) + +sources = glob( + ["src/*.cc"], + exclude = [ + "src/*_TEST.cc", + "src/plugin.cc", + ], +) + +cc_library( + name = "bullet-featherstone", + srcs = sources + private_headers, + includes = ["include"], + visibility = GZ_VISIBILITY, + deps = [ + GZ_ROOT + "common", + GZ_ROOT + "common/graphics", + GZ_ROOT + "math/eigen3", + GZ_ROOT + "physics", + GZ_ROOT + "physics:sdf", + GZ_ROOT + "third_party/bullet3:BulletCollision", + GZ_ROOT + "third_party/bullet3:BulletDynamics", + ], +) + +cc_binary( + name = "libgz-physics-bullet-featherstone-plugin.so", + srcs = [ + "src/plugin.cc", + ], + linkshared = True, + visibility = GZ_VISIBILITY, + deps = [ + ":bullet-featherstone", + GZ_ROOT + "plugin:register", + ], +) diff --git a/dartsim/BUILD.bazel b/dartsim/BUILD.bazel new file mode 100644 index 000000000..17a14c709 --- /dev/null +++ b/dartsim/BUILD.bazel @@ -0,0 +1,55 @@ +load( + "@gz//bazel/skylark:build_defs.bzl", + "GZ_FEATURES", + "GZ_ROOT", + "GZ_VISIBILITY", + "gz_configure_header", + "gz_export_header", + "gz_include_header", +) + +public_headers = ["include/gz/physics/dartsim/World.hh"] + +private_headers = glob(["src/*.hh"]) + +sources = glob( + ["src/*.cc"], + exclude = [ + "src/*_TEST.cc", + "src/plugin.cc", + ], +) + +cc_library( + name = "dartsim", + srcs = sources + private_headers, + hdrs = public_headers, + includes = ["include"], + visibility = GZ_VISIBILITY, + deps = [ + GZ_ROOT + "common", + GZ_ROOT + "common/geospatial", + GZ_ROOT + "common/profiler", + GZ_ROOT + "math/eigen3", + GZ_ROOT + "physics", + GZ_ROOT + "physics:heightmap", + GZ_ROOT + "physics:mesh", + GZ_ROOT + "physics:sdf", + GZ_ROOT + "third_party/dart:simulation", + GZ_ROOT + "third_party/dart:collision-bullet", + GZ_ROOT + "third_party/dart:collision-ode", + ], +) + +cc_binary( + name = "libgz-physics-dartsim-plugin.so", + srcs = [ + "src/plugin.cc", + ], + linkshared = True, + visibility = GZ_VISIBILITY, + deps = [ + ":dartsim", + GZ_ROOT + "plugin:register", + ], +) diff --git a/include/gz/physics/config.hh.in b/include/gz/physics/config.hh.in index d0c68462d..332fb10e4 100644 --- a/include/gz/physics/config.hh.in +++ b/include/gz/physics/config.hh.in @@ -27,6 +27,7 @@ #define GZ_PHYSICS_VERSION "${PROJECT_VERSION}" #define GZ_PHYSICS_VERSION_FULL "${PROJECT_VERSION_FULL}" +#define GZ_PHYSICS_VERSION_NAMESPACE v${PROJECT_VERSION_MAJOR} #define GZ_PHYSICS_ENGINE_INSTALL_DIR _Pragma ("GCC warning \"'GZ_PHYSICS_ENGINE_INSTALL_DIR' macro is deprecated, use gz::physics::getEngineInstallDir() function instead. \"") "${GZ_PHYSICS_ENGINE_INSTALL_DIR}" diff --git a/src/InstallationDirectories.cc b/src/InstallationDirectories.cc index 7b9bcf459..56735383f 100644 --- a/src/InstallationDirectories.cc +++ b/src/InstallationDirectories.cc @@ -28,7 +28,7 @@ namespace gz { namespace physics { -inline namespace GZ_PHYSICS_VERSION_NAMESPACE { +namespace { // We locally import the gz::common::joinPaths function // See https://github.com/gazebosim/gz-physics/pull/507#discussion_r1186919267 @@ -36,19 +36,20 @@ inline namespace GZ_PHYSICS_VERSION_NAMESPACE { // Function imported from // https://github.com/gazebosim/gz-common/blob/ignition-common4_4.6.2/src/FilesystemBoost.cc#L507 -#ifndef WIN32 -static const char preferred_separator = '/'; +#ifndef _WIN32 +const char preferred_separator = '/'; #else // Windows static const char preferred_separator = '\\'; #endif -const std::string separator(const std::string &_p) +std::string separator(const std::string &_p) { return _p + preferred_separator; } +#ifdef _WIN32 // Function imported from // https://github.com/gazebosim/gz-common/blob/ignition-common4_4.6.2/src/Filesystem.cc#L227 -std::string checkWindowsPath(const std::string _path) +std::string checkWindowsPath(const std::string &_path) { if (_path.empty()) return _path; @@ -75,6 +76,7 @@ std::string checkWindowsPath(const std::string _path) result, std::regex("[<>:\"|?*]"), ""); return result; } +#endif // Function imported from // https://github.com/gazebosim/gz-common/blob/ignition-common4_4.6.2/src/Filesystem.cc#L256 @@ -142,14 +144,18 @@ std::string joinPaths(const std::string &_path1, #endif // _WIN32 return path; } +} // namespace +inline namespace GZ_PHYSICS_VERSION_NAMESPACE { +#ifdef GZ_PHYSICS_BAZEL_BUILD +std::string getInstallPrefix() { return "physics"; } +#endif std::string getEngineInstallDir() { return gz::physics::joinPaths( getInstallPrefix(), GZ_PHYSICS_ENGINE_RELATIVE_INSTALL_DIR); } - -} -} -} +} // namespace GZ_PHYSICS_VERSION_NAMESPACE +} // namespace physics +} // namespace gz diff --git a/test/BUILD.bazel b/test/BUILD.bazel new file mode 100644 index 000000000..3bb662543 --- /dev/null +++ b/test/BUILD.bazel @@ -0,0 +1,269 @@ +load( + "@gz//bazel/skylark:build_defs.bzl", + "GZ_ROOT", + "GZ_VISIBILITY", +) + +cc_library( + name = "test_headers", + srcs = glob(["include/**/*.hh"]), + includes = ["include"], + visibility = GZ_VISIBILITY, + deps = [ + GZ_ROOT + "common", + GZ_ROOT + "common/testing", + GZ_ROOT + "physics", + GZ_ROOT + "plugin:loader", + ], +) + +cc_library( + name = "common_test", + hdrs = ["common_test/Worlds.hh"], + includes = ["common_test"], + data = [ + ":resources", + "common_test/worlds", + ], + deps = [ + ":test_headers", + ] +) + +cc_library( + name = "common_test_dartsim", + data = [ + GZ_ROOT + "physics/dartsim:libgz-physics-dartsim-plugin.so", + ], + deps = [ + ":common_test" + ] +) + +cc_library( + name = "common_test_bullet-featherstone", + data = [ + GZ_ROOT + "physics/bullet-featherstone:libgz-physics-bullet-featherstone-plugin.so", + ], + deps = [ + ":common_test" + ] +) + +physics_engines = [ + "dartsim", + "bullet-featherstone", +] + +[cc_test( + name = "addexternalforcetorque_%s" % engine, + srcs = [ + "common_test/addexternalforcetorque.cc", + ], + env = { + "GZ_BAZEL": "1", + "GZ_BAZEL_PATH": "physics", + "LIB_TO_TEST": "physics/%s/libgz-physics-%s-plugin.so" % (engine, engine), + }, + deps = [ + ":common_test_%s" % engine, + GZ_ROOT + "math/eigen3", + GZ_ROOT + "physics:sdf", + "@gtest", + "@gtest//:gtest_main", + ], +) for engine in physics_engines] + +[cc_test( + name = "basic_test_%s" % engine, + srcs = [ + "common_test/basic_test.cc", + ], + env = { + "GZ_BAZEL": "1", + "GZ_BAZEL_PATH": "physics", + "LIB_TO_TEST": "physics/%s/libgz-physics-%s-plugin.so" % (engine, engine), + }, + deps = [ + ":common_test_%s" % engine, + "@gtest", + "@gtest//:gtest_main", + ], +) for engine in physics_engines] + +[cc_test( + name = "collisions_%s" % engine, + srcs = [ + "common_test/collisions.cc", + ], + defines = [ + 'GZ_PHYSICS_RESOURCE_DIR=\\"physics/resources/\\"', + ], + env = { + "GZ_BAZEL": "1", + "GZ_BAZEL_PATH": "physics", + "LIB_TO_TEST": "physics/%s/libgz-physics-%s-plugin.so" % (engine, engine), + }, + deps = [ + ":common_test_%s" % engine, + GZ_ROOT + "common/graphics", + GZ_ROOT + "math/eigen3", + GZ_ROOT + "physics:mesh", + GZ_ROOT + "physics:sdf", + "@gtest", + "@gtest//:gtest_main", + ], +) for engine in physics_engines] + +[cc_test( + name = "construct_empty_world_%s" % engine, + srcs = [ + "common_test/construct_empty_world.cc", + ], + env = { + "GZ_BAZEL": "1", + "GZ_BAZEL_PATH": "physics", + "LIB_TO_TEST": "physics/%s/libgz-physics-%s-plugin.so" % (engine, engine), + }, + deps = [ + ":common_test_%s" % engine, + GZ_ROOT + "math/eigen3", + "@gtest", + "@gtest//:gtest_main", + ], +) for engine in physics_engines] + +[cc_test( + name = "free_joint_features_%s" % engine, + srcs = [ + "common_test/free_joint_features.cc", + ], + env = { + "GZ_BAZEL": "1", + "GZ_BAZEL_PATH": "physics", + "LIB_TO_TEST": "physics/%s/libgz-physics-%s-plugin.so" % (engine, engine), + }, + deps = [ + ":common_test_%s" % engine, + GZ_ROOT + "math/eigen3", + GZ_ROOT + "physics:sdf", + "@gtest", + "@gtest//:gtest_main", + ], +) for engine in physics_engines] + +[cc_test( + name = "joint_features_%s" % engine, + srcs = [ + "common_test/joint_features.cc", + ], + env = { + "GZ_BAZEL": "1", + "GZ_BAZEL_PATH": "physics", + "LIB_TO_TEST": "physics/%s/libgz-physics-%s-plugin.so" % (engine, engine), + }, + deps = [ + ":common_test_%s" % engine, + GZ_ROOT + "math/eigen3", + GZ_ROOT + "physics:sdf", + "@gtest", + "@gtest//:gtest_main", + ], +) for engine in physics_engines] + +[cc_test( + name = "kinematic_features_%s" % engine, + srcs = [ + "common_test/kinematic_features.cc", + ], + env = { + "GZ_BAZEL": "1", + "GZ_BAZEL_PATH": "physics", + "LIB_TO_TEST": "physics/%s/libgz-physics-%s-plugin.so" % (engine, engine), + }, + deps = [ + ":common_test_%s" % engine, + GZ_ROOT + "physics:sdf", + "@eigen3", + "@gtest", + "@gtest//:gtest_main", + ], +) for engine in physics_engines] + +[cc_test( + name = "link_features_%s" % engine, + srcs = [ + "common_test/link_features.cc", + ], + env = { + "GZ_BAZEL": "1", + "GZ_BAZEL_PATH": "physics", + "LIB_TO_TEST": "physics/%s/libgz-physics-%s-plugin.so" % (engine, engine), + }, + deps = [ + ":common_test_%s" % engine, + GZ_ROOT + "math/eigen3", + GZ_ROOT + "physics:sdf", + "@gtest", + "@gtest//:gtest_main", + ], +) for engine in physics_engines] + +[cc_test( + name = "shape_features_%s" % engine, + srcs = [ + "common_test/shape_features.cc", + ], + env = { + "GZ_BAZEL": "1", + "GZ_BAZEL_PATH": "physics", + "LIB_TO_TEST": "physics/%s/libgz-physics-%s-plugin.so" % (engine, engine), + }, + deps = [ + ":common_test_%s" % engine, + GZ_ROOT + "math/eigen3", + GZ_ROOT + "physics:sdf", + "@gtest", + "@gtest//:gtest_main", + ], +) for engine in physics_engines] + +[cc_test( + name = "simulation_features_%s" % engine, + srcs = [ + "common_test/simulation_features.cc", + ], + env = { + "GZ_BAZEL": "1", + "GZ_BAZEL_PATH": "physics", + "LIB_TO_TEST": "physics/%s/libgz-physics-%s-plugin.so" % (engine, engine), + }, + deps = [ + ":common_test_%s" % engine, + GZ_ROOT + "math/eigen3", + GZ_ROOT + "physics:sdf", + "@gtest", + "@gtest//:gtest_main", + ], +) for engine in physics_engines] + +[cc_test( + name = "world_features_%s" % engine, + srcs = [ + "common_test/world_features.cc", + ], + env = { + "GZ_BAZEL": "1", + "GZ_BAZEL_PATH": "physics", + "LIB_TO_TEST": "physics/%s/libgz-physics-%s-plugin.so" % (engine, engine), + }, + deps = [ + ":common_test_%s" % engine, + GZ_ROOT + "math/eigen3", + GZ_ROOT + "physics:sdf", + "@gtest", + "@gtest//:gtest_main", + ], +) for engine in physics_engines] + +exports_files(["resources"]) diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index a35e2127a..49c904635 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -44,7 +44,6 @@ function(configure_common_test PHYSICS_ENGINE_NAME test_name) endif() endfunction() - # Get bullet version using pkg_check_modules as it is not available # through the cmake module gz_pkg_check_modules_quiet(bullet_version_check "bullet") @@ -71,7 +70,6 @@ foreach(test ${tests}) ) endif() - install(TARGETS ${test_executable} DESTINATION ${TEST_INSTALL_DIR}) configure_common_test("bullet" ${test_executable}) configure_common_test("bullet-featherstone" ${test_executable}) diff --git a/test/include/test/TestLibLoader.hh b/test/include/test/TestLibLoader.hh index a7f27abd3..b07d05528 100644 --- a/test/include/test/TestLibLoader.hh +++ b/test/include/test/TestLibLoader.hh @@ -41,13 +41,29 @@ class TestLibLoader { if (argc != 2) { - std::cerr << "Please provide the path to an engine plugin.\n" - << "Usage " << argv[0] << " \n"; - return false; + std::string envLibToTest; + if(!gz::utils::env("LIB_TO_TEST", envLibToTest)) + { + std::cerr << "Please provide the path to an engine plugin.\n" + << "Usage " << argv[0] << " \n"; + return false; + } + else + { + std::string &libToTest = LibToTest(); + libToTest = envLibToTest; + + std::cout << std::filesystem::current_path() << std::endl; + std::cout << envLibToTest << std::endl; + return true; + } + } + else + { + std::string &libToTest = LibToTest(); + libToTest = argv[1]; + return true; } - std::string &libToTest = LibToTest(); - libToTest = argv[1]; - return true; } /// \brief Get the name of the library to test diff --git a/tpe/plugin/src/SimulationFeatures_TEST.cc b/tpe/plugin/src/SimulationFeatures_TEST.cc index ddecee5de..25fff9ecc 100644 --- a/tpe/plugin/src/SimulationFeatures_TEST.cc +++ b/tpe/plugin/src/SimulationFeatures_TEST.cc @@ -52,7 +52,6 @@ #include "World.hh" - using namespace gz; struct TestFeatureList : physics::FeatureList< From ea90ada5f5d9bce94ec07db3f345f0f313b7c733 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 6 Feb 2024 13:27:47 -0600 Subject: [PATCH 02/18] Revert "bazel: updates for garden (#513)" This reverts commit 10165398348111c0ac55e1af592c06fade1980a7. Signed-off-by: Michael Carroll --- BUILD.bazel | 130 ----------- bullet-featherstone/BUILD.bazel | 45 ---- dartsim/BUILD.bazel | 55 ----- include/gz/physics/config.hh.in | 1 - src/InstallationDirectories.cc | 24 +- test/BUILD.bazel | 269 ---------------------- test/common_test/CMakeLists.txt | 2 + test/include/test/TestLibLoader.hh | 28 +-- tpe/plugin/src/SimulationFeatures_TEST.cc | 1 + 9 files changed, 18 insertions(+), 537 deletions(-) delete mode 100644 BUILD.bazel delete mode 100644 bullet-featherstone/BUILD.bazel delete mode 100644 dartsim/BUILD.bazel delete mode 100644 test/BUILD.bazel diff --git a/BUILD.bazel b/BUILD.bazel deleted file mode 100644 index a4296ba48..000000000 --- a/BUILD.bazel +++ /dev/null @@ -1,130 +0,0 @@ -load( - "@gz//bazel/skylark:build_defs.bzl", - "GZ_FEATURES", - "GZ_ROOT", - "GZ_VISIBILITY", - "gz_configure_header", - "gz_export_header", - "gz_include_header", -) - -package( - default_visibility = GZ_VISIBILITY, - features = GZ_FEATURES, -) - -licenses(["notice"]) # Apache-2.0 - -exports_files(["LICENSE"]) - -gz_configure_header( - name = "physics_config_hh", - src = "include/gz/physics/config.hh.in", - cmakelists = ["CMakeLists.txt"], - defines = { - # These definitions are unused, - # this is merely to suppress generator warnings - "GZ_PHYSICS_ENGINE_INSTALL_DIR": "unused", - }, - package = "physics", -) - -gz_export_header( - name = "include/gz/physics/Export.hh", - export_base = "GZ_PHYSICS", - lib_name = "gz-physics", - visibility = ["//visibility:private"], -) - -public_headers_no_gen = glob([ - "include/gz/physics/*.hh", - "include/gz/physics/detail/*.hh", -]) - -sources = glob( - ["src/*.cc"], - exclude = [ - "src/*_TEST.cc", - ], -) - -gz_include_header( - name = "physics_hh_genrule", - out = "include/gz/physics.hh", - hdrs = public_headers_no_gen + [ - "include/gz/physics/config.hh", - "include/gz/physics/Export.hh", - ], -) - -public_headers = public_headers_no_gen + [ - "include/gz/physics/config.hh", - "include/gz/physics/Export.hh", - "include/gz/physics.hh", -] - -cc_library( - name = "physics", - srcs = sources, - hdrs = public_headers, - defines = [ - 'GZ_PHYSICS_ENGINE_RELATIVE_INSTALL_DIR=\\"unused\\"', - "GZ_PHYSICS_BAZEL_BUILD=true", - ], - includes = ["include"], - deps = [ - GZ_ROOT + "math", - GZ_ROOT + "math/eigen3", - GZ_ROOT + "plugin:core", - GZ_ROOT + "plugin:loader", - GZ_ROOT + "utils", - ], -) - -cc_library( - name = "heightmap", - hdrs = [ - "heightmap/include/gz/physics/heightmap/HeightmapShape.hh", - "heightmap/include/gz/physics/heightmap/detail/HeightmapShape.hh", - ], - includes = ["heightmap/include"], -) - -cc_library( - name = "mesh", - hdrs = [ - "mesh/include/gz/physics/mesh/MeshShape.hh", - "mesh/include/gz/physics/mesh/detail/MeshShape.hh", - ], - includes = ["mesh/include"], -) - -cc_library( - name = "sdf", - hdrs = glob([ - "sdf/include/gz/physics/sdf/*.hh", - ]), - includes = ["sdf/include"], - deps = [ - GZ_ROOT + "sdformat", - ], -) - -test_sources = glob( - include = ["src/*_TEST.cc"], -) - -[cc_test( - name = src.replace("/", "_").replace(".cc", "").replace("src_", ""), - srcs = [src], - env = { - "GZ_BAZEL": "1", - "GZ_BAZEL_PATH": "physics", - }, - deps = [ - ":physics", - GZ_ROOT + "physics/test:test_headers", - "@gtest", - "@gtest//:gtest_main", - ], -) for src in test_sources] diff --git a/bullet-featherstone/BUILD.bazel b/bullet-featherstone/BUILD.bazel deleted file mode 100644 index 1296d4658..000000000 --- a/bullet-featherstone/BUILD.bazel +++ /dev/null @@ -1,45 +0,0 @@ -load( - "@gz//bazel/skylark:build_defs.bzl", - "GZ_FEATURES", - "GZ_ROOT", - "GZ_VISIBILITY", -) - -private_headers = glob(["src/*.hh"]) - -sources = glob( - ["src/*.cc"], - exclude = [ - "src/*_TEST.cc", - "src/plugin.cc", - ], -) - -cc_library( - name = "bullet-featherstone", - srcs = sources + private_headers, - includes = ["include"], - visibility = GZ_VISIBILITY, - deps = [ - GZ_ROOT + "common", - GZ_ROOT + "common/graphics", - GZ_ROOT + "math/eigen3", - GZ_ROOT + "physics", - GZ_ROOT + "physics:sdf", - GZ_ROOT + "third_party/bullet3:BulletCollision", - GZ_ROOT + "third_party/bullet3:BulletDynamics", - ], -) - -cc_binary( - name = "libgz-physics-bullet-featherstone-plugin.so", - srcs = [ - "src/plugin.cc", - ], - linkshared = True, - visibility = GZ_VISIBILITY, - deps = [ - ":bullet-featherstone", - GZ_ROOT + "plugin:register", - ], -) diff --git a/dartsim/BUILD.bazel b/dartsim/BUILD.bazel deleted file mode 100644 index 17a14c709..000000000 --- a/dartsim/BUILD.bazel +++ /dev/null @@ -1,55 +0,0 @@ -load( - "@gz//bazel/skylark:build_defs.bzl", - "GZ_FEATURES", - "GZ_ROOT", - "GZ_VISIBILITY", - "gz_configure_header", - "gz_export_header", - "gz_include_header", -) - -public_headers = ["include/gz/physics/dartsim/World.hh"] - -private_headers = glob(["src/*.hh"]) - -sources = glob( - ["src/*.cc"], - exclude = [ - "src/*_TEST.cc", - "src/plugin.cc", - ], -) - -cc_library( - name = "dartsim", - srcs = sources + private_headers, - hdrs = public_headers, - includes = ["include"], - visibility = GZ_VISIBILITY, - deps = [ - GZ_ROOT + "common", - GZ_ROOT + "common/geospatial", - GZ_ROOT + "common/profiler", - GZ_ROOT + "math/eigen3", - GZ_ROOT + "physics", - GZ_ROOT + "physics:heightmap", - GZ_ROOT + "physics:mesh", - GZ_ROOT + "physics:sdf", - GZ_ROOT + "third_party/dart:simulation", - GZ_ROOT + "third_party/dart:collision-bullet", - GZ_ROOT + "third_party/dart:collision-ode", - ], -) - -cc_binary( - name = "libgz-physics-dartsim-plugin.so", - srcs = [ - "src/plugin.cc", - ], - linkshared = True, - visibility = GZ_VISIBILITY, - deps = [ - ":dartsim", - GZ_ROOT + "plugin:register", - ], -) diff --git a/include/gz/physics/config.hh.in b/include/gz/physics/config.hh.in index 332fb10e4..d0c68462d 100644 --- a/include/gz/physics/config.hh.in +++ b/include/gz/physics/config.hh.in @@ -27,7 +27,6 @@ #define GZ_PHYSICS_VERSION "${PROJECT_VERSION}" #define GZ_PHYSICS_VERSION_FULL "${PROJECT_VERSION_FULL}" -#define GZ_PHYSICS_VERSION_NAMESPACE v${PROJECT_VERSION_MAJOR} #define GZ_PHYSICS_ENGINE_INSTALL_DIR _Pragma ("GCC warning \"'GZ_PHYSICS_ENGINE_INSTALL_DIR' macro is deprecated, use gz::physics::getEngineInstallDir() function instead. \"") "${GZ_PHYSICS_ENGINE_INSTALL_DIR}" diff --git a/src/InstallationDirectories.cc b/src/InstallationDirectories.cc index 56735383f..7b9bcf459 100644 --- a/src/InstallationDirectories.cc +++ b/src/InstallationDirectories.cc @@ -28,7 +28,7 @@ namespace gz { namespace physics { -namespace { +inline namespace GZ_PHYSICS_VERSION_NAMESPACE { // We locally import the gz::common::joinPaths function // See https://github.com/gazebosim/gz-physics/pull/507#discussion_r1186919267 @@ -36,20 +36,19 @@ namespace { // Function imported from // https://github.com/gazebosim/gz-common/blob/ignition-common4_4.6.2/src/FilesystemBoost.cc#L507 -#ifndef _WIN32 -const char preferred_separator = '/'; +#ifndef WIN32 +static const char preferred_separator = '/'; #else // Windows static const char preferred_separator = '\\'; #endif -std::string separator(const std::string &_p) +const std::string separator(const std::string &_p) { return _p + preferred_separator; } -#ifdef _WIN32 // Function imported from // https://github.com/gazebosim/gz-common/blob/ignition-common4_4.6.2/src/Filesystem.cc#L227 -std::string checkWindowsPath(const std::string &_path) +std::string checkWindowsPath(const std::string _path) { if (_path.empty()) return _path; @@ -76,7 +75,6 @@ std::string checkWindowsPath(const std::string &_path) result, std::regex("[<>:\"|?*]"), ""); return result; } -#endif // Function imported from // https://github.com/gazebosim/gz-common/blob/ignition-common4_4.6.2/src/Filesystem.cc#L256 @@ -144,18 +142,14 @@ std::string joinPaths(const std::string &_path1, #endif // _WIN32 return path; } -} // namespace -inline namespace GZ_PHYSICS_VERSION_NAMESPACE { -#ifdef GZ_PHYSICS_BAZEL_BUILD -std::string getInstallPrefix() { return "physics"; } -#endif std::string getEngineInstallDir() { return gz::physics::joinPaths( getInstallPrefix(), GZ_PHYSICS_ENGINE_RELATIVE_INSTALL_DIR); } -} // namespace GZ_PHYSICS_VERSION_NAMESPACE -} // namespace physics -} // namespace gz + +} +} +} diff --git a/test/BUILD.bazel b/test/BUILD.bazel deleted file mode 100644 index 3bb662543..000000000 --- a/test/BUILD.bazel +++ /dev/null @@ -1,269 +0,0 @@ -load( - "@gz//bazel/skylark:build_defs.bzl", - "GZ_ROOT", - "GZ_VISIBILITY", -) - -cc_library( - name = "test_headers", - srcs = glob(["include/**/*.hh"]), - includes = ["include"], - visibility = GZ_VISIBILITY, - deps = [ - GZ_ROOT + "common", - GZ_ROOT + "common/testing", - GZ_ROOT + "physics", - GZ_ROOT + "plugin:loader", - ], -) - -cc_library( - name = "common_test", - hdrs = ["common_test/Worlds.hh"], - includes = ["common_test"], - data = [ - ":resources", - "common_test/worlds", - ], - deps = [ - ":test_headers", - ] -) - -cc_library( - name = "common_test_dartsim", - data = [ - GZ_ROOT + "physics/dartsim:libgz-physics-dartsim-plugin.so", - ], - deps = [ - ":common_test" - ] -) - -cc_library( - name = "common_test_bullet-featherstone", - data = [ - GZ_ROOT + "physics/bullet-featherstone:libgz-physics-bullet-featherstone-plugin.so", - ], - deps = [ - ":common_test" - ] -) - -physics_engines = [ - "dartsim", - "bullet-featherstone", -] - -[cc_test( - name = "addexternalforcetorque_%s" % engine, - srcs = [ - "common_test/addexternalforcetorque.cc", - ], - env = { - "GZ_BAZEL": "1", - "GZ_BAZEL_PATH": "physics", - "LIB_TO_TEST": "physics/%s/libgz-physics-%s-plugin.so" % (engine, engine), - }, - deps = [ - ":common_test_%s" % engine, - GZ_ROOT + "math/eigen3", - GZ_ROOT + "physics:sdf", - "@gtest", - "@gtest//:gtest_main", - ], -) for engine in physics_engines] - -[cc_test( - name = "basic_test_%s" % engine, - srcs = [ - "common_test/basic_test.cc", - ], - env = { - "GZ_BAZEL": "1", - "GZ_BAZEL_PATH": "physics", - "LIB_TO_TEST": "physics/%s/libgz-physics-%s-plugin.so" % (engine, engine), - }, - deps = [ - ":common_test_%s" % engine, - "@gtest", - "@gtest//:gtest_main", - ], -) for engine in physics_engines] - -[cc_test( - name = "collisions_%s" % engine, - srcs = [ - "common_test/collisions.cc", - ], - defines = [ - 'GZ_PHYSICS_RESOURCE_DIR=\\"physics/resources/\\"', - ], - env = { - "GZ_BAZEL": "1", - "GZ_BAZEL_PATH": "physics", - "LIB_TO_TEST": "physics/%s/libgz-physics-%s-plugin.so" % (engine, engine), - }, - deps = [ - ":common_test_%s" % engine, - GZ_ROOT + "common/graphics", - GZ_ROOT + "math/eigen3", - GZ_ROOT + "physics:mesh", - GZ_ROOT + "physics:sdf", - "@gtest", - "@gtest//:gtest_main", - ], -) for engine in physics_engines] - -[cc_test( - name = "construct_empty_world_%s" % engine, - srcs = [ - "common_test/construct_empty_world.cc", - ], - env = { - "GZ_BAZEL": "1", - "GZ_BAZEL_PATH": "physics", - "LIB_TO_TEST": "physics/%s/libgz-physics-%s-plugin.so" % (engine, engine), - }, - deps = [ - ":common_test_%s" % engine, - GZ_ROOT + "math/eigen3", - "@gtest", - "@gtest//:gtest_main", - ], -) for engine in physics_engines] - -[cc_test( - name = "free_joint_features_%s" % engine, - srcs = [ - "common_test/free_joint_features.cc", - ], - env = { - "GZ_BAZEL": "1", - "GZ_BAZEL_PATH": "physics", - "LIB_TO_TEST": "physics/%s/libgz-physics-%s-plugin.so" % (engine, engine), - }, - deps = [ - ":common_test_%s" % engine, - GZ_ROOT + "math/eigen3", - GZ_ROOT + "physics:sdf", - "@gtest", - "@gtest//:gtest_main", - ], -) for engine in physics_engines] - -[cc_test( - name = "joint_features_%s" % engine, - srcs = [ - "common_test/joint_features.cc", - ], - env = { - "GZ_BAZEL": "1", - "GZ_BAZEL_PATH": "physics", - "LIB_TO_TEST": "physics/%s/libgz-physics-%s-plugin.so" % (engine, engine), - }, - deps = [ - ":common_test_%s" % engine, - GZ_ROOT + "math/eigen3", - GZ_ROOT + "physics:sdf", - "@gtest", - "@gtest//:gtest_main", - ], -) for engine in physics_engines] - -[cc_test( - name = "kinematic_features_%s" % engine, - srcs = [ - "common_test/kinematic_features.cc", - ], - env = { - "GZ_BAZEL": "1", - "GZ_BAZEL_PATH": "physics", - "LIB_TO_TEST": "physics/%s/libgz-physics-%s-plugin.so" % (engine, engine), - }, - deps = [ - ":common_test_%s" % engine, - GZ_ROOT + "physics:sdf", - "@eigen3", - "@gtest", - "@gtest//:gtest_main", - ], -) for engine in physics_engines] - -[cc_test( - name = "link_features_%s" % engine, - srcs = [ - "common_test/link_features.cc", - ], - env = { - "GZ_BAZEL": "1", - "GZ_BAZEL_PATH": "physics", - "LIB_TO_TEST": "physics/%s/libgz-physics-%s-plugin.so" % (engine, engine), - }, - deps = [ - ":common_test_%s" % engine, - GZ_ROOT + "math/eigen3", - GZ_ROOT + "physics:sdf", - "@gtest", - "@gtest//:gtest_main", - ], -) for engine in physics_engines] - -[cc_test( - name = "shape_features_%s" % engine, - srcs = [ - "common_test/shape_features.cc", - ], - env = { - "GZ_BAZEL": "1", - "GZ_BAZEL_PATH": "physics", - "LIB_TO_TEST": "physics/%s/libgz-physics-%s-plugin.so" % (engine, engine), - }, - deps = [ - ":common_test_%s" % engine, - GZ_ROOT + "math/eigen3", - GZ_ROOT + "physics:sdf", - "@gtest", - "@gtest//:gtest_main", - ], -) for engine in physics_engines] - -[cc_test( - name = "simulation_features_%s" % engine, - srcs = [ - "common_test/simulation_features.cc", - ], - env = { - "GZ_BAZEL": "1", - "GZ_BAZEL_PATH": "physics", - "LIB_TO_TEST": "physics/%s/libgz-physics-%s-plugin.so" % (engine, engine), - }, - deps = [ - ":common_test_%s" % engine, - GZ_ROOT + "math/eigen3", - GZ_ROOT + "physics:sdf", - "@gtest", - "@gtest//:gtest_main", - ], -) for engine in physics_engines] - -[cc_test( - name = "world_features_%s" % engine, - srcs = [ - "common_test/world_features.cc", - ], - env = { - "GZ_BAZEL": "1", - "GZ_BAZEL_PATH": "physics", - "LIB_TO_TEST": "physics/%s/libgz-physics-%s-plugin.so" % (engine, engine), - }, - deps = [ - ":common_test_%s" % engine, - GZ_ROOT + "math/eigen3", - GZ_ROOT + "physics:sdf", - "@gtest", - "@gtest//:gtest_main", - ], -) for engine in physics_engines] - -exports_files(["resources"]) diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index 49c904635..a35e2127a 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -44,6 +44,7 @@ function(configure_common_test PHYSICS_ENGINE_NAME test_name) endif() endfunction() + # Get bullet version using pkg_check_modules as it is not available # through the cmake module gz_pkg_check_modules_quiet(bullet_version_check "bullet") @@ -70,6 +71,7 @@ foreach(test ${tests}) ) endif() + install(TARGETS ${test_executable} DESTINATION ${TEST_INSTALL_DIR}) configure_common_test("bullet" ${test_executable}) configure_common_test("bullet-featherstone" ${test_executable}) diff --git a/test/include/test/TestLibLoader.hh b/test/include/test/TestLibLoader.hh index b07d05528..a7f27abd3 100644 --- a/test/include/test/TestLibLoader.hh +++ b/test/include/test/TestLibLoader.hh @@ -41,29 +41,13 @@ class TestLibLoader { if (argc != 2) { - std::string envLibToTest; - if(!gz::utils::env("LIB_TO_TEST", envLibToTest)) - { - std::cerr << "Please provide the path to an engine plugin.\n" - << "Usage " << argv[0] << " \n"; - return false; - } - else - { - std::string &libToTest = LibToTest(); - libToTest = envLibToTest; - - std::cout << std::filesystem::current_path() << std::endl; - std::cout << envLibToTest << std::endl; - return true; - } - } - else - { - std::string &libToTest = LibToTest(); - libToTest = argv[1]; - return true; + std::cerr << "Please provide the path to an engine plugin.\n" + << "Usage " << argv[0] << " \n"; + return false; } + std::string &libToTest = LibToTest(); + libToTest = argv[1]; + return true; } /// \brief Get the name of the library to test diff --git a/tpe/plugin/src/SimulationFeatures_TEST.cc b/tpe/plugin/src/SimulationFeatures_TEST.cc index 25fff9ecc..ddecee5de 100644 --- a/tpe/plugin/src/SimulationFeatures_TEST.cc +++ b/tpe/plugin/src/SimulationFeatures_TEST.cc @@ -52,6 +52,7 @@ #include "World.hh" + using namespace gz; struct TestFeatureList : physics::FeatureList< From 7ff508d8b5779d8d27c88069450ac4a5428c9e51 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 28 Feb 2024 14:33:44 -0600 Subject: [PATCH 03/18] Require gz-math 6.13 for ign->gz transition (#601) Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c4236e18d..6862813f7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -44,7 +44,7 @@ set(IGN_COMMON_VER ${ignition-common4_VERSION_MAJOR}) #-------------------------------------- # Find ignition-math -ign_find_package(ignition-math6 REQUIRED COMPONENTS eigen3) +ign_find_package(ignition-math6 REQUIRED COMPONENTS eigen3 VERSION 6.13) set(IGN_MATH_VER ${ignition-math6_VERSION_MAJOR}) #-------------------------------------- From 18e0daddd6a3f034cc3b6d7bcded899e815af036 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 5 Mar 2024 20:07:52 -0800 Subject: [PATCH 04/18] bullet-featherstone: Support nested models (#574) Extended bullet-featurestone implementation to support these features related to nested models: * ConstructSdfJoint (limited to creating fixed joints to the world) * ConstructSdfNestedModel * GetNestedModelFromModel * WorldModelFeature * RemoveEntities The nested models are currently added to a single multibody since bullet-featherstone does not currently support joints between two different multibodies. The nested model keeps a shared ptr to the root link's btMultibody. With the addition of the above features, more common tests are now being run using bullet-feathersone plugin. Closes #546, #547. Signed-off-by: Shameek Ganguly Signed-off-by: Steve Peters Signed-off-by: Ian Chen Co-authored-by: Shameek Ganguly Co-authored-by: Steve Peters --- bullet-featherstone/src/Base.hh | 145 +++- .../src/EntityManagementFeatures.cc | 154 +++- .../src/EntityManagementFeatures.hh | 23 +- bullet-featherstone/src/FreeGroupFeatures.cc | 13 +- bullet-featherstone/src/SDFFeatures.cc | 793 +++++++++++++----- bullet-featherstone/src/SDFFeatures.hh | 19 + test/common_test/CMakeLists.txt | 5 + test/common_test/Worlds.hh | 2 + test/common_test/free_joint_features.cc | 11 + test/common_test/joint_features.cc | 3 + test/common_test/world_features.cc | 100 +++ .../worlds/world_single_nested_model.sdf | 61 ++ 12 files changed, 1065 insertions(+), 264 deletions(-) create mode 100644 test/common_test/worlds/world_single_nested_model.sdf diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 184d958c2..4e9bb4338 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -84,12 +84,14 @@ struct ModelInfo Identity world; int indexInWorld; Eigen::Isometry3d baseInertiaToLinkFrame; - std::unique_ptr body; + std::shared_ptr body; std::vector linkEntityIds; std::vector jointEntityIds; + std::vector nestedModelEntityIds; std::unordered_map linkNameToEntityId; std::unordered_map jointNameToEntityId; + std::unordered_map nestedModelNameToEntityId; /// These are joints that connect this model to other models, e.g. fixed /// constraints. @@ -99,7 +101,7 @@ struct ModelInfo std::string _name, Identity _world, Eigen::Isometry3d _baseInertiaToLinkFrame, - std::unique_ptr _body) + std::shared_ptr _body) : name(std::move(_name)), world(std::move(_world)), baseInertiaToLinkFrame(_baseInertiaToLinkFrame), @@ -273,14 +275,24 @@ class Base : public Implements3d> const auto id = this->GetNextEntity(); auto world = std::make_shared(std::move(_worldInfo)); this->worlds[id] = world; - return this->GenerateIdentity(id, world); + auto worldID = this->GenerateIdentity(id, world); + + auto worldModel = std::make_shared( + world->name, worldID, + Eigen::Isometry3d::Identity(), nullptr); + this->models[id] = worldModel; + world->modelNameToEntityId[worldModel->name] = id; + worldModel->indexInWorld = -1; + world->modelIndexToEntityId[worldModel->indexInWorld] = id; + + return worldID; } public: inline Identity AddModel( std::string _name, Identity _worldID, Eigen::Isometry3d _baseInertialToLinkFrame, - std::unique_ptr _body) + std::shared_ptr _body) { const auto id = this->GetNextEntity(); auto model = std::make_shared( @@ -292,6 +304,30 @@ class Base : public Implements3d> world->modelNameToEntityId[model->name] = id; model->indexInWorld = world->nextModelIndex++; world->modelIndexToEntityId[model->indexInWorld] = id; + + auto worldModel = this->models.at(model->world); + worldModel->nestedModelEntityIds.push_back(id); + worldModel->nestedModelNameToEntityId[model->name] = id; + + return this->GenerateIdentity(id, model); + } + + public: inline Identity AddNestedModel( + std::string _name, + Identity _parentID, + Identity _worldID, + Eigen::Isometry3d _baseInertialToLinkFrame, + std::shared_ptr _body) + { + const auto id = this->GetNextEntity(); + auto model = std::make_shared( + std::move(_name), std::move(_worldID), + std::move(_baseInertialToLinkFrame), std::move(_body)); + + this->models[id] = model; + const auto parentModel = this->models.at(_parentID); + parentModel->nestedModelEntityIds.push_back(id); + parentModel->nestedModelNameToEntityId[model->name] = id; return this->GenerateIdentity(id, model); } @@ -303,13 +339,7 @@ class Base : public Implements3d> auto *model = this->ReferenceInterface(_linkInfo.model); model->linkNameToEntityId[link->name] = id; - if (link->indexInModel.has_value()) - { - // We expect the links to be added in order - assert(static_cast(*link->indexInModel + 1) == - model->linkEntityIds.size()); - } - else + if (!link->indexInModel.has_value()) { // We are adding the root link. This means the model should not already // have a root link @@ -357,6 +387,99 @@ class Base : public Implements3d> return this->GenerateIdentity(id, joint); } + public: bool RemoveModelImpl(const Identity &_parentID, + const Identity &_modelID) + { + auto *model = this->ReferenceInterface(_modelID); + if (!model) + return false; + + // Remove nested models + for (auto &nestedModelID : model->nestedModelEntityIds) + { + this->RemoveModelImpl(_modelID, this->GenerateIdentity(nestedModelID, + this->models.at(nestedModelID))); + } + model->nestedModelEntityIds.clear(); + + // remove references in parent model or world model + auto parentModelIt = this->models.find(_parentID); + if (parentModelIt != this->models.end()) + { + auto parentModel = parentModelIt->second; + auto nestedModelIt = + parentModel->nestedModelNameToEntityId.find(model->name); + if (nestedModelIt != + parentModel->nestedModelNameToEntityId.end()) + { + std::size_t nestedModelID = nestedModelIt->second; + parentModel->nestedModelNameToEntityId.erase(nestedModelIt); + parentModel->nestedModelEntityIds.erase(std::remove( + parentModel->nestedModelEntityIds.begin(), + parentModel->nestedModelEntityIds.end(), nestedModelID), + parentModel->nestedModelEntityIds.end()); + } + } + + // If nested, no need to remove multibody + // \todo(iche033) Remove links and joints in nested model + bool isNested = this->worlds.find(_parentID) == this->worlds.end(); + if (isNested) + { + return true; + } + + // Remove model from world + auto *world = this->ReferenceInterface(model->world); + if (!world) + return false; + if (world->modelIndexToEntityId.erase(model->indexInWorld) == 0) + { + // The model has already been removed at some point + return false; + } + world->modelNameToEntityId.erase(model->name); + + // Remove all constraints related to this model + for (const auto jointID : model->jointEntityIds) + { + const auto joint = this->joints.at(jointID); + if (joint->motor) + { + world->world->removeMultiBodyConstraint(joint->motor.get()); + } + if (joint->fixedConstraint) + { + world->world->removeMultiBodyConstraint(joint->fixedConstraint.get()); + } + if (joint->jointLimits) + { + world->world->removeMultiBodyConstraint(joint->jointLimits.get()); + } + this->joints.erase(jointID); + } + // \todo(iche033) Remove external constraints related to this model + // (model->external_constraints) once this is supported + + world->world->removeMultiBody(model->body.get()); + for (const auto linkID : model->linkEntityIds) + { + const auto &link = this->links.at(linkID); + if (link->collider) + { + world->world->removeCollisionObject(link->collider.get()); + for (const auto shapeID : link->collisionEntityIds) + this->collisions.erase(shapeID); + } + + this->links.erase(linkID); + } + + this->models.erase(_modelID); + + return true; + } + public: ~Base() override { // The order of destruction between meshesGImpact and triangleMeshes is // important. diff --git a/bullet-featherstone/src/EntityManagementFeatures.cc b/bullet-featherstone/src/EntityManagementFeatures.cc index 3a491862a..99e40b827 100644 --- a/bullet-featherstone/src/EntityManagementFeatures.cc +++ b/bullet-featherstone/src/EntityManagementFeatures.cc @@ -206,53 +206,16 @@ Identity EntityManagementFeatures::ConstructEmptyWorld( bool EntityManagementFeatures::RemoveModel(const Identity &_modelID) { auto *model = this->ReferenceInterface(_modelID); - auto *world = this->ReferenceInterface(model->world); - if (world->modelIndexToEntityId.erase(model->indexInWorld) == 0) - { - // The model has already been removed at some point. + if (!model) return false; - } - - world->modelNameToEntityId.erase(model->name); - - // Remove all constraints related to this model - for (auto constraint_index : model->external_constraints) - { - const auto joint = this->joints.at(constraint_index); - const auto &constraint = - std::get>(joint->identifier); - world->world->removeMultiBodyConstraint(constraint.get()); - this->joints.erase(constraint_index); - } - - world->world->removeMultiBody(model->body.get()); - for (const auto linkID : model->linkEntityIds) - { - const auto &link = this->links.at(linkID); - if (link->collider) - { - world->world->removeCollisionObject(link->collider.get()); - for (const auto shapeID : link->collisionEntityIds) - this->collisions.erase(shapeID); - } - - this->links.erase(linkID); - } - - for (const auto jointID : model->jointEntityIds) - this->joints.erase(jointID); - - this->models.erase(_modelID); - return true; + return this->RemoveModelImpl(model->world, _modelID); } ///////////////////////////////////////////////// bool EntityManagementFeatures::ModelRemoved( const Identity &_modelID) const { - auto *model = this->ReferenceInterface(_modelID); - auto *world = this->ReferenceInterface(model->world); - return world->modelIndexToEntityId.count(model->indexInWorld) == 0; + return this->models.find(_modelID) == this->models.end(); } ///////////////////////////////////////////////// @@ -283,6 +246,55 @@ bool EntityManagementFeatures::RemoveModelByName( this->GenerateIdentity(it->second, this->models.at(it->second))); } +///////////////////////////////////////////////// +bool EntityManagementFeatures::RemoveNestedModelByIndex( + const Identity &_modelID, std::size_t _nestedModelIndex) +{ + auto *model = this->ReferenceInterface(_modelID); + if (!model) + return false; + if (_nestedModelIndex >= model->nestedModelEntityIds.size()) + return false; + const auto nestedModelID = model->nestedModelEntityIds[_nestedModelIndex]; + + auto modelIt = this->models.find(nestedModelID); + if (modelIt != this->models.end()) + { + return RemoveModelImpl(_modelID, + this->GenerateIdentity(modelIt->first, modelIt->second)); + } + return false; +} + +///////////////////////////////////////////////// +bool EntityManagementFeatures::RemoveNestedModelByName(const Identity &_modelID, + const std::string &_modelName) +{ + auto *model = this->ReferenceInterface(_modelID); + if (!model) + return false; + + unsigned int nestedModelID = 0u; + auto nestedModelIt = model->nestedModelNameToEntityId.find(_modelName); + if (nestedModelIt != model->nestedModelNameToEntityId.end()) + { + nestedModelID = nestedModelIt->second; + } + else + { + return false; + } + + auto modelIt = this->models.find(nestedModelID); + if (modelIt != this->models.end()) + { + return RemoveModelImpl(_modelID, + this->GenerateIdentity(modelIt->first, modelIt->second)); + } + return false; +} + + ///////////////////////////////////////////////// const std::string &EntityManagementFeatures::GetEngineName( const Identity &) const @@ -359,9 +371,15 @@ Identity EntityManagementFeatures::GetEngineOfWorld( ///////////////////////////////////////////////// std::size_t EntityManagementFeatures::GetModelCount( - const Identity &) const + const Identity &_worldID) const { - return this->models.size(); + // Get world model and return its nested model count + auto modelIt = this->models.find(_worldID); + if (modelIt != this->models.end()) + { + return modelIt->second->nestedModelEntityIds.size(); + } + return 0u; } ///////////////////////////////////////////////// @@ -414,6 +432,58 @@ Identity EntityManagementFeatures::GetWorldOfModel( return this->ReferenceInterface(_modelID)->world; } +///////////////////////////////////////////////// +std::size_t EntityManagementFeatures::GetNestedModelCount( + const Identity &_modelID) const +{ + return this->ReferenceInterface( + _modelID)->nestedModelEntityIds.size(); +} + +///////////////////////////////////////////////// +Identity EntityManagementFeatures::GetNestedModel( + const Identity &_modelID, const std::size_t _modelIndex) const +{ + const auto modelInfo = this->ReferenceInterface(_modelID); + if (_modelIndex >= modelInfo->nestedModelEntityIds.size()) + { + return this->GenerateInvalidId(); + } + + const auto nestedModelID = modelInfo->nestedModelEntityIds[_modelIndex]; + + // If the model doesn't exist in "models", it means the containing entity has + // been removed. + if (this->models.find(nestedModelID) != this->models.end()) + { + return this->GenerateIdentity(nestedModelID, + this->models.at(nestedModelID)); + } + else + { + return this->GenerateInvalidId(); + } +} + +///////////////////////////////////////////////// +Identity EntityManagementFeatures::GetNestedModel( + const Identity &_modelID, const std::string &_modelName) const +{ + const auto modelInfo = this->ReferenceInterface(_modelID); + auto modelIt = modelInfo->nestedModelNameToEntityId.find(_modelName); + if (modelIt == modelInfo->nestedModelNameToEntityId.end()) + return this->GenerateInvalidId(); + auto nestedModelID = modelIt->second; + return this->GenerateIdentity(nestedModelID, + this->models.at(nestedModelID)); +} + +///////////////////////////////////////////////// +Identity EntityManagementFeatures::GetWorldModel(const Identity &_worldID) const +{ + return this->GenerateIdentity(_worldID, this->models.at(_worldID)); +} + } // namespace bullet_featherstone } // namespace physics } // namespace gz diff --git a/bullet-featherstone/src/EntityManagementFeatures.hh b/bullet-featherstone/src/EntityManagementFeatures.hh index 09d45bdba..1cbb47813 100644 --- a/bullet-featherstone/src/EntityManagementFeatures.hh +++ b/bullet-featherstone/src/EntityManagementFeatures.hh @@ -37,9 +37,11 @@ struct EntityManagementFeatureList : gz::physics::FeatureList< GetJointFromModel, GetLinkFromModel, GetModelFromWorld, + GetNestedModelFromModel, GetShapeFromLink, GetWorldFromEngine, - RemoveModelFromWorld + RemoveEntities, + WorldModelFeature > { }; class EntityManagementFeatures : @@ -155,9 +157,28 @@ class EntityManagementFeatures : public: bool ModelRemoved(const Identity &_modelID) const override; + public: bool RemoveNestedModelByIndex( + const Identity &_modelID, std::size_t _nestedModelIndex) override; + + public: bool RemoveNestedModelByName( + const Identity &_modelID, const std::string &_modelName) override; + // ----- Construct empty entities ----- public: Identity ConstructEmptyWorld( const Identity &_engineID, const std::string & _name) override; + + // ----- GetNestedModelFromModel ----- + public: std::size_t GetNestedModelCount( + const Identity &_modelID) const override; + + public: Identity GetNestedModel( + const Identity &_modelID, std::size_t _modelIndex) const override; + + public: Identity GetNestedModel( + const Identity &_modelID, const std::string &_modelName) const override; + + // ----- World model feature ----- + public: Identity GetWorldModel(const Identity &_worldID) const override; }; } // namespace bullet_featherstone diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index c557d6465..22e219528 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -54,8 +54,8 @@ Identity FreeGroupFeatures::GetFreeGroupRootLink(const Identity &_groupID) const // Free groups in bullet-featherstone are always represented by ModelInfo const auto *model = this->ReferenceInterface(_groupID); - // The first link entity in the model is always the root link - const std::size_t rootID = model->linkEntityIds.front(); + // btMultiBody user index stores the gz-phsics model root link id + std::size_t rootID = static_cast(model->body->getUserIndex()); return this->GenerateIdentity(rootID, this->links.at(rootID)); } @@ -66,7 +66,7 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity( // Free groups in bullet-featherstone are always represented by ModelInfo const auto *model = this->ReferenceInterface(_groupID); - if (model != nullptr) + if (model) { model->body->setBaseOmega(convertVec(_angularVelocity)); } @@ -79,7 +79,7 @@ void FreeGroupFeatures::SetFreeGroupWorldLinearVelocity( // Free groups in bullet-featherstone are always represented by ModelInfo const auto *model = this->ReferenceInterface(_groupID); // Set Base Vel - if(model) + if (model) { model->body->setBaseVel(convertVec(_linearVelocity)); } @@ -91,7 +91,10 @@ void FreeGroupFeatures::SetFreeGroupWorldPose( const PoseType &_pose) { const auto *model = this->ReferenceInterface(_groupID); - model->body->setBaseWorldTransform(convertTf(_pose)); + if (model) + { + model->body->setBaseWorldTransform(convertTf(_pose)); + } } } // namespace bullet_featherstone diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index da528ae35..0bbdfc11e 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -106,6 +106,8 @@ struct ParentInfo { const ::sdf::Joint *joint; const ::sdf::Model *model; + const ::sdf::Link *link; + const ::sdf::Link *parent; }; ///////////////////////////////////////////////// @@ -113,6 +115,7 @@ struct Structure { /// The root link of the model const ::sdf::Link *rootLink; + const ::sdf::Model *model; const ::sdf::Joint *rootJoint; btScalar mass; btVector3 inertia; @@ -143,167 +146,209 @@ void extractInertial( } ///////////////////////////////////////////////// -std::optional ValidateModel(const ::sdf::Model &_sdfModel) +/// \brief Get pose of joint relative to link +/// \param[out] _resolvedPose Pose of joint relative to link +/// \param[in] _model Parent model of joint +/// \param[in] _joint Joint name +/// \param[in] _link Scoped link name +::sdf::Errors resolveJointPoseRelToLink(math::Pose3d &_resolvedPose, + const ::sdf::Model *_model, + const std::string &_joint, const std::string &_link) { - std::unordered_map parentOf; - const ::sdf::Link *rootLink = nullptr; - const ::sdf::Joint *rootJoint = nullptr; - bool fixed = false; - const std::string &rootModelName = _sdfModel.Name(); - // a map of parent link to a vector of its child links - std::unordered_map> - linkTree; - - const auto checkModel = - [&rootLink, &rootJoint, &fixed, &parentOf, &rootModelName, &linkTree]( - const ::sdf::Model &model) -> bool - { - for (std::size_t i = 0; i < model.JointCount(); ++i) - { - const auto *joint = model.JointByIndex(i); - const auto &parentLinkName = joint->ParentName(); - const auto *parent = model.LinkByName(parentLinkName); - const auto &childLinkName = joint->ChildName(); - const auto *child = model.LinkByName(childLinkName); - - switch (joint->Type()) - { - case ::sdf::JointType::FIXED: - case ::sdf::JointType::REVOLUTE: - case ::sdf::JointType::PRISMATIC: - case ::sdf::JointType::BALL: - break; - default: - gzerr << "Joint type [" << (std::size_t)(joint->Type()) - << "] is not supported by " - << "gz-physics-bullet-featherstone-plugin. " - << "Replaced by a fixed joint.\n"; - } - - if (child == parent) - { - gzerr << "The Link [" << parentLinkName << "] is being attached to " - << "itself by Joint [" << joint->Name() << "] in Model [" - << rootModelName << "]. That is not allowed.\n"; - return false; - } + ::sdf::Errors errors; + const auto *joint = _model->JointByName(_joint); + if (!joint) + { + gzerr << "No joint [" << _joint << "] found in model [" + << _model->Name() << "]" << std::endl; + return errors; + } - if (nullptr == parent && parentLinkName != "world") - { - gzerr << "The link [" << parentLinkName << "] cannot be found in " - << "Model [" << rootModelName << "], but joint [" - << joint->Name() << "] wants to use it as its parent link\n"; - return false; - } - else if (nullptr == parent) - { - // This link is attached to the world, making it the root - if (nullptr != rootLink) - { - // A root already exists for this model - gzerr << "Two root links were found for Model [" << rootModelName - << "]: [" << rootLink->Name() << "] and [" << childLinkName - << "], but gz-physics-bullet-featherstone-plugin only " - << "supports one root per Model.\n"; - return false; - } + std::string childLinkName; + errors = joint->ResolveChildLink(childLinkName); + if (!errors.empty()) + { + childLinkName = joint->ChildName(); + } - if (joint->Type() != ::sdf::JointType::FIXED) - { - gzerr << "Link [" << child->Name() << "] in Model [" - << rootModelName << "] is being connected to the " - << "world by Joint [" << joint->Name() << "] with a [" - << (std::size_t)(joint->Type()) << "] joint type, but only " - << "Fixed (" << (std::size_t)(::sdf::JointType::FIXED) - << ") is supported by " - << "gz-physics-bullet-featherstone-plugin\n"; - return false; - } + // joint pose is expressed relative to child link so return joint pose + // if input link is the child link + if (childLinkName == _link) + { + errors = joint->SemanticPose().Resolve(_resolvedPose); + return errors; + } - rootLink = child; - rootJoint = joint; - fixed = true; + // compute joint pose relative to specified link + const auto *link = _model->LinkByName(_link); + if (!link) + { + gzerr << "No link [" << _link << "] found in model [" + << _model->Name() << "]" << std::endl; + return errors; + } - // Do not add the root link to the set of links that have parents - continue; - } + math::Pose3d jointPoseRelToModel; + errors = _model->SemanticPose().Resolve(jointPoseRelToModel, + _model->Name() + "::" + _joint); + jointPoseRelToModel = jointPoseRelToModel.Inverse(); - if (!parentOf.insert( - std::make_pair(child, ParentInfo{joint, &model})).second) - { - gzerr << "The Link [" << childLinkName << "] in Model [" - << rootModelName << "] has multiple parent joints. That is not " - << "supported by the gz-physics-bullet-featherstone plugin.\n"; - } - if (parent != nullptr) - { - linkTree[parent].push_back(child); - } - } + math::Pose3d modelPoseRelToLink; + errors = _model->SemanticPose().Resolve(modelPoseRelToLink, + _model->Name() + "::" + _link); - return true; - }; + _resolvedPose = modelPoseRelToLink * jointPoseRelToModel; - if (!checkModel(_sdfModel)) - return std::nullopt; + return errors; +} - for (std::size_t i = 0; i < _sdfModel.ModelCount(); ++i) +///////////////////////////////////////////////// +/// \brief Recursively build a tree of parent-child data structures from the +/// input Model SDF. +/// \param[in] _sdfModel input Model SDF. +/// \param[out] _parentOf A map of child link to its parent +/// \param[out] _linkTree A map of parent link to its child links +bool buildTrees(const ::sdf::Model *_sdfModel, + std::unordered_map &_parentOf, + std::unordered_map> &_linkTree) +{ + for (std::size_t i = 0; i < _sdfModel->JointCount(); ++i) { - if (!checkModel(*_sdfModel.ModelByIndex(i))) - return std::nullopt; - } + const auto *joint = _sdfModel->JointByIndex(i); + std::string parentLinkName; + ::sdf::Errors errors = joint->ResolveParentLink(parentLinkName); + if (!errors.empty()) + { + parentLinkName = joint->ParentName(); + } + std::string childLinkName; + errors = joint->ResolveChildLink(childLinkName); + if (!errors.empty()) + { + childLinkName = joint->ChildName(); + } + const std::string &modelName = _sdfModel->Name(); + const auto *parent = _sdfModel->LinkByName(parentLinkName); + const auto *child = _sdfModel->LinkByName(childLinkName); - // Find root link in model and verify that there is only one root link in - // the model. Returns false if more than one root link is found - const auto findRootLink = - [&rootLink, &parentOf, &rootModelName](const ::sdf::Model &model) -> bool + switch (joint->Type()) + { + case ::sdf::JointType::FIXED: + case ::sdf::JointType::REVOLUTE: + case ::sdf::JointType::PRISMATIC: + case ::sdf::JointType::BALL: + break; + default: + gzerr << "Joint type [" << (std::size_t)(joint->Type()) + << "] is not supported by " + << "gz-physics-bullet-featherstone-plugin. " + << "Replaced by a fixed joint.\n"; + } + if (child == parent) + { + gzerr << "The Link [" << parentLinkName << "] is being attached to " + << "itself by Joint [" << joint->Name() << "] in Model [" + << modelName << "]. That is not allowed.\n"; + return false; + } + if (nullptr == parent && parentLinkName != "world") + { + gzerr << "The link [" << parentLinkName << "] cannot be found in " + << "Model [" << modelName << "], but joint [" + << joint->Name() << "] wants to use it as its parent link\n"; + return false; + } + else if (nullptr == parent) { - for (std::size_t i = 0; i < model.LinkCount(); ++i) + // This link is attached to the world, making it the root + if (joint->Type() != ::sdf::JointType::FIXED) { - const auto *link = model.LinkByIndex(i); - if (parentOf.count(link) == 0) - { - // This link must be the root. If a different link was already - // identified as the root then we have a conflict. - if (rootLink && rootLink != link) - { - gzerr << "Two root links were found for Model [" << rootModelName - << "]: [" << rootLink->Name() << "] and [" << link->Name() - << "]. The Link [" << link->Name() << "] is implicitly a " - << "root because it has no parent joint.\n"; - return false; - } - - rootLink = link; - } + gzerr << "Link [" << child->Name() << "] in Model [" + << modelName << "] is being connected to the " + << "world by Joint [" << joint->Name() << "] with a [" + << (std::size_t)(joint->Type()) << "] joint type, but only " + << "Fixed (" << (std::size_t)(::sdf::JointType::FIXED) + << ") is supported by " + << "gz-physics-bullet-featherstone-plugin\n"; + return false; } + } - return true; - }; + if (!_parentOf.insert( + std::make_pair(child, ParentInfo{joint, _sdfModel, child, parent})) + .second) + { + gzerr << "The Link [" << childLinkName << "] in Model [" + << modelName << "] has multiple parent joints. That is not " + << "supported by the gz-physics-bullet-featherstone plugin.\n"; + } + if (parent != nullptr) + { + _linkTree[parent].push_back(child); + } + } - if (rootLink == nullptr && !findRootLink(_sdfModel)) + // Recursively build tree from nested models + for (std::size_t i = 0; i < _sdfModel->ModelCount(); ++i) { - // No root link was found in this model - return std::nullopt; + const auto *model = _sdfModel->ModelByIndex(i); + if (!buildTrees(model, _parentOf, _linkTree)) + return false; } + return true; +} - // find root link in nested models if one was not already found - for (std::size_t i = 0; i < _sdfModel.ModelCount(); ++i) +///////////////////////////////////////////////// +/// \brief Find all the root links given a model SDF +/// \param[in] _sdfModel Model SDF +/// \param[in] _parentOf A map of child link to its parent info +/// \param[out] _rootLinks A vector of root links paired with their +/// immediate parent model +void findRootLinks(const ::sdf::Model *_sdfModel, + const std::unordered_map &_parentOf, + std::vector> &_rootLinks) +{ + for (std::size_t i = 0; i < _sdfModel->LinkCount(); ++i) { - if (rootLink != nullptr) + const auto *link = _sdfModel->LinkByIndex(i); + auto parentOfIt = _parentOf.find(link); + if (parentOfIt == _parentOf.end() || !parentOfIt->second.parent) { - break; - } - if (!findRootLink(*_sdfModel.ModelByIndex(i))) - { - return std::nullopt; + // If there is not parent or parent is null (world), + // this link must be a root. + _rootLinks.push_back({link, _sdfModel}); } } - if (!rootLink) + for (std::size_t i = 0; i < _sdfModel->ModelCount(); ++i) { - gzerr << "No root link was found for model [" << _sdfModel.Name() << "\n"; - return std::nullopt; + const auto *model = _sdfModel->ModelByIndex(i); + findRootLinks(model, _parentOf, _rootLinks); + } +} + +///////////////////////////////////////////////// +/// \brief Build a structure for each root link +/// \param[in] _rootLink Root link in a model tree +/// \param[in] _parentOf A map of child link to its parent +/// \param[in] _linkTree A map of parent link to its child links +std::optional buildStructure( + const ::sdf::Link * _rootLink, + const ::sdf::Model * _model, + const std::unordered_map &_parentOf, + std::unordered_map> + &_linkTree) +{ + bool fixed = false; + const ::sdf::Joint *rootJoint = nullptr; + // rootJoint only exists if root link is connected to world by a joint + auto linkIt = _parentOf.find(_rootLink); + if (linkIt != _parentOf.end()) + { + const auto &parentInfo = linkIt->second; + rootJoint = parentInfo.joint; + fixed = true; } // The documentation for bullet does not mention whether parent links must @@ -317,63 +362,215 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) std::function flattenLinkTree = [&](const ::sdf::Link *link) { - if (link != rootLink) + if (link != _rootLink) { flatLinks.push_back(link); } - if (auto it = linkTree.find(link); it != linkTree.end()) + if (auto it = _linkTree.find(link); it != _linkTree.end()) { - for (const auto &child_link : it->second) + for (const auto &childLink : it->second) { - flattenLinkTree(child_link); + flattenLinkTree(childLink); } } }; - flattenLinkTree(rootLink); + flattenLinkTree(_rootLink); btScalar mass; btVector3 inertia; math::Pose3d linkToPrincipalAxesPose; - extractInertial(rootLink->Inertial(), mass, inertia, linkToPrincipalAxesPose); + extractInertial(_rootLink->Inertial(), mass, inertia, + linkToPrincipalAxesPose); + + // Uncomment to debug structure + // std::cout << "Structure: " << std::endl; + // std::cout << " model: " << _model->Name() << std::endl; + // std::cout << " root link: " << _rootLink->Name() << std::endl; + // std::cout << " root joint: " << ((rootJoint) ? rootJoint->Name() : "N/A") + // << std::endl; + // std::cout << " mass: " << mass << std::endl; + // std::cout << " fixed: " << fixed << std::endl; + // std::cout << " flatLinks size: " << flatLinks.size() << std::endl; return Structure{ - rootLink, rootJoint, mass, inertia, linkToPrincipalAxesPose, fixed, - parentOf, flatLinks}; + _rootLink, _model, rootJoint, mass, inertia, linkToPrincipalAxesPose, fixed, + _parentOf, flatLinks}; } ///////////////////////////////////////////////// -Identity SDFFeatures::ConstructSdfModel( - const Identity &_worldID, +/// \brief Validate input model SDF and build a vector of structures. +/// Each structure represents a single kinematic tree in the model +/// \param[in] _sdfModel Input model SDF +/// \return A vector of structures +std::vector validateModel(const ::sdf::Model &_sdfModel) +{ + // A map of child link and its parent info + std::unordered_map parentOf; + + // A map of parent link to a vector of its child links + std::unordered_map> + linkTree; + + // A vector of root link of its parent model + // Use a vector to preseve order of entities defined in sdf + std::vector> rootLinks; + + buildTrees(&_sdfModel, parentOf, linkTree); + findRootLinks(&_sdfModel, parentOf, rootLinks); + + std::vector structures; + if (rootLinks.empty()) + { + // No root link was found in this model + gzerr << "No root links are found in this model" << std::endl; + return structures; + } + + // Build subtrees + for (const auto &rootLinkIt : rootLinks) + { + auto structure = buildStructure(rootLinkIt.first, rootLinkIt.second, + parentOf, linkTree); + if (structure.has_value()) + { + structures.push_back(*structure); + } + } + + return structures; +} + +///////////////////////////////////////////////// +Identity SDFFeatures::ConstructSdfNestedModel(const Identity &_parentID, + const ::sdf::Model &_sdfModel) +{ + return this->ConstructSdfModelImpl(_parentID, _sdfModel); +} + +///////////////////////////////////////////////// +Identity SDFFeatures::ConstructSdfModelImpl( + std::size_t _parentID, const ::sdf::Model &_sdfModel) { - const auto validation = ValidateModel(_sdfModel); - if (!validation.has_value()) - return this->GenerateInvalidId(); + // The ConstructSdfModelImpl function constructs the entire sdf model tree, + // including nested models So return the nested model identity if it is + // constructed already + auto wIt = this->worlds.find(_parentID); + if (wIt == this->worlds.end()) + { + auto mIt = this->models.find(_parentID); + std::size_t nestedModelID = mIt->second->nestedModelNameToEntityId.at( + _sdfModel.Name()); + return this->GenerateIdentity(nestedModelID, + this->models.at(nestedModelID)); + } - const auto &structure = *validation; - const bool isStatic = _sdfModel.Static(); + auto structures = validateModel(_sdfModel); + if (structures.empty()) + return this->GenerateInvalidId(); - const auto *world = this->ReferenceInterface(_worldID); + if (structures.size() > 1) + { + // multiple sub-trees detected in model + // \todo(iche033) support multiple kinematic trees and + // multiple floating links in a single model + // https://github.com/gazebosim/gz-physics/issues/550 + gzerr << "Multiple sub-trees / floating links detected in a model. " + << "This is not supported in bullet-featherstone implementation yet." + << std::endl; + } + // Create model for the first structure. + auto &structure = structures[0]; + const bool isStatic = _sdfModel.Static(); + WorldInfo *world = nullptr; const auto rootInertialToLink = gz::math::eigen3::convert(structure.linkToPrincipalAxesPose).inverse(); - const auto modelID = this->AddModel( - _sdfModel.Name(), _worldID, rootInertialToLink, - std::make_unique( - static_cast(structure.flatLinks.size()), - structure.mass, - structure.inertia, - structure.fixedBase || isStatic, - true)); + // A map of link sdf to parent model identity + std::unordered_map linkParentModelIds; + + std::unordered_map modelIDs; + std::size_t rootModelID = 0u; + std::shared_ptr rootMultiBody; + // Add all models, including nested models + auto addModels = [&](std::size_t _modelOrWorldID, const ::sdf::Model *_model, + auto &&_addModels) + { + if (!_model) + return false; + + auto worldIt = this->worlds.find(_modelOrWorldID); + const bool isNested = worldIt == this->worlds.end(); + auto modelID = [&] { + if (isNested) + { + auto modelIt = this->models.find(_modelOrWorldID); + auto worldIdentity = modelIt->second->world; + auto modelIdentity = + this->GenerateIdentity(_modelOrWorldID, modelIt->second); + return this->AddNestedModel( + _model->Name(), modelIdentity, worldIdentity, rootInertialToLink, + rootMultiBody); + } + else + { + auto worldIdentity = this->GenerateIdentity( + _modelOrWorldID, worldIt->second); + rootMultiBody = std::make_shared( + static_cast(structure.flatLinks.size()), + structure.mass, + structure.inertia, + structure.fixedBase || isStatic, + true); + world = this->ReferenceInterface(worldIdentity); + auto id = this->AddModel( + _model->Name(), worldIdentity, rootInertialToLink, + rootMultiBody); + rootModelID = id; + return id; + } + }(); + + // build link to model map for use later when adding links + for (std::size_t i = 0; i < _model->LinkCount(); ++i) + { + const ::sdf::Link *link = _model->LinkByIndex(i); + linkParentModelIds[link] = modelID; + } + modelIDs.insert(std::make_pair(_model, modelID)); + + // recursively add nested models + for (std::size_t i = 0; i < _model->ModelCount(); ++i) + { + if (!_addModels(modelID, _model->ModelByIndex(i), _addModels)) + return false; + } + return true; + }; + if (!addModels(_parentID, &_sdfModel, addModels)) + { + return this->GenerateInvalidId(); + } + + // Add base link + std::size_t baseLinkModelID = linkParentModelIds.at(structure.rootLink); const auto rootID = this->AddLink(LinkInfo{ - structure.rootLink->Name(), std::nullopt, modelID, rootInertialToLink + structure.rootLink->Name(), std::nullopt, + this->GenerateIdentity(baseLinkModelID, this->models.at(baseLinkModelID)), + rootInertialToLink }); + rootMultiBody->setUserIndex(std::size_t(rootID)); + + auto modelID = + this->GenerateIdentity(rootModelID, this->models[rootModelID]); const auto *model = this->ReferenceInterface(modelID); + // Add world joint if (structure.rootJoint) { + const auto &parentInfo = structure.parentOf.at(structure.rootLink); this->AddJoint( JointInfo{ structure.rootJoint->Name(), @@ -382,7 +579,7 @@ Identity SDFFeatures::ConstructSdfModel( rootID, Eigen::Isometry3d::Identity(), Eigen::Isometry3d::Identity(), - modelID + modelIDs.find(parentInfo.model)->second }); } @@ -392,6 +589,7 @@ Identity SDFFeatures::ConstructSdfModel( std::unordered_map linkIDs; linkIDs.insert(std::make_pair(structure.rootLink, rootID)); + // Add all links and joints for (int i = 0; i < static_cast(structure.flatLinks.size()); ++i) { const auto *link = structure.flatLinks[static_cast(i)]; @@ -404,17 +602,26 @@ Identity SDFFeatures::ConstructSdfModel( if (linkIDs.find(link) == linkIDs.end()) { + std::size_t parentModelID = linkParentModelIds[link]; const auto linkID = this->AddLink( - LinkInfo{link->Name(), i, modelID, linkToComTf.inverse()}); + LinkInfo{link->Name(), i, + this->GenerateIdentity(parentModelID, this->models.at(parentModelID)), + linkToComTf.inverse()}); linkIDs.insert(std::make_pair(link, linkID)); } - if (structure.parentOf.size()) + if (!structure.parentOf.empty()) { const auto &parentInfo = structure.parentOf.at(link); const auto *joint = parentInfo.joint; + std::string parentLinkName; + ::sdf::Errors errors = joint->ResolveParentLink(parentLinkName); + if (!errors.empty()) + { + parentLinkName = joint->ParentName(); + } const auto &parentLinkID = linkIDs.at( - parentInfo.model->LinkByName(joint->ParentName())); + parentInfo.model->LinkByName(parentLinkName)); const auto *parentLinkInfo = this->ReferenceInterface( parentLinkID); @@ -426,12 +633,14 @@ Identity SDFFeatures::ConstructSdfModel( Eigen::Isometry3d poseParentComToJoint; { gz::math::Pose3d gzPoseParentToJoint; - const auto errors = joint->SemanticPose().Resolve( - gzPoseParentToJoint, joint->ParentName()); + errors = resolveJointPoseRelToLink(gzPoseParentToJoint, + parentInfo.model, joint->Name(), parentLinkName); + if (!errors.empty()) { gzerr << "An error occurred while resolving the transform of Joint [" - << joint->Name() << "] in Model [" << model->name << "]:\n"; + << joint->Name() << "] in Model [" << parentInfo.model->Name() + << "]:\n"; for (const auto &error : errors) { gzerr << error << "\n"; @@ -440,6 +649,7 @@ Identity SDFFeatures::ConstructSdfModel( return this->GenerateInvalidId(); } + poseParentLinkToJoint = gz::math::eigen3::convert(gzPoseParentToJoint); poseParentComToJoint = poseParentLinkToJoint * parentLinkInfo->inertiaToLinkFrame; @@ -447,9 +657,17 @@ Identity SDFFeatures::ConstructSdfModel( Eigen::Isometry3d poseJointToChild; { - gz::math::Pose3d gzPoseJointToChild; - const auto errors = - link->SemanticPose().Resolve(gzPoseJointToChild, joint->Name()); + gz::math::Pose3d gzPoseChildToJoint; + // this retrieves the joint pose relative to link + std::string childLinkName; + errors = joint->ResolveChildLink(childLinkName); + if (!errors.empty()) + { + childLinkName = joint->ChildName(); + } + errors = resolveJointPoseRelToLink(gzPoseChildToJoint, + parentInfo.model, joint->Name(), childLinkName); + if (!errors.empty()) { gzerr << "An error occured while resolving the transform of Link [" @@ -461,7 +679,8 @@ Identity SDFFeatures::ConstructSdfModel( return this->GenerateInvalidId(); } - + gz::math::Pose3d gzPoseJointToChild; + gzPoseJointToChild = gzPoseChildToJoint.Inverse(); poseJointToChild = gz::math::eigen3::convert(gzPoseJointToChild); } @@ -469,45 +688,36 @@ Identity SDFFeatures::ConstructSdfModel( convertMat(poseParentComToJoint.linear()) .getRotation(btRotParentComToJoint); - auto linkParent = _sdfModel.LinkByName(joint->ParentName()); - gz::math::Pose3d parentTransformInWorldSpace; - const auto errors = linkParent->SemanticPose().Resolve( - parentTransformInWorldSpace); - - gz::math::Pose3d parent2joint; - const auto errors2 = linkParent->SemanticPose().Resolve( - parent2joint, joint->Name()); // X_JP - - btTransform parentLocalInertialFrame = convertTf( - parentLinkInfo->inertiaToLinkFrame); - btTransform parent2jointBt = convertTf( - gz::math::eigen3::convert(parent2joint.Inverse())); // X_PJ - - // offsetInABt = parent COM to pivot (X_IpJ) - // offsetInBBt = current COM to pivot (X_IcJ) - // - // X_PIp - // X_PJ - // X_IpJ = X_PIp^-1 * X_PJ - // - // X_IcJ = X_CIc^-1 * X_CJ - btTransform offsetInABt, offsetInBBt; - offsetInABt = parentLocalInertialFrame * parent2jointBt; - offsetInBBt = - convertTf(linkToComTf.inverse() * poseJointToChild.inverse()); - // R_IcJ * R_IpJ ^ -1 = R_IcIp; - btQuaternion parentRotToThis = - offsetInBBt.getRotation() * offsetInABt.inverse().getRotation(); + btTransform parentLocalInertialFrame = convertTf( + parentLinkInfo->inertiaToLinkFrame); + btTransform parent2jointBt = convertTf( + poseParentLinkToJoint); // X_PJ + + // offsetInABt = parent COM to pivot (X_IpJ) + // offsetInBBt = current COM to pivot (X_IcJ) + // + // X_PIp + // X_PJ + // X_IpJ = X_PIp^-1 * X_PJ + // + // X_IcJ = X_CIc^-1 * X_CJ + btTransform offsetInABt, offsetInBBt; + offsetInABt = parentLocalInertialFrame * parent2jointBt; + offsetInBBt = + convertTf(linkToComTf.inverse() * poseJointToChild.inverse()); + // R_IcJ * R_IpJ ^ -1 = R_IcIp; + btQuaternion parentRotToThis = + offsetInBBt.getRotation() * offsetInABt.inverse().getRotation(); auto jointID = this->AddJoint( JointInfo{ joint->Name(), InternalJoint{i}, - model->linkEntityIds[static_cast(parentIndex+1)], + linkIDs.find(parentInfo.parent)->second, linkIDs.find(link)->second, poseParentLinkToJoint, poseJointToChild, - modelID + modelIDs.find(parentInfo.model)->second }); auto jointInfo = this->ReferenceInterface(jointID); @@ -586,7 +796,6 @@ Identity SDFFeatures::ConstructSdfModel( } } - model->body->setHasSelfCollision(_sdfModel.SelfCollide()); model->body->finalizeMultiDof(); @@ -594,13 +803,56 @@ Identity SDFFeatures::ConstructSdfModel( if (!worldToModel) return this->GenerateInvalidId(); + auto modelToNestedModel = Eigen::Isometry3d::Identity(); + // if the model of the root link is nested in a top level model, compute + // its pose relative to top level model + if (structure.model != &_sdfModel) + { + // get the scoped name of the model + // This is used to resolve the model pose + std::string modelScopedName; + auto getModelScopedName = [&](const ::sdf::Model *_targetModel, + const ::sdf::Model *_parentModel, const std::string &_prefix, + auto &&_getModelScopedName) + { + for (std::size_t i = 0u; i < _parentModel->ModelCount(); ++i) + { + auto childModel = _parentModel->ModelByIndex(i); + if (childModel == _targetModel) + { + modelScopedName = _prefix + childModel->Name(); + return true; + } + else + { + if (_getModelScopedName(_targetModel, childModel, + _prefix + childModel->Name() + "::", _getModelScopedName)) + return true; + } + } + return false; + }; + + math::Pose3d modelPose; + if (!getModelScopedName(structure.model, &_sdfModel, + _sdfModel.Name() + "::", getModelScopedName)) + return this->GenerateInvalidId(); + + auto errors = _sdfModel.SemanticPose().Resolve(modelPose, + modelScopedName); + if (!errors.empty()) + return this->GenerateInvalidId(); + modelToNestedModel = math::eigen3::convert(modelPose).inverse(); + } + const auto modelToRootLink = ResolveSdfPose(structure.rootLink->SemanticPose()); if (!modelToRootLink) return this->GenerateInvalidId(); const auto worldToRootCom = - *worldToModel * *modelToRootLink * rootInertialToLink.inverse(); + *worldToModel * modelToNestedModel * *modelToRootLink * + rootInertialToLink.inverse(); model->body->setBaseWorldTransform(convertTf(worldToRootCom)); model->body->setBaseVel(btVector3(0, 0, 0)); @@ -629,9 +881,58 @@ Identity SDFFeatures::ConstructSdfModel( } } + // Add the remaining links in the model without constructing the bullet + // objects. These are dummy links for book-keeping purposes + // \todo(iche033) The code will need to be updated when multiple subtrees in + // a single model is supported. + for (std::size_t i = 1u; i < structures.size(); ++i) + { + auto otherStructure = structures[i]; + // Add base link + std::size_t rootLinkModelID = linkParentModelIds[otherStructure.rootLink]; + auto rootLinkModelInfo = this->models.at(rootLinkModelID); + this->AddLink(LinkInfo{ + otherStructure.rootLink->Name(), + std::nullopt, + this->GenerateIdentity(rootLinkModelID, rootLinkModelInfo), + gz::math::eigen3::convert( + otherStructure.linkToPrincipalAxesPose).inverse() + }); + gzwarn << "Floating body / sub-tree detected. Disabling link: '" + << otherStructure.rootLink->Name() << "' " + << "from model '" << rootLinkModelInfo->name << "'." << std::endl; + // other links + for (int j = 0; j < static_cast(otherStructure.flatLinks.size()); ++j) + { + const auto *link = otherStructure.flatLinks[static_cast(j)]; + std::size_t parentModelID = linkParentModelIds[link]; + btScalar mass; + btVector3 inertia; + math::Pose3d linkToPrincipalAxesPose; + extractInertial(link->Inertial(), mass, inertia, linkToPrincipalAxesPose); + auto parentModelInfo = this->models.at(parentModelID); + const auto linkID = this->AddLink( + LinkInfo{link->Name(), std::nullopt, + this->GenerateIdentity(parentModelID, parentModelInfo), + gz::math::eigen3::convert(linkToPrincipalAxesPose).inverse()}); + gzwarn << "Floating body / sub-tree detected. Disabling link: '" + << link->Name() << "' " << "from model '" << parentModelInfo->name + << "'." << std::endl; + + } + } + return modelID; } +///////////////////////////////////////////////// +Identity SDFFeatures::ConstructSdfModel( + const Identity &_worldID, + const ::sdf::Model &_sdfModel) +{ + return this->ConstructSdfModelImpl(_worldID, _sdfModel); +} + ///////////////////////////////////////////////// bool SDFFeatures::AddSdfCollision( const Identity &_linkID, @@ -958,6 +1259,88 @@ Identity SDFFeatures::ConstructSdfCollision( return this->GenerateInvalidId(); } +///////////////////////////////////////////////// +Identity SDFFeatures::ConstructSdfJoint( + const Identity &_modelID, + const ::sdf::Joint &_sdfJoint) +{ +#if BT_BULLET_VERSION < 289 + // The btMultiBody::setFixedBase function is only available in versions + // >= 2.89. This is needed for dynamically creating world joints, + // i.e. setting the btMultiBody to be fixed. So output an error letting + // users know the joint will not be created. + // \todo(iche033) A workaround for this is to loop through all the joints + // in the world first in ConstructSdfWorld, keep track of the models who are + // a child of the world joint, then when creating the btMultiBody + // in ConstructSdfModelImpl, pass fixedBase as true in its constructor. + (void)_modelID; + gzerr << "ConstructSdfJoint feature is not supported for bullet version " + << "less than 2.89. Joint '" << _sdfJoint.Name() << "' will not " + << "be created." << std::endl; +#else + auto modelInfo = this->ReferenceInterface(_modelID); + if (_sdfJoint.ChildName() == "world") + { + gzerr << "Asked to create a joint with the world as the child in model " + << "[" << modelInfo->name << "]. This is currently not " + << "supported\n"; + + return this->GenerateInvalidId(); + } + + std::string parentLinkName; + const auto resolveParentErrors = _sdfJoint.ResolveParentLink(parentLinkName); + if (!resolveParentErrors.empty()) { + parentLinkName = _sdfJoint.ParentName(); + } + std::string childLinkName; + const auto childResolveErrors = _sdfJoint.ResolveChildLink(childLinkName); + if (!childResolveErrors.empty()) { + childLinkName = _sdfJoint.ChildName(); + } + + // Currently only supports constructing fixed joint with world as parent + if (parentLinkName == "world" && _sdfJoint.Type() == ::sdf::JointType::FIXED) + { + auto worldModelIt = this->models.find(_modelID); + if (worldModelIt == this->models.end()) + return this->GenerateInvalidId(); + const auto worldModel = worldModelIt->second; + std::size_t idx = childLinkName.find("::"); + if (idx == std::string::npos) + return this->GenerateInvalidId(); + + const std::string modelName = childLinkName.substr(0, idx); + std::size_t modelID = worldModel->nestedModelNameToEntityId.at(modelName); + auto model = this->models.at(modelID); + + model->body->setFixedBase(true); + std::size_t linkID = model->body->getUserIndex(); + auto rootID = this->GenerateIdentity(linkID, this->links.at(linkID)); + return this->AddJoint( + JointInfo{ + _sdfJoint.Name(), + RootJoint{}, + std::nullopt, + rootID, + Eigen::Isometry3d::Identity(), + Eigen::Isometry3d::Identity(), + _modelID + }); + } + + // \todo(iche033) Support fixed joint between 2 different models + gzerr << "Unable to create joint between parent: " << parentLinkName << " " + << "and child: " << childLinkName << ". " + << "ConstructSdfJoint in bullet-featherstone implementation currently " + << "only supports creating a fixed joint with the world as parent link." + << std::endl; +#endif + + return this->GenerateInvalidId(); +} + + } // namespace bullet_featherstone } // namespace physics } // namespace gz diff --git a/bullet-featherstone/src/SDFFeatures.hh b/bullet-featherstone/src/SDFFeatures.hh index 2285c1a4b..bea5faa70 100644 --- a/bullet-featherstone/src/SDFFeatures.hh +++ b/bullet-featherstone/src/SDFFeatures.hh @@ -21,7 +21,9 @@ #include #include +#include #include +#include #include #include @@ -34,7 +36,9 @@ namespace physics { namespace bullet_featherstone { struct SDFFeatureList : gz::physics::FeatureList< + sdf::ConstructSdfJoint, sdf::ConstructSdfModel, + sdf::ConstructSdfNestedModel, sdf::ConstructSdfWorld, sdf::ConstructSdfCollision > { }; @@ -51,14 +55,29 @@ class SDFFeatures : const Identity &_worldID, const ::sdf::Model &_sdfModel) override; + public: Identity ConstructSdfNestedModel( + const Identity &_parentID, + const ::sdf::Model &_sdfModel) override; + public: bool AddSdfCollision( const Identity &_linkID, const ::sdf::Collision &_collision, bool isStatic); + public: Identity ConstructSdfJoint( + const Identity &_modelID, + const ::sdf::Joint &_sdfJoint) override; + private: Identity ConstructSdfCollision( const Identity &_linkID, const ::sdf::Collision &_collision) override; + + /// \brief Construct a bullet entity from a sdf::Model + /// \param[in] _parentID Parent identity + /// \param[in] _sdfModel sdf::Model to construct entity from + /// \return The entity identity if constructed otherwise an invalid identity + private: Identity ConstructSdfModelImpl(std::size_t _parentID, + const ::sdf::Model &_sdfModel); }; } // namespace bullet_featherstone diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index a35e2127a..de887560b 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -65,6 +65,11 @@ foreach(test ${tests}) target_include_directories(${test_executable} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + if (bullet_version_check_VERSION VERSION_GREATER_EQUAL 2.89) + target_compile_definitions(${test_executable} PRIVATE + "BT_BULLET_VERSION_GE_289" + ) + endif() if (bullet_version_check_VERSION VERSION_LESS_EQUAL 3.25) target_compile_definitions(${test_executable} PRIVATE "BT_BULLET_VERSION_LE_325" diff --git a/test/common_test/Worlds.hh b/test/common_test/Worlds.hh index d57aa32d3..6ad145f8f 100644 --- a/test/common_test/Worlds.hh +++ b/test/common_test/Worlds.hh @@ -48,6 +48,8 @@ const auto kStringPendulumSdf = CommonTestWorld("string_pendulum.sdf"); const auto kTestWorld = CommonTestWorld("test.world"); const auto kWorldJointTestSdf = CommonTestWorld("world_joint_test.sdf"); const auto kWorldUnsortedLinksSdf = CommonTestWorld("world_unsorted_links.sdf"); +const auto kWorldSingleNestedModelSdf = + CommonTestWorld("world_single_nested_model.sdf"); const auto kWorldWithNestedModelSdf = CommonTestWorld("world_with_nested_model.sdf"); } // namespace common_test::worlds diff --git a/test/common_test/free_joint_features.cc b/test/common_test/free_joint_features.cc index d7bd4903c..2cea27766 100644 --- a/test/common_test/free_joint_features.cc +++ b/test/common_test/free_joint_features.cc @@ -105,6 +105,11 @@ TEST_F(FreeGroupFeaturesTest, NestedFreeGroup) { for (const std::string &name : pluginNames) { + // https://github.com/gazebosim/gz-physics/issues/550 + // bullet-feathersone does not support multiple kinematic trees in + // a model so nested_model2 and nested_model3 are not constructed. + CHECK_UNSUPPORTED_ENGINE(name, "bullet-featherstone") + std::cout << "Testing plugin: " << name << std::endl; gz::plugin::PluginPtr plugin = loader.Instantiate(name); @@ -148,6 +153,7 @@ TEST_F(FreeGroupFeaturesTest, NestedFreeGroup) // Expect false because the link in nested_model is referenced by a joint. EXPECT_FALSE(checkFreeGroupForModel("parent_model::nested_model")); } + EXPECT_TRUE(checkFreeGroupForModel("parent_model::nested_model2")); EXPECT_TRUE(checkFreeGroupForModel("parent_model::nested_model3")); } @@ -157,6 +163,11 @@ TEST_F(FreeGroupFeaturesTest, NestedFreeGroupSetWorldPose) { for (const std::string &name : pluginNames) { + // https://github.com/gazebosim/gz-physics/issues/550 + // bullet-feathersone does not support multiple kinematic trees in + // a model so nested_model2 and nested_model3 are not constructed. + CHECK_UNSUPPORTED_ENGINE(name, "bullet-featherstone") + std::cout << "Testing plugin: " << name << std::endl; gz::plugin::PluginPtr plugin = loader.Instantiate(name); diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 5f0a695b6..2e0d5b47d 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1526,6 +1526,9 @@ TEST_F(WorldModelTest, JointSetCommand) { for (const std::string &name : this->pluginNames) { + // bullet-feathersone does not support joints between models yet + CHECK_UNSUPPORTED_ENGINE(name, "bullet-featherstone") + auto world = this->LoadWorld(name); ASSERT_NE(nullptr, world); diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc index ad28f8bbb..e1e68c0f0 100644 --- a/test/common_test/world_features.cc +++ b/test/common_test/world_features.cc @@ -357,6 +357,106 @@ TEST_F(WorldModelTest, WorldModelAPI) } } +struct WorldNestedModelFeatureList : gz::physics::FeatureList< + GravityFeatures, + gz::physics::ForwardStep, + gz::physics::GetNestedModelFromModel, + gz::physics::sdf::ConstructSdfJoint, + gz::physics::sdf::ConstructSdfModel, + gz::physics::sdf::ConstructSdfNestedModel, + gz::physics::RemoveEntities, + gz::physics::WorldModelFeature +> { }; + + +class WorldNestedModelTest : public WorldFeaturesTest +{ + public: gz::physics::World3dPtr LoadWorld( + const std::string &_pluginName) + { + gz::plugin::PluginPtr plugin = this->loader.Instantiate(_pluginName); + + auto engine = + gz::physics::RequestEngine3d::From(plugin); + + sdf::Root root; + const sdf::Errors errors = root.Load( + common_test::worlds::kWorldSingleNestedModelSdf); + EXPECT_TRUE(errors.empty()) << errors; + if (errors.empty()) + { + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + return world; + } + return nullptr; + } +}; + +TEST_F(WorldNestedModelTest, WorldConstructNestedModel) +{ + for (const std::string &name : this->pluginNames) + { + auto world = this->LoadWorld(name); + ASSERT_NE(nullptr, world); + + auto worldModel = world->GetWorldModel(); + ASSERT_NE(nullptr, worldModel); + EXPECT_EQ(world, worldModel->GetWorld()); + EXPECT_EQ("nested_model_world", worldModel->GetName()); + EXPECT_EQ(0, worldModel->GetLinkCount()); + EXPECT_EQ(0, worldModel->GetIndex()); + EXPECT_EQ(1u, world->GetModelCount()); + EXPECT_EQ(world->GetModelCount(), worldModel->GetNestedModelCount()); + const auto nestedModel = worldModel->GetNestedModel(0); + ASSERT_NE(nullptr, nestedModel); + EXPECT_EQ("parent_model", nestedModel->GetName()); + + // Test joint creation + sdf::Joint joint; + joint.SetName("world_joint"); + joint.SetType(sdf::JointType::FIXED); + joint.SetParentName("world"); + joint.SetChildName("invalid_link"); + EXPECT_FALSE(worldModel->ConstructJoint(joint)); + joint.SetChildName("parent_model::link1"); + if (PhysicsEngineName(name) != "bullet-featherstone") + { + EXPECT_TRUE(worldModel->ConstructJoint(joint)); + } + else + { +#ifdef BT_BULLET_VERSION_GE_289 + EXPECT_TRUE(worldModel->ConstructJoint(joint)); +#endif + } + + gz::physics::ForwardStep::Input input; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Output output; + + // Check invalid input to RemoveNestedModel method + EXPECT_FALSE(worldModel->RemoveNestedModel(1)); + EXPECT_FALSE(worldModel->RemoveNestedModel("invalid")); + + // Check that we can remove models via RemoveNestedModel + EXPECT_TRUE(worldModel->RemoveNestedModel(0)); + EXPECT_TRUE(nestedModel->Removed()); + EXPECT_EQ(0u, world->GetModelCount()); + EXPECT_EQ(0u, worldModel->GetNestedModelCount()); + EXPECT_EQ(nullptr, worldModel->GetNestedModel(0)); + EXPECT_EQ(nullptr, worldModel->GetNestedModel("parent_model")); + + // verify we can step the world after model removal + const size_t numSteps = 1000; + for (size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + } + EXPECT_EQ(0u, world->GetModelCount()); + EXPECT_EQ(0u, worldModel->GetNestedModelCount()); + } +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/test/common_test/worlds/world_single_nested_model.sdf b/test/common_test/worlds/world_single_nested_model.sdf new file mode 100644 index 000000000..14f724f2b --- /dev/null +++ b/test/common_test/worlds/world_single_nested_model.sdf @@ -0,0 +1,61 @@ + + + + + + 1 2 2 0 0 0 + + 3 1 1 0 0 1.5707 + + + + 2 + + + + + + 0 1 0 1.5707 0 0 + + + + 2 + + + + + + nested_link1 + nested_link2 + + 1 0 0 + + + + + + + + 1 2 3 + + + + + + + 1 2 3 + + + + + + + link1 + nested_model::nested_link1 + + 0 0 1 + + + + + From e42cf3456f0f95c8a89a56d4294ac13705681c52 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 6 Mar 2024 17:59:03 -0800 Subject: [PATCH 05/18] bullet-featherstone: Improve mesh collision stability (#600) * Tune erp2 and mesh margin * increase tol in joint features test Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.cc | 7 +++ bullet-featherstone/src/SDFFeatures.cc | 2 +- test/common_test/joint_features.cc | 62 ++++++++++++++++---------- 3 files changed, 46 insertions(+), 25 deletions(-) diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index 13dd6c8fa..d157e33d8 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -42,6 +42,13 @@ WorldInfo::WorldInfo(std::string name_) // Needed for force-torque sensor this->world->getSolverInfo().m_jointFeedbackInJointFrame = true; this->world->getSolverInfo().m_jointFeedbackInWorldSpace = false; + + // By default a large impulse is applied when collisions penetrate + // which causes unstable behavior. Bullet featherstone does not support + // configuring split impulse and penetration threshold parameters. Instead the + // penentration impulse depends on the erp2 parameter so set to a small value + // (default is 0.2). + this->world->getSolverInfo().m_erp2 = 0.002; } } // namespace bullet_featherstone diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 0bbdfc11e..cd5d0d6dd 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -1076,7 +1076,7 @@ bool SDFFeatures::AddSdfCollision( std::make_unique( this->triangleMeshes.back().get())); this->meshesGImpact.back()->updateBound(); - this->meshesGImpact.back()->setMargin(btScalar(0.001)); + this->meshesGImpact.back()->setMargin(btScalar(0.01)); compoundShape->addChildShape(btTransform::getIdentity(), this->meshesGImpact.back().get()); } diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 2e0d5b47d..346821bb6 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1132,13 +1132,22 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetach) } // After a while, body2 should reach the ground and come to a stop - for (std::size_t i = 0; i < 1000; ++i) + std::size_t stepCount = 0u; + const std::size_t maxNumSteps = 1000u; + while (stepCount++ < maxNumSteps) { world->Step(output, state, input); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + // Expected Z height of model2 is 0.75 when both boxes are stacked on top + // of each other since each is 0.5 high. + if (fabs(frameDataModel2Body.pose.translation().z() - 0.75) < 2e-2 && + fabs(frameDataModel2Body.linearVelocity.z()) < 1e-3) + { + break; + } } - frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); - - EXPECT_NEAR(0.0, frameDataModel2Body.linearVelocity.z(), 1e-3); + EXPECT_GT(stepCount, 1u); + EXPECT_LT(stepCount, maxNumSteps); } } @@ -1246,8 +1255,8 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) EXPECT_EQ(initialModel3Pose, gz::math::eigen3::convert(frameDataModel3Body.pose)); + // Step through initial transients const std::size_t numSteps = 100; - /// Step through initial transients for (std::size_t i = 0; i < numSteps; ++i) { world->Step(output, state, input); @@ -1260,15 +1269,18 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) // Expect all the bodies to be at rest. // (since they're held in place by the joints) - gz::math::Vector3d body1LinearVelocity = - gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); - gz::math::Vector3d body2LinearVelocity = - gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); - gz::math::Vector3d body3LinearVelocity = - gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); - EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-3); - EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); - EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 1e-3); + { + world->Step(output, state, input); + EXPECT_NEAR(initialModel1Pose.Z(), + frameDataModel1Body.pose.translation().z(), 1e-3); + EXPECT_NEAR(initialModel2Pose.Z(), + frameDataModel2Body.pose.translation().z(), 1e-3); + EXPECT_NEAR(initialModel3Pose.Z(), + frameDataModel3Body.pose.translation().z(), 1e-3); + EXPECT_NEAR(0.0, frameDataModel1Body.linearVelocity.z(), 3e-3); + EXPECT_NEAR(0.0, frameDataModel2Body.linearVelocity.z(), 1e-3); + EXPECT_NEAR(0.0, frameDataModel3Body.linearVelocity.z(), 3e-3); + } } // Detach the joints. M1 and M3 should fall as there is now nothing stopping @@ -1276,6 +1288,13 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) fixedJoint_m2m1->Detach(); fixedJoint_m2m3->Detach(); + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + const double preStepBody1LinearVelocityZ = + frameDataModel1Body.linearVelocity.z(); + const double preStepBody3LinearVelocityZ = + frameDataModel3Body.linearVelocity.z(); + /// Step through initial transients for (std::size_t i = 0; i < numSteps; ++i) { @@ -1289,16 +1308,11 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) // Expect the middle box to be still as it is already at rest. // Expect the two side boxes to fall away. - gz::math::Vector3d body1LinearVelocity = - gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); - gz::math::Vector3d body2LinearVelocity = - gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); - gz::math::Vector3d body3LinearVelocity = - gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); - - EXPECT_NEAR(dt * (numSteps) * -10, body1LinearVelocity.Z(), 1e-3); - EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); - EXPECT_NEAR(dt * (numSteps) * -10, body3LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(preStepBody1LinearVelocityZ + dt * (numSteps) * -10, + frameDataModel1Body.linearVelocity.z(), 1e-3); + EXPECT_NEAR(0.0, frameDataModel2Body.linearVelocity.z(), 1e-3); + EXPECT_NEAR(preStepBody3LinearVelocityZ + dt * (numSteps) * -10, + frameDataModel3Body.linearVelocity.z(), 1e-3); } } } From 5076d6b29ed7ce8d7ca2a1a432feaacab65a439c Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 14 Mar 2024 13:10:23 -0500 Subject: [PATCH 06/18] Remove @mxgrey as code owner (#605) Signed-off-by: Addisu Z. Taddese --- .github/CODEOWNERS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 5534c1d87..fa88665d8 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,4 +1,4 @@ # More info: # https://help.github.com/en/github/creating-cloning-and-archiving-repositories/about-code-owners -* @azeey @mxgrey @scpeters +* @azeey @scpeters From 13bf7f50ce0c66d942884b502294014360190e82 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 8 Apr 2024 18:14:24 -0700 Subject: [PATCH 07/18] Disable test failing due to ODE/libccd (#621) Part of #620. Signed-off-by: Steve Peters --- test/common_test/joint_features.cc | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 346821bb6..68d0dbc76 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -966,9 +966,23 @@ TYPED_TEST(JointFeaturesDetachTest, JointDetach) // sanity check on velocity values EXPECT_LT(1e-5, upperLinkLinearVelocity.Z()); EXPECT_GT(-0.03, upperLinkAngularVelocity.X()); - EXPECT_NEAR(0.0, upperLinkLinearVelocity.X(), 1e-6); +#ifdef __APPLE__ + // Disable some expectations for dartsim plugin on homebrew, + // see https://github.com/gazebosim/gz-physics/issues/620. + if (this->PhysicsEngineName(name) != "dartsim") +#endif + { + EXPECT_NEAR(0.0, upperLinkLinearVelocity.X(), 1e-6); + } EXPECT_NEAR(0.0, upperLinkLinearVelocity.Y(), 1e-6); - EXPECT_NEAR(0.0, upperLinkAngularVelocity.Y(), 1e-6); +#ifdef __APPLE__ + // Disable some expectations for dartsim plugin on homebrew, + // see https://github.com/gazebosim/gz-physics/issues/620. + if (this->PhysicsEngineName(name) != "dartsim") +#endif + { + EXPECT_NEAR(0.0, upperLinkAngularVelocity.Y(), 1e-6); + } EXPECT_NEAR(0.0, upperLinkAngularVelocity.Z(), 1e-6); upperJoint->Detach(); From d0f3867ffc99c666f3bc4a405bb14abf0e703cf6 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sat, 20 Apr 2024 10:08:09 -0700 Subject: [PATCH 08/18] Fix windows compiler warning (#629) Match comment style in harmonic. Signed-off-by: Steve Peters --- bullet-featherstone/src/Base.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index d157e33d8..7c230a31d 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -45,10 +45,10 @@ WorldInfo::WorldInfo(std::string name_) // By default a large impulse is applied when collisions penetrate // which causes unstable behavior. Bullet featherstone does not support - // configuring split impulse and penetration threshold parameters. Instead the - // penentration impulse depends on the erp2 parameter so set to a small value - // (default is 0.2). - this->world->getSolverInfo().m_erp2 = 0.002; + // configuring split impulse and penetration threshold parameters. Instead + // the penentration impulse depends on the erp2 parameter so set to a small + // value (default in bullet is 0.2). + this->world->getSolverInfo().m_erp2 = btScalar(0.002); } } // namespace bullet_featherstone From 966265bf9292abc0422063fe8d2d3db15fcdf498 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Tue, 21 May 2024 13:56:15 -0300 Subject: [PATCH 09/18] Disable test failing due to ODE/libccd (backport) (#642) Signed-off-by: Jorge Perez --- dartsim/src/JointFeatures_TEST.cc | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/dartsim/src/JointFeatures_TEST.cc b/dartsim/src/JointFeatures_TEST.cc index 2bd173c5d..d2d0a9c76 100644 --- a/dartsim/src/JointFeatures_TEST.cc +++ b/dartsim/src/JointFeatures_TEST.cc @@ -734,9 +734,17 @@ TEST_F(JointFeaturesFixture, JointDetach) // sanity check on velocity values EXPECT_LT(1e-5, upperLinkLinearVelocity.Z()); EXPECT_GT(-0.03, upperLinkAngularVelocity.X()); +#ifndef __APPLE__ + // Disable some expectations for dartsim plugin on homebrew, + // see https://github.com/gazebosim/gz-physics/issues/620. EXPECT_NEAR(0.0, upperLinkLinearVelocity.X(), 1e-6); +#endif EXPECT_NEAR(0.0, upperLinkLinearVelocity.Y(), 1e-6); +#ifndef __APPLE__ + // Disable some expectations for dartsim plugin on homebrew, + // see https://github.com/gazebosim/gz-physics/issues/620. EXPECT_NEAR(0.0, upperLinkAngularVelocity.Y(), 1e-6); +#endif EXPECT_NEAR(0.0, upperLinkAngularVelocity.Z(), 1e-6); upperJoint->Detach(); From 9a2986643b87064ad8447bb57af4db65fe52a9d8 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Mon, 20 May 2024 13:48:01 -0300 Subject: [PATCH 10/18] Disable test failing due to ODE/libccd (backport #621) Signed-off-by: Jorge Perez --- dartsim/src/JointFeatures_TEST.cc | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/dartsim/src/JointFeatures_TEST.cc b/dartsim/src/JointFeatures_TEST.cc index 9e2d49f16..7a275b89a 100644 --- a/dartsim/src/JointFeatures_TEST.cc +++ b/dartsim/src/JointFeatures_TEST.cc @@ -743,9 +743,17 @@ TEST_F(JointFeaturesFixture, JointDetach) // sanity check on velocity values EXPECT_LT(1e-5, upperLinkLinearVelocity.Z()); EXPECT_GT(-0.03, upperLinkAngularVelocity.X()); +#ifndef __APPLE__ + // Disable some expectations for dartsim plugin on homebrew, + // see https://github.com/gazebosim/gz-physics/issues/620. EXPECT_NEAR(0.0, upperLinkLinearVelocity.X(), 1e-6); +#endif EXPECT_NEAR(0.0, upperLinkLinearVelocity.Y(), 1e-6); +#ifndef __APPLE__ + // Disable some expectations for dartsim plugin on homebrew, + // see https://github.com/gazebosim/gz-physics/issues/620. EXPECT_NEAR(0.0, upperLinkAngularVelocity.Y(), 1e-6); +#endif EXPECT_NEAR(0.0, upperLinkAngularVelocity.Z(), 1e-6); upperJoint->Detach(); From 78e87e42101adefc7eb922c6d83c78896d3d6ad7 Mon Sep 17 00:00:00 2001 From: AzulRadio <50132891+AzulRadio@users.noreply.github.com> Date: Fri, 7 Jun 2024 11:38:57 -0500 Subject: [PATCH 11/18] Add no gravity link support (#633) * add no gravity link support and test for DartSim Signed-off-by: youhy * remove debug code Signed-off-by: youhy * fix test Signed-off-by: Ian Chen --------- Signed-off-by: youhy Signed-off-by: Ian Chen Co-authored-by: Ian Chen --- dartsim/src/SDFFeatures.cc | 2 ++ dartsim/src/WorldFeatures_TEST.cc | 12 ++++++++++++ dartsim/worlds/falling.world | 12 ++++++++++++ 3 files changed, 26 insertions(+) diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 62137286e..27b8be036 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -600,6 +600,8 @@ Identity SDFFeatures::ConstructSdfLink( bodyProperties.mInertia.setLocalCOM(localCom); + bodyProperties.mGravityMode = _sdfLink.EnableGravity(); + dart::dynamics::FreeJoint::Properties jointProperties; jointProperties.mName = bodyProperties.mName + "_FreeJoint"; // TODO(MXG): Consider adding a UUID to this joint name in order to avoid any diff --git a/dartsim/src/WorldFeatures_TEST.cc b/dartsim/src/WorldFeatures_TEST.cc index 0126e2621..24a094865 100644 --- a/dartsim/src/WorldFeatures_TEST.cc +++ b/dartsim/src/WorldFeatures_TEST.cc @@ -140,6 +140,12 @@ TEST_F(WorldFeaturesFixture, Gravity) auto link = model->GetLink(0); ASSERT_NE(nullptr, link); + auto modelNoGravity = world->GetModel("sphere_no_gravity"); + ASSERT_NE(nullptr, modelNoGravity); + + auto linkNoGravity = modelNoGravity->GetLink(0); + ASSERT_NE(nullptr, linkNoGravity); + // initial link pose const Eigen::Vector3d initialLinkPosition(0, 0, 2); { @@ -190,6 +196,12 @@ TEST_F(WorldFeaturesFixture, Gravity) EXPECT_PRED_FORMAT2(vectorPredicate3, Eigen::Vector3d(0.5, 0, 2.5), pos); + // pose for link without gravity should not change + Eigen::Vector3d posNoGravity = linkNoGravity->FrameDataRelativeToWorld() + .pose.translation(); + EXPECT_PRED_FORMAT2(vectorPredicate3, + Eigen::Vector3d(10, 10, 10), + posNoGravity); } } diff --git a/dartsim/worlds/falling.world b/dartsim/worlds/falling.world index 3bae3f366..881daa5b9 100644 --- a/dartsim/worlds/falling.world +++ b/dartsim/worlds/falling.world @@ -71,5 +71,17 @@ + + + false + 10 10 10 0 0 0 + + 1 + + + 1 + + + From 914d841e5ef0cd98b043b34a27e11ab669518afd Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 11 Jun 2024 04:53:06 +0800 Subject: [PATCH 12/18] dartsim: optimize picking contact points with ODE collision detector (#584) * add test for selecting contact with max depth Signed-off-by: Ian Chen --- dartsim/src/GzOdeCollisionDetector.cc | 33 ++++++- test/common_test/Worlds.hh | 2 + test/common_test/simulation_features.cc | 94 ++++++++++++++++++- .../worlds/collision_pair_contact_point.sdf | 68 ++++++++++++++ 4 files changed, 191 insertions(+), 6 deletions(-) create mode 100644 test/common_test/worlds/collision_pair_contact_point.sdf diff --git a/dartsim/src/GzOdeCollisionDetector.cc b/dartsim/src/GzOdeCollisionDetector.cc index 228ee9562..6a81b4e06 100644 --- a/dartsim/src/GzOdeCollisionDetector.cc +++ b/dartsim/src/GzOdeCollisionDetector.cc @@ -17,6 +17,7 @@ #include #include +#include #include @@ -92,21 +93,45 @@ void GzOdeCollisionDetector::LimitCollisionPairMaxContacts( auto allContacts = _result->getContacts(); _result->clear(); + + if (this->maxCollisionPairContacts == 0u) + return; + + // A map of collision pairs and their contact info + // Contact info is stored in std::pair. The elements are: + // std::unordered_map> + std::unordered_map>> contactMap; for (auto &contact : allContacts) { - auto &count = - contactMap[contact.collisionObject1][contact.collisionObject2]; + auto &[count, lastContactIdx] = + contactMap[contact.collisionObject1][contact.collisionObject2]; count++; - auto &otherCount = + auto &[otherCount, otherLastContactIdx] = contactMap[contact.collisionObject2][contact.collisionObject1]; + std::size_t total = count + otherCount; if (total <= this->maxCollisionPairContacts) { + if (total == this->maxCollisionPairContacts) + { + lastContactIdx = _result->getNumContacts(); + otherLastContactIdx = lastContactIdx; + } _result->addContact(contact); } + else + { + // If too many contacts were generated, replace the last contact point + // of the collision pair with one that has a larger penetration depth + auto &c = _result->getContact(lastContactIdx); + if (contact.penetrationDepth > c.penetrationDepth) + { + c = contact; + } + } } } diff --git a/test/common_test/Worlds.hh b/test/common_test/Worlds.hh index 6ad145f8f..bf02f8730 100644 --- a/test/common_test/Worlds.hh +++ b/test/common_test/Worlds.hh @@ -29,6 +29,8 @@ inline std::string CommonTestWorld(const std::string &_world) namespace common_test::worlds { +const auto kCollisionPairContactPointSdf = + CommonTestWorld("collision_pair_contact_point.sdf"); const auto kContactSdf = CommonTestWorld("contact.sdf"); const auto kDetachableJointWorld = CommonTestWorld("detachable_joint.world"); const auto kEmptySdf = CommonTestWorld("empty.sdf"); diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index dfb657c1b..bc437b201 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -223,9 +223,13 @@ TYPED_TEST(SimulationFeaturesContactsTest, Contacts) // The features that an engine must have to be loaded by this loader. struct FeaturesCollisionPairMaxContacts : gz::physics::FeatureList< gz::physics::sdf::ConstructSdfWorld, - gz::physics::GetContactsFromLastStepFeature, + gz::physics::CollisionPairMaxContacts, + gz::physics::FindFreeGroupFeature, gz::physics::ForwardStep, - gz::physics::CollisionPairMaxContacts + gz::physics::FreeGroupFrameSemantics, + gz::physics::GetContactsFromLastStepFeature, + gz::physics::GetModelFromWorld, + gz::physics::SetFreeGroupWorldPose > {}; template @@ -276,6 +280,92 @@ TYPED_TEST(SimulationFeaturesCollisionPairMaxContactsTest, } } +///////////////////////////////////////////////// +TYPED_TEST(SimulationFeaturesCollisionPairMaxContactsTest, + CollisionPairMaxContactsSelection) +{ + for (const std::string &name : this->pluginNames) + { + auto world = LoadPluginAndWorld( + this->loader, + name, + common_test::worlds::kCollisionPairContactPointSdf); + auto checkedOutput = StepWorld( + world, true, 1).first; + EXPECT_TRUE(checkedOutput); + + // Verify initial pose + const gz::math::Pose3d initialPose = gz::math::Pose3d::Zero; + auto ellipsoid = world->GetModel("ellipsoid"); + ASSERT_NE(nullptr, ellipsoid); + auto ellipsoidFreeGroup = ellipsoid->FindFreeGroup(); + ASSERT_NE(nullptr, ellipsoidFreeGroup); + auto box = world->GetModel("box"); + ASSERT_NE(nullptr, box); + auto boxFreeGroup = box->FindFreeGroup(); + ASSERT_NE(nullptr, boxFreeGroup); + auto ellipsoidFrameData = ellipsoidFreeGroup->FrameDataRelativeToWorld(); + auto boxFrameData = boxFreeGroup->FrameDataRelativeToWorld(); + EXPECT_EQ(initialPose, + gz::math::eigen3::convert(ellipsoidFrameData.pose)); + EXPECT_EQ(initialPose, + gz::math::eigen3::convert(boxFrameData.pose)); + + // Get all contacts between box and ellipsoid + auto contacts = world->GetContactsFromLastStep(); + EXPECT_EQ(std::numeric_limits::max(), + world->GetCollisionPairMaxContacts()); + EXPECT_GT(contacts.size(), 30u); + + // Find contact point with max penetration depth + double maxDepth = 0; + for (const auto &contact : contacts) + { + const auto* extraContactData = + contact.template Query< + gz::physics::World3d< + FeaturesCollisionPairMaxContacts>::ExtraContactData>(); + ASSERT_NE(nullptr, extraContactData); + if (extraContactData->depth > maxDepth) + maxDepth = extraContactData->depth; + } + EXPECT_GT(maxDepth, 0.0); + + // Reset pose back to initial pose + ellipsoidFreeGroup->SetWorldPose( + gz::math::eigen3::convert(initialPose)); + boxFreeGroup->SetWorldPose( + gz::math::eigen3::convert(initialPose)); + ellipsoidFrameData = ellipsoidFreeGroup->FrameDataRelativeToWorld(); + boxFrameData = boxFreeGroup->FrameDataRelativeToWorld(); + + EXPECT_EQ(initialPose, + gz::math::eigen3::convert(ellipsoidFrameData.pose)); + EXPECT_EQ(initialPose, + gz::math::eigen3::convert(boxFrameData.pose)); + + // Set max contact between collision pairs to be 1 + world->SetCollisionPairMaxContacts(1u); + EXPECT_EQ(1u, world->GetCollisionPairMaxContacts()); + checkedOutput = StepWorld( + world, true, 1).first; + EXPECT_TRUE(checkedOutput); + + contacts = world->GetContactsFromLastStep(); + EXPECT_EQ(1u, contacts.size()); + + // Verify that the physics engine picked the contact with max penetration + // depth + auto contact = contacts[0]; + const auto* extraContactData = + contact.template Query< + gz::physics::World3d< + FeaturesCollisionPairMaxContacts>::ExtraContactData>(); + ASSERT_NE(nullptr, extraContactData); + EXPECT_FLOAT_EQ(maxDepth, extraContactData->depth); + } +} + // The features that an engine must have to be loaded by this loader. struct FeaturesStep : gz::physics::FeatureList< gz::physics::sdf::ConstructSdfWorld, diff --git a/test/common_test/worlds/collision_pair_contact_point.sdf b/test/common_test/worlds/collision_pair_contact_point.sdf new file mode 100644 index 000000000..74377d4b6 --- /dev/null +++ b/test/common_test/worlds/collision_pair_contact_point.sdf @@ -0,0 +1,68 @@ + + + + + + true + 0 0 0.0 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 100 100 1 + + + + + + + 100 100 1 + + + + + + + + 0 -0 0.0 0 0 0 + + + + 0.068 + 0 + 0 + 0.058 + 0 + 0.026 + + 1.0 + + + + + 0.2 0.3 0.5 + + + + + + + 0.2 0.3 0.5 + + + + + + + From 1fa4f63c9ed6494fb625ccc4d87b502ba9264b60 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 11 Jun 2024 21:00:58 +0200 Subject: [PATCH 13/18] Prepare for 6.6.0 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- CMakeLists.txt | 2 +- Changelog.md | 68 ++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 69 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a2d1f57cc..90437bb07 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(gz-physics6 VERSION 6.5.1) +project(gz-physics6 VERSION 6.6.0) #============================================================================ # Find gz-cmake diff --git a/Changelog.md b/Changelog.md index 3ecde71b1..77311a2bf 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,73 @@ ## Gazebo Physics 6.x +### Gazebo Physics 6.6.0 (2024-06-11) + +1. dartsim: optimize picking contact points with ODE collision detectorq + * [Pull request #584](https://github.com/gazebosim/gz-physics/pull/584) + +1. Fix windows compiler warning + * [Pull request #629](https://github.com/gazebosim/gz-physics/pull/629) + +1. Disable test failing due to ODE/libccd + * [Pull request #621](https://github.com/gazebosim/gz-physics/pull/621) + +1. bullet-featherstone: Improve mesh collision stability + * [Pull request #600](https://github.com/gazebosim/gz-physics/pull/600) + +1. bullet-featherstone: Support nested models + * [Pull request #574](https://github.com/gazebosim/gz-physics/pull/574) + +1. Revert "bazel: updates for garden (#513)" + * [Pull request #513)"](https://github.com/gazebosim/gz-physics/pull/513)") + +1. bazel: updates for garden + * [Pull request #513](https://github.com/gazebosim/gz-physics/pull/513) + +1. Garden test cleanup + * [Pull request #587](https://github.com/gazebosim/gz-physics/pull/587) + +1. Support setting max contacts in dartsim's ODE collision detector + * [Pull request #582](https://github.com/gazebosim/gz-physics/pull/582) + +1. Get bullet version from cmake instead of API + * [Pull request #591](https://github.com/gazebosim/gz-physics/pull/591) + +1. Prepare for 2.6.2 release + * [Pull request #585](https://github.com/gazebosim/gz-physics/pull/585) + +1. Reduce error to debug messsage for mesh construction (#531) + * [Pull request #531) (#581](https://github.com/gazebosim/gz-physics/pull/531) (#581) + +1. bullet-featherstone: Set collision spinning friction + * [Pull request #579](https://github.com/gazebosim/gz-physics/pull/579) + +1. CI workflow: use 22.04 for codecheck, coverage + * [Pull request #578](https://github.com/gazebosim/gz-physics/pull/578) + +1. Update github action workflows + * [Pull request #572](https://github.com/gazebosim/gz-physics/pull/572) + +1. dartsim: fix handling inertia matrix pose rotation + * [Pull request #351](https://github.com/gazebosim/gz-physics/pull/351) + +1. bullet-featherstone: fix setting angular velocity + * [Pull request #567](https://github.com/gazebosim/gz-physics/pull/567) + +1. bullet-featherstone: support off-diagonal inertia + * [Pull request #544](https://github.com/gazebosim/gz-physics/pull/544) + +1. bullet-featherstoneFix how links are flattened in ConstructSdfModel + * [Pull request #562](https://github.com/gazebosim/gz-physics/pull/562) + +1. Add sample ctest cmds to tutorial + * [Pull request #566](https://github.com/gazebosim/gz-physics/pull/566) + +1. Add a test to verify behavior of detachable joints + * [Pull request #563](https://github.com/gazebosim/gz-physics/pull/563) + +1. Use correct link indicies when constructing fixed constraints + * [Pull request #530](https://github.com/gazebosim/gz-physics/pull/530) + ### Gazebo Physics 6.5.1 (2023-09-26) 1. joint_features test: reduce console spam From 804a3ef4532f1b0000686ea8f0aca56b3f9780de Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 12 Jun 2024 17:20:40 +0200 Subject: [PATCH 14/18] Tweaks MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- Changelog.md | 14 +++----------- 1 file changed, 3 insertions(+), 11 deletions(-) diff --git a/Changelog.md b/Changelog.md index 77311a2bf..b24a33a0d 100644 --- a/Changelog.md +++ b/Changelog.md @@ -20,9 +20,6 @@ 1. Revert "bazel: updates for garden (#513)" * [Pull request #513)"](https://github.com/gazebosim/gz-physics/pull/513)") -1. bazel: updates for garden - * [Pull request #513](https://github.com/gazebosim/gz-physics/pull/513) - 1. Garden test cleanup * [Pull request #587](https://github.com/gazebosim/gz-physics/pull/587) @@ -32,19 +29,14 @@ 1. Get bullet version from cmake instead of API * [Pull request #591](https://github.com/gazebosim/gz-physics/pull/591) -1. Prepare for 2.6.2 release - * [Pull request #585](https://github.com/gazebosim/gz-physics/pull/585) - -1. Reduce error to debug messsage for mesh construction (#531) - * [Pull request #531) (#581](https://github.com/gazebosim/gz-physics/pull/531) (#581) +1. Reduce error to debug messsage for mesh construction (#581) + * [Pull request #581)(https://github.com/gazebosim/gz-physics/pull/581) 1. bullet-featherstone: Set collision spinning friction * [Pull request #579](https://github.com/gazebosim/gz-physics/pull/579) -1. CI workflow: use 22.04 for codecheck, coverage +1. Infrastructure * [Pull request #578](https://github.com/gazebosim/gz-physics/pull/578) - -1. Update github action workflows * [Pull request #572](https://github.com/gazebosim/gz-physics/pull/572) 1. dartsim: fix handling inertia matrix pose rotation From 72f9d9bad085fd4fbaba8b3842f431d4b907b1f0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 12 Jun 2024 18:24:33 +0200 Subject: [PATCH 15/18] Update Changelog.md MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Ian Chen Signed-off-by: Carlos Agüero --- Changelog.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Changelog.md b/Changelog.md index b24a33a0d..ccc9955d6 100644 --- a/Changelog.md +++ b/Changelog.md @@ -2,7 +2,7 @@ ### Gazebo Physics 6.6.0 (2024-06-11) -1. dartsim: optimize picking contact points with ODE collision detectorq +1. dartsim: optimize picking contact points with ODE collision detector * [Pull request #584](https://github.com/gazebosim/gz-physics/pull/584) 1. Fix windows compiler warning From c219b452a98f1f59ced91889d5632d9991556b23 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 12 Jun 2024 18:24:41 +0200 Subject: [PATCH 16/18] Update Changelog.md MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Ian Chen Signed-off-by: Carlos Agüero --- Changelog.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Changelog.md b/Changelog.md index ccc9955d6..285c81eb9 100644 --- a/Changelog.md +++ b/Changelog.md @@ -18,7 +18,7 @@ * [Pull request #574](https://github.com/gazebosim/gz-physics/pull/574) 1. Revert "bazel: updates for garden (#513)" - * [Pull request #513)"](https://github.com/gazebosim/gz-physics/pull/513)") + * [Pull request #513](https://github.com/gazebosim/gz-physics/pull/513) 1. Garden test cleanup * [Pull request #587](https://github.com/gazebosim/gz-physics/pull/587) From bddd5eccccefcf6a0f8fb4ecb24847b75be4c264 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 12 Jun 2024 18:24:47 +0200 Subject: [PATCH 17/18] Update Changelog.md MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Ian Chen Signed-off-by: Carlos Agüero --- Changelog.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Changelog.md b/Changelog.md index 285c81eb9..60a86058e 100644 --- a/Changelog.md +++ b/Changelog.md @@ -48,7 +48,7 @@ 1. bullet-featherstone: support off-diagonal inertia * [Pull request #544](https://github.com/gazebosim/gz-physics/pull/544) -1. bullet-featherstoneFix how links are flattened in ConstructSdfModel +1. bullet-featherstone: Fix how links are flattened in ConstructSdfModel * [Pull request #562](https://github.com/gazebosim/gz-physics/pull/562) 1. Add sample ctest cmds to tutorial From ddcba9d66669d246309d8d6670a5b105f7b2853b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 12 Jun 2024 18:24:53 +0200 Subject: [PATCH 18/18] Update Changelog.md MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Ian Chen Signed-off-by: Carlos Agüero --- Changelog.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Changelog.md b/Changelog.md index 60a86058e..de4611780 100644 --- a/Changelog.md +++ b/Changelog.md @@ -30,7 +30,7 @@ * [Pull request #591](https://github.com/gazebosim/gz-physics/pull/591) 1. Reduce error to debug messsage for mesh construction (#581) - * [Pull request #581)(https://github.com/gazebosim/gz-physics/pull/581) + * [Pull request #581](https://github.com/gazebosim/gz-physics/pull/581) 1. bullet-featherstone: Set collision spinning friction * [Pull request #579](https://github.com/gazebosim/gz-physics/pull/579)