chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 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/user/user_api.h
+1
View File
@@ -30,6 +30,7 @@ typedef struct _mjmOrientation {
double xyaxes[6]; // x and y axes
double zaxis[3]; // z axis (use minimal rotation)
double euler[3]; // euler rotations
double fullinertia[6]; // non-axis-aligned inertia matrix
} mjmOrientation;
typedef struct _mjmSite {
src/user/user_flexcomp.cc
+1 -1
View File
@@ -117,7 +117,7 @@ bool mjCFlexcomp::Make(mjCModel* model, mjCBody* body, char* error, int error_sz
}
// compute orientation
const char* alterr = alt.Set(quat, model->degree, model->euler);
const char* alterr = alt.Set(quat, NULL, model->degree, model->euler);
if (alterr) {
return comperr(error, alterr, error_sz);
}
src/user/user_model.cc
+3 -2
View File
@@ -2551,8 +2551,9 @@ void mjCModel::FuseStatic(void) {
}
// compute principal axes of inertia
mjuu_copyvec(par->fullinertia, toti, 6);
const char* err1 = par->FullInertia(par->iquat, par->inertia);
mjCAlternative alt;
mjuu_copyvec(alt.fullinertia, toti, 6);
const char* err1 = alt.Set(par->iquat, par->inertia, degree, euler);
if (err1) {
throw mjCError(NULL, "error '%s' in fusing static body inertias", err1);
}
src/user/user_objects.cc
+42 -46
View File
@@ -134,13 +134,14 @@ mjCError::mjCError(const mjCBase* obj, const char* msg, const char* str, int pos
// constructor
mjCAlternative::mjCAlternative() {
axisangle[0] = xyaxes[0] = zaxis[0] = euler[0] = mjNAN;
axisangle[0] = xyaxes[0] = zaxis[0] = euler[0] = fullinertia[0] = mjNAN;
}
// compute frame orientation given alternative specifications
// used for geom, site, body and camera frames
const char* mjCAlternative::Set(double* quat, bool degree, const char* sequence) {
const char* mjCAlternative::Set(double* quat, double* inertia,
bool degree, const char* sequence) {
// set quat using axisangle
if (mjuu_defined(axisangle[0])) {
// convert to radians if necessary, normalize axis
@@ -194,6 +195,32 @@ const char* mjCAlternative::Set(double* quat, bool degree, const char* sequence)
mjuu_z2quat(quat, zaxis);
}
// handle fullinertia
if (mjuu_defined(fullinertia[0])) {
mjtNum eigval[3], eigvec[9], quattmp[4];
mjtNum full[9] = {
fullinertia[0], fullinertia[3], fullinertia[4],
fullinertia[3], fullinertia[1], fullinertia[5],
fullinertia[4], fullinertia[5], fullinertia[2]
};
mju_eig3(eigval, eigvec, quattmp, full);
// copy
for (int i=0; i<4; i++) {
quat[i] = quattmp[i];
}
if (inertia) {
for (int i=0; i<3; i++) {
inertia[i] = eigval[i];
}
}
// check mimimal eigenvalue
if (eigval[2]<mjEPS) {
return "inertia must have positive eigenvalues";
}
}
// handle euler
if (mjuu_defined(euler[0])) {
@@ -553,7 +580,6 @@ mjCBody::mjCBody(mjCModel* _model) {
pos[0] = ipos[0] = mjNAN;
// clear variables
fullinertia[0] = mjNAN;
explicitinertial = false;
mocap = false;
mjuu_setvec(quat, 1, 0, 0, 0);
@@ -889,48 +915,17 @@ void mjCBody::GeomFrame(void) {
}
// compute principal axes of inertia
mjuu_copyvec(fullinertia, toti, 6);
const char* errq = FullInertia(iquat, inertia);
if (errq) {
throw mjCError(this, "error '%s' in alternative for principal axes", errq);
mjCAlternative alt;
mjuu_copyvec(alt.fullinertia, toti, 6);
const char* err1 = alt.Set(iquat, inertia, model->degree, model->euler);
if (err1) {
throw mjCError(this, "error '%s' in alternative for principal axes", err1);
}
}
}
// compute full inertia
const char* mjCBody::FullInertia(double quat[4], double inertia[3]) {
if (mjuu_defined(fullinertia[0])) {
mjtNum eigval[3], eigvec[9], quattmp[4];
mjtNum full[9] = {
fullinertia[0], fullinertia[3], fullinertia[4],
fullinertia[3], fullinertia[1], fullinertia[5],
fullinertia[4], fullinertia[5], fullinertia[2]
};
mju_eig3(eigval, eigvec, quattmp, full);
// copy
for (int i=0; i<4; i++) {
quat[i] = quattmp[i];
}
if (inertia) {
for (int i=0; i<3; i++) {
inertia[i] = eigval[i];
}
}
// check mimimal eigenvalue
if (eigval[2]<mjEPS) {
return "inertia must have positive eigenvalues";
}
}
return 0;
}
// set explicitinertial to true
void mjCBody::MakeInertialExplicit() {
explicitinertial = true;
@@ -962,13 +957,13 @@ void mjCBody::Compile(void) {
}
// check and process orientation alternatives for body
const char* err = alt.Set(quat, model->degree, model->euler);
const char* err = alt.Set(quat, inertia, model->degree, model->euler);
if (err) {
throw mjCError(this, "error '%s' in frame alternative", err);
}
// check and process orientation alternatives for inertia
const char* ierr = FullInertia(iquat, inertia);
const char* ierr = ialt.Set(iquat, inertia, model->degree, model->euler);
if (ierr) {
throw mjCError(this, "error '%s' in inertia alternative", ierr);
}
@@ -1145,7 +1140,7 @@ void mjCFrame::Compile() {
return;
}
const char* err = alt.Set(quat, model->degree, model->euler);
const char* err = alt.Set(quat, 0, model->degree, model->euler);
if (err) {
throw mjCError(this, "orientation specification error '%s' in site %d", err, id);
}
@@ -1828,7 +1823,7 @@ void mjCGeom::Compile(void) {
// not 'fromto': try alternative
else {
const char* err = alt.Set(quat, model->degree, model->euler);
const char* err = alt.Set(quat, inertia, model->degree, model->euler);
if (err) {
throw mjCError(this, "orientation specification error '%s' in geom %d", err, id);
}
@@ -1952,7 +1947,7 @@ mjCSite::mjCSite(mjCModel* _model, mjCDef* _def) {
spec.fromto[0] = mjNAN;
spec_userdata_.clear();
spec.alt.axisangle[0] = spec.alt.xyaxes[0] = spec.alt.zaxis[0] =
spec.alt.euler[0] = mjNAN;
spec.alt.euler[0] = spec.alt.fullinertia[0] = mjNAN;
// clear internal variables
body = 0;
@@ -1992,6 +1987,7 @@ void mjCSite::CopyFromSpec() {
mju_copy(alt_.xyaxes, alt.xyaxes, 6);
mju_copy3(alt_.zaxis, alt.zaxis);
mju_copy3(alt_.euler, alt.euler);
mju_copy(alt_.fullinertia, alt.fullinertia, 6);
}
@@ -2063,7 +2059,7 @@ void mjCSite::Compile(void) {
// alternative orientation
else {
const char* err = alt_.Set(quat, model->degree, model->euler);
const char* err = alt_.Set(quat, 0, model->degree, model->euler);
if (err) {
throw mjCError(this, "orientation specification error '%s' in site %d", err, id);
}
@@ -2129,7 +2125,7 @@ void mjCCamera::Compile(void) {
userdata.resize(model->nuser_cam);
// process orientation specifications
const char* err = alt.Set(quat, model->degree, model->euler);
const char* err = alt.Set(quat, 0, model->degree, model->euler);
if (err) {
throw mjCError(this, "orientation specification error '%s' in camera %d", err, id);
}
src/user/user_objects.h
+6 -11
View File
@@ -111,9 +111,9 @@ class [[nodiscard]] mjCError {
class mjCAlternative : public mjmOrientation {
public:
mjCAlternative(); // constuctor
const char* Set(double* quat, // set frame quat
bool degree, // angle format: degree/radian
const char* sequence); // euler sequence format: "xyz"
const char* Set(double* quat, double* inertia, // set frame quat and diag. inertia
bool degree, // angle format: degree/radian
const char* sequence); // euler sequence format: "xyz"
};
@@ -244,10 +244,6 @@ class mjCBody : public mjCBase {
// set explicitinertial to true
void MakeInertialExplicit();
// compute quat and diag inertia from fullinertia
// return nullptr on success, error string on failure
const char* FullInertia(double quat[4], double inertia[3]);
// variables set by user or 'Compile'
bool mocap; // is this a mocap body
double pos[3]; // frame position
@@ -258,17 +254,16 @@ class mjCBody : public mjCBase {
double inertia[3]; // diagonal inertia (in i-frame)
double gravcomp; // gravity compensation
std::vector<double> userdata; // user data
double fullinertia[6]; // non-axis-aligned inertia matrix
mjCAlternative alt; // alternative orientation specification
mjCAlternative ialt; // alternative for inertial frame
// variables computed by 'Compile' and 'AddXXX'
private:
mjCBody(mjCModel*); // constructor
~mjCBody(); // destructor
mjCBody(mjCModel*); // constructor
~mjCBody(); // destructor
void Compile(void); // compiler
void GeomFrame(void); // get inertial info from geoms
void GeomFrame(void); // get inertial info from geoms
int parentid; // parent index in global array
int weldid; // top index of body we are welded to
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");
}
}
}
test/xml/xml_native_reader_test.cc
+2 -2
View File
@@ -414,7 +414,7 @@ TEST_F(XMLReaderTest, InvalidDoubleOrientation) {
ASSERT_THAT(model, IsNull());
EXPECT_THAT(
error.data(),
HasSubstr("multiple orientation specifiers are not allowed"));
HasSubstr("multiple orientation specifiers for the same field"));
}
}
}
@@ -525,7 +525,7 @@ TEST_F(XMLReaderTest, InvalidInertialOrientation) {
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(),
HasSubstr("multiple orientation specifiers are not allowed"));
HasSubstr("multiple orientation specifiers for the same field"));
}
TEST_F(XMLReaderTest, ReadShellParameter) {