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

Changes to inertia inference from meshes.

Browse Source 
PiperOrigin-RevId: 726051033
Change-Id: I6edfc118280d103a2dd9d07f29f0094858769761
This commit is contained in:
Tom Power
2025-02-12 07:42:15 -08:00
committed by Copybara-Service
parent e39a5df06a
commit 89253d957d
21 changed files with 438 additions and 299 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/xml/xml_native_reader.cc
+6 -5
View File
@@ -781,10 +781,11 @@ const mjMap meshtype_map[2] = {
// mesh inertia type
const mjMap meshinertia_map[3] = {
{"convex", mjINERTIA_CONVEX},
{"legacy", mjINERTIA_LEGACY},
{"exact", mjINERTIA_EXACT}
const mjMap meshinertia_map[4] = {
{"convex", mjMESH_INERTIA_CONVEX},
{"legacy", mjMESH_INERTIA_LEGACY},
{"exact", mjMESH_INERTIA_EXACT},
{"shell", mjMESH_INERTIA_SHELL}
};
@@ -1432,7 +1433,7 @@ void mjXReader::OneMesh(XMLElement* elem, mjsMesh* mesh, const mjVFS* vfs) {
ReadAttr(elem, "refpos", 3, mesh->refpos, text);
ReadAttr(elem, "refquat", 4, mesh->refquat, text);
ReadAttr(elem, "scale", 3, mesh->scale, text);
if (MapValue(elem, "inertia", &n, meshinertia_map, 3)) {
if (MapValue(elem, "inertia", &n, meshinertia_map, 4)) {
mesh->inertia = (mjtMeshInertia)n;
}
src/xml/xml_native_writer.cc
+9 -6
View File
@@ -217,7 +217,7 @@ void mjXWriter::OneMesh(XMLElement* elem, const mjCMesh* mesh, mjCDef* def) {
WriteAttrTxt(elem, "content_type", mesh->ContentType());
WriteAttrTxt(elem, "file", mesh->File());
if (mesh->Inertia() != def->Mesh().Inertia()) {
WriteAttrTxt(elem, "inertia", FindValue(meshinertia_map, 3, mesh->Inertia()));
WriteAttrTxt(elem, "inertia", FindValue(meshinertia_map, 4, mesh->Inertia()));
}
// write vertex data
@@ -422,14 +422,14 @@ void mjXWriter::OneGeom(XMLElement* elem, const mjCGeom* geom, mjCDef* def, stri
mjCMesh* mesh = geom->mesh;
// write pos/quat if there is a difference
if (!SameVector(geom->pos, mesh->GetPosPtr(geom->typeinertia), 3) ||
!SameVector(geom->quat, mesh->GetQuatPtr(geom->typeinertia), 4)) {
if (!SameVector(geom->pos, mesh->GetPosPtr(), 3) ||
!SameVector(geom->quat, mesh->GetQuatPtr(), 4)) {
// recover geom pos/quat before mesh frame transformation
double p[3], q[4];
mjuu_copyvec(p, geom->pos, 3);
mjuu_copyvec(q, geom->quat, 4);
mjuu_frameaccuminv(p, q, mesh->GetPosPtr(geom->typeinertia),
mesh->GetQuatPtr(geom->typeinertia));
mjuu_frameaccuminv(p, q, mesh->GetPosPtr(),
mesh->GetQuatPtr());
// write
WriteAttr(elem, "pos", 3, p, unitq+1);
@@ -462,7 +462,10 @@ void mjXWriter::OneGeom(XMLElement* elem, const mjCGeom* geom, mjCDef* def, stri
WriteAttr(elem, "gap", 1, &geom->gap, &def->Geom().gap);
WriteAttrKey(elem, "fluidshape", fluid_map, 2, geom->fluid_ellipsoid, def->Geom().fluid_ellipsoid);
WriteAttr(elem, "fluidcoef", 5, geom->fluid_coefs, def->Geom().fluid_coefs);
WriteAttrKey(elem, "shellinertia", meshtype_map, 2, geom->typeinertia, def->Geom().typeinertia);
if (geom->type != mjGEOM_MESH) {
WriteAttrKey(elem, "shellinertia", meshtype_map, 2, geom->typeinertia,
def->Geom().typeinertia);
}
if (mjuu_defined(geom->mass)) {
WriteAttr(elem, "mass", 1, &geom->mass_, &mass);
} else {