bullet-featherstone: Fix bounding box for collisions with pose offset (…

…#647)

Currently if a collision has a pose offset, the bullet-featherstone plugin returns an incorrect axis aligned bounding box for the link containing the collision. This is found to be caused by incorrect frame data pose computed for collisions - It currently ignores the collision pose offset.

This PR makes a few changes to KinematicsFeatures::FrameDataRelativeToWorld in bullet-featherstone plugin:

- fixed getting collision pose relative to world
- fixed getting model pose relative to world
- minor refactoring
The link_features's bounding box test was previously skipped for bullet-featherstone plugin. This PR enables this test for bullet-featherstone by requiring only a minimal set of features needed to run the test.

---------

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/Worlds.hh

@@ -47,6 +47,7 @@ const auto kMimicUniversalWorld = CommonTestWorld("mimic_universal_world.sdf");
 const auto kMultipleCollisionsSdf = CommonTestWorld("multiple_collisions.sdf");
 const auto kPendulumJointWrenchSdf =
   CommonTestWorld("pendulum_joint_wrench.sdf");
+const auto kPoseOffsetSdf = CommonTestWorld("pose_offset.sdf");
 const auto kShapesWorld = CommonTestWorld("shapes.world");
 const auto kShapesBitmaskWorld = CommonTestWorld("shapes_bitmask.sdf");
 const auto kSlipComplianceSdf = CommonTestWorld("slip_compliance.sdf");

test/common_test/kinematic_features.cc

@@ -17,6 +17,7 @@
 #include <gtest/gtest.h>
 
 #include <gz/common/Console.hh>
+#include <gz/math/eigen3/Conversions.hh>
 #include <gz/plugin/Loader.hh>
 
 #include "test/TestLibLoader.hh"
@@ -29,6 +30,7 @@
 #include <gz/physics/FindFeatures.hh>
 #include <gz/physics/ForwardStep.hh>
 #include <gz/physics/GetEntities.hh>
+#include <gz/physics/Link.hh>
 #include <gz/physics/RequestEngine.hh>
 #include <gz/physics/sdf/ConstructWorld.hh>
 
@@ -72,11 +74,13 @@ struct KinematicFeaturesList : gz::physics::FeatureList<
     gz::physics::GetEngineInfo,
     gz::physics::ForwardStep,
     gz::physics::sdf::ConstructSdfWorld,
+    gz::physics::GetShapeFromLink,
     gz::physics::GetModelFromWorld,
     gz::physics::GetLinkFromModel,
     gz::physics::GetJointFromModel,
+    gz::physics::JointFrameSemantics,
     gz::physics::LinkFrameSemantics,
-    gz::physics::JointFrameSemantics
+    gz::physics::ShapeFrameSemantics
 > { };
 
 using KinematicFeaturesTestTypes =
@@ -146,12 +150,18 @@ TYPED_TEST(KinematicFeaturesTest, JointFrameSemantics)
     EXPECT_EQ(
           F_WCexpected.pose.rotation(),
           childLinkFrameData.pose.rotation());
-    // TODO(ahcorde): Rewiew this in bullet-featherstone
-    if(this->PhysicsEngineName(name) == "bullet_featherstone")
+    // TODO(ahcorde): Review this in bullet-featherstone
+    if (this->PhysicsEngineName(name) != "bullet-featherstone")
     {
-      EXPECT_EQ(
-            F_WCexpected.pose.translation(),
-            childLinkFrameData.pose.translation());
+      EXPECT_NEAR(
+            F_WCexpected.pose.translation().x(),
+            childLinkFrameData.pose.translation().x(), 1e-6);
+      EXPECT_NEAR(
+            F_WCexpected.pose.translation().y(),
+            childLinkFrameData.pose.translation().y(), 1e-6);
+      EXPECT_NEAR(
+            F_WCexpected.pose.translation().z(),
+            childLinkFrameData.pose.translation().z(), 1e-6);
     }
     EXPECT_TRUE(
       gz::physics::test::Equal(
@@ -166,6 +176,70 @@ TYPED_TEST(KinematicFeaturesTest, JointFrameSemantics)
   }
 }
 
+TYPED_TEST(KinematicFeaturesTest, LinkFrameSemanticsPose)
+{
+  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<KinematicFeaturesList>::From(plugin);
+    ASSERT_NE(nullptr, engine);
+
+    sdf::Root root;
+    const sdf::Errors errors = root.Load(
+       common_test::worlds::kPoseOffsetSdf);
+    ASSERT_TRUE(errors.empty()) << errors.front();
+
+    auto world = engine->ConstructWorld(*root.WorldByIndex(0));
+    ASSERT_NE(nullptr, world);
+
+    auto model = world->GetModel("model");
+    ASSERT_NE(nullptr, model);
+    auto baseLink = model->GetLink("base");
+    ASSERT_NE(nullptr, baseLink);
+    auto nonBaseLink = model->GetLink("link");
+    ASSERT_NE(nullptr, nonBaseLink);
+    auto baseCol = baseLink->GetShape("base_collision");
+    ASSERT_NE(nullptr, baseCol);
+    auto linkCol = nonBaseLink->GetShape("link_collision");
+    ASSERT_NE(nullptr, linkCol);
+
+    gz::math::Pose3d actualModelPose(1, 0, 0, 0, 0, 0);
+    auto baseLinkFrameData = baseLink->FrameDataRelativeToWorld();
+    auto baseLinkPose = gz::math::eigen3::convert(baseLinkFrameData.pose);
+    gz::math::Pose3d actualLinkLocalPose(0, 1, 0, 0, 0, 0);
+    gz::math::Pose3d expectedLinkWorldPose =
+        actualModelPose * actualLinkLocalPose;
+    EXPECT_EQ(expectedLinkWorldPose, baseLinkPose);
+
+    auto baseColFrameData = baseCol->FrameDataRelativeToWorld();
+    auto baseColPose = gz::math::eigen3::convert(baseColFrameData.pose);
+    gz::math::Pose3d actualColLocalPose(0, 0, 0.01, 0, 0, 0);
+    gz::math::Pose3d expectedColWorldPose =
+        actualModelPose * actualLinkLocalPose * actualColLocalPose;
+    EXPECT_EQ(expectedColWorldPose.Pos(), baseColPose.Pos());
+    EXPECT_EQ(expectedColWorldPose.Rot().Euler(),
+        baseColPose.Rot().Euler());
+
+    auto nonBaseLinkFrameData = nonBaseLink->FrameDataRelativeToWorld();
+    auto nonBaseLinkPose = gz::math::eigen3::convert(nonBaseLinkFrameData.pose);
+    actualLinkLocalPose = gz::math::Pose3d (0, 0, 2.1, -1.5708, 0, 0);
+    expectedLinkWorldPose = actualModelPose * actualLinkLocalPose;
+    EXPECT_EQ(expectedLinkWorldPose, nonBaseLinkPose);
+
+    auto linkColFrameData = linkCol->FrameDataRelativeToWorld();
+    auto linkColPose = gz::math::eigen3::convert(linkColFrameData.pose);
+    actualColLocalPose = gz::math::Pose3d(-0.05, 0, 0, 0, 1.5708, 0);
+    expectedColWorldPose =
+        actualModelPose * actualLinkLocalPose * actualColLocalPose;
+    EXPECT_EQ(expectedColWorldPose.Pos(), linkColPose.Pos());
+    EXPECT_EQ(expectedColWorldPose.Rot().Euler(),
+        linkColPose.Rot().Euler());
+  }
+}
+
 int main(int argc, char *argv[])
 {
   ::testing::InitGoogleTest(&argc, argv);

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

test/common_test/worlds/pose_offset.sdf

@@ -0,0 +1,46 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="pose_offset">
+    <model name="model">
+      <pose>1 0 0 0 0 0</pose>
+      <link name="base">
+        <pose>0 1 0 0 0 0</pose>
+        <inertial>
+          <mass>100</mass>
+        </inertial>
+        <collision name="base_collision">
+          <pose>0 0 0.01 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.8</radius>
+              <length>0.02</length>
+            </cylinder>
+          </geometry>
+        </collision>
+      </link>
+      <link name="link">
+        <pose>0 0 2.1 -1.5708 0 0</pose>
+        <self_collide>0</self_collide>
+        <inertial>
+          <pose>0 0 0.5 0 0 0</pose>
+        </inertial>
+        <collision name="link_collision">
+          <pose>-0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+        </collision>
+      </link>
+      <joint name="joint" type="fixed">
+        <parent>base</parent>
+        <child>link</child>
+        <axis>
+          <xyz>1.0 0 0</xyz>
+        </axis>
+      </joint>
+    </model>
+  </world>
+</sdf>