From ed0477f14ac2d6617f649d62df05a2ce1eb75060 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 24 Jul 2023 11:35:27 -0500 Subject: [PATCH] Handle deeply nested merged models when custom parsers are present Signed-off-by: Addisu Z. Taddese --- include/sdf/Model.hh | 18 +++++ src/Model.cc | 189 ++++++++++++++++++++++++++++++++++++++++++- src/Utils.cc | 7 ++ src/World.cc | 37 ++++++++- src/parser.cc | 20 +++++ 5 files changed, 268 insertions(+), 3 deletions(-) diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index bb747d7be..dbece5917 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -529,6 +529,24 @@ namespace sdf private: const std::vector, sdf::InterfaceModelConstPtr>> &MergedInterfaceModels() const; + /// \brief Get whether the model was merge-included and needs to be + /// processed to carry out the merge. + /// \return True if the model was merge-included. + private: bool IsMerged() const; + + /// \brief Prepare the model to be merged into the parent model or world. + /// As part of the perparation, this will create the proxy frame that would + /// be need to be added to the parent object. + /// \param[out] _errors A list of errors encountered during the operation. + /// \param[in] _parentOfProxyFrame Name of parent of the proxy frame that + /// will be created. This can only be "__model__" or "world". + /// \return The proxy frame for the merged model that will need to be added + /// to the parent object. + /// \note This is a destructive call. After this call, the model will be in + /// an invalid state unless it is merged into the parent object. + private: sdf::Frame PrepareForMerge(sdf::Errors &_errors, + const std::string &_parentOfProxyFrame); + /// \brief Allow Root::Load, World::SetPoseRelativeToGraph, or /// World::SetFrameAttachedToGraph to call SetPoseRelativeToGraph and /// SetFrameAttachedToGraph diff --git a/src/Model.cc b/src/Model.cc index bbda7aab3..a22bf5466 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -127,6 +127,10 @@ class sdf::Model::Implementation /// ToElement() function to output only the plugin specified in the /// tag when the ToElementUseIncludeTag policy is true.. public: std::vector includePlugins; + + /// \brief Whether the model was merge-included and needs to be processed to + /// carry out the merge. + public: bool isMerged{false}; }; ///////////////////////////////////////////////// @@ -184,6 +188,11 @@ Errors Model::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) } } + // Note: this attribute is not defined in the spec. It is used internally for + // implementing merge-includes when custom parsers are present. + this->dataPtr->isMerged = + _sdf->Get("merge", this->dataPtr->isMerged).first; + this->dataPtr->placementFrameName = _sdf->Get("placement_frame", this->dataPtr->placementFrameName).first; @@ -293,8 +302,29 @@ Errors Model::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) { continue; } - implicitFrameNames.insert(model.Name()); - this->dataPtr->models.push_back(std::move(model)); + if (model.IsMerged()) + { + sdf::Frame proxyFrame = model.PrepareForMerge(errors, "__model__"); + this->AddFrame(proxyFrame); + + auto moveElements = [](const auto &_src, auto &_dest) + { + std::move(_src.begin(), _src.end(), std::back_inserter(_dest)); + }; + moveElements(model.dataPtr->links, this->dataPtr->links); + moveElements(model.dataPtr->frames, this->dataPtr->frames); + moveElements(model.dataPtr->joints, this->dataPtr->joints); + moveElements(model.dataPtr->models, this->dataPtr->models); + moveElements(model.dataPtr->interfaceModels, + this->dataPtr->interfaceModels); + moveElements(model.dataPtr->mergedInterfaceModels, + this->dataPtr->mergedInterfaceModels); + } + else + { + implicitFrameNames.insert(model.Name()); + this->dataPtr->models.push_back(std::move(model)); + } } else if (elementName == "link") { @@ -1192,3 +1222,158 @@ void Model::AddPlugin(const Plugin &_plugin) { this->dataPtr->plugins.push_back(_plugin); } + +///////////////////////////////////////////////// +bool Model::IsMerged() const +{ + return this->dataPtr->isMerged; +} + +///////////////////////////////////////////////// +sdf::Frame Model::PrepareForMerge(sdf::Errors &_errors, + const std::string &_parentOfProxyFrame) +{ + // Build the pose graph of the model so we can adjust the pose of the proxy + // frame to take the placement frame into account. + auto poseGraph = std::make_shared(); + sdf::ScopedGraph scopedPoseGraph(poseGraph); + sdf::Errors poseGraphErrors = + sdf::buildPoseRelativeToGraph(scopedPoseGraph, this); + _errors.insert(_errors.end(), poseGraphErrors.begin(), poseGraphErrors.end()); + sdf::Errors poseValidationErrors = + validatePoseRelativeToGraph(scopedPoseGraph); + _errors.insert(_errors.end(), poseValidationErrors.begin(), + poseValidationErrors.end()); + + auto frameGraph = std::make_shared(); + sdf::ScopedGraph scopedFrameGraph(frameGraph); + sdf::Errors frameGraphErrors = + sdf::buildFrameAttachedToGraph(scopedFrameGraph, this); + _errors.insert(_errors.end(), frameGraphErrors.begin(), + frameGraphErrors.end()); + sdf::Errors frameValidationErrors = + validateFrameAttachedToGraph(scopedFrameGraph); + _errors.insert(_errors.end(), frameValidationErrors.begin(), + frameValidationErrors.end()); + + this->SetPoseRelativeToGraph(scopedPoseGraph); + this->SetFrameAttachedToGraph(scopedFrameGraph); + + const std::string proxyModelFrameName = + computeMergedModelProxyFrameName(this->Name()); + + sdf::Frame proxyFrame; + proxyFrame.SetName(proxyModelFrameName); + proxyFrame.SetAttachedTo(this->CanonicalLinkAndRelativeName().second); + gz::math::Pose3d modelPose = this->RawPose(); + if (!this->PlacementFrameName().empty()) + { + // Build the pose graph of the model so we can adjust the pose of the proxy + // frame to take the placement frame into account. + if (poseGraphErrors.empty()) + { + // M - model frame (__model__) + // R - The `relative_to` frame of the placement frame's //pose element. + // See resolveModelPoseWithPlacementFrame in FrameSemantics.cc for + // notation and documentation + // Note, when the frame graph is built for a model with placement frame, + // it is built with an pose offset to take into account the placement + // frame. Therefore, we only need to resolve the pose of the model + // relative to `R` here. We could manually compute it as + // X_RM = X_RL * X_LM; where `L` is the link specified in the placement + // frame attribute. + gz::math::Pose3d X_RM = this->RawPose(); + sdf::Errors resolveErrors = this->SemanticPose().Resolve(X_RM); + _errors.insert(_errors.end(), resolveErrors.begin(), resolveErrors.end()); + modelPose = X_RM; + } + } + + proxyFrame.SetRawPose(modelPose); + proxyFrame.SetPoseRelativeTo(this->PoseRelativeTo().empty() + ? _parentOfProxyFrame + : this->PoseRelativeTo()); + + auto isEmptyOrModelFrame = [](const std::string &_attr) + { + return _attr.empty() || _attr == "__model__"; + }; + + // Merge links, frames, joints, and nested models. + for (auto &link : this->dataPtr->links) + { + if (isEmptyOrModelFrame(link.PoseRelativeTo())) + { + link.SetPoseRelativeTo(proxyModelFrameName); + } + } + + for (auto &frame : this->dataPtr->frames) + { + if (isEmptyOrModelFrame(frame.AttachedTo())) + { + frame.SetAttachedTo(proxyModelFrameName); + } + if (frame.PoseRelativeTo() == "__model__") + { + frame.SetPoseRelativeTo(proxyModelFrameName); + } + } + + for (auto &joint : this->dataPtr->joints) + { + if (joint.PoseRelativeTo() == "__model__") + { + joint.SetPoseRelativeTo(proxyModelFrameName); + } + if (joint.ParentName() == "__model__") + { + joint.SetParentName(proxyModelFrameName); + } + if (joint.ChildName() == "__model__") + { + joint.SetChildName(proxyModelFrameName); + } + for (unsigned int ai : {0, 1}) + { + const sdf::JointAxis *axis = joint.Axis(ai); + if (axis && axis->XyzExpressedIn() == "__model__") + { + sdf::JointAxis axisCopy = *axis; + axisCopy.SetXyzExpressedIn(proxyModelFrameName); + joint.SetAxis(ai, axisCopy); + } + } + } + + for (auto &nestedModel : this->dataPtr->models) + { + if (isEmptyOrModelFrame(nestedModel.PoseRelativeTo())) + { + nestedModel.SetPoseRelativeTo(proxyModelFrameName); + } + } + // Note: Since Model::Load is called recursively, all merge-include nested + // models would already have been merged by this point, so there is no need + // to call PrepareForMerge recursively here. + + for (auto &ifaceModel : this->dataPtr->interfaceModels) + { + if (isEmptyOrModelFrame( + ifaceModel.first->IncludePoseRelativeTo().value_or(""))) + { + ifaceModel.first->SetIncludePoseRelativeTo(proxyModelFrameName); + } + } + + for (auto &ifaceModel : this->dataPtr->mergedInterfaceModels) + { + if (isEmptyOrModelFrame( + ifaceModel.first->IncludePoseRelativeTo().value_or(""))) + { + ifaceModel.first->SetIncludePoseRelativeTo(proxyModelFrameName); + } + } + + return proxyFrame; +} diff --git a/src/Utils.cc b/src/Utils.cc index b480f2c74..5da72a493 100644 --- a/src/Utils.cc +++ b/src/Utils.cc @@ -191,6 +191,13 @@ static std::optional computeAbsoluteName( { if (parent->HasAttribute("name")) { + // Ignore this parent model if it's a merged model, since the child will + // be merged into the grand parent model/world. + if (parent->GetName() == "model" && + parent->Get("merge", false).first) + { + continue; + } names.push_back(parent->GetAttribute("name")->GetAsString()); } else diff --git a/src/World.cc b/src/World.cc index 6d81ce0af..504835147 100644 --- a/src/World.cc +++ b/src/World.cc @@ -264,7 +264,42 @@ Errors World::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) continue; } implicitFrameNames.insert(model.Name()); - this->dataPtr->models.push_back(std::move(model)); + if (model.IsMerged()) + { + sdf::Frame proxyFrame = model.PrepareForMerge(errors, "world"); + this->AddFrame(proxyFrame); + + for (uint64_t fi = 0; fi < model.FrameCount(); ++fi) + { + this->dataPtr->frames.push_back(std::move(*model.FrameByIndex(fi))); + } + + for (uint64_t ji = 0; ji < model.JointCount(); ++ji) + { + this->dataPtr->joints.push_back(std::move(*model.JointByIndex(ji))); + } + + for (uint64_t mi = 0; mi < model.ModelCount(); ++mi) + { + this->dataPtr->models.push_back(std::move(*model.ModelByIndex(mi))); + } + + for (uint64_t imi = 0; imi < model.InterfaceModelCount(); ++imi) + { + InterfaceModelConstPtr ifaceModel = model.InterfaceModelByIndex(imi); + NestedInclude nestedInclude = + *model.InterfaceModelNestedIncludeByIndex(imi); + this->dataPtr->interfaceModels.emplace_back(std::move(nestedInclude), + ifaceModel); + } + + // TODO(azeey) Support Merge-included interface models when `World` + // supports them. + } + else + { + this->dataPtr->models.push_back(std::move(model)); + } } else if (elementName == "joint") { diff --git a/src/parser.cc b/src/parser.cc index 310313ead..0e3b2c9f4 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -232,6 +232,26 @@ static void insertIncludedElement(sdf::SDFPtr _includeSDF, } // Validate included model's frame semantics + if (!_config.CustomModelParsers().empty()) + { + // Since we have custom parsers, we can't create a throwaway sdf::Root + // object to validate the merge-included model. This is because calling + // `sdf::Root::Load` here would call the custom parsers if this model + // contains a nested model that is custom parsed. But the custom parsers + // will be called again later when we construct the final `sdf::Root` + // object. We also can't do the merge here since we'd be doing so without + // validating the model. + // We could forego validating the model and just merge all it's children to + // the parent element, but we wouldn't be able to handle placement frames + // since that requires building a frame graph for the model. + // So instead we add a hidden flag here to tell `sdf::Model` or `sdf::World` + // that this model is meant to be merged. + firstElem->AddAttribute("merge", "bool", "false", false, + "Indicates whether this is a merge included model"); + firstElem->GetAttribute("merge")->Set(true); + _parent->InsertElement(firstElem, true); + return; + } // We create a throwaway sdf::Root object in order to validate the // included entity. sdf::Root includedRoot;