From f457789de16436b86d217702909b86efde7f1d1a Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 23 Apr 2024 08:39:10 -0700 Subject: [PATCH] Improve full inertia user API. PiperOrigin-RevId: 627399310 Change-Id: Ie2bdc6778e80b2b8127b52c73a3cc4b4755d8e0a --- src/user/user_api.cc | 7 ++-- src/user/user_api.h | 4 +-- src/user/user_model.cc | 2 +- src/user/user_objects.cc | 76 ++++++++++++++++++++-------------------- src/user/user_objects.h | 23 +++++------- src/xml/xml_urdf.cc | 2 +- 6 files changed, 55 insertions(+), 59 deletions(-) diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 641afc89..7d0d95f5 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -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(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) { diff --git a/src/user/user_api.h b/src/user/user_api.h index 7361ff86..261a882e 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -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 ----------------------------------------------- diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 1497e7d8..2d4b14e5 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -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); } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index abe94ae0..99c2ec10 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -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]fullinertia); if (ierr) { throw mjCError(this, "error '%s' in inertia alternative", ierr); } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index a073157f..1166fb83 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -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; diff --git a/src/xml/xml_urdf.cc b/src/xml/xml_urdf.cc index 0d7f6381..4c1b624b 100644 --- a/src/xml/xml_urdf.cc +++ b/src/xml/xml_urdf.cc @@ -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;