Yuval Tassa
9553926158
Interpolated flexes with pinned nodes could not be reloaded after saving:
pinned nodes share their parent body, and their positions within it lived
only in mjsFlex.node, which had no MJCF attribute. On reload the pinned
nodes collapsed onto the parent body origin, degenerating the trilinear
interpolation grid ("flex grid rotation R0 is not orthonormal"). This
made model/flex/strain.xml and gripper_trilinear.xml fail to round-trip.
Add flex/nodecoord, real(3*nnode), the node analog of flex/vertex: local
node coordinates within the corresponding body frames. The reader picks
it up from the regenerated schema tables; the writer emits it with the
precision-aware WriteVector, since VectorToString ignores the XML
precision setting and truncating node coordinates to 6 digits while body
positions carry 17 fails the R0 orthonormality check at full precision.
Add a WritesPinnedFlexNodes round-trip regression test, and remove the
two write-read sweep exclusions documenting this bug. The removed
substring filter "strain" was also matching core_constraint, silently
excluding that entire testdata directory from the sweep; its ~40 models
are now covered and pass.
PiperOrigin-RevId: 959025281
Change-Id: I2fed28c01491c5a8431e813102a423d12b659911
2330 lines
75 KiB
C++
2330 lines
75 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);
|
|
}
|
|
if (!flex->get_node().empty()) {
|
|
WriteVector(elem, "nodecoord", flex->get_node());
|
|
}
|
|
|
|
// contact subelement
|
|
XMLElement* cont = InsertEnd(elem, "contact");
|
|
WriteAttrTable(cont, static_cast<const mjsFlex*>(flex),
|
|
static_cast<const mjsFlex*>(&defflex), kFlexcomp_contactAttrs,
|
|
kFlexcomp_contactAttrsN);
|
|
|
|
// remove contact is no attributes
|
|
if (!cont->FirstAttribute()) {
|
|
elem->DeleteChild(cont);
|
|
}
|
|
|
|
// elasticity subelement
|
|
XMLElement* elastic = InsertEnd(elem, "elasticity");
|
|
WriteAttrTable(elastic, static_cast<const mjsFlex*>(flex),
|
|
static_cast<const mjsFlex*>(&defflex), kElasticityAttrs,
|
|
kElasticityAttrsN);
|
|
|
|
// edge subelement
|
|
XMLElement* edge = InsertEnd(elem, "edge");
|
|
WriteAttrTable(edge, static_cast<const mjsFlex*>(flex),
|
|
static_cast<const mjsFlex*>(&defflex), kFlex_edgeAttrs,
|
|
kFlex_edgeAttrsN);
|
|
|
|
// remove edge if no attributes
|
|
if (!edge->FirstAttribute()) {
|
|
elem->DeleteChild(edge);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// write mesh
|
|
void mjXWriter::OneMesh(XMLElement* elem, const mjCMesh* mesh, mjCDef* def) {
|
|
string text;
|
|
|
|
// regular
|
|
if (!writingdefaults) {
|
|
WriteAttrTxt(elem, "name", mesh->name);
|
|
if (mesh->classname != "main") {
|
|
WriteAttrTxt(elem, "class", mesh->classname);
|
|
}
|
|
WriteAttrTxt(elem, "content_type", mesh->ContentType());
|
|
WriteAttrTxt(elem, "file", mesh->File());
|
|
|
|
// write vertex data
|
|
if (!mesh->UserVert().empty()) {
|
|
text = VectorToString(mesh->UserVert());
|
|
WriteAttrTxt(elem, "vertex", text);
|
|
}
|
|
|
|
// write normal data
|
|
if (!mesh->UserNormal().empty()) {
|
|
text = VectorToString(mesh->UserNormal());
|
|
WriteAttrTxt(elem, "normal", text);
|
|
}
|
|
|
|
// write texcoord data
|
|
if (!mesh->UserTexcoord().empty()) {
|
|
text = VectorToString(mesh->UserTexcoord());
|
|
WriteAttrTxt(elem, "texcoord", text);
|
|
}
|
|
|
|
// write face data
|
|
if (!mesh->UserFace().empty()) {
|
|
text = VectorToString(mesh->UserFace());
|
|
WriteAttrTxt(elem, "face", text);
|
|
}
|
|
}
|
|
|
|
// defaults and regular
|
|
WriteAttrTable(elem, static_cast<const mjsMesh*>(mesh), &def->Mesh().spec,
|
|
kMeshAttrs, kMeshAttrsN);
|
|
if (mesh->Material() != def->Mesh().Material()) {
|
|
WriteAttrTxt(elem, "material", mesh->Material());
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// write skin
|
|
void mjXWriter::OneSkin(XMLElement* elem, const mjCSkin* skin) {
|
|
string text;
|
|
mjCSkin defskin;
|
|
|
|
// write attributes
|
|
WriteAttrTxt(elem, "name", skin->name);
|
|
WriteAttrTxt(elem, "file", skin->File());
|
|
WriteAttrTxt(elem, "material", skin->get_material());
|
|
WriteAttrInt(elem, "group", skin->group, 0);
|
|
WriteAttrTable(elem, static_cast<const mjsSkin*>(skin),
|
|
static_cast<const mjsSkin*>(&defskin), kSkinAttrs,
|
|
kSkinAttrsN);
|
|
|
|
// write data if no file
|
|
if (skin->File().empty()) {
|
|
// mesh vert
|
|
text = VectorToString(skin->get_vert());
|
|
WriteAttrTxt(elem, "vertex", text);
|
|
|
|
// mesh texcoord
|
|
if (!skin->get_texcoord().empty()) {
|
|
text = VectorToString(skin->get_texcoord());
|
|
WriteAttrTxt(elem, "texcoord", text);
|
|
}
|
|
|
|
// mesh face
|
|
text = VectorToString(skin->get_face());
|
|
WriteAttrTxt(elem, "face", text);
|
|
|
|
// bones
|
|
for (size_t i=0; i < skin->get_bodyname().size(); i++) {
|
|
// make bone
|
|
XMLElement* bone = InsertEnd(elem, "bone");
|
|
|
|
// write attributes
|
|
WriteAttrTxt(bone, "body", skin->get_bodyname()[i]);
|
|
WriteAttr(bone, "bindpos", 3, skin->get_bindpos().data()+3*i);
|
|
WriteAttr(bone, "bindquat", 4, skin->get_bindquat().data()+4*i);
|
|
|
|
// write vertid
|
|
text = VectorToString(skin->get_vertid()[i]);
|
|
WriteAttrTxt(bone, "vertid", text);
|
|
|
|
// write vertweight
|
|
text = VectorToString(skin->get_vertweight()[i]);
|
|
WriteAttrTxt(bone, "vertweight", text);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// write material
|
|
void mjXWriter::OneMaterial(XMLElement* elem, const mjCMaterial* material, mjCDef* def) {
|
|
// regular
|
|
if (!writingdefaults) {
|
|
WriteAttrTxt(elem, "name", material->name);
|
|
if (material->classname != "main") {
|
|
WriteAttrTxt(elem, "class", material->classname);
|
|
}
|
|
}
|
|
|
|
// defaults and regular
|
|
// check if we have non-rgb textures
|
|
bool has_non_rgb = false;
|
|
for (int i=1; i < mjNTEXROLE; i++) {
|
|
if (!material->textures_[i].empty()) {
|
|
if (i != mjTEXROLE_RGB) {
|
|
has_non_rgb = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
// if we have non-rgb textures, write them as layers
|
|
if (has_non_rgb) {
|
|
for (int i=1; i < mjNTEXROLE; i++) {
|
|
if (!material->textures_[i].empty()) {
|
|
XMLElement * child_elem = InsertEnd(elem, "layer");
|
|
WriteAttrTxt(child_elem, "texture", material->textures_[i]);
|
|
WriteAttrTxt(child_elem, "role", FindValue(texrole_map, 9, i));
|
|
}
|
|
}
|
|
} else {
|
|
if (material->textures_[mjTEXROLE_RGB] != def->Material().textures_[mjTEXROLE_RGB]) {
|
|
WriteAttrTxt(elem, "texture", material->get_texture(mjTEXROLE_RGB));
|
|
}
|
|
}
|
|
|
|
WriteAttrTable(elem, static_cast<const mjsMaterial*>(material),
|
|
&def->Material().spec, kMaterialAttrs, kMaterialAttrsN);
|
|
}
|
|
|
|
|
|
|
|
// write joint
|
|
void mjXWriter::OneJoint(XMLElement* elem, const mjCJoint* joint, mjCDef* def,
|
|
string_view classname) {
|
|
// regular
|
|
if (!writingdefaults) {
|
|
WriteAttrTxt(elem, "name", joint->name);
|
|
if (classname != joint->classname && joint->classname != "main") {
|
|
WriteAttrTxt(elem, "class", joint->classname);
|
|
}
|
|
}
|
|
|
|
// defaults and regular
|
|
WriteAttrTable(elem, static_cast<const mjsJoint*>(joint), &def->Joint().spec,
|
|
kJointAttrs, kJointAttrsN);
|
|
if (joint->type != mjJNT_FREE) {
|
|
WriteAttr(elem, "pos", 3, joint->pos, def->Joint().spec.pos);
|
|
}
|
|
if (joint->type != mjJNT_FREE && joint->type != mjJNT_BALL) {
|
|
WriteAttr(elem, "axis", 3, joint->axis, def->Joint().spec.axis);
|
|
}
|
|
if (joint->type != mjJNT_FREE) {
|
|
WriteAttrKey(elem, "limited", FalseTrueAuto_map, 3, joint->limited, def->Joint().limited);
|
|
}
|
|
if (joint->type != mjJNT_FREE && joint->type != mjJNT_BALL) {
|
|
WriteAttrKey(elem, "actuatorfrclimited", FalseTrueAuto_map, 3, joint->actfrclimited,
|
|
def->Joint().actfrclimited);
|
|
}
|
|
|
|
// userdata
|
|
if (writingdefaults) {
|
|
WriteVector(elem, "user", joint->get_userdata());
|
|
} else {
|
|
WriteVector(elem, "user", joint->get_userdata(), def->Joint().get_userdata());
|
|
}
|
|
}
|
|
|
|
// write geom
|
|
void mjXWriter::OneGeom(XMLElement* elem, const mjCGeom* geom, mjCDef* def, string_view classname) {
|
|
double unitq[4] = {1, 0, 0, 0};
|
|
double mass = 0;
|
|
|
|
// regular
|
|
if (!writingdefaults) {
|
|
WriteAttrTxt(elem, "name", geom->name);
|
|
if (classname != geom->classname && geom->classname != "main") {
|
|
WriteAttrTxt(elem, "class", geom->classname);
|
|
}
|
|
if (mjGEOMINFO[geom->type]) {
|
|
WriteAttr(elem, "size", mjGEOMINFO[geom->type], geom->size, def->Geom().size);
|
|
}
|
|
if (mjuu_defined(geom->mass)) {
|
|
mass = geom->GetVolume() * def->Geom().density;
|
|
}
|
|
|
|
// mesh geom
|
|
if (geom->type == mjGEOM_MESH || geom->type == mjGEOM_SDF) {
|
|
mjCMesh* mesh = geom->mesh;
|
|
|
|
// write pos/quat if there is a difference
|
|
if (!SameVector(geom->pos, mesh->GetPosPtr(), 3) ||
|
|
!SameVector(geom->quat, mesh->GetQuatPtr(), 4)) {
|
|
// recover geom pos/quat before mesh frame transformation
|
|
double p[3], q[4];
|
|
mjuu_copyvec(p, geom->pos, 3);
|
|
mjuu_copyvec(q, geom->quat, 4);
|
|
mjuu_frameaccuminv(p, q, mesh->GetPosPtr(),
|
|
mesh->GetQuatPtr());
|
|
|
|
// write
|
|
WriteAttr(elem, "pos", 3, p, unitq+1);
|
|
WriteAttr(elem, "quat", 4, q, unitq);
|
|
}
|
|
}
|
|
|
|
// non-mesh geom
|
|
else {
|
|
WriteAttr(elem, "pos", 3, geom->pos, unitq+1);
|
|
WriteAttr(elem, "quat", 4, geom->quat, unitq);
|
|
}
|
|
} else {
|
|
WriteAttr(elem, "size", 3, geom->size, def->Geom().size);
|
|
}
|
|
|
|
// defaults and regular
|
|
WriteAttrTable(elem, static_cast<const mjsGeom*>(geom), &def->Geom().spec,
|
|
kGeomAttrs, kGeomAttrsN);
|
|
WriteAttrKey(elem, "fluidshape",
|
|
fluidshape_map, 2, geom->fluid_ellipsoid, def->Geom().fluid_ellipsoid);
|
|
if (geom->type != mjGEOM_MESH) {
|
|
WriteAttrKey(elem, "shellinertia", bool_map, 2, geom->typeinertia,
|
|
def->Geom().typeinertia);
|
|
}
|
|
if (mjuu_defined(geom->mass)) {
|
|
WriteAttr(elem, "mass", 1, &geom->mass_, &mass);
|
|
} else {
|
|
WriteAttr(elem, "density", 1, &geom->density, &def->Geom().density);
|
|
}
|
|
if (geom->get_material() != def->Geom().get_material()) {
|
|
WriteAttrTxt(elem, "material", geom->get_material());
|
|
}
|
|
|
|
// hfield and mesh attributes
|
|
if (geom->type == mjGEOM_HFIELD) {
|
|
WriteAttrTxt(elem, "hfield", geom->get_hfieldname());
|
|
}
|
|
if (geom->type == mjGEOM_MESH || geom->type == mjGEOM_SDF) {
|
|
WriteAttrTxt(elem, "mesh", geom->get_meshname());
|
|
}
|
|
|
|
// userdata
|
|
if (writingdefaults) {
|
|
WriteVector(elem, "user", geom->get_userdata());
|
|
} else {
|
|
WriteVector(elem, "user", geom->get_userdata(), def->Geom().get_userdata());
|
|
}
|
|
|
|
// write plugin
|
|
if (geom->plugin.active) {
|
|
OnePlugin(InsertEnd(elem, "plugin"), &geom->plugin);
|
|
}
|
|
}
|
|
|
|
// write site
|
|
void mjXWriter::OneSite(XMLElement* elem, const mjCSite* site, mjCDef* def, string_view classname) {
|
|
double unitq[4] = {1, 0, 0, 0};
|
|
|
|
// regular
|
|
if (!writingdefaults) {
|
|
WriteAttrTxt(elem, "name", site->name);
|
|
if (classname != site->classname && site->classname != "main") {
|
|
WriteAttrTxt(elem, "class", site->classname);
|
|
}
|
|
WriteAttr(elem, "quat", 4, site->quat, unitq);
|
|
if (mjGEOMINFO[site->type]) {
|
|
WriteAttr(elem, "size", mjGEOMINFO[site->type], site->size, def->Site().size);
|
|
}
|
|
} else {
|
|
WriteAttr(elem, "size", 3, site->size, def->Site().size);
|
|
}
|
|
|
|
// defaults and regular
|
|
WriteAttrTable(elem, static_cast<const mjsSite*>(site), &def->Site().spec,
|
|
kSiteAttrs, kSiteAttrsN);
|
|
if (site->get_material() != def->Site().get_material()) {
|
|
WriteAttrTxt(elem, "material", site->get_material());
|
|
}
|
|
|
|
// userdata
|
|
if (writingdefaults) {
|
|
WriteVector(elem, "user", site->get_userdata());
|
|
} else {
|
|
WriteVector(elem, "user", site->get_userdata(), def->Site().get_userdata());
|
|
}
|
|
}
|
|
|
|
// write camera
|
|
void mjXWriter::OneCamera(XMLElement* elem, const mjCCamera* camera, mjCDef* def,
|
|
string_view classname) {
|
|
double unitq[4] = {1, 0, 0, 0};
|
|
|
|
// regular
|
|
if (!writingdefaults) {
|
|
WriteAttrTxt(elem, "name", camera->name);
|
|
if (classname != camera->classname && camera->classname != "main") {
|
|
WriteAttrTxt(elem, "class", camera->classname);
|
|
}
|
|
WriteAttrTxt(elem, "target", camera->get_targetbody());
|
|
WriteAttr(elem, "quat", 4, camera->quat, unitq);
|
|
}
|
|
|
|
// defaults and regular
|
|
WriteAttrTable(elem, static_cast<const mjsCamera*>(camera),
|
|
&def->Camera().spec, kCameraAttrs, kCameraAttrsN);
|
|
|
|
// camera intrinsics if specified
|
|
if (camera->sensor_size[0] > 0 && camera->sensor_size[1] > 0) {
|
|
WriteAttr(elem, "sensorsize", 2, camera->sensor_size);
|
|
WriteAttr(elem, "focal", 2, camera->focal_length, def->Camera().focal_length);
|
|
WriteAttr(elem, "focalpixel", 2, camera->focal_pixel, def->Camera().focal_pixel);
|
|
WriteAttr(elem, "principal", 2, camera->principal_length, def->Camera().principal_length);
|
|
WriteAttr(elem, "principalpixel", 2, camera->principal_pixel, def->Camera().principal_pixel);
|
|
} else {
|
|
WriteAttr(elem, "fovy", 1, &camera->fovy, &def->Camera().fovy);
|
|
}
|
|
|
|
// userdata
|
|
if (writingdefaults) {
|
|
WriteVector(elem, "user", camera->get_userdata());
|
|
} else {
|
|
WriteVector(elem, "user", camera->get_userdata(), def->Camera().get_userdata());
|
|
}
|
|
}
|
|
|
|
// write light
|
|
void mjXWriter::OneLight(XMLElement* elem, const mjCLight* light, mjCDef* def,
|
|
string_view classname) {
|
|
// regular
|
|
if (!writingdefaults) {
|
|
WriteAttrTxt(elem, "name", light->name);
|
|
if (classname != light->classname && light->classname != "main") {
|
|
WriteAttrTxt(elem, "class", light->classname);
|
|
}
|
|
WriteAttrTxt(elem, "target", light->get_targetbody());
|
|
}
|
|
|
|
// defaults and regular
|
|
WriteAttrTable(elem, static_cast<const mjsLight*>(light), &def->Light().spec,
|
|
kLightAttrs, kLightAttrsN);
|
|
WriteAttrKey(elem, "type", lighttype_map, lighttype_sz, light->type, def->Light().type);
|
|
WriteAttrTxt(elem, "texture", light->get_texture());
|
|
}
|
|
|
|
// write pair
|
|
// write the mechanical attributes of an element, driven by the same
|
|
// generated rows the reader uses; see the declaration for the contract
|
|
template <typename T>
|
|
void mjXWriter::WriteAttrTable(XMLElement* elem, const T* obj, const T* def,
|
|
const mjXAttr* rows, int nrow) {
|
|
const char* live = reinterpret_cast<const char*>(obj);
|
|
const char* dflt = reinterpret_cast<const char*>(def);
|
|
for (int i = 0; i < nrow; i++) {
|
|
const mjXAttr& row = rows[i];
|
|
if (row.handwrite || (writingdefaults && row.nodefault)) {
|
|
continue;
|
|
}
|
|
const char* base = live + row.offset;
|
|
// a null default object means the element has no defaults to compare
|
|
// against: every defined value is written
|
|
const char* dbase = dflt ? dflt + row.offset : nullptr;
|
|
const int dkey = dflt ? 0 : -12345; // WriteAttrKey's write-always default
|
|
switch (row.kind) {
|
|
case mjXAttr::kInt:
|
|
WriteAttr(elem, row.attr, row.len, (const int*)base, (const int*)dbase,
|
|
/*trim=*/!row.exact);
|
|
break;
|
|
case mjXAttr::kDouble:
|
|
WriteAttr(elem, row.attr, row.len, (const double*)base,
|
|
(const double*)dbase, /*trim=*/!row.exact);
|
|
break;
|
|
case mjXAttr::kNum:
|
|
WriteAttr(elem, row.attr, row.len, (const mjtNum*)base,
|
|
(const mjtNum*)dbase, /*trim=*/!row.exact);
|
|
break;
|
|
case mjXAttr::kFloat:
|
|
WriteAttr(elem, row.attr, row.len, (const float*)base,
|
|
(const float*)dbase, /*trim=*/!row.exact);
|
|
break;
|
|
case mjXAttr::kEnum:
|
|
WriteAttrKey(elem, row.attr, row.map, row.mapsz, *(const int*)base,
|
|
dbase ? *(const int*)dbase : dkey);
|
|
break;
|
|
case mjXAttr::kEnumByte:
|
|
WriteAttrKey(elem, row.attr, row.map, row.mapsz, *(const mjtByte*)base,
|
|
dbase ? *(const mjtByte*)dbase : dkey);
|
|
break;
|
|
case mjXAttr::kBool:
|
|
WriteAttrKey(elem, row.attr, bool_map, 2, *(const mjtByte*)base,
|
|
dbase ? *(const mjtByte*)dbase : dkey);
|
|
break;
|
|
case mjXAttr::kFlags:
|
|
if (!dbase || *(const int*)base != *(const int*)dbase) {
|
|
int value = *(const int*)base;
|
|
std::vector<int> data;
|
|
for (int j = 0; j < row.mapsz; j++) {
|
|
if (value & row.map[j].value) {
|
|
data.push_back(row.map[j].value);
|
|
}
|
|
}
|
|
if (!data.empty()) {
|
|
WriteAttrKeys(elem, row.attr, row.map, row.mapsz, data.data(),
|
|
data.size(), 0);
|
|
}
|
|
}
|
|
break;
|
|
default:
|
|
// names, strings, files and custom-read attributes: OneX() remnant
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void mjXWriter::OnePair(XMLElement* elem, const mjCPair* pair, mjCDef* def) {
|
|
// regular
|
|
if (!writingdefaults) {
|
|
if (pair->classname != "main") {
|
|
WriteAttrTxt(elem, "class", pair->classname);
|
|
}
|
|
WriteAttrTxt(elem, "geom1", pair->get_geomname1());
|
|
WriteAttrTxt(elem, "geom2", pair->get_geomname2());
|
|
}
|
|
|
|
// defaults and regular
|
|
WriteAttrTxt(elem, "name", pair->name);
|
|
WriteAttrTable(elem, static_cast<const mjsPair*>(pair), &def->Pair().spec,
|
|
kPairAttrs, kPairAttrsN);
|
|
}
|
|
|
|
|
|
|
|
// write equality
|
|
void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* equality, mjCDef* def) {
|
|
// regular
|
|
if (!writingdefaults) {
|
|
WriteAttrTxt(elem, "name", equality->name);
|
|
if (equality->classname != "main") {
|
|
WriteAttrTxt(elem, "class", equality->classname);
|
|
}
|
|
|
|
switch (equality->type) {
|
|
case mjEQ_CONNECT:
|
|
if (equality->objtype == mjOBJ_BODY) {
|
|
WriteAttrTxt(elem, "body1", mjs_getString(equality->name1));
|
|
WriteAttrTxt(elem, "body2", mjs_getString(equality->name2));
|
|
WriteAttr(elem, "anchor", 3, equality->data);
|
|
} else {
|
|
WriteAttrTxt(elem, "site1", mjs_getString(equality->name1));
|
|
WriteAttrTxt(elem, "site2", mjs_getString(equality->name2));
|
|
}
|
|
break;
|
|
|
|
case mjEQ_WELD:
|
|
if (equality->objtype == mjOBJ_BODY) {
|
|
WriteAttrTxt(elem, "body1", mjs_getString(equality->name1));
|
|
WriteAttrTxt(elem, "body2", mjs_getString(equality->name2));
|
|
// unlike connect, weld's body semantic does not require anchor,
|
|
// and the reader zeroes it when absent: zeros is the default,
|
|
// not the constructor's union payload
|
|
double zero3[3] = {0, 0, 0};
|
|
WriteAttr(elem, "anchor", 3, equality->data, zero3);
|
|
WriteAttr(elem, "relpose", 7, equality->data+3,
|
|
def->Equality().spec.data+3);
|
|
} else {
|
|
WriteAttrTxt(elem, "site1", mjs_getString(equality->name1));
|
|
WriteAttrTxt(elem, "site2", mjs_getString(equality->name2));
|
|
}
|
|
WriteAttr(elem, "torquescale", 1, equality->data+10,
|
|
def->Equality().spec.data+10);
|
|
break;
|
|
|
|
case mjEQ_JOINT:
|
|
WriteAttrTxt(elem, "joint1", mjs_getString(equality->name1));
|
|
WriteAttrTxt(elem, "joint2", mjs_getString(equality->name2));
|
|
WriteAttr(elem, "polycoef", 5, equality->data,
|
|
def->Equality().spec.data);
|
|
break;
|
|
|
|
case mjEQ_TENDON:
|
|
WriteAttrTxt(elem, "tendon1", mjs_getString(equality->name1));
|
|
WriteAttrTxt(elem, "tendon2", mjs_getString(equality->name2));
|
|
WriteAttr(elem, "polycoef", 5, equality->data,
|
|
def->Equality().spec.data);
|
|
break;
|
|
|
|
case mjEQ_FLEX:
|
|
case mjEQ_FLEXVERT:
|
|
WriteAttrTxt(elem, "flex", mjs_getString(equality->name1));
|
|
break;
|
|
|
|
case mjEQ_FLEXSTRAIN:
|
|
WriteAttrTxt(elem, "flex", mjs_getString(equality->name1));
|
|
WriteAttr(elem, "cell", 3, equality->data, def->Equality().spec.data);
|
|
break;
|
|
|
|
default:
|
|
mju_error("mjXWriter: unknown equality type.");
|
|
}
|
|
}
|
|
|
|
// defaults and regular
|
|
WriteAttrTable(elem, static_cast<const mjsEquality*>(equality),
|
|
&def->Equality().spec, kEqualityBaseAttrs,
|
|
kEqualityBaseAttrsN);
|
|
}
|
|
|
|
|
|
|
|
// write tendon
|
|
void mjXWriter::OneTendon(XMLElement* elem, const mjCTendon* tendon, mjCDef* def) {
|
|
bool fixed = (tendon->GetWrap(0) && tendon->GetWrap(0)->Type() == mjWRAP_JOINT);
|
|
|
|
// regular
|
|
if (!writingdefaults) {
|
|
WriteAttrTxt(elem, "name", tendon->name);
|
|
if (tendon->classname != "main") {
|
|
WriteAttrTxt(elem, "class", tendon->classname);
|
|
}
|
|
}
|
|
|
|
// defaults and regular; the fixed rows are the spatial rows without the
|
|
// appearance attributes, which is exactly the tag difference
|
|
if (fixed) {
|
|
WriteAttrTable(elem, static_cast<const mjsTendon*>(tendon),
|
|
&def->Tendon().spec, kFixedAttrs, kFixedAttrsN);
|
|
} else {
|
|
WriteAttrTable(elem, static_cast<const mjsTendon*>(tendon),
|
|
&def->Tendon().spec, kSpatialAttrs, kSpatialAttrsN);
|
|
}
|
|
if (tendon->springlength[0] != tendon->springlength[1] ||
|
|
def->Tendon().springlength[0] != def->Tendon().springlength[1]) {
|
|
WriteAttr(elem, "springlength", 2, tendon->springlength, def->Tendon().springlength);
|
|
} else {
|
|
WriteAttr(elem, "springlength", 1, tendon->springlength, def->Tendon().springlength);
|
|
}
|
|
if (!fixed && tendon->get_material() != def->Tendon().get_material()) {
|
|
WriteAttrTxt(elem, "material", tendon->get_material());
|
|
}
|
|
|
|
// userdata
|
|
if (writingdefaults) {
|
|
WriteVector(elem, "user", tendon->get_userdata());
|
|
} else {
|
|
WriteVector(elem, "user", tendon->get_userdata(), def->Tendon().get_userdata());
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// write actuator
|
|
void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* actuator, mjCDef* def) {
|
|
// regular
|
|
if (!writingdefaults) {
|
|
WriteAttrTxt(elem, "name", actuator->name);
|
|
if (actuator->classname != "main") {
|
|
WriteAttrTxt(elem, "class", actuator->classname);
|
|
}
|
|
|
|
// transmission target
|
|
switch (actuator->trntype) {
|
|
case mjTRN_JOINT:
|
|
WriteAttrTxt(elem, "joint", actuator->get_target());
|
|
break;
|
|
|
|
case mjTRN_JOINTINPARENT:
|
|
WriteAttrTxt(elem, "jointinparent", actuator->get_target());
|
|
break;
|
|
|
|
case mjTRN_TENDON:
|
|
WriteAttrTxt(elem, "tendon", actuator->get_target());
|
|
break;
|
|
|
|
case mjTRN_SLIDERCRANK:
|
|
WriteAttrTxt(elem, "cranksite", actuator->get_target());
|
|
WriteAttrTxt(elem, "slidersite", actuator->get_slidersite());
|
|
break;
|
|
|
|
case mjTRN_SITE:
|
|
WriteAttrTxt(elem, "site", actuator->get_target());
|
|
WriteAttrTxt(elem, "refsite", actuator->get_refsite());
|
|
break;
|
|
|
|
case mjTRN_BODY:
|
|
WriteAttrTxt(elem, "body", actuator->get_target());
|
|
break;
|
|
|
|
default: // SHOULD NOT OCCUR
|
|
break;
|
|
}
|
|
}
|
|
|
|
// defaults and regular
|
|
WriteAttrTable(elem, static_cast<const mjsActuator*>(actuator),
|
|
&def->Actuator().spec, kGeneralAttrs, kGeneralAttrsN);
|
|
WriteAttr(elem, "cranklength", 1, &actuator->cranklength, &def->Actuator().cranklength);
|
|
// special handling of actdim which has default value of -1
|
|
if (writingdefaults) {
|
|
WriteAttrInt(elem, "actdim", actuator->actdim, def->Actuator().actdim);
|
|
} else {
|
|
int default_actdim = (actuator->dyntype != mjDYN_NONE && actuator->dyntype != mjDYN_DCMOTOR);
|
|
WriteAttrInt(elem, "actdim", actuator->actdim, default_actdim);
|
|
}
|
|
|
|
// plugins: write config attributes
|
|
if (actuator->plugin.active) {
|
|
OnePlugin(elem, &actuator->plugin);
|
|
}
|
|
|
|
// non-plugins: write actuator parameters
|
|
else {
|
|
WriteAttrKey(elem, "gaintype", gain_map, gain_sz, actuator->gaintype, def->Actuator().gaintype);
|
|
if (actuator->gaintype == mjGAIN_SO3) {
|
|
WriteAttrKey(elem, "input", inputchart_map, inputchart_sz, actuator->ctrlspec,
|
|
def->Actuator().ctrlspec);
|
|
} else if (actuator->ctrlspec != def->Actuator().ctrlspec) {
|
|
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);
|
|
}
|
|
}
|