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_api.cc
+4 -3
View File
@@ -679,11 +679,12 @@ void mjs_setActivePlugins(mjSpec* s, void* activeplugins) {
// compute full inertia
const char* mjs_setFullInertia(mjsBody* bodyspec, double quat[4], double inertia[3]) {
mjCBody* body = static_cast<mjCBody*>(bodyspec->element);
return body->FullInertia(quat, inertia);
const char* mjs_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]) {
return FullInertia(quat, inertia, fullinertia);
}
// -------------------------- GLOBAL ASSET CACHE -------------------------------
void mj_setCacheSize(mjCache cache, std::size_t size) {
src/user/user_api.h
+2 -2
View File
@@ -961,8 +961,8 @@ MJAPI void mjs_setFrame(mjElement* dest, mjsFrame* frame);
MJAPI const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence,
const mjsOrientation* orientation);
// Compute quat and inertia from fullinertia, return error if any.
MJAPI const char* mjs_setFullInertia(mjsBody* body, double quat[4], double inertia[3]);
// Compute quat and diag inertia from full inertia matrix, return error if any.
MJAPI const char* mjs_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]);
//---------------------------------- Initialization -----------------------------------------------
src/user/user_model.cc
+1 -1
View File
@@ -2861,7 +2861,7 @@ void mjCModel::FuseStatic(void) {
// compute principal axes of inertia
mjuu_copyvec(par->fullinertia, toti, 6);
const char* err1 = par->FullInertia(par->iquat, par->inertia);
const char* err1 = FullInertia(par->iquat, par->inertia, par->fullinertia);
if (err1) {
throw mjCError(NULL, "error '%s' in fusing static body inertias", err1);
}
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);
}
src/user/user_objects.h
+9 -14
View File
@@ -39,9 +39,9 @@ class mjCSite;
class mjCCamera;
class mjCLight;
class mjCHField;
class mjCFlex; // defined in user_mesh
class mjCMesh; // defined in user_mesh
class mjCSkin; // defined in user_mesh
class mjCFlex; // defined in user_mesh.h
class mjCMesh; // defined in user_mesh.h
class mjCSkin; // defined in user_mesh.h
class mjCTexture;
class mjCMaterial;
class mjCPair;
@@ -55,17 +55,16 @@ class mjCNumeric;
class mjCText;
class mjCTuple;
class mjCDef;
class mjCModel; // defined in user_model
class mjXWriter; // defined in xml_native
class mjXURDF; // defined in xml_urdf
class mjCModel; // defined in user_model.h
class mjXWriter; // defined in xml_native.h
class mjXURDF; // defined in xml_urdf.h
//------------------------- helper classes and constants -------------------------------------------
//------------------------- helper constants, classes and functions --------------------------------
// number of positive size parameters for each geom type
const int mjGEOMINFO[mjNGEOMTYPES] = {3, 0, 1, 2, 3, 2, 3, 0};
// error information
class [[nodiscard]] mjCError {
public:
@@ -79,14 +78,14 @@ class [[nodiscard]] mjCError {
bool warning; // is this a warning instead of error
};
// alternative specifications of frame orientation
const char* ResolveOrientation(double* quat, // set frame quat
bool degree, // angle format: degree/radian
const char* sequence, // euler sequence format: "xyz"
const mjsOrientation& orient);
// 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]);
//------------------------- class mjCBoundingVolumeHierarchy ---------------------------------------
@@ -292,10 +291,6 @@ class mjCBody : public mjCBody_, private mjsBody {
// 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
mjsBody spec;
src/xml/xml_urdf.cc
+1 -1
View File
@@ -271,7 +271,7 @@ void mjXURDF::Body(XMLElement* body_elem) {
// lquat = rotation from specified to default (joint/body) inertial frame
double lquat[4] = {1, 0, 0, 0};
double tmpquat[4] = {1, 0, 0, 0};
const char* altres = mjs_setFullInertia(pbody, lquat, pbody->inertia);
const char* altres = mjs_fullInertia(lquat, pbody->inertia, pbody->fullinertia);
// inertia are sometimes 0 in URDF files: ignore error in altres, fix later
(void) altres;