From f0d52813b80a3602a1f1642eaa750210ddb88ec0 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 24 Sep 2024 12:25:37 -0700 Subject: [PATCH] Copy joint state to the modified mjSpec when detaching bodies. Fixes #2085. PiperOrigin-RevId: 678351210 Change-Id: I2a546877dd50abc7805099f8d5c40807ffe780e6 --- src/user/user_model.cc | 34 +++++++++++++++++++--------------- test/user/user_api_test.cc | 31 ++++++++++++++++++++----------- 2 files changed, 39 insertions(+), 26 deletions(-) diff --git a/src/user/user_model.cc b/src/user/user_model.cc index b189677b..ff9d6888 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -402,21 +402,28 @@ mjCModel& mjCModel::operator-=(const mjCBody& subtree) { oldmodel.ProcessLists(/*checkrepeat=*/false); } + // create global lists in this model if not compiled + if (!IsCompiled()) { + MakeLists(bodies_[0]); + ProcessLists(/*checkrepeat=*/false); + } + + // TODO: carry over pending keyframes + key_pending_.clear(); + StoreKeyframes(); + + // all keyframes are now pending and they will be resized + DeleteAll(keys_); + // remove body from tree mjCBody* world = bodies_[0]; *world -= subtree; - // create global lists - if (compiled) { - ResetTreeLists(); - } + // update global lists + ResetTreeLists(); MakeLists(world); ProcessLists(/*checkrepeat=*/false); - // store keyframes in the old model - oldmodel.key_pending_.clear(); - oldmodel.StoreKeyframes(); - // check if we have to remove anything else RemoveFromList(pairs_, oldmodel); RemoveFromList(excludes_, oldmodel); @@ -425,12 +432,6 @@ mjCModel& mjCModel::operator-=(const mjCBody& subtree) { RemoveFromList(actuators_, oldmodel); RemoveFromList(sensors_, oldmodel); - // move all keyframes to pending so that they will be resized - DeleteAll(keys_); - for (const auto& key : oldmodel.key_pending_) { - key_pending_.push_back(key); - } - // restore to the original state if (!compiled) { ResetTreeLists(); @@ -3091,7 +3092,10 @@ void mjCModel::StoreKeyframes() { resetlists = true; } - SaveDofOffsets(); + // do not change the offset computed during compilation in case the user wants to recompile + if (!compiled) { + SaveDofOffsets(); + } for (auto& key : keys_) { mjKeyInfo info; diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 39811a30..548c22ca 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -538,7 +538,10 @@ static constexpr char xml_child[] = R"( - + + + + @@ -563,8 +566,8 @@ static constexpr char xml_child[] = R"( - - + + )"; @@ -594,7 +597,10 @@ TEST_F(MujocoTest, AttachSame) { - + + + + @@ -634,10 +640,10 @@ TEST_F(MujocoTest, AttachSame) { - - - - + + + + )"; @@ -942,7 +948,10 @@ void TestDetachBody(bool compile) { - + + + + @@ -951,8 +960,8 @@ void TestDetachBody(bool compile) { - - + + )";