Avoid duplicated code in mjCModel by adding ResetTreeLists().

PiperOrigin-RevId: 653575505
Change-Id: I61ee1462deae2a371cf6f36ca1cf3dbbd88d174c

src/user/user_model.cc

@@ -188,19 +188,26 @@ static void resetlist(std::vector<T*>& list) {
 
 
 
+void mjCModel::ResetTreeLists() {
+  mjCBody *world = bodies_[0];
+  resetlist(bodies_);
+  resetlist(joints_);
+  resetlist(geoms_);
+  resetlist(sites_);
+  resetlist(cameras_);
+  resetlist(lights_);
+  resetlist(frames_);
+  world->id = 0;
+  bodies_.push_back(world);
+}
+
+
+
 mjCModel& mjCModel::operator+=(const mjCModel& other) {
   // create global lists
   mjCBody *world = bodies_[0];
   if (compiled) {
-    resetlist(bodies_);
+    ResetTreeLists();
-    resetlist(joints_);
-    resetlist(geoms_);
-    resetlist(sites_);
-    resetlist(cameras_);
-    resetlist(lights_);
-    resetlist(frames_);
-    world->id = 0;
-    bodies_.push_back(world);
   }
   MakeLists(world);
   ProcessLists(/*checkrepeat=*/false);
@@ -233,15 +240,7 @@ mjCModel& mjCModel::operator+=(const mjCModel& other) {
 
   // restore to the original state
   if (!compiled) {
-    resetlist(bodies_);
+    ResetTreeLists();
-    resetlist(joints_);
-    resetlist(geoms_);
-    resetlist(sites_);
-    resetlist(cameras_);
-    resetlist(lights_);
-    resetlist(frames_);
-    world->id = 0;
-    bodies_.push_back(world);
   }
 
   PointToLocal();
@@ -291,15 +290,7 @@ mjCModel& mjCModel::operator-=(const mjCBody& subtree) {
 
   // create global lists
   if (compiled) {
-    resetlist(bodies_);
+    ResetTreeLists();
-    resetlist(joints_);
-    resetlist(geoms_);
-    resetlist(sites_);
-    resetlist(cameras_);
-    resetlist(lights_);
-    resetlist(frames_);
-    world->id = 0;
-    bodies_.push_back(world);
   }
   MakeLists(world);
   ProcessLists(/*checkrepeat=*/false);
@@ -314,15 +305,7 @@ mjCModel& mjCModel::operator-=(const mjCBody& subtree) {
 
   // restore to the original state
   if (!compiled) {
-    resetlist(bodies_);
+    ResetTreeLists();
-    resetlist(joints_);
-    resetlist(geoms_);
-    resetlist(sites_);
-    resetlist(cameras_);
-    resetlist(lights_);
-    resetlist(frames_);
-    world->id = 0;
-    bodies_.push_back(world);
   }
 
   PointToLocal();

src/user/user_model.h

@@ -353,6 +353,9 @@ class mjCModel : public mjCModel_, private mjSpec {
   // populate objects ids
   void ProcessLists(bool checkrepeat = true);
 
+  // reset lists of kinematic tree
+  void ResetTreeLists();
+
   mjListKeyMap ids;   // map from object names to ids
   mjCError errInfo;   // last error info
   bool plugin_owner;  // this class allocated the plugins