Do not remove elements that contain user errors when detaching a subtree.

PiperOrigin-RevId: 657190780
Change-Id: I073a437013f80596a341f7b80bed991536efeb43

src/user/user_model.cc

@@ -250,15 +250,22 @@ mjCModel& mjCModel::operator+=(const mjCModel& other) {
 
 
 template <class T>
-void mjCModel::RemoveFromList(std::vector<T*>& list) {
+void mjCModel::RemoveFromList(std::vector<T*>& list, const mjCModel& other) {
   int nlist = (int)list.size();
   int removed = 0;
   for (int i = 0; i < nlist; i++) {
     T* element = list[i];
     element->id -= removed;
+    try {
+      // check if the element contains an error
+      element->NameSpace(&other);
+      element->CopyFromSpec();
+      element->ResolveReferences(&other);
+    } catch (mjCError err) {
+      continue;
+    }
     try {
       // check if the element references something that was removed
-      // TODO: do not remove elements that contain user errors
       element->NameSpace(this);
       element->CopyFromSpec();
       element->ResolveReferences(this);
@@ -296,12 +303,12 @@ mjCModel& mjCModel::operator-=(const mjCBody& subtree) {
   ProcessLists(/*checkrepeat=*/false);
 
   // check if we have to remove anything else
-  RemoveFromList(pairs_);
+  RemoveFromList(pairs_, oldmodel);
-  RemoveFromList(excludes_);
+  RemoveFromList(excludes_, oldmodel);
-  RemoveFromList(tendons_);
+  RemoveFromList(tendons_, oldmodel);
-  RemoveFromList(equalities_);
+  RemoveFromList(equalities_, oldmodel);
-  RemoveFromList(actuators_);
+  RemoveFromList(actuators_, oldmodel);
-  RemoveFromList(sensors_);
+  RemoveFromList(sensors_, oldmodel);
 
   // restore to the original state
   if (!compiled) {

src/user/user_model.h

@@ -345,7 +345,7 @@ class mjCModel : public mjCModel_, private mjSpec {
                                    const std::vector<T*>& sources);
 
   // delete from list the elements that cause an error
-  template <class T> void RemoveFromList(std::vector<T*>& list);
+  template <class T> void RemoveFromList(std::vector<T*>& list, const mjCModel& other);
 
   // create mjCBase lists from children lists
   void CreateObjectLists();