Merge branch 'main' into chapulina/4/collision_detector

Signed-off-by: Louise Poubel <louise@openrobotics.org>

dartsim/src/SDFFeatures.cc

@@ -24,7 +24,9 @@
 #include <dart/constraint/ConstraintSolver.hpp>
 #include <dart/dynamics/BallJoint.hpp>
 #include <dart/dynamics/BoxShape.hpp>
+#include <dart/dynamics/CapsuleShape.hpp>
 #include <dart/dynamics/CylinderShape.hpp>
+#include <dart/dynamics/EllipsoidShape.hpp>
 #include <dart/dynamics/FreeJoint.hpp>
 #include <dart/dynamics/MeshShape.hpp>
 #include <dart/dynamics/PlaneShape.hpp>
@@ -37,12 +39,16 @@
 #include <dart/dynamics/WeldJoint.hpp>
 
 #include <ignition/common/Console.hh>
+#include <ignition/common/Mesh.hh>
+#include <ignition/common/MeshManager.hh>
 #include <ignition/math/eigen3/Conversions.hh>
 #include <ignition/math/Helpers.hh>
 
 #include <sdf/Box.hh>
 #include <sdf/Collision.hh>
+#include <sdf/Capsule.hh>
 #include <sdf/Cylinder.hh>
+#include <sdf/Ellipsoid.hh>
 #include <sdf/Geometry.hh>
 #include <sdf/Joint.hh>
 #include <sdf/JointAxis.hh>
@@ -55,6 +61,8 @@
 #include <sdf/Visual.hh>
 #include <sdf/World.hh>
 
+#include "CustomMeshShape.hh"
+
 namespace ignition {
 namespace physics {
 namespace dartsim {
@@ -273,6 +281,14 @@ static ShapeAndTransform ConstructSphere(
   return {std::make_shared<dart::dynamics::SphereShape>(_sphere.Radius())};
 }
 
+/////////////////////////////////////////////////
+static ShapeAndTransform ConstructCapsule(
+    const ::sdf::Capsule &_capsule)
+{
+  return {std::make_shared<dart::dynamics::CapsuleShape>(
+        _capsule.Radius(), _capsule.Length())};
+}
+
 /////////////////////////////////////////////////
 static ShapeAndTransform ConstructPlane(
     const ::sdf::Plane &_plane)
@@ -315,8 +331,31 @@ static ShapeAndTransform ConstructGeometry(
 {
   if (_geometry.BoxShape())
     return ConstructBox(*_geometry.BoxShape());
+  else if (_geometry.CapsuleShape())
+  {
+    return ConstructCapsule(*_geometry.CapsuleShape());
+  }
   else if (_geometry.CylinderShape())
     return ConstructCylinder(*_geometry.CylinderShape());
+  else if (_geometry.EllipsoidShape())
+  {
+    // TODO(anyone): Replace this code when Ellipsoid is supported by DART
+    common::MeshManager *meshMgr = common::MeshManager::Instance();
+    std::string ellipsoidMeshName = std::string("ellipsoid_mesh")
+      + "_" + std::to_string(_geometry.EllipsoidShape()->Radii().X())
+      + "_" + std::to_string(_geometry.EllipsoidShape()->Radii().Y())
+      + "_" + std::to_string(_geometry.EllipsoidShape()->Radii().Z());
+    meshMgr->CreateEllipsoid(
+      ellipsoidMeshName,
+      _geometry.EllipsoidShape()->Radii(),
+      6, 12);
+    const ignition::common::Mesh * _mesh =
+      meshMgr->MeshByName(ellipsoidMeshName);
+
+    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.SphereShape())
     return ConstructSphere(*_geometry.SphereShape());
   else if (_geometry.PlaneShape())
@@ -739,6 +778,8 @@ Identity SDFFeatures::ConstructSdfCollision(
   if (!shape)
   {
     // The geometry element was empty, or the shape type is not supported
+    ignerr << "The geometry element of collision [" << _collision.Name() << "] "
+           << "couldn't be created\n";
     return this->GenerateInvalidId();
   }
 
@@ -853,6 +894,8 @@ Identity SDFFeatures::ConstructSdfVisual(
   if (!shape)
   {
     // The geometry element was empty, or the shape type is not supported
+    ignerr << "The geometry element of visual [" << _visual.Name() << "] "
+           << "couldn't be created\n";
     return this->GenerateInvalidId();
   }
 

dartsim/src/SDFFeatures_TEST.cc

@@ -862,6 +862,27 @@ TEST_P(SDFFeatures_FrameSemantics, ExplicitWorldFrames)
       expPose, link1->getWorldTransform(), 1e-5));
 }
 
+/////////////////////////////////////////////////
+TEST_P(SDFFeatures_TEST, Shapes)
+{
+  auto world = this->LoadWorld(TEST_WORLD_DIR"/shapes.sdf");
+  ASSERT_NE(nullptr, world);
+
+  auto dartWorld = world->GetDartsimWorld();
+  ASSERT_NE(nullptr, dartWorld);
+
+  ASSERT_EQ(5u, dartWorld->getNumSkeletons());
+
+  int count{0};
+  for (auto name : {"box", "cylinder", "sphere", "capsule", "ellipsoid"})
+  {
+    const auto skeleton = dartWorld->getSkeleton(count++);
+    ASSERT_NE(nullptr, skeleton);
+    EXPECT_EQ(name, skeleton->getName());
+    ASSERT_EQ(1u, skeleton->getNumBodyNodes());
+  }
+}
+
 int main(int argc, char *argv[])
 {
   ::testing::InitGoogleTest(&argc, argv);

dartsim/src/ShapeFeatures.cc

@@ -20,12 +20,17 @@
 #include <memory>
 
 #include <dart/dynamics/BoxShape.hpp>
+#include <dart/dynamics/CapsuleShape.hpp>
 #include <dart/dynamics/CylinderShape.hpp>
+#include <dart/dynamics/EllipsoidShape.hpp>
 #include <dart/dynamics/MeshShape.hpp>
 #include <dart/dynamics/PlaneShape.hpp>
 #include <dart/dynamics/Shape.hpp>
 #include <dart/dynamics/SphereShape.hpp>
 
+#include <ignition/common/Mesh.hh>
+#include <ignition/common/MeshManager.hh>
+
 #include "CustomMeshShape.hh"
 
 namespace ignition {
@@ -93,6 +98,67 @@ Identity ShapeFeatures::AttachBoxShape(
   return this->GenerateIdentity(shapeID, this->shapes.at(shapeID));
 }
 
+/////////////////////////////////////////////////
+Identity ShapeFeatures::CastToCapsuleShape(const Identity &_shapeID) const
+{
+  const auto *shapeInfo = this->ReferenceInterface<ShapeInfo>(_shapeID);
+
+  const dart::dynamics::ShapePtr &shape = shapeInfo->node->getShape();
+
+  if (dynamic_cast<dart::dynamics::CapsuleShape *>(shape.get()))
+    return this->GenerateIdentity(_shapeID, this->Reference(_shapeID));
+
+  return this->GenerateInvalidId();
+}
+
+/////////////////////////////////////////////////
+double ShapeFeatures::GetCapsuleShapeRadius(
+    const Identity &_capsuleID) const
+{
+  const auto *shapeInfo = this->ReferenceInterface<ShapeInfo>(_capsuleID);
+
+  dart::dynamics::CapsuleShape *capsule =
+      static_cast<dart::dynamics::CapsuleShape *>(
+          shapeInfo->node->getShape().get());
+
+  return capsule->getRadius();
+}
+
+/////////////////////////////////////////////////
+double ShapeFeatures::GetCapsuleShapeLength(
+    const Identity &_capsuleID) const
+{
+  const auto *shapeInfo = this->ReferenceInterface<ShapeInfo>(_capsuleID);
+  dart::dynamics::CapsuleShape *capsule =
+      static_cast<dart::dynamics::CapsuleShape *>(
+          shapeInfo->node->getShape().get());
+
+  return capsule->getHeight();
+}
+
+/////////////////////////////////////////////////
+Identity ShapeFeatures::AttachCapsuleShape(
+    const Identity &_linkID,
+    const std::string &_name,
+    const double _radius,
+    const double _length,
+    const Pose3d &_pose)
+{
+  auto capsule = std::make_shared<dart::dynamics::CapsuleShape>(
+        _radius, _length);
+
+  auto bn = this->ReferenceInterface<LinkInfo>(_linkID)->link;
+  dart::dynamics::ShapeNode *sn =
+      bn->createShapeNodeWith<dart::dynamics::CollisionAspect,
+                              dart::dynamics::DynamicsAspect>(
+          capsule, 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
 {
@@ -154,6 +220,62 @@ Identity ShapeFeatures::AttachCylinderShape(
   return this->GenerateIdentity(shapeID, this->shapes.at(shapeID));
 }
 
+/////////////////////////////////////////////////
+Identity ShapeFeatures::CastToEllipsoidShape(const Identity &_shapeID) const
+{
+  const auto *shapeInfo = this->ReferenceInterface<ShapeInfo>(_shapeID);
+
+  const dart::dynamics::ShapePtr &shape = shapeInfo->node->getShape();
+
+  if (dynamic_cast<dart::dynamics::MeshShape *>(shape.get()))
+    return this->GenerateIdentity(_shapeID, this->Reference(_shapeID));
+
+  return this->GenerateInvalidId();
+}
+
+/////////////////////////////////////////////////
+Vector3d ShapeFeatures::GetEllipsoidShapeRadii(
+    const Identity &_ellipsoidID) const
+{
+  const auto *shapeInfo = this->ReferenceInterface<ShapeInfo>(_ellipsoidID);
+
+  dart::dynamics::EllipsoidShape *ellipsoid =
+      static_cast<dart::dynamics::EllipsoidShape *>(
+          shapeInfo->node->getShape().get());
+
+  return ellipsoid->getRadii();
+}
+
+/////////////////////////////////////////////////
+Identity ShapeFeatures::AttachEllipsoidShape(
+    const Identity &_linkID,
+    const std::string &_name,
+    const Vector3d _radii,
+    const Pose3d &_pose)
+{
+  common::MeshManager *meshMgr = common::MeshManager::Instance();
+  std::string ellipsoidMeshName = _name + "_ellipsoid_mesh"
+    + "_" + std::to_string(_radii[0])
+    + "_" + std::to_string(_radii[1])
+    + "_" + std::to_string(_radii[2]);
+  meshMgr->CreateEllipsoid(ellipsoidMeshName,
+    ignition::math::Vector3d(_radii[0], _radii[1], _radii[2]),
+    16, 16);
+  const ignition::common::Mesh * _mesh = meshMgr->MeshByName(ellipsoidMeshName);
+
+  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::CastToSphereShape(
     const Identity &_shapeID) const

dartsim/src/ShapeFeatures.hh

@@ -22,7 +22,9 @@
 
 #include <ignition/physics/Shape.hh>
 #include <ignition/physics/BoxShape.hh>
+#include <ignition/physics/CapsuleShape.hh>
 #include <ignition/physics/CylinderShape.hh>
+#include <ignition/physics/EllipsoidShape.hh>
 #include <ignition/physics/mesh/MeshShape.hh>
 #include <ignition/physics/PlaneShape.hh>
 #include <ignition/physics/SphereShape.hh>
@@ -48,10 +50,18 @@ struct ShapeFeatureList : FeatureList<
 //  SetBoxShapeProperties,
   AttachBoxShapeFeature,
 
+  GetCapsuleShapeProperties,
+  //  SetCapsulerShapeProperties,
+  AttachCapsuleShapeFeature,
+
   GetCylinderShapeProperties,
 //  SetCylinderShapeProperties,
   AttachCylinderShapeFeature,
 
+  GetEllipsoidShapeProperties,
+  //  SetEllipsoidShapeProperties,
+  AttachEllipsoidShapeFeature,
+
   GetSphereShapeProperties,
 //  SetSphereShapeProperties,
   AttachSphereShapeFeature,
@@ -89,6 +99,22 @@ class ShapeFeatures :
       const LinearVector3d &_size,
       const Pose3d &_pose) override;
 
+  // ----- Capsule Features -----
+  public: Identity CastToCapsuleShape(
+      const Identity &_shapeID) const override;
+
+  public: double GetCapsuleShapeRadius(
+      const Identity &_capsuleID) const override;
+
+  public: double GetCapsuleShapeLength(
+      const Identity &_capsuleID) const override;
+
+  public: Identity AttachCapsuleShape(
+      const Identity &_linkID,
+      const std::string &_name,
+      double _radius,
+      double _length,
+      const Pose3d &_pose) override;
 
   // ----- Cylinder Features -----
   public: Identity CastToCylinderShape(
@@ -107,6 +133,18 @@ class ShapeFeatures :
       double _height,
       const Pose3d &_pose) override;
 
+  // ----- Ellipsoid Features -----
+  public: Identity CastToEllipsoidShape(
+      const Identity &_shapeID) const override;
+
+  public: Vector3d GetEllipsoidShapeRadii(
+      const Identity &_ellipsoidID) const override;
+
+  public: Identity AttachEllipsoidShape(
+      const Identity &_linkID,
+      const std::string &_name,
+      const Vector3d _radii,
+      const Pose3d &_pose) override;
 
   // ----- Sphere Features -----
   public: Identity CastToSphereShape(