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

Do not use inertia during mesh compilation.

Browse Source 
Fixes #2479.

PiperOrigin-RevId: 735387473
Change-Id: Ib241f2a0efc100bbe2cec24e0399bfcc87268094
This commit is contained in:
Alessio Quaglino
2025-03-10 08:34:38 -07:00
committed by Copybara-Service
parent e7a715497e
commit 14dc6fd33b
4 changed files with 65 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_mesh.cc
+10 -10
View File
@@ -1517,8 +1517,6 @@ void mjCMesh::Process() {
// find centroid of faces
ComputeFaceCentroid(facecen);
double density = model->def_map[classname]->Geom().density;
// compute inertia and transform mesh. The mesh is transformed such that it is
// centered at the CoM and the axes are the principle axes of inertia
double CoM[3] = {0, 0, 0};
@@ -1565,20 +1563,22 @@ void mjCMesh::Process() {
}
// compute sizes of equivalent inertia box
double mass = GetVolumeRef() * density;
double volume = GetVolumeRef();
double* boxsz = GetInertiaBoxPtr();
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;
boxsz[0] = sqrt(6*(eigval[1]+eigval[2]-eigval[0])/volume)/2;
boxsz[1] = sqrt(6*(eigval[0]+eigval[2]-eigval[1])/volume)/2;
boxsz[2] = sqrt(6*(eigval[0]+eigval[1]-eigval[2])/volume)/2;
// transform CoM to origin
Transform(CoM, quattmp);
}
// compute abstract (unitless) inertia
void mjCMesh::ComputeInertia(double inert[6], double CoM[3]) {
double nrm[3];
double cen[3];
double density = model->def_map[classname]->Geom().density;
// copy vertices to avoid modifying the original mesh
std::vector<double> vert_centered(vert_);
@@ -1613,11 +1613,11 @@ void mjCMesh::ComputeInertia(double inert[6], double CoM[3]) {
// apply formula, accumulate
GetVolumeRef() += vol;
for (int j=0; j<6; j++) {
P[j] += density*vol /
P[j] += vol /
(inertia == mjMESH_INERTIA_SHELL ? 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]]) +
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]]);
test/user/testdata/inertia_compare.xml
Vendored
+1 -7
View File
@@ -3,19 +3,13 @@
<mesh name="incomplete_convex" inertia="convex"
vertex="0 0 0 1 0 0 0 1 0 0 0 1"
face="2 0 3 0 1 3 1 2 3" />
<mesh name="incomplete"
vertex="0 0 0 1 0 0 0 1 0 0 0 1"
face="2 0 3 0 1 3 1 2 3"/>
<mesh name="complete"
vertex="0 0 0 1 0 0 0 1 0 0 0 1"
face="2 0 3 0 1 3 1 2 3 1 0 2"/>
</asset>
<worldbody>
<body>
<geom type="mesh" mesh="incomplete_convex" contype="0" conaffinity="0"/>
</body>
<body>
<geom type="mesh" mesh="incomplete"/>
<geom type="mesh" mesh="incomplete_convex"/>
</body>
<body>
<geom type="mesh" mesh="complete"/>
test/user/testdata/inertia_concave.xml
Vendored
+8 -8
View File
@@ -10,20 +10,20 @@
<worldbody>
<light directional="true" diffuse=".6 .6 .6" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1"/>
<body pos="-1.5 0 1">
<geom type="mesh" mesh="cube_cup" density="1"/>
<geom type="mesh" mesh="cube_cup" density="2"/>
</body>
<body pos="0 0 1">
<geom type="mesh" mesh="cube_cup_quad" density="1"/>
<geom type="mesh" mesh="cube_cup_quad" density="2"/>
</body>
<body pos="1.5 0 1">
<geom type="mesh" mesh="cube_cup_hi" density="1"/>
<geom type="mesh" mesh="cube_cup_hi" density="2"/>
</body>
<body pos="3 0 1">
<geom type="box" pos="0 0 0.05" size=".5 .5 .05" density="1"/>
<geom type="box" pos="-.45 0 .55" size=".05 .5 .45" density="1"/>
<geom type="box" pos=" .45 0 .55" size=".05 .5 .45" density="1"/>
<geom type="box" pos="0 -.45 .55" size=".4 .05 .45" density="1"/>
<geom type="box" pos="0 .45 .55" size=".4 .05 .45" density="1"/>
<geom type="box" pos="0 0 0.05" size=".5 .5 .05" density="2"/>
<geom type="box" pos="-.45 0 .55" size=".05 .5 .45" density="2"/>
<geom type="box" pos=" .45 0 .55" size=".05 .5 .45" density="2"/>
<geom type="box" pos="0 -.45 .55" size=".4 .05 .45" density="2"/>
<geom type="box" pos="0 .45 .55" size=".4 .05 .45" density="2"/>
</body>
</worldbody>
</mujoco>
test/user/user_mesh_test.cc
+46 -25
View File
@@ -74,6 +74,8 @@ using ::testing::HasSubstr;
using ::testing::IsNull;
using ::testing::NotNull;
static constexpr mjtNum kMaxAbsErr = std::numeric_limits<float>::epsilon();
// ------------- test invalid filenames ----------------------------------------
TEST_F(MjCMeshTest, UnknownMeshFormat) {
@@ -556,13 +558,10 @@ TEST_F(MjCMeshTest, MissingFaceAllowedConvexInertia) {
char error[1024];
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
EXPECT_THAT(model->nmeshface, 10);
EXPECT_NE(model->body_inertia[3], model->body_inertia[9]);
EXPECT_NE(model->body_inertia[4], model->body_inertia[10]);
EXPECT_NE(model->body_inertia[5], model->body_inertia[11]);
EXPECT_NE(model->body_inertia[3], model->body_inertia[6]);
EXPECT_NE(model->body_inertia[4], model->body_inertia[7]);
EXPECT_NE(model->body_inertia[5], model->body_inertia[8]);
EXPECT_THAT(model->nmeshface, 7);
EXPECT_NEAR(model->body_inertia[3], model->body_inertia[6], kMaxAbsErr);
EXPECT_NEAR(model->body_inertia[4], model->body_inertia[7], kMaxAbsErr);
EXPECT_NEAR(model->body_inertia[5], model->body_inertia[8], kMaxAbsErr);
mj_deleteModel(model);
}
@@ -860,9 +859,30 @@ TEST_F(MjCMeshTest, VolumeNegativeThrowsError) {
}
}
// ------------- test concave and shell inertia --------------------------------
TEST_F(MjCMeshTest, MeshIgnoresDefaultDensity) {
static constexpr char xml[] = R"(
<mujoco>
<default>
<geom density="0" />
</default>
<asset>
<mesh name="a" vertex="0 0 0 1 0 0 0 1 0 0 0 1" scale="10 10 10" />
</asset>
<worldbody/>
</mujoco>)";
char error[1024];
mjSpec* spec = mj_parseXMLString(xml, 0, error, sizeof(error));
EXPECT_THAT(spec, NotNull()) << error;
mjModel* m1 = mj_compile(spec, nullptr);
EXPECT_THAT(m1, NotNull());
mj_deleteModel(m1);
mjModel* m2 = mj_compile(spec, nullptr);
EXPECT_THAT(m2, NotNull());
mj_deleteModel(m2);
mj_deleteSpec(spec);
}
const mjtNum max_abs_err = std::numeric_limits<float>::epsilon();
// ------------- test concave and shell inertia --------------------------------
TEST_F(MjCMeshTest, ExactConcaveInertia) {
const std::string xml_path = GetTestDataFilePath(kConcaveInertiaPath);
@@ -870,8 +890,9 @@ TEST_F(MjCMeshTest, ExactConcaveInertia) {
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
// analytic computation of 1x1x1 cube with a .8x.8x.9 hole
// see https://en.wikipedia.org/wiki/List_of_moments_of_inertia
mjtNum m_hole = .9 * .8 * .8;
mjtNum m_cube = 1.;
mjtNum density = 2.;
mjtNum m_hole = .9 * .8 * .8 * density;
mjtNum m_cube = 1. * density;
mjtNum m_concave_cube = m_cube - m_hole;
mjtNum I_cube = m_cube/6.;
// due to the asymmetric hole, the com position has changed
@@ -881,14 +902,14 @@ TEST_F(MjCMeshTest, ExactConcaveInertia) {
mjtNum I1 = I_cube - m_hole*(.8*.8 + .8*.8)/12;
mjtNum I2 = I_cube - m_hole*(.8*.8 + .9*.9)/12
+ m_cube*d_cube*d_cube - m_hole*d_hole*d_hole;
EXPECT_LE(mju_abs(model->body_mass[1] - m_concave_cube), max_abs_err);
EXPECT_LE(mju_abs(model->body_mass[2] - m_concave_cube), max_abs_err);
EXPECT_LE(mju_abs(model->body_mass[3] - m_concave_cube), max_abs_err);
EXPECT_LE(mju_abs(model->body_mass[4] - m_concave_cube), max_abs_err);
EXPECT_NEAR(model->body_mass[1], m_concave_cube, kMaxAbsErr);
EXPECT_NEAR(model->body_mass[2], m_concave_cube, kMaxAbsErr);
EXPECT_NEAR(model->body_mass[3], m_concave_cube, kMaxAbsErr);
EXPECT_NEAR(model->body_mass[4], m_concave_cube, kMaxAbsErr);
for (int i = 3; i < 15; i += 3) {
EXPECT_LE(mju_abs(model->body_inertia[i] - I1), max_abs_err);
EXPECT_LE(mju_abs(model->body_inertia[i+1] - I2), max_abs_err);
EXPECT_LE(mju_abs(model->body_inertia[i+2] - I2), max_abs_err);
EXPECT_NEAR(model->body_inertia[i], I1, kMaxAbsErr);
EXPECT_NEAR(model->body_inertia[i+1], I2, kMaxAbsErr);
EXPECT_NEAR(model->body_inertia[i+2], I2, kMaxAbsErr);
}
mj_deleteModel(model);
}
@@ -900,10 +921,10 @@ TEST_F(MjCMeshTest, ExactConvexInertia) {
// https://en.wikipedia.org/wiki/List_of_moments_of_inertia
mjtNum m_solid_cube = 1.;
mjtNum I_solid_cube = 1./6. * m_solid_cube;
EXPECT_LE(mju_abs(model->body_mass[1] - m_solid_cube), max_abs_err);
EXPECT_LE(mju_abs(model->body_mass[2] - m_solid_cube), max_abs_err);
EXPECT_LE(mju_abs(model->body_mass[1] - m_solid_cube), kMaxAbsErr);
EXPECT_LE(mju_abs(model->body_mass[2] - m_solid_cube), kMaxAbsErr);
for (int i = 3; i < 9; i++) {
EXPECT_LE(mju_abs(model->body_inertia[i] - I_solid_cube), max_abs_err);
EXPECT_LE(mju_abs(model->body_inertia[i] - I_solid_cube), kMaxAbsErr);
}
mj_deleteModel(model);
}
@@ -915,10 +936,10 @@ TEST_F(MjCMeshTest, ExactShellInertia) {
// see https://en.wikipedia.org/wiki/List_of_moments_of_inertia
mjtNum m_hollow_cube = 6.;
mjtNum I_hollow_cube = 5./18. * m_hollow_cube;
EXPECT_LE(mju_abs(model->body_mass[1] - m_hollow_cube), max_abs_err);
EXPECT_LE(mju_abs(model->body_inertia[3] - I_hollow_cube), max_abs_err);
EXPECT_LE(mju_abs(model->body_inertia[4] - I_hollow_cube), max_abs_err);
EXPECT_LE(mju_abs(model->body_inertia[5] - I_hollow_cube), max_abs_err);
EXPECT_LE(mju_abs(model->body_mass[1] - m_hollow_cube), kMaxAbsErr);
EXPECT_LE(mju_abs(model->body_inertia[3] - I_hollow_cube), kMaxAbsErr);
EXPECT_LE(mju_abs(model->body_inertia[4] - I_hollow_cube), kMaxAbsErr);
EXPECT_LE(mju_abs(model->body_inertia[5] - I_hollow_cube), kMaxAbsErr);
mj_deleteModel(model);
}