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 test_pairwise_collisions(self):
result = fcl.CollisionResult()
box = fcl.CollisionObject(self.geometry['box'])
cone = fcl.CollisionObject(self.geometry['cone'])
mesh = fcl.CollisionObject(self.geometry['mesh'])
result = fcl.CollisionResult()
ret = fcl.collide(box, cone, self.crequest, result)
self.assertTrue(ret > 0)
self.assertTrue(result.is_collision)
result = fcl.CollisionResult()
ret = fcl.collide(box, mesh, self.crequest, result)
self.assertTrue(ret > 0)
self.assertTrue(result.is_collision)
result = fcl.CollisionResult()
ret = fcl.collide(cone, mesh, self.crequest, result)
self.assertTrue(ret > 0)
self.assertTrue(result.is_collision)
cone.setTranslation(np.array([0.0, 0.0, -0.6]))
result = fcl.CollisionResult()
ret = fcl.collide(box, cone, self.crequest, result)
self.assertTrue(ret > 0)
self.assertTrue(result.is_collision)
result = fcl.CollisionResult()
ret = fcl.collide(cone, mesh, self.crequest, result)
self.assertTrue(ret == 0)
self.assertFalse(result.is_collision)
cone.setTranslation(np.array([0.0, -0.9, 0.0]))
cone.setRotation(self.x_axis_rot)
result = fcl.CollisionResult()
ret = fcl.collide(box, cone, self.crequest, result)
self.assertTrue(ret > 0)
self.assertTrue(result.is_collision)
cone.setTranslation(np.array([0.0, -1.1, 0.0]))
result = fcl.CollisionResult()
ret = fcl.collide(box, cone, self.crequest, result)
self.assertTrue(ret == 0)
self.assertFalse(result.is_collision)
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 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 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 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 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 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 __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 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 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 __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 _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 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 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 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 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
