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 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 __init__(self, radius, length, approx_faces=8):
self.r = radius
self.l = length
# the number of faces to use in the polyherdron approximation
self.nfac = approx_faces
self.num_edges = 3 * self.nfac
self.fcl_shape = fcl.Cylinder(radius, length)
self.request = fcl.CollisionRequest()
self.result = fcl.CollisionResult()
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, 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 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 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 point2fcl(self, p):
point = fcl.Cylinder(0.01, 1)
tf = fcl.Transform([p[0], p[1], 0])
return fcl.CollisionObject(point, tf)
def lidar_configuration(trans_x, trans_y, radius):
rotation = np.array([[1,0,0],[0,1,0],[0,0,1]])
translation = np.array([trans_x, trans_y, 0.0])
Transform = fcl.Transform(rotation, translation)
lidar = fcl.CollisionObject(fcl.Cylinder(radius, 0), Transform)
return lidar
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()
objs1 = [
fcl.CollisionObject(
b,
import numpy as np
import fcl
import torch
# R = np.array([[0.0, -1.0, 0.0],
# [1.0, 0.0, 0.0],
# [0.0, 0.0, 1.0]])
R = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])
T = np.array([1.0, 1.865, 0])
g1 = fcl.Box(1, 2, 3)
t1 = fcl.Transform()
o1 = fcl.CollisionObject(g1, t1)
# g2 = fcl.Cone(1,3)
g2 = fcl.Cylinder(0.01, 1000)
t2 = fcl.Transform()
o2 = fcl.CollisionObject(g2, t2)
# request = fcl.DistanceRequest(gjk_solver_type=fcl.GJKSolverType.GST_INDEP)
# result = fcl.DistanceResult()
request = fcl.CollisionRequest(enable_contact=True)
result = fcl.CollisionResult()
# ret = fcl.distance(o1, o2, request, result)
# ret = fcl.collide(o1, o2, request, result)
size = 50, 50
yy, xx = torch.meshgrid(torch.linspace(-5, 5, size[0]),
torch.linspace(-5, 5, size[1]))
grid_points = torch.stack([xx, yy], axis=2).reshape((-1, 2))
self.fkine_backup = torch.stack([t[:, :3, 3] for t in cum_tfs], dim=1)
return self.fkine_backup
if __name__ == "__main__":
lw_data = 0.3
robot = RevolutePlanarRobot(1, dof=7, link_width=lw_data)
num_frames = 100
q = 2 * (torch.rand(num_frames, 7) - 0.5) * np.pi / 1.5
points = robot.fkine(q)
points = torch.cat([torch.zeros(num_frames, 1, 2), points], axis=1)
print(points)
robot_links = robot.update_polygons(q[0])
cir_pos = torch.FloatTensor([2, 2, 0])
obs = [fcl.CollisionObject(fcl.Cylinder(1, 1), fcl.Transform(cir_pos))]
robot_manager = fcl.DynamicAABBTreeCollisionManager()
obs_manager = fcl.DynamicAABBTreeCollisionManager()
robot_manager.registerObjects(robot_links)
obs_manager.registerObjects(obs)
robot_manager.setup()
obs_manager.setup()
req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
rdata = fcl.CollisionData(request=req)
robot_manager.collide(obs_manager, rdata, fcl.defaultCollisionCallback)
in_collision = rdata.result.is_collision
from matplotlib import pyplot as plt
import seaborn as sns
sns.set()
import matplotlib.patheffects as path_effects
translation = np.array([0, 0, 0.0])
Transform = fcl.Transform(rotation, translation)
box1 = fcl.CollisionObject(fcl.Box(1, 1, 0.0), Transform) #x,y,z length center at the origin
#box.setTranslation(np.array([0.0, 1.05000, 0.0])) #useful
#box.setRotation(rotation) #useful
#other options: setTransform setQuatRotation
rota_degree = 0
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([1.4999, 0, 0.0])
Transform = fcl.Transform(rotation, translation)
box2 = fcl.CollisionObject(fcl.Box(3, 3, 0.0), Transform) #x,y,z length center at the origin
cylinder = fcl.CollisionObject(fcl.Cylinder(1, 0.0),Transform) #radius = 1
objs = [box1,cylinder]
manager = fcl.DynamicAABBTreeCollisionManager()
manager.registerObjects(objs)
manager.setup()
crequest = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
cdata = fcl.CollisionData(crequest, fcl.CollisionResult())
manager.collide(cdata, fcl.defaultCollisionCallback)
print(cdata.result.contacts)
for contact in cdata.result.contacts:
print(contact.pos)
print(contact.normal)
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
#=====================================================================
print('=' * 60)
import fcl
import numpy as np
# Create collision geometry and objects
geom1 = fcl.Cylinder(1.0, 1.0)
obj1 = fcl.CollisionObject(geom1)
geom2 = fcl.Cylinder(1.0, 1.0)
obj2 = fcl.CollisionObject(geom2, fcl.Transform(np.array([0.0, 0.0, 0.3])))
geom3 = fcl.Cylinder(1.0, 1.0)
obj3 = fcl.CollisionObject(geom3, fcl.Transform(np.array([0.0, 0.0, 3.0])))
geoms = [geom1, geom2, geom3]
objs = [obj1, obj2, obj3]
names = ['obj1', 'obj2', 'obj3']
# 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())
