[Bullet] Collision Features (#173)

* Collision working

Signed-off-by: Tomas Lorente <jtlorente@ekumenlabs.com>

Co-authored-by: Jorge Perez <j.j.perez13@hotmail.com>

bullet/src/Base.hh

@@ -68,6 +68,14 @@ struct LinkInfo
   Identity model;
 };
 
+struct CollisionInfo
+{
+  std::string name;
+  btCollisionShape* shape;
+  Identity link;
+  Identity model;
+};
+
 inline btMatrix3x3 convertMat(Eigen::Matrix3d mat)
 {
   return btMatrix3x3(mat(0, 0), mat(0, 1), mat(0, 2),
@@ -138,13 +146,22 @@ class Base : public Implements3d<FeatureList<Feature>>
 
     return this->GenerateIdentity(id, this->links.at(id));
   }
+  public: inline Identity AddCollision(CollisionInfo _collisionInfo)
+  {
+   const auto id = this->GetNextEntity();
+   this->collisions[id] = std::make_shared<CollisionInfo>(_collisionInfo);
+
+   return this->GenerateIdentity(id, this->collisions.at(id));
+  }
 
   public: using WorldInfoPtr = std::shared_ptr<WorldInfo>;
   public: using ModelInfoPtr = std::shared_ptr<ModelInfo>;
   public: using LinkInfoPtr  = std::shared_ptr<LinkInfo>;
+  public: using CollisionInfoPtr = std::shared_ptr<CollisionInfo>;
   public: std::unordered_map<std::size_t, WorldInfoPtr> worlds;
   public: std::unordered_map<std::size_t, ModelInfoPtr> models;
   public: std::unordered_map<std::size_t, LinkInfoPtr> links;
+  public: std::unordered_map<std::size_t, CollisionInfoPtr> collisions;
 
   public: int internalTicksDivider = 0;
 

bullet/src/EntityManagementFeatures.cc

@@ -34,18 +34,33 @@ bool EntityManagementFeatures::RemoveModel(const Identity &_modelID)
   if (this->models.find(_modelID.id) == this->models.end()){
     return false;
   }
-  // Current implementation does not include collisions nor joints
+  // Current implementation does not include joints
   // Those should be removed here before removing the model
+  auto model = this->models.at(_modelID);
+
+  // Clean up collisions
+  std::unordered_map<std::size_t, CollisionInfoPtr>::iterator collision_it = this->collisions.begin();
+  while (collision_it != this->collisions.end())
+  {
+    const auto &collisionInfo = collision_it->second;
+    if (collisionInfo->model.id == _modelID.id)
+    {
+      delete collisionInfo->shape;
+      collision_it = this->collisions.erase(collision_it);
+      continue;
+    }
+    collision_it++;
+  }
 
   // Clean up links
   std::unordered_map<std::size_t, LinkInfoPtr>::iterator it = this->links.begin();
   while (it != this->links.end())
   {
     const auto &linkInfo = it->second;
-    auto model = this->models.at(_modelID);
     if (linkInfo->model.id == _modelID.id)
     {
       this->worlds.at(model->world)->world->removeRigidBody(linkInfo->link);
+      delete linkInfo->link;
       it = this->links.erase(it);
       continue;
     }
@@ -72,15 +87,30 @@ bool EntityManagementFeatures::RemoveModelByIndex(
       this->models.at(_modelIndex)->world.id != _worldID.id) {
     return false;
   }
-  // Current implementation does not include collisions nor joints
+  // Current implementation does not include joints
   // Those should be removed here before removing the model
 
+  auto model = this->models.at(_modelIndex);
+  // Clean up collisions
+  std::unordered_map<std::size_t, CollisionInfoPtr>::iterator collision_it = this->collisions.begin();
+  while (collision_it != this->collisions.end())
+  {
+    const auto &collisionInfo = collision_it->second;
+    if (collisionInfo->model.id == _modelIndex)
+    {
+      delete collisionInfo->shape;
+      collision_it = this->collisions.erase(collision_it);
+      continue;
+    }
+    collision_it++;
+  }
+
   // Clean up links
   std::unordered_map<std::size_t, LinkInfoPtr>::iterator it = this->links.begin();
   while (it != this->links.end())
   {
     const auto &linkInfo = it->second;
-    auto model = this->models.at(_modelIndex);
+
     if (linkInfo->model.id == _modelIndex)
     {
       this->worlds.at(model->world)->world->removeRigidBody(linkInfo->link);

bullet/src/SDFFeatures.cc

@@ -1,6 +1,12 @@
 #include "SDFFeatures.hh"
 #include <ignition/math/eigen3/Conversions.hh>
 
+#include <sdf/Geometry.hh>
+#include <sdf/Box.hh>
+#include <sdf/Sphere.hh>
+#include <sdf/Cylinder.hh>
+#include <sdf/Plane.hh>
+
 namespace ignition {
 namespace physics {
 namespace bullet {
@@ -51,12 +57,12 @@ Identity SDFFeatures::ConstructSdfLink(
   const std::string name = _sdfLink.Name();
   const auto pose = _sdfLink.RawPose();
   const auto inertial = _sdfLink.Inertial();
-  const auto mass = inertial.MassMatrix().Mass();
+  auto mass = inertial.MassMatrix().Mass();
   const auto diagonalMoments = inertial.MassMatrix().DiagonalMoments();
 
   // Get link properties
   // const btScalar linkMass = mass;
-  const btVector3 linkInertiaDiag =
+  btVector3 linkInertiaDiag =
       convertVec(ignition::math::eigen3::convert(diagonalMoments));
 
   math::Pose3d base_pose = this->models.at(_modelID)->pose;
@@ -69,23 +75,103 @@ Identity SDFFeatures::ConstructSdfLink(
 
   // Create link
   // (TO-DO: do we want to use MotionState?) 2nd part: Do motion state use the same transformation?
-  btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(0.), btScalar(0.), btScalar(0.))); // Collision 0 until we have collision features
+  btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(0.), btScalar(0.), btScalar(0.)));
+	if (this->models.at(_modelID)->fixed)
+  {
+    mass = 0;
+    linkInertiaDiag = btVector3(0,0,0);
+  }
   btDefaultMotionState* myMotionState = new btDefaultMotionState(baseTransform);
   btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, linkInertiaDiag);
   btRigidBody* body = new btRigidBody(rbInfo);
   body->setWorldTransform(baseTransform);
 
-  // Add it to the internal world:
-  const auto _worldID = this->models.at(_modelID)->world;
-  const auto &world = this->worlds.at(_worldID)->world;
-  world->addRigidBody(body);
-
   // Generate an identity for it
   const auto linkIdentity = this->AddLink({name, body, _modelID});
 
+  for (std::size_t i = 0; i < _sdfLink.CollisionCount(); ++i)
+  {
+    this->ConstructSdfCollision(linkIdentity, *_sdfLink.CollisionByIndex(i));
+  }
+
   return linkIdentity;
 }
 
+Identity SDFFeatures::ConstructSdfCollision(
+      const Identity &_linkID,
+      const ::sdf::Collision &_collision)
+{
+  if (!_collision.Geom())
+  {
+    ignerr << "The geometry element of collision [" << _collision.Name() << "] "
+           << "was a nullptr\n";
+    return this->GenerateInvalidId();
+  }
+
+  const auto &geom = _collision.Geom();
+  btCollisionShape* shape = nullptr;
+
+  if (geom->BoxShape())
+  {
+    const auto box = geom->BoxShape();
+    const auto size = math::eigen3::convert(box->Size());
+    const auto halfExtents = convertVec(size)*0.5;
+    shape = new btBoxShape(halfExtents);
+  }
+  else if (geom->SphereShape())
+  {
+    const auto sphere = geom->SphereShape();
+    const auto radius = sphere->Radius();
+    shape = new btSphereShape(radius);
+  }
+  else if (geom->CylinderShape())
+  {
+    const auto cylinder = geom->CylinderShape();
+    const auto radius = cylinder->Radius();
+    const auto halfLength = cylinder->Length()*0.5;
+    shape = new btCylinderShapeZ(btVector3(radius, 0, halfLength));
+  }
+  else if (geom->PlaneShape())
+  {
+    const auto plane = geom->PlaneShape();
+    const auto normal = convertVec(math::eigen3::convert(plane->Normal()));
+    shape = new btStaticPlaneShape(normal, 0);
+  }
+
+  // TODO(lobotuerk) find if there's a  way to use friction and related
+  // info in bullet dynamics
+
+  // const auto &surfaceElement = _collision.Element()->GetElement("surface");
+  //
+  // // Get friction
+  // const auto &odeFriction = surfaceElement->GetElement("friction")
+  //                             ->GetElement("ode");
+  // const auto mu = odeFriction->Get<btScalar>("mu");
+  //
+  // // Get restitution
+  // const auto restitution = surfaceElement->GetElement("bounce")
+  //                             ->Get<btScalar>("restitution_coefficient");
+  if (shape != nullptr)
+  {
+    const auto &linkInfo = this->links.at(_linkID);
+    const auto &modelID = linkInfo->model;
+    const auto &modelInfo = this->models.at(modelID);
+    const auto &link = linkInfo->link;
+    const auto &world = this->worlds.at(modelInfo->world)->world;
+    // btTransform transform;
+    delete link->getCollisionShape();
+    link->setCollisionShape(shape);
+
+    // We add the rigidbody to the world after it has collision, as
+    // non collision bodies don't get simulated on a dynamics world
+    world->addRigidBody(link);
+
+    return this->AddCollision({_collision.Name(), shape, _linkID,
+                               modelID});
+  }
+  return this->GenerateInvalidId();
+}
+
 }
 }
 }

bullet/src/SDFFeatures.hh

@@ -5,6 +5,7 @@
 #include <ignition/physics/sdf/ConstructLink.hh>
 #include <ignition/physics/sdf/ConstructModel.hh>
 #include <ignition/physics/sdf/ConstructWorld.hh>
+#include <ignition/physics/sdf/ConstructCollision.hh>
 
 #include <ignition/physics/Implements.hh>
 
@@ -17,6 +18,7 @@ namespace bullet {
 using SDFFeatureList = FeatureList<
   sdf::ConstructSdfLink,
   sdf::ConstructSdfModel,
+  sdf::ConstructSdfCollision,
   sdf::ConstructSdfWorld
 >;
 
@@ -35,6 +37,9 @@ class SDFFeatures :
   private: Identity ConstructSdfLink(
       const Identity &_modelID,
       const ::sdf::Link &_sdfLink) override;
+  private: Identity ConstructSdfCollision(
+      const Identity &_linkID,
+      const ::sdf::Collision &_collision) override;
 };
 
 }

bullet/src/SimulationFeatures.hh

@@ -20,6 +20,7 @@
 
 #include <vector>
 #include <ignition/physics/ForwardStep.hh>
+#include <ignition/physics/GetContacts.hh>
 
 #include "Base.hh"
 
@@ -28,7 +29,8 @@ namespace physics {
 namespace bullet {
 
 using SimulationFeatureList = FeatureList<
-  ForwardStep
+  ForwardStep,
+  GetContactsFromLastStepFeature
 >;
 
 class SimulationFeatures :
@@ -40,6 +42,14 @@ class SimulationFeatures :
       ForwardStep::Output &_h,
       ForwardStep::State &_x,
       const ForwardStep::Input &_u) override;
+
+  public: std::vector<ContactInternal> GetContactsFromLastStep(
+      const Identity &/* _worldID */) const override
+      {
+        // TODO(lobotuerk): Implement contacts getter, could be like https://pybullet.org/Bullet/phpBB3/viewtopic.php?t=2855
+        ignerr << "Dummy GetContactsFromLastStep implementation";
+        return std::vector<ContactInternal>();
+      };
 };
 
 }