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

Automated g4 rollback of changelist 604607692.

Browse Source 
*** Reason for rollback ***

breakage resulting in "inertia must have positive eigenvalues" when parsing URDF's

*** Original change description ***

Move fullinertia from mjCAlternative to mjCBody.

***

PiperOrigin-RevId: 604677400
Change-Id: Iab8263b5b0aee310bd75404e314f3b707ce3300e
This commit is contained in:
Matthew Bennice
2024-02-06 09:46:02 -08:00
committed by Copybara-Service
parent 737342bdfe
commit 84f20ffb66
10 changed files with 72 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/xml/xml_urdf.cc
+10 -8
View File
@@ -242,17 +242,19 @@ void mjXURDF::Body(XMLElement* body_elem) {
// inertia
temp = FindSubElem(elem, "inertia", 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);
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);
// process inertia
// lquat = rotation from specified to default (joint/body) inertial frame
double lquat[4], tmpquat[4];
const char* altres = pbody->FullInertia(lquat, pbody->inertia);
const char* altres =
alt.Set(lquat, pbody->inertia, model->degree, model->euler);
// inertia are sometimes 0 in URDF files: ignore error in altres, fix later
(void) altres;
@@ -604,7 +606,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, "XYZ");
alt.Set(quat, 0, 0, "XYZ");
}
}
}