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(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 @@ 1.0 + 0.0 0 0.3 0 0 0 0.0068 0