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 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 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 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 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 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 in_collision_internal(self):
'''
Check if any pair of objects in the manager collide with one another.
Returns
-------
is_collision: bool, True if a collision occured between any pair of objects
and False otherwise
'''
cdata = fcl.CollisionData()
self._manager.collide(cdata, fcl.defaultCollisionCallback)
return cdata.result.is_collision
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 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 in_collision_other(self, other_manager):
'''
Check if any object from this manager collides with any object from another manager.
Parameters
----------
other_manager: CollisionManager, another collision manager object
Returns
-------
is_collision: bool, True if a collision occured between any pair of objects
and False otherwise
'''
cdata = fcl.CollisionData()
self._manager.collide(other_manager._manager, cdata,
fcl.defaultCollisionCallback)
return cdata.result.is_collision
def update(i):
link_plot.set_data(points[i, :, 0], points[i, :, 1])
joint_plot.set_data(points[i, :-1, 0], points[i, :-1, 1])
eff_plot.set_data(points[i, -1:, 0], points[i, -1:, 1])
robot.update_polygons(q[i])
robot_manager.update()
assert len(robot_manager.getObjects()) == 7
rdata = fcl.CollisionData(request=req)
robot_manager.collide(obs_manager, rdata, fcl.defaultCollisionCallback)
in_collision = rdata.result.is_collision
if in_collision:
print('Collision!!')
ax.set_title('In Collision?: {} {}'.format(
in_collision,
robot_manager.getObjects()[3].getTranslation()))
return link_plot, joint_plot, eff_plot
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 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
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
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)
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)
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))
