def __init__(self, dx, dy, dz):
self.dx = dx
self.dy = dy
self.dz = dz
self.num_edges = 12
self.fcl_shape = fcl.Box(dx, dy, dz)
self.request = fcl.CollisionRequest()
self.result = fcl.CollisionResult()
def __init__(self, radius, length, approx_faces=8):
self.r = radius
self.l = length
# the number of faces to use in the polyherdron approximation
self.nfac = approx_faces
self.num_edges = 3 * self.nfac
self.fcl_shape = fcl.Cylinder(radius, length)
self.request = fcl.CollisionRequest()
self.result = fcl.CollisionResult()
def test_many_objects(self):
manager = fcl.DynamicAABBTreeCollisionManager()
objs = [
fcl.CollisionObject(self.geometry['sphere']),
fcl.CollisionObject(self.geometry['sphere'],
fcl.Transform(np.array([0.0, 0.0, -5.0]))),
fcl.CollisionObject(self.geometry['sphere'],
fcl.Transform(np.array([0.0, 0.0, 5.0])))
]
manager.registerObjects(objs)
manager.setup()
self.assertTrue(len(manager.getObjects()) == 3)
# Many-to-many, internal
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager.collide(cdata, fcl.defaultCollisionCallback)
self.assertFalse(cdata.result.is_collision)
objs[1].setTranslation(np.array([0.0, 0.0, -0.3]))
manager.update(objs[1])
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager.collide(cdata, fcl.defaultCollisionCallback)
self.assertTrue(cdata.result.is_collision)
objs[1].setTranslation(np.array([0.0, 0.0, -5.0]))
manager.update(objs[1])
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager.collide(cdata, fcl.defaultCollisionCallback)
self.assertFalse(cdata.result.is_collision)
objs[2].setTranslation(np.array([0.0, 0.0, 0.3]))
manager.update(objs[2])
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager.collide(cdata, fcl.defaultCollisionCallback)
self.assertTrue(cdata.result.is_collision)
objs[2].setTranslation(np.array([0.0, 0.0, 5.0]))
manager.update(objs[2])
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager.collide(cdata, fcl.defaultCollisionCallback)
self.assertFalse(cdata.result.is_collision)
def checkCollision(self): g1 = fcl.Sphere(self.Radius) t1 = fcl.Transform() o1 = fcl.CollisionObject(g1, t1) g2 = fcl.sphere(self.Radius) t2 = fcl.Transform() o2 = fcl.CollisionObject(g2, t2) request = fcl.CollisionRequest() result = fcl.CollisionResult() ret = fcl.collide(o1, o2, request, result) return result, ret #result.is_collide is the required function
def test_pairwise_collisions(self):
result = fcl.CollisionResult()
box = fcl.CollisionObject(self.geometry['box'])
cone = fcl.CollisionObject(self.geometry['cone'])
mesh = fcl.CollisionObject(self.geometry['mesh'])
result = fcl.CollisionResult()
ret = fcl.collide(box, cone, self.crequest, result)
self.assertTrue(ret > 0)
self.assertTrue(result.is_collision)
result = fcl.CollisionResult()
ret = fcl.collide(box, mesh, self.crequest, result)
self.assertTrue(ret > 0)
self.assertTrue(result.is_collision)
result = fcl.CollisionResult()
ret = fcl.collide(cone, mesh, self.crequest, result)
self.assertTrue(ret > 0)
self.assertTrue(result.is_collision)
cone.setTranslation(np.array([0.0, 0.0, -0.6]))
result = fcl.CollisionResult()
ret = fcl.collide(box, cone, self.crequest, result)
self.assertTrue(ret > 0)
self.assertTrue(result.is_collision)
result = fcl.CollisionResult()
ret = fcl.collide(cone, mesh, self.crequest, result)
self.assertTrue(ret == 0)
self.assertFalse(result.is_collision)
cone.setTranslation(np.array([0.0, -0.9, 0.0]))
cone.setRotation(self.x_axis_rot)
result = fcl.CollisionResult()
ret = fcl.collide(box, cone, self.crequest, result)
self.assertTrue(ret > 0)
self.assertTrue(result.is_collision)
cone.setTranslation(np.array([0.0, -1.1, 0.0]))
result = fcl.CollisionResult()
ret = fcl.collide(box, cone, self.crequest, result)
self.assertTrue(ret == 0)
self.assertFalse(result.is_collision)
def collision_check(self):
#world = ['blue_box', 'white_sphere', 'red_cylinder', 'green_box', 'turquoise_box', 'blue_cylinder', 'white_box', 'fetch']
robot = fcl.Cylinder(0.4, 1)
tR = fcl.Transform(self.quaternion[7], self.translation[7])
print self.translation[7]
oR = fcl.CollisionObject(robot, tR)
ob0 = fcl.Box(0.3, 1, 0.8)
tr0 = fcl.Transform(self.quaternion[0], self.translation[0])
self.obj[0] = fcl.CollisionObject(ob0, tr0)
ob1 = fcl.Sphere(0.5)
tr1 = fcl.Transform(self.quaternion[1], self.translation[1])
self.obj[1] = fcl.CollisionObject(ob1, tr1)
ob2 = fcl.Cylinder(0.5, 1)
tr2 = fcl.Transform(self.quaternion[2], self.translation[2])
self.obj[2] = fcl.CollisionObject(ob2, tr2)
ob3 = fcl.Box(0.5, 1.4, 0.8)
tr3 = fcl.Transform(self.quaternion[3], self.translation[3])
self.obj[3] = fcl.CollisionObject(ob3, tr3)
ob4 = fcl.Box(1, 5, 1)
tr4 = fcl.Transform(self.quaternion[4], self.translation[4])
self.obj[4] = fcl.CollisionObject(ob4, tr4)
ob5 = fcl.Cylinder(0.5, 1)
tr5 = fcl.Transform(self.quaternion[5], self.translation[5])
self.obj[5] = fcl.CollisionObject(ob5, tr5)
ob6 = fcl.Box(5, 1, 1)
tr6 = fcl.Transform(self.quaternion[6], self.translation[6])
self.obj[6] = fcl.CollisionObject(ob6, tr6)
request = fcl.CollisionRequest()
result = fcl.CollisionResult()
for i in range(7):
self.ret[i] = fcl.collide(oR, self.obj[i], request, result)
# ret = fcl.continuousCollide(oR, tR, o_wb, t_wb, request, result)
if self.ret[i]:
print "--------------- YES ", self.ret[
i], " --------------------"
else:
print "--------------- NO ", self.ret[
i], " -------------------"
def checkLidarCollistion(lidar, verts, tris):
mesh = fcl.BVHModel()
mesh.beginModel(len(verts), len(tris))
mesh.addSubModel(verts, tris)
mesh.endModel()
mesh_obj = fcl.CollisionObject(mesh)
objs = [mesh_obj, lidar]
manager = fcl.DynamicAABBTreeCollisionManager()
manager.registerObjects(objs)
manager.setup()
crequest = fcl.CollisionRequest(enable_contact=True)
drequest = fcl.DistanceRequest(enable_nearest_points=True)
cdata = fcl.CollisionData(crequest, fcl.CollisionResult())
ddata = fcl.DistanceData(drequest, fcl.DistanceResult())
manager.collide(cdata, fcl.defaultCollisionCallback)
manager.distance(ddata, fcl.defaultDistanceCallback)
minimum_distance = ddata.result.min_distance
return minimum_distance
def CheckCollison(rotate_direction, verts, tris, stick, rota_degree, trans_x,
trans_y, tolerance):
global previous_minimum_distance
mesh = fcl.BVHModel()
mesh.beginModel(len(verts), len(tris))
mesh.addSubModel(verts, tris)
mesh.endModel()
mesh_obj = fcl.CollisionObject(mesh)
objs = [mesh_obj, stick]
manager = fcl.DynamicAABBTreeCollisionManager()
manager.registerObjects(objs)
manager.setup()
crequest = fcl.CollisionRequest(enable_contact=True)
drequest = fcl.DistanceRequest(enable_nearest_points=True)
cdata = fcl.CollisionData(crequest, fcl.CollisionResult())
ddata = fcl.DistanceData(drequest, fcl.DistanceResult())
manager.collide(cdata, fcl.defaultCollisionCallback)
manager.distance(ddata, fcl.defaultDistanceCallback)
#print("Is collision: ", cdata.result.is_collision)
minimum_distance = ddata.result.min_distance
nearest_point = ddata.result.nearest_points[0][:2]
nearest_point_stick = ddata.result.nearest_points[1]
nearest_point_stick = Transform(rota_degree, trans_x, trans_y,
nearest_point_stick)
collision = -10000
if previous_minimum_distance != -10000:
if minimum_distance == -1:
collision = -1
print("The tip of the stick is inside the soft tissues.")
elif minimum_distance >= tolerance:
collision = 0
print("No collision")
elif minimum_distance >= 0 and minimum_distance < tolerance and minimum_distance < previous_minimum_distance:
print("Collision")
collision = 1
# if nearest_point in verts:
# print("nearest point of the stick:", nearest_point_stick)
tmp = previous_minimum_distance
previous_minimum_distance = minimum_distance
return nearest_point, collision, minimum_distance
from helper import helper, roshelper
from scipy.spatial import ConvexHull
from scipy.spatial import Delaunay
from shapely.geometry import Polygon
from tf.transformations import quaternion_from_euler
import util
from stl import mesh
import os
import tf
import trimesh
from copy import deepcopy
import fcl
import rospy
request = fcl.CollisionRequest()
result = fcl.CollisionResult()
def initialize_collision_object(tris, verts):
m = fcl.BVHModel()
m.beginModel(len(verts), len(tris))
m.addSubModel(verts, tris)
m.endModel()
t = fcl.Transform()
return fcl.CollisionObject(m, t)
def is_collision(body1, body2):
return fcl.collide(body1, body2, request, result)
class Body(object):
def __init__(self, mesh_name):
# self.mesh = mesh.Mesh.from_file(os.environ["CODE_BASE"] + "/catkin_ws/src/" + mesh_name)
def collision(ball1, ball2, ball3, ball4):
geom1 = fcl.Sphere(1.0)
geom2 = fcl.Sphere(1.0)
geom3 = fcl.Sphere(1.0)
geom4 = fcl.Sphere(1.0)
T1 = [ball1.pos.x, ball1.pos.y, ball1.pos.z]
t1 = fcl.Transform(T1)
obj1 = fcl.CollisionObject(geom1, t1)
T2 = [ball2.pos.x, ball2.pos.y, ball2.pos.z]
t2 = fcl.Transform(T2)
obj2 = fcl.CollisionObject(geom2, t2)
T3 = [ball3.pos.x, ball3.pos.y, ball3.pos.z]
t3 = fcl.Transform(T3)
obj3 = fcl.CollisionObject(geom3, t3)
T4 = [ball4.pos.x, ball4.pos.y, ball4.pos.z]
t4 = fcl.Transform(T4)
obj4 = fcl.CollisionObject(geom4, t4)
geoms = [geom1, geom2, geom3, geom4]
objs = [obj1, obj2, obj3, obj4]
names = ['obj1', 'obj2', 'obj3', 'obj4']
# Create map from geometry IDs to objects
geom_id_to_obj = {id(geom): obj for geom, obj in zip(geoms, objs)}
# Create map from geometry IDs to string names
geom_id_to_name = {id(geom): name for geom, name in zip(geoms, names)}
# Create manager
manager = fcl.DynamicAABBTreeCollisionManager()
manager.registerObjects(objs)
manager.setup()
# Create collision request structure
crequest = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
cdata = fcl.CollisionData(crequest, fcl.CollisionResult())
# Run collision request
manager.collide(cdata, fcl.defaultCollisionCallback)
# Extract collision data from contacts and use that to infer set of
# objects that are in collision
objs_in_collision = set()
for contact in cdata.result.contacts:
# Extract collision geometries that are in contact
coll_geom_0 = contact.o1
coll_geom_1 = contact.o2
# Get their names
coll_names = [
geom_id_to_name[id(coll_geom_0)], geom_id_to_name[id(coll_geom_1)]
]
coll_names = tuple(sorted(coll_names))
objs_in_collision.add(coll_names)
for coll_pair in objs_in_collision:
print('Object {} in collision with object {}!'.format(
coll_pair[0], coll_pair[1]))
return objs_in_collision
def test_managed_collisions(self):
manager1 = fcl.DynamicAABBTreeCollisionManager()
manager2 = fcl.DynamicAABBTreeCollisionManager()
manager3 = fcl.DynamicAABBTreeCollisionManager()
objs1 = [
fcl.CollisionObject(self.geometry['box']),
fcl.CollisionObject(self.geometry['cylinder'])
]
objs2 = [
fcl.CollisionObject(self.geometry['cone'],
fcl.Transform(np.array([0.0, 0.0, 5.0]))),
fcl.CollisionObject(self.geometry['cylinder'],
fcl.Transform(np.array([0.0, 0.0, -5.0])))
]
objs3 = [
fcl.CollisionObject(self.geometry['mesh']),
fcl.CollisionObject(self.geometry['box'],
fcl.Transform(np.array([0.0, 0.0, -5.0])))
]
manager1.registerObjects(objs1)
manager2.registerObjects(objs2)
manager3.registerObjects(objs3)
manager1.setup()
manager2.setup()
manager3.setup()
self.assertTrue(len(manager1.getObjects()) == 2)
self.assertTrue(len(manager2.getObjects()) == 2)
self.assertTrue(len(manager3.getObjects()) == 2)
# One-to-many
o1 = fcl.CollisionObject(self.geometry['box'])
o2 = fcl.CollisionObject(self.geometry['cylinder'],
fcl.Transform(np.array([0.0, 0.0, -4.6])))
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager1.collide(o1, cdata, fcl.defaultCollisionCallback)
self.assertTrue(cdata.result.is_collision)
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager1.collide(o2, cdata, fcl.defaultCollisionCallback)
self.assertFalse(cdata.result.is_collision)
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager2.collide(o1, cdata, fcl.defaultCollisionCallback)
self.assertFalse(cdata.result.is_collision)
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager2.collide(o2, cdata, fcl.defaultCollisionCallback)
self.assertTrue(cdata.result.is_collision)
# Many-to-many, internal
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager1.collide(cdata, fcl.defaultCollisionCallback)
self.assertTrue(cdata.result.is_collision)
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager2.collide(cdata, fcl.defaultCollisionCallback)
self.assertFalse(cdata.result.is_collision)
# Many-to-many, grouped
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager1.collide(manager2, cdata, fcl.defaultCollisionCallback)
self.assertFalse(cdata.result.is_collision)
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager2.collide(manager3, cdata, fcl.defaultCollisionCallback)
self.assertTrue(cdata.result.is_collision)
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager1.collide(manager3, cdata, fcl.defaultCollisionCallback)
self.assertTrue(cdata.result.is_collision)
box1 = fcl.CollisionObject(fcl.Box(1, 1, 0.0), Transform) #x,y,z length center at the origin
#box.setTranslation(np.array([0.0, 1.05000, 0.0])) #useful
#box.setRotation(rotation) #useful
#other options: setTransform setQuatRotation
rota_degree = 0
rotation = np.array([[np.cos(rota_degree/180*np.pi), -np.sin(rota_degree/180*np.pi), 0.0],
[np.sin(rota_degree/180*np.pi), np.cos(rota_degree/180*np.pi), 0.0],
[0.0, 0.0, 1.0]])
translation = np.array([1.4999, 0, 0.0])
Transform = fcl.Transform(rotation, translation)
box2 = fcl.CollisionObject(fcl.Box(3, 3, 0.0), Transform) #x,y,z length center at the origin
cylinder = fcl.CollisionObject(fcl.Cylinder(1, 0.0),Transform) #radius = 1
objs = [box1,cylinder]
manager = fcl.DynamicAABBTreeCollisionManager()
manager.registerObjects(objs)
manager.setup()
crequest = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
cdata = fcl.CollisionData(crequest, fcl.CollisionResult())
manager.collide(cdata, fcl.defaultCollisionCallback)
print(cdata.result.contacts)
for contact in cdata.result.contacts:
print(contact.pos)
print(contact.normal)
print(contact.penetration_depth)
