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

Renamed mjs_fullInertia to mjuu_fullInertia and moved it to user_util.

Browse Source 
PiperOrigin-RevId: 644769379
Change-Id: I83511eeb4c0972ce9f46f0e3443d438cccdd4d9f
This commit is contained in:
Alessio Quaglino
2024-06-19 09:38:02 -07:00
committed by Copybara-Service
parent 1eb708646d
commit 7bd7065e0e
12 changed files with 40 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/functions.rst
-9
View File
@@ -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
doc/includes/references.h
-1
View File
@@ -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);
include/mujoco/mujoco.h
-3
View File
@@ -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 ---------------------------------------
introspect/functions.py
-31
View File
@@ -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',
src/user/user_api.cc
-7
View File
@@ -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) {
src/user/user_api.h
-3
View File
@@ -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 -----------------------------------------------
src/user/user_model.cc
+1 -1
View File
@@ -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);
}
src/user/user_objects.cc
+2 -38
View File
@@ -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]<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;
}
// fetches cached image from PNG asset, returns nullopt if not available
@@ -1360,7 +1324,7 @@ void mjCBody::GeomFrame(void) {
// compute principal axes of inertia
mjuu_copyvec(fullinertia, toti, 6);
const char* errq = FullInertia(iquat, inertia, fullinertia);
const char* errq = mjuu_fullInertia(iquat, inertia, fullinertia);
if (errq) {
throw mjCError(this, "error '%s' in alternative for principal axes", errq);
}
@@ -1429,7 +1393,7 @@ void mjCBody::Compile(void) {
}
// check and process orientation alternatives for inertia
const char* ierr = FullInertia(iquat, inertia, this->fullinertia);
const char* ierr = mjuu_fullInertia(iquat, inertia, this->fullinertia);
if (ierr) {
throw mjCError(this, "error '%s' in inertia alternative", ierr);
}
src/user/user_objects.h
-2
View File
@@ -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 ---------------------------------------
src/user/user_util.cc
+33
View File
@@ -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]<mjEPS) {
return "inertia must have positive eigenvalues";
}
// copy
if (quat) {
mjuu_copyvec(quat, quattmp, 4);
}
if (inertia) {
mjuu_copyvec(inertia, eigval, 3);
}
return nullptr;
}
// strip extension
string mjuu_stripext(string filename) {
// find last dot
src/user/user_util.h
+3
View File
@@ -152,6 +152,9 @@ int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double m
// transform vector by pose
void mjuu_trnVecPose(double res[3], const double pos[3], const double quat[4], const double vec[3]);
// 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]);
// strip path from filename
std::string mjuu_strippath(std::string filename);
src/xml/xml_urdf.cc
+1 -1
View File
@@ -274,7 +274,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_fullInertia(lquat, pbody->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;