Use mesh for DART collisions as cone not supported.

Signed-off-by: Benjamin Perseghetti <[email protected]>
gazebosim · May 15, 2024 · b0dba0e · b0dba0e
1 parent 371b6f2
commit b0dba0e
Show file tree

Hide file tree

Showing 5 changed files with 47 additions and 17 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,25 @@ static ShapeAndTransform ConstructGeometry(
   }
   else if (_geometry.ConeShape())
   {
-    return ConstructCone(*_geometry.ConeShape());
+    // TODO(anyone): Replace this code when Cone is supported by DART
+    gzwarn << "DART: Cone is not a supported collision geomerty"
+           << " primitive, using generated mesh of a cone instead"
+           << std::endl;
+    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())
   {
@@ -361,6 +371,9 @@ static ShapeAndTransform ConstructGeometry(
   else if (_geometry.EllipsoidShape())
   {
     // TODO(anyone): Replace this code when Ellipsoid is supported by DART
+    gzwarn << "DART: Ellipsoid is not a supported collision geomerty"
+           << " primitive, using generated mesh of an ellipsoid instead"
+           << std::endl;
     common::MeshManager *meshMgr = common::MeshManager::Instance();
     std::string ellipsoidMeshName = std::string("ellipsoid_mesh")
       + "_" + std::to_string(_geometry.EllipsoidShape()->Radii().X())

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,26 @@ Identity ShapeFeatures::AttachConeShape(
     const double _height,
     const Pose3d &_pose)
 {
-  auto cone = std::make_shared<dart::dynamics::ConeShape>(
-        _radius, _height);
+  gzwarn << "DART: Cone is not a supported collision geomerty"
+         << " primitive, using generated mesh of a cone instead"
+         << std::endl;
+  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));
 }
@@ -317,6 +326,9 @@ Identity ShapeFeatures::AttachEllipsoidShape(
     const Vector3d &_radii,
     const Pose3d &_pose)
 {
+  gzwarn << "DART: Ellipsoid is not a supported collision geomerty"
+         << " primitive, using generated mesh of an ellipsoid instead"
+         << std::endl;
   common::MeshManager *meshMgr = common::MeshManager::Instance();
   std::string ellipsoidMeshName = _name + "_ellipsoid_mesh"
     + "_" + std::to_string(_radii[0])

diff --git a/include/gz/physics/ConeShape.hh b/include/gz/physics/ConeShape.hh
@@ -1,5 +1,7 @@
 /*
- * Copyright (C) 2018 Open Source Robotics Foundation
+ * Copyright 2024 CogniPilot Foundation
+ * Copyright 2024 Open Source Robotics Foundation
+ * Copyright 2024 Rudis Laboratories
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -113,6 +115,7 @@ namespace gz
         public: using ShapePtrType = ConeShapePtr<PolicyT, FeaturesT>;
 
         /// \brief Rigidly attach a ConeShape to this link.
+        /// \param[in] _name
         /// \param[in] _radius
         ///   The radius of the cone.
         /// \param[in] _height

diff --git a/include/gz/physics/detail/ConeShape.hh b/include/gz/physics/detail/ConeShape.hh
@@ -1,5 +1,7 @@
 /*
- * Copyright (C) 2018 Open Source Robotics Foundation
+ * Copyright 2024 CogniPilot Foundation
+ * Copyright 2024 Open Source Robotics Foundation
+ * Copyright 2024 Rudis Laboratories
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.