From 58a49bad96244ddbb69852497bd6ac2cf053a081 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Sun, 29 Sep 2024 11:56:50 -0700 Subject: [PATCH] Add nested keyframes attachment test. This is a no-op change for keyframes but it shows an example of behavior. Also, fixed two bugs: - Missing namespace when attaching frames. - Incorrect body id and subtreedof when attaching a compiled spec. Fixes #2063. PiperOrigin-RevId: 680289467 Change-Id: I2ddaa7ad2226fa466ec3bbf74a7553d497bb4bb9 --- src/user/user_api.cc | 8 +++ src/user/user_objects.cc | 7 ++- test/user/user_api_test.cc | 102 +++++++++++++++++++++++++++++++++++++ 3 files changed, 116 insertions(+), 1 deletion(-) diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 739cc863..be7605d6 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -119,6 +119,10 @@ mjModel* mj_compile(mjSpec* s, const mjVFS* vfs) { // attach body to a frame of the parent mjsBody* mjs_attachBody(mjsFrame* parent, const mjsBody* child, const char* prefix, const char* suffix) { + if (!parent) { + mju_error("parent frame is null"); + return nullptr; + } mjCFrame* frame_parent = static_cast(parent->element); mjCBody* child_body = static_cast(child->element); try { @@ -137,6 +141,10 @@ mjsBody* mjs_attachBody(mjsFrame* parent, const mjsBody* child, // attach frame to a parent body mjsFrame* mjs_attachFrame(mjsBody* parent, const mjsFrame* child, const char* prefix, const char* suffix) { + if (!parent) { + mju_error("parent body is null"); + return nullptr; + } mjCBody* body_parent = static_cast(parent->element); mjCFrame* child_frame = static_cast(child->element); try { diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index a5da1d58..182d367e 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -802,7 +802,8 @@ mjCBody& mjCBody::operator=(const mjCBody& other) { sites.clear(); cameras.clear(); lights.clear(); - id = other.id; + id = -1; + subtreedofs = 0; // add elements to lists *this += other; @@ -1040,6 +1041,10 @@ void mjCBody::NameSpace_(const mjCModel* m, bool propagate) { for (auto& light : lights) { light->NameSpace(m); } + + for (auto& frame : frames) { + frame->NameSpace(m); + } } diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 0548a15d..6977e27a 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -1284,5 +1284,107 @@ TEST_F(MujocoTest, InitTexture) { mj_deleteSpec(spec); } +TEST_F(MujocoTest, AttachNestedKeyframe) { + static constexpr char parent_xml[] = R"( + + + + + + + + + )"; + + static constexpr char child_xml[] = R"( + + + + + + + + + + + + )"; + + static constexpr char gchild_xml[] = R"( + + + + + + + + + + + )"; + + static constexpr char expected_xml[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + )"; + + std::array er; + mjSpec* parent = mj_parseXMLString(parent_xml, 0, er.data(), er.size()); + EXPECT_THAT(parent, NotNull()) << er.data(); + mjSpec* child = mj_parseXMLString(child_xml, 0, er.data(), er.size()); + EXPECT_THAT(child, NotNull()) << er.data(); + mjSpec* gchild = mj_parseXMLString(gchild_xml, 0, er.data(), er.size()); + EXPECT_THAT(gchild, NotNull()) << er.data(); + + // attach gchild to child + mjs_attachBody(mjs_findFrame(child, "frame"), + mjs_findBody(gchild, "body"), "gchild-", ""); + + // compile required before further attachment + mjModel* m_child = mj_compile(child, 0); + + // attach child to parent + mjs_attachBody(mjs_findFrame(parent, "frame"), + mjs_findBody(child, "body"), "child-", ""); + + mjtNum tol = 0; + std::string field = ""; + mjModel* m_attached = mj_compile(parent, 0); + EXPECT_THAT(m_attached, NotNull()); + mjModel* m_expected = LoadModelFromString(expected_xml, er.data(), er.size()); + EXPECT_THAT(m_expected, NotNull()) << er.data(); + EXPECT_LE(CompareModel(m_attached, m_expected, field), tol) + << "Expected and attached models are different!\n" + << "Different field: " << field << '\n';; + + mj_deleteSpec(parent); + mj_deleteSpec(child); + mj_deleteSpec(gchild); + mj_deleteModel(m_expected); + mj_deleteModel(m_attached); + mj_deleteModel(m_child); +} + } // namespace } // namespace mujoco