python-fcl icon indicating copy to clipboard operation
python-fcl copied to clipboard

Erratic multiple Halfspace objects

Open farshidasadi47 opened this issue 2 years ago • 0 comments

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

farshidasadi47 avatar Feb 13 '23 00:02 farshidasadi47