[Bullet] Bullet classic, Add sdf entities (#167)

* Add SDF features to build models
* Fix class inheritance problem
* Comment unused variables in this commit
* Add vector with ids of links added in a model

Signed-off-by: Jorge Perez <jjperez@ekumenlabs.com>

bullet/src/Base.hh

@@ -18,9 +18,7 @@
 #ifndef IGNITION_PHYSICS_BULLET_BASE_HH_
 #define IGNITION_PHYSICS_BULLET_BASE_HH_
 
-//MISSIN INCLUDES
 #include <btBulletDynamicsCommon.h>
-
 #include <Eigen/Geometry>
 
 #include <assert.h>
@@ -56,18 +54,15 @@ struct WorldInfo
 
 struct ModelInfo
 {
-  btRigidBody* model;
   std::string name;
   Identity world;
+  std::vector<std::size_t> links = {};
 };
 
 struct LinkInfo
 {
   std::string name;
-  int linkIndex;
-  btScalar linkMass;
-  btVector3 linkInertiaDiag;
-  Eigen::Isometry3d poseIsometry;
+  btRigidBody* link;
   Identity model;
 };
 
@@ -136,6 +131,9 @@ class Base : public Implements3d<FeatureList<Feature>>
     const auto id = this->GetNextEntity();
     this->links[id] = std::make_shared<LinkInfo>(_linkInfo);
 
+    auto model = this->models.at(_linkInfo.model);
+    model->links.push_back(id);
+
     return this->GenerateIdentity(id, this->links.at(id));
   }
 

bullet/src/EntityManagementFeatures.cc

@@ -1,4 +1,3 @@
-//MISSIN INCLUDES
 #include <btBulletDynamicsCommon.h>
 
 #include <string>
@@ -43,18 +42,17 @@ bool EntityManagementFeatures::RemoveModel(const Identity &_modelID)
   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);
       it = this->links.erase(it);
       continue;
     }
     it++;
   }
 
-  // Clean up model
-  auto model = this->models.at(_modelID.id);
-  this->worlds.at(model->world)->world->removeRigidBody(model->model);
-  delete this->models.at(_modelID.id)->model;
+  // Clean up model, links are erased and the model is just a name to tie them together
   this->models.erase(_modelID.id);
 
   return true;
@@ -82,18 +80,17 @@ bool EntityManagementFeatures::RemoveModelByIndex(
   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);
       it = this->links.erase(it);
       continue;
     }
     it++;
   }
 
   // Clean up model
-  auto model = this->models.at(_modelIndex);
-  this->worlds.at(model->world)->world->removeRigidBody(model->model);
-  delete this->models.at(_modelIndex)->model;
   this->models.erase(_modelIndex);
 
   return true;

bullet/src/SDFFeatures.cc

@@ -0,0 +1,89 @@
+#include "SDFFeatures.hh"
+#include <ignition/math/eigen3/Conversions.hh>
+
+namespace ignition {
+namespace physics {
+namespace bullet {
+
+/////////////////////////////////////////////////
+Identity SDFFeatures::ConstructSdfWorld(
+    const Identity &_engine,
+    const ::sdf::World &_sdfWorld)
+{
+  const Identity worldID = this->ConstructEmptyWorld(_engine, _sdfWorld.Name());
+
+  const WorldInfoPtr &worldInfo = this->worlds.at(worldID);
+
+  auto gravity = _sdfWorld.Gravity();
+  worldInfo->world->setGravity(btVector3(gravity[0], gravity[1], gravity[2]));
+  return worldID;
+}
+
+/////////////////////////////////////////////////
+Identity SDFFeatures::ConstructSdfModel(
+  const Identity &_worldID,
+  const ::sdf::Model &_sdfModel)
+{
+  // Read sdf params
+  const std::string name = _sdfModel.Name();
+  const auto pose = _sdfModel.RawPose();
+  // const bool isStatic = _sdfModel.Static();
+  // const bool selfCollide = _sdfModel.SelfCollide();
+
+  // const auto &world = this->worlds.at(_worldID)->world;
+  const auto modelIdentity = this->AddModel({name, _worldID});
+
+  // Build links
+  for (std::size_t i = 0; i < _sdfModel.LinkCount(); ++i)
+  {
+    this->ConstructSdfLink(modelIdentity, *_sdfModel.LinkByIndex(i));
+  }
+
+  return modelIdentity;
+}
+
+/////////////////////////////////////////////////
+Identity SDFFeatures::ConstructSdfLink(
+  const Identity &_modelID,
+  const ::sdf::Link &_sdfLink)
+{
+  // Read sdf params
+  const std::string name = _sdfLink.Name();
+  const auto pose = _sdfLink.RawPose();
+  const auto inertial = _sdfLink.Inertial();
+  const auto mass = inertial.MassMatrix().Mass();
+  const auto diagonalMoments = inertial.MassMatrix().DiagonalMoments();
+
+  // Get link properties
+  // const btScalar linkMass = mass;
+  const btVector3 linkInertiaDiag =
+      convertVec(ignition::math::eigen3::convert(diagonalMoments));
+
+  // (TO-DO: do those calculation need to take into account in some way the base model?)
+  const auto poseIsometry = ignition::math::eigen3::convert(pose);
+  const auto poseTranslation = poseIsometry.translation();
+  const auto poseLinear = poseIsometry.linear();
+  btTransform baseTransform;
+  baseTransform.setOrigin(convertVec(poseTranslation));
+  baseTransform.setBasis(convertMat(poseLinear));
+
+  // Create link
+  // (TO-DO: do we want to use MotionState?)
+  // First zero is motionState, the second one is the colision shape
+  btRigidBody* body = new btRigidBody(mass, 0, 0, linkInertiaDiag);
+  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});
+
+  return linkIdentity;
+}
+
+}
+}
+}

bullet/src/SDFFeatures.hh

@@ -0,0 +1,44 @@
+
+#ifndef IGNITION_PHYSICS_BULLET_SRC_SDFFEATURES_HH_
+#define IGNITION_PHYSICS_BULLET_SRC_SDFFEATURES_HH_
+
+#include <ignition/physics/sdf/ConstructLink.hh>
+#include <ignition/physics/sdf/ConstructModel.hh>
+#include <ignition/physics/sdf/ConstructWorld.hh>
+
+#include <ignition/physics/Implements.hh>
+
+#include "EntityManagementFeatures.hh"
+
+namespace ignition {
+namespace physics {
+namespace bullet {
+
+using SDFFeatureList = FeatureList<
+  sdf::ConstructSdfLink,
+  sdf::ConstructSdfModel,
+  sdf::ConstructSdfWorld
+>;
+
+class SDFFeatures :
+    public virtual EntityManagementFeatures,
+    public virtual Implements3d<SDFFeatureList>
+{
+  public: Identity ConstructSdfWorld(
+      const Identity &/*_engine*/,
+      const ::sdf::World &_sdfWorld) override;
+
+  public: Identity ConstructSdfModel(
+      const Identity &_worldID,
+      const ::sdf::Model &_sdfModel) override;
+
+  private: Identity ConstructSdfLink(
+      const Identity &_modelID,
+      const ::sdf::Link &_sdfLink) override;
+};
+
+}
+}
+}
+
+#endif

bullet/src/plugin.cc

@@ -23,21 +23,25 @@
 #include "Base.hh"
 #include "EntityManagementFeatures.hh"
 #include "SimulationFeatures.hh"
+#include "SDFFeatures.hh"
 
 namespace ignition {
 namespace physics {
 namespace bullet {
 
 struct BulletFeatures : FeatureList <
   EntityManagementFeatureList,
-  SimulationFeatureList
+  SimulationFeatureList,
+  SDFFeatureList
 > { };
 
 class Plugin :
     public virtual Implements3d<BulletFeatures>,
     public virtual Base,
     public virtual EntityManagementFeatures,
-    public virtual SimulationFeatures {};
+    public virtual SimulationFeatures,
+    public virtual SDFFeatures
+{};
 
 IGN_PHYSICS_ADD_PLUGIN(Plugin, FeaturePolicy3d, BulletFeatures)