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 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 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 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 is_collision(body1, body2):
return fcl.collide(body1, body2, request, result)
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))
grid_labels = torch.zeros_like(grid_points)[:, 0]
for i, (x, y) in enumerate(grid_points):
print(x, y)
o2.setTranslation([x, y, 0])
fcl.update()
ret = fcl.collide(o1, o2, request, result)
grid_labels[i] = result.is_collision
print(result.is_collision)
import matplotlib.pyplot as plt
plt.scatter(grid_points[grid_labels == True, 0],
grid_points[grid_labels == True, 1])
plt.show()
# print(ret, result.contacts[0].penetration_depth)
mesh.addSubModel(verts, tris)
mesh.endModel()
#=====================================================================
# Pairwise collision checking
#=====================================================================
print '='*60
print 'Testing Pairwise Collision Checking'
print '='*60
print ''
req = fcl.CollisionRequest(enable_contact=True)
res = fcl.CollisionResult()
n_contacts = fcl.collide(fcl.CollisionObject(box, fcl.Transform()),
fcl.CollisionObject(cone, fcl.Transform()),
req, res)
print_collision_result('Box', 'Cone', res)
n_contacts = fcl.collide(fcl.CollisionObject(box, fcl.Transform()),
fcl.CollisionObject(cyl, fcl.Transform(np.array([6.0,0.0,0.0]))),
req, res)
print_collision_result('Box', 'Cylinder', res)
n_contacts = fcl.collide(fcl.CollisionObject(mesh, fcl.Transform(np.array([0.0,0.0,-1.0]))),
fcl.CollisionObject(cyl, fcl.Transform()),
req, res)
print_collision_result('Box', 'Mesh', res)
#=====================================================================
# Pairwise distance checking
mesh.beginModel(len(verts), len(tris))
mesh.addSubModel(verts, tris)
mesh.endModel()
#=====================================================================
# Pairwise collision checking
#=====================================================================
print('=' * 60)
print('Testing Pairwise Collision Checking')
print('=' * 60)
print()
req = fcl.CollisionRequest(enable_contact=True)
res = fcl.CollisionResult()
n_contacts = fcl.collide(fcl.CollisionObject(box, fcl.Transform()),
fcl.CollisionObject(cone, fcl.Transform()), req, res)
print_collision_result('Box', 'Cone', res)
n_contacts = fcl.collide(
fcl.CollisionObject(box, fcl.Transform()),
fcl.CollisionObject(cyl, fcl.Transform(np.array([6.0, 0.0, 0.0]))), req,
res)
print_collision_result('Box', 'Cylinder', res)
n_contacts = fcl.collide(
fcl.CollisionObject(mesh, fcl.Transform(np.array([0.0, 0.0, -1.0]))),
fcl.CollisionObject(cyl, fcl.Transform()), req, res)
print_collision_result('Box', 'Mesh', res)
#=====================================================================
# Pairwise distance checking