Added Shapes Features to bullet-featherstone

CMakeLists.txt

@@ -84,7 +84,7 @@ gz_find_package(DART
 # Find bullet for the bullet plugin wrapper
 gz_find_package(GzBullet
   VERSION 2.87
-  REQUIRED_BY bullet
+  REQUIRED_BY bullet bullet-featherstone
   PKGCONFIG bullet
   PKGCONFIG_VER_COMPARISON >=)
 

bullet-featherstone/CMakeLists.txt

@@ -5,7 +5,7 @@ gz_add_component(bullet-featherstone INTERFACE
   GET_TARGET_NAME features)
 
 link_directories(${BULLET_LIBRARY_DIRS})
-target_link_libraries(${features} INTERFACE IgnBullet::IgnBullet)
+target_link_libraries(${features} INTERFACE GzBullet::GzBullet)
 
 gz_get_libsources_and_unittests(sources test_sources)
 

bullet-featherstone/src/Base.cc

@@ -37,8 +37,6 @@ WorldInfo::WorldInfo(std::string name_)
     dispatcher.get(), broadphase.get(), solver.get(),
     collisionConfiguration.get());
 
-  /* TO-DO(Lobotuerk): figure out what this line does*/
-  world->getSolverInfo().m_globalCfm = 0;
   btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher.get());
 }
 

bullet-featherstone/src/KinematicsFeatures.cc

@@ -50,18 +50,22 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld(
   }
   else
   {
-    // If the Frame was not a Link then we can assume it's a Model because Free
-    // Groups are always Models underneath. If we ever support frame semantics
-    // for more than links, models, and free groups, then this implementation
-    // needs to change.
-    model = this->FrameInterface<ModelInfo>(_id);
+    // If the Frame was not a Link then check if it's a collision object
+    const auto collisionIt = this->collisions.find(_id.ID());
+    if (collisionIt != this->collisions.end())
+    {
+      throw std::runtime_error("Okay it happened");
+    }
   }
 
   FrameData data;
-  data.pose = convert(model->body->getBaseWorldTransform())
-    * model->baseInertiaToLinkFrame;
-  data.linearVelocity = convert(model->body->getBaseVel());
-  data.linearAcceleration = convert(model->body->getBaseOmega());
+  if(model && model->body)
+  {
+    data.pose = convert(model->body->getBaseWorldTransform())
+      * model->baseInertiaToLinkFrame;
+    data.linearVelocity = convert(model->body->getBaseVel());
+    data.linearAcceleration = convert(model->body->getBaseOmega());
+  }
   return data;
 }