diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index e102b001..eb24e2b1 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -739,13 +739,15 @@ mjCBody& mjCBody::operator+=(const mjCFrame& other) { frames.back()->body = this; frames.back()->model = model; frames.back()->frame = other.frame; + int i = frames.size(); // map input frames to index in this->frames std::map fmap; for (auto frame : subtree->frames) { if (frame == static_cast(&other)) { fmap[frame] = frames.size() - 1; - break; + } else if (other.IsAncestor(frame)) { + fmap[frame] = i++; } } @@ -758,7 +760,7 @@ mjCBody& mjCBody::operator+=(const mjCFrame& other) { CopyList(lights, subtree->lights, fmap, &other); for (int i=0; ibodies.size(); i++) { - if (subtree->bodies[i]->frame != &other) { + if (!other.IsAncestor(subtree->bodies[i]->frame)) { continue; } bodies.push_back(new mjCBody(*subtree->bodies[i], model)); // triggers recursive call @@ -786,7 +788,7 @@ void mjCBody::CopyList(std::vector& dst, const std::vector& src, std::map& fmap, const mjCFrame* pframe) { int nsrc = (int)src.size(); for (int i=0; iframe != pframe) { + if (pframe && !pframe->IsAncestor(src[i]->frame)) { continue; // skip if the element is not inside pframe } dst.push_back(new T(*src[i])); @@ -1473,6 +1475,21 @@ mjCFrame& mjCFrame::operator+=(const mjCBody& other) { +// return true if child is descendent of this frame +bool mjCFrame::IsAncestor(const mjCFrame* child) const { + if (!child) { + return false; + } + + if (child == this) { + return true; + } + + return IsAncestor(child->frame); +} + + + void mjCFrame::SetParent(mjCBody* _body) { body = _body; } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index cf4ff177..f6e91a81 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -362,6 +362,8 @@ class mjCFrame : public mjCFrame_, private mjsFrame { mjCFrame& operator+=(const mjCBody& other); + bool IsAncestor(const mjCFrame* child) const; // true if child is contained in this frame + private: mjCFrame(mjCModel* = 0, mjCFrame* = 0); // constructor mjCFrame(const mjCFrame& other); // copy constructor diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 86988a5b..3e712310 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -200,13 +200,15 @@ static constexpr char xml_child[] = R"( - - - - - - - + + + + + + + + + @@ -437,13 +439,15 @@ TEST_F(MujocoTest, AttachFrame) { - - - - - - - + + + + + + + + +