Commit 6b23932f authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

Fix capsule moment of inertia

parent c0486da8
......@@ -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,
......
......@@ -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)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment