diff --git a/bullet/src/Base.hh b/bullet/src/Base.hh index a1d066204..6bb055f10 100644 --- a/bullet/src/Base.hh +++ b/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> 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); + + return this->GenerateIdentity(id, this->collisions.at(id)); + } public: using WorldInfoPtr = std::shared_ptr; public: using ModelInfoPtr = std::shared_ptr; public: using LinkInfoPtr = std::shared_ptr; + public: using CollisionInfoPtr = std::shared_ptr; public: std::unordered_map worlds; public: std::unordered_map models; public: std::unordered_map links; + public: std::unordered_map collisions; public: int internalTicksDivider = 0; diff --git a/bullet/src/EntityManagementFeatures.cc b/bullet/src/EntityManagementFeatures.cc index 2a823d1cf..8eebcb82d 100644 --- a/bullet/src/EntityManagementFeatures.cc +++ b/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::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::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::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::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); diff --git a/bullet/src/SDFFeatures.cc b/bullet/src/SDFFeatures.cc index fda24423e..74c752c0d 100644 --- a/bullet/src/SDFFeatures.cc +++ b/bullet/src/SDFFeatures.cc @@ -1,6 +1,12 @@ #include "SDFFeatures.hh" #include +#include +#include +#include +#include +#include + 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("mu"); + // + // // Get restitution + // const auto restitution = surfaceElement->GetElement("bounce") + // ->Get("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(); +} + } } } diff --git a/bullet/src/SDFFeatures.hh b/bullet/src/SDFFeatures.hh index 1fd9fda4f..3202c05e4 100644 --- a/bullet/src/SDFFeatures.hh +++ b/bullet/src/SDFFeatures.hh @@ -5,6 +5,7 @@ #include #include #include +#include #include @@ -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; }; } diff --git a/bullet/src/SimulationFeatures.hh b/bullet/src/SimulationFeatures.hh index 48167af34..56d3b1026 100644 --- a/bullet/src/SimulationFeatures.hh +++ b/bullet/src/SimulationFeatures.hh @@ -20,6 +20,7 @@ #include #include +#include #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 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(); + }; }; }