def _check_collision(self, obstacle, robot):
if self.section == 1:
ret, res = fcl.collide(obstacle, robot, fcl.CollisionRequest())
else:
for bot_part in robot:
ret, res = fcl.collide(obstacle, bot_part, fcl.CollisionRequest())
if ret = False:
break
def test_triangle(self):
"""
TriangleP is not well supported...
Perhaps not much of an issue, since BVHModel will deal with meshes / triangle soup
So, what'evs?
"""
import numpy as np
li = [
[0, 100, 100],
[0, 0, 0],
[100, 0, 100],
]
arr = np.array(li, "f")
_p = fcl.TriangleP(arr[0], arr[1], arr[2])
p = fcl.CollisionObject(_p)
collision_object = fcl.CollisionObject(
self.box, transform.Transform(transform.Quaternion()))
# TODO: segfault!
# ....Warning: distance function between node type 9 and node type 17 is not supported
# dis, result = fcl.distance(collision_object, p, fcl.DistanceRequest(True))
ret, result = fcl.collide(collision_object, p, fcl.CollisionRequest())
# Warning: collision function between node type 9 and node type 17 is not supported
self.assertTrue(ret == 0)
def test_collision_box_sphere(self):
ret, result = fcl.collide(
fcl.CollisionObject(fcl.Box(*self._side),
transform.Transform(transform.Quaternion())),
fcl.CollisionObject(
fcl.Sphere(self._radius),
transform.Transform(transform.Quaternion(), [0.0, 0.0, 0.0])),
fcl.CollisionRequest())
self.assertTrue(len(result.contacts) == 1)
self.assertTrue(len(result.cost_sources) == 0)
con = result.contacts[0]
self.assertAlmostEqual(con.penetration_depth, 0.0)
self.assertTrue(con.penetration_depth < sys.float_info.min)
self.assertTrue(isinstance(con.o1, fcl.Box))
self.assertTrue(isinstance(con.o2, fcl.Sphere))
def test_distance_box_sphere_translated(self):
ret, result = fcl.collide(
fcl.CollisionObject(self.box, self.trans1),
fcl.CollisionObject(self.sphere, self.trans2),
fcl.CollisionRequest())
self.assertEqual(result.contacts, [])
self.assertEqual(result.cost_sources, [])
dis, result = fcl.distance(
fcl.CollisionObject(self.box, self.trans1),
fcl.CollisionObject(self.sphere, self.trans2),
fcl.DistanceRequest(True))
self.assertEqual(dis, self._ref_dist)
self.assertEqual(result.min_distance, self._ref_dist)
self.assertEqual(result.nearest_points, self._nearest_points)
self.assertEqual(result.o2.radius, self._radius)
self.assertEqual(result.o1.side, self._side)
def collide_callback_func(obj1, obj2, res):
ret, res = fcl.collide(obj1, obj2, fcl.CollisionRequest())
return ret
def cb_func(obj1, obj2, res):
print "cb_func start"
ret, res = fcl.collide(obj1, obj2, fcl.CollisionRequest())
print "result: ", ret
return ret
print "result: ", ret
return ret
res = fcl.CollisionResult()
manager.collide(res, cb_func)
# Collision calcuration
ret, result = fcl.collide(
fcl.CollisionObject(
fcl.Box(1.0, 2.0, 3.0),
transform.Transform(transform.Quaternion(), [10.0, 0.0, 0.0])),
fcl.CollisionObject(
fcl.Sphere(4.0),
transform.Transform(transform.Quaternion(), [-10.0, 0.0, 0.0])),
fcl.CollisionRequest())
print "-- Collision result: ", ret
for contact in result.contacts:
print contact.o1
print contact.o2
for cost_source in result.cost_sources:
print cost_source
dis, result = fcl.distance(
fcl.CollisionObject(
fcl.Box(1.0, 2.0, 3.0),
transform.Transform(transform.Quaternion(), [10.0, 0.0, 0.0])),
fcl.CollisionObject(
fcl.Sphere(4.0),
transform.Transform(transform.Quaternion(), [-10.0, 0.0, 0.0])),