Parallel Collision Check for Sensor Based Real-Time Motion Planning

Massimo Cefalo, Emanuele Magrini, Giuseppe Oriolo

Abstract— In this paper we present a real-time collision checkalgorithm based on the parallel computation capabilities ofrecent graphics card’s GPUs. We show an effective applicationof the proposed algorithm to solve the task-constrained real-time motion planning problem for a redundant manipulator.We propose a proof-of-concept motion planner based on fastcollision check of predicted robot motion over a given planninghorizon. Obstacles are avoided exploiting the redundancy of therobot. Reactive velocities are computed for some control pointsplaced on the robot and projected in the null space of the taskJacobian. The approach is validated through simulations inV-Rep environments and experiments on the KUKA LWR-IV7-DoF manipulator.

I. INTRODUCTION

The presence of robots in everyday life is rapidly growingas well as their applications, which span in increasinglydifferent contexts, always more frequently involving interac-tions with humans, especially in service robotics. The successof new robotics frontiers involving human robot interactionslargely depends on the safety levels of coexistence andcollaboration between humans and robots.

Currently, all technologies (no one of which may be stillconsidered mature) are based on the environmental monitor-ing to acquire information and apply, on the base of a properstrategy, a modification of the robot behavior when necessaryto achieve safety. The approaches that lie at the extremes ofthe possible strategies for the real time applications are purereaction and fast replanning. Pure reaction methods consistin avoiding possible collisions by making the robot reactiveto the obstacles, for instance by means of repulsive potentialfields. Typical problems of these methods are local minimaand unstable oscillations in presence of narrow corridors orsudden changes of the environment. Fast replanning methodsare indeed planning algorithms based on the idea to re-plan,as fast as possible, a new robot motion whenever possiblecollisions are detected.

Artificial Potential Fields ([1], [2], [3] - a pioneering work-, [4] , [5] and [6] among others) and variations are amid thefirst developed and still the most commonly applied strategiesfor achieving pure control reactiveness.

Opposed to the control-based approaches, methods basedon fast replanning are on-line motion planning approacheswhere paths and trajectories are calculated in the config-uration × time space in real time as proposed in [7]. In[8] the authors use virtual springs and damping elements

The authors are with the Dipartimento di Ingegneria Informatica, Auto-matica e Gestionale, Sapienza Universita di Roma, Via Ariosto 25, 00185Rome, Italy. E-mail: {cefalo,magrini,oriolo}@diag.uniroma1.it . This workis supported by the EU H2020 project COMANOID.

for a Cartesian impedance controller that generates motiontrajectories. In [9] both the robot and the human are repre-sented by a number of spheres and a collision-free trajectoryis obtained exploring possible end-effector movements inpredefined directions. In [10] a method to model objectsusing surfaces that closely imitate their shape is studied.The collision cone approach is used to determine collisionsbetween moving objects, whose shape can be modeled bycertain type of surfaces. Another method that uses simplercollision cones is presented in [11], where the concept ofvirtual plane is introduced.

All these works are limited by the computation timerequired to generate the proposed solution, and consequentlyare also limited by the dimensionality of the configurationspace. Thus, they can only be applied to simple contexts.

Last decade developments in the electronic field has givenrise to the multi-core processor technology and, in thesame time, to the parallel programming paradigm. In 2007,NVIDIA introduced the CUDA architecture, an hardwareand software system for supporting GPU programming, thatallows to easily demand certain computations to the GPUcores. The CPUs are nowadays multi cores systems, madeof dozens of cores, but GPUs have thousands of cores andimplement a much higher degree of parallelism.

More recent works, like [12] and [13], exploit the avail-ability of software libraries for image and point cloud pro-cessing, recently implemented on GPU, to propose real timealgorithms. Date back only a few years ago the first attemptsto exploit GPU programming for realizing real-time motionplanners, but the results were still far from being optimal.In [14] the authors propose an optimization-based motionplanner by computing collision avoidance objective functionson GPU. The planner provided a solution in a few seconds. In[15] the authors propose a parallel algorithm based on GPUscomputations for fast collision detections that, nevertheless,is not intended for real time applications.

Here we address the real-time Task-Constrained MotionPlanning (TCMP) problem in dynamic environments forredundant robots. We present a proof-of-concept plannerthat exploits a novel parallel collision detection algorithmrunning completely on GPU. The planner and the collisiondetection algorithm have been implemented and tested insimulation and on a real robot, providing exciting results.The proposed approach opens to several possible extensionsand applications.

The paper is organized as follows. Section II and III,respectively, introduce the proposed collision detection al-gorithm and the proposed planner. Section IV shows some

2017 IEEE International Conference on Robotics and Automation (ICRA)Singapore, May 29 - June 3, 2017

978-1-5090-4632-4/17/$31.00 ©2017 IEEE 1936

v[0] v[1] v[2] v[3] v[4] v[5] v[6] v[7] v[8] ...

w[0] w[1] w[2] w[3] w[4] w[5] w[6] w[7] w[8] ...

collides/not collides

parallel depth comparison

vectorialization

real depthimage

vectorialization

virtual depthimage

clock

clock

Fig. 1. The parallel collision check.

preliminary experimental results, while conclusions and pos-sible future developments are discussed in Section V.

II. THE COLLISION DETECTION ALGORITHM

A. Overview

The proposed collision detection algorithm is based onthe visual feedback provided by a 2.5D image sensor. A2.5D image sensor is a special type of range camera, thatassociates to each pixel of a bidimensional image, a valuethat corresponds to the distance of the 3D point in theCartesian space to which the pixel refers to, from thecamera image plane (depth information). Nevertheless, dueto relative occlusions between bodies, not all points in ascene can be associated to a depth information. This is whymost of the users consider not completely appropriate to callsuch sensors 3D sensors and call them 2.5D sensors.

In the last years, some depth image cameras have becomevery popular mainly due to their diffusion in the gamingworld. One the most famous is the Kinect, nowadays largelyused also in robotics thanks to its low cost and relativelyhigh reliability.

Our approach consists in processing two 2.5D images withthe same resolution: the real depth image, representing theobstacles, and the virtual depth image, representing onlythe robot in a future configuration to be tested. They aregenerated in real time using a Kinect and the robot CADmodels. Section II-B describes in details the virtual and realdepth images building process.

The two images are loaded into the GPU memory as rowvectors with the depth information. Components in the twovectors with the same index correspond to the same pixelin the depth images. A parallel comparison of the depthinformation for any pair of corresponding components is thanexecuted to detect collisions (see Fig. 1).

If two corresponding pixels of the row vectors result tobe in collision an atomic increment is done into a sharedmemory GPU variable used to count the pixels in collision.

A collision is detected whenever the robot touches anobstacle or is covered (even partially) by an obstacle. Wedistinguish two possibile cases of mutual occlusions betweenthe robot and the obstacles. The first case of occlusion arises

Fig. 2. The robot is in the shadow cone of the obstacle.

when the robot is in the shadow cone of an obstacle (seeFig. 2). Also in this case, it is not possible to know if therobot is in effect in contact or not with any obstacle, thereforethe entire shadow cone generated by the obstacle is prudentlyconsidered as part of the obstacle itself and a collision isreported.

The second case occurs when, with respect to the camerapoint of view, the robot is in front of some obstacles. In thiscase, in general, using a single static image it is not possibleto deduce if the robot is in collision or not. However, indynamic situations, making some hypothesis on the relativevelocities between the robot and the obstacles, it is possibleto predict if a collision has occurred or not. The proposedalgorithm is able to recognize if the robot is in collisionor not in situations like this, using information relative tothe previous state and the guess that at the beginning of themotion the robot in a safe configuration.

B. Virtual and real depth images

The virtual depth image is an image of the robot withoutobstacles artificially created in the depth space. A CADmodel of the robot is loaded into a virtual environmentusing OpenGL. A scene object collects hierarchically themovable components of the robot (for instance, the links ofa manipulator) as distinguished nodes, to which it is possibleto apply motion commands. The nodes in the scene objectare mesh elements that contain all necessary information forthe rendering, like vertices position and the orientation of thenormal vectors to the surfaces. The goal is to obtain an imagethat could be subtracted from the camera image to cancel therobot and build a map containing only the obstacles.

Once the CAD model has been loaded, by using the directkinematics, we move the virtual model to match the realrobot configuration. At this point we apply a sequence oflinear transformations defined by matrices to obtain a 2.5Dimage with the same resolution and relative to the samepoint and field of view of the Kinect. In particular, we firstapply a transformation to map the point coordinates froma local reference frame (defined in the CAD model) to aworld reference frame, with respect to witch all objects inthe environment are defined (Mmodel). Then, we transformthe world coordinates into a view-space coordinates making

1937

Camera depth imageObstacle map:Real depth image

Collision check

CAD model Robot map

Virtual depth image

Fig. 3. Collision check process.

everything relative to the camera point of view (Mview). Thismapping transforms 3D points into 2D points with a depthinformation. The third transformation determines what isvisible and what is not in a given field of view. This mappingprojects the coordinates from the view-space to the clip-space(Mpr). The last transformation determines the screen pointcoordinates of a 2D frame (Mscr).

The coordinates of a point in the virtual depth image aretherefore determined applying the following formula to the3D points of a CAD model:

pvdi =

(pxpy

)= Mscr ·Mpr ·Mview ·Mmodel · vlocal (1)

where vdi stands for virtual depth image andvlocal = (vx vy vz)T .

The real depth image is an image containing only theobstacles. It is obtained subtracting to the camera imagea virtual depth image representing the robot only. Anyinaccuracy in the geometric model arises in a non perfectcancellation of the robot in the camera image. Other factorsthat may also influence the robot cancellation are the calibra-tion of the virtual camera (used to generate the virtual depthimage) w.r.t. the real one. In order to filter the imperfectionsand obtain a robust process, the virtual depth image is dilateda few pixels before the cancellation. This operation producesan enlargement of the geometric models corresponding todisplace only the points on the surfaces, keeping unchangedthe position of any interior point and their relative distances.The result is similar to assign a thickness to the surfaces.If a pixel representing the robot has a shorter depth than

its corresponding pixel in the camera image, then the latteris deleted from the camera image. The result is a mapcontaining only the obstacles that are visible to the Kinect.Finally a maximum depth is assigned to each pixel belongingto the holes left by the robot cancellation. This process canbe considered as a first level of image filtering.

A second filter acts on the counter of the pixels incollision. If the counter has a value greater than a giventhreshold, a robot collision is returned. The aim of thisthreshold is to filter image imperfections generated by thecamera and thus skip false collision results. This parametercan be properly chosen according to the obstacles’ geometry,bearing in mind that if an obstacle is placed in the scene so asto appear very thin and be represented only by a few pixels inthe obstacle map, high threshold values imply false negativesand the inability to detect collisions with such obstacle.

Finally we mention the possibility to define an obstacleclearance. This can be implemented considering in the depthplane, for any pixel to be tested for collisions, a set of furtherfour points placed at the vertices of a square centered withthe pixel. The size of the square, may change accordinglyto the depth of the pixel in which it is centered, in order torealize a fixed tolerance in the Cartesian space. All pixels ofthe square than must be tested for collisions (recall that thismay be done in parallel on the GPU).

Fig. 3 summarizes the whole collision check process.

C. Tools for parallel computingThe proposed collision detection algorithm has been im-

plemented as a GPU distributed program and is able to

1938

exploit the parallelism of a graphic board. In particular, itis based on the CUDA framework and the OpenGL library.OpenGL is a platform independent rendering library thatprovides hardware accelerated rendering functions. CUDAis a framework for mass parallel programming based on theNVIDIA architecture. Differently from the CPU, in a GPUeach core has the capability to execute hundreds of processessimultaneously, thanks also to a high speed memory closelyinterconnected to the cores. This high degree of parallelismgives to any GPU based application a huge performanceboost.

In order to develop scalable and parallel applicationsthat are able to exploit the power of a GPU, new parallelprogramming models are required. CUDA architecture isan emerging technology that gives to the developers thepossibility to access GPU resources for general purposes,making the GPU an open architecture similarly to the CPU.We used the GLSL shading language to directly programsome tasks in the GPU (like, the robot augmentation, thetransformations defined in (1) and the collision tests) andthe OpenGL for CAD model manipulation.

III. A REAL TIME VALIDATION-BASED PLANNER

In this section we present a real-time planner to solvein real time the Task-Constrained Motion Planning problempresented in [16]. We will adopt the same terminology andframework therein introduced. Differently from that work,here, for simplicity, we assume that the robot is not subjectto nonholonomic constraints.

Consider a robot whose generalized coordinates are repre-sented by an nq-dimensional vector q which takes values inthe configuration space C. The robot is assumed to move ina workspace W , subset of IR2 or IR3, populated by possiblymoving obstacles.

Since there are no differential constraints, the kinematicmodel of the robot consists of simple integrators

q = v (2)

reflecting the fact that the generalized coordinates q canmove arbitrarily in C.

The robot is assumed to be constrained to perform as-signed tasks expressed in m-dimensional coordinate vectorsy, associated to the space Y . Task coordinates and configu-ration coordinates can be related through the kinematic mapy = f(q) which, differentiated, assumes the form

y = J(q)q,

where J = ∂f/∂q is the m× n task Jacobian. For reasonsthat will be soon clear, we will assume that the robot iskinematically redundant with respect to the assigned task,that is m < n.

Let yd(t) ∈ Y be a non-derogable assigned task, i.e. atask that cannot be violated for any reason. The possibility torelax the task constraint in presence of unavoidable collisionsis outside the scope of this paper. The robot redundancy istherefore required to provide the robot with the necessary

primitivesgeneration

integration andcollision check

qq

yd(t)

exteroceptivedata

v

REAL-TIME PLANNER ROBOT

Kinect

qr

qq

loop till a collisionfree solution over

Tp is found

Tp Tc

Tk

+

-

Fig. 4. General scheme of the proposed real-time planner.

flexibility to continue the execution of the task in presenceof obstacles.

The robot is also assumed to be equipped with a depthsensor fixed in the environment, that provides real-timeinformation on the depth of any object that falls within agiven field of view (e.g. a Kinect camera).

The problem consists in real-time planning a robot trajec-tory in C that is collision-free and realizes the desired task.Observe that the assigned task is specified as a function oftime, therefore, there is no possibility for the planner to finda solution simply scaling in time a motion that results incollision. In case a collision free motion cannot be found,the robot stops and the planer returns a failure.

A. The algorithm

The proposed planner is an anytime task-constrainedmotion planner that generates collision free motion on aplanning horizon Th using a kinematic controller (the motiongeneration scheme, further discussed in this Section). Figure4 synthesizes the scheme of the planner.

Joint velocities are chosen to guarantee the exact and safeexecution of the task during the period Th by setting

v = J†(yd + Key) + (I − J†J)w (3)

where J† is the pseudoinverse of the task Jacobian, K is apositive definite matrix, ey = yd − y is the task error andw is an (m−n)-dimensional auxiliary velocity vector whichcan be arbitrarily specified without affecting the task (recallthat I − J†J is the orthogonal projection matrix in the nullspace of J ).

Vector w produces self motions that do not influencethe position or the orientation of the end-effector. It canbe freely defined, and is usually exploited to accomplish asecondary task compatible with the main one. In Section III-B we propose a possible strategy for choosing w that allowscollision avoidance without affecting the assigned task.

The motion generation scheme generates the joint veloc-ities defined by equation (3) using a proper choice for theauxiliary vector w. Than the kinematic model of the robotis integrated with a sample period Tc, to predict the motionof the robot over the planning horizon Th. Tc is the sample

1939

mainthread

collision checkthreads

controlthread

robot plannerthread

kinect

.

.

.

.

.

.

Tc

Tk

Tp

tcc

save a new image

update w

read w read q

apply qnewupdate qnew

.

.

.

read and test qnew

Fig. 5. Time sequence chart.

rate of the low-level controller which is responsible for theactuation of the commanded joint velocities to the real robot.

Figure 5 shows a diagram sequence of the main parts of thealgorithm that may run in parallel. Each part is represented inthe chart as a separate thread. The main thread represents thecore of the algorithm put in charge of the communicationswith all other processes. The control thread is responsiblefor updating, every Tc seconds, the joint velocity commandson the robot actuators. Such commands are computed usingequation (3) where w is updated by the planner every Tpseconds. The collision check process is executed on the GPUand is run in parallel to any other process. This process runscontinuously, performing the collision tests on each point ofa camera image. Being the collision check much faster thanthe camera refresh rate, the same test is repeated many timeson the same image, but without influencing the behaviorof the planner. Whenever a new camera image becomesavailable, the collision tests are automatically performed onthe new image. This process communicates with the mainthread to signal if a collision has been detected or not.In the end, the planner thread is the part of the algorithmresponsible for generating the auxiliary velocity vector wusing an appropriate strategy.

During the integration process, new robot configurationsare checked for collisions with the obstacles detected atthe beginning of the period Th. In this preliminary workwe consider the obstacles fixed during the period Th. Thishypothesis works fine also in case of obstacles that movewith low velocities with respect to the robot and to thecamera sampling frame.

If the integration of the kinematic controller can becomputed without collisions over the entire period Th, thenthe pseudo velocities w are used to apply new velocitycommands to the robot. The vector w is kept constant for aplanning period Tp < Th, which should be long enough togive to the planner the time to compute new collision-freevelocities but also as short as possible so as to update wwith high frequency. In particular, fixed Tc, Tp and Th mustsatisfy

Tp ≥ q · tcc · Th/Tc (4)

where tcc is the collision check time, Th/Tc is the number ofintegration steps and q is the number of different alternativesfor the auxiliary vector w to be tested. Figure 6 shows the

Th

Tc

Tp

... ...

Tp

...

time required to compute w

here w is applied and kept constant

over Th the robot motion is predicted to be safe using w

Fig. 6. Planning time Tp, planning horizon Th and control period Tc.

relation between Tc, Tp and Th. In equation (4), q may be putto 1 if the different choices for w can be tested in parallel.

B. Primitive-based motion generation scheme

The idea at the heart of our planner is to use the proposedreal-time collision check algorithm to predict if a givenchoice for w, held constant, generates safe motions overthe planning horizon Th. This is obtained by integratingequation (3) over the interval Th and checking the obtainedconfiguration space path for collisions with the obstaclesdetected at the beginning of the planning phase.

We assume that the obstacles do not disturb the motionof the end-effector along the assigned trajectory. If not, theassigned planning problem has no solutions.

A safe motion requires to move the manipulator as muchas possible far from the obstacles. Exploiting the redundancythis can be achieved while pursuing the primary task. Takinginspiration from a classical approach (see, for instance, [17]),we exploit the null space projection to obtain self motionsthat stave off some robot points from the obstacles. Inparticular, we consider a set of p control points along therobot structure. We use the control points ci to compute thedistances with the obstacles and consequently generate anauxiliary velocity vector w in such a way to increases suchdistances and move the robot far from the obstacles.

In general, if f(q) is a performance function defined in theconfiguration space, to maximize f , in accordance with thegradient descent method, the configuration space velocitiescan be defined as

w = α∇f

where ∇f = (∂f/∂q1, . . . , ∂f/∂qn)T and α is a positivegain. On the base of this general result, we propose thefollowing definition for the auxiliary velocity vector w:

w =

p∑i=1

qci (5)

where qci is the velocity in the joint space of the controlpoint ci defined as

qci =

{0 if di > dth

(dth − di)/dth αiJ†ci∇di if di ≤ dth

(6)

in which,∇di is the gradient of the shortest distance betweenthe control point ci and the obstacles in the Cartesian space,Jci is the Jacobian matrix associated to the control pointci and dth is a threshold, beyond which the reaction to theobstacles is null. The term (dth − di)/dth helps to obtain

1940

a smooth transition with the motion guided only by thepseudoinverse (w = 0).

Equation (5) defines the auxiliary robot joint velocities asthe sum of p reactive velocities applied to p control points.The reactive velocity of a single control point is zero if it isfar enough from any obstacle. Using equation (5), the robotmoves on the assigned task keeping the distances between thecontrol points and the obstacles always within an assignedthreshold, compatibly with the primary task.

Equation (5) generates a single choice w for w. Theplanner integrates the motion generation scheme using w =0 and w = w. Therefore, in our implementation q = 2, butsince the two integration processes may run in parallel ontwo different CPU threads, in equation (4) we can put q = 1.At the beginning, the planner tries to move the robot on thetask using w = 0. If the integration with w = 0 can becompleted without collisions over the planning horizon Th,the choice w = 0 is passed to the control thread which usesagain equation (3) to compute and send velocity commandsto the robot any Tc seconds. This behavior corresponds tomove the robot on the task in absence of collisions witha minimum norm displacement in the configuration space(minimum energy).

If, on the contrary, the choice w = 0 generates collisions,than the value w is considered and tested and in the positivecase passed to the control thread to generate the velocitycommands for the robot.

Whenever a choice for w results to be collision free, thansuch value is considered in the next integration period as thefirst possible choice. This helps to maintain the same valuefor a primitive over a longer period of time, avoiding a jerkybehavior of the robot that may instead result if the primitiveis continuously changed.

C. Implementation issues

In order to compute the Cartesian distance d(O,P ) be-tween a robot point P and an obstacle point O using thedepth information, we use the formula proposed in [5] thatwe recall here for convenience:

d(O,P ) =√v2x + v2y + v2z ≥

√v2x + v2y

with

vx =(ox − cx)do − (px − cx)dp

f · sx

vy =(oy − cy)do − (py − cy)dp

f · syvz = do − dp

where (ox, oy) and (px, py) are the coordinates in the depthspace of the points O and P respectively, do and dp are theirdepth w.r.t. the camera, cx and cy are the pixel coordinatesof the center of the image plane (on the focal axis), f is thefocal length of the camera and sx and sy are the dimensionsof a pixel in meters. The last five parameters constitutethe intrinsic parameters of the camera and can be usuallyretrieved by the device manufacturer.

TABLE I

Tc Tp Th dth Kp αi

0.005 s 0.02 s 0.04 s 0.3 m 20 15

Equation (6) requires to compute the gradient of thedistances between the control points and the obstacles. Ex-ploiting the CUDA functionalities, it is possible to computethe distances between any control point and any point on theobstacle map in parallel, and than select the minimum di. Allthis computations can be done by an atomic operation, whosecost in terms of execution time, is almost zero.

Once the distances di have been calculated, the gradientin (6) may be computed as

∇di = R(vx vy 0)T /d (7)

where R is the rotation matrix necessary to align the robotframe with the camera frame.

During the integration of the motion generation scheme inthe prediction phase, in order to speed up the computations,the integration step may be set up as a multiple of Tc, at theexpense of a larger numerical error. Besides, as long as theplanning horizon is chosen small enough (for instance lessthan 1 second), it is admissible to check for collisions onlythe last configuration obtained by the integration process.These considerations may be exploited to setup a longerplanning horizon Th with respect to the planning period Tp.

IV. EXPERIMENTAL RESULTS

We present here some preliminary results of the proposedplanner applied to the KUKA LWR-IV 7-DOF manipulator.We tested the planner on V-REP, a software developmentkit for robotic simulations, and also on the real robot. Thehardware platform was a 64-bit Intel Core i7-4790 CPU,equipped with 16 GB DDR3 RAM, running at 4 GHz. Thehigh-performance graphic board had a NVIDIA GTX970GPU, organized in 1884 CUDA cores and capable of 26624concurrent threads.

Both in simulation and in the real experiment we assignedto the end-effector of the robot a linear task in the Cartesianspace, with the time law (x0, y0 + 0.2 sin(2πt/T ), z0)T ,where (x0, y0, z0)T was the position of the end-effector atthe beginning of the motion and T was 4 seconds in VREPand 8 in the real experiment.

We considered three control points placed at the centerof the joints 3, 4 (the elbow) and 5. Other points could beadded along the structure, but any control point too close tothe base or to the end-effector is useless for reconfiguringthe manipulator.

Table I collects some planner parameters. We used thesame values for the simulation and the real experiment. Theaverage time for running the collision test was around 1.5milliseconds on the entire camera image.

We dilated the robot CADs along the normals 3 cm beforethe cancelation and we fixed to 500 the threshold for thecollisions counter. Finally, we fixed to 50 cm the obstacleclearance.

1941

0

1

0 5 10 15 20 25 30 35

time0 5 10 15 20 25 30 35

0

1x 10 −3

time

task error norm (m) collision detection

Fig. 7. Simulation: Error norm (on the left) and collision detections onthe predicted motion (on the right).

0 10 20 30 40 50 60 70 800

1.5x 10

−3

timetime

task error norm (m) collision detection

0

1

0 10 20 30 40 50 60 70 80

Fig. 10. Real experiment: Error norm (on the left) and collision detectionson the predicted motion (on the right).

Figure 9 shows some snapshots of the simulation. Therobot starts moving on the assigned task from a safe con-figuration and continues to move along the task using thepseudoinverse till its posture becomes too close to a humanwhich is in its workspace (3-rd frame). In correspondence tothat configuration, the predicted motion using the pseudoin-verse generates collisions and the planner applies to the robotthe auxiliary velocities w that tend to increase the distancesbetween the control points and the human. Later, the humanapproaches to the manipulator (4-th frame) and the plannercomputes new auxiliary velocities, with larger values thatyield a prompt reaction of the robot, which self-reconfigureswhile continuing to perform the assigned task.

Figure 7 shows, on the left, the norm of the error alongthe assigned task (always smaller than one millimeter) and,on the right, the results of the collision tests executed in realtime during the prediction phase.

Similarly to the simulation, some snapshots of the experi-ment performed on real robot are shown in Figure 8. Startingfrom a collision free configuration, the robot moves alongthe assigned task. About 11 seconds later, a fixed obstacleis placed close to the robot, with a resulting collision onpredicted motion. New reactive velocities w push away thecontrol points from obstacles (2-nd and 3-th frame). In thesecond part of the experiment, a moving obstacle approachesto the robot. The collisions are still avoided by the robot,maintaining the end-effector on the desired task (black line).Also in this case the norm of the task error is almost alwayssmaller than one millimeter. Figure 10 shows the resultstogether with the collisions detected in real time on thepredicted motion.

Note that compared to the methods purely reactive, as themethod of the artificial potential fields, the solution found bythe planner leads the robot to move away from obstacles onlywhat is strictly necessary, passing near them as permitted bythe settings on safe distance.

V. CONCLUSIONS

We developed a novel collision detection algorithm basedon GPU parallel computing and a real-time planner for

redundant robots. To the best of our knowledge, we presenthere the first a real-time task constrained motion planningalgorithm based on parallel collision check. It is a proof-of-concept planner that prove the effectiveness of the ourcollision detection method.

Interesting future developments may concern the possibil-ity to predict the obstacles’ motion using a filter, or doinga 3D reconstruction based on camera images. A furtherextension may also concern the application of the presentedalgorithms to an experimental setup that uses more than asingle Kinect, as in [18].

REFERENCES

[1] B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo, Robotics: Mod-elling, Planning and Control. Springer-Verlag, 2009.

[2] J. Minguez, F. Lamiraux, and J. Laumond, Motion planning andobstacle avoidance, B. Siciliano, O. Khatib ed. Springer Handbookof Robotics, 2008.

[3] O. Khatib, “Real-time obstacle avoidance for manipulators and mobilerobots,” Int. J. of Robotics Research, vol. 5, no. 1, pp. 90–98, 1986.

[4] P. Ogren, N. Egerstedt, and X. Hu, “Reactive mobile manipulationusing dynamic trajectory tracking,” in 2000 IEEE Int. Conf. onRobotics and Automation, San Francisco, USA, 2000.

[5] F. Flacco, T. Kroger, and A. De Luca, “A depth space approach tohuman-robot collision avoidance,” in 2012 IEEE Int. Conf. on Roboticsand Automation, Saint Paul, USA, 2012.

[6] S. Haddalin, S. Belder, and A. Albu-Schaeffer, “Dynamic motionplanning for robots in partially unknown environments,” IFAC WorldCongress, vol. 44, no. 1, pp. 6842–6850, 2011.

[7] J. Vannoy and J. Xiao, “Real-time adaptive motion planning (ramp)of mobile manipulators in dynamic environment with unforeseenchanges,” IEEE Trans. on Robotics, vol. 24, no. 5, pp. 1199–1212,2008.

[8] S. Haddadin, H. Urbanek, S. Parusel, D. Burschka, J. Romann,A. Albu-Schffer, and G. Hirzinger, “Real-time reactive motion gen-eration based on variable attractor dynamics and shaped velocities,”in 2010 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, Taipei,Taiwan, 2010.

[9] L. Balan and G. M. Bone, “Real-time 3d collision avoidance methodfor safe human and robot coexistence,” in 2006 IEEE/RSJ Int. Conf.on Intelligent Robots and Systems, Beijing, China, 2006.

[10] A. Chakravarthy and D. Ghose, “Generalization of the collision coneapproach for motion safety in 3d environments,” Autonomous Robots,vol. 32, pp. 243–266, 2012.

[11] F. Belkouche, “Reactive path planning in a dynamic environment,”IEEE Trans. on Robotics, vol. 25, no. 4, pp. 902–911, 2010.

[12] K. Kaldestad, S. Haddadin, R. Belder, G. Hovland, and D. Anisi,“Collision avoidance with potential fields based on parallel processingof 3d-point cloud data on the gpu,” in 2014 IEEE Int. Conf. onRobotics and Automation, Hong Kong, China, 2014.

[13] S. Ueki, T. Mouri, and H. Kawasaki, “Collision avoidance methodfor hand-arm robot using both structural model and 3d point cloud,”in 2015 IEEE/SICE International Symposium on System Integration(SII), 2015.

[14] R. Wagner, U. Frese, and B. Buml, “3d modeling, distance andgradient computation for motion planning: A direct gpgpu approach,”in 2013 IEEE Int. Conf. on Robotics and Automation, Karlsruhe,Germany, 2013.

[15] J. Pan and D. Manocha, “GPU-based parallel collision detection forfast motion planning,” Int. J. of Robotics Research, vol. 31, no. 2, pp.187–200, 2012.

[16] G. Oriolo and M. Vendittelli, “A control-based approach to task-constrained motion planning,” in 2009 IEEE/RSJ Int. Conf. on In-telligent Robots and Systems, St. Louis, MO, 2009, pp. 297–302.

[17] A. Maciejewski and C. Klein, “Obstacle avoidance for kinematicallyredundant manipulators in dynamically varying environments,” Int. J.of Robotics Research, vol. 4, no. 3, pp. 109–117, 1985.

[18] F. Flacco and A. De Luca, “Real-time computation of distance todynamic obstacles with multiple depth sensors,” IEEE Robotics andAutomation Letters, vol. 2, no. 1, pp. 56–63, 2017.

1942

the robot starts the task execution in a safeconfiguration

t=0 t=11 t=23

t=51 t=64 t=74

a box is placed close to the robot; collisionsare predicted and reactive velocities applied

a reaction generated by a single control pointmaintains the robot far from the obstacle

a moving obstacle approaches and new reactivevelocities push away the robot

the robot never abandon the task whilecontinuing to avoid collisions

here, the task leads the end-effector towardsthe obstacle, but collisions are still avoided

Fig. 8. Sample frames of the experiment on the real robot. The assigned task is highlighted in black, with its orientation specified by a white arrow.The robot motion is highlighted by white curved arrows. Collisions are highlighted with yellows dots in the depth image, on the right. Cartesian reactivevelocities are represented by yellow arrows.

RGB image

Depth image

Collisions viewreactive velocities

in the cartesian space

collisions detectedon predicted motion

robot CAD dilated

the robot starts the task execution in a safeconfiguration

now the human approaches the robot and the reactive velocities push away the control points

the robot self-reconfigures while continuing to perform the assigned task

here, the new configuration becomes too closeto the human and future collisions are detected

the motion predicted along the task isconsidered safe

the human can safely operate close to the robotwhile it continues the execution of the task

t=0 t=1

t=13 t=24 t=25

t=3.5

Fig. 9. Sample frames of the simulation run in V-REP. The assigned task is highlighted in black. The RGB image captured by the Kinect is on thecenter top. Collisions are highlighted with yellows dots in top right image. The depth image in represented in the box on the right. The Cartesian reactivevelocities are represented by yellow arrows and human’s approaching direction is represented by a white arrow.

1943