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

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

README.md

@@ -58,7 +58,7 @@ Gazebo Physics provides the following functionality:
   at runtime.
 * Features for common aspects of rigid body dynamic simulation
     - Construct model from [SDFormat](http://sdformat.org/) file.
-    - Collision shapes (such as box, sphere, cylinder, capsule, ellipsoid, mesh, heightmap).
+    - Collision shapes (such as box, sphere, cylinder, cone, capsule, ellipsoid, mesh, heightmap).
     - Joint types (such as revolute, prismatic, fixed, ball, screw, universal).
     - Step simulation, get/set state, apply inputs.
 * Reference implementation of physics plugin using

bullet-featherstone/README.md

@@ -65,6 +65,8 @@ These are the specific physics API features implemented.
   * AttachBoxShapeFeature
   * GetCapsuleShapeProperties
   * AttachCapsuleShapeFeature
+  * GetConeShapeProperties
+  * AttachConeShapeFeature
   * GetCylinderShapeProperties
   * AttachCylinderShapeFeature
   * GetEllipsoidShapeProperties

bullet-featherstone/src/SDFFeatures.cc

@@ -25,6 +25,7 @@
 
 #include <sdf/Box.hh>
 #include <sdf/Capsule.hh>
+#include <sdf/Cone.hh>
 #include <sdf/Cylinder.hh>
 #include <sdf/Ellipsoid.hh>
 #include <sdf/Geometry.hh>
@@ -963,6 +964,14 @@ bool SDFFeatures::AddSdfCollision(
     const auto radius = sphere->Radius();
     shape = std::make_unique<btSphereShape>(static_cast<btScalar>(radius));
   }
+  else if (const auto *cone = geom->ConeShape())
+  {
+    const auto radius = static_cast<btScalar>(cone->Radius());
+    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())
   {
     const auto radius = static_cast<btScalar>(cylinder->Radius());

bullet-featherstone/src/ShapeFeatures.cc

@@ -177,6 +177,86 @@ Identity ShapeFeatures::AttachCapsuleShape(
   return identity;
 }
 
+/////////////////////////////////////////////////
+Identity ShapeFeatures::CastToConeShape(const Identity &_shapeID) const
+{
+  const auto *shapeInfo = this->ReferenceInterface<CollisionInfo>(_shapeID);
+  if (shapeInfo != nullptr)
+  {
+    const auto &shape = shapeInfo->collider;
+    if (dynamic_cast<btConeShape*>(shape.get()))
+      return this->GenerateIdentity(_shapeID, this->Reference(_shapeID));
+  }
+
+  return this->GenerateInvalidId();
+}
+
+/////////////////////////////////////////////////
+double ShapeFeatures::GetConeShapeRadius(
+    const Identity &_coneID) const
+{
+  auto it = this->collisions.find(_coneID);
+  if (it != this->collisions.end() && it->second != nullptr)
+  {
+    if (it->second->collider != nullptr)
+    {
+      auto *cone = static_cast<btConeShape*>(
+        it->second->collider.get());
+      if (cone)
+      {
+        return cone->getRadius();
+      }
+    }
+  }
+
+  return -1;
+}
+
+/////////////////////////////////////////////////
+double ShapeFeatures::GetConeShapeHeight(
+    const Identity &_coneID) const
+{
+  auto it = this->collisions.find(_coneID);
+  if (it != this->collisions.end() && it->second != nullptr)
+  {
+    if (it->second->collider != nullptr)
+    {
+      auto *cone = static_cast<btConeShape*>(
+        it->second->collider.get());
+      if (cone)
+      {
+        return cone->getHeight();
+      }
+    }
+  }
+
+  return -1;
+}
+
+/////////////////////////////////////////////////
+Identity ShapeFeatures::AttachConeShape(
+    const Identity &_linkID,
+    const std::string &_name,
+    const double _radius,
+    const double _height,
+    const Pose3d &_pose)
+{
+  const auto radius = static_cast<btScalar>(_radius);
+  const auto height = static_cast<btScalar>(_height);
+  auto shape =
+    std::make_unique<btConeShapeZ>(radius, height);
+  shape->setMargin(0.0);
+
+  auto identity = this->AddCollision(
+    CollisionInfo{
+      _name,
+      std::move(shape),
+      _linkID,
+      _pose});
+
+  return identity;
+}
+
 /////////////////////////////////////////////////
 Identity ShapeFeatures::CastToCylinderShape(const Identity &_shapeID) const
 {

bullet-featherstone/src/ShapeFeatures.hh

@@ -21,6 +21,7 @@
 #include <gz/physics/Shape.hh>
 #include <gz/physics/BoxShape.hh>
 #include <gz/physics/CapsuleShape.hh>
+#include <gz/physics/ConeShape.hh>
 #include <gz/physics/CylinderShape.hh>
 #include <gz/physics/EllipsoidShape.hh>
 #include <gz/physics/SphereShape.hh>
@@ -42,6 +43,9 @@ struct ShapeFeatureList : FeatureList<
   GetCapsuleShapeProperties,
   AttachCapsuleShapeFeature,
 
+  GetConeShapeProperties,
+  AttachConeShapeFeature,
+
   GetCylinderShapeProperties,
   AttachCylinderShapeFeature,
 
@@ -90,6 +94,23 @@ class ShapeFeatures :
       double _length,
       const Pose3d &_pose) override;
 
+  // ----- Cone Features -----
+  public: Identity CastToConeShape(
+      const Identity &_shapeID) const override;
+
+  public: double GetConeShapeRadius(
+      const Identity &_coneID) const override;
+
+  public: double GetConeShapeHeight(
+      const Identity &_coneID) const override;
+
+  public: Identity AttachConeShape(
+      const Identity &_linkID,
+      const std::string &_name,
+      double _radius,
+      double _height,
+      const Pose3d &_pose) override;
+
   // ----- Cylinder Features -----
   public: Identity CastToCylinderShape(
       const Identity &_shapeID) const override;

bullet/src/SDFFeatures.cc

@@ -22,9 +22,10 @@
 #include <sdf/Geometry.hh>
 #include <sdf/Box.hh>
 #include <sdf/Capsule.hh>
+#include <sdf/Cone.hh>
+#include <sdf/Cylinder.hh>
 #include <sdf/Ellipsoid.hh>
 #include <sdf/Sphere.hh>
-#include <sdf/Cylinder.hh>
 #include <sdf/Plane.hh>
 #include <sdf/JointAxis.hh>
 
@@ -228,6 +229,14 @@ Identity SDFFeatures::ConstructSdfCollision(
     const auto radius = static_cast<btScalar>(sphere->Radius());
     shape = std::make_shared<btSphereShape>(radius);
   }
+  else if (geom->ConeShape())
+  {
+    const auto cone = geom->ConeShape();
+    const auto radius = static_cast<btScalar>(cone->Radius());
+    const auto height = static_cast<btScalar>(cone->Length());
+    shape =
+      std::make_shared<btConeShapeZ>(radius, height);
+  }
   else if (geom->CylinderShape())
   {
     const auto cylinder = geom->CylinderShape();

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 = 1,
+      int _meshSegments = 36);
+
+  public: gz::math::Coned cone;
+};
+
+}
+}
+}
+
+#endif  // GZ_PHYSICS_DARTSIM_SRC_CUSTOMMESHSHAPE_HH_

dartsim/src/SDFFeatures.cc

@@ -20,12 +20,14 @@
 #include <cmath>
 #include <limits>
 #include <memory>
+#include <string>
 #include <utility>
 
 #include <dart/constraint/ConstraintSolver.hpp>
 #include <dart/dynamics/BallJoint.hpp>
 #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/FreeJoint.hpp>
@@ -49,6 +51,7 @@
 #include <sdf/Box.hh>
 #include <sdf/Collision.hh>
 #include <sdf/Capsule.hh>
+#include <sdf/Cone.hh>
 #include <sdf/Cylinder.hh>
 #include <sdf/Ellipsoid.hh>
 #include <sdf/Geometry.hh>
@@ -65,6 +68,7 @@
 #include <sdf/World.hh>
 
 #include "AddedMassFeatures.hh"
+#include "CustomConeMeshShape.hh"
 #include "CustomMeshShape.hh"
 
 namespace gz {
@@ -340,11 +344,27 @@ static ShapeAndTransform ConstructGeometry(
   {
     return ConstructCapsule(*_geometry.CapsuleShape());
   }
+  else if (_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;
+    auto mesh =
+      std::make_shared<CustomConeMeshShape>(_geometry.ConeShape()->Shape());
+    auto mesh2 = std::dynamic_pointer_cast<dart::dynamics::MeshShape>(mesh);
+    return {mesh2};
+  }
   else if (_geometry.CylinderShape())
+  {
     return ConstructCylinder(*_geometry.CylinderShape());
+  }
   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())

dartsim/src/SDFFeatures_TEST.cc

@@ -972,10 +972,11 @@ 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", "ellipsoid"})
+  for (auto name : {"sphere", "box", "cylinder", "capsule", "ellipsoid",
+                    "cone"})
   {
     const auto skeleton = dartWorld->getSkeleton(count++);
     ASSERT_NE(nullptr, skeleton);