BerkeleyAutomation / python-fcl

Python binding of FCL library
BSD 3-Clause "New" or "Revised" License
203 stars 58 forks source link

Wrong result on collision check #68

Open iinjdpa opened 1 year ago

iinjdpa commented 1 year ago

Hello, I am testing python-fcl for using in collision detection. I 'm getting wrong results from library using this code example:

import numpy as np
import fcl

# Cylinder geometry displaced
geom1 = fcl.Cylinder(1.0, 4.0)
obj1 = fcl.CollisionObject(geom1)
obj1.setTranslation(np.array([0.5, 2, 1.0]))

# Same cylinder geometry rotated around x axis 90 degrees and displaced
geom2 = fcl.Cylinder(1.0, 4)
obj2 = fcl.CollisionObject(geom2)
obj2.setRotation(np.array([[ 1.,0. ,0.],[ 0., 0., 1.],[ 0., -1., 0.]]))
obj2.setTranslation(np.array([0.5, -1.1, 2]))

# Check collision
request = fcl.CollisionRequest(num_max_contacts=1, enable_contact=True)
result = fcl.CollisionResult()
cdata = fcl.CollisionData(request, result)
ret = fcl.collide(obj1, obj2, request, result)

# Print results
print('Collision?: ','No' if ret==0 else 'Yes')
print([x.pos for x in cdata.result.contacts])

Here I show the objects drawn in FreeCAD: 2023-08-12-20h56-32

However python-fcl indicates that there is no collision. ¿could it my mistake with code values?

Thank you for your awesome piece of code, Dani.

iinjdpa commented 1 year ago

Sorry, It was my error. I made a mistake when moving objects in FreeCAD.