From 1195a26eef4a3e122138a02132affdc7e8945227 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 5 Jun 2024 17:41:36 +0000 Subject: [PATCH] Fix joint frame data pose Signed-off-by: Ian Chen --- bullet-featherstone/src/KinematicsFeatures.cc | 11 ++++++++++- test/common_test/kinematic_features.cc | 18 +++--------------- 2 files changed, 13 insertions(+), 16 deletions(-) diff --git a/bullet-featherstone/src/KinematicsFeatures.cc b/bullet-featherstone/src/KinematicsFeatures.cc index 82fb6421d..352946f3b 100644 --- a/bullet-featherstone/src/KinematicsFeatures.cc +++ b/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,8 +42,10 @@ 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()) @@ -63,6 +66,7 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( 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); @@ -70,9 +74,12 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( { const auto &linkInfo2 = linkIt2->second; model = this->ReferenceInterface(linkInfo2->model); + jointPoseOffset = jointInfo->tf_to_child.inverse(); if (linkInfo2->indexInModel.has_value()) { - return getNonBaseLinkFrameData(model, linkInfo2.get()); + auto data = getNonBaseLinkFrameData(model, linkInfo2.get()); + data.pose = data.pose * jointPoseOffset; + return data; } } } @@ -125,6 +132,8 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( } 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()); } diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index c2f047513..cdf117223 100644 --- a/test/common_test/kinematic_features.cc +++ b/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,