Free Group Features

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

bullet/src/Base.hh

@@ -56,7 +56,8 @@ struct ModelInfo
 {
   std::string name;
   Identity world;
-  std::vector<std::size_t> links = {};
+  bool fixed;
+  std::vector<std::size_t> links = {};  
 };
 
 struct LinkInfo

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);
+  }
+}
+
+}
+}
+}

bullet/src/FreeGroupFeatures.hh

@@ -0,0 +1,38 @@
+#ifndef IGNITION_PHYSICS_BULLET_SRC_FREEGROUPFEATURES_HH_
+#define IGNITION_PHYSICS_BULLET_SRC_FREEGROUPFEATURES_HH_
+
+#include <ignition/physics/FreeGroup.hh>
+
+#include "Base.hh"
+
+namespace ignition {
+namespace physics {
+namespace bullet {
+
+using FreeGroupFeatureList = FeatureList<
+  FindFreeGroupFeature,
+  SetFreeGroupWorldPose
+>;
+
+class FreeGroupFeatures
+    : public virtual Base,
+      public virtual Implements3d<FreeGroupFeatureList>
+{
+  // ----- 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

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

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
 {};