Merge pull request #604 from gazebosim/merge_7_main_20240313

Merge 7 -> main

.github/workflows/ci.yml

@@ -1,14 +1,20 @@
 name: Ubuntu CI
 
-on: [push, pull_request]
+on:
+  pull_request:
+  push:
+    branches:
+      - 'ign-physics[0-9]'
+      - 'gz-physics[0-9]?'
+      - 'main'
 
 jobs:
   jammy-ci:
     runs-on: ubuntu-latest
     name: Ubuntu Jammy CI
     steps:
       - name: Checkout
-        uses: actions/checkout@v3
+        uses: actions/checkout@v4
       - name: Compile and test
         id: ci
         uses: gazebo-tooling/action-gz-ci@jammy

.github/workflows/triage.yml

@@ -14,4 +14,3 @@ jobs:
         with:
           project-url: https://github.com/orgs/gazebosim/projects/7
           github-token: ${{ secrets.TRIAGE_TOKEN }}
-

CMakeLists.txt

@@ -42,6 +42,9 @@ gz_find_package(gz-common6
   REQUIRED_BY heightmap mesh dartsim tpe tpelib bullet)
 set(GZ_COMMON_VER ${gz-common6_VERSION_MAJOR})
 
+# This is only used for test support
+gz_find_package(gz-common6 REQUIRED COMPONENTS testing)
+
 #--------------------------------------
 # Find gz-math
 gz_find_package(gz-math8 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

Changelog.md

@@ -662,6 +662,19 @@
 
 ## Gazebo Physics 2.x
 
+### Gazebo Physics 2.6.2 (2024-01-05)
+
+1. dartsim: fix handling inertia matrix pose rotation
+    * [Pull request #351](https://github.com/gazebosim/gz-physics/pull/351)
+
+1. Fix a crash due to an invalid pointer
+    * [Pull request #486](https://github.com/gazebosim/gz-physics/pull/486)
+
+1. Infrastructure
+    * [Pull request #488](https://github.com/gazebosim/gz-physics/pull/488)
+    * [Pull request #487](https://github.com/gazebosim/gz-physics/pull/487)
+    * [Pull request #572](https://github.com/gazebosim/gz-physics/pull/572)
+
 ### Gazebo Physics 2.6.1 (2023-01-09)
 
 1. Fix build errors and warnings for DART 6.13.0

README.md

@@ -9,10 +9,10 @@
 
 Build | Status
 -- | --
-Test coverage | [![codecov](https://codecov.io/gh/gazebosim/gz-physics/branch/gz-physics8/graph/badge.svg)](https://codecov.io/gh/gazebosim/gz-physics/branch/gz-physics8)
-Ubuntu Focal | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_physics-ci-gz-physics8-focal-amd64)](https://build.osrfoundation.org/job/ignition_physics-ci-gz-physics8-focal-amd64)
-Homebrew      | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_physics-ci-gz-physics8-homebrew-amd64)](https://build.osrfoundation.org/job/ignition_physics-ci-gz-physics8-homebrew-amd64)
-Windows       | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ign_physics-gz-8-win)](https://build.osrfoundation.org/job/ign_physics-gz-8-win)
+Test coverage | [![codecov](https://codecov.io/gh/gazebosim/gz-physics/tree/gz-physics8/graph/badge.svg)](https://codecov.io/gh/gazebosim/gz-physics/tree/gz-physics8)
+Ubuntu Jammy  | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=gz_physics-ci-gz-physics8-jammy-amd64)](https://build.osrfoundation.org/job/gz_physics-ci-gz-physics8-jammy-amd64)
+Homebrew      | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=gz_physics-ci-gz-physics8-homebrew-amd64)](https://build.osrfoundation.org/job/gz_physics-ci-gz-physics8-homebrew-amd64)
+Windows       | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=gz_physics-8-win)](https://build.osrfoundation.org/job/gz_physics-8-win)
 
 Gazebo Physics, a component of [Gazebo](https://gazebosim.org), provides an abstract physics interface
 designed to support simulation and rapid development of robot applications.

bullet-featherstone/src/Base.cc

@@ -42,6 +42,13 @@ WorldInfo::WorldInfo(std::string name_)
   // Needed for force-torque sensor
   this->world->getSolverInfo().m_jointFeedbackInJointFrame = true;
   this->world->getSolverInfo().m_jointFeedbackInWorldSpace = false;
+
+  // By default a large impulse is applied when collisions penetrate
+  // which causes unstable behavior. Bullet featherstone does not support
+  // configuring split impulse and penetration threshold parameters. Instead the
+  // penentration impulse depends on the erp2 parameter so set to a small value
+  // (default is 0.2).
+  this->world->getSolverInfo().m_erp2 = btScalar(0.002);
 }
 
 }  // namespace bullet_featherstone

bullet-featherstone/src/Base.hh

@@ -85,12 +85,14 @@ struct ModelInfo
   Identity world;
   int indexInWorld;
   Eigen::Isometry3d baseInertiaToLinkFrame;
-  std::unique_ptr<btMultiBody> body;
+  std::shared_ptr<btMultiBody> body;
 
   std::vector<std::size_t> linkEntityIds;
   std::vector<std::size_t> jointEntityIds;
+  std::vector<std::size_t> nestedModelEntityIds;
   std::unordered_map<std::string, std::size_t> linkNameToEntityId;
   std::unordered_map<std::string, std::size_t> jointNameToEntityId;
+  std::unordered_map<std::string, std::size_t> nestedModelNameToEntityId;
 
   /// These are joints that connect this model to other models, e.g. fixed
   /// constraints.
@@ -100,7 +102,7 @@ struct ModelInfo
     std::string _name,
     Identity _world,
     Eigen::Isometry3d _baseInertiaToLinkFrame,
-    std::unique_ptr<btMultiBody> _body)
+    std::shared_ptr<btMultiBody> _body)
     : name(std::move(_name)),
       world(std::move(_world)),
       baseInertiaToLinkFrame(_baseInertiaToLinkFrame),
@@ -275,14 +277,24 @@ class Base : public Implements3d<FeatureList<Feature>>
     const auto id = this->GetNextEntity();
     auto world = std::make_shared<WorldInfo>(std::move(_worldInfo));
     this->worlds[id] = world;
-    return this->GenerateIdentity(id, world);
+    auto worldID = this->GenerateIdentity(id, world);
+
+    auto worldModel = std::make_shared<ModelInfo>(
+      world->name, worldID,
+      Eigen::Isometry3d::Identity(), nullptr);
+    this->models[id] = worldModel;
+    world->modelNameToEntityId[worldModel->name] = id;
+    worldModel->indexInWorld = -1;
+    world->modelIndexToEntityId[worldModel->indexInWorld] = id;
+
+    return worldID;
   }
 
   public: inline Identity AddModel(
     std::string _name,
     Identity _worldID,
     Eigen::Isometry3d _baseInertialToLinkFrame,
-    std::unique_ptr<btMultiBody> _body)
+    std::shared_ptr<btMultiBody> _body)
   {
     const auto id = this->GetNextEntity();
     auto model = std::make_shared<ModelInfo>(
@@ -294,6 +306,30 @@ class Base : public Implements3d<FeatureList<Feature>>
     world->modelNameToEntityId[model->name] = id;
     model->indexInWorld = world->nextModelIndex++;
     world->modelIndexToEntityId[model->indexInWorld] = id;
+
+    auto worldModel = this->models.at(model->world);
+    worldModel->nestedModelEntityIds.push_back(id);
+    worldModel->nestedModelNameToEntityId[model->name] = id;
+
+    return this->GenerateIdentity(id, model);
+  }
+
+  public: inline Identity AddNestedModel(
+    std::string _name,
+    Identity _parentID,
+    Identity _worldID,
+    Eigen::Isometry3d _baseInertialToLinkFrame,
+    std::shared_ptr<btMultiBody> _body)
+  {
+    const auto id = this->GetNextEntity();
+    auto model = std::make_shared<ModelInfo>(
+      std::move(_name), std::move(_worldID),
+      std::move(_baseInertialToLinkFrame), std::move(_body));
+
+    this->models[id] = model;
+    const auto parentModel = this->models.at(_parentID);
+    parentModel->nestedModelEntityIds.push_back(id);
+    parentModel->nestedModelNameToEntityId[model->name] = id;
     return this->GenerateIdentity(id, model);
   }
 
@@ -305,13 +341,7 @@ class Base : public Implements3d<FeatureList<Feature>>
 
     auto *model = this->ReferenceInterface<ModelInfo>(_linkInfo.model);
     model->linkNameToEntityId[link->name] = id;
-    if (link->indexInModel.has_value())
-    {
-      // We expect the links to be added in order
-      assert(static_cast<std::size_t>(*link->indexInModel + 1) ==
-             model->linkEntityIds.size());
-    }
-    else
+    if (!link->indexInModel.has_value())
     {
       // We are adding the root link. This means the model should not already
       // have a root link
@@ -359,6 +389,99 @@ class Base : public Implements3d<FeatureList<Feature>>
     return this->GenerateIdentity(id, joint);
   }
 
+  public: bool RemoveModelImpl(const Identity &_parentID,
+                               const Identity &_modelID)
+  {
+    auto *model = this->ReferenceInterface<ModelInfo>(_modelID);
+    if (!model)
+      return false;
+
+    // Remove nested models
+    for (auto &nestedModelID : model->nestedModelEntityIds)
+    {
+      this->RemoveModelImpl(_modelID, this->GenerateIdentity(nestedModelID,
+                            this->models.at(nestedModelID)));
+    }
+    model->nestedModelEntityIds.clear();
+
+    // remove references in parent model or world model
+    auto parentModelIt = this->models.find(_parentID);
+    if (parentModelIt != this->models.end())
+    {
+      auto parentModel = parentModelIt->second;
+      auto nestedModelIt =
+          parentModel->nestedModelNameToEntityId.find(model->name);
+      if (nestedModelIt !=
+          parentModel->nestedModelNameToEntityId.end())
+      {
+        std::size_t nestedModelID = nestedModelIt->second;
+        parentModel->nestedModelNameToEntityId.erase(nestedModelIt);
+        parentModel->nestedModelEntityIds.erase(std::remove(
+            parentModel->nestedModelEntityIds.begin(),
+            parentModel->nestedModelEntityIds.end(), nestedModelID),
+            parentModel->nestedModelEntityIds.end());
+      }
+    }
+
+    // If nested, no need to remove multibody
+    // \todo(iche033) Remove links and joints in nested model
+    bool isNested =  this->worlds.find(_parentID) == this->worlds.end();
+    if (isNested)
+    {
+      return true;
+    }
+
+    // Remove model from world
+    auto *world = this->ReferenceInterface<WorldInfo>(model->world);
+    if (!world)
+      return false;
+    if (world->modelIndexToEntityId.erase(model->indexInWorld) == 0)
+    {
+      // The model has already been removed at some point
+      return false;
+    }
+    world->modelNameToEntityId.erase(model->name);
+
+    // Remove all constraints related to this model
+    for (const auto jointID : model->jointEntityIds)
+    {
+      const auto joint = this->joints.at(jointID);
+      if (joint->motor)
+      {
+        world->world->removeMultiBodyConstraint(joint->motor.get());
+      }
+      if (joint->fixedConstraint)
+      {
+        world->world->removeMultiBodyConstraint(joint->fixedConstraint.get());
+      }
+      if (joint->jointLimits)
+      {
+        world->world->removeMultiBodyConstraint(joint->jointLimits.get());
+      }
+      this->joints.erase(jointID);
+    }
+    // \todo(iche033) Remove external constraints related to this model
+    // (model->external_constraints) once this is supported
+
+    world->world->removeMultiBody(model->body.get());
+    for (const auto linkID : model->linkEntityIds)
+    {
+      const auto &link = this->links.at(linkID);
+      if (link->collider)
+      {
+        world->world->removeCollisionObject(link->collider.get());
+        for (const auto shapeID : link->collisionEntityIds)
+          this->collisions.erase(shapeID);
+      }
+
+      this->links.erase(linkID);
+    }
+
+    this->models.erase(_modelID);
+
+    return true;
+  }
+
   public: ~Base() override {
     // The order of destruction between meshesGImpact and triangleMeshes is
     // important.