diff --git a/src/user/user_api.h b/src/user/user_api.h index 8a983b32..893ff83e 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -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 { diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index da8d4e43..b6f24b96 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -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); } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 8e9c1d27..7b484944 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -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); } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 8d7eaffe..07852c78 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -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]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]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); } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index ac2e5247..2ed3f58e 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -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 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 diff --git a/src/xml/xml_base.cc b/src/xml/xml_base.cc index 2fd255bb..82229747 100644 --- a/src/xml/xml_base.cc +++ b/src/xml/xml_base.cc @@ -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"); } diff --git a/src/xml/xml_base.h b/src/xml/xml_base.h index 7ac718dc..ddff3542 100644 --- a/src/xml/xml_base.h +++ b/src/xml/xml_base.h @@ -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 diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index baa00745..fe5c09b3 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -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 diff --git a/src/xml/xml_urdf.cc b/src/xml/xml_urdf.cc index 6d55d63d..74b7075b 100644 --- a/src/xml/xml_urdf.cc +++ b/src/xml/xml_urdf.cc @@ -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"); } } } diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 9e51bf24..8269b6e6 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -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) {