Fix bounding box for collisions with pose offset

Signed-off-by: Ian Chen <ichen@openrobotics.org>

bullet-featherstone/src/KinematicsFeatures.cc

@@ -22,30 +22,40 @@ namespace gz {
 namespace physics {
 namespace bullet_featherstone {
 
+
+FrameData3d getNonBaseLinkFrameData(const ModelInfo *_modelInfo,
+                                    const LinkInfo *_linkInfo)
+{
+  const auto index = _linkInfo->indexInModel.value();
+  FrameData3d data;
+  data.pose = GetWorldTransformOfLink(*_modelInfo, *_linkInfo);
+  const auto &link = _modelInfo->body->getLink(index);
+  data.linearVelocity = convert(link.m_absFrameTotVelocity.getLinear());
+  data.angularVelocity = convert(link.m_absFrameTotVelocity.getAngular());
+  return data;
+}
+
 /////////////////////////////////////////////////
 FrameData3d KinematicsFeatures::FrameDataRelativeToWorld(
     const FrameID &_id) const
 {
-  const auto linkIt = this->links.find(_id.ID());
+  bool isModel = false;
+  bool isCollision = false;
   const ModelInfo *model = nullptr;
+  Eigen::Isometry3d collisionPoseOffset = Eigen::Isometry3d();
+
+  const auto linkIt = this->links.find(_id.ID());
   if (linkIt != this->links.end())
   {
     const auto &linkInfo = linkIt->second;
-    const auto indexOpt = linkInfo->indexInModel;
     model = this->ReferenceInterface<ModelInfo>(linkInfo->model);
 
-    if (indexOpt.has_value())
+    if (linkInfo->indexInModel.has_value())
     {
-      const auto index = *indexOpt;
-      FrameData data;
-      data.pose = GetWorldTransformOfLink(*model, *linkInfo);
-      const auto &link = model->body->getLink(index);
-      data.linearVelocity = convert(link.m_absFrameTotVelocity.getLinear());
-      data.angularVelocity = convert(link.m_absFrameTotVelocity.getAngular());
-      return data;
+      return getNonBaseLinkFrameData(model, linkInfo.get());
     }
 
-    // If indexOpt is nullopt then the link is the base link which will be
+    // If indexInModel is nullopt then the link is the base link which will be
     // calculated below.
   }
   else
@@ -59,60 +69,55 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld(
       if (linkIt2 != this->links.end())
       {
         const auto &linkInfo2 = linkIt2->second;
-        const auto indexOpt2 = linkInfo2->indexInModel;
         model = this->ReferenceInterface<ModelInfo>(linkInfo2->model);
-
-        if (indexOpt2.has_value())
+        if (linkInfo2->indexInModel.has_value())
         {
-          const auto index2 = *indexOpt2;
-          FrameData data;
-          data.pose = GetWorldTransformOfLink(*model, *linkInfo2);
-          const auto &link = model->body->getLink(index2);
-          data.linearVelocity = convert(
-            link.m_absFrameTotVelocity.getLinear());
-          data.angularVelocity = convert(
-            link.m_absFrameTotVelocity.getAngular());
-          return data;
+          return getNonBaseLinkFrameData(model, linkInfo2.get());
         }
       }
     }
-
-    auto collisionIt = this->collisions.find(_id.ID());
-    if (collisionIt != this->collisions.end())
+    else
     {
-      const auto &collisionInfo = collisionIt->second;
-
-      const auto linkIt2 = this->links.find(collisionInfo->link);
-      if (linkIt2 != this->links.end())
+      auto collisionIt = this->collisions.find(_id.ID());
+      if (collisionIt != this->collisions.end())
       {
-        const auto &linkInfo2 = linkIt2->second;
-        const auto indexOpt2 = linkInfo2->indexInModel;
-        model = this->ReferenceInterface<ModelInfo>(linkInfo2->model);
+        isCollision = true;
+        const auto &collisionInfo = collisionIt->second;
 
-        if (indexOpt2.has_value())
+        const auto linkIt2 = this->links.find(collisionInfo->link);
+        if (linkIt2 != this->links.end())
         {
-          const auto index2 = *indexOpt2;
-          FrameData data;
-          data.pose = GetWorldTransformOfLink(*model, *linkInfo2);
-          const auto &link = model->body->getLink(index2);
-          data.linearVelocity = convert(
-            link.m_absFrameTotVelocity.getLinear());
-          data.angularVelocity = convert(
-            link.m_absFrameTotVelocity.getAngular());
-          return data;
+          const auto &linkInfo2 = linkIt2->second;
+          model = this->ReferenceInterface<ModelInfo>(linkInfo2->model);
+          collisionPoseOffset = collisionInfo->linkToCollision;
+          if (linkInfo2->indexInModel.has_value())
+          {
+            auto data = getNonBaseLinkFrameData(model, linkInfo2.get());
+            data.pose = data.pose * collisionPoseOffset;
+            return data;
+          }
+        }
+      }
+      else
+      {
+        auto modelIt = this->models.find(_id.ID());
+        if (modelIt != this->models.end())
+        {
+          model = modelIt->second.get();
+          isModel = true;
         }
       }
     }
-
-    if (!model || model->body == nullptr)
-      model = this->FrameInterface<ModelInfo>(_id);
   }
 
   FrameData data;
-  if(model && model->body)
+  if (model && model->body)
   {
-    data.pose = convert(model->body->getBaseWorldTransform())
-      * model->baseInertiaToLinkFrame;
+    data.pose = convert(model->body->getBaseWorldTransform());
+    if (!isModel)
+      data.pose = data.pose * model->baseInertiaToLinkFrame;
+    if (isCollision)
+      data.pose = data.pose * collisionPoseOffset;
     data.linearVelocity = convert(model->body->getBaseVel());
     data.angularVelocity = convert(model->body->getBaseOmega());
   }

bullet-featherstone/src/KinematicsFeatures.hh

@@ -30,7 +30,8 @@ namespace bullet_featherstone {
 struct KinematicsFeatureList : gz::physics::FeatureList<
   LinkFrameSemantics,
   ModelFrameSemantics,
-  FreeGroupFrameSemantics
+  FreeGroupFrameSemantics,
+  ShapeFrameSemantics
 > { };
 
 class KinematicsFeatures :

bullet-featherstone/src/ShapeFeatures.cc

@@ -37,8 +37,6 @@ AlignedBox3d ShapeFeatures::GetShapeAxisAlignedBoundingBox(
     t.setIdentity();
     btVector3 minBox(0, 0, 0);
     btVector3 maxBox(0, 0, 0);
-    btVector3 minBox2(0, 0, 0);
-    btVector3 maxBox2(0, 0, 0);
     collider->collider->getAabb(t, minBox, maxBox);
     return math::eigen3::convert(math::AxisAlignedBox(
       math::Vector3d(minBox[0], minBox[1], minBox[2]),

test/common_test/link_features.cc

@@ -293,14 +293,26 @@ TYPED_TEST(LinkFeaturesTest, JointSetCommand)
   }
 }
 
-TYPED_TEST(LinkFeaturesTest, AxisAlignedBoundingBox)
+using LinkBoundingBoxFeaturesList = gz::physics::FeatureList<
+    gz::physics::ForwardStep,
+    gz::physics::sdf::ConstructSdfWorld,
+    gz::physics::GetEntities,
+    gz::physics::GetLinkBoundingBox,
+    gz::physics::GetModelBoundingBox
+>;
+
+using LinkBoundingBoxFeaturesTestTypes =
+  LinkFeaturesTest<LinkBoundingBoxFeaturesList>;
+
+TEST_F(LinkBoundingBoxFeaturesTestTypes, AxisAlignedBoundingBox)
 {
   for (const std::string &name : this->pluginNames)
   {
     std::cout << "Testing plugin: " << name << std::endl;
     gz::plugin::PluginPtr plugin = this->loader.Instantiate(name);
 
-    auto engine = gz::physics::RequestEngine3d<LinkFeaturesList>::From(plugin);
+    auto engine =
+        gz::physics::RequestEngine3d<LinkBoundingBoxFeaturesList>::From(plugin);
     ASSERT_NE(nullptr, engine);
 
     sdf::Root root;
@@ -313,9 +325,6 @@ TYPED_TEST(LinkFeaturesTest, AxisAlignedBoundingBox)
     auto world = engine->ConstructWorld(*root.WorldByIndex(0));
     EXPECT_NE(nullptr, world);
 
-    EXPECT_NE(nullptr, world);
-    world->SetGravity(Eigen::Vector3d{0, 0, -9.8});
-
     auto model = world->GetModel("double_pendulum_with_base");
     auto baseLink = model->GetLink("base");
     auto bbox = baseLink->GetAxisAlignedBoundingBox();
@@ -364,9 +373,6 @@ TYPED_TEST(LinkFeaturesTest, ModelAxisAlignedBoundingBox)
     auto world = engine->ConstructWorld(*root.WorldByIndex(0));
     EXPECT_NE(nullptr, world);
 
-    EXPECT_NE(nullptr, world);
-    world->SetGravity(Eigen::Vector3d{0, 0, -9.8});
-
     auto model = world->GetModel("sphere");
     auto bbox = model->GetAxisAlignedBoundingBox();
     AssertVectorApprox vectorPredicate(1e-4);