Add Cone as a collision shape

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,13 @@ 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);
+  }
   else if (const auto *cylinder = geom->CylinderShape())
   {
     const auto radius = static_cast<btScalar>(cylinder->Radius());

bullet-featherstone/src/ShapeFeatures.cc

@@ -179,6 +179,85 @@ 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);
+
+  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/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>
@@ -340,11 +343,38 @@ 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;
+    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())
+  {
     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

@@ -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);

dartsim/src/ShapeFeatures.cc

@@ -17,10 +17,13 @@
 
 #include "ShapeFeatures.hh"
 
+#include <cstddef>
 #include <memory>
+#include <string>
 
 #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>
@@ -161,6 +164,76 @@ Identity ShapeFeatures::AttachCapsuleShape(
   return this->GenerateIdentity(shapeID, this->shapes.at(shapeID));
 }
 
+/////////////////////////////////////////////////
+Identity ShapeFeatures::CastToConeShape(const Identity &_shapeID) const
+{
+  const auto *shapeInfo = this->ReferenceInterface<ShapeInfo>(_shapeID);
+
+  const dart::dynamics::ShapePtr &shape = shapeInfo->node->getShape();
+
+  if (dynamic_cast<dart::dynamics::ConeShape *>(shape.get()))
+    return this->GenerateIdentity(_shapeID, this->Reference(_shapeID));
+
+  return this->GenerateInvalidId();
+}
+
+/////////////////////////////////////////////////
+double ShapeFeatures::GetConeShapeRadius(
+    const Identity &_coneID) const
+{
+  const auto *shapeInfo = this->ReferenceInterface<ShapeInfo>(_coneID);
+
+  dart::dynamics::ConeShape *cone =
+      static_cast<dart::dynamics::ConeShape *>(
+          shapeInfo->node->getShape().get());
+
+  return cone->getRadius();
+}
+
+/////////////////////////////////////////////////
+double ShapeFeatures::GetConeShapeHeight(
+    const Identity &_coneID) const
+{
+  const auto *shapeInfo = this->ReferenceInterface<ShapeInfo>(_coneID);
+  dart::dynamics::ConeShape *cone =
+      static_cast<dart::dynamics::ConeShape *>(
+          shapeInfo->node->getShape().get());
+
+  return cone->getHeight();
+}
+
+/////////////////////////////////////////////////
+Identity ShapeFeatures::AttachConeShape(
+    const Identity &_linkID,
+    const std::string &_name,
+    const double _radius,
+    const double _height,
+    const Pose3d &_pose)
+{
+  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));
+
+  DartBodyNode *bn = this->ReferenceInterface<LinkInfo>(_linkID)->link.get();
+  dart::dynamics::ShapeNode *sn =
+      bn->createShapeNodeWith<dart::dynamics::CollisionAspect,
+                              dart::dynamics::DynamicsAspect>(
+          mesh, bn->getName() + ":" + _name);
+
+  sn->setRelativeTransform(_pose);
+  const std::size_t shapeID = this->AddShape({sn, _name});
+  return this->GenerateIdentity(shapeID, this->shapes.at(shapeID));
+}
+
 /////////////////////////////////////////////////
 Identity ShapeFeatures::CastToCylinderShape(const Identity &_shapeID) const
 {
@@ -255,6 +328,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])