Skip to content

Commit

Permalink
merge all submeshes before decomposition. Generalize test
Browse files Browse the repository at this point in the history 
Signed-off-by: Ian Chen <ichen@openrobotics.org>
  • Loading branch information
@iche033
iche033 committed Mar 26, 2024
1 parent 8d35d79 commit 1f2aaa7
Show file tree
Hide file tree
Showing 2 changed files with 89 additions and 59 deletions.
38 changes: 20 additions & 18 deletions bullet-featherstone/src/SDFFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1046,16 +1046,14 @@ bool SDFFeatures::AddSdfCollision(

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();
bool meshCreated = false;
if (meshSdf->Optimization() ==
::sdf::MeshOptimization::CONVEX_DECOMPOSITION ||
meshSdf->Optimization() ==
::sdf::MeshOptimization::CONVEX_HULL)
auto mergedMesh = gz::common::MeshManager::MergeSubMeshes(*mesh);
if (mergedMesh && mergedMesh->SubMeshCount() == 1u)
{
std::size_t maxConvexHulls = 16u;
if (meshSdf->Optimization() == ::sdf::MeshOptimization::CONVEX_HULL)
Expand All @@ -1068,14 +1066,12 @@ bool SDFFeatures::AddSdfCollision(
// limit max number of convex hulls to generate
maxConvexHulls = meshSdf->ConvexDecomposition()->MaxConvexHulls();

Check warning on line 1067 in bullet-featherstone/src/SDFFeatures.cc

View check run for this annotation

Codecov / codecov/patch

bullet-featherstone/src/SDFFeatures.cc#L1067 

Added line #L1067 was not covered by tests
}
auto s = mergedMesh->SubMeshByIndex(0u).lock();
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;
std::move(gz::common::MeshManager::ConvexDecomposition(
*s.get(), maxConvexHulls));
gzdbg << "Optimizing mesh (" << meshSdf->OptimizationStr() << "): "
<< mesh->Name() << std::endl;
if (!decomposed.empty())
{
for (const auto & submesh : decomposed)
Expand Down Expand Up @@ -1103,11 +1099,17 @@ bool SDFFeatures::AddSdfCollision(
compoundShape->addChildShape(trans, convexShape);
}
}
meshCreated = true;
}
meshCreated = true;
}

if (!meshCreated)
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;
Expand Down
110 changes: 69 additions & 41 deletions test/common_test/collisions.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
#include <gtest/gtest.h>

#include <string>
#include <unordered_set>

#include <gz/common/Console.hh>
#include <gz/common/MeshManager.hh>
Expand Down Expand Up @@ -166,23 +167,36 @@ 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>)";

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)
{
Expand Down Expand Up @@ -210,39 +224,53 @@ TYPED_TEST(CollisionMeshTest, MeshDecomposition)
auto &meshManager = *gz::common::MeshManager::Instance();
ASSERT_NE(nullptr, meshManager.Load(meshFilename));

// create the chassis model
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(modelOptimizedStr);
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 modelOptimizedName{"model_optimized"};
const std::string bodyName{"body"};
auto modelOptimized = world->GetModel(modelOptimizedName);
auto modelOptimizedBody = modelOptimized->GetLink(bodyName);

auto frameDataModelOptimizedBody =
modelOptimizedBody->FrameDataRelativeToWorld();
const std::string bodyName{"body"};
auto modelOptimized = world->GetModel(modelOptimizedName);
auto modelOptimizedBody = modelOptimized->GetLink(bodyName);

const gz::math::Pose3d initialModelOptimizedPose(0, 0, 2, 0, 0, 0);
EXPECT_EQ(initialModelOptimizedPose,
gz::math::eigen3::convert(frameDataModelOptimizedBody.pose));
auto frameDataModelOptimizedBody =
modelOptimizedBody->FrameDataRelativeToWorld();

// 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);
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();
EXPECT_NEAR(0.1,
frameDataModelOptimizedBody.pose.translation().z(), 1e-3);
EXPECT_NEAR(0.0, frameDataModelOptimizedBody.linearVelocity.z(), 1e-3);
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);
}
}
}

Expand Down

0 comments on commit 1f2aaa7

Please sign in to comment.