Add Cone as a collision shape #639

bperseghetti · 2024-05-15T20:33:55Z

🦟 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:

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.

Squashing commits to make requested target of main with backports to harmonic. Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>

dartsim/src/SDFFeatures.cc

dartsim/src/ShapeFeatures.cc

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>

bperseghetti · 2024-05-17T18:40:49Z

dartsim/src/SDFFeatures.cc

+    meshMgr->CreateCone(
+      coneMeshName,
+      _geometry.ConeShape()->Radius(),
+      _geometry.ConeShape()->Length(),
+      3, 40);


@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

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.

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?

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 · 2024-05-29T12:41:14Z

dartsim/src/ShapeFeatures.cc

+
+  const dart::dynamics::ShapePtr &shape = shapeInfo->node->getShape();
+
+  if (dynamic_cast<dart::dynamics::ConeShape *>(shape.get()))


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

Signed-off-by: Steve Peters <scpeters@openrobotics.org>

bperseghetti · 2024-06-03T17:51:27Z

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 · 2024-06-03T22:45:14Z

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 · 2024-06-10T20:45:16Z

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

Signed-off-by: Steve Peters <scpeters@openrobotics.org>

bperseghetti · 2024-06-12T03:03:42Z

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 · 2024-06-12T20:26:03Z

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 · 2024-06-12T20:45:57Z

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.

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>

bperseghetti · 2024-06-12T20:50:48Z

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

dartsim/src/SDFFeatures_TEST.cc

Co-authored-by: Steve Peters <computersthatmove@gmail.com> Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>

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 · 2024-06-13T17:18:19Z

rebuilding nightly debs:

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>

bperseghetti added 2 commits May 15, 2024 16:14

Adding cone primitives.

7d48115

Squashing commits to make requested target of main with backports to harmonic. Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>

Use mesh for DART collisions as cone not supported.

b447f34

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>

bperseghetti requested review from azeey, mxgrey and scpeters as code owners May 15, 2024 20:33

github-actions bot added the 🏛️ ionic Gazebo Ionic label May 15, 2024

ahcorde previously requested changes May 16, 2024

View reviewed changes

dartsim/src/SDFFeatures.cc Outdated Show resolved Hide resolved

dartsim/src/ShapeFeatures.cc Show resolved Hide resolved

dartsim/src/ShapeFeatures.cc Show resolved Hide resolved

Apply review suggestions.

d786fab

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>

bperseghetti requested a review from ahcorde May 16, 2024 19:23

bperseghetti commented May 17, 2024

View reviewed changes

scpeters reviewed May 29, 2024

View reviewed changes

Add cone to shapes tests

880f442

Signed-off-by: Steve Peters <scpeters@openrobotics.org>

bperseghetti requested a review from scpeters June 3, 2024 17:57

scpeters added 2 commits June 11, 2024 23:02

Fix tpe cone test

d05546e

Signed-off-by: Steve Peters <scpeters@openrobotics.org>

dartsim: add CustomConeMeshShape, fix tests

c41510a

Signed-off-by: Steve Peters <scpeters@openrobotics.org>

Fixes for bullet-featherstone.

d8401c2

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>

scpeters reviewed Jun 12, 2024

View reviewed changes

dartsim/src/SDFFeatures_TEST.cc Outdated Show resolved Hide resolved

bperseghetti and others added 2 commits June 13, 2024 00:23

Update dartsim/src/SDFFeatures_TEST.cc

1430917

Co-authored-by: Steve Peters <computersthatmove@gmail.com> Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>

Reduce number of overall segments for cone mesh collision.

6426920

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>

scpeters changed the title ~~Adding cone primitives.~~ Add Cone as a collision shape Jun 13, 2024

scpeters approved these changes Jun 13, 2024

View reviewed changes

scpeters merged commit 4f1b1df into gazebosim:main Jun 13, 2024
6 checks passed

bperseghetti added a commit to rudislabs/gz-physics that referenced this pull request Jun 18, 2024

Backport: Add Cone as a collision shape (gazebosim#639)

8b95a40

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>

bperseghetti deleted the pr-cone-main branch June 21, 2024 01:20

bperseghetti added a commit to rudislabs/gz-physics that referenced this pull request Jun 21, 2024

Backport: Add Cone as a collision shape (gazebosim#639)

d7b1709

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>

bperseghetti added a commit to rudislabs/gz-physics that referenced this pull request Jun 21, 2024

Backport: Add Cone as a collision shape (gazebosim#639)

5b2bd16

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add Cone as a collision shape #639

Add Cone as a collision shape #639

bperseghetti commented May 15, 2024 •

edited

bperseghetti May 17, 2024

azeey May 20, 2024

bperseghetti May 20, 2024

azeey May 20, 2024

scpeters left a comment

scpeters May 29, 2024

bperseghetti commented Jun 3, 2024

scpeters commented Jun 3, 2024

scpeters commented Jun 10, 2024

bperseghetti commented Jun 12, 2024

scpeters commented Jun 12, 2024

bperseghetti commented Jun 12, 2024 •

edited

bperseghetti commented Jun 12, 2024

scpeters left a comment

scpeters commented Jun 13, 2024


		const dart::dynamics::ShapePtr &shape = shapeInfo->node->getShape();

		if (dynamic_cast<dart::dynamics::ConeShape *>(shape.get()))

Add Cone as a collision shape #639

Add Cone as a collision shape #639

Conversation

bperseghetti commented May 15, 2024 • edited

🦟 Bug fix

Summary

Checklist

bperseghetti May 17, 2024

Choose a reason for hiding this comment

azeey May 20, 2024

Choose a reason for hiding this comment

bperseghetti May 20, 2024

Choose a reason for hiding this comment

azeey May 20, 2024

Choose a reason for hiding this comment

scpeters left a comment

Choose a reason for hiding this comment

scpeters May 29, 2024

Choose a reason for hiding this comment

bperseghetti commented Jun 3, 2024

scpeters commented Jun 3, 2024

scpeters commented Jun 10, 2024

bperseghetti commented Jun 12, 2024

scpeters commented Jun 12, 2024

bperseghetti commented Jun 12, 2024 • edited

bperseghetti commented Jun 12, 2024

scpeters left a comment

Choose a reason for hiding this comment

scpeters commented Jun 13, 2024

bperseghetti commented May 15, 2024 •

edited

bperseghetti commented Jun 12, 2024 •

edited