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

Compute moment of inertia for concave and boundary meshes. Resolve #338.

Browse Source 
- Mesh inertias can now be computed exactly for well-formed (no holes) non-convex meshes.
- To activate this feature, set `<compiler exactmeshinertia="true">` (defaults to `false`). This default may change in the future.
- Added `<geom shellinertia="true/false">` (defaults to `false`). When true, geom inertia is computed assuming all the mass is concentrated on the surface, and `density` is interpreted as surface density (mass/area). Currently only mesh geoms are supported.

PiperOrigin-RevId: 464368395
Change-Id: I17afd99b9b221c9d24ae951f6e63d5c61ff89820
This commit is contained in:
Alessio Quaglino
2022-07-31 04:31:46 -07:00
committed by Copybara-Service
parent 52d78f8d24
commit 5c5449bf82
19 changed files with 37807 additions and 272 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_mesh.cc
+272 -233
View File
@@ -65,9 +65,12 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
useredge.clear();
// clear internal variables
mjuu_setvec(pos, 0, 0, 0);
mjuu_setvec(quat, 1, 0, 0, 0);
mjuu_setvec(boxsz, 0, 0, 0);
mjuu_setvec(pos_surface, 0, 0, 0);
mjuu_setvec(pos_volume, 0, 0, 0);
mjuu_setvec(quat_surface, 1, 0, 0, 0);
mjuu_setvec(quat_volume, 1, 0, 0, 0);
mjuu_setvec(boxsz_surface, 0, 0, 0);
mjuu_setvec(boxsz_volume, 0, 0, 0);
mjuu_setvec(aabb, 0, 0, 0);
nvert = 0;
nface = 0;
@@ -251,20 +254,23 @@ void mjCMesh::Compile(const mjVFS* vfs) {
// get position
void mjCMesh::GetPos(double* _pos) {
_pos[0] = pos[0];
_pos[1] = pos[1];
_pos[2] = pos[2];
double* mjCMesh::GetPosPtr(mjtMeshType type) {
if (type==mjSHELL_MESH) {
return pos_surface;
} else {
return pos_volume;
}
}
// get orientation
void mjCMesh::GetQuat(double* _quat) {
_quat[0] = quat[0];
_quat[1] = quat[1];
_quat[2] = quat[2];
_quat[3] = quat[3];
double* mjCMesh::GetQuatPtr(mjtMeshType type) {
if (type==mjSHELL_MESH) {
return quat_surface;
} else {
return quat_volume;
}
}
@@ -275,7 +281,8 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
// use inertial box
if (!model->fitaabb) {
// compute depending on type
// get inertia box type (shell or volume)
double* boxsz = GetInertiaBoxPtr(geom->typeinertia);
switch (geom->type) {
case mjGEOM_SPHERE:
geom->size[0] = (boxsz[0] + boxsz[1] + boxsz[2])/3;
@@ -303,7 +310,7 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
}
// copy mesh pos into meshpos
mjuu_copyvec(meshpos, pos, 3);
mjuu_copyvec(meshpos, GetPosPtr(geom->typeinertia), 3);
}
// use AABB
@@ -860,252 +867,284 @@ static double _areaNrmCen(double* normal, double* center,
// apply transformations
void mjCMesh::Process(void) {
double CoM[3] = {0, 0, 0};
double facecen[3] = {0, 0, 0};
double area = 0;
double inert[6] = {0, 0, 0, 0, 0, 0};
double volume;
void mjCMesh::Process() {
for ( const auto type : { mjtMeshType::mjVOLUME_MESH, mjtMeshType::mjSHELL_MESH } ) {
double CoM[3] = {0, 0, 0};
double facecen[3] = {0, 0, 0};
double area = 0;
double inert[6] = {0, 0, 0, 0, 0, 0};
int i, j;
double nrm[3];
double cen[3];
int i, j;
double nrm[3];
double cen[3];
// translate
if (refpos[0]!=0 || refpos[1]!=0 || refpos[2]!=0) {
// prepare translation
float rp[3] = {(float)refpos[0], (float)refpos[1], (float)refpos[2]};
if (type==mjVOLUME_MESH) {
// translate
if (refpos[0]!=0 || refpos[1]!=0 || refpos[2]!=0) {
// prepare translation
float rp[3] = {(float)refpos[0], (float)refpos[1], (float)refpos[2]};
// process vertices
for (i=0; i<nvert; i++) {
// positions
vert[3*i] -= rp[0];
vert[3*i+1] -= rp[1];
vert[3*i+2] -= rp[2];
// process vertices
for (i=0; i<nvert; i++) {
// positions
vert[3*i] -= rp[0];
vert[3*i+1] -= rp[1];
vert[3*i+2] -= rp[2];
// normals not affected by translation
}
}
// normals not affected by translation
}
}
// rotate
if (refquat[0]!=1 || refquat[1]!=0 || refquat[2]!=0 || refquat[3]!=0) {
// prepare rotation
mjtNum quat[4] = {refquat[0], refquat[1], refquat[2], refquat[3]};
mjtNum mat[9];
mju_normalize4(quat);
mju_quat2Mat(mat, quat);
// rotate
if (refquat[0]!=1 || refquat[1]!=0 || refquat[2]!=0 || refquat[3]!=0) {
// prepare rotation
mjtNum quat[4] = {refquat[0], refquat[1], refquat[2], refquat[3]};
mjtNum mat[9];
mju_normalize4(quat);
mju_quat2Mat(mat, quat);
// process vertices
for (i=0; i<nvert; i++) {
// positions
mjtNum p1[3], p0[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]};
mju_rotVecMatT(p1, p0, mat);
vert[3*i] = (float) p1[0];
vert[3*i+1] = (float) p1[1];
vert[3*i+2] = (float) p1[2];
// process vertices
for (i=0; i<nvert; i++) {
// positions
mjtNum p1[3], p0[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]};
mju_rotVecMatT(p1, p0, mat);
vert[3*i] = (float) p1[0];
vert[3*i+1] = (float) p1[1];
vert[3*i+2] = (float) p1[2];
// nromals
mjtNum n1[3], n0[3] = {normal[3*i], normal[3*i+1], normal[3*i+2]};
mju_rotVecMatT(n1, n0, mat);
normal[3*i] = (float) n1[0];
normal[3*i+1] = (float) n1[1];
normal[3*i+2] = (float) n1[2];
}
}
// nromals
mjtNum n1[3], n0[3] = {normal[3*i], normal[3*i+1], normal[3*i+2]};
mju_rotVecMatT(n1, n0, mat);
normal[3*i] = (float) n1[0];
normal[3*i+1] = (float) n1[1];
normal[3*i+2] = (float) n1[2];
}
}
// scale
if (scale[0]!=1 || scale[1]!=1 || scale[2]!=1) {
for (i=0; i<nvert; i++) {
// positions
vert[3*i] *= scale[0];
vert[3*i+1] *= scale[1];
vert[3*i+2] *= scale[2];
// scale
if (scale[0]!=1 || scale[1]!=1 || scale[2]!=1) {
for (i=0; i<nvert; i++) {
// positions
vert[3*i] *= scale[0];
vert[3*i+1] *= scale[1];
vert[3*i+2] *= scale[2];
// normals
normal[3*i] *= scale[0];
normal[3*i+1] *= scale[1];
normal[3*i+2] *= scale[2];
}
}
// normals
normal[3*i] *= scale[0];
normal[3*i+1] *= scale[1];
normal[3*i+2] *= scale[2];
}
}
// normalize normals
for (i=0; i<nvert; i++) {
// compute length
float len = normal[3*i]*normal[3*i] + normal[3*i+1]*normal[3*i+1] + normal[3*i+2]*normal[3*i+2];
// normalize normals
for (i=0; i<nvert; i++) {
// compute length
float len = normal[3*i]*normal[3*i] + normal[3*i+1]*normal[3*i+1] + normal[3*i+2]*normal[3*i+2];
// rescale
if (len>mjMINVAL) {
float scl = 1/sqrtf(len);
normal[3*i] *= scl;
normal[3*i+1] *= scl;
normal[3*i+2] *= scl;
} else {
normal[3*i] = 0;
normal[3*i+1] = 0;
normal[3*i+2] = 1;
}
}
// rescale
if (len>mjMINVAL) {
float scl = 1/sqrtf(len);
normal[3*i] *= scl;
normal[3*i+1] *= scl;
normal[3*i+2] *= scl;
} else {
normal[3*i] = 0;
normal[3*i+1] = 0;
normal[3*i+2] = 1;
}
}
// find centroid of faces
for (i=0; i<nface; i++) {
// check vertex indices
for (j=0; j<3; j++) {
if (face[3*i+j]<0 || face[3*i+j]>=nvert) {
throw mjCError(this, "vertex index out of range in %s (index = %d)", name.c_str(), i);
// find centroid of faces
for (i=0; i<nface; i++) {
// check vertex indices
for (j=0; j<3; j++) {
if (face[3*i+j]<0 || face[3*i+j]>=nvert) {
throw mjCError(this, "vertex index out of range in %s (index = %d)", name.c_str(), i);
}
}
// get area and center
double a = _areaNrmCen(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]);
// accumulate
for (j=0; j<3; j++) {
facecen[j] += a*cen[j];
}
area += a;
}
// require positive area
if (area < mjMINVAL) {
throw mjCError(this, "mesh surface area is too small: %s", name.c_str());
}
// finalize centroid of faces
for (j=0; j<3; j++) {
facecen[j] /= area;
}
}
// get area and center
double a = _areaNrmCen(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]);
// compute CoM and volume from pyramid volumes
GetVolumeRef(type) = 0;
for (i=0; i<nface; i++) {
// get area, normal and center
double a = _areaNrmCen(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]);
// accumulate
for (j=0; j<3; j++) {
facecen[j] += a*cen[j];
}
area += a;
}
// compute and add volume
const double vec[3] = {cen[0]-facecen[0], cen[1]-facecen[1], cen[2]-facecen[2]};
double vol = type==mjSHELL_MESH ? a : mjuu_dot3(vec, nrm) * a / 3;
GetVolumeRef(type) += vol;
// require positive area
if (area < mjMINVAL) {
throw mjCError(this, "mesh surface area is too small: %s", name.c_str());
}
// finalize centroid of faces
for (j=0; j<3; j++) {
facecen[j] /= area;
}
// compute CoM and volume from pyramid volumes
volume = 0;
for (i=0; i<nface; i++) {
// get area, normal and center
double a = _areaNrmCen(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]);
// compute and add volume
const double vec[3] = {cen[0]-facecen[0], cen[1]-facecen[1], cen[2]-facecen[2]};
double vol = fabs(mjuu_dot3(vec, nrm)) * a / 3;
volume += vol;
// add pyramid com
for (j=0; j<3; j++) {
CoM[j] += vol*(cen[j]*3.0/4.0 + facecen[j]/4.0);
}
}
// require positive volume
if (volume < mjMINVAL) {
throw mjCError(this, "mesh volume is too small: %s", name.c_str());
}
// finalize CoM, save as mesh center
for (j=0; j<3; j++) {
CoM[j] /= volume;
}
mjuu_copyvec(pos, CoM, 3);
// re-center mesh at CoM
for (i=0; i<nvert; i++) {
for (j=0; j<3; j++) {
vert[3*i+j] -= CoM[j];
}
}
// accumulate products of inertia, recompute volume
const int k[6][2] = {{0, 0}, {1, 1}, {2, 2}, {0, 1}, {0, 2}, {1, 2}};
double P[6] = {0, 0, 0, 0, 0, 0};
volume = 0;
for (i=0; i<nface; i++) {
float* D = vert+3*face[3*i];
float* E = vert+3*face[3*i+1];
float* F = vert+3*face[3*i+2];
// get area, normal and center; update volume
double a = _areaNrmCen(nrm, cen, D, E, F);
double vol = fabs(mjuu_dot3(cen, nrm)) * a / 3;
volume += vol;
// apply formula, accumulate
for (j=0; j<6; j++) {
P[j] += def->geom.density*vol/20 * (
2*(D[k[j][0]] * D[k[j][1]] +
E[k[j][0]] * E[k[j][1]] +
F[k[j][0]] * F[k[j][1]]) +
D[k[j][0]] * E[k[j][1]] + D[k[j][1]] * E[k[j][0]] +
D[k[j][0]] * F[k[j][1]] + D[k[j][1]] * F[k[j][0]] +
E[k[j][0]] * F[k[j][1]] + E[k[j][1]] * F[k[j][0]]);
}
}
// convert from products of inertia to moments of inertia
inert[0] = P[1] + P[2];
inert[1] = P[0] + P[2];
inert[2] = P[0] + P[1];
inert[3] = -P[3];
inert[4] = -P[4];
inert[5] = -P[5];
// get quaternion and diagonal inertia
mjtNum eigval[3], eigvec[9], quattmp[4];
mjtNum full[9] = {
inert[0], inert[3], inert[4],
inert[3], inert[1], inert[5],
inert[4], inert[5], inert[2]
};
mju_eig3(eigval, eigvec, quattmp, full);
// check eigval - SHOULD NOT OCCUR
if (eigval[2]<=0) {
throw mjCError(this, "eigenvalue of mesh inertia must be positive: %s", name.c_str());
}
if (eigval[0] + eigval[1] < eigval[2] ||
eigval[0] + eigval[2] < eigval[1] ||
eigval[1] + eigval[2] < eigval[0]) {
throw mjCError(this,
"eigenvalues of mesh inertia violate A + B >= C condition: %s", name.c_str());
}
// compute sizes of equivalent inertia box
double mass = volume * def->geom.density;
boxsz[0] = sqrt(6*(eigval[1]+eigval[2]-eigval[0])/mass)/2;
boxsz[1] = sqrt(6*(eigval[0]+eigval[2]-eigval[1])/mass)/2;
boxsz[2] = sqrt(6*(eigval[0]+eigval[1]-eigval[2])/mass)/2;
// copy quat
for (j=0; j<4; j++) {
quat[j] = quattmp[j];
}
// rotate vertices and normals into axis-aligned frame
double neg[4] = {quattmp[0], -quattmp[1], -quattmp[2], -quattmp[3]};
double mat[9];
mjuu_quat2mat(mat, neg);
for (i=0; i<nvert; i++) {
// vertices
const double vec[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]};
double res[3];
mjuu_mulvecmat(res, vec, mat);
for (j=0; j<3; j++) {
vert[3*i+j] = (float) res[j];
// add pyramid com
for (j=0; j<3; j++) {
CoM[j] += vol*(cen[j]*3.0/4.0 + facecen[j]/4.0);
}
}
// normals
const double nrm[3] = {normal[3*i], normal[3*i+1], normal[3*i+2]};
mjuu_mulvecmat(res, nrm, mat);
for (j=0; j<3; j++) {
normal[3*i+j] = (float) res[j];
// require positive volume
if (GetVolumeRef(type) < mjMINVAL) {
throw mjCError(this, "mesh volume is too small: %s", name.c_str());
}
}
// compute axis-aligned bounding box
for (i=0; i<nvert; i++) {
float* v = vert+3*i;
// finalize CoM, save as mesh center
for (j=0; j<3; j++) {
aabb[j] = mjMAX(aabb[j], fabs(v[j]));
CoM[j] /= GetVolumeRef(type);
}
mjuu_copyvec(GetPosPtr(type), CoM, 3);
// re-center mesh at CoM
if (type==mjVOLUME_MESH) {
for (i=0; i<nvert; i++) {
for (j=0; j<3; j++) {
vert[3*i+j] -= CoM[j];
}
}
}
// accumulate products of inertia, recompute volume
const int k[6][2] = {{0, 0}, {1, 1}, {2, 2}, {0, 1}, {0, 2}, {1, 2}};
double P[6] = {0, 0, 0, 0, 0, 0};
GetVolumeRef(type) = 0;
for (i=0; i<nface; i++) {
float* D = vert+3*face[3*i];
float* E = vert+3*face[3*i+1];
float* F = vert+3*face[3*i+2];
// get area, normal and center; update volume
double a = _areaNrmCen(nrm, cen, D, E, F);
double vol = type==mjSHELL_MESH ? a : mjuu_dot3(cen, nrm) * a / 3;
// if legacy computation requested, then always positive
if (!model->exactmeshinertia) {
vol = fabs(vol);
}
// apply formula, accumulate
GetVolumeRef(type) += vol;
for (j=0; j<6; j++) {
P[j] += def->geom.density*vol /
(type==mjSHELL_MESH ? 12 : 20) * (
2*(D[k[j][0]] * D[k[j][1]] +
E[k[j][0]] * E[k[j][1]] +
F[k[j][0]] * F[k[j][1]]) +
D[k[j][0]] * E[k[j][1]] + D[k[j][1]] * E[k[j][0]] +
D[k[j][0]] * F[k[j][1]] + D[k[j][1]] * F[k[j][0]] +
E[k[j][0]] * F[k[j][1]] + E[k[j][1]] * F[k[j][0]]);
}
}
// convert from products of inertia to moments of inertia
inert[0] = P[1] + P[2];
inert[1] = P[0] + P[2];
inert[2] = P[0] + P[1];
inert[3] = -P[3];
inert[4] = -P[4];
inert[5] = -P[5];
// get quaternion and diagonal inertia
mjtNum eigval[3], eigvec[9], quattmp[4];
mjtNum full[9] = {
inert[0], inert[3], inert[4],
inert[3], inert[1], inert[5],
inert[4], inert[5], inert[2]
};
mju_eig3(eigval, eigvec, quattmp, full);
// check eigval - SHOULD NOT OCCUR
if (eigval[2]<=0) {
throw mjCError(this, "eigenvalue of mesh inertia must be positive: %s", name.c_str());
}
if (eigval[0] + eigval[1] < eigval[2] ||
eigval[0] + eigval[2] < eigval[1] ||
eigval[1] + eigval[2] < eigval[0]) {
throw mjCError(this,
"eigenvalues of mesh inertia violate A + B >= C condition: %s", name.c_str());
}
// compute sizes of equivalent inertia box
double mass = GetVolumeRef(type) * def->geom.density;
double* boxsz = GetInertiaBoxPtr(type);
boxsz[0] = sqrt(6*(eigval[1]+eigval[2]-eigval[0])/mass)/2;
boxsz[1] = sqrt(6*(eigval[0]+eigval[2]-eigval[1])/mass)/2;
boxsz[2] = sqrt(6*(eigval[0]+eigval[1]-eigval[2])/mass)/2;
// copy quat
for (j=0; j<4; j++) {
GetQuatPtr(type)[j] = quattmp[j];
}
// rotate vertices and normals into axis-aligned frame
if (type==mjVOLUME_MESH) {
double neg[4] = {quattmp[0], -quattmp[1], -quattmp[2], -quattmp[3]};
double mat[9];
mjuu_quat2mat(mat, neg);
for (i=0; i<nvert; i++) {
// vertices
const double vec[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]};
double res[3];
mjuu_mulvecmat(res, vec, mat);
for (j=0; j<3; j++) {
vert[3*i+j] = (float) res[j];
}
// normals
const double nrm[3] = {normal[3*i], normal[3*i+1], normal[3*i+2]};
mjuu_mulvecmat(res, nrm, mat);
for (j=0; j<3; j++) {
normal[3*i+j] = (float) res[j];
}
}
// compute axis-aligned bounding box
for (i=0; i<nvert; i++) {
float* v = vert+3*i;
for (j=0; j<3; j++) {
aabb[j] = mjMAX(aabb[j], fabs(v[j]));
}
}
}
}
}
// compute inertia
double* mjCMesh::GetInertiaBoxPtr(mjtMeshType type) {
if (type==mjSHELL_MESH) {
return boxsz_surface;
} else {
return boxsz_volume;
}
}
double& mjCMesh::GetVolumeRef(mjtMeshType type) {
if (type) {
return surface;
} else {
return volume;
}
}
// make graph describing convex hull
void mjCMesh::MakeGraph(void) {
src/user/user_model.cc
+1
View File
@@ -99,6 +99,7 @@ mjCModel::mjCModel() {
inertiafromgeom = mjINERTIAFROMGEOM_AUTO;
inertiagrouprange[0] = 0;
inertiagrouprange[1] = mjNGROUP-1;
exactmeshinertia = false;
mj_defaultLROpt(&LRopt);
//------------------------ statistics override
src/user/user_model.h
+1
View File
@@ -124,6 +124,7 @@ class mjCModel {
bool fusestatic; // fuse static bodies with parent
int inertiafromgeom; // use geom inertias (mjtInertiaFromGeom)
int inertiagrouprange[2]; // range of geom groups used to compute inertia
bool exactmeshinertia; // if false, use old formula
mjLROpt LRopt; // options for lengthrange computation
//------------------------ statistics override (if defined)
src/user/user_objects.cc
+16 -7
View File
@@ -967,6 +967,7 @@ mjCGeom::mjCGeom(mjCModel* _model, mjCDef* _def) {
rgba[0] = rgba[1] = rgba[2] = 0.5f;
rgba[3] = 1.0f;
userdata.clear();
typeinertia = mjVOLUME_MESH;
// clear internal variables
mjuu_setvec(quat, 1, 0, 0, 0);
@@ -1003,7 +1004,11 @@ double mjCGeom::GetVolume(void) {
}
mjCMesh* pmesh = model->meshes[meshid];
return pmesh->boxsz[0]*pmesh->boxsz[1]*pmesh->boxsz[2]*8;
if (model->exactmeshinertia) {
return pmesh->GetVolumeRef(typeinertia);
} else {
return pmesh->boxsz_volume[0]*pmesh->boxsz_volume[1]*pmesh->boxsz_volume[2]*8;
}
}
// compute from geom shape
@@ -1045,13 +1050,17 @@ void mjCGeom::SetInertia(void) {
}
mjCMesh* pmesh = model->meshes[meshid];
inertia[0] = mass*(pmesh->boxsz[1]*pmesh->boxsz[1] + pmesh->boxsz[2]*pmesh->boxsz[2]) / 3;
inertia[1] = mass*(pmesh->boxsz[0]*pmesh->boxsz[0] + pmesh->boxsz[2]*pmesh->boxsz[2]) / 3;
inertia[2] = mass*(pmesh->boxsz[0]*pmesh->boxsz[0] + pmesh->boxsz[1]*pmesh->boxsz[1]) / 3;
double* boxsz = pmesh->GetInertiaBoxPtr(typeinertia);
inertia[0] = mass*(boxsz[1]*boxsz[1] + boxsz[2]*boxsz[2]) / 3;
inertia[1] = mass*(boxsz[0]*boxsz[0] + boxsz[2]*boxsz[2]) / 3;
inertia[2] = mass*(boxsz[0]*boxsz[0] + boxsz[1]*boxsz[1]) / 3;
}
// compute from geom shape
else {
if (typeinertia)
throw mjCError(this, "typeinertia currently only available for meshes'%s' (id = %d)",
name.c_str(), id);
switch (type) {
case mjGEOM_SPHERE:
inertia[0] = inertia[1] = inertia[2] = 2*mass*size[0]*size[0]/5;
@@ -1068,7 +1077,7 @@ void mjCGeom::SetInertia(void) {
// add two hemispheres, displace along third axis
double sphere_inertia = 2*sphere_mass*radius*radius/5;
inertia[0] += sphere_inertia + sphere_mass*height*(3*radius + 2*height)/8;
inertia[1] += sphere_inertia + sphere_mass*height*(3*radius + 2*height)/8;;
inertia[1] += sphere_inertia + sphere_mass*height*(3*radius + 2*height)/8;
inertia[2] += sphere_inertia;
return;
}
@@ -1354,11 +1363,11 @@ void mjCGeom::Compile(void) {
mesh.clear();
meshid = -1;
} else {
mjuu_copyvec(meshpos, pmesh->pos, 3);
mjuu_copyvec(meshpos, pmesh->GetPosPtr(typeinertia), 3);
}
// apply geom pos/quat as offset
mjuu_frameaccum(pos, quat, meshpos, pmesh->quat);
mjuu_frameaccum(pos, quat, meshpos, pmesh->GetQuatPtr(typeinertia));
}
// check size parameters
src/user/user_objects.h
+23 -7
View File
@@ -76,6 +76,13 @@ typedef enum _mjtMark {
} mjtMark;
// type of mesh
typedef enum _mjtMeshType {
mjVOLUME_MESH,
mjSHELL_MESH,
} mjtMeshType;
// error information
class mjCError {
public:
@@ -298,6 +305,7 @@ class mjCGeom : public mjCBase {
std::string material; // name of material used for rendering
std::vector<double> userdata; // user data
float rgba[4]; // rgba when material is omitted
mjtMeshType typeinertia; // selects between surface and volume inertia
// variables set by user and used during compilation
double _mass; // used to compute density
@@ -443,9 +451,11 @@ class mjCMesh: public mjCBase {
friend class mjXWriter;
public:
void GetPos(double* pos); // get position
void GetQuat(double* quat); // get orientation
void FitGeom(mjCGeom* geom, double* meshpos); // approximate mesh with simple geom
double* GetPosPtr(mjtMeshType type); // get position
double* GetQuatPtr(mjtMeshType type); // get orientation
double* GetInertiaBoxPtr(mjtMeshType type); // get inertia box
double& GetVolumeRef(mjtMeshType type); // get volume
void FitGeom(mjCGeom* geom, double* meshpos); // approximate mesh with simple geom
std::string file; // mesh file
double refpos[3]; // reference position (translate)
@@ -469,14 +479,20 @@ class mjCMesh: public mjCBase {
void MakeGraph(void); // make graph of convex hull
void CopyGraph(void); // copy graph into face data
void MakeNormal(void); // compute vertex normals
void Process(void); // apply transformations
void Process(); // apply transformations
void RemoveRepeated(void); // remove repeated vertices
void ComputeInertia(mjtMeshType type); // compute inertia
// mesh properties computed by Compile
double pos[3]; // CoM position
double quat[4]; // inertia orientation
double boxsz[3]; // half-sizes of equivalent inertia box
double pos_volume[3]; // CoM position
double pos_surface[3]; // CoM position
double quat_volume[4]; // inertia orientation
double quat_surface[4]; // inertia orientation
double boxsz_volume[3]; // half-sizes of equivalent inertia box (volume)
double boxsz_surface[3]; // half-sizes of equivalent inertia box (surface)
double aabb[3]; // half-sizes of axis-aligned bounding box
double volume; // volume of the mesh
double surface; // surface of the mesh
// mesh data to be copied into mjModel
int nvert; // number of vertices
src/xml/xml_base.h
+1
View File
@@ -68,6 +68,7 @@ extern const mjMap gain_map[];
extern const mjMap bias_map[];
extern const mjMap stage_map[];
extern const mjMap datatype_map[];
extern const mjMap meshtype_map[];
//---------------------------------- Base XML class ------------------------------------------------
src/xml/xml_native_reader.cc
+22 -7
View File
@@ -46,10 +46,10 @@ static const int nMJCF = 163;
static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"mujoco", "!", "1", "model"},
{"<"},
{"compiler", "*", "17", "boundmass", "boundinertia", "settotalmass", "balanceinertia",
{"compiler", "*", "18", "boundmass", "boundinertia", "settotalmass", "balanceinertia",
"strippath", "coordinate", "angle", "fitaabb", "eulerseq",
"meshdir", "texturedir", "discardvisual", "convexhull", "usethread",
"fusestatic", "inertiafromgeom", "inertiagrouprange"},
"fusestatic", "inertiafromgeom", "inertiagrouprange", "exactmeshinertia"},
{"<"},
{"lengthrange", "?", "10", "mode", "useexisting", "uselimit",
"accel", "maxforce", "timeconst", "timestep",
@@ -103,9 +103,9 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"limited", "solreflimit", "solimplimit",
"solreffriction", "solimpfriction", "stiffness", "range", "margin",
"ref", "springref", "armature", "damping", "frictionloss", "user"},
{"geom", "?", "30", "type", "pos", "quat", "contype", "conaffinity", "condim",
{"geom", "?", "31", "type", "pos", "quat", "contype", "conaffinity", "condim",
"group", "priority", "size", "material", "friction", "mass", "density",
"solmix", "solref", "solimp",
"shellinertia", "solmix", "solref", "solimp",
"margin", "gap", "fromto", "axisangle", "xyaxes", "zaxis", "euler",
"hfield", "mesh", "fitscale", "rgba", "fluidshape", "fluidcoef", "user"},
{"site", "?", "13", "type", "group", "pos", "quat", "material",
@@ -160,7 +160,7 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"asset", "*", "0"},
{"<"},
{"texture", "*", "21", "name", "type", "file", "gridsize", "gridlayout",
"fileright", "fileleft", "fileup", "filedown","filefront", "fileback",
"fileright", "fileleft", "fileup", "filedown", "filefront", "fileback",
"builtin", "rgb1", "rgb2", "mark", "markrgb", "random", "width", "height",
"hflip", "vflip"},
{"hfield", "*", "5", "name", "file", "nrow", "ncol", "size"},
@@ -186,9 +186,9 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"stiffness", "range", "margin", "ref", "springref", "armature", "damping",
"frictionloss", "user"},
{"freejoint", "*", "2", "name", "group"},
{"geom", "*", "32", "name", "class", "type", "contype", "conaffinity", "condim",
{"geom", "*", "33", "name", "class", "type", "contype", "conaffinity", "condim",
"group", "priority", "size", "material", "friction", "mass", "density",
"solmix", "solref", "solimp",
"shellinertia", "solmix", "solref", "solimp",
"margin", "gap", "fromto", "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler",
"hfield", "mesh", "fitscale", "rgba", "fluidshape", "fluidcoef", "user"},
{"site", "*", "15", "name", "class", "type", "group", "pos", "quat",
@@ -608,6 +608,13 @@ const mjMap tkind_map[2] = {
};
// mesh type
const mjMap meshtype_map[2] = {
{"false", mjVOLUME_MESH},
{"true", mjSHELL_MESH},
};
//---------------------------------- class mjXReader implementation --------------------------------
@@ -793,6 +800,9 @@ void mjXReader::Compiler(XMLElement* section, mjCModel* mod) {
}
MapValue(section, "inertiafromgeom", &mod->inertiafromgeom, TFAuto_map, 3);
ReadAttr(section, "inertiagrouprange", 2, mod->inertiagrouprange, text);
if (MapValue(section, "exactmeshinertia", &n, bool_map, 2)){
mod->exactmeshinertia = (n==1);
}
// lengthrange subelement
XMLElement* elem = FindSubElem(section, "lengthrange");
@@ -1149,6 +1159,11 @@ void mjXReader::OneGeom(XMLElement* elem, mjCGeom* pgeom) {
ReadAttr(elem, "quat", 4, pgeom->quat, text);
ReadAlternative(elem, pgeom->alt);
// compute inertia using either solid or shell geometry
if (MapValue(elem, "shellinertia", &n, meshtype_map, 2)) {
pgeom->typeinertia = (mjtMeshType)n;
}
GetXMLPos(elem, pgeom);
}
src/xml/xml_native_writer.cc
+6 -3
View File
@@ -253,13 +253,14 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
mjCMesh* pmesh = model->meshes[pgeom->meshid];
// write pos/quat if there is a difference
if (!SameVector(pgeom->locpos, pmesh->pos, 3) ||
!SameVector(pgeom->locquat, pmesh->quat, 4)) {
if (!SameVector(pgeom->locpos, pmesh->GetPosPtr(pgeom->typeinertia), 3) ||
!SameVector(pgeom->locquat, pmesh->GetQuatPtr(pgeom->typeinertia), 4)) {
// recover geom pos/quat before mesh frame transformation
double p[3], q[4];
mjuu_copyvec(p, pgeom->locpos, 3);
mjuu_copyvec(q, pgeom->locquat, 4);
mjuu_frameaccuminv(p, q, pmesh->pos, pmesh->quat);
mjuu_frameaccuminv(p, q, pmesh->GetPosPtr(pgeom->typeinertia),
pmesh->GetQuatPtr(pgeom->typeinertia));
// write
WriteAttr(elem, "pos", 3, p, unitq+1);
@@ -292,6 +293,7 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
WriteAttr(elem, "gap", 1, &pgeom->gap, &def->geom.gap);
WriteAttrKey(elem, "fluidshape", fluid_map, 2, pgeom->fluid_switch, def->geom.fluid_switch);
WriteAttr(elem, "fluidcoef", 5, pgeom->fluid_coefs, def->geom.fluid_coefs);
WriteAttrKey(elem, "shellinertia", meshtype_map, 2, pgeom->typeinertia, def->geom.typeinertia);
if (mjuu_defined(pgeom->_mass)) {
double mass = pgeom->GetVolume() * def->geom.density;
WriteAttr(elem, "mass", 1, &pgeom->mass, &mass);
@@ -657,6 +659,7 @@ void mjXWriter::Compiler(XMLElement* root) {
if (!model->usethread) {
WriteAttrTxt(section, "usethread", "false");
}
WriteAttrTxt(section, "exactmeshinertia", FindValue(bool_map, 2, model->exactmeshinertia));
}
src/xml/xml_util.h
+1 -1
View File
@@ -46,7 +46,7 @@ class mjXError {
// max number of attribute fields in schema (plus 3)
#define mjXATTRNUM 35
#define mjXATTRNUM 36
// Custom XML file validation