def update_polygons(self, q):
joints = self.fkine(q)[0]
joints = torch.cat([torch.zeros(1, 2, dtype=joints.dtype), joints],
dim=0)
centers = (joints[:-1] + joints[1:]) / 2
q = torch.reshape(q, (-1, self.dof))
angles = torch.cumsum(q, dim=1)[0]
if self.collision_objs is None:
self.collision_objs = []
for trans, angle, l in zip(centers, angles, self.link_length):
obj = fcl.Box(l, self.link_width, 1000)
self.collision_objs.append(
fcl.CollisionObject(
obj,
fcl.Transform(
Rotation.from_rotvec([0, 0, angle
]).as_quat()[[3, 0, 1, 2]],
[trans[0], trans[1], 0])))
else:
for obj, trans, angle, l in zip(self.collision_objs, centers,
angles, self.link_length):
obj.setTransform(
fcl.Transform(
Rotation.from_rotvec([0, 0,
angle]).as_quat()[[3, 0, 1, 2]],
[trans[0], trans[1], 0]))
# self.collsion_objs.append(fcl.CollisionObject(obj, fcl.Transform(
# tgm.angle_axis_to_quaternion(torch.FloatTensor([0, 0, angle])),
# [point[0], point[1], 0])))
return self.collision_objs
def is_in_collision(self, tf, other, tf_other):
""" Collision checking with another shape for the given transforms. """
fcl_tf_1 = fcl.Transform(tf[:3, :3], tf[:3, 3])
fcl_tf_2 = fcl.Transform(tf_other[:3, :3], tf_other[:3, 3])
o1 = fcl.CollisionObject(self.fcl_shape, fcl_tf_1)
o2 = fcl.CollisionObject(other.fcl_shape, fcl_tf_2)
return fcl.collide(o1, o2, self.request, self.result)
def test_pairwise_continuous_collisions(self):
request = fcl.ContinuousCollisionRequest()
result = fcl.ContinuousCollisionResult()
box = fcl.CollisionObject(self.geometry['box'])
cone = fcl.CollisionObject(self.geometry['cone'],
fcl.Transform(np.array([0.0, 0.0, -2.0])))
ret = fcl.continuousCollide(box, fcl.Transform(), cone,
fcl.Transform(), request, 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 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 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 collision_check(self, x, y):
t2 = fcl.Transform(np.array([x, y, 0]))
o2 = fcl.CollisionObject(self.g2, t2)
request = fcl.ContinuousCollisionRequest()
result = fcl.ContinuousCollisionResult()
ret = fcl.continuousCollide(self.o1, self.t1, o2, t2, request, result)
return ret == 0.0
def __init__(self):
self.check_collision = True
self.ang_comp = False
self.ang_comp2 = False
self.lin_comp = False
self.init = False
self.init_arm = False
self.angG = 0
self.ang = 0
self.send_g = True
self.status = 0
self.goal_id = 0
self.ret = np.zeros(7)
self.obj = [
fcl.CollisionObject(fcl.Sphere(0.5), fcl.Transform())
for i in range(7)
]
self.quaternion = np.zeros((8, 4))
self.translation = np.zeros((8, 3))
for i in range(8):
self.quaternion[i] = np.array([0, 0, 0, 0])
self.translation[i] = np.array([0, 0, 0])
self.quaternionG = np.zeros((11, 4))
self.translationG = np.zeros((11, 3))
for i in range(10):
self.quaternionG[i] = np.array([0, 0, 0, 0])
self.translationG[i] = np.array([0, 0, 0])
self.set_goal()
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 is_path_in_collision(self, tf, tf_target, other, tf_other):
# assert np.sum(np.abs(tf - tf_target)) > 1e-12
fcl_tf_1 = fcl.Transform(tf[:3, :3], tf[:3, 3])
fcl_tf_2 = fcl.Transform(tf_other[:3, :3], tf_other[:3, 3])
fcl_tf_1_target = fcl.Transform(tf_target[:3, :3], tf_target[:3, 3])
o1 = fcl.CollisionObject(self.fcl_shape, fcl_tf_1)
o2 = fcl.CollisionObject(other.fcl_shape, fcl_tf_2)
self.c_req.ccd_motion_type = fcl.CCDMotionType.CCDM_LINEAR
fcl.continuousCollide(o1, fcl_tf_1_target, o2, fcl_tf_2, self.c_req,
self.c_res)
return self.c_res.is_collide
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 __init__(self, collision_dict):
Collision_Object.__init__(self, collision_dict)
if not type(self.params) == float:
raise TypeError(bc.FAIL + 'ERROR: parameters for collision sphere must be a float value (for radius).' + bc.ENDC)
self.r = self.params
self.g = fcl.Sphere(self.r)
self.t = fcl.Transform()
self.obj = fcl.CollisionObject(self.g, self.t)
self.make_rviz_marker()
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 cylinder must be list of 2 floats.' + bc.ENDC)
self.r, self.lz = self.params[0], self.params[1]
self.g = fcl.Cylinder(self.r, self.lz)
self.t = fcl.Transform()
self.obj = fcl.CollisionObject(self.g, self.t)
self.make_rviz_marker()
def __init__(self, collision_dict):
Collision_Object.__init__(self, collision_dict)
if not len(self.params) == 3:
raise TypeError(bc.FAIL + 'ERROR: parameters for collision ellipsoid must be list of 3 floats.' + bc.ENDC)
self.x, self.y, self.z = self.params[0], self.params[1], self.params[2]
self.g = fcl.Ellipsoid(self.x, self.y, self.z)
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 update_transform(self, translation, rotation):
if len(translation) == 1:
translation = translation[0]
self.t = fcl.Transform(rotation, translation)
self.obj.setTransform(self.t)
self.marker.pose.position.x = translation[0]
self.marker.pose.position.y = translation[1]
self.marker.pose.position.z = translation[2]
self.marker.pose.orientation.w = rotation[0]
self.marker.pose.orientation.x = rotation[1]
self.marker.pose.orientation.y = rotation[2]
self.marker.pose.orientation.z = rotation[3]
def _build_tube(curve, rad):
"""Generates object list from given curve and radius lists.
Creates a cylinder with given radius between every pair of [x, y, z]
points. Each cylinder is initialized with the given radius and the
computed length and given a transformation to move it from the origin
(cylinders are initially centered on the origin in every axis) to the
points, where the points are in the center of each face of the cylinder.
Parameters
----------
curve : list of list of 4x4 numpy arrays (SE3)
list of SE3 for each curve, with points given in last column
rad : list of float
radii of the tubes
Returns
-------
list of CollisionObject
collection of cylinders that discretize the curve/tube
"""
tube = []
for n in range(len(curve)):
for index, from_g in enumerate(curve[n][:-1]):
to_g = curve[n][index + 1]
from_point = from_g[0:3, 3]
to_point = to_g[0:3, 3]
vec = [(t - f) for f, t in zip(from_point, to_point)]
mid_point = [(f + v / 2) for f, v in zip(from_point, vec)]
length = norm(vec)
unit_vec = vec / length
cyl = fcl.Cylinder(rad[n], length)
if unit_vec[2] == -1.0: # if vector is in -z direction
unit_quat = [0, 1, 0, 0] # gives 180 degree rotation
else:
# quat = [1 + dot(init_vec, unit_vec), cross(init_vec, unit_vec)]
# where init_vec is the z-axis unit vector [0, 0, 1]
quat = [1 + unit_vec[2], -unit_vec[1], unit_vec[0], 0]
quat_mag = norm(quat)
unit_quat = [q / quat_mag for q in quat]
translate = np.array(mid_point)
rotate = np.array(unit_quat)
transform = fcl.Transform(rotate, translate)
obj = fcl.CollisionObject(cyl, transform)
tube.append(obj)
return tube
def __init__(self, shape, position, size, category=None):
self.size = size
self.position = position
if shape == 'circle':
pos_3d = torch.FloatTensor([position[0], position[1], 0])
self.geom = fcl.Cylinder(size, 1000)
elif shape == 'rect':
pos_3d = torch.FloatTensor([position[0], position[1], 0])
self.geom = fcl.Box(size[0], size[1], 1000)
self.cobj = fcl.CollisionObject(self.geom, fcl.Transform(pos_3d))
self.category = category
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 __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 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
Homogeneous transform matrix for the object
"""
# if no transform passed, assume identity transform
if transform is None:
transform = np.eye(4)
transform = np.asanyarray(transform, dtype=np.float32)
if transform.shape != (4, 4):
raise ValueError('transform must be (4,4)!')
# create or recall from cache BVH
bvh = self._get_BVH(mesh)
# create the FCL transform from (4,4) matrix
t = fcl.Transform(transform[:3, :3], transform[:3, 3])
o = fcl.CollisionObject(bvh, t)
# Add collision object to set
if name in self._objs:
self._manager.unregisterObject(self._objs[name])
self._objs[name] = {'obj': o,
'geom': bvh}
# store the name of the geometry
self._names[id(bvh)] = name
self._manager.registerObject(o)
self._manager.update()
return o
def obs2fcl(obs, dimW):
obs_fcl = []
for i in range(0, obs.shape[0] // (2 * dimW)): # plot obstacle patches
width = obs[i * 2 * dimW + dimW] - obs[i * 2 * dimW]
height = obs[i * 2 * dimW + dimW + 1] - obs[i * 2 * dimW + 1]
# width = height = 1
ob = fcl.Box(width, height, 1)
x = obs[i * 2 * dimW] + width/2
y = obs[i * 2 * dimW + 1] + height/2
# x = y = 0
T = np.array([x, y, 0])
t = fcl.Transform(T)
co = fcl.CollisionObject(ob, t)
obs_fcl.append(co)
# print("x: ", x, " y: ", y, " width: ", width, " height: ", height)
obs_manager = fcl.DynamicAABBTreeCollisionManager()
obs_manager.registerObjects(obs_fcl)
obs_manager.setup()
return obs_manager
def stick_configuration(rota_degree, trans_x, trans_y, length, width, top_left,
top_right, bottom_left, bottom_right):
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([trans_x, trans_y, 0.0])
Transform = fcl.Transform(rotation, translation)
#before transform
stick = fcl.CollisionObject(fcl.Box(length, width, 0.0),
Transform) #x,y,z length center at the origin
#after transform
transform_matrix = np.c_[rotation, translation]
transform_matrix = np.row_stack((transform_matrix, np.array([0, 0, 0, 1])))
top_left_homo = np.r_[top_left, np.array([
1,
])]
top_right_homo = np.r_[top_right, np.array([
1,
])]
bottom_left_homo = np.r_[bottom_left, np.array([
1,
])]
bottom_right_homo = np.r_[bottom_right, np.array([
1,
])]
top_left_trans = transform_matrix @ top_left_homo
top_left_trans = top_left_trans[:2]
top_right_trans = transform_matrix @ top_right_homo
top_right_trans = top_right_trans[:2]
bottom_left_trans = transform_matrix @ bottom_left_homo
bottom_left_trans = bottom_left_trans[:2]
bottom_right_trans = transform_matrix @ bottom_right_homo
bottom_right_trans = bottom_right_trans[:2]
return transform_matrix, [
top_left_trans, top_right_trans, bottom_right_trans, bottom_left_trans
], stick
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 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 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 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
