mujoco

// Copyright 2021 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "xml/xml_native_writer.h"

#include <array>
#include <cstddef>
#include <cstdio>
#include <string>
#include <string_view>
#include <unordered_set>
#include <vector>

#include <mujoco/mjmodel.h>
#include <mujoco/mjplugin.h>
#include <mujoco/mjspec.h>
#include <mujoco/mujoco.h>
#include "engine/engine_io.h"
#include "engine/engine_plugin.h"
#include "engine/engine_support.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "user/user_model.h"
#include "user/user_objects.h"
#include "user/user_util.h"
#include "xml/xml_base.h"
#include "xml/xml_util.h"
#include "tinyxml2.h"

// typed attribute rows, generated from mjcf.schema; shared with the reader
#include "xml/generated/mjcf_read_table.inc"

namespace {

using mujoco::user::VectorToString;
using std::string;
using std::string_view;
using tinyxml2::XMLComment;
using tinyxml2::XMLDocument;
using tinyxml2::XMLElement;
using tinyxml2::XMLText;

}  // namespace


// custom XML indentation: 2 spaces rather than the default 4
class mj_XMLPrinter : public tinyxml2::XMLPrinter {
  using tinyxml2::XMLPrinter::XMLPrinter;

 public:
  void PrintSpace(int depth) {
    for (int i = 0; i < depth; ++i) { Write("  "); }
  }
};


// save XML file using custom 2-space indentation
static string WriteDoc(XMLDocument& doc, char* error, size_t error_sz) {
  doc.ClearError();
  mj_XMLPrinter stream(nullptr, /*compact=*/false);
  doc.Print(&stream);
  if (doc.ErrorID()) {
    mjCopyError(error, doc.ErrorStr(), error_sz);
    return "";
  }
  string str = string(stream.CStr());

  // top level sections
  std::array<string, 17> sections = {"<actuator",
                                     "<asset",
                                     "<compiler",
                                     "<contact>",
                                     "<custom",
                                     "<default>",
                                     "<deformable",
                                     "<equality",
                                     "<extension",
                                     "<keyframe",
                                     "<option",
                                     "<sensor",
                                     "<size",
                                     "<statistic",
                                     "<tendon",
                                     "<visual",
                                     "<worldbody"};

  // position of newline before first section
  size_t first_pos = string::npos;

  // insert newlines before section headers
  for (const string& section : sections) {
    std::size_t pos = 0;
    while ((pos = str.find(section, pos)) != string::npos) {
      // find newline before this section
      std::size_t line_pos = str.rfind('\n', pos);

      // save position of first section
      if (line_pos < first_pos) first_pos = line_pos;

      // insert another newline
      if (line_pos != string::npos) {
        str.insert(line_pos + 1, 1, '\n');
        pos++;  // account for inserted newline
      }

      // advance
      pos += section.length();
    }
  }

  // remove added newline before the first section
  if (first_pos != string::npos) { str.erase(first_pos, 1); }

  return str;
}


// insert end child with given name, return child
XMLElement* mjXWriter::InsertEnd(XMLElement* parent, const char* name) {
  XMLElement* result = parent->GetDocument()->NewElement(name);
  parent->InsertEndChild(result);

  return result;
}


//---------------------------------- class mjXWriter: one-element writers --------------------------

// write flex
void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* flex) {
  string  text;
  mjCFlex defflex;

  // common attributes
  WriteAttrTxt(elem, "name", flex->name);
  WriteAttrTable(elem,
                 static_cast<const mjsFlex*>(flex),
                 static_cast<const mjsFlex*>(&defflex),
                 kFlexAttrs,
                 kFlexAttrsN);
  if (flex->get_material() != defflex.get_material()) {
    WriteAttrTxt(elem, "material", flex->get_material());
  }
  WriteAttr(elem, "cellcount", 3, flex->spec.cellcount, defflex.spec.cellcount);
  if (flex->spec.order != defflex.spec.order) {
    string dof_str = "full";
    if (flex->spec.order == 1)
      dof_str = "trilinear";
    else if (flex->spec.order == 2)
      dof_str = "quadratic";
    WriteAttrTxt(elem, "dof", dof_str);
  }

  // data vectors
  if (!flex->get_vertbody().empty()) {
    text = VectorToString(flex->get_vertbody());
    WriteAttrTxt(elem, "body", text);
  }
  if (!flex->get_vert().empty()) {
    text = VectorToString(flex->get_vert());
    WriteAttrTxt(elem, "vertex", text);
  }
  if (!flex->get_elem().empty()) {
    text = VectorToString(flex->get_elem());
    WriteAttrTxt(elem, "element", text);
  }
  if (!flex->get_texcoord().empty()) {
    text = VectorToString(flex->get_texcoord());
    WriteAttrTxt(elem, "texcoord", text);
  }
  if (!flex->get_elemtexcoord().empty()) {
    text = VectorToString(flex->get_elemtexcoord());
    WriteAttrTxt(elem, "elemtexcoord", text);
  }
  if (!flex->get_nodebody().empty()) {
    text = VectorToString(flex->get_nodebody());
    WriteAttrTxt(elem, "node", text);
  }
  if (!flex->get_node().empty()) { WriteVector(elem, "nodecoord", flex->get_node()); }

  // contact subelement
  XMLElement* cont = InsertEnd(elem, "contact");
  WriteAttrTable(cont,
                 static_cast<const mjsFlex*>(flex),
                 static_cast<const mjsFlex*>(&defflex),
                 kFlexcomp_contactAttrs,
                 kFlexcomp_contactAttrsN);

  // remove contact is no attributes
  if (!cont->FirstAttribute()) { elem->DeleteChild(cont); }

  // elasticity subelement
  XMLElement* elastic = InsertEnd(elem, "elasticity");
  WriteAttrTable(elastic,
                 static_cast<const mjsFlex*>(flex),
                 static_cast<const mjsFlex*>(&defflex),
                 kElasticityAttrs,
                 kElasticityAttrsN);

  // edge subelement
  XMLElement* edge = InsertEnd(elem, "edge");
  WriteAttrTable(edge,
                 static_cast<const mjsFlex*>(flex),
                 static_cast<const mjsFlex*>(&defflex),
                 kFlex_edgeAttrs,
                 kFlex_edgeAttrsN);

  // remove edge if no attributes
  if (!edge->FirstAttribute()) { elem->DeleteChild(edge); }
}


// write mesh
void mjXWriter::OneMesh(XMLElement* elem, const mjCMesh* mesh, mjCDef* def) {
  string text;

  // regular
  if (!writingdefaults) {
    WriteAttrTxt(elem, "name", mesh->name);
    if (mesh->classname != "main") { WriteAttrTxt(elem, "class", mesh->classname); }
    WriteAttrTxt(elem, "content_type", mesh->ContentType());
    WriteAttrTxt(elem, "file", mesh->File());

    // write vertex data
    if (!mesh->UserVert().empty()) {
      text = VectorToString(mesh->UserVert());
      WriteAttrTxt(elem, "vertex", text);
    }

    // write normal data
    if (!mesh->UserNormal().empty()) {
      text = VectorToString(mesh->UserNormal());
      WriteAttrTxt(elem, "normal", text);
    }

    // write texcoord data
    if (!mesh->UserTexcoord().empty()) {
      text = VectorToString(mesh->UserTexcoord());
      WriteAttrTxt(elem, "texcoord", text);
    }

    // write face data
    if (!mesh->UserFace().empty()) {
      text = VectorToString(mesh->UserFace());
      WriteAttrTxt(elem, "face", text);
    }
  }

  // defaults and regular
  WriteAttrTable(elem,
                 static_cast<const mjsMesh*>(mesh),
                 &def->Mesh().spec,
                 kMeshAttrs,
                 kMeshAttrsN);
  if (mesh->Material() != def->Mesh().Material()) {
    WriteAttrTxt(elem, "material", mesh->Material());
  }
}


// write skin
void mjXWriter::OneSkin(XMLElement* elem, const mjCSkin* skin) {
  string  text;
  mjCSkin defskin;

  // write attributes
  WriteAttrTxt(elem, "name", skin->name);
  WriteAttrTxt(elem, "file", skin->File());
  WriteAttrTxt(elem, "material", skin->get_material());
  WriteAttrInt(elem, "group", skin->group, 0);
  WriteAttrTable(elem,
                 static_cast<const mjsSkin*>(skin),
                 static_cast<const mjsSkin*>(&defskin),
                 kSkinAttrs,
                 kSkinAttrsN);

  // write data if no file
  if (skin->File().empty()) {
    // mesh vert
    text = VectorToString(skin->get_vert());
    WriteAttrTxt(elem, "vertex", text);

    // mesh texcoord
    if (!skin->get_texcoord().empty()) {
      text = VectorToString(skin->get_texcoord());
      WriteAttrTxt(elem, "texcoord", text);
    }

    // mesh face
    text = VectorToString(skin->get_face());
    WriteAttrTxt(elem, "face", text);

    // bones
    for (size_t i = 0; i < skin->get_bodyname().size(); i++) {
      // make bone
      XMLElement* bone = InsertEnd(elem, "bone");

      // write attributes
      WriteAttrTxt(bone, "body", skin->get_bodyname()[i]);
      WriteAttr(bone, "bindpos", 3, skin->get_bindpos().data() + 3 * i);
      WriteAttr(bone, "bindquat", 4, skin->get_bindquat().data() + 4 * i);

      // write vertid
      text = VectorToString(skin->get_vertid()[i]);
      WriteAttrTxt(bone, "vertid", text);

      // write vertweight
      text = VectorToString(skin->get_vertweight()[i]);
      WriteAttrTxt(bone, "vertweight", text);
    }
  }
}


// write material
void mjXWriter::OneMaterial(XMLElement* elem, const mjCMaterial* material, mjCDef* def) {
  // regular
  if (!writingdefaults) {
    WriteAttrTxt(elem, "name", material->name);
    if (material->classname != "main") { WriteAttrTxt(elem, "class", material->classname); }
  }

  // defaults and regular
  // check if we have non-rgb textures
  bool has_non_rgb = false;
  for (int i = 1; i < mjNTEXROLE; i++) {
    if (!material->textures_[i].empty()) {
      if (i != mjTEXROLE_RGB) { has_non_rgb = true; }
    }
  }

  // if we have non-rgb textures, write them as layers
  if (has_non_rgb) {
    for (int i = 1; i < mjNTEXROLE; i++) {
      if (!material->textures_[i].empty()) {
        XMLElement* child_elem = InsertEnd(elem, "layer");
        WriteAttrTxt(child_elem, "texture", material->textures_[i]);
        WriteAttrTxt(child_elem, "role", FindValue(texrole_map, 9, i));
      }
    }
  } else {
    if (material->textures_[mjTEXROLE_RGB] != def->Material().textures_[mjTEXROLE_RGB]) {
      WriteAttrTxt(elem, "texture", material->get_texture(mjTEXROLE_RGB));
    }
  }

  WriteAttrTable(elem,
                 static_cast<const mjsMaterial*>(material),
                 &def->Material().spec,
                 kMaterialAttrs,
                 kMaterialAttrsN);
}


// write joint
void mjXWriter::OneJoint(XMLElement*     elem,
                         const mjCJoint* joint,
                         mjCDef*         def,
                         string_view     classname) {
  // regular
  if (!writingdefaults) {
    WriteAttrTxt(elem, "name", joint->name);
    if (classname != joint->classname && joint->classname != "main") {
      WriteAttrTxt(elem, "class", joint->classname);
    }
  }

  // defaults and regular
  WriteAttrTable(elem,
                 static_cast<const mjsJoint*>(joint),
                 &def->Joint().spec,
                 kJointAttrs,
                 kJointAttrsN);
  if (joint->type != mjJNT_FREE) { WriteAttr(elem, "pos", 3, joint->pos, def->Joint().spec.pos); }
  if (joint->type != mjJNT_FREE && joint->type != mjJNT_BALL) {
    WriteAttr(elem, "axis", 3, joint->axis, def->Joint().spec.axis);
  }
  if (joint->type != mjJNT_FREE) {
    WriteAttrKey(elem, "limited", FalseTrueAuto_map, 3, joint->limited, def->Joint().limited);
  }
  if (joint->type != mjJNT_FREE && joint->type != mjJNT_BALL) {
    WriteAttrKey(elem,
                 "actuatorfrclimited",
                 FalseTrueAuto_map,
                 3,
                 joint->actfrclimited,
                 def->Joint().actfrclimited);
  }

  // userdata
  if (writingdefaults) {
    WriteVector(elem, "user", joint->get_userdata());
  } else {
    WriteVector(elem, "user", joint->get_userdata(), def->Joint().get_userdata());
  }
}

// write geom
void mjXWriter::OneGeom(XMLElement* elem, const mjCGeom* geom, mjCDef* def, string_view classname) {
  double unitq[4] = {1, 0, 0, 0};
  double mass     = 0;

  // regular
  if (!writingdefaults) {
    WriteAttrTxt(elem, "name", geom->name);
    if (classname != geom->classname && geom->classname != "main") {
      WriteAttrTxt(elem, "class", geom->classname);
    }
    if (mjGEOMINFO[geom->type]) {
      WriteAttr(elem, "size", mjGEOMINFO[geom->type], geom->size, def->Geom().size);
    }
    if (mjuu_defined(geom->mass)) { mass = geom->GetVolume() * def->Geom().density; }

    // mesh geom
    if (geom->type == mjGEOM_MESH || geom->type == mjGEOM_SDF) {
      mjCMesh* mesh = geom->mesh;

      // write pos/quat if there is a difference
      if (!SameVector(geom->pos, mesh->GetPosPtr(), 3) ||
          !SameVector(geom->quat, mesh->GetQuatPtr(), 4)) {
        // recover geom pos/quat before mesh frame transformation
        double p[3], q[4];
        mjuu_copyvec(p, geom->pos, 3);
        mjuu_copyvec(q, geom->quat, 4);
        mjuu_frameaccuminv(p, q, mesh->GetPosPtr(), mesh->GetQuatPtr());

        // write
        WriteAttr(elem, "pos", 3, p, unitq + 1);
        WriteAttr(elem, "quat", 4, q, unitq);
      }
    }

    // non-mesh geom
    else {
      WriteAttr(elem, "pos", 3, geom->pos, unitq + 1);
      WriteAttr(elem, "quat", 4, geom->quat, unitq);
    }
  } else {
    WriteAttr(elem, "size", 3, geom->size, def->Geom().size);
  }

  // defaults and regular
  WriteAttrTable(elem,
                 static_cast<const mjsGeom*>(geom),
                 &def->Geom().spec,
                 kGeomAttrs,
                 kGeomAttrsN);
  WriteAttrKey(elem,
               "fluidshape",
               fluidshape_map,
               2,
               geom->fluid_ellipsoid,
               def->Geom().fluid_ellipsoid);
  if (geom->type != mjGEOM_MESH) {
    WriteAttrKey(elem, "shellinertia", bool_map, 2, geom->typeinertia, def->Geom().typeinertia);
  }
  if (mjuu_defined(geom->mass)) {
    WriteAttr(elem, "mass", 1, &geom->mass_, &mass);
  } else {
    WriteAttr(elem, "density", 1, &geom->density, &def->Geom().density);
  }
  if (geom->get_material() != def->Geom().get_material()) {
    WriteAttrTxt(elem, "material", geom->get_material());
  }

  // hfield and mesh attributes
  if (geom->type == mjGEOM_HFIELD) { WriteAttrTxt(elem, "hfield", geom->get_hfieldname()); }
  if (geom->type == mjGEOM_MESH || geom->type == mjGEOM_SDF) {
    WriteAttrTxt(elem, "mesh", geom->get_meshname());
  }

  // userdata
  if (writingdefaults) {
    WriteVector(elem, "user", geom->get_userdata());
  } else {
    WriteVector(elem, "user", geom->get_userdata(), def->Geom().get_userdata());
  }

  // write plugin
  if (geom->plugin.active) { OnePlugin(InsertEnd(elem, "plugin"), &geom->plugin); }
}

// write site
void mjXWriter::OneSite(XMLElement* elem, const mjCSite* site, mjCDef* def, string_view classname) {
  double unitq[4] = {1, 0, 0, 0};

  // regular
  if (!writingdefaults) {
    WriteAttrTxt(elem, "name", site->name);
    if (classname != site->classname && site->classname != "main") {
      WriteAttrTxt(elem, "class", site->classname);
    }
    WriteAttr(elem, "quat", 4, site->quat, unitq);
    if (mjGEOMINFO[site->type]) {
      WriteAttr(elem, "size", mjGEOMINFO[site->type], site->size, def->Site().size);
    }
  } else {
    WriteAttr(elem, "size", 3, site->size, def->Site().size);
  }

  // defaults and regular
  WriteAttrTable(elem,
                 static_cast<const mjsSite*>(site),
                 &def->Site().spec,
                 kSiteAttrs,
                 kSiteAttrsN);
  if (site->get_material() != def->Site().get_material()) {
    WriteAttrTxt(elem, "material", site->get_material());
  }

  // userdata
  if (writingdefaults) {
    WriteVector(elem, "user", site->get_userdata());
  } else {
    WriteVector(elem, "user", site->get_userdata(), def->Site().get_userdata());
  }
}

// write camera
void mjXWriter::OneCamera(XMLElement*      elem,
                          const mjCCamera* camera,
                          mjCDef*          def,
                          string_view      classname) {
  double unitq[4] = {1, 0, 0, 0};

  // regular
  if (!writingdefaults) {
    WriteAttrTxt(elem, "name", camera->name);
    if (classname != camera->classname && camera->classname != "main") {
      WriteAttrTxt(elem, "class", camera->classname);
    }
    WriteAttrTxt(elem, "target", camera->get_targetbody());
    WriteAttr(elem, "quat", 4, camera->quat, unitq);
  }

  // defaults and regular
  WriteAttrTable(elem,
                 static_cast<const mjsCamera*>(camera),
                 &def->Camera().spec,
                 kCameraAttrs,
                 kCameraAttrsN);

  // camera intrinsics if specified
  if (camera->sensor_size[0] > 0 && camera->sensor_size[1] > 0) {
    WriteAttr(elem, "sensorsize", 2, camera->sensor_size);
    WriteAttr(elem, "focal", 2, camera->focal_length, def->Camera().focal_length);
    WriteAttr(elem, "focalpixel", 2, camera->focal_pixel, def->Camera().focal_pixel);
    WriteAttr(elem, "principal", 2, camera->principal_length, def->Camera().principal_length);
    WriteAttr(elem, "principalpixel", 2, camera->principal_pixel, def->Camera().principal_pixel);
  } else {
    WriteAttr(elem, "fovy", 1, &camera->fovy, &def->Camera().fovy);
  }

  // userdata
  if (writingdefaults) {
    WriteVector(elem, "user", camera->get_userdata());
  } else {
    WriteVector(elem, "user", camera->get_userdata(), def->Camera().get_userdata());
  }
}

// write light
void mjXWriter::OneLight(XMLElement*     elem,
                         const mjCLight* light,
                         mjCDef*         def,
                         string_view     classname) {
  // regular
  if (!writingdefaults) {
    WriteAttrTxt(elem, "name", light->name);
    if (classname != light->classname && light->classname != "main") {
      WriteAttrTxt(elem, "class", light->classname);
    }
    WriteAttrTxt(elem, "target", light->get_targetbody());
  }

  // defaults and regular
  WriteAttrTable(elem,
                 static_cast<const mjsLight*>(light),
                 &def->Light().spec,
                 kLightAttrs,
                 kLightAttrsN);
  WriteAttrKey(elem, "type", lighttype_map, lighttype_sz, light->type, def->Light().type);
  WriteAttrTxt(elem, "texture", light->get_texture());
}

// write pair
// write the mechanical attributes of an element, driven by the same
// generated rows the reader uses; see the declaration for the contract
template <typename T>
void mjXWriter::WriteAttrTable(
    XMLElement* elem, const T* obj, const T* def, const mjXAttr* rows, int nrow) {
  const char* live = reinterpret_cast<const char*>(obj);
  const char* dflt = reinterpret_cast<const char*>(def);
  for (int i = 0; i < nrow; i++) {
    const mjXAttr& row = rows[i];
    if (row.handwrite || (writingdefaults && row.nodefault)) { continue; }
    const char* base = live + row.offset;
    // a null default object means the element has no defaults to compare
    // against: every defined value is written
    const char* dbase = dflt ? dflt + row.offset : nullptr;
    const int   dkey  = dflt ? 0 : -12345;  // WriteAttrKey's write-always default
    switch (row.kind) {
      case mjXAttr::kInt:
        WriteAttr(elem,
                  row.attr,
                  row.len,
                  (const int*)base,
                  (const int*)dbase,
                  /*trim=*/!row.exact);
        break;
      case mjXAttr::kDouble:
        WriteAttr(elem,
                  row.attr,
                  row.len,
                  (const double*)base,
                  (const double*)dbase,
                  /*trim=*/!row.exact);
        break;
      case mjXAttr::kNum:
        WriteAttr(elem,
                  row.attr,
                  row.len,
                  (const mjtNum*)base,
                  (const mjtNum*)dbase,
                  /*trim=*/!row.exact);
        break;
      case mjXAttr::kFloat:
        WriteAttr(elem,
                  row.attr,
                  row.len,
                  (const float*)base,
                  (const float*)dbase,
                  /*trim=*/!row.exact);
        break;
      case mjXAttr::kEnum:
        WriteAttrKey(elem,
                     row.attr,
                     row.map,
                     row.mapsz,
                     *(const int*)base,
                     dbase ? *(const int*)dbase : dkey);
        break;
      case mjXAttr::kEnumByte:
        WriteAttrKey(elem,
                     row.attr,
                     row.map,
                     row.mapsz,
                     *(const mjtByte*)base,
                     dbase ? *(const mjtByte*)dbase : dkey);
        break;
      case mjXAttr::kBool:
        WriteAttrKey(elem,
                     row.attr,
                     bool_map,
                     2,
                     *(const mjtByte*)base,
                     dbase ? *(const mjtByte*)dbase : dkey);
        break;
      case mjXAttr::kFlags:
        if (!dbase || *(const int*)base != *(const int*)dbase) {
          int              value = *(const int*)base;
          std::vector<int> data;
          for (int j = 0; j < row.mapsz; j++) {
            if (value & row.map[j].value) { data.push_back(row.map[j].value); }
          }
          if (!data.empty()) {
            WriteAttrKeys(elem, row.attr, row.map, row.mapsz, data.data(), data.size(), 0);
          }
        }
        break;
      default:
        // names, strings, files and custom-read attributes: OneX() remnant
        break;
    }
  }
}


void mjXWriter::OnePair(XMLElement* elem, const mjCPair* pair, mjCDef* def) {
  // regular
  if (!writingdefaults) {
    if (pair->classname != "main") { WriteAttrTxt(elem, "class", pair->classname); }
    WriteAttrTxt(elem, "geom1", pair->get_geomname1());
    WriteAttrTxt(elem, "geom2", pair->get_geomname2());
  }

  // defaults and regular
  WriteAttrTxt(elem, "name", pair->name);
  WriteAttrTable(elem,
                 static_cast<const mjsPair*>(pair),
                 &def->Pair().spec,
                 kPairAttrs,
                 kPairAttrsN);
}


// write equality
void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* equality, mjCDef* def) {
  // regular
  if (!writingdefaults) {
    WriteAttrTxt(elem, "name", equality->name);
    if (equality->classname != "main") { WriteAttrTxt(elem, "class", equality->classname); }

    switch (equality->type) {
      case mjEQ_CONNECT:
        if (equality->objtype == mjOBJ_BODY) {
          WriteAttrTxt(elem, "body1", mjs_getString(equality->name1));
          WriteAttrTxt(elem, "body2", mjs_getString(equality->name2));
          WriteAttr(elem, "anchor", 3, equality->data);
        } else {
          WriteAttrTxt(elem, "site1", mjs_getString(equality->name1));
          WriteAttrTxt(elem, "site2", mjs_getString(equality->name2));
        }
        break;

      case mjEQ_WELD:
        if (equality->objtype == mjOBJ_BODY) {
          WriteAttrTxt(elem, "body1", mjs_getString(equality->name1));
          WriteAttrTxt(elem, "body2", mjs_getString(equality->name2));
          // unlike connect, weld's body semantic does not require anchor,
          // and the reader zeroes it when absent: zeros is the default,
          // not the constructor's union payload
          double zero3[3] = {0, 0, 0};
          WriteAttr(elem, "anchor", 3, equality->data, zero3);
          WriteAttr(elem, "relpose", 7, equality->data + 3, def->Equality().spec.data + 3);
        } else {
          WriteAttrTxt(elem, "site1", mjs_getString(equality->name1));
          WriteAttrTxt(elem, "site2", mjs_getString(equality->name2));
        }
        WriteAttr(elem, "torquescale", 1, equality->data + 10, def->Equality().spec.data + 10);
        break;

      case mjEQ_JOINT:
        WriteAttrTxt(elem, "joint1", mjs_getString(equality->name1));
        WriteAttrTxt(elem, "joint2", mjs_getString(equality->name2));
        WriteAttr(elem, "polycoef", 5, equality->data, def->Equality().spec.data);
        break;

      case mjEQ_TENDON:
        WriteAttrTxt(elem, "tendon1", mjs_getString(equality->name1));
        WriteAttrTxt(elem, "tendon2", mjs_getString(equality->name2));
        WriteAttr(elem, "polycoef", 5, equality->data, def->Equality().spec.data);
        break;

      case mjEQ_FLEX:
      case mjEQ_FLEXVERT:
        WriteAttrTxt(elem, "flex", mjs_getString(equality->name1));
        break;

      case mjEQ_FLEXSTRAIN:
        WriteAttrTxt(elem, "flex", mjs_getString(equality->name1));
        WriteAttr(elem, "cell", 3, equality->data, def->Equality().spec.data);
        break;

      default:
        mju_error("mjXWriter: unknown equality type.");
    }
  }

  // defaults and regular
  WriteAttrTable(elem,
                 static_cast<const mjsEquality*>(equality),
                 &def->Equality().spec,
                 kEqualityBaseAttrs,
                 kEqualityBaseAttrsN);
}


// write tendon
void mjXWriter::OneTendon(XMLElement* elem, const mjCTendon* tendon, mjCDef* def) {
  bool fixed = (tendon->GetWrap(0) && tendon->GetWrap(0)->Type() == mjWRAP_JOINT);

  // regular
  if (!writingdefaults) {
    WriteAttrTxt(elem, "name", tendon->name);
    if (tendon->classname != "main") { WriteAttrTxt(elem, "class", tendon->classname); }
  }

  // defaults and regular; the fixed rows are the spatial rows without the
  // appearance attributes, which is exactly the tag difference
  if (fixed) {
    WriteAttrTable(elem,
                   static_cast<const mjsTendon*>(tendon),
                   &def->Tendon().spec,
                   kFixedAttrs,
                   kFixedAttrsN);
  } else {
    WriteAttrTable(elem,
                   static_cast<const mjsTendon*>(tendon),
                   &def->Tendon().spec,
                   kSpatialAttrs,
                   kSpatialAttrsN);
  }
  if (tendon->springlength[0] != tendon->springlength[1] ||
      def->Tendon().springlength[0] != def->Tendon().springlength[1]) {
    WriteAttr(elem, "springlength", 2, tendon->springlength, def->Tendon().springlength);
  } else {
    WriteAttr(elem, "springlength", 1, tendon->springlength, def->Tendon().springlength);
  }
  if (!fixed && tendon->get_material() != def->Tendon().get_material()) {
    WriteAttrTxt(elem, "material", tendon->get_material());
  }

  // userdata
  if (writingdefaults) {
    WriteVector(elem, "user", tendon->get_userdata());
  } else {
    WriteVector(elem, "user", tendon->get_userdata(), def->Tendon().get_userdata());
  }
}


// write actuator
void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* actuator, mjCDef* def) {
  // regular
  if (!writingdefaults) {
    WriteAttrTxt(elem, "name", actuator->name);
    if (actuator->classname != "main") { WriteAttrTxt(elem, "class", actuator->classname); }

    // transmission target
    switch (actuator->trntype) {
      case mjTRN_JOINT:
        WriteAttrTxt(elem, "joint", actuator->get_target());
        break;

      case mjTRN_JOINTINPARENT:
        WriteAttrTxt(elem, "jointinparent", actuator->get_target());
        break;

      case mjTRN_TENDON:
        WriteAttrTxt(elem, "tendon", actuator->get_target());
        break;

      case mjTRN_SLIDERCRANK:
        WriteAttrTxt(elem, "cranksite", actuator->get_target());
        WriteAttrTxt(elem, "slidersite", actuator->get_slidersite());
        break;

      case mjTRN_SITE:
        WriteAttrTxt(elem, "site", actuator->get_target());
        WriteAttrTxt(elem, "refsite", actuator->get_refsite());
        break;

      case mjTRN_BODY:
        WriteAttrTxt(elem, "body", actuator->get_target());
        break;

      default:  // SHOULD NOT OCCUR
        break;
    }
  }

  // defaults and regular
  WriteAttrTable(elem,
                 static_cast<const mjsActuator*>(actuator),
                 &def->Actuator().spec,
                 kGeneralAttrs,
                 kGeneralAttrsN);
  WriteAttr(elem, "cranklength", 1, &actuator->cranklength, &def->Actuator().cranklength);
  // special handling of actdim which has default value of -1
  if (writingdefaults) {
    WriteAttrInt(elem, "actdim", actuator->actdim, def->Actuator().actdim);
  } else {
    int default_actdim = (actuator->dyntype != mjDYN_NONE && actuator->dyntype != mjDYN_DCMOTOR);
    WriteAttrInt(elem, "actdim", actuator->actdim, default_actdim);
  }

  // plugins: write config attributes
  if (actuator->plugin.active) {
    OnePlugin(elem, &actuator->plugin);
  }

  // non-plugins: write actuator parameters
  else {
    WriteAttrKey(elem, "gaintype", gain_map, gain_sz, actuator->gaintype, def->Actuator().gaintype);
    if (actuator->gaintype == mjGAIN_SO3) {
      WriteAttrKey(elem,
                   "input",
                   inputchart_map,
                   inputchart_sz,
                   actuator->ctrlspec,
                   def->Actuator().ctrlspec);
    } else if (actuator->ctrlspec != def->Actuator().ctrlspec) {
      if (actuator->ctrlspec == mjINPUT_NONE) {
        WriteAttrTxt(elem, "input", "none");
      } else {
        std::string tokens;
        for (int k = 0; k < inputbit_sz; k++) {
          if (actuator->ctrlspec & inputbit_map[k].value) {
            tokens += std::string(tokens.empty() ? "" : " ") + inputbit_map[k].key;
          }
        }
        WriteAttrTxt(elem, "input", tokens);
      }
    }
    WriteAttrKey(elem, "biastype", bias_map, bias_sz, actuator->biastype, def->Actuator().biastype);
    WriteAttr(elem, "gainprm", mjNGAIN, actuator->gainprm, def->Actuator().gainprm, true);
    WriteAttr(elem, "biasprm", mjNBIAS, actuator->biasprm, def->Actuator().biasprm, true);
  }

  // userdata
  if (writingdefaults) {
    WriteVector(elem, "user", actuator->get_userdata());
  } else {
    WriteVector(elem, "user", actuator->get_userdata(), def->Actuator().get_userdata());
  }
}


// write plugin
void mjXWriter::OnePlugin(XMLElement* elem, const mjsPlugin* plugin) {
  const string instance_name = string(mjs_getString(plugin->name));
  const string plugin_name   = string(mjs_getString(plugin->plugin_name));
  if (!instance_name.empty()) {
    WriteAttrTxt(elem, "instance", instance_name);
  } else {
    WriteAttrTxt(elem, "plugin", plugin_name);
    const mjpPlugin* pplugin =
        mjp_getPluginAtSlot(static_cast<mjCPlugin*>(plugin->element)->plugin_slot);
    const char* c = &(static_cast<mjCPlugin*>(plugin->element)->flattened_attributes[0]);
    for (int i = 0; i < pplugin->nattribute; ++i) {
      string value(c);
      if (!value.empty()) {
        XMLElement* config_elem = InsertEnd(elem, "config");
        WriteAttrTxt(config_elem, "key", pplugin->attributes[i]);
        WriteAttrTxt(config_elem, "value", value);
        c += value.size();
      }
      ++c;
    }
  }
}


//---------------------------------- class mjXWriter: top-level API --------------------------------

// constructor
mjXWriter::mjXWriter(void) {
  writingdefaults = false;
}


// cast model
void mjXWriter::SetModel(mjSpec* _spec, const mjModel* m) {
  if (_spec) { model = static_cast<mjCModel*>(_spec->element); }
  if (m) { mj_copyBack(&model->spec, m); }
}


// save existing model in MJCF canonical format, must be compiled
string mjXWriter::Write(char* error, size_t error_sz) {
  // check model
  if (!model || !model->IsCompiled()) {
    mjCopyError(error, "XML Write error: Only compiled model can be written", error_sz);
    return "";
  }

  // create document and root
  XMLDocument doc;
  XMLElement* root = doc.NewElement("mujoco");
  root->SetAttribute("model", mjs_getString(model->modelname));

  // insert root
  doc.InsertFirstChild(root);

  // write comment if present
  string text = mjs_getString(model->comment);
  if (!text.empty()) {
    XMLComment* comment = doc.NewComment(text.c_str());
    root->LinkEndChild(comment);
  }

  // create DOM
  Compiler(root);
  Option(root);
  Size(root);
  Statistic(root);
  Visual(root);
  writingdefaults = true;
  Default(root, model->Default());
  writingdefaults = false;
  Extension(root);
  Asset(root);
  Body(InsertEnd(root, "worldbody"), model->GetWorld(), nullptr);
  Deformable(root);
  Contact(root);
  Tendon(root);
  Equality(root);
  Actuator(root);
  Sensor(root);
  Custom(root);
  Keyframe(root);

  return WriteDoc(doc, error, error_sz);
}


// compiler section
void mjXWriter::Compiler(XMLElement* root) {
  XMLElement* section = InsertEnd(root, "compiler");

  // settings
  WriteAttrTxt(section, "angle", "radian");
  if (!model->get_meshdir().empty()) { WriteAttrTxt(section, "meshdir", model->get_meshdir()); }
  if (!model->get_texturedir().empty()) {
    WriteAttrTxt(section, "texturedir", model->get_texturedir());
  }
  if (!model->compiler.usethread) { WriteAttrTxt(section, "usethread", "false"); }

  if (model->compiler.boundmass) { WriteAttr(section, "boundmass", 1, &model->compiler.boundmass); }
  if (model->compiler.boundinertia) {
    WriteAttr(section, "boundinertia", 1, &model->compiler.boundinertia);
  }
  if (model->compiler.alignfree) { WriteAttrTxt(section, "alignfree", "true"); }
  if (!model->compiler.autolimits) { WriteAttrTxt(section, "autolimits", "false"); }
  WriteAttrKey(section,
               "conflict",
               conflict_map,
               conflict_sz,
               model->compiler.conflict,
               mjCONFLICT_WARNING);
}


// option section
void mjXWriter::Option(XMLElement* root) {
  mjOption opt;
  mj_defaultOption(&opt);

  XMLElement* section = InsertEnd(root, "option");

  // option; the freshly-defaulted struct is the comparison object
  WriteAttrTable(section, &model->option, &opt, kOptionAttrs, kOptionAttrsN);

  // actuator group disable
  int disabled_groups[31];
  int ndisabled = 0;
  for (int i = 0; i < 31; ++i) {
    if (model->option.disableactuator & (1 << i)) { disabled_groups[ndisabled++] = i; }
  }
  WriteAttr(section, "actuatorgroupdisable", ndisabled, disabled_groups);

  // write disable/enable flags if any of them are set; invert while writing
  if (model->option.disableflags || model->option.enableflags) {
    XMLElement* sub = InsertEnd(section, "flag");

#define WRITEDSBL(NAME, MASK)                                                       \
  if (model->option.disableflags & MASK) WriteAttrKey(sub, NAME, enable_map, 2, 0);
    // clang-format off
    WRITEDSBL("constraint",     mjDSBL_CONSTRAINT)
    WRITEDSBL("equality",       mjDSBL_EQUALITY)
    WRITEDSBL("frictionloss",   mjDSBL_FRICTIONLOSS)
    WRITEDSBL("limit",          mjDSBL_LIMIT)
    WRITEDSBL("contact",        mjDSBL_CONTACT)
    WRITEDSBL("spring",         mjDSBL_SPRING)
    WRITEDSBL("damper",         mjDSBL_DAMPER)
    WRITEDSBL("gravity",        mjDSBL_GRAVITY)
    WRITEDSBL("clampctrl",      mjDSBL_CLAMPCTRL)
    WRITEDSBL("warmstart",      mjDSBL_WARMSTART)
    WRITEDSBL("filterparent",   mjDSBL_FILTERPARENT)
    WRITEDSBL("actuation",      mjDSBL_ACTUATION)
    WRITEDSBL("refsafe",        mjDSBL_REFSAFE)
    WRITEDSBL("sensor",         mjDSBL_SENSOR)
    WRITEDSBL("midphase",       mjDSBL_MIDPHASE)
    WRITEDSBL("eulerdamp",      mjDSBL_EULERDAMP)
    WRITEDSBL("autoreset",      mjDSBL_AUTORESET)
    WRITEDSBL("nativeccd",      mjDSBL_NATIVECCD)
    WRITEDSBL("island",         mjDSBL_ISLAND)
    WRITEDSBL("multiccd",       mjDSBL_MULTICCD)
    // clang-format on
#undef WRITEDSBL

#define WRITEENBL(NAME, MASK)                                                      \
  if (model->option.enableflags & MASK) WriteAttrKey(sub, NAME, enable_map, 2, 1);
    // clang-format off
    WRITEENBL("override",       mjENBL_OVERRIDE)
    WRITEENBL("energy",         mjENBL_ENERGY)
    WRITEENBL("fwdinv",         mjENBL_FWDINV)
    WRITEENBL("invdiscrete",    mjENBL_INVDISCRETE)
    WRITEENBL("sleep",          mjENBL_SLEEP)
    WRITEENBL("diagexact",      mjENBL_DIAGEXACT)
    // clang-format on
#undef WRITEENBL
  }

  // remove entire section if no attributes or elements
  if (!section->FirstAttribute() && !section->FirstChildElement()) { root->DeleteChild(section); }
}


// size section
void mjXWriter::Size(XMLElement* root) {
  XMLElement* section = InsertEnd(root, "size");

  // write memory
  if (model->memory != -1) { WriteAttrTxt(section, "memory", mju_writeNumBytes(model->memory)); }

  // deprecated sizes, hand-read into locals with range checks
  WriteAttrInt(section, "njmax", model->njmax, -1);
  WriteAttrInt(section, "nconmax", model->nconmax, -1);
  WriteAttrInt(section, "nstack", model->nstack, -1);

  // write sizes; the spec defaults are -1 (auto), but compilation resolves
  // them to the actual counts, so the comparison object is zero-initialized
  mjSpec zerospec = {};
  WriteAttrTable(section, static_cast<const mjSpec*>(model), &zerospec, kSizeAttrs, kSizeAttrsN);

  // remove entire section if no attributes
  if (!section->FirstAttribute()) root->DeleteChild(section);
}


// statistic section
void mjXWriter::Statistic(XMLElement* root) {
  XMLElement* section = InsertEnd(root, "statistic");

  // statistics are unset (mjNAN) rather than defaulted: there is nothing to
  // compare against, and WriteAttr skips the undefined values by itself
  WriteAttrTable(section,
                 &model->stat,
                 (const mjStatistic*)nullptr,
                 kStatisticAttrs,
                 kStatisticAttrsN);

  // remove entire section if no attributes
  if (!section->FirstAttribute()) root->DeleteChild(section);
}


// visual section
void mjXWriter::Visual(XMLElement* root) {
  mjVisual visdef, *vis = &model->visual;
  mj_defaultVisual(&visdef);

  XMLElement* section = InsertEnd(root, "visual");

  // the sub-sections are projections into mjVisual: their rows carry
  // member-path offsets, so one struct pair drives them all
  struct {
    const char*    tag;
    const mjXAttr* rows;
    int            n;
  } subs[] = {
      {"global",    kGlobalAttrs,    kGlobalAttrsN   },
      {"quality",   kQualityAttrs,   kQualityAttrsN  },
      {"headlight", kHeadlightAttrs, kHeadlightAttrsN},
      {"map",       kMapAttrs,       kMapAttrsN      },
      {"scale",     kScaleAttrs,     kScaleAttrsN    },
      {"rgba",      kRgbaAttrs,      kRgbaAttrsN     },
  };
  for (const auto& sub : subs) {
    XMLElement* elem = InsertEnd(section, sub.tag);
    WriteAttrTable(elem, vis, &visdef, sub.rows, sub.n);
    if (!elem->FirstAttribute()) { section->DeleteChild(elem); }
  }

  // remove entire section if no elements
  if (!section->FirstChildElement()) { root->DeleteChild(section); }
}


// default section
void mjXWriter::Default(XMLElement* root, mjCDef* def) {
  XMLElement* elem;
  XMLElement* section;

  // pointer to parent defaults
  mjCDef* parent;
  if (def->parent) {
    parent = def->parent;
  } else {
    parent = new mjCDef;
  }

  // create section, write class name
  section = InsertEnd(root, "default");
  if (def->name != "main") { WriteAttrTxt(section, "class", def->name); }

  // mesh
  elem = InsertEnd(section, "mesh");
  OneMesh(elem, &def->Mesh(), parent);
  if (!elem->FirstAttribute()) section->DeleteChild(elem);

  // material
  elem = InsertEnd(section, "material");
  OneMaterial(elem, &def->Material(), parent);
  if (!elem->FirstAttribute()) section->DeleteChild(elem);

  // joint
  elem = InsertEnd(section, "joint");
  OneJoint(elem, &def->Joint(), parent);
  if (!elem->FirstAttribute()) section->DeleteChild(elem);

  // geom
  elem = InsertEnd(section, "geom");
  OneGeom(elem, &def->Geom(), parent);
  if (!elem->FirstAttribute()) section->DeleteChild(elem);

  // site
  elem = InsertEnd(section, "site");
  OneSite(elem, &def->Site(), parent);
  if (!elem->FirstAttribute()) section->DeleteChild(elem);

  // camera
  elem = InsertEnd(section, "camera");
  OneCamera(elem, &def->Camera(), parent);
  if (!elem->FirstAttribute()) section->DeleteChild(elem);

  // light
  elem = InsertEnd(section, "light");
  OneLight(elem, &def->Light(), parent);
  if (!elem->FirstAttribute()) section->DeleteChild(elem);

  // pair
  elem = InsertEnd(section, "pair");
  OnePair(elem, &def->Pair(), parent);
  if (!elem->FirstAttribute()) section->DeleteChild(elem);

  // equality
  elem = InsertEnd(section, "equality");
  OneEquality(elem, &def->Equality(), parent);
  if (!elem->FirstAttribute()) section->DeleteChild(elem);

  // tendon
  elem = InsertEnd(section, "tendon");
  OneTendon(elem, &def->Tendon(), parent);
  if (!elem->FirstAttribute()) section->DeleteChild(elem);

  // actuator
  elem = InsertEnd(section, "general");
  OneActuator(elem, &def->Actuator(), parent);
  if (!elem->FirstAttribute()) section->DeleteChild(elem);

  // if top-level class has no members or children, delete it and return
  if (!def->parent && section->NoChildren() && def->child.empty()) {
    root->DeleteChild(section);
    delete parent;
    return;
  }

  // add children recursively
  for (int i = 0; i < (int)def->child.size(); i++) { Default(section, def->child[i]); }

  // delete parent defaults if allocated here
  if (!def->parent) { delete parent; }
}


// extension section
void mjXWriter::Extension(XMLElement* root) {
  // skip section if there is no required plugin
  if (model->ActivePlugins().empty()) { return; }

  // create section
  XMLElement* section = InsertEnd(root, "extension");

  // keep track of plugins whose <plugin> section have been created
  std::unordered_set<const mjpPlugin*> seen_plugins;

  // write all plugins
  const mjpPlugin* last_plugin = nullptr;
  XMLElement*      plugin_elem = nullptr;
  for (int i = 0; i < model->Plugins().size(); ++i) {
    mjCPlugin* pp = static_cast<mjCPlugin*>(model->GetObject(mjOBJ_PLUGIN, i));

    if (pp->name.empty()) {
      // reached the first unnamed plugin instance, meaning that it was created through an
      // "implicit" plugin element, e.g. sensor or actuator
      break;
    }

    // check if we need to open a new <plugin> section
    const mjpPlugin* plugin = mjp_getPluginAtSlot(pp->plugin_slot);
    if (plugin != last_plugin) {
      plugin_elem = InsertEnd(section, "plugin");
      WriteAttrTxt(plugin_elem, "plugin", plugin->name);
      seen_plugins.insert(plugin);
      last_plugin = plugin;
    }

    // write instance element
    XMLElement* elem = InsertEnd(plugin_elem, "instance");
    WriteAttrTxt(elem, "name", pp->name);

    // write plugin config attributes
    const char* c = &pp->flattened_attributes[0];
    for (int i = 0; i < plugin->nattribute; ++i) {
      string value(c);
      if (!value.empty()) {
        XMLElement* config_elem = InsertEnd(elem, "config");
        WriteAttrTxt(config_elem, "key", plugin->attributes[i]);
        WriteAttrTxt(config_elem, "value", value);
        c += value.size();
      }
      ++c;
    }
  }

  // write <plugin> elements for plugins without explicit instances
  for (const auto& [plugin, slot] : model->ActivePlugins()) {
    if (seen_plugins.find(plugin) == seen_plugins.end()) {
      plugin_elem = InsertEnd(section, "plugin");
      WriteAttrTxt(plugin_elem, "plugin", plugin->name);
    }
  }
}


// custom section
void mjXWriter::Custom(XMLElement* root) {
  XMLElement* elem;

  // get sizes, skip section if empty
  int nnum = model->NumObjects(mjOBJ_NUMERIC);
  int ntxt = model->NumObjects(mjOBJ_TEXT);
  int ntup = model->NumObjects(mjOBJ_TUPLE);

  // skip section if empty
  if (nnum == 0 && ntxt == 0 && ntup == 0) { return; }

  // create section
  XMLElement* section = InsertEnd(root, "custom");

  // write all numerics
  for (int i = 0; i < nnum; i++) {
    mjCNumeric* numeric = (mjCNumeric*)model->GetObject(mjOBJ_NUMERIC, i);

    elem = InsertEnd(section, "numeric");
    WriteAttrTxt(elem, "name", numeric->name);
    WriteAttrInt(elem, "size", numeric->size);
    WriteAttr(elem, "data", numeric->size, numeric->data_.data());
  }

  // write all texts
  for (int i = 0; i < ntxt; i++) {
    mjCText* text = (mjCText*)model->GetObject(mjOBJ_TEXT, i);

    elem = InsertEnd(section, "text");
    WriteAttrTxt(elem, "name", text->name);
    if (text->data_.find_first_of("\n\r<>&\"'") != std::string::npos) {
      XMLText* text_node = elem->GetDocument()->NewText(text->data_.c_str());
      text_node->SetCData(true);
      elem->InsertEndChild(text_node);
    } else {
      WriteAttrTxt(elem, "data", text->data_.c_str());
    }
  }

  // write all tuples
  for (int i = 0; i < ntup; i++) {
    mjCTuple* tuple = (mjCTuple*)model->GetObject(mjOBJ_TUPLE, i);

    elem = InsertEnd(section, "tuple");
    WriteAttrTxt(elem, "name", tuple->name);

    // write objects in tuple
    for (int j = 0; j < (int)tuple->objtype_.size(); j++) {
      XMLElement* obj = InsertEnd(elem, "element");
      WriteAttrTxt(obj, "objtype", mju_type2Str((int)tuple->objtype_[j]));
      WriteAttrTxt(obj, "objname", tuple->objname_[j].c_str());
      double oprm = tuple->objprm_[j];
      if (oprm != 0) { WriteAttr(obj, "prm", 1, &oprm); }
    }
  }
}


// asset section
void mjXWriter::Asset(XMLElement* root) {
  XMLElement* elem;

  // get sizes
  int ntex    = model->NumObjects(mjOBJ_TEXTURE);
  int nmat    = model->NumObjects(mjOBJ_MATERIAL);
  int nmesh   = model->NumObjects(mjOBJ_MESH);
  int nhfield = model->NumObjects(mjOBJ_HFIELD);

  // return if empty
  if (ntex == 0 && nmat == 0 && nmesh == 0 && nhfield == 0) { return; }

  // create section
  XMLElement* section = InsertEnd(root, "asset");

  // write textures
  mjCTexture deftex(0);
  for (int i = 0; i < ntex; i++) {
    // create element
    mjCTexture* texture = (mjCTexture*)model->GetObject(mjOBJ_TEXTURE, i);

    elem = InsertEnd(section, "texture");

    // write common attributes
    WriteAttrKey(elem, "type", texture_map, texture_sz, texture->type);
    WriteAttrKey(elem, "colorspace", colorspace_map, colorspace_sz, texture->colorspace);
    WriteAttrTxt(elem, "name", texture->name);

    // write builtin
    if (texture->builtin != mjBUILTIN_NONE) {
      WriteAttrKey(elem, "builtin", builtin_map, builtin_sz, texture->builtin);
      WriteAttrKey(elem, "mark", mark_map, mark_sz, texture->mark, deftex.mark);
      WriteAttr(elem, "rgb1", 3, texture->rgb1, deftex.rgb1);
      WriteAttr(elem, "rgb2", 3, texture->rgb2, deftex.rgb2);
      WriteAttr(elem, "markrgb", 3, texture->markrgb, deftex.markrgb);
      WriteAttr(elem, "random", 1, &texture->random, &deftex.random);
      WriteAttrInt(elem, "width", texture->width);
      WriteAttrInt(elem, "height", texture->height);
    }

    // write buffer
    else if (texture->get_cubefiles()[0].empty() &&
             texture->get_cubefiles()[1].empty() &&
             texture->get_cubefiles()[2].empty() &&
             texture->get_cubefiles()[3].empty() &&
             texture->get_cubefiles()[4].empty() &&
             texture->get_cubefiles()[5].empty() &&
             texture->File().empty() &&
             texture->gridsize[0] == 1 &&
             texture->gridsize[1] == 1) {
      throw mjXError(0, "no support for buffer textures.");
    }

    // write textures loaded from files
    else {
      // write single file
      WriteAttrTxt(elem, "content_type", texture->get_content_type());
      WriteAttrTxt(elem, "file", texture->File());

      // write separate files
      WriteAttrTxt(elem, "fileright", texture->get_cubefiles()[0]);
      WriteAttrTxt(elem, "fileleft", texture->get_cubefiles()[1]);
      WriteAttrTxt(elem, "fileup", texture->get_cubefiles()[2]);
      WriteAttrTxt(elem, "filedown", texture->get_cubefiles()[3]);
      WriteAttrTxt(elem, "filefront", texture->get_cubefiles()[4]);
      WriteAttrTxt(elem, "fileback", texture->get_cubefiles()[5]);
      if (texture->hflip) { WriteAttrKey(elem, "hflip", bool_map, 2, 1); }
      if (texture->vflip) { WriteAttrKey(elem, "vflip", bool_map, 2, 1); }

      // write grid
      if (texture->gridsize[0] != 1 || texture->gridsize[1] != 1) {
        double gsize[2] = {(double)texture->gridsize[0], (double)texture->gridsize[1]};
        WriteAttr(elem, "gridsize", 2, gsize);
        WriteAttrTxt(elem, "gridlayout", texture->gridlayout);
      }
    }
  }

  // write materials
  for (int i = 0; i < nmat; i++) {
    // create element and write
    mjCMaterial* material = (mjCMaterial*)model->GetObject(mjOBJ_MATERIAL, i);

    elem = InsertEnd(section, "material");
    OneMaterial(elem, material, model->def_map[material->classname]);
  }

  // write meshes
  for (int i = 0; i < nmesh; i++) {
    // create element and write
    mjCMesh* mesh = (mjCMesh*)model->GetObject(mjOBJ_MESH, i);
    if (mesh->Plugin().active) {
      elem = InsertEnd(section, "mesh");
      WriteAttrTxt(elem, "name", mesh->name);
      WriteAttrTxt(elem, "file", mesh->File());
      OnePlugin(InsertEnd(elem, "plugin"), &mesh->Plugin());
    } else {
      elem = InsertEnd(section, "mesh");
      OneMesh(elem, mesh, model->def_map[mesh->classname]);
    }
  }

  // write hfields
  for (int i = 0; i < nhfield; i++) {
    // create element
    mjCHField* hfield = (mjCHField*)model->GetObject(mjOBJ_HFIELD, i);

    elem = InsertEnd(section, "hfield");

    // write attributes
    WriteAttrTxt(elem, "name", hfield->name);
    WriteAttr(elem, "size", 4, hfield->size);
    if (!hfield->file_.empty()) {
      WriteAttrTxt(elem, "content_type", hfield->content_type_);
      WriteAttrTxt(elem, "file", hfield->file_);
    } else {
      int nrow = hfield->nrow;
      int ncol = hfield->ncol;
      WriteAttrInt(elem, "nrow", nrow);
      WriteAttrInt(elem, "ncol", ncol);
      if (!hfield->get_userdata().empty()) {
        // copy in reverse row order, so XML string is top-to-bottom
        std::vector<float>        flipped(nrow * ncol);
        const std::vector<float>& userdata = hfield->get_userdata();
        for (int i = 0; i < nrow; i++) {
          int flip = nrow - 1 - i;
          for (int j = 0; j < ncol; j++) { flipped[i * ncol + j] = userdata[flip * ncol + j]; }
        }

        string text;
        Vector2String(text, flipped, ncol);
        WriteAttrTxt(elem, "elevation", text);
      }
    }
  }
}


XMLElement* mjXWriter::OneFrame(XMLElement* elem, mjCFrame* frame) {
  if (!frame) { return elem; }

  // TODO: empty classname should not occur (but does)
  if (frame->name.empty() && (frame->classname.empty() || frame->classname == "main")) {
    return elem;
  }

  XMLElement* frame_elem = InsertEnd(elem, "frame");
  WriteAttrTxt(frame_elem, "name", frame->name);
  if (frame->classname != "main") { WriteAttrTxt(frame_elem, "childclass", frame->classname); }
  return frame_elem;
}


// recursive body and frame writer
void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame, string_view childclass) {
  double unitq[4] = {1, 0, 0, 0};

  if (!body) {
    throw mjXError(0, "missing body in XML write");  // SHOULD NOT OCCUR
  }

  // write body attributes and inertial
  else if (!frame && body != model->GetWorld()) {
    WriteAttrTxt(elem, "name", body->name);
    if (childclass != body->classname && body->classname != "main") {
      WriteAttrTxt(elem, "childclass", body->classname);
    }

    // write pos if it's not {0, 0, 0}
    if (body->pos[0] || body->pos[1] || body->pos[2]) { WriteAttr(elem, "pos", 3, body->pos); }
    WriteAttr(elem, "quat", 4, body->quat, unitq);
    if (body->mocap) { WriteAttrKey(elem, "mocap", bool_map, 2, 1); }

    // gravity compensation
    if (body->gravcomp) { WriteAttr(elem, "gravcomp", 1, &body->gravcomp); }

    // sleep policy
    if (body->sleep != mjSLEEP_AUTO &&
        body->sleep != mjSLEEP_AUTO_NEVER &&
        body->sleep != mjSLEEP_AUTO_ALLOWED) {
      WriteAttrKey(elem, "sleep", bodysleep_map, bodysleep_sz, body->sleep);
    }

    // simple optimization
    WriteAttrKey(elem, "simple", FalseAuto_map, 2, body->simple, 1);

    // userdata
    WriteVector(elem, "user", body->get_userdata());

    // write inertial
    if (model->compiler.saveinertial ||
        (body->explicitinertial && model->compiler.inertiafromgeom != mjINERTIAFROMGEOM_TRUE)) {
      XMLElement* inertial = InsertEnd(elem, "inertial");
      WriteAttr(inertial, "pos", 3, body->ipos);
      WriteAttr(inertial, "quat", 4, body->iquat, unitq);
      WriteAttr(inertial, "mass", 1, &body->mass);
      WriteAttr(inertial, "diaginertia", 3, body->inertia);
    }
  }

  // joints in this frame
  for (int i = 0; i < body->joints.size(); i++) {
    if (body->joints[i]->frame != frame) { continue; }
    string classname = body->joints[i]->frame && !body->joints[i]->frame->classname.empty()
                           ? body->joints[i]->frame->classname
                           : body->classname;
    OneJoint(InsertEnd(elem, "joint"),
             body->joints[i],
             model->def_map[body->joints[i]->classname],
             classname.empty() ? childclass : classname);
  }

  // geoms in this frame
  for (int i = 0; i < body->geoms.size(); i++) {
    if (body->geoms[i]->frame != frame) { continue; }
    string classname = body->geoms[i]->frame && !body->geoms[i]->frame->classname.empty()
                           ? body->geoms[i]->frame->classname
                           : body->classname;
    OneGeom(InsertEnd(elem, "geom"),
            body->geoms[i],
            model->def_map[body->geoms[i]->classname],
            classname.empty() ? childclass : classname);
  }

  // sites in this frame
  for (int i = 0; i < body->sites.size(); i++) {
    if (body->sites[i]->frame != frame) { continue; }
    string classname = body->sites[i]->frame && !body->sites[i]->frame->classname.empty()
                           ? body->sites[i]->frame->classname
                           : body->classname;
    OneSite(InsertEnd(elem, "site"),
            body->sites[i],
            model->def_map[body->sites[i]->classname],
            classname.empty() ? childclass : classname);
  }

  // cameras in this frame
  for (int i = 0; i < body->cameras.size(); i++) {
    if (body->cameras[i]->frame != frame) { continue; }
    string classname = body->cameras[i]->frame && !body->cameras[i]->frame->classname.empty()
                           ? body->cameras[i]->frame->classname
                           : body->classname;
    OneCamera(InsertEnd(elem, "camera"),
              body->cameras[i],
              model->def_map[body->cameras[i]->classname],
              classname.empty() ? childclass : classname);
  }

  // lights in this frame
  for (int i = 0; i < body->lights.size(); i++) {
    if (body->lights[i]->frame != frame) { continue; }
    string classname = body->lights[i]->frame && !body->lights[i]->frame->classname.empty()
                           ? body->lights[i]->frame->classname
                           : body->classname;
    OneLight(InsertEnd(elem, "light"),
             body->lights[i],
             model->def_map[body->lights[i]->classname],
             classname.empty() ? childclass : classname);
  }

  // write plugin
  if (body->plugin.active) { OnePlugin(InsertEnd(elem, "plugin"), &body->plugin); }

  // write children recursively
  int i = 0, j = 0;
  while (i < body->bodies.size() || body->bodies.empty()) {
    mjCFrame* bframe = body->bodies.empty() ? nullptr : body->bodies[i]->frame;

    // write body if its frame matches the current frame, avoid access if there are no bodies
    if (bframe == frame && !body->bodies.empty()) {
      string classname = bframe && !bframe->classname.empty() ? bframe->classname : body->classname;
      Body(InsertEnd(elem, "body"),
           body->bodies[i],
           nullptr,
           classname.empty() ? childclass : classname);
    }

    i++;

    // do not go to frames until we reach a body with a frame or we are done with bodies
    if (!bframe && i < body->bodies.size()) { continue; }

    // loop over the remaining frames in the current body
    while (j < body->frames.size()) {
      mjCFrame* fframe = body->frames[j++];

      // write frame if its frame matches the current frame
      if (fframe->frame == frame) {
        string classname =
            fframe && !fframe->classname.empty() ? fframe->classname : body->classname;
        Body(OneFrame(elem, fframe), body, fframe, childclass);
      }
    }

    // if there are no bodies, we only want to run the loop once
    if (body->bodies.empty()) { break; }
  }
}


// collision section
void mjXWriter::Contact(XMLElement* root) {
  XMLElement* elem;

  // get number of pairs of each type
  int npair    = model->NumObjects(mjOBJ_PAIR);
  int nexclude = model->NumObjects(mjOBJ_EXCLUDE);

  // skip if section is empty
  if (npair == 0 && nexclude == 0) { return; }

  // create section
  XMLElement* section = InsertEnd(root, "contact");

  // write all geom pairs
  for (int i = 0; i < npair; i++) {
    // create element and write
    mjCPair* pair = (mjCPair*)model->GetObject(mjOBJ_PAIR, i);

    elem = InsertEnd(section, "pair");
    OnePair(elem, pair, model->def_map[pair->classname]);
  }

  // write all exclude pairs
  for (int i = 0; i < nexclude; i++) {
    // create element
    mjCBodyPair* exclude = (mjCBodyPair*)model->GetObject(mjOBJ_EXCLUDE, i);

    elem = InsertEnd(section, "exclude");

    // write attributes
    WriteAttrTxt(elem, "name", exclude->name);
    WriteAttrTxt(elem, "body1", exclude->get_bodyname1());
    WriteAttrTxt(elem, "body2", exclude->get_bodyname2());
  }
}


// constraint section
void mjXWriter::Equality(XMLElement* root) {
  // skip section if empty
  int num;
  if ((num = model->NumObjects(mjOBJ_EQUALITY)) == 0) { return; }

  // create section
  XMLElement* section = InsertEnd(root, "equality");

  // write all constraints
  for (int i = 0; i < num; i++) {
    mjCEquality* equality = (mjCEquality*)model->GetObject(mjOBJ_EQUALITY, i);
    XMLElement*  elem =
        InsertEnd(section, FindValue(equality_map, equality_sz, equality->type).c_str());
    OneEquality(elem, equality, model->def_map[equality->classname]);
  }
}


// deformable section
void mjXWriter::Deformable(XMLElement* root) {
  XMLElement* elem;

  // get sizes
  int nflex = model->NumObjects(mjOBJ_FLEX);
  int nskin = model->NumObjects(mjOBJ_SKIN);

  // return if empty
  if (nflex == 0 && nskin == 0) { return; }

  // create section
  XMLElement* section = InsertEnd(root, "deformable");

  // write flexes
  for (int i = 0; i < nflex; i++) {
    // create element and write
    mjCFlex* flex = (mjCFlex*)model->GetObject(mjOBJ_FLEX, i);
    elem          = InsertEnd(section, "flex");
    OneFlex(elem, flex);
  }

  // write skins
  for (int i = 0; i < nskin; i++) {
    // create element and write
    mjCSkin* skin = (mjCSkin*)model->GetObject(mjOBJ_SKIN, i);
    elem          = InsertEnd(section, "skin");
    OneSkin(elem, skin);
  }
}


// tendon section
void mjXWriter::Tendon(XMLElement* root) {
  // skip section if empty
  int num;
  if ((num = model->NumObjects(mjOBJ_TENDON)) == 0) { return; }

  // create section
  XMLElement* section = InsertEnd(root, "tendon");

  // write all tendons
  for (int i = 0; i < num; i++) {
    // write tendon element and attributes
    mjCTendon* tendon = (mjCTendon*)model->GetObject(mjOBJ_TENDON, i);
    if (!tendon->NumWraps()) {  // SHOULD NOT OCCUR
      continue;
    }
    XMLElement* elem =
        InsertEnd(section, tendon->GetWrap(0)->Type() == mjWRAP_JOINT ? "fixed" : "spatial");
    OneTendon(elem, tendon, model->def_map[tendon->classname]);

    // write wraps
    XMLElement* wrapelem;
    for (int j = 0; j < tendon->NumWraps(); j++) {
      const mjCWrap* wrap = tendon->GetWrap(j);
      switch (wrap->Type()) {
        case mjWRAP_JOINT:
          wrapelem = InsertEnd(elem, "joint");
          WriteAttrTxt(wrapelem, "joint", wrap->obj->name);
          WriteAttr(wrapelem, "coef", 1, &wrap->prm);
          break;

        case mjWRAP_SITE:
          wrapelem = InsertEnd(elem, "site");
          WriteAttrTxt(wrapelem, "site", wrap->obj->name);
          break;

        case mjWRAP_SPHERE:
        case mjWRAP_CYLINDER:
          wrapelem = InsertEnd(elem, "geom");
          WriteAttrTxt(wrapelem, "geom", wrap->obj->name);
          if (!wrap->sidesite.empty()) { WriteAttrTxt(wrapelem, "sidesite", wrap->sidesite); }
          break;

        case mjWRAP_PULLEY:
          wrapelem = InsertEnd(elem, "pulley");
          WriteAttr(wrapelem, "divisor", 1, &wrap->prm);
          break;

        default:
          break;
      }
    }
  }
}


// actuator section
void mjXWriter::Actuator(XMLElement* root) {
  // skip section if empty
  int num;
  if ((num = model->NumObjects(mjOBJ_ACTUATOR)) == 0) { return; }

  // create section
  XMLElement* section = InsertEnd(root, "actuator");

  // write all actuators
  for (int i = 0; i < num; i++) {
    mjCActuator* actuator = (mjCActuator*)model->GetObject(mjOBJ_ACTUATOR, i);
    XMLElement*  elem;
    if (actuator->plugin.active) {
      elem = InsertEnd(section, "plugin");
    } else {
      elem = InsertEnd(section, "general");
    }
    OneActuator(elem, actuator, model->def_map[actuator->classname]);
  }
}


// sensor section
void mjXWriter::Sensor(XMLElement* root) {
  double zero = 0;

  // skip section if empty
  int num;
  if ((num = model->NumObjects(mjOBJ_SENSOR)) == 0) { return; }

  // create section
  XMLElement* section = InsertEnd(root, "sensor");

  // write all sensors
  for (int i = 0; i < num; i++) {
    XMLElement* elem          = 0;
    mjCSensor*  sensor        = model->Sensors()[i];
    string      instance_name = "";
    string      plugin_name   = "";

    // write sensor type and type-specific attributes
    switch (sensor->type) {
      // common robotic sensors, attached to a site
      case mjSENS_TOUCH:
        elem = InsertEnd(section, "touch");
        WriteAttrTxt(elem, "site", sensor->get_objname());
        break;
      case mjSENS_ACCELEROMETER:
        elem = InsertEnd(section, "accelerometer");
        WriteAttrTxt(elem, "site", sensor->get_objname());
        break;
      case mjSENS_VELOCIMETER:
        elem = InsertEnd(section, "velocimeter");
        WriteAttrTxt(elem, "site", sensor->get_objname());
        break;
      case mjSENS_GYRO:
        elem = InsertEnd(section, "gyro");
        WriteAttrTxt(elem, "site", sensor->get_objname());
        break;
      case mjSENS_FORCE:
        elem = InsertEnd(section, "force");
        WriteAttrTxt(elem, "site", sensor->get_objname());
        break;
      case mjSENS_TORQUE:
        elem = InsertEnd(section, "torque");
        WriteAttrTxt(elem, "site", sensor->get_objname());
        break;
      case mjSENS_MAGNETOMETER:
        elem = InsertEnd(section, "magnetometer");
        WriteAttrTxt(elem, "site", sensor->get_objname());
        break;
      case mjSENS_RANGEFINDER: {
        elem = InsertEnd(section, "rangefinder");
        if (sensor->objtype == mjOBJ_SITE) {
          WriteAttrTxt(elem, "site", sensor->get_objname());
        } else {
          WriteAttrTxt(elem, "camera", sensor->get_objname());
        }
        int dataspec = sensor->intprm[0];
        int data[mjNRAYDATA];
        int ndata = 0;
        for (int i = 0; i < mjNRAYDATA; i++) {
          if (dataspec & (1 << i)) { data[ndata++] = i; }
        }
        WriteAttrKeys(elem, "data", raydata_map, mjNRAYDATA, data, ndata, 0);
      } break;
      case mjSENS_CAMPROJECTION:
        elem = InsertEnd(section, "camprojection");
        WriteAttrTxt(elem, "site", sensor->get_objname());
        WriteAttrTxt(elem, "camera", sensor->get_refname());
        break;

      // sensors related to scalar joints, tendons, actuators
      case mjSENS_JOINTPOS:
        elem = InsertEnd(section, "jointpos");
        WriteAttrTxt(elem, "joint", sensor->get_objname());
        break;
      case mjSENS_JOINTVEL:
        elem = InsertEnd(section, "jointvel");
        WriteAttrTxt(elem, "joint", sensor->get_objname());
        break;
      case mjSENS_TENDONPOS:
        elem = InsertEnd(section, "tendonpos");
        WriteAttrTxt(elem, "tendon", sensor->get_objname());
        break;
      case mjSENS_TENDONVEL:
        elem = InsertEnd(section, "tendonvel");
        WriteAttrTxt(elem, "tendon", sensor->get_objname());
        break;
      case mjSENS_ACTUATORPOS:
        elem = InsertEnd(section, "actuatorpos");
        WriteAttrTxt(elem, "actuator", sensor->get_objname());
        break;
      case mjSENS_ACTUATORVEL:
        elem = InsertEnd(section, "actuatorvel");
        WriteAttrTxt(elem, "actuator", sensor->get_objname());
        break;
      case mjSENS_ACTUATORFRC:
        elem = InsertEnd(section, "actuatorfrc");
        WriteAttrTxt(elem, "actuator", sensor->get_objname());
        break;
      case mjSENS_JOINTACTFRC:
        elem = InsertEnd(section, "jointactuatorfrc");
        WriteAttrTxt(elem, "joint", sensor->get_objname());
        break;
      case mjSENS_TENDONACTFRC:
        elem = InsertEnd(section, "tendonactuatorfrc");
        WriteAttrTxt(elem, "tendon", sensor->get_objname());
        break;

      // sensors related to ball joints
      case mjSENS_BALLQUAT:
        elem = InsertEnd(section, "ballquat");
        WriteAttrTxt(elem, "joint", sensor->get_objname());
        break;
      case mjSENS_BALLANGVEL:
        elem = InsertEnd(section, "ballangvel");
        WriteAttrTxt(elem, "joint", sensor->get_objname());
        break;

      // joint and tendon limit sensors
      case mjSENS_JOINTLIMITPOS:
        elem = InsertEnd(section, "jointlimitpos");
        WriteAttrTxt(elem, "joint", sensor->get_objname());
        break;
      case mjSENS_JOINTLIMITVEL:
        elem = InsertEnd(section, "jointlimitvel");
        WriteAttrTxt(elem, "joint", sensor->get_objname());
        break;
      case mjSENS_JOINTLIMITFRC:
        elem = InsertEnd(section, "jointlimitfrc");
        WriteAttrTxt(elem, "joint", sensor->get_objname());
        break;
      case mjSENS_TENDONLIMITPOS:
        elem = InsertEnd(section, "tendonlimitpos");
        WriteAttrTxt(elem, "tendon", sensor->get_objname());
        break;
      case mjSENS_TENDONLIMITVEL:
        elem = InsertEnd(section, "tendonlimitvel");
        WriteAttrTxt(elem, "tendon", sensor->get_objname());
        break;
      case mjSENS_TENDONLIMITFRC:
        elem = InsertEnd(section, "tendonlimitfrc");
        WriteAttrTxt(elem, "tendon", sensor->get_objname());
        break;

      // sensors attached to an object with spatial frame: (x)body, geom, site, camera
      case mjSENS_FRAMEPOS:
        elem = InsertEnd(section, "framepos");
        WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
        WriteAttrTxt(elem, "objname", sensor->get_objname());
        if (sensor->reftype != mjOBJ_UNKNOWN) {
          WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype));
          WriteAttrTxt(elem, "refname", sensor->get_refname());
        }
        break;
      case mjSENS_FRAMEQUAT:
        elem = InsertEnd(section, "framequat");
        WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
        WriteAttrTxt(elem, "objname", sensor->get_objname());
        if (sensor->reftype != mjOBJ_UNKNOWN) {
          WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype));
          WriteAttrTxt(elem, "refname", sensor->get_refname());
        }
        break;
      case mjSENS_FRAMEXAXIS:
        elem = InsertEnd(section, "framexaxis");
        WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
        WriteAttrTxt(elem, "objname", sensor->get_objname());
        if (sensor->reftype != mjOBJ_UNKNOWN) {
          WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype));
          WriteAttrTxt(elem, "refname", sensor->get_refname());
        }
        break;
      case mjSENS_FRAMEYAXIS:
        elem = InsertEnd(section, "frameyaxis");
        WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
        WriteAttrTxt(elem, "objname", sensor->get_objname());
        if (sensor->reftype != mjOBJ_UNKNOWN) {
          WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype));
          WriteAttrTxt(elem, "refname", sensor->get_refname());
        }
        break;
      case mjSENS_FRAMEZAXIS:
        elem = InsertEnd(section, "framezaxis");
        WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
        WriteAttrTxt(elem, "objname", sensor->get_objname());
        if (sensor->reftype != mjOBJ_UNKNOWN) {
          WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype));
          WriteAttrTxt(elem, "refname", sensor->get_refname());
        }
        break;
      case mjSENS_FRAMELINVEL:
        elem = InsertEnd(section, "framelinvel");
        WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
        WriteAttrTxt(elem, "objname", sensor->get_objname());
        if (sensor->reftype != mjOBJ_UNKNOWN) {
          WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype));
          WriteAttrTxt(elem, "refname", sensor->get_refname());
        }
        break;
      case mjSENS_FRAMEANGVEL:
        elem = InsertEnd(section, "frameangvel");
        WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
        WriteAttrTxt(elem, "objname", sensor->get_objname());
        if (sensor->reftype != mjOBJ_UNKNOWN) {
          WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype));
          WriteAttrTxt(elem, "refname", sensor->get_refname());
        }
        break;
      case mjSENS_FRAMELINACC:
        elem = InsertEnd(section, "framelinacc");
        WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
        WriteAttrTxt(elem, "objname", sensor->get_objname());
        if (sensor->reftype != mjOBJ_UNKNOWN) {
          WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype));
          WriteAttrTxt(elem, "refname", sensor->get_refname());
        }
        break;
      case mjSENS_FRAMEANGACC:
        elem = InsertEnd(section, "frameangacc");
        WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
        WriteAttrTxt(elem, "objname", sensor->get_objname());
        if (sensor->reftype != mjOBJ_UNKNOWN) {
          WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype));
          WriteAttrTxt(elem, "refname", sensor->get_refname());
        }
        break;

      // sensors related to kinematic subtrees; attached to a body (which is the subtree root)
      case mjSENS_SUBTREECOM:
        elem = InsertEnd(section, "subtreecom");
        WriteAttrTxt(elem, "body", sensor->get_objname());
        break;
      case mjSENS_SUBTREELINVEL:
        elem = InsertEnd(section, "subtreelinvel");
        WriteAttrTxt(elem, "body", sensor->get_objname());
        break;
      case mjSENS_SUBTREEANGMOM:
        elem = InsertEnd(section, "subtreeangmom");
        WriteAttrTxt(elem, "body", sensor->get_objname());
        break;
      case mjSENS_INSIDESITE:
        elem = InsertEnd(section, "insidesite");
        WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
        WriteAttrTxt(elem, "objname", sensor->get_objname());
        WriteAttrTxt(elem, "site", sensor->get_refname());
        break;
      case mjSENS_GEOMDIST:
        elem = InsertEnd(section, "distance");
        WriteAttrTxt(elem,
                     sensor->objtype == mjOBJ_BODY ? "body1" : "geom1",
                     sensor->get_objname());
        WriteAttrTxt(elem,
                     sensor->reftype == mjOBJ_BODY ? "body2" : "geom2",
                     sensor->get_refname());
        break;
      case mjSENS_GEOMNORMAL:
        elem = InsertEnd(section, "normal");
        WriteAttrTxt(elem,
                     sensor->objtype == mjOBJ_BODY ? "body1" : "geom1",
                     sensor->get_objname());
        WriteAttrTxt(elem,
                     sensor->reftype == mjOBJ_BODY ? "body2" : "geom2",
                     sensor->get_refname());
        break;
      case mjSENS_GEOMFROMTO:
        elem = InsertEnd(section, "fromto");
        WriteAttrTxt(elem,
                     sensor->objtype == mjOBJ_BODY ? "body1" : "geom1",
                     sensor->get_objname());
        WriteAttrTxt(elem,
                     sensor->reftype == mjOBJ_BODY ? "body2" : "geom2",
                     sensor->get_refname());
        break;
      case mjSENS_CONTACT: {
        elem = InsertEnd(section, "contact");
        if (sensor->objtype == mjOBJ_BODY) {
          WriteAttrTxt(elem, "body1", sensor->get_objname());
        } else if (sensor->objtype == mjOBJ_XBODY) {
          WriteAttrTxt(elem, "subtree1", sensor->get_objname());
        } else if (sensor->objtype == mjOBJ_GEOM) {
          WriteAttrTxt(elem, "geom1", sensor->get_objname());
        } else if (sensor->objtype == mjOBJ_SITE) {
          WriteAttrTxt(elem, "site", sensor->get_objname());
        }
        if (sensor->reftype == mjOBJ_BODY) {
          WriteAttrTxt(elem, "body2", sensor->get_refname());
        } else if (sensor->reftype == mjOBJ_XBODY) {
          WriteAttrTxt(elem, "subtree2", sensor->get_refname());
        } else if (sensor->reftype == mjOBJ_GEOM) {
          WriteAttrTxt(elem, "geom2", sensor->get_refname());
        }
        int dataspec = sensor->intprm[0];
        WriteAttrInt(elem, "num", sensor->dim / mju_condataSize(dataspec), 1);
        int data[mjNCONDATA];
        int ndata = 0;
        for (int i = 0; i < mjNCONDATA; i++) {
          if (dataspec & (1 << i)) { data[ndata++] = i; }
        }
        WriteAttrKeys(elem, "data", condata_map, mjNCONDATA, data, ndata, 0);
        WriteAttrKey(elem, "reduce", reduce_map, reduce_sz, sensor->intprm[1], 0);
      } break;
      case mjSENS_TACTILE:
        elem = InsertEnd(section, "tactile");
        WriteAttrTxt(elem, "geom", sensor->get_refname());
        WriteAttrTxt(elem, "mesh", sensor->get_objname());
        break;
      // global sensors
      case mjSENS_E_POTENTIAL:
        elem = InsertEnd(section, "potential");
        break;
      case mjSENS_E_KINETIC:
        elem = InsertEnd(section, "kinetic");
        break;
      case mjSENS_CLOCK:
        elem = InsertEnd(section, "clock");
        break;


      // plugin-controlled sensor
      case mjSENS_PLUGIN:
        elem = InsertEnd(section, "plugin");
        if (sensor->objtype != mjOBJ_UNKNOWN) {
          WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
          WriteAttrTxt(elem, "objname", sensor->get_objname());
        }
        OnePlugin(elem, &sensor->plugin);
        break;

      // user-defined sensor
      case mjSENS_USER:
        elem = InsertEnd(section, "user");
        if (mju_type2Str(sensor->objtype)) {
          WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
        }
        WriteAttrTxt(elem, "objname", sensor->get_objname());
        WriteAttrInt(elem, "dim", sensor->dim);
        WriteAttrKey(elem, "needstage", stage_map, stage_sz, (int)sensor->needstage);
        WriteAttrKey(elem, "datatype", datatype_map, datatype_sz, (int)sensor->datatype);
        break;

      default:
        mju_error("Unknown sensor type in XML write");
    }

    // write name, noise, userdata
    WriteAttrTxt(elem, "name", sensor->name);
    WriteAttr(elem, "cutoff", 1, &sensor->cutoff, &zero);
    if (sensor->type != mjSENS_PLUGIN && sensor->type != mjSENS_TACTILE) {
      WriteAttr(elem, "noise", 1, &sensor->noise, &zero);
    }
    WriteAttrInt(elem, "nsample", sensor->nsample, 0);
    WriteAttrKey(elem, "interp", interp_map, interp_sz, sensor->interp, 0);
    WriteAttr(elem, "delay", 1, &sensor->delay, &zero);
    double zeros[2] = {0, 0};
    WriteAttr(elem, "interval", 2, sensor->interval, zeros);
    WriteVector(elem, "user", sensor->get_userdata());
  }

  // remove section if empty
  if (!section->FirstChildElement()) { root->DeleteChild(section); }
}


// keyframe section
void mjXWriter::Keyframe(XMLElement* root) {
  // create section
  XMLElement* section = InsertEnd(root, "keyframe");

  if (!model->key_pending_.empty()) {
    throw mjXError(0, "Model has pending keyframes. It must be (re)compiled before writing XML.");
  }

  // write all keyframes
  for (int i = 0; i < model->nkey; i++) {
    XMLElement* elem   = InsertEnd(section, "key");
    bool        change = false;

    mjCKey* key = model->Keys()[i];

    // check name and write
    if (!key->name.empty()) {
      WriteAttrTxt(elem, "name", key->name);
      change = true;
    }

    // check time and write
    if (key->time != 0) {
      WriteAttr(elem, "time", 1, &key->time);
      change = true;
    }

    // check qpos and write
    for (int j = 0; j < model->nq; j++) {
      if (key->qpos_[j] != model->qpos0[j]) {
        WriteAttr(elem, "qpos", model->nq, key->qpos_.data());
        change = true;
        break;
      }
    }

    // check qvel and write
    for (int j = 0; j < model->nv; j++) {
      if (key->qvel_[j] != 0) {
        WriteAttr(elem, "qvel", model->nv, key->qvel_.data());
        change = true;
        break;
      }
    }

    // check act and write
    for (int j = 0; j < model->na; j++) {
      if (key->act_[j] != 0) {
        WriteAttr(elem, "act", model->na, key->act_.data());
        change = true;
        break;
      }
    }

    // check mpos and write
    if (model->nmocap) {
      for (int j = 0; j < model->nbody; j++) {
        if (model->Bodies()[j]->mocap) {
          mjCBody* body = model->Bodies()[j];
          int      id   = body->mocapid;
          if (body->pos[0] != key->mpos_[3 * id] ||
              body->pos[1] != key->mpos_[3 * id + 1] ||
              body->pos[2] != key->mpos_[3 * id + 2]) {
            WriteAttr(elem, "mpos", 3 * model->nmocap, key->mpos_.data());
            change = true;
            break;
          }
        }
      }
    }

    // check mquat and write
    if (model->nmocap) {
      for (int j = 0; j < model->nbody; j++) {
        if (model->Bodies()[j]->mocap) {
          mjCBody* body = model->Bodies()[j];
          int      id   = body->mocapid;
          if (body->quat[0] != key->mquat_[4 * id] ||
              body->quat[1] != key->mquat_[4 * id + 1] ||
              body->quat[2] != key->mquat_[4 * id + 2] ||
              body->quat[3] != key->mquat_[4 * id + 3]) {
            WriteAttr(elem, "mquat", 4 * model->nmocap, key->mquat_.data());
            change = true;
            break;
          }
        }
      }
    }

    // check ctrl and write
    for (int j = 0; j < model->nu; j++) {
      if (key->ctrl_[j] != 0) {
        WriteAttr(elem, "ctrl", model->nu, key->ctrl_.data());
        change = true;
        break;
      }
    }

    // remove elem if empty
    if (!change) { section->DeleteChild(elem); }
  }

  // remove section if empty
  if (!section->FirstChildElement()) { root->DeleteChild(section); }
}