bullet-featherstone: Support convex decomposition for meshes

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/SDFFeatures.cc

@@ -1036,49 +1036,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)
+      {
+        /// create 1 convex hull for the whole submesh
+        maxConvexHulls = 1u;
+      }
+      else if (meshSdf->ConvexDecomposition())
       {
-        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]));
+        // 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 s = mergedMesh->SubMeshByIndex(0u).lock();
+          std::vector<common::SubMesh> decomposed =
+            gz::common::MeshManager::ConvexDecomposition(
+            *s.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);
+        }
       }
 
-      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());
+      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());
+      }
     }
     shape = std::move(compoundShape);
   }

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

test/common_test/collisions.cc

@@ -17,12 +17,16 @@
 #include <gtest/gtest.h>
 
 #include <string>
+#include <unordered_set>
 
 #include <gz/common/Console.hh>
+#include <gz/common/MeshManager.hh>
+#include <gz/math/eigen3/Conversions.hh>
 #include <gz/plugin/Loader.hh>
 
 #include "test/Resources.hh"
 #include "test/TestLibLoader.hh"
+#include "Worlds.hh"
 
 #include <gz/physics/FindFeatures.hh>
 #include <gz/physics/RequestEngine.hh>
@@ -34,9 +38,10 @@
 #include <gz/physics/mesh/MeshShape.hh>
 #include <gz/physics/PlaneShape.hh>
 #include <gz/physics/FixedJoint.hh>
+#include <gz/physics/sdf/ConstructModel.hh>
 #include <gz/physics/sdf/ConstructWorld.hh>
 
-#include <gz/common/MeshManager.hh>
+#include <sdf/Root.hh>
 
 template <class T>
 class CollisionTest:
@@ -104,6 +109,7 @@ TYPED_TEST(CollisionTest, MeshAndPlane)
     const std::string meshFilename = gz::physics::test::resources::kChassisDae;
     auto &meshManager = *gz::common::MeshManager::Instance();
     auto *mesh = meshManager.Load(meshFilename);
+    ASSERT_NE(nullptr, mesh);
 
     // TODO(anyone): This test is somewhat awkward because we lift up the mesh
     // from the center of the link instead of lifting up the link or the model.
@@ -141,6 +147,133 @@ TYPED_TEST(CollisionTest, MeshAndPlane)
   }
 }
 
+struct CollisionMeshFeaturesList : gz::physics::FeatureList<
+  gz::physics::sdf::ConstructSdfModel,
+  gz::physics::sdf::ConstructSdfWorld,
+  gz::physics::LinkFrameSemantics,
+  gz::physics::ForwardStep,
+  gz::physics::GetEntities
+> { };
+
+template <class T>
+class CollisionMeshTest :
+  public CollisionTest<T>{};
+using CollisionMeshTestTypes =
+  ::testing::Types<CollisionMeshFeaturesList>;
+TYPED_TEST_SUITE(CollisionMeshTest,
+                 CollisionMeshTestTypes);
+
+TYPED_TEST(CollisionMeshTest, MeshDecomposition)
+{
+  // Load an optimized mesh, drop it from some height,
+  // and verify it collides with the ground plane
+
+  auto getModelOptimizedStr = [](const std::string &_optimization,
+                                 const std::string &_name,
+                                 const gz::math::Pose3d &_pose)
+  {
+    std::stringstream modelOptimizedStr;
+    modelOptimizedStr << R"(
+    <sdf version="1.11">
+      <model name=")";
+    modelOptimizedStr << _name << R"(">
+        <pose>)";
+    modelOptimizedStr << _pose;
+    modelOptimizedStr << R"(</pose>
+        <link name="body">
+          <collision name="collision">
+            <geometry>
+              <mesh optimization=")";
+    modelOptimizedStr << _optimization;
+    modelOptimizedStr << R"(">
+                <uri>)";
+    modelOptimizedStr << gz::physics::test::resources::kChassisDae;
+    modelOptimizedStr << R"(</uri>
+              </mesh>
+            </geometry>
+          </collision>
+        </link>
+      </model>
+    </sdf>)";
+    return modelOptimizedStr.str();
+  };
+
+  for (const std::string &name : this->pluginNames)
+  {
+    // currently only bullet-featherstone supports mesh decomposition
+    if (this->PhysicsEngineName(name) != "bullet-featherstone")
+      continue;
+    std::cout << "Testing plugin: " << name << std::endl;
+    gz::plugin::PluginPtr plugin = this->loader.Instantiate(name);
+
+    sdf::Root rootWorld;
+    const sdf::Errors errorsWorld =
+        rootWorld.Load(common_test::worlds::kGroundSdf);
+    ASSERT_TRUE(errorsWorld.empty()) << errorsWorld.front();
+
+    auto engine =
+        gz::physics::RequestEngine3d<CollisionMeshFeaturesList>::From(plugin);
+    ASSERT_NE(nullptr, engine);
+
+    auto world = engine->ConstructWorld(*rootWorld.WorldByIndex(0));
+    ASSERT_NE(nullptr, world);
+
+    // load the mesh into mesh manager first to create a cache
+    // so the model can be constructed later - needed by bullet-featherstone
+    const std::string meshFilename = gz::physics::test::resources::kChassisDae;
+    auto &meshManager = *gz::common::MeshManager::Instance();
+    ASSERT_NE(nullptr, meshManager.Load(meshFilename));
+
+    std::unordered_set<std::string> optimizations;
+    optimizations.insert("");
+    optimizations.insert("convex_decomposition");
+    optimizations.insert("convex_hull");
+
+    gz::math::Pose3d initialModelPose(0, 0, 2, 0, 0, 0);
+    // Test all optimization methods
+    for (const auto &optimizationStr : optimizations)
+    {
+      // create the chassis model
+      const std::string modelOptimizedName = "model_optimized_" + optimizationStr;
+      sdf::Root root;
+      sdf::Errors errors = root.LoadSdfString(getModelOptimizedStr(
+          optimizationStr, modelOptimizedName, initialModelPose));
+      ASSERT_TRUE(errors.empty()) << errors.front();
+      ASSERT_NE(nullptr, root.Model());
+      world->ConstructModel(*root.Model());
+
+      const std::string bodyName{"body"};
+      auto modelOptimized = world->GetModel(modelOptimizedName);
+      auto modelOptimizedBody = modelOptimized->GetLink(bodyName);
+
+      auto frameDataModelOptimizedBody =
+          modelOptimizedBody->FrameDataRelativeToWorld();
+
+      EXPECT_EQ(initialModelPose,
+                gz::math::eigen3::convert(frameDataModelOptimizedBody.pose));
+
+      // After a while, the mesh model should reach the ground and come to a stop
+      gz::physics::ForwardStep::Output output;
+      gz::physics::ForwardStep::State state;
+      gz::physics::ForwardStep::Input input;
+      std::size_t stepCount = 3000u;
+      for (unsigned int i = 0; i < stepCount; ++i)
+        world->Step(output, state, input);
+
+      frameDataModelOptimizedBody =
+          modelOptimizedBody->FrameDataRelativeToWorld();
+
+      // convex decomposition gives more accurate results
+      double tol = (optimizationStr == "convex_decomposition") ? 1e-3 : 1e-2;
+      EXPECT_NEAR(0.1,
+                  frameDataModelOptimizedBody.pose.translation().z(), tol);
+      EXPECT_NEAR(0.0, frameDataModelOptimizedBody.linearVelocity.z(), tol);
+
+      initialModelPose.Pos() += gz::math::Vector3d(0, 2, 0);
+    }
+  }
+}
+
 int main(int argc, char *argv[])
 {
   ::testing::InitGoogleTest(&argc, argv);