Add Capsule class with inertia calculation method

[scpeters]

This package has classes for several shapes (Box, Cylinder, Sphere) as well as methods in MassMatrix3 for computing inertial properties of these shapes assuming uniform density. This pull request adds a new Capsule shape class that is like a Cylinder with hemispheres capping each end. Methods for computing the inertial properties of a Capsule are added to MassMatrix3 and validated in unit tests by comparing to the formulae used by Open Dynamics Engine.

Update: I've removed the MassMatrix3 methods and removed the Quaternion parameter from the Capsule class.

[EricCousineau-TRI]

Looks good at a high-level. The API here seems to be highly redundant and coupled (e.g. shapes + offsets, inertia computations, etc.), but understandably it'd be hard to refactor any time soon.

Made some picky comments, but nothing blocking.

Is there also a high-level issue that this is aiming towards?
e.g. gazebosim/sdformat#176 gazebosim/sdformat#376

[EricCousineau-TRI]

It would be good to also state that the radius defines the circle when the capsule intersects with G's XY-plane.

[EricCousineau-TRI]

Also, the shapes defined in ign-math seem like they have a shared contract, but there doesn't seem to be any form of contract enforcement (e.g. dynamic inheritance, static inheritance / mixins with CRTP, etc.).

Am I missing something there?

[scpeters]

Is there also a high-level issue that this is aiming towards?
e.g. osrf/sdformat#176 osrf/sdformat#376

it's in support of gazebosim/sdformat#376, along with gazebosim/sdformat#389

[scpeters]

The API here seems to be highly redundant and coupled (e.g. shapes + offsets, inertia computations, etc.), but understandably it'd be hard to refactor any time soon.

It took me a little while to remember, but I remembered a bit of the original intent behind this design. We added the MassMatrix3 class (started in 2.3.0 and a bit more in 2.4.0) before adding the shape classes like Sphere and Cylinder (5.0.0). The MassMatrix3 class is intended to represent a scalar mass and a 3x3 moment of inertia matrix. If you diagonalize the inertia matrix to get the principal moments of inertia and the orientation of its principal axes, then you can use these values along with its mass to compute the dimensions and orientation of a box of uniform density that has equivalent inertial properties (see MassMatrix3::EquivalentBox from 2.4.0 for reference).

That EquivalentBox is used to visualize the moment of inertia in Gazebo. After adding EquivalentBox, we added its inverse SetFromBox as well as SetFromSphere and SetFromCylinderZ for good measure (we also use these in gazebo). The quaternion parameter was added to SetFromBox for symmetry with EquivalentBox and to SetFromCylinderZ to accommodate cylinders with different axis alignments.

When the shape functions were added in 5.0.0, they included a Quaternion parameter in the constructor, and in retrospect, I think they might be confusing things more than helping. I'll remove them from the Capsule constructors; they'll still be in the other shape classes, but we'll need a major version bump to get rid of those

I'll think further on whether we need MassMatrix3::SetFromCapsuleZ functions or not; if not I'll just put the math in the Capsule class

[scpeters]

I'll think further on whether we need MassMatrix3::SetFromCapsuleZ functions or not; if not I'll just put the math in the Capsule class

I thought about it, and we can use the Inertial class directly from Capsule, which we can't do from MassMatrix3, so I've done it in 0ac1488, and it's a bit more elegant

I've removed the MassMatrix3::SetFromCapsuleZ helper functions in d9c8d73

[chapulina]

Looks good to me, I just have a question about the hemispheres, but I'm trusting the test that the math checks out.

[scpeters]

Also, the shapes defined in ign-math seem like they have a shared contract, but there doesn't seem to be any form of contract enforcement (e.g. dynamic inheritance, static inheritance / mixins with CRTP, etc.).

Am I missing something there?

no I don't think you're missing anything. I think the shape objects have similar API's but it's not enforced

[EricCousineau-TRI]

Aye, sounds good. Minor concern is that the Capsule API may now deviates from others (given that it computes its own inertia, rather than kinda doing some psuedo static-double-dispatch things).

[azeey]

What do you think about not providing a default constructor? That way the only time we would have invalid Capsule objects would be if the input length and radius values are invalid.

[scpeters]

I like having the default constructor. If you want to create a shape without remembering the order of the arguments in the constructor:

Capsuled capsule;
capsule.SetLength(length);
capsule.SetRadius(radius);

if other people don't like it, we can remove it, but I'm inclined to keep it

[mxgrey]

If I put on my pedantry hat, the "right" way to do what @scpeters is suggesting would be to have something like a Capsule::Builder class that lets you fill in the information necessary to build a Capsule object, and then you could call build() on it (or something similar) and it constructs a capsule for you, perhaps throwing an exception if the parameters are invalid (e.g. radius <= 0, length <= 0).

Some would (fairly) argue that's overkill for this use case, but the more I've had to deal with writing production-quality code, the more I've been feeling it's worth the effort to make every assumption explicit inside the code itself.