add mjs_detachDefault to delete a default class from mjSpec

PiperOrigin-RevId: 738452369
Change-Id: I5a53a6dc8f89e1e8136aeefc09d7d9b7b7204e1f

src/engine/engine_io.c

@@ -2137,6 +2137,7 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) {
 //   -2: invalid objtype
 static int numObjects(const mjModel* m, mjtObj objtype) {
   switch (objtype) {
+  case mjOBJ_DEFAULT:
   case mjOBJ_FRAME:
   case mjOBJ_UNKNOWN:
     return -1;

src/user/user_api.cc

@@ -227,7 +227,7 @@ const char* mjs_getError(mjSpec* s) {
 
 
 
-// Detach body from mjSpec, return 0 if success.
+// detach body from mjSpec, return 0 on success
 int mjs_detachBody(mjSpec* s, mjsBody* b) {
   mjCModel* model = static_cast<mjCModel*>(s->element);
   mjCBody* body = static_cast<mjCBody*>(b->element);
@@ -241,7 +241,22 @@ int mjs_detachBody(mjSpec* s, mjsBody* b) {
   return 0;
 }
 
-
+// detach default from mjSpec, return 0 on success
+int mjs_detachDefault(mjSpec* s, mjsDefault* def) {
+  mjCModel* modelC = static_cast<mjCModel*>(s->element);
+  if (!def) {
+    modelC->SetError(mjCError(0, "Cannot detach, default is null"));
+    return -1;
+  }
+  mjCDef* defC = static_cast<mjCDef*>(def->element);
+  try {
+    *modelC -= *defC;
+  } catch (mjCError& e) {
+    modelC->SetError(e);
+    return -1;
+  }
+  return 0;
+}
 
 // check if model has warnings
 int mjs_isWarning(mjSpec* s) {
@@ -292,15 +307,19 @@ int mjs_setDeepCopy(mjSpec* s, int deepcopy) {
 
 
 
-// delete object, return 0 if success
+// delete object, return 0 on success
 int mjs_delete(mjsElement* element) {
-  mjCBase* object = static_cast<mjCBase*>(element);
+  mjCModel* model;
+  if (element->elemtype == mjOBJ_DEFAULT)
+    model = static_cast<mjCDef*>(element)->model;
+  else
+    model = static_cast<mjCBase*>(element)->model;
   try {
     // it will call the appropriate destructor since ~mjCBase is virtual
-    object->model->DeleteElement(element);
+    model->DeleteElement(element);
     return 0;
   } catch (mjCError& e) {
-    object->model->SetError(e);
+    model->SetError(e);
     return -1;
   }
 }
@@ -625,7 +644,7 @@ mjsDefault* mjs_getDefault(mjsElement* element) {
 
 
 // Find default with given name in model.
-const mjsDefault* mjs_findDefault(mjSpec* s, const char* classname) {
+mjsDefault* mjs_findDefault(mjSpec* s, const char* classname) {
   mjCModel* modelC = static_cast<mjCModel*>(s->element);
   mjCDef* cdef = modelC->FindDefault(classname);
   if (!cdef) {

src/user/user_api.h

@@ -88,6 +88,8 @@ MJAPI mjsFrame* mjs_attachFrameToSite(mjsSite* parent, const mjsFrame* child,
 // Detach body from mjSpec, remove all references and delete the body, return 0 on success.
 MJAPI int mjs_detachBody(mjSpec* s, mjsBody* b);
 
+// Detach default from mjSpec, remove all references and delete the default, return 0 on success.
+MJAPI int mjs_detachDefault(mjSpec* s, mjsDefault* d);
 
 //---------------------------------- Add tree elements ---------------------------------------------
 
@@ -221,7 +223,7 @@ MJAPI mjsFrame* mjs_findFrame(mjSpec* s, const char* name);
 MJAPI mjsDefault* mjs_getDefault(mjsElement* element);
 
 // Find default in model by class name.
-MJAPI const mjsDefault* mjs_findDefault(mjSpec* s, const char* classname);
+MJAPI mjsDefault* mjs_findDefault(mjSpec* s, const char* classname);
 
 // Get global default from model.
 MJAPI mjsDefault* mjs_getSpecDefault(mjSpec* s);

src/user/user_model.cc

@@ -24,6 +24,7 @@
 #include <cstdlib>
 #include <cstring>
 #include <exception>
+#include <functional>
 #include <mutex>
 #include <string>
 #include <string_view>
@@ -144,7 +145,7 @@ mjCModel::mjCModel() {
   Clear();
 
   //------------------------ master default set
-  defaults_.push_back(new mjCDef);
+  defaults_.push_back(new mjCDef(this));
   defaults_.back()->name = "main";
 
   // point to model from spec
@@ -564,6 +565,7 @@ mjCModel& mjCModel::operator-=(const mjCBody& subtree) {
 mjCModel_& mjCModel::operator+=(mjCDef& subtree) {
   defaults_.push_back(&subtree);
   def_map[subtree.name] = &subtree;
+  subtree.model = this;
 
   // set parent to the main default if this is not the only default in the model
   if (!subtree.parent && &subtree != defaults_[0]) {
@@ -579,6 +581,58 @@ mjCModel_& mjCModel::operator+=(mjCDef& subtree) {
 
 
 
+// remove default class from array
+mjCModel& mjCModel::operator-=(const mjCDef& subtree) {
+
+  // check we aren't trying to remove the 'main' default
+  if (subtree.id == 0) {
+    throw mjCError(0, "cannot remove the global default ('main')");
+  }
+
+  // remove this default from parent's child list
+  mjCDef* parent = subtree.parent;
+  if (parent) {
+    for (int i = 0; i < parent->child.size(); ++i) {
+      if (parent->child[i] == &subtree) {
+        parent->child.erase(parent->child.begin() + i);
+        break;
+      }
+    }
+  }
+
+  // traverse tree to find all descendants starting from subtree.id
+  std::vector<int> default_ids_to_remove;
+  std::vector<int> stack;
+  stack.push_back(subtree.id);
+  while (!stack.empty()) {
+    int id = stack.back();
+    stack.pop_back();
+    default_ids_to_remove.push_back(id);
+    for (int i=0; i<defaults_[id]->child.size(); i++) {
+      stack.push_back(defaults_[id]->child[i]->id);
+    }
+  }
+
+  // remove from the tree
+  std::sort(default_ids_to_remove.begin(),
+            default_ids_to_remove.end(),
+            std::greater<int>());
+
+  for (int id : default_ids_to_remove) {
+    delete defaults_[id];
+    defaults_.erase(defaults_.begin() + id);
+  }
+
+  // reset default ids
+  for (int i = 0; i < defaults_.size(); ++i) {
+    defaults_[i]->id = i;
+  }
+
+  return *this;
+}
+
+
+
 template <class T>
 void deletefromlist(std::vector<T*>* list, mjsElement* element) {
   if (!list) {
@@ -609,6 +663,10 @@ void mjCModel::DeleteElement(mjsElement* el) {
       throw mjCError(nullptr, "bodies cannot be deleted, use detach instead");
       break;
 
+    case mjOBJ_DEFAULT:
+      throw mjCError(nullptr, "defaults cannot be deleted, use detach instead");
+      break;
+
     case mjOBJ_GEOM:
     {
       mjCGeom* geom = static_cast<mjCGeom*>(el);
@@ -1180,7 +1238,7 @@ mjCDef* mjCModel::AddDefault(string name, mjCDef* parent) {
   }
 
   // create new object
-  mjCDef* def = new mjCDef;
+  mjCDef* def = new mjCDef(parent->model);
   defaults_.push_back(def);
   def->id = thisid;
 

src/user/user_model.h

@@ -183,10 +183,11 @@ class mjCModel : public mjCModel_, private mjSpec {
   void CopyFromSpec();  // copy spec to private attributes
   void PointToLocal();
 
-  mjCModel& operator=(const mjCModel& other);  // copy other into this, if they are not the same
-  mjCModel& operator+=(const mjCModel& other);  // add other into this, even if they are the same
-  mjCModel& operator-=(const mjCBody& subtree);  // remove subtree and all references from model
-  mjCModel_& operator+=(mjCDef& subtree);  // add default tree to this model
+  mjCModel& operator=(const mjCModel& other);     // copy other into this, if they are not the same
+  mjCModel& operator+=(const mjCModel& other);    // add other into this, even if they are the same
+  mjCModel& operator-=(const mjCBody& subtree);   // remove subtree and all references from model
+  mjCModel_& operator+=(mjCDef& subtree);         // add default tree to this model
+  mjCModel& operator-=(const mjCDef& subtree);    // remove default tree from this model
 
   mjSpec spec;
 
@@ -225,6 +226,9 @@ class mjCModel : public mjCModel_, private mjSpec {
   // delete object from the corresponding list
   void DeleteElement(mjsElement* el);
 
+  // delete default and all descendants
+  void RemoveDefault(mjCDef* def);
+
   // detach subtree from model
   void Detach(mjCBody* subtree);
 

src/user/user_objects.cc

@@ -536,12 +536,16 @@ int mjCBoundingVolumeHierarchy::MakeBVH(
 
 //------------------------- class mjCDef implementation --------------------------------------------
 
+
+
 // constructor
 mjCDef::mjCDef() {
   name.clear();
   id = 0;
   parent = nullptr;
+  model = 0;
   child.clear();
+  elemtype = mjOBJ_DEFAULT;
   mjs_defaultJoint(&joint_.spec);
   mjs_defaultGeom(&geom_.spec);
   mjs_defaultSite(&site_.spec);
@@ -561,6 +565,13 @@ mjCDef::mjCDef() {
 
 
 
+// constructor with model
+mjCDef::mjCDef(mjCModel* _model) : mjCDef() {
+  model = _model;
+}
+
+
+
 // copy constructor
 mjCDef::mjCDef(const mjCDef& other) {
   *this = other;

src/user/user_objects.h

@@ -1866,6 +1866,7 @@ class mjCDef : public mjsElement {
 
  public:
   mjCDef();
+  mjCDef(mjCModel*);
   mjCDef(const mjCDef& other);
   mjCDef& operator=(const mjCDef& other);
   mjCDef& operator+=(const mjCDef& other);
@@ -1898,6 +1899,7 @@ class mjCDef : public mjsElement {
   std::vector<mjCDef*> child;     // child classes
 
   mjsDefault spec;
+  mjCModel* model;                // pointer to model that owns object
 
  private:
   mjCJoint joint_;