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

Move fullinertia from mjCAlternative to mjCBody with correction to handle URDF with 0 inertias.

Browse Source 
PiperOrigin-RevId: 604812870
Change-Id: I018254d1362a654a4f99bf5e621d5abc7129a0c6
This commit is contained in:
Matthew Bennice
2024-02-06 17:20:24 -08:00
committed by Copybara-Service
parent 19c41aa965
commit b9c50ad79c
10 changed files with 96 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/xml/xml_urdf.cc
+20 -10
View File
@@ -242,19 +242,29 @@ void mjXURDF::Body(XMLElement* body_elem) {
// inertia
temp = FindSubElem(elem, "inertia", true);
mjCAlternative alt;
ReadAttr(temp, "ixx", 1, alt.fullinertia+0, text, true);
ReadAttr(temp, "iyy", 1, alt.fullinertia+1, text, true);
ReadAttr(temp, "izz", 1, alt.fullinertia+2, text, true);
ReadAttr(temp, "ixy", 1, alt.fullinertia+3, text, true);
ReadAttr(temp, "ixz", 1, alt.fullinertia+4, text, true);
ReadAttr(temp, "iyz", 1, alt.fullinertia+5, text, true);
ReadAttr(temp, "ixx", 1, pbody->fullinertia+0, text, true);
ReadAttr(temp, "iyy", 1, pbody->fullinertia+1, text, true);
ReadAttr(temp, "izz", 1, pbody->fullinertia+2, text, true);
ReadAttr(temp, "ixy", 1, pbody->fullinertia+3, text, true);
ReadAttr(temp, "ixz", 1, pbody->fullinertia+4, text, true);
ReadAttr(temp, "iyz", 1, pbody->fullinertia+5, text, true);
// If the inertias are all 0 in a URDF then it is still undefined.
bool inertia_defined = false;
for (int i = 0; i < 6; ++i) {
if (pbody->fullinertia[i] != 0) {
inertia_defined = true;
break;
}
}
if (!inertia_defined) {
pbody->fullinertia[0] = mjNAN;
}
// process inertia
// lquat = rotation from specified to default (joint/body) inertial frame
double lquat[4], tmpquat[4];
const char* altres =
alt.Set(lquat, pbody->inertia, model->degree, model->euler);
const char* altres = pbody->FullInertia(lquat, pbody->inertia);
// inertia are sometimes 0 in URDF files: ignore error in altres, fix later
(void) altres;
@@ -606,7 +616,7 @@ void mjXURDF::Origin(XMLElement* origin_elem, double* pos, double* quat) {
// orientation
mjCAlternative alt;
if (ReadAttr(temp, "rpy", 3, alt.euler, text)) {
alt.Set(quat, 0, 0, "XYZ");
alt.Set(quat, 0, "XYZ");
}
}
}