diff --git a/bullet-featherstone/src/KinematicsFeatures.cc b/bullet-featherstone/src/KinematicsFeatures.cc index 85341ce6a..c65cc5b48 100644 --- a/bullet-featherstone/src/KinematicsFeatures.cc +++ b/bullet-featherstone/src/KinematicsFeatures.cc @@ -22,30 +22,40 @@ namespace gz { namespace physics { namespace bullet_featherstone { + +FrameData3d getNonBaseLinkFrameData(const ModelInfo *_modelInfo, + const LinkInfo *_linkInfo) +{ + 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()); + return data; +} + ///////////////////////////////////////////////// FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( const FrameID &_id) const { - const auto linkIt = this->links.find(_id.ID()); + bool isModel = false; + bool isCollision = false; const ModelInfo *model = nullptr; + Eigen::Isometry3d collisionPoseOffset = Eigen::Isometry3d(); + + const auto linkIt = this->links.find(_id.ID()); if (linkIt != this->links.end()) { const auto &linkInfo = linkIt->second; - const auto indexOpt = linkInfo->indexInModel; model = this->ReferenceInterface(linkInfo->model); - if (indexOpt.has_value()) + if (linkInfo->indexInModel.has_value()) { - const auto index = *indexOpt; - FrameData data; - data.pose = GetWorldTransformOfLink(*model, *linkInfo); - const auto &link = model->body->getLink(index); - data.linearVelocity = convert(link.m_absFrameTotVelocity.getLinear()); - data.angularVelocity = convert(link.m_absFrameTotVelocity.getAngular()); - return data; + return getNonBaseLinkFrameData(model, linkInfo.get()); } - // If indexOpt is nullopt then the link is the base link which will be + // If indexInModel is nullopt then the link is the base link which will be // calculated below. } else @@ -59,60 +69,55 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( if (linkIt2 != this->links.end()) { const auto &linkInfo2 = linkIt2->second; - const auto indexOpt2 = linkInfo2->indexInModel; model = this->ReferenceInterface(linkInfo2->model); - - if (indexOpt2.has_value()) + if (linkInfo2->indexInModel.has_value()) { - const auto index2 = *indexOpt2; - FrameData data; - data.pose = GetWorldTransformOfLink(*model, *linkInfo2); - const auto &link = model->body->getLink(index2); - data.linearVelocity = convert( - link.m_absFrameTotVelocity.getLinear()); - data.angularVelocity = convert( - link.m_absFrameTotVelocity.getAngular()); - return data; + return getNonBaseLinkFrameData(model, linkInfo2.get()); } } } - - auto collisionIt = this->collisions.find(_id.ID()); - if (collisionIt != this->collisions.end()) + else { - const auto &collisionInfo = collisionIt->second; - - const auto linkIt2 = this->links.find(collisionInfo->link); - if (linkIt2 != this->links.end()) + auto collisionIt = this->collisions.find(_id.ID()); + if (collisionIt != this->collisions.end()) { - const auto &linkInfo2 = linkIt2->second; - const auto indexOpt2 = linkInfo2->indexInModel; - model = this->ReferenceInterface(linkInfo2->model); + isCollision = true; + const auto &collisionInfo = collisionIt->second; - if (indexOpt2.has_value()) + const auto linkIt2 = this->links.find(collisionInfo->link); + if (linkIt2 != this->links.end()) { - const auto index2 = *indexOpt2; - FrameData data; - data.pose = GetWorldTransformOfLink(*model, *linkInfo2); - const auto &link = model->body->getLink(index2); - data.linearVelocity = convert( - link.m_absFrameTotVelocity.getLinear()); - data.angularVelocity = convert( - link.m_absFrameTotVelocity.getAngular()); - return data; + const auto &linkInfo2 = linkIt2->second; + model = this->ReferenceInterface(linkInfo2->model); + collisionPoseOffset = collisionInfo->linkToCollision; + if (linkInfo2->indexInModel.has_value()) + { + auto data = getNonBaseLinkFrameData(model, linkInfo2.get()); + data.pose = data.pose * collisionPoseOffset; + return data; + } + } + } + else + { + auto modelIt = this->models.find(_id.ID()); + if (modelIt != this->models.end()) + { + model = modelIt->second.get(); + isModel = true; } } } - - if (!model || model->body == nullptr) - model = this->FrameInterface(_id); } FrameData data; - if(model && model->body) + if (model && model->body) { - data.pose = convert(model->body->getBaseWorldTransform()) - * model->baseInertiaToLinkFrame; + data.pose = convert(model->body->getBaseWorldTransform()); + if (!isModel) + data.pose = data.pose * model->baseInertiaToLinkFrame; + if (isCollision) + data.pose = data.pose * collisionPoseOffset; data.linearVelocity = convert(model->body->getBaseVel()); data.angularVelocity = convert(model->body->getBaseOmega()); } diff --git a/bullet-featherstone/src/KinematicsFeatures.hh b/bullet-featherstone/src/KinematicsFeatures.hh index 259db0dd2..54ff7554c 100644 --- a/bullet-featherstone/src/KinematicsFeatures.hh +++ b/bullet-featherstone/src/KinematicsFeatures.hh @@ -30,7 +30,8 @@ namespace bullet_featherstone { struct KinematicsFeatureList : gz::physics::FeatureList< LinkFrameSemantics, ModelFrameSemantics, - FreeGroupFrameSemantics + FreeGroupFrameSemantics, + ShapeFrameSemantics > { }; class KinematicsFeatures : diff --git a/bullet-featherstone/src/ShapeFeatures.cc b/bullet-featherstone/src/ShapeFeatures.cc index 1f05f51fc..34f78007c 100644 --- a/bullet-featherstone/src/ShapeFeatures.cc +++ b/bullet-featherstone/src/ShapeFeatures.cc @@ -37,8 +37,6 @@ AlignedBox3d ShapeFeatures::GetShapeAxisAlignedBoundingBox( t.setIdentity(); btVector3 minBox(0, 0, 0); btVector3 maxBox(0, 0, 0); - btVector3 minBox2(0, 0, 0); - btVector3 maxBox2(0, 0, 0); collider->collider->getAabb(t, minBox, maxBox); return math::eigen3::convert(math::AxisAlignedBox( math::Vector3d(minBox[0], minBox[1], minBox[2]), diff --git a/test/common_test/Worlds.hh b/test/common_test/Worlds.hh index 2328cb033..326ac201d 100644 --- a/test/common_test/Worlds.hh +++ b/test/common_test/Worlds.hh @@ -47,6 +47,7 @@ const auto kMimicUniversalWorld = CommonTestWorld("mimic_universal_world.sdf"); const auto kMultipleCollisionsSdf = CommonTestWorld("multiple_collisions.sdf"); const auto kPendulumJointWrenchSdf = CommonTestWorld("pendulum_joint_wrench.sdf"); +const auto kPoseOffsetSdf = CommonTestWorld("pose_offset.sdf"); const auto kShapesWorld = CommonTestWorld("shapes.world"); const auto kShapesBitmaskWorld = CommonTestWorld("shapes_bitmask.sdf"); const auto kSlipComplianceSdf = CommonTestWorld("slip_compliance.sdf"); diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index effa773dc..3b5459bee 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -17,6 +17,7 @@ #include #include +#include #include #include "test/TestLibLoader.hh" @@ -29,6 +30,7 @@ #include #include #include +#include #include #include @@ -72,11 +74,13 @@ struct KinematicFeaturesList : gz::physics::FeatureList< gz::physics::GetEngineInfo, gz::physics::ForwardStep, gz::physics::sdf::ConstructSdfWorld, + gz::physics::GetShapeFromLink, gz::physics::GetModelFromWorld, gz::physics::GetLinkFromModel, gz::physics::GetJointFromModel, + gz::physics::JointFrameSemantics, gz::physics::LinkFrameSemantics, - gz::physics::JointFrameSemantics + gz::physics::ShapeFrameSemantics > { }; using KinematicFeaturesTestTypes = @@ -146,12 +150,18 @@ TYPED_TEST(KinematicFeaturesTest, JointFrameSemantics) EXPECT_EQ( F_WCexpected.pose.rotation(), childLinkFrameData.pose.rotation()); - // TODO(ahcorde): Rewiew this in bullet-featherstone - if(this->PhysicsEngineName(name) == "bullet_featherstone") + // TODO(ahcorde): Review this in bullet-featherstone + if (this->PhysicsEngineName(name) != "bullet-featherstone") { - EXPECT_EQ( - F_WCexpected.pose.translation(), - childLinkFrameData.pose.translation()); + 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); } EXPECT_TRUE( gz::physics::test::Equal( @@ -166,6 +176,70 @@ TYPED_TEST(KinematicFeaturesTest, JointFrameSemantics) } } +TYPED_TEST(KinematicFeaturesTest, LinkFrameSemanticsPose) +{ + for (const std::string &name : this->pluginNames) + { + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = + gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors errors = root.Load( + common_test::worlds::kPoseOffsetSdf); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + ASSERT_NE(nullptr, world); + + auto model = world->GetModel("model"); + ASSERT_NE(nullptr, model); + auto baseLink = model->GetLink("base"); + ASSERT_NE(nullptr, baseLink); + auto nonBaseLink = model->GetLink("link"); + ASSERT_NE(nullptr, nonBaseLink); + auto baseCol = baseLink->GetShape("base_collision"); + ASSERT_NE(nullptr, baseCol); + auto linkCol = nonBaseLink->GetShape("link_collision"); + ASSERT_NE(nullptr, linkCol); + + gz::math::Pose3d actualModelPose(1, 0, 0, 0, 0, 0); + auto baseLinkFrameData = baseLink->FrameDataRelativeToWorld(); + auto baseLinkPose = gz::math::eigen3::convert(baseLinkFrameData.pose); + gz::math::Pose3d actualLinkLocalPose(0, 1, 0, 0, 0, 0); + gz::math::Pose3d expectedLinkWorldPose = + actualModelPose * actualLinkLocalPose; + EXPECT_EQ(expectedLinkWorldPose, baseLinkPose); + + auto baseColFrameData = baseCol->FrameDataRelativeToWorld(); + auto baseColPose = gz::math::eigen3::convert(baseColFrameData.pose); + gz::math::Pose3d actualColLocalPose(0, 0, 0.01, 0, 0, 0); + gz::math::Pose3d expectedColWorldPose = + actualModelPose * actualLinkLocalPose * actualColLocalPose; + EXPECT_EQ(expectedColWorldPose.Pos(), baseColPose.Pos()); + EXPECT_EQ(expectedColWorldPose.Rot().Euler(), + baseColPose.Rot().Euler()); + + auto nonBaseLinkFrameData = nonBaseLink->FrameDataRelativeToWorld(); + auto nonBaseLinkPose = gz::math::eigen3::convert(nonBaseLinkFrameData.pose); + actualLinkLocalPose = gz::math::Pose3d (0, 0, 2.1, -1.5708, 0, 0); + expectedLinkWorldPose = actualModelPose * actualLinkLocalPose; + EXPECT_EQ(expectedLinkWorldPose, nonBaseLinkPose); + + auto linkColFrameData = linkCol->FrameDataRelativeToWorld(); + auto linkColPose = gz::math::eigen3::convert(linkColFrameData.pose); + actualColLocalPose = gz::math::Pose3d(-0.05, 0, 0, 0, 1.5708, 0); + expectedColWorldPose = + actualModelPose * actualLinkLocalPose * actualColLocalPose; + EXPECT_EQ(expectedColWorldPose.Pos(), linkColPose.Pos()); + EXPECT_EQ(expectedColWorldPose.Rot().Euler(), + linkColPose.Rot().Euler()); + } +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/test/common_test/link_features.cc b/test/common_test/link_features.cc index adcb54817..5f5ae3112 100644 --- a/test/common_test/link_features.cc +++ b/test/common_test/link_features.cc @@ -293,14 +293,26 @@ TYPED_TEST(LinkFeaturesTest, JointSetCommand) } } -TYPED_TEST(LinkFeaturesTest, AxisAlignedBoundingBox) +using LinkBoundingBoxFeaturesList = gz::physics::FeatureList< + gz::physics::ForwardStep, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::GetEntities, + gz::physics::GetLinkBoundingBox, + gz::physics::GetModelBoundingBox +>; + +using LinkBoundingBoxFeaturesTestTypes = + LinkFeaturesTest; + +TEST_F(LinkBoundingBoxFeaturesTestTypes, AxisAlignedBoundingBox) { for (const std::string &name : this->pluginNames) { std::cout << "Testing plugin: " << name << std::endl; gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); - auto engine = gz::physics::RequestEngine3d::From(plugin); + auto engine = + gz::physics::RequestEngine3d::From(plugin); ASSERT_NE(nullptr, engine); sdf::Root root; @@ -313,9 +325,6 @@ TYPED_TEST(LinkFeaturesTest, AxisAlignedBoundingBox) auto world = engine->ConstructWorld(*root.WorldByIndex(0)); EXPECT_NE(nullptr, world); - EXPECT_NE(nullptr, world); - world->SetGravity(Eigen::Vector3d{0, 0, -9.8}); - auto model = world->GetModel("double_pendulum_with_base"); auto baseLink = model->GetLink("base"); auto bbox = baseLink->GetAxisAlignedBoundingBox(); @@ -364,9 +373,6 @@ TYPED_TEST(LinkFeaturesTest, ModelAxisAlignedBoundingBox) auto world = engine->ConstructWorld(*root.WorldByIndex(0)); EXPECT_NE(nullptr, world); - EXPECT_NE(nullptr, world); - world->SetGravity(Eigen::Vector3d{0, 0, -9.8}); - auto model = world->GetModel("sphere"); auto bbox = model->GetAxisAlignedBoundingBox(); AssertVectorApprox vectorPredicate(1e-4); diff --git a/test/common_test/worlds/pose_offset.sdf b/test/common_test/worlds/pose_offset.sdf new file mode 100644 index 000000000..97b2145cd --- /dev/null +++ b/test/common_test/worlds/pose_offset.sdf @@ -0,0 +1,46 @@ + + + + + 1 0 0 0 0 0 + + 0 1 0 0 0 0 + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + + base + link + + 1.0 0 0 + + + + +