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