def setUp(self):
self.objs = [
fcl.CollisionObject(fcl.Box(1.0, 2.0, 3.0)),
fcl.CollisionObject(fcl.Sphere(4.0)),
fcl.CollisionObject(fcl.Cone(5.0, 6.0))
]
self.manager = fcl.DynamicAABBTreeCollisionManager()
self.res = fcl.CollisionResult()
# self.manager.collide(self.res, self.collide_callback_func)
self._side = (1.0, 2.0, 3.0)
self._radius = 4.0
self._nearest_points = [(-0.5, 0.0, 0.0), (4.0, 0.0, 0.0)]
self._ref_dist = 15.5
self.pt1 = [10.0, 0.0, 0.0]
self.pt2 = [-10.0, 0.0, 0.0]
self.box = fcl.Box(*self._side)
self.sphere = fcl.Sphere(self._radius)
self.cyl = fcl.Cylinder(7.0, 8.0)
self.trans1 = transform.Transform(transform.Quaternion(), self.pt1)
self.trans2 = transform.Transform(transform.Quaternion(), self.pt2)
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))
from fcl import fcl, transform
objs = [
fcl.CollisionObject(fcl.Box(1.0, 2.0, 3.0)),
fcl.CollisionObject(fcl.Sphere(4.0)),
fcl.CollisionObject(fcl.Cone(5.0, 6.0))
]
# Register objects to DynamicAABBTreeCollisionManager
manager = fcl.DynamicAABBTreeCollisionManager()
print "Before resgister: ", manager.size()
manager.registerObjects(objs)
print "After register 1 : ", manager.size()
manager.registerObject(fcl.CollisionObject(fcl.Cylinder(7.0, 8.0)))
print "After register 2 : ", manager.size()
# Use Callback function
def cb_func(obj1, obj2, res):
print "cb_func start"
ret, res = fcl.collide(obj1, obj2, fcl.CollisionRequest())
print "result: ", ret
return ret
res = fcl.CollisionResult()
manager.collide(res, cb_func)
# Collision calcuration
ret, result = fcl.collide(
fcl.CollisionObject(