From a52955a0ea6bdad0099b709ac95e7852410f4cf6 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 22 Apr 2024 16:37:33 -0700 Subject: [PATCH] bullet-featherstone: Support convex decomposition for meshes (#606) Supports convex decomposition on meshes. Bullet-featherstone implementation will parse the new mesh optimization attribute, decompose the mesh into convex meshes, and builds btConvexHullShape collision shapes. Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.cc | 2 +- bullet-featherstone/src/Base.hh | 2 + bullet-featherstone/src/SDFFeatures.cc | 154 ++++++++++++--- dartsim/src/EntityManagement_TEST.cc | 1 + test/common_test/collisions.cc | 247 ++++++++++++++++++++++++- test/include/test/Resources.hh | 1 + test/resources/v_shape.obj | 87 +++++++++ 7 files changed, 464 insertions(+), 30 deletions(-) create mode 100644 test/resources/v_shape.obj diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index 66f713223..ac66f5746 100644 --- a/bullet-featherstone/src/Base.cc +++ b/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 diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 9ed1d624c..652584f90 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -487,6 +487,7 @@ class Base : public Implements3d> // important. this->meshesGImpact.clear(); this->triangleMeshes.clear(); + this->meshesConvex.clear(); this->joints.clear(); @@ -520,6 +521,7 @@ class Base : public Implements3d> public: std::vector> triangleMeshes; public: std::vector> meshesGImpact; + public: std::vector> meshesConvex; }; } // namespace bullet_featherstone diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 832839b84..0679fb831 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/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(); - 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 convertedVerts; - convertedVerts.reserve(static_cast(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(s->Vertex(i).X()) * scale[0], - static_cast(s->Vertex(i).Y()) * scale[1], - static_cast(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()); - 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 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(submesh->VertexCount()); + btAlignedObjectArray 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( + &(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 convertedVerts; + convertedVerts.reserve(static_cast(vertexCount)); + for (unsigned int i = 0; i < vertexCount; i++) + { + convertedVerts.push_back(btVector3( + static_cast(s->Vertex(i).X()) * scale[0], + static_cast(s->Vertex(i).Y()) * scale[1], + static_cast(s->Vertex(i).Z()) * scale[2])); + } - this->meshesGImpact.push_back( - std::make_unique( - 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()); + 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( + 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); } @@ -1186,6 +1277,15 @@ bool SDFFeatures::AddSdfCollision( btVector3(static_cast(mu), static_cast(mu2), 1), btCollisionObject::CF_ANISOTROPIC_FRICTION); + if (geom->MeshShape()) + { + // Set meshes to use softer contacts for stability + // \todo(iche033) load and 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 = diff --git a/dartsim/src/EntityManagement_TEST.cc b/dartsim/src/EntityManagement_TEST.cc index 52a3e9b4a..b885a73f0 100644 --- a/dartsim/src/EntityManagement_TEST.cc +++ b/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(); diff --git a/test/common_test/collisions.cc b/test/common_test/collisions.cc index db2567e41..647bffa39 100644 --- a/test/common_test/collisions.cc +++ b/test/common_test/collisions.cc @@ -18,8 +18,11 @@ #include #include +#include #include +#include +#include #include #include "test/Resources.hh" @@ -40,8 +43,6 @@ #include #include -#include - #include template @@ -110,6 +111,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. @@ -147,6 +149,247 @@ TYPED_TEST(CollisionTest, MeshAndPlane) } } +using CollisionMeshFeaturesList = gz::physics::FeatureList< + gz::physics::sdf::ConstructSdfModel, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::LinkFrameSemantics, + gz::physics::ForwardStep, + gz::physics::GetEntities +>; + +using CollisionMeshTestFeaturesList = CollisionTest; + +TEST_F(CollisionMeshTestFeaturesList, MeshOptimization) +{ + // 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"( + + + )"; + modelOptimizedStr << _pose; + modelOptimizedStr << R"( + + + + + )"; + modelOptimizedStr << gz::physics::test::resources::kChassisDae; + modelOptimizedStr << R"( + + + + + + )"; + 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::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 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; + 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); + } + } +} + +TEST_F(CollisionMeshTestFeaturesList, MeshDecomposition) +{ + // Load a convex decomposed V shape mesh, drop a sphere over it + // and verify the sphere falls inside of the V shape groove and rests + // on top of it + + std::string modelStr = R"( + + + 0 0 0.0 0 0 0 + + + + + + 64 + + )"; + modelStr += gz::physics::test::resources::kVShapeObj; + modelStr += R"( + + + + + + )"; + + const std::string sphereStr = R"( + + + 0 0 1.0 0 0 0 + + + + + 0.1 + + + + + + )"; + + 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; + + auto engine = + gz::physics::RequestEngine3d::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::kVShapeObj; + auto &meshManager = *gz::common::MeshManager::Instance(); + ASSERT_NE(nullptr, meshManager.Load(meshFilename)); + + // create the v shape model + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(modelStr); + ASSERT_TRUE(errors.empty()) << errors; + ASSERT_NE(nullptr, root.Model()); + world->ConstructModel(*root.Model()); + + auto model = world->GetModel("v_shape"); + auto link = model->GetLink("link"); + auto frameDataLink = link->FrameDataRelativeToWorld(); + EXPECT_EQ(gz::math::Pose3d::Zero, + gz::math::eigen3::convert(frameDataLink.pose)); + + // spawn a sphere over the v shape model + errors = root.LoadSdfString(sphereStr); + ASSERT_TRUE(errors.empty()) << errors; + ASSERT_NE(nullptr, root.Model()); + world->ConstructModel(*root.Model()); + auto sphere = world->GetModel("sphere"); + auto sphereLink = sphere->GetLink("link"); + auto frameDataSphereLink = sphereLink->FrameDataRelativeToWorld(); + EXPECT_EQ( + gz::math::Pose3d(0, 0, 1, 0, 0, 0), + gz::math::eigen3::convert(frameDataSphereLink.pose)); + + // After a while, the sphere model should drop inside the V shape model + 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); + + frameDataLink = link->FrameDataRelativeToWorld(); + frameDataSphereLink = sphereLink->FrameDataRelativeToWorld(); + + // V shape mesh should be at the same pose + EXPECT_NEAR(0.0, frameDataLink.pose.translation().x(), 1e-3); + EXPECT_NEAR(0.0, frameDataLink.pose.translation().y(), 1e-3); + EXPECT_NEAR(0.0, frameDataLink.pose.translation().z(), 1e-2); + + // sphere should rest inside the of V shape + EXPECT_NEAR(0.0, frameDataSphereLink.pose.translation().x(), 1e-3); + EXPECT_NEAR(0.0, frameDataSphereLink.pose.translation().y(), 1e-2); + EXPECT_NEAR(0.523, frameDataSphereLink.pose.translation().z(), 1e-2); + EXPECT_NEAR(0.0, frameDataSphereLink.linearVelocity.x(), 1e-3); + EXPECT_NEAR(0.0, frameDataSphereLink.linearVelocity.y(), 1e-3); + EXPECT_NEAR(0.0, frameDataSphereLink.linearVelocity.z(), 1e-3); + } +} + using CollisionStaticFeaturesList = gz::physics::FeatureList< gz::physics::sdf::ConstructSdfModel, gz::physics::sdf::ConstructSdfWorld, diff --git a/test/include/test/Resources.hh b/test/include/test/Resources.hh index 8b2eff9c7..398580667 100644 --- a/test/include/test/Resources.hh +++ b/test/include/test/Resources.hh @@ -33,6 +33,7 @@ const auto kChassisDae = TestResource("chassis.dae"); const auto kHeightmapBowlPng = TestResource("heightmap_bowl.png"); const auto kRrbotXml = TestResource("rrbot.xml"); const auto kVolcanoTif = TestResource("volcano.tif"); +const auto kVShapeObj = TestResource("v_shape.obj"); } // namespace gz::physics::test #endif // TEST_RESOURCES_HH_ diff --git a/test/resources/v_shape.obj b/test/resources/v_shape.obj new file mode 100644 index 000000000..fd1afe0bb --- /dev/null +++ b/test/resources/v_shape.obj @@ -0,0 +1,87 @@ +# Blender 4.0.2 +# www.blender.org +o V_shape +v 0.080247 -0.297536 0.711850 +v 0.080247 -0.200536 0.711850 +v 0.080247 0.003464 0.232850 +v 0.080247 0.208464 0.711850 +v 0.080247 0.306464 0.711850 +v 0.080247 0.011464 0.023850 +v 0.080247 -0.008536 0.023850 +v -0.083133 -0.297536 0.711850 +v -0.083133 -0.200536 0.711850 +v -0.083133 0.003464 0.232850 +v -0.083133 0.208464 0.711850 +v -0.083133 0.306464 0.711850 +v -0.083133 0.011464 0.023850 +v -0.083133 -0.008536 0.023850 +vn 1.0000 -0.0000 -0.0000 +vn -1.0000 -0.0000 -0.0000 +vn -0.0000 -0.0000 1.0000 +vn -0.0000 0.9200 0.3918 +vn -0.0000 -0.9220 -0.3873 +vn -0.0000 -0.0000 -1.0000 +vn -0.0000 0.9191 -0.3941 +vn -0.0000 -0.9193 0.3935 +vt 0.833333 0.000000 +vt 0.666667 0.000000 +vt 0.500000 0.000000 +vt 0.333333 0.000000 +vt 0.166667 0.000000 +vt 0.000000 0.000000 +vt 1.000000 0.000000 +s 0 +usemtl Material +f 6/1/1 5/2/1 4/3/1 +f 6/1/1 4/3/1 3/4/1 +f 3/4/1 2/5/1 1/6/1 +f 3/4/1 1/6/1 7/7/1 +f 6/1/1 3/4/1 7/7/1 +f 13/1/2 11/3/2 12/2/2 +f 13/1/2 10/4/2 11/3/2 +f 10/4/2 8/6/2 9/5/2 +f 10/4/2 14/7/2 8/6/2 +f 13/1/2 14/7/2 10/4/2 +f 1/6/3 2/5/3 9/5/3 8/6/3 +f 4/3/3 5/2/3 12/2/3 11/3/3 +f 2/5/4 3/4/4 10/4/4 9/5/4 +f 7/7/5 1/6/5 8/6/5 14/7/5 +f 6/1/6 7/7/6 14/7/6 13/1/6 +f 5/2/7 6/1/7 13/1/7 12/2/7 +f 3/4/8 4/3/8 11/3/8 10/4/8 +o base +v -0.500000 0.500000 0.003279 +v -0.500000 0.500000 0.023279 +v 0.500000 0.500000 0.003279 +v 0.500000 0.500000 0.023279 +v -0.500000 -0.500000 0.003279 +v -0.500000 -0.500000 0.023279 +v 0.500000 -0.500000 0.003279 +v 0.500000 -0.500000 0.023279 +vn -0.0000 1.0000 -0.0000 +vn 1.0000 -0.0000 -0.0000 +vn -0.0000 -1.0000 -0.0000 +vn -1.0000 -0.0000 -0.0000 +vn -0.0000 -0.0000 -1.0000 +vn -0.0000 -0.0000 1.0000 +vt 0.375000 0.000000 +vt 0.625000 0.000000 +vt 0.625000 0.250000 +vt 0.375000 0.250000 +vt 0.625000 0.500000 +vt 0.375000 0.500000 +vt 0.625000 0.750000 +vt 0.375000 0.750000 +vt 0.625000 1.000000 +vt 0.375000 1.000000 +vt 0.125000 0.500000 +vt 0.125000 0.750000 +vt 0.875000 0.500000 +vt 0.875000 0.750000 +s 0 +f 15/8/9 16/9/9 18/10/9 17/11/9 +f 17/11/10 18/10/10 22/12/10 21/13/10 +f 21/13/11 22/12/11 20/14/11 19/15/11 +f 19/15/12 20/14/12 16/16/12 15/17/12 +f 17/18/13 21/13/13 19/15/13 15/19/13 +f 22/12/14 18/20/14 16/21/14 20/14/14