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

Add replicate element to MJCF. Fixes #553

Browse Source 
PiperOrigin-RevId: 629393578
Change-Id: Ia1db1b9e47eba8868117b0f7b85edcd0cc1b6d79
This commit is contained in:
Alessio Quaglino
2024-04-30 06:28:09 -07:00
committed by Copybara-Service
parent 6a5bbb5157
commit 26f23066d1
30 changed files with 8756 additions and 2807 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/xml/xml_native_reader.cc
+81 -5
View File
@@ -74,6 +74,16 @@ void ReadPluginConfigs(tinyxml2::XMLElement* elem, mjsPlugin* p) {
mjs_setPluginAttributes(p, &config_attribs);
}
}
static void UpdateString(std::string& psuffix, int count, int i) {
int ndigits = std::to_string(count).length();
std::string i_string = std::to_string(i);
std::string prefix = "";
while (ndigits-- > i_string.length()) {
prefix += '0';
}
psuffix += prefix + i_string;
}
} // namespace
@@ -870,11 +880,6 @@ void mjXReader::Parse(XMLElement* root) {
Asset(section);
}
for (XMLElement* section = FirstChildElement(root, "worldbody"); section;
section = NextSiblingElement(section, "worldbody")) {
Body(section, mjs_findBody(model, "world"), nullptr);
}
for (XMLElement* section = FirstChildElement(root, "contact"); section;
section = NextSiblingElement(section, "contact")) {
Contact(section);
@@ -909,6 +914,11 @@ void mjXReader::Parse(XMLElement* root) {
section = NextSiblingElement(section, "keyframe")) {
Keyframe(section);
}
for (XMLElement* section = FirstChildElement(root, "worldbody"); section;
section = NextSiblingElement(section, "worldbody")) {
Body(section, mjs_findBody(model, "world"), nullptr);
}
}
@@ -3371,6 +3381,72 @@ void mjXReader::Body(XMLElement* section, mjsBody* pbody, mjsFrame* frame) {
Body(elem, pbody, pframe);
}
// replicate sub-element
else if (name=="replicate") {
int count;
double offset[3] = {0, 0, 0};
double euler[3] = {0, 0, 0};
std::string separator = "";
ReadAttr(elem, "count", 1, &count, text, true);
ReadAttr(elem, "offset", 3, offset, text);
ReadAttr(elem, "euler", 3, euler, text);
ReadAttrTxt(elem, "sep", separator);
// store rotation difference
mjsOrientation alt;
mjs_defaultOrientation(alt);
alt.type = mjORIENTATION_EULER;
mjuu_copyvec(alt.euler, euler, 3);
double rotation[4] = {1, 0, 0, 0};
mjs_resolveOrientation(rotation, model->degree, model->euler, &alt);
// read childdef
mjsDefault* childdef = 0;
if (ReadAttrTxt(elem, "childclass", text)) {
childdef = mjs_findDefault(model, text.c_str());
mjs_findDefault(model, text.c_str());
if (!childdef) {
throw mjXError(elem, "unknown default childclass");
}
}
// create subtree
mjsBody* subtree = mjs_addBody(pbody, childdef);
double pos[3] = {0, 0, 0};
double quat[4] = {1, 0, 0, 0};
for (int i = 0; i < count; i++) {
// create parent frame
mjsFrame* pframe = mjs_addFrame(subtree, frame);
mjs_setString(pframe->info, ("line = " + std::to_string(elem->GetLineNum())).c_str());
mjs_setDefault(pframe->element, childdef ? childdef : def);
// accumulate rotation
mjuu_setvec(pframe->pos, pos[0], pos[1], pos[2]);
mjuu_frameaccum(pos, quat, offset, rotation);
// overwrite orientation to increase precision
alt.euler[0] = i*euler[0];
alt.euler[1] = i*euler[1];
alt.euler[2] = i*euler[2];
mjs_resolveOrientation(quat, model->degree, model->euler, &alt);
mjuu_setvec(pframe->quat, quat[0], quat[1], quat[2], quat[3]);
// process suffix
std::string suffix = separator;
UpdateString(suffix, count, i);
// process subtree
Body(elem, subtree, pframe);
// attach to parent
mjs_attachFrame(pbody, pframe, /*prefix=*/"", suffix.c_str());
}
// delete subtree
mjs_detachBody(model, subtree);
}
// body sub-element
else if (name=="body") {
// read childdef
src/xml/xml_native_writer.cc
+31 -38
View File
@@ -811,7 +811,7 @@ string mjXWriter::Write(char *error, size_t error_sz) {
Extension(root);
Custom(root);
Asset(root);
Body(InsertEnd(root, "worldbody"), model->GetWorld(), /*frame=*/nullptr);
Body(InsertEnd(root, "worldbody"), model->GetWorld());
Contact(root);
Deformable(root);
Equality(root);
@@ -1456,20 +1456,31 @@ void mjXWriter::Asset(XMLElement* root) {
XMLElement* mjXWriter::OneFrame(XMLElement* elem, mjCFrame* frame) {
if (!frame) {
return elem;
}
if (frame->name.empty() && frame->classname.empty()) {
return elem;
}
XMLElement* frame_elem = InsertEnd(elem, "frame");
WriteAttrTxt(frame_elem, "name", frame->name);
WriteAttrTxt(frame_elem, "childclass", frame->classname);
return frame_elem;
}
// recursive body and frame writer
void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame) {
void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
double unitq[4] = {1, 0, 0, 0};
if (!body) {
throw mjXError(0, "missing body in XML write"); // SHOULD NOT OCCUR
}
// write frame if defined
if (frame) {
WriteAttrTxt(elem, "name", frame->name);
WriteAttrTxt(elem, "childclass", frame->classname);
}
// write body attributes and inertial
else if (body!=model->GetWorld()) {
WriteAttrTxt(elem, "name", body->name);
@@ -1504,32 +1515,32 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame) {
// write joints
for (int i=0; i<body->joints.size(); i++) {
if (body->joints[i]->frame != frame) continue;
OneJoint(InsertEnd(elem, "joint"), body->joints[i], body->joints[i]->def);
XMLElement* celem = OneFrame(elem, body->joints[i]->frame);
OneJoint(InsertEnd(celem, "joint"), body->joints[i], body->joints[i]->def);
}
// write geoms
for (int i=0; i<body->geoms.size(); i++) {
if (body->geoms[i]->frame != frame) continue;
OneGeom(InsertEnd(elem, "geom"), body->geoms[i], body->geoms[i]->def);
XMLElement* celem = OneFrame(elem, body->geoms[i]->frame);
OneGeom(InsertEnd(celem, "geom"), body->geoms[i], body->geoms[i]->def);
}
// write sites
for (int i=0; i<body->sites.size(); i++) {
if (body->sites[i]->frame != frame) continue;
OneSite(InsertEnd(elem, "site"), body->sites[i], body->sites[i]->def);
XMLElement* celem = OneFrame(elem, body->sites[i]->frame);
OneSite(InsertEnd(celem, "site"), body->sites[i], body->sites[i]->def);
}
// write cameras
for (int i=0; i<body->cameras.size(); i++) {
if (body->cameras[i]->frame != frame) continue;
OneCamera(InsertEnd(elem, "camera"), body->cameras[i], body->cameras[i]->def);
XMLElement* celem = OneFrame(elem, body->cameras[i]->frame);
OneCamera(InsertEnd(celem, "camera"), body->cameras[i], body->cameras[i]->def);
}
// write lights
for (int i=0; i<body->lights.size(); i++) {
if (body->lights[i]->frame != frame) continue;
OneLight(InsertEnd(elem, "light"), body->lights[i], body->lights[i]->def);
XMLElement* celem = OneFrame(elem, body->lights[i]->frame);
OneLight(InsertEnd(celem, "light"), body->lights[i], body->lights[i]->def);
}
// write plugin
@@ -1537,28 +1548,10 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame) {
OnePlugin(InsertEnd(elem, "plugin"), &body->plugin);
}
// write frames
for (int i=0; i<body->frames.size(); i++) {
// skip current frame
if (body->frames[i]->frame != frame) {
continue;
}
// write frame if named or has defaults
if (!body->frames[i]->name.empty() || !body->frames[i]->classname.empty()) {
Body(InsertEnd(elem, "frame"), body, body->frames[i]);
}
// otherwise skip
else {
Body(elem, body, body->frames[i]);
}
}
// write child bodies recursively
for (int i=0; i<body->bodies.size(); i++) {
if (body->bodies[i]->frame != frame) continue;
Body(InsertEnd(elem, "body"), body->bodies[i], nullptr);
XMLElement* celem = OneFrame(elem, body->bodies[i]->frame);
Body(InsertEnd(celem, "body"), body->bodies[i]);
}
}
src/xml/xml_native_writer.h
+2 -1
View File
@@ -49,7 +49,7 @@ class mjXWriter : public mjXBase {
void Extension(tinyxml2::XMLElement* root); // extension section
void Custom(tinyxml2::XMLElement* root); // custom section
void Asset(tinyxml2::XMLElement* root); // asset section
void Body(tinyxml2::XMLElement* elem, mjCBody* body, mjCFrame* frame); // body/world section
void Body(tinyxml2::XMLElement* elem, mjCBody* body); // body/world section
void Contact(tinyxml2::XMLElement* root); // contact section
void Deformable(tinyxml2::XMLElement* root); // deformable section
void Equality(tinyxml2::XMLElement* root); // equality section
@@ -73,6 +73,7 @@ class mjXWriter : public mjXBase {
void OneTendon(tinyxml2::XMLElement* elem, mjCTendon* ptendon, mjCDef* def);
void OneActuator(tinyxml2::XMLElement* elem, mjCActuator* pactuator, mjCDef* def);
void OnePlugin(tinyxml2::XMLElement* elem, mjsPlugin* plugin);
tinyxml2::XMLElement* OneFrame(tinyxml2::XMLElement* elem, mjCFrame* frame);
bool writingdefaults; // true during defaults write
};
src/xml/xml_util.cc
+2 -1
View File
@@ -344,7 +344,8 @@ bool mjXSchema::NameMatch(XMLElement* elem, int level) {
if (name_ == "body" &&
((level == 1 && !strcmp(elem->Value(), "worldbody")) ||
(level != 1 && !strcmp(elem->Value(), "body")) ||
(level >= 1 && !strcmp(elem->Value(), "frame")))) {
(level >= 1 && !strcmp(elem->Value(), "frame")) ||
(level >= 1 && !strcmp(elem->Value(), "replicate")))) {
return true;
}