Add Flex component.
PiperOrigin-RevId: 572830650 Change-Id: I6908228087b7b9683be3506c8d9cdc725ed5dcd5
This commit is contained in:
committed by
Saran Tunyasuvunakool
parent
649a474788
commit
5a70ad08ab
@@ -70,6 +70,7 @@ extern const mjMap bias_map[];
|
||||
extern const mjMap stage_map[];
|
||||
extern const mjMap datatype_map[];
|
||||
extern const mjMap meshtype_map[];
|
||||
extern const mjMap flexself_map[];
|
||||
|
||||
|
||||
//---------------------------------- Base XML class ------------------------------------------------
|
||||
|
||||
+298
-11
@@ -34,6 +34,7 @@
|
||||
#include "engine/engine_util_errmem.h"
|
||||
#include "engine/engine_util_misc.h"
|
||||
#include "user/user_composite.h"
|
||||
#include "user/user_flexcomp.h"
|
||||
#include "user/user_model.h"
|
||||
#include "user/user_objects.h"
|
||||
#include "user/user_util.h"
|
||||
@@ -77,7 +78,7 @@ void ReadPluginConfigs(tinyxml2::XMLElement* elem, mjCPlugin* pp) {
|
||||
|
||||
//---------------------------------- MJCF schema ---------------------------------------------------
|
||||
|
||||
static const int nMJCF = 204;
|
||||
static const int nMJCF = 223;
|
||||
static const char* MJCF[nMJCF][mjXATTRNUM] = {
|
||||
{"mujoco", "!", "1", "model"},
|
||||
{"<"},
|
||||
@@ -92,10 +93,10 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
|
||||
"inttotal", "interval", "tolrange"},
|
||||
{">"},
|
||||
|
||||
{"option", "*", "25",
|
||||
{"option", "*", "26",
|
||||
"timestep", "apirate", "impratio", "tolerance", "ls_tolerance", "noslip_tolerance",
|
||||
"mpr_tolerance", "gravity", "wind", "magnetic", "density", "viscosity",
|
||||
"o_margin", "o_solref", "o_solimp",
|
||||
"o_margin", "o_solref", "o_solimp", "o_friction",
|
||||
"integrator", "cone", "jacobian",
|
||||
"solver", "iterations", "ls_iterations", "noslip_iterations", "mpr_iterations",
|
||||
"sdf_iterations", "sdf_initpoints"},
|
||||
@@ -112,8 +113,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
|
||||
|
||||
{"visual", "*", "0"},
|
||||
{"<"},
|
||||
{"global", "?", "10", "fovy", "ipd", "azimuth", "elevation", "linewidth", "glow", "offwidth",
|
||||
"offheight", "realtime", "ellipsoidinertia"},
|
||||
{"global", "?", "10", "fovy", "ipd", "azimuth", "elevation", "linewidth", "glow",
|
||||
"offwidth", "offheight", "realtime", "ellipsoidinertia"},
|
||||
{"quality", "?", "5", "shadowsize", "offsamples", "numslices", "numstacks",
|
||||
"numquads"},
|
||||
{"headlight", "?", "4", "ambient", "diffuse", "specular", "active"},
|
||||
@@ -292,6 +293,32 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
|
||||
{"config", "*", "2", "key", "value"},
|
||||
{">"},
|
||||
{">"},
|
||||
{"flexcomp", "*", "26", "name", "class", "type", "dim", "flatskin",
|
||||
"count", "spacing", "radius", "rigid", "mass", "inertiabox",
|
||||
"scale", "file", "point", "element", "texcoord", "material", "rgba", "selfcollide",
|
||||
"flatskin", "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler"},
|
||||
{"<"},
|
||||
{"edge", "?", "5", "equality", "solref", "solimp", "stiffness", "damping"},
|
||||
{"contact", "?", "10", "contype", "conaffinity", "condim", "priority",
|
||||
"friction", "solmix", "solref", "solimp", "margin", "gap", },
|
||||
{"pin", "*", "4", "id", "range", "grid", "gridrange"},
|
||||
{">"},
|
||||
{">"},
|
||||
|
||||
{"deformable", "*", "0"},
|
||||
{"<"},
|
||||
{"flex", "*", "12", "name", "group", "dim", "radius", "material", "rgba", "flatskin",
|
||||
"selfcollide", "body", "vertex", "element", "texcoord"},
|
||||
{"<"},
|
||||
{"contact", "?", "10", "contype", "conaffinity", "condim", "priority",
|
||||
"friction", "solmix", "solref", "solimp", "margin", "gap"},
|
||||
{"edge", "?", "2", "stiffness", "damping"},
|
||||
{">"},
|
||||
{"skin", "*", "9", "name", "file", "material", "rgba", "inflate",
|
||||
"vertex", "texcoord", "face", "group"},
|
||||
{"<"},
|
||||
{"bone", "*", "5", "body", "bindpos", "bindquat", "vertid", "vertweight"},
|
||||
{">"},
|
||||
{">"},
|
||||
|
||||
{"contact", "*", "0"},
|
||||
@@ -311,6 +338,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
|
||||
"active", "solref", "solimp"},
|
||||
{"tendon", "*", "8", "name", "class", "tendon1", "tendon2", "polycoef",
|
||||
"active", "solref", "solimp"},
|
||||
{"flex", "*", "6", "name", "class", "flex",
|
||||
"active", "solref", "solimp"},
|
||||
{">"},
|
||||
|
||||
{"tendon", "*", "0"},
|
||||
@@ -561,12 +590,13 @@ const mjMap solver_map[solver_sz] = {
|
||||
|
||||
|
||||
// constraint type
|
||||
const int equality_sz = 5;
|
||||
const int equality_sz = 6;
|
||||
const mjMap equality_map[equality_sz] = {
|
||||
{"connect", mjEQ_CONNECT},
|
||||
{"weld", mjEQ_WELD},
|
||||
{"joint", mjEQ_JOINT},
|
||||
{"tendon", mjEQ_TENDON},
|
||||
{"flex", mjEQ_FLEX},
|
||||
{"distance", mjEQ_DISTANCE}
|
||||
};
|
||||
|
||||
@@ -654,7 +684,7 @@ const mjMap datatype_map[datatype_sz] = {
|
||||
|
||||
// LR mode
|
||||
const int lrmode_sz = 4;
|
||||
const mjMap lrmode_map[datatype_sz] = {
|
||||
const mjMap lrmode_map[lrmode_sz] = {
|
||||
{"none", mjLRMODE_NONE},
|
||||
{"muscle", mjLRMODE_MUSCLE},
|
||||
{"muscleuser", mjLRMODE_MUSCLEUSER},
|
||||
@@ -702,12 +732,35 @@ const mjMap tkind_map[2] = {
|
||||
|
||||
|
||||
// mesh type
|
||||
const mjMap meshtype_map[2] = {
|
||||
const mjMap meshtype_map[2] = {
|
||||
{"false", mjVOLUME_MESH},
|
||||
{"true", mjSHELL_MESH},
|
||||
};
|
||||
|
||||
|
||||
// flexcomp type
|
||||
const mjMap fcomp_map[mjNFCOMPTYPES] = {
|
||||
{"grid", mjFCOMPTYPE_GRID},
|
||||
{"box", mjFCOMPTYPE_BOX},
|
||||
{"cylinder", mjFCOMPTYPE_CYLINDER},
|
||||
{"ellipsoid", mjFCOMPTYPE_ELLIPSOID},
|
||||
{"mesh", mjFCOMPTYPE_MESH},
|
||||
{"gmsh", mjFCOMPTYPE_GMSH},
|
||||
{"direct", mjFCOMPTYPE_DIRECT}
|
||||
};
|
||||
|
||||
|
||||
// flex selfcollide type
|
||||
const mjMap flexself_map[5] = {
|
||||
{"none", mjFLEXSELF_NONE},
|
||||
{"narrow", mjFLEXSELF_NARROW},
|
||||
{"bvh", mjFLEXSELF_BVH},
|
||||
{"sap", mjFLEXSELF_SAP},
|
||||
{"auto", mjFLEXSELF_AUTO},
|
||||
};
|
||||
|
||||
|
||||
|
||||
//---------------------------------- class mjXReader implementation --------------------------------
|
||||
|
||||
// constructor
|
||||
@@ -821,6 +874,11 @@ void mjXReader::Parse(XMLElement* root) {
|
||||
Contact(section);
|
||||
}
|
||||
|
||||
for (XMLElement* section = root->FirstChildElement("deformable"); section;
|
||||
section = section->NextSiblingElement("deformable")) {
|
||||
Deformable(section);
|
||||
}
|
||||
|
||||
for (XMLElement* section = root->FirstChildElement("equality"); section;
|
||||
section = section->NextSiblingElement("equality")) {
|
||||
Equality(section);
|
||||
@@ -959,6 +1017,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) {
|
||||
ReadAttr(section, "o_margin", 1, &opt->o_margin, text);
|
||||
ReadAttr(section, "o_solref", mjNREF, opt->o_solref, text, false, false);
|
||||
ReadAttr(section, "o_solimp", mjNIMP, opt->o_solimp, text, false, false);
|
||||
ReadAttr(section, "o_friction", 5, opt->o_friction, text, false, false);
|
||||
|
||||
MapValue(section, "integrator", &opt->integrator, integrator_map, integrator_sz);
|
||||
MapValue(section, "cone", &opt->cone, cone_map, cone_sz);
|
||||
@@ -1190,6 +1249,68 @@ void mjXReader::Statistic(XMLElement* section) {
|
||||
|
||||
//---------------------------------- one-element parsers -------------------------------------------
|
||||
|
||||
// flex element parser
|
||||
void mjXReader::OneFlex(XMLElement* elem, mjCFlex* pflex) {
|
||||
string text;
|
||||
int n;
|
||||
|
||||
// read attributes
|
||||
ReadAttrTxt(elem, "name", pflex->name);
|
||||
ReadAttr(elem, "radius", 1, &pflex->radius, text);
|
||||
ReadAttrTxt(elem, "material", pflex->material);
|
||||
ReadAttr(elem, "rgba", 4, pflex->rgba, text);
|
||||
if (MapValue(elem, "flatskin", &n, bool_map, 2)) {
|
||||
pflex->flatskin = (n==1);
|
||||
}
|
||||
ReadAttrInt(elem, "dim", &pflex->dim);
|
||||
ReadAttrInt(elem, "group", &pflex->group);
|
||||
|
||||
// read data vectors
|
||||
if (ReadAttrTxt(elem, "body", text, true)) {
|
||||
String2Vector(text, pflex->vertbody);
|
||||
}
|
||||
if (ReadAttrTxt(elem, "vertex", text)) {
|
||||
String2Vector(text, pflex->vert);
|
||||
}
|
||||
if (ReadAttrTxt(elem, "element", text, true)) {
|
||||
String2Vector(text, pflex->elem);
|
||||
}
|
||||
if (ReadAttrTxt(elem, "texcoord", text)) {
|
||||
String2Vector(text, pflex->texcoord);
|
||||
}
|
||||
|
||||
// contact subelement
|
||||
XMLElement* cont = elem->FirstChildElement("contact");
|
||||
if (cont) {
|
||||
ReadAttrInt(cont, "contype", &pflex->contype);
|
||||
ReadAttrInt(cont, "conaffinity", &pflex->conaffinity);
|
||||
ReadAttrInt(cont, "condim", &pflex->condim);
|
||||
ReadAttrInt(cont, "priority", &pflex->priority);
|
||||
ReadAttr(cont, "friction", 3, pflex->friction, text, false, false);
|
||||
ReadAttr(cont, "solmix", 1, &pflex->solmix, text);
|
||||
ReadAttr(cont, "solref", mjNREF, pflex->solref, text, false, false);
|
||||
ReadAttr(cont, "solimp", mjNIMP, pflex->solimp, text, false, false);
|
||||
ReadAttr(cont, "margin", 1, &pflex->margin, text);
|
||||
ReadAttr(cont, "gap", 1, &pflex->gap, text);
|
||||
if (MapValue(cont, "internal", &n, bool_map, 2)) {
|
||||
pflex->internal = (n==1);
|
||||
}
|
||||
MapValue(cont, "selfcollide", &pflex->selfcollide, flexself_map, 5);
|
||||
ReadAttrInt(cont, "activelayers", &pflex->activelayers);
|
||||
}
|
||||
|
||||
// edge subelement
|
||||
XMLElement* edge = elem->FirstChildElement("edge");
|
||||
if (edge) {
|
||||
ReadAttr(edge, "stiffness", 1, &pflex->edgestiffness, text);
|
||||
ReadAttr(edge, "damping", 1, &pflex->edgedamping, text);
|
||||
}
|
||||
|
||||
GetXMLPos(elem, pflex);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// mesh element parser
|
||||
void mjXReader::OneMesh(XMLElement* elem, mjCMesh* pmesh) {
|
||||
int n;
|
||||
@@ -1596,6 +1717,10 @@ void mjXReader::OneEquality(XMLElement* elem, mjCEquality* pequality) {
|
||||
ReadAttr(elem, "polycoef", 5, pequality->data, text);
|
||||
break;
|
||||
|
||||
case mjEQ_FLEX:
|
||||
ReadAttrTxt(elem, "flex", pequality->name1, true);
|
||||
break;
|
||||
|
||||
case mjEQ_DISTANCE:
|
||||
throw mjXError(elem, "support for distance equality constraints was removed in MuJoCo 2.2.2");
|
||||
break;
|
||||
@@ -2136,6 +2261,124 @@ void mjXReader::OneComposite(XMLElement* elem, mjCBody* pbody, mjCDef* def) {
|
||||
|
||||
|
||||
|
||||
// make flexcomp
|
||||
void mjXReader::OneFlexcomp(XMLElement* elem, mjCBody* pbody) {
|
||||
string text;
|
||||
int n;
|
||||
|
||||
// create out-of-DOM element
|
||||
mjCFlexcomp fcomp;
|
||||
|
||||
// common properties
|
||||
ReadAttrTxt(elem, "name", fcomp.name, true);
|
||||
if (MapValue(elem, "type", &n, fcomp_map, mjNFCOMPTYPES)) {
|
||||
fcomp.type = (mjtFcompType)n;
|
||||
}
|
||||
ReadAttr(elem, "count", 3, fcomp.count, text);
|
||||
ReadAttr(elem, "spacing", 3, fcomp.spacing, text);
|
||||
ReadAttr(elem, "scale", 3, fcomp.scale, text);
|
||||
ReadAttr(elem, "mass", 1, &fcomp.mass, text);
|
||||
ReadAttr(elem, "inertiabox", 1, &fcomp.inertiabox, text);
|
||||
ReadAttrTxt(elem, "file", fcomp.file);
|
||||
ReadAttrTxt(elem, "material", fcomp.def.flex.material);
|
||||
ReadAttr(elem, "rgba", 4, fcomp.def.flex.rgba, text);
|
||||
if (MapValue(elem, "flatskin", &n, bool_map, 2)) {
|
||||
fcomp.def.flex.flatskin = (n==1);
|
||||
}
|
||||
ReadAttrInt(elem, "dim", &fcomp.def.flex.dim);
|
||||
ReadAttr(elem, "radius", 1, &fcomp.def.flex.radius, text);
|
||||
ReadAttrInt(elem, "group", &fcomp.def.flex.group);
|
||||
|
||||
// pose
|
||||
ReadAttr(elem, "pos", 3, fcomp.pos, text);
|
||||
ReadAttr(elem, "quat", 4, fcomp.quat, text);
|
||||
ReadAlternative(elem, fcomp.alt);
|
||||
|
||||
// user or internal
|
||||
if (MapValue(elem, "rigid", &n, bool_map, 2)) {
|
||||
fcomp.rigid = (n==1);
|
||||
}
|
||||
if (ReadAttrTxt(elem, "point", text)){
|
||||
String2Vector(text, fcomp.point);
|
||||
}
|
||||
if (ReadAttrTxt(elem, "element", text)){
|
||||
String2Vector(text, fcomp.element);
|
||||
}
|
||||
if (ReadAttrTxt(elem, "texcoord", text)) {
|
||||
String2Vector(text, fcomp.texcoord);
|
||||
}
|
||||
|
||||
// edge
|
||||
XMLElement* edge = elem->FirstChildElement("edge");
|
||||
if (edge) {
|
||||
if (MapValue(edge, "equality", &n, bool_map, 2)) {
|
||||
fcomp.equality = (n==1);
|
||||
}
|
||||
ReadAttr(edge, "solref", mjNREF, fcomp.def.equality.solref, text, false, false);
|
||||
ReadAttr(edge, "solimp", mjNIMP, fcomp.def.equality.solimp, text, false, false);
|
||||
ReadAttr(edge, "stiffness", 1, &fcomp.def.flex.edgestiffness, text);
|
||||
ReadAttr(edge, "damping", 1, &fcomp.def.flex.edgedamping, text);
|
||||
}
|
||||
|
||||
// contact
|
||||
XMLElement* cont = elem->FirstChildElement("contact");
|
||||
if (cont) {
|
||||
ReadAttrInt(cont, "contype", &fcomp.def.flex.contype);
|
||||
ReadAttrInt(cont, "conaffinity", &fcomp.def.flex.conaffinity);
|
||||
ReadAttrInt(cont, "condim", &fcomp.def.flex.condim);
|
||||
ReadAttrInt(cont, "priority", &fcomp.def.flex.priority);
|
||||
ReadAttr(cont, "friction", 3, fcomp.def.flex.friction, text, false, false);
|
||||
ReadAttr(cont, "solmix", 1, &fcomp.def.flex.solmix, text);
|
||||
ReadAttr(cont, "solref", mjNREF, fcomp.def.flex.solref, text, false, false);
|
||||
ReadAttr(cont, "solimp", mjNIMP, fcomp.def.flex.solimp, text, false, false);
|
||||
ReadAttr(cont, "margin", 1, &fcomp.def.flex.margin, text);
|
||||
ReadAttr(cont, "gap", 1, &fcomp.def.flex.gap, text);
|
||||
if (MapValue(cont, "internal", &n, bool_map, 2)) {
|
||||
fcomp.def.flex.internal = (n==1);
|
||||
}
|
||||
MapValue(cont, "selfcollide", &fcomp.def.flex.selfcollide, flexself_map, 5);
|
||||
ReadAttrInt(cont, "activelayers", &fcomp.def.flex.activelayers);
|
||||
}
|
||||
|
||||
// pin
|
||||
XMLElement* epin = elem->FirstChildElement("pin");
|
||||
while (epin) {
|
||||
// accumulate id, coord, range
|
||||
vector<int> temp;
|
||||
if (ReadAttrTxt(epin, "id", text)){
|
||||
String2Vector(text, temp);
|
||||
fcomp.pinid.insert(fcomp.pinid.end(), temp.begin(), temp.end());
|
||||
}
|
||||
if (ReadAttrTxt(epin, "range", text)){
|
||||
String2Vector(text, temp);
|
||||
fcomp.pinrange.insert(fcomp.pinrange.end(), temp.begin(), temp.end());
|
||||
}
|
||||
if (ReadAttrTxt(epin, "grid", text)){
|
||||
String2Vector(text, temp);
|
||||
fcomp.pingrid.insert(fcomp.pingrid.end(), temp.begin(), temp.end());
|
||||
}
|
||||
if (ReadAttrTxt(epin, "gridrange", text)){
|
||||
String2Vector(text, temp);
|
||||
fcomp.pingridrange.insert(fcomp.pingridrange.end(), temp.begin(), temp.end());
|
||||
}
|
||||
|
||||
// advance
|
||||
epin = epin->NextSiblingElement("pin");
|
||||
}
|
||||
|
||||
// make flexcomp
|
||||
char error[200];
|
||||
bool res = fcomp.Make(pbody->model, pbody, error, 200);
|
||||
|
||||
// throw error
|
||||
if (!res) {
|
||||
throw mjXError(elem, error);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// add plugin
|
||||
void mjXReader::OnePlugin(XMLElement* elem, mjCBase* object) {
|
||||
object->is_plugin = true;
|
||||
ReadAttrTxt(elem, "plugin", object->plugin_name);
|
||||
@@ -2623,9 +2866,9 @@ void mjXReader::Asset(XMLElement* section) {
|
||||
OneMesh(elem, pmesh);
|
||||
}
|
||||
|
||||
// skin sub-element
|
||||
// skin sub-element... deprecate ???
|
||||
else if (name=="skin") {
|
||||
// create mesh and parse
|
||||
// create skin and parse
|
||||
mjCSkin* pskin = model->AddSkin();
|
||||
OneSkin(elem, pskin);
|
||||
}
|
||||
@@ -2771,10 +3014,16 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) {
|
||||
|
||||
// composite sub-element
|
||||
else if (name=="composite") {
|
||||
// create composite and parse
|
||||
// parse composite
|
||||
OneComposite(elem, pbody, def);
|
||||
}
|
||||
|
||||
// flexcomp sub-element
|
||||
else if (name=="flexcomp") {
|
||||
// parse flexcomp
|
||||
OneFlexcomp(elem, pbody);
|
||||
}
|
||||
|
||||
// body sub-element
|
||||
else if (name=="body") {
|
||||
// read childdef
|
||||
@@ -2888,6 +3137,44 @@ void mjXReader::Equality(XMLElement* section) {
|
||||
|
||||
|
||||
|
||||
// deformable section parser
|
||||
void mjXReader::Deformable(XMLElement* section) {
|
||||
string name;
|
||||
XMLElement* elem;
|
||||
|
||||
// iterate over child elements
|
||||
elem = section->FirstChildElement();
|
||||
while (elem) {
|
||||
// get sub-element name
|
||||
name = elem->Value();
|
||||
|
||||
// get class if specified, otherwise use default0
|
||||
mjCDef* def = GetClass(elem);
|
||||
if (!def) {
|
||||
def = model->defaults[0];
|
||||
}
|
||||
|
||||
// flex sub-element
|
||||
if (name=="flex") {
|
||||
// create flex and parse
|
||||
mjCFlex* pflex = model->AddFlex();
|
||||
OneFlex(elem, pflex);
|
||||
}
|
||||
|
||||
// skin sub-element
|
||||
else if (name=="skin") {
|
||||
// create skin and parse
|
||||
mjCSkin* pskin = model->AddSkin();
|
||||
OneSkin(elem, pskin);
|
||||
}
|
||||
|
||||
// advance to next element
|
||||
elem = elem->NextSiblingElement();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// tendon section parser
|
||||
void mjXReader::Tendon(XMLElement* section) {
|
||||
string text, text1;
|
||||
|
||||
@@ -44,6 +44,7 @@ class mjXReader : public mjXBase {
|
||||
void Asset(tinyxml2::XMLElement* section); // asset section
|
||||
void Body(tinyxml2::XMLElement* section, mjCBody* pbody); // body/world section
|
||||
void Contact(tinyxml2::XMLElement* section); // contact section
|
||||
void Deformable(tinyxml2::XMLElement* section); // deformable section
|
||||
void Equality(tinyxml2::XMLElement* section); // equality section
|
||||
void Tendon(tinyxml2::XMLElement* section); // tendon section
|
||||
void Actuator(tinyxml2::XMLElement* section); // actuator section
|
||||
@@ -51,6 +52,7 @@ class mjXReader : public mjXBase {
|
||||
void Keyframe(tinyxml2::XMLElement* section); // keyframe section
|
||||
|
||||
// single element parsers, used in defaults and main body
|
||||
void OneFlex(tinyxml2::XMLElement* elem, mjCFlex* pflex);
|
||||
void OneMesh(tinyxml2::XMLElement* elem, mjCMesh* pmesh);
|
||||
void OneSkin(tinyxml2::XMLElement* elem, mjCSkin* pskin);
|
||||
void OneMaterial(tinyxml2::XMLElement* elem, mjCMaterial* pmaterial);
|
||||
@@ -64,6 +66,7 @@ class mjXReader : public mjXBase {
|
||||
void OneTendon(tinyxml2::XMLElement* elem, mjCTendon* ptendon);
|
||||
void OneActuator(tinyxml2::XMLElement* elem, mjCActuator* pactuator);
|
||||
void OneComposite(tinyxml2::XMLElement* elem, mjCBody* pbody, mjCDef* def);
|
||||
void OneFlexcomp(tinyxml2::XMLElement* elem, mjCBody* pbody);
|
||||
void OnePlugin(tinyxml2::XMLElement* elem, mjCBase* object);
|
||||
|
||||
mjXSchema schema; // schema used for validation
|
||||
|
||||
+116
-16
@@ -82,6 +82,74 @@ XMLElement* mjXWriter::InsertEnd(XMLElement* parent, const char* name) {
|
||||
|
||||
//---------------------------------- class mjXWriter: one-element writers --------------------------
|
||||
|
||||
// write flex
|
||||
void mjXWriter::OneFlex(XMLElement* elem, mjCFlex* pflex) {
|
||||
string text;
|
||||
mjCFlex defflex;
|
||||
|
||||
// common attributes
|
||||
WriteAttrTxt(elem, "name", pflex->name);
|
||||
WriteAttr(elem, "radius", 1, &pflex->radius, &defflex.radius);
|
||||
if (pflex->material != defflex.material) {
|
||||
WriteAttrTxt(elem, "material", pflex->material);
|
||||
}
|
||||
WriteAttr(elem, "rgba", 4, pflex->rgba, defflex.rgba);
|
||||
WriteAttrKey(elem, "flatskin", bool_map, 2, pflex->flatskin, defflex.flatskin);
|
||||
WriteAttrInt(elem, "dim", pflex->dim, defflex.dim);
|
||||
WriteAttrInt(elem, "group", pflex->group, defflex.group);
|
||||
|
||||
// data vectors
|
||||
if (!pflex->vertbody.empty()) {
|
||||
Vector2String(text, pflex->vertbody);
|
||||
WriteAttrTxt(elem, "body", text);
|
||||
}
|
||||
if (!pflex->vert.empty()) {
|
||||
Vector2String(text, pflex->vert);
|
||||
WriteAttrTxt(elem, "vertex", text);
|
||||
}
|
||||
if (!pflex->elem.empty()) {
|
||||
Vector2String(text, pflex->elem);
|
||||
WriteAttrTxt(elem, "element", text);
|
||||
}
|
||||
if (!pflex->texcoord.empty()) {
|
||||
Vector2String(text, pflex->texcoord);
|
||||
WriteAttrTxt(elem, "texcoord", text);
|
||||
}
|
||||
|
||||
// contact subelement
|
||||
XMLElement* cont = InsertEnd(elem, "contact");
|
||||
WriteAttrInt(cont, "contype", pflex->contype, defflex.contype);
|
||||
WriteAttrInt(cont, "conaffinity", pflex->conaffinity, defflex.conaffinity);
|
||||
WriteAttrInt(cont, "condim", pflex->condim, defflex.condim);
|
||||
WriteAttrInt(cont, "priority", pflex->priority, defflex.priority);
|
||||
WriteAttr(cont, "friction", 3, pflex->friction, defflex.friction);
|
||||
WriteAttr(cont, "solmix", 1, &pflex->solmix, &defflex.solmix);
|
||||
WriteAttr(cont, "solref", mjNREF, pflex->solref, defflex.solref);
|
||||
WriteAttr(cont, "solimp", mjNIMP, pflex->solimp, defflex.solimp);
|
||||
WriteAttr(cont, "margin", 1, &pflex->margin, &defflex.margin);
|
||||
WriteAttr(cont, "gap", 1, &pflex->gap, &defflex.gap);
|
||||
WriteAttrKey(cont, "internal", bool_map, 2, pflex->internal, defflex.internal);
|
||||
WriteAttrKey(cont, "selfcollide", flexself_map, 5, pflex->selfcollide, defflex.selfcollide);
|
||||
WriteAttrInt(cont, "activelayers", pflex->activelayers, defflex.activelayers);
|
||||
|
||||
// remove contact is no attributes
|
||||
if (!cont->FirstAttribute()) {
|
||||
elem->DeleteChild(cont);
|
||||
}
|
||||
|
||||
// edge subelement
|
||||
XMLElement* edge = InsertEnd(elem, "edge");
|
||||
WriteAttr(edge, "stiffness", 1, &pflex->edgestiffness, &defflex.edgestiffness);
|
||||
WriteAttr(edge, "damping", 1, &pflex->edgedamping, &defflex.edgedamping);
|
||||
|
||||
// remove edge if no attributes
|
||||
if (!edge->FirstAttribute()) {
|
||||
elem->DeleteChild(edge);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// write mesh
|
||||
void mjXWriter::OneMesh(XMLElement* elem, mjCMesh* pmesh, mjCDef* def) {
|
||||
string text;
|
||||
@@ -221,12 +289,12 @@ void mjXWriter::OneJoint(XMLElement* elem, mjCJoint* pjoint, mjCDef* def) {
|
||||
|
||||
// special handling of limits
|
||||
bool range_defined = pjoint->range[0]!=0 || pjoint->range[1]!=0;
|
||||
bool limited_inferred = def->joint.limited==2 && pjoint->limited==range_defined;
|
||||
bool limited_inferred = def->joint.limited==2 && pjoint->limited==(int)range_defined;
|
||||
if (writingdefaults || !limited_inferred) {
|
||||
WriteAttrKey(elem, "limited", TFAuto_map, 3, pjoint->limited, def->joint.limited);
|
||||
}
|
||||
bool afrange_defined = pjoint->actfrcrange[0]!=0 || pjoint->actfrcrange[1]!=0;
|
||||
bool aflimited_inferred = def->joint.actfrclimited==2 && pjoint->actfrclimited==afrange_defined;
|
||||
bool aflimited_inferred = def->joint.actfrclimited==2 && pjoint->actfrclimited==(int)afrange_defined;
|
||||
if (writingdefaults || !aflimited_inferred) {
|
||||
WriteAttrKey(elem, "actuatorfrclimited", TFAuto_map, 3,
|
||||
pjoint->actfrclimited, def->joint.actfrclimited);
|
||||
@@ -515,6 +583,10 @@ void mjXWriter::OneEquality(XMLElement* elem, mjCEquality* peq, mjCDef* def) {
|
||||
WriteAttr(elem, "polycoef", 5, peq->data);
|
||||
break;
|
||||
|
||||
case mjEQ_FLEX:
|
||||
WriteAttrTxt(elem, "flex", peq->name1);
|
||||
break;
|
||||
|
||||
default:
|
||||
mju_error("mjXWriter: unknown equality type.");
|
||||
}
|
||||
@@ -540,7 +612,7 @@ void mjXWriter::OneTendon(XMLElement* elem, mjCTendon* pten, mjCDef* def) {
|
||||
|
||||
// special handling of limits
|
||||
bool range_defined = pten->range[0]!=0 || pten->range[1]!=0;
|
||||
bool limited_inferred = def->tendon.limited==2 && pten->limited==range_defined;
|
||||
bool limited_inferred = def->tendon.limited==2 && pten->limited==(int)range_defined;
|
||||
if (writingdefaults || !limited_inferred) {
|
||||
WriteAttrKey(elem, "limited", TFAuto_map, 3, pten->limited, def->tendon.limited);
|
||||
}
|
||||
@@ -624,17 +696,17 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
|
||||
// special handling of limits
|
||||
bool range_defined, limited_inferred;
|
||||
range_defined = pact->ctrlrange[0]!=0 || pact->ctrlrange[1]!=0;
|
||||
limited_inferred = def->actuator.ctrllimited==2 && pact->ctrllimited==range_defined;
|
||||
limited_inferred = def->actuator.ctrllimited==2 && pact->ctrllimited==(int)range_defined;
|
||||
if (writingdefaults || !limited_inferred) {
|
||||
WriteAttrKey(elem, "ctrllimited", TFAuto_map, 3, pact->ctrllimited, def->actuator.ctrllimited);
|
||||
}
|
||||
range_defined = pact->forcerange[0]!=0 || pact->forcerange[1]!=0;
|
||||
limited_inferred = def->actuator.forcelimited==2 && pact->forcelimited==range_defined;
|
||||
limited_inferred = def->actuator.forcelimited==2 && pact->forcelimited==(int)range_defined;
|
||||
if (writingdefaults || !limited_inferred) {
|
||||
WriteAttrKey(elem, "forcelimited", TFAuto_map, 3, pact->forcelimited, def->actuator.forcelimited);
|
||||
}
|
||||
range_defined = pact->actrange[0]!=0 || pact->actrange[1]!=0;
|
||||
limited_inferred = def->actuator.actlimited==2 && pact->actlimited==range_defined;
|
||||
limited_inferred = def->actuator.actlimited==2 && pact->actlimited==(int)range_defined;
|
||||
if (writingdefaults || !limited_inferred) {
|
||||
WriteAttrKey(elem, "actlimited", TFAuto_map, 3, pact->actlimited, def->actuator.actlimited);
|
||||
}
|
||||
@@ -750,6 +822,7 @@ string mjXWriter::Write(char *error, size_t error_sz) {
|
||||
Asset(root);
|
||||
Body(InsertEnd(root, "worldbody"), model->GetWorld());
|
||||
Contact(root);
|
||||
Deformable(root);
|
||||
Equality(root);
|
||||
Tendon(root);
|
||||
Actuator(root);
|
||||
@@ -819,6 +892,7 @@ void mjXWriter::Option(XMLElement* root) {
|
||||
WriteAttr(section, "o_margin", 1, &model->option.o_margin, &opt.o_margin);
|
||||
WriteAttr(section, "o_solref", mjNREF, model->option.o_solref, opt.o_solref);
|
||||
WriteAttr(section, "o_solimp", mjNIMP, model->option.o_solimp, opt.o_solimp);
|
||||
WriteAttr(section, "o_friction", 5, model->option.o_friction, opt.o_friction);
|
||||
|
||||
WriteAttrKey(section, "integrator", integrator_map, integrator_sz,
|
||||
model->option.integrator, opt.integrator);
|
||||
@@ -1261,11 +1335,10 @@ void mjXWriter::Asset(XMLElement* root) {
|
||||
int ntex = model->NumObjects(mjOBJ_TEXTURE);
|
||||
int nmat = model->NumObjects(mjOBJ_MATERIAL);
|
||||
int nmesh = model->NumObjects(mjOBJ_MESH);
|
||||
int nskin = model->NumObjects(mjOBJ_SKIN);
|
||||
int nhfield = model->NumObjects(mjOBJ_HFIELD);
|
||||
|
||||
// return if empty
|
||||
if (ntex==0 && nmat==0 && nmesh==0 && nhfield==0 && nskin==0) {
|
||||
if (ntex==0 && nmat==0 && nmesh==0 && nhfield==0) {
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -1346,14 +1419,6 @@ void mjXWriter::Asset(XMLElement* root) {
|
||||
}
|
||||
}
|
||||
|
||||
// write skins
|
||||
for (int i=0; i<nskin; i++) {
|
||||
// create element and write
|
||||
mjCSkin* pskin = (mjCSkin*)model->GetObject(mjOBJ_SKIN, i);
|
||||
elem = InsertEnd(section, "skin");
|
||||
OneSkin(elem, pskin);
|
||||
}
|
||||
|
||||
// write hfields
|
||||
for (int i=0; i<nhfield; i++) {
|
||||
// create element
|
||||
@@ -1513,6 +1578,41 @@ void mjXWriter::Equality(XMLElement* root) {
|
||||
|
||||
|
||||
|
||||
// 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* pflex = (mjCFlex*)model->GetObject(mjOBJ_FLEX, i);
|
||||
elem = InsertEnd(section, "flex");
|
||||
OneFlex(elem, pflex);
|
||||
}
|
||||
|
||||
// write skins
|
||||
for (int i=0; i<nskin; i++) {
|
||||
// create element and write
|
||||
mjCSkin* pskin = (mjCSkin*)model->GetObject(mjOBJ_SKIN, i);
|
||||
elem = InsertEnd(section, "skin");
|
||||
OneSkin(elem, pskin);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// tendon section
|
||||
void mjXWriter::Tendon(XMLElement* root) {
|
||||
// skip section if empty
|
||||
|
||||
@@ -45,13 +45,15 @@ class mjXWriter : public mjXBase {
|
||||
void Asset(tinyxml2::XMLElement* root); // asset section
|
||||
void Body(tinyxml2::XMLElement* elem, mjCBody* body); // body/world section
|
||||
void Contact(tinyxml2::XMLElement* root); // contact section
|
||||
void Equality(tinyxml2::XMLElement* root); // equality constraint section
|
||||
void Deformable(tinyxml2::XMLElement* root); // deformable section
|
||||
void Equality(tinyxml2::XMLElement* root); // equality section
|
||||
void Tendon(tinyxml2::XMLElement* root); // tendon section
|
||||
void Actuator(tinyxml2::XMLElement* root); // actuator section
|
||||
void Sensor(tinyxml2::XMLElement* root); // sensor section
|
||||
void Keyframe(tinyxml2::XMLElement* root); // keyframe section
|
||||
|
||||
// single element writers, used in defaults and main body
|
||||
void OneFlex(tinyxml2::XMLElement* elem, mjCFlex* pflex);
|
||||
void OneMesh(tinyxml2::XMLElement* elem, mjCMesh* pmesh, mjCDef* def);
|
||||
void OneSkin(tinyxml2::XMLElement* elem, mjCSkin* pskin);
|
||||
void OneMaterial(tinyxml2::XMLElement* elem, mjCMaterial* pmaterial, mjCDef* def);
|
||||
|
||||
@@ -767,6 +767,42 @@ bool mjXUtil::ReadAttrInt(XMLElement* elem, const char* attr, int* data, bool re
|
||||
|
||||
|
||||
|
||||
// read vector<string> from string
|
||||
void mjXUtil::String2Vector(const string& txt, vector<string>& vec) {
|
||||
stringstream strm(txt);
|
||||
vec.clear();
|
||||
|
||||
while (!strm.eof()) {
|
||||
string word;
|
||||
strm >> word;
|
||||
if (strm.fail()) {
|
||||
break;
|
||||
} else {
|
||||
vec.push_back(word);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// read vector<mjtNum> from string
|
||||
void mjXUtil::String2Vector(const string& txt, vector<double>& vec) {
|
||||
stringstream strm(txt);
|
||||
vec.clear();
|
||||
|
||||
while (!strm.eof()) {
|
||||
double num;
|
||||
strm >> num;
|
||||
if (strm.fail()) {
|
||||
break;
|
||||
} else {
|
||||
vec.push_back(num);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// read vector<float> from string
|
||||
void mjXUtil::String2Vector(const string& txt, vector<float>& vec) {
|
||||
stringstream strm(txt);
|
||||
@@ -803,6 +839,38 @@ void mjXUtil::String2Vector(const string& txt, vector<int>& vec) {
|
||||
|
||||
|
||||
|
||||
// write vector<float> to string
|
||||
void mjXUtil::Vector2String(string& txt, const vector<string>& vec) {
|
||||
stringstream strm;
|
||||
|
||||
for (size_t i=0; i<vec.size(); i++) {
|
||||
if (i>0) {
|
||||
strm << " ";
|
||||
}
|
||||
strm << vec[i];
|
||||
}
|
||||
|
||||
txt = strm.str();
|
||||
}
|
||||
|
||||
|
||||
|
||||
// write vector<double> to string
|
||||
void mjXUtil::Vector2String(string& txt, const vector<double>& vec) {
|
||||
stringstream strm;
|
||||
|
||||
for (size_t i=0; i<vec.size(); i++) {
|
||||
if (i>0) {
|
||||
strm << " ";
|
||||
}
|
||||
strm << vec[i];
|
||||
}
|
||||
|
||||
txt = strm.str();
|
||||
}
|
||||
|
||||
|
||||
|
||||
// write vector<float> to string
|
||||
void mjXUtil::Vector2String(string& txt, const vector<float>& vec) {
|
||||
stringstream strm;
|
||||
|
||||
@@ -160,12 +160,24 @@ class mjXUtil {
|
||||
static bool ReadAttrInt(tinyxml2::XMLElement* elem, const char* attr, int* data,
|
||||
bool required = false);
|
||||
|
||||
// read vector<string> from string
|
||||
static void String2Vector(const std::string& txt, std::vector<std::string>& vec);
|
||||
|
||||
// read vector<double> from string
|
||||
static void String2Vector(const std::string& txt, std::vector<double>& vec);
|
||||
|
||||
// read vector<float> from string
|
||||
static void String2Vector(const std::string& txt, std::vector<float>& vec);
|
||||
|
||||
// read vector<int> from string
|
||||
static void String2Vector(const std::string& txt, std::vector<int>& vec);
|
||||
|
||||
// write vector<string> to string
|
||||
static void Vector2String(std::string& txt, const std::vector<std::string>& vec);
|
||||
|
||||
// write vector<double> to string
|
||||
static void Vector2String(std::string& txt, const std::vector<double>& vec);
|
||||
|
||||
// write vector<float> to string
|
||||
static void Vector2String(std::string& txt, const std::vector<float>& vec);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user