geometric_shapes.py 6.91 KB
Newer Older
Gabriele Buondonno's avatar
Gabriele Buondonno committed
1
import unittest
2
from test_case import TestCase
Gabriele Buondonno's avatar
Gabriele Buondonno committed
3
import hppfcl
4
hppfcl.switchToNumpyMatrix()
Gabriele Buondonno's avatar
Gabriele Buondonno committed
5
6
import numpy as np

7
class TestGeometricShapes(TestCase):
Gabriele Buondonno's avatar
Gabriele Buondonno committed
8
9
10
11
12
13
14
15

    def test_capsule(self):
        capsule = hppfcl.Capsule(1.,2.)
        self.assertIsInstance(capsule, hppfcl.Capsule)
        self.assertIsInstance(capsule, hppfcl.ShapeBase)
        self.assertIsInstance(capsule, hppfcl.CollisionGeometry)
        self.assertEqual(capsule.getNodeType(), hppfcl.NODE_TYPE.GEOM_CAPSULE)
        self.assertEqual(capsule.radius,1.)
16
        self.assertEqual(capsule.halfLength,1.)
Gabriele Buondonno's avatar
Gabriele Buondonno committed
17
        capsule.radius = 3.
18
        capsule.halfLength = 4.
Gabriele Buondonno's avatar
Gabriele Buondonno committed
19
        self.assertEqual(capsule.radius,3.)
20
        self.assertEqual(capsule.halfLength,4.)
21
22
23
24
25
26
27
28
29
30
31
32
        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.
33
34
35
36
37
38
39
40
41
42
        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)
Gabriele Buondonno's avatar
Gabriele Buondonno committed
43
44
45
46
47
48
49
50
51
52

    def test_box1(self):
        box = hppfcl.Box(np.matrix([1.,2.,3.]).T)
        self.assertIsInstance(box, hppfcl.Box)
        self.assertIsInstance(box, hppfcl.ShapeBase)
        self.assertIsInstance(box, hppfcl.CollisionGeometry)
        self.assertEqual(box.getNodeType(), hppfcl.NODE_TYPE.GEOM_BOX)
        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))
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
        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) 
Gabriele Buondonno's avatar
Gabriele Buondonno committed
69
70
71
72
73
74
75
76
77
78

    def test_box2(self):
        box = hppfcl.Box(1.,2.,3)
        self.assertIsInstance(box, hppfcl.Box)
        self.assertIsInstance(box, hppfcl.ShapeBase)
        self.assertIsInstance(box, hppfcl.CollisionGeometry)
        self.assertEqual(box.getNodeType(), hppfcl.NODE_TYPE.GEOM_BOX)
        self.assertEqual(box.halfSide[0],0.5)
        self.assertEqual(box.halfSide[1],1.0)
        self.assertEqual(box.halfSide[2],1.5)
79
 
Gabriele Buondonno's avatar
Gabriele Buondonno committed
80
81
82
83
84
85
    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)
86
        self.assertEqual(sphere.radius, 1.)
Gabriele Buondonno's avatar
Gabriele Buondonno committed
87
        sphere.radius = 2.
88
89
90
91
92
93
94
95
96
97
98
        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)
Gabriele Buondonno's avatar
Gabriele Buondonno committed
99
100
101
102
103
104
105
106

    def test_cylinder(self):
        cylinder = hppfcl.Cylinder(1.,2.)
        self.assertIsInstance(cylinder, hppfcl.Cylinder)
        self.assertIsInstance(cylinder, hppfcl.ShapeBase)
        self.assertIsInstance(cylinder, hppfcl.CollisionGeometry)
        self.assertEqual(cylinder.getNodeType(), hppfcl.NODE_TYPE.GEOM_CYLINDER)
        self.assertEqual(cylinder.radius,1.)
107
        self.assertEqual(cylinder.halfLength,1.)
108
109
110
111
        cylinder.radius = 3.
        cylinder.halfLength = 4.
        self.assertEqual(cylinder.radius,3.)
        self.assertEqual(cylinder.halfLength,4.)
112
113
114
115
116
117
118
119
120
121
122
123
        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)
Gabriele Buondonno's avatar
Gabriele Buondonno committed
124
125
126
127
128
129
130
131

    def test_cone(self):
        cone = hppfcl.Cone(1.,2.)
        self.assertIsInstance(cone, hppfcl.Cone)
        self.assertIsInstance(cone, hppfcl.ShapeBase)
        self.assertIsInstance(cone, hppfcl.CollisionGeometry)
        self.assertEqual(cone.getNodeType(), hppfcl.NODE_TYPE.GEOM_CONE)
        self.assertEqual(cone.radius,1.)
132
        self.assertEqual(cone.halfLength,1.)
Gabriele Buondonno's avatar
Gabriele Buondonno committed
133
        cone.radius = 3.
134
        cone.halfLength = 4.
Gabriele Buondonno's avatar
Gabriele Buondonno committed
135
        self.assertEqual(cone.radius,3.)
136
        self.assertEqual(cone.halfLength,4.)
137
138
139
140
141
142
143
144
145
146
147
148
149
150
        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)
Gabriele Buondonno's avatar
Gabriele Buondonno committed
151
152

if __name__ == '__main__':
153
    unittest.main()