From 5a5f1a42adbf24c738e1c4898b897978db040991 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Tue, 24 Nov 2020 18:04:11 -0300 Subject: [PATCH] [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 --- bullet/src/Base.hh | 12 ++-- bullet/src/EntityManagementFeatures.cc | 13 ++-- bullet/src/SDFFeatures.cc | 89 ++++++++++++++++++++++++++ bullet/src/SDFFeatures.hh | 44 +++++++++++++ bullet/src/plugin.cc | 8 ++- 5 files changed, 149 insertions(+), 17 deletions(-) create mode 100644 bullet/src/SDFFeatures.cc create mode 100644 bullet/src/SDFFeatures.hh diff --git a/bullet/src/Base.hh b/bullet/src/Base.hh index 7163c522f..40c11ad26 100644 --- a/bullet/src/Base.hh +++ b/bullet/src/Base.hh @@ -18,9 +18,7 @@ #ifndef IGNITION_PHYSICS_BULLET_BASE_HH_ #define IGNITION_PHYSICS_BULLET_BASE_HH_ -//MISSIN INCLUDES #include - #include #include @@ -56,18 +54,15 @@ struct WorldInfo struct ModelInfo { - btRigidBody* model; std::string name; Identity world; + std::vector 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> const auto id = this->GetNextEntity(); this->links[id] = std::make_shared(_linkInfo); + auto model = this->models.at(_linkInfo.model); + model->links.push_back(id); + return this->GenerateIdentity(id, this->links.at(id)); } diff --git a/bullet/src/EntityManagementFeatures.cc b/bullet/src/EntityManagementFeatures.cc index 43a92e39a..843cce191 100644 --- a/bullet/src/EntityManagementFeatures.cc +++ b/bullet/src/EntityManagementFeatures.cc @@ -1,4 +1,3 @@ -//MISSIN INCLUDES #include #include @@ -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,8 +80,10 @@ 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; } @@ -91,9 +91,6 @@ bool EntityManagementFeatures::RemoveModelByIndex( } // 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; diff --git a/bullet/src/SDFFeatures.cc b/bullet/src/SDFFeatures.cc new file mode 100644 index 000000000..5917dcc3a --- /dev/null +++ b/bullet/src/SDFFeatures.cc @@ -0,0 +1,89 @@ +#include "SDFFeatures.hh" +#include + +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; +} + +} +} +} diff --git a/bullet/src/SDFFeatures.hh b/bullet/src/SDFFeatures.hh new file mode 100644 index 000000000..1fd9fda4f --- /dev/null +++ b/bullet/src/SDFFeatures.hh @@ -0,0 +1,44 @@ + +#ifndef IGNITION_PHYSICS_BULLET_SRC_SDFFEATURES_HH_ +#define IGNITION_PHYSICS_BULLET_SRC_SDFFEATURES_HH_ + +#include +#include +#include + +#include + +#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 +{ + 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 diff --git a/bullet/src/plugin.cc b/bullet/src/plugin.cc index e167e3b32..f0201a504 100644 --- a/bullet/src/plugin.cc +++ b/bullet/src/plugin.cc @@ -23,6 +23,7 @@ #include "Base.hh" #include "EntityManagementFeatures.hh" #include "SimulationFeatures.hh" +#include "SDFFeatures.hh" namespace ignition { namespace physics { @@ -30,14 +31,17 @@ namespace bullet { struct BulletFeatures : FeatureList < EntityManagementFeatureList, - SimulationFeatureList + SimulationFeatureList, + SDFFeatureList > { }; class Plugin : public virtual Implements3d, public virtual Base, public virtual EntityManagementFeatures, - public virtual SimulationFeatures {}; + public virtual SimulationFeatures, + public virtual SDFFeatures +{}; IGN_PHYSICS_ADD_PLUGIN(Plugin, FeaturePolicy3d, BulletFeatures)