Copy joint state to the modified mjSpec when detaching bodies.

Fixes #2085.

PiperOrigin-RevId: 678351210
Change-Id: I2a546877dd50abc7805099f8d5c40807ffe780e6

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
+  // update global lists
-  if (compiled) {
+  ResetTreeLists();
-    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;

test/user/user_api_test.cc

@@ -538,7 +538,10 @@ static constexpr char xml_child[] = R"(
           </body>
         </frame>
       </frame>
-      <body name="ignore"/>
+      <body name="ignore">
+        <geom size=".1"/>
+        <joint type="slide"/>
+      </body>
       <frame name="frame" pos=".1 0 0" euler="0 90 0"/>
     </worldbody>
 
@@ -563,8 +566,8 @@ static constexpr char xml_child[] = R"(
     </contact>
 
     <keyframe>
-      <key name="two" time="2" qpos="2" act="2 2" ctrl="2 2"/>
+      <key name="two" time="2" qpos="2 22" act="2 2" ctrl="2 2"/>
-      <key name="three" time="3" qpos="3" act="3 3" ctrl="3 3"/>
+      <key name="three" time="3" qpos="3 33" act="3 3" ctrl="3 3"/>
     </keyframe>
   </mujoco>)";
 
@@ -594,7 +597,10 @@ TEST_F(MujocoTest, AttachSame) {
         <body name="targetbody"/>
         <body/>
       </body>
-      <body name="ignore"/>
+      <body name="ignore">
+        <geom size=".1"/>
+        <joint type="slide"/>
+      </body>
       <frame name="frame" pos=".1 0 0" euler="0 90 0">
         <body name="attached-body-1">
           <joint type="hinge" name="attached-hinge-1"/>
@@ -634,10 +640,10 @@ TEST_F(MujocoTest, AttachSame) {
     </contact>
 
     <keyframe>
-      <key name="two" time="2" qpos="2 0" act="2 2 0 0" ctrl="2 2 0 0"/>
+      <key name="two" time="2" qpos="2 22 0" act="2 2 0 0" ctrl="2 2 0 0"/>
-      <key name="three" time="3" qpos="3 0" act="3 3 0 0" ctrl="3 3 0 0"/>
+      <key name="three" time="3" qpos="3 33 0" act="3 3 0 0" ctrl="3 3 0 0"/>
-      <key name="attached-two-1" time="2" qpos="0 2" act="0 0 2 2" ctrl="0 0 2 2"/>
+      <key name="attached-two-1" time="2" qpos="0 22 2" act="0 0 2 2" ctrl="0 0 2 2"/>
-      <key name="attached-three-1" time="3" qpos="0 3" act="0 0 3 3" ctrl="0 0 3 3"/>
+      <key name="attached-three-1" time="3" qpos="0 33 3" act="0 0 3 3" ctrl="0 0 3 3"/>
     </keyframe>
   </mujoco>)";
 
@@ -942,7 +948,10 @@ void TestDetachBody(bool compile) {
         <frame name="cframe">
         </frame>
       </frame>
-      <body name="ignore"/>
+      <body name="ignore">
+        <geom size=".1"/>
+        <joint type="slide"/>
+      </body>
       <frame name="frame" pos=".1 0 0" euler="0 90 0"/>
     </worldbody>
 
@@ -951,8 +960,8 @@ void TestDetachBody(bool compile) {
     </sensor>
 
     <keyframe>
-      <key name="two" time="2"/>
+      <key name="two" time="2" qpos="22"/>
-      <key name="three" time="3"/>
+      <key name="three" time="3" qpos="33"/>
     </keyframe>
   </mujoco>)";