Add shellinertia support for primitives: sphere, capsule, cylinder, ellipsoid, and box.
PiperOrigin-RevId: 661407997 Change-Id: Icddac58c540c71ea26ac7a3be874788443bd1122
This commit is contained in:
Taylor Howell
committed by
Copybara-Service
Copybara-Service
parent
ec43ec7c30
commit
466368efc6
@@ -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:
|
||||
|
||||
|
||||
+9
-1
@@ -2,6 +2,13 @@
|
||||
Changelog
|
||||
=========
|
||||
|
||||
Upcoming version (not yet released)
|
||||
-----------------------------------
|
||||
|
||||
General
|
||||
^^^^^^^
|
||||
- :ref:`shellinertia <body-geom-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
|
||||
^^^^^^^^^
|
||||
|
||||
+8
-4
@@ -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
|
||||
<body-geom-density>`. Inertia is computed from the mass, shape, and uniform density assumption. If the
|
||||
:ref:`shellinertia <body-geom-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
|
||||
|
||||
+257
-72
@@ -38,6 +38,7 @@
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mjplugin.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "cc/array_safety.h"
|
||||
#include "engine/engine_passive.h"
|
||||
#include <mujoco/mjspec.h>
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -32,6 +32,8 @@
|
||||
namespace mujoco {
|
||||
namespace {
|
||||
|
||||
constexpr double kInertiaTol = 1e-6;
|
||||
|
||||
std::vector<mjtNum> AsVector(const mjtNum* array, int n) {
|
||||
return std::vector<mjtNum>(array, array + n);
|
||||
}
|
||||
@@ -700,6 +702,380 @@ TEST_F(MjCGeomTest, CapsuleInertiaX) {
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
TEST_F(MjCGeomTest, ShellInertiaSphere) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<body>
|
||||
<geom type="sphere"size="1.5" shellinertia="true"/>
|
||||
</body>
|
||||
<body>
|
||||
<!-- mass is difference of body 4 and 3 masses -->
|
||||
<geom type="sphere" size="1.5" mass="28.274333953857422" shellinertia="true"/>
|
||||
</body>
|
||||
<body>
|
||||
<geom type="sphere" size="1.5" density="1e8"/>
|
||||
</body>
|
||||
<body>
|
||||
<geom type="sphere" size="1.50000001" density="1e8"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
std::array<char, 1000> 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"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<body>
|
||||
<geom type="capsule" size="0.1 0.25" shellinertia="true"/>
|
||||
</body>
|
||||
<!-- mass is difference of body 4 and 3 masses -->
|
||||
<body>
|
||||
<geom type="capsule" size="0.1 0.25" mass="4.3982325" shellinertia="true"/>
|
||||
</body>
|
||||
<body>
|
||||
<geom type="capsule" size="0.1 0.25" density="1e8"/>
|
||||
</body>
|
||||
<body>
|
||||
<geom type="capsule" size="0.1000001 0.25" density="1e8"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
std::array<char, 1000> 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"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<body>
|
||||
<geom type="cylinder" size="0.1 0.25" shellinertia="true"/>
|
||||
</body>
|
||||
<!-- mass is difference of body 4 and 3 masses -->
|
||||
<body>
|
||||
<geom type="cylinder" size="0.1 0.25" mass="3.7699139" shellinertia="true"/>
|
||||
</body>
|
||||
<body>
|
||||
<geom type="cylinder" size="0.1 0.25" density="1e8"/>
|
||||
</body>
|
||||
<body>
|
||||
<geom type="cylinder" size="0.1000001 0.2500001" density="1e8"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
std::array<char, 1000> 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"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<body>
|
||||
<geom type="ellipsoid" size="0.1 0.1 0.1" shellinertia="true"/>
|
||||
</body>
|
||||
<body>
|
||||
<!-- mass is difference of body 4 and 3 masses -->
|
||||
<geom type="ellipsoid" size="0.1 0.1 0.1" mass="0.12566371" shellinertia="true"/>
|
||||
</body>
|
||||
<body>
|
||||
<geom type="ellipsoid" size="0.1 0.1 0.1" density="1e8"/>
|
||||
</body>
|
||||
<body>
|
||||
<geom type="ellipsoid" size="0.10000001 0.10000001 0.10000001" density="1e8"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
std::array<char, 1000> 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"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<body>
|
||||
<geom type="box" size="0.1 0.2 0.3" shellinertia="true"/>
|
||||
</body>
|
||||
<!-- mass is difference of body 4 and 3 masses -->
|
||||
<body>
|
||||
<geom type="box" size="0.1 0.2 0.3" mass="8.800005" shellinertia="true"/>
|
||||
</body>
|
||||
<body>
|
||||
<geom type="box" size="0.1 0.2 0.3" density="1e8"/>
|
||||
</body>
|
||||
<body>
|
||||
<geom type="box" size="0.1000001 0.2000001 0.3000001" density="1e8"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
std::array<char, 1000> 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) {
|
||||
|
||||
Reference in New Issue
Block a user