diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index a77b95c3d..abdac823b 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,6 +1,12 @@ name: Ubuntu CI -on: [push, pull_request] +on: + pull_request: + push: + branches: + - 'ign-physics[0-9]' + - 'gz-physics[0-9]?' + - 'main' jobs: jammy-ci: 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/CMakeLists.txt b/CMakeLists.txt index 3ceb7d765..9e6350c16 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -42,6 +42,9 @@ gz_find_package(gz-common5 REQUIRED_BY heightmap mesh dartsim tpe tpelib bullet) set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR}) +# This is only used for test support +gz_find_package(gz-common5 REQUIRED COMPONENTS testing) + #-------------------------------------- # Find gz-math gz_find_package(gz-math7 REQUIRED COMPONENTS eigen3) @@ -90,8 +93,6 @@ gz_find_package(GzBullet message(STATUS "-------------------------------------------\n") -set(GZ_PHYSICS_RESOURCE_DIR "${CMAKE_SOURCE_DIR}/resources") - # Plugin install dirs set(GZ_PHYSICS_ENGINE_RELATIVE_INSTALL_DIR ${GZ_LIB_INSTALL_DIR}/gz-${GZ_DESIGNATION}-${PROJECT_VERSION_MAJOR}/engine-plugins 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/dartsim/CMakeLists.txt b/dartsim/CMakeLists.txt index e71d70ef4..fbf06aab7 100644 --- a/dartsim/CMakeLists.txt +++ b/dartsim/CMakeLists.txt @@ -80,22 +80,21 @@ gz_build_tests( LIB_DEPS ${features} ${dartsim_plugin} - gz-plugin${GZ_PLUGIN_VER}::loader - gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} + gz-physics-test gz-common${GZ_COMMON_VER}::geospatial ${PROJECT_LIBRARY_TARGET_NAME}-sdf ${PROJECT_LIBRARY_TARGET_NAME}-heightmap ${PROJECT_LIBRARY_TARGET_NAME}-mesh TEST_LIST tests ENVIRONMENT - GZ_PHYSICS_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}) + GZ_PHYSICS_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} + INCLUDE_DIRS + ${CMAKE_CURRENT_SOURCE_DIR}/src +) foreach(test ${tests}) - target_compile_definitions(${test} PRIVATE - "dartsim_plugin_LIB=\"$\"" - "TEST_WORLD_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/worlds/\"" - "GZ_PHYSICS_RESOURCE_DIR=\"${GZ_PHYSICS_RESOURCE_DIR}\"") + "dartsim_plugin_LIB=\"$\"") if (DART_HAS_CONTACT_SURFACE_HEADER) target_compile_definitions(${test} PRIVATE DART_HAS_CONTACT_SURFACE) @@ -104,10 +103,3 @@ foreach(test ${tests}) # Helps when we want to build a single test after making changes to dartsim_plugin add_dependencies(${test} ${dartsim_plugin}) endforeach() - -foreach(test UNIT_FindFeatures_TEST UNIT_RequestFeatures_TEST) - if(TARGET ${test}) - target_compile_definitions(${test} PRIVATE - "dartsim_plugin_LIB=\"$\"") - endif() -endforeach() diff --git a/dartsim/src/AddedMassFeatures_TEST.cc b/dartsim/src/AddedMassFeatures_TEST.cc index 525ce3e2b..0e747b9a1 100644 --- a/dartsim/src/AddedMassFeatures_TEST.cc +++ b/dartsim/src/AddedMassFeatures_TEST.cc @@ -38,6 +38,7 @@ #include #include +#include "Worlds.hh" #include "World.hh" @@ -122,7 +123,7 @@ TEST(AddedMassFeatures, AddedMass) 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1; - auto world = LoadWorld(TEST_WORLD_DIR"/added_mass.sdf"); + const auto world = LoadWorld(dartsim::worlds::kAddedMassSdf); ASSERT_NE(nullptr, world); auto dartWorld = world->GetDartsimWorld(); diff --git a/dartsim/src/Base_TEST.cc b/dartsim/src/Base_TEST.cc index 1d8a37352..c0b5be3c5 100644 --- a/dartsim/src/Base_TEST.cc +++ b/dartsim/src/Base_TEST.cc @@ -38,8 +38,9 @@ #include "EntityManagementFeatures.hh" #include "SDFFeatures.hh" -using namespace gz::physics; +#include "test/Resources.hh" +using namespace gz::physics; TEST(BaseClass, RemoveModel) @@ -166,7 +167,7 @@ TEST(BaseClass, SdfConstructionBookkeeping) ::sdf::Root root; - auto errors = root.Load(GZ_PHYSICS_RESOURCE_DIR "/rrbot.xml"); + auto errors = root.Load(gz::physics::test::resources::kRrbotXml); ASSERT_TRUE(errors.empty()); const ::sdf::Model *sdfModel = root.Model(); ASSERT_NE(nullptr, sdfModel); diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc index 48ae13e07..990ca5631 100644 --- a/dartsim/src/EntityManagementFeatures.cc +++ b/dartsim/src/EntityManagementFeatures.cc @@ -22,13 +22,14 @@ #include #include -#include #include #include #include #include +#include "GzOdeCollisionDetector.hh" + namespace gz { namespace physics { namespace dartsim { @@ -724,11 +725,12 @@ Identity EntityManagementFeatures::ConstructEmptyWorld( const Identity &/*_engineID*/, const std::string &_name) { const auto &world = std::make_shared(_name); - world->getConstraintSolver()->setCollisionDetector( - dart::collision::OdeCollisionDetector::create()); + auto collisionDetector = dart::collision::GzOdeCollisionDetector::create(); + world->getConstraintSolver()->setCollisionDetector(collisionDetector); - // TODO(anyone) We need a machanism to configure maxNumContacts at runtime. auto &collOpt = world->getConstraintSolver()->getCollisionOption(); + // Set the max number of contacts for all collision objects + // in the world collOpt.maxNumContacts = 10000; world->getConstraintSolver()->getCollisionOption().collisionFilter = diff --git a/dartsim/src/EntityManagement_TEST.cc b/dartsim/src/EntityManagement_TEST.cc index 07e056d4f..52a3e9b4a 100644 --- a/dartsim/src/EntityManagement_TEST.cc +++ b/dartsim/src/EntityManagement_TEST.cc @@ -35,6 +35,8 @@ #include "KinematicsFeatures.hh" #include "ShapeFeatures.hh" +#include "test/Resources.hh" + using namespace gz; struct TestFeatureList : physics::FeatureList< @@ -172,8 +174,7 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) auto meshLink = model->ConstructEmptyLink("mesh_link"); meshLink->AttachFixedJoint(child, "fixed"); - const std::string meshFilename = common::joinPaths( - GZ_PHYSICS_RESOURCE_DIR, "chassis.dae"); + const std::string meshFilename = gz::physics::test::resources::kChassisDae; auto &meshManager = *common::MeshManager::Instance(); auto *mesh = meshManager.Load(meshFilename); @@ -212,8 +213,7 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) auto heightmapLink = model->ConstructEmptyLink("heightmap_link"); heightmapLink->AttachFixedJoint(child, "heightmap_joint"); - auto heightmapFilename = common::joinPaths( - GZ_PHYSICS_RESOURCE_DIR, "heightmap_bowl.png"); + auto heightmapFilename = gz::physics::test::resources::kHeightmapBowlPng; common::ImageHeightmap data; EXPECT_EQ(0, data.Load(heightmapFilename)); @@ -239,8 +239,7 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) auto demLink = model->ConstructEmptyLink("dem_link"); demLink->AttachFixedJoint(child, "dem_joint"); - auto demFilename = common::joinPaths( - GZ_PHYSICS_RESOURCE_DIR, "volcano.tif"); + auto demFilename = gz::physics::test::resources::kVolcanoTif; common::Dem dem; EXPECT_EQ(0, dem.Load(demFilename)); diff --git a/dartsim/src/GzOdeCollisionDetector.cc b/dartsim/src/GzOdeCollisionDetector.cc new file mode 100644 index 000000000..228ee9562 --- /dev/null +++ b/dartsim/src/GzOdeCollisionDetector.cc @@ -0,0 +1,112 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include + +#include + +#include "GzOdeCollisionDetector.hh" + +using namespace dart; +using namespace collision; + +///////////////////////////////////////////////// +GzOdeCollisionDetector::GzOdeCollisionDetector() + : OdeCollisionDetector() +{ +} + +///////////////////////////////////////////////// +GzOdeCollisionDetector::Registrar + GzOdeCollisionDetector::mRegistrar{ + GzOdeCollisionDetector::getStaticType(), + []() -> std::shared_ptr { + return GzOdeCollisionDetector::create(); + }}; + +///////////////////////////////////////////////// +std::shared_ptr GzOdeCollisionDetector::create() +{ + return std::shared_ptr(new GzOdeCollisionDetector()); +} + +///////////////////////////////////////////////// +bool GzOdeCollisionDetector::collide( + CollisionGroup *_group, + const CollisionOption &_option, + CollisionResult *_result) +{ + bool ret = OdeCollisionDetector::collide(_group, _option, _result); + this->LimitCollisionPairMaxContacts(_result); + return ret; +} + +///////////////////////////////////////////////// +bool GzOdeCollisionDetector::collide( + CollisionGroup *_group1, + CollisionGroup *_group2, + const CollisionOption &_option, + CollisionResult *_result) +{ + bool ret = OdeCollisionDetector::collide(_group1, _group2, _option, _result); + this->LimitCollisionPairMaxContacts(_result); + return ret; +} + +///////////////////////////////////////////////// +void GzOdeCollisionDetector::SetCollisionPairMaxContacts( + std::size_t _maxContacts) +{ + this->maxCollisionPairContacts = _maxContacts; +} + +///////////////////////////////////////////////// +std::size_t GzOdeCollisionDetector::GetCollisionPairMaxContacts() const +{ + return this->maxCollisionPairContacts; +} + +///////////////////////////////////////////////// +void GzOdeCollisionDetector::LimitCollisionPairMaxContacts( + CollisionResult *_result) +{ + if (this->maxCollisionPairContacts == + std::numeric_limits::max()) + return; + + auto allContacts = _result->getContacts(); + _result->clear(); + + std::unordered_map> + contactMap; + + for (auto &contact : allContacts) + { + auto &count = + contactMap[contact.collisionObject1][contact.collisionObject2]; + count++; + auto &otherCount = + contactMap[contact.collisionObject2][contact.collisionObject1]; + std::size_t total = count + otherCount; + if (total <= this->maxCollisionPairContacts) + { + _result->addContact(contact); + } + } +} diff --git a/dartsim/src/GzOdeCollisionDetector.hh b/dartsim/src/GzOdeCollisionDetector.hh new file mode 100644 index 000000000..de8233e93 --- /dev/null +++ b/dartsim/src/GzOdeCollisionDetector.hh @@ -0,0 +1,73 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include + +#include + +namespace dart { +namespace collision { + +class GzOdeCollisionDetector : public dart::collision::OdeCollisionDetector +{ + // Documentation inherited + public: bool collide( + CollisionGroup* group, + const CollisionOption& option = CollisionOption(false, 1u, nullptr), + CollisionResult* result = nullptr) override; + + // Documentation inherited + public: bool collide( + CollisionGroup* group1, + CollisionGroup* group2, + const CollisionOption& option = CollisionOption(false, 1u, nullptr), + CollisionResult* result = nullptr) override; + + /// \brief Set the maximum number of contacts between a pair of collision + /// objects + /// \param[in] _maxContacts Maximum number of contacts between a pair of + /// collision objects. + public: void SetCollisionPairMaxContacts(std::size_t _maxContacts); + + /// \brief Get the maximum number of contacts between a pair of collision + /// objects + /// \return Maximum number of contacts between a pair of collision objects. + public: std::size_t GetCollisionPairMaxContacts() const; + + + /// \brief Create the GzOdeCollisionDetector + public: static std::shared_ptr create(); + + /// Constructor + protected: GzOdeCollisionDetector(); + + /// \brief Limit max number of contacts between a pair of collision objects. + /// The function modifies the contacts vector inside the CollisionResult + /// object to cap the number of contacts for each collision pair based on the + /// maxCollisionPairContacts value + private: void LimitCollisionPairMaxContacts(CollisionResult *_result); + + /// \brief Maximum number of contacts between a pair of collision objects. + private: std::size_t maxCollisionPairContacts = + std::numeric_limits::max(); + + private: static Registrar mRegistrar; +}; + +} +} diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index e2ded061f..d3db87464 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -51,6 +51,8 @@ #include #include +#include +#include "Worlds.hh" #include "World.hh" @@ -313,7 +315,7 @@ INSTANTIATE_TEST_SUITE_P(LoadWorld, SDFFeatures_TEST, // Test that the dartsim plugin loaded all the relevant information correctly. TEST_P(SDFFeatures_TEST, CheckDartsimData) { - WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/test.world"); + WorldPtr world = this->LoadWorld(common_test::worlds::kTestWorld); ASSERT_NE(nullptr, world); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); @@ -421,7 +423,7 @@ TEST_P(SDFFeatures_TEST, CheckDartsimData) // Test that joint limits are working by running the simulation TEST_P(SDFFeatures_TEST, CheckJointLimitEnforcement) { - WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/test.world"); + WorldPtr world = this->LoadWorld(common_test::worlds::kTestWorld); ASSERT_NE(nullptr, world); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); @@ -581,7 +583,7 @@ TEST_P(SDFFeatures_TEST, WorldIsParentOrChild) TEST_P(SDFFeatures_TEST, WorldWithNestedModel) { WorldPtr world = - this->LoadWorld(TEST_WORLD_DIR "/world_with_nested_model.sdf"); + this->LoadWorld(common_test::worlds::kWorldWithNestedModelSdf); ASSERT_NE(nullptr, world); EXPECT_EQ(2u, world->GetModelCount()); @@ -646,7 +648,7 @@ TEST_P(SDFFeatures_TEST, WorldWithNestedModel) TEST_P(SDFFeatures_TEST, WorldWithNestedModelJointToWorld) { WorldPtr world = this->LoadWorld( - TEST_WORLD_DIR "/world_with_nested_model_joint_to_world.sdf"); + dartsim::worlds::kWorldWithNestedModelJointToWorldSdf); ASSERT_NE(nullptr, world); EXPECT_EQ(1u, world->GetModelCount()); @@ -690,7 +692,7 @@ TEST_P(SDFFeatures_TEST, WorldWithNestedModelJointToWorld) // Test that joint type falls back to fixed if the type is not supported TEST_P(SDFFeatures_TEST, FallbackToFixedJoint) { - WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/test.world"); + WorldPtr world = this->LoadWorld(common_test::worlds::kTestWorld); ASSERT_NE(nullptr, world); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); @@ -717,7 +719,7 @@ TEST_P(SDFFeatures_TEST, FallbackToFixedJoint) TEST_P(SDFFeatures_TEST, JointsAcrossNestedModels) { WorldPtr world = this->LoadWorld( - TEST_WORLD_DIR "/joint_across_nested_models.sdf"); + dartsim::worlds::kJointAcrossNestedModelsSdf); ASSERT_NE(nullptr, world); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); @@ -775,7 +777,7 @@ INSTANTIATE_TEST_SUITE_P(LoadWorld, SDFFeatures_FrameSemantics, ///////////////////////////////////////////////// TEST_P(SDFFeatures_FrameSemantics, LinkRelativeTo) { - WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/model_frames.sdf"); + WorldPtr world = this->LoadWorld(dartsim::worlds::kModelFramesSdf); ASSERT_NE(nullptr, world); const std::string modelName = "link_relative_to"; @@ -807,7 +809,7 @@ TEST_P(SDFFeatures_FrameSemantics, LinkRelativeTo) ///////////////////////////////////////////////// TEST_P(SDFFeatures_FrameSemantics, CollisionRelativeTo) { - WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/model_frames.sdf"); + WorldPtr world = this->LoadWorld(dartsim::worlds::kModelFramesSdf); ASSERT_NE(nullptr, world); const std::string modelName = "collision_relative_to"; @@ -844,7 +846,7 @@ TEST_P(SDFFeatures_FrameSemantics, CollisionRelativeTo) ///////////////////////////////////////////////// TEST_P(SDFFeatures_FrameSemantics, ExplicitFramesWithLinks) { - WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/model_frames.sdf"); + WorldPtr world = this->LoadWorld(dartsim::worlds::kModelFramesSdf); ASSERT_NE(nullptr, world); const std::string modelName = "explicit_frames_with_links"; @@ -891,7 +893,7 @@ TEST_P(SDFFeatures_FrameSemantics, ExplicitFramesWithLinks) ///////////////////////////////////////////////// TEST_P(SDFFeatures_FrameSemantics, ExplicitFramesWithCollision) { - WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/model_frames.sdf"); + WorldPtr world = this->LoadWorld(dartsim::worlds::kModelFramesSdf); ASSERT_NE(nullptr, world); const std::string modelName = "explicit_frames_with_collisions"; @@ -928,7 +930,7 @@ TEST_P(SDFFeatures_FrameSemantics, ExplicitFramesWithCollision) ///////////////////////////////////////////////// TEST_P(SDFFeatures_FrameSemantics, ExplicitWorldFrames) { - WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/world_frames.sdf"); + WorldPtr world = this->LoadWorld(dartsim::worlds::kWorldFramesSdf); ASSERT_NE(nullptr, world); const std::string modelName = "M"; @@ -938,7 +940,6 @@ TEST_P(SDFFeatures_FrameSemantics, ExplicitWorldFrames) const dart::dynamics::SkeletonPtr skeleton = dartWorld->getSkeleton(modelName); - ASSERT_NE(nullptr, skeleton); ASSERT_EQ(1u, skeleton->getNumBodyNodes()); @@ -965,7 +966,7 @@ TEST_P(SDFFeatures_FrameSemantics, ExplicitWorldFrames) ///////////////////////////////////////////////// TEST_P(SDFFeatures_TEST, Shapes) { - auto world = this->LoadWorld(TEST_WORLD_DIR"/shapes.sdf"); + WorldPtr world = this->LoadWorld(common_test::worlds::kShapesWorld); ASSERT_NE(nullptr, world); auto dartWorld = world->GetDartsimWorld(); @@ -974,7 +975,7 @@ TEST_P(SDFFeatures_TEST, Shapes) ASSERT_EQ(5u, dartWorld->getNumSkeletons()); int count{0}; - for (auto name : {"box", "cylinder", "sphere", "capsule", "ellipsoid"}) + for (auto name : {"sphere", "box", "cylinder", "capsule", "ellipsoid"}) { const auto skeleton = dartWorld->getSkeleton(count++); ASSERT_NE(nullptr, skeleton); diff --git a/dartsim/src/WorldFeatures.cc b/dartsim/src/WorldFeatures.cc index d05eeefb8..da7088580 100644 --- a/dartsim/src/WorldFeatures.cc +++ b/dartsim/src/WorldFeatures.cc @@ -21,7 +21,6 @@ #include #include #include -#include #include #include #include @@ -30,8 +29,10 @@ #include +#include "GzOdeCollisionDetector.hh" #include "WorldFeatures.hh" + namespace gz { namespace physics { namespace dartsim { @@ -53,7 +54,7 @@ void WorldFeatures::SetWorldCollisionDetector( } else if (_collisionDetector == "ode") { - collisionDetector = dart::collision::OdeCollisionDetector::create(); + collisionDetector = dart::collision::GzOdeCollisionDetector::create(); } else if (_collisionDetector == "dart") { @@ -96,6 +97,46 @@ WorldFeatures::LinearVectorType WorldFeatures::GetWorldGravity( return world->getGravity(); } +///////////////////////////////////////////////// +void WorldFeatures::SetWorldCollisionPairMaxContacts( + const Identity &_id, std::size_t _maxContacts) +{ + auto world = this->ReferenceInterface(_id); + auto collisionDetector = + world->getConstraintSolver()->getCollisionDetector(); + + auto odeCollisionDetector = + std::dynamic_pointer_cast( + collisionDetector); + if (odeCollisionDetector) + { + odeCollisionDetector->SetCollisionPairMaxContacts(_maxContacts); + } + else + { + gzwarn << "Currently max contacts feature is only supported by the " + << "ode collision detector in dartsim." << std::endl; + } +} + +///////////////////////////////////////////////// +std::size_t WorldFeatures::GetWorldCollisionPairMaxContacts( + const Identity &_id) const +{ + auto world = this->ReferenceInterface(_id); + auto collisionDetector = + world->getConstraintSolver()->getCollisionDetector(); + auto odeCollisionDetector = + std::dynamic_pointer_cast( + collisionDetector); + if (odeCollisionDetector) + { + return odeCollisionDetector->GetCollisionPairMaxContacts(); + } + + return 0u; +} + ///////////////////////////////////////////////// void WorldFeatures::SetWorldSolver(const Identity &_id, const std::string &_solver) diff --git a/dartsim/src/WorldFeatures.hh b/dartsim/src/WorldFeatures.hh index a7fd9855b..da15810dd 100644 --- a/dartsim/src/WorldFeatures.hh +++ b/dartsim/src/WorldFeatures.hh @@ -30,6 +30,7 @@ namespace dartsim { struct WorldFeatureList : FeatureList< CollisionDetector, + CollisionPairMaxContacts, Gravity, Solver > { }; @@ -53,6 +54,14 @@ class WorldFeatures : // Documentation inherited public: LinearVectorType GetWorldGravity(const Identity &_id) const override; + // Documentation inherited + public: void SetWorldCollisionPairMaxContacts( + const Identity &_id, std::size_t _maxContacts) override; + + // Documentation inherited + public: std::size_t GetWorldCollisionPairMaxContacts(const Identity &_id) + const override; + // Documentation inherited public: void SetWorldSolver(const Identity &_id, const std::string &_solver) override; diff --git a/dartsim/src/WorldFeatures_TEST.cc b/dartsim/src/WorldFeatures_TEST.cc index 459460ba2..fe0eef69a 100644 --- a/dartsim/src/WorldFeatures_TEST.cc +++ b/dartsim/src/WorldFeatures_TEST.cc @@ -32,6 +32,7 @@ #include #include "test/Utils.hh" +#include "test/common_test/Worlds.hh" struct TestFeatureList : gz::physics::FeatureList< gz::physics::CollisionDetector, @@ -82,8 +83,7 @@ TestWorldPtr LoadWorld( ////////////////////////////////////////////////// TEST_F(WorldFeaturesFixture, CollisionDetector) { - auto world = LoadWorld(this->engine, - gz::common::joinPaths(TEST_WORLD_DIR, "empty.sdf")); + const auto world = LoadWorld(this->engine, common_test::worlds::kEmptySdf); EXPECT_EQ("ode", world->GetCollisionDetector()); world->SetCollisionDetector("banana"); @@ -105,8 +105,7 @@ TEST_F(WorldFeaturesFixture, CollisionDetector) ////////////////////////////////////////////////// TEST_F(WorldFeaturesFixture, Solver) { - auto world = LoadWorld(this->engine, - gz::common::joinPaths(TEST_WORLD_DIR, "empty.sdf")); + const auto world = LoadWorld(this->engine, common_test::worlds::kEmptySdf); EXPECT_EQ("DantzigBoxedLcpSolver", world->GetSolver()); diff --git a/dartsim/src/Worlds.hh b/dartsim/src/Worlds.hh new file mode 100644 index 000000000..4ffa870da --- /dev/null +++ b/dartsim/src/Worlds.hh @@ -0,0 +1,43 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef DARTSIM_WORLDS_HH_ +#define DARTSIM_WORLDS_HH_ + +#include + +#include + +// \brief retrieve a filename from the dartsim/worlds directory +// \param[in] _world filename to retrieve +// \return full path to the request world +inline std::string DartsimTestWorld(const std::string &_world) +{ + return gz::common::testing::SourceFile("dartsim", "worlds", _world); +} + +namespace dartsim::worlds +{ +const auto kAddedMassSdf = DartsimTestWorld("added_mass.sdf"); +const auto kJointAcrossNestedModelsSdf = + DartsimTestWorld("joint_across_nested_models.sdf"); +const auto kModelFramesSdf = DartsimTestWorld("model_frames.sdf"); +const auto kWorldFramesSdf = DartsimTestWorld("world_frames.sdf"); +const auto kWorldWithNestedModelJointToWorldSdf = + DartsimTestWorld("world_with_nested_model_joint_to_world.sdf"); +} // namespace dartsim::worlds +#endif // DARTSIM_WORLDS_HH_ diff --git a/dartsim/worlds/empty.sdf b/dartsim/worlds/empty.sdf deleted file mode 100644 index 62150c14a..000000000 --- a/dartsim/worlds/empty.sdf +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/dartsim/worlds/falling.world b/dartsim/worlds/falling.world deleted file mode 100644 index 1d2ea2f43..000000000 --- a/dartsim/worlds/falling.world +++ /dev/null @@ -1,75 +0,0 @@ - - - - - - - 0 0 2 0 0.78539816339 0 - - 0.0 0.0 0.0 0 0 0 - - - 0.4 - 0 - 0 - 0.4 - 0 - 0.4 - - 1.0 - - - 0.0 0.0 0.0 0 0 0 - - - 1 - - - - - 0.0 0.0 0.0 0 0 0 - - - 1 - - - - - - - true - 0 0 -0.5 0 0 0.78539816339 - - - - 1 - 0 - 0 - 1 - 0 - 1 - - 1.0 - - 0.0 0.0 0.0 0 0 0 - - - - 100 100 1 - - - - - - - - 100 100 1 - - - - - - - diff --git a/dartsim/worlds/shapes.sdf b/dartsim/worlds/shapes.sdf deleted file mode 100644 index 733c82ac2..000000000 --- a/dartsim/worlds/shapes.sdf +++ /dev/null @@ -1,191 +0,0 @@ - - - - - 0 0 0.5 0 0 0 - - - - 0.16666 - 0 - 0 - 0.16666 - 0 - 0.16666 - - 1.0 - - - - - 1 1 1 - - - - - - - - 1 1 1 - - - - 1 0 0 1 - 1 0 0 1 - 1 0 0 1 - - - - - - - 0 -1.5 0.5 0 0 0 - - - - 0.1458 - 0 - 0 - 0.1458 - 0 - 0.125 - - 1.0 - - - - - 0.5 - 1.0 - - - - - - - - 0.5 - 1.0 - - - - 0 1 0 1 - 0 1 0 1 - 0 1 0 1 - - - - - - - 0 1.5 0.5 0 0 0 - - - - 0.1 - 0 - 0 - 0.1 - 0 - 0.1 - - 1.0 - - - - - 0.5 - - - - - - - - 0.5 - - - - 0 0 1 1 - 0 0 1 1 - 0 0 1 1 - - - - - - - 0 -3.0 0.5 0 0 0 - - - - 0.074154 - 0 - 0 - 0.074154 - 0 - 0.018769 - - 1.0 - - - - - 0.2 - 0.6 - - - - - - - 0.2 - 0.6 - - - - 1 1 0 1 - 1 1 0 1 - 1 1 0 1 - - - - - - - 0 3.0 0.5 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 - - - - 1 0 1 1 - 1 0 1 1 - 1 0 1 1 - - - - - - diff --git a/dartsim/worlds/shapes_bitmask.sdf b/dartsim/worlds/shapes_bitmask.sdf deleted file mode 100644 index 8aa289fce..000000000 --- a/dartsim/worlds/shapes_bitmask.sdf +++ /dev/null @@ -1,154 +0,0 @@ - - - - - true - - - - - 0 0 1 - - - - - - - 0 0 1 - 100 100 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - - - - 0 0 10.0 0 0 0 - - - - 1 - 0 - 0 - 1 - 0 - 1 - - 1.0 - - - - - 1 1 1 - - - - - 0x01 - - - - - - - 1 1 1 - - - - 1 0 0 1 - 1 0 0 1 - 1 0 0 1 - - - - - - - 0 0.75 10.0 0 0 0 - - - - 1 - 0 - 0 - 1 - 0 - 1 - - 1.0 - - - - - 1 1 1 - - - - - 0x02 - - - - - - - 1 1 1 - - - - 0 1 0 1 - 0 1 0 1 - 0 1 0 1 - - - - - - - 0 -0.75 10.0 0 0 0 - - - - 1 - 0 - 0 - 1 - 0 - 1 - - 1.0 - - - - - 1 1 1 - - - - - 0x03 - - - - - - - 1 1 1 - - - - 1 1 0 1 - 1 1 0 1 - 1 1 0 1 - - - - - - - diff --git a/dartsim/worlds/test.world b/dartsim/worlds/test.world deleted file mode 100644 index b75496857..000000000 --- a/dartsim/worlds/test.world +++ /dev/null @@ -1,383 +0,0 @@ - - - - - true - - - - - 0 0 1 - 100 100 - - - - - - 100 - 50 - - - - - - false - - - 0 0 1 - 100 100 - - - - 0.9 0.9 0.9 1 - - - - - - 1 0 0 0 0 0 - - - 100 - - - 0 0 0.01 0 0 0 - - - 0.8 - 0.02 - - - - 1 1 0 1 - - - - -0.275 0 1.1 0 0 0 - - - 0.2 0.2 2.2 - - - - 1 1 0 1 - - - - 0 0 0.01 0 0 0 - - - 0.8 - 0.02 - - - - - -0.275 0 1.1 0 0 0 - - - 0.2 0.2 2.2 - - - - - - 1.1 - - - - - - - - 0 0 2.1 -1.5708 0 0 - 0 - - 0 0 0.5 0 0 0 - - - -0.05 0 0 0 1.5708 0 - - - 0.1 - 0.3 - - - - 1 1 0 1 - - - - 0 0 1.0 0 1.5708 0 - - - 0.1 - 0.2 - - - - 1 1 0 1 - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - 1 1 0 1 - - - - -0.05 0 0 0 1.5708 0 - - - 0.1 - 0.3 - - - - - - 0.1 - - - - - - 0 0 1.0 0 1.5708 0 - - - 0.1 - 0.2 - - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - - - - 0.25 1.0 2.1 -2 0 0 - 0 - - 0 0 0.5 0 0 0 - - - 0 0 0 0 1.5708 0 - - - 0.08 - 0.3 - - - - 1 1 0 1 - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - 1 1 0 1 - - - - 0 0 0 0 1.5708 0 - - - 0.08 - 0.3 - - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - - - - base - upper_link - - 1.0 0 0 - - 3.0 - - - - - - upper_link - lower_link - - 1.0 0 0 - - 3.0 - - - - - - - 0.0 10.0 10.0 0.0 0.0 0.0 - - 0.0 0.0 0.0 0.0 0.0 0.0 - - - - 1.0 - - - - - 0.8 - - - - - - - - 10 0 2 0 0 0 - - - 100 - - - 0 0 0 -1.57 0 0 - - - 0.1 - 0.2 - - - - - - 1 0 0 0 0 0 - - 1 - - - - - 0.1 0.1 0.1 - - - - - - world - base - - - -1 0 0 0 0 0 - base - bar - - 0 1 0 - - -0.5 - 0.5 - 100 - - - - - - - 10 10 2 0 0 0 - - - 100 - - - 0 0 0 -1.57 0 0 - - - 0.1 - 0.2 - - - - - - 0 0 -1 0 0 0 - - 1 - - - - - 0.1 0.1 0.1 - - - - - - world - base - - - 0 0 1 0 0 0 - base - bar - - 0 1 0 - - - - - - - - link0 - link1 - 2 - - - - - - - - link0 - link1 - - - - - link2 - link3 - - - - - link4 - link5 - - - - diff --git a/dartsim/worlds/world_with_nested_model.sdf b/dartsim/worlds/world_with_nested_model.sdf deleted file mode 100644 index 0b71ad0d7..000000000 --- a/dartsim/worlds/world_with_nested_model.sdf +++ /dev/null @@ -1,75 +0,0 @@ - - - - - - 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 - - - - - - - - - - - - - - - - - - - - diff --git a/include/gz/physics/World.hh b/include/gz/physics/World.hh index f99e65951..dabc453ef 100644 --- a/include/gz/physics/World.hh +++ b/include/gz/physics/World.hh @@ -130,6 +130,46 @@ namespace gz }; }; + ///////////////////////////////////////////////// + class GZ_PHYSICS_VISIBLE CollisionPairMaxContacts: + public virtual Feature + { + /// \brief The World API for getting and setting the maximum + /// number of contacts between two entities. + public: template + class World : public virtual Feature::World + { + /// \brief Set the maximum number of contacts allowed between two + /// entities. + /// \param[in] _maxContacts Maximum number of contacts. + public: void SetCollisionPairMaxContacts(std::size_t _maxContacts); + + /// \brief Get the maximum number of contacts allowed between two + /// entities. + /// \return Maximum number of contacts. + public: std::size_t GetCollisionPairMaxContacts() const; + }; + + /// \private The implementation API for getting and setting max contacts. + public: template + class Implementation : public virtual Feature::Implementation + { + /// \brief Implementation API for setting the maximum number of + /// contacts between two entities. + /// \param[in] _id Identity of the world. + /// \param[in] _maxContacts Maximum number of contacts. + public: virtual void SetWorldCollisionPairMaxContacts( + const Identity &_id, std::size_t _maxContacts) = 0; + + /// \brief Implementation API for getting the maximum number of + /// contacts between two entities. + /// \param[in] _id Identity of the world. + /// \return Maximum number of contacts. + public: virtual std::size_t GetWorldCollisionPairMaxContacts( + const Identity &_id) const = 0; + }; + }; + ///////////////////////////////////////////////// class GZ_PHYSICS_VISIBLE Solver : public virtual Feature { 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/include/gz/physics/detail/World.hh b/include/gz/physics/detail/World.hh index 00ee40042..d5270913c 100644 --- a/include/gz/physics/detail/World.hh +++ b/include/gz/physics/detail/World.hh @@ -99,6 +99,24 @@ auto Gravity::World::GetGravity( _inCoordinatesOf); } +///////////////////////////////////////////////// +template +void CollisionPairMaxContacts::World::SetCollisionPairMaxContacts(std::size_t _maxContacts) +{ + this->template Interface() + ->SetWorldCollisionPairMaxContacts(this->identity, _maxContacts); +} + +///////////////////////////////////////////////// +template +std::size_t CollisionPairMaxContacts::World:: + GetCollisionPairMaxContacts() const +{ + return this->template Interface() + ->GetWorldCollisionPairMaxContacts(this->identity); +} + ///////////////////////////////////////////////// template void Solver::World::SetSolver( diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 3f57a1355..8e00aed09 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -25,8 +25,11 @@ endif() gz_build_tests( TYPE UNIT SOURCES ${gtest_sources} + LIB_DEPS + gz-physics-test ENVIRONMENT - GZ_PHYSICS_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}) + GZ_PHYSICS_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} +) if(TARGET UNIT_FindFeatures_TEST) target_link_libraries(UNIT_FindFeatures_TEST diff --git a/src/Cloneable_TEST.cc b/src/Cloneable_TEST.cc index 08f8bf7c5..db5766197 100644 --- a/src/Cloneable_TEST.cc +++ b/src/Cloneable_TEST.cc @@ -18,7 +18,7 @@ #include #include "gz/physics/Cloneable.hh" -#include "utils/TestDataTypes.hh" +#include "test/TestDataTypes.hh" using namespace gz; using physics::Cloneable; diff --git a/src/CompositeData_TEST.cc b/src/CompositeData_TEST.cc index 4e76776d0..3a9e1a2b2 100644 --- a/src/CompositeData_TEST.cc +++ b/src/CompositeData_TEST.cc @@ -18,7 +18,7 @@ #include #include "gz/physics/CompositeData.hh" -#include "utils/TestDataTypes.hh" +#include "test/TestDataTypes.hh" using namespace gz; diff --git a/src/FindFeatures_TEST.cc b/src/FindFeatures_TEST.cc index 165e3f42f..4309807ab 100644 --- a/src/FindFeatures_TEST.cc +++ b/src/FindFeatures_TEST.cc @@ -24,7 +24,7 @@ #include -#include "TestUtilities.hh" +#include "test/Utils.hh" using namespace gz; @@ -59,7 +59,7 @@ TEST(FindFeatures_TEST, ForwardStep) TEST(FindFeatures_TEST, Unimplemented) { using TestFeatures = - physics::FeatureList; + physics::FeatureList; plugin::Loader loader; PrimeTheLoader(loader); 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/src/SpecifyData_TEST.cc b/src/SpecifyData_TEST.cc index 4b46f28c4..2018f2018 100644 --- a/src/SpecifyData_TEST.cc +++ b/src/SpecifyData_TEST.cc @@ -19,7 +19,7 @@ #define GZ_UNITTEST_EXPECTDATA_ACCESS -#include "utils/TestDataTypes.hh" +#include "test/TestDataTypes.hh" #include "gz/physics/SpecifyData.hh" #include "gz/math/Vector3.hh" diff --git a/src/TestUtilities.hh b/src/TestUtilities.hh deleted file mode 100644 index 75c0c7cad..000000000 --- a/src/TestUtilities.hh +++ /dev/null @@ -1,53 +0,0 @@ -/* - * Copyright (C) 2020 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -#ifndef SRC_GZ_PHYSICS_TESTUTILITIES_HH_ -#define SRC_GZ_PHYSICS_TESTUTILITIES_HH_ - -#include - -#include -#include - -#include - -using namespace gz; -namespace test -{ - class UnimplementedFeature : public virtual physics::Feature - { - public: template - class Implementation : public virtual Feature::Implementation - { - public: virtual void someUnimplementedVirtualFunction() = 0; - - public: ~Implementation() override; - }; - }; -} - -inline void PrimeTheLoader(plugin::Loader &_loader) -{ - for (const std::string &library - : physics::test::g_PhysicsPluginLibraries) - { - if (!library.empty()) - _loader.LoadLib(library); - } -} - -#endif 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/CMakeLists.txt b/test/CMakeLists.txt index 5e1e3e0ec..47dbfe30f 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -19,6 +19,17 @@ ExternalProject_Add( "-DCMAKE_INSTALL_PREFIX=${FAKE_INSTALL_PREFIX}" ) +add_library(gz-physics-test INTERFACE) +target_include_directories(gz-physics-test INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/include) +target_compile_definitions(gz-physics-test INTERFACE + "TESTING_PROJECT_SOURCE_DIR=\"${PROJECT_SOURCE_DIR}\"" +) +target_link_libraries(gz-physics-test INTERFACE + gz-plugin${GZ_PLUGIN_VER}::loader + gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} + gz-common${GZ_COMMON_VER}::testing +) + add_subdirectory(gtest_vendor) add_subdirectory(benchmark) add_subdirectory(common_test) diff --git a/test/benchmark/CMakeLists.txt b/test/benchmark/CMakeLists.txt index 292a4f003..c9e800fde 100644 --- a/test/benchmark/CMakeLists.txt +++ b/test/benchmark/CMakeLists.txt @@ -4,5 +4,4 @@ set(tests ExpectData.cc ) -gz_add_benchmarks(SOURCES ${tests} - INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/src) +gz_add_benchmarks(SOURCES ${tests} LIB_DEPS gz-physics-test) diff --git a/test/benchmark/ExpectData.cc b/test/benchmark/ExpectData.cc index 1a8277276..08c30bc20 100644 --- a/test/benchmark/ExpectData.cc +++ b/test/benchmark/ExpectData.cc @@ -1,6 +1,6 @@ #include -#include "utils/TestDataTypes.hh" +#include "test/TestDataTypes.hh" std::size_t gNumTests = 100000; diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index b8421b48e..3225dced8 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -58,8 +58,7 @@ foreach(test ${tests}) target_link_libraries(${test_executable} PUBLIC - gz-plugin${GZ_PLUGIN_VER}::loader - gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} + gz-physics-test ${PROJECT_LIBRARY_TARGET_NAME} ${PROJECT_LIBRARY_TARGET_NAME}-sdf ${PROJECT_LIBRARY_TARGET_NAME}-mesh @@ -67,10 +66,7 @@ foreach(test ${tests}) gtest_main ) - target_compile_definitions(${test_executable} PRIVATE - "TEST_WORLD_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/worlds/\"" - "GZ_PHYSICS_RESOURCE_DIR=\"${GZ_PHYSICS_RESOURCE_DIR}\"" - ) + target_include_directories(${test_executable} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) if (bullet_version_check_VERSION VERSION_LESS_EQUAL 3.25) target_compile_definitions(${test_executable} PRIVATE diff --git a/test/common_test/Worlds.hh b/test/common_test/Worlds.hh new file mode 100644 index 000000000..d57aa32d3 --- /dev/null +++ b/test/common_test/Worlds.hh @@ -0,0 +1,54 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef COMMON_TEST_WORLDS_WORLDS_HH_ +#define COMMON_TEST_WORLDS_WORLDS_HH_ + +#include + +#include + +inline std::string CommonTestWorld(const std::string &_world) +{ + return gz::common::testing::TestFile("common_test", "worlds", _world); +} + +namespace common_test::worlds +{ +const auto kContactSdf = CommonTestWorld("contact.sdf"); +const auto kDetachableJointWorld = CommonTestWorld("detachable_joint.world"); +const auto kEmptySdf = CommonTestWorld("empty.sdf"); +const auto kFallingWorld = CommonTestWorld("falling.world"); +const auto kFallingAddedMassWorld = CommonTestWorld("falling_added_mass.world"); +const auto kGroundSdf = CommonTestWorld("ground.sdf"); +const auto kJointAcrossModelsSdf = CommonTestWorld("joint_across_models.sdf"); +const auto kJointConstraintSdf = CommonTestWorld("joint_constraint.sdf"); +const auto kMultipleCollisionsSdf = CommonTestWorld("multiple_collisions.sdf"); +const auto kPendulumJointWrenchSdf = + CommonTestWorld("pendulum_joint_wrench.sdf"); +const auto kShapesWorld = CommonTestWorld("shapes.world"); +const auto kShapesBitmaskWorld = CommonTestWorld("shapes_bitmask.sdf"); +const auto kSlipComplianceSdf = CommonTestWorld("slip_compliance.sdf"); +const auto kSphereSdf = CommonTestWorld("sphere.sdf"); +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 kWorldWithNestedModelSdf = + CommonTestWorld("world_with_nested_model.sdf"); +} // namespace common_test::worlds +#endif // COMMON_TEST_WORLDS_WORLDS_HH_ diff --git a/test/common_test/added_mass.cc b/test/common_test/added_mass.cc index f46468bab..c3d5cb902 100644 --- a/test/common_test/added_mass.cc +++ b/test/common_test/added_mass.cc @@ -17,10 +17,12 @@ #include #include + #include -#include "TestLibLoader.hh" -#include "../Utils.hh" +#include "test/TestLibLoader.hh" +#include "test/Utils.hh" +#include "Worlds.hh" #include #include @@ -87,7 +89,7 @@ TEST_F(AddedMassFeaturesTest, Gravity) sdf::Root root; const sdf::Errors errors = root.Load( - gz::common::joinPaths(TEST_WORLD_DIR, "falling_added_mass.world")); + common_test::worlds::kFallingAddedMassWorld); EXPECT_TRUE(errors.empty()) << errors; const sdf::World *sdfWorld = root.WorldByIndex(0); EXPECT_NE(nullptr, sdfWorld); diff --git a/test/common_test/addexternalforcetorque.cc b/test/common_test/addexternalforcetorque.cc index 93409f345..21794458e 100644 --- a/test/common_test/addexternalforcetorque.cc +++ b/test/common_test/addexternalforcetorque.cc @@ -24,8 +24,9 @@ #include #include -#include "TestLibLoader.hh" -#include "../Utils.hh" +#include "test/TestLibLoader.hh" +#include "test/Utils.hh" +#include "Worlds.hh" #include #include @@ -108,7 +109,7 @@ TYPED_TEST(LinkFeaturesTest, JointSetCommand) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "sphere.sdf")); + const sdf::Errors errors = root.Load(common_test::worlds::kSphereSdf); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"sphere"}; diff --git a/test/common_test/basic_test.cc b/test/common_test/basic_test.cc index a3b91d87b..96af1eeb5 100644 --- a/test/common_test/basic_test.cc +++ b/test/common_test/basic_test.cc @@ -19,7 +19,7 @@ #include #include -#include "TestLibLoader.hh" +#include "test/TestLibLoader.hh" #include #include diff --git a/test/common_test/collisions.cc b/test/common_test/collisions.cc index 48189718c..f78495126 100644 --- a/test/common_test/collisions.cc +++ b/test/common_test/collisions.cc @@ -16,10 +16,13 @@ */ #include +#include + #include #include -#include "TestLibLoader.hh" +#include "test/Resources.hh" +#include "test/TestLibLoader.hh" #include #include @@ -35,8 +38,6 @@ #include -// #include - template class CollisionTest: public testing::Test, public gz::physics::TestLibLoader @@ -100,8 +101,7 @@ TYPED_TEST(CollisionTest, MeshAndPlane) auto model = world->ConstructEmptyModel("mesh"); auto link = model->ConstructEmptyLink("link"); - const std::string meshFilename = gz::common::joinPaths( - GZ_PHYSICS_RESOURCE_DIR, "chassis.dae"); + const std::string meshFilename = gz::physics::test::resources::kChassisDae; auto &meshManager = *gz::common::MeshManager::Instance(); auto *mesh = meshManager.Load(meshFilename); diff --git a/test/common_test/construct_empty_world.cc b/test/common_test/construct_empty_world.cc index a2974aaba..f454b9ee4 100644 --- a/test/common_test/construct_empty_world.cc +++ b/test/common_test/construct_empty_world.cc @@ -19,7 +19,7 @@ #include #include -#include "TestLibLoader.hh" +#include "test/TestLibLoader.hh" #include #include diff --git a/test/common_test/detachable_joint.cc b/test/common_test/detachable_joint.cc index ab06e6bb9..fad71648d 100644 --- a/test/common_test/detachable_joint.cc +++ b/test/common_test/detachable_joint.cc @@ -24,8 +24,9 @@ #include #include -#include "TestLibLoader.hh" -#include "../Utils.hh" +#include "test/TestLibLoader.hh" +#include "test/Utils.hh" +#include "Worlds.hh" #include "gz/physics/FrameSemantics.hh" #include @@ -111,7 +112,7 @@ TYPED_TEST(DetachableJointTest, CorrectAttachmentPoints) sdf::Root root; const sdf::Errors errors = root.Load( - gz::common::joinPaths(TEST_WORLD_DIR, "detachable_joint.world")); + common_test::worlds::kDetachableJointWorld); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); diff --git a/test/common_test/free_joint_features.cc b/test/common_test/free_joint_features.cc index 5c97e0559..0b1b93d71 100644 --- a/test/common_test/free_joint_features.cc +++ b/test/common_test/free_joint_features.cc @@ -18,7 +18,8 @@ #include -#include "TestLibLoader.hh" +#include "test/TestLibLoader.hh" +#include "Worlds.hh" #include #include @@ -36,7 +37,6 @@ #include "gz/physics/sdf/ConstructModel.hh" #include "gz/physics/sdf/ConstructNestedModel.hh" #include "gz/physics/sdf/ConstructWorld.hh" -// #include "test/Utils.hh" struct TestFeatureList : gz::physics::FeatureList< gz::physics::GetEngineInfo, @@ -112,7 +112,8 @@ TEST_F(FreeGroupFeaturesTest, NestedFreeGroup) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors &errors = root.Load(TEST_WORLD_DIR "/world_with_nested_model.sdf"); + const sdf::Errors &errors = root.Load( + common_test::worlds::kWorldWithNestedModelSdf); EXPECT_EQ(0u, errors.size()) << errors; EXPECT_EQ(1u, root.WorldCount()); @@ -163,7 +164,8 @@ TEST_F(FreeGroupFeaturesTest, NestedFreeGroupSetWorldPose) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors &errors = root.Load(TEST_WORLD_DIR "/world_with_nested_model.sdf"); + const sdf::Errors &errors = root.Load( + common_test::worlds::kWorldWithNestedModelSdf); EXPECT_EQ(0u, errors.size()) << errors; EXPECT_EQ(1u, root.WorldCount()); diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 2ceb48b83..5f0a695b6 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -24,8 +24,9 @@ #include #include -#include "TestLibLoader.hh" -#include "../Utils.hh" +#include "test/TestLibLoader.hh" +#include "test/Utils.hh" +#include "Worlds.hh" #include "gz/physics/FrameSemantics.hh" #include @@ -115,7 +116,7 @@ TYPED_TEST(JointFeaturesTest, JointSetCommand) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kTestWorld); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"double_pendulum_with_base"}; @@ -225,7 +226,7 @@ TYPED_TEST(JointFeaturesPositionLimitsTest, JointSetPositionLimitsWithForceContr ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kTestWorld); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -331,7 +332,7 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetVelocityLimitsWi ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kTestWorld); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -418,7 +419,7 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetEffortLimitsWith ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kTestWorld); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -495,7 +496,7 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetCombinedLimitsWi ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kTestWorld); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -603,7 +604,7 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, DISABLED_JointSetPositio ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kTestWorld); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"simple_joint_test"}; @@ -655,7 +656,7 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetVelocityLimitsWi ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kTestWorld); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -726,7 +727,7 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetEffortLimitsWith ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kTestWorld); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -786,7 +787,7 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetCombinedLimitsWi ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kTestWorld); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -861,7 +862,7 @@ TYPED_TEST(JointFeaturesDetachTest, JointDetach) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kTestWorld); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"double_pendulum_with_base"}; @@ -1022,7 +1023,7 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetach) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "joint_across_models.sdf")); + const sdf::Errors errors = root.Load(common_test::worlds::kJointAcrossModelsSdf); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -1166,7 +1167,7 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "joint_constraint.sdf")); + const sdf::Errors errors = root.Load(common_test::worlds::kJointConstraintSdf); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -1315,7 +1316,7 @@ TYPED_TEST(JointFeaturesAttachDetachTest, LinkCountsInJointAttachDetach) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "joint_across_models.sdf")); + const sdf::Errors errors = root.Load(common_test::worlds::kJointAcrossModelsSdf); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -1412,7 +1413,7 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachSpawnedModel) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "ground.sdf")); + const sdf::Errors errors = root.Load(common_test::worlds::kGroundSdf); ASSERT_TRUE(errors.empty()) << errors.front(); world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -1510,8 +1511,7 @@ class WorldModelTest : public JointFeaturesTest gz::physics::RequestEngine3d::From(plugin); sdf::Root root; - const sdf::Errors errors = root.Load( - gz::common::joinPaths(TEST_WORLD_DIR, "world_joint_test.sdf")); + const sdf::Errors errors = root.Load(common_test::worlds::kWorldJointTestSdf); EXPECT_TRUE(errors.empty()) << errors; if (errors.empty()) { diff --git a/test/common_test/joint_transmitted_wrench_features.cc b/test/common_test/joint_transmitted_wrench_features.cc index 21fb52576..be558361a 100644 --- a/test/common_test/joint_transmitted_wrench_features.cc +++ b/test/common_test/joint_transmitted_wrench_features.cc @@ -24,8 +24,9 @@ #include #include -#include "TestLibLoader.hh" -#include "../Utils.hh" +#include "test/TestLibLoader.hh" +#include "test/Utils.hh" +#include "Worlds.hh" #include "gz/physics/FrameSemantics.hh" #include @@ -102,7 +103,7 @@ class JointTransmittedWrenchFixture : ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "pendulum_joint_wrench.sdf")); + const sdf::Errors errors = root.Load(common_test::worlds::kPendulumJointWrenchSdf); ASSERT_TRUE(errors.empty()) << errors.front(); this->world = engine->ConstructWorld(*root.WorldByIndex(0)); diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index c479800a7..effa773dc 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -19,8 +19,9 @@ #include #include -#include "TestLibLoader.hh" -#include "../Utils.hh" +#include "test/TestLibLoader.hh" +#include "test/Utils.hh" +#include "Worlds.hh" #include #include @@ -94,8 +95,8 @@ TYPED_TEST(KinematicFeaturesTest, JointFrameSemantics) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = - root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "string_pendulum.sdf")); + const sdf::Errors errors = root.Load( + common_test::worlds::kStringPendulumSdf); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); diff --git a/test/common_test/link_features.cc b/test/common_test/link_features.cc index 8b36b6424..adcb54817 100644 --- a/test/common_test/link_features.cc +++ b/test/common_test/link_features.cc @@ -24,8 +24,9 @@ #include #include -#include "TestLibLoader.hh" -#include "../Utils.hh" +#include "test/TestLibLoader.hh" +#include "test/Utils.hh" +#include "Worlds.hh" #include #include @@ -105,7 +106,7 @@ TYPED_TEST(LinkFeaturesTest, JointSetCommand) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "empty.sdf")); + const sdf::Errors errors = root.Load(common_test::worlds::kEmptySdf); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"double_pendulum_with_base"}; @@ -303,7 +304,7 @@ TYPED_TEST(LinkFeaturesTest, AxisAlignedBoundingBox) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kTestWorld); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"double_pendulum_with_base"}; @@ -354,7 +355,7 @@ TYPED_TEST(LinkFeaturesTest, ModelAxisAlignedBoundingBox) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "contact.sdf")); + const sdf::Errors errors = root.Load(common_test::worlds::kContactSdf); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"double_pendulum_with_base"}; diff --git a/test/common_test/shape_features.cc b/test/common_test/shape_features.cc index c2de4cf96..596c1f717 100644 --- a/test/common_test/shape_features.cc +++ b/test/common_test/shape_features.cc @@ -19,8 +19,9 @@ #include #include -#include "TestLibLoader.hh" -#include "../Utils.hh" +#include "test/TestLibLoader.hh" +#include "test/Utils.hh" +#include "Worlds.hh" // Features #include @@ -103,7 +104,8 @@ TYPED_TEST(ShapeFeaturesTest, PrimarySlipCompliance) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "slip_compliance.sdf")); + const sdf::Errors errors = root.Load( + common_test::worlds::kSlipComplianceSdf); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"box"}; @@ -175,7 +177,8 @@ TYPED_TEST(ShapeFeaturesTest, SecondarySlipCompliance) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "slip_compliance.sdf")); + const sdf::Errors errors = root.Load( + common_test::worlds::kSlipComplianceSdf); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"box"}; diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index 479ee87b3..d92e474f9 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -25,8 +25,9 @@ #include -#include "TestLibLoader.hh" -#include "../Utils.hh" +#include "test/TestLibLoader.hh" +#include "test/Utils.hh" +#include "Worlds.hh" #include #include @@ -49,6 +50,7 @@ #include #include #include +#include #include @@ -209,16 +211,71 @@ TYPED_TEST(SimulationFeaturesContactsTest, Contacts) auto world = LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); + common_test::worlds::kShapesWorld); auto checkedOutput = StepWorld(world, true, 1).first; EXPECT_TRUE(checkedOutput); auto contacts = world->GetContactsFromLastStep(); - // Only box_colliding should collide with box_base + // Large box collides with other shapes EXPECT_NE(0u, contacts.size()); } } +// 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::ForwardStep, + gz::physics::CollisionPairMaxContacts +> {}; + +template +class SimulationFeaturesCollisionPairMaxContactsTest : + public SimulationFeaturesTest{}; +using SimulationFeaturesCollisionPairMaxContactsTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(SimulationFeaturesCollisionPairMaxContactsTest, + SimulationFeaturesCollisionPairMaxContactsTestTypes); + +///////////////////////////////////////////////// +TYPED_TEST(SimulationFeaturesCollisionPairMaxContactsTest, + CollisionPairMaxContacts) +{ + for (const std::string &name : this->pluginNames) + { + auto world = LoadPluginAndWorld( + this->loader, + name, + common_test::worlds::kShapesWorld); + auto checkedOutput = StepWorld( + world, true, 1).first; + EXPECT_TRUE(checkedOutput); + + auto contacts = world->GetContactsFromLastStep(); + EXPECT_EQ(std::numeric_limits::max(), + world->GetCollisionPairMaxContacts()); + // Large box collides with other shapes + EXPECT_GT(contacts.size(), 30u); + + world->SetCollisionPairMaxContacts(1u); + EXPECT_EQ(1u, world->GetCollisionPairMaxContacts()); + checkedOutput = StepWorld( + world, true, 1).first; + EXPECT_TRUE(checkedOutput); + + contacts = world->GetContactsFromLastStep(); + EXPECT_EQ(4u, contacts.size()); + + world->SetCollisionPairMaxContacts(0u); + EXPECT_EQ(0u, world->GetCollisionPairMaxContacts()); + checkedOutput = StepWorld( + world, true, 1).first; + EXPECT_TRUE(checkedOutput); + + contacts = world->GetContactsFromLastStep(); + EXPECT_EQ(0u, contacts.size()); + } +} // The features that an engine must have to be loaded by this loader. struct FeaturesStep : gz::physics::FeatureList< @@ -246,7 +303,7 @@ TYPED_TEST(SimulationFeaturesStepTest, StepWorld) auto world = LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); + common_test::worlds::kShapesWorld); auto checkedOutput = StepWorld(world, true, 1000).first; EXPECT_TRUE(checkedOutput); } @@ -279,10 +336,9 @@ TYPED_TEST(SimulationFeaturesFallingTest, Falling) // See https://github.com/gazebosim/gz-physics/issues/483 CHECK_UNSUPPORTED_ENGINE(name, "bullet") #endif - auto world = LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "falling.world")); + common_test::worlds::kFallingWorld); auto [checkedOutput, output] = StepWorld(world, true, 1000); @@ -345,7 +401,7 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) #endif auto world = LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); + common_test::worlds::kShapesWorld); std::cerr << "world model count " << world->GetModelCount() << '\n'; // test ShapeFeatures auto sphere = world->GetModel("sphere"); @@ -524,7 +580,7 @@ TYPED_TEST(SimulationFeaturesTestFreeGroup, FreeGroup) auto world = LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "sphere.sdf")); + common_test::worlds::kSphereSdf); // model free group test auto model = world->GetModel("sphere"); @@ -612,7 +668,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, ShapeBoundingBox) auto world = LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "falling.world")); + common_test::worlds::kFallingWorld); auto sphere = world->GetModel("sphere"); auto sphereCollision = sphere->GetLink(0)->GetShape(0); auto ground = world->GetModel("box"); @@ -661,7 +717,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, CollideBitmasks) auto world = LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "shapes_bitmask.sdf")); + common_test::worlds::kShapesBitmaskWorld); auto baseBox = world->GetModel("box_base"); auto filteredBox = world->GetModel("box_filtered"); @@ -706,7 +762,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); + common_test::worlds::kShapesWorld); auto sphere = world->GetModel("sphere"); auto sphereFreeGroup = sphere->FindFreeGroup(); @@ -918,7 +974,7 @@ TYPED_TEST(SimulationFeaturesTestFeaturesContactPropertiesCallback, ContactPrope LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "contact.sdf")); + common_test::worlds::kContactSdf); auto sphere = world->GetModel("sphere"); auto groundPlane = world->GetModel("ground_plane"); @@ -1148,7 +1204,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, MultipleCollisions) auto world = LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "multiple_collisions.sdf")); + common_test::worlds::kMultipleCollisionsSdf); // model free group test auto model = world->GetModel("box"); diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc index 6c08ddb98..ad28f8bbb 100644 --- a/test/common_test/world_features.cc +++ b/test/common_test/world_features.cc @@ -19,8 +19,9 @@ #include #include -#include "TestLibLoader.hh" -#include "../Utils.hh" +#include "test/TestLibLoader.hh" +#include "test/Utils.hh" +#include "Worlds.hh" #include #include @@ -93,8 +94,7 @@ TEST_F(WorldFeaturesTestGravity, GravityFeatures) std::string::npos); sdf::Root root; - const sdf::Errors errors = root.Load( - gz::common::joinPaths(TEST_WORLD_DIR, "falling.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kFallingWorld); EXPECT_TRUE(errors.empty()) << errors; const sdf::World *sdfWorld = root.WorldByIndex(0); EXPECT_NE(nullptr, sdfWorld); @@ -206,7 +206,8 @@ TEST_F(WorldFeaturesTestConstructModel, ConstructModelUnsortedLinks) sdf::Root root; const sdf::Errors errors = root.Load( - gz::common::joinPaths(TEST_WORLD_DIR, "world_unsorted_links.sdf")); + common_test::worlds::kWorldUnsortedLinksSdf); + EXPECT_TRUE(errors.empty()) << errors; const sdf::World *sdfWorld = root.WorldByIndex(0); ASSERT_NE(nullptr, sdfWorld); @@ -245,7 +246,7 @@ class WorldModelTest : public WorldFeaturesTest sdf::Root root; const sdf::Errors errors = root.Load( - gz::common::joinPaths(TEST_WORLD_DIR, "world_joint_test.sdf")); + common_test::worlds::kWorldJointTestSdf); EXPECT_TRUE(errors.empty()) << errors; if (errors.empty()) { diff --git a/test/MockCenterOfMass.hh b/test/include/mock/MockCenterOfMass.hh similarity index 100% rename from test/MockCenterOfMass.hh rename to test/include/mock/MockCenterOfMass.hh diff --git a/test/MockCreateEntities.hh b/test/include/mock/MockCreateEntities.hh similarity index 100% rename from test/MockCreateEntities.hh rename to test/include/mock/MockCreateEntities.hh diff --git a/test/MockDoublePendulum.hh b/test/include/mock/MockDoublePendulum.hh similarity index 100% rename from test/MockDoublePendulum.hh rename to test/include/mock/MockDoublePendulum.hh diff --git a/test/MockFeatures.hh b/test/include/mock/MockFeatures.hh similarity index 100% rename from test/MockFeatures.hh rename to test/include/mock/MockFeatures.hh diff --git a/test/MockFrameSemantics.hh b/test/include/mock/MockFrameSemantics.hh similarity index 100% rename from test/MockFrameSemantics.hh rename to test/include/mock/MockFrameSemantics.hh diff --git a/test/MockGetByName.hh b/test/include/mock/MockGetByName.hh similarity index 100% rename from test/MockGetByName.hh rename to test/include/mock/MockGetByName.hh diff --git a/test/MockJoints.hh b/test/include/mock/MockJoints.hh similarity index 100% rename from test/MockJoints.hh rename to test/include/mock/MockJoints.hh diff --git a/test/MockSetName.hh b/test/include/mock/MockSetName.hh similarity index 100% rename from test/MockSetName.hh rename to test/include/mock/MockSetName.hh diff --git a/test/PhysicsPluginsList.hh b/test/include/test/PhysicsPluginsList.hh similarity index 100% rename from test/PhysicsPluginsList.hh rename to test/include/test/PhysicsPluginsList.hh diff --git a/test/include/test/Resources.hh b/test/include/test/Resources.hh new file mode 100644 index 000000000..8b2eff9c7 --- /dev/null +++ b/test/include/test/Resources.hh @@ -0,0 +1,38 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef TEST_RESOURCES_HH_ +#define TEST_RESOURCES_HH_ + +#include + +#include + +inline std::string TestResource(const std::string &_filename) +{ + return gz::common::testing::TestFile("resources", _filename); +} + +namespace gz::physics::test::resources +{ +const auto kChassisDae = TestResource("chassis.dae"); +const auto kHeightmapBowlPng = TestResource("heightmap_bowl.png"); +const auto kRrbotXml = TestResource("rrbot.xml"); +const auto kVolcanoTif = TestResource("volcano.tif"); +} // namespace gz::physics::test + +#endif // TEST_RESOURCES_HH_ diff --git a/src/utils/TestDataTypes.hh b/test/include/test/TestDataTypes.hh similarity index 100% rename from src/utils/TestDataTypes.hh rename to test/include/test/TestDataTypes.hh diff --git a/test/common_test/TestLibLoader.hh b/test/include/test/TestLibLoader.hh similarity index 85% rename from test/common_test/TestLibLoader.hh rename to test/include/test/TestLibLoader.hh index 5d5910eea..b07d05528 100644 --- a/test/common_test/TestLibLoader.hh +++ b/test/include/test/TestLibLoader.hh @@ -24,6 +24,9 @@ #include #include +#include + +#include namespace gz { @@ -38,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/test/Utils.hh b/test/include/test/Utils.hh similarity index 91% rename from test/Utils.hh rename to test/include/test/Utils.hh index 5755c6618..81d865ef2 100644 --- a/test/Utils.hh +++ b/test/include/test/Utils.hh @@ -23,9 +23,37 @@ #include #include #include +#include + +#include + +#include + +///////////////////////////////////////////////// +inline void PrimeTheLoader(gz::plugin::Loader &_loader) +{ + for (const std::string &library + : gz::physics::test::g_PhysicsPluginLibraries) + { + if (!library.empty()) + _loader.LoadLib(library); + } +} namespace gz::physics::test { +///////////////////////////////////////////////// +class UnimplementedFeature : public virtual gz::physics::Feature +{ + public: template + class Implementation : public virtual gz::physics::Feature::Implementation + { + public: virtual void someUnimplementedVirtualFunction() = 0; + + public: ~Implementation() override; + }; +}; + ///////////////////////////////////////////////// template VectorType RandomVector(const double range) diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 6cf8c9eb6..8c0b32183 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -11,12 +11,11 @@ gz_build_tests( SOURCES ${tests} LIB_DEPS Eigen3::Eigen - gz-plugin${GZ_PLUGIN_VER}::loader - INCLUDE_DIRS - ${PROJECT_SOURCE_DIR}/src + gz-physics-test TEST_LIST list ENVIRONMENT - GZ_PHYSICS_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}) + GZ_PHYSICS_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} + TEST_LIST list) if (BUILD_TESTING) foreach(test ${list}) diff --git a/test/integration/CanReadWrite.cc b/test/integration/CanReadWrite.cc index bc9583ef6..1385c08e6 100644 --- a/test/integration/CanReadWrite.cc +++ b/test/integration/CanReadWrite.cc @@ -23,7 +23,7 @@ #include "gz/physics/CanReadData.hh" #include "gz/physics/CanWriteData.hh" -#include "utils/TestDataTypes.hh" +#include "test/TestDataTypes.hh" using gz::physics::CanReadRequiredData; using gz::physics::CanReadExpectedData; diff --git a/test/integration/DoublePendulum.cc b/test/integration/DoublePendulum.cc index 329d1e5e2..482e2e137 100644 --- a/test/integration/DoublePendulum.cc +++ b/test/integration/DoublePendulum.cc @@ -26,7 +26,7 @@ #include #include -#include "../MockDoublePendulum.hh" +#include "mock/MockDoublePendulum.hh" using namespace gz::physics; diff --git a/test/integration/FeatureSystem.cc b/test/integration/FeatureSystem.cc index 48fd3b453..46ee7a74f 100644 --- a/test/integration/FeatureSystem.cc +++ b/test/integration/FeatureSystem.cc @@ -20,7 +20,7 @@ #include #include -#include "../MockFeatures.hh" +#include "mock/MockFeatures.hh" using namespace gz::physics; diff --git a/test/integration/FrameSemantics.hh b/test/integration/FrameSemantics.hh index 57de63556..093776189 100644 --- a/test/integration/FrameSemantics.hh +++ b/test/integration/FrameSemantics.hh @@ -27,8 +27,8 @@ #include #include -#include "../Utils.hh" -#include "../MockFrameSemantics.hh" +#include "test/Utils.hh" +#include "mock/MockFrameSemantics.hh" using gz::physics::FrameData; using gz::physics::FrameID; diff --git a/test/integration/JointTypes.hh b/test/integration/JointTypes.hh index d658604c9..dc4f730f1 100644 --- a/test/integration/JointTypes.hh +++ b/test/integration/JointTypes.hh @@ -29,8 +29,8 @@ #include #include -#include "../MockJoints.hh" -#include "../Utils.hh" +#include "mock/MockJoints.hh" +#include "test/Utils.hh" using namespace gz::physics; using namespace gz::physics::test; diff --git a/test/integration/RequestFeatures.cc b/test/integration/RequestFeatures.cc index 7e0512cf0..81f70c5fc 100644 --- a/test/integration/RequestFeatures.cc +++ b/test/integration/RequestFeatures.cc @@ -29,8 +29,8 @@ #include -#include "../../src/TestUtilities.hh" -#include "../MockFeatures.hh" +#include "test/Utils.hh" +#include "mock/MockFeatures.hh" TEST(RequestFeatures_TEST, Casting) { @@ -48,7 +48,7 @@ TEST(RequestFeatures_TEST, Casting) mock::MockCenterOfMass>; using UnavailableFeatures = - gz::physics::FeatureList; + gz::physics::FeatureList; // Get a list of all plugins who satisfy the features that are needed for // testing diff --git a/test/performance/CMakeLists.txt b/test/performance/CMakeLists.txt index 6a1627d67..dcb1ae5f7 100644 --- a/test/performance/CMakeLists.txt +++ b/test/performance/CMakeLists.txt @@ -10,8 +10,8 @@ endif() gz_build_tests( TYPE PERFORMANCE SOURCES ${tests} - INCLUDE_DIRS - ${PROJECT_SOURCE_DIR}/src ENVIRONMENT GZ_PHYSICS_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} + LIB_DEPS + gz-physics-test ) diff --git a/test/performance/ExpectData.cc b/test/performance/ExpectData.cc index 1c1842343..41ff92eca 100644 --- a/test/performance/ExpectData.cc +++ b/test/performance/ExpectData.cc @@ -21,7 +21,7 @@ #include #include -#include "utils/TestDataTypes.hh" +#include "test/TestDataTypes.hh" std::size_t gNumTests = 100000; diff --git a/test/plugins/CMakeLists.txt b/test/plugins/CMakeLists.txt index 6d76ac351..92c038744 100644 --- a/test/plugins/CMakeLists.txt +++ b/test/plugins/CMakeLists.txt @@ -17,6 +17,7 @@ endif() foreach(plugin IN LISTS plugins) target_link_libraries(${plugin} PRIVATE + gz-physics-test ${PROJECT_LIBRARY_TARGET_NAME}) endforeach() @@ -27,8 +28,6 @@ if (DART_FOUND) set(DART_LIBRARIES_NO_BULLET ${DART_LIBRARIES}) list(REMOVE_ITEM DART_LIBRARIES_NO_BULLET dart-collision-bullet) target_link_libraries(MockDoublePendulum PUBLIC ${DART_LIBRARIES_NO_BULLET}) - target_compile_definitions(MockDoublePendulum PRIVATE - "GZ_PHYSICS_RESOURCE_DIR=\"${GZ_PHYSICS_RESOURCE_DIR}\"") if (MSVC) # needed by DART, see https://github.com/dartsim/dart/issues/753 target_compile_options(MockDoublePendulum PUBLIC "/permissive-") diff --git a/test/plugins/DARTDoublePendulum.cc b/test/plugins/DARTDoublePendulum.cc index a5a0f9836..d18f8684a 100644 --- a/test/plugins/DARTDoublePendulum.cc +++ b/test/plugins/DARTDoublePendulum.cc @@ -26,7 +26,8 @@ #include #include -#include "../MockDoublePendulum.hh" +#include "mock/MockDoublePendulum.hh" +#include "test/Resources.hh" namespace mock { @@ -65,7 +66,8 @@ namespace mock lastId(0) { ::dart::utils::DartLoader loader; - this->robot = loader.parseSkeleton(GZ_PHYSICS_RESOURCE_DIR "/rrbot.xml"); + this->robot = loader.parseSkeleton( + gz::physics::test::resources::kRrbotXml); this->world->addSkeleton(this->robot); this->joint1 = this->robot->getJoint("joint1"); diff --git a/test/plugins/MockEntities.cc b/test/plugins/MockEntities.cc index 20bd674a6..7a9bd8c96 100644 --- a/test/plugins/MockEntities.cc +++ b/test/plugins/MockEntities.cc @@ -15,7 +15,7 @@ * */ -#include "../MockFeatures.hh" +#include "mock/MockFeatures.hh" #include diff --git a/test/plugins/MockJoints.cc b/test/plugins/MockJoints.cc index e80e671ce..54dd3b06c 100644 --- a/test/plugins/MockJoints.cc +++ b/test/plugins/MockJoints.cc @@ -20,7 +20,7 @@ #include -#include "../MockJoints.hh" +#include "mock/MockJoints.hh" namespace mock { diff --git a/test/plugins/frames.cc b/test/plugins/frames.cc index 1eb6e6fb0..5b02e3831 100644 --- a/test/plugins/frames.cc +++ b/test/plugins/frames.cc @@ -15,7 +15,7 @@ * */ -#include "../MockFrameSemantics.hh" +#include "mock/MockFrameSemantics.hh" #include #include diff --git a/resources/chassis.dae b/test/resources/chassis.dae similarity index 100% rename from resources/chassis.dae rename to test/resources/chassis.dae diff --git a/resources/heightmap_bowl.png b/test/resources/heightmap_bowl.png similarity index 100% rename from resources/heightmap_bowl.png rename to test/resources/heightmap_bowl.png diff --git a/resources/rrbot.xml b/test/resources/rrbot.xml similarity index 100% rename from resources/rrbot.xml rename to test/resources/rrbot.xml diff --git a/resources/volcano.tif b/test/resources/volcano.tif similarity index 100% rename from resources/volcano.tif rename to test/resources/volcano.tif diff --git a/tpe/plugin/CMakeLists.txt b/tpe/plugin/CMakeLists.txt index 3d73d6828..573fe8adb 100644 --- a/tpe/plugin/CMakeLists.txt +++ b/tpe/plugin/CMakeLists.txt @@ -55,20 +55,17 @@ gz_build_tests( SOURCES ${test_sources} LIB_DEPS ${features} - gz-plugin${GZ_PLUGIN_VER}::loader + gz-physics-test gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} ${PROJECT_LIBRARY_TARGET_NAME}-sdf ${PROJECT_LIBRARY_TARGET_NAME}-mesh TEST_LIST tests ENVIRONMENT - GZ_PHYSICS_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}) + GZ_PHYSICS_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} + INCLUDE_DIRS + ${tpelib_dir}) foreach(test ${tests}) - - target_include_directories(${test} PRIVATE ${tpelib_dir}) target_compile_definitions(${test} PRIVATE - "tpe_plugin_LIB=\"$\"" - "TEST_WORLD_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/worlds/\"" - "GZ_PHYSICS_RESOURCE_DIR=\"${GZ_PHYSICS_RESOURCE_DIR}\"") - + "tpe_plugin_LIB=\"$\"") endforeach() diff --git a/tpe/plugin/src/SDFFeatures_TEST.cc b/tpe/plugin/src/SDFFeatures_TEST.cc index b9b592d86..ec92eb380 100644 --- a/tpe/plugin/src/SDFFeatures_TEST.cc +++ b/tpe/plugin/src/SDFFeatures_TEST.cc @@ -23,6 +23,7 @@ #include #include +#include #include @@ -90,11 +91,11 @@ World LoadWorld(const std::string &_world) // Test that the tpe plugin loaded all the relevant information correctly. TEST(SDFFeatures_TEST, CheckTpeData) { - World world = LoadWorld(TEST_WORLD_DIR"/test.world"); + auto world = LoadWorld(common_test::worlds::kTestWorld); auto tpeWorld = world.GetTpeLibWorld(); ASSERT_NE(nullptr, tpeWorld); - ASSERT_EQ(6u, tpeWorld->GetChildCount()); + ASSERT_EQ(7u, tpeWorld->GetChildCount()); // check model 01 { @@ -253,7 +254,13 @@ TEST(SDFFeatures_TEST, CheckTpeData) link.GetId()); EXPECT_EQ("link", link.GetName()); EXPECT_EQ(math::Pose3d::Zero, link.GetPose()); - EXPECT_EQ(0u, link.GetChildCount()); + EXPECT_EQ(1u, link.GetChildCount()); + + auto &collision = link.GetChildByName("collision1"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), + collision.GetId()); + EXPECT_EQ("collision1", collision.GetName()); + EXPECT_EQ(0u, collision.GetChildCount()); } // check model 04 @@ -363,26 +370,26 @@ TEST(SDFFeatures_TEST, CheckTpeData) // Test that the tpe plugin loaded nested models correctly. TEST(SDFFeatures_TEST, NestedModel) { - World world = LoadWorld(TEST_WORLD_DIR"/nested_model.world"); + auto world = LoadWorld(common_test::worlds::kWorldWithNestedModelSdf); auto tpeWorld = world.GetTpeLibWorld(); ASSERT_NE(nullptr, tpeWorld); - ASSERT_EQ(1u, tpeWorld->GetChildCount()); - EXPECT_EQ(1u, world.GetModelCount()); + ASSERT_EQ(2u, tpeWorld->GetChildCount()); + EXPECT_EQ(2u, world.GetModelCount()); // check top level model physics::tpelib::Entity &model = - tpeWorld->GetChildByName("model"); + tpeWorld->GetChildByName("parent_model"); ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), model.GetId()); - EXPECT_EQ("model", model.GetName()); + EXPECT_EQ("parent_model", model.GetName()); EXPECT_EQ(math::Pose3d::Zero, model.GetPose()); - EXPECT_EQ(2u, model.GetChildCount()); + EXPECT_EQ(4u, model.GetChildCount()); - physics::tpelib::Entity &link = model.GetChildByName("link"); + physics::tpelib::Entity &link = model.GetChildByName("link1"); ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link.GetId()); - EXPECT_EQ("link", link.GetName()); + EXPECT_EQ("link1", link.GetName()); EXPECT_EQ(math::Pose3d::Zero, link.GetPose()); EXPECT_EQ(1u, link.GetChildCount()); @@ -400,27 +407,26 @@ TEST(SDFFeatures_TEST, NestedModel) nestedModel.GetId()); EXPECT_EQ("nested_model", nestedModel.GetName()); EXPECT_EQ(math::Pose3d(1, 2, 2, 0, 0, 0), nestedModel.GetPose()); - EXPECT_EQ(1u, nestedModel.GetChildCount()); + EXPECT_EQ(2u, nestedModel.GetChildCount()); physics::tpelib::Entity &nestedLink = - nestedModel.GetChildByName("nested_link"); + nestedModel.GetChildByName("nested_link1"); ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), nestedLink.GetId()); - EXPECT_EQ("nested_link", nestedLink.GetName()); + EXPECT_EQ("nested_link1", nestedLink.GetName()); EXPECT_EQ(math::Pose3d(3, 1, 1, 0, 0, 1.5707), nestedLink.GetPose()); EXPECT_EQ(1u, nestedLink.GetChildCount()); physics::tpelib::Entity &nestedCollision = - nestedLink.GetChildByName("nested_collision"); + nestedLink.GetChildByName("nested_collision1"); ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), nestedCollision.GetId()); - EXPECT_EQ("nested_collision", nestedCollision.GetName()); + EXPECT_EQ("nested_collision1", nestedCollision.GetName()); EXPECT_EQ(math::Pose3d::Zero, nestedCollision.GetPose()); // canonical link - physics::tpelib::Model *m = - static_cast(&model); + auto *m = dynamic_cast(&model); physics::tpelib::Entity canLink = m->GetCanonicalLink(); EXPECT_EQ(link.GetId(), canLink.GetId()); } diff --git a/tpe/plugin/src/SimulationFeatures_TEST.cc b/tpe/plugin/src/SimulationFeatures_TEST.cc index 7fa3de841..25fff9ecc 100644 --- a/tpe/plugin/src/SimulationFeatures_TEST.cc +++ b/tpe/plugin/src/SimulationFeatures_TEST.cc @@ -18,6 +18,7 @@ #include #include + #include #include @@ -40,6 +41,7 @@ #include #include +#include #include #include @@ -49,6 +51,7 @@ #include "SimulationFeatures.hh" #include "World.hh" + using namespace gz; struct TestFeatureList : physics::FeatureList< @@ -273,7 +276,7 @@ TEST_P(SimulationFeatures_TEST, StepWorld) return; gzdbg << "Testing library " << library << std::endl; - auto worlds = LoadWorlds(library, TEST_WORLD_DIR "/shapes.world"); + const auto worlds = LoadWorlds(library, common_test::worlds::kShapesWorld); for (const auto &world : worlds) { @@ -296,7 +299,7 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures) if (library.empty()) return; - auto worlds = LoadWorlds(library, TEST_WORLD_DIR "/shapes.world"); + const auto worlds = LoadWorlds(library, common_test::worlds::kShapesWorld); for (const auto &world : worlds) { @@ -435,7 +438,7 @@ TEST_P(SimulationFeatures_TEST, FreeGroup) if (library.empty()) return; - auto worlds = LoadWorlds(library, TEST_WORLD_DIR "/shapes.world"); + const auto worlds = LoadWorlds(library, common_test::worlds::kShapesWorld); for (const auto &world : worlds) { @@ -483,8 +486,8 @@ TEST_P(SimulationFeatures_TEST, NestedFreeGroup) if (library.empty()) return; - auto worlds = - LoadWorldsPiecemeal(library, TEST_WORLD_DIR "/nested_model.world"); + const auto worlds = + LoadWorlds(library, common_test::worlds::kWorldWithNestedModelSdf); for (const auto &world : worlds) { @@ -492,7 +495,7 @@ TEST_P(SimulationFeatures_TEST, NestedFreeGroup) ASSERT_NE(nullptr, tpeWorld); // model free group test - auto model = world->GetModel("model"); + auto model = world->GetModel("parent_model"); ASSERT_NE(nullptr, model); auto freeGroup = model->FindFreeGroup(); ASSERT_NE(nullptr, freeGroup); @@ -502,7 +505,7 @@ TEST_P(SimulationFeatures_TEST, NestedFreeGroup) freeGroup->SetWorldPose(math::eigen3::convert(newPose)); { - auto link = model->GetLink("link"); + auto link = model->GetLink("link1"); ASSERT_NE(nullptr, link); auto frameData = link->FrameDataRelativeToWorld(); EXPECT_EQ(newPose, @@ -511,7 +514,7 @@ TEST_P(SimulationFeatures_TEST, NestedFreeGroup) { auto nestedModel = model->GetNestedModel("nested_model"); ASSERT_NE(nullptr, nestedModel); - auto nestedLink = nestedModel->GetLink("nested_link"); + auto nestedLink = nestedModel->GetLink("nested_link1"); ASSERT_NE(nullptr, nestedLink); // Poses from SDF @@ -534,7 +537,8 @@ TEST_P(SimulationFeatures_TEST, CollideBitmasks) if (library.empty()) return; - auto worlds = LoadWorlds(library, TEST_WORLD_DIR "/shapes_bitmask.sdf"); + const auto worlds = + LoadWorlds(library, common_test::worlds::kShapesBitmaskWorld); for (const auto &world : worlds) { @@ -578,7 +582,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) if (library.empty()) return; - auto worlds = LoadWorlds(library, TEST_WORLD_DIR "/shapes.world"); + const auto worlds = LoadWorlds(library, common_test::worlds::kShapesWorld); for (const auto &world : worlds) { diff --git a/tpe/plugin/worlds/nested_model.world b/tpe/plugin/worlds/nested_model.world deleted file mode 100644 index a2182472a..000000000 --- a/tpe/plugin/worlds/nested_model.world +++ /dev/null @@ -1,37 +0,0 @@ - - - - - true - - 1 2 2 0 0 0 - - 3 1 1 0 0 1.5707 - - - - 2 - - - - - - - - - - 1 2 3 - - - - - - - 1 2 3 - - - - - - - diff --git a/tpe/plugin/worlds/shapes.world b/tpe/plugin/worlds/shapes.world deleted file mode 100644 index cb3bce5d9..000000000 --- a/tpe/plugin/worlds/shapes.world +++ /dev/null @@ -1,169 +0,0 @@ - - - - - 0 1.5 0.5 0 0 0 - - - - 3 - 0 - 0 - 3 - 0 - 3 - - 1.0 - - - - - 1 - - - - - - - 1 - - - - - - - - true - 0 0 0.5 0 0 0 - - - - 1 - 0 - 0 - 1 - 0 - 1 - - 1.0 - - - - - 100 100 1 - - - - - - - 100 100 1 - - - - - - - - 0 -1.5 0.5 0 0 0 - - - - 2 - 0 - 0 - 2 - 0 - 2 - - 2.0 - - - - - 0.5 - 1.1 - - - - - - - 0.5 - 1.1 - - - - - - - - 0 -3.0 0.5 0 0 0 - - - - 0.074154 - 0 - 0 - 0.074154 - 0 - 0.018769 - - 1.0 - - - - - 0.2 - 0.6 - - - - - - - 0.2 - 0.6 - - - - 1 1 0 1 - 1 1 0 1 - 1 1 0 1 - - - - - - - 0 -5 0.7 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 - - - - - - - diff --git a/tpe/plugin/worlds/shapes_bitmask.sdf b/tpe/plugin/worlds/shapes_bitmask.sdf deleted file mode 100644 index 8aa289fce..000000000 --- a/tpe/plugin/worlds/shapes_bitmask.sdf +++ /dev/null @@ -1,154 +0,0 @@ - - - - - true - - - - - 0 0 1 - - - - - - - 0 0 1 - 100 100 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - - - - 0 0 10.0 0 0 0 - - - - 1 - 0 - 0 - 1 - 0 - 1 - - 1.0 - - - - - 1 1 1 - - - - - 0x01 - - - - - - - 1 1 1 - - - - 1 0 0 1 - 1 0 0 1 - 1 0 0 1 - - - - - - - 0 0.75 10.0 0 0 0 - - - - 1 - 0 - 0 - 1 - 0 - 1 - - 1.0 - - - - - 1 1 1 - - - - - 0x02 - - - - - - - 1 1 1 - - - - 0 1 0 1 - 0 1 0 1 - 0 1 0 1 - - - - - - - 0 -0.75 10.0 0 0 0 - - - - 1 - 0 - 0 - 1 - 0 - 1 - - 1.0 - - - - - 1 1 1 - - - - - 0x03 - - - - - - - 1 1 1 - - - - 1 1 0 1 - 1 1 0 1 - 1 1 0 1 - - - - - - - diff --git a/tpe/plugin/worlds/test.world b/tpe/plugin/worlds/test.world deleted file mode 100644 index edbbe74cf..000000000 --- a/tpe/plugin/worlds/test.world +++ /dev/null @@ -1,328 +0,0 @@ - - - - - true - - - - - 0 0 1 - 100 100 - - - - - - 100 - 50 - - - - - - false - - - 0 0 1 - 100 100 - - - - 0.9 0.9 0.9 1 - - - - - - 1 0 0 0 0 0 - - - 100 - - - 0 0 0.01 0 0 0 - - - 0.8 - 0.02 - - - - 1 1 0 1 - - - - -0.275 0 1.1 0 0 0 - - - 0.2 0.2 2.2 - - - - 1 1 0 1 - - - - 0 0 0.01 0 0 0 - - - 0.8 - 0.02 - - - - - -0.275 0 1.1 0 0 0 - - - 0.2 0.2 2.2 - - - - - - 1.1 - - - - - - - - 0 0 2.1 -1.5708 0 0 - 0 - - 0 0 0.5 0 0 0 - - - -0.05 0 0 0 1.5708 0 - - - 0.1 - 0.3 - - - - 1 1 0 1 - - - - 0 0 1.0 0 1.5708 0 - - - 0.1 - 0.2 - - - - 1 1 0 1 - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - 1 1 0 1 - - - - -0.05 0 0 0 1.5708 0 - - - 0.1 - 0.3 - - - - - - 0.1 - - - - - - 0 0 1.0 0 1.5708 0 - - - 0.1 - 0.2 - - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - - - - 0.25 1.0 2.1 -2 0 0 - 0 - - 0 0 0.5 0 0 0 - - - 0 0 0 0 1.5708 0 - - - 0.08 - 0.3 - - - - 1 1 0 1 - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - 1 1 0 1 - - - - 0 0 0 0 1.5708 0 - - - 0.08 - 0.3 - - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - - - - base - upper_link - - 1.0 0 0 - - 3.0 - - - - - - upper_link - lower_link - - 1.0 0 0 - - 3.0 - - - - - - - 0.0 10.0 10.0 0.0 0.0 0.0 - - 0.0 0.0 0.0 0.0 0.0 0.0 - - - - - 10 0 2 0 0 0 - - - 100 - - - 0 0 0 -1.57 0 0 - - - 0.1 - 0.2 - - - - - - 1 0 0 0 0 0 - - 1 - - - - - 0.1 0.1 0.1 - - - - - - world - base - - - -1 0 0 0 0 0 - base - bar - - 0 1 0 - - -0.5 - 0.5 - 100 - - - - - - - - - link0 - link1 - 2 - - - - - - - - link0 - link1 - - - - - link2 - link3 - - - - - link4 - link5 - - - -