Merge branch 'main' into bullet_kinematic

Changelog.md

@@ -4,6 +4,74 @@
 
 ## Gazebo Physics 7.x
 
+### Gazebo Physics 7.2.0 (2024-04-10)
+
+1. Use relative install paths for plugin shared libraries
+    * [Pull request #616](https://github.com/gazebosim/gz-physics/pull/616)
+
+1. Disable test failing due to ODE/libccd
+    * [Pull request #621](https://github.com/gazebosim/gz-physics/pull/621)
+
+1. bullet-featherstone: Ignore collision between static objects and objects with world joint
+    * [Pull request #611](https://github.com/gazebosim/gz-physics/pull/611)
+
+1. Disable check in DetachableJointTest, CorrectAttachmentPoints for dartsim plugin on macOS
+    * [Pull request #613](https://github.com/gazebosim/gz-physics/pull/613)
+
+1. bullet-featherstone: Fix attaching fixed joint
+    * [Pull request #610](https://github.com/gazebosim/gz-physics/pull/610)
+
+### Gazebo Physics 7.1.0 (2024-03-14)
+
+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. 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. Update CI badges in README
+    * [Pull request #583](https://github.com/gazebosim/gz-physics/pull/583)
+
+1. Reduce error to debug messsage for mesh construction
+    * [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 7.0.0 (2023-09-29)
 
 1. dartsim: Fix sign convention error with contact surface motion velocities

bullet-featherstone/CMakeLists.txt

@@ -25,7 +25,7 @@ target_link_libraries(${bullet_plugin}
     gz-math${GZ_MATH_VER}::eigen3)
 
 # Note that plugins are currently being installed in 2 places: /lib and the engine-plugins dir
-install(TARGETS ${bullet_plugin} DESTINATION ${GZ_PHYSICS_ENGINE_INSTALL_DIR})
+install(TARGETS ${bullet_plugin} DESTINATION ${GZ_PHYSICS_ENGINE_RELATIVE_INSTALL_DIR})
 
 # The library created by `gz_add_component` includes the ign-physics version
 # (i.e. libgz-physics1-name-plugin.so), but for portability,
@@ -40,5 +40,5 @@ if (WIN32)
       ${GZ_PHYSICS_ENGINE_INSTALL_DIR}\/${unversioned})")
 else()
   EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E create_symlink ${versioned} ${unversioned})
-  INSTALL(FILES ${PROJECT_BINARY_DIR}/${unversioned} DESTINATION ${GZ_PHYSICS_ENGINE_INSTALL_DIR})
+  INSTALL(FILES ${PROJECT_BINARY_DIR}/${unversioned} DESTINATION ${GZ_PHYSICS_ENGINE_RELATIVE_INSTALL_DIR})
 endif()

bullet-featherstone/src/Base.cc

@@ -48,7 +48,7 @@ WorldInfo::WorldInfo(std::string name_)
   // 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);
+  this->world->getSolverInfo().m_erp2 = btScalar(0.02);
 
   // Set solver iterations to the same as the default value in SDF,
   // //world/physics/solver/bullet/iters

bullet-featherstone/src/Base.hh

@@ -487,6 +487,7 @@ class Base : public Implements3d<FeatureList<Feature>>
     // important.
     this->meshesGImpact.clear();
     this->triangleMeshes.clear();
+    this->meshesConvex.clear();
 
     this->joints.clear();
 
@@ -520,6 +521,7 @@ class Base : public Implements3d<FeatureList<Feature>>
 
   public: std::vector<std::unique_ptr<btTriangleMesh>> triangleMeshes;
   public: std::vector<std::unique_ptr<btGImpactMeshShape>> meshesGImpact;
+  public: std::vector<std::unique_ptr<btConvexHullShape>> meshesConvex;
 };
 
 }  // namespace bullet_featherstone

bullet-featherstone/src/FreeGroupFeatures.cc

@@ -93,7 +93,8 @@ void FreeGroupFeatures::SetFreeGroupWorldPose(
   const auto *model = this->ReferenceInterface<ModelInfo>(_groupID);
   if (model)
   {
-    model->body->setBaseWorldTransform(convertTf(_pose));
+    model->body->setBaseWorldTransform(
+        convertTf(_pose * model->baseInertiaToLinkFrame.inverse()));
   }
 }
 

bullet-featherstone/src/JointFeatures.cc

@@ -368,8 +368,11 @@ void JointFeatures::SetJointTransformFromParent(
 
   if (jointInfo->fixedConstraint)
   {
+      auto tf = convertTf(_pose);
       jointInfo->fixedConstraint->setPivotInA(
-        convertVec(_pose.translation()));
+        tf.getOrigin());
+      jointInfo->fixedConstraint->setFrameInA(
+        tf.getBasis());
   }
 }
 

bullet-featherstone/src/SDFFeatures.cc

@@ -1048,49 +1048,140 @@ bool SDFFeatures::AddSdfCollision(
   {
     auto &meshManager = *gz::common::MeshManager::Instance();
     auto *mesh = meshManager.Load(meshSdf->Uri());
-    const btVector3 scale = convertVec(meshSdf->Scale());
     if (nullptr == mesh)
     {
       gzwarn << "Failed to load mesh from [" << meshSdf->Uri()
              << "]." << std::endl;
       return false;
     }
+    const btVector3 scale = convertVec(meshSdf->Scale());
 
     auto compoundShape = std::make_unique<btCompoundShape>();
 
-    for (unsigned int submeshIdx = 0;
-         submeshIdx < mesh->SubMeshCount();
-         ++submeshIdx)
+    bool meshCreated = false;
+    if (meshSdf->Optimization() ==
+        ::sdf::MeshOptimization::CONVEX_DECOMPOSITION ||
+        meshSdf->Optimization() ==
+        ::sdf::MeshOptimization::CONVEX_HULL)
     {
-      auto s = mesh->SubMeshByIndex(submeshIdx).lock();
-      auto vertexCount = s->VertexCount();
-      auto indexCount = s->IndexCount();
-      btAlignedObjectArray<btVector3> convertedVerts;
-      convertedVerts.reserve(static_cast<int>(vertexCount));
-      for (unsigned int i = 0; i < vertexCount; i++)
+      std::size_t maxConvexHulls = 16u;
+      if (meshSdf->Optimization() == ::sdf::MeshOptimization::CONVEX_HULL)
       {
-        convertedVerts.push_back(btVector3(
-              static_cast<btScalar>(s->Vertex(i).X()) * scale[0],
-              static_cast<btScalar>(s->Vertex(i).Y()) * scale[1],
-              static_cast<btScalar>(s->Vertex(i).Z()) * scale[2]));
+        /// create 1 convex hull for the whole submesh
+        maxConvexHulls = 1u;
+      }
+      else if (meshSdf->ConvexDecomposition())
+      {
+        // limit max number of convex hulls to generate
+        maxConvexHulls = meshSdf->ConvexDecomposition()->MaxConvexHulls();
       }
 
-      this->triangleMeshes.push_back(std::make_unique<btTriangleMesh>());
-      for (unsigned int i = 0; i < indexCount/3; i++)
+      // Check if MeshManager contains the decomposed mesh already. If not
+      // add it to the MeshManager so we do not need to decompose it again.
+      const std::string convexMeshName =
+          mesh->Name() + "_CONVEX_" + std::to_string(maxConvexHulls);
+      auto *decomposedMesh = meshManager.MeshByName(convexMeshName);
+      if (!decomposedMesh)
       {
-        const btVector3& v0 = convertedVerts[s->Index(i*3)];
-        const btVector3& v1 = convertedVerts[s->Index(i*3 + 1)];
-        const btVector3& v2 = convertedVerts[s->Index(i*3 + 2)];
-        this->triangleMeshes.back()->addTriangle(v0, v1, v2);
+        // Merge meshes before convex decomposition
+        auto mergedMesh = gz::common::MeshManager::MergeSubMeshes(*mesh);
+        if (mergedMesh && mergedMesh->SubMeshCount() == 1u)
+        {
+          // Decompose and add mesh to MeshManager
+          auto mergedSubmesh = mergedMesh->SubMeshByIndex(0u).lock();
+          std::vector<common::SubMesh> decomposed =
+            gz::common::MeshManager::ConvexDecomposition(
+            *mergedSubmesh.get(), maxConvexHulls);
+          gzdbg << "Optimizing mesh (" << meshSdf->OptimizationStr() << "): "
+                <<  mesh->Name() << std::endl;
+          // Create decomposed mesh and add it to MeshManager
+          // Note: MeshManager will call delete on this mesh in its destructor
+          // \todo(iche033) Consider updating MeshManager to accept
+          // unique pointers instead
+          common::Mesh *convexMesh = new common::Mesh;
+          convexMesh->SetName(convexMeshName);
+          for (const auto & submesh : decomposed)
+            convexMesh->AddSubMesh(submesh);
+          meshManager.AddMesh(convexMesh);
+          if (decomposed.empty())
+          {
+            // Print an error if convex decomposition returned empty submeshes
+            // but still add it to MeshManager to avoid going through the
+            // expensive convex decomposition process for the same mesh again
+            gzerr << "Convex decomposition generated zero meshes: "
+                   << mesh->Name() << std::endl;
+          }
+          decomposedMesh = meshManager.MeshByName(convexMeshName);
+        }
+      }
+
+      if (decomposedMesh)
+      {
+        for (std::size_t j = 0u; j < decomposedMesh->SubMeshCount(); ++j)
+        {
+          auto submesh = decomposedMesh->SubMeshByIndex(j).lock();
+          gz::math::Vector3d centroid;
+          for (std::size_t i = 0; i < submesh->VertexCount(); ++i)
+              centroid += submesh->Vertex(i);
+          centroid *= 1.0/static_cast<double>(submesh->VertexCount());
+          btAlignedObjectArray<btVector3> vertices;
+          for (std::size_t i = 0; i < submesh->VertexCount(); ++i)
+          {
+            gz::math::Vector3d v = submesh->Vertex(i) - centroid;
+            vertices.push_back(convertVec(v) * scale);
+          }
+
+          float collisionMargin = 0.001f;
+          this->meshesConvex.push_back(std::make_unique<btConvexHullShape>(
+              &(vertices[0].getX()), vertices.size()));
+          auto *convexShape = this->meshesConvex.back().get();
+          convexShape->setMargin(collisionMargin);
+
+          btTransform trans;
+          trans.setIdentity();
+          trans.setOrigin(convertVec(centroid) * scale);
+          compoundShape->addChildShape(trans, convexShape);
+        }
+        meshCreated = true;
       }
+    }
+
+    if (!meshCreated)
+    {
+      for (unsigned int submeshIdx = 0;
+           submeshIdx < mesh->SubMeshCount();
+           ++submeshIdx)
+      {
+        auto s = mesh->SubMeshByIndex(submeshIdx).lock();
+        auto vertexCount = s->VertexCount();
+        auto indexCount = s->IndexCount();
+        btAlignedObjectArray<btVector3> convertedVerts;
+        convertedVerts.reserve(static_cast<int>(vertexCount));
+        for (unsigned int i = 0; i < vertexCount; i++)
+        {
+          convertedVerts.push_back(btVector3(
+                static_cast<btScalar>(s->Vertex(i).X()) * scale[0],
+                static_cast<btScalar>(s->Vertex(i).Y()) * scale[1],
+                static_cast<btScalar>(s->Vertex(i).Z()) * scale[2]));
+        }
+
+        this->triangleMeshes.push_back(std::make_unique<btTriangleMesh>());
+        for (unsigned int i = 0; i < indexCount/3; i++)
+        {
+          const btVector3& v0 = convertedVerts[s->Index(i*3)];
+          const btVector3& v1 = convertedVerts[s->Index(i*3 + 1)];
+          const btVector3& v2 = convertedVerts[s->Index(i*3 + 2)];
+          this->triangleMeshes.back()->addTriangle(v0, v1, v2);
+        }
 
-      this->meshesGImpact.push_back(
-        std::make_unique<btGImpactMeshShape>(
-          this->triangleMeshes.back().get()));
-      this->meshesGImpact.back()->updateBound();
-      this->meshesGImpact.back()->setMargin(btScalar(0.01));
-      compoundShape->addChildShape(btTransform::getIdentity(),
-        this->meshesGImpact.back().get());
+        this->meshesGImpact.push_back(
+          std::make_unique<btGImpactMeshShape>(
+            this->triangleMeshes.back().get()));
+        this->meshesGImpact.back()->updateBound();
+        this->meshesGImpact.back()->setMargin(btScalar(0.01));
+        compoundShape->addChildShape(btTransform::getIdentity(),
+          this->meshesGImpact.back().get());
+      }
     }
     shape = std::move(compoundShape);
   }
@@ -1198,6 +1289,15 @@ bool SDFFeatures::AddSdfCollision(
         btVector3(static_cast<btScalar>(mu), static_cast<btScalar>(mu2), 1),
         btCollisionObject::CF_ANISOTROPIC_FRICTION);
 
+      if (geom->MeshShape())
+      {
+        // Set meshes to use softer contacts for stability
+        // \todo(iche033) load <kp> and <kd> values from SDF
+        const btScalar kp = btScalar(1e15);
+        const btScalar kd = btScalar(1e14);
+        linkInfo->collider->setContactStiffnessAndDamping(kp, kd);
+      }
+
       if (linkIndexInModel >= 0)
       {
         model->body->getLink(linkIndexInModel).m_collider =
@@ -1217,7 +1317,24 @@ bool SDFFeatures::AddSdfCollision(
 
       auto *world = this->ReferenceInterface<WorldInfo>(model->world);
 
-      if (isStatic)
+      // Set static filter for collisions in
+      // 1) a static model
+      // 2) a fixed base link
+      // 3) a (non-base) link with zero dofs
+      bool isFixed = false;
+      if (model->body->hasFixedBase())
+      {
+        // check if it's a base link
+        isFixed = std::size_t(_linkID) ==
+            static_cast<std::size_t>(model->body->getUserIndex());
+        // check if link has zero dofs
+        if (!isFixed && linkInfo->indexInModel.has_value())
+        {
+           isFixed = model->body->getLink(
+              linkInfo->indexInModel.value()).m_dofCount == 0;
+        }
+      }
+      if (isStatic || isFixed)
       {
         world->world->addCollisionObject(
           linkInfo->collider.get(),

bullet/CMakeLists.txt

@@ -25,7 +25,7 @@ target_link_libraries(${bullet_plugin}
     gz-math${GZ_MATH_VER}::eigen3)
 
 # Note that plugins are currently being installed in 2 places: /lib and the engine-plugins dir
-install(TARGETS ${bullet_plugin} DESTINATION ${GZ_PHYSICS_ENGINE_INSTALL_DIR})
+install(TARGETS ${bullet_plugin} DESTINATION ${GZ_PHYSICS_ENGINE_RELATIVE_INSTALL_DIR})
 
 # The library created by `gz_add_component` includes the gz-physics version
 # (i.e. libgz-physics1-name-plugin.so), but for portability,
@@ -40,5 +40,5 @@ if (WIN32)
       ${GZ_PHYSICS_ENGINE_INSTALL_DIR}\/${unversioned})")
 else()
   EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E create_symlink ${versioned} ${unversioned})
-  INSTALL(FILES ${PROJECT_BINARY_DIR}/${unversioned} DESTINATION ${GZ_PHYSICS_ENGINE_INSTALL_DIR})
+  INSTALL(FILES ${PROJECT_BINARY_DIR}/${unversioned} DESTINATION ${GZ_PHYSICS_ENGINE_RELATIVE_INSTALL_DIR})
 endif()

bullet/src/plugin.cc

@@ -57,6 +57,7 @@ class Plugin :
   public: Identity InitiateEngine(std::size_t /*_engineID*/) override
   {
     const auto id = this->GetNextEntity();
+    (void)(id);
     assert(id == 0);
 
     return this->GenerateIdentity(0);

dartsim/CMakeLists.txt

@@ -55,7 +55,7 @@ else()
 endif()
 
 # Note that plugins are currently being installed in 2 places: /lib and the engine-plugins dir
-install(TARGETS ${dartsim_plugin} DESTINATION ${GZ_PHYSICS_ENGINE_INSTALL_DIR})
+install(TARGETS ${dartsim_plugin} DESTINATION ${GZ_PHYSICS_ENGINE_RELATIVE_INSTALL_DIR})
 
 # The library created by `gz_add_component` includes the gz-physics version
 # (i.e. libgz-physics1-name-plugin.so), but for portability,
@@ -70,7 +70,7 @@ if (WIN32)
       ${GZ_PHYSICS_ENGINE_INSTALL_DIR}\/${unversioned})")
 else()
   EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E create_symlink ${versioned} ${unversioned})
-  INSTALL(FILES ${PROJECT_BINARY_DIR}/${unversioned} DESTINATION ${GZ_PHYSICS_ENGINE_INSTALL_DIR})
+  INSTALL(FILES ${PROJECT_BINARY_DIR}/${unversioned} DESTINATION ${GZ_PHYSICS_ENGINE_RELATIVE_INSTALL_DIR})
 endif()
 
 # Testing

dartsim/src/EntityManagement_TEST.cc

@@ -177,6 +177,7 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld)
   const std::string meshFilename = gz::physics::test::resources::kChassisDae;
   auto &meshManager = *common::MeshManager::Instance();
   auto *mesh = meshManager.Load(meshFilename);
+  ASSERT_NE(nullptr, mesh);
 
   auto meshShape = meshLink->AttachMeshShape("chassis", *mesh);
   const auto originalMeshSize = mesh->Max() - mesh->Min();