class Collision_Object_Container:
def __init__(self, yaml_path):
self.collision_objects = []
f = open(yaml_path)
y = yaml.load(f)
keys = y.keys()
for k in keys:
if not y[k] == None:
if k == 'robot_link_radius' or k == 'sample_states' or k == 'training_states' or k == 'problem_states':
continue
for i in range(len(y[k])):
if k == 'boxes':
self.collision_objects.append(Collision_Box(y[k][i]))
elif k == 'spheres':
self.collision_objects.append(Collision_Sphere(
y[k][i]))
elif k == 'ellipsoids':
self.collision_objects.append(
Collision_Ellipsoid(y[k][i]))
elif k == 'capsules':
self.collision_objects.append(
Collision_Capsule(y[k][i]))
elif k == 'cones':
self.collision_objects.append(Collision_Cone(y[k][i]))
elif k == 'cylinders':
self.collision_objects.append(
Collision_Cylinder(y[k][i]))
elif k == 'meshes':
self.collision_objects.append(Collision_Mesh(y[k][i]))
self.collision_objects[-1].type = 'environment_object'
self.robot_link_radius = 0.05
if 'robot_link_radius' in keys:
self.robot_link_radius = float(y['robot_link_radius'])
if 'sample_states' in keys:
self.sample_states = y['sample_states']
else:
raise Exception(
'Must specify at least one sample state in collision yaml file!'
)
self.set_rviz_ids()
def set_rviz_ids(self):
for i, c in enumerate(self.collision_objects):
c.marker.id = i
def get_min_distance(self, (a, b)):
obja = self.collision_objects[a].obj
objb = self.collision_objects[b].obj
self.request = fcl.DistanceRequest()
self.result = fcl.DistanceResult()
ret = fcl.distance(obja, objb, self.request, self.result)
return self.result.min_distance
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_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 test_pairwise_distances(self):
result = fcl.DistanceResult()
box = fcl.CollisionObject(self.geometry['box'])
cone = fcl.CollisionObject(self.geometry['cone'])
mesh = fcl.CollisionObject(self.geometry['mesh'])
result = fcl.DistanceResult()
ret = fcl.distance(box, cone, self.drequest, result)
self.assertTrue(ret < 0)
result = fcl.DistanceResult()
ret = fcl.distance(box, mesh, self.drequest, result)
self.assertTrue(ret < 0)
result = fcl.DistanceResult()
ret = fcl.distance(cone, mesh, self.drequest, result)
self.assertTrue(ret < 0)
cone.setTranslation(np.array([0.0, 0.0, -0.6]))
result = fcl.DistanceResult()
ret = fcl.distance(box, cone, self.drequest, result)
self.assertTrue(ret < 0)
result = fcl.DistanceResult()
ret = fcl.distance(cone, mesh, self.drequest, result)
self.assertAlmostEqual(ret, 0.1)
cone.setTranslation(np.array([0.0, -0.9, 0.0]))
cone.setRotation(self.x_axis_rot)
result = fcl.DistanceResult()
ret = fcl.distance(box, cone, self.drequest, result)
self.assertTrue(ret < 0)
cone.setTranslation(np.array([0.0, -1.1, 0.0]))
result = fcl.DistanceResult()
ret = fcl.distance(box, cone, self.drequest, result)
self.assertAlmostEqual(ret, 0.1)
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 check_collision(self, curve, rad):
"""Determine if the curve given collides with obstacles or goal.
The curve is translated to discretized cylinders with the given radius
and checked for collisions with the obstacles and goal. Minimum distance
to obstacles and tip distance to goal are returned, with a negative
value denoting a collision.
Parameters
----------
curve : list of list of 4x4 numpy arrays
The SE3 g values for each curve
rad : list of float
radii of the tubes
Returns
-------
float
minimum distance between curve and obstacles
float
minimum distance between curve and goal
"""
tube = self._build_tube(curve, rad)
tube_manager = fcl.DynamicAABBTreeCollisionManager()
tube_manager.registerObjects(tube)
tube_manager.setup()
obstacle_min = self._distance_check(tube_manager, self.obstacles)
s = fcl.Sphere(rad[-1])
final_point = curve[-1][-1][0:3, 3]
t = fcl.Transform(final_point) # coordinates of last point of tube
tip = fcl.CollisionObject(s, t)
request = fcl.DistanceRequest()
result = fcl.DistanceResult()
goal_dist = fcl.distance(tip, self.goal, request, result)
return obstacle_min, goal_dist
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 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)
n_contacts = fcl.collide(
fcl.CollisionObject(mesh, fcl.Transform(np.array([0.0, 0.0, -1.0]))),
fcl.CollisionObject(cyl, fcl.Transform()), req, res)
print_collision_result('Box', 'Mesh', res)
#=====================================================================
# Pairwise distance checking
#=====================================================================
print('=' * 60)
print('Testing Pairwise Distance Checking')
print('=' * 60)
print()
req = fcl.DistanceRequest(enable_nearest_points=True)
res = fcl.DistanceResult()
dist = fcl.distance(fcl.CollisionObject(box, fcl.Transform()),
fcl.CollisionObject(cone, fcl.Transform()), req, res)
print_distance_result('Box', 'Cone', res)
dist = fcl.distance(
fcl.CollisionObject(box, fcl.Transform()),
fcl.CollisionObject(cyl, fcl.Transform(np.array([6.0, 0.0, 0.0]))), req,
res)
print_distance_result('Box', 'Cylinder', res)
dist = fcl.distance(
fcl.CollisionObject(box, fcl.Transform()),
fcl.CollisionObject(box, fcl.Transform(np.array([1.01, 0.0, 0.0]))), req,
res)
