Compute moment of inertia for concave and boundary meshes. Resolve #338.
- 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
committed by
Copybara-Service
Copybara-Service
parent
52d78f8d24
commit
5c5449bf82
Vendored
+83
@@ -0,0 +1,83 @@
|
||||
v -0.050000 0.050000 0.100000
|
||||
v -0.050000 0.050000 0.000000
|
||||
v -0.050000 -0.050000 0.000000
|
||||
v -0.050000 -0.050000 0.100000
|
||||
v -0.050000 -0.050000 0.100000
|
||||
v -0.050000 -0.050000 0.000000
|
||||
v 0.050000 -0.050000 0.000000
|
||||
v 0.050000 -0.050000 0.100000
|
||||
v 0.050000 -0.050000 0.100000
|
||||
v 0.050000 -0.050000 0.000000
|
||||
v 0.050000 0.050000 0.000000
|
||||
v 0.050000 0.050000 0.100000
|
||||
v 0.050000 0.050000 0.100000
|
||||
v 0.050000 0.050000 0.000000
|
||||
v -0.050000 0.050000 0.000000
|
||||
v -0.050000 0.050000 0.100000
|
||||
v 0.050000 0.050000 0.000000
|
||||
v 0.050000 -0.050000 0.000000
|
||||
v -0.050000 -0.050000 0.000000
|
||||
v -0.050000 0.050000 0.000000
|
||||
v -0.050000 0.050000 0.100000
|
||||
v -0.050000 -0.050000 0.100000
|
||||
v -0.040000 -0.040000 0.100000
|
||||
v 0.050000 -0.050000 0.100000
|
||||
v 0.040000 -0.040000 0.100000
|
||||
v 0.050000 0.050000 0.100000
|
||||
v 0.040000 0.040000 0.100000
|
||||
v -0.040000 0.040000 0.100000
|
||||
v 0.040000 -0.040000 0.100000
|
||||
v 0.040000 -0.040000 0.010000
|
||||
v -0.040000 -0.040000 0.010000
|
||||
v -0.040000 -0.040000 0.100000
|
||||
v 0.040000 0.040000 0.100000
|
||||
v 0.040000 0.040000 0.010000
|
||||
v 0.040000 -0.040000 0.010000
|
||||
v 0.040000 -0.040000 0.100000
|
||||
v -0.040000 0.040000 0.100000
|
||||
v -0.040000 0.040000 0.010000
|
||||
v 0.040000 0.040000 0.010000
|
||||
v 0.040000 0.040000 0.100000
|
||||
v -0.040000 -0.040000 0.100000
|
||||
v -0.040000 -0.040000 0.010000
|
||||
v -0.040000 0.040000 0.010000
|
||||
v -0.040000 0.040000 0.100000
|
||||
v -0.040000 0.040000 0.010000
|
||||
v -0.040000 -0.040000 0.010000
|
||||
v 0.040000 -0.040000 0.010000
|
||||
v 0.040000 0.040000 0.010000
|
||||
vn -1.0000 0.0000 0.0000
|
||||
vn 0.0000 -1.0000 0.0000
|
||||
vn 1.0000 0.0000 0.0000
|
||||
vn 0.0000 1.0000 0.0000
|
||||
vn 0.0000 0.0000 -1.0000
|
||||
vn 0.0000 0.0000 1.0000
|
||||
s 1
|
||||
f 1//1 2//1 3//1
|
||||
f 3//1 4//1 1//1
|
||||
f 5//2 6//2 7//2
|
||||
f 7//2 8//2 5//2
|
||||
f 9//3 10//3 11//3
|
||||
f 11//3 12//3 9//3
|
||||
f 13//4 14//4 15//4
|
||||
f 15//4 16//4 13//4
|
||||
f 17//5 18//5 19//5
|
||||
f 19//5 20//5 17//5
|
||||
f 21//6 22//6 23//6
|
||||
f 22//6 24//6 25//6
|
||||
f 24//6 26//6 27//6
|
||||
f 26//6 21//6 28//6
|
||||
f 25//6 23//6 22//6
|
||||
f 28//6 27//6 26//6
|
||||
f 27//6 25//6 24//6
|
||||
f 23//6 28//6 21//6
|
||||
f 29//4 30//4 31//4
|
||||
f 31//4 32//4 29//4
|
||||
f 33//1 34//1 35//1
|
||||
f 35//1 36//1 33//1
|
||||
f 37//2 38//2 39//2
|
||||
f 39//2 40//2 37//2
|
||||
f 41//3 42//3 43//3
|
||||
f 43//3 44//3 41//3
|
||||
f 45//6 46//6 47//6
|
||||
f 47//6 48//6 45//6
|
||||
Vendored
+22323
File diff suppressed because it is too large
Load Diff
Vendored
+14885
File diff suppressed because it is too large
Load Diff
+27
@@ -0,0 +1,27 @@
|
||||
<mujoco>
|
||||
<compiler exactmeshinertia="true"/>
|
||||
<asset>
|
||||
<mesh file="cube_cup.obj" scale="10 10 10"/>
|
||||
<mesh file="cube_cup_quad.obj" scale="10 10 10"/>
|
||||
<mesh file="cube_cup_hi.obj" scale="10 10 10"/>
|
||||
</asset>
|
||||
<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"/>
|
||||
</body>
|
||||
<body pos="0 0 1">
|
||||
<geom type="mesh" mesh="cube_cup_quad" density="1"/>
|
||||
</body>
|
||||
<body pos="1.5 0 1">
|
||||
<geom type="mesh" mesh="cube_cup_hi" density="1"/>
|
||||
</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"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
+23
@@ -0,0 +1,23 @@
|
||||
<mujoco>
|
||||
<asset>
|
||||
<mesh file="cube.obj"/>
|
||||
<mesh name="inline_cube"
|
||||
vertex="
|
||||
-.5 -.5 -.5
|
||||
.5 -.5 -.5
|
||||
.5 .5 -.5
|
||||
-.5 .5 -.5
|
||||
-.5 -.5 .5
|
||||
.5 -.5 .5
|
||||
.5 .5 .5
|
||||
-.5 .5 .5"/>
|
||||
</asset>
|
||||
<worldbody>
|
||||
<body>
|
||||
<geom type="mesh" mesh="cube" density="1"/>
|
||||
</body>
|
||||
<body>
|
||||
<geom type="mesh" mesh="inline_cube" density="1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
Vendored
+11
@@ -0,0 +1,11 @@
|
||||
<mujoco>
|
||||
<compiler exactmeshinertia="true"/>
|
||||
<asset>
|
||||
<mesh file="cube.obj" name="hollow_cube"/>
|
||||
</asset>
|
||||
<worldbody>
|
||||
<body>
|
||||
<geom type="mesh" mesh="hollow_cube" density="1" shellinertia="true"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -37,6 +37,12 @@ static const char* const kCubePath =
|
||||
"user/testdata/cube.xml";
|
||||
static const char* const kTorusPath =
|
||||
"user/testdata/torus.xml";
|
||||
static const char* const kConvexInertiaPath =
|
||||
"user/testdata/inertia_convex.xml";
|
||||
static const char* const kConcaveInertiaPath =
|
||||
"user/testdata/inertia_concave.xml";
|
||||
static const char* const kShellInertiaPath =
|
||||
"user/testdata/inertia_shell.xml";
|
||||
static const char* const kTorusQuadsPath =
|
||||
"user/testdata/torus_quads.xml";
|
||||
static const char* const kTexturedTorusPath =
|
||||
@@ -134,6 +140,8 @@ TEST_F(MujocoTest, TinyMeshLoads) {
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
// ------------- test inertia -------------------------------------------------
|
||||
|
||||
TEST_F(MujocoTest, SmallInertiaLoads) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
@@ -202,5 +210,64 @@ TEST_F(MujocoTest, FlippedFaceFails) {
|
||||
EXPECT_THAT(error.data(), HasSubstr("faces have inconsistent orientation"));
|
||||
}
|
||||
|
||||
const mjtNum max_abs_err = std::numeric_limits<float>::epsilon();
|
||||
|
||||
TEST_F(MujocoTest, ExactConcaveInertia) {
|
||||
const std::string xml_path = GetTestDataFilePath(kConcaveInertiaPath);
|
||||
std::array<char, 1024> error;
|
||||
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 m_concave_cube = m_cube - m_hole;
|
||||
mjtNum I_cube = m_cube/6.;
|
||||
// due to the asymmetric hole, the com position has changed
|
||||
// so we need to use https://en.wikipedia.org/wiki/Parallel_axis_theorem
|
||||
mjtNum d_cube = .5 - model->body_ipos[5];
|
||||
mjtNum d_hole = .55 - model->body_ipos[5];
|
||||
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(fabs(model->body_mass[1] - m_concave_cube), max_abs_err);
|
||||
EXPECT_LE(fabs(model->body_mass[2] - m_concave_cube), max_abs_err);
|
||||
EXPECT_LE(fabs(model->body_mass[3] - m_concave_cube), max_abs_err);
|
||||
EXPECT_LE(fabs(model->body_mass[4] - m_concave_cube), max_abs_err);
|
||||
for (int i=3; i<15; i+=3) {
|
||||
EXPECT_LE(fabs(model->body_inertia[i] - I1), max_abs_err);
|
||||
EXPECT_LE(fabs(model->body_inertia[i+1] - I2), max_abs_err);
|
||||
EXPECT_LE(fabs(model->body_inertia[i+2] - I2), max_abs_err);
|
||||
}
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
TEST_F(MujocoTest, ExactConvexInertia) {
|
||||
const std::string xml_path = GetTestDataFilePath(kConvexInertiaPath);
|
||||
std::array<char, 1024> error;
|
||||
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
||||
// 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(fabs(model->body_mass[1] - m_solid_cube), max_abs_err);
|
||||
EXPECT_LE(fabs(model->body_mass[2] - m_solid_cube), max_abs_err);
|
||||
for (int i=3; i<9; i++) {
|
||||
EXPECT_LE(fabs(model->body_inertia[i] - I_solid_cube), max_abs_err);
|
||||
}
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
TEST_F(MujocoTest, ExactShellInertia) {
|
||||
const std::string xml_path = GetTestDataFilePath(kShellInertiaPath);
|
||||
std::array<char, 1024> error;
|
||||
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
||||
// 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(fabs(model->body_mass[1] - m_hollow_cube), max_abs_err);
|
||||
EXPECT_LE(fabs(model->body_inertia[3] - I_hollow_cube), max_abs_err);
|
||||
EXPECT_LE(fabs(model->body_inertia[4] - I_hollow_cube), max_abs_err);
|
||||
EXPECT_LE(fabs(model->body_inertia[5] - I_hollow_cube), max_abs_err);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mujoco
|
||||
|
||||
@@ -294,6 +294,25 @@ TEST_F(UserDataTest, InvalidInertialOrientation) {
|
||||
EXPECT_THAT(error.data(), HasSubstr("multiple orientation specifiers for the same field"));
|
||||
}
|
||||
|
||||
TEST_F(UserDataTest, ReadShellParameter) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<asset>
|
||||
<mesh name="example_mesh"
|
||||
vertex="0 0 0 1 0 0 0 1 0 0 0 1"
|
||||
face="0 2 1 2 0 3" />
|
||||
</asset>
|
||||
<worldbody>
|
||||
<geom type="mesh" mesh="example_mesh" shellinertia="true"/>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
std::array<char, 1024> error;
|
||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
||||
ASSERT_THAT(model, NotNull());
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
TEST_F(UserDataTest, ReadsDamper) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
|
||||
Reference in New Issue
Block a user