Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Guilhem Saurel
hpp-fcl
Commits
6b23932f
Commit
6b23932f
authored
Nov 15, 2019
by
Gabriele Buondonno
Browse files
Fix capsule moment of inertia
parent
c0486da8
Changes
2
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/shape/geometric_shapes.h
View file @
6b23932f
...
...
@@ -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
,
...
...
test/python_unit/geometric_shapes.py
View file @
6b23932f
...
...
@@ -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
)
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment