From 6bcf1af48b016a1e8300e0eed6dcd008fa044cea Mon Sep 17 00:00:00 2001
From: Ian Chen <ichen@openrobotics.org>
Date: Thu, 13 Jun 2024 17:30:45 -0700
Subject: [PATCH] bullet-featherstone: Fix attaching fixed joint between models
 with inertial pose offset (#653)

When creating a fixed constraint between links, the SetJointTransformFromParent currently does not take into account the inertial pose. If an inertial pose offset exists, the fixed constraint causes the 2 models to be displaced from their original position. This PR updates the calculation in SetJointTransformFromParent to include inertial pose offset.

The detachable_joint.world test world is updated to use a model with inertial pose offset - this world is used by the CorrectAttachmentPoints test in detachable_joint common test. The test fails without the changes in this PR.
---------

Signed-off-by: Ian Chen <ichen@openrobotics.org>
---
 bullet-featherstone/src/JointFeatures.cc       | 18 +++++++++++++-----
 test/common_test/worlds/detachable_joint.world |  1 +
 2 files changed, 14 insertions(+), 5 deletions(-)

diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc
index 9618bb20c..e2c460a32 100644
--- a/bullet-featherstone/src/JointFeatures.cc
+++ b/bullet-featherstone/src/JointFeatures.cc
@@ -509,11 +509,19 @@ void JointFeatures::SetJointTransformFromParent(
 
   if (jointInfo->fixedConstraint)
   {
-      auto tf = convertTf(_pose);
-      jointInfo->fixedConstraint->setPivotInA(
-        tf.getOrigin());
-      jointInfo->fixedConstraint->setFrameInA(
-        tf.getBasis());
+    Eigen::Isometry3d parentInertiaToLinkFrame = Eigen::Isometry3d::Identity();
+    if (jointInfo->parentLinkID.has_value())
+    {
+      auto parentLinkInfo = this->links.at(jointInfo->parentLinkID.value());
+      parentInertiaToLinkFrame = parentLinkInfo->inertiaToLinkFrame;
+    }
+    auto *linkInfo = this->ReferenceInterface<LinkInfo>(jointInfo->childLinkID);
+    auto tf = convertTf(parentInertiaToLinkFrame * _pose *
+                        linkInfo->inertiaToLinkFrame.inverse());
+    jointInfo->fixedConstraint->setPivotInA(
+      tf.getOrigin());
+    jointInfo->fixedConstraint->setFrameInA(
+      tf.getBasis());
   }
 }
 
diff --git a/test/common_test/worlds/detachable_joint.world b/test/common_test/worlds/detachable_joint.world
index b7e4c5310..e6922e931 100644
--- a/test/common_test/worlds/detachable_joint.world
+++ b/test/common_test/worlds/detachable_joint.world
@@ -101,6 +101,7 @@
       <link name="link2">
         <inertial>
           <mass>1.0</mass>
+          <pose>0.0 0 0.3 0 0 0</pose>
           <inertia>
             <ixx>0.0068</ixx>
             <ixy>0</ixy>