Dart doesn't like Devo, needs "mesh pit".

Signed-off-by: Benjamin Perseghetti <[email protected]>
gazebosim · May 14, 2024 · 078395d · 078395d
1 parent fa0b698
commit 078395d
Show file tree

Hide file tree

Showing 3 changed files with 28 additions and 15 deletions.
diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc
@@ -258,14 +258,6 @@ static ShapeAndTransform ConstructBox(
         math::eigen3::convert(_box.Size()))};
 }
 
-/////////////////////////////////////////////////
-static ShapeAndTransform ConstructCone(
-    const ::sdf::Cone &_cone)
-{
-  return {std::make_shared<dart::dynamics::ConeShape>(
-        _cone.Radius(), _cone.Length())};
-}
-
 /////////////////////////////////////////////////
 static ShapeAndTransform ConstructCylinder(
     const ::sdf::Cylinder &_cylinder)
@@ -352,7 +344,22 @@ static ShapeAndTransform ConstructGeometry(
   }
   else if (_geometry.ConeShape())
   {
-    return ConstructCone(*_geometry.ConeShape());
+    // TODO(anyone): Replace this code when Cone is supported by DART
+    common::MeshManager *meshMgr = common::MeshManager::Instance();
+    std::string coneMeshName = std::string("cone_mesh")
+      + "_" + std::to_string(_geometry.ConeShape()->Radius())
+      + "_" + std::to_string(_geometry.ConeShape()->Length());
+    meshMgr->CreateCone(
+      coneMeshName,
+      _geometry.ConeShape()->Radius(),
+      _geometry.ConeShape()->Length(),
+      3, 40);
+    const gz::common::Mesh * _mesh =
+      meshMgr->MeshByName(coneMeshName);
+
+    auto mesh = std::make_shared<CustomMeshShape>(*_mesh, Vector3d(1, 1, 1));
+    auto mesh2 = std::dynamic_pointer_cast<dart::dynamics::MeshShape>(mesh);
+    return {mesh2};
   }
   else if (_geometry.CylinderShape())
   {

diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc
@@ -975,7 +975,7 @@ TEST_P(SDFFeatures_TEST, Shapes)
   ASSERT_EQ(5u, dartWorld->getNumSkeletons());
 
   int count{0};
-  for (auto name : {"sphere", "box", "cylinder", "capsule", "ellipsoid"})
+  for (auto name : {"sphere", "box", "cylinder", "capsule", "cone", "ellipsoid"})
   {
     const auto skeleton = dartWorld->getSkeleton(count++);
     ASSERT_NE(nullptr, skeleton);

diff --git a/dartsim/src/ShapeFeatures.cc b/dartsim/src/ShapeFeatures.cc
@@ -208,17 +208,23 @@ Identity ShapeFeatures::AttachConeShape(
     const double _height,
     const Pose3d &_pose)
 {
-  auto cone = std::make_shared<dart::dynamics::ConeShape>(
-        _radius, _height);
+  common::MeshManager *meshMgr = common::MeshManager::Instance();
+  std::string coneMeshName = _name + "_cone_mesh"
+    + "_" + std::to_string(_radius)
+    + "_" + std::to_string(_height);
+  meshMgr->CreateCone(coneMeshName,_radius, _height,
+    3, 40);
+  const gz::common::Mesh * _mesh = meshMgr->MeshByName(coneMeshName);
 
-  auto bn = this->ReferenceInterface<LinkInfo>(_linkID)->link;
+  auto mesh = std::make_shared<CustomMeshShape>(*_mesh, Vector3d(1, 1, 1));
+
+  DartBodyNode *bn = this->ReferenceInterface<LinkInfo>(_linkID)->link.get();
   dart::dynamics::ShapeNode *sn =
       bn->createShapeNodeWith<dart::dynamics::CollisionAspect,
                               dart::dynamics::DynamicsAspect>(
-          cone, bn->getName() + ":" + _name);
+          mesh, bn->getName() + ":" + _name);
 
   sn->setRelativeTransform(_pose);
-
   const std::size_t shapeID = this->AddShape({sn, _name});
   return this->GenerateIdentity(shapeID, this->shapes.at(shapeID));
 }