chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Add Flex component.

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files
src/xml/xml_native_reader.cc
+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;