From 4f35c1ed6da947b660b2784670fad9e30036cb10 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 3 Jun 2024 16:34:08 +0000 Subject: [PATCH 1/6] Fix bounding box for collisions with pose offset Signed-off-by: Ian Chen --- bullet-featherstone/src/KinematicsFeatures.cc | 103 +++++++++--------- bullet-featherstone/src/KinematicsFeatures.hh | 3 +- bullet-featherstone/src/ShapeFeatures.cc | 2 - test/common_test/link_features.cc | 22 ++-- 4 files changed, 70 insertions(+), 60 deletions(-) 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/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); From 5c6ec0ff7adc80245b0653580c96cf306a48a7bc Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 4 Jun 2024 18:03:11 +0000 Subject: [PATCH 2/6] add link frame semantics pose test Signed-off-by: Ian Chen --- test/common_test/kinematic_features.cc | 67 +++++++++++++++++++++++--- 1 file changed, 61 insertions(+), 6 deletions(-) diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index effa773dc..939255613 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,51 @@ 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::kTestWorld); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + ASSERT_NE(nullptr, world); + + auto model = world->GetModel("double_pendulum_with_base"); + ASSERT_NE(nullptr, model); + auto upperLink = model->GetLink("upper_link"); + ASSERT_NE(nullptr, upperLink); + auto upperCol = upperLink->GetShape("col_upper_joint"); + ASSERT_NE(nullptr, upperCol); + + gz::math::Pose3d actualModelPose(1, 0, 0, 0, 0, 0); + auto upperLinkFrameData = upperLink->FrameDataRelativeToWorld(); + auto upperLinkPose = gz::math::eigen3::convert(upperLinkFrameData.pose); + gz::math::Pose3d actualLinkLocalPose(0, 0, 2.1, -1.5708, 0, 0); + gz::math::Pose3d expectedLinkWorldPose = + actualModelPose * actualLinkLocalPose; + EXPECT_EQ(expectedLinkWorldPose, upperLinkPose); + + auto upperColFrameData = upperCol->FrameDataRelativeToWorld(); + auto upperColPose = gz::math::eigen3::convert(upperColFrameData.pose); + gz::math::Pose3d actualColLocalPose(-0.05, 0, 0, 0, 1.5708, 0); + gz::math::Pose3d expectedColWorldPose = + actualModelPose * actualLinkLocalPose * actualColLocalPose; + EXPECT_EQ(expectedColWorldPose.Pos(), upperColPose.Pos()); + EXPECT_EQ(expectedColWorldPose.Rot().Euler(), + upperColPose.Rot().Euler()); + } +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); From 84054faf625898319f469f3f6eff0a11d18f238d Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 4 Jun 2024 18:27:21 +0000 Subject: [PATCH 3/6] update test Signed-off-by: Ian Chen --- test/common_test/Worlds.hh | 1 + test/common_test/kinematic_features.cc | 49 +++++++++++++++++-------- test/common_test/worlds/pose_offset.sdf | 46 +++++++++++++++++++++++ 3 files changed, 81 insertions(+), 15 deletions(-) create mode 100644 test/common_test/worlds/pose_offset.sdf 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 939255613..3b5459bee 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -189,35 +189,54 @@ TYPED_TEST(KinematicFeaturesTest, LinkFrameSemanticsPose) sdf::Root root; const sdf::Errors errors = root.Load( - common_test::worlds::kTestWorld); + 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("double_pendulum_with_base"); + auto model = world->GetModel("model"); ASSERT_NE(nullptr, model); - auto upperLink = model->GetLink("upper_link"); - ASSERT_NE(nullptr, upperLink); - auto upperCol = upperLink->GetShape("col_upper_joint"); - ASSERT_NE(nullptr, upperCol); + 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 upperLinkFrameData = upperLink->FrameDataRelativeToWorld(); - auto upperLinkPose = gz::math::eigen3::convert(upperLinkFrameData.pose); - gz::math::Pose3d actualLinkLocalPose(0, 0, 2.1, -1.5708, 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, upperLinkPose); + EXPECT_EQ(expectedLinkWorldPose, baseLinkPose); - auto upperColFrameData = upperCol->FrameDataRelativeToWorld(); - auto upperColPose = gz::math::eigen3::convert(upperColFrameData.pose); - gz::math::Pose3d actualColLocalPose(-0.05, 0, 0, 0, 1.5708, 0); + 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(), upperColPose.Pos()); + EXPECT_EQ(expectedColWorldPose.Pos(), baseColPose.Pos()); EXPECT_EQ(expectedColWorldPose.Rot().Euler(), - upperColPose.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()); } } 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 + + + + + From 27deae998d00387e8dbffc3c10cfeaf42f22d21a Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 4 Jun 2024 22:35:10 -0500 Subject: [PATCH 4/6] Add failing test showing that model framedata calculation is incorrect Signed-off-by: Addisu Z. Taddese --- test/common_test/kinematic_features.cc | 5 ++++- test/common_test/worlds/pose_offset.sdf | 1 + 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index 3b5459bee..fb58f223e 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -80,7 +80,8 @@ struct KinematicFeaturesList : gz::physics::FeatureList< gz::physics::GetJointFromModel, gz::physics::JointFrameSemantics, gz::physics::LinkFrameSemantics, - gz::physics::ShapeFrameSemantics + gz::physics::ShapeFrameSemantics, + gz::physics::ModelFrameSemantics > { }; using KinematicFeaturesTestTypes = @@ -207,6 +208,8 @@ TYPED_TEST(KinematicFeaturesTest, LinkFrameSemanticsPose) ASSERT_NE(nullptr, linkCol); gz::math::Pose3d actualModelPose(1, 0, 0, 0, 0, 0); + auto modelFrameData = model->FrameDataRelativeToWorld(); + EXPECT_EQ(actualModelPose, gz::math::eigen3::convert(modelFrameData.pose)); auto baseLinkFrameData = baseLink->FrameDataRelativeToWorld(); auto baseLinkPose = gz::math::eigen3::convert(baseLinkFrameData.pose); gz::math::Pose3d actualLinkLocalPose(0, 1, 0, 0, 0, 0); diff --git a/test/common_test/worlds/pose_offset.sdf b/test/common_test/worlds/pose_offset.sdf index 97b2145cd..9521e646c 100644 --- a/test/common_test/worlds/pose_offset.sdf +++ b/test/common_test/worlds/pose_offset.sdf @@ -6,6 +6,7 @@ 0 1 0 0 0 0 + 0 0 3 0 0 0 100 From 069668f799da2035d9613a923843355fdd0fdc35 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 5 Jun 2024 15:22:10 +0000 Subject: [PATCH 5/6] fix frame data for models and nested models Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.hh | 26 +++++- bullet-featherstone/src/KinematicsFeatures.cc | 15 +++- bullet-featherstone/src/SDFFeatures.cc | 81 ++++++++++++------- dartsim/src/KinematicsFeatures.cc | 2 + test/common_test/kinematic_features.cc | 41 +++++++++- test/common_test/worlds/pose_offset.sdf | 30 +++++++ 6 files changed, 155 insertions(+), 40 deletions(-) diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 8df941a4c..9a3710ce9 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -84,9 +84,13 @@ struct ModelInfo std::string name; Identity world; int indexInWorld; + Eigen::Isometry3d modelToRootLinkTf; Eigen::Isometry3d baseInertiaToLinkFrame; + Eigen::Isometry3d nestedModelFromRootModelTf; std::shared_ptr body; + bool isNestedModel = false; + std::vector linkEntityIds; std::vector jointEntityIds; std::vector nestedModelEntityIds; @@ -101,11 +105,15 @@ struct ModelInfo ModelInfo( std::string _name, Identity _world, + Eigen::Isometry3d _modelToRootLinkTf, Eigen::Isometry3d _baseInertiaToLinkFrame, + Eigen::Isometry3d _nestedModelFromRootModelTf, std::shared_ptr _body) : name(std::move(_name)), world(std::move(_world)), + modelToRootLinkTf(_modelToRootLinkTf), baseInertiaToLinkFrame(_baseInertiaToLinkFrame), + nestedModelFromRootModelTf(_nestedModelFromRootModelTf), body(std::move(_body)) { // Do nothing @@ -300,8 +308,8 @@ class Base : public Implements3d> auto worldID = this->GenerateIdentity(id, world); auto worldModel = std::make_shared( - world->name, worldID, - Eigen::Isometry3d::Identity(), nullptr); + world->name, worldID, Eigen::Isometry3d::Identity(), + Eigen::Isometry3d::Identity(), Eigen::Isometry3d::Identity(), nullptr); this->models[id] = worldModel; world->modelNameToEntityId[worldModel->name] = id; worldModel->indexInWorld = -1; @@ -313,13 +321,17 @@ class Base : public Implements3d> public: inline Identity AddModel( std::string _name, Identity _worldID, + Eigen::Isometry3d _modelToRootLinkTf, Eigen::Isometry3d _baseInertialToLinkFrame, std::shared_ptr _body) { const auto id = this->GetNextEntity(); auto model = std::make_shared( std::move(_name), std::move(_worldID), - std::move(_baseInertialToLinkFrame), std::move(_body)); + std::move(_modelToRootLinkTf), + std::move(_baseInertialToLinkFrame), + Eigen::Isometry3d::Identity(), + std::move(_body)); this->models[id] = model; auto *world = this->ReferenceInterface(model->world); @@ -338,14 +350,20 @@ class Base : public Implements3d> std::string _name, Identity _parentID, Identity _worldID, + Eigen::Isometry3d _modelToRootLinkTf, Eigen::Isometry3d _baseInertialToLinkFrame, + Eigen::Isometry3d _nestedModelFromRootModelTf, std::shared_ptr _body) { const auto id = this->GetNextEntity(); auto model = std::make_shared( std::move(_name), std::move(_worldID), - std::move(_baseInertialToLinkFrame), std::move(_body)); + std::move(_modelToRootLinkTf), + std::move(_baseInertialToLinkFrame), + std::move(_nestedModelFromRootModelTf), + std::move(_body)); + model->isNestedModel = true; this->models[id] = model; const auto parentModel = this->models.at(_parentID); parentModel->nestedModelEntityIds.push_back(id); diff --git a/bullet-featherstone/src/KinematicsFeatures.cc b/bullet-featherstone/src/KinematicsFeatures.cc index c65cc5b48..82fb6421d 100644 --- a/bullet-featherstone/src/KinematicsFeatures.cc +++ b/bullet-featherstone/src/KinematicsFeatures.cc @@ -113,10 +113,17 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( FrameData data; if (model && model->body) { - data.pose = convert(model->body->getBaseWorldTransform()); - if (!isModel) - data.pose = data.pose * model->baseInertiaToLinkFrame; - if (isCollision) + data.pose = convert(model->body->getBaseWorldTransform()) + * model->baseInertiaToLinkFrame; + if (isModel) + { + data.pose = model->modelToRootLinkTf.inverse() * data.pose; + if (model->isNestedModel) + { + data.pose = data.pose * model->nestedModelFromRootModelTf; + } + } + else 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/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 15e7125a5..d4fea6aef 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -484,6 +484,12 @@ Identity SDFFeatures::ConstructSdfModelImpl( const bool isStatic = _sdfModel.Static(); WorldInfo *world = nullptr; + + const auto modelToRootLink = + ResolveSdfPose(structure.rootLink->SemanticPose()); + if (!modelToRootLink) + return this->GenerateInvalidId(); + const auto rootInertialToLink = gz::math::eigen3::convert(structure.linkToPrincipalAxesPose).inverse(); @@ -493,7 +499,31 @@ Identity SDFFeatures::ConstructSdfModelImpl( std::unordered_map modelIDs; std::size_t rootModelID = 0u; std::shared_ptr rootMultiBody; - // Add all models, including nested models + + auto getModelScopedName = [&](const ::sdf::Model *_targetModel, + const ::sdf::Model *_parentModel, const std::string &_prefix, + std::string &_modelScopedName, auto &&_getModelScopedName) + { + for (std::size_t i = 0u; i < _parentModel->ModelCount(); ++i) + { + auto childModel = _parentModel->ModelByIndex(i); + if (childModel == _targetModel) + { + _modelScopedName = _prefix + childModel->Name(); + return true; + } + else + { + if (_getModelScopedName(_targetModel, childModel, + _prefix + childModel->Name() + "::", _modelScopedName, + _getModelScopedName)) + return true; + } + } + return false; + }; + + // Add all models, including nested models auto addModels = [&](std::size_t _modelOrWorldID, const ::sdf::Model *_model, auto &&_addModels) { @@ -509,8 +539,22 @@ Identity SDFFeatures::ConstructSdfModelImpl( auto worldIdentity = modelIt->second->world; auto modelIdentity = this->GenerateIdentity(_modelOrWorldID, modelIt->second); + + std::string modelScopedName; + math::Pose3d modelPose; + if (!getModelScopedName(_model, &_sdfModel, + _sdfModel.Name() + "::", modelScopedName, getModelScopedName)) + this->GenerateInvalidId(); + auto errors = _sdfModel.SemanticPose().Resolve(modelPose, + modelScopedName); + if (!errors.empty()) + this->GenerateInvalidId(); + auto modelToNestedModel = math::eigen3::convert(modelPose).inverse(); + return this->AddNestedModel( - _model->Name(), modelIdentity, worldIdentity, rootInertialToLink, + _model->Name(), modelIdentity, worldIdentity, + *modelToRootLink, rootInertialToLink, + modelToNestedModel, rootMultiBody); } else @@ -525,7 +569,9 @@ Identity SDFFeatures::ConstructSdfModelImpl( true); world = this->ReferenceInterface(worldIdentity); auto id = this->AddModel( - _model->Name(), worldIdentity, rootInertialToLink, + _model->Name(), worldIdentity, + *modelToRootLink, + rootInertialToLink, rootMultiBody); rootModelID = id; return id; @@ -811,31 +857,9 @@ Identity SDFFeatures::ConstructSdfModelImpl( // get the scoped name of the model // This is used to resolve the model pose std::string modelScopedName; - auto getModelScopedName = [&](const ::sdf::Model *_targetModel, - const ::sdf::Model *_parentModel, const std::string &_prefix, - auto &&_getModelScopedName) - { - for (std::size_t i = 0u; i < _parentModel->ModelCount(); ++i) - { - auto childModel = _parentModel->ModelByIndex(i); - if (childModel == _targetModel) - { - modelScopedName = _prefix + childModel->Name(); - return true; - } - else - { - if (_getModelScopedName(_targetModel, childModel, - _prefix + childModel->Name() + "::", _getModelScopedName)) - return true; - } - } - return false; - }; - math::Pose3d modelPose; if (!getModelScopedName(structure.model, &_sdfModel, - _sdfModel.Name() + "::", getModelScopedName)) + _sdfModel.Name() + "::", modelScopedName, getModelScopedName)) return this->GenerateInvalidId(); auto errors = _sdfModel.SemanticPose().Resolve(modelPose, @@ -845,11 +869,6 @@ Identity SDFFeatures::ConstructSdfModelImpl( modelToNestedModel = math::eigen3::convert(modelPose).inverse(); } - const auto modelToRootLink = - ResolveSdfPose(structure.rootLink->SemanticPose()); - if (!modelToRootLink) - return this->GenerateInvalidId(); - const auto worldToRootCom = *worldToModel * modelToNestedModel * *modelToRootLink * rootInertialToLink.inverse(); diff --git a/dartsim/src/KinematicsFeatures.cc b/dartsim/src/KinematicsFeatures.cc index bc1f0711c..ffc7412c2 100644 --- a/dartsim/src/KinematicsFeatures.cc +++ b/dartsim/src/KinematicsFeatures.cc @@ -69,6 +69,7 @@ const dart::dynamics::Frame *KinematicsFeatures::SelectFrame( { // This is a model FreeGroup frame, so we'll use the first root link as the // frame + std::cerr << " free group frame " << std::endl; return model_it->second->model->getRootBodyNode(); } @@ -77,6 +78,7 @@ const dart::dynamics::Frame *KinematicsFeatures::SelectFrame( { return nullptr; } + std::cerr << " frame it " << std::endl; return framesIt->second; } diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index fb58f223e..c2f047513 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -76,6 +76,7 @@ struct KinematicFeaturesList : gz::physics::FeatureList< gz::physics::sdf::ConstructSdfWorld, gz::physics::GetShapeFromLink, gz::physics::GetModelFromWorld, + gz::physics::GetNestedModelFromModel, gz::physics::GetLinkFromModel, gz::physics::GetJointFromModel, gz::physics::JointFrameSemantics, @@ -207,9 +208,20 @@ TYPED_TEST(KinematicFeaturesTest, LinkFrameSemanticsPose) auto linkCol = nonBaseLink->GetShape("link_collision"); ASSERT_NE(nullptr, linkCol); + auto nestedModel = model->GetNestedModel("nested_model"); + ASSERT_NE(nullptr, nestedModel); + auto nestedLink = nestedModel->GetLink("nested_link"); + ASSERT_NE(nullptr, nestedLink); + auto nestedLinkCol = nestedLink->GetShape("nested_link_collision"); + ASSERT_NE(nullptr, nestedLinkCol); + gz::math::Pose3d actualModelPose(1, 0, 0, 0, 0, 0); auto modelFrameData = model->FrameDataRelativeToWorld(); - EXPECT_EQ(actualModelPose, gz::math::eigen3::convert(modelFrameData.pose)); + if (this->PhysicsEngineName(name) == "bullet-featherstone") + { + EXPECT_EQ(actualModelPose, + gz::math::eigen3::convert(modelFrameData.pose)); + } auto baseLinkFrameData = baseLink->FrameDataRelativeToWorld(); auto baseLinkPose = gz::math::eigen3::convert(baseLinkFrameData.pose); gz::math::Pose3d actualLinkLocalPose(0, 1, 0, 0, 0, 0); @@ -240,6 +252,33 @@ TYPED_TEST(KinematicFeaturesTest, LinkFrameSemanticsPose) EXPECT_EQ(expectedColWorldPose.Pos(), linkColPose.Pos()); EXPECT_EQ(expectedColWorldPose.Rot().Euler(), linkColPose.Rot().Euler()); + + gz::math::Pose3d actualNestedModelLocalPose(0, 0, 1, 0, 0, 0); + if (this->PhysicsEngineName(name) == "bullet-featherstone") + { + auto nestedModelFrameData = nestedModel->FrameDataRelativeToWorld(); + gz::math::Pose3d expectedNestedModelWorldPose = + actualModelPose * actualNestedModelLocalPose; + EXPECT_EQ(expectedNestedModelWorldPose, + gz::math::eigen3::convert(nestedModelFrameData.pose)); + } + + auto nestedLinkFrameData = nestedLink->FrameDataRelativeToWorld(); + auto nestedLinkPose = gz::math::eigen3::convert(nestedLinkFrameData.pose); + gz::math::Pose3d actualNestedLinkLocalPose(0, 2, 0, 0, 0, 0); + gz::math::Pose3d expectedNestedLinkWorldPose = + actualModelPose * actualNestedModelLocalPose * + actualNestedLinkLocalPose; + EXPECT_EQ(expectedNestedLinkWorldPose, nestedLinkPose); + + auto nestedLinkColFrameData = nestedLinkCol->FrameDataRelativeToWorld(); + auto nestedLinkColPose = + gz::math::eigen3::convert(nestedLinkColFrameData.pose); + auto actualNestedColLocalPose = gz::math::Pose3d(-0.5, 0, 0, 0, 1.5708, 0); + auto expectedNestedColWorldPose = + actualModelPose * actualNestedModelLocalPose * + actualNestedLinkLocalPose * actualNestedColLocalPose; + EXPECT_EQ(expectedNestedColWorldPose, nestedLinkColPose); } } diff --git a/test/common_test/worlds/pose_offset.sdf b/test/common_test/worlds/pose_offset.sdf index 9521e646c..2eb70f968 100644 --- a/test/common_test/worlds/pose_offset.sdf +++ b/test/common_test/worlds/pose_offset.sdf @@ -35,6 +35,27 @@ + + + 0 0 1 0 0 0 + + + 0 2 0 0 0 0 + + 0 0 0.5 0 0 0 + + + -0.5 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + + base link @@ -42,6 +63,15 @@ 1.0 0 0 + + + link + nested_model::nested_link + + 0.0 0 1 + + + From 081c672a3d1d5b207f4dcf4a3583df00049d8894 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 5 Jun 2024 15:26:45 +0000 Subject: [PATCH 6/6] remove debug print Signed-off-by: Ian Chen --- dartsim/src/KinematicsFeatures.cc | 2 -- 1 file changed, 2 deletions(-) diff --git a/dartsim/src/KinematicsFeatures.cc b/dartsim/src/KinematicsFeatures.cc index ffc7412c2..bc1f0711c 100644 --- a/dartsim/src/KinematicsFeatures.cc +++ b/dartsim/src/KinematicsFeatures.cc @@ -69,7 +69,6 @@ const dart::dynamics::Frame *KinematicsFeatures::SelectFrame( { // This is a model FreeGroup frame, so we'll use the first root link as the // frame - std::cerr << " free group frame " << std::endl; return model_it->second->model->getRootBodyNode(); } @@ -78,7 +77,6 @@ const dart::dynamics::Frame *KinematicsFeatures::SelectFrame( { return nullptr; } - std::cerr << " frame it " << std::endl; return framesIt->second; }