Add Flex component.

PiperOrigin-RevId: 572830650
Change-Id: I6908228087b7b9683be3506c8d9cdc725ed5dcd5
This commit is contained in:
Alessio Quaglino
2023-10-12 10:15:46 +01:00
committed by Saran Tunyasuvunakool
parent 649a474788
commit 5a70ad08ab
82 changed files with 9658 additions and 1581 deletions
+1
View File
@@ -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
View File
@@ -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;
+3
View File
@@ -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
View File
@@ -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
+3 -1
View File
@@ -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);
+68
View File
@@ -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;
+12
View File
@@ -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);