dartsim: add CustomConeMeshShape, fix tests

Signed-off-by: Steve Peters <[email protected]>

dartsim/src/CustomConeMeshShape.cc

@@ -0,0 +1,62 @@
+/*
+ * Copyright (C) 2018 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include "CustomConeMeshShape.hh"
+
+#include <memory>
+#include <string>
+
+#include <gz/common/Console.hh>
+#include <gz/common/MeshManager.hh>
+#include <gz/math/Cone.hh>
+
+namespace gz {
+namespace physics {
+namespace dartsim {
+
+/////////////////////////////////////////////////
+const gz::common::Mesh* MakeCustomConeMesh(
+    const gz::math::Coned &_cone,
+    int _meshRings,
+    int _meshSegments)
+{
+  common::MeshManager *meshMgr = common::MeshManager::Instance();
+  std::string coneMeshName = std::string("cone_mesh")
+    + "_" + std::to_string(_cone.Radius())
+    + "_" + std::to_string(_cone.Length());
+  meshMgr->CreateCone(
+    coneMeshName,
+    _cone.Radius(),
+    _cone.Length(),
+    _meshRings, _meshSegments);
+  return meshMgr->MeshByName(coneMeshName);
+}
+
+/////////////////////////////////////////////////
+CustomConeMeshShape::CustomConeMeshShape(
+    const gz::math::Coned &_cone,
+    int _meshRings,
+    int _meshSegments)
+  : CustomMeshShape(*MakeCustomConeMesh(_cone, _meshRings, _meshSegments),
+                    Eigen::Vector3d(1, 1, 1)),
+    cone(_cone)
+{
+}
+
+}
+}
+}

dartsim/src/CustomConeMeshShape.hh

@@ -0,0 +1,44 @@
+/*
+ * Copyright (C) 2024 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef GZ_PHYSICS_DARTSIM_SRC_CUSTOMCONEMESHSHAPE_HH_
+#define GZ_PHYSICS_DARTSIM_SRC_CUSTOMCONEMESHSHAPE_HH_
+
+#include <gz/math/Cone.hh>
+#include "CustomMeshShape.hh"
+
+namespace gz {
+namespace physics {
+namespace dartsim {
+
+/// \brief This class creates a custom derivative of the CustomMeshShape to
+/// allow casting to a Cone shape.
+class CustomConeMeshShape : public CustomMeshShape
+{
+  public: CustomConeMeshShape(
+      const gz::math::Coned &_cone,
+      int _meshRings = 3,
+      int _meshSegments = 40);
+
+  public: gz::math::Coned cone;
+};
+
+}
+}
+}
+
+#endif  // GZ_PHYSICS_DARTSIM_SRC_CUSTOMMESHSHAPE_HH_

dartsim/src/SDFFeatures.cc

@@ -68,6 +68,7 @@
 #include <sdf/World.hh>
 
 #include "AddedMassFeatures.hh"
+#include "CustomConeMeshShape.hh"
 #include "CustomMeshShape.hh"
 
 namespace gz {
@@ -349,19 +350,8 @@ static ShapeAndTransform ConstructGeometry(
     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 mesh =
+      std::make_shared<CustomConeMeshShape>(_geometry.ConeShape()->Shape());
     auto mesh2 = std::dynamic_pointer_cast<dart::dynamics::MeshShape>(mesh);
     return {mesh2};
   }

dartsim/src/SDFFeatures_TEST.cc

@@ -972,10 +972,10 @@ TEST_P(SDFFeatures_TEST, Shapes)
   auto dartWorld = world->GetDartsimWorld();
   ASSERT_NE(nullptr, dartWorld);
 
-  ASSERT_EQ(5u, dartWorld->getNumSkeletons());
+  ASSERT_EQ(6u, dartWorld->getNumSkeletons());
 
   int count{0};
-  for (auto name : {"sphere", "box", "cylinder", "capsule", "cone", "ellipsoid"})
+  for (auto name : {"sphere", "box", "cylinder", "capsule", "ellipsoid", "cone"})
   {
     const auto skeleton = dartWorld->getSkeleton(count++);
     ASSERT_NE(nullptr, skeleton);

dartsim/src/ShapeFeatures.cc

@@ -23,7 +23,6 @@
 
 #include <dart/dynamics/BoxShape.hpp>
 #include <dart/dynamics/CapsuleShape.hpp>
-#include <dart/dynamics/ConeShape.hpp>
 #include <dart/dynamics/CylinderShape.hpp>
 #include <dart/dynamics/EllipsoidShape.hpp>
 #include <dart/dynamics/HeightmapShape.hpp>
@@ -35,6 +34,7 @@
 #include <gz/common/Mesh.hh>
 #include <gz/common/MeshManager.hh>
 
+#include "CustomConeMeshShape.hh"
 #include "CustomHeightmapShape.hh"
 #include "CustomMeshShape.hh"
 
@@ -171,7 +171,7 @@ Identity ShapeFeatures::CastToConeShape(const Identity &_shapeID) const
 
   const dart::dynamics::ShapePtr &shape = shapeInfo->node->getShape();
 
-  if (dynamic_cast<dart::dynamics::ConeShape *>(shape.get()))
+  if (dynamic_cast<CustomMeshShape *>(shape.get()))
     return this->GenerateIdentity(_shapeID, this->Reference(_shapeID));
 
   return this->GenerateInvalidId();
@@ -183,23 +183,24 @@ double ShapeFeatures::GetConeShapeRadius(
 {
   const auto *shapeInfo = this->ReferenceInterface<ShapeInfo>(_coneID);
 
-  dart::dynamics::ConeShape *cone =
-      static_cast<dart::dynamics::ConeShape *>(
+  auto *coneShape =
+      static_cast<CustomConeMeshShape *>(
           shapeInfo->node->getShape().get());
 
-  return cone->getRadius();
+  return coneShape->cone.Radius();
 }
 
 /////////////////////////////////////////////////
 double ShapeFeatures::GetConeShapeHeight(
     const Identity &_coneID) const
 {
   const auto *shapeInfo = this->ReferenceInterface<ShapeInfo>(_coneID);
-  dart::dynamics::ConeShape *cone =
-      static_cast<dart::dynamics::ConeShape *>(
+
+  auto *coneShape =
+      static_cast<CustomConeMeshShape *>(
           shapeInfo->node->getShape().get());
 
-  return cone->getHeight();
+  return coneShape->cone.Length();
 }
 
 /////////////////////////////////////////////////
@@ -213,15 +214,8 @@ Identity ShapeFeatures::AttachConeShape(
   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 mesh = std::make_shared<CustomMeshShape>(*_mesh, Vector3d(1, 1, 1));
+  auto mesh =
+    std::make_shared<CustomConeMeshShape>(gz::math::Coned(_height, _radius));
 
   DartBodyNode *bn = this->ReferenceInterface<LinkInfo>(_linkID)->link.get();
   dart::dynamics::ShapeNode *sn =