diff --git a/.gitignore b/.gitignore
index 52306cf13e332aaa2f8e6c642d8616929c39b07c..f0ef57469b034edcd1dadabc57a7639ea5610519 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,2 +1,5 @@
build*
Xcode/*
+*~
+*.pyc
+
diff --git a/include/hpp/fcl/collision_object.h b/include/hpp/fcl/collision_object.h
index 1e743dbd08709af80e789ca23e63901c97c0bf45..31fc86d82fb3fc6b73409e413e67d1ccfe1cc15a 100644
--- a/include/hpp/fcl/collision_object.h
+++ b/include/hpp/fcl/collision_object.h
@@ -125,10 +125,10 @@ public:
FCL_REAL threshold_free;
/// @brief compute center of mass
- virtual Vec3f computeCOM() const { return Vec3f(); }
+ virtual Vec3f computeCOM() const { return Vec3f::Zero(); }
/// @brief compute the inertia matrix, related to the origin
- virtual Matrix3f computeMomentofInertia() const { return Matrix3f(); }
+ virtual Matrix3f computeMomentofInertia() const { return Matrix3f::Constant(NAN); }
/// @brief compute the volume
virtual FCL_REAL computeVolume() const { return 0; }
diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h
index d991f4e02f0e1d81813fad1bb50c5458e485c84e..e0a9dfb343227f9d7c34f368b6485cc327d767d1 100644
--- a/include/hpp/fcl/shape/geometric_shapes.h
+++ b/include/hpp/fcl/shape/geometric_shapes.h
@@ -143,7 +143,7 @@ public:
FCL_REAL computeVolume() const
{
- return 4 * boost::math::constants::pi<FCL_REAL>() * radius * radius / 3;
+ return 4 * boost::math::constants::pi<FCL_REAL>() * radius * radius * radius / 3;
}
};
@@ -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,
@@ -214,7 +216,6 @@ public:
return boost::math::constants::pi<FCL_REAL>() * radius * radius * (halfLength * 2) / 3;
}
- /// \todo verify this formula as it seems different from https://en.wikipedia.org/wiki/List_of_moments_of_inertia#List_of_3D_inertia_tensors
Matrix3f computeMomentofInertia() const
{
FCL_REAL V = computeVolume();
diff --git a/test/python_unit/geometric_shapes.py b/test/python_unit/geometric_shapes.py
index 6e9109e0f054535156ba27904ecd746accbe6a47..09c0f42ece98f88da37ffb2b842997d8774dce5c 100644
--- a/test/python_unit/geometric_shapes.py
+++ b/test/python_unit/geometric_shapes.py
@@ -1,9 +1,10 @@
import unittest
+from test_case import TestCase
import hppfcl
hppfcl.switchToNumpyMatrix()
import numpy as np
-class TestGeometricShapes(unittest.TestCase):
+class TestGeometricShapes(TestCase):
def test_capsule(self):
capsule = hppfcl.Capsule(1.,2.)
@@ -17,6 +18,28 @@ class TestGeometricShapes(unittest.TestCase):
capsule.halfLength = 4.
self.assertEqual(capsule.radius,3.)
self.assertEqual(capsule.halfLength,4.)
+ com = capsule.computeCOM()
+ self.assertApprox(com, np.zeros(3))
+ V = capsule.computeVolume()
+ V_cylinder = capsule.radius * capsule.radius * np.pi * 2. * capsule.halfLength
+ V_sphere = 4. * np.pi/3 * capsule.radius**3
+ V_ref = V_cylinder + V_sphere
+ self.assertApprox(V, V_ref)
+ I0 = capsule.computeMomentofInertia()
+ Iz_cylinder = V_cylinder * capsule.radius**2 / 2.
+ 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.
+ 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)
@@ -27,6 +50,22 @@ class TestGeometricShapes(unittest.TestCase):
self.assertTrue(np.array_equal(box.halfSide,np.matrix([.5,1.,1.5]).T))
box.halfSide = np.matrix([4.,5.,6.]).T
self.assertTrue(np.array_equal(box.halfSide,np.matrix([4.,5.,6.]).T))
+ com = box.computeCOM()
+ self.assertApprox(com, np.zeros(3))
+ V = box.computeVolume()
+ x = float(2*box.halfSide[0])
+ y = float(2*box.halfSide[1])
+ z = float(2*box.halfSide[2])
+ V_ref = x * y * z
+ self.assertApprox(V, V_ref)
+ I0 = box.computeMomentofInertia()
+ Ix = V_ref * (y*y + z*z) / 12.
+ Iy = V_ref * (x*x + z*z) / 12.
+ Iz = V_ref * (y*y + x*x) / 12.
+ I0_ref = np.diag([Ix,Iy,Iz])
+ self.assertApprox(I0, I0_ref)
+ Ic = box.computeMomentofInertiaRelatedToCOM()
+ self.assertApprox(Ic, I0_ref)
def test_box2(self):
box = hppfcl.Box(1.,2.,3)
@@ -37,22 +76,26 @@ class TestGeometricShapes(unittest.TestCase):
self.assertEqual(box.halfSide[0],0.5)
self.assertEqual(box.halfSide[1],1.0)
self.assertEqual(box.halfSide[2],1.5)
- box.halfSide[0] = 4.
- box.halfSide[0] = 5.
- box.halfSide[0] = 6.
-# self.assertEqual(box.halfSide[0],4.)
-# self.assertEqual(box.halfSide[1],5.)
-# self.assertEqual(box.halfSide[2],6.)
-
+
def test_sphere(self):
sphere = hppfcl.Sphere(1.)
self.assertIsInstance(sphere, hppfcl.Sphere)
self.assertIsInstance(sphere, hppfcl.ShapeBase)
self.assertIsInstance(sphere, hppfcl.CollisionGeometry)
self.assertEqual(sphere.getNodeType(), hppfcl.NODE_TYPE.GEOM_SPHERE)
- self.assertEqual(sphere.radius,1.)
+ self.assertEqual(sphere.radius, 1.)
sphere.radius = 2.
- self.assertEqual(sphere.radius,2.)
+ self.assertEqual(sphere.radius, 2.)
+ com = sphere.computeCOM()
+ self.assertApprox(com, np.zeros(3))
+ V = sphere.computeVolume()
+ V_ref = 4. * np.pi/3 * sphere.radius**3
+ self.assertApprox(V, V_ref)
+ I0 = sphere.computeMomentofInertia()
+ I0_ref = 0.4 * V_ref * sphere.radius * sphere.radius * np.identity(3)
+ self.assertApprox(I0, I0_ref)
+ Ic = sphere.computeMomentofInertiaRelatedToCOM()
+ self.assertApprox(Ic, I0_ref)
def test_cylinder(self):
cylinder = hppfcl.Cylinder(1.,2.)
@@ -62,6 +105,22 @@ class TestGeometricShapes(unittest.TestCase):
self.assertEqual(cylinder.getNodeType(), hppfcl.NODE_TYPE.GEOM_CYLINDER)
self.assertEqual(cylinder.radius,1.)
self.assertEqual(cylinder.halfLength,1.)
+ cylinder.radius = 3.
+ cylinder.halfLength = 4.
+ self.assertEqual(cylinder.radius,3.)
+ self.assertEqual(cylinder.halfLength,4.)
+ com = cylinder.computeCOM()
+ self.assertApprox(com, np.zeros(3))
+ V = cylinder.computeVolume()
+ V_ref = cylinder.radius * cylinder.radius * np.pi * 2. * cylinder.halfLength
+ self.assertApprox(V, V_ref)
+ I0 = cylinder.computeMomentofInertia()
+ Ix_ref = V_ref*(3 * cylinder.radius**2 + 4 * cylinder.halfLength**2)/12.
+ Iz_ref = V_ref * cylinder.radius**2 / 2.
+ I0_ref = np.diag([Ix_ref,Ix_ref,Iz_ref])
+ self.assertApprox(I0, I0_ref)
+ Ic = cylinder.computeMomentofInertiaRelatedToCOM()
+ self.assertApprox(Ic, I0_ref)
def test_cone(self):
cone = hppfcl.Cone(1.,2.)
@@ -75,6 +134,20 @@ class TestGeometricShapes(unittest.TestCase):
cone.halfLength = 4.
self.assertEqual(cone.radius,3.)
self.assertEqual(cone.halfLength,4.)
+ com = cone.computeCOM()
+ self.assertApprox(com, np.matrix([0.,0.,-0.5 * cone.halfLength]).T)
+ V = cone.computeVolume()
+ V_ref = np.pi * cone.radius**2 * 2. * cone.halfLength / 3.
+ self.assertApprox(V, V_ref)
+ I0 = cone.computeMomentofInertia()
+ Ix_ref = V_ref * (3./20. * cone.radius**2 + 0.4 * cone.halfLength**2)
+ Iz_ref = 0.3 * V_ref * cone.radius**2
+ I0_ref = np.diag([Ix_ref,Ix_ref,Iz_ref])
+ self.assertApprox(I0, I0_ref)
+ Ic = cone.computeMomentofInertiaRelatedToCOM()
+ Icx_ref = V_ref * 3./20. * (cone.radius**2 + cone.halfLength**2)
+ Ic_ref = np.diag([Icx_ref,Icx_ref,Iz_ref])
+ self.assertApprox(Ic, Ic_ref)
if __name__ == '__main__':
- unittest.main()
\ No newline at end of file
+ unittest.main()
diff --git a/test/python_unit/test_case.py b/test/python_unit/test_case.py
new file mode 100644
index 0000000000000000000000000000000000000000..e6bec282b6e9af3a6d2c936260764c182c90760c
--- /dev/null
+++ b/test/python_unit/test_case.py
@@ -0,0 +1,7 @@
+import unittest
+import numpy as np
+
+class TestCase(unittest.TestCase):
+ def assertApprox(self, a, b, epsilon=1e-6):
+ return self.assertTrue(np.allclose(a, b, epsilon), "%s !~= %s" % (a, b))
+