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 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 __init__(self, dx, dy, dz):
self.dx = dx
self.dy = dy
self.dz = dz
self.num_edges = 12
self.fcl_shape = fcl.Box(dx, dy, dz)
self.request = fcl.CollisionRequest()
self.result = fcl.CollisionResult()
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 __init__(self, collision_dict):
Collision_Object.__init__(self, collision_dict)
if not len(self.params) == 3:
raise TypeError(bc.FAIL + 'ERROR: parameters for collision box must be list of 3 floats.' + bc.ENDC)
self.x = self.params[0]
self.y = self.params[1]
self.z = self.params[2]
self.g = fcl.Box(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 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 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 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)
def rec2fcl(self, rec):
box = fcl.Box(rec[2], rec[3], 1.0)
tf = fcl.Transform([rec[0], rec[1], 0])
return fcl.CollisionObject(box, tf)
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()
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))
import fcl
import numpy as np
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([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)
print('Time of collision: {}'.format(result.time_of_contact))
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()
#=====================================================================
import numpy as np
import fcl
g1 = fcl.Box(2, 2, 2)
trans = np.array([0, 0.0, 0.0])
t1 = fcl.Transform(trans)
print("g1:", id(g1))
o1 = fcl.CollisionObject(g1, t1)
b = fcl.Box(2, 2, 2)
trans = np.array([3, 0.0, 0.0])
t = fcl.Transform(trans)
print("b:", id(b))
o = fcl.CollisionObject(b, t)
g3 = fcl.Box(2, 2, 2)
trans = np.array([7, 0.0, 0.0])
t3 = fcl.Transform(trans)
print("g3:", id(g3))
o5 = fcl.CollisionObject(g3, t3)
# request = fcl.CollisionRequest()
# result = fcl.CollisionResult()
objs1 = [o1, o5, o]
req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
rdata = fcl.CollisionData(request=req)
manager1 = fcl.DynamicAABBTreeCollisionManager()
manager1.registerObjects(objs1)
manager1.setup()
