From dc3aae167b853fd171c5450b12ab9300c9434f56 Mon Sep 17 00:00:00 2001 From: Lobotuerk Date: Wed, 25 Nov 2020 17:27:07 -0300 Subject: [PATCH] Free Group Features Signed-off-by: Lobotuerk --- bullet/src/Base.hh | 3 +- bullet/src/FreeGroupFeatures.cc | 67 +++++++++++++++++++++++++++++++++ bullet/src/FreeGroupFeatures.hh | 38 +++++++++++++++++++ bullet/src/SDFFeatures.cc | 8 ++-- bullet/src/plugin.cc | 3 ++ 5 files changed, 114 insertions(+), 5 deletions(-) create mode 100644 bullet/src/FreeGroupFeatures.cc create mode 100644 bullet/src/FreeGroupFeatures.hh diff --git a/bullet/src/Base.hh b/bullet/src/Base.hh index 40c11ad26..05b393540 100644 --- a/bullet/src/Base.hh +++ b/bullet/src/Base.hh @@ -56,7 +56,8 @@ struct ModelInfo { std::string name; Identity world; - std::vector links = {}; + bool fixed; + std::vector links = {}; }; struct LinkInfo diff --git a/bullet/src/FreeGroupFeatures.cc b/bullet/src/FreeGroupFeatures.cc new file mode 100644 index 000000000..09e1e37e5 --- /dev/null +++ b/bullet/src/FreeGroupFeatures.cc @@ -0,0 +1,67 @@ +#include "FreeGroupFeatures.hh" + +namespace ignition { +namespace physics { +namespace bullet { + +///////////////////////////////////////////////// +Identity FreeGroupFeatures::FindFreeGroupForModel( + const Identity &_modelID) const +{ + const auto &model = this->models.at(_modelID); + + // If there are no links at all in this model, then the FreeGroup functions + // will not work properly, so we'll just reject these cases. + if (model->links.size() == 0) + return this->GenerateInvalidId(); + + // Reject also if the model has fixed base + if (model->fixed) + return this->GenerateInvalidId(); + + return _modelID; +} + +///////////////////////////////////////////////// +Identity FreeGroupFeatures::FindFreeGroupForLink( + const Identity &_linkID) const +{ + const auto &link_it = this->links.find(_linkID); + + if (link_it != this->links.end() && link_it->second != nullptr) + return this->GenerateIdentity(_linkID.id, link_it->second); + return this->GenerateInvalidId(); +} + +///////////////////////////////////////////////// +Identity FreeGroupFeatures::GetFreeGroupCanonicalLink( + const Identity &_groupID) const +{ + (void) _groupID; + // Todo(Lobotuerk) implement canonical links + return this->GenerateInvalidId(); +} + +///////////////////////////////////////////////// +void FreeGroupFeatures::SetFreeGroupWorldPose( + const Identity &_groupID, + const PoseType &_pose) +{ + // Convert pose + const auto poseTranslation = _pose.translation(); + const auto poseLinear = _pose.linear(); + btTransform baseTransform; + baseTransform.setOrigin(convertVec(poseTranslation)); + baseTransform.setBasis(convertMat(poseLinear)); + + // Set base transform + const auto &model = this->models.at(_groupID); + for (auto link : model->links) + { + this->links.at(link)->link->setCenterOfMassTransform(baseTransform); + } +} + +} +} +} diff --git a/bullet/src/FreeGroupFeatures.hh b/bullet/src/FreeGroupFeatures.hh new file mode 100644 index 000000000..dfba17159 --- /dev/null +++ b/bullet/src/FreeGroupFeatures.hh @@ -0,0 +1,38 @@ +#ifndef IGNITION_PHYSICS_BULLET_SRC_FREEGROUPFEATURES_HH_ +#define IGNITION_PHYSICS_BULLET_SRC_FREEGROUPFEATURES_HH_ + +#include + +#include "Base.hh" + +namespace ignition { +namespace physics { +namespace bullet { + +using FreeGroupFeatureList = FeatureList< + FindFreeGroupFeature, + SetFreeGroupWorldPose +>; + +class FreeGroupFeatures + : public virtual Base, + public virtual Implements3d +{ + // ----- FindFreeGroupFeature ----- + Identity FindFreeGroupForModel(const Identity &_modelID) const override; + + Identity FindFreeGroupForLink(const Identity &_linkID) const override; + + Identity GetFreeGroupCanonicalLink(const Identity &_groupID) const override; + + // ----- SetFreeGroupWorldPose ----- + void SetFreeGroupWorldPose( + const Identity &_groupID, + const PoseType &_pose) override; +}; + +} +} +} + +#endif diff --git a/bullet/src/SDFFeatures.cc b/bullet/src/SDFFeatures.cc index 5917dcc3a..aad798451 100644 --- a/bullet/src/SDFFeatures.cc +++ b/bullet/src/SDFFeatures.cc @@ -27,11 +27,11 @@ Identity SDFFeatures::ConstructSdfModel( // Read sdf params const std::string name = _sdfModel.Name(); const auto pose = _sdfModel.RawPose(); - // const bool isStatic = _sdfModel.Static(); + 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}); + const auto modelIdentity = this->AddModel({name, _worldID, isStatic}); // Build links for (std::size_t i = 0; i < _sdfModel.LinkCount(); ++i) @@ -66,7 +66,7 @@ Identity SDFFeatures::ConstructSdfLink( 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 @@ -75,7 +75,7 @@ Identity SDFFeatures::ConstructSdfLink( // Add it to the internal world: const auto _worldID = this->models.at(_modelID)->world; - const auto &world = this->worlds.at(_worldID)->world; + const auto &world = this->worlds.at(_worldID)->world; world->addRigidBody(body); // Generate an identity for it diff --git a/bullet/src/plugin.cc b/bullet/src/plugin.cc index 07a12f5db..cee38d926 100644 --- a/bullet/src/plugin.cc +++ b/bullet/src/plugin.cc @@ -25,6 +25,7 @@ #include "SimulationFeatures.hh" #include "SDFFeatures.hh" #include "KinematicsFeatures.hh" +#include "FreeGroupFeatures.hh" namespace ignition { namespace physics { @@ -33,6 +34,7 @@ namespace bullet { struct BulletFeatures : FeatureList < EntityManagementFeatureList, SimulationFeatureList, + FreeGroupFeatureList, KinematicsFeatureList, SDFFeatureList > { }; @@ -42,6 +44,7 @@ class Plugin : public virtual Base, public virtual EntityManagementFeatures, public virtual SimulationFeatures, + public virtual FreeGroupFeatures, public virtual KinematicsFeatures, public virtual SDFFeatures {};