python_fcl_box_sphere_collision_and_distance_check.py

1import fcl
2import numpy as np
3
4# 1. Create collision geometries
5# Create a box with dimensions 1.0 x 1.0 x 1.0
6box = fcl.Box(1.0, 1.0, 1.0)
7# Create a sphere with radius 0.5
8sphere = fcl.Sphere(0.5)
9
10# 2. Create collision objects with a specific transform (rotation and translation)
11# Transform for the box: identity rotation and at the origin
12t1 = fcl.Transform()
13obj1 = fcl.CollisionObject(box, t1)
14
15# Transform for the sphere: identity rotation and translated by 2.0 along the X-axis
16t2 = fcl.Transform(np.array([2.0, 0.0, 0.0]))
17obj2 = fcl.CollisionObject(sphere, t2)
18
19# 3. Perform collision request
20request = fcl.CollisionRequest()
21result = fcl.CollisionResult()
22
23ret = fcl.collide(obj1, obj2, request, result)
24
25print("In collision:", result.is_collision)
26
27# 4. Perform distance request
28dist_request = fcl.DistanceRequest()
29dist_result = fcl.DistanceResult()
30
31ret = fcl.distance(obj1, obj2, dist_request, dist_result)
32
33print("Distance:", dist_result.min_distance)