python-fcl
python-fcl copied to clipboard
Erratic multiple Halfspace objects
Hi I would like to combine multiple Halfspace objects to form a work space, but seems that there is some problem (at least the way I do it).
Here is a minimum working example:
import numpy as np
import fcl
obstacles = [
fcl.CollisionObject(fcl.Halfspace(np.array([ 1.0, 0.0, 0.0]), -5.0)),
fcl.CollisionObject(fcl.Halfspace(np.array([-1.0, 0.0, 0.0]), -5.0)),
]
# Collision checking.
collision_manager = fcl.DynamicAABBTreeCollisionManager()
collision_manager.registerObjects(obstacles)
collision_manager.setup()
# Robots collision objects.
ball0 = fcl.Sphere(0.01)
pos0 = fcl.Transform(np.array([0.0, 0.0, 0.0], dtype=float))
robo0 = fcl.CollisionObject(ball0, pos0)
#
req = fcl.CollisionRequest(num_max_contacts=10, enable_contact=True)
cdata = fcl.CollisionData(request=req)
collision_manager.collide(robo0, cdata, fcl.defaultCollisionCallback)
print(cdata.result.is_collision)
The goal is to check whether the point robo0 is between planes X< 5 and X>-5.
But The program runs without output. If I runf the code in Jupyter, the kernel stops and I will receive the following message:
Canceled future for execute_request message before replies were done
Am I doing this wrong or this is a bug?
Best Farshid