FCL: A General Purpose Library for Collision and Proximity Queries
Jia Pan
1
Sachin Chitta
2
Dinesh Manocha
1
Abstract— We present a new collision and proximity library
that integrates several techniques for fast and accurate collision
checking and proximity computation. Our library is based on
hierarchical representations and designed to perform multiple
proximity queries on different model representations. The set of
queries includes discrete collision detection, continuous collision
detection, separation distance computation and penetration
depth estimation. The input models may correspond to tri-
angulated rigid or deformable models and articulated models.
Moreover, FCL can perform probabilistic collision checking
between noisy point clouds that are captured using cameras or
LIDAR sensors. The main benefit of FCL lies in the fact that
it provides a unified interface that can be used by various
applications. Furthermore, its flexible architecture makes it
easier to implement new algorithms within this framework. The
runtime performance of the library is comparable to state of
the art collision and proximity algorithms. We demonstrate its
performance on synthetic datasets as well as motion planning
and grasping computations performed using a two-armed
mobile manipulation robot.
I. INTRODUCTION
The problems of collision and proximity computation are
widely studied in various fields including robotics, simulated
environments, haptics, computer games and computational
geometry. The set of queries includes discrete collision
checking, separation distance computation between two non-
overlapping objects, first point of contact computation be-
tween continuous moving objects, and penetration depth
computation between overlapping objects. Furthermore, the
underlying geometric representations may correspond to
rigid objects (e.g., computer games), articulated models (e.g.,
mobile manipulators), deformable models (e.g., surgical or
cloth simulators) or point-cloud datasets (e.g., captured using
camera or LIDAR sensors on a robot).
Many efficient algorithms have been proposed to perform
collision and proximity queries on various types of models.
At a broad level, they can be classified based on the un-
derlying query or the model representation [16], [6]. Some
of the commonly-used techniques for polygonal models are
based on bounding volume hierarchies, which can be used
for collision and separation distance queries, and can be
extended to deformable models. Moreover, many of these
algorithms have been used to design widely used libraries
such as I-COLLIDE, Bullet, ODE, RAPID, PQP, SOLID,
OPCODE, V-Clip, Self-CCD, etc. However, these libraries
1
Jia Pan and Dinesh Manocha are with the Department of
Computer Science, the University of North Carolina, Chapel Hill
(panj,dm)@cs.unc.edu
2
Sachin Chitta is with Willow Garage Inc., Menlo Park, CA 94025, USA
(sachinc)@willowgarage.com
This work was supported by ARO Contract W911NF-10-1-0506, NSF
awards 1000579 and 1117127, and Willow Garage.
Fig. 1: The PR2 is a mobile manipulation system with integrated stereo
and laser sensors. Left: A pick and place task with the PR2 was among
the experimental tasks used to validate FCL. Right: A visualization of the
environment that the PR2 is working with.
have two main restrictions: 1) They are limited to specific
queries (e.g., discrete collision checking or separation dis-
tance computation) on certain types of models (e.g., convex
polytopes or rigid objects); and 2) it is hard to modify or
extend these libraries in terms of using a different algorithm
or representation. For example, SOLID [3] is designed to
perform collision checking using axis-aligned bounding box
(AABB) trees; RAPID is designed for collision detection
using oriented bounding box (OBB) trees [9], and PQP
performs separation distance queries using rectangular swept
sphere (RSS) trees [13]. It is hard to use a different bound-
ing volume with each of these libraries or use a different
hierarchy computation or traversal scheme.
Many applications need to perform different collision and
proximity queries. Figure 1 shows an example task where the
PR2 mobile manipulation robot is executing a pick and place
task using a combination of grasping, motion planning and
control algorithms. Continuous collision detection queries
are useful for grasp planning executed by the robot to
generate grasps for the objects. The robot uses sample-based
motion planners to compute collision-free paths. It is well
known that a high fraction of running time for sample-based
planning is spent in collision/proximity queries, underlining
the need for fast efficient proximity and collision queries.
The local planning algorithms that form the underlying
basis of sampling-based planners usually perform multiple
discretized collision queries or may use continuous collision
checking algorithms. Many sampling schemes either use
separation distance computation [15] or penetration-depth
estimation (e.g. retraction planners) to compute samples in
narrow passages. Proximity information can also be used to
plan paths that are further away from obstacles, allowing the
robot to execute the plans at higher speeds.
Contributions: We present a new collision checking library,
labeled FCL (Flexible Collision Library), which provides
a unified interface to perform different proximity queries.
Furthermore, it is able to handle a wide class of models,
including rigid and deformable objects, articulated models
and noisy point clouds. We propose a system architecture
that is flexible in terms of performing different sets of queries
and can be extended in terms of adding new algorithms
and representations. These include proximity computations
between convex polytopes, general polygonal models, artic-
ulated models, deformable models and noisy point clouds.
In order to perform different queries, FCL models query
computation as a traversal process along a bounding volume
hierarchy. Different queries use the same traversal frame-
work, but differ in terms of intermediate data structures and
traversal strategies. The overall performance of the FCL is
comparable to state-of-the-art algorithms.
We validate our techniques on a real-world system through
integration with a pick and place manipulation task per-
formed on the PR2 robot (Figure 1). The PR2 robot has
both stereo and laser range finders that provide point cloud
data at a high rate. The robot needs to calculate feasible
motion (i.e., collision-free motion that satisfies some dy-
namics constraints), quickly through cluttered environments.
We integrate our collision checking methods with the open
source OMPL motion planning library [2] and demonstrate
fast and accurate motion planning that allows the robot
to complete its task. FCL is available as an independent
library at https://kforge.ros.org/projects/fcl/. A
ROS interface to FCL is also provided so that users can
use FCL to perform collision and proximity queries with
different robots.
The rest of the paper is organized as follows. We survey
related work on collision and proximity queries in Section II.
Section III gives an overview of the library and the detailed
architecture is described in Section IV. We highlight a few
applications and performance in Section V.
II. BACKGROUND AND RELATED WORK
In this section, we first give a brief overview of prior
work on collision and proximity queries. Furthermore, we
highlight the underlying algorithms that are used in FCL to
perform different queries.
A. Collision and Proximity Computation Algorithms
The problems of collision detection and distance compu-
tations are well studied [6], [7], [16]. At a broad level, they
can be classified based on algorithms for convex polytopes,
bounding volume hierarchies, continuous collision detection,
broad-phase collision detection and point-cloud collisions.
Table I shows all the queries currently supported by FCL.
1) Convex Polytope based Collision: Many methods have
been proposed to compute the Euclidean distance between
two convex polytopes, such as the Gilbert–Johnson–Keerthi
(GJK) algorithm [8] and the Lin-Canny algorithm [17]. The
optimization technique based on the Minkowski formulation
in the GJK algorithm can also be used to compute trans-
lational penetration depth. The convex polytope collision
in FCL is based on GJK and EPA (Expanding Polytope
Algorithm) [24].
2) Bounding Volume Hierarchy based Collision: Some
of the most widely used algorithms for triangulated or
polygonal models are based on bounding volume hierarchies.
Typical examples of bounding volumes include axis-aligned
bounding box (AABB) [3], spheres, oriented bounding box
(OBB) [9], discrete oriented polytope (k-DOP) [12] and
swept sphere volume (SSV) [13], and they have been mainly
used to perform discrete collision detection and separa-
tion distance queries. Furthermore, they can be extended
to deformable models by updating the hierarchies during
each step of the simulation [22]. Spatial decomposition
techniques, such as kd-trees and octrees, have also been used
for collision checking, though techniques based on BVHs
are considered faster. FCL can support different bounding
volume hierarchies to perform various queries on rigid and
deformable models.
3) Continuous Collision Detection: In many applications
(e.g., local planning in motion planning [15]), we need to
check the objects for collision moving along a continuous
path. One solution is to sample along the path and then
perform collision checking at discrete time steps. Such
discrete collision checking methods may miss collisions
between the sampled time steps. While adaptive sampling
strategies and predictive methods can be used to alleviate
this problem [15], resulting algorithms can be relatively slow.
In order to provide rigorous guarantees, continuous collision
detection (CCD) techniques have been proposed [21], which
compute the first time of contact between two moving objects
along a path. CCD is typically performed by using bounding
volume hierarchies. The BVH used for CCD computations
provides a conservative bound for the swept volume of an
object generated during the given time interval. The BVH
is usually generated by refitting the static object BVH in
a bottom-up manner and is based on the trajectory of the
object. These include algorithms for linearly interpolating
motion between two static configurations [10] and arbitrary
in-between rigid motions [21]. Another popular approach for
CCD is based on conservative advancement (CA), which
incrementally advances objects by a time step while avoiding
collisions. In order to determine the conservative time step,
it needs to compute the minimal separation distance between
the objects and uses it to estimate conservative motion
bounds. Tang et al. [23] present algorithms to compute
motion bound for screw motion. Conservative advancement
algorithms can be applied to non-convex models based on
BVHs. FCL uses these CCD and CA algorithms.
4) Broad-Phase Collision Detection: Many applications
need to perform collisions between a large number of ob-
jects, including different links of articulate models and self-
collisions in deformable models. In order to avoid O(n
2
)
collision checkings between the objects or primitives, broad-
phase collision detection algorithms are used to cull away
object pairs that are far-away from each other. Widely
used N-body collision algorithms include sweep and prune
(SaP) [5] and spatial subdivision [7]. FCL currently uses
SaP, which works well when the moving objects have high
spatial coherence. In the ROS interface of FCL, the broad-
Rigid Objects Point Cloud Deformable Objects Articulated Objects
Collision Detection
√ √ √ √
Continuous Collision Detection
√ √
∗
√ √
Self Collision Detection
√ √ √ √
Penetration Estimation
√
X X X
Distance Computation
√
X
√ √
#
Broad-phase Collision
√ √ √ √
#
TABLE I: The design of FCL makes it possible to perform different proximity queries on various models. Current capabilities of FCL are shown with
√
symbol. Symbol X are capabilities that are currently not supported and may be added in the future. The symbol
√
#
refers to queries that can be
implemented by existing capabilities of FCL. The continuous collision detection between point clouds (
√
∗
) is not completely implemented, and the current
version only supports CCD queries between a point cloud and a triangulated (mesh) representation.
phase collision algorithm is combined with the kinematic
model implementation in ROS to perform robot self-collision
as well as collision detection with the environment.
5) Point Cloud Collision Detection: There has been rel-
atively little work in terms of handling collisions between
point clouds or between point clouds and unstructured
meshes. With the recent advances in RGB-D cameras and
LIDAR sensors, there is increased interest in performing
various queries on noisy point-cloud datasets. FCL supports
collision checking between triangle meshes/soups and point
clouds as well as collision checking between point clouds.
The former is useful for collision checking between robot
parts and the environment, while the latter is used for
collision checking between scanned objects (e.g. held by a
robot gripper) and the environment. The point cloud collision
checking algorithms in FCL also take into account noise in
the point cloud data arising from various sensors as well
as the inherent shape uncertainty in point cloud data arising
from discretization [19].
6) Parallel Collision and Proximity Computation: The
advent of multi-core CPUs and many-core GPUs makes it
necessary to design parallel collision and proximity compu-
tation algorithms that can exploit the capabilities of multi-
core processors [22]. Many GPU-based parallel collision
and proximity computation algorithms have been proposed,
especially for a single collision query [14], multiple collision
queries [20] or N-body collision [18]. In practice, GPU-based
algorithms can offer considerable increase in speed over
CPU-based algorithms and are good candidates for future
extensions to FCL.
III. OVERVIEW
In this section, we introduce different proximity queries
that are supported in FCL and give an overview of the library.
A. Definitions
Given two objects A and B as well as their configurations
q
A
and q
B
, the discrete collision query (DCD query) returns a
yes/no answer about whether the two objects are in collision
or not, i.e., whether
A(q
A
) ∩B(q
B
) 6= /0
is true. Optionally, collision query can also compute the
contact points where the object boundary overlaps and the
corresponding contact normals.
Given two objects A and B as well as their motions q
A
(t)
and q
B
(t), where t ∈ [0, 1], the continuous collision query
(CCD query) returns the a yes/no answer about whether the
two objects are in collision within interval [0, 1], i.e., whether
∃t ∈ [0, 1], A(q
A
(t)) ∩B(q
B
(t)) 6= /0.
If a collision occurs, it also returns the first time of contact:
toc = inf{t : A(q
A
(t)) ∩B(q
B
(t)) 6= /0}.
Given two non-overlapping objects A and B, as well as
their configurations q
A
and q
B
, the separation distance query
(SD query) returns the distance between them:
dis = inf{kx −yk
2
: x ∈ A(q
A
), y ∈B(q
B
)}.
Optionally, distance query can also return the closest pair of
points:
argmin
x∈A(q
A
),y∈B(q
B
)
kx −yk
2
.
Given two objects A and B as well as their configurations
q
A
and q
B
, the penetration depth query (PD query) returns
the translational penetration depth between the objects when
they are in collision:
pd = inf{kdk
2
: (A(q
A
) + d) ∩B(q
B
) = /0}.
The exact computation of translational penetration depth
between non-convex or deformable models has a high com-
plexity (O(n
6
)) [11]. As a result, FCL only provides the
capability to approximate penetration depth between two
mesh-based models or a mesh-polytope pair by computing
the penetration between two colliding triangles or triangle-
polytope pairs. FCL can also compute penetration depth
between two convex polytopes using EPA algorithm [24].
Given a set of objects {A
i
}
n
i=1
with their configurations q
i
,
broad-phase collision query returns a yes/no answer about
whether any two of the objects are in collision or not : i.e.,
whether
∃i 6= j ∈ {1, 2, ..., n}, R(A
i
(q
i
)) ∩R(A
j
(q
j
)) 6= /0,
where R() computes the AABB of each object. Optionally,
it also returns all the pairs of in-collision objects.
B. FCL Overview
FCL is a fully templated C++ library that can perform
various collision and proximity queries highlighted above.
From an application perspective, FCL is designed to pro-
vide unified and extendable interfaces for collision and
proximity computation algorithms. Moreover, it is designed
to support different data representations, including triangle
meshes/soups and well-known shape primitives (e.g., sphere,
cylinder). To achieve these goals, FCL models all collision
and proximity queries between two objects as a traversal
process along a hierarchical structure. Different queries can
share the same traversal framework but may use different in-
termediate data structures and traversal strategies. As shown
in Figure 2, the traversal process is performed in three steps
in FCL:
1) Object representation: The objects in a query are
represented by a hierarchical structure suitable for that
specific query. For example, basic geometric shapes
(e.g., cones, cylinders, spheres) are represented using a
single-level hierarchy with the corresponding bounding
volume as the unique node. Arbitrary geometric objects
are represented using a bounding volume hierarchy.
The hierarchical structure along with the object’s con-
figuration information is stored in a structure called
CollisionObject, which also contains the object’s
shape representation from the previous time step for
deformable models.
2) Traversal node initialization: The traversal node is the
structure that stores the complete information required
to perform the traversal for a specific query. Such
information may be different among various types of
queries on different object representations. For ex-
ample, for a continuous collision query, we need to
store the object configuration and shape representation
for the previous time frame. The traversal node also
decides the traversal strategy for a given query. For
example, if only a yes/no answer is required for
collision query, the traversal can stop once the collision
is found. Such an early termination strategy may not
be applicable for seperation distance queries.
3) Hierarchy traversal: After the traversal node initial-
ization step, we traverse the hierarchical structure to
perform a specific collision or proximity query.
Collision queries for articulated bodies or environments
with multiple moving/deformable objects need to be per-
formed efficiently. In FCL, this is handled by Collision
Manager, which uses the N-body collision detection SaP
algorithm to handle such scenarios for different queries.
Point Cloud
Triangle
Mesh
Geometric
Shape
Collision
Object
Time Frame
Information
Traversal
Node
Initialization
Collision
Detection
Collision
Manager
Articulated
Body
Collision
Result
Fig. 2: Flow chart for FCL architecture: The black arrows represent the
data flow, including construction of the hierarchical structure for each object
and the collision manager for multiple objects. The red arrows represent the
algorithm flow, including traversal node preparation and hierarchal traversal.
IV. FCL ARCHITECTURE
FCL consists of three main components: (a) object repre-
sentation, (b) traversal node initialization, and (c) hierarchy
traversal.
A. Object Representation
The object representation component in FCL deals with
representing the objects in a hierarchical data structure so that
collision and proximity queries can be performed efficiently.
FCL supports objects in the form of unstructured triangle
meshes/soups and basic geometric shapes, such as spheres
and cylinders, which are widely used in many robotics appli-
cations. FCL can also handle deformable models. It doesn’t
make any assumptions about the deformation and assumes
that the object boundary is represented as a triangulated mesh
during each time step.
FCL supports seven types of basic geometric shapes,
including sphere, box, cone, cylinder, capsule, convex mesh
and plane, which are represented as a hierarchy with a
single node. All these shape representations consist of two
interfaces: 1) overlap, which checks for overlap between
the bounding volume of the geometric shape with a bounding
volume corresponding to a node in the hierarchy of some
other object. It can also be used to perform culling oper-
ations. 2) intersect, which checks for exact intersection
between the geometric objects or the triangle/point primitives
of the other object. This is mainly used to perform exact
proximity queries between the low level primitives.
The unstructured mesh/soup is represented as a bounding
volume hierarchy and the specific type of bounding volume
is specified as a template parameter. In FCL, four BV types
are currently supported: AABB, OBB, RSS and kDOP. FCL
includes the routines to perform overlap and distance queries
using each of these BVs. Each BV is suitable for differ-
ent kinds of objects, queries or applications. For example,
OBB is regarded as a tight fitting bounding volume to the
underlying shape or primitives, but performing an overlap
test using OBB is more expensive as compared to AABB
or kDOPs [9]. RSS is regarded as an efficient primitive to
perform separation distance computation [13]. OBB and RSS
are considered as more efficient BVs in terms of performing
CCD between rigid models, because the BVH structure is
unchanged during the motion or simulation, and only the
transformation matrix associated with each BV needs to
be updated. kDOP and AABB are more suitable for CCD
between deformable models, because the cost of refitting
these BVHs is relatively low compared to OBB or RSS. The
refitting step is typically performed in a bottom-up manner,
where each BV is updated.
The BVH data structure in FCL stores both the vertices
and triangles of the underlying object, though the triangle
information is not used for point cloud models. In order to
perform CCD computations, the BVH data structure also
keeps track of the position of the vertices from the last
time step. As a result, the BVH structure can handle triangle
meshes/soups and point clouds in a consistent manner.
FCL provides functions to compute the hierarchical rep-
resentation for a given object. Furthermore, it uses a state
machine to guarantee that the output of the construction
process is a valid BVH structure. The usage of a state
machine can help user to avoid generating invalid BVH
structures. As shown in Figure 3, the state machine consists
of three parts. The first is the standard way to construct a
BVH: we start from an empty BVH and construct a valid
BVH structure by adding vertices/triangles into it (empty
→ building → built in Figure 3). However, in many
applications (e.g., the local planning in motion planning),
there is high spatial coherence between adjacent queries. In
this case, we keep the structure of a BVH unchanged and
only update the positions of the triangles associated with the
leaf nodes and the intermediate BVs (i.e. refitting the BVH).
Therefore, we provide a second method to replace the BVH:
we start from a valid BVH, replace the object geometric
representation by a new geometric representation and finally
compute a updated BVH (built → replacing → built in
Figure 3). In order to perform CCD queries, the BVH is used
to compute a conservative bound of the swept volume of an
object during a given time interval. Therefore, we update the
BVH in order to consider the motion information and obtain
a valid BVH for CCD (built → updating → updated in
Figure 3). When the underlying topology of the object (i.e.,
the triangle information) changes, we empty the BVH state
and build it from scratch (built → empty or updated →
empty in Figure 3).
The BVH construction or refitting recursively splits the
underlying geometry primitives into two parts and fits a tight
bounding box to each part. There are different approaches
to perform split and fit operations and it is useful to pro-
vide flexibility so that users can choose or implement the
appropriate operations based on the underlying application.
Therefore, our BVH data structure is composed of base
classes for split and fit operations, and we also provide a
default implementation for these operations.
Note that object representation may influence the results
of a collision query. For example, assuming that A is a
small box within a large cylinder B, a collision query
will report a collision if both A and B are represented as
polytopes, because each polytope corresponds to a solid
shape. However, if both A and B are represented as triangle
meshes, a collision query will report no collision because a
triangle mesh only represents the object boundary, and not
the interior.
B. Traversal Node Initialization
Given two bounding volume hierarchies, the collision or
proximity computation between them is usually performed
by traversing the bounding volume test tree (BVTT) gen-
erated from the two BVHs [9]. Different collision or prox-
imity computation algorithms tend to use different traversal
schemes though the traversal framework is the same among
different algorithms. In FCL, we separate the actual traversal
framework from the traversal data and traversal strategies.
The advantage of such a design is in terms of implementing
a new collision or proximity algorithm: the users only need
to implement the new traversal hierarchy and traversal strate-
gies, instead of implementing the entire collision framework
from scratch.
In FCL, we use a traversal node to provide all the
necessary information to access BVH hierarchy struc-
ture and determine the traversal order. As shown in Fig-
ure 4, all traversal node types are derived from one base
class TraversalNode. Among the routines provided by
TraversalNode, getFirstOverSecond is used to deter-
mine the traversal orders, i.e., which subtree of BVTT to
traverse. All other routines provide the necessary informa-
tion to traverse BVTT’s tree hierarchy. Notice that these
interfaces are also applicable to convex polytopes as they
are recognized as special BVH with a single BV node.
For example, if the collision query is performed between a
mesh and a cylinder, we need to ensure that the routines
isSecondNodeLeaf and firstOverSecond return true
values.
We use two subclasses CollisionTraversalNode and
DistanceTraversalNode that correspond to the routines
used to perform collision and distance queries, respectively.
BVTesting checks the intersection between two bounding
volumes and leafTesting checks the intersection between
primitives (e.g., triangles or point clouds). The function
canStop determines whether there is a collision or the
shortest distance is computed, and we can terminate the
recursion.
CollisionTraversalNode has three subclasses to han-
dle collision between two BVHs, collision between a BVH
and a basic shape and collision between two basic shapes, re-
spectively. The leaf classes are traversal node types for differ-
ent collision algorithms, such as continuous collision, point
cloud collision, etc. When the bounding volume corresponds
to an OBB or a RSS, we provide efficient implementation
of overlap tests or separation distance computation between
these BVs via template specialization.
C. Hierarchy Traversal
The recursive traversal framework is used to perform all
the queries. We provide recursive traversal schemes for vari-
ous queries. The first is the recursive algorithm for collision
queries, as shown in Algorithm 1, whose input could be any
traversal node derived from CollisionTraversalNode.
The second is the recursive algorithm for separation distance
computation, as shown in Algorithm 2, whose input traversal
nodes are derived from DistanceTraversalNode. We also
provide a self-collision recursive traversal scheme, which is
mainly used for deformable models.
The traversal recursive framework in FCL also provides
support to maintain a front list, which can accelerate collision
and proximity queries, if there is high spatial coherence
between two subsequent queries. Intuitively, the front list is a
set of internal and leaf nodes in the BVTT hierarchy, where
the traversal terminates while performing a query during a
given time instance. The front list reflects the subset of a
BVTT that is traversed for that particular proximity query.
