Continuous Collision With Simple Mesh Example Fails
weshoke opened this issue · comments
Wesley Smith commented
import numpy as np
import fcl
from trimesh import Trimesh
def print_collision_result(o1_name, o2_name, result):
print('Collision between {} and {}:'.format(o1_name, o2_name))
print('-'*30)
print('Collision?: {}'.format(result.is_collision))
print('Number of contacts: {}'.format(len(result.contacts)))
print('')
def print_continuous_collision_result(o1_name, o2_name, result):
print('Continuous collision between {} and {}:'.format(o1_name, o2_name))
print('-'*30)
print('Collision?: {}'.format(result.is_collide))
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('')
def create_fcl_bvh(mesh: Trimesh) -> fcl.BVHModel:
bvh = fcl.BVHModel()
err = bvh.beginModel(len(mesh.vertices), len(mesh.faces))
err = bvh.addSubModel(mesh.vertices, mesh.faces)
err = bvh.endModel()
return bvh
def create_fcl_transform(homog) -> fcl.Transform:
res = fcl.Transform(homog[:3, :3], homog[:3, 3])
return res
def print_continuous_collision_result(o1_name, o2_name, result):
print('Continuous collision between {} and {}:'.format(o1_name, o2_name))
print('-' * 30)
print('Collision?: {}'.format(result.is_collide))
print('Time of collision: {}'.format(result.time_of_contact))
print('')
def print_collision_result(o1_name, o2_name, result):
print('Collision between {} and {}:'.format(o1_name, o2_name))
print('-'*30)
print('Collision?: {}'.format(result.is_collision))
print('Number of contacts: {}'.format(len(result.contacts)))
print('')
def check_collision(obj1, obj2):
request = fcl.CollisionRequest()
result = fcl.CollisionResult()
ret = fcl.collide(obj1, obj2, request, result)
print_collision_result("obj1", "obj2", result)
vertices = np.array([
[-1.0, -1.0, 0.0],
[1.0, -1.0, 0.0],
[-1.0, 1.0, 0.0],
[1.0, 1.0, 0.0],
[-1.0, -1.0, 1.0],
[1.0, -1.0, 1.0],
[-1.0, 1.0, 1.0],
[1.0, 1.0, 1.0]
])
faces = np.array([
[3, 1, 0],
[3, 0, 2],
[0, 1, 5],
[0, 5, 4],
[1, 3, 7],
[1, 7, 5],
[3, 2, 6],
[3, 6, 7],
[2, 0, 4],
[2, 4, 6],
[4, 5, 7],
[4, 7, 6],
])
mesh1 = Trimesh(vertices=vertices, faces=faces)
mesh2 = mesh1.copy()
mesh1.apply_translation((0., 0., 3.))
mesh2.apply_scale((2., 2., 0.5))
homog1_start = np.array([
[1., 0., 0., 1.],
[0., 1., 0., 0.],
[0., 0., 1., -1.],
[0., 0., 0., 1.],
])
homog1_end = np.array([
[1., 0., 0., 1.],
[0., 1., 0., 0.],
[0., 0., 1., -2.],
[0., 0., 0., 1.],
])
homog2_start = np.array([
[1., 0., 0., -2.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.],
])
homog2_end = np.array([
[1., 0., 0., 2.],
[0., 1., 0., 0.],
[0., 0., 1., 1.5],
[0., 0., 0., 1.],
])
bvh1 = create_fcl_bvh(mesh1)
bvh2 = create_fcl_bvh(mesh2)
obj1_start = fcl.CollisionObject(bvh1, create_fcl_transform(homog1_start))
obj1_end = fcl.CollisionObject(bvh1, create_fcl_transform(homog1_end))
obj2_start = fcl.CollisionObject(bvh2, create_fcl_transform(homog2_start))
obj2_end = fcl.CollisionObject(bvh2, create_fcl_transform(homog2_end))
check_collision(obj1_start, obj2_start)
check_collision(obj2_end, obj2_end)
request = fcl.ContinuousCollisionRequest(
num_max_iterations=100, gjk_solver_type=fcl.GJKSolverType.GST_INDEP)
result = fcl.ContinuousCollisionResult()
ret = fcl.continuousCollide(obj1_start, create_fcl_transform(homog1_end), obj2_start,
create_fcl_transform(homog2_end), request, result)
print_continuous_collision_result("obj1", "obj2", result)
Output:
Collision between obj1 and obj2:
------------------------------
Collision?: False
Number of contacts: 0
Collision between obj1 and obj2:
------------------------------
Collision?: True
Number of contacts: 1
Continuous collision between obj1 and obj2:
------------------------------
Collision?: False
Time of collision: 1.0
I would expect that if collision is True for any of the fcl.collide
calls, that fcl.continuousCollide
would detect a collision. This doesn't seem to be true.
Wesley Smith commented
FYI, the fix: flexible-collision-library/fcl#550