def in_collision_single(self, mesh, transform=None):
'''
Check a single object for collisions against all objects in the manager.
Parameters
----------
mesh: Trimesh object, the geometry of the collision object
transform: (4,4) float, homogenous transform matrix for the object
Returns
-------
is_collision: bool, True if a collision occurs and False otherwise
'''
if transform is None:
transform = np.eye(4)
# Create FCL data
b = fcl.BVHModel()
b.beginModel(len(mesh.vertices), len(mesh.faces))
b.addSubModel(mesh.vertices, mesh.faces)
b.endModel()
t = fcl.Transform(transform[:3, :3], transform[:3, 3])
o = fcl.CollisionObject(b, t)
# Collide with manager's objects
cdata = fcl.CollisionData()
self._manager.collide(o, cdata, fcl.defaultCollisionCallback)
return cdata.result.is_collision
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 add_object(self, name, mesh, transform=None):
'''
Add an object to the collision manager.
If an object with the given name is already in the manager, replace it.
Parameters
----------
name: str, an identifier for the object
mesh: Trimesh object, the geometry of the collision object
transform: (4,4) float, homogenous transform matrix for the object
'''
if transform is None:
transform = np.eye(4)
# Create FCL data
b = fcl.BVHModel()
b.beginModel(len(mesh.vertices), len(mesh.faces))
b.addSubModel(mesh.vertices, mesh.faces)
b.endModel()
t = fcl.Transform(transform[:3, :3], transform[:3, 3])
o = fcl.CollisionObject(b, t)
# Add collision object to set
if name in self._objs:
self._manager.unregisterObject(self._objs[name])
self._objs[name] = {'obj': o, 'geom': b}
self._manager.registerObject(o)
self._manager.update()
def in_collision_single(self, mesh, transform=None, return_names=False):
'''
Check a single object for collisions against all objects in the manager.
Parameters
----------
mesh: Trimesh object, the geometry of the collision object
transform: (4,4) float, homogenous transform matrix for the object
return_names : bool, If true, a set is returned containing the names
of all objects in collision with the provided object
Returns
-------
is_collision: bool, True if a collision occurs and False otherwise
names: set of str, The set of names of objects that collided with the provided one
'''
if transform is None:
transform = np.eye(4)
# Create FCL data
b = fcl.BVHModel()
b.beginModel(len(mesh.vertices), len(mesh.faces))
b.addSubModel(mesh.vertices, mesh.faces)
b.endModel()
t = fcl.Transform(transform[:3, :3], transform[:3, 3])
o = fcl.CollisionObject(b, t)
# Collide with manager's objects
cdata = fcl.CollisionData()
if return_names:
cdata = fcl.CollisionData(request=fcl.CollisionRequest(
num_max_contacts=100000, enable_contact=True))
self._manager.collide(o, cdata, fcl.defaultCollisionCallback)
result = cdata.result.is_collision
# If we want to return the objects that were collision, collect them.
if return_names:
objs_in_collision = set()
for contact in cdata.result.contacts:
cg = contact.o1
if cg == b:
cg = contact.o2
name = self._extract_name(cg)
objs_in_collision.add(name)
return result, objs_in_collision
else:
return result
def mesh_to_BVH(mesh):
"""
Create a BVHModel object from a Trimesh object
Parameters
-----------
mesh: Trimesh object
Returns
------------
bvh: fcl.BVHModel object
"""
bvh = fcl.BVHModel()
bvh.beginModel(num_tris_=len(mesh.faces), num_vertices_=len(mesh.vertices))
bvh.addSubModel(verts=mesh.vertices, triangles=mesh.faces)
bvh.endModel()
return bvh
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 __init__(self, collision_dict):
Collision_Object.__init__(self, collision_dict)
if not len(self.params) == 2:
raise TypeError(bc.FAIL + 'ERROR: parameters for collision mesh must be a dictionary consisting of a list of verts and tris.' + bc.ENDC)
if not len(self.params['verts']) == len(self.params['tris']):
raise TypeError(bc.FAIL + 'ERROR: number of tris must equal the number of verts in collision mesh.' + bc.ENDC)
self.verts = np.array(self.params['verts'])
self.tris = np.array(self.params['tris'])
self.g = fcl.BVHModel()
self.g.beginModel(len(self.verts), len(self.tris))
self.g.addSubModel(self.verts, self.tris)
self.g.endModel()
self.t = fcl.Transform()
self.obj = fcl.CollisionObject(self.g, self.t)
self.make_rviz_marker()
def parse_json(object_type, init_objects_file):
f = init_objects_file.open()
full_json = json.load(f)
f.close()
list_objects = full_json.get(object_type)
collision_objects = []
for o in list_objects:
if o.get('mesh'):
m = fcl.BVHModel()
path = init_objects_file.parent
file = path / o.get('filename')
vertices, tris = parse_mesh(str(file))
m.beginModel(len(vertices), len(tris))
m.addSubModel(vertices, tris)
m.endModel()
this_transform = fcl.Transform(o.get('transform'))
obj = fcl.CollisionObject(m, this_transform)
else:
if o.get('shape') == 'Sphere':
this_obj = fcl.Sphere(o.get('radius'))
this_transform = fcl.Transform(o.get('transform'))
elif o.get('shape') == 'Cylinder':
this_obj = fcl.Cylinder(o.get('radius'), o.get('length'))
direction = o.get('direction')
rot = rotate_align(np.asarray(direction))
this_transform = fcl.Transform(rot, o.get('transform'))
elif o.get('shape') == 'Box':
this_obj = fcl.Box(o.get('x_length'), o.get('y_length'),
o.get('z_length'))
this_transform = fcl.Transform(o.get('transform'))
else:
raise NotImplementedError(
f'Shape type {o.get("shape")} not supported.')
obj = fcl.CollisionObject(this_obj, this_transform)
collision_objects.append(obj)
return collision_objects
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
def setUp(self):
verts = np.array([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0],
[0.0, 0.0, 1.0]])
tris = np.array([[0, 2, 1], [0, 3, 2], [0, 1, 3], [1, 2, 3]])
mesh = fcl.BVHModel()
mesh.beginModel(len(verts), len(tris))
mesh.addSubModel(verts, tris)
mesh.endModel()
self.geometry = {
'box': fcl.Box(1.0, 1.0, 1.0),
'sphere': fcl.Sphere(1.0),
'cone': fcl.Cone(1.0, 1.0),
'cylinder': fcl.Cylinder(1.0, 1.0),
'mesh': mesh
}
self.crequest = fcl.CollisionRequest(num_max_contacts=100,
enable_contact=True)
self.drequest = fcl.DistanceRequest(enable_nearest_points=True)
self.x_axis_rot = np.array([[1.0, 0.0, 0.0], [0.0, 0.0, -1.0],
[0.0, 1.0, 0.0]])
t = fcl.TriangleP(v1, v2, v3) # Triangle defined by three points
b = fcl.Box(x, y, z) # Axis-aligned box with given side lengths
s = fcl.Sphere(rad) # Sphere with given radius
e = fcl.Ellipsoid(x, y, z) # Axis-aligned ellipsoid with given radii
# c = fcl.Capsule(rad, lz) # Capsule with given radius and height along z-axis
# c = fcl.Cone(rad, lz) # Cone with given radius and cylinder height along z-axis
c = fcl.Cylinder(rad, lz) # Cylinder with given radius and height along z-axis
h = fcl.Halfspace(n, d) # Half-space defined by {x : <n, x> < d}
p = fcl.Plane(n, d) # Plane defined by {x : <n, x> = d}
verts = np.array([[1.0, 1.0, 1.0], [2.0, 1.0, 1.0], [1.0, 2.0, 1.0],
[1.0, 1.0, 2.0]])
tris = np.array([[0, 2, 1], [0, 3, 2], [0, 1, 3], [1, 2, 3]])
m = fcl.BVHModel()
m.beginModel(len(verts), len(tris))
m.addSubModel(verts, tris)
m.endModel()
objs1 = [
fcl.CollisionObject(
b,
fcl.Transform(
Rotation.from_euler(
'XYZ', [0, 0, np.arccos(2.0 / 3) - np.arctan(0.5)]).as_quat()[[
3, 0, 1, 2
]], [1.5, 0, 0]))
] #, fcl.CollisionObject(s)] np.arccos(2.0/3) Rotation.from_euler('XYZ', [0, 0, np.pi/2]).as_quat()[[3,0,1,2]], [1.5, 0, 0]
print(Rotation.from_rotvec([0, 0, np.arccos(2.0 / 3)]).as_quat()[[3, 0, 1, 2]])
objs2 = [fcl.CollisionObject(c)] #, fcl.CollisionObject(m)]
print(result.nearest_points[1])
print()
# Create simple geometries
box = fcl.Box(1.0, 2.0, 3.0)
sphere = fcl.Sphere(4.0)
cone = fcl.Cone(5.0, 6.0)
cyl = fcl.Cylinder(2.0, 2.0)
verts = np.array([[1.0, 1.0, 1.0], [2.0, 1.0, 1.0], [1.0, 2.0, 1.0],
[1.0, 1.0, 2.0]])
tris = np.array([[0, 2, 1], [0, 3, 2], [0, 1, 3], [1, 2, 3]])
# Create mesh geometry
mesh = fcl.BVHModel()
mesh.beginModel(len(verts), len(tris))
mesh.addSubModel(verts, tris)
mesh.endModel()
#=====================================================================
# Pairwise collision checking
#=====================================================================
print('=' * 60)
print('Testing Pairwise Collision Checking')
print('=' * 60)
print()
req = fcl.CollisionRequest(enable_contact=True)
res = fcl.CollisionResult()
