Initial open sourcing of MuJoCo.

PiperOrigin-RevId: 450374687
Change-Id: Ie3225a46ce095fc28ae8e63c326a640261f562bb

src/xml/CMakeLists.txt

@@ -0,0 +1,34 @@
+# 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
+#
+#     https://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.
+
+set(MUJOCO_XML_SRCS
+    xml_api.cc
+    xml_api.h
+    xml_base.cc
+    xml_base.h
+    xml.cc
+    xml.h
+    xml_native_reader.cc
+    xml_native_reader.h
+    xml_numeric_format.cc
+    xml_numeric_format.h
+    xml_native_writer.cc
+    xml_native_writer.h
+    xml_urdf.cc
+    xml_urdf.h
+    xml_util.cc
+    xml_util.h
+)
+
+target_sources(mujoco PRIVATE ${MUJOCO_XML_SRCS})

src/xml/xml.cc

@@ -0,0 +1,321 @@
+// 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.h"
+
+#include <locale.h>
+
+#if defined(__APPLE__) || defined(__FreeBSD__)
+#include <xlocale.h>
+#endif
+
+#include <string>
+
+#include "cc/array_safety.h"
+#include "engine/engine_crossplatform.h"
+#include "engine/engine_vfs.h"
+#include "user/user_model.h"
+#include "user/user_util.h"
+#include "xml/xml_native_reader.h"
+#include "xml/xml_native_writer.h"
+#include "xml/xml_urdf.h"
+#include "xml/xml_util.h"
+#include "tinyxml2.h"
+
+namespace {
+
+using std::string;
+using std::vector;
+using tinyxml2::XMLDocument;
+using tinyxml2::XMLElement;
+using tinyxml2::XMLNode;
+
+namespace mju = ::mujoco::util;
+
+// We are using "locale-sensitive" sprintf to read and write XML.
+// When MuJoCo is being used as a plug-in for an application that respects the system locale
+// (e.g. Unity), the user's locale setting can affect the formatting of numbers into strings.
+// Specifically, a number of European locales (e.g. de_DE) uses commas to as decimal separators.
+// In order to ensure that XMLs are locale-inpendent, we temporarily switch to the "C" locale
+// when handling. Since the standard C `setlocale` is not thread-safe, we instead use
+// platform-specific extensions to override the locale only in the calling thread.
+// See also https://github.com/deepmind/mujoco/issues/131.
+#ifdef _WIN32
+class LocaleOverride {
+ public:
+  LocaleOverride()
+      : old_per_thread_locale_type_(_configthreadlocale(0)),
+        old_locale_(setlocale(LC_ALL, nullptr)) {
+    _configthreadlocale(_ENABLE_PER_THREAD_LOCALE);
+    setlocale(LC_ALL, "C");
+  }
+
+  ~LocaleOverride() {
+    setlocale(LC_ALL, old_locale_);
+    _configthreadlocale(old_per_thread_locale_type_);
+  }
+
+ private:
+  int old_per_thread_locale_type_;
+  char* old_locale_;
+};
+#else
+class LocaleOverride {
+ public:
+  static locale_t PosixLocale() {
+    static locale_t posix_locale = newlocale(LC_ALL_MASK, "C", 0);
+    return posix_locale;
+  }
+
+  LocaleOverride() : old_locale_(uselocale(PosixLocale())) {}
+
+  ~LocaleOverride() {
+    uselocale(old_locale_);
+  }
+
+ private:
+  locale_t old_locale_;
+};
+#endif
+
+}  // namespace
+
+// Main writer function - calls mjXWrite
+bool mjWriteXML(mjCModel* model, string filename, char* error, int error_sz) {
+  LocaleOverride locale_override;
+
+  // check for empty model
+  if (!model) {
+    mjCopyError(error, "Cannot write empty model", error_sz);
+    return false;
+  }
+
+  // write
+  FILE* fp = fopen(filename.c_str(), "w");
+  if (!fp) {
+    mjCopyError(error, "File not found", error_sz);
+    return false;
+  }
+
+  try {
+    mjXWriter writer;
+    writer.SetModel(model);
+    writer.Write(fp);
+  }
+
+  // catch known errors
+  catch (mjXError err) {
+    mjCopyError(error, err.message, error_sz);
+    fclose(fp);
+    return false;
+  }
+
+  fclose(fp);
+  return true;
+}
+
+
+
+// find include elements recursively, replace them with subtree from xml file
+static XMLElement* mjIncludeXML(XMLElement* elem, string dir,
+                                const mjVFS* vfs, vector<string>& included) {
+  // include element: process
+  if (!strcasecmp(elem->Value(), "include")) {
+    // make sure include has no children
+    if (!elem->NoChildren()) {
+      throw mjXError(elem, "Include element cannot have children");
+    }
+
+    // get filename
+    string filename;
+    mjXUtil::ReadAttrTxt(elem, "file", filename, true);
+    filename = dir + filename;
+
+    // block repeated include files
+    for (size_t i=0; i<included.size(); i++) {
+      if (!strcasecmp(included[i].c_str(), filename.c_str())) {
+        throw mjXError(elem, "File '%s' already included", filename.c_str());
+      }
+    }
+
+    // get data source
+    const char* xmlstring = 0;
+    int buffer_size = 0;
+    if (vfs) {
+      int id = mj_findFileVFS(vfs, filename.c_str());
+      if (id>=0) {
+        xmlstring = (const char*)vfs->filedata[id];
+        buffer_size = vfs->filesize[id];
+      }
+    }
+
+    // load XML file or parse string
+    XMLDocument doc;
+    if (xmlstring) {
+      doc.Parse(xmlstring, buffer_size);
+    } else {
+      doc.LoadFile(filename.c_str());
+    }
+
+    // check error
+    if (doc.Error()) {
+      char err[1000];
+      mju::sprintf_arr(err, "XML parse error %d:\n%s\n", doc.ErrorID(), doc.ErrorStr());
+      throw mjXError(elem, "Include error: '%s'", err);
+    }
+
+    // remember that file was included
+    included.push_back(filename);
+
+    // get and check root element
+    XMLElement* docroot = doc.RootElement();
+    if (!docroot) {
+      throw mjXError(elem, "Root element missing in file '%s'", filename.c_str());
+    }
+
+    // get and check first child
+    XMLElement* eleminc = docroot->FirstChildElement();
+    if (!eleminc) {
+      throw mjXError(elem, "Empty include file '%s'", filename.c_str());
+    }
+
+    // get parent of <include>
+    XMLElement* parent = (XMLElement*)elem->Parent();
+
+    // clone first child of included document, insert it after <include>
+    XMLNode* first = parent->InsertAfterChild(elem, eleminc->DeepClone(parent->GetDocument()));
+
+    // delete <include> element, point to first
+    parent->DeleteChild(elem);
+    elem = first->ToElement();
+
+    // insert remaining elements from included document as siblings
+    eleminc = eleminc->NextSiblingElement();
+    while (eleminc) {
+      elem = (XMLElement*)parent->InsertAfterChild(elem, eleminc->DeepClone(parent->GetDocument()));
+      eleminc = eleminc->NextSiblingElement();
+    }
+
+    // run XMLInclude on first new child
+    return mjIncludeXML(first->ToElement(), dir, vfs, included);
+  }
+
+  // otherwise check all child elements, return self
+  else {
+    XMLElement* child = elem->FirstChildElement();
+    while (child) {
+      child = mjIncludeXML(child, dir, vfs, included);
+      if (child) {
+        child = child->NextSiblingElement();
+      }
+    }
+
+    return elem;
+  }
+}
+
+
+
+// Main parser function: from file or VFS
+mjCModel* mjParseXML(const char* filename, const mjVFS* vfs, char* error, int error_sz) {
+  LocaleOverride locale_override;
+
+  // check arguments
+  if (!filename) {
+    if (error) {
+      snprintf(error, error_sz, "mjParseXML: filename argument required\n");
+    }
+    return 0;
+  }
+
+  // clear
+  mjCModel* model = 0;
+  if (error) {
+    error[0] = 0;
+  }
+
+  // get data source
+  const char* xmlstring = 0;
+  int buffer_size = 0;
+  if (vfs) {
+    int id = mj_findFileVFS(vfs, filename);
+    if (id>=0) {
+      xmlstring = (const char*)vfs->filedata[id];
+      buffer_size = vfs->filesize[id];
+    }
+  }
+
+  // load XML file or parse string
+  XMLDocument doc;
+  if (xmlstring) {
+    doc.Parse(xmlstring, buffer_size);
+  } else {
+    doc.LoadFile(filename);
+  }
+
+  // error checking
+  if (doc.Error()) {
+    if (error) {
+      snprintf(error, error_sz, "XML parse error %d:\n%s\n",
+               doc.ErrorID(), doc.ErrorStr());
+    }
+    return 0;
+  }
+
+  // get top-level element
+  XMLElement* root = doc.RootElement();
+  if (!root) {
+    mjCopyError(error, "XML root element not found", error_sz);
+    return 0;
+  }
+
+  // create model, set filedir
+  model = new mjCModel;
+  model->modelfiledir = mjuu_getfiledir(filename);
+
+  // parse with exceptions
+  try {
+    if (!strcasecmp(root->Value(), "mujoco")) {
+      // find include elements, replace them with subtree from xml file
+      vector<string> included;
+      included.push_back(filename);
+      mjIncludeXML(root, model->modelfiledir, vfs, included);
+
+      // parse MuJoCo model
+      mjXReader parser;
+      parser.SetModel(model);
+      parser.Parse(root);
+    }
+
+    else if (!strcasecmp(root->Value(), "robot")) {
+      // parse URDF model
+      mjXURDF parser;
+      parser.SetModel(model);
+      parser.Parse(root);
+    }
+
+    else {
+      throw mjXError(0, "Unrecognized XML model type: '%s'", root->Value());
+    }
+  }
+
+  // catch known errors
+  catch (mjXError err) {
+    mjCopyError(error, err.message, error_sz);
+    delete model;
+    return 0;
+  }
+
+  return model;
+}

src/xml/xml.h

@@ -0,0 +1,32 @@
+// 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.
+
+#ifndef MUJOCO_SRC_XML_XML_H_
+#define MUJOCO_SRC_XML_XML_H_
+
+#include <string>
+
+#include <mujoco/mjmodel.h>
+#include "user/user_model.h"
+
+// Top level API
+
+// Main writer function
+bool mjWriteXML(mjCModel* model, std::string filename, char* error, int error_sz);
+
+// Main parser function: from file or VFS
+mjCModel* mjParseXML(const char* filename, const mjVFS* vfs, char* error, int error_sz);
+
+
+#endif  // MUJOCO_SRC_XML_XML_H_

src/xml/xml_api.cc

@@ -0,0 +1,182 @@
+// 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_api.h"
+
+#include <cstring>
+#include <fstream>
+#include <iostream>
+#include <mutex>
+#include <random>
+
+#include "user/user_model.h"
+#include "xml/xml.h"
+#include "xml/xml_native_reader.h"
+#include "xml/xml_util.h"
+
+//---------------------------------- Globals -------------------------------------------------------
+
+// global user model class
+class GlobalModel {
+ public:
+  GlobalModel();
+  ~GlobalModel();
+  void Clear(void);
+
+  mjCModel* model;
+};
+
+
+GlobalModel::GlobalModel() {
+  // clear pointers
+  model = 0;
+}
+
+
+GlobalModel::~GlobalModel() {
+  Clear();
+}
+
+
+void GlobalModel::Clear() {
+  // de-allocate models
+  if (model) {
+    delete model;
+  }
+
+  // clear pointers
+  model = 0;
+}
+
+
+
+// single instance of global model, protected with mutex
+GlobalModel themodel;
+std::mutex themutex;
+
+
+//---------------------------------- Functions -----------------------------------------------------
+
+// Return 1 (for backward compatibility).
+int mj_activate(const char* filename) {
+  return 1;
+}
+
+
+
+// Do nothing (for backward compatibility).
+void mj_deactivate(void) {
+}
+
+
+
+// parse XML file in MJCF or URDF format, compile it, return low-level model
+//  if vfs is not NULL, look up files in vfs before reading from disk
+//  error can be NULL; otherwise assumed to have size error_sz
+mjModel* mj_loadXML(const char* filename, const mjVFS* vfs,
+                    char* error, int error_sz) {
+  // serialize access to themodel
+  std::lock_guard<std::mutex> lock(themutex);
+
+  // parse new model
+  mjCModel* newmodel = mjParseXML(filename, vfs, error, error_sz);
+  if (!newmodel) {
+    return 0;
+  }
+
+  // compile new model
+  mjModel* m = newmodel->Compile(vfs);
+  if (!m) {
+    mjCopyError(error, newmodel->GetError().message, error_sz);
+    delete newmodel;
+    return 0;
+  }
+
+  // clear old and assign new
+  themodel.Clear();
+  themodel.model = newmodel;
+
+  // handle compile warning
+  if (themodel.model->GetError().warning) {
+    mjCopyError(error, themodel.model->GetError().message, error_sz);
+  } else if (error) {
+    error[0] = 0;
+  }
+
+  return m;
+}
+
+
+
+
+// update XML data structures with info from low-level model, save as MJCF
+int mj_saveLastXML(const char* filename, const mjModel* m, char* error, int error_sz) {
+  // serialize access to themodel
+  std::lock_guard<std::mutex> lock(themutex);
+
+  if (!themodel.model) {
+    mjCopyError(error, "No XML model loaded", error_sz);
+    return 0;
+  }
+
+  themodel.model->CopyBack(m);
+  if (mjWriteXML(themodel.model, filename, error, error_sz)) {
+    if (error) {
+      error[0] = 0;
+    }
+    return 1;
+  } else {
+    return 0;
+  }
+}
+
+
+// free last XML
+void mj_freeLastXML(void) {
+  // serialize access to themodel
+  std::lock_guard<std::mutex> lock(themutex);
+
+  themodel.Clear();
+}
+
+
+
+
+// print internal XML schema as plain text or HTML, with style-padding or &nbsp;
+int mj_printSchema(const char* filename, char* buffer, int buffer_sz, int flg_html, int flg_pad) {
+  // serialize access, even though it is not necessary
+  std::lock_guard<std::mutex> lock(themutex);
+
+  // print to stringstream
+  mjXReader reader;
+  std::stringstream str;
+  reader.PrintSchema(str, flg_html!=0, flg_pad!=0);
+
+  // filename given: write to file
+  if (filename) {
+    std::ofstream file;
+    file.open(filename);
+    file << str.str();
+    file.close();
+  }
+
+  // buffer given: write to buffer
+  if (buffer && buffer_sz) {
+    strncpy(buffer, str.str().c_str(), buffer_sz);
+    buffer[buffer_sz-1] = 0;
+  }
+
+  // return string length
+  return str.str().size();
+}

src/xml/xml_api.h

@@ -0,0 +1,52 @@
+// 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.
+
+#ifndef MUJOCO_SRC_XML_XML_API_H_
+#define MUJOCO_SRC_XML_XML_API_H_
+
+#include <mujoco/mjexport.h>
+#include <mujoco/mjmodel.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+// activate license, call mju_error on failure; return 1 if ok, 0 if failure
+MJAPI int mj_activate(const char* filename);
+
+// deactivate license, free memory
+MJAPI void mj_deactivate(void);
+
+// parse XML file in MJCF or URDF format, compile it, return low-level model
+//  if vfs is not NULL, look up files in vfs before reading from disk
+//  error can be NULL; otherwise assumed to have size error_sz
+MJAPI mjModel* mj_loadXML(const char* filename, const mjVFS* vfs, char* error, int error_sz);
+
+// update XML data structures with info from low-level model, save as MJCF
+MJAPI int mj_saveLastXML(const char* filename, const mjModel* m, char* error, int error_sz);
+
+// free last XML model if loaded; called internally at each load
+MJAPI void mj_freeLastXML(void);
+
+// print internal XML schema as plain text or HTML, with style-padding or &nbsp;
+MJAPI int mj_printSchema(const char* filename, char* buffer, int buffer_sz,
+                         int flg_html, int flg_pad);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // MUJOCO_SRC_XML_XML_API_H_

src/xml/xml_base.cc

@@ -0,0 +1,60 @@
+// 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_base.h"
+
+#include <cfloat>
+#include <cstddef>
+#include <iostream>
+#include <sstream>
+#include <string>
+#include <vector>
+
+#include "user/user_model.h"
+#include "user/user_objects.h"
+#include "tinyxml2.h"
+
+namespace {
+
+using std::string;
+using tinyxml2::XMLElement;
+
+}  // namespace
+
+
+//--------------------------------- Base class, helper functions -----------------------------------
+
+// base constructor
+mjXBase::mjXBase() {
+  model = NULL;
+}
+
+
+
+// set model field
+void mjXBase::SetModel(mjCModel* _model) {
+  model = _model;
+}
+
+
+
+// read alternative orientation specification
+void mjXBase::ReadAlternative(XMLElement* elem, mjCAlternative& alt) {
+  string text;
+  ReadAttr(elem, "axisangle", 4, alt.axisangle, text);
+  ReadAttr(elem, "xyaxes", 6, alt.xyaxes, text);
+  ReadAttr(elem, "zaxis", 3, alt.zaxis, text);
+  ReadAttr(elem, "euler", 3, alt.euler, text);
+  ReadAttr(elem, "fullinertia", 6, alt.fullinertia, text);
+}

src/xml/xml_base.h

@@ -0,0 +1,95 @@
+// 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.
+
+#ifndef MUJOCO_SRC_XML_XML_BASE_H_
+#define MUJOCO_SRC_XML_XML_BASE_H_
+
+#include <string>
+
+#include "tinyxml2.h"
+#include <mujoco/mjmodel.h>
+#include "user/user_model.h"
+#include "user/user_objects.h"
+#include "xml/xml_util.h"
+
+
+// keyword maps (defined in implementation files)
+extern const int joint_sz;
+extern const int camlight_sz;
+extern const int light_sz;
+extern const int integrator_sz;
+extern const int collision_sz;
+extern const int cone_sz;
+extern const int jac_sz;
+extern const int solver_sz;
+extern const int equality_sz;
+extern const int texture_sz;
+extern const int builtin_sz;
+extern const int mark_sz;
+extern const int dyn_sz;
+extern const int gain_sz;
+extern const int bias_sz;
+extern const int stage_sz;
+extern const int datatype_sz;
+extern const mjMap coordinate_map[];
+extern const mjMap angle_map[];
+extern const mjMap enable_map[];
+extern const mjMap bool_map[];
+extern const mjMap TFAuto_map[];
+extern const mjMap joint_map[];
+extern const mjMap geom_map[];
+extern const mjMap camlight_map[];
+extern const mjMap light_map[];
+extern const mjMap integrator_map[];
+extern const mjMap collision_map[];
+extern const mjMap impedance_map[];
+extern const mjMap reference_map[];
+extern const mjMap cone_map[];
+extern const mjMap jac_map[];
+extern const mjMap solver_map[];
+extern const mjMap equality_map[];
+extern const mjMap texture_map[];
+extern const mjMap builtin_map[];
+extern const mjMap mark_map[];
+extern const mjMap dyn_map[];
+extern const mjMap gain_map[];
+extern const mjMap bias_map[];
+extern const mjMap stage_map[];
+extern const mjMap datatype_map[];
+
+
+//---------------------------------- Base XML class ------------------------------------------------
+
+class mjXBase : public mjXUtil {
+ public:
+  mjXBase();
+  virtual ~mjXBase() = default;
+
+  // parse: implemented in derived parser classes
+  virtual void Parse(tinyxml2::XMLElement* root) {};
+
+  // write: implemented in derived writer class
+  virtual void Write(FILE* fp) {};
+
+  // set the model allocated externally
+  void SetModel(mjCModel*);
+
+  // read alternative orientation specification
+  static void ReadAlternative(tinyxml2::XMLElement* elem, mjCAlternative& alt);
+
+ protected:
+  mjCModel* model;                    // internally-allocated mjCModel object
+};
+
+#endif  // MUJOCO_SRC_XML_XML_BASE_H_

src/xml/xml_native_reader.h

@@ -0,0 +1,74 @@
+// 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.
+
+#ifndef MUJOCO_SRC_XML_XML_NATIVE_READER_H_
+#define MUJOCO_SRC_XML_XML_NATIVE_READER_H_
+
+#include <sstream>
+
+#include "user/user_model.h"
+#include "xml/xml_base.h"
+#include "tinyxml2.h"
+
+class mjXReader : public mjXBase {
+ public:
+  mjXReader();                                                         // constructor
+  virtual ~mjXReader() = default;                                      // destructor
+
+  void Parse(tinyxml2::XMLElement* root);                              // parse XML document
+  void PrintSchema(std::stringstream& str, bool html, bool pad);       // print text or HTML schema
+
+  // XML sections embedded in all formats
+  static void Compiler(tinyxml2::XMLElement* section, mjCModel* mod);  // compiler section
+  static void Option(tinyxml2::XMLElement* section, mjOption* opt);    // option section
+  static void Size(tinyxml2::XMLElement* section, mjCModel* mod);      // size section
+
+ private:
+  // XML section specific to MJCF
+  void Default(tinyxml2::XMLElement* section,   int parentid);         // default section
+  void Custom(tinyxml2::XMLElement* section);                          // custom section
+  void Visual(tinyxml2::XMLElement* section);                          // visual section
+  void Statistic(tinyxml2::XMLElement* section);                       // statistic section
+  void Asset(tinyxml2::XMLElement* section);                           // asset section
+  void Body(tinyxml2::XMLElement* section, mjCBody* pbody);            // body/world section
+  void Contact(tinyxml2::XMLElement* section);                         // contact section
+  void Equality(tinyxml2::XMLElement* section);                        // equality section
+  void Tendon(tinyxml2::XMLElement* section);                          // tendon section
+  void Actuator(tinyxml2::XMLElement* section);                        // actuator section
+  void Sensor(tinyxml2::XMLElement* section);                          // sensor section
+  void Keyframe(tinyxml2::XMLElement* section);                        // keyframe section
+
+  // single element parsers, used in defaults and main body
+  void OneMesh(tinyxml2::XMLElement* elem, mjCMesh* pmesh);
+  void OneSkin(tinyxml2::XMLElement* elem, mjCSkin* pskin);
+  void OneMaterial(tinyxml2::XMLElement* elem, mjCMaterial* pmaterial);
+  void OneJoint(tinyxml2::XMLElement* elem, mjCJoint* pjoint);
+  void OneGeom(tinyxml2::XMLElement* elem, mjCGeom* pgeom);
+  void OneSite(tinyxml2::XMLElement* elem, mjCSite* psite);
+  void OneCamera(tinyxml2::XMLElement* elem, mjCCamera* pcamera);
+  void OneLight(tinyxml2::XMLElement* elem, mjCLight* plight);
+  void OnePair(tinyxml2::XMLElement* elem, mjCPair* ppair);
+  void OneEquality(tinyxml2::XMLElement* elem, mjCEquality* pequality);
+  void OneTendon(tinyxml2::XMLElement* elem, mjCTendon* ptendon);
+  void OneActuator(tinyxml2::XMLElement* elem, mjCActuator* pactuator);
+  void OneComposite(tinyxml2::XMLElement* elem, mjCBody* pbody, mjCDef* def);
+
+  mjXSchema schema;                                                   // schema used for validation
+  mjCDef* GetClass(tinyxml2::XMLElement* section);                    // get default class name
+  static void GetXMLPos(tinyxml2::XMLElement* elem, mjCBase* obj);    // get xml position
+
+  bool readingdefaults; // true while reading defaults
+};
+
+#endif  // MUJOCO_SRC_XML_XML_NATIVE_READER_H_

src/xml/xml_native_writer.h

@@ -0,0 +1,67 @@
+// 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.
+
+#ifndef MUJOCO_SRC_XML_XML_NATIVE_WRITER_H_
+#define MUJOCO_SRC_XML_XML_NATIVE_WRITER_H_
+
+#include <string>
+
+#include "xml/xml_base.h"
+#include "tinyxml2.h"
+
+class mjXWriter : public mjXBase {
+ public:
+  mjXWriter();                                        // constructor
+  virtual ~mjXWriter() = default;                     // destructor
+  void Write(FILE* fp);                               // write XML document
+
+ private:
+  // insert end child with given name, return child
+  tinyxml2::XMLElement* InsertEnd(tinyxml2::XMLElement* parent, const char* name);
+
+  // XML section writers
+  void Compiler(tinyxml2::XMLElement* root);                    // compiler section
+  void Option(tinyxml2::XMLElement* root);                      // option section
+  void Size(tinyxml2::XMLElement* root);                        // size section
+  void Visual(tinyxml2::XMLElement* root);                      // visual section
+  void Statistic(tinyxml2::XMLElement* root);                   // statistic section
+  void Default(tinyxml2::XMLElement* root, mjCDef* def);        // default section
+  void Custom(tinyxml2::XMLElement* root);                      // custom section
+  void Asset(tinyxml2::XMLElement* root);                       // asset section
+  void Body(tinyxml2::XMLElement* elem, mjCBody* body);         // body/world section
+  void Contact(tinyxml2::XMLElement* root);                     // contact section
+  void Equality(tinyxml2::XMLElement* root);                    // equality constraint section
+  void Tendon(tinyxml2::XMLElement* root);                      // tendon section
+  void Actuator(tinyxml2::XMLElement* root);                    // actuator section
+  void Sensor(tinyxml2::XMLElement* root);                      // sensor section
+  void Keyframe(tinyxml2::XMLElement* root);                    // keyframe section
+
+  // single element writers, used in defaults and main body
+  void OneMesh(tinyxml2::XMLElement* elem,      mjCMesh* pmesh,         mjCDef* def);
+  void OneSkin(tinyxml2::XMLElement* elem,      mjCSkin* pskin);
+  void OneMaterial(tinyxml2::XMLElement* elem,  mjCMaterial* pmaterial, mjCDef* def);
+  void OneJoint(tinyxml2::XMLElement* elem,     mjCJoint* pjoint,       mjCDef* def);
+  void OneGeom(tinyxml2::XMLElement* elem,      mjCGeom* pgeom,         mjCDef* def);
+  void OneSite(tinyxml2::XMLElement* elem,      mjCSite* psite,         mjCDef* def);
+  void OneCamera(tinyxml2::XMLElement* elem,    mjCCamera* pcamera,     mjCDef* def);
+  void OneLight(tinyxml2::XMLElement* elem,     mjCLight* plight,       mjCDef* def);
+  void OnePair(tinyxml2::XMLElement* elem,      mjCPair* ppair,         mjCDef* def);
+  void OneEquality(tinyxml2::XMLElement* elem,  mjCEquality* pequality, mjCDef* def);
+  void OneTendon(tinyxml2::XMLElement* elem,    mjCTendon* ptendon,     mjCDef* def);
+  void OneActuator(tinyxml2::XMLElement* elem,  mjCActuator* pactuator, mjCDef* def);
+
+  bool writingdefaults;                       // true during defaults write
+};
+
+#endif  // MUJOCO_SRC_XML_XML_NATIVE_WRITER_H_

src/xml/xml_numeric_format.cc

@@ -0,0 +1,31 @@
+// 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_numeric_format.h"
+
+namespace mujoco {
+
+namespace {
+  thread_local const char* precision = "%g";
+}
+
+const char* _mjPRIVATE__get_xml_precision() {
+  return precision;
+}
+
+void _mjPRIVATE__set_xml_precision(const char* new_precision) {
+  precision = new_precision;
+}
+
+}  // namespace mujoco

src/xml/xml_numeric_format.h

@@ -0,0 +1,37 @@
+// 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.
+
+#ifndef MUJOCO_SRC_XML_XML_NUMERIC_FORMAT_H_
+#define MUJOCO_SRC_XML_XML_NUMERIC_FORMAT_H_
+
+#include <mujoco/mjexport.h>
+
+namespace mujoco {
+
+extern "C" {
+  MJAPI const char* _mjPRIVATE__get_xml_precision();
+  MJAPI void _mjPRIVATE__set_xml_precision(const char* precision);
+}
+
+// Full precision printing of floating point numbers in saved XMLs, useful for testing
+class FullFloatPrecision {
+ public:
+  FullFloatPrecision() { _mjPRIVATE__set_xml_precision("%.17g");}
+  ~FullFloatPrecision() { _mjPRIVATE__set_xml_precision("%g");}
+};
+
+}  // namespace mujoco
+
+#endif  // MUJOCO_SRC_XML_NUMERIC_FORMAT_H_
+

src/xml/xml_urdf.cc

@@ -0,0 +1,640 @@
+// 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 <cstring>
+#include <string>
+#include <vector>
+
+#include <mujoco/mjmodel.h>
+#include "user/user_model.h"
+#include "user/user_objects.h"
+#include "user/user_util.h"
+#include "xml/xml_native_reader.h"
+#include "xml/xml_urdf.h"
+
+#include "tinyxml2.h"
+
+using tinyxml2::XMLElement;
+
+// URDF joint type
+static const int urJoint_sz = 6;
+static const mjMap urJoint_map[urJoint_sz] = {
+  {"revolute",    0},
+  {"continuous",  1},
+  {"prismatic",   2},
+  {"fixed",       3},
+  {"floating",    4},
+  {"planar",      5}
+};
+
+
+
+//---------------------------------- class mjXURDF -------------------------------------------------
+
+// constructor
+mjXURDF::mjXURDF() {
+  Clear();
+}
+
+
+
+// destructor
+mjXURDF::~mjXURDF() {
+  Clear();
+}
+
+
+// clear internal variables
+void mjXURDF::Clear(void) {
+  model = 0;
+
+  urName.clear();
+  urParent.clear();
+  urChildren.clear();
+  urMat.clear();
+  urRGBA.clear();
+}
+
+
+
+// actual parser
+void mjXURDF::Parse(XMLElement* root) {
+  std::string name, text;
+  XMLElement *elem, *temp;
+  int id_parent, id_child, i;
+
+  // set compiler defaults suitable for URDF
+  model->strippath = true;
+  model->discardvisual = true;
+  model->fusestatic = true;
+
+  // parse MuJoCo sections (not part of URDF)
+  XMLElement* mjc = FindSubElem(root, "mujoco");
+  if (mjc) {
+    XMLElement *section;
+    if ((section = FindSubElem(mjc, "compiler"))) {
+      mjXReader::Compiler(section, model);
+    }
+
+    if ((section = FindSubElem(mjc, "option"))) {
+      mjXReader::Option(section, &model->option);
+    }
+
+    if ((section = FindSubElem(mjc, "size"))) {
+      mjXReader::Size(section, model);
+    }
+  }
+
+  // enfore required compiler defaults for URDF
+  model->global = false;
+  model->degree = false;
+
+  // get model name
+  ReadAttrTxt(root, "name", model->modelname);
+
+  // find and register all materials
+  MakeMaterials(root);
+
+  // find all links/bodies, save names
+  elem = root->FirstChildElement();
+  while (elem) {
+    // identify link elements
+    name = elem->Value();
+    if (name=="link") {
+      ReadAttrTxt(elem, "name", text, true);
+      AddBody(text);
+    }
+
+    // advance to next element
+    elem = elem->NextSiblingElement();
+  }
+
+  // find all joints, assign parent and child pointers
+  elem = root->FirstChildElement();
+  while (elem) {
+    // identify joint elements
+    name = elem->Value();
+    if (name=="joint") {
+      // find parent, get name and id
+      temp = FindSubElem(elem, "parent", true);
+      ReadAttrTxt(temp, "link", text, true);
+      id_parent = FindName(text, urName);
+
+      // find child, get name and id
+      temp = FindSubElem(elem, "child", true);
+      ReadAttrTxt(temp, "link", text, true);
+      id_child = FindName(text, urName);
+
+      // make sure parent and child exist
+      if (id_parent<0 || id_child<0) {
+        throw mjXError(elem, "URDF joint parent or child missing");
+      }
+
+      // check for multiple parents
+      if (urParent[id_child]>=0) {
+        throw mjXError(elem, "URDF body has multiple parents:", urName[id_child].c_str());
+      }
+
+      // add parent and child info
+      urParent[id_child] = id_parent;
+      urChildren[id_parent].push_back(id_child);
+    }
+
+    // advance to next element
+    elem = elem->NextSiblingElement();
+  }
+
+  // find all top-level bodies, call recursive tree constructor
+  for (i=0; i<(int)urName.size(); i++) {
+    if (urParent[i] < 0) {
+      AddToTree(i);
+    }
+  }
+
+  // parse bodies
+  elem = root->FirstChildElement();
+  while (elem) {
+    // identify body/link elements
+    name = elem->Value();
+    if (name=="link") {
+      Body(elem);
+    }
+
+    // advance to next element
+    elem = elem->NextSiblingElement();
+  }
+
+  // parse joints
+  elem = root->FirstChildElement();
+  while (elem) {
+    // identify body/link elements
+    name = elem->Value();
+    if (name=="joint") {
+      Joint(elem);
+    }
+
+    // advance to next element
+    elem = elem->NextSiblingElement();
+  }
+}
+
+
+
+// parse body/link
+void mjXURDF::Body(XMLElement* body_elem) {
+  std::string name, text;
+  XMLElement *elem, *temp, *temp1;
+  mjCBody* pbody;
+  mjCGeom* pgeom;
+
+  // get body name and pointer to mjCBody
+  ReadAttrTxt(body_elem, "name", name, true);
+  pbody = (mjCBody*) model->GetWorld()->FindObject(mjOBJ_BODY, name);
+  if (!pbody) {
+    throw mjXError(body_elem, "URDF body not found");  // SHOULD NOT OCCUR
+  }
+
+  // inertial element: copy into alternative body frame
+  if ((elem = FindSubElem(body_elem, "inertial"))) {
+    pbody->explicit_inertial = true;
+    // origin- relative to joint frame for now
+    Origin(elem, pbody->ipos, pbody->iquat);
+
+    // mass
+    temp = FindSubElem(elem, "mass", true);
+    ReadAttr(temp, "value", 1, &pbody->mass, text, true);
+
+    // inertia
+    temp = FindSubElem(elem, "inertia", true);
+    mjCAlternative alt;
+    ReadAttr(temp, "ixx", 1, alt.fullinertia+0, text, true);
+    ReadAttr(temp, "iyy", 1, alt.fullinertia+1, text, true);
+    ReadAttr(temp, "izz", 1, alt.fullinertia+2, text, true);
+    ReadAttr(temp, "ixy", 1, alt.fullinertia+3, text, true);
+    ReadAttr(temp, "ixz", 1, alt.fullinertia+4, text, true);
+    ReadAttr(temp, "iyz", 1, alt.fullinertia+5, text, true);
+
+    // process inertia
+    //  lquat = rotation from specified to default (joint/body) inertial frame
+    double lquat[4], tmpquat[4];
+    const char* altres =
+      alt.Set(lquat, pbody->inertia, model->degree, model->euler);
+
+    // inertia are sometimes 0 in URDF files: ignore error in altres, fix later
+    (void) altres;
+
+    // correct for alignment of full inertia matrix
+    mjuu_mulquat(tmpquat, pbody->iquat, lquat);
+    mjuu_copyvec(pbody->iquat, tmpquat, 4);
+  }
+
+  // clear body frame; set by joint later
+  mjuu_setvec(pbody->pos, 0, 0, 0);
+  mjuu_setvec(pbody->quat, 1, 0, 0, 0);
+
+  // process all visual and geometry elements in order
+  float rgba[4] = {-1, 0, 0, 0};
+  elem = body_elem->FirstChildElement();
+  while (elem) {
+    name = elem->Value();
+
+    // visual element
+    if (name=="visual") {
+      // parse material
+      if ((temp = FindSubElem(elem, "material"))) {
+        // if color specified - use directly
+        if ((temp1 = FindSubElem(temp, "color"))) {
+          ReadAttr(temp1, "rgba", 4, rgba, text);
+        }
+
+        // otherwise use material table
+        else {
+          ReadAttrTxt(temp, "name", name, true);
+          int imat = FindName(name, urMat);
+          if (imat>=0) {
+            std::memcpy(rgba, urRGBA[imat].val, 4*sizeof(float));
+          }
+        }
+      }
+
+      // create geom if not discarded
+      if (!model->discardvisual) {
+        pgeom = Geom(elem, pbody, false);
+
+        // save color
+        if (rgba[0]>=0) {
+          std::memcpy(pgeom->rgba, rgba, 4*sizeof(float));
+        }
+      }
+    }
+
+    // collision element
+    else if (name=="collision") {
+      pgeom = Geom(elem, pbody, true);
+
+      // use color from last visual
+      if (rgba[0]>=0) {
+        std::memcpy(pgeom->rgba, rgba, 4*sizeof(float));
+      }
+    }
+
+    // advance
+    elem = elem->NextSiblingElement();
+  }
+}
+
+
+
+// parse joint
+void mjXURDF::Joint(XMLElement* joint_elem) {
+  std::string jntname, name, text;
+  XMLElement *elem;
+  mjCBody *pbody, *parent;
+  mjCJoint *pjoint=0, *pjoint1=0, *pjoint2=0;
+  int jointtype;
+
+  // get type and name
+  ReadAttrTxt(joint_elem, "type", text, true);
+  jointtype = FindKey(urJoint_map, urJoint_sz, text);
+  ReadAttrTxt(joint_elem, "name", jntname, true);
+
+  // get parent, check
+  elem = FindSubElem(joint_elem, "parent", true);
+  ReadAttrTxt(elem, "link", name, true);
+  parent = (mjCBody*) model->GetWorld()->FindObject(mjOBJ_BODY, name);
+  if (!parent) {                      // SHOULD NOT OCCUR
+    mjXError(elem, "invalid parent name in URDF joint definition");
+  }
+
+  // get child=this, check
+  elem = FindSubElem(joint_elem, "child", true);
+  ReadAttrTxt(elem, "link", name, true);
+  pbody = (mjCBody*) model->GetWorld()->FindObject(mjOBJ_BODY, name);
+  if (!pbody) {                       // SHOULD NOT OCCUR
+    throw mjXError(elem, "invalid child name in URDF joint definition");
+  }
+
+  // read origin and axis
+  double axis[3] = {1, 0, 0};
+  Origin(joint_elem, pbody->pos, pbody->quat);
+  if ((elem = FindSubElem(joint_elem, "axis"))) {
+    ReadAttr(elem, "xyz", 3, axis, text);
+  }
+
+  // create joint (unless fixed)
+  double mat[9], quat[4], tmpaxis[3];
+  switch (jointtype) {
+  case 0:     // revolute
+  case 1:     // continuous
+    pjoint = pbody->AddJoint();
+    pjoint->name = jntname;
+    pjoint->type = mjJNT_HINGE;
+    mjuu_setvec(pjoint->pos, 0, 0, 0);
+    mjuu_copyvec(pjoint->axis, axis, 3);
+    break;
+
+  case 2:     // prismatic
+    pjoint = pbody->AddJoint();
+    pjoint->name = jntname;
+    pjoint->type = mjJNT_SLIDE;
+    mjuu_setvec(pjoint->pos, 0, 0, 0);
+    mjuu_copyvec(pjoint->axis, axis, 3);
+    break;
+
+  case 3:     // fixed- no joint, return
+    return;
+
+  case 4:     // floating
+    pjoint = pbody->AddJoint();
+    pjoint->name = jntname;
+    pjoint->type = mjJNT_FREE;
+    break;
+
+  case 5:     // planar- construct complex joint
+    // make frame with axis = z
+    mjuu_z2quat(quat, axis);
+    mjuu_quat2mat(mat, quat);
+
+    // construct slider along x
+    pjoint = pbody->AddJoint();
+    pjoint->name = jntname + "_TX";
+    pjoint->type = mjJNT_SLIDE;
+    tmpaxis[0] = mat[0];
+    tmpaxis[1] = mat[3];
+    tmpaxis[2] = mat[6];
+    mjuu_setvec(pjoint->pos, 0, 0, 0);
+    mjuu_copyvec(pjoint->axis, tmpaxis, 3);
+
+    // construct slider along y
+    pjoint1 = pbody->AddJoint();
+    pjoint1->name = jntname + "_TY";
+    pjoint1->type = mjJNT_SLIDE;
+    tmpaxis[0] = mat[1];
+    tmpaxis[1] = mat[4];
+    tmpaxis[2] = mat[7];
+    mjuu_setvec(pjoint1->pos, 0, 0, 0);
+    mjuu_copyvec(pjoint1->axis, tmpaxis, 3);
+
+    // construct hinge around z = locaxis
+    pjoint2 = pbody->AddJoint();
+    pjoint2->name = jntname + "_RZ";
+    pjoint2->type = mjJNT_HINGE;
+    mjuu_setvec(pjoint2->pos, 0, 0, 0);
+    mjuu_copyvec(pjoint2->axis, axis, 3);
+  }
+
+  // dynamics element
+  if ((elem = FindSubElem(joint_elem, "dynamics"))) {
+    ReadAttr(elem, "damping", 1, &pjoint->damping, text);
+    ReadAttr(elem, "friction", 1, &pjoint->frictionloss, text);
+
+    // copy parameters to all elements of planar joint
+    if (pjoint1) {
+      pjoint1->damping = pjoint2->damping = pjoint->damping;
+      pjoint1->frictionloss = pjoint2->frictionloss = pjoint->frictionloss;
+    }
+  }
+
+  // limit element
+  if ((elem = FindSubElem(joint_elem, "limit"))) {
+    ReadAttr(elem, "lower", 1, pjoint->range, text);
+    ReadAttr(elem, "upper", 1, pjoint->range+1, text);
+    pjoint->limited = (mjuu_defined(pjoint->range[0]) &&
+                       mjuu_defined(pjoint->range[1]) &&
+                       pjoint->range[0] < pjoint->range[1]);
+
+    // ReadAttr(elem, "velocity", 1, &pjoint->maxvel, text); // no maxvel in MuJoCo
+    ReadAttr(elem, "effort", 1, &pjoint->urdfeffort, text);
+  } else {
+    pjoint->limited = 0;
+  }
+}
+
+
+
+// parse origin and geometry elements of visual or collision
+mjCGeom* mjXURDF::Geom(XMLElement* geom_elem, mjCBody* pbody, bool collision) {
+  XMLElement *elem, *temp;
+  std::string text, meshfile;
+
+  // get geometry element
+  elem = FindSubElem(geom_elem, "geometry", true);
+
+  // add BOX geom, modify type later
+  mjCGeom* pgeom = pbody->AddGeom();
+  pgeom->name = "";
+  pgeom->type = mjGEOM_BOX;
+  if (collision) {
+    pgeom->contype = 1;
+    pgeom->conaffinity = 1;
+  } else {
+    pgeom->contype = 0;
+    pgeom->conaffinity = 0;
+    pgeom->group = 1;
+    pgeom->density = 0;
+  }
+
+  // box
+  if ((temp = FindSubElem(elem, "box"))) {
+    ReadAttr(temp, "size", 3, pgeom->size, text, true, true);
+    for (int i=0; i<3; i++) {
+      pgeom->size[i] /= 2;  // MuJoCo uses half-length
+    }
+  }
+
+  // cylinder
+  else if ((temp = FindSubElem(elem, "cylinder"))) {
+    pgeom->type = mjGEOM_CYLINDER;
+    ReadAttr(temp, "radius", 1, pgeom->size, text, true, true);
+    ReadAttr(temp, "length", 1, pgeom->size+1, text, true, true);
+    pgeom->size[1] /= 2;            // MuJoCo uses half-length
+  }
+
+  // sphere
+  else if ((temp = FindSubElem(elem, "sphere"))) {
+    pgeom->type = mjGEOM_SPHERE;
+    ReadAttr(temp, "radius", 1, pgeom->size, text, true, true);
+  }
+
+  // mesh
+  else if ((temp = FindSubElem(elem, "mesh"))) {
+    // set geom type and read mesh attributes
+    double meshscale[3] = {1, 1, 1};
+    pgeom->type = mjGEOM_MESH;
+    ReadAttrTxt(temp, "filename", meshfile, true);
+    ReadAttr(temp, "scale", 3, meshscale, text);
+
+    // strip file name if necessary
+    if (model->strippath) {
+      meshfile = mjuu_strippath(meshfile);
+    }
+
+    // construct mesh name: always stripped
+    std::string meshname = mjuu_strippath(meshfile);
+    meshname = mjuu_stripext(meshname);
+
+    // look for existing mesh
+    mjCMesh* pmesh = (mjCMesh*)model->FindObject(mjOBJ_MESH, meshname);
+
+    // does not exist: create
+    if (!pmesh) {
+      pmesh = model->AddMesh();
+    }
+
+    // exists with different scale: append name with '1', create
+    else if (pmesh->scale[0]!=meshscale[0] ||
+             pmesh->scale[1]!=meshscale[1] ||
+             pmesh->scale[2]!=meshscale[2]) {
+      pmesh = model->AddMesh();
+      meshname = meshname + "1";
+    }
+
+    // set fields
+    pmesh->file = meshfile;
+    pmesh->name = meshname;
+    pgeom->mesh = meshname;
+    mjuu_copyvec(pmesh->scale, meshscale, 3);
+  }
+
+  else {
+    throw mjXError(elem, "visual geometry specification not found");
+  }
+
+  // origin element
+  Origin(geom_elem, pgeom->pos, pgeom->quat);
+
+  return pgeom;
+}
+
+
+
+// parse origin element
+void mjXURDF::Origin(XMLElement* origin_elem, double* pos, double* quat) {
+  XMLElement* temp;
+  std::string text;
+
+  // set defaults
+  mjuu_setvec(pos, 0, 0, 0);
+  mjuu_setvec(quat, 1, 0, 0, 0);
+
+  // read origin element if present
+  if ((temp = FindSubElem(origin_elem, "origin"))) {
+    // position
+    ReadAttr(temp, "xyz", 3, pos, text);
+
+    // orientation
+    mjCAlternative alt;
+    if (ReadAttr(temp, "rpy", 3, alt.euler, text)) {
+      alt.Set(quat, 0, 0, "XYZ");
+    }
+  }
+}
+
+
+
+// find body with given name in list, return -1 if not found
+int mjXURDF::FindName(std::string name, std::vector<std::string>& list) {
+  for (unsigned int i=0; i<list.size(); i++)
+    if (list[i] == name) {
+      return i;
+    }
+
+  return -1;
+}
+
+
+
+// add name to list, error if name already exists
+void mjXURDF::AddName(std::string name, std::vector<std::string>& list) {
+  // make sure name is unique
+  if (FindName(name, list)>=0) {
+    throw mjXError(0, "repeated URDF name: ", name.c_str());
+  }
+
+  list.push_back(name);
+}
+
+
+
+// add body name to list of URDF bodies, error if name already exists
+void mjXURDF::AddBody(std::string name) {
+  // add body name, make sure it is unique
+  AddName(name, urName);
+
+  // add parent and child elements
+  urParent.push_back(-1);
+  std::vector<int> children;
+  children.clear();
+  urChildren.push_back(children);
+}
+
+
+
+// add body with given number to the mjCModel tree, process children
+void mjXURDF::AddToTree(int n) {
+  // get pointer to parent in mjCModel tree
+  mjCBody *parent = 0, *child = 0;
+  if (urParent[n]>=0) {
+    parent = (mjCBody*) model->GetWorld()->FindObject(mjOBJ_BODY, urName[urParent[n]]);
+
+    if (!parent)
+      throw mjXError(0, "URDF body parent should already be in tree: %s",
+                     urName[urParent[n]].c_str());       // SHOULD NOT OCCUR
+  } else {
+    parent = model->GetWorld();
+  }
+
+  // add this body
+  if (urName[n] != "world") {
+    child = parent->AddBody();
+    child->name = urName[n];
+  }
+
+  // add children recursively
+  for (int i=0; i<(int)urChildren[n].size(); i++) {
+    AddToTree(urChildren[n][i]);
+  }
+}
+
+
+
+// find all materials recursively
+void mjXURDF::MakeMaterials(XMLElement* elem) {
+  std::string name, text;
+  XMLElement* color = 0;
+  mjRGBA rgba;
+
+  // process this element
+  if (!std::strcmp(elem->Value(), "material")) {
+    // make sure material is named
+    if (ReadAttrTxt(elem, "name", name)) {
+      // make sure name is not already registered
+      if (FindName(name, urMat) < 0) {
+        // add rgba value if available
+        if ((color = FindSubElem(elem, "color"))) {
+          ReadAttr(color, "rgba", 4, rgba.val, text);
+          AddName(name, urMat);
+          urRGBA.push_back(rgba);
+        }
+      }
+    }
+  }
+
+  // process children recursively
+  elem = elem->FirstChildElement();
+  while (elem) {
+    MakeMaterials(elem);
+    elem = elem->NextSiblingElement();
+  }
+}

src/xml/xml_urdf.h

@@ -0,0 +1,60 @@
+// 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.
+
+#ifndef THIRD_PARTY_MUJOCO_SRC_XML_XML_URDF_
+#define THIRD_PARTY_MUJOCO_SRC_XML_XML_URDF_
+
+#include <string>
+#include <vector>
+
+#include "user/user_model.h"
+#include "xml/xml_base.h"
+#include "tinyxml2.h"
+
+// rgb data
+typedef struct _mjRGBA {
+  float val[4];
+} mjRGBA;
+
+// XML parser for URDF files
+class mjXURDF : public mjXBase {
+ public:
+  mjXURDF();                                          // constructor
+  virtual ~mjXURDF();                                 // destructor
+
+  void Parse(tinyxml2::XMLElement* root);             // main parser
+
+ private:
+  int FindName(std::string name, std::vector<std::string>& list);  // find name in list
+  void AddName(std::string name, std::vector<std::string>& list);  // add name to list
+  void AddBody(std::string name);                                  // add body to local table
+  void AddToTree(int n);                                           // add body to mjCModel tree
+  void Body(tinyxml2::XMLElement* body_elem);                      // parse body
+  void Joint(tinyxml2::XMLElement* joint_elem);                    // parse joint
+  mjCGeom* Geom(tinyxml2::XMLElement* geom_elem,
+                mjCBody* pbody, bool collision);      // parse origin and geometry of geom
+  void Origin(tinyxml2::XMLElement* origin_elem, double* pos, double* quat); // parse origin element
+
+  void MakeMaterials(tinyxml2::XMLElement* elem);     // find all materials recursively
+  void Clear(void);                                   // clear local objects
+
+  // URDF parser variables
+  std::vector<std::string> urName;             // body name
+  std::vector<int> urParent;                   // body parent (index in name vector)
+  std::vector<std::vector<int> > urChildren;   // body children (index in name vector)
+  std::vector<std::string> urMat;              // material name
+  std::vector<mjRGBA> urRGBA;                  // material RBG value
+};
+
+#endif  // THIRD_PARTY_MUJOCO_SRC_XML_XML_URDF_

src/xml/xml_util.h

@@ -0,0 +1,185 @@
+// 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.
+
+
+#ifndef MUJOCO_SRC_XML_XML_UTIL_H_
+#define MUJOCO_SRC_XML_XML_UTIL_H_
+
+// stl
+#include <string>
+#include <vector>
+#include <sstream>
+
+#include <mujoco/mjmodel.h>
+
+
+// TinyXML
+#include "tinyxml2.h"
+
+
+// error string copy
+void mjCopyError(char* dst, const char* src, int maxlen);
+
+
+// XML Error info
+class mjXError {
+ public:
+  mjXError(const tinyxml2::XMLElement* elem = 0,
+           const char* msg = 0,
+           const char* str = 0,
+           int pos = 0);
+  ~mjXError() = default;
+
+  char message[1000];             // error message
+};
+
+
+// max number of attribute fields in schema (plus 3)
+#define mjXATTRNUM 35
+
+
+// Custom XML file validation
+class mjXSchema {
+ public:
+  mjXSchema(const char* schema[][mjXATTRNUM],         // constructor
+            int nrow, bool checkptr = true);
+  ~mjXSchema();                                       // destructor
+
+  std::string GetError(void);                         // return error
+  void Print(std::stringstream& str, int level);      // print schema
+  void PrintHTML(std::stringstream& str, int level, bool pad);
+
+  bool NameMatch(tinyxml2::XMLElement* elem, int level);               // does name match
+  tinyxml2::XMLElement* Check(tinyxml2::XMLElement* elem, int level);  // validator
+
+ private:
+  std::string name;                   // element name
+  char type;                          // element type: '?', '!', '*', 'R'
+  std::vector<std::string> attr;      // allowed attributes
+  std::vector<mjXSchema*> child;      // allowed child elements
+
+  int refcnt;                         // refcount used for validation
+  std::string error;                  // error from constructor or Check
+};
+
+
+// key(string) : value(int) map
+struct _mjMap {
+  std::string key;
+  int value;
+};
+typedef struct _mjMap mjMap;
+
+
+// XML read and write utility functions
+class mjXUtil {
+ public:
+  mjXUtil() = default;
+  virtual ~mjXUtil() = default;
+
+  // compare two vectors
+  static bool SameVector(const double* vec1, const double* vec2, int n);
+  static bool SameVector(const float* vec1, const float* vec2, int n);
+
+  // find key in map, return value (-1: not found)
+  static int FindKey(const mjMap* map, int mapsz, std::string key);
+
+  // find value in map, return key ("": not found)
+  static std::string FindValue(const mjMap* map, int mapsz, int value);
+
+  // read DOUBLE array from attribute, return number read
+  static int ReadAttr(tinyxml2::XMLElement* elem, const char* attr, const int len,
+                      double* data, std::string& text,
+                      bool required = false, bool exact = true);
+
+  // read FLOAT array from attribute, return number read
+  static int ReadAttr(tinyxml2::XMLElement* elem, const char* attr, const int len,
+                      float* data, std::string& text,
+                      bool required = false, bool exact = true);
+
+  // read INT array from attribute, return number read
+  static int ReadAttr(tinyxml2::XMLElement* elem, const char* attr, const int len,
+                      int* data, std::string& text,
+                      bool required = false, bool exact = true);
+
+  // read BYTE array from attribute, return number read
+  static int ReadAttr(tinyxml2::XMLElement* elem, const char* attr, const int len,
+                      mjtByte* data, std::string& text,
+                      bool required = false, bool exact = true);
+
+  // read DOUBLE array into C++ vector, return number read
+  static int ReadVector(tinyxml2::XMLElement* elem, const char* attr,
+                        std::vector<double>& vec, std::string& text, bool required = false);
+
+  // read text attribute
+  static bool ReadAttrTxt(tinyxml2::XMLElement* elem, const char* attr, std::string& text,
+                          bool required = false);
+
+  // read int attribute
+  static bool ReadAttrInt(tinyxml2::XMLElement* elem, const char* attr, int* data,
+                          bool required = false);
+
+  // read vector<float> from string
+  static void String2Vector(const std::string& txt, std::vector<float>& vec);
+
+  // read vector<int> from string
+  static void String2Vector(const std::string& txt, std::vector<int>& vec);
+
+  // write vector<float> to string
+  static void Vector2String(std::string& txt, const std::vector<float>& vec);
+
+  // write vector<int> to string
+  static void Vector2String(std::string& txt, const std::vector<int>& vec);
+
+  // find subelement with given name, make sure it is unique
+  static tinyxml2::XMLElement* FindSubElem(tinyxml2::XMLElement* elem, std::string name,
+                                           bool required = false);
+
+  // find attribute, translate key, return int value
+  static bool MapValue(tinyxml2::XMLElement* elem, const char* attr, int* data,
+                       const mjMap* map, int mapSz, bool required = false);
+
+  // write attribute- double
+  static void WriteAttr(tinyxml2::XMLElement* elem, std::string name, int n, double* data,
+                        const double* def = 0);
+
+  // write attribute- float
+  static void WriteAttr(tinyxml2::XMLElement* elem, std::string name, int n, float* data,
+                        const float* def = 0);
+
+  // write attribute- byte
+  static void WriteAttr(tinyxml2::XMLElement* elem, std::string name, int n, mjtByte* data,
+                        const mjtByte* def = 0);
+
+  // write attribute- int
+  static void WriteAttr(tinyxml2::XMLElement* elem, std::string name, int n, int* data,
+                        const int* def = 0);
+
+  // write vector<double> attribute, with and without default
+  static void WriteVector(tinyxml2::XMLElement* elem, std::string name, std::vector<double>& vec);
+  static void WriteVector(tinyxml2::XMLElement* elem, std::string name, std::vector<double>& vec,
+                          std::vector<double>& def);
+
+  // write attribute- string
+  static void WriteAttrTxt(tinyxml2::XMLElement* elem, std::string name, std::string value);
+
+  // write attribute- single int
+  static void WriteAttrInt(tinyxml2::XMLElement* elem, std::string name, int data, int def = -12345);
+
+  // write attribute- keyword
+  static void WriteAttrKey(tinyxml2::XMLElement* elem, std::string name,
+                           const mjMap* map, int mapsz, int data, int def = -12345);
+};
+
+#endif  // MUJOCO_SRC_XML_XML_UTIL_H_