High Precision Real Time Collision Detection

by   Alexandre Coulombe, et al.
McGill University

Collision detection and collision avoidance are essential components in these systems for safe human-robot interactions. Robotics systems that can work "out-of-the-box" without excessive amount of installation and calibration from the experts is highly ideal. For this, we propose a generic, high precision, collision detect system that only requires the unified robot description format (URDF) and is capable of running in real time. We extended the Gilbert-Johnson-Keerthi (GJK) algorithm by utilizing a geometrical approach to determine the distance between each rigid body in the environment and check for collisions. The proposed system's performance is shown by checking the self-collision of the KUKA LBR iiwa 7 R800 and the Mecademic Meca500. The performance is compared to the Flexible Collision Library (FCL).



There are no comments yet.


page 3


Dynamic collision avoidance for multiple robotic manipulators based on a non-cooperative multi-agent game

A flexible operation of multiple robotic manipulators in a shared worksp...

Barycode-based GJK Algorithm

In this paper, we present a more efficient GJK algorithm to solve the co...

Fast Task-Specific Target Detection via Graph Based Constraints Representation and Checking

In this work, we present a fast target detection framework for real-worl...

When Being Soft Makes You Tough: A Collision Resilient Quadcopter Inspired by Arthropod Exoskeletons

Flying robots are usually rather delicate, and require protective enclos...

SMASH: Physics-guided Reconstruction of Collisions from Videos

Collision sequences are commonly used in games and entertainment to add ...

A Flexible Exoskeleton for Collision Resilience

With inspiration from arthropods' exoskeletons, we designed a simple, ea...
This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

With the arrival of Industry 4.0, an increase in robotic systems has emerged in manufacturing. Collision detection and collision avoidance are essential components in these systems for safe human-robot interactions [7]. In order to guarantee the safety of the users and the robots, these components need to work in real time (i.e., the computations must be performed within one millisecond). However, these systems can be very complex to install and require experts to calibrate and operate. Therefore, robotics systems that follow an ”out-of-the-box” convention is highly preferable.

Many approaches use geometric volumes to model the robot and detect collisions between objects [4]. Models such as Bounding Spheres [11], Axis-Aligned Bounding Boxes (AABB) [1], Oriented Bounding Boxes (OBB) [3] and Bounding Cylinders [6] are used as an abstraction of the robot’s true model and provide fast approximations. However, they achieve this at the expense of conserving the true form of the robot, losing the ability to get the true distances between objects.

Some methods perform collision detection by using convex hulls that represent the true form of the robot, such as the Gilbert-Johnson-Keerthi (GJK) distance algorithm [5] which can also find the distance between the convex hulls. Improvements have been applied to this method, such as adding recursion to reduce computation [9], finding the penetration distance of objects that are intersecting [2], and making the algorithm more numerically stable [8].

The Flexible Collision Library (FCL) is an open-source C++ collision detection library 

[10] that features many collision detection algorithms and is integrated in the Robot Operating System (ROS). However, FCL is too slow in calculating the distance between meshes, making it unsuitable for real-time use. Also, a lot of development time and effort needs to be invested to use FCL and ROS on a robot.

In this paper, we propose a collision detection method that is capable of being used ”out-of-the-box”, run in real time, and, in addition to verifying collisions between objects, get the distance between objects in the modelled environment. We also propose a variant of the GJK distance sub-algorithm using a geometrical approach. The method is generic, and only requires the robot’s unified robot description format (URDF) and the objects’ meshes with their respective transformations. Background is provided in Section II, our proposed method is explored in Section III, and the method is compared to the Flexible Collision Library (FCL) [10] in Section IV.

Fig. 1: The KUKA LBR iiwa (right) and the Mecademic Meca500 (left) were examples for collision detection.

Ii Related Work

A typical approach for collision detection is to describe each link of the robot as a convex object and calculate the distance between each pair of objects. In this section, we provide a brief summary to distance querying between convex objects.

Ii-a Minkowski difference for distance query

Given two objects defined by and , the sets of points composing them respectively, the Minkowski difference between them is defined as:


If the origin is within the convex hull formed by , then and intersect. Otherwise, the shortest distance between and , denoted , is equivalent to the shortest distance between and .


Ii-B Gilbert-Johnson-Keerthi Algorithm

Evaluating the entire Minkowski difference in Eq. 2 is infeasible in real time; alternatively, the GJK algorithm iteratively approximates the convex hull of as a simplex [5]. In , the simplexes are the point, line, triangle and tetrahedron as seen in Fig. 2.

Fig. 2: The possible simplexes that can be used in three dimensions for the GJK algorithm

At each iteration of the GJK algorithm, a witness point is chosen, and the set of witness points are used to form a simplex and the direction from the simplex to the origin . The algorithm repeats until (i) contains the origin, which means the objects are intersecting, or (ii) no better can be formed. In the latter case, the point on closest to the origin has as its magnitude the shortest distance between the objects. The method can be visualized in Fig. 3.

Ideally, the witness point

is chosen as the farthest point from the hyperplane defined by the current search direction

. This is done through a support function, which finds the point in that has the highest scalar product with . The function is implemented in a way that does not need to be explicitly computed by applying the support function on and by Eq. 3.


Given the current simplex , the distance function finds the point that is closest to the origin and defines the search direction . This function computes the barycentric coordinates and a subset from to identify the edge/face of the simplex that is closest to the origin. Geometrically, is perpendicular to an edge/face of the simplex. After finding the closest point, is reduced to a set with the witness points corresponding to the point/edge/face on which the closest point lies. In the next iteration, is added to , forming a new simplex. The algorithm stops when is found within or the generated repeats in which case the bestSimplex is found. A generic GJK is summarized in Algorithm 1.

(a) Initially, the witness point is A and is A. The direction .
(b) Witness point B is added to . Point C is the closest point to the origin. The direction is .
(c) Witness point D is added to . Point E is the closest point to the origin. The direction is .
(d) Witness point F is added to . Point G is the closest point to the origin. This is the best simplex.
Fig. 3: A two dimensional example of the GJK algorithm over four iterations.
Input : ,
Output : : the shortest distance between and
Initialize (any initial value will give the same result)
       (, )
       Add to
       if  contains  then
       if bestSimplex then
Algorithm 1 GJK Distance Algorithm

Ii-C Recursive Gilbert-Johnson-Keerthi Algorithm

The recursive GJK (RGJK) algorithm adds a recursive element to the support function in order to avoid exhaustive computations [9]. At each iteration, we start from the point of the previous iteration and check its neighbouring points. The point on the object with the highest scalar product can be found by a hill-climbing technique. This greatly reduces the number of points to verify and the computation time.

Iii Proposed Method

The proposed method is split into two parts. First, the techniques used for modelling the robot and workspace are presented, along with the pipeline for generating the models. Afterwards, our variant of the GJK distance sub-algorithm using a geometric approach is described.

Iii-a Environment Modeling

In order to be able to use the advantages of the RGJK algorithm, the components in the model of the environment, such as the links of the robot, the objects in the workspace, and the end effector, need to be represented in a way that the neighbours of any given point of the mesh can be known. To gain this feature, the mesh can be imported into a graph data structure, where the vertices of the mesh are the nodes of the graph and the edges of the mesh populate the adjacency list of the nodes of the graph. This representation gives the neighbouring vertices in the mesh through the adjacency list of a node, providing the ability to use the recursive support function of the RGJK algorithm.

Meshes are easily imported from files which have a 3D file format, such as Standard Triangle Language (STL)(*.stl), COLLADA (*.dae), and Wavefront (*.obj). To import a mesh into the collision detection system, a method for converting the 3D geometry contained in a file format to a graph data structure is required. In the case of an STL file, the mesh is described by triangles formed by the vertices, which makes it easy to find neighbouring vertices. The graph representation of the mesh can be built while reading the file, giving a new index for each new vertex, and adding vertices to the adjacency list of vertices that they form triangles with.

For each component to be modelled in the environment, the component needs a mesh representation with a frame of reference for the vertices and a transformation matrix that places it in the environment. In the case of the robot links, the reference frame of the mesh representing the link needs to match the reference frame of the forward kinematics of the robot. By matching the reference frames of the links with the reference frames of the forward kinematics of the robot, we model the robot for all its poses as in Fig. 4.

(a) KUKA LBR iiwa 7 R800 Mesh Model
(b) Mecademic Meca500 Mesh Model
Fig. 4: Mesh Models of the robots used for experimentation taken from their STL files with all joint angles equal to zero.

Since the robot can move between time steps, the model of the environment needs to be able to update the components of any mobile component in the environment. This is done by having a data structure of graphs representing the components in the environment and only updating the components that can be moved. This saves resources rewriting the same data to stationary components in the environment. For mobile components, a backup of the original graphs generated from the meshes imported from the files are kept in memory, and, for each time step, the new transformations of the mobile components are applied to the meshes and saved into the data structure holding the graphs on the current model. This process is performed once every time step and keeps the model updated with the current information about the environment. The collision detection system using this method of modelling the environment is described in Algorithm 2.

Input : , , , : Joint Angles
Output : Detected Collision (Boolean)
= update the position of all components with , , , into the same frame
for  to  do
       for  to  do
             Compute the distance RGJK(, )
             if  then
                   return True
return False
Algorithm 2 Real-time RGJK Collision Detection

Iii-B The Distance Algorithm

The function in Algorithm 1 is where all the heavy lifting of the GJK algorithm is done. This function finds the closest point to the origin on the current simplex , which is also used to find the search direction . Previous work used the barycentric coordinates to find this point [5][2][8], but here, we modified the function with a simpler approach from linear algebra.

Since we are using simplexes that only require two to four witness points, we only need to keep these witness points on . It is shown that GJK algorithm monotonically converges to the true convex hull of  [8]. Thus, we know that the search direction can only move closer to the solution but not further. We divided the search area into smaller regions, and we can discard regions that are further away from the closest point to the origin at the current iteration.

For the line case, there are three regions where the origin can be as seen in Fig. 4(a). However, since we know that is the closest point to the origin in , our approach rules out the 3rd region and only checks regions 1 and 2. If there is a point in region 1 that is the closest point to the origin, then

is true and the closest point can be found by using the vector rejection formula,

. If is false, then the point is the closest point and we can remove point from .

(a) Line simplex regions
(b) Triangle simplex regions
Fig. 5: Regions where the origin can be found.

For the triangle simplex, there are 8 regions where the origin could be. Again, using the knowledge that point is the closest point to date, our approach rules out the regions covered by , and the line as seen in Fig. 4(b). Our approach only needs to verify 5 regions to find the closest point to the origin. We can verify region 1 first by checking if and then if . If these conditions are true, we can treat and as a line and go to the line case. We can verify region 4 first by checking if and if , or if and if . This is because we are making a plane without a fixed position. For regions 2 and 3, the verification is simply and . The closest point for this case is computed by using the vector projection of on , . We can verify region 5 by ruling out all the other regions.

For the tetrahedron, there are 15 regions to verify. However, we can reject 7 regions by using the same idea that is the closest point. We can cover the 8 remaining regions by checking the triangle case of each face with point as one of its vertices. If the origin is within the tetrahedron, we can make and return from the RGJK algorithm.

Iv Results

In this section, the performance of the collision detection system described previously is explored and discussed. The collision detection system, implemented in C, is compared to the Flexible Collision Library (FCL). Both our system described previously and FCL model the robot, apply the transformation to the components of the model and perform the collision checking. Since we wish to be able to use the collision checking in real time, the computation time must be under one millisecond. Both collision systems were used on the KUKA LBR iiwa 7 R800 and the Mecademic Meca500 to compare the flexibility and performance of each system on different models. The simulations only took into account self-collisions, but the system can also be adapted to verify collision in the robot’s workspace. Both systems were simulated on a  GHz AMD Ryzen 5 pro 3500u processor running Linux. In Algorithm 2, we use .

For the KUKA LBR iiwa, the model was taken from the ROS Industrial Experimental packages for the Kuka manipulators111https://github.com/ros-industrial/kuka_experimental. The STL files containing the collision model were processed and converted into convex hull representations. The new convex hulls were used in the experiments.

For the Mecademic Meca500, the model was taken from Mecademic’s ROS package222https://github.com/Mecademic/ROS. No pre-processing was required since the meshes in the STL files were already convex hulls.

For both robots, the collision checking was performed on different poses. The oriented bounding boxes (OBB) bounded volume hierarchy (BVH) scheme from FCL was used for the comparisons. This scheme was used since OBB is a tight-fitting bounding volume to the underlying geometry and distance querying is said to take longer [10]. Both systems yielded the same conclusions for the collision checking for all the poses, showing that both approaches identify collisions effectively and are functionally equivalent. For the timing performance, our RGJK method is much faster than FCL, where the results are found in TABLE I.

Robot FCL library [10] Proposed Method
Kuka LWR
TABLE I: Timing for the self-collision detection averaged over different poses (in milli-seconds)

We can see for both robots that the our method is capable of being used in real time while FCL is well over  ms for both cases. Our method took of the time FCL needed to compute the self-collision on the KUKA robot and of the time FCL needed for the Mecademic robot. One reason for this is the way both approaches model the components in the environment. In our method described previously, the transformation of each component is applied once before starting collision checking. In contrast, in FCL, the transformation needs to be applied on the component each collision check. Our method cuts down time by doing the work once and reusing it for every collision check.

We also see that our method is better at adapting to more complex models. The method’s computation time for performing the self-collision checking on the Mecademic robot increased by when computing the self-collision on the KUKA robot. Using FCL, the increase is of . This is due to the recursive support algorithm making the RGJK method faster by removing computations that do not need to be performed to achieve the same outcome.

V Conclusion

In conclusion, we proposed a generic method for real-time collision detection. The method takes the unified robot description format (URDF) and extends the GJK distance query algorithm for faster computation. Experimental results show that the proposed method is capable of running in real time and outperforms the existing open source library in terms of computation time.

For future work, we will utilize the distance query method developed in this work for real-time collision avoidance systems. Also, we will extend the proposed method to detect collision between the known components (provided by the URDF) and dynamic components (e.g., objects). This can be done by incorporating sensory feedback to gain additional information about the environment in the workspace of the robots.


  • [1] G. v. d. Bergen (1997) Efficient collision detection of complex deformable models using AABB trees. Journal of graphics tools 2 (4), pp. 1–13. Cited by: §I.
  • [2] S. Cameron (1997) Enhancing GJK: computing minimum and penetration distances between convex polyhedra. In IEEE International Conference on Robotics and Automation, Vol. 4, pp. 3112–3117. Cited by: §I, §III-B.
  • [3] J. Chang, W. Wang, and M. Kim (2010) Efficient collision detection using a dual OBB-sphere bounding volume hierarchy. Computer-Aided Design 42 (1), pp. 50–57. Cited by: §I.
  • [4] C. Ericson (2004) Real-time collision detection. CRC Press. Cited by: §I.
  • [5] E. G. Gilbert, D. W. Johnson, and S. S. Keerthi (1988) A fast procedure for computing the distance between complex objects in three-dimensional space. IEEE Journal on Robotics and Automation 4 (2), pp. 193–203. Cited by: §I, §II-B, §III-B.
  • [6] J. Ketchel and P. Larochelle (2006) Collision detection of cylindrical rigid bodies for motion planning. In IEEE International Conference on Robotics and Automation, pp. 1530–1535. Cited by: §I.
  • [7] H. Lin, J. Smith, K. K. Babarahmati, N. Dehio, and M. Mistry (2018) A projected inverse dynamics approach for multi-arm cartesian impedance control. In IEEE International Conference on Robotics and Automation, pp. 1–5. Cited by: §I.
  • [8] M. Montanari, N. Petrinic, and E. Barbieri (2017) Improving the GJK algorithm for faster and more reliable distance queries between convex objects. ACM Transactions on Graphics (TOG) 36 (3), pp. 1–17. Cited by: §I, §III-B, §III-B.
  • [9] C. J. Ong and E. G. Gilbert (1997) The Gilbert-Johnson-Keerthi distance algorithm: a fast version for incremental motions. In IEEE International Conference on Robotics and Automation, Vol. 2, pp. 1183–1189. Cited by: §I, §II-C.
  • [10] J. Pan, S. Chitta, and D. Manocha (2012) FCL: a general purpose library for collision and proximity queries. In IEEE International Conference on Robotics and Automation, pp. 3859–3866. Cited by: §I, §I, TABLE I, §IV.
  • [11] X. Wu (1992) A linear-time simple bounding volume algorithm. In Graphics Gems III, pp. 301–306. Cited by: §I.