Merge remote-tracking branch 'origin/ahcorde/add-bullet-featherstone/…

…shapesFeatures' into bullet-featherstone-collisions

.github/ci/packages.apt

@@ -1,10 +1,10 @@
 libbenchmark-dev
 libeigen3-dev
-libignition-cmake3-dev
-libignition-common5-dev
-libignition-math7-dev
-libignition-math7-eigen3-dev
-libignition-plugin2-dev
-libignition-utils2-cli-dev
-libignition-utils2-dev
+libgz-cmake3-dev
+libgz-common5-dev
+libgz-math7-dev
+libgz-math7-eigen3-dev
+libgz-plugin2-dev
+libgz-utils2-cli-dev
+libgz-utils2-dev
 libsdformat13-dev

.github/workflows/ci.yml

@@ -11,7 +11,7 @@ jobs:
         uses: actions/checkout@v2
       - name: Compile and test
         id: ci
-        uses: ignition-tooling/action-ignition-ci@focal
+        uses: gazebo-tooling/action-gz-ci@focal
         with:
           codecov-enabled: true
           cppcheck-enabled: true
@@ -24,4 +24,4 @@ jobs:
         uses: actions/checkout@v2
       - name: Compile and test
         id: ci
-        uses: ignition-tooling/action-ignition-ci@jammy
+        uses: gazebo-tooling/action-gz-ci@jammy

.github/workflows/pr-collection-labeler.yml

@@ -8,6 +8,6 @@ jobs:
     steps:
     - name: Add collection labels
       if: github.event.action == 'opened'
-      uses: ignition-tooling/pr-collection-labeler@v1
+      uses: gazebo-tooling/pr-collection-labeler@v1
       with:
         github-token: ${{ secrets.GITHUB_TOKEN }}

CMakeLists.txt

@@ -16,9 +16,7 @@ find_package(gz-cmake3 REQUIRED)
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
-gz_configure_project(
-  REPLACE_IGNITION_INCLUDE_PATH gz/physics
-  VERSION_SUFFIX pre1)
+gz_configure_project(VERSION_SUFFIX pre1)
 
 #============================================================================
 # Set project-specific options
@@ -96,7 +94,7 @@ set(GZ_PHYSICS_RESOURCE_DIR "${CMAKE_SOURCE_DIR}/resources")
 
 # Plugin install dirs
 set(GZ_PHYSICS_ENGINE_INSTALL_DIR
-  ${CMAKE_INSTALL_PREFIX}/${IGN_LIB_INSTALL_DIR}/gz-${IGN_DESIGNATION}-${PROJECT_VERSION_MAJOR}/engine-plugins
+  ${CMAKE_INSTALL_PREFIX}/${GZ_LIB_INSTALL_DIR}/gz-${GZ_DESIGNATION}-${PROJECT_VERSION_MAJOR}/engine-plugins
 )
 
 #============================================================================

Changelog.md

@@ -9,7 +9,7 @@
 
 ### Gazebo Physics 5.1.0 (2021-11-12)
 
-1. Remove unused ign_auto_headers.hh.in
+1. Remove unused gz_auto_headers.hh.in
     * [Pull request #305](https://github.com/gazebosim/gz-physics/pull/305)
 
 1. Added DART feature for setting joint limits dynamically.
@@ -38,7 +38,7 @@
 1. Fix TPE Link velocity not being updated and Model velocity not having any effect.
     * [Pull request #289](https://github.com/gazebosim/gz-physics/pull/289)
 
-1. Make ignition-physics CMake config files relocatable
+1. Make gz-physics CMake config files relocatable
     * [Pull request #282](https://github.com/gazebosim/gz-physics/pull/282)
 
 1. Added DART feature for setting joint limits dynamically.
@@ -69,7 +69,7 @@
 
 ### Gazebo Physics 4.3.0 (2021-11-11)
 
-1. Remove unused ign_auto_headers.hh.in
+1. Remove unused gz_auto_headers.hh.in
     * [Pull request #305](https://github.com/gazebosim/gz-physics/pull/305)
 
 1. Added DART feature for setting joint limits dynamically.
@@ -91,7 +91,7 @@
     * [Pull request #287](https://github.com/gazebosim/gz-physics/pull/287)
     * [Pull request #281](https://github.com/gazebosim/gz-physics/pull/281)
 
-1. Make ignition-physics CMake config files relocatable
+1. Make gz-physics CMake config files relocatable
     * [Pull request #282](https://github.com/gazebosim/gz-physics/pull/282)
 
 ### Gazebo Physics 4.2.0 (2021-07-16)
@@ -323,7 +323,7 @@
 
 ### Gazebo Physics 2.5.0 (2021-11-09)
 
-1. Remove unused ign_auto_headers.hh.in
+1. Remove unused gz_auto_headers.hh.in
     * [Pull request #305](https://github.com/gazebosim/gz-physics/pull/305)
 
 1. Added DART feature for setting joint limits dynamically.

Migration.md

@@ -61,7 +61,7 @@ release will remove the deprecated code.
 
 ### Modifications
 
-1. Depends on ignition-utils1.
+1. Depends on gz-utils1.
 
 1. Depends on sdformat11.
 

README.md

@@ -128,12 +128,12 @@ Follow these steps to run tests and static code analysis in your clone of this r
 Refer to the following table for information about important directories and files in this repository.
 
 ```
-ign-physics
+gz-physics
 ├── bullet                    Files for bullet plugin component.
 ├── dartsim                   Files for dartsim plugin component.
 ├── example                   Examples about how to use the library
 ├── heightmap                 Heightmap related header files.
-├── include/ignition/physics  Header files.
+├── include/gz/physics  Header files.
 ├── mesh                      Files for mesh component.
 ├── resources                 Model and mesh resource files used by tests.
 ├── sdf                       Files for sdf component.

api.md.in

@@ -1,6 +1,6 @@
-## Gazebo @IGN_DESIGNATION_CAP@
+## Gazebo @GZ_DESIGNATION_CAP@
 
-Gazebo @IGN_DESIGNATION_CAP@ is a component in Gazebo, a set of libraries
+Gazebo @GZ_DESIGNATION_CAP@ is a component in Gazebo, a set of libraries
 designed to rapidly develop robot and simulation applications.
 
 ## License

bullet-featherstone/src/Base.hh

@@ -206,6 +206,32 @@ inline Eigen::Isometry3d convert(const btTransform& tf)
   return output;
 }
 
+inline btTransform GetWorldTransformOfLinkInertiaFrame(
+    const btMultiBody &body,
+    const std::size_t linkIndexInModel)
+{
+  const auto p = body.localPosToWorld(
+    linkIndexInModel, btVector3(0, 0, 0));
+  const auto rot = body.localFrameToWorld(
+    linkIndexInModel, btMatrix3x3::getIdentity());
+  return btTransform(rot, p);
+}
+
+inline Eigen::Isometry3d GetWorldTransformOfLink(
+    const ModelInfo &model,
+    const LinkInfo &linkInfo)
+{
+  const auto &body = *model.body;
+  const auto indexOpt = linkInfo.indexInModel;
+  if (indexOpt.has_value())
+  {
+    return convert(GetWorldTransformOfLinkInertiaFrame(body, *indexOpt))
+        * linkInfo.inertiaToLinkFrame;
+  }
+
+  return convert(body.getBaseWorldTransform()) * model.baseInertiaToLinkFrame;
+}
+
 class Base : public Implements3d<FeatureList<Feature>>
 {
   // Note: Entity ID 0 is reserved for the "engine"
@@ -266,7 +292,8 @@ class Base : public Implements3d<FeatureList<Feature>>
     {
       // We are adding the root link. This means the model should not already
       // have a root link
-      assert(model->linkEntityIds.empty());
+      // This check makes `ConstructEmptyLink` to fail
+      // assert(model->linkEntityIds.empty());
     }
     model->linkEntityIds.push_back(id);
 

bullet-featherstone/src/EntityManagementFeatures.cc

@@ -225,6 +225,19 @@ bool EntityManagementFeatures::RemoveModel(const Identity &_modelID)
   }
 
   world->world->removeMultiBody(model->body.get());
+  for (const auto linkID : model->linkEntityIds)
+  {
+    const auto &link = this->links.at(linkID);
+    world->world->removeCollisionObject(link->collider.get());
+    for (const auto shapeID : link->collisionEntityIds)
+      this->collisions.erase(shapeID);
+
+    this->links.erase(linkID);
+  }
+
+  for (const auto jointID : model->jointEntityIds)
+    this->joints.erase(jointID);
+
   this->models.erase(_modelID);
   return true;
 }
@@ -265,6 +278,137 @@ bool EntityManagementFeatures::RemoveModelByName(
     this->GenerateIdentity(it->second, this->models.at(it->second)));
 }
 
+
+/////////////////////////////////////////////////
+const std::string &EntityManagementFeatures::GetEngineName(
+  const Identity &) const
+{
+  static const std::string engineName = "bullet-featherstone";
+  return engineName;
+}
+
+/////////////////////////////////////////////////
+std::size_t EntityManagementFeatures::GetEngineIndex(const Identity &) const
+{
+  return 0;
+}
+
+/////////////////////////////////////////////////
+std::size_t EntityManagementFeatures::GetWorldCount(
+    const Identity &) const
+{
+  return worlds.size();
+}
+
+/////////////////////////////////////////////////
+Identity EntityManagementFeatures::GetWorld(
+    const Identity &, const std::size_t _requestedWorldIndex) const
+{
+  // _worldIndex is not the same as a WorldID. The value of _worldIndex should
+  // range from 0 to GetWorldCount()-1. The most efficient implementation
+  // would be to maintain a std::vector of WorldIDs, but then we'd have to
+  // manage that data when worlds are added and removed.
+  std::size_t currentWorldIndex = 0;
+  for (const auto &[worldID, world] : this->worlds)
+  {
+    if (currentWorldIndex == _requestedWorldIndex)
+      return this->GenerateIdentity(worldID, world);
+  }
+
+  return this->GenerateInvalidId();
+}
+
+/////////////////////////////////////////////////
+Identity EntityManagementFeatures::GetWorld(
+    const Identity &, const std::string &_requestedWorldName) const
+{
+  // We could speed this up by maintaining a hashmap from world name to world ID
+  for (const auto &[worldID, world] : this->worlds)
+  {
+    if (world->name == _requestedWorldName)
+      return this->GenerateIdentity(worldID, world);
+  }
+
+  return this->GenerateInvalidId();
+}
+
+/////////////////////////////////////////////////
+const std::string &EntityManagementFeatures::GetWorldName(
+    const Identity &_worldID) const
+{
+  return this->ReferenceInterface<WorldInfo>(_worldID)->name;
+}
+
+/////////////////////////////////////////////////
+std::size_t EntityManagementFeatures::GetWorldIndex(
+    const Identity &) const
+{
+  return 0;
+}
+
+/////////////////////////////////////////////////
+Identity EntityManagementFeatures::GetEngineOfWorld(
+    const Identity &) const
+{
+  return this->GenerateIdentity(0);
+}
+
+/////////////////////////////////////////////////
+std::size_t EntityManagementFeatures::GetModelCount(
+    const Identity &) const
+{
+  return this->models.size();
+}
+
+/////////////////////////////////////////////////
+Identity EntityManagementFeatures::GetModel(
+    const Identity &_worldID, std::size_t _modelIndex) const
+{
+  const auto *world = this->ReferenceInterface<WorldInfo>(_worldID);
+  const auto it = world->modelIndexToEntityId.find(_modelIndex);
+  if (it == world->modelIndexToEntityId.end())
+    return this->GenerateInvalidId();
+
+  return this->GenerateIdentity(it->second, this->models.at(it->second));
+}
+
+/////////////////////////////////////////////////
+Identity EntityManagementFeatures::GetModel(
+    const Identity &_worldID, const std::string &_modelName) const
+{
+  const auto *world = this->ReferenceInterface<WorldInfo>(_worldID);
+  const auto it = world->modelNameToEntityId.find(_modelName);
+  if (it == world->modelNameToEntityId.end())
+    return this->GenerateInvalidId();
+
+  return this->GenerateIdentity(it->second, this->models.at(it->second));
+}
+
+/////////////////////////////////////////////////
+const std::string &EntityManagementFeatures::GetModelName(
+    const Identity &_modelID) const
+{
+  return this->ReferenceInterface<ModelInfo>(_modelID)->name;
+}
+
+/////////////////////////////////////////////////
+std::size_t EntityManagementFeatures::GetModelIndex(
+    const Identity &_modelID) const
+{
+  // The root link does not have an index, so we give it an index of 0 and bump
+  // the rest up by one when providing an index to gazebo
+  const auto index = this->ReferenceInterface<ModelInfo>(
+    _modelID)->indexInWorld;
+  return index+1;
+}
+
+/////////////////////////////////////////////////
+Identity EntityManagementFeatures::GetWorldOfModel(
+    const Identity &_modelID) const
+{
+  return this->ReferenceInterface<ModelInfo>(_modelID)->world;
+}
+
 }  // namespace bullet_featherstone
 }  // namespace physics
 }  // namespace gz