[Citadel] [TPE] Add GetContactsFromLastStepFeature (#61)

* add entity bbox function

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* update bbox dirty default value

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* add collision detector

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* add contact features

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* cleanup

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

tpe/lib/src/Entity.cc

@@ -107,6 +107,12 @@ std::string Entity::GetName() const
   return this->dataPtr->name;
 }
 
+//////////////////////////////////////////////////
+const std::string &Entity::GetNameRef() const
+{
+  return this->dataPtr->name;
+}
+
 //////////////////////////////////////////////////
 void Entity::SetPose(const math::Pose3d &_pose)
 {
@@ -158,6 +164,22 @@ Entity &Entity::GetChildByName(const std::string &_name) const
   return kNullEntity;
 }
 
+//////////////////////////////////////////////////
+Entity &Entity::GetChildByIndex(unsigned int _index) const
+{
+  if (_index >= this->dataPtr->children.size())
+    return kNullEntity;
+
+  auto it = this->dataPtr->children.begin();
+  std::advance(it, _index);
+  if (it != this->dataPtr->children.end())
+  {
+    return *it->second.get();
+  }
+
+  return kNullEntity;
+}
+
 //////////////////////////////////////////////////
 bool Entity::RemoveChildById(std::size_t _id)
 {

tpe/lib/src/Entity.hh

@@ -74,6 +74,10 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Entity
   /// \return Name of entity
   public: virtual std::string GetName() const;
 
+  /// \brief Get a const reference to the name of the entity
+  /// \return Name of entity
+  public: virtual const std::string &GetNameRef() const;
+
   /// \brief Set the id of the entity
   /// \param[in] _unique Id
   public: virtual void SetId(std::size_t _id);
@@ -100,6 +104,11 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Entity
   /// \return Child entity
   public: virtual Entity &GetChildByName(const std::string &_name) const;
 
+  /// \brief Get a child entity by index
+  /// \param[in] _index Index of child entity
+  /// \return Child entity
+  public: virtual Entity &GetChildByIndex(unsigned int _index) const;
+
   /// \brief Remove a child entity by id
   /// \param[in] _id Id of child entity to remove
   public: virtual bool RemoveChildById(std::size_t _id);

tpe/lib/src/Model_TEST.cc

@@ -35,6 +35,7 @@ TEST(Model, BasicAPI)
 
   model.SetName("model_1");
   EXPECT_EQ("model_1", model.GetName());
+  EXPECT_EQ("model_1", model.GetNameRef());
 
   model.SetPose(math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3));
   EXPECT_EQ(math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3), model.GetPose());
@@ -83,6 +84,9 @@ TEST(Model, Link)
   Entity entByName = model.GetChildByName("link_1");
   EXPECT_EQ("link_1", entByName.GetName());
 
+  Entity entByIdx = model.GetChildByIndex(0u);
+  EXPECT_EQ("link_1", entByIdx.GetName());
+
   // test casting to link
   Link *link = static_cast<Link *>(&linkEnt);
   EXPECT_NE(nullptr, link);
@@ -92,6 +96,9 @@ TEST(Model, Link)
   Entity &linkEnt2 = model.AddLink();
   EXPECT_EQ(2u, model.GetChildCount());
 
+  Entity ent2ByIdx = model.GetChildByIndex(1u);
+  EXPECT_EQ(linkEnt2.GetId(), ent2ByIdx.GetId());
+
   Link *link2 = static_cast<Link *>(&linkEnt2);
   EXPECT_NE(nullptr, link2);
   EXPECT_EQ(linkEnt2.GetId(), link2->GetId());

tpe/plugin/src/EntityManagementFeatures.cc

@@ -81,9 +81,7 @@ Identity EntityManagementFeatures::GetWorld(
 const std::string &EntityManagementFeatures::GetWorldName(
   const Identity &_worldID) const
 {
-  static std::string worldName =
-    this->ReferenceInterface<WorldInfo>(_worldID)->world->GetName();
-  return worldName;
+  return this->ReferenceInterface<WorldInfo>(_worldID)->world->GetNameRef();
 }
 
 /////////////////////////////////////////////////
@@ -148,9 +146,7 @@ Identity EntityManagementFeatures::GetModel(
 const std::string &EntityManagementFeatures::GetModelName(
   const Identity &_modelID) const
 {
-  static std::string modelName =
-    this->ReferenceInterface<ModelInfo>(_modelID)->model->GetName();
-  return modelName;
+  return this->ReferenceInterface<ModelInfo>(_modelID)->model->GetNameRef();
 }
 
 /////////////////////////////////////////////////
@@ -223,9 +219,7 @@ Identity EntityManagementFeatures::GetLink(
 const std::string &EntityManagementFeatures::GetLinkName(
   const Identity &_linkID) const
 {
-  static std::string linkName =
-    this->ReferenceInterface<LinkInfo>(_linkID)->link->GetName();
-  return linkName;
+  return this->ReferenceInterface<LinkInfo>(_linkID)->link->GetNameRef();
 }
 
 /////////////////////////////////////////////////
@@ -300,9 +294,8 @@ Identity EntityManagementFeatures::GetShape(
 const std::string &EntityManagementFeatures::GetShapeName(
   const Identity &_shapeID) const
 {
-  static std::string shapeName =
-    this->ReferenceInterface<CollisionInfo>(_shapeID)->collision->GetName();
-  return shapeName;
+  return this->ReferenceInterface<CollisionInfo>(
+      _shapeID)->collision->GetNameRef();
 }
 
 /////////////////////////////////////////////////

tpe/plugin/src/SimulationFeatures.cc

@@ -18,7 +18,10 @@
 #include "SimulationFeatures.hh"
 
 #include <ignition/common/Console.hh>
-#include "ignition/common/Profiler.hh"
+#include <ignition/common/Profiler.hh>
+
+#include <ignition/math/eigen3/Conversions.hh>
+
 
 using namespace ignition;
 using namespace physics;
@@ -59,9 +62,38 @@ void SimulationFeatures::WorldForwardStep(
 std::vector<SimulationFeatures::ContactInternal>
 SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const
 {
-  // TODO(anyone):
-  // Implement contact points after collision detection is added
   std::vector<SimulationFeatures::ContactInternal> outContacts;
-  std::shared_ptr<tpelib::World> world = this->worlds.at(_worldID)->world;
+  auto const world = this->ReferenceInterface<WorldInfo>(_worldID)->world;
+  const auto contacts = world->GetContacts();
+
+  for (const auto &c : contacts)
+  {
+    CompositeData extraData;
+
+    // Contact expects identity to be associated with shapes not models
+    // but tpe computes collisions between models
+    // Workaround is to return the first shape of a model
+    auto s1 = this->GetModelCollision(c.entity1);
+    auto s2 = this->GetModelCollision(c.entity2);
+
+    outContacts.push_back(
+        {this->GenerateIdentity(s1.GetId(), this->collisions.at(s1.GetId())),
+         this->GenerateIdentity(s2.GetId(), this->collisions.at(s2.GetId())),
+         math::eigen3::convert(c.point), extraData});
+  }
+
   return outContacts;
 }
+
+tpelib::Entity &SimulationFeatures::GetModelCollision(std::size_t _id) const
+{
+  auto m = this->models.at(_id);
+  if (!m || !m->model)
+    return tpelib::Entity::kNullEntity;
+
+  tpelib::Entity &link = m->model->GetCanonicalLink();
+  if (link.GetChildCount() == 0u)
+    return tpelib::Entity::kNullEntity;
+
+  return link.GetChildByIndex(0u);
+}

tpe/plugin/src/SimulationFeatures.hh

@@ -45,6 +45,11 @@ class SimulationFeatures :
 
   public: std::vector<ContactInternal> GetContactsFromLastStep(
     const Identity &_worldID) const override;
+
+  /// \brief Get a collision from the canonical link of a model
+  /// \param[in] _id Model ID
+  /// \return Collision entity
+  private: tpelib::Entity &GetModelCollision(std::size_t _id) const;
 };
 
 }

tpe/plugin/src/SimulationFeatures_TEST.cc

@@ -45,12 +45,14 @@ struct TestFeatureList : ignition::physics::FeatureList<
   ignition::physics::tpeplugin::ShapeFeatureList,
   ignition::physics::tpeplugin::EntityManagementFeatureList,
   ignition::physics::tpeplugin::FreeGroupFeatureList,
+  ignition::physics::GetContactsFromLastStepFeature,
   ignition::physics::LinkFrameSemantics,
   ignition::physics::sdf::ConstructSdfWorld
 > { };
 
 using TestWorldPtr = ignition::physics::World3dPtr<TestFeatureList>;
 using TestShapePtr = ignition::physics::Shape3dPtr<TestFeatureList>;
+using ContactPoint = ignition::physics::World3d<TestFeatureList>::ContactPoint;
 
 std::unordered_set<TestWorldPtr> LoadWorlds(
     const std::string &_library,
@@ -235,6 +237,119 @@ TEST_P(SimulationFeatures_TEST, FreeGroup)
   }
 }
 
+TEST_P(SimulationFeatures_TEST, RetrieveContacts)
+{
+  const std::string library = GetParam();
+  if (library.empty())
+    return;
+
+  auto worlds = LoadWorlds(library, TEST_WORLD_DIR "/shapes.world");
+
+  for (const auto &world : worlds)
+  {
+    auto sphere = world->GetModel("sphere");
+    auto sphereFreeGroup = sphere->FindFreeGroup();
+    EXPECT_NE(nullptr, sphereFreeGroup);
+
+    auto cylinder = world->GetModel("cylinder");
+    auto cylinderFreeGroup = cylinder->FindFreeGroup();
+    EXPECT_NE(nullptr, cylinderFreeGroup);
+
+    auto box = world->GetModel("box");
+
+    // step and get contacts
+    StepWorld(world, 1);
+    auto contacts = world->GetContactsFromLastStep();
+
+    // large box in the middle should be intersecting with sphere and cylinder
+    EXPECT_EQ(2u, contacts.size());
+    unsigned int contactBoxSphere = 0u;
+    unsigned int contactBoxCylinder = 0u;
+
+    for (auto &contact : contacts)
+    {
+      const auto &contactPoint = contact.Get<ContactPoint>();
+      ASSERT_TRUE(contactPoint.collision1);
+      ASSERT_TRUE(contactPoint.collision2);
+      EXPECT_NE(contactPoint.collision1, contactPoint.collision2);
+
+      auto c1 = contactPoint.collision1;
+      auto c2 = contactPoint.collision2;
+      auto m1 = c1->GetLink()->GetModel();
+      auto m2 = c2->GetLink()->GetModel();
+      if ((m1->GetName() == "sphere" && m2->GetName() == "box") ||
+          (m1->GetName() == "box" && m2->GetName() == "sphere"))
+      {
+        contactBoxSphere++;
+        Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0, 1.5, 0.5);
+        EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos,
+            contactPoint.point, 1e-6));
+      }
+      else if ((m1->GetName() == "box" && m2->GetName() == "cylinder") ||
+          (m1->GetName() == "cylinder" && m2->GetName() == "box"))
+      {
+        contactBoxCylinder++;
+        Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0, -1.5, 0.5);
+        EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos,
+            contactPoint.point, 1e-6));
+      }
+      else
+      {
+        FAIL() << "There should not be contacts between: "
+               << m1->GetName() << " " << m2->GetName();
+      }
+    }
+    EXPECT_EQ(1u, contactBoxSphere);
+    EXPECT_EQ(1u, contactBoxCylinder);
+
+    // move sphere away
+    sphereFreeGroup->SetWorldPose(ignition::math::eigen3::convert(
+        ignition::math::Pose3d(0, 100, 0.5, 0, 0, 0)));
+
+    // step and get contacts
+    StepWorld(world, 1);
+    contacts = world->GetContactsFromLastStep();
+
+    // large box in the middle should be intersecting with cylinder
+    EXPECT_EQ(1u, contacts.size());
+
+    contactBoxCylinder = 0u;
+    for (auto contact : contacts)
+    {
+      const auto &contactPoint = contact.Get<::ContactPoint>();
+      ASSERT_TRUE(contactPoint.collision1);
+      ASSERT_TRUE(contactPoint.collision2);
+      EXPECT_NE(contactPoint.collision1, contactPoint.collision2);
+
+      auto c1 = contactPoint.collision1;
+      auto c2 = contactPoint.collision2;
+      auto m1 = c1->GetLink()->GetModel();
+      auto m2 = c2->GetLink()->GetModel();
+      if ((m1->GetName() == "box" && m2->GetName() == "cylinder") ||
+          (m1->GetName() == "cylinder" && m2->GetName() == "box"))
+        contactBoxCylinder++;
+      else
+        FAIL() << "There should only be contacts between box and cylinder";
+
+      Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0, -1.5, 0.5);
+      EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos,
+          contactPoint.point, 1e-6));
+    }
+    EXPECT_EQ(1u, contactBoxCylinder);
+
+    // move cylinder away
+    cylinderFreeGroup->SetWorldPose(ignition::math::eigen3::convert(
+        ignition::math::Pose3d(0, -100, 0.5, 0, 0, 0)));
+
+    // step and get contacts
+    StepWorld(world, 1);
+    contacts = world->GetContactsFromLastStep();
+
+    // no entities should be colliding
+    EXPECT_TRUE(contacts.empty());
+  }
+}
+
 INSTANTIATE_TEST_CASE_P(PhysicsPlugins, SimulationFeatures_TEST,
   ::testing::ValuesIn(ignition::physics::test::g_PhysicsPluginLibraries),); // NOLINT