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,13 +1036,13 @@
   {
     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>();
 
@@ -1051,34 +1051,92 @@
          ++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++)
+      bool meshCreated = false;
+      if (meshSdf->Optimization() ==
+          ::sdf::MeshOptimization::CONVEX_DECOMPOSITION ||
+          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]));
+        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())
+        {
+          // limit max number of convex hulls to generate
+          maxConvexHulls = meshSdf->ConvexDecomposition()->MaxConvexHulls();
+        }
+        std::vector<common::SubMesh> decomposed =
+          std::move(meshManager.ConvexDecomposition(*s.get(), maxConvexHulls));
+
+        gzdbg << "Optimizing mesh using convex decomposition. " << std::endl;
+        if (!s->Name().empty())
+          gzdbg << "  Submesh: " << s->Name() << std::endl;
+        gzdbg << "  Mesh: " << mesh->Name() << std::endl;
+        gzdbg << "  Convex hull count: " << decomposed.size() << std::endl;
+        if (!decomposed.empty())
+        {
+          for (const auto & submesh : decomposed)
+          {
+            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;
       }
 
-      this->triangleMeshes.push_back(std::make_unique<btTriangleMesh>());
-      for (unsigned int i = 0; i < indexCount/3; i++)
+      if (!meshCreated)
       {
-        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);
-      }
+        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->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->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

@@ -19,10 +19,13 @@
 #include <string>
 
 #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 +37,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 +108,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 +146,106 @@ 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
+  std::string modelOptimizedStr = R"(
+  <sdf version="1.11">
+    <model name="model_optimized">
+      <pose>0 0 2.0 0 0 0</pose>
+      <link name="body">
+        <collision name="collision">
+          <geometry>
+            <mesh optimization="convex_decomposition">
+              <uri>)";
+  modelOptimizedStr += gz::physics::test::resources::kChassisDae;
+  modelOptimizedStr += R"(</uri>
+            </mesh>
+          </geometry>
+        </collision>
+      </link>
+    </model>
+  </sdf>)";
+
+  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));
+
+    // create the chassis model
+    {
+      sdf::Root root;
+      sdf::Errors errors = root.LoadSdfString(modelOptimizedStr);
+      ASSERT_TRUE(errors.empty()) << errors.front();
+      ASSERT_NE(nullptr, root.Model());
+      world->ConstructModel(*root.Model());
+    }
+    const std::string modelOptimizedName{"model_optimized"};
+    const std::string bodyName{"body"};
+    auto modelOptimized = world->GetModel(modelOptimizedName);
+    auto modelOptimizedBody = modelOptimized->GetLink(bodyName);
+
+    auto frameDataModelOptimizedBody =
+        modelOptimizedBody->FrameDataRelativeToWorld();
+
+    const gz::math::Pose3d initialModelOptimizedPose(0, 0, 2, 0, 0, 0);
+    EXPECT_EQ(initialModelOptimizedPose,
+              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 = 1000u;
+    for (unsigned int i = 0; i < stepCount; ++i)
+      world->Step(output, state, input);
+
+    frameDataModelOptimizedBody =
+        modelOptimizedBody->FrameDataRelativeToWorld();
+    EXPECT_NEAR(0.1,
+                frameDataModelOptimizedBody.pose.translation().z(), 1e-3);
+    EXPECT_NEAR(0.0, frameDataModelOptimizedBody.linearVelocity.z(), 1e-3);
+  }
+}
+
 int main(int argc, char *argv[])
 {
   ::testing::InitGoogleTest(&argc, argv);