Skip to content

Commit 14dc6fd

Browse files
quaglacopybara-github
quagla
authored and
copybara-github
committed
Do not use inertia during mesh compilation.
Fixes #2479. PiperOrigin-RevId: 735387473 Change-Id: Ib241f2a0efc100bbe2cec24e0399bfcc87268094
1 parent e7a7154 commit 14dc6fd

File tree

4 files changed

+65
-50
lines changed

4 files changed

+65
-50
lines changed

‎src/user/user_mesh.cc

+10-10
Original file line numberDiff line numberDiff line change
@@ -1517,8 +1517,6 @@ void mjCMesh::Process() {
15171517
// find centroid of faces
15181518
ComputeFaceCentroid(facecen);
15191519

1520-
double density = model->def_map[classname]->Geom().density;
1521-
15221520
// compute inertia and transform mesh. The mesh is transformed such that it is
15231521
// centered at the CoM and the axes are the principle axes of inertia
15241522
double CoM[3] = {0, 0, 0};
@@ -1565,20 +1563,22 @@ void mjCMesh::Process() {
15651563
}
15661564

15671565
// compute sizes of equivalent inertia box
1568-
double mass = GetVolumeRef() * density;
1566+
double volume = GetVolumeRef();
15691567
double* boxsz = GetInertiaBoxPtr();
1570-
boxsz[0] = sqrt(6*(eigval[1]+eigval[2]-eigval[0])/mass)/2;
1571-
boxsz[1] = sqrt(6*(eigval[0]+eigval[2]-eigval[1])/mass)/2;
1572-
boxsz[2] = sqrt(6*(eigval[0]+eigval[1]-eigval[2])/mass)/2;
1568+
boxsz[0] = sqrt(6*(eigval[1]+eigval[2]-eigval[0])/volume)/2;
1569+
boxsz[1] = sqrt(6*(eigval[0]+eigval[2]-eigval[1])/volume)/2;
1570+
boxsz[2] = sqrt(6*(eigval[0]+eigval[1]-eigval[2])/volume)/2;
15731571

15741572
// transform CoM to origin
15751573
Transform(CoM, quattmp);
15761574
}
15771575

1576+
1577+
1578+
// compute abstract (unitless) inertia
15781579
void mjCMesh::ComputeInertia(double inert[6], double CoM[3]) {
15791580
double nrm[3];
15801581
double cen[3];
1581-
double density = model->def_map[classname]->Geom().density;
15821582

15831583
// copy vertices to avoid modifying the original mesh
15841584
std::vector<double> vert_centered(vert_);
@@ -1613,11 +1613,11 @@ void mjCMesh::ComputeInertia(double inert[6], double CoM[3]) {
16131613
// apply formula, accumulate
16141614
GetVolumeRef() += vol;
16151615
for (int j=0; j<6; j++) {
1616-
P[j] += density*vol /
1616+
P[j] += vol /
16171617
(inertia == mjMESH_INERTIA_SHELL ? 12 : 20) * (
16181618
2*(D[k[j][0]] * D[k[j][1]] +
1619-
E[k[j][0]] * E[k[j][1]] +
1620-
F[k[j][0]] * F[k[j][1]]) +
1619+
E[k[j][0]] * E[k[j][1]] +
1620+
F[k[j][0]] * F[k[j][1]]) +
16211621
D[k[j][0]] * E[k[j][1]] + D[k[j][1]] * E[k[j][0]] +
16221622
D[k[j][0]] * F[k[j][1]] + D[k[j][1]] * F[k[j][0]] +
16231623
E[k[j][0]] * F[k[j][1]] + E[k[j][1]] * F[k[j][0]]);

‎test/user/testdata/inertia_compare.xml

+1-7
Original file line numberDiff line numberDiff line change
@@ -3,19 +3,13 @@
33
<mesh name="incomplete_convex" inertia="convex"
44
vertex="0 0 0 1 0 0 0 1 0 0 0 1"
55
face="2 0 3 0 1 3 1 2 3" />
6-
<mesh name="incomplete"
7-
vertex="0 0 0 1 0 0 0 1 0 0 0 1"
8-
face="2 0 3 0 1 3 1 2 3"/>
96
<mesh name="complete"
107
vertex="0 0 0 1 0 0 0 1 0 0 0 1"
118
face="2 0 3 0 1 3 1 2 3 1 0 2"/>
129
</asset>
1310
<worldbody>
1411
<body>
15-
<geom type="mesh" mesh="incomplete_convex" contype="0" conaffinity="0"/>
16-
</body>
17-
<body>
18-
<geom type="mesh" mesh="incomplete"/>
12+
<geom type="mesh" mesh="incomplete_convex"/>
1913
</body>
2014
<body>
2115
<geom type="mesh" mesh="complete"/>

‎test/user/testdata/inertia_concave.xml

+8-8
Original file line numberDiff line numberDiff line change
@@ -10,20 +10,20 @@
1010
<worldbody>
1111
<light directional="true" diffuse=".6 .6 .6" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1"/>
1212
<body pos="-1.5 0 1">
13-
<geom type="mesh" mesh="cube_cup" density="1"/>
13+
<geom type="mesh" mesh="cube_cup" density="2"/>
1414
</body>
1515
<body pos="0 0 1">
16-
<geom type="mesh" mesh="cube_cup_quad" density="1"/>
16+
<geom type="mesh" mesh="cube_cup_quad" density="2"/>
1717
</body>
1818
<body pos="1.5 0 1">
19-
<geom type="mesh" mesh="cube_cup_hi" density="1"/>
19+
<geom type="mesh" mesh="cube_cup_hi" density="2"/>
2020
</body>
2121
<body pos="3 0 1">
22-
<geom type="box" pos="0 0 0.05" size=".5 .5 .05" density="1"/>
23-
<geom type="box" pos="-.45 0 .55" size=".05 .5 .45" density="1"/>
24-
<geom type="box" pos=" .45 0 .55" size=".05 .5 .45" density="1"/>
25-
<geom type="box" pos="0 -.45 .55" size=".4 .05 .45" density="1"/>
26-
<geom type="box" pos="0 .45 .55" size=".4 .05 .45" density="1"/>
22+
<geom type="box" pos="0 0 0.05" size=".5 .5 .05" density="2"/>
23+
<geom type="box" pos="-.45 0 .55" size=".05 .5 .45" density="2"/>
24+
<geom type="box" pos=" .45 0 .55" size=".05 .5 .45" density="2"/>
25+
<geom type="box" pos="0 -.45 .55" size=".4 .05 .45" density="2"/>
26+
<geom type="box" pos="0 .45 .55" size=".4 .05 .45" density="2"/>
2727
</body>
2828
</worldbody>
2929
</mujoco>

‎test/user/user_mesh_test.cc

+46-25
Original file line numberDiff line numberDiff line change
@@ -74,6 +74,8 @@ using ::testing::HasSubstr;
7474
using ::testing::IsNull;
7575
using ::testing::NotNull;
7676

77+
static constexpr mjtNum kMaxAbsErr = std::numeric_limits<float>::epsilon();
78+
7779
// ------------- test invalid filenames ----------------------------------------
7880

7981
TEST_F(MjCMeshTest, UnknownMeshFormat) {
@@ -556,13 +558,10 @@ TEST_F(MjCMeshTest, MissingFaceAllowedConvexInertia) {
556558
char error[1024];
557559
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));
558560
ASSERT_THAT(model, NotNull()) << error;
559-
EXPECT_THAT(model->nmeshface, 10);
560-
EXPECT_NE(model->body_inertia[3], model->body_inertia[9]);
561-
EXPECT_NE(model->body_inertia[4], model->body_inertia[10]);
562-
EXPECT_NE(model->body_inertia[5], model->body_inertia[11]);
563-
EXPECT_NE(model->body_inertia[3], model->body_inertia[6]);
564-
EXPECT_NE(model->body_inertia[4], model->body_inertia[7]);
565-
EXPECT_NE(model->body_inertia[5], model->body_inertia[8]);
561+
EXPECT_THAT(model->nmeshface, 7);
562+
EXPECT_NEAR(model->body_inertia[3], model->body_inertia[6], kMaxAbsErr);
563+
EXPECT_NEAR(model->body_inertia[4], model->body_inertia[7], kMaxAbsErr);
564+
EXPECT_NEAR(model->body_inertia[5], model->body_inertia[8], kMaxAbsErr);
566565
mj_deleteModel(model);
567566
}
568567

@@ -860,18 +859,40 @@ TEST_F(MjCMeshTest, VolumeNegativeThrowsError) {
860859
}
861860
}
862861

863-
// ------------- test concave and shell inertia --------------------------------
862+
TEST_F(MjCMeshTest, MeshIgnoresDefaultDensity) {
863+
static constexpr char xml[] = R"(
864+
<mujoco>
865+
<default>
866+
<geom density="0" />
867+
</default>
868+
<asset>
869+
<mesh name="a" vertex="0 0 0 1 0 0 0 1 0 0 0 1" scale="10 10 10" />
870+
</asset>
871+
<worldbody/>
872+
</mujoco>)";
873+
char error[1024];
874+
mjSpec* spec = mj_parseXMLString(xml, 0, error, sizeof(error));
875+
EXPECT_THAT(spec, NotNull()) << error;
876+
mjModel* m1 = mj_compile(spec, nullptr);
877+
EXPECT_THAT(m1, NotNull());
878+
mj_deleteModel(m1);
879+
mjModel* m2 = mj_compile(spec, nullptr);
880+
EXPECT_THAT(m2, NotNull());
881+
mj_deleteModel(m2);
882+
mj_deleteSpec(spec);
883+
}
864884

865-
const mjtNum max_abs_err = std::numeric_limits<float>::epsilon();
885+
// ------------- test concave and shell inertia --------------------------------
866886

867887
TEST_F(MjCMeshTest, ExactConcaveInertia) {
868888
const std::string xml_path = GetTestDataFilePath(kConcaveInertiaPath);
869889
std::array<char, 1024> error;
870890
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
871891
// analytic computation of 1x1x1 cube with a .8x.8x.9 hole
872892
// see https://en.wikipedia.org/wiki/List_of_moments_of_inertia
873-
mjtNum m_hole = .9 * .8 * .8;
874-
mjtNum m_cube = 1.;
893+
mjtNum density = 2.;
894+
mjtNum m_hole = .9 * .8 * .8 * density;
895+
mjtNum m_cube = 1. * density;
875896
mjtNum m_concave_cube = m_cube - m_hole;
876897
mjtNum I_cube = m_cube/6.;
877898
// due to the asymmetric hole, the com position has changed
@@ -881,14 +902,14 @@ TEST_F(MjCMeshTest, ExactConcaveInertia) {
881902
mjtNum I1 = I_cube - m_hole*(.8*.8 + .8*.8)/12;
882903
mjtNum I2 = I_cube - m_hole*(.8*.8 + .9*.9)/12
883904
+ m_cube*d_cube*d_cube - m_hole*d_hole*d_hole;
884-
EXPECT_LE(mju_abs(model->body_mass[1] - m_concave_cube), max_abs_err);
885-
EXPECT_LE(mju_abs(model->body_mass[2] - m_concave_cube), max_abs_err);
886-
EXPECT_LE(mju_abs(model->body_mass[3] - m_concave_cube), max_abs_err);
887-
EXPECT_LE(mju_abs(model->body_mass[4] - m_concave_cube), max_abs_err);
905+
EXPECT_NEAR(model->body_mass[1], m_concave_cube, kMaxAbsErr);
906+
EXPECT_NEAR(model->body_mass[2], m_concave_cube, kMaxAbsErr);
907+
EXPECT_NEAR(model->body_mass[3], m_concave_cube, kMaxAbsErr);
908+
EXPECT_NEAR(model->body_mass[4], m_concave_cube, kMaxAbsErr);
888909
for (int i = 3; i < 15; i += 3) {
889-
EXPECT_LE(mju_abs(model->body_inertia[i] - I1), max_abs_err);
890-
EXPECT_LE(mju_abs(model->body_inertia[i+1] - I2), max_abs_err);
891-
EXPECT_LE(mju_abs(model->body_inertia[i+2] - I2), max_abs_err);
910+
EXPECT_NEAR(model->body_inertia[i], I1, kMaxAbsErr);
911+
EXPECT_NEAR(model->body_inertia[i+1], I2, kMaxAbsErr);
912+
EXPECT_NEAR(model->body_inertia[i+2], I2, kMaxAbsErr);
892913
}
893914
mj_deleteModel(model);
894915
}
@@ -900,10 +921,10 @@ TEST_F(MjCMeshTest, ExactConvexInertia) {
900921
// https://en.wikipedia.org/wiki/List_of_moments_of_inertia
901922
mjtNum m_solid_cube = 1.;
902923
mjtNum I_solid_cube = 1./6. * m_solid_cube;
903-
EXPECT_LE(mju_abs(model->body_mass[1] - m_solid_cube), max_abs_err);
904-
EXPECT_LE(mju_abs(model->body_mass[2] - m_solid_cube), max_abs_err);
924+
EXPECT_LE(mju_abs(model->body_mass[1] - m_solid_cube), kMaxAbsErr);
925+
EXPECT_LE(mju_abs(model->body_mass[2] - m_solid_cube), kMaxAbsErr);
905926
for (int i = 3; i < 9; i++) {
906-
EXPECT_LE(mju_abs(model->body_inertia[i] - I_solid_cube), max_abs_err);
927+
EXPECT_LE(mju_abs(model->body_inertia[i] - I_solid_cube), kMaxAbsErr);
907928
}
908929
mj_deleteModel(model);
909930
}
@@ -915,10 +936,10 @@ TEST_F(MjCMeshTest, ExactShellInertia) {
915936
// see https://en.wikipedia.org/wiki/List_of_moments_of_inertia
916937
mjtNum m_hollow_cube = 6.;
917938
mjtNum I_hollow_cube = 5./18. * m_hollow_cube;
918-
EXPECT_LE(mju_abs(model->body_mass[1] - m_hollow_cube), max_abs_err);
919-
EXPECT_LE(mju_abs(model->body_inertia[3] - I_hollow_cube), max_abs_err);
920-
EXPECT_LE(mju_abs(model->body_inertia[4] - I_hollow_cube), max_abs_err);
921-
EXPECT_LE(mju_abs(model->body_inertia[5] - I_hollow_cube), max_abs_err);
939+
EXPECT_LE(mju_abs(model->body_mass[1] - m_hollow_cube), kMaxAbsErr);
940+
EXPECT_LE(mju_abs(model->body_inertia[3] - I_hollow_cube), kMaxAbsErr);
941+
EXPECT_LE(mju_abs(model->body_inertia[4] - I_hollow_cube), kMaxAbsErr);
942+
EXPECT_LE(mju_abs(model->body_inertia[5] - I_hollow_cube), kMaxAbsErr);
922943
mj_deleteModel(model);
923944
}
924945

0 commit comments

Comments
 (0)