Merge from gz-physics6

Signed-off-by: Ian Chen <[email protected]>

.github/CODEOWNERS

@@ -1,4 +1,4 @@
 # More info:
 # https://help.github.com/en/github/creating-cloning-and-archiving-repositories/about-code-owners
 
-*                     @azeey @mxgrey @scpeters
+*                     @azeey @scpeters

CMakeLists.txt

@@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR)
 #============================================================================
 # Initialize the project
 #============================================================================
-project(gz-physics6 VERSION 6.5.1)
+project(gz-physics6 VERSION 6.6.0)
 
 #============================================================================
 # Find gz-cmake

Changelog.md

@@ -1,5 +1,65 @@
 ## Gazebo Physics 6.x
 
+### Gazebo Physics 6.6.0 (2024-06-11)
+
+1. dartsim: optimize picking contact points with ODE collision detector
+    * [Pull request #584](https://github.com/gazebosim/gz-physics/pull/584)
+
+1. Fix windows compiler warning
+    * [Pull request #629](https://github.com/gazebosim/gz-physics/pull/629)
+
+1. Disable test failing due to ODE/libccd
+    * [Pull request #621](https://github.com/gazebosim/gz-physics/pull/621)
+
+1. bullet-featherstone: Improve mesh collision stability
+    * [Pull request #600](https://github.com/gazebosim/gz-physics/pull/600)
+
+1. bullet-featherstone: Support nested models
+    * [Pull request #574](https://github.com/gazebosim/gz-physics/pull/574)
+
+1. Revert "bazel: updates for garden (#513)"
+    * [Pull request #513](https://github.com/gazebosim/gz-physics/pull/513)
+
+1. Garden test cleanup
+    * [Pull request #587](https://github.com/gazebosim/gz-physics/pull/587)
+
+1. Support setting max contacts in dartsim's ODE collision detector
+    * [Pull request #582](https://github.com/gazebosim/gz-physics/pull/582)
+
+1. Get bullet version from cmake instead of API
+    * [Pull request #591](https://github.com/gazebosim/gz-physics/pull/591)
+
+1. Reduce error to debug messsage for mesh construction (#581)
+    * [Pull request #581](https://github.com/gazebosim/gz-physics/pull/581)
+
+1. bullet-featherstone: Set collision spinning friction
+    * [Pull request #579](https://github.com/gazebosim/gz-physics/pull/579)
+
+1. Infrastructure
+    * [Pull request #578](https://github.com/gazebosim/gz-physics/pull/578)
+    * [Pull request #572](https://github.com/gazebosim/gz-physics/pull/572)
+
+1. dartsim: fix handling inertia matrix pose rotation
+    * [Pull request #351](https://github.com/gazebosim/gz-physics/pull/351)
+
+1. bullet-featherstone: fix setting angular velocity
+    * [Pull request #567](https://github.com/gazebosim/gz-physics/pull/567)
+
+1. bullet-featherstone: support off-diagonal inertia
+    * [Pull request #544](https://github.com/gazebosim/gz-physics/pull/544)
+
+1. bullet-featherstone: Fix how links are flattened in  ConstructSdfModel
+    * [Pull request #562](https://github.com/gazebosim/gz-physics/pull/562)
+
+1. Add sample ctest cmds to tutorial
+    * [Pull request #566](https://github.com/gazebosim/gz-physics/pull/566)
+
+1. Add a test to verify behavior of detachable joints
+    * [Pull request #563](https://github.com/gazebosim/gz-physics/pull/563)
+
+1. Use correct link indicies when constructing fixed constraints
+    * [Pull request #530](https://github.com/gazebosim/gz-physics/pull/530)
+
 ### Gazebo Physics 6.5.1 (2023-09-26)
 
 1. joint_features test: reduce console spam

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 in bullet is 0.2).
+  this->world->getSolverInfo().m_erp2 = btScalar(0.002);
 }
 
 }  // namespace bullet_featherstone

bullet-featherstone/src/Base.hh

@@ -84,12 +84,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.
@@ -99,7 +101,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),
@@ -273,14 +275,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>(
@@ -292,6 +304,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);
   }
 
@@ -303,13 +339,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
@@ -357,6 +387,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.