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 __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 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 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 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 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 setUp(self):
verts = np.array([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0],
[0.0, 0.0, 1.0]])
tris = np.array([[0, 2, 1], [0, 3, 2], [0, 1, 3], [1, 2, 3]])
mesh = fcl.BVHModel()
mesh.beginModel(len(verts), len(tris))
mesh.addSubModel(verts, tris)
mesh.endModel()
self.geometry = {
'box': fcl.Box(1.0, 1.0, 1.0),
'sphere': fcl.Sphere(1.0),
'cone': fcl.Cone(1.0, 1.0),
'cylinder': fcl.Cylinder(1.0, 1.0),
'mesh': mesh
}
self.crequest = fcl.CollisionRequest(num_max_contacts=100,
enable_contact=True)
self.drequest = fcl.DistanceRequest(enable_nearest_points=True)
self.x_axis_rot = np.array([[1.0, 0.0, 0.0], [0.0, 0.0, -1.0],
[0.0, 1.0, 0.0]])
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
def collision_obj(p):
geom = fcl.Sphere(1.0)
T = [ball.pos.x, ball.pos.y, ball.pos.z]
t = fcl.Transform(Tp)
obj = fcl.CollisionObject(geom, tp)
def create_fcl_objs(self, offset):
self.g1 = fcl.Box(self.width, self.height, 3)
self.t1 = fcl.Transform(np.array(self.center))
self.o1 = fcl.CollisionObject(self.g1, self.t1)
self.g2 = fcl.Sphere(offset)
import numpy as np
import fcl
from scipy.spatial.transform import Rotation
v1 = np.array([1.0, 2.0, 3.0])
v2 = np.array([2.0, 1.0, 3.0])
v3 = np.array([3.0, 2.0, 1.0])
x, y, z = 1.0, 2.0, 3.0
rad, lz = 1.0, 3.0
n = np.array([1.0, 0.0, 0.0])
d = 5.0
t = fcl.TriangleP(v1, v2, v3) # Triangle defined by three points
b = fcl.Box(x, y, z) # Axis-aligned box with given side lengths
s = fcl.Sphere(rad) # Sphere with given radius
e = fcl.Ellipsoid(x, y, z) # Axis-aligned ellipsoid with given radii
# c = fcl.Capsule(rad, lz) # Capsule with given radius and height along z-axis
# c = fcl.Cone(rad, lz) # Cone with given radius and cylinder height along z-axis
c = fcl.Cylinder(rad, lz) # Cylinder with given radius and height along z-axis
h = fcl.Halfspace(n, d) # Half-space defined by {x : <n, x> < d}
p = fcl.Plane(n, d) # Plane defined by {x : <n, x> = d}
verts = np.array([[1.0, 1.0, 1.0], [2.0, 1.0, 1.0], [1.0, 2.0, 1.0],
[1.0, 1.0, 2.0]])
tris = np.array([[0, 2, 1], [0, 3, 2], [0, 1, 3], [1, 2, 3]])
m = fcl.BVHModel()
m.beginModel(len(verts), len(tris))
m.addSubModel(verts, tris)
m.endModel()
print()
def print_distance_result(o1_name, o2_name, result):
print('Distance between {} and {}:'.format(o1_name, o2_name))
print('-' * 30)
print('Distance: {}'.format(result.min_distance))
print('Closest Points:')
print(result.nearest_points[0])
print(result.nearest_points[1])
print()
# Create simple geometries
box = fcl.Box(1.0, 2.0, 3.0)
sphere = fcl.Sphere(4.0)
cone = fcl.Cone(5.0, 6.0)
cyl = fcl.Cylinder(2.0, 2.0)
verts = np.array([[1.0, 1.0, 1.0], [2.0, 1.0, 1.0], [1.0, 2.0, 1.0],
[1.0, 1.0, 2.0]])
tris = np.array([[0, 2, 1], [0, 3, 2], [0, 1, 3], [1, 2, 3]])
# Create mesh geometry
mesh = fcl.BVHModel()
mesh.beginModel(len(verts), len(tris))
mesh.addSubModel(verts, tris)
mesh.endModel()
#=====================================================================
# Pairwise collision checking