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

Add site semantics to weld constraint. Fixes #1896

Browse Source 
PiperOrigin-RevId: 675112981
Change-Id: Ie4c01440d54116075df89f480591700762892cbd
This commit is contained in:
Taylor Howell
2024-09-16 05:44:53 -07:00
committed by Copybara-Service
parent 2da15cf137
commit 60a1921b34
13 changed files with 368 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/xml/xml_native_reader.cc
+46 -9
View File
@@ -365,8 +365,8 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
{"<"},
{"connect", "*", "10", "name", "class", "body1", "body2", "anchor",
"site1", "site2", "active", "solref", "solimp"},
{"weld", "*", "10", "name", "class", "body1", "body2", "relpose", "anchor",
"active", "solref", "solimp", "torquescale"},
{"weld", "*", "12", "name", "class", "body1", "body2", "relpose", "anchor",
"site1", "site2", "active", "solref", "solimp", "torquescale"},
{"joint", "*", "8", "name", "class", "joint1", "joint2", "polycoef",
"active", "solref", "solimp"},
{"tendon", "*", "8", "name", "class", "tendon1", "tendon2", "polycoef",
@@ -1934,13 +1934,50 @@ void mjXReader::OneEquality(XMLElement* elem, mjsEquality* equality) {
}
break;
case mjEQ_WELD:
ReadAttrTxt(elem, "body1", name1, true);
ReadAttrTxt(elem, "body2", name2);
ReadAttr(elem, "relpose", 7, equality->data+3, text);
ReadAttr(elem, "torquescale", 1, equality->data+10, text);
if (!ReadAttr(elem, "anchor", 3, equality->data, text)) {
mjuu_zerovec(equality->data, 3);
case mjEQ_WELD: {
auto maybe_site1 = ReadAttrStr(elem, "site1");
auto maybe_site2 = ReadAttrStr(elem, "site2");
auto maybe_body1 = ReadAttrStr(elem, "body1");
auto maybe_body2 = ReadAttrStr(elem, "body2");
bool has_anchor = ReadAttr(elem, "anchor", 3, equality->data, text);
bool has_relpose = ReadAttr(elem, "relpose", 7, equality->data+3, text);
bool maybe_site = maybe_site1.has_value() || maybe_site2.has_value();
bool maybe_body = maybe_body1.has_value() ||
maybe_body2.has_value() ||
has_anchor ||
has_relpose;
if (maybe_site && maybe_body) {
throw mjXError(elem, "body and site semantics cannot be mixed");
}
bool site_semantic = maybe_site1.has_value() && maybe_site2.has_value();
bool body_semantic = maybe_body1.has_value();
if (site_semantic == body_semantic) {
throw mjXError(
elem,
"either body1 must be defined and optionally {body2, anchor, relpose},"
" or site1 and site2 must be defined");
}
if (body_semantic) {
name1 = maybe_body1.value();
if (maybe_body2.has_value()) {
name2 = maybe_body2.value();
}
equality->objtype = mjOBJ_BODY;
if (!has_anchor) {
mjuu_zerovec(equality->data, 3);
}
} else {
name1 = maybe_site1.value();
name2 = maybe_site2.value();
equality->objtype = mjOBJ_SITE;
}
ReadAttr(elem, "torquescale", 1, equality->data+10, text);
}
break;