From 6b23932fcf07ccdb227c3b3f351498eb755400e1 Mon Sep 17 00:00:00 2001
From: Gabriele Buondonno <gbuondon@laas.fr>
Date: Fri, 15 Nov 2019 14:05:31 +0100
Subject: [PATCH] Fix capsule moment of inertia
---
include/hpp/fcl/shape/geometric_shapes.h | 4 +++-
test/python_unit/geometric_shapes.py | 19 ++++++++++---------
2 files changed, 13 insertions(+), 10 deletions(-)
diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h
index f08d4741..e0a9dfb3 100644
--- a/include/hpp/fcl/shape/geometric_shapes.h
+++ b/include/hpp/fcl/shape/geometric_shapes.h
@@ -178,7 +178,9 @@ public:
FCL_REAL v_cyl = radius * radius * (halfLength * 2) * boost::math::constants::pi<FCL_REAL>();
FCL_REAL v_sph = radius * radius * radius * boost::math::constants::pi<FCL_REAL>() * 4 / 3.0;
- FCL_REAL ix = v_cyl * halfLength * halfLength / 3. + 0.25 * v_cyl * radius + 0.4 * v_sph * radius * radius + v_sph * halfLength * halfLength;
+ FCL_REAL h2 = halfLength * halfLength;
+ FCL_REAL r2 = radius * radius;
+ FCL_REAL ix = v_cyl * (h2 / 3. + r2 / 4.) + v_sph * (0.4 * r2 + h2 + 0.75 * radius * halfLength);
FCL_REAL iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius;
return (Matrix3f() << ix, 0, 0,
diff --git a/test/python_unit/geometric_shapes.py b/test/python_unit/geometric_shapes.py
index cf96f327..09c0f42e 100644
--- a/test/python_unit/geometric_shapes.py
+++ b/test/python_unit/geometric_shapes.py
@@ -30,15 +30,16 @@ class TestGeometricShapes(TestCase):
Iz_sphere = 0.4 * V_sphere * capsule.radius * capsule.radius
Iz_ref = Iz_cylinder + Iz_sphere
Ix_cylinder = V_cylinder*(3 * capsule.radius**2 + 4 * capsule.halfLength**2)/12.
-# TODO: check this code
-# Ix_sphere = Iz_sphere + V_sphere*capsule.halfLength**2
-# Ix_ref = Ix_cylinder + Ix_sphere
-# print(Ix_cylinder)
-# print(Ix_sphere)
-# I0_ref = np.diag([Ix_ref,Ix_ref,Iz_ref])
-# self.assertApprox(I0.diagonal(), I0_ref.diagonal())
-# Ic = capsule.computeMomentofInertiaRelatedToCOM()
-# self.assertApprox(Ic, I0_ref)
+ V_hemi = 0.5 * V_sphere # volume of hemisphere
+ I0x_hemi = 0.5 * Iz_sphere # inertia of hemisphere w.r.t. origin
+ com_hemi = 3. * capsule.radius / 8. # CoM of hemisphere w.r.t. origin
+ Icx_hemi = I0x_hemi - V_hemi * com_hemi * com_hemi # inertia of hemisphere w.r.t. CoM
+ Ix_hemi = Icx_hemi + V_hemi * (capsule.halfLength + com_hemi)**2 # inertia of hemisphere w.r.t. tip of cylinder
+ Ix_ref = Ix_cylinder + 2*Ix_hemi # total inertia of capsule
+ I0_ref = np.diag([Ix_ref,Ix_ref,Iz_ref])
+ self.assertApprox(I0, I0_ref)
+ Ic = capsule.computeMomentofInertiaRelatedToCOM()
+ self.assertApprox(Ic, I0_ref)
def test_box1(self):
box = hppfcl.Box(np.matrix([1.,2.,3.]).T)
--
GitLab