Merge 6 ➡️ 7

.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:

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

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=\"$<TARGET_FILE:${dartsim_plugin}>\""
-    "TEST_WORLD_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/worlds/\""
-    "GZ_PHYSICS_RESOURCE_DIR=\"${GZ_PHYSICS_RESOURCE_DIR}\"")
+    "dartsim_plugin_LIB=\"$<TARGET_FILE:${dartsim_plugin}>\"")
 
   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=\"$<TARGET_FILE:${dartsim_plugin}>\"")
-  endif()
-endforeach()

dartsim/src/AddedMassFeatures_TEST.cc

@@ -38,6 +38,7 @@
 #include <sdf/World.hh>
 
 #include <test/Utils.hh>
+#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();

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);

dartsim/src/EntityManagementFeatures.cc

@@ -22,13 +22,14 @@
 #include <unordered_map>
 
 #include <dart/config.hpp>
-#include <dart/collision/ode/OdeCollisionDetector.hpp>
 #include <dart/constraint/ConstraintSolver.hpp>
 #include <dart/dynamics/FreeJoint.hpp>
 
 #include <dart/collision/CollisionFilter.hpp>
 #include <dart/collision/CollisionObject.hpp>
 
+#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<dart::simulation::World>(_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 =

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));
 

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 <memory>
+#include <unordered_map>
+
+#include <dart/collision/CollisionObject.hpp>
+
+#include "GzOdeCollisionDetector.hh"
+
+using namespace dart;
+using namespace collision;
+
+/////////////////////////////////////////////////
+GzOdeCollisionDetector::GzOdeCollisionDetector()
+  : OdeCollisionDetector()
+{
+}
+
+/////////////////////////////////////////////////
+GzOdeCollisionDetector::Registrar<GzOdeCollisionDetector>
+    GzOdeCollisionDetector::mRegistrar{
+        GzOdeCollisionDetector::getStaticType(),
+        []() -> std::shared_ptr<GzOdeCollisionDetector> {
+          return GzOdeCollisionDetector::create();
+        }};
+
+/////////////////////////////////////////////////
+std::shared_ptr<GzOdeCollisionDetector> GzOdeCollisionDetector::create()
+{
+  return std::shared_ptr<GzOdeCollisionDetector>(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<std::size_t>::max())
+    return;
+
+  auto allContacts = _result->getContacts();
+  _result->clear();
+
+  std::unordered_map<dart::collision::CollisionObject *,
+      std::unordered_map<dart::collision::CollisionObject *, std::size_t>>
+      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);
+    }
+  }
+}

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 <limits>
+#include <memory>
+
+#include <dart/collision/ode/OdeCollisionDetector.hpp>
+
+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<GzOdeCollisionDetector> 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<std::size_t>::max();
+
+  private: static Registrar<GzOdeCollisionDetector> mRegistrar;
+};
+
+}
+}