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_base.cc
+5 -6
View File
@@ -51,15 +51,14 @@ void mjXBase::SetModel(mjCModel* _model) {
// read alternative orientation specification
int mjXBase::ReadAlternative(XMLElement* elem, mjmOrientation& alt) {
void mjXBase::ReadAlternative(XMLElement* elem, mjmOrientation& alt) {
string text;
int read = (int)(elem->Attribute("quat") != 0) +
(ReadAttr(elem, "axisangle", 4, alt.axisangle, text) ? 1 : 0) +
(ReadAttr(elem, "xyaxes", 6, alt.xyaxes, text) ? 1 : 0) +
(ReadAttr(elem, "zaxis", 3, alt.zaxis, text) ? 1 : 0) +
(ReadAttr(elem, "euler", 3, alt.euler, text) ? 1 : 0);
if (read > 1) {
throw mjXError(elem, "multiple orientation specifiers are not allowed");
}
return read;
(ReadAttr(elem, "euler", 3, alt.euler, text) ? 1 : 0) +
(ReadAttr(elem, "fullinertia", 6, alt.fullinertia, text) ? 1 : 0);
if (read > 1)
throw mjXError(elem, "multiple orientation specifiers for the same field are not allowed");
}
src/xml/xml_base.h
+1 -1
View File
@@ -92,7 +92,7 @@ class mjXBase : public mjXUtil {
void SetModel(mjCModel*);
// read alternative orientation specification
static int ReadAlternative(tinyxml2::XMLElement* elem, mjmOrientation& alt);
static void ReadAlternative(tinyxml2::XMLElement* elem, mjmOrientation& alt);
protected:
mjCModel* model; // internally-allocated mjCModel object
src/xml/xml_native_reader.cc
+1 -5
View File
@@ -3021,11 +3021,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) {
ReadQuat(elem, "quat", pbody->iquat, text);
ReadAttr(elem, "mass", 1, &pbody->mass, text, true);
ReadAttr(elem, "diaginertia", 3, pbody->inertia, text);
bool alt = ReadAlternative(elem, pbody->ialt);
bool full = ReadAttr(elem, "fullinertia", 6, pbody->fullinertia, text);
if (alt && full) {
throw mjXError(elem, "multiple orientation specifiers are not allowed");
}
ReadAlternative(elem, pbody->ialt);
}
// joint sub-element
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");
}
}
}