Add Cone as a collision shape

[bperseghetti]

🦟 Bug fix

Summary

This helps add the missing cone geometry for primitive/basic parametric shapes:

And is also valuable for visualizations of emitters/source that typically have conic-based spread as seen in this acoustic attack on an IMU by showing the affected area:

Associated PRs:

• Adding cone primitives. gz-gui#621
• Adding cone primitives. gz-math#594
• Adding cone primitives. gz-msgs#442
• Add Cone as a collision shape #639
• Adding cone primitives. gz-rendering#1003
• Adding cone primitives. gz-sim#2410
• Adding cone primitives. sdformat#1418

Checklist

• Signed all commits for DCO
• Added tests
• Updated documentation (as needed)
• Updated migration guide (as needed)
• Consider updating Python bindings (if the library has them)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

[bperseghetti]

@azeey @scpeters @ahcorde I set these values as they seemed to be "reasonable" tradeoff between increased computation and avoiding "lumpy" cones that can't roll. I think the problem really stems from the implementation in gz-common though, it would be nice if segments were actually a function of arc length or surface area so you get consistent segment sizes. IE the bounded area of each segment by the top vertex ring set is much much much smaller than the area bounded by the ringset adjacent to the bottom cap. I think maybe changing that here to be calculated from a segment area and number of rings instead of number of segments and number of rings would make a lot more sense: https://github.com/gazebosim/gz-common/blob/27f7017c5c1b1fd2ba9c603e92b694f98417175d/graphics/src/MeshManager.cc#L1242-L1313

[azeey]

Can the number of rings be set to 0? I would think the rolling issue is affected by the number of segments, but not rings.

Regarding the idea of making segments be a function of the arc length, I think we can do that here---if the rings can be set to 0, segments could be a simple function of the radius.

[bperseghetti]

Correct, and even for visualization I was thinking it might not be bad to have less segments with the same logic but just implemented in the gz-common I showed earlier? I guess, is there ever a case where we would desire more than one ring? I would feel like having the input to the mesh function just be number of segments might be enough for all cases?

[azeey]

is there ever a case where we would desire more than one ring?

I can't think of one. The unit_cone which is created in gz::common::MeshManager is used in gz::rendering for the cone in the transform tool. That's the only place I see it used. And for that, I don't think we need more than one ring (0 if you don't count the base circle). Maybe someone That being said, is changing the API worth the hassle?

[scpeters]

I've updated some shape tests in 71d883e (feel free to cherry-pick) to include the cone (just by duplicating cylinder and search/replace s/ylinder/one/), and I'm seeing a test failure in dart related to casting to the ConeShape. I commented on the dartsim ShapeFeatures.cc file where I think the problem comes from.

Perhaps we could create a CustomConeShape that inherits from CustomMeshShape, so we can cast to CustomConeShape instead of dart::dynamics::ConeShape?

[scpeters]

I believe this cast is failing because we are actually using a MeshShape instead of a ConeShape

[bperseghetti]

I've updated some shape tests in 71d883e (feel free to cherry-pick) to include the cone (just by duplicating cylinder and search/replace s/ylinder/one/), and I'm seeing a test failure in dart related to casting to the ConeShape. I commented on the dartsim ShapeFeatures.cc file where I think the problem comes from.

Perhaps we could create a CustomConeShape that inherits from CustomMeshShape, so we can cast to CustomConeShape instead of dart::dynamics::ConeShape?

Cherry-picked and pushed.
You have a test implementation of that method?

[scpeters]

Cherry-picked and pushed. You have a test implementation of that method?

not yet; I was just tinkering with it and since the CustomMeshShape constructor wants a gz::common::Mesh, I think we would need a factory function to return a gz::common::Mesh to use in the CustomMeshShape initializer. I'll tinker a bit more

[scpeters]

Cherry-picked and pushed. You have a test implementation of that method?

not yet; I was just tinkering with it and since the CustomMeshShape constructor wants a gz::common::Mesh, I think we would need a factory function to return a gz::common::Mesh to use in the CustomMeshShape initializer. I'll tinker a bit more

ok, I think I've gotten most of the tests fixed, except for bullet-featherstone, which complains about the bounding box size being larger due to the collision margin parameter, which we should discuss directly

• fix for tpe test: scpeters@79e38ff
• fix for dartsim tests by adding CustomConeMeshShape: scpeters@2053f15

[bperseghetti]

Cherry-picked and pushed. You have a test implementation of that method?

not yet; I was just tinkering with it and since the CustomMeshShape constructor wants a gz::common::Mesh, I think we would need a factory function to return a gz::common::Mesh to use in the CustomMeshShape initializer. I'll tinker a bit more

ok, I think I've gotten most of the tests fixed, except for bullet-featherstone, which complains about the bounding box size being larger due to the collision margin parameter, which we should discuss directly

• fix for tpe test: scpeters@79e38ff
• fix for dartsim tests by adding CustomConeMeshShape: scpeters@2053f15

Thanks, cherry-picked!

[scpeters]

the following patch will fix tests by reducing the bullet-featherstone margin to 0, but I'm not sure if that will cause problems with contact simulation

diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc
index 82fe6744..3ad3b3f5 100644
--- a/bullet-featherstone/src/SDFFeatures.cc
+++ b/bullet-featherstone/src/SDFFeatures.cc
@@ -970,6 +970,7 @@ bool SDFFeatures::AddSdfCollision(
     const auto height = static_cast<btScalar>(cone->Length());
     shape =
       std::make_unique<btConeShapeZ>(radius, height);
+    shape->setMargin(0.0);
   }
   else if (const auto *cylinder = geom->CylinderShape())
   {
diff --git a/bullet-featherstone/src/ShapeFeatures.cc b/bullet-featherstone/src/ShapeFeatures.cc
index c4702764..038ad835 100644
--- a/bullet-featherstone/src/ShapeFeatures.cc
+++ b/bullet-featherstone/src/ShapeFeatures.cc
@@ -247,6 +247,7 @@ Identity ShapeFeatures::AttachConeShape(
   const auto height = static_cast<btScalar>(_height);
   auto shape =
     std::make_unique<btConeShapeZ>(radius, height);
+  shape->setMargin(0.0);
 
   auto identity = this->AddCollision(
     CollisionInfo{

[bperseghetti]

the following patch will fix tests by reducing the bullet-featherstone margin to 0, but I'm not sure if that will cause problems with contact simulation

diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc
index 82fe6744..3ad3b3f5 100644
--- a/bullet-featherstone/src/SDFFeatures.cc
+++ b/bullet-featherstone/src/SDFFeatures.cc
@@ -970,6 +970,7 @@ bool SDFFeatures::AddSdfCollision(
     const auto height = static_cast<btScalar>(cone->Length());
     shape =
       std::make_unique<btConeShapeZ>(radius, height);
+    shape->setMargin(0.0);
   }
   else if (const auto *cylinder = geom->CylinderShape())
   {
diff --git a/bullet-featherstone/src/ShapeFeatures.cc b/bullet-featherstone/src/ShapeFeatures.cc
index c4702764..038ad835 100644
--- a/bullet-featherstone/src/ShapeFeatures.cc
+++ b/bullet-featherstone/src/ShapeFeatures.cc
@@ -247,6 +247,7 @@ Identity ShapeFeatures::AttachConeShape(
   const auto height = static_cast<btScalar>(_height);
   auto shape =
     std::make_unique<btConeShapeZ>(radius, height);
+  shape->setMargin(0.0);
 
   auto identity = this->AddCollision(
     CollisionInfo{

BTW, I do have your personal github as one of my remotes for that repo so you can always just push to the branch and I can fetch/cherry-pick anything in. 😃

But i'll just apply this diff for now anyhow.

[bperseghetti]

@scpeters thanks once again, I applied your patch, I think it just needs CI approval.

[scpeters]

thanks for working on this! I'm going to merge now to support further testing of the gz-sim PR. We can come back to adjust the bullet collision margin if necessary

the request changes have been applied

[scpeters]

rebuilding nightly debs:

• https://build.osrfoundation.org/view/gz-ionic/job/gz-physics8-debbuilder/376/
• https://build.osrfoundation.org/view/gz-ionic/job/gz-physics8-debbuilder/377/