Fix capsule moment of inertia

[gabrielebndn]

The formula for ix in the capsule was wrong, I fixed it.
Additionally, I grabbed the occasion to group some common terms in the other expressions too

---

This change is 

[sherm1]

@tehbelinda for review, please

Reviewable status: 0 of 1 files reviewed, all discussions resolved

[tehbelinda]

[tehbelinda]

Reviewed 1 of 1 files at r1.
Reviewable status: complete! all files reviewed, all discussions resolved

[tehbelinda]

---

include/fcl/geometry/shape/capsule-inl.h, line 94 at r1 (raw file):

  S v_sph = r2 * radius * constants<S>::pi() * 4 / 3.0;

  S ix = v_cyl * (l2 / 12. + r2 / 4.) + v_sph * (0.4 * r2 + 0.25 * l2 + 3. * radius * lz / 8.);

could you also add a comment with a link to your source for this?

[sherm1]

@gabrielebndn thanks -- this is an improvement since it was broken before. However, one might wonder how it could have been wrong for so long. IMO that is because there was no unit testing and that remains true with the fix you are proposing. We have been trying to upgrade FCL's software quality as we make changes to it. GTEST is easily usable and you can find a test case we added in fcl/test/geometry/shape/test_convex.cpp. Please consider adding a short test_capsule.cpp in that same directory that verifies correct capsule inertias for a few representative cases where you can compute the correct answers by hand.

Please note that the main benefit of unit tests is not to check the code you just wrote (which I trust you engineered carefully and Bel verified) but to make sure that the code continues to work correctly forever in the face of unknown future naive programmers who might think they can improve on your work. It also provides proof to future skeptics that your code is correct.

[gabrielebndn]

@tehbelinda, I could not find authoritative sources.
Verification was prompted, among other things, by the fact that I noticed the term 0.25 * v_cyl * radius was clearly incorrect, since dimensionally it did not make sense.
I followed the approach at https://www.gamedev.net/articles/programming/math-and-physics/capsule-inertia-tensor-r3856/ and I personally verified the calculations by hand. To complicate things, notice Eq. (14) contains a typo: the term H^2/2 is not consistent with Eq. (12) or with the code below.
Is it ok as a reference? Should I add this link as a comment to the code?

@sherm1, I fully understand the necessity of unit tests. I will try to provide one.

[tehbelinda]

thanks @gabrielebndn, that link would be great

[gabrielebndn]

I pushed a unit test.

1. Some tests are quite obvious. In particular, the CoM is just checking that it is zero and the volume check is almost a copy of the actual method
2. To compute the moment of inertia, I am basically performing the computation inside the unit test, assuming the formula for the inertia of a sphere, formula for the inertia of a cylinder, and the CoM of a hemisphere
3. To compare moments of inertia, I am using Eigen's isApprox instead of CompareMatrices: I was getting differences of a few units for large inertia values (order of 10^7). isApprox measures difference by percent, which is more important, I think. It's the method I've always used.
4. Something funny is going on with the floating point literals. If I write S(4./3.) or S(4.)/S(3.) instead of 4./3.at line 71 of the unit test, it does not pass. The fact is floating point literals are not converted to the correct scalar type inside the code. So this is what happens for instance in

   fcl/include/fcl/geometry/shape/capsule-inl.h

   Line 81 in 08d0bb2

   return constants<S>::pi() * radius * radius *(lz + radius * 4/3.0);

   1. The literals are interpreted as double
   2. float variables, since they are operating with double, are promoted to double
   3. Operations are carried out as double
   4. The final result is implicitly converted back to float
      If this behavior is unwanted then the whole code should be verified so that each floating point literal is explicitly converted to the correct scalar type.
5. I added a comment to computeMomentofInertia including the link I posted above

[sherm1]

This is great, @gabrielebndn -- thanks very much for doing such a professional job. This will serve as a good example for other people's contributions! I have a few comments below.

@tehbelinda, please take another look since there is a lot of new code.

Reviewed 3 of 3 files at r2.
Reviewable status: all files reviewed, 9 unresolved discussions (waiting on @gabrielebndn)

---

test/geometry/shape/test_capsule.cpp, line 35 at r2 (raw file):

 */

/** @author Sean Curtis ([email protected]) (2018) */

minor: not the right author!

---

test/geometry/shape/test_capsule.cpp, line 37 at r2 (raw file):

/** @author Sean Curtis ([email protected]) (2018) */

// Tests the implementation of a convex polytope geometry.

minor: copy pasta

---

test/geometry/shape/test_capsule.cpp, line 39 at r2 (raw file):

// Tests the implementation of a convex polytope geometry.

#include "fcl/geometry/shape/convex.h"

minor: the first include should be the class under test. So this should be ".../capsule.h". And you don't need convex.h at all.

---

test/geometry/shape/test_capsule.cpp, line 47 at r2 (raw file):

#include "eigen_matrix_compare.h"
#include "fcl/geometry/shape/capsule.h"

minor: please move this up to the top

---

test/geometry/shape/test_capsule.cpp, line 98 at r2 (raw file):

  S Ixc_hemi = Ix0_hemi - v_hemi * com_hemi * com_hemi; // inertia around CoM
  S dz = l / S(2.) + com_hemi;                          // CoM translation along z-axis
  S Ix_hemi = Ixc_hemi + v_hemi * dz * dz;              // translated inertia around x

minor: please keep line length to 80 characters.

---

test/geometry/shape/test_capsule.cpp, line 108 at r2 (raw file):

           S(0.), S(0.),    Iz;

  //EXPECT_TRUE(CompareMatrices(shape.computeMomentofInertia(), I_cap, tol));

nit: please remove this dead code

---

test/geometry/shape/test_capsule.cpp, line 122 at r2 (raw file):

    double lf = static_cast<float>(pair.second);
    Capsulef capsule_f(rf, lf);
    testVolumeComputation(capsule_f, 1e-9f);

If you can get this level of accuracy with float precision, you should be able to do much better in double. So the double test tolerance should be much tighter, maybe 1e-14 ish? BTW isn't 1e-9 a little extreme for float precision? -- I'm surprised it matches that closely. Consider using a small multiple of numeric_limits<S>::epsilon() instead to make sure the test is robust.

---

test/geometry/shape/test_capsule.cpp, line 147 at r2 (raw file):

    double ld = pair.second;
    Capsuled capsule_d(rd, ld);
    testMomentOfInertiaComputation(capsule_d, 1e-6);

That seems very loose for a double precision test.

[gabrielebndn]

test/geometry/shape/test_capsule.cpp, line 122 at r2 (raw file):

    double lf = static_cast<float>(pair.second);
    Capsulef capsule_f(rf, lf);
    testVolumeComputation(capsule_f, 1e-9f);

If you can get this level of accuracy with float precision, you should be able to do much better in double. So the double test tolerance should be much tighter, maybe 1e-14 ish? BTW isn't 1e-9 a little extreme for float precision? -- I'm surprised it matches that closely. Consider using a small multiple of numeric_limits<S>::epsilon() instead to make sure the test is robust.

For the test with double precision, I manage to get down to 1e-10.
The fact I am able to reach a good accuracy with float precision might be due to what I explain above: I think that, due to the fact floating point literals are not cast to the templated scalar type, computations are actually performed in double precision.
I am unfamiliar with floats, as I always use doubles, so I have no idea of how much error to expect. Should I cast the literals so that all computations are carried out in float precision? This will likely reduce accuracy a lot. Is there any circumstance under which this change may be desirable?

test/geometry/shape/test_capsule.cpp, line 147 at r2 (raw file):

    double ld = pair.second;
    Capsuled capsule_d(rd, ld);
    testMomentOfInertiaComputation(capsule_d, 1e-6);

That seems very loose for a double precision test.

Indeed, I manage to bring this to 1e-14. I remind that this is the tolerance sent to isApprox.

[sherm1]

Looks good to me with one requested change, please take a look (PTAL).

Reviewed 1 of 1 files at r3.
Reviewable status: all files reviewed, 2 unresolved discussions (waiting on @gabrielebndn)

---

test/geometry/shape/test_capsule.cpp, line 122 at r2 (raw file):

Previously, gabrielebndn wrote…

For the test with double precision, I manage to get down to 1e-10. The fact I am able to reach a good accuracy with float precision might be due to what I explain above: I think that, due to the fact floating point literals are not cast to the templated scalar type, computations are actually performed in double precision. I am unfamiliar with floats, as I always use doubles, so I have no idea of how much error to expect. Should I cast the literals so that all computations are carried out in float precision? This will likely reduce accuracy a lot. Is there any circumstance under which this change may be desirable?

I'm not worried about the float extra precision -- like you I only use double and using float rarely provides the performance improvements that programmers often think it does.

Howevever, the 1e-10 error in double is worrisome. That is a six orders-of-magnitude loss from the underlying precision of 2e-16 ish. For such a simple computation, that would only make sense if this is an absolute tolerance applied to a large-scale problem (like the actual volume is 10⁶ ish. If that's the case it would be better to change the testVolumeComputation() method to accept a relative tolerance rather than absolute so that it is clear that you are not suffering a dramatic precision loss somewhere.

---

test/geometry/shape/test_capsule.cpp, line 147 at r2 (raw file):

Previously, gabrielebndn wrote…

Indeed, I manage to bring this to 1e-14. I remind that this is the tolerance sent to isApprox.

Nice!

[jmirabel]

Howevever, the 1e-10 error in double is worrisome. That is a six orders-of-magnitude loss from the underlying precision of 2e-16 ish. For such a simple computation, that would only make sense if this is an absolute tolerance applied to a large-scale problem (like the actual volume is 10⁶ ish. If that's the case it would be better to change the testVolumeComputation() method to accept a relative tolerance rather than absolute so that it is clear that you are not suffering a dramatic precision loss somewhere.

You very easily reach 10^6 with a formula with has terms like radius^3 * length... The unit test has radius = 30 and length = 20.

[gabrielebndn]

Looks good to me with one requested change, please take a look (PTAL).

Reviewed 1 of 1 files at r3.
Reviewable status: all files reviewed, 2 unresolved discussions (waiting on @gabrielebndn)

test/geometry/shape/test_capsule.cpp, line 122 at r2 (raw file):

Previously, gabrielebndn wrote…

For the test with double precision, I manage to get down to 1e-10. The fact I am able to reach a good accuracy with float precision might be due to what I explain above: I think that, due to the fact floating point literals are not cast to the templated scalar type, computations are actually performed in double precision. I am unfamiliar with floats, as I always use doubles, so I have no idea of how much error to expect. Should I cast the literals so that all computations are carried out in float precision? This will likely reduce accuracy a lot. Is there any circumstance under which this change may be desirable?

I'm not worried about the float extra precision -- like you I only use double and using float rarely provides the performance improvements that programmers often think it does.

Howevever, the 1e-10 error in double is worrisome. That is a six orders-of-magnitude loss from the underlying precision of 2e-16 ish. For such a simple computation, that would only make sense if this is an absolute tolerance applied to a large-scale problem (like the actual volume is 10⁶ ish. If that's the case it would be better to change the testVolumeComputation() method to accept a relative tolerance rather than absolute so that it is clear that you are not suffering a dramatic precision loss somewhere.

As @jmirabel points out, the sizes are indeed quite big. The one test that does not pass 1e-11 involves a volume of about 71209 m^3, for all others I can get to 1e-13 (after this threshold there is one more test, 71.209 m^3, which barely exceeds 1e-14).

I set up the tests so that several different sizes are employed using test_convex as a blueprint, as I thought it wise to test several different values in order to avoid that the test passes only because of a coincidental choice of the parameters; to achieve this goal, I might as well have chosen numbers in the same order of magnitude. I did not think I would incur in such problems with the scaling.
However, I do think that relative error makes more sense when dealing with floating point values. Therefore, if you like this solution, I was happy to implement it and I did not change the test values

[sherm1]

Thanks -- one last request, ptal.

Reviewed 1 of 1 files at r4.
Reviewable status: all files reviewed, 2 unresolved discussions (waiting on @gabrielebndn)

---

test/geometry/shape/test_capsule.cpp, line 122 at r2 (raw file):

Previously, gabrielebndn wrote…

As @jmirabel points out, the sizes are indeed quite big. The one test that does not pass 1e-11 involves a volume of about 71209 m^3, for all others I can get to 1e-13 (after this threshold there is one more test, 71.209 m^3, which barely exceeds 1e-14).

I set up the tests so that several different sizes are employed using test_convex as a blueprint, as I thought it wise to test several different values in order to avoid that the test passes only because of a coincidental choice of the parameters; to achieve this goal, I might as well have chosen numbers in the same order of magnitude. I did not think I would incur in such problems with the scaling. However, I do think that relative error makes more sense when dealing with floating point values. Therefore, if you like this solution, I was happy to implement it and I did not change the test values

Great, thanks.

---

test/geometry/shape/test_capsule.cpp, line 121 at r4 (raw file):

    double lf = static_cast<float>(pair.second);
    Capsulef capsule_f(rf, lf);
    testVolumeComputation(capsule_f, 1e-15f);

minor: I would be more comfortable with something more like 1e-8f, which is all we can expect from float precision. The fact that these computations are being done secretly in double provides more accuracy, but I don't think we should insist on that in the test (it would be nice if the test could still pass if someone sets compiler flags properly to force computations to actually use float).

[sherm1]

Also, CI is failing on one of the Travis Mac builds:

18/37 Test #21: test_fcl_signed_distance ........................***Exception: SegFault  0.51 sec
[==========] Running 7 tests from 2 test cases.
[----------] Global test environment set-up.
[----------] 4 tests from FCL_NEGATIVE_DISTANCE
[ RUN      ] FCL_NEGATIVE_DISTANCE.sphere_sphere_ccd
[       OK ] FCL_NEGATIVE_DISTANCE.sphere_sphere_ccd (2 ms)
[ RUN      ] FCL_NEGATIVE_DISTANCE.sphere_sphere_indep
[       OK ] FCL_NEGATIVE_DISTANCE.sphere_sphere_indep (0 ms)
[ RUN      ] FCL_NEGATIVE_DISTANCE.sphere_capsule_ccd
[       OK ] FCL_NEGATIVE_DISTANCE.sphere_capsule_ccd (0 ms)
[ RUN      ] FCL_NEGATIVE_DISTANCE.sphere_capsule_indep
[       OK ] FCL_NEGATIVE_DISTANCE.sphere_capsule_indep (0 ms)
[----------] 4 tests from FCL_NEGATIVE_DISTANCE (2 ms total)
[----------] 3 tests from FCL_SIGNED_DISTANCE
[ RUN      ] FCL_SIGNED_DISTANCE.cylinder_sphere1_ccd
[       OK ] FCL_SIGNED_DISTANCE.cylinder_sphere1_ccd (0 ms)
[ RUN      ] FCL_SIGNED_DISTANCE.cylinder_box_ccd
[       OK ] FCL_SIGNED_DISTANCE.cylinder_box_ccd (1 ms)
[ RUN      ] FCL_SIGNED_DISTANCE.RealWorldRegression

It's hard to see how the change here could cause that, but I didn't see this failing earlier. Does anyone know if this is a flake? @SeanCurtis-TRI ?

Reviewable status: all files reviewed, 2 unresolved discussions (waiting on @gabrielebndn)

[gabrielebndn]

minor: I would be more comfortable with something more like 1e-8f, which is all we can expect from float precision. The fact that these computations are being done secretly in double provides more accuracy, but I don't think we should insist on that in the test (it would be nice if the test could still pass if someone sets compiler flags properly to force computations to actually use float).

Agreed, I was wondering which choice would be more accurate, I put 1e-8

[gabrielebndn]

Yes, I saw the failing test. I am inclined to think it is unrelated to my change, or at least not directly related.

That test, test_fcl_signed_distance, seems to only perform distance computations. I would be surprised if they involved the moments of inertia.
If I interpret the results correctly, the failing unit test is RealWorldRegression: this test does not seem to even involve capsules at all, only boxes.
If you look at the historical record of the CI builds, my first PR was passing. Then they are failing, but the only changes were to the unit test and a comment in the main code. It is always RealWorldRegression in test_fcl_signed_distance which is failing, but not only on Mac: Build 1392 fails on a Linux, 1393 on a Linux and on Mac, 1395 on Mac only.

Also, that test seems to pass on my machine.

My guess is that the error appears randomly because of something like uninitialized memory involved in that test: either in the routines, or in the unit test itself. I recommend verifying it with valgrind. I cannot do it myself because I am on my laptop right now, with an old OS not supporting it

[tehbelinda]

thanks for being thorough on this!

Reviewed 1 of 1 files at r5.
Reviewable status: all files reviewed, 2 unresolved discussions (waiting on @gabrielebndn)

---

test/geometry/shape/test_capsule.cpp, line 59 at r5 (raw file):

      paird(3.,2.),
      paird(30.,20.)
      };

minor: } is over indented

[gabrielebndn]

minor: } is over indented

Fixed :)

[tehbelinda]

Reviewed 1 of 1 files at r6.
Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @gabrielebndn)

[gabrielebndn]

Well, it seems Travis is passing right now. Somebody should really take a look into test_fcl_signed_distance, it seems to have problems. But as far as this PR is concerned, everything seems fine.

[sherm1]

I agree this is an unrelated flake :(
No need to hold this nice PR up any longer.

Thanks!

Reviewed 1 of 1 files at r6.
Reviewable status: complete! all files reviewed, all discussions resolved