diff --git a/tpe/lib/src/Collision_TEST.cc b/tpe/lib/src/Collision_TEST.cc index e81285ade..ae35bb856 100644 --- a/tpe/lib/src/Collision_TEST.cc +++ b/tpe/lib/src/Collision_TEST.cc @@ -44,7 +44,7 @@ TEST(Collision, BasicAPI) ASSERT_NE(nullptr, collisionEnt.GetParent()); collisionEnt.SetPose(math::Pose3d(0, 0.2, 0.5, 0, 1, 0)); - EXPECT_EQ(math::Pose3d(3.06472, 2.2, 1.27944, 0.691965, 1.10889, 0.707449), + EXPECT_EQ(math::Pose3d(1.05416, 2.17281, 3.50715, 0.265579, 1.18879, 0.527304), collisionEnt.GetWorldPose()); EXPECT_EQ(0xFF, collision.GetCollideBitmask()); diff --git a/tpe/lib/src/Entity.cc b/tpe/lib/src/Entity.cc index ce99e2ddf..c777ed583 100644 --- a/tpe/lib/src/Entity.cc +++ b/tpe/lib/src/Entity.cc @@ -139,7 +139,7 @@ math::Pose3d Entity::GetPose() const math::Pose3d Entity::GetWorldPose() const { if (this->dataPtr->parent) - return this->dataPtr->pose * this->dataPtr->parent->GetWorldPose(); + return this->dataPtr->parent->GetWorldPose() * this->dataPtr->pose; return this->dataPtr->pose; } diff --git a/tpe/lib/src/Link_TEST.cc b/tpe/lib/src/Link_TEST.cc index 351650d89..6481b47c6 100644 --- a/tpe/lib/src/Link_TEST.cc +++ b/tpe/lib/src/Link_TEST.cc @@ -45,7 +45,7 @@ TEST(Link, BasicAPI) ASSERT_NE(nullptr, linkEnt.GetParent()); linkEnt.SetPose(math::Pose3d(0, 0.2, 0.5, 0, 1, 0)); - EXPECT_EQ(math::Pose3d(7.08596, 0.2, -6.83411, 1, 1, 0), + EXPECT_EQ(math::Pose3d(10, -0.312675, 2.43845, 1.23686, 0.471978, 0.918989), linkEnt.GetWorldPose()); Link link2; diff --git a/tpe/plugin/src/FreeGroupFeatures.cc b/tpe/plugin/src/FreeGroupFeatures.cc index 84d0e781c..288a208ab 100644 --- a/tpe/plugin/src/FreeGroupFeatures.cc +++ b/tpe/plugin/src/FreeGroupFeatures.cc @@ -17,6 +17,8 @@ #include +#include + #include #include @@ -31,7 +33,7 @@ using namespace tpeplugin; Identity FreeGroupFeatures::FindFreeGroupForModel( const Identity &_modelID) const { - auto it = this->models.find(_modelID); + auto it = this->models.find(_modelID.id); if (it == this->models.end() || it->second == nullptr) return this->GenerateInvalidId(); auto modelPtr = it->second; @@ -46,9 +48,9 @@ Identity FreeGroupFeatures::FindFreeGroupForModel( Identity FreeGroupFeatures::FindFreeGroupForLink( const Identity &_linkID) const { - auto it = this->links.find(_linkID); + auto it = this->links.find(_linkID.id); if (it != this->links.end() && it->second != nullptr) - return this->GenerateIdentity(_linkID, it->second); + return this->GenerateIdentity(_linkID.id, it->second); return this->GenerateInvalidId(); } @@ -58,11 +60,11 @@ Identity FreeGroupFeatures::GetFreeGroupCanonicalLink( { // assume no canonical link for now // assume groupID ~= modelID - const auto model_it = this->models.find(_groupID); - if (model_it != this->models.end() && model_it->second != nullptr) + const auto modelIt = this->models.find(_groupID.id); + if (modelIt != this->models.end() && modelIt->second != nullptr) { // assume canonical link is the first link in model - tpelib::Entity &link = model_it->second->model->GetCanonicalLink(); + tpelib::Entity &link = modelIt->second->model->GetCanonicalLink(); auto linkPtr = std::make_shared(); linkPtr->link = static_cast(&link); return this->GenerateIdentity(link.GetId(), linkPtr); @@ -75,12 +77,24 @@ void FreeGroupFeatures::SetFreeGroupWorldPose( const Identity &_groupID, const PoseType &_pose) { - // assume no canonical link for now - // assume groupID ~= modelID - auto it = this->models.find(_groupID); - if (it != this->models.end() && it->second != nullptr) - // convert Eigen::Tranform to Math::Pose3d - it->second->model->SetPose(math::eigen3::convert(_pose)); + auto modelIt = this->models.find(_groupID.id); + auto linkIt = this->links.find(_groupID.id); + if (modelIt != this->models.end()) + { + if (modelIt->second != nullptr) + modelIt->second->model->SetPose(math::eigen3::convert(_pose)); + } + else if (linkIt != this->links.end()) + { + if (linkIt->second != nullptr) + linkIt->second->link->SetPose(math::eigen3::convert(_pose)); + } + else + { + ignwarn << "No free group with id [" << _groupID.id << "] found." + << std::endl; + return; + } } ///////////////////////////////////////////////// @@ -90,7 +104,7 @@ void FreeGroupFeatures::SetFreeGroupWorldLinearVelocity( { // assume no canonical link for now // assume groupID ~= modelID - auto it = this->models.find(_groupID); + auto it = this->models.find(_groupID.id); // set model linear velocity if (it != this->models.end() && it->second != nullptr) it->second->model->SetLinearVelocity( @@ -103,7 +117,7 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity( { // assume no canonical link for now // assume groupID ~= modelID - auto it = this->models.find(_groupID); + auto it = this->models.find(_groupID.id); // set model angular velocity if (it != this->models.end() && it->second != nullptr) it->second->model->SetAngularVelocity( diff --git a/tpe/plugin/src/KinematicsFeatures.cc b/tpe/plugin/src/KinematicsFeatures.cc index 1d1368f4a..4ff1a6050 100644 --- a/tpe/plugin/src/KinematicsFeatures.cc +++ b/tpe/plugin/src/KinematicsFeatures.cc @@ -38,34 +38,26 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( assert(false); return data; } - // check if id is present and skip if not - auto it = this->childIdToParentId.find(_id.ID()); - if (it == this->childIdToParentId.end()) - { - ignwarn << "Entity [" << _id.ID() << "] is not found." << std::endl; - return data; - } - auto modelIt = this->models.find(it->second); - auto worldIt = this->worlds.find(it->second); + auto modelIt = this->models.find(_id.ID()); + auto linkIt = this->links.find(_id.ID()); + if (modelIt != this->models.end()) { - // \todo(anyone): add link offset to consider link pose auto model = modelIt->second->model; - data.pose = math::eigen3::convert(model->GetPose()); + data.pose = math::eigen3::convert(model->GetWorldPose()); data.linearVelocity = math::eigen3::convert(model->GetLinearVelocity()); data.angularVelocity = math::eigen3::convert(model->GetAngularVelocity()); } - else if (worldIt != this->worlds.end()) + else if (linkIt != this->links.end()) { - auto world = worldIt->second->world; - data.pose = math::eigen3::convert(world->GetPose()); + auto link = linkIt->second->link; + data.pose = math::eigen3::convert(link->GetWorldPose()); } else { - ignwarn << "Neither world or model with id [" - << it->second << "] is not found" << std::endl; + ignwarn << "Entity with id [" + << _id.ID() << "] is not found" << std::endl; } - return data; } diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc index d6693d9d1..35883e14b 100644 --- a/tpe/plugin/src/SDFFeatures.cc +++ b/tpe/plugin/src/SDFFeatures.cc @@ -28,6 +28,32 @@ namespace ignition { namespace physics { namespace tpeplugin { +namespace { +///////////////////////////////////////////////// +/// \brief Resolve the pose of an SDF DOM object with respect to its relative_to +/// frame. If that fails, return the raw pose +static math::Pose3d ResolveSdfPose(const ::sdf::SemanticPose &_semPose) +{ + math::Pose3d pose; + ::sdf::Errors errors = _semPose.Resolve(pose); + if (!errors.empty()) + { + if (!_semPose.RelativeTo().empty()) + { + ignerr << "There was an error in SemanticPose::Resolve\n"; + for (const auto &err : errors) + { + ignerr << err.Message() << std::endl; + } + ignerr << "There is no optimal fallback since the relative_to attribute[" + << _semPose.RelativeTo() << "] of the pose is not empty. " + << "Falling back to using the raw Pose.\n"; + } + pose = _semPose.RawPose(); + } + return pose; +} +} ///////////////////////////////////////////////// Identity SDFFeatures::ConstructSdfWorld( const Identity &_engine, @@ -51,7 +77,7 @@ Identity SDFFeatures::ConstructSdfModel( { // Read sdf params const std::string name = _sdfModel.Name(); - const auto pose = _sdfModel.RawPose(); + const auto pose = ResolveSdfPose(_sdfModel.SemanticPose()); auto it = this->worlds.find(_worldID.id); if (it == this->worlds.end()) @@ -87,7 +113,7 @@ Identity SDFFeatures::ConstructSdfLink( { // Read sdf params const std::string name = _sdfLink.Name(); - const auto pose = _sdfLink.RawPose(); + const auto pose = ResolveSdfPose(_sdfLink.SemanticPose()); auto it = this->models.find(_modelID); if (it == this->models.end()) @@ -123,7 +149,7 @@ Identity SDFFeatures::ConstructSdfCollision( { // Read sdf params const std::string name = _sdfCollision.Name(); - const auto pose = _sdfCollision.RawPose(); + const auto pose = ResolveSdfPose(_sdfCollision.SemanticPose()); const auto geom = _sdfCollision.Geom(); auto it = this->links.find(_linkID); diff --git a/tpe/plugin/src/SDFFeatures_TEST.cc b/tpe/plugin/src/SDFFeatures_TEST.cc index 9695080e6..32f723d23 100644 --- a/tpe/plugin/src/SDFFeatures_TEST.cc +++ b/tpe/plugin/src/SDFFeatures_TEST.cc @@ -97,6 +97,7 @@ TEST(SDFFeatures_TEST, CheckTpeData) model.GetId()); EXPECT_EQ("ground_plane", model.GetName()); EXPECT_EQ(ignition::math::Pose3d::Zero, model.GetPose()); + EXPECT_EQ(ignition::math::Pose3d::Zero, model.GetWorldPose()); EXPECT_EQ(1u, model.GetChildCount()); ignition::physics::tpelib::Entity &link = model.GetChildByName("link"); @@ -104,6 +105,7 @@ TEST(SDFFeatures_TEST, CheckTpeData) link.GetId()); EXPECT_EQ("link", link.GetName()); EXPECT_EQ(ignition::math::Pose3d::Zero, link.GetPose()); + EXPECT_EQ(ignition::math::Pose3d::Zero, link.GetWorldPose()); EXPECT_EQ(1u, link.GetChildCount()); ignition::physics::tpelib::Entity &collision = @@ -112,6 +114,7 @@ TEST(SDFFeatures_TEST, CheckTpeData) collision.GetId()); EXPECT_EQ("collision", collision.GetName()); EXPECT_EQ(ignition::math::Pose3d::Zero, collision.GetPose()); + EXPECT_EQ(ignition::math::Pose3d::Zero, collision.GetWorldPose()); } // check model 02 @@ -122,6 +125,7 @@ TEST(SDFFeatures_TEST, CheckTpeData) model.GetId()); EXPECT_EQ("double_pendulum_with_base", model.GetName()); EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, 0, 0), model.GetPose()); + EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, 0, 0), model.GetWorldPose()); EXPECT_EQ(3u, model.GetChildCount()); ignition::physics::tpelib::Entity &link = model.GetChildByName("base"); @@ -129,6 +133,7 @@ TEST(SDFFeatures_TEST, CheckTpeData) link.GetId()); EXPECT_EQ("base", link.GetName()); EXPECT_EQ(ignition::math::Pose3d::Zero, link.GetPose()); + EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, 0, 0), model.GetWorldPose()); EXPECT_EQ(2u, link.GetChildCount()); ignition::physics::tpelib::Entity &collision = @@ -137,6 +142,7 @@ TEST(SDFFeatures_TEST, CheckTpeData) collision.GetId()); EXPECT_EQ("col_plate_on_ground", collision.GetName()); EXPECT_EQ(ignition::math::Pose3d(0, 0, 0.01, 0, 0, 0), collision.GetPose()); + EXPECT_EQ(ignition::math::Pose3d(1, 0, 0.01, 0, 0, 0), collision.GetWorldPose()); ignition::physics::tpelib::Entity &collision02 = link.GetChildByName("col_pole"); @@ -145,15 +151,18 @@ TEST(SDFFeatures_TEST, CheckTpeData) EXPECT_EQ("col_pole", collision02.GetName()); EXPECT_EQ(ignition::math::Pose3d(-0.275, 0, 1.1, 0, 0, 0), collision02.GetPose()); + EXPECT_EQ(ignition::math::Pose3d(0.725, 0, 1.1, 0, 0, 0), + collision02.GetWorldPose()); ignition::physics::tpelib::Entity &link02 = model.GetChildByName("upper_link"); ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), link02.GetId()); EXPECT_EQ("upper_link", link02.GetName()); - EXPECT_EQ( - ignition::math::Pose3d(0, 0, 2.1, -1.5708, 0, 0), + EXPECT_EQ(ignition::math::Pose3d(0, 0, 2.1, -1.5708, 0, 0), link02.GetPose()); + EXPECT_EQ(ignition::math::Pose3d(1, 0, 2.1, -1.5708, 0, 0), + link02.GetWorldPose()); EXPECT_EQ(3u, link02.GetChildCount()); ignition::physics::tpelib::Entity &collision03 = @@ -163,6 +172,9 @@ TEST(SDFFeatures_TEST, CheckTpeData) EXPECT_EQ("col_upper_joint", collision03.GetName()); EXPECT_EQ(ignition::math::Pose3d(-0.05, 0, 0, 0, 1.5708, 0), collision03.GetPose()); + EXPECT_EQ( + ignition::math::Pose3d(0.95, 0, 2.1, -1.5708, -4e-06, -1.5708), + collision03.GetWorldPose()); ignition::physics::tpelib::Entity &collision04 = link02.GetChildByName("col_lower_joint"); @@ -171,6 +183,9 @@ TEST(SDFFeatures_TEST, CheckTpeData) EXPECT_EQ("col_lower_joint", collision04.GetName()); EXPECT_EQ(ignition::math::Pose3d(0, 0, 1.0, 0, 1.5708, 0), collision04.GetPose()); + EXPECT_EQ( + ignition::math::Pose3d(1, 1, 2.1, -1.5708, -4e-06, -1.5708), + collision04.GetWorldPose()); ignition::physics::tpelib::Entity &collision05 = link02.GetChildByName("col_cylinder"); @@ -179,6 +194,8 @@ TEST(SDFFeatures_TEST, CheckTpeData) EXPECT_EQ("col_cylinder", collision05.GetName()); EXPECT_EQ(ignition::math::Pose3d(0, 0, 0.5, 0, 0, 0), collision05.GetPose()); + EXPECT_EQ(ignition::math::Pose3d(1, 0.5, 2.1, -1.5708, 0, 0), + collision05.GetWorldPose()); ignition::physics::tpelib::Entity &link03 = model.GetChildByName("lower_link"); @@ -187,6 +204,8 @@ TEST(SDFFeatures_TEST, CheckTpeData) EXPECT_EQ("lower_link", link03.GetName()); EXPECT_EQ(ignition::math::Pose3d(0.25, 1.0, 2.1, -2, 0, 0), link03.GetPose()); + EXPECT_EQ(ignition::math::Pose3d(1.25, 1.0, 2.1, -2, 0, 0), + link03.GetWorldPose()); EXPECT_EQ(2u, link03.GetChildCount()); ignition::physics::tpelib::Entity &collision06 = @@ -196,6 +215,9 @@ TEST(SDFFeatures_TEST, CheckTpeData) EXPECT_EQ("col_lower_joint", collision06.GetName()); EXPECT_EQ(ignition::math::Pose3d(0, 0, 0, 0, 1.5708, 0), collision06.GetPose()); + EXPECT_EQ( + ignition::math::Pose3d(1.25, 1, 2.1, -1.57079, -0.429204, -1.5708), + collision06.GetWorldPose()); ignition::physics::tpelib::Entity &collision07 = link03.GetChildByName("col_cylinder"); @@ -204,6 +226,8 @@ TEST(SDFFeatures_TEST, CheckTpeData) EXPECT_EQ("col_cylinder", collision07.GetName()); EXPECT_EQ(ignition::math::Pose3d(0, 0, 0.5, 0, 0, 0), collision07.GetPose()); + EXPECT_EQ(ignition::math::Pose3d(1.25, 1.45465, 1.89193, -2, 0, 0), + collision07.GetWorldPose()); } // check model 03 diff --git a/tpe/plugin/src/SimulationFeatures_TEST.cc b/tpe/plugin/src/SimulationFeatures_TEST.cc index 741698d8f..363b4fee4 100644 --- a/tpe/plugin/src/SimulationFeatures_TEST.cc +++ b/tpe/plugin/src/SimulationFeatures_TEST.cc @@ -214,11 +214,16 @@ TEST_P(SimulationFeatures_TEST, FreeGroup) for (const auto &world : worlds) { + // model free group test auto model = world->GetModel("sphere"); auto freeGroup = model->FindFreeGroup(); ASSERT_NE(nullptr, freeGroup); ASSERT_NE(nullptr, freeGroup->CanonicalLink()); + auto link = model->GetLink("sphere_link"); + auto freeGroupLink = link->FindFreeGroup(); + ASSERT_NE(nullptr, freeGroupLink); + freeGroup->SetWorldPose( ignition::math::eigen3::convert( ignition::math::Pose3d(0, 0, 2, 0, 0, 0))); @@ -230,10 +235,6 @@ TEST_P(SimulationFeatures_TEST, FreeGroup) auto frameData = model->GetLink(0)->FrameDataRelativeToWorld(); EXPECT_EQ(ignition::math::Pose3d(0, 0, 2, 0, 0, 0), ignition::math::eigen3::convert(frameData.pose)); - EXPECT_EQ(ignition::math::Vector3d(0.5, 0, 0.1), - ignition::math::eigen3::convert(frameData.linearVelocity)); - EXPECT_EQ(ignition::math::Vector3d(0.1, 0.2, 0), - ignition::math::eigen3::convert(frameData.angularVelocity)); } }