python_unit/geometric_shapes.py
Go to the documentation of this file.
1 import unittest
2 from test_case import TestCase
3 import coal
4 import numpy as np
5 
6 
7 class TestGeometricShapes(TestCase):
8  def test_capsule(self):
9  capsule = coal.Capsule(1.0, 2.0)
10  self.assertIsInstance(capsule, coal.Capsule)
11  self.assertIsInstance(capsule, coal.ShapeBase)
12  self.assertIsInstance(capsule, coal.CollisionGeometry)
13  self.assertEqual(capsule.getNodeType(), coal.NODE_TYPE.GEOM_CAPSULE)
14  self.assertEqual(capsule.radius, 1.0)
15  self.assertEqual(capsule.halfLength, 1.0)
16  capsule.radius = 3.0
17  capsule.halfLength = 4.0
18  self.assertEqual(capsule.radius, 3.0)
19  self.assertEqual(capsule.halfLength, 4.0)
20  com = capsule.computeCOM()
21  self.assertApprox(com, np.zeros(3))
22  V = capsule.computeVolume()
23  V_cylinder = capsule.radius * capsule.radius * np.pi * 2.0 * capsule.halfLength
24  V_sphere = 4.0 * np.pi / 3 * capsule.radius**3
25  V_ref = V_cylinder + V_sphere
26  self.assertApprox(V, V_ref)
27  I0 = capsule.computeMomentofInertia()
28  Iz_cylinder = V_cylinder * capsule.radius**2 / 2.0
29  Iz_sphere = 0.4 * V_sphere * capsule.radius * capsule.radius
30  Iz_ref = Iz_cylinder + Iz_sphere
31  Ix_cylinder = (
32  V_cylinder * (3 * capsule.radius**2 + 4 * capsule.halfLength**2) / 12.0
33  )
34  V_hemi = 0.5 * V_sphere # volume of hemisphere
35  I0x_hemi = 0.5 * Iz_sphere # inertia of hemisphere w.r.t. origin
36  com_hemi = 3.0 * capsule.radius / 8.0 # CoM of hemisphere w.r.t. origin
37  Icx_hemi = (
38  I0x_hemi - V_hemi * com_hemi * com_hemi
39  ) # inertia of hemisphere w.r.t. CoM
40  Ix_hemi = (
41  Icx_hemi + V_hemi * (capsule.halfLength + com_hemi) ** 2
42  ) # inertia of hemisphere w.r.t. tip of cylinder
43  Ix_ref = Ix_cylinder + 2 * Ix_hemi # total inertia of capsule
44  I0_ref = np.diag([Ix_ref, Ix_ref, Iz_ref])
45  self.assertApprox(I0, I0_ref)
46  Ic = capsule.computeMomentofInertiaRelatedToCOM()
47  self.assertApprox(Ic, I0_ref)
48 
49  def test_box1(self):
50  box = coal.Box(np.array([1.0, 2.0, 3.0]))
51  self.assertIsInstance(box, coal.Box)
52  self.assertIsInstance(box, coal.ShapeBase)
53  self.assertIsInstance(box, coal.CollisionGeometry)
54  self.assertEqual(box.getNodeType(), coal.NODE_TYPE.GEOM_BOX)
55  self.assertTrue(np.array_equal(box.halfSide, np.array([0.5, 1.0, 1.5])))
56  box.halfSide = np.array([4.0, 5.0, 6.0])
57  self.assertTrue(np.array_equal(box.halfSide, np.array([4.0, 5.0, 6.0])))
58  com = box.computeCOM()
59  self.assertApprox(com, np.zeros(3))
60  V = box.computeVolume()
61  x = float(2 * box.halfSide[0])
62  y = float(2 * box.halfSide[1])
63  z = float(2 * box.halfSide[2])
64  V_ref = x * y * z
65  self.assertApprox(V, V_ref)
66  I0 = box.computeMomentofInertia()
67  Ix = V_ref * (y * y + z * z) / 12.0
68  Iy = V_ref * (x * x + z * z) / 12.0
69  Iz = V_ref * (y * y + x * x) / 12.0
70  I0_ref = np.diag([Ix, Iy, Iz])
71  self.assertApprox(I0, I0_ref)
72  Ic = box.computeMomentofInertiaRelatedToCOM()
73  self.assertApprox(Ic, I0_ref)
74 
75  def test_box2(self):
76  box = coal.Box(1.0, 2.0, 3)
77  self.assertIsInstance(box, coal.Box)
78  self.assertIsInstance(box, coal.ShapeBase)
79  self.assertIsInstance(box, coal.CollisionGeometry)
80  self.assertEqual(box.getNodeType(), coal.NODE_TYPE.GEOM_BOX)
81  self.assertEqual(box.halfSide[0], 0.5)
82  self.assertEqual(box.halfSide[1], 1.0)
83  self.assertEqual(box.halfSide[2], 1.5)
84 
85  def test_sphere(self):
86  sphere = coal.Sphere(1.0)
87  self.assertIsInstance(sphere, coal.Sphere)
88  self.assertIsInstance(sphere, coal.ShapeBase)
89  self.assertIsInstance(sphere, coal.CollisionGeometry)
90  self.assertEqual(sphere.getNodeType(), coal.NODE_TYPE.GEOM_SPHERE)
91  self.assertEqual(sphere.radius, 1.0)
92  sphere.radius = 2.0
93  self.assertEqual(sphere.radius, 2.0)
94  com = sphere.computeCOM()
95  self.assertApprox(com, np.zeros(3))
96  V = sphere.computeVolume()
97  V_ref = 4.0 * np.pi / 3 * sphere.radius**3
98  self.assertApprox(V, V_ref)
99  I0 = sphere.computeMomentofInertia()
100  I0_ref = 0.4 * V_ref * sphere.radius * sphere.radius * np.identity(3)
101  self.assertApprox(I0, I0_ref)
102  Ic = sphere.computeMomentofInertiaRelatedToCOM()
103  self.assertApprox(Ic, I0_ref)
104 
105  def test_cylinder(self):
106  cylinder = coal.Cylinder(1.0, 2.0)
107  self.assertIsInstance(cylinder, coal.Cylinder)
108  self.assertIsInstance(cylinder, coal.ShapeBase)
109  self.assertIsInstance(cylinder, coal.CollisionGeometry)
110  self.assertEqual(cylinder.getNodeType(), coal.NODE_TYPE.GEOM_CYLINDER)
111  self.assertEqual(cylinder.radius, 1.0)
112  self.assertEqual(cylinder.halfLength, 1.0)
113  cylinder.radius = 3.0
114  cylinder.halfLength = 4.0
115  self.assertEqual(cylinder.radius, 3.0)
116  self.assertEqual(cylinder.halfLength, 4.0)
117  com = cylinder.computeCOM()
118  self.assertApprox(com, np.zeros(3))
119  V = cylinder.computeVolume()
120  V_ref = cylinder.radius * cylinder.radius * np.pi * 2.0 * cylinder.halfLength
121  self.assertApprox(V, V_ref)
122  I0 = cylinder.computeMomentofInertia()
123  Ix_ref = V_ref * (3 * cylinder.radius**2 + 4 * cylinder.halfLength**2) / 12.0
124  Iz_ref = V_ref * cylinder.radius**2 / 2.0
125  I0_ref = np.diag([Ix_ref, Ix_ref, Iz_ref])
126  self.assertApprox(I0, I0_ref)
127  Ic = cylinder.computeMomentofInertiaRelatedToCOM()
128  self.assertApprox(Ic, I0_ref)
129 
130  def test_cone(self):
131  cone = coal.Cone(1.0, 2.0)
132  self.assertIsInstance(cone, coal.Cone)
133  self.assertIsInstance(cone, coal.ShapeBase)
134  self.assertIsInstance(cone, coal.CollisionGeometry)
135  self.assertEqual(cone.getNodeType(), coal.NODE_TYPE.GEOM_CONE)
136  self.assertEqual(cone.radius, 1.0)
137  self.assertEqual(cone.halfLength, 1.0)
138  cone.radius = 3.0
139  cone.halfLength = 4.0
140  self.assertEqual(cone.radius, 3.0)
141  self.assertEqual(cone.halfLength, 4.0)
142  com = cone.computeCOM()
143  self.assertApprox(com, np.array([0.0, 0.0, -0.5 * cone.halfLength]))
144  V = cone.computeVolume()
145  V_ref = np.pi * cone.radius**2 * 2.0 * cone.halfLength / 3.0
146  self.assertApprox(V, V_ref)
147  I0 = cone.computeMomentofInertia()
148  Ix_ref = V_ref * (3.0 / 20.0 * cone.radius**2 + 0.4 * cone.halfLength**2)
149  Iz_ref = 0.3 * V_ref * cone.radius**2
150  I0_ref = np.diag([Ix_ref, Ix_ref, Iz_ref])
151  self.assertApprox(I0, I0_ref)
152  Ic = cone.computeMomentofInertiaRelatedToCOM()
153  Icx_ref = V_ref * 3.0 / 20.0 * (cone.radius**2 + cone.halfLength**2)
154  Ic_ref = np.diag([Icx_ref, Icx_ref, Iz_ref])
155  self.assertApprox(Ic, Ic_ref)
156 
157  def test_BVH(self):
158  bvh = coal.BVHModelOBBRSS()
159  self.assertEqual(bvh.num_vertices, 0)
160  self.assertEqual(bvh.vertices().shape, (0, 3))
161 
162  def test_convex(self):
163  verts = coal.StdVec_Vec3s()
164  faces = coal.StdVec_Triangle()
165  verts.extend(
166  [
167  np.array([0, 0, 0]),
168  np.array([0, 1, 0]),
169  np.array([1, 0, 0]),
170  ]
171  )
172  faces.append(coal.Triangle(0, 1, 2))
173  coal.Convex(verts, faces)
174 
175  verts.append(np.array([0, 0, 1]))
176  try:
177  coal.Convex.convexHull(verts, False, None)
178  qhullAvailable = True
179  except Exception as e:
180  self.assertIn(
181  "Library built without qhull. Cannot build object of this type.", str(e)
182  )
183  qhullAvailable = False
184 
185  if qhullAvailable:
186  coal.Convex.convexHull(verts, False, "")
187  coal.Convex.convexHull(verts, True, "")
188 
189  try:
190  coal.Convex.convexHull(verts[:3], False, None)
191  except Exception as e:
192  self.assertIn(
193  "You shouldn't use this function with less than 4 points.", str(e)
194  )
195 
196 
197 if __name__ == "__main__":
198  unittest.main()
geometric_shapes.TestGeometricShapes.test_convex
def test_convex(self)
Definition: python_unit/geometric_shapes.py:162
geometric_shapes.TestGeometricShapes.test_sphere
def test_sphere(self)
Definition: python_unit/geometric_shapes.py:85
test_case.TestCase
Definition: test_case.py:5
coal::Capsule
Capsule It is where is the distance between the point x and the capsule segment AB,...
Definition: coal/shape/geometric_shapes.h:383
coal::CollisionGeometry
The geometry for the object for collision or distance computation.
Definition: coal/collision_object.h:94
coal::Box
Center at zero point, axis aligned box.
Definition: coal/shape/geometric_shapes.h:166
coal::Sphere
Center at zero point sphere.
Definition: coal/shape/geometric_shapes.h:240
coal::ShapeBase
Base class for all basic geometric shapes.
Definition: coal/shape/geometric_shapes.h:58
coal::Convex
Convex polytope.
Definition: coal/serialization/collision_object.h:51
geometric_shapes.TestGeometricShapes.test_cylinder
def test_cylinder(self)
Definition: python_unit/geometric_shapes.py:105
geometric_shapes.TestGeometricShapes.test_cone
def test_cone(self)
Definition: python_unit/geometric_shapes.py:130
coal::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: coal/shape/geometric_shapes.h:560
geometric_shapes.TestGeometricShapes.test_capsule
def test_capsule(self)
Definition: python_unit/geometric_shapes.py:8
geometric_shapes.TestGeometricShapes.test_box2
def test_box2(self)
Definition: python_unit/geometric_shapes.py:75
coal::Cone
Cone The base of the cone is at and the top is at .
Definition: coal/shape/geometric_shapes.h:467
geometric_shapes.TestGeometricShapes.test_box1
def test_box1(self)
Definition: python_unit/geometric_shapes.py:49
test_case.TestCase.assertApprox
def assertApprox(self, a, b, epsilon=1e-6)
Definition: test_case.py:6
geometric_shapes.TestGeometricShapes.test_BVH
def test_BVH(self)
Definition: python_unit/geometric_shapes.py:157
coal::Triangle
Triangle with 3 indices for points.
Definition: coal/data_types.h:111
geometric_shapes.TestGeometricShapes
Definition: python_unit/geometric_shapes.py:7
str
const char * str()
Definition: doxygen_xml_parser.py:885

hpp-fcl
Author(s):
autogenerated on Fri Feb 14 2025 03:45:50