def test_collision_and_distance_box_sphere(self):
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.b1, -1)
self.assertEqual(result.b2, -1)
dis, result = fcl.distance(
fcl.CollisionObject(self.box,
transform.Transform(transform.Quaternion())),
fcl.CollisionObject(
self.sphere,
transform.Transform(transform.Quaternion(), [0.0, 0.0, 0.0])),
fcl.DistanceRequest(True))
self.assertEqual(dis, -1.0)
self.assertEqual(result.min_distance, -1.0)
for i in result.nearest_points:
for j in i:
self.assertAlmostEqual(0.0, j)
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 __init__(self, part_num):
""" Room Parameters """
self.room_dimensions = [7.50, 7.50]
self.room_offset = [-1.25, -1.25]
""" Walls """
self.wall_1 = fcl.CollisionObject(fcl.Box(8.0, 0.5, 0.5),transform.Transform(transform.Quaternion(), [2.50, -1.25, 0.25]))
self.wall_2 = fcl.CollisionObject(fcl.Box(0.5, 8.0, 0.5),transform.Transform(transform.Quaternion(), [6.25, 2.50, 0.25]))
self.wall_3 = fcl.CollisionObject(fcl.Box(8.0, 0.5, 0.5),transform.Transform(transform.Quaternion(), [2.50, 6.25, 0.25]))
self.wall_4 = fcl.CollisionObject(fcl.Box(0.5, 8.0, 0.5),transform.Transform(transform.Quaternion(), [-1.25, 2.50, 0.25]))
""" Obstacles """
self.obs_1 = fcl.CollisionObject(fcl.Box(2.0, 2.0, 1.0),transform.Transform(transform.Quaternion(), [2.50, 2.50, 0.50]))
self.obs_2 = fcl.CollisionObject(fcl.Box(0.75, 0.75, 0.75),transform.Transform(transform.Quaternion(), [5.0, 1.0, 0.375]))
self.obs_3 = fcl.CollisionObject(fcl.Box(0.75, 0.75, 0.75),transform.Transform(transform.Quaternion(), [3.0, 5.0, 0.375]))
self.obs_4 = fcl.CollisionObject(fcl.Box(0.5, 0.5, 0.5),transform.Transform(transform.Quaternion(), [0.0, 3.0, 0.25]))
""" Table Obstacle """
self.obs_5_1 = fcl.CollisionObject(fcl.Box(0.4, 0.4, 0.2),transform.Transform(transform.Quaternion(), [0.6, 0.0, 0.1]))
self.obs_5_2 = fcl.CollisionObject(fcl.Box(0.05, 0.05, 0.15),transform.Transform(transform.Quaternion(), [0.5, 0.1, 0.275]))
self.obs_5_3 = fcl.CollisionObject(fcl.Box(0.05, 0.05, 0.15),transform.Transform(transform.Quaternion(), [0.5, -0.1, 0.275]))
self.obs_5_4 = fcl.CollisionObject(fcl.Box(0.05, 0.25, 0.05),transform.Transform(transform.Quaternion(), [0.5, 0.0, 0.375]))
""" Part 1 YouBot """
self.youbot_Box = fcl.Box(0.7, 0.5, 0.6)
""" Part 2 YouBot """
self.youbot_base = fcl.Box(0.5701, 0.3570, 0.1060)
self.youbot_arm_1 = fcl.Box(0.1697, 0.1699, 0.1060)
self.youbot_arm_2 = fcl.Box(0.22, 0.0745, 0.0823)
self.youbot_arm_3 = fcl.Box(0.1920, 0.0591, 0.0750)
self.youbot_arm_4 = fcl.Box(0.2401, 0.0581, 0.0987)
""" YouBot """
self.section = part_num
self.youbot = fcl.CollisionObject(self.youbot_Box,transform.Transform(transform.Quaternion(), [0.0, 0.0, 0.0]))
""" Obstacles as a list """
self.obs = [self.wall_1, self.wall_2, self.wall_3, self.wall_4,
self.obs_1, self.obs_2, self.obs_3, self.obs_4,
self.obs_5_1, self.obs_5_2, self.obs_5_3, self.obs_5_4]
self.res = fcl.CollisionResult()
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 _create_youbot_object(self, config):
if self.section == 1:
self.youbot = fcl.CollisionObject(self.youbot_Box, transform.Transform(transform.Quaternion(), config))
else:
self.youbot = []
self.yobot.append(fcl.CollisionObject(self.youbot_base, transform.Transform(transform.Quaternion(), [-0.0014, 0.0, 0.0956])))
self.yobot.append(fcl.CollisionObject(self.youbot_arm_1, transform.Transform(transform.Quaternion(), [0.167, 0.0, 0.086])))
quat_arm_1 = [math.cos(config[0]/2), 0, -math.sin(config[0]/2), 0]
pos_arm_1 = [0.167+0.074*math.cos(config[0]), -0.0412, 0.086+0.074*math.sin(config[0])]
self.yobot.append(fcl.CollisionObject(self.youbot_arm_2, transform.Transform(quat_arm_1, pos_arm_1)))
quat_arm_2 = [math.cos(config[1]/2), 0, -math.sin(config[1]/2), 0]
quat_arm_2 = self._multiply_quaternion(quat_arm_1, quat_arm_2)
pos_arm_2 = pos_arm_1 + [0.0662*math.cos(config[1]), 0.0370 + 0.0412, 0.0662*math.sin(config[1])]
self.yobot.append(fcl.CollisionObject(self.youbot_arm_3, transform.Transform(quat_arm_2, pos_arm_2)))
quat_arm_3 = [math.cos(config[2]/2), 0, -math.sin(config[2]/2), 0]
quat_arm_3 = self._multiply_quaternion(quat_arm_2, quat_arm_3)
pos_arm_3 = pos_arm_2 + [0.0869*math.cos(config[2]), -0.0370 , 0.0869*math.sin(config[2])]
self.yobot.append(fcl.CollisionObject(self.youbot_arm_4, transform.Transform(quat_arm_3, pos_arm_3)))
# 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(
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),