def in_collision_other(self, other_manager,
return_names=False, return_data=False):
"""
Check if any object from this manager collides with any object from another manager.
Parameters
----------
other_manager: CollisionManager, another collision manager object
return_names: bool, If true, a set is returned containing the names
of all pairs of objects in collision.
return_data: bool, If true, a list of ContactData is returned as well
Returns
-------
is_collision: bool, True if a collision occurred between any pair of objects
and False otherwise
names: set of 2-tup, The set of pairwise collisions. Each tuple
contains two names (first from this manager,
second from the other_manager) indicating
that the two corresponding objects are in collision.
contacts: list of ContactData, All contacts detected
"""
cdata = fcl.CollisionData()
if return_names or return_data:
cdata = fcl.CollisionData(
request=fcl.CollisionRequest(
num_max_contacts=100000,
enable_contact=True))
self._manager.collide(other_manager._manager,
cdata,
fcl.defaultCollisionCallback)
result = cdata.result.is_collision
objs_in_collision = set()
contact_data = []
if return_names or return_data:
for contact in cdata.result.contacts:
reverse = False
names = (self._extract_name(contact.o1),
other_manager._extract_name(contact.o2))
if names[0] is None:
names = (self._extract_name(contact.o2),
other_manager._extract_name(contact.o1))
reverse = True
if return_names:
objs_in_collision.add(names)
if return_data:
if reverse:
names = reversed(names)
contact_data.append(ContactData(names, contact))
if return_names and return_data:
return result, objs_in_collision, contact_data
elif return_names:
return result, objs_in_collision
elif return_data:
return result, contact_data
else:
return result
def isValid(self, state):
# print("collision")
sample = np.array([state[0], state[1], 0])
# sample = np.array([state().getX(), state().getY(), state().getYaw()])
req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
rdata = fcl.CollisionData(request = req)
cyl = fcl.Cylinder(0.01, 2)
t = fcl.Transform(sample)
agent = fcl.CollisionObject(cyl, t)
self.fcl_manager.collide(agent, rdata, fcl.defaultCollisionCallback)
# if(rdata.result.is_collision):
# print("state: ", sample, " collision: ", rdata.result.is_collision)
# print ('Collision between manager 1 and agent?: {}'.format(rdata.result.is_collision))
# print( 'Contacts:')
# for c in rdata.result.contacts:
# print( '\tO1: {}, O2: {}'.format(c.o1, c.o2))
self.count += 1
if(rdata.result.is_collision):
self.collision_count += 1
# self.states_bad.append(sample)
# else:
# self.states_ok.append(sample)
# return not rdata.result.is_collision
return True
def predict(self, X, distance=True):
labels = torch.FloatTensor(len(X), len(self.obs_managers))
dists = torch.FloatTensor(len(X), len(
self.obs_managers)) if distance else None
req = fcl.CollisionRequest(num_max_contacts=1000 if distance else 1,
enable_contact=distance)
for i, cfg in enumerate(X):
self.robot.update_polygons(cfg)
self.robot_manager.update()
assert len(self.robot_manager.getObjects()) == self.robot.dof
for cat, obs_mng in enumerate(self.obs_managers):
rdata = fcl.CollisionData(request=req)
self.robot_manager.collide(obs_mng, rdata,
fcl.defaultCollisionCallback)
in_collision = rdata.result.is_collision
labels[i, cat] = 1 if in_collision else -1
if distance:
ddata = fcl.DistanceData()
self.robot_manager.distance(obs_mng, ddata,
fcl.defaultDistanceCallback)
depths = torch.FloatTensor(
[c.penetration_depth for c in rdata.result.contacts])
dists[i, cat] = depths.abs().max(
) if in_collision else -ddata.result.min_distance
if distance:
return labels, dists
else:
return labels
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 in_collision_internal(self, return_names=False, return_data=False):
"""
Check if any pair of objects in the manager collide with one another.
Parameters
----------
return_names : bool
If true, a set is returned containing the names
of all pairs of objects in collision.
return_data : bool
If true, a list of ContactData is returned as well
Returns
-------
is_collision : bool
True if a collision occurred between any pair of objects
and False otherwise
names : set of 2-tup
The set of pairwise collisions. Each tuple
contains two names in alphabetical order indicating
that the two corresponding objects are in collision.
contacts : list of ContactData
All contacts detected
"""
cdata = fcl.CollisionData()
if return_names or return_data:
cdata = fcl.CollisionData(request=fcl.CollisionRequest(
num_max_contacts=100000, enable_contact=True))
self._manager.collide(cdata, fcl.defaultCollisionCallback)
result = cdata.result.is_collision
objs_in_collision = set()
contact_data = []
if return_names or return_data:
for contact in cdata.result.contacts:
names = (self._extract_name(contact.o1),
self._extract_name(contact.o2))
if return_names:
objs_in_collision.add(tuple(sorted(names)))
if return_data:
contact_data.append(ContactData(names, contact))
if return_names and return_data:
return result, objs_in_collision, contact_data
elif return_names:
return result, objs_in_collision
elif return_data:
return result, contact_data
else:
return result
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 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 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 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 isValidPoint(self, s_q):
# check if the sampled point is inside the world"
if not (self.x_min[0] < s_q[0] < self.x_max[0]
and self.x_min[1] < s_q[1] < self.x_max[1]
and self.v_min[0] < s_q[2] < self.v_max[0]
and self.v_min[1] < s_q[3] < self.v_max[1]):
return False
# 一对many
fclPoint = self.point2fcl(s_q)
req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
rdata = fcl.CollisionData(request=req)
self.fcl_manager.collide(fclPoint, rdata, fcl.defaultCollisionCallback)
# print('Collision between manager 1 and Mesh?: {}'.format(rdata.result.is_collision))
# print('Contacts:')
# for c in rdata.result.contacts:
# print('\tO1: {}, O2: {}'.format(c.o1, c.o2))
return not rdata.result.is_collision
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 in_collision_other(self, other_manager, return_names=False):
'''
Check if any object from this manager collides with any object from another manager.
Parameters
----------
other_manager: CollisionManager, another collision manager object
return_names: bool, If true, a set is returned containing the names
of all pairs of objects in collision.
Returns
-------
is_collision: bool, True if a collision occured between any pair of objects
and False otherwise
names: set of 2-tup, The set of pairwise collisions. Each tuple
contains two names (first from this manager,
second from the other_manager) indicating
that the two correspoinding objects are in collision.
'''
cdata = fcl.CollisionData()
if return_names:
cdata = fcl.CollisionData(request=fcl.CollisionRequest(num_max_contacts=100000,
enable_contact=True))
self._manager.collide(other_manager._manager,
cdata,
fcl.defaultCollisionCallback)
result = cdata.result.is_collision
if return_names:
objs_in_collision = set()
for contact in cdata.result.contacts:
name1, name2 = self._extract_name(
contact.o1), other_manager._extract_name(contact.o2)
if name1 is None:
name1, name2 = self._extract_name(
contact.o2), other_manager._extract_name(contact.o1)
objs_in_collision.add((name1, name2))
return result, objs_in_collision
else:
return result
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]])
def in_collision_internal(self, return_names=False):
'''
Check if any pair of objects in the manager collide with one another.
Parameters
----------
return_names : bool
If true, a set is returned containing the names
of all pairs of objects in collision.
Returns
-------
is_collision: bool, True if a collision occured between any pair of objects
and False otherwise
names: set of 2-tup, The set of pairwise collisions. Each tuple
contains two names in alphabetical order indicating
that the two correspoinding objects are in collision.
'''
cdata = fcl.CollisionData()
if return_names:
cdata = fcl.CollisionData(request=fcl.CollisionRequest(
num_max_contacts=100000, enable_contact=True))
self._manager.collide(cdata, fcl.defaultCollisionCallback)
result = cdata.result.is_collision
if return_names:
objs_in_collision = set()
for contact in cdata.result.contacts:
name1, name2 = (self._extract_name(contact.o1),
self._extract_name(contact.o2))
names = tuple(sorted((name1, name2)))
objs_in_collision.add(names)
return result, objs_in_collision
else:
return result
def isValid(self, q_set):
# check validity for multiple points.
# will be used for piecewize path consited of multiple points
# for q in q_set:
# if not self.isValidPoint(q):
# return False
# return True
if len(q_set.shape) < 2:
return self.isValidPoint(q_set)
manager_q = fcl.DynamicAABBTreeCollisionManager()
qs = []
for q in q_set:
qs.append(self.point2fcl(q))
manager_q.registerObjects(qs)
req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
rdata = fcl.CollisionData(request=req)
self.fcl_manager.collide(manager_q, rdata,
fcl.defaultCollisionCallback)
# print('Collision between manager 1 and Mesh?: {}'.format(rdata.result.is_collision))
# print('Contacts:')
# for c in rdata.result.contacts:
# print('\tO1: {}, O2: {}'.format(c.o1, c.o2))
return not rdata.result.is_collision
def in_collision_single(self,
mesh,
transform=None,
return_names=False,
return_data=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
return_names: bool, If true, a set is returned containing the names
of all objects in collision with the object
return_data: bool, If true, a list of ContactData is returned as well
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
contacts: list of ContactData, All contacts detected
"""
if transform is None:
transform = np.eye(4)
# Create FCL data
b = self._get_BVH(mesh)
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 or return_data:
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.
objs_in_collision = set()
contact_data = []
if return_names or return_data:
for contact in cdata.result.contacts:
cg = contact.o1
if cg == b:
cg = contact.o2
name = self._extract_name(cg)
names = (name, '__external')
if cg == contact.o2:
names = reversed(names)
if return_names:
objs_in_collision.add(name)
if return_data:
contact_data.append(ContactData(names, contact))
if return_names and return_data:
return result, objs_in_collision, contact_data
elif return_names:
return result, objs_in_collision
elif return_data:
return result, contact_data
else:
return result
sys.path.append(os.environ['CODE_BASE'] + '/catkin_ws/src/config/src')
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):
def generate_one(robot,
obs_num,
folder,
label_type='binary',
num_class=None,
num_points=8000,
env_id='',
vis=True):
obstacles = []
types = ['rect', 'circle']
link_length = robot.link_length[0].item()
for i in range(obs_num):
obs_t = types[randint(2)]
if types[0] in obs_t: # rectangle, size = 0.5-3.5, pos = -7~7
while True:
s = rand(2) * 3 + 0.5
if obs_num <= 2:
p = rand(2) * 10 - 5
else:
p = rand(2) * 14 - 7
if any(p - s / 2 > link_length) or any(
p +
s / 2 < -link_length): # the near-origin area is clear
break
elif types[1] in obs_t: # circle, size = 0.25-2, pos = -7~7
while True:
s = rand() * 1.75 + 0.25
if obs_num <= 2:
p = rand(2) * 10 - 5
else:
p = rand(2) * 14 - 7
if np.linalg.norm(p) > s + link_length:
break
obstacles.append((obs_t, p, s))
fcl_obs = [FCLObstacle(*param) for param in obstacles]
fcl_collision_obj = [fobs.cobj for fobs in fcl_obs]
# geom2instnum = {id(g): i for i, (_, g) in enumerate(fcl_obs)}
cfgs = torch.rand((num_points, robot.dof), dtype=torch.float32)
cfgs = cfgs * (robot.limits[:, 1] - robot.limits[:, 0]) + robot.limits[:,
0]
if label_type == 'binary':
labels = torch.zeros(num_points, 1, dtype=torch.float)
dists = torch.zeros(num_points, 1, dtype=torch.float)
obs_managers = [fcl.DynamicAABBTreeCollisionManager()]
obs_managers[0].registerObjects(fcl_collision_obj)
obs_managers[0].setup()
elif label_type == 'instance':
labels = torch.zeros(num_points, len(obstacles), dtype=torch.float)
dists = torch.zeros(num_points, len(obstacles), dtype=torch.float)
obs_managers = [fcl.DynamicAABBTreeCollisionManager() for _ in fcl_obs]
for mng, cobj in zip(obs_managers, fcl_collision_obj):
mng.registerObjects([cobj])
elif label_type == 'class':
labels = torch.zeros(num_points, num_class, dtype=torch.float)
dists = torch.zeros(num_points, num_class, dtype=torch.float)
obs_managers = [
fcl.DynamicAABBTreeCollisionManager() for _ in range(num_class)
]
obj_by_cls = [[] for _ in range(num_class)]
for obj in fcl_obs:
obj_by_cls[obj.category].append(obj.cobj)
for mng, obj_group in zip(obs_managers, obj_by_cls):
mng.registerObjects(obj_group)
robot_links = robot.update_polygons(cfgs[0])
robot_manager = fcl.DynamicAABBTreeCollisionManager()
robot_manager.registerObjects(robot_links)
robot_manager.setup()
for mng in obs_managers:
mng.setup()
req = fcl.CollisionRequest(num_max_contacts=1000, enable_contact=True)
times = []
st = time()
for i, cfg in enumerate(cfgs):
st1 = time()
robot.update_polygons(cfg)
robot_manager.update()
assert len(robot_manager.getObjects()) == robot.dof
for cat, obs_mng in enumerate(obs_managers):
rdata = fcl.CollisionData(request=req)
robot_manager.collide(obs_mng, rdata, fcl.defaultCollisionCallback)
in_collision = rdata.result.is_collision
ddata = fcl.DistanceData()
robot_manager.distance(obs_mng, ddata, fcl.defaultDistanceCallback)
depths = torch.FloatTensor(
[c.penetration_depth for c in rdata.result.contacts])
labels[i, cat] = 1 if in_collision else -1
dists[i, cat] = depths.abs().max(
) if in_collision else -ddata.result.min_distance
end1 = time()
times.append(end1 - st1)
end = time()
times = np.array(times)
print('std: {}, mean {}, avg {}'.format(times.std(), times.mean(),
(end - st) / len(cfgs)))
in_collision = (labels == 1).sum(1) > 0
if label_type == 'binary':
labels = labels.squeeze_(1)
dists = dists.squeeze_(1)
print('env_id {}, {} collisons, {} free'.format(
env_id, torch.sum(in_collision == 1), torch.sum(in_collision == 0)))
if torch.sum(in_collision == 1) == 0:
print('0 Collision. You may want to regenerate env {}obs{}'.format(
obs_num, env_id))
dataset = {
'data': cfgs,
'label': labels,
'dist': dists,
'obs': obstacles,
'robot': robot.__class__,
'rparam': [
robot.link_length,
robot.link_width,
robot.dof,
]
}
torch.save(
dataset,
os.path.join(
folder, '2d_{}dof_{}obs_{}_{}.pt'.format(robot.dof, obs_num,
label_type, env_id)))
if vis:
create_plots(robot, obstacles, cfg=None)
plt.savefig(
os.path.join(
folder,
'2d_{}dof_{}obs_{}_{}.png'.format(robot.dof, obs_num,
label_type, env_id)))
plt.close()
return
# [0.0, 0.0, 1.0]])
R = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])
T = np.array([1.0, 1.865, 0])
g1 = fcl.Box(1, 2, 3)
t1 = fcl.Transform()
o1 = fcl.CollisionObject(g1, t1)
# g2 = fcl.Cone(1,3)
g2 = fcl.Cylinder(0.01, 1000)
t2 = fcl.Transform()
o2 = fcl.CollisionObject(g2, t2)
# request = fcl.DistanceRequest(gjk_solver_type=fcl.GJKSolverType.GST_INDEP)
# result = fcl.DistanceResult()
request = fcl.CollisionRequest(enable_contact=True)
result = fcl.CollisionResult()
# ret = fcl.distance(o1, o2, request, result)
# ret = fcl.collide(o1, o2, request, result)
size = 50, 50
yy, xx = torch.meshgrid(torch.linspace(-5, 5, size[0]),
torch.linspace(-5, 5, size[1]))
grid_points = torch.stack([xx, yy], axis=2).reshape((-1, 2))
grid_labels = torch.zeros_like(grid_points)[:, 0]
for i, (x, y) in enumerate(grid_points):
print(x, y)
o2.setTranslation([x, y, 0])
fcl.update()
ret = fcl.collide(o1, o2, request, result)
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
# # Managed internal (sub-n^2) distance checking
# #=====================================================================
# ddata = fcl.DistanceData()
# manager1.distance(ddata, fcl.defaultDistanceCallback)
# print('Closest distance within manager 1?: {}'.format(ddata.result.min_distance))
#=====================================================================
# Managed one to many collision checking
#=====================================================================
# req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
# rdata = fcl.CollisionData(request = req)
# manager1.collide(fcl.CollisionObject(m), rdata, fcl.defaultCollisionCallback)
# print('Collision between manager 1 and Mesh?: {}'.format(rdata.result.is_collision))
# print('Contacts:')
# for c in rdata.result.contacts:
# print('\tO1: {}, O2: {}, depth: {}'.format(c.o1, c.o2, c.penetration_depth))
#=====================================================================
# Managed many to many collision checking
#=====================================================================
req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
rdata = fcl.CollisionData(request=req)
manager1.collide(manager2, rdata, fcl.defaultCollisionCallback)
print('Collision between manager 1 and manager 2?: {}'.format(
rdata.result.is_collision))
print('Contacts:')
for c in rdata.result.contacts:
print('\tO1: {}, O2: {}, depth: {}, pos: {}, normal: {}'.format(
c.o1, c.o2, c.penetration_depth, c.pos, c.normal))
print(1 - 2 / np.sqrt(5))
