diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 96721ad4..b6f240ec 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -2451,14 +2451,15 @@ helps clarify the role of bodies and geoms in MuJoCo. given mass, using the geom shape and the assumption of uniform density. The computed density is then used to obtain the geom inertia. Recall that the geom mass and inertia are only used during compilation, to infer the body mass and inertia if necessary. At runtime only the body inertial properties affect the simulation; the geom mass and inertia - are not even saved in mjModel. + are not saved in mjModel. .. _body-geom-density: :at:`density`: :at-val:`real, "1000"` Material density used to compute the geom mass and inertia. The computation is based on the geom shape and the assumption of uniform density. The internal default of 1000 is the density of water in SI units. This attribute is - used only when the mass attribute above is unspecified. + used only when the mass attribute above is unspecified. If `shellinertia` is "false" (the default), density has + semantics of mass/volume; if "true", it has semantics of mass/area. .. _body-geom-shellinertia: diff --git a/doc/changelog.rst b/doc/changelog.rst index b9484b53..b49bf217 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,6 +2,13 @@ Changelog ========= +Upcoming version (not yet released) +----------------------------------- + +General +^^^^^^^ +- :ref:`shellinertia ` is now supported by all geom types. + Version 3.2.2 (Aug 8, 2024) --------------------------- @@ -34,7 +41,8 @@ MJX Python bindings ^^^^^^^^^^^^^^^ 10. Added support for asset dictionary argument in ``mujoco.spec.from_file``, ``mujoco.spec.from_string`` and - ``mujoco.spec.compile``. + ``mujoco.spec.compile``. + Bug fixes ^^^^^^^^^ diff --git a/doc/overview.rst b/doc/overview.rst index 6e62d31c..f9c0797b 100644 --- a/doc/overview.rst +++ b/doc/overview.rst @@ -851,10 +851,14 @@ Geoms (short for geometric primitive) are used to specify appearance and collisi and is rigidly attached to that body. Multiple geoms can be attached to the same body. This is particularly useful in light of the fact that MuJoCo's collision detector assumes that all geoms are convex (it internally replaces meshes with their convex hulls if the meshes are not convex). Thus if you want to model a non-convex shape, you have to decompose it -into a union of convex geoms and attach all of them to the same body. Geoms can also have mass and inertia in the XML -model (or rather material density which is used to compute the mass and inertia), but that is only used to compute the -body mass and inertia in the model compiler. In the actual ``mjModel`` being simulated geoms do not have inertial -properties. +into a union of convex geoms and attach all of them to the same body. + +A geom can also have density or mass values specified in the XML, which the model compiler uses to compute the parent +body's mass and inertia. Mass is either specified or computed from a geom's volume and :ref:`density +`. Inertia is computed from the mass, shape, and uniform density assumption. If the +:ref:`shellinertia ` flag is set, mass is assumed to be uniformly distributed on the **surface**, +:at:`density` is interpreted as mass-per-area, and the inertia contribution to the parent body is computed accordingly. +In the actual ``mjModel`` being simulated, geoms do not have inertial properties. Sites are light geoms. They have the same appearance properties but cannot participate in collisions and cannot be used to infer body masses. On the other hand sites can do things that geoms cannot do: they can specify the volumes of touch diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 91322954..667a766d 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -38,6 +38,7 @@ #include #include #include +#include #include "cc/array_safety.h" #include "engine/engine_passive.h" #include @@ -2057,10 +2058,8 @@ void mjCGeom::NameSpace(const mjCModel* m) { -// compute geom volume +// compute geom volume / surface area double mjCGeom::GetVolume() const { - double height; - // get from mesh if (type==mjGEOM_MESH || type==mjGEOM_SDF) { if (mesh->id<0 || !((std::size_t) mesh->id <= model->Meshes().size())) { @@ -2070,30 +2069,74 @@ double mjCGeom::GetVolume() const { return mesh->GetVolumeRef(typeinertia); } - // compute from geom shape - else { - switch (type) { - case mjGEOM_SPHERE: - return 4*mjPI*size[0]*size[0]*size[0]/3; - - case mjGEOM_CAPSULE: - height = 2*size[1]; - return mjPI*(size[0]*size[0]*height + 4*size[0]*size[0]*size[0]/3); - - case mjGEOM_CYLINDER: - height = 2*size[1]; - return mjPI*size[0]*size[0]*height; - - case mjGEOM_ELLIPSOID: - return 4*mjPI*size[0]*size[1]*size[2]/3; - + // compute from geom shape (type) and inertia (typeinertia) + switch (type) { + case mjGEOM_SPHERE: { + switch (typeinertia) { + case mjINERTIA_SHELL: { + return 4 * mjPI * size[0] * size[0]; + } + case mjINERTIA_VOLUME: { + return 4 * mjPI * size[0] * size[0] * size[0] / 3; + } + } + } + case mjGEOM_CAPSULE: { + switch (typeinertia) { + case mjINERTIA_SHELL: { + double radius = size[0]; + double height = 2 * size[1]; + return 4 * mjPI * radius * radius + 2 * mjPI * radius * height; + } + case mjINERTIA_VOLUME: { + double height = 2 * size[1]; + return mjPI * (size[0] * size[0] * height + 4 * size[0] * size[0] * size[0] / 3); + } + } + } + case mjGEOM_CYLINDER: { + switch (typeinertia) { + case mjINERTIA_SHELL: { + double radius = size[0]; + double height = 2 * size[1]; + return 2 * mjPI * radius * radius + 2 * mjPI * radius * height; + } + case mjINERTIA_VOLUME: { + double height = 2 * size[1]; + return mjPI * size[0] * size[0] * height; + } + } + } + case mjGEOM_ELLIPSOID: { + switch (typeinertia) { + case mjINERTIA_SHELL: { + // Thomsen approximation + // https://www.numericana.com/answer/ellipsoid.htm#thomsen + double p = 1.6075; + double tmp = mju_pow(size[0] * size[1], p) + + mju_pow(size[1] * size[2], p) + + mju_pow(size[2] * size[0], p); + return 4 * mjPI * mju_pow(tmp / 3, 1 / p); + } + case mjINERTIA_VOLUME: { + return 4 * mjPI * size[0] * size[1] * size[2] / 3; + } + } + } case mjGEOM_HFIELD: - case mjGEOM_BOX: - return size[0]*size[1]*size[2]*8; + case mjGEOM_BOX: { + switch (typeinertia) { + case mjINERTIA_SHELL: { + return 8 * (size[0] * size[1] + size[1] * size[2] + size[2] * size[0]); + } + case mjINERTIA_VOLUME: { + return size[0] * size[1] * size[2] * 8; + } + } + } default: return 0; - } } } @@ -2112,68 +2155,210 @@ void mjCGeom::SetBoundingVolume(mjCBoundingVolume* bv) const { // set geom diagonal inertia given density void mjCGeom::SetInertia(void) { - double height; - // get from mesh - if (type==mjGEOM_MESH || type==mjGEOM_SDF) { - if (mesh->id<0 || !((std::size_t) mesh->id <= model->Meshes().size())) { + if (type == mjGEOM_MESH || type == mjGEOM_SDF) { + if (mesh->id < 0 || !((std::size_t)mesh->id <= model->Meshes().size())) { throw mjCError(this, "invalid mesh id in mesh geom"); } double* boxsz = mesh->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; + 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; + + return; } - // 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; - return; - - case mjGEOM_CAPSULE: { - height = 2*size[1]; - double radius = size[0]; - double sphere_mass = mass_*4*radius/(4*radius + 3*height); // mass*(sphere_vol/total_vol) - double cylinder_mass = mass_ - sphere_mass; - // cylinder part - inertia[0] = inertia[1] = cylinder_mass*(3*radius*radius + height*height)/12; - inertia[2] = cylinder_mass*radius*radius/2; - // 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[2] += sphere_inertia; - return; + // compute from geom shape (type) and inertia (typeinertia) + switch (type) { + case mjGEOM_SPHERE: { + switch (typeinertia) { + case mjINERTIA_SHELL: { + inertia[0] = inertia[1] = inertia[2] = 2 * mass_ * size[0] * size[0] / 3; + return; + } + case mjINERTIA_VOLUME: { + inertia[0] = inertia[1] = inertia[2] = 2 * mass_ * size[0] * size[0] / 5; + return; + } + } } + case mjGEOM_CAPSULE: { + double halfheight = size[1]; + double height = 2 * size[1]; + double radius = size[0]; + switch (typeinertia) { + case mjINERTIA_SHELL: { + // surface area + double Asphere = 4 * mjPI * radius * radius; + double Acylinder = 2 * mjPI * radius * height; + double Atotal = Asphere + Acylinder; - case mjGEOM_CYLINDER: - height = 2*size[1]; - inertia[0] = inertia[1] = mass_*(3*size[0]*size[0]+height*height)/12; - inertia[2] = mass_*size[0]*size[0]/2; - return; + // mass + double sphere_mass = mass_ * Asphere / Atotal; // mass*(sphere_area/total_area) + double cylinder_mass = mass_ - sphere_mass; - case mjGEOM_ELLIPSOID: - inertia[0] = mass_*(size[1]*size[1]+size[2]*size[2])/5; - inertia[1] = mass_*(size[0]*size[0]+size[2]*size[2])/5; - inertia[2] = mass_*(size[0]*size[0]+size[1]*size[1])/5; - return; + // cylinder part + inertia[0] = inertia[1] = cylinder_mass * (6 * radius * radius + height * height) / 12; + inertia[2] = cylinder_mass * radius * radius; + // add two hemispheres, displace along third axis + double sphere_inertia = 2 * sphere_mass * radius * radius / 3; + double hs_com = radius / 2; // hemisphere center of mass + double hs_pos = halfheight + hs_com; // hemisphere position + inertia[0] += sphere_inertia + sphere_mass * (hs_pos * hs_pos - hs_com * hs_com); + inertia[1] += sphere_inertia + sphere_mass * (hs_pos * hs_pos - hs_com * hs_com); + inertia[2] += sphere_inertia; + return; + } + case mjINERTIA_VOLUME: { + double sphere_mass = + mass_ * 4 * radius / (4 * radius + 3 * height); // mass*(sphere_vol/total_vol) + double cylinder_mass = mass_ - sphere_mass; + + // cylinder part + inertia[0] = inertia[1] = cylinder_mass * (3 * radius * radius + height * height) / 12; + inertia[2] = cylinder_mass * radius * radius / 2; + + // 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[2] += sphere_inertia; + return; + } + } + } + case mjGEOM_CYLINDER: { + double halfheight = size[1]; + double height = 2 * halfheight; + double radius = size[0]; + switch (typeinertia) { + case mjINERTIA_SHELL: { + // surface area + double Adisk = mjPI * radius * radius; + double Acylinder = 2 * mjPI * radius * height; + double Atotal = 2 * Adisk + Acylinder; + + // mass + double mass_disk = mass_ * Adisk / Atotal; + double mass_cylinder = mass_ - 2 * mass_disk; + + // cylinder contribution + inertia[0] = inertia[1] = mass_cylinder * (6 * radius * radius + height * height) / 12; + inertia[2] = mass_cylinder * radius * radius; + + // disk inertia + double inertia_disk_x = mass_disk * radius * radius / 4 + + mass_disk * halfheight * halfheight; + double inertia_disk_z = mass_disk * radius * radius / 2; + + // top and bottom disk contributions + inertia[0] += 2 * inertia_disk_x; + inertia[1] += 2 * inertia_disk_x; + inertia[2] += 2 * inertia_disk_z; + return; + } + case mjINERTIA_VOLUME: { + inertia[0] = inertia[1] = mass_ * (3 * radius * radius + height * height) / 12; + inertia[2] = mass_ * radius * radius / 2; + return; + } + } + } + case mjGEOM_ELLIPSOID: { + switch (typeinertia) { + case mjINERTIA_SHELL: { + // approximate shell inertia by subtracting ellipsoid from expanded + // ellipsoid + double eps = 1e-6; + + // solid volume (a) + double Va = 4 * mjPI * size[0] * size[1] * size[2] / 3; + + // expanded volume (b) + double ae = size[0] + eps; + double be = size[1] + eps; + double ce = size[2] + eps; + double Vb = 4 * mjPI * ae * be * ce / 3; + + // density + double density = mass_ / (Vb - Va); + + // inertia + double mass_a = Va * density; + double inertia_a[3]; + inertia_a[0] = mass_a * (size[1] * size[1] + size[2] * size[2]) / 5; + inertia_a[1] = mass_a * (size[0] * size[0] + size[2] * size[2]) / 5; + inertia_a[2] = mass_a * (size[0] * size[0] + size[1] * size[1]) / 5; + + double mass_b = Vb * density; + double inertia_b[3]; + inertia_b[0] = mass_b * (be * be + ce * ce) / 5; + inertia_b[1] = mass_b * (ae * ae + ce * ce) / 5; + inertia_b[2] = mass_b * (ae * ae + be * be) / 5; + + // shell inertia + mju_sub3(inertia, inertia_b, inertia_a); + return; + } + case mjINERTIA_VOLUME: { + inertia[0] = mass_ * (size[1] * size[1] + size[2] * size[2]) / 5; + inertia[1] = mass_ * (size[0] * size[0] + size[2] * size[2]) / 5; + inertia[2] = mass_ * (size[0] * size[0] + size[1] * size[1]) / 5; + return; + } + } + } case mjGEOM_HFIELD: - case mjGEOM_BOX: - inertia[0] = mass_*(size[1]*size[1]+size[2]*size[2])/3; - inertia[1] = mass_*(size[0]*size[0]+size[2]*size[2])/3; - inertia[2] = mass_*(size[0]*size[0]+size[1]*size[1])/3; - return; + case mjGEOM_BOX: { + switch (typeinertia) { + case mjINERTIA_SHELL: { + // length + double lx = 2 * size[0]; // side 0 + double ly = 2 * size[1]; // side 1 + double lz = 2 * size[2]; // side 2 - default: - inertia[0] = inertia[1] = inertia[2] = 0; - return; + // surface area + double A0 = lx * ly; // side 0 + double A1 = ly * lz; // side 1 + double A2 = lz * lx; // side 2 + double Atotal = 2 * (A0 + A1 + A2); + + // side 0 + double mass0 = mass_ * A0 / Atotal; + double Ix0 = mass0 * ly * ly / 12; + double Iy0 = mass0 * lx * lx / 12; + double Iz0 = mass0 * (lx * lx + ly * ly) / 12; + + // side 1 + double mass1 = mass_ * A1 / Atotal; + double Ix1 = mass1 * (ly * ly + lz * lz) / 12; + double Iy1 = mass1 * lz * lz / 12; + double Iz1 = mass1 * ly * ly / 12; + + // side 3 + double mass2 = mass_ * A2 / Atotal; + double Ix2 = mass2 * lz * lz / 12; + double Iy2 = mass2 * (lx * lx + lz * lz) / 12; + double Iz2 = mass2 * lx * lx / 12; + + // total inertia + inertia[0] = 2 * (Ix0 + mass0 * size[2] * size[2] + Ix1 + Ix2 + mass2 * size[1] * size[1]); + inertia[1] = 2 * (Iy0 + mass0 * size[2] * size[2] + Iy1 + mass1 * size[0] * size[0] + Iy2); + inertia[2] = 2 * (Iz0 + Iz1 + mass1 * size[0] * size[0] + Iz2 + mass2 * size[1] * size[1]); + return; + } + case mjINERTIA_VOLUME: { + inertia[0] = mass_ * (size[1] * size[1] + size[2] * size[2]) / 3; + inertia[1] = mass_ * (size[0] * size[0] + size[2] * size[2]) / 3; + inertia[2] = mass_ * (size[0] * size[0] + size[1] * size[1]) / 3; + return; + } + } + default: + inertia[0] = inertia[1] = inertia[2] = 0; + return; } } } diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index f7c90528..a23d937e 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -32,6 +32,8 @@ namespace mujoco { namespace { +constexpr double kInertiaTol = 1e-6; + std::vector AsVector(const mjtNum* array, int n) { return std::vector(array, array + n); } @@ -700,6 +702,380 @@ TEST_F(MjCGeomTest, CapsuleInertiaX) { mj_deleteModel(model); } +TEST_F(MjCGeomTest, ShellInertiaSphere) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + // radius + mjtNum r = 1.5; + mjtNum r2 = r * r; + + // body 1: shell inertia + mjtNum mass1 = 4 * mjPI * r2 * 1000; // surface area * surface density + EXPECT_NEAR(m->body_mass[1], mass1, kInertiaTol); + mjtNum I1 = 2 * mass1 * r2 / 3; + EXPECT_NEAR(m->body_inertia[3], I1, kInertiaTol); + EXPECT_NEAR(m->body_inertia[4], I1, kInertiaTol); + EXPECT_NEAR(m->body_inertia[5], I1, kInertiaTol); + + // body 2: shell inertia, with specified mass + mjtNum I2 = 2 * m->body_mass[2] * r2 / 3; + EXPECT_NEAR(m->body_inertia[6], I2, kInertiaTol); + EXPECT_NEAR(m->body_inertia[7], I2, kInertiaTol); + EXPECT_NEAR(m->body_inertia[8], I2, kInertiaTol); + + mjtNum mass3 = m->body_mass[3]; + mjtNum mass4 = m->body_mass[4]; + EXPECT_FLOAT_EQ(mass4 - mass3, m->body_mass[2]); + + // compute approximate shell inertia by subtracting inertias of massive bodies + // with small radius difference + mjtNum* inertia3 = m->body_inertia + 9; + mjtNum* inertia4 = m->body_inertia + 12; + mjtNum shell_inertia[3]; + mju_sub3(shell_inertia, inertia4, inertia3); + EXPECT_NEAR(shell_inertia[0], m->body_inertia[6], kInertiaTol); + EXPECT_NEAR(shell_inertia[1], m->body_inertia[7], kInertiaTol); + EXPECT_NEAR(shell_inertia[2], m->body_inertia[8], kInertiaTol); + + mj_deleteModel(m); +} + +TEST_F(MjCGeomTest, ShellInertiaCapsule) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + // dimensions + mjtNum r = 0.1; + mjtNum r2 = r * r; + mjtNum hh = 0.25; + mjtNum h = 2 * hh; + mjtNum h2 = h * h; + + // hemisphere + mjtNum hs_com = r / 2; // height of hemisphere center of mass + mjtNum hs_pos = hh + hs_com; // distance from origin to hemisphere com + + // surface area + double Asphere = 4 * mjPI * r2; // sphere + double Acylinder = 2 * mjPI * r * h; // cylinder + double Atotal = Asphere + Acylinder; + + // body 1: shell inertia + mjtNum mass1 = Atotal * 1000; // surface area * surface density + EXPECT_NEAR(m->body_mass[1], mass1, kInertiaTol); + mjtNum mass1_sphere = mass1 * Asphere / Atotal; + mjtNum mass1_cylinder = mass1 - mass1_sphere; + double sphere1_inertia = 2 * mass1_sphere * r2 / 3; + mjtNum I1x = mass1_cylinder * (6 * r2 + h2) / 12 + sphere1_inertia + + mass1_sphere * (hs_pos * hs_pos - hs_com * hs_com); + mjtNum I1z = mass1_cylinder * r2 + sphere1_inertia; + EXPECT_NEAR(m->body_inertia[3], I1x, kInertiaTol); + EXPECT_NEAR(m->body_inertia[4], I1x, kInertiaTol); + EXPECT_NEAR(m->body_inertia[5], I1z, kInertiaTol); + + // body 2: shell inertia, with specified mass + mjtNum mass2 = 4.3982325; + EXPECT_NEAR(m->body_mass[2], mass2, kInertiaTol); + EXPECT_FLOAT_EQ(m->body_mass[4] - m->body_mass[3], m->body_mass[2]); + + mjtNum mass2_sphere = mass2 * Asphere / Atotal; + mjtNum mass2_cylinder = mass2 - mass2_sphere; + double sphere2_inertia = 2 * mass2_sphere * r2 / 3; + mjtNum I2x = mass2_cylinder * (6 * r2 + h2) / 12 + sphere2_inertia + + mass2_sphere * (hs_pos * hs_pos - hs_com * hs_com); + mjtNum I2z = mass2_cylinder * r2 + sphere2_inertia; + EXPECT_NEAR(m->body_inertia[6], I2x, kInertiaTol); + EXPECT_NEAR(m->body_inertia[7], I2x, kInertiaTol); + EXPECT_NEAR(m->body_inertia[8], I2z, kInertiaTol); + + // compute approximate shell inertia by subtracting inertias of massive bodies + // with small radius difference + mjtNum* inertia3 = m->body_inertia + 9; + mjtNum* inertia4 = m->body_inertia + 12; + mjtNum shell_inertia[3]; + mju_sub3(shell_inertia, inertia4, inertia3); + EXPECT_NEAR(shell_inertia[0], m->body_inertia[6], kInertiaTol); + EXPECT_NEAR(shell_inertia[1], m->body_inertia[7], kInertiaTol); + EXPECT_NEAR(shell_inertia[2], m->body_inertia[8], kInertiaTol); + + mj_deleteModel(m); +} + +TEST_F(MjCGeomTest, ShellInertiaCylinder) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + // dimensions + mjtNum r = 0.1; + mjtNum hh = 0.25; + mjtNum r2 = r * r; + mjtNum h = 2 * hh; + mjtNum h2 = h * h; + + // surface area + double Adisk = mjPI * r2; // disk + double Acylinder = 2 * mjPI * r * h; // cylinder + double Atotal = 2 * Adisk + Acylinder; + + // body 1: shell inertia + mjtNum mass1 = Atotal * 1000; // surface area * surface density + EXPECT_NEAR(m->body_mass[1], mass1, kInertiaTol); + mjtNum mass1_disk = mass1 * Adisk / Atotal; + mjtNum mass1_cylinder = mass1 - 2 * mass1_disk; + mjtNum I1x = mass1_cylinder * (6 * r2 + h2) / 12 + + 2 * (mass1_disk * r2 / 4 + mass1_disk * hh * hh); + mjtNum I1z = mass1_cylinder * r2 + mass1_disk * r2; + EXPECT_NEAR(m->body_inertia[3], I1x, kInertiaTol); + EXPECT_NEAR(m->body_inertia[4], I1x, kInertiaTol); + EXPECT_NEAR(m->body_inertia[5], I1z, kInertiaTol); + + // body 2: shell inertia, with specified mass + mjtNum mass2 = 3.7699139; + EXPECT_NEAR(m->body_mass[2], mass2, kInertiaTol); + EXPECT_FLOAT_EQ(m->body_mass[4] - m->body_mass[3], m->body_mass[2]); + + mjtNum mass2_disk = mass2 * Adisk / Atotal; + mjtNum mass2_cylinder = mass2 - 2 * mass2_disk; + mjtNum I2x = mass2_cylinder * (6 * r2 + h2) / 12 + + 2 * (mass2_disk * r2 / 4 + mass2_disk * hh * hh); + mjtNum I2z = mass2_cylinder * r2 + mass2_disk * r2; + EXPECT_NEAR(m->body_inertia[6], I2x, kInertiaTol); + EXPECT_NEAR(m->body_inertia[7], I2x, kInertiaTol); + EXPECT_NEAR(m->body_inertia[8], I2z, kInertiaTol); + + // compute approximate shell inertia by subtracting inertias of massive bodies + // with small radius difference + mjtNum* inertia3 = m->body_inertia + 9; + mjtNum* inertia4 = m->body_inertia + 12; + mjtNum shell_inertia[3]; + mju_sub3(shell_inertia, inertia4, inertia3); + EXPECT_NEAR(shell_inertia[0], m->body_inertia[6], kInertiaTol); + EXPECT_NEAR(shell_inertia[1], m->body_inertia[7], kInertiaTol); + EXPECT_NEAR(shell_inertia[2], m->body_inertia[8], kInertiaTol); + + mj_deleteModel(m); +} + +TEST_F(MjCGeomTest, ShellInertiaEllipsoid) { + // test special case of ellipsoid with dimensions: a = b = c + // TODO(taylorhowell): add test for ellipsoid with dimensions: a != b != c + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + // dimensions + mjtNum r = 0.1; + mjtNum r2 = r * r; + + // body 1: shell inertia + mjtNum mass1 = 4 * mjPI * r2 * 1000; // surface area * surface density + EXPECT_NEAR(m->body_mass[1], mass1, kInertiaTol); + mjtNum I1 = 2 * mass1 * r2 / 3; + + // note: increased tolerance, this is due to ellipsoid approximation + EXPECT_NEAR(m->body_inertia[3], I1, 10 * kInertiaTol); + EXPECT_NEAR(m->body_inertia[4], I1, 10 * kInertiaTol); + EXPECT_NEAR(m->body_inertia[5], I1, 10 * kInertiaTol); + + // body 2: shell inertia, with specified mass + mjtNum mass2 = 0.12566371; + EXPECT_NEAR(m->body_mass[2], mass2, kInertiaTol); + EXPECT_FLOAT_EQ(m->body_mass[4] - m->body_mass[3], m->body_mass[2]); + + mjtNum I2 = 2 * m->body_mass[2] * r2 / 3; + EXPECT_NEAR(m->body_inertia[6], I2, 10 * kInertiaTol); + EXPECT_NEAR(m->body_inertia[7], I2, 10 * kInertiaTol); + EXPECT_NEAR(m->body_inertia[8], I2, 10 * kInertiaTol); + + // compute approximate shell inertia by subtracting inertias of massive bodies + // with small radius difference + mjtNum* inertia3 = m->body_inertia + 9; + mjtNum* inertia4 = m->body_inertia + 12; + mjtNum shell_inertia[3]; + mju_sub3(shell_inertia, inertia4, inertia3); + EXPECT_NEAR(shell_inertia[0], m->body_inertia[6], 10 * kInertiaTol); + EXPECT_NEAR(shell_inertia[1], m->body_inertia[7], 10 * kInertiaTol); + EXPECT_NEAR(shell_inertia[2], m->body_inertia[8], 10 * kInertiaTol); + + mj_deleteModel(m); +} + +TEST_F(MjCGeomTest, ShellInertiaBox) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + // dimensions + mjtNum dx = 0.1; + mjtNum dy = 0.2; + mjtNum dz = 0.3; + + // length + mjtNum lx = 2 * dx; + mjtNum ly = 2 * dy; + mjtNum lz = 2 * dz; + + // surface area + double A0 = lx * ly; + double A1 = ly * lz; + double A2 = lz * lx; + double Atotal = 2 * (A0 + A1 + A2); + + // body 1: shell inertia + mjtNum mass1 = Atotal * 1000; // surface area * surface density + EXPECT_NEAR(m->body_mass[1], mass1, kInertiaTol); + + mjtNum mass1_0 = mass1 * A0 / Atotal; + mjtNum mass1_1 = mass1 * A1 / Atotal; + mjtNum mass1_2 = mass1 * A2 / Atotal; + mjtNum I1x = 2 * (mass1_0 * ly * ly / 12 + mass1_0 * dz * dz + + mass1_1 * (ly * ly + lz * lz) / 12 + + mass1_2 * lz * lz / 12 + mass1_2 * dy * dy); + mjtNum I1y = + 2 * (mass1_0 * lx * lx / 12 + mass1_0 * dz * dz + mass1_1 * lz * lz / 12 + + mass1_1 * dx * dx + mass1_2 * (lx * lx + lz * lz) / 12); + mjtNum I1z = + 2 * (mass1_0 * (lx * lx + ly * ly) / 12 + mass1_1 * ly * ly / 12 + + mass1_1 * dx * dx + mass1_2 * lx * lx / 12 + mass1_2 * dy * dy); + + EXPECT_NEAR(m->body_inertia[3], I1x, kInertiaTol); + EXPECT_NEAR(m->body_inertia[4], I1y, kInertiaTol); + EXPECT_NEAR(m->body_inertia[5], I1z, kInertiaTol); + + // body 2: shell inertia, with specified mass + mjtNum mass2 = 8.800005; + EXPECT_NEAR(m->body_mass[2], mass2, 1e-6); + EXPECT_FLOAT_EQ(m->body_mass[4] - m->body_mass[3], m->body_mass[2]); + + mjtNum mass2_0 = mass2 * A0 / Atotal; + mjtNum mass2_1 = mass2 * A1 / Atotal; + mjtNum mass2_2 = mass2 * A2 / Atotal; + mjtNum I2x = 2 * (mass2_0 * ly * ly / 12 + mass2_0 * dz * dz + + mass2_1 * (ly * ly + lz * lz) / 12 + + mass2_2 * lz * lz / 12 + mass2_2 * dy * dy); + mjtNum I2y = + 2 * (mass2_0 * lx * lx / 12 + mass2_0 * dz * dz + mass2_1 * lz * lz / 12 + + mass2_1 * dx * dx + mass2_2 * (lx * lx + lz * lz) / 12); + mjtNum I2z = + 2 * (mass2_0 * (lx * lx + ly * ly) / 12 + mass2_1 * ly * ly / 12 + + mass2_1 * dx * dx + mass2_2 * lx * lx / 12 + mass2_2 * dy * dy); + + EXPECT_NEAR(m->body_inertia[6], I2x, kInertiaTol); + EXPECT_NEAR(m->body_inertia[7], I2y, kInertiaTol); + EXPECT_NEAR(m->body_inertia[8], I2z, kInertiaTol); + + // compute approximate shell inertia by subtracting inertias of massive bodies + // with small radius difference + mjtNum* inertia3 = m->body_inertia + 9; + mjtNum* inertia4 = m->body_inertia + 12; + mjtNum shell_inertia[3]; + mju_sub3(shell_inertia, inertia4, inertia3); + EXPECT_NEAR(shell_inertia[0], m->body_inertia[6], kInertiaTol); + EXPECT_NEAR(shell_inertia[1], m->body_inertia[7], kInertiaTol); + EXPECT_NEAR(shell_inertia[2], m->body_inertia[8], kInertiaTol); + + mj_deleteModel(m); +} + // ------------- test inertiagrouprange ---------------------------------------- TEST_F(MjCGeomTest, IgnoreGeomOutsideInertiagrouprange) {