Commit 6b23932f by Gabriele Buondonno

### 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 v_sph = radius * radius * radius * boost::math::constants::pi() * 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