Collision Detection in PyBullet

All of the code for this post can be found here. However, as of pyb_utils v2.0 released on Oct. 20, 2023, the collision API discussed below has been revised; refer to the updated example here.

PyBullet is a popular physics simulator often used for robotics research. An important requirement for any robotics simulator is the ability to check for collisions between a robot and itself or its surroundings, in order to plan a collision-free path. More generally, we want to be able to compute the shortest distances between arbitrary objects in arbitrary configurations—if the distance is non-positive, the objects are in collision.

Shortest distance computations are fairly straightforward to do with PyBullet, though the solution was not immediately obvious to me: run a separate (headless) physics server only for collision checking. Consider a basic PyBullet setup which loads a robot, a ground plane, and some obstacles:

def load_environment(client_id):
    pyb.setAdditionalSearchPath(
        pybullet_data.getDataPath(), physicsClientId=client_id
    )

    # ground plane
    ground_id = pyb.loadURDF(
        "plane.urdf",
        [0, 0, 0],
        useFixedBase=True,
        physicsClientId=client_id,
    )

    # KUKA iiwa robot arm
    kuka_id = pyb.loadURDF(
        "kuka_iiwa/model.urdf",
        [0, 0, 0],
        useFixedBase=True,
        physicsClientId=client_id,
    )

    # some cubes for obstacles
    cube1_id = pyb.loadURDF(
        "cube.urdf",
        [1, 1, 0.5],
        useFixedBase=True,
        physicsClientId=client_id,
    )
    cube2_id = pyb.loadURDF(
        "cube.urdf",
        [-1, -1, 0.5],
        useFixedBase=True,
        physicsClientId=client_id,
    )
    cube3_id = pyb.loadURDF(
        "cube.urdf",
        [1, -1, 0.5],
        useFixedBase=True,
        physicsClientId=client_id,
    )

    # store body indices in a dict with more convenient key names
    bodies = {
        "robot": kuka_id,
        "ground": ground_id,
        "cube1": cube1_id,
        "cube2": cube2_id,
        "cube3": cube3_id,
    }
    return bodies


# start the main physics server and load the environment
gui_id = pyb.connect(pyb.GUI)
bodies = load_environment(gui_id)

To set up an additional physics server for collision checking, we can add:

col_id = pyb.connect(pyb.DIRECT)

# collision simulator has the same objects as the main one
collision_bodies = load_environment(col_id)

for which we don't require a GUI. In fact, we don't require dynamics simulation at all: all we need to do is put the robot in a particular configuration and then compute the shortest distances between the objects of interest.

Computing the shortest distances between pairs of objects is rather cumbersome in PyBullet, since one cannot directly specify link names to check (PyBullet uses joint indices, which are less intuitive for this purpose). As such, I wrote a little code that allows us to set up distance computation using something like:

# NamedCollisionObjects contain the name of the body, and optionally
# the name of the link on the body to check for collisions
ground = NamedCollisionObject("ground")
cube1 = NamedCollisionObject("cube1")
cube2 = NamedCollisionObject("cube2")
cube3 = NamedCollisionObject("cube3")
link7 = NamedCollisionObject("robot", "lbr_iiwa_link_7")  # last link

# then we set up collision detection for desired pairs of objects
col_detector = CollisionDetector(
    col_id,  # client ID for collision physics server
    collision_bodies,  # bodies in the simulation
    # these are the pairs of objects to compute distances between
    [(link7, ground), (link7, cube1), (link7, cube2), (link7, cube3)],
)

Now we can compute shortest distances between pairs of objects (specified by name!) for whatever configuration of the robot we want, without affecting the main GUI-based simulation:

while True:
    # compute shortest distances for a random configuration
    q = np.pi * (np.random.random(7) - 0.5)
    d = col_detector.compute_distances(q)
    in_col = col_detector.in_collision(q)

    print(f"Configuration = {q}")
    print(f"Distance to obstacles = {d}")
    print(f"In collision = {in_col}")

    # wait for user to press enter to continue
    input()

    # the main GUI-based simulation is not affected
    # we could do whatever motions we want here
    pyb.stepSimulation(physicsClientId=gui_id)

If you found this useful, consider checking out the full pyb_utils project that came out of it. In addition to collision detection, it also provides ghost (i.e. visual-only) objects, cameras, and more.