Add childclass support to frames.
PiperOrigin-RevId: 615487831 Change-Id: I0e4d2efd52211701b4286a0b0de84424470a3a47
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
0d16587135
commit
bda03af49a
@@ -3228,7 +3228,7 @@ void mjXReader::Body(XMLElement* section, mjmBody* pbody, mjmFrame* frame) {
|
||||
// get class if specified, otherwise use body
|
||||
mjmDefault* def = GetClass(elem);
|
||||
if (!def) {
|
||||
def = (mjmDefault*)mjm_getDefault(pbody->element);
|
||||
def = mjm_getDefault(frame ? frame->element : pbody->element);
|
||||
}
|
||||
|
||||
// inertial sub-element
|
||||
@@ -3335,9 +3335,29 @@ void mjXReader::Body(XMLElement* section, mjmBody* pbody, mjmFrame* frame) {
|
||||
|
||||
// frame sub-element
|
||||
else if (name=="frame") {
|
||||
// read childdef
|
||||
mjmDefault* childdef = 0;
|
||||
if (ReadAttrTxt(elem, "childclass", text)) {
|
||||
childdef = mjm_findDefault(model, text.c_str());
|
||||
mjm_findDefault(model, text.c_str());
|
||||
if (!childdef) {
|
||||
throw mjXError(elem, "unknown default childclass");
|
||||
}
|
||||
}
|
||||
|
||||
// create frame
|
||||
mjmFrame* pframe = mjm_addFrame(pbody, frame);
|
||||
mjm_setString(pframe->info, ("line = " + std::to_string(elem->GetLineNum())).c_str());
|
||||
mjm_setDefault(pframe->element, childdef ? childdef : def);
|
||||
|
||||
// read attributes
|
||||
std::string name, childclass;
|
||||
if (ReadAttrTxt(elem, "name", name)) {
|
||||
mjm_setString(pframe->name, name.c_str());
|
||||
}
|
||||
if (ReadAttrTxt(elem, "childclass", childclass)) {
|
||||
mjm_setString(pframe->childclass, childclass.c_str());
|
||||
}
|
||||
ReadAttr(elem, "pos", 3, pframe->pos, text);
|
||||
ReadQuat(elem, "quat", pframe->quat, text);
|
||||
ReadAlternative(elem, pframe->alt);
|
||||
@@ -3368,7 +3388,7 @@ void mjXReader::Body(XMLElement* section, mjmBody* pbody, mjmFrame* frame) {
|
||||
mjm_setString(pchild->name, name.c_str());
|
||||
}
|
||||
if (ReadAttrTxt(elem, "childclass", childclass)) {
|
||||
mjm_setString(pchild->classname, childclass.c_str());
|
||||
mjm_setString(pchild->childclass, childclass.c_str());
|
||||
}
|
||||
ReadAttr(elem, "pos", 3, pchild->pos, text);
|
||||
ReadQuat(elem, "quat", pchild->quat, text);
|
||||
|
||||
@@ -807,7 +807,7 @@ string mjXWriter::Write(char *error, size_t error_sz) {
|
||||
Extension(root);
|
||||
Custom(root);
|
||||
Asset(root);
|
||||
Body(InsertEnd(root, "worldbody"), model->GetWorld());
|
||||
Body(InsertEnd(root, "worldbody"), model->GetWorld(), /*frame=*/nullptr);
|
||||
Contact(root);
|
||||
Deformable(root);
|
||||
Equality(root);
|
||||
@@ -1449,13 +1449,19 @@ void mjXWriter::Asset(XMLElement* root) {
|
||||
|
||||
|
||||
// recursive body writer
|
||||
void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
|
||||
void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame) {
|
||||
double unitq[4] = {1, 0, 0, 0};
|
||||
|
||||
if (!body) {
|
||||
throw mjXError(0, "missing body in XML write"); // SHOULD NOT OCCUR
|
||||
}
|
||||
|
||||
// write frame if classname is defined
|
||||
if (frame) {
|
||||
WriteAttrTxt(elem, "name", frame->name);
|
||||
WriteAttrTxt(elem, "childclass", frame->classname);
|
||||
}
|
||||
|
||||
// write body attributes and inertial
|
||||
if (body!=model->GetWorld()) {
|
||||
WriteAttrTxt(elem, "name", body->name);
|
||||
@@ -1490,26 +1496,31 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
@@ -1518,9 +1529,20 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
|
||||
OnePlugin(InsertEnd(elem, "plugin"), &body->plugin);
|
||||
}
|
||||
|
||||
// write frames
|
||||
for (int i=0; i<body->frames.size(); i++) {
|
||||
if (body->frames[i]->frame != frame) continue;
|
||||
if (!body->frames[i]->name.empty() || !body->frames[i]->classname.empty()) {
|
||||
Body(InsertEnd(elem, "frame"), body, body->frames[i]);
|
||||
} else {
|
||||
Body(elem, body, body->frames[i]);
|
||||
}
|
||||
}
|
||||
|
||||
// write child bodies recursively
|
||||
for (int i=0; i<body->bodies.size(); i++) {
|
||||
Body(InsertEnd(elem, "body"), body->bodies[i]);
|
||||
if (body->bodies[i]->frame != frame) continue;
|
||||
Body(InsertEnd(elem, "body"), body->bodies[i], nullptr);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
+17
-17
@@ -40,23 +40,23 @@ class mjXWriter : public mjXBase {
|
||||
mjCModel* model = 0;
|
||||
|
||||
// XML section writers
|
||||
void Compiler(tinyxml2::XMLElement* root); // compiler section
|
||||
void Option(tinyxml2::XMLElement* root); // option section
|
||||
void Size(tinyxml2::XMLElement* root); // size section
|
||||
void Visual(tinyxml2::XMLElement* root); // visual section
|
||||
void Statistic(tinyxml2::XMLElement* root); // statistic section
|
||||
void Default(tinyxml2::XMLElement* root, mjCDef* def); // default section
|
||||
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); // body/world section
|
||||
void Contact(tinyxml2::XMLElement* root); // contact 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
|
||||
void Compiler(tinyxml2::XMLElement* root); // compiler section
|
||||
void Option(tinyxml2::XMLElement* root); // option section
|
||||
void Size(tinyxml2::XMLElement* root); // size section
|
||||
void Visual(tinyxml2::XMLElement* root); // visual section
|
||||
void Statistic(tinyxml2::XMLElement* root); // statistic section
|
||||
void Default(tinyxml2::XMLElement* root, mjCDef* def); // default section
|
||||
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 Contact(tinyxml2::XMLElement* root); // contact 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);
|
||||
|
||||
Reference in New Issue
Block a user