bullet-featherstone: Fix attaching fixed joint between models with in…

…ertial pose offset (gazebosim#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 <[email protected]>
rudislabs · Jun 14, 2024 · 6bcf1af · 6bcf1af
1 parent b5d1508
commit 6bcf1af
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 5 deletions.
diff --git 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
@@ -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>