chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity
Files
5e99ca6cb86e08393cf63692a6fb96c7e24f0040
Mujoco_WASM/src/xml/xml_util.cc
T
Yuval Tassa 5e99ca6cb8 Table-driven attribute writing: rebase the writer on the shared rows. 
The writer consumes the same generated rows as the reader.
mjXWriter::WriteAttrTable drives the mechanical attributes of an
element from its mjXAttr rows: each bound field is compared against
the class default at the same offset -- the default object is the same
struct type, so the rows carry no comparison values -- and attributes
equal to their default are skipped. A null default object means the
element has no defaults, and every defined value is written.
Ranged-arity rows write with trailing-default trimming, which the
reader makes round-trip exact by refilling from the same default.
Call sites upcast to the private mjs base (the friend declarations
permit it; mjCMesh gains the friendship its siblings had); the
comparison object is def->X().spec, a freshly-defaulted struct for
the sections, or zero-initialized for size, whose spec defaults (-1,
auto) are resolved by compilation.

Converted: pair, geom, site, joint, camera, light, material, the
equality family, both tendon types (the fixed rows are the spatial
rows without appearance attributes -- exactly the tag difference), the
actuator, flex with its three sub-elements, mesh, skin, option, the
six visual sub-sections, statistic and size. The remnants keep names,
files, resolved reference strings (the mjC classes null their private
base's string pointers; resolved names live behind accessors), and the
writing=custom policies the schema declares: compile directives never
saved (fromto, springdamper, fitscale), type-dependent lengths and
attributes (sizes, joint pos/axis/limited, shellinertia), and
alternatives (mass/density, fovy-versus-intrinsics, the plugin-gated
gain/bias family). Compiler keeps its write-if-nonzero policy;
keyframe keeps its model-sized vectors.

Saved files are canonical: attributes follow schema declaration order
with remnants trailing, and sections follow the schema's dependency
order (statistic before visual, deformable before the contact and
equality sections that name flexes, tendon before the equality
constraints that name tendons, custom demoted to the data tail).
Uniform behavior fixes fall out: default-equal positionals are
dropped, dynprm is trimmed like every other ranged vector, and mesh
material -- read into the spec but never written -- now survives
save/load round trips. Changelog entries ride along.

Verified: full suite, doc_test, and the two-tier A/B harness --
saved XML reorders attributes, and every corpus model reloads to a
byte-identical binary.
PiperOrigin-RevId: 958255003
Change-Id: I5fe7346014450db88b2f3f8680a8f616f7d31266
2026-08-03 02:27:22 -07:00

1290 lines
36 KiB
C++
Raw Blame History

// 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 <algorithm>
#include <cerrno>
#include <climits>
#include <cmath>
#include <cstddef>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <functional>
#include <iostream>
#include <limits>
#include <optional>
#include <set>
#include <sstream>
#include <stack>
#include <string>
#include <type_traits>
#include <unordered_set>
#include <utility>
#include <vector>
#include <mujoco/mujoco.h>
#include "cc/array_safety.h"
#include "engine/engine_util_errmem.h"
#include "user/user_resource.h"
#include "user/user_util.h"
#include "xml/xml_util.h"
#include "xml/xml_numeric_format.h"
#include "tinyxml2.h"
namespace {
using tinyxml2::XMLAttribute;
using tinyxml2::XMLElement;
using mujoco::user::FilePath;
namespace mju = ::mujoco::util;
template <typename T>
static std::optional<T> ParseInfOrNan(const std::string& s) {
const char* str = s.c_str();
if constexpr (std::is_floating_point_v<T>) {
T sign = 1;
if (s.size() == 4 && s[0] == '-') {
sign = -1;
++str;
} else if (s.size() != 3) {
return std::nullopt;
}
if (std::numeric_limits<T>::has_infinity &&
(str[0] == 'i' || str[0] == 'I') &&
(str[1] == 'n' || str[1] == 'N') &&
(str[2] == 'f' || str[2] == 'F')) {
return sign * std::numeric_limits<T>::infinity();
} else if (std::numeric_limits<T>::has_quiet_NaN &&
(str[0] == 'n' || str[0] == 'N') &&
(str[1] == 'a' || str[1] == 'A') &&
(str[2] == 'n' || str[2] == 'N')) {
return sign * std::numeric_limits<T>::quiet_NaN();
}
}
return std::nullopt;
}
FilePath ResolveFilePath(XMLElement* e, const FilePath& filename,
const FilePath& dir, const mjVFS* vfs) {
std::string path = "";
if (filename.IsAbs()) {
return filename;
}
// TODO(kylebayes): We first look in the base model directory for files to
// remain backwards compatible.
FilePath fullname = dir + filename;
mjResource *resource = mju_openResource("", fullname.c_str(), vfs,
nullptr, 0);
if (resource != nullptr) {
mju_closeResource(resource);
return filename;
}
XMLElement* parent = e->Parent()->ToElement();
for (; parent; parent = parent->Parent()->ToElement()) {
if (!std::strcmp(parent->Value(), "include")) {
auto file_attr = mjXUtil::ReadAttrStr(parent, "dir", false);
if (file_attr.has_value()) {
path = file_attr.value();
}
break;
}
}
return FilePath(path) + filename;
}
void AccumulateFiles(std::unordered_set<std::string> &files,
tinyxml2::XMLElement *root, const FilePath &model_dir) {
std::optional<FilePath> asset_dir;
std::optional<FilePath> mesh_dir;
std::optional<FilePath> texture_dir;
std::set<std::string> include_and_model_files;
std::set<std::string> texture_files;
std::set<std::string> mesh_files;
std::set<std::string> hfield_files;
auto accumulate_files = [&](const std::set<std::string> &candidate_files,
std::optional<FilePath> prefix) {
for (const auto &file : candidate_files) {
FilePath file_with_prefix = !prefix.has_value() ? FilePath(file) : prefix.value() + FilePath(file);
if (file_with_prefix.IsAbs()) {
files.insert(file_with_prefix.Str());
} else {
// Else insert dir_path / prefix / file.
auto full_path = model_dir + file_with_prefix;
files.insert(full_path.Str());
}
}
};
std::stack<tinyxml2::XMLElement *> elements;
elements.push(root);
while (!elements.empty()) {
tinyxml2::XMLElement *elem = elements.top();
elements.pop();
if (!std::strcmp(elem->Value(), "include") ||
!std::strcmp(elem->Value(), "model")) {
auto file_attr = mjXUtil::ReadAttrFile(elem, "file", nullptr);
if (file_attr.has_value()) {
include_and_model_files.insert(file_attr->Str());
// Neither of these elements should have children.
continue;
}
} else if (!std::strcmp(elem->Value(), "compiler")) {
auto assetdir_str = mjXUtil::ReadAttrStr(elem, "assetdir", false);
if (assetdir_str.has_value()) asset_dir = FilePath(assetdir_str.value());
auto meshdir_str = mjXUtil::ReadAttrStr(elem, "meshdir", false);
if (meshdir_str.has_value()) mesh_dir = FilePath(meshdir_str.value());
auto texturedir_str = mjXUtil::ReadAttrStr(elem, "texturedir", false);
if (texturedir_str.has_value()) texture_dir = FilePath(texturedir_str.value());
// compiler elements don't have children.
continue;
} else if (!std::strcmp(elem->Value(), "mesh") ||
!std::strcmp(elem->Value(), "flexcomp") ||
!std::strcmp(elem->Value(), "skin")) {
// mesh elements don't have children.
auto file_attr = mjXUtil::ReadAttrFile(elem, "file", nullptr);
if (file_attr.has_value()) {
mesh_files.insert(file_attr->Str());
}
continue;
} else if (!std::strcmp(elem->Value(), "hfield")) {
// hfield elements don't have children.
auto file_attr = mjXUtil::ReadAttrFile(elem, "file", nullptr);
if (file_attr.has_value()) {
hfield_files.insert(file_attr->Str());
}
continue;
} else if (!std::strcmp(elem->Value(), "texture")) {
static const char *attributes[] = {"file", "fileright", "fileup",
"fileleft", "filedown", "filefront",
"fileback"};
for (const auto &attribute : attributes) {
auto file_attr = mjXUtil::ReadAttrFile(elem, attribute, nullptr);
if (file_attr.has_value()) {
texture_files.insert(file_attr->Str());
}
}
}
tinyxml2::XMLElement *child = elem->FirstChildElement();
while (child) {
elements.push(child);
child = child->NextSiblingElement();
}
}
// TODO(shaves): When we have resource decoders implemented they should have a
// "get dependencies" function to call here. For non XML types we assume they
// have no dependencies here.
// First resolve all dependent XML files.
for (const auto &file : include_and_model_files) {
mjStringVec subdeps;
FilePath full_path = model_dir + FilePath(file);
mju_getXMLDependencies(full_path.Str().c_str(), &subdeps);
for (const auto &subdep : subdeps) {
files.insert(subdep);
}
}
// Then for each non MJCF resource file, add them to the set of files using their respective
// compiler prefixes (if they exist).
accumulate_files(texture_files,
texture_dir.has_value() ? texture_dir : asset_dir);
accumulate_files(mesh_files, mesh_dir.has_value() ? mesh_dir : asset_dir);
accumulate_files(hfield_files, asset_dir);
}
}
//---------------------------------- utility functions ---------------------------------------------
// error string copy
void mjCopyError(char* dst, const char* src, int maxlen) {
if (dst && maxlen > 0) {
strncpy(dst, src, maxlen);
dst[maxlen-1] = 0;
}
}
void mju_getXMLDependencies(const char* filename, mjStringVec* dependencies) {
// Open through the resource provider so the function works against any
// registered backend (OS file system, VFS, HTTP, "github:", ...) rather
// than only the OS file system.
mjResource* resource = mju_openResource("", filename, nullptr, nullptr, 0);
if (resource == nullptr) {
mju_error("Could not open '%s'", filename);
}
// Read the XML bytes from the resource.
const void* buffer = nullptr;
int size = mju_readResource(resource, &buffer);
if (size < 0 || !size) {
mju_closeResource(resource);
mju_error("Could not read '%s'", filename);
}
// Capture the model directory while the resource is still open.
const char* dir = nullptr;
int ndir = 0;
mju_getResourceDir(resource, &dir, &ndir);
FilePath model_dir(std::string(dir, ndir));
// Parse from buffer and close (the parsed DOM is independent of the
// resource buffer once Parse returns).
tinyxml2::XMLDocument doc;
tinyxml2::XMLError err =
doc.Parse(static_cast<const char*>(buffer), static_cast<size_t>(size));
mju_closeResource(resource);
if (err != tinyxml2::XML_SUCCESS) {
mju_error("Problem reading XML file '%s': %s", filename,
doc.ErrorStr() ? doc.ErrorStr() : "");
}
tinyxml2::XMLElement* root = doc.RootElement();
if (!root) {
mju_error("XML root element not found in '%s'", filename);
}
std::unordered_set<std::string> files = {filename};
AccumulateFiles(files, root, model_dir);
*dependencies = {files.begin(), files.end()};
}
// error constructor
mjXError::mjXError(const XMLElement* elem, const char* msg, const char* str, int pos) {
char temp[500];
// construct error message
mju::sprintf_arr(message, "XML Error");
if (msg) {
mju::sprintf_arr(temp, msg, str, pos);
mju::strcat_arr(message, ": ");
mju::strcat_arr(message, temp);
}
// append element, line numbers
if (elem) {
mju::sprintf_arr(temp, "\nElement '%s', line %d\n", elem->Value(), elem->GetLineNum());
mju::strcat_arr(message, temp);
}
}
//---------------------------------- class mjXSchema implementation --------------------------------
XMLElement* FirstChildElement(XMLElement* e, const char* name) {
XMLElement* child = e->FirstChildElement();
for (; child; child = child->NextSiblingElement()) {
if (!std::strcmp(child->Name(), "include")) {
XMLElement* temp = FirstChildElement(child, name);
if (temp) {
return temp;
}
continue;
}
if (!name || !std::strcmp(child->Name(), name)) {
return child;
}
}
return nullptr;
}
XMLElement* NextSiblingElement(XMLElement* e, const char* name) {
XMLElement* elem = e->NextSiblingElement();
for (; elem; elem = elem->NextSiblingElement()) {
if (!std::strcmp(elem->Name(), "include")) {
XMLElement* temp = FirstChildElement(elem, name);
if (temp) {
return temp;
}
continue;
}
if (!name || !std::strcmp(elem->Name(), name)) {
return elem;
}
}
XMLElement* parent = e->Parent()->ToElement();
if (parent && !std::strcmp(parent->Name(), "include")) {
return NextSiblingElement(parent, name);
}
return nullptr;
}
// constructor
mjXSchema::mjXSchema(std::vector<const char*> schema[], unsigned nrow,
const mjXConstraintDef* constraints, int nconstraint,
int first_row) {
// set name and type
name_ = schema[0][0];
type_ = schema[0][1][0];
// adopt the presence constraints declared for this row
for (int i = 0; i < nconstraint; i++) {
if (constraints[i].row == first_row) {
constraints_.push_back(&constraints[i]);
}
}
// set attributes
int nattr = schema[0].size() - 2;
for (int i = 0; i < nattr; i++) {
attr_.emplace(schema[0][2 + i]);
}
// process sub-elements of complex element
if (nrow > 1) {
// parse block into simple and complex elements, create children
int start = 2;
while (start < nrow-1) {
int end = start;
// look for bracketed block at start+1
if (schema[start+1][0][0] == '<') {
// look for corresponding closing bracket
int cnt = 0;
while (end <= nrow-1) {
if (schema[end][0][0] == '<') {
cnt++;
} else if (schema[end][0][0] == '>') {
cnt--;
if (cnt == 0) {
break;
}
}
end++;
}
}
// add child element
subschema_.emplace_back(schema+start, end-start+1, constraints,
nconstraint, first_row+start);
// proceed with next subelement
start = end+1;
}
}
}
// quoted list of constraint bundles: 'a' or ('a', 'b'), comma-joined
static std::string BundleList(const std::vector<std::vector<std::string>>& bundles) {
std::string out;
for (size_t i = 0; i < bundles.size(); i++) {
if (i) {
out += ", ";
}
if (bundles[i].size() == 1) {
out += "'" + bundles[i][0] + "'";
} else {
out += '(';
for (size_t j = 0; j < bundles[i].size(); j++) {
out += (j ? ", '" : "'") + bundles[i][j] + "'";
}
out += ')';
}
}
return out;
}
// enforce the presence constraints declared for this element
XMLElement* mjXSchema::CheckConstraints(XMLElement* elem) {
for (const mjXConstraintDef* con : constraints_) {
// split the spec into bundles of attribute names
std::vector<std::vector<std::string>> bundles(1);
std::string token;
for (const char* c = con->spec;; c++) {
if (*c == ' ' || *c == '|' || *c == '\0') {
if (!token.empty()) {
bundles.back().push_back(token);
token.clear();
}
if (*c == '|') {
bundles.emplace_back();
}
if (*c == '\0') {
break;
}
} else {
token += *c;
}
}
// per-bundle presence: any member present / all members present
int n_any = 0, n_all = 0, n_attr = 0, n_present = 0;
for (const auto& bundle : bundles) {
bool any = false, all = true;
for (const std::string& attr : bundle) {
bool present = elem->Attribute(attr.c_str()) != nullptr;
any |= present;
all &= present;
n_attr++;
n_present += present;
}
n_any += any;
n_all += all;
}
switch (con->kind) {
case 'e': // at most one bundle may be present
if (n_any > 1) {
error = "at most one of " + BundleList(bundles) +
" can be specified";
return elem;
}
break;
case 't': // all listed attributes appear together or not at all
if (n_present != 0 && n_present != n_attr) {
error = "attributes " + BundleList(bundles) +
" must be specified together";
return elem;
}
break;
case 'r': // first attribute requires the second
if (n_any && elem->Attribute(bundles[0][0].c_str()) &&
!elem->Attribute(bundles[1][0].c_str())) {
error = "attribute '" + bundles[0][0] + "' requires attribute '" +
bundles[1][0] + "'";
return elem;
}
break;
case 'o': // at least one bundle must be complete
if (n_all == 0) {
error = "one of " + BundleList(bundles) + " must be specified";
return elem;
}
break;
}
}
return nullptr;
}
// get pointer to error message
std::string mjXSchema::GetError() {
return error;
}
// print spaces
static void printspace(std::stringstream& str, int n, const char* space) {
for (int i=0; i < n; i++) {
str << space;
}
}
// print schema as text
void mjXSchema::Print(std::stringstream& str, int level) const {
// replace body with (world)body
std::string name1 = (name_ == "body") ? "(world)body" : name_;
// space, name, type
printspace(str, 3*level, " ");
str << name1 << " (" << type_ << ")";
int baselen = 3*level + (int)name1.size() + 4;
if (baselen < 30) {
printspace(str, 30-baselen, " ");
}
// attributes
int cnt = std::max(baselen, 30);
for (const std::string& attr : attr_) {
if (cnt > 60) {
str << "\n";
printspace(str, (cnt = std::max(30, baselen)), " ");
}
str << attr << " ";
cnt += (int)attr.size() + 1;
}
str << "\n";
// children
for (const mjXSchema& subschema : subschema_) {
subschema.Print(str, level+1);
}
}
// print schema as HTML table
void mjXSchema::PrintHTML(std::stringstream& str, int level, bool pad) const {
// replace body with (world)body
std::string name1 = (name_ == "body" ? "(world)body" : name_);
// open table
if (level == 0) {
str << "<table border=\"1\">\n";
}
// name: with HTML padding
if (pad) {
str << "<tr>\n\t<td style=\"padding-left:" << 5 + 15*level;
str << "\" bgcolor=\"#EEEEEE\" class=\"el\">" << name1 << "</td>\n";
}
// name: with &nbsp; for browsers that ignore padding
else {
str << "<tr>\n\t<td bgcolor=\"#EEEEEE\" class=\"el\">";
if (level) {
printspace(str, 4*level, "&nbsp;");
}
str << name1 << "</td>\n";
}
// type
str << "\t<td class=\"ty\">" << type_ << "</td>\n";
// attributes
str << "\t<td class=\"at\">";
if (!attr_.empty()) {
for (const std::string& attr : attr_) {
str << attr << " ";
}
} else {
str << "<span style=\"color:black\"><i>no attributes</i></span>";
}
str << "</td>\n</tr>\n";
// children
for (const mjXSchema& subschema : subschema_) {
subschema.PrintHTML(str, level+1, pad);
}
// close table
if (!level) {
str << "</table>\n";
}
}
// check for name match
bool mjXSchema::NameMatch(XMLElement* elem, int level) {
// special handling of body, worldbody, and frame
if (name_ == "body" &&
((level == 1 && !strcmp(elem->Value(), "worldbody")) ||
(level != 1 && !strcmp(elem->Value(), "body")) ||
(level >= 1 && !strcmp(elem->Value(), "frame")) ||
(level >= 1 && !strcmp(elem->Value(), "replicate")))) {
return true;
}
// regular check
return name_ == elem->Value();
}
// validator
XMLElement* mjXSchema::Check(XMLElement* elem, int level) {
bool missing;
char msg[100];
XMLElement *bad, *sub;
error.clear();
if (!elem) {
return 0; // SHOULD NOT OCCUR
}
// check name (already done by parent, but hard to avoid)
if (!NameMatch(elem, level)) {
error = "unrecognized element";
return elem;
}
// check attributes
const XMLAttribute* attribute = elem->FirstAttribute();
for (; attribute != nullptr; attribute = attribute->Next()) {
if (attr_.find(attribute->Name()) == attr_.end()) {
error = "unrecognized attribute: '" + std::string(attribute->Name()) + "'";
return elem;
}
}
// check presence constraints
if ((bad = CheckConstraints(elem))) {
return bad;
}
// handle recursion
if (type_ == 'R') {
// check child elements with same name
sub = FirstChildElement(elem, name_.c_str());
for (; sub != nullptr; sub = NextSiblingElement(sub, name_.c_str())) {
if ((bad = Check(sub, level+1))) {
return bad;
}
}
}
// clear reference counts
for (mjXSchema& subschema : subschema_) {
subschema.refcnt_ = 0;
}
// check sub-elements, update refcnt
sub = FirstChildElement(elem);
for (; sub != nullptr; sub = NextSiblingElement(sub)) {
missing = true;
for (mjXSchema& subschema : subschema_) {
if (subschema.NameMatch(sub, level+1)) {
// check sub-tree
if ((bad = subschema.Check(sub, level+1))) {
error = subschema.error;
return bad;
}
// mark found
missing = false;
subschema.refcnt_++;
break;
}
}
// missing, unless recursive
if (missing && !(type_ == 'R' && NameMatch(sub, level+1))) {
error = "unrecognized element";
return sub;
}
}
// enforce sub-element types
msg[0] = '\0';
for (mjXSchema& subschema : subschema_) {
switch (subschema.type_) {
case '!':
if (subschema.refcnt_ > 1)
mju::sprintf_arr(msg, "unique element '%s' found %d times",
subschema.name_.c_str(), subschema.refcnt_);
else if (subschema.refcnt_ < 1)
mju::sprintf_arr(msg, "element '%s' is required",
subschema.name_.c_str());
break;
case '?':
if (subschema.refcnt_ > 1)
mju::sprintf_arr(msg, "unique element '%s' found %d times",
subschema.name_.c_str(), subschema.refcnt_);
break;
default:
break;
}
}
// handle error
if (msg[0]) {
error = msg;
return elem;
}
return nullptr;
}
//---------------------------------- class mjXUtil implementation ----------------------------------
// helper function to read multiple numerical values from an attribute
// return false if the entire attribute wasn't read (max was reached)
// throw error if syntax error while trying to read numerical data
template<typename T>
bool mjXUtil::ReadAttrValues(XMLElement* elem, const char* attr,
std::function<void (int, T)> push, int max) {
const char* pstr = elem->Attribute(attr);
T item;
if (pstr == nullptr) {
return true;
}
// get input stream
std::string str = std::string(pstr);
std::istringstream strm(str);
std::string token;
// read numbers
for (int i = 0; (max < 0 || i < max) && !strm.eof(); ++i) {
strm >> token;
std::istringstream token_strm(token);
token_strm >> item;
if (token_strm.fail() || !token_strm.eof()) {
// C++ standard libraries do not always parse inf and nan as valid floating point values.
std::optional<T> maybe_result = ParseInfOrNan<T>(token);
if (maybe_result.has_value()) {
item = maybe_result.value();
} else {
throw mjXError(elem, "problem reading attribute '%s'", attr);
}
}
push(i, item);
if constexpr (std::is_floating_point_v<T>) {
if (std::isnan(item)) {
mju_warning("XML contains a 'NaN'. Please check it carefully.");
}
}
// clear any trailing whitespace
strm >> std::ws;
}
return strm.eof();
}
template bool mjXUtil::ReadAttrValues(XMLElement* elem, const char* attr,
std::function<void (int, double)> push, int max);
template bool mjXUtil::ReadAttrValues(XMLElement* elem, const char* attr,
std::function<void (int, float)> push, int max);
template bool mjXUtil::ReadAttrValues(XMLElement* elem, const char* attr,
std::function<void (int, int)> push, int max);
template bool mjXUtil::ReadAttrValues(XMLElement* elem, const char* attr,
std::function<void (int, unsigned char)> push, int max);
// compare two vectors
template<typename T>
bool mjXUtil::SameVector(const T* vec1, const T* vec2, int n) {
if (!vec1 || !vec2) {
return false;
}
for (int i = 0; i < n; i++) {
if (std::abs(vec1[i] - vec2[i]) > std::numeric_limits<T>::epsilon()) {
return false;
}
}
return true;
}
template bool mjXUtil::SameVector(const double* vec1, const double* vec2, int n);
template bool mjXUtil::SameVector(const float* vec1, const float* vec2, int n);
template bool mjXUtil::SameVector(const int* vec1, const int* vec2, int n);
template bool mjXUtil::SameVector(const unsigned char* vec1, const unsigned char* vec2, int n);
// find string in map, return corresponding integer (-1: not found)
int mjXUtil::FindKey(const mjMap* map, int mapsz, std::string key) {
for (int i=0; i < mapsz; i++) {
if (map[i].key == key) {
return map[i].value;
}
}
return -1;
}
// find integer in map, return corresponding string ("": not found)
std::string mjXUtil::FindValue(const mjMap* map, int mapsz, int value) {
for (int i=0; i < mapsz; i++) {
if (map[i].value == value) {
return map[i].key;
}
}
return "";
}
// if attribute is present, return vector of numerical data
template<typename T>
std::optional<std::vector<T> > mjXUtil::ReadAttrVec(XMLElement* elem, const char* attr,
bool required) {
std::vector<T> v;
const char* raw_cstr = elem->Attribute(attr);
if (raw_cstr) {
v = mujoco::user::StringToVector<T>(raw_cstr);
if (errno == EDOM) {
mju_warning("XML contains a 'NaN'. Please check it carefully.");
} else if (errno == ERANGE) {
throw mjXError(elem, "number is too large in attribute '%s'", attr);
} else if (errno == EINVAL) {
throw mjXError(elem, "bad format in attribute '%s'", attr);
} else if (errno != 0) {
throw mjXError(elem, "unknown error in attribute '%s'", attr);
}
}
if (!v.size()) {
if (required) {
throw mjXError(elem, "required attribute missing: '%s'", attr);
} else {
return std::nullopt;
}
}
return v;
}
template std::optional<std::vector<double> >
mjXUtil::ReadAttrVec(XMLElement* elem, const char* attr, bool required);
template std::optional<std::vector<float> >
mjXUtil::ReadAttrVec(XMLElement* elem, const char* attr, bool required);
template std::optional<std::vector<int> >
mjXUtil::ReadAttrVec(XMLElement* elem, const char* attr, bool required);
template std::optional<std::vector<unsigned char> >
mjXUtil::ReadAttrVec(XMLElement* elem, const char* attr, bool required);
// if attribute is present, return attribute as a string
std::optional<std::string>
mjXUtil::ReadAttrStr(XMLElement* elem, const char* attr, bool required) {
const char* pstr = elem->Attribute(attr);
// check if attribute exists
if (pstr == nullptr) {
if (required) {
throw mjXError(elem, "required attribute missing: '%s'", attr);
} else {
return std::nullopt;
}
}
return std::string(pstr);
}
// if attribute is present, return attribute as a filename
std::optional<FilePath>
mjXUtil::ReadAttrFile(XMLElement* elem, const char* attr, const mjVFS* vfs,
const FilePath& dir, bool required) {
auto maybe_str = ReadAttrStr(elem, attr, required);
if (!maybe_str.has_value()) {
return std::nullopt;
}
FilePath filename(maybe_str.value());
return ResolveFilePath(elem, filename, dir, vfs);
}
// if attribute is present, return numerical value of attribute
template<typename T>
std::optional<T> mjXUtil::ReadAttrNum(XMLElement* elem, const char* attr,
bool required) {
auto maybe_arr = ReadAttrArr<T, 1>(elem, attr, required);
if (!maybe_arr.has_value()) {
return std::nullopt;
}
return maybe_arr.value()[0];
}
template std::optional<double>
mjXUtil::ReadAttrNum(XMLElement* elem, const char* attr, bool required);
template std::optional<float>
mjXUtil::ReadAttrNum(XMLElement* elem, const char* attr, bool required);
template std::optional<int>
mjXUtil::ReadAttrNum(XMLElement* elem, const char* attr, bool required);
template std::optional<unsigned char>
mjXUtil::ReadAttrNum(XMLElement* elem, const char* attr, bool required);
// read attribute "attr" of element "elem"
// "len" is the number of floats or doubles to be read
// the content is returned in "text", the numeric data in "data"
// return number of elements found
template<typename T>
int mjXUtil::ReadAttr(XMLElement* elem, const char* attr, const int len,
T* data, std::string& text, bool required, bool exact) {
auto maybe_vec = ReadAttrVec<T>(elem, attr, required);
if (!maybe_vec.has_value()) {
return 0;
}
// check if there is not enough data
if (exact && maybe_vec->size() < len) {
throw mjXError(elem, "attribute '%s' does not have enough data", attr);
}
// check if there is too much data
if (maybe_vec->size() > len) {
throw mjXError(elem, "attribute '%s' has too much data", attr);
}
std::copy(maybe_vec->begin(), maybe_vec->end(), data);
return maybe_vec->size();
}
template int mjXUtil::ReadAttr(XMLElement* elem, const char* attr, int len,
double* data, std::string& text, bool required, bool exact);
template int mjXUtil::ReadAttr(XMLElement* elem, const char* attr, int len,
float* data, std::string& text, bool required, bool exact);
template int mjXUtil::ReadAttr(XMLElement* elem, const char* attr, int len,
int* data, std::string& text, bool required, bool exact);
template int mjXUtil::ReadAttr(XMLElement* elem, const char* attr, int len,
unsigned char* data, std::string& text, bool required,
bool exact);
// read quaternion attribute
// throw error if identically zero
int mjXUtil::ReadQuat(XMLElement* elem, const char* attr, double* data, std::string& text,
bool required) {
int n = ReadAttr(elem, attr, /*len=*/4, data, text, required, /*exact=*/true);
if (n == 0) return 0;
// check for 0 quaternion
if (data[0] == 0 && data[1] == 0 && data[2] == 0 && data[3] == 0) {
throw mjXError(elem, "zero quaternion is not allowed");
}
return n;
}
// read DOUBLE array into C++ vector, return number read
int mjXUtil::ReadVector(XMLElement* elem, const char* attr,
std::vector<double>& vec, std::string& text, bool required) {
auto maybe_vec = ReadAttrVec<double>(elem, attr, required);
if (!maybe_vec.has_value()) {
return 0;
}
vec = std::move(maybe_vec.value());
return vec.size();
}
// read text field
bool mjXUtil::ReadAttrTxt(tinyxml2::XMLElement* elem, const char* attr,
std::string& text, bool required) {
auto maybe_str = ReadAttrStr(elem, attr, required);
if (!maybe_str.has_value()) {
return false;
}
text = maybe_str.value();
return true;
}
// read single int
bool mjXUtil::ReadAttrInt(XMLElement* elem, const char* attr, int* data, bool required) {
auto maybe_int = ReadAttrNum<int>(elem, attr, required);
if (!maybe_int.has_value()) {
return false;
}
*data = maybe_int.value();
return true;
}
// write vector<float> to string
void mjXUtil::Vector2String(std::string& txt, const std::vector<float>& vec, int ncol) {
std::stringstream strm;
for (size_t i=0; i < vec.size(); i++) {
if (ncol && (i % ncol) == 0) {
strm << "\n ";
} else if (i > 0) {
strm << " ";
}
strm << vec[i];
}
txt = strm.str();
}
// find subelement with given name, make sure it is unique
XMLElement* mjXUtil::FindSubElem(XMLElement* elem, std::string name, bool required) {
XMLElement* subelem = 0;
XMLElement* iter = elem->FirstChildElement();
while (iter) {
// identify elements with given name
if (name == iter->Value()) {
// make sure name is not repeated
if (subelem) {
throw mjXError(subelem, "repeated element: '%s'", name.c_str());
}
// save found element
subelem = iter;
}
// advance to next element
iter = iter->NextSiblingElement();
}
if (required && !subelem) {
throw mjXError(elem, "missing element: '%s'", name.c_str());
}
return subelem;
}
// find attribute, translate key into data, return true if found
bool mjXUtil::MapValue(XMLElement* elem, const char* attr, int* data,
const mjMap* map, int mapSz, bool required) {
// get attribute text
auto maybe_text = ReadAttrStr(elem, attr, required);
if (!maybe_text.has_value()) {
return false;
}
// find keyword in map
int value = FindKey(map, mapSz, maybe_text.value());
if (value < 0) {
throw mjXError(elem, "invalid keyword: '%s'", maybe_text->c_str());
}
// copy
*data = value;
return true;
}
// find attribute, translate unique space-separated keys to data, return number of keys found
int mjXUtil::MapValues(XMLElement* elem, const char* attr, int* data,
const mjMap* map, int mapSz, bool required) {
// get attribute text
auto maybe_text = ReadAttrStr(elem, attr, required);
if (!maybe_text.has_value()) {
return 0;
}
std::string text = maybe_text.value();
std::istringstream strm(text);
std::string key;
std::set<std::string> found_keys;
int count = 0;
while (strm >> key) {
if (found_keys.count(key)) {
throw mjXError(elem, "duplicate keyword: '%s'");
return 0;
}
int value = FindKey(map, mapSz, key);
if (value == -1) {
throw mjXError(elem, "invalid keyword: '%s'");
return 0;
}
found_keys.insert(key);
data[count++] = value;
}
return count;
}
//---------------------------------- write functions -----------------------------------------------
// check if double is int
static bool isint(double x) {
return ((std::abs(x - floor(x)) < 1E-12) || (std::abs(x - ceil(x)) < 1E-12));
}
// round to nearest int
static int Round(double x) {
if (std::abs(x - floor(x)) < std::abs(x - ceil(x))) {
return (int)floor(x);
} else {
return (int)ceil(x);
}
}
// write attribute
template<typename T>
void mjXUtil::WriteAttr(XMLElement* elem, std::string name, int n, const T* data, const T* def,
bool trim) {
// make sure all are defined
if constexpr (std::is_floating_point_v<T>) {
for (int i=0; i < n; i++) {
if (std::isnan(data[i])) {
return;
}
}
}
// skip default attributes
if (SameVector(data, def, n)) {
return;
}
// trim identical trailing default values
if (trim) {
while (n > 0 && data[n-1] == def[n-1]) {
n--;
}
}
// increase precision for testing
std::stringstream stream;
stream.precision(mujoco::_mjPRIVATE__get_xml_precision());
// process all numbers
for (int i=0; i < n; i++) {
// add space between numbers
if (i > 0) {
stream << " ";
}
// append number
double doubledata = static_cast<double>(data[i]);
if (doubledata < INT_MAX && doubledata > -INT_MAX && isint(data[i])) {
stream << Round(data[i]);
} else {
stream << data[i];
}
}
// set attribute as string
WriteAttrTxt(elem, name, stream.str());
}
template void mjXUtil::WriteAttr(XMLElement* elem, std::string name, int n,
const double* data, const double* def, bool trim);
template void mjXUtil::WriteAttr(XMLElement* elem, std::string name, int n,
const float* data, const float* def, bool trim);
template void mjXUtil::WriteAttr(XMLElement* elem, std::string name, int n,
const int* data, const int* def, bool trim);
template void mjXUtil::WriteAttr(XMLElement* elem, std::string name, int n,
const unsigned char* data,
const unsigned char* def, bool trim);
// write vector<double> attribute, default = zero array
void mjXUtil::WriteVector(XMLElement* elem, std::string name, const std::vector<double>& vec) {
// proceed only if non-zero found
bool ok = false;
for (size_t i=0; i < vec.size(); i++) {
if (vec[i]) {
ok = true;
break;
}
}
if (!ok) {
return;
}
// write
WriteAttr(elem, name, vec.size(), vec.data());
}
// write vector<double> attribute, default with same size
void mjXUtil::WriteVector(XMLElement* elem, std::string name, const std::vector<double>& vec,
const std::vector<double>& def) {
// proceed only if non-zero found
bool ok = false;
for (size_t i=0; i < vec.size(); i++) {
if (vec[i] != def[i]) {
ok = true;
break;
}
}
if (!ok) {
return;
}
// write
WriteAttr(elem, name, vec.size(), vec.data());
}
// write attribute- string
void mjXUtil::WriteAttrTxt(XMLElement* elem, std::string name, std::string value) {
// skip if value is empty
if (value.empty()) {
return;
}
// set attribute
elem->SetAttribute(name.c_str(), value.c_str());
}
// write attribute- single int
void mjXUtil::WriteAttrInt(XMLElement* elem, std::string name, int data, int def) {
// skip default
if (data == def) {
return;
}
elem->SetAttribute(name.c_str(), data);
}
// write attribute- keyword
void mjXUtil::WriteAttrKey(XMLElement* elem, std::string name,
const mjMap* map, int mapsz, int data, int def) {
// skip default
if (data == def) {
return;
}
WriteAttrTxt(elem, name, FindValue(map, mapsz, data));
}
// write attribute- space-separated keywords
void mjXUtil::WriteAttrKeys(XMLElement* elem, std::string name, const mjMap* map,
int mapsz, int* data, int ndata, int def) {
if (ndata <= 0) {
return;
}
// skip default
if (ndata == 1 && data[0] == def) {
return;
}
std::string text = FindValue(map, mapsz, data[0]);
for (int i = 1; i < ndata; ++i) {
text += " ";
text += FindValue(map, mapsz, data[i]);
}
WriteAttrTxt(elem, name, text);
}
Reference in New Issue View Git Blame Copy Permalink