bullet-featherstone: Fix joint frame data pose

bullet-featherstone/src/Base.hh

@@ -136,6 +136,8 @@ struct ModelInfo
   Eigen::Isometry3d baseInertiaToLinkFrame;
   std::shared_ptr<GzMultiBody> body;
 
+  bool isNestedModel = false;
+
   std::vector<std::size_t> linkEntityIds;
   std::vector<std::size_t> jointEntityIds;
   std::vector<std::size_t> nestedModelEntityIds;
@@ -416,6 +418,7 @@ class Base : public Implements3d<FeatureList<Feature>>
       std::move(_baseInertialToLinkFrame),
       std::move(_body));
 
+    model->isNestedModel = true;
     this->models[id] = model;
     const auto parentModel = this->models.at(_parentID);
     parentModel->nestedModelEntityIds.push_back(id);

bullet-featherstone/src/KinematicsFeatures.cc

@@ -29,6 +29,7 @@ FrameData3d getNonBaseLinkFrameData(const ModelInfo *_modelInfo,
   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());
@@ -41,39 +42,46 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld(
 {
   bool isModel = false;
   bool isCollision = false;
+  bool isJoint = false;
   const ModelInfo *model = nullptr;
   Eigen::Isometry3d collisionPoseOffset = Eigen::Isometry3d();
+  Eigen::Isometry3d jointPoseOffset = Eigen::Isometry3d();
 
   const auto linkIt = this->links.find(_id.ID());
   if (linkIt != this->links.end())
   {
     const auto &linkInfo = linkIt->second;
     model = this->ReferenceInterface<ModelInfo>(linkInfo->model);
 
+    // If indexInModel has value, then this is a non-base link
+    // otherwise, it is a base link and the calculations will be performed
+    // later below in this function
     if (linkInfo->indexInModel.has_value())
     {
       return getNonBaseLinkFrameData(model, linkInfo.get());
     }
-
-    // If indexInModel is nullopt then the link is the base link which will be
-    // calculated below.
   }
   else
   {
     auto jointIt = this->joints.find(_id.ID());
     if (jointIt != this->joints.end())
     {
+      isJoint = true;
       const auto &jointInfo = jointIt->second;
 
       const auto linkIt2 = this->links.find(jointInfo->childLinkID);
       if (linkIt2 != this->links.end())
       {
         const auto &linkInfo2 = linkIt2->second;
         model = this->ReferenceInterface<ModelInfo>(linkInfo2->model);
-        // If indexInModel has value, then this is a non-base link
+        jointPoseOffset = jointInfo->tf_to_child.inverse();
+        // If indexInModel has value, then this is a joint connected to
+        // non-base link
         if (linkInfo2->indexInModel.has_value())
         {
-          return getNonBaseLinkFrameData(model, linkInfo2.get());
+          auto data = getNonBaseLinkFrameData(model, linkInfo2.get());
+          data.pose = data.pose * jointPoseOffset;
+          return data;
         }
       }
     }
@@ -91,7 +99,8 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld(
           const auto &linkInfo2 = linkIt2->second;
           model = this->ReferenceInterface<ModelInfo>(linkInfo2->model);
           collisionPoseOffset = collisionInfo->linkToCollision;
-          // If indexInModel has value, then this is a non-base link
+          // If indexInModel has value, then this is a collision in a
+          // non-base link
           if (linkInfo2->indexInModel.has_value())
           {
             auto data = getNonBaseLinkFrameData(model, linkInfo2.get());
@@ -113,7 +122,8 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld(
   }
 
   // The function reached here so that means the entity is either
-  // a model, a base link, or a collision in the base link
+  // a model, a base link, a collision in the base link, or a joint
+  // connected to a base link.
   FrameData data;
   if (model && model->body)
   {
@@ -123,6 +133,8 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld(
       data.pose = data.pose * model->rootLinkToModelTf;
     else if (isCollision)
       data.pose = data.pose * collisionPoseOffset;
+    else if (isJoint)
+      data.pose = data.pose * jointPoseOffset;
     data.linearVelocity = convert(model->body->getBaseVel());
     data.angularVelocity = convert(model->body->getBaseOmega());
   }

test/common_test/kinematic_features.cc

@@ -150,21 +150,9 @@ TYPED_TEST(KinematicFeaturesTest, JointFrameSemantics)
 
     auto childLinkFrameData = childLink->FrameDataRelativeToWorld();
     EXPECT_EQ(
-          F_WCexpected.pose.rotation(),
-          childLinkFrameData.pose.rotation());
-    // TODO(ahcorde): Review this in bullet-featherstone
-    if (this->PhysicsEngineName(name) != "bullet-featherstone")
-    {
-      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);
-    }
+          gz::math::eigen3::convert(F_WCexpected.pose),
+          gz::math::eigen3::convert(childLinkFrameData.pose));
+
     EXPECT_TRUE(
       gz::physics::test::Equal(
           F_WCexpected.linearVelocity,