diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 569d3ee6..c55c15ea 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -4341,15 +4341,6 @@ mjs_resolveOrientation Resolve alternative orientations to quat, return error if any. -.. _mjs_fullInertia: - -mjs_fullInertia -~~~~~~~~~~~~~~~ - -.. mujoco-include:: mjs_fullInertia - -Compute quat and diag inertia from full inertia matrix, return error if any. - .. _ElementInitialization: Element initialization diff --git a/doc/includes/references.h b/doc/includes/references.h index 9bdb0b1f..2e3e3429 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3559,7 +3559,6 @@ void mjs_setDefault(mjsElement* element, mjsDefault* def); void mjs_setFrame(mjsElement* dest, mjsFrame* frame); const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence, const mjsOrientation* orientation); -const char* mjs_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]); void mjs_defaultSpec(mjSpec* spec); void mjs_defaultOrientation(mjsOrientation* orient); void mjs_defaultBody(mjsBody* body); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 86d48cd7..95893542 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1612,9 +1612,6 @@ MJAPI void mjs_setFrame(mjsElement* dest, mjsFrame* frame); MJAPI const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence, const mjsOrientation* orientation); -// 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]); - //---------------------------------- Element initialization --------------------------------------- diff --git a/introspect/functions.py b/introspect/functions.py index 0658ba1d..9733755b 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -10216,37 +10216,6 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Resolve alternative orientations to quat, return error if any.', )), - ('mjs_fullInertia', - FunctionDecl( - name='mjs_fullInertia', - return_type=PointerType( - inner_type=ValueType(name='char', is_const=True), - ), - parameters=( - FunctionParameterDecl( - name='quat', - type=ArrayType( - inner_type=ValueType(name='double'), - extents=(4,), - ), - ), - FunctionParameterDecl( - name='inertia', - type=ArrayType( - inner_type=ValueType(name='double'), - extents=(3,), - ), - ), - FunctionParameterDecl( - name='fullinertia', - type=ArrayType( - inner_type=ValueType(name='double', is_const=True), - extents=(6,), - ), - ), - ), - doc='Compute quat and diag inertia from full inertia matrix, return error if any.', # pylint: disable=line-too-long - )), ('mjs_defaultSpec', FunctionDecl( name='mjs_defaultSpec', diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 2efa3856..de3e58af 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -696,13 +696,6 @@ void mjs_setActivePlugins(mjSpec* s, void* activeplugins) { -// compute full 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 d7c03f0f..168ae6d2 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -273,9 +273,6 @@ MJAPI void mjs_setFrame(mjsElement* dest, mjsFrame* frame); MJAPI const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence, const mjsOrientation* orientation); -// 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 b92232e8..7bd1ebbe 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2921,7 +2921,7 @@ void mjCModel::FuseStatic(void) { // compute principal axes of inertia mjuu_copyvec(par->fullinertia, toti, 6); - const char* err1 = FullInertia(par->iquat, par->inertia, par->fullinertia); + const char* err1 = mjuu_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 bacf8129..68fb0a92 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -91,42 +91,6 @@ 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; - } - - double eigval[3], eigvec[9], quattmp[4]; - double full[9] = { - fullinertia[0], fullinertia[3], fullinertia[4], - fullinertia[3], fullinertia[1], fullinertia[5], - fullinertia[4], fullinertia[5], fullinertia[2] - }; - - mjuu_eig3(eigval, eigvec, quattmp, full); - - // check mimimal eigenvalue - if (eigval[2]fullinertia); + const char* ierr = mjuu_fullInertia(iquat, inertia, this->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 6b3cdf4e..4e1b8a9f 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -86,8 +86,6 @@ const char* ResolveOrientation(double* quat, // set frame quat 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 --------------------------------------- diff --git a/src/user/user_util.cc b/src/user/user_util.cc index 13dc5610..652ab05b 100644 --- a/src/user/user_util.cc +++ b/src/user/user_util.cc @@ -751,6 +751,39 @@ string mjuu_strippath(string filename) { } +// compute frame quat and diagonal inertia from full inertia matrix, return error if any +const char* mjuu_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]) { + if (!mjuu_defined(fullinertia[0])) { + return nullptr; + } + + double eigval[3], eigvec[9], quattmp[4]; + double full[9] = { + fullinertia[0], fullinertia[3], fullinertia[4], + fullinertia[3], fullinertia[1], fullinertia[5], + fullinertia[4], fullinertia[5], fullinertia[2] + }; + + mjuu_eig3(eigval, eigvec, quattmp, full); + + // check mimimal eigenvalue + if (eigval[2]inertia, pbody->fullinertia); + const char* altres = mjuu_fullInertia(lquat, pbody->inertia, pbody->fullinertia); // inertia are sometimes 0 in URDF files: ignore error in altres, fix later (void) altres;