Yuval Tassa
b475bb6f36
PiperOrigin-RevId: 958296165 Change-Id: I48cacc72c7df5994f5f816489ba069a5813845a1
2319 lines
74 KiB
C++
2319 lines
74 KiB
C++
// Copyright 2021 DeepMind Technologies Limited
|
|
//
|
|
// Licensed under the Apache License, Version 2.0 (the "License");
|
|
// you may not use this file except in compliance with the License.
|
|
// You may obtain a copy of the License at
|
|
//
|
|
// http://www.apache.org/licenses/LICENSE-2.0
|
|
//
|
|
// Unless required by applicable law or agreed to in writing, software
|
|
// distributed under the License is distributed on an "AS IS" BASIS,
|
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
// See the License for the specific language governing permissions and
|
|
// limitations under the License.
|
|
|
|
#include "xml/xml_native_writer.h"
|
|
|
|
#include <array>
|
|
#include <cstddef>
|
|
#include <cstdio>
|
|
#include <string>
|
|
#include <string_view>
|
|
#include <unordered_set>
|
|
#include <vector>
|
|
|
|
#include <mujoco/mjmodel.h>
|
|
#include <mujoco/mjplugin.h>
|
|
#include <mujoco/mjspec.h>
|
|
#include <mujoco/mujoco.h>
|
|
#include "engine/engine_io.h"
|
|
#include "engine/engine_plugin.h"
|
|
#include "engine/engine_support.h"
|
|
#include "engine/engine_util_errmem.h"
|
|
#include "engine/engine_util_misc.h"
|
|
#include "user/user_model.h"
|
|
#include "user/user_objects.h"
|
|
#include "user/user_util.h"
|
|
#include "xml/xml_base.h"
|
|
#include "xml/xml_util.h"
|
|
#include "tinyxml2.h"
|
|
|
|
// typed attribute rows, generated from mjcf.schema; shared with the reader
|
|
#include "xml/generated/mjcf_read_table.inc"
|
|
|
|
namespace {
|
|
|
|
using std::string;
|
|
using std::string_view;
|
|
using tinyxml2::XMLComment;
|
|
using tinyxml2::XMLDocument;
|
|
using tinyxml2::XMLElement;
|
|
using mujoco::user::VectorToString;
|
|
|
|
} // namespace
|
|
|
|
|
|
|
|
// custom XML indentation: 2 spaces rather than the default 4
|
|
class mj_XMLPrinter : public tinyxml2::XMLPrinter {
|
|
using tinyxml2::XMLPrinter::XMLPrinter;
|
|
|
|
public:
|
|
void PrintSpace( int depth ) {
|
|
for (int i=0; i < depth; ++i) {
|
|
Write( " " );
|
|
}
|
|
}
|
|
};
|
|
|
|
|
|
// save XML file using custom 2-space indentation
|
|
static string WriteDoc(XMLDocument& doc, char *error, size_t error_sz) {
|
|
doc.ClearError();
|
|
mj_XMLPrinter stream(nullptr, /*compact=*/false);
|
|
doc.Print(&stream);
|
|
if (doc.ErrorID()) {
|
|
mjCopyError(error, doc.ErrorStr(), error_sz);
|
|
return "";
|
|
}
|
|
string str = string(stream.CStr());
|
|
|
|
// top level sections
|
|
std::array<string, 17> sections = {
|
|
"<actuator", "<asset", "<compiler", "<contact>", "<custom",
|
|
"<default>", "<deformable", "<equality", "<extension", "<keyframe",
|
|
"<option", "<sensor", "<size", "<statistic", "<tendon",
|
|
"<visual", "<worldbody"};
|
|
|
|
// position of newline before first section
|
|
size_t first_pos = string::npos;
|
|
|
|
// insert newlines before section headers
|
|
for (const string& section : sections) {
|
|
std::size_t pos = 0;
|
|
while ((pos = str.find(section, pos)) != string::npos) {
|
|
// find newline before this section
|
|
std::size_t line_pos = str.rfind('\n', pos);
|
|
|
|
// save position of first section
|
|
if (line_pos < first_pos) first_pos = line_pos;
|
|
|
|
// insert another newline
|
|
if (line_pos != string::npos) {
|
|
str.insert(line_pos + 1, 1, '\n');
|
|
pos++; // account for inserted newline
|
|
}
|
|
|
|
// advance
|
|
pos += section.length();
|
|
}
|
|
}
|
|
|
|
// remove added newline before the first section
|
|
if (first_pos != string::npos) {
|
|
str.erase(first_pos, 1);
|
|
}
|
|
|
|
return str;
|
|
}
|
|
|
|
|
|
// insert end child with given name, return child
|
|
XMLElement* mjXWriter::InsertEnd(XMLElement* parent, const char* name) {
|
|
XMLElement* result = parent->GetDocument()->NewElement(name);
|
|
parent->InsertEndChild(result);
|
|
|
|
return result;
|
|
}
|
|
|
|
|
|
//---------------------------------- class mjXWriter: one-element writers --------------------------
|
|
|
|
// write flex
|
|
void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* flex) {
|
|
string text;
|
|
mjCFlex defflex;
|
|
|
|
// common attributes
|
|
WriteAttrTxt(elem, "name", flex->name);
|
|
WriteAttrTable(elem, static_cast<const mjsFlex*>(flex),
|
|
static_cast<const mjsFlex*>(&defflex), kFlexAttrs,
|
|
kFlexAttrsN);
|
|
if (flex->get_material() != defflex.get_material()) {
|
|
WriteAttrTxt(elem, "material", flex->get_material());
|
|
}
|
|
WriteAttr(elem, "cellcount", 3, flex->spec.cellcount, defflex.spec.cellcount);
|
|
if (flex->spec.order != defflex.spec.order) {
|
|
string dof_str = "full";
|
|
if (flex->spec.order == 1) dof_str = "trilinear";
|
|
else if (flex->spec.order == 2) dof_str = "quadratic";
|
|
WriteAttrTxt(elem, "dof", dof_str);
|
|
}
|
|
|
|
// data vectors
|
|
if (!flex->get_vertbody().empty()) {
|
|
text = VectorToString(flex->get_vertbody());
|
|
WriteAttrTxt(elem, "body", text);
|
|
}
|
|
if (!flex->get_vert().empty()) {
|
|
text = VectorToString(flex->get_vert());
|
|
WriteAttrTxt(elem, "vertex", text);
|
|
}
|
|
if (!flex->get_elem().empty()) {
|
|
text = VectorToString(flex->get_elem());
|
|
WriteAttrTxt(elem, "element", text);
|
|
}
|
|
if (!flex->get_texcoord().empty()) {
|
|
text = VectorToString(flex->get_texcoord());
|
|
WriteAttrTxt(elem, "texcoord", text);
|
|
}
|
|
if (!flex->get_elemtexcoord().empty()) {
|
|
text = VectorToString(flex->get_elemtexcoord());
|
|
WriteAttrTxt(elem, "elemtexcoord", text);
|
|
}
|
|
if (!flex->get_nodebody().empty()) {
|
|
text = VectorToString(flex->get_nodebody());
|
|
WriteAttrTxt(elem, "node", text);
|
|
}
|
|
|
|
// contact subelement
|
|
XMLElement* cont = InsertEnd(elem, "contact");
|
|
WriteAttrTable(cont, static_cast<const mjsFlex*>(flex),
|
|
static_cast<const mjsFlex*>(&defflex), kFlexcomp_contactAttrs,
|
|
kFlexcomp_contactAttrsN);
|
|
|
|
// remove contact is no attributes
|
|
if (!cont->FirstAttribute()) {
|
|
elem->DeleteChild(cont);
|
|
}
|
|
|
|
// elasticity subelement
|
|
XMLElement* elastic = InsertEnd(elem, "elasticity");
|
|
WriteAttrTable(elastic, static_cast<const mjsFlex*>(flex),
|
|
static_cast<const mjsFlex*>(&defflex), kElasticityAttrs,
|
|
kElasticityAttrsN);
|
|
|
|
// edge subelement
|
|
XMLElement* edge = InsertEnd(elem, "edge");
|
|
WriteAttrTable(edge, static_cast<const mjsFlex*>(flex),
|
|
static_cast<const mjsFlex*>(&defflex), kFlex_edgeAttrs,
|
|
kFlex_edgeAttrsN);
|
|
|
|
// remove edge if no attributes
|
|
if (!edge->FirstAttribute()) {
|
|
elem->DeleteChild(edge);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// write mesh
|
|
void mjXWriter::OneMesh(XMLElement* elem, const mjCMesh* mesh, mjCDef* def) {
|
|
string text;
|
|
|
|
// regular
|
|
if (!writingdefaults) {
|
|
WriteAttrTxt(elem, "name", mesh->name);
|
|
if (mesh->classname != "main") {
|
|
WriteAttrTxt(elem, "class", mesh->classname);
|
|
}
|
|
WriteAttrTxt(elem, "content_type", mesh->ContentType());
|
|
WriteAttrTxt(elem, "file", mesh->File());
|
|
|
|
// write vertex data
|
|
if (!mesh->UserVert().empty()) {
|
|
text = VectorToString(mesh->UserVert());
|
|
WriteAttrTxt(elem, "vertex", text);
|
|
}
|
|
|
|
// write normal data
|
|
if (!mesh->UserNormal().empty()) {
|
|
text = VectorToString(mesh->UserNormal());
|
|
WriteAttrTxt(elem, "normal", text);
|
|
}
|
|
|
|
// write texcoord data
|
|
if (!mesh->UserTexcoord().empty()) {
|
|
text = VectorToString(mesh->UserTexcoord());
|
|
WriteAttrTxt(elem, "texcoord", text);
|
|
}
|
|
|
|
// write face data
|
|
if (!mesh->UserFace().empty()) {
|
|
text = VectorToString(mesh->UserFace());
|
|
WriteAttrTxt(elem, "face", text);
|
|
}
|
|
}
|
|
|
|
// defaults and regular
|
|
WriteAttrTable(elem, static_cast<const mjsMesh*>(mesh), &def->Mesh().spec,
|
|
kMeshAttrs, kMeshAttrsN);
|
|
if (mesh->Material() != def->Mesh().Material()) {
|
|
WriteAttrTxt(elem, "material", mesh->Material());
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// write skin
|
|
void mjXWriter::OneSkin(XMLElement* elem, const mjCSkin* skin) {
|
|
string text;
|
|
mjCSkin defskin;
|
|
|
|
// write attributes
|
|
WriteAttrTxt(elem, "name", skin->name);
|
|
WriteAttrTxt(elem, "file", skin->File());
|
|
WriteAttrTxt(elem, "material", skin->get_material());
|
|
WriteAttrInt(elem, "group", skin->group, 0);
|
|
WriteAttrTable(elem, static_cast<const mjsSkin*>(skin),
|
|
static_cast<const mjsSkin*>(&defskin), kSkinAttrs,
|
|
kSkinAttrsN);
|
|
|
|
// write data if no file
|
|
if (skin->File().empty()) {
|
|
// mesh vert
|
|
text = VectorToString(skin->get_vert());
|
|
WriteAttrTxt(elem, "vertex", text);
|
|
|
|
// mesh texcoord
|
|
if (!skin->get_texcoord().empty()) {
|
|
text = VectorToString(skin->get_texcoord());
|
|
WriteAttrTxt(elem, "texcoord", text);
|
|
}
|
|
|
|
// mesh face
|
|
text = VectorToString(skin->get_face());
|
|
WriteAttrTxt(elem, "face", text);
|
|
|
|
// bones
|
|
for (size_t i=0; i < skin->get_bodyname().size(); i++) {
|
|
// make bone
|
|
XMLElement* bone = InsertEnd(elem, "bone");
|
|
|
|
// write attributes
|
|
WriteAttrTxt(bone, "body", skin->get_bodyname()[i]);
|
|
WriteAttr(bone, "bindpos", 3, skin->get_bindpos().data()+3*i);
|
|
WriteAttr(bone, "bindquat", 4, skin->get_bindquat().data()+4*i);
|
|
|
|
// write vertid
|
|
text = VectorToString(skin->get_vertid()[i]);
|
|
WriteAttrTxt(bone, "vertid", text);
|
|
|
|
// write vertweight
|
|
text = VectorToString(skin->get_vertweight()[i]);
|
|
WriteAttrTxt(bone, "vertweight", text);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// write material
|
|
void mjXWriter::OneMaterial(XMLElement* elem, const mjCMaterial* material, mjCDef* def) {
|
|
// regular
|
|
if (!writingdefaults) {
|
|
WriteAttrTxt(elem, "name", material->name);
|
|
if (material->classname != "main") {
|
|
WriteAttrTxt(elem, "class", material->classname);
|
|
}
|
|
}
|
|
|
|
// defaults and regular
|
|
// check if we have non-rgb textures
|
|
bool has_non_rgb = false;
|
|
for (int i=1; i < mjNTEXROLE; i++) {
|
|
if (!material->textures_[i].empty()) {
|
|
if (i != mjTEXROLE_RGB) {
|
|
has_non_rgb = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
// if we have non-rgb textures, write them as layers
|
|
if (has_non_rgb) {
|
|
for (int i=1; i < mjNTEXROLE; i++) {
|
|
if (!material->textures_[i].empty()) {
|
|
XMLElement * child_elem = InsertEnd(elem, "layer");
|
|
WriteAttrTxt(child_elem, "texture", material->textures_[i]);
|
|
WriteAttrTxt(child_elem, "role", FindValue(texrole_map, 9, i));
|
|
}
|
|
}
|
|
} else {
|
|
if (material->textures_[mjTEXROLE_RGB] != def->Material().textures_[mjTEXROLE_RGB]) {
|
|
WriteAttrTxt(elem, "texture", material->get_texture(mjTEXROLE_RGB));
|
|
}
|
|
}
|
|
|
|
WriteAttrTable(elem, static_cast<const mjsMaterial*>(material),
|
|
&def->Material().spec, kMaterialAttrs, kMaterialAttrsN);
|
|
}
|
|
|
|
|
|
|
|
// write joint
|
|
void mjXWriter::OneJoint(XMLElement* elem, const mjCJoint* joint, mjCDef* def,
|
|
string_view classname) {
|
|
// regular
|
|
if (!writingdefaults) {
|
|
WriteAttrTxt(elem, "name", joint->name);
|
|
if (classname != joint->classname && joint->classname != "main") {
|
|
WriteAttrTxt(elem, "class", joint->classname);
|
|
}
|
|
if (joint->type != mjJNT_FREE) {
|
|
WriteAttr(elem, "pos", 3, joint->pos);
|
|
}
|
|
if (joint->type != mjJNT_FREE && joint->type != mjJNT_BALL) {
|
|
WriteAttr(elem, "axis", 3, joint->axis);
|
|
}
|
|
}
|
|
|
|
// defaults and regular
|
|
WriteAttrTable(elem, static_cast<const mjsJoint*>(joint), &def->Joint().spec,
|
|
kJointAttrs, kJointAttrsN);
|
|
if (joint->type != mjJNT_FREE) {
|
|
WriteAttrKey(elem, "limited", FalseTrueAuto_map, 3, joint->limited, def->Joint().limited);
|
|
}
|
|
if (joint->type != mjJNT_FREE && joint->type != mjJNT_BALL) {
|
|
WriteAttrKey(elem, "actuatorfrclimited", FalseTrueAuto_map, 3, joint->actfrclimited,
|
|
def->Joint().actfrclimited);
|
|
}
|
|
|
|
// userdata
|
|
if (writingdefaults) {
|
|
WriteVector(elem, "user", joint->get_userdata());
|
|
} else {
|
|
WriteVector(elem, "user", joint->get_userdata(), def->Joint().get_userdata());
|
|
}
|
|
}
|
|
|
|
// write geom
|
|
void mjXWriter::OneGeom(XMLElement* elem, const mjCGeom* geom, mjCDef* def, string_view classname) {
|
|
double unitq[4] = {1, 0, 0, 0};
|
|
double mass = 0;
|
|
|
|
// regular
|
|
if (!writingdefaults) {
|
|
WriteAttrTxt(elem, "name", geom->name);
|
|
if (classname != geom->classname && geom->classname != "main") {
|
|
WriteAttrTxt(elem, "class", geom->classname);
|
|
}
|
|
if (mjGEOMINFO[geom->type]) {
|
|
WriteAttr(elem, "size", mjGEOMINFO[geom->type], geom->size, def->Geom().size);
|
|
}
|
|
if (mjuu_defined(geom->mass)) {
|
|
mass = geom->GetVolume() * def->Geom().density;
|
|
}
|
|
|
|
// mesh geom
|
|
if (geom->type == mjGEOM_MESH || geom->type == mjGEOM_SDF) {
|
|
mjCMesh* mesh = geom->mesh;
|
|
|
|
// write pos/quat if there is a difference
|
|
if (!SameVector(geom->pos, mesh->GetPosPtr(), 3) ||
|
|
!SameVector(geom->quat, mesh->GetQuatPtr(), 4)) {
|
|
// recover geom pos/quat before mesh frame transformation
|
|
double p[3], q[4];
|
|
mjuu_copyvec(p, geom->pos, 3);
|
|
mjuu_copyvec(q, geom->quat, 4);
|
|
mjuu_frameaccuminv(p, q, mesh->GetPosPtr(),
|
|
mesh->GetQuatPtr());
|
|
|
|
// write
|
|
WriteAttr(elem, "pos", 3, p, unitq+1);
|
|
WriteAttr(elem, "quat", 4, q, unitq);
|
|
}
|
|
}
|
|
|
|
// non-mesh geom
|
|
else {
|
|
WriteAttr(elem, "pos", 3, geom->pos, unitq+1);
|
|
WriteAttr(elem, "quat", 4, geom->quat, unitq);
|
|
}
|
|
} else {
|
|
WriteAttr(elem, "size", 3, geom->size, def->Geom().size);
|
|
}
|
|
|
|
// defaults and regular
|
|
WriteAttrTable(elem, static_cast<const mjsGeom*>(geom), &def->Geom().spec,
|
|
kGeomAttrs, kGeomAttrsN);
|
|
WriteAttrKey(elem, "fluidshape",
|
|
fluidshape_map, 2, geom->fluid_ellipsoid, def->Geom().fluid_ellipsoid);
|
|
if (geom->type != mjGEOM_MESH) {
|
|
WriteAttrKey(elem, "shellinertia", bool_map, 2, geom->typeinertia,
|
|
def->Geom().typeinertia);
|
|
}
|
|
if (mjuu_defined(geom->mass)) {
|
|
WriteAttr(elem, "mass", 1, &geom->mass_, &mass);
|
|
} else {
|
|
WriteAttr(elem, "density", 1, &geom->density, &def->Geom().density);
|
|
}
|
|
if (geom->get_material() != def->Geom().get_material()) {
|
|
WriteAttrTxt(elem, "material", geom->get_material());
|
|
}
|
|
|
|
// hfield and mesh attributes
|
|
if (geom->type == mjGEOM_HFIELD) {
|
|
WriteAttrTxt(elem, "hfield", geom->get_hfieldname());
|
|
}
|
|
if (geom->type == mjGEOM_MESH || geom->type == mjGEOM_SDF) {
|
|
WriteAttrTxt(elem, "mesh", geom->get_meshname());
|
|
}
|
|
|
|
// userdata
|
|
if (writingdefaults) {
|
|
WriteVector(elem, "user", geom->get_userdata());
|
|
} else {
|
|
WriteVector(elem, "user", geom->get_userdata(), def->Geom().get_userdata());
|
|
}
|
|
|
|
// write plugin
|
|
if (geom->plugin.active) {
|
|
OnePlugin(InsertEnd(elem, "plugin"), &geom->plugin);
|
|
}
|
|
}
|
|
|
|
// write site
|
|
void mjXWriter::OneSite(XMLElement* elem, const mjCSite* site, mjCDef* def, string_view classname) {
|
|
double unitq[4] = {1, 0, 0, 0};
|
|
|
|
// regular
|
|
if (!writingdefaults) {
|
|
WriteAttrTxt(elem, "name", site->name);
|
|
if (classname != site->classname && site->classname != "main") {
|
|
WriteAttrTxt(elem, "class", site->classname);
|
|
}
|
|
WriteAttr(elem, "quat", 4, site->quat, unitq);
|
|
if (mjGEOMINFO[site->type]) {
|
|
WriteAttr(elem, "size", mjGEOMINFO[site->type], site->size, def->Site().size);
|
|
}
|
|
} else {
|
|
WriteAttr(elem, "size", 3, site->size, def->Site().size);
|
|
}
|
|
|
|
// defaults and regular
|
|
WriteAttrTable(elem, static_cast<const mjsSite*>(site), &def->Site().spec,
|
|
kSiteAttrs, kSiteAttrsN);
|
|
if (site->get_material() != def->Site().get_material()) {
|
|
WriteAttrTxt(elem, "material", site->get_material());
|
|
}
|
|
|
|
// userdata
|
|
if (writingdefaults) {
|
|
WriteVector(elem, "user", site->get_userdata());
|
|
} else {
|
|
WriteVector(elem, "user", site->get_userdata(), def->Site().get_userdata());
|
|
}
|
|
}
|
|
|
|
// write camera
|
|
void mjXWriter::OneCamera(XMLElement* elem, const mjCCamera* camera, mjCDef* def,
|
|
string_view classname) {
|
|
double unitq[4] = {1, 0, 0, 0};
|
|
|
|
// regular
|
|
if (!writingdefaults) {
|
|
WriteAttrTxt(elem, "name", camera->name);
|
|
if (classname != camera->classname && camera->classname != "main") {
|
|
WriteAttrTxt(elem, "class", camera->classname);
|
|
}
|
|
WriteAttrTxt(elem, "target", camera->get_targetbody());
|
|
WriteAttr(elem, "quat", 4, camera->quat, unitq);
|
|
}
|
|
|
|
// defaults and regular
|
|
WriteAttrTable(elem, static_cast<const mjsCamera*>(camera),
|
|
&def->Camera().spec, kCameraAttrs, kCameraAttrsN);
|
|
|
|
// camera intrinsics if specified
|
|
if (camera->sensor_size[0] > 0 && camera->sensor_size[1] > 0) {
|
|
WriteAttr(elem, "sensorsize", 2, camera->sensor_size);
|
|
WriteAttr(elem, "focal", 2, camera->focal_length, def->Camera().focal_length);
|
|
WriteAttr(elem, "focalpixel", 2, camera->focal_pixel, def->Camera().focal_pixel);
|
|
WriteAttr(elem, "principal", 2, camera->principal_length, def->Camera().principal_length);
|
|
WriteAttr(elem, "principalpixel", 2, camera->principal_pixel, def->Camera().principal_pixel);
|
|
} else {
|
|
WriteAttr(elem, "fovy", 1, &camera->fovy, &def->Camera().fovy);
|
|
}
|
|
|
|
// userdata
|
|
if (writingdefaults) {
|
|
WriteVector(elem, "user", camera->get_userdata());
|
|
} else {
|
|
WriteVector(elem, "user", camera->get_userdata(), def->Camera().get_userdata());
|
|
}
|
|
}
|
|
|
|
// write light
|
|
void mjXWriter::OneLight(XMLElement* elem, const mjCLight* light, mjCDef* def,
|
|
string_view classname) {
|
|
// regular
|
|
if (!writingdefaults) {
|
|
WriteAttrTxt(elem, "name", light->name);
|
|
if (classname != light->classname && light->classname != "main") {
|
|
WriteAttrTxt(elem, "class", light->classname);
|
|
}
|
|
WriteAttrTxt(elem, "target", light->get_targetbody());
|
|
}
|
|
|
|
// defaults and regular
|
|
WriteAttrTable(elem, static_cast<const mjsLight*>(light), &def->Light().spec,
|
|
kLightAttrs, kLightAttrsN);
|
|
WriteAttrKey(elem, "type", lighttype_map, lighttype_sz, light->type, def->Light().type);
|
|
WriteAttrTxt(elem, "texture", light->get_texture());
|
|
}
|
|
|
|
// write pair
|
|
// write the mechanical attributes of an element, driven by the same
|
|
// generated rows the reader uses; see the declaration for the contract
|
|
template <typename T>
|
|
void mjXWriter::WriteAttrTable(XMLElement* elem, const T* obj, const T* def,
|
|
const mjXAttr* rows, int nrow) {
|
|
const char* live = reinterpret_cast<const char*>(obj);
|
|
const char* dflt = reinterpret_cast<const char*>(def);
|
|
for (int i = 0; i < nrow; i++) {
|
|
const mjXAttr& row = rows[i];
|
|
if (row.handwrite || (writingdefaults && row.nodefault)) {
|
|
continue;
|
|
}
|
|
const char* base = live + row.offset;
|
|
// a null default object means the element has no defaults to compare
|
|
// against: every defined value is written
|
|
const char* dbase = dflt ? dflt + row.offset : nullptr;
|
|
const int dkey = dflt ? 0 : -12345; // WriteAttrKey's write-always default
|
|
switch (row.kind) {
|
|
case mjXAttr::kInt:
|
|
WriteAttr(elem, row.attr, row.len, (const int*)base, (const int*)dbase,
|
|
/*trim=*/!row.exact);
|
|
break;
|
|
case mjXAttr::kDouble:
|
|
WriteAttr(elem, row.attr, row.len, (const double*)base,
|
|
(const double*)dbase, /*trim=*/!row.exact);
|
|
break;
|
|
case mjXAttr::kNum:
|
|
WriteAttr(elem, row.attr, row.len, (const mjtNum*)base,
|
|
(const mjtNum*)dbase, /*trim=*/!row.exact);
|
|
break;
|
|
case mjXAttr::kFloat:
|
|
WriteAttr(elem, row.attr, row.len, (const float*)base,
|
|
(const float*)dbase, /*trim=*/!row.exact);
|
|
break;
|
|
case mjXAttr::kEnum:
|
|
WriteAttrKey(elem, row.attr, row.map, row.mapsz, *(const int*)base,
|
|
dbase ? *(const int*)dbase : dkey);
|
|
break;
|
|
case mjXAttr::kEnumByte:
|
|
WriteAttrKey(elem, row.attr, row.map, row.mapsz, *(const mjtByte*)base,
|
|
dbase ? *(const mjtByte*)dbase : dkey);
|
|
break;
|
|
case mjXAttr::kBool:
|
|
WriteAttrKey(elem, row.attr, bool_map, 2, *(const mjtByte*)base,
|
|
dbase ? *(const mjtByte*)dbase : dkey);
|
|
break;
|
|
case mjXAttr::kFlags:
|
|
if (!dbase || *(const int*)base != *(const int*)dbase) {
|
|
int value = *(const int*)base;
|
|
std::vector<int> data;
|
|
for (int j = 0; j < row.mapsz; j++) {
|
|
if (value & row.map[j].value) {
|
|
data.push_back(row.map[j].value);
|
|
}
|
|
}
|
|
if (!data.empty()) {
|
|
WriteAttrKeys(elem, row.attr, row.map, row.mapsz, data.data(),
|
|
data.size(), 0);
|
|
}
|
|
}
|
|
break;
|
|
default:
|
|
// names, strings, files and custom-read attributes: OneX() remnant
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void mjXWriter::OnePair(XMLElement* elem, const mjCPair* pair, mjCDef* def) {
|
|
// regular
|
|
if (!writingdefaults) {
|
|
if (pair->classname != "main") {
|
|
WriteAttrTxt(elem, "class", pair->classname);
|
|
}
|
|
WriteAttrTxt(elem, "geom1", pair->get_geomname1());
|
|
WriteAttrTxt(elem, "geom2", pair->get_geomname2());
|
|
}
|
|
|
|
// defaults and regular
|
|
WriteAttrTxt(elem, "name", pair->name);
|
|
WriteAttrTable(elem, static_cast<const mjsPair*>(pair), &def->Pair().spec,
|
|
kPairAttrs, kPairAttrsN);
|
|
}
|
|
|
|
|
|
|
|
// write equality
|
|
void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* equality, mjCDef* def) {
|
|
// regular
|
|
if (!writingdefaults) {
|
|
WriteAttrTxt(elem, "name", equality->name);
|
|
if (equality->classname != "main") {
|
|
WriteAttrTxt(elem, "class", equality->classname);
|
|
}
|
|
|
|
switch (equality->type) {
|
|
case mjEQ_CONNECT:
|
|
if (equality->objtype == mjOBJ_BODY) {
|
|
WriteAttrTxt(elem, "body1", mjs_getString(equality->name1));
|
|
WriteAttrTxt(elem, "body2", mjs_getString(equality->name2));
|
|
WriteAttr(elem, "anchor", 3, equality->data);
|
|
} else {
|
|
WriteAttrTxt(elem, "site1", mjs_getString(equality->name1));
|
|
WriteAttrTxt(elem, "site2", mjs_getString(equality->name2));
|
|
}
|
|
break;
|
|
|
|
case mjEQ_WELD:
|
|
if (equality->objtype == mjOBJ_BODY) {
|
|
WriteAttrTxt(elem, "body1", mjs_getString(equality->name1));
|
|
WriteAttrTxt(elem, "body2", mjs_getString(equality->name2));
|
|
WriteAttr(elem, "anchor", 3, equality->data);
|
|
WriteAttr(elem, "relpose", 7, equality->data+3);
|
|
} else {
|
|
WriteAttrTxt(elem, "site1", mjs_getString(equality->name1));
|
|
WriteAttrTxt(elem, "site2", mjs_getString(equality->name2));
|
|
}
|
|
WriteAttr(elem, "torquescale", 1, equality->data+10);
|
|
break;
|
|
|
|
case mjEQ_JOINT:
|
|
WriteAttrTxt(elem, "joint1", mjs_getString(equality->name1));
|
|
WriteAttrTxt(elem, "joint2", mjs_getString(equality->name2));
|
|
WriteAttr(elem, "polycoef", 5, equality->data);
|
|
break;
|
|
|
|
case mjEQ_TENDON:
|
|
WriteAttrTxt(elem, "tendon1", mjs_getString(equality->name1));
|
|
WriteAttrTxt(elem, "tendon2", mjs_getString(equality->name2));
|
|
WriteAttr(elem, "polycoef", 5, equality->data);
|
|
break;
|
|
|
|
case mjEQ_FLEX:
|
|
case mjEQ_FLEXVERT:
|
|
WriteAttrTxt(elem, "flex", mjs_getString(equality->name1));
|
|
break;
|
|
|
|
case mjEQ_FLEXSTRAIN:
|
|
WriteAttrTxt(elem, "flex", mjs_getString(equality->name1));
|
|
WriteAttr(elem, "cell", 3, equality->data);
|
|
break;
|
|
|
|
default:
|
|
mju_error("mjXWriter: unknown equality type.");
|
|
}
|
|
}
|
|
|
|
// defaults and regular
|
|
WriteAttrTable(elem, static_cast<const mjsEquality*>(equality),
|
|
&def->Equality().spec, kEqualityBaseAttrs,
|
|
kEqualityBaseAttrsN);
|
|
}
|
|
|
|
|
|
|
|
// write tendon
|
|
void mjXWriter::OneTendon(XMLElement* elem, const mjCTendon* tendon, mjCDef* def) {
|
|
bool fixed = (tendon->GetWrap(0) && tendon->GetWrap(0)->Type() == mjWRAP_JOINT);
|
|
|
|
// regular
|
|
if (!writingdefaults) {
|
|
WriteAttrTxt(elem, "name", tendon->name);
|
|
if (tendon->classname != "main") {
|
|
WriteAttrTxt(elem, "class", tendon->classname);
|
|
}
|
|
}
|
|
|
|
// defaults and regular; the fixed rows are the spatial rows without the
|
|
// appearance attributes, which is exactly the tag difference
|
|
if (fixed) {
|
|
WriteAttrTable(elem, static_cast<const mjsTendon*>(tendon),
|
|
&def->Tendon().spec, kFixedAttrs, kFixedAttrsN);
|
|
} else {
|
|
WriteAttrTable(elem, static_cast<const mjsTendon*>(tendon),
|
|
&def->Tendon().spec, kSpatialAttrs, kSpatialAttrsN);
|
|
}
|
|
if (tendon->springlength[0] != tendon->springlength[1] ||
|
|
def->Tendon().springlength[0] != def->Tendon().springlength[1]) {
|
|
WriteAttr(elem, "springlength", 2, tendon->springlength, def->Tendon().springlength);
|
|
} else {
|
|
WriteAttr(elem, "springlength", 1, tendon->springlength, def->Tendon().springlength);
|
|
}
|
|
if (!fixed && tendon->get_material() != def->Tendon().get_material()) {
|
|
WriteAttrTxt(elem, "material", tendon->get_material());
|
|
}
|
|
|
|
// userdata
|
|
if (writingdefaults) {
|
|
WriteVector(elem, "user", tendon->get_userdata());
|
|
} else {
|
|
WriteVector(elem, "user", tendon->get_userdata(), def->Tendon().get_userdata());
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// write actuator
|
|
void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* actuator, mjCDef* def) {
|
|
// regular
|
|
if (!writingdefaults) {
|
|
WriteAttrTxt(elem, "name", actuator->name);
|
|
if (actuator->classname != "main") {
|
|
WriteAttrTxt(elem, "class", actuator->classname);
|
|
}
|
|
|
|
// transmission target
|
|
switch (actuator->trntype) {
|
|
case mjTRN_JOINT:
|
|
WriteAttrTxt(elem, "joint", actuator->get_target());
|
|
break;
|
|
|
|
case mjTRN_JOINTINPARENT:
|
|
WriteAttrTxt(elem, "jointinparent", actuator->get_target());
|
|
break;
|
|
|
|
case mjTRN_TENDON:
|
|
WriteAttrTxt(elem, "tendon", actuator->get_target());
|
|
break;
|
|
|
|
case mjTRN_SLIDERCRANK:
|
|
WriteAttrTxt(elem, "cranksite", actuator->get_target());
|
|
WriteAttrTxt(elem, "slidersite", actuator->get_slidersite());
|
|
break;
|
|
|
|
case mjTRN_SITE:
|
|
WriteAttrTxt(elem, "site", actuator->get_target());
|
|
WriteAttrTxt(elem, "refsite", actuator->get_refsite());
|
|
break;
|
|
|
|
case mjTRN_BODY:
|
|
WriteAttrTxt(elem, "body", actuator->get_target());
|
|
break;
|
|
|
|
default: // SHOULD NOT OCCUR
|
|
break;
|
|
}
|
|
}
|
|
|
|
// defaults and regular
|
|
WriteAttrTable(elem, static_cast<const mjsActuator*>(actuator),
|
|
&def->Actuator().spec, kGeneralAttrs, kGeneralAttrsN);
|
|
WriteAttr(elem, "cranklength", 1, &actuator->cranklength, &def->Actuator().cranklength);
|
|
// special handling of actdim which has default value of -1
|
|
if (writingdefaults) {
|
|
WriteAttrInt(elem, "actdim", actuator->actdim, def->Actuator().actdim);
|
|
} else {
|
|
int default_actdim = (actuator->dyntype != mjDYN_NONE && actuator->dyntype != mjDYN_DCMOTOR);
|
|
WriteAttrInt(elem, "actdim", actuator->actdim, default_actdim);
|
|
}
|
|
|
|
// plugins: write config attributes
|
|
if (actuator->plugin.active) {
|
|
OnePlugin(elem, &actuator->plugin);
|
|
}
|
|
|
|
// non-plugins: write actuator parameters
|
|
else {
|
|
WriteAttrKey(elem, "gaintype", gain_map, gain_sz, actuator->gaintype, def->Actuator().gaintype);
|
|
if (actuator->gaintype == mjGAIN_SO3) {
|
|
WriteAttrKey(elem, "input", inputchart_map, inputchart_sz, actuator->ctrlspec,
|
|
def->Actuator().ctrlspec);
|
|
} else if (actuator->ctrlspec != def->Actuator().ctrlspec) {
|
|
std::string tokens;
|
|
for (int k=0; k < inputbit_sz; k++) {
|
|
if (actuator->ctrlspec & inputbit_map[k].value) {
|
|
tokens += std::string(tokens.empty() ? "" : " ") + inputbit_map[k].key;
|
|
}
|
|
}
|
|
WriteAttrTxt(elem, "input", tokens);
|
|
}
|
|
WriteAttrKey(elem, "biastype", bias_map, bias_sz, actuator->biastype, def->Actuator().biastype);
|
|
WriteAttr(elem, "gainprm", mjNGAIN, actuator->gainprm, def->Actuator().gainprm, true);
|
|
WriteAttr(elem, "biasprm", mjNBIAS, actuator->biasprm, def->Actuator().biasprm, true);
|
|
}
|
|
|
|
// userdata
|
|
if (writingdefaults) {
|
|
WriteVector(elem, "user", actuator->get_userdata());
|
|
} else {
|
|
WriteVector(elem, "user", actuator->get_userdata(), def->Actuator().get_userdata());
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// write plugin
|
|
void mjXWriter::OnePlugin(XMLElement* elem, const mjsPlugin* plugin) {
|
|
const string instance_name = string(mjs_getString(plugin->name));
|
|
const string plugin_name = string(mjs_getString(plugin->plugin_name));
|
|
if (!instance_name.empty()) {
|
|
WriteAttrTxt(elem, "instance", instance_name);
|
|
} else {
|
|
WriteAttrTxt(elem, "plugin", plugin_name);
|
|
const mjpPlugin* pplugin = mjp_getPluginAtSlot(
|
|
static_cast<mjCPlugin*>(plugin->element)->plugin_slot);
|
|
const char* c = &(static_cast<mjCPlugin*>(plugin->element)->flattened_attributes[0]);
|
|
for (int i = 0; i < pplugin->nattribute; ++i) {
|
|
string value(c);
|
|
if (!value.empty()) {
|
|
XMLElement* config_elem = InsertEnd(elem, "config");
|
|
WriteAttrTxt(config_elem, "key", pplugin->attributes[i]);
|
|
WriteAttrTxt(config_elem, "value", value);
|
|
c += value.size();
|
|
}
|
|
++c;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
//---------------------------------- class mjXWriter: top-level API --------------------------------
|
|
|
|
// constructor
|
|
mjXWriter::mjXWriter(void) {
|
|
writingdefaults = false;
|
|
}
|
|
|
|
|
|
// cast model
|
|
void mjXWriter::SetModel(mjSpec* _spec, const mjModel* m) {
|
|
if (_spec) {
|
|
model = static_cast<mjCModel*>(_spec->element);
|
|
}
|
|
if (m) {
|
|
mj_copyBack(&model->spec, m);
|
|
}
|
|
}
|
|
|
|
|
|
// save existing model in MJCF canonical format, must be compiled
|
|
string mjXWriter::Write(char *error, size_t error_sz) {
|
|
// check model
|
|
if (!model || !model->IsCompiled()) {
|
|
mjCopyError(error, "XML Write error: Only compiled model can be written", error_sz);
|
|
return "";
|
|
}
|
|
|
|
// create document and root
|
|
XMLDocument doc;
|
|
XMLElement* root = doc.NewElement("mujoco");
|
|
root->SetAttribute("model", mjs_getString(model->modelname));
|
|
|
|
// insert root
|
|
doc.InsertFirstChild(root);
|
|
|
|
// write comment if present
|
|
string text = mjs_getString(model->comment);
|
|
if (!text.empty()) {
|
|
XMLComment* comment = doc.NewComment(text.c_str());
|
|
root->LinkEndChild(comment);
|
|
}
|
|
|
|
// create DOM
|
|
Compiler(root);
|
|
Option(root);
|
|
Size(root);
|
|
Statistic(root);
|
|
Visual(root);
|
|
writingdefaults = true;
|
|
Default(root, model->Default());
|
|
writingdefaults = false;
|
|
Extension(root);
|
|
Asset(root);
|
|
Body(InsertEnd(root, "worldbody"), model->GetWorld(), nullptr);
|
|
Deformable(root);
|
|
Contact(root);
|
|
Tendon(root);
|
|
Equality(root);
|
|
Actuator(root);
|
|
Sensor(root);
|
|
Custom(root);
|
|
Keyframe(root);
|
|
|
|
return WriteDoc(doc, error, error_sz);
|
|
}
|
|
|
|
|
|
|
|
// compiler section
|
|
void mjXWriter::Compiler(XMLElement* root) {
|
|
XMLElement* section = InsertEnd(root, "compiler");
|
|
|
|
// settings
|
|
WriteAttrTxt(section, "angle", "radian");
|
|
if (!model->get_meshdir().empty()) {
|
|
WriteAttrTxt(section, "meshdir", model->get_meshdir());
|
|
}
|
|
if (!model->get_texturedir().empty()) {
|
|
WriteAttrTxt(section, "texturedir", model->get_texturedir());
|
|
}
|
|
if (!model->compiler.usethread) {
|
|
WriteAttrTxt(section, "usethread", "false");
|
|
}
|
|
|
|
if (model->compiler.boundmass) {
|
|
WriteAttr(section, "boundmass", 1, &model->compiler.boundmass);
|
|
}
|
|
if (model->compiler.boundinertia) {
|
|
WriteAttr(section, "boundinertia", 1, &model->compiler.boundinertia);
|
|
}
|
|
if (model->compiler.alignfree) {
|
|
WriteAttrTxt(section, "alignfree", "true");
|
|
}
|
|
if (!model->compiler.autolimits) {
|
|
WriteAttrTxt(section, "autolimits", "false");
|
|
}
|
|
WriteAttrKey(section, "conflict", conflict_map, conflict_sz,
|
|
model->compiler.conflict, mjCONFLICT_WARNING);
|
|
}
|
|
|
|
|
|
|
|
// option section
|
|
void mjXWriter::Option(XMLElement* root) {
|
|
mjOption opt;
|
|
mj_defaultOption(&opt);
|
|
|
|
XMLElement* section = InsertEnd(root, "option");
|
|
|
|
// option; the freshly-defaulted struct is the comparison object
|
|
WriteAttrTable(section, &model->option, &opt, kOptionAttrs, kOptionAttrsN);
|
|
|
|
// actuator group disable
|
|
int disabled_groups[31];
|
|
int ndisabled = 0;
|
|
for (int i = 0; i < 31; ++i) {
|
|
if (model->option.disableactuator & (1 << i)) {
|
|
disabled_groups[ndisabled++] = i;
|
|
}
|
|
}
|
|
WriteAttr(section, "actuatorgroupdisable", ndisabled, disabled_groups);
|
|
|
|
// write disable/enable flags if any of them are set; invert while writing
|
|
if (model->option.disableflags || model->option.enableflags) {
|
|
XMLElement* sub = InsertEnd(section, "flag");
|
|
|
|
#define WRITEDSBL(NAME, MASK) \
|
|
if (model->option.disableflags & MASK) \
|
|
WriteAttrKey(sub, NAME, enable_map, 2, 0);
|
|
WRITEDSBL("constraint", mjDSBL_CONSTRAINT)
|
|
WRITEDSBL("equality", mjDSBL_EQUALITY)
|
|
WRITEDSBL("frictionloss", mjDSBL_FRICTIONLOSS)
|
|
WRITEDSBL("limit", mjDSBL_LIMIT)
|
|
WRITEDSBL("contact", mjDSBL_CONTACT)
|
|
WRITEDSBL("spring", mjDSBL_SPRING)
|
|
WRITEDSBL("damper", mjDSBL_DAMPER)
|
|
WRITEDSBL("gravity", mjDSBL_GRAVITY)
|
|
WRITEDSBL("clampctrl", mjDSBL_CLAMPCTRL)
|
|
WRITEDSBL("warmstart", mjDSBL_WARMSTART)
|
|
WRITEDSBL("filterparent", mjDSBL_FILTERPARENT)
|
|
WRITEDSBL("actuation", mjDSBL_ACTUATION)
|
|
WRITEDSBL("refsafe", mjDSBL_REFSAFE)
|
|
WRITEDSBL("sensor", mjDSBL_SENSOR)
|
|
WRITEDSBL("midphase", mjDSBL_MIDPHASE)
|
|
WRITEDSBL("eulerdamp", mjDSBL_EULERDAMP)
|
|
WRITEDSBL("autoreset", mjDSBL_AUTORESET)
|
|
WRITEDSBL("nativeccd", mjDSBL_NATIVECCD)
|
|
WRITEDSBL("island", mjDSBL_ISLAND)
|
|
WRITEDSBL("multiccd", mjDSBL_MULTICCD)
|
|
#undef WRITEDSBL
|
|
|
|
#define WRITEENBL(NAME, MASK) \
|
|
if (model->option.enableflags & MASK) \
|
|
WriteAttrKey(sub, NAME, enable_map, 2, 1);
|
|
WRITEENBL("override", mjENBL_OVERRIDE)
|
|
WRITEENBL("energy", mjENBL_ENERGY)
|
|
WRITEENBL("fwdinv", mjENBL_FWDINV)
|
|
WRITEENBL("invdiscrete", mjENBL_INVDISCRETE)
|
|
WRITEENBL("sleep", mjENBL_SLEEP)
|
|
WRITEENBL("diagexact", mjENBL_DIAGEXACT)
|
|
#undef WRITEENBL
|
|
}
|
|
|
|
// remove entire section if no attributes or elements
|
|
if (!section->FirstAttribute() && !section->FirstChildElement()) {
|
|
root->DeleteChild(section);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// size section
|
|
void mjXWriter::Size(XMLElement* root) {
|
|
XMLElement* section = InsertEnd(root, "size");
|
|
|
|
// write memory
|
|
if (model->memory != -1) {
|
|
WriteAttrTxt(section, "memory", mju_writeNumBytes(model->memory));
|
|
}
|
|
|
|
// deprecated sizes, hand-read into locals with range checks
|
|
WriteAttrInt(section, "njmax", model->njmax, -1);
|
|
WriteAttrInt(section, "nconmax", model->nconmax, -1);
|
|
WriteAttrInt(section, "nstack", model->nstack, -1);
|
|
|
|
// write sizes; the spec defaults are -1 (auto), but compilation resolves
|
|
// them to the actual counts, so the comparison object is zero-initialized
|
|
mjSpec zerospec = {};
|
|
WriteAttrTable(section, static_cast<const mjSpec*>(model), &zerospec,
|
|
kSizeAttrs, kSizeAttrsN);
|
|
|
|
// remove entire section if no attributes
|
|
if (!section->FirstAttribute()) root->DeleteChild(section);
|
|
}
|
|
|
|
|
|
|
|
// statistic section
|
|
void mjXWriter::Statistic(XMLElement* root) {
|
|
XMLElement* section = InsertEnd(root, "statistic");
|
|
|
|
// statistics are unset (mjNAN) rather than defaulted: there is nothing to
|
|
// compare against, and WriteAttr skips the undefined values by itself
|
|
WriteAttrTable(section, &model->stat, (const mjStatistic*)nullptr,
|
|
kStatisticAttrs, kStatisticAttrsN);
|
|
|
|
// remove entire section if no attributes
|
|
if (!section->FirstAttribute()) root->DeleteChild(section);
|
|
}
|
|
|
|
|
|
|
|
// visual section
|
|
void mjXWriter::Visual(XMLElement* root) {
|
|
mjVisual visdef, *vis = &model->visual;
|
|
mj_defaultVisual(&visdef);
|
|
|
|
XMLElement* section = InsertEnd(root, "visual");
|
|
|
|
// the sub-sections are projections into mjVisual: their rows carry
|
|
// member-path offsets, so one struct pair drives them all
|
|
struct { const char* tag; const mjXAttr* rows; int n; } subs[] = {
|
|
{"global", kGlobalAttrs, kGlobalAttrsN},
|
|
{"quality", kQualityAttrs, kQualityAttrsN},
|
|
{"headlight", kHeadlightAttrs, kHeadlightAttrsN},
|
|
{"map", kMapAttrs, kMapAttrsN},
|
|
{"scale", kScaleAttrs, kScaleAttrsN},
|
|
{"rgba", kRgbaAttrs, kRgbaAttrsN},
|
|
};
|
|
for (const auto& sub : subs) {
|
|
XMLElement* elem = InsertEnd(section, sub.tag);
|
|
WriteAttrTable(elem, vis, &visdef, sub.rows, sub.n);
|
|
if (!elem->FirstAttribute()) {
|
|
section->DeleteChild(elem);
|
|
}
|
|
}
|
|
|
|
// remove entire section if no elements
|
|
if (!section->FirstChildElement()) {
|
|
root->DeleteChild(section);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// default section
|
|
void mjXWriter::Default(XMLElement* root, mjCDef* def) {
|
|
XMLElement* elem;
|
|
XMLElement* section;
|
|
|
|
// pointer to parent defaults
|
|
mjCDef* parent;
|
|
if (def->parent) {
|
|
parent = def->parent;
|
|
} else {
|
|
parent = new mjCDef;
|
|
}
|
|
|
|
// create section, write class name
|
|
section = InsertEnd(root, "default");
|
|
if (def->name != "main") {
|
|
WriteAttrTxt(section, "class", def->name);
|
|
}
|
|
|
|
// mesh
|
|
elem = InsertEnd(section, "mesh");
|
|
OneMesh(elem, &def->Mesh(), parent);
|
|
if (!elem->FirstAttribute()) section->DeleteChild(elem);
|
|
|
|
// material
|
|
elem = InsertEnd(section, "material");
|
|
OneMaterial(elem, &def->Material(), parent);
|
|
if (!elem->FirstAttribute()) section->DeleteChild(elem);
|
|
|
|
// joint
|
|
elem = InsertEnd(section, "joint");
|
|
OneJoint(elem, &def->Joint(), parent);
|
|
if (!elem->FirstAttribute()) section->DeleteChild(elem);
|
|
|
|
// geom
|
|
elem = InsertEnd(section, "geom");
|
|
OneGeom(elem, &def->Geom(), parent);
|
|
if (!elem->FirstAttribute()) section->DeleteChild(elem);
|
|
|
|
// site
|
|
elem = InsertEnd(section, "site");
|
|
OneSite(elem, &def->Site(), parent);
|
|
if (!elem->FirstAttribute()) section->DeleteChild(elem);
|
|
|
|
// camera
|
|
elem = InsertEnd(section, "camera");
|
|
OneCamera(elem, &def->Camera(), parent);
|
|
if (!elem->FirstAttribute()) section->DeleteChild(elem);
|
|
|
|
// light
|
|
elem = InsertEnd(section, "light");
|
|
OneLight(elem, &def->Light(), parent);
|
|
if (!elem->FirstAttribute()) section->DeleteChild(elem);
|
|
|
|
// pair
|
|
elem = InsertEnd(section, "pair");
|
|
OnePair(elem, &def->Pair(), parent);
|
|
if (!elem->FirstAttribute()) section->DeleteChild(elem);
|
|
|
|
// equality
|
|
elem = InsertEnd(section, "equality");
|
|
OneEquality(elem, &def->Equality(), parent);
|
|
if (!elem->FirstAttribute()) section->DeleteChild(elem);
|
|
|
|
// tendon
|
|
elem = InsertEnd(section, "tendon");
|
|
OneTendon(elem, &def->Tendon(), parent);
|
|
if (!elem->FirstAttribute()) section->DeleteChild(elem);
|
|
|
|
// actuator
|
|
elem = InsertEnd(section, "general");
|
|
OneActuator(elem, &def->Actuator(), parent);
|
|
if (!elem->FirstAttribute()) section->DeleteChild(elem);
|
|
|
|
// if top-level class has no members or children, delete it and return
|
|
if (!def->parent && section->NoChildren() && def->child.empty()) {
|
|
root->DeleteChild(section);
|
|
delete parent;
|
|
return;
|
|
}
|
|
|
|
// add children recursively
|
|
for (int i=0; i < (int)def->child.size(); i++) {
|
|
Default(section, def->child[i]);
|
|
}
|
|
|
|
// delete parent defaults if allocated here
|
|
if (!def->parent) {
|
|
delete parent;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// extension section
|
|
void mjXWriter::Extension(XMLElement* root) {
|
|
// skip section if there is no required plugin
|
|
if (model->ActivePlugins().empty()) {
|
|
return;
|
|
}
|
|
|
|
// create section
|
|
XMLElement* section = InsertEnd(root, "extension");
|
|
|
|
// keep track of plugins whose <plugin> section have been created
|
|
std::unordered_set<const mjpPlugin*> seen_plugins;
|
|
|
|
// write all plugins
|
|
const mjpPlugin* last_plugin = nullptr;
|
|
XMLElement* plugin_elem = nullptr;
|
|
for (int i = 0; i < model->Plugins().size(); ++i) {
|
|
mjCPlugin* pp = static_cast<mjCPlugin*>(model->GetObject(mjOBJ_PLUGIN, i));
|
|
|
|
if (pp->name.empty()) {
|
|
// reached the first unnamed plugin instance, meaning that it was created through an
|
|
// "implicit" plugin element, e.g. sensor or actuator
|
|
break;
|
|
}
|
|
|
|
// check if we need to open a new <plugin> section
|
|
const mjpPlugin* plugin = mjp_getPluginAtSlot(pp->plugin_slot);
|
|
if (plugin != last_plugin) {
|
|
plugin_elem = InsertEnd(section, "plugin");
|
|
WriteAttrTxt(plugin_elem, "plugin", plugin->name);
|
|
seen_plugins.insert(plugin);
|
|
last_plugin = plugin;
|
|
}
|
|
|
|
// write instance element
|
|
XMLElement* elem = InsertEnd(plugin_elem, "instance");
|
|
WriteAttrTxt(elem, "name", pp->name);
|
|
|
|
// write plugin config attributes
|
|
const char* c = &pp->flattened_attributes[0];
|
|
for (int i = 0; i < plugin->nattribute; ++i) {
|
|
string value(c);
|
|
if (!value.empty()) {
|
|
XMLElement* config_elem = InsertEnd(elem, "config");
|
|
WriteAttrTxt(config_elem, "key", plugin->attributes[i]);
|
|
WriteAttrTxt(config_elem, "value", value);
|
|
c += value.size();
|
|
}
|
|
++c;
|
|
}
|
|
}
|
|
|
|
// write <plugin> elements for plugins without explicit instances
|
|
for (const auto& [plugin, slot] : model->ActivePlugins()) {
|
|
if (seen_plugins.find(plugin) == seen_plugins.end()) {
|
|
plugin_elem = InsertEnd(section, "plugin");
|
|
WriteAttrTxt(plugin_elem, "plugin", plugin->name);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// custom section
|
|
void mjXWriter::Custom(XMLElement* root) {
|
|
XMLElement* elem;
|
|
|
|
// get sizes, skip section if empty
|
|
int nnum = model->NumObjects(mjOBJ_NUMERIC);
|
|
int ntxt = model->NumObjects(mjOBJ_TEXT);
|
|
int ntup = model->NumObjects(mjOBJ_TUPLE);
|
|
|
|
// skip section if empty
|
|
if (nnum == 0 && ntxt == 0 && ntup == 0) {
|
|
return;
|
|
}
|
|
|
|
// create section
|
|
XMLElement* section = InsertEnd(root, "custom");
|
|
|
|
// write all numerics
|
|
for (int i=0; i < nnum; i++) {
|
|
mjCNumeric* numeric = (mjCNumeric*)model->GetObject(mjOBJ_NUMERIC, i);
|
|
elem = InsertEnd(section, "numeric");
|
|
WriteAttrTxt(elem, "name", numeric->name);
|
|
WriteAttrInt(elem, "size", numeric->size);
|
|
WriteAttr(elem, "data", numeric->size, numeric->data_.data());
|
|
}
|
|
|
|
// write all texts
|
|
for (int i=0; i < ntxt; i++) {
|
|
mjCText* text = (mjCText*)model->GetObject(mjOBJ_TEXT, i);
|
|
elem = InsertEnd(section, "text");
|
|
WriteAttrTxt(elem, "name", text->name);
|
|
WriteAttrTxt(elem, "data", text->data_.c_str());
|
|
}
|
|
|
|
// write all tuples
|
|
for (int i=0; i < ntup; i++) {
|
|
mjCTuple* tuple = (mjCTuple*)model->GetObject(mjOBJ_TUPLE, i);
|
|
elem = InsertEnd(section, "tuple");
|
|
WriteAttrTxt(elem, "name", tuple->name);
|
|
|
|
// write objects in tuple
|
|
for (int j=0; j < (int)tuple->objtype_.size(); j++) {
|
|
XMLElement* obj = InsertEnd(elem, "element");
|
|
WriteAttrTxt(obj, "objtype", mju_type2Str((int)tuple->objtype_[j]));
|
|
WriteAttrTxt(obj, "objname", tuple->objname_[j].c_str());
|
|
double oprm = tuple->objprm_[j];
|
|
if (oprm != 0) {
|
|
WriteAttr(obj, "prm", 1, &oprm);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// asset section
|
|
void mjXWriter::Asset(XMLElement* root) {
|
|
XMLElement* elem;
|
|
|
|
// get sizes
|
|
int ntex = model->NumObjects(mjOBJ_TEXTURE);
|
|
int nmat = model->NumObjects(mjOBJ_MATERIAL);
|
|
int nmesh = model->NumObjects(mjOBJ_MESH);
|
|
int nhfield = model->NumObjects(mjOBJ_HFIELD);
|
|
|
|
// return if empty
|
|
if (ntex == 0 && nmat == 0 && nmesh == 0 && nhfield == 0) {
|
|
return;
|
|
}
|
|
|
|
// create section
|
|
XMLElement* section = InsertEnd(root, "asset");
|
|
|
|
// write textures
|
|
mjCTexture deftex(0);
|
|
for (int i=0; i < ntex; i++) {
|
|
// create element
|
|
mjCTexture* texture = (mjCTexture*)model->GetObject(mjOBJ_TEXTURE, i);
|
|
elem = InsertEnd(section, "texture");
|
|
|
|
// write common attributes
|
|
WriteAttrKey(elem, "type", texture_map, texture_sz, texture->type);
|
|
WriteAttrKey(elem, "colorspace", colorspace_map, colorspace_sz, texture->colorspace);
|
|
WriteAttrTxt(elem, "name", texture->name);
|
|
|
|
// write builtin
|
|
if (texture->builtin != mjBUILTIN_NONE) {
|
|
WriteAttrKey(elem, "builtin", builtin_map, builtin_sz, texture->builtin);
|
|
WriteAttrKey(elem, "mark", mark_map, mark_sz, texture->mark, deftex.mark);
|
|
WriteAttr(elem, "rgb1", 3, texture->rgb1, deftex.rgb1);
|
|
WriteAttr(elem, "rgb2", 3, texture->rgb2, deftex.rgb2);
|
|
WriteAttr(elem, "markrgb", 3, texture->markrgb, deftex.markrgb);
|
|
WriteAttr(elem, "random", 1, &texture->random, &deftex.random);
|
|
WriteAttrInt(elem, "width", texture->width);
|
|
WriteAttrInt(elem, "height", texture->height);
|
|
}
|
|
|
|
// write buffer
|
|
else if (texture->get_cubefiles()[0].empty() && texture->get_cubefiles()[1].empty() &&
|
|
texture->get_cubefiles()[2].empty() && texture->get_cubefiles()[3].empty() &&
|
|
texture->get_cubefiles()[4].empty() && texture->get_cubefiles()[5].empty() &&
|
|
texture->File().empty() && texture->gridsize[0] == 1 && texture->gridsize[1] == 1) {
|
|
throw mjXError(0, "no support for buffer textures.");
|
|
}
|
|
|
|
// write textures loaded from files
|
|
else {
|
|
// write single file
|
|
WriteAttrTxt(elem, "content_type", texture->get_content_type());
|
|
WriteAttrTxt(elem, "file", texture->File());
|
|
|
|
// write separate files
|
|
WriteAttrTxt(elem, "fileright", texture->get_cubefiles()[0]);
|
|
WriteAttrTxt(elem, "fileleft", texture->get_cubefiles()[1]);
|
|
WriteAttrTxt(elem, "fileup", texture->get_cubefiles()[2]);
|
|
WriteAttrTxt(elem, "filedown", texture->get_cubefiles()[3]);
|
|
WriteAttrTxt(elem, "filefront", texture->get_cubefiles()[4]);
|
|
WriteAttrTxt(elem, "fileback", texture->get_cubefiles()[5]);
|
|
if (texture->hflip) {
|
|
WriteAttrKey(elem, "hflip", bool_map, 2, 1);
|
|
}
|
|
if (texture->vflip) {
|
|
WriteAttrKey(elem, "vflip", bool_map, 2, 1);
|
|
}
|
|
|
|
// write grid
|
|
if (texture->gridsize[0] != 1 || texture->gridsize[1] != 1) {
|
|
double gsize[2] = { (double)texture->gridsize[0], (double)texture->gridsize[1] };
|
|
WriteAttr(elem, "gridsize", 2, gsize);
|
|
WriteAttrTxt(elem, "gridlayout", texture->gridlayout);
|
|
}
|
|
}
|
|
}
|
|
|
|
// write materials
|
|
for (int i=0; i < nmat; i++) {
|
|
// create element and write
|
|
mjCMaterial* material = (mjCMaterial*)model->GetObject(mjOBJ_MATERIAL, i);
|
|
elem = InsertEnd(section, "material");
|
|
OneMaterial(elem, material, model->def_map[material->classname]);
|
|
}
|
|
|
|
// write meshes
|
|
for (int i=0; i < nmesh; i++) {
|
|
// create element and write
|
|
mjCMesh* mesh = (mjCMesh*)model->GetObject(mjOBJ_MESH, i);
|
|
if (mesh->Plugin().active) {
|
|
elem = InsertEnd(section, "mesh");
|
|
WriteAttrTxt(elem, "name", mesh->name);
|
|
WriteAttrTxt(elem, "file", mesh->File());
|
|
OnePlugin(InsertEnd(elem, "plugin"), &mesh->Plugin());
|
|
} else{
|
|
elem = InsertEnd(section, "mesh");
|
|
OneMesh(elem, mesh, model->def_map[mesh->classname]);
|
|
}
|
|
}
|
|
|
|
// write hfields
|
|
for (int i=0; i < nhfield; i++) {
|
|
// create element
|
|
mjCHField* hfield = (mjCHField*)model->GetObject(mjOBJ_HFIELD, i);
|
|
elem = InsertEnd(section, "hfield");
|
|
|
|
// write attributes
|
|
WriteAttrTxt(elem, "name", hfield->name);
|
|
WriteAttr(elem, "size", 4, hfield->size);
|
|
if (!hfield->file_.empty()) {
|
|
WriteAttrTxt(elem, "content_type", hfield->content_type_);
|
|
WriteAttrTxt(elem, "file", hfield->file_);
|
|
} else {
|
|
int nrow = hfield->nrow;
|
|
int ncol = hfield->ncol;
|
|
WriteAttrInt(elem, "nrow", nrow);
|
|
WriteAttrInt(elem, "ncol", ncol);
|
|
if (!hfield->get_userdata().empty()) {
|
|
// copy in reverse row order, so XML string is top-to-bottom
|
|
std::vector<float> flipped(nrow * ncol);
|
|
const std::vector<float>& userdata = hfield->get_userdata();
|
|
for (int i = 0; i < nrow; i++) {
|
|
int flip = nrow - 1 - i;
|
|
for (int j = 0; j < ncol; j++) {
|
|
flipped[i * ncol + j] = userdata[flip * ncol + j];
|
|
}
|
|
}
|
|
|
|
string text;
|
|
Vector2String(text, flipped, ncol);
|
|
WriteAttrTxt(elem, "elevation", text);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
XMLElement* mjXWriter::OneFrame(XMLElement* elem, mjCFrame* frame) {
|
|
if (!frame) {
|
|
return elem;
|
|
}
|
|
|
|
// TODO: empty classname should not occur (but does)
|
|
if (frame->name.empty() && (frame->classname.empty() || frame->classname == "main")) {
|
|
return elem;
|
|
}
|
|
|
|
XMLElement* frame_elem = InsertEnd(elem, "frame");
|
|
WriteAttrTxt(frame_elem, "name", frame->name);
|
|
if (frame->classname != "main") {
|
|
WriteAttrTxt(frame_elem, "childclass", frame->classname);
|
|
}
|
|
return frame_elem;
|
|
}
|
|
|
|
|
|
|
|
// recursive body and frame writer
|
|
void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame, string_view childclass) {
|
|
double unitq[4] = {1, 0, 0, 0};
|
|
|
|
if (!body) {
|
|
throw mjXError(0, "missing body in XML write"); // SHOULD NOT OCCUR
|
|
}
|
|
|
|
// write body attributes and inertial
|
|
else if (!frame && body != model->GetWorld()) {
|
|
WriteAttrTxt(elem, "name", body->name);
|
|
if (childclass != body->classname && body->classname != "main") {
|
|
WriteAttrTxt(elem, "childclass", body->classname);
|
|
}
|
|
|
|
// write pos if it's not {0, 0, 0}
|
|
if (body->pos[0] || body->pos[1] || body->pos[2]) {
|
|
WriteAttr(elem, "pos", 3, body->pos);
|
|
}
|
|
WriteAttr(elem, "quat", 4, body->quat, unitq);
|
|
if (body->mocap) {
|
|
WriteAttrKey(elem, "mocap", bool_map, 2, 1);
|
|
}
|
|
|
|
// gravity compensation
|
|
if (body->gravcomp) {
|
|
WriteAttr(elem, "gravcomp", 1, &body->gravcomp);
|
|
}
|
|
|
|
// sleep policy
|
|
if (body->sleep != mjSLEEP_AUTO &&
|
|
body->sleep != mjSLEEP_AUTO_NEVER &&
|
|
body->sleep != mjSLEEP_AUTO_ALLOWED) {
|
|
WriteAttrKey(elem, "sleep", bodysleep_map, bodysleep_sz, body->sleep);
|
|
}
|
|
|
|
// simple optimization
|
|
WriteAttrKey(elem, "simple", FalseAuto_map, 2, body->simple, 1);
|
|
|
|
// userdata
|
|
WriteVector(elem, "user", body->get_userdata());
|
|
|
|
// write inertial
|
|
if (model->compiler.saveinertial ||
|
|
(body->explicitinertial && model->compiler.inertiafromgeom != mjINERTIAFROMGEOM_TRUE)) {
|
|
XMLElement* inertial = InsertEnd(elem, "inertial");
|
|
WriteAttr(inertial, "pos", 3, body->ipos);
|
|
WriteAttr(inertial, "quat", 4, body->iquat, unitq);
|
|
WriteAttr(inertial, "mass", 1, &body->mass);
|
|
WriteAttr(inertial, "diaginertia", 3, body->inertia);
|
|
}
|
|
}
|
|
|
|
// joints in this frame
|
|
for (int i = 0; i < body->joints.size(); i++) {
|
|
if (body->joints[i]->frame != frame) {
|
|
continue;
|
|
}
|
|
string classname = body->joints[i]->frame && !body->joints[i]->frame->classname.empty()
|
|
? body->joints[i]->frame->classname
|
|
: body->classname;
|
|
OneJoint(InsertEnd(elem, "joint"), body->joints[i],
|
|
model->def_map[body->joints[i]->classname],
|
|
classname.empty() ? childclass : classname);
|
|
}
|
|
|
|
// geoms in this frame
|
|
for (int i = 0; i < body->geoms.size(); i++) {
|
|
if (body->geoms[i]->frame != frame) {
|
|
continue;
|
|
}
|
|
string classname = body->geoms[i]->frame && !body->geoms[i]->frame->classname.empty()
|
|
? body->geoms[i]->frame->classname
|
|
: body->classname;
|
|
OneGeom(InsertEnd(elem, "geom"), body->geoms[i],
|
|
model->def_map[body->geoms[i]->classname],
|
|
classname.empty() ? childclass : classname);
|
|
}
|
|
|
|
// sites in this frame
|
|
for (int i = 0; i < body->sites.size(); i++) {
|
|
if (body->sites[i]->frame != frame) {
|
|
continue;
|
|
}
|
|
string classname = body->sites[i]->frame && !body->sites[i]->frame->classname.empty()
|
|
? body->sites[i]->frame->classname
|
|
: body->classname;
|
|
OneSite(InsertEnd(elem, "site"), body->sites[i],
|
|
model->def_map[body->sites[i]->classname],
|
|
classname.empty() ? childclass : classname);
|
|
}
|
|
|
|
// cameras in this frame
|
|
for (int i = 0; i < body->cameras.size(); i++) {
|
|
if (body->cameras[i]->frame != frame) {
|
|
continue;
|
|
}
|
|
string classname = body->cameras[i]->frame && !body->cameras[i]->frame->classname.empty()
|
|
? body->cameras[i]->frame->classname
|
|
: body->classname;
|
|
OneCamera(InsertEnd(elem, "camera"), body->cameras[i],
|
|
model->def_map[body->cameras[i]->classname],
|
|
classname.empty() ? childclass : classname);
|
|
}
|
|
|
|
// lights in this frame
|
|
for (int i = 0; i < body->lights.size(); i++) {
|
|
if (body->lights[i]->frame != frame) {
|
|
continue;
|
|
}
|
|
string classname = body->lights[i]->frame && !body->lights[i]->frame->classname.empty()
|
|
? body->lights[i]->frame->classname
|
|
: body->classname;
|
|
OneLight(InsertEnd(elem, "light"), body->lights[i],
|
|
model->def_map[body->lights[i]->classname],
|
|
classname.empty() ? childclass : classname);
|
|
}
|
|
|
|
// write plugin
|
|
if (body->plugin.active) {
|
|
OnePlugin(InsertEnd(elem, "plugin"), &body->plugin);
|
|
}
|
|
|
|
// write children recursively
|
|
int i = 0, j = 0;
|
|
while (i < body->bodies.size() || body->bodies.empty()) {
|
|
mjCFrame* bframe = body->bodies.empty() ? nullptr : body->bodies[i]->frame;
|
|
|
|
// write body if its frame matches the current frame, avoid access if there are no bodies
|
|
if (bframe == frame && !body->bodies.empty()) {
|
|
string classname = bframe && !bframe->classname.empty()
|
|
? bframe->classname
|
|
: body->classname;
|
|
Body(InsertEnd(elem, "body"), body->bodies[i], nullptr,
|
|
classname.empty() ? childclass : classname);
|
|
}
|
|
|
|
i++;
|
|
|
|
// do not go to frames until we reach a body with a frame or we are done with bodies
|
|
if (!bframe && i < body->bodies.size()) {
|
|
continue;
|
|
}
|
|
|
|
// loop over the remaining frames in the current body
|
|
while (j < body->frames.size()) {
|
|
mjCFrame* fframe = body->frames[j++];
|
|
|
|
// write frame if its frame matches the current frame
|
|
if (fframe->frame == frame) {
|
|
string classname = fframe && !fframe->classname.empty()
|
|
? fframe->classname
|
|
: body->classname;
|
|
Body(OneFrame(elem, fframe), body, fframe, childclass);
|
|
}
|
|
}
|
|
|
|
// if there are no bodies, we only want to run the loop once
|
|
if (body->bodies.empty()) {
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// collision section
|
|
void mjXWriter::Contact(XMLElement* root) {
|
|
XMLElement* elem;
|
|
|
|
// get number of pairs of each type
|
|
int npair = model->NumObjects(mjOBJ_PAIR);
|
|
int nexclude = model->NumObjects(mjOBJ_EXCLUDE);
|
|
|
|
// skip if section is empty
|
|
if (npair == 0 && nexclude == 0) {
|
|
return;
|
|
}
|
|
|
|
// create section
|
|
XMLElement* section = InsertEnd(root, "contact");
|
|
|
|
// write all geom pairs
|
|
for (int i=0; i < npair; i++) {
|
|
// create element and write
|
|
mjCPair* pair = (mjCPair*)model->GetObject(mjOBJ_PAIR, i);
|
|
elem = InsertEnd(section, "pair");
|
|
OnePair(elem, pair, model->def_map[pair->classname]);
|
|
}
|
|
|
|
// write all exclude pairs
|
|
for (int i=0; i < nexclude; i++) {
|
|
// create element
|
|
mjCBodyPair* exclude = (mjCBodyPair*)model->GetObject(mjOBJ_EXCLUDE, i);
|
|
elem = InsertEnd(section, "exclude");
|
|
|
|
// write attributes
|
|
WriteAttrTxt(elem, "name", exclude->name);
|
|
WriteAttrTxt(elem, "body1", exclude->get_bodyname1());
|
|
WriteAttrTxt(elem, "body2", exclude->get_bodyname2());
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// constraint section
|
|
void mjXWriter::Equality(XMLElement* root) {
|
|
// skip section if empty
|
|
int num;
|
|
if ((num=model->NumObjects(mjOBJ_EQUALITY)) == 0) {
|
|
return;
|
|
}
|
|
|
|
// create section
|
|
XMLElement* section = InsertEnd(root, "equality");
|
|
|
|
// write all constraints
|
|
for (int i=0; i < num; i++) {
|
|
mjCEquality* equality = (mjCEquality*)model->GetObject(mjOBJ_EQUALITY, i);
|
|
XMLElement* elem = InsertEnd(section,
|
|
FindValue(equality_map, equality_sz, equality->type).c_str());
|
|
OneEquality(elem, equality, model->def_map[equality->classname]);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// deformable section
|
|
void mjXWriter::Deformable(XMLElement* root) {
|
|
XMLElement* elem;
|
|
|
|
// get sizes
|
|
int nflex = model->NumObjects(mjOBJ_FLEX);
|
|
int nskin = model->NumObjects(mjOBJ_SKIN);
|
|
|
|
// return if empty
|
|
if (nflex == 0 && nskin == 0) {
|
|
return;
|
|
}
|
|
|
|
// create section
|
|
XMLElement* section = InsertEnd(root, "deformable");
|
|
|
|
// write flexes
|
|
for (int i=0; i < nflex; i++) {
|
|
// create element and write
|
|
mjCFlex* flex = (mjCFlex*)model->GetObject(mjOBJ_FLEX, i);
|
|
elem = InsertEnd(section, "flex");
|
|
OneFlex(elem, flex);
|
|
}
|
|
|
|
// write skins
|
|
for (int i=0; i < nskin; i++) {
|
|
// create element and write
|
|
mjCSkin* skin = (mjCSkin*)model->GetObject(mjOBJ_SKIN, i);
|
|
elem = InsertEnd(section, "skin");
|
|
OneSkin(elem, skin);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// tendon section
|
|
void mjXWriter::Tendon(XMLElement* root) {
|
|
// skip section if empty
|
|
int num;
|
|
if ((num=model->NumObjects(mjOBJ_TENDON)) == 0) {
|
|
return;
|
|
}
|
|
|
|
// create section
|
|
XMLElement* section = InsertEnd(root, "tendon");
|
|
|
|
// write all tendons
|
|
for (int i=0; i < num; i++) {
|
|
// write tendon element and attributes
|
|
mjCTendon* tendon = (mjCTendon*)model->GetObject(mjOBJ_TENDON, i);
|
|
if (!tendon->NumWraps()) { // SHOULD NOT OCCUR
|
|
continue;
|
|
}
|
|
XMLElement* elem = InsertEnd(section,
|
|
tendon->GetWrap(0)->Type() == mjWRAP_JOINT ? "fixed" : "spatial");
|
|
OneTendon(elem, tendon, model->def_map[tendon->classname]);
|
|
|
|
// write wraps
|
|
XMLElement* wrapelem;
|
|
for (int j=0; j < tendon->NumWraps(); j++) {
|
|
const mjCWrap* wrap = tendon->GetWrap(j);
|
|
switch (wrap->Type()) {
|
|
case mjWRAP_JOINT:
|
|
wrapelem = InsertEnd(elem, "joint");
|
|
WriteAttrTxt(wrapelem, "joint", wrap->obj->name);
|
|
WriteAttr(wrapelem, "coef", 1, &wrap->prm);
|
|
break;
|
|
|
|
case mjWRAP_SITE:
|
|
wrapelem = InsertEnd(elem, "site");
|
|
WriteAttrTxt(wrapelem, "site", wrap->obj->name);
|
|
break;
|
|
|
|
case mjWRAP_SPHERE:
|
|
case mjWRAP_CYLINDER:
|
|
wrapelem = InsertEnd(elem, "geom");
|
|
WriteAttrTxt(wrapelem, "geom", wrap->obj->name);
|
|
if (!wrap->sidesite.empty()) {
|
|
WriteAttrTxt(wrapelem, "sidesite", wrap->sidesite);
|
|
}
|
|
break;
|
|
|
|
case mjWRAP_PULLEY:
|
|
wrapelem = InsertEnd(elem, "pulley");
|
|
WriteAttr(wrapelem, "divisor", 1, &wrap->prm);
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// actuator section
|
|
void mjXWriter::Actuator(XMLElement* root) {
|
|
// skip section if empty
|
|
int num;
|
|
if ((num=model->NumObjects(mjOBJ_ACTUATOR)) == 0) {
|
|
return;
|
|
}
|
|
|
|
// create section
|
|
XMLElement* section = InsertEnd(root, "actuator");
|
|
|
|
// write all actuators
|
|
for (int i=0; i < num; i++) {
|
|
mjCActuator* actuator = (mjCActuator*)model->GetObject(mjOBJ_ACTUATOR, i);
|
|
XMLElement* elem;
|
|
if (actuator->plugin.active) {
|
|
elem = InsertEnd(section, "plugin");
|
|
} else {
|
|
elem = InsertEnd(section, "general");
|
|
}
|
|
OneActuator(elem, actuator, model->def_map[actuator->classname]);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// sensor section
|
|
void mjXWriter::Sensor(XMLElement* root) {
|
|
double zero = 0;
|
|
|
|
// skip section if empty
|
|
int num;
|
|
if ((num=model->NumObjects(mjOBJ_SENSOR)) == 0) {
|
|
return;
|
|
}
|
|
|
|
// create section
|
|
XMLElement* section = InsertEnd(root, "sensor");
|
|
|
|
// write all sensors
|
|
for (int i=0; i < num; i++) {
|
|
XMLElement* elem = 0;
|
|
mjCSensor* sensor = model->Sensors()[i];
|
|
string instance_name = "";
|
|
string plugin_name = "";
|
|
|
|
// write sensor type and type-specific attributes
|
|
switch (sensor->type) {
|
|
// common robotic sensors, attached to a site
|
|
case mjSENS_TOUCH:
|
|
elem = InsertEnd(section, "touch");
|
|
WriteAttrTxt(elem, "site", sensor->get_objname());
|
|
break;
|
|
case mjSENS_ACCELEROMETER:
|
|
elem = InsertEnd(section, "accelerometer");
|
|
WriteAttrTxt(elem, "site", sensor->get_objname());
|
|
break;
|
|
case mjSENS_VELOCIMETER:
|
|
elem = InsertEnd(section, "velocimeter");
|
|
WriteAttrTxt(elem, "site", sensor->get_objname());
|
|
break;
|
|
case mjSENS_GYRO:
|
|
elem = InsertEnd(section, "gyro");
|
|
WriteAttrTxt(elem, "site", sensor->get_objname());
|
|
break;
|
|
case mjSENS_FORCE:
|
|
elem = InsertEnd(section, "force");
|
|
WriteAttrTxt(elem, "site", sensor->get_objname());
|
|
break;
|
|
case mjSENS_TORQUE:
|
|
elem = InsertEnd(section, "torque");
|
|
WriteAttrTxt(elem, "site", sensor->get_objname());
|
|
break;
|
|
case mjSENS_MAGNETOMETER:
|
|
elem = InsertEnd(section, "magnetometer");
|
|
WriteAttrTxt(elem, "site", sensor->get_objname());
|
|
break;
|
|
case mjSENS_RANGEFINDER:
|
|
{
|
|
elem = InsertEnd(section, "rangefinder");
|
|
if (sensor->objtype == mjOBJ_SITE) {
|
|
WriteAttrTxt(elem, "site", sensor->get_objname());
|
|
} else {
|
|
WriteAttrTxt(elem, "camera", sensor->get_objname());
|
|
}
|
|
int dataspec = sensor->intprm[0];
|
|
int data[mjNRAYDATA];
|
|
int ndata = 0;
|
|
for (int i=0; i < mjNRAYDATA; i++) {
|
|
if (dataspec & (1 << i)) {
|
|
data[ndata++] = i;
|
|
}
|
|
}
|
|
WriteAttrKeys(elem, "data", raydata_map, mjNRAYDATA, data, ndata, 0);
|
|
}
|
|
break;
|
|
case mjSENS_CAMPROJECTION:
|
|
elem = InsertEnd(section, "camprojection");
|
|
WriteAttrTxt(elem, "site", sensor->get_objname());
|
|
WriteAttrTxt(elem, "camera", sensor->get_refname());
|
|
break;
|
|
|
|
// sensors related to scalar joints, tendons, actuators
|
|
case mjSENS_JOINTPOS:
|
|
elem = InsertEnd(section, "jointpos");
|
|
WriteAttrTxt(elem, "joint", sensor->get_objname());
|
|
break;
|
|
case mjSENS_JOINTVEL:
|
|
elem = InsertEnd(section, "jointvel");
|
|
WriteAttrTxt(elem, "joint", sensor->get_objname());
|
|
break;
|
|
case mjSENS_TENDONPOS:
|
|
elem = InsertEnd(section, "tendonpos");
|
|
WriteAttrTxt(elem, "tendon", sensor->get_objname());
|
|
break;
|
|
case mjSENS_TENDONVEL:
|
|
elem = InsertEnd(section, "tendonvel");
|
|
WriteAttrTxt(elem, "tendon", sensor->get_objname());
|
|
break;
|
|
case mjSENS_ACTUATORPOS:
|
|
elem = InsertEnd(section, "actuatorpos");
|
|
WriteAttrTxt(elem, "actuator", sensor->get_objname());
|
|
break;
|
|
case mjSENS_ACTUATORVEL:
|
|
elem = InsertEnd(section, "actuatorvel");
|
|
WriteAttrTxt(elem, "actuator", sensor->get_objname());
|
|
break;
|
|
case mjSENS_ACTUATORFRC:
|
|
elem = InsertEnd(section, "actuatorfrc");
|
|
WriteAttrTxt(elem, "actuator", sensor->get_objname());
|
|
break;
|
|
case mjSENS_JOINTACTFRC:
|
|
elem = InsertEnd(section, "jointactuatorfrc");
|
|
WriteAttrTxt(elem, "joint", sensor->get_objname());
|
|
break;
|
|
case mjSENS_TENDONACTFRC:
|
|
elem = InsertEnd(section, "tendonactuatorfrc");
|
|
WriteAttrTxt(elem, "tendon", sensor->get_objname());
|
|
break;
|
|
|
|
// sensors related to ball joints
|
|
case mjSENS_BALLQUAT:
|
|
elem = InsertEnd(section, "ballquat");
|
|
WriteAttrTxt(elem, "joint", sensor->get_objname());
|
|
break;
|
|
case mjSENS_BALLANGVEL:
|
|
elem = InsertEnd(section, "ballangvel");
|
|
WriteAttrTxt(elem, "joint", sensor->get_objname());
|
|
break;
|
|
|
|
// joint and tendon limit sensors
|
|
case mjSENS_JOINTLIMITPOS:
|
|
elem = InsertEnd(section, "jointlimitpos");
|
|
WriteAttrTxt(elem, "joint", sensor->get_objname());
|
|
break;
|
|
case mjSENS_JOINTLIMITVEL:
|
|
elem = InsertEnd(section, "jointlimitvel");
|
|
WriteAttrTxt(elem, "joint", sensor->get_objname());
|
|
break;
|
|
case mjSENS_JOINTLIMITFRC:
|
|
elem = InsertEnd(section, "jointlimitfrc");
|
|
WriteAttrTxt(elem, "joint", sensor->get_objname());
|
|
break;
|
|
case mjSENS_TENDONLIMITPOS:
|
|
elem = InsertEnd(section, "tendonlimitpos");
|
|
WriteAttrTxt(elem, "tendon", sensor->get_objname());
|
|
break;
|
|
case mjSENS_TENDONLIMITVEL:
|
|
elem = InsertEnd(section, "tendonlimitvel");
|
|
WriteAttrTxt(elem, "tendon", sensor->get_objname());
|
|
break;
|
|
case mjSENS_TENDONLIMITFRC:
|
|
elem = InsertEnd(section, "tendonlimitfrc");
|
|
WriteAttrTxt(elem, "tendon", sensor->get_objname());
|
|
break;
|
|
|
|
// sensors attached to an object with spatial frame: (x)body, geom, site, camera
|
|
case mjSENS_FRAMEPOS:
|
|
elem = InsertEnd(section, "framepos");
|
|
WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
|
|
WriteAttrTxt(elem, "objname", sensor->get_objname());
|
|
if (sensor->reftype != mjOBJ_UNKNOWN) {
|
|
WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype));
|
|
WriteAttrTxt(elem, "refname", sensor->get_refname());
|
|
}
|
|
break;
|
|
case mjSENS_FRAMEQUAT:
|
|
elem = InsertEnd(section, "framequat");
|
|
WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
|
|
WriteAttrTxt(elem, "objname", sensor->get_objname());
|
|
if (sensor->reftype != mjOBJ_UNKNOWN) {
|
|
WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype));
|
|
WriteAttrTxt(elem, "refname", sensor->get_refname());
|
|
}
|
|
break;
|
|
case mjSENS_FRAMEXAXIS:
|
|
elem = InsertEnd(section, "framexaxis");
|
|
WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
|
|
WriteAttrTxt(elem, "objname", sensor->get_objname());
|
|
if (sensor->reftype != mjOBJ_UNKNOWN) {
|
|
WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype));
|
|
WriteAttrTxt(elem, "refname", sensor->get_refname());
|
|
}
|
|
break;
|
|
case mjSENS_FRAMEYAXIS:
|
|
elem = InsertEnd(section, "frameyaxis");
|
|
WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
|
|
WriteAttrTxt(elem, "objname", sensor->get_objname());
|
|
if (sensor->reftype != mjOBJ_UNKNOWN) {
|
|
WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype));
|
|
WriteAttrTxt(elem, "refname", sensor->get_refname());
|
|
}
|
|
break;
|
|
case mjSENS_FRAMEZAXIS:
|
|
elem = InsertEnd(section, "framezaxis");
|
|
WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
|
|
WriteAttrTxt(elem, "objname", sensor->get_objname());
|
|
if (sensor->reftype != mjOBJ_UNKNOWN) {
|
|
WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype));
|
|
WriteAttrTxt(elem, "refname", sensor->get_refname());
|
|
}
|
|
break;
|
|
case mjSENS_FRAMELINVEL:
|
|
elem = InsertEnd(section, "framelinvel");
|
|
WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
|
|
WriteAttrTxt(elem, "objname", sensor->get_objname());
|
|
if (sensor->reftype != mjOBJ_UNKNOWN) {
|
|
WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype));
|
|
WriteAttrTxt(elem, "refname", sensor->get_refname());
|
|
}
|
|
break;
|
|
case mjSENS_FRAMEANGVEL:
|
|
elem = InsertEnd(section, "frameangvel");
|
|
WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
|
|
WriteAttrTxt(elem, "objname", sensor->get_objname());
|
|
if (sensor->reftype != mjOBJ_UNKNOWN) {
|
|
WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype));
|
|
WriteAttrTxt(elem, "refname", sensor->get_refname());
|
|
}
|
|
break;
|
|
case mjSENS_FRAMELINACC:
|
|
elem = InsertEnd(section, "framelinacc");
|
|
WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
|
|
WriteAttrTxt(elem, "objname", sensor->get_objname());
|
|
if (sensor->reftype != mjOBJ_UNKNOWN) {
|
|
WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype));
|
|
WriteAttrTxt(elem, "refname", sensor->get_refname());
|
|
}
|
|
break;
|
|
case mjSENS_FRAMEANGACC:
|
|
elem = InsertEnd(section, "frameangacc");
|
|
WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
|
|
WriteAttrTxt(elem, "objname", sensor->get_objname());
|
|
if (sensor->reftype != mjOBJ_UNKNOWN) {
|
|
WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype));
|
|
WriteAttrTxt(elem, "refname", sensor->get_refname());
|
|
}
|
|
break;
|
|
|
|
// sensors related to kinematic subtrees; attached to a body (which is the subtree root)
|
|
case mjSENS_SUBTREECOM:
|
|
elem = InsertEnd(section, "subtreecom");
|
|
WriteAttrTxt(elem, "body", sensor->get_objname());
|
|
break;
|
|
case mjSENS_SUBTREELINVEL:
|
|
elem = InsertEnd(section, "subtreelinvel");
|
|
WriteAttrTxt(elem, "body", sensor->get_objname());
|
|
break;
|
|
case mjSENS_SUBTREEANGMOM:
|
|
elem = InsertEnd(section, "subtreeangmom");
|
|
WriteAttrTxt(elem, "body", sensor->get_objname());
|
|
break;
|
|
case mjSENS_INSIDESITE:
|
|
elem = InsertEnd(section, "insidesite");
|
|
WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
|
|
WriteAttrTxt(elem, "objname", sensor->get_objname());
|
|
WriteAttrTxt(elem, "site", sensor->get_refname());
|
|
break;
|
|
case mjSENS_GEOMDIST:
|
|
elem = InsertEnd(section, "distance");
|
|
WriteAttrTxt(elem, sensor->objtype == mjOBJ_BODY ? "body1" : "geom1", sensor->get_objname());
|
|
WriteAttrTxt(elem, sensor->reftype == mjOBJ_BODY ? "body2" : "geom2", sensor->get_refname());
|
|
break;
|
|
case mjSENS_GEOMNORMAL:
|
|
elem = InsertEnd(section, "normal");
|
|
WriteAttrTxt(elem, sensor->objtype == mjOBJ_BODY ? "body1" : "geom1", sensor->get_objname());
|
|
WriteAttrTxt(elem, sensor->reftype == mjOBJ_BODY ? "body2" : "geom2", sensor->get_refname());
|
|
break;
|
|
case mjSENS_GEOMFROMTO:
|
|
elem = InsertEnd(section, "fromto");
|
|
WriteAttrTxt(elem, sensor->objtype == mjOBJ_BODY ? "body1" : "geom1", sensor->get_objname());
|
|
WriteAttrTxt(elem, sensor->reftype == mjOBJ_BODY ? "body2" : "geom2", sensor->get_refname());
|
|
break;
|
|
case mjSENS_CONTACT:
|
|
{
|
|
elem = InsertEnd(section, "contact");
|
|
if (sensor->objtype == mjOBJ_BODY) {
|
|
WriteAttrTxt(elem, "body1", sensor->get_objname());
|
|
} else if (sensor->objtype == mjOBJ_XBODY) {
|
|
WriteAttrTxt(elem, "subtree1", sensor->get_objname());
|
|
} else if (sensor->objtype == mjOBJ_GEOM) {
|
|
WriteAttrTxt(elem, "geom1", sensor->get_objname());
|
|
} else if (sensor->objtype == mjOBJ_SITE) {
|
|
WriteAttrTxt(elem, "site", sensor->get_objname());
|
|
}
|
|
if (sensor->reftype == mjOBJ_BODY) {
|
|
WriteAttrTxt(elem, "body2", sensor->get_refname());
|
|
} else if (sensor->reftype == mjOBJ_XBODY) {
|
|
WriteAttrTxt(elem, "subtree2", sensor->get_refname());
|
|
} else if (sensor->reftype == mjOBJ_GEOM) {
|
|
WriteAttrTxt(elem, "geom2", sensor->get_refname());
|
|
}
|
|
int dataspec = sensor->intprm[0];
|
|
WriteAttrInt(elem, "num", sensor->dim / mju_condataSize(dataspec), 1);
|
|
int data[mjNCONDATA];
|
|
int ndata = 0;
|
|
for (int i=0; i < mjNCONDATA; i++) {
|
|
if (dataspec & (1 << i)) {
|
|
data[ndata++] = i;
|
|
}
|
|
}
|
|
WriteAttrKeys(elem, "data", condata_map, mjNCONDATA, data, ndata, 0);
|
|
WriteAttrKey(elem, "reduce", reduce_map, reduce_sz, sensor->intprm[1], 0);
|
|
}
|
|
break;
|
|
case mjSENS_TACTILE:
|
|
elem = InsertEnd(section, "tactile");
|
|
WriteAttrTxt(elem, "geom", sensor->get_refname());
|
|
WriteAttrTxt(elem, "mesh", sensor->get_objname());
|
|
break;
|
|
// global sensors
|
|
case mjSENS_E_POTENTIAL:
|
|
elem = InsertEnd(section, "potential");
|
|
break;
|
|
case mjSENS_E_KINETIC:
|
|
elem = InsertEnd(section, "kinetic");
|
|
break;
|
|
case mjSENS_CLOCK:
|
|
elem = InsertEnd(section, "clock");
|
|
break;
|
|
|
|
|
|
// plugin-controlled sensor
|
|
case mjSENS_PLUGIN:
|
|
elem = InsertEnd(section, "plugin");
|
|
if (sensor->objtype != mjOBJ_UNKNOWN) {
|
|
WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
|
|
WriteAttrTxt(elem, "objname", sensor->get_objname());
|
|
}
|
|
OnePlugin(elem, &sensor->plugin);
|
|
break;
|
|
|
|
// user-defined sensor
|
|
case mjSENS_USER:
|
|
elem = InsertEnd(section, "user");
|
|
if (mju_type2Str(sensor->objtype)) {
|
|
WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype));
|
|
}
|
|
WriteAttrTxt(elem, "objname", sensor->get_objname());
|
|
WriteAttrInt(elem, "dim", sensor->dim);
|
|
WriteAttrKey(elem, "needstage", stage_map, stage_sz, (int)sensor->needstage);
|
|
WriteAttrKey(elem, "datatype", datatype_map, datatype_sz, (int)sensor->datatype);
|
|
break;
|
|
|
|
default:
|
|
mju_error("Unknown sensor type in XML write");
|
|
}
|
|
|
|
// write name, noise, userdata
|
|
WriteAttrTxt(elem, "name", sensor->name);
|
|
WriteAttr(elem, "cutoff", 1, &sensor->cutoff, &zero);
|
|
if (sensor->type != mjSENS_PLUGIN) {
|
|
WriteAttr(elem, "noise", 1, &sensor->noise, &zero);
|
|
}
|
|
WriteAttrInt(elem, "nsample", sensor->nsample, 0);
|
|
WriteAttrKey(elem, "interp", interp_map, interp_sz, sensor->interp, 0);
|
|
WriteAttr(elem, "delay", 1, &sensor->delay, &zero);
|
|
double zeros[2] = {0, 0};
|
|
WriteAttr(elem, "interval", 2, sensor->interval, zeros);
|
|
WriteVector(elem, "user", sensor->get_userdata());
|
|
}
|
|
|
|
// remove section if empty
|
|
if (!section->FirstChildElement()) {
|
|
root->DeleteChild(section);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// keyframe section
|
|
void mjXWriter::Keyframe(XMLElement* root) {
|
|
// create section
|
|
XMLElement* section = InsertEnd(root, "keyframe");
|
|
|
|
if (!model->key_pending_.empty()) {
|
|
throw mjXError(0, "Model has pending keyframes. It must be (re)compiled before writing XML.");
|
|
}
|
|
|
|
// write all keyframes
|
|
for (int i=0; i < model->nkey; i++) {
|
|
XMLElement* elem = InsertEnd(section, "key");
|
|
bool change = false;
|
|
|
|
mjCKey* key = model->Keys()[i];
|
|
|
|
// check name and write
|
|
if (!key->name.empty()) {
|
|
WriteAttrTxt(elem, "name", key->name);
|
|
change = true;
|
|
}
|
|
|
|
// check time and write
|
|
if (key->time != 0) {
|
|
WriteAttr(elem, "time", 1, &key->time);
|
|
change = true;
|
|
}
|
|
|
|
// check qpos and write
|
|
for (int j=0; j < model->nq; j++) {
|
|
if (key->qpos_[j] != model->qpos0[j]) {
|
|
WriteAttr(elem, "qpos", model->nq, key->qpos_.data());
|
|
change = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// check qvel and write
|
|
for (int j=0; j < model->nv; j++) {
|
|
if (key->qvel_[j] != 0) {
|
|
WriteAttr(elem, "qvel", model->nv, key->qvel_.data());
|
|
change = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// check act and write
|
|
for (int j=0; j < model->na; j++) {
|
|
if (key->act_[j] != 0) {
|
|
WriteAttr(elem, "act", model->na, key->act_.data());
|
|
change = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// check mpos and write
|
|
if (model->nmocap) {
|
|
for (int j=0; j < model->nbody; j++) {
|
|
if (model->Bodies()[j]->mocap) {
|
|
mjCBody* body = model->Bodies()[j];
|
|
int id = body->mocapid;
|
|
if (body->pos[0] != key->mpos_[3*id] ||
|
|
body->pos[1] != key->mpos_[3*id+1] ||
|
|
body->pos[2] != key->mpos_[3*id+2]) {
|
|
WriteAttr(elem, "mpos", 3*model->nmocap, key->mpos_.data());
|
|
change = true;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// check mquat and write
|
|
if (model->nmocap) {
|
|
for (int j=0; j < model->nbody; j++) {
|
|
if (model->Bodies()[j]->mocap) {
|
|
mjCBody* body = model->Bodies()[j];
|
|
int id = body->mocapid;
|
|
if (body->quat[0] != key->mquat_[4*id] ||
|
|
body->quat[1] != key->mquat_[4*id+1] ||
|
|
body->quat[2] != key->mquat_[4*id+2] ||
|
|
body->quat[3] != key->mquat_[4*id+3]) {
|
|
WriteAttr(elem, "mquat", 4*model->nmocap, key->mquat_.data());
|
|
change = true;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// check ctrl and write
|
|
for (int j=0; j < model->nu; j++) {
|
|
if (key->ctrl_[j] != 0) {
|
|
WriteAttr(elem, "ctrl", model->nu, key->ctrl_.data());
|
|
change = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// remove elem if empty
|
|
if (!change) {
|
|
section->DeleteChild(elem);
|
|
}
|
|
}
|
|
|
|
// remove section if empty
|
|
if (!section->FirstChildElement()) {
|
|
root->DeleteChild(section);
|
|
}
|
|
}
|