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

Improve full inertia user API.

Browse Source 
PiperOrigin-RevId: 627399310
Change-Id: Ie2bdc6778e80b2b8127b52c73a3cc4b4755d8e0a
This commit is contained in:
Yuval Tassa
2024-04-23 08:39:10 -07:00
committed by Copybara-Service
parent c6ab354bd7
commit f457789de1
6 changed files with 55 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_objects.cc
+38 -38
View File
@@ -96,6 +96,42 @@ static bool islimited(int limited, const double range[2]) {
return false;
}
// compute frame quat and diagonal inertia from full inertia matrix, return error if any
const char* FullInertia(double quat[4], double inertia[3], const double fullinertia[6]) {
if (!mjuu_defined(fullinertia[0])) {
return nullptr;
}
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);
// check mimimal eigenvalue
if (eigval[2]<mjEPS) {
return "inertia must have positive eigenvalues";
}
// copy
if (quat) {
for (int i=0; i<4; i++) {
quat[i] = quattmp[i];
}
}
if (inertia) {
for (int i=0; i<3; i++) {
inertia[i] = eigval[i];
}
}
return nullptr;
}
//------------------------- class mjCError implementation ------------------------------------------
@@ -1182,7 +1218,7 @@ void mjCBody::GeomFrame(void) {
// compute principal axes of inertia
mjuu_copyvec(fullinertia, toti, 6);
const char* errq = FullInertia(iquat, inertia);
const char* errq = FullInertia(iquat, inertia, fullinertia);
if (errq) {
throw mjCError(this, "error '%s' in alternative for principal axes", errq);
}
@@ -1191,42 +1227,6 @@ void mjCBody::GeomFrame(void) {
// compute full inertia
const char* mjCBody::FullInertia(double quat[4], double inertia[3]) {
if (!mjuu_defined(fullinertia[0])) {
return 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);
// check mimimal eigenvalue
if (eigval[2]<mjEPS) {
return "inertia must have positive eigenvalues";
}
// 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];
}
}
return 0;
}
// set explicitinertial to true
void mjCBody::MakeInertialExplicit() {
spec.explicitinertial = true;
@@ -1271,7 +1271,7 @@ void mjCBody::Compile(void) {
}
// check and process orientation alternatives for inertia
const char* ierr = FullInertia(iquat, inertia);
const char* ierr = FullInertia(iquat, inertia, this->fullinertia);
if (ierr) {
throw mjCError(this, "error '%s' in inertia alternative", ierr);
}