def min_distance_other(self,
other_manager,
return_names=False,
return_data=False):
"""
Get the minimum distance between any pair of objects, one in each manager.
Parameters
----------
other_manager: CollisionManager, another collision manager object
return_names: bool, If true, a 2-tuple is returned containing
the names of the closest objects.
return_data: bool, If true, a DistanceData object is returned as well
Returns
-------
distance: float, The min distance between a pair of objects,
one from each manager.
names: 2-tup of str, A 2-tuple containing two names (first from this manager,
second from the other_manager) indicating
the two closest objects.
data: DistanceData, Extra data about the distance query
"""
ddata = fcl.DistanceData()
if return_data:
ddata = fcl.DistanceData(
fcl.DistanceRequest(enable_nearest_points=True),
fcl.DistanceResult())
self._manager.distance(other_manager._manager, ddata,
fcl.defaultDistanceCallback)
distance = ddata.result.min_distance
names, data = None, None
if return_names or return_data:
reverse = False
names = (self._extract_name(ddata.result.o1),
other_manager._extract_name(ddata.result.o2))
if names[0] is None:
reverse = True
names = (self._extract_name(ddata.result.o2),
other_manager._extract_name(ddata.result.o1))
dnames = tuple(names)
if reverse:
dnames = reversed(dnames)
data = DistanceData(dnames, ddata.result)
if return_names and return_data:
return distance, names, data
elif return_names:
return distance, names
elif return_data:
return distance, data
else:
return distance
def min_distance_other(self, other_manager, return_names=False):
'''
Get the minimum distance between any pair of objects, one in each manager.
Parameters
----------
other_manager: CollisionManager, another collision manager object
return_names: bool, If true, a 2-tuple is returned containing
the names of the closest objects.
Returns
-------
distance: float, The min distance between a pair of objects,
one from each manager.
names: 2-tup of str, A 2-tuple containing two names (first from this manager,
second from the other_manager) indicating
the two closest objects.
'''
ddata = fcl.DistanceData()
self._manager.distance(other_manager._manager, ddata,
fcl.defaultDistanceCallback)
distance = ddata.result.min_distance
if return_names:
name1, name2 = (self._extract_name(ddata.result.o1),
other_manager._extract_name(ddata.result.o2))
if name1 is None:
name1, name2 = (self._extract_name(ddata.result.o2),
other_manager._extract_name(ddata.result.o1))
return distance, (name1, name2)
else:
return distance
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 min_distance_internal(self, return_names=False):
'''
Get the minimum distance between any pair of objects in the manager.
Parameters
----------
return_names : bool
If true, a 2-tuple is returned containing the names of the closest objects.
Returns
-------
distance: float, Min distance between any two managed objects
names: (2,) str, The names of the closest objects
'''
ddata = fcl.DistanceData()
self._manager.distance(ddata, fcl.defaultDistanceCallback)
distance = ddata.result.min_distance
if return_names:
names = tuple(
sorted((self._extract_name(ddata.result.o1),
self._extract_name(ddata.result.o2))))
return distance, names
else:
return distance
def min_distance_internal(self, return_names=False, return_data=False):
"""
Get the minimum distance between any pair of objects in the manager.
Parameters
-------------
return_names : bool
If true, a 2-tuple is returned containing the names
of the closest objects.
return_data : bool
If true, a DistanceData object is returned as well
Returns
-----------
distance : float
Min distance between any two managed objects
names : (2,) str
The names of the closest objects
data : DistanceData
Extra data about the distance query
"""
ddata = fcl.DistanceData()
if return_data:
ddata = fcl.DistanceData(
fcl.DistanceRequest(enable_nearest_points=True),
fcl.DistanceResult()
)
self._manager.distance(ddata, fcl.defaultDistanceCallback)
distance = ddata.result.min_distance
names, data = None, None
if return_names or return_data:
names = (self._extract_name(ddata.result.o1),
self._extract_name(ddata.result.o2))
data = DistanceData(names, ddata.result)
names = tuple(sorted(names))
if return_names and return_data:
return distance, names, data
elif return_names:
return distance, names
elif return_data:
return distance, data
else:
return distance
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 _distance_check(tube_manager, environment):
"""Checks distance between given collision manager and object list.
Parameters
----------
tube_manager : DynamicAABBTreeCollisionManager
environment : list of CollisionObjects
Returns
-------
float
minimum distance between the two collections of collision objects
"""
env_manager = fcl.DynamicAABBTreeCollisionManager()
env_manager.registerObjects(environment)
env_manager.setup()
data = fcl.DistanceData()
tube_manager.distance(env_manager, data, fcl.defaultDistanceCallback)
return data.result.min_distance
def min_distance_single(self, mesh, transform=None, return_name=False):
'''
Get the minimum distance between a single object and any object 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, return name of the closest object
Returns
-------
distance: float, Min distance between mesh and any object in the manager
name: str, The name of the object in the manager that was closest
'''
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
ddata = fcl.DistanceData()
self._manager.distance(o, ddata, fcl.defaultDistanceCallback)
distance = ddata.result.min_distance
# If we want to return the objects that were collision, collect them.
if return_name:
cg = ddata.result.o1
if cg == b:
cg = ddata.result.o2
name = self._extract_name(cg)
return distance, name
else:
return 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
def min_distance_single(self,
mesh,
transform=None,
return_name=False,
return_data=False):
"""
Get the minimum distance between a single object and any object 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, return name of the closest object
return_data: bool, If true, a DistanceData object is returned as well
Returns
-------
distance: float, Min distance between mesh and any object in the manager
name: str, The name of the object in the manager that was closest
data: DistanceData, Extra data about the distance query
"""
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
ddata = fcl.DistanceData()
if return_data:
ddata = fcl.DistanceData(
fcl.DistanceRequest(enable_nearest_points=True),
fcl.DistanceResult())
self._manager.distance(o, ddata, fcl.defaultDistanceCallback)
distance = ddata.result.min_distance
# If we want to return the objects that were collision, collect them.
name, data = None, None
if return_name or return_data:
cg = ddata.result.o1
if cg == b:
cg = ddata.result.o2
name = self._extract_name(cg)
names = (name, '__external')
if cg == ddata.result.o2:
names = reversed(names)
data = DistanceData(names, ddata.result)
if return_name and return_data:
return distance, name, data
elif return_name:
return distance, name
elif return_data:
return distance, data
else:
return distance
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
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()) == 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
def test_managed_distances(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.DistanceData(self.drequest, fcl.DistanceResult())
manager1.distance(o1, cdata, fcl.defaultDistanceCallback)
self.assertTrue(cdata.result.min_distance < 0)
cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
manager1.distance(o2, cdata, fcl.defaultDistanceCallback)
assert abs(cdata.result.min_distance - 3.6) < 1e-4
cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
manager2.distance(o1, cdata, fcl.defaultDistanceCallback)
self.assertAlmostEqual(cdata.result.min_distance, 4.0)
cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
manager2.distance(o2, cdata, fcl.defaultDistanceCallback)
self.assertTrue(cdata.result.min_distance < 0)
# Many-to-many, internal
cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
manager1.distance(cdata, fcl.defaultDistanceCallback)
self.assertTrue(cdata.result.min_distance < 0)
cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
manager2.distance(cdata, fcl.defaultDistanceCallback)
self.assertAlmostEqual(cdata.result.min_distance, 9.0)
# Many-to-many, grouped
cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
manager1.distance(manager2, cdata, fcl.defaultDistanceCallback)
self.assertAlmostEqual(cdata.result.min_distance, 4.0)
cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
manager2.distance(manager3, cdata, fcl.defaultDistanceCallback)
self.assertTrue(cdata.result.min_distance < 0)
cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
manager1.distance(manager3, cdata, fcl.defaultDistanceCallback)
self.assertTrue(cdata.result.min_distance < 0)
