chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Associate children of deleted body to the newly created frame in mjs_bodyToFrame.

Browse Source 
Also associate all children that need a parent body to the parent of the deleted body.

This is a bug that was causing the former children of the body that gets transformed to a frame to not be children of the new frame.

PiperOrigin-RevId: 702702853
Change-Id: I36d48f0446bc87c665b18d34e0b0609d82c70f51
This commit is contained in:
Alessio Quaglino
2024-12-04 06:29:41 -08:00
committed by Copybara-Service
parent 0ba717a653
commit afc86ac1b3
3 changed files with 34 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_objects.cc
+19 -20
View File
@@ -134,6 +134,18 @@ PNGImage PNGImage::Load(const mjCBase* obj, mjResource* resource,
return image;
}
// associate all child list elements with a frame and copy them to parent list, clear child list
template <typename T>
void MapFrame(std::vector<T*>& parent, std::vector<T*>& child,
mjCFrame* frame, mjCBody* parent_body) {
std::for_each(child.begin(), child.end(), [frame, parent_body](T* element) {
element->SetFrame(frame);
element->SetParent(parent_body);
});
parent.insert(parent.end(), child.begin(), child.end());
child.clear();
}
} // namespace
@@ -1195,20 +1207,13 @@ mjCFrame* mjCBody::ToFrame() {
mjCFrame* newframe = parent->AddFrame(frame);
mjuu_copyvec(newframe->spec.pos, spec.pos, 3);
mjuu_copyvec(newframe->spec.quat, spec.quat, 4);
parent->bodies.insert(parent->bodies.end(), bodies.begin(), bodies.end());
parent->geoms.insert(parent->geoms.end(), geoms.begin(), geoms.end());
parent->joints.insert(parent->joints.end(), joints.begin(), joints.end());
parent->sites.insert(parent->sites.end(), sites.begin(), sites.end());
parent->cameras.insert(parent->cameras.end(), cameras.begin(), cameras.end());
parent->lights.insert(parent->lights.end(), lights.begin(), lights.end());
parent->frames.insert(parent->frames.end(), frames.begin(), frames.end());
bodies.clear();
geoms.clear();
joints.clear();
sites.clear();
cameras.clear();
lights.clear();
frames.clear();
MapFrame(parent->bodies, bodies, newframe, parent);
MapFrame(parent->geoms, geoms, newframe, parent);
MapFrame(parent->joints, joints, newframe, parent);
MapFrame(parent->sites, sites, newframe, parent);
MapFrame(parent->cameras, cameras, newframe, parent);
MapFrame(parent->lights, lights, newframe, parent);
MapFrame(parent->frames, frames, newframe, parent);
parent->bodies.erase(
std::remove_if(parent->bodies.begin(), parent->bodies.end(),
[this](mjCBody* body) { return body == this; }),
@@ -1892,12 +1897,6 @@ bool mjCFrame::IsAncestor(const mjCFrame* child) const {
void mjCFrame::SetParent(mjCBody* _body) {
body = _body;
}
void mjCFrame::PointToLocal() {
spec.element = static_cast<mjsElement*>(this);
spec.name = &name;