Reactive Planning on a Collaborative Robot for Industrial Applications

Gautier Dumonteil

, Guido Manfredi

, Michel Devy

, Ambroise Confetti

and Daniel Sidobre

2,3

Siemens Industry Software SAS, Miniparc 2, 478 rue de la Decouverte, 31670 Labege, France

CNRS, LAAS, 7 avenue du colonel Roche, F-31400 Toulouse, France

Univ. de Toulouse, UPS, LAAS, F-31400 Toulouse, France

Keywords:

Industrial Robotics, Cobot, Reactive Planning, Obstacle Detection, ROS Architecture.

Abstract:

A challenge for roboticists consists in promoting collaborative robotics for industrial applications, i.e. allowing

robots to be used close to humans, without barriers. Safety becomes the key issue. For manipulation tasks,

a part of the problem is solved using new arms like the KUKA LWR, which is able to physically detect a

collision from torque measurements on each joint. Nevertheless it is better to avoid collisions, especially if

the obstacle is the arm or the head of an operator. This paper describes how obstacles could be detected and

avoided, using a single Kinect sensor for the monitoring of the workspace and the reactive planner of the

KineoWorks

T M

software library for the real-time selection of an avoidance trajectory. Experimental results

are provided as a proof that this dynamic obstacle avoidance strategy works properly.

1 INTRODUCTION

An important challenge for industrial countries con-

cerns the renovation of industrial sites, with several

objectives, such as higher productivity, improved ver-

satility and ﬂexibility of the production system, and

improved safety for the workers, especially the reduc-

tion of injury and musculoskeletal disorders.

For this reason new robots appeared 10 years ago,

such as the KUKA Light-Weight-Robot (LWR) or the

Neuronics Katana arms, among others. These robots

have been designed with respect to the ISO standard

10218-1 deﬁning the safety rules to be respected by

collaborative robots, i.e. robots which can be de-

ployed close to humans. Figure 1 shows some con-

texts for the deployment of collaborative robots as co-

workers for industial applications. We study these use

cases in the context of the Industrial Cooperative As-

sistant Robotics (ICARO) project.

In the use case considered in this paper, it is

mainly a manipulator executing motions close to an

operator. The complete experiment involves a phys-

ical interaction between the robot and the operator,

i.e. the operator executes an insertion on an object

grasped by the manipulator. In the Factory of the Fu-

ture, such collaborative tasks will be authorized only

if safety is guaranteed for the operator, i.e. by the use

of intrinsic and extrinsic behaviours to minimize col-

lision risks, especially with the operator.

These robots are endowed with intrinsic safety

behaviours; they are able to detect a collision from

torque measurements made in real time on every joint.

This is not sufﬁcient, since an operator could be in-

jured by collisions before the robot stops effectively.

Therefore it is necessary to detect from extrinsic sens-

ing, the presence of obstacles along a planned trajec-

tory, and then to modify the trajectory online, so that

these obstacles are avoided. First, using the modules

available on the ROS web site (ros, 2010), it is possi-

ble to perform an experiment with the robot stopping

as soon as an obstacle is detected on the trajectory,

and restarting as soon as the path is free.

This paper presents a more reactive approach, in-

tegrating the ROS middleware with the reactive plan-

ner developped by Siemens PLM Software (formerly

Kineo CAM), and evaluating using a dedicated set-

up with a KUKA arm available at LAAS-CNRS. It is

also proposed to integrate the SoftMotion controller

developped by LAAS-CNRS for collaborative manip-

ulators interacting with humans, in place of the Re-

ﬂexxes module available with ROS. Presented here is

the principle of the planner with initial experimental

results.

Section II proposes a very synthetic state of the

art. Section III recalls how to use the OctoMap mod-

ule in order to monitor the robot path during motion

execution. It has been improved to be used efﬁciently

for obstacle detection. Section IV describes the reac-

450

Dumonteil G., Manfredi G., Devy M., Confetti A. and Sidobre D..

Reactive Planning on a Collaborative Robot for Industrial Applications.

DOI: 10.5220/0005575804500457

In Proceedings of the 12th International Conference on Informatics in Control, Automation and Robotics (ICINCO-2015), pages 450-457

ISBN: 978-989-758-123-6

 2015 SCITEPRESS (Science and Technology Publications, Lda.)

Figure 1: Some examples where co-workers execute collab-

orative tasks with humans, either during the learning step of

an action, or for executing assembly operations in cluttered

environments.

tive planner, while Section V shows how it has been

integrated with the other ROS modules. Section VI

proposes some experimental results. Finally section

VII summarizes our contribution and speaks about

our on-going works.

2 STATE OF THE ART FOR

OBSTACLE DETECTION AND

AVOIDANCE

Obstacle detection and avoidance is a classical topic

in robotics, that has been overall studied for mobile

robots navigating in cluttered environments. Con-

cerning manipulators, generally, it is considered that

the robot is alone, knows perfectly its workspace, so

that paths are always generated in the free space; these

hypothesis are no more possible for collaborative ma-

nipulation tasks.

Several recent works address dynamic obstacle

detection and avoidance while an arm executes a

planned trajectory. Flacco et al (Flacco et al., 2012)

present a simple variant of a classical potential ﬁeld

method for safe human-robot cooperation. It enables

real-time generation of repulsive commands for the

robot to avoid collisions. First the concept of depth

space is introduced in order to evaluate distances be-

tween the robot and possibly moving obstacles, and to

estimate their velocities, directly from depth images

acquired by a Kinect sensor. These distances and ve-

locities are exploited to generate repulsive vectors that

are integrated in the control law applied on the robot

while executing a generic motion task. The complete

collision avoidance framework, has been validated on

a KUKA LWR IV executing tasks in a dynamic envi-

ronments with obstacles and a human.

Yang and Brock (Yang and Brock, 2010) pre-

sented an original solution, though lacking convinc-

ing experimental validation and generic obstacle de-

tection capabilities. It is based on a novel motion gen-

eration technique, called elastic roadmap, proposed to

generate robust and globally task-consistent motion in

dynamic environments. Building and following a re-

active path in real time, the motion must satisfy dif-

ferent classes of constraints for a redundant mobile

manipulator for which the end effector follows a path.

Considering our use cases about collaborative

robotics, these two approaches have important draw-

backs. Flacco et al reacts only at control level by try-

ing to increase distance to obstacles; Yang and Brock

provide a solution taking advantage of a mobile arm,

but they do not take into account human constraints.

In our applications, the set-up is simpler, with only

a ﬁxed manipulator (without considering that this arm

is mounted on a mobile platform by now); this arm

has to execute a plan, and to reactively adapt this plan

if an obstacle is detected. The path is directly re-

planned in real time.

The ROS-based robot motion control architectures

make use of the Reﬂexxes framework (Kr

oger and

Wahl, 2010) (T.Kr

oger, 2011) which allows to trans-

form a path in an on-line time-deﬁned trajectory. This

intermediate layer allows to generate jerk-limited and

continuous motions, taking into account constraints

on the dynamic robot capabilities with low latencies

(1ms for the low-level control loop). Our reactive

planner has been ﬁrstly integrated with the Reﬂexxes

framework.

Even if the Reﬂexxes framework is very efﬁcient,

it does not guarantee an error bound between the ini-

tial path and the executed trajectory. The current

version of our system takes proﬁt of the SoftMotion

framework as the robot controller (Broquere et al.,

2008) (Broqu

ere and Sidobre, 2010). The SoftMo-

tion controller has been independantly developped in

order to generate motions for collaborative manipula-

tors, i.e. soft motions more tolerable by an operator;

this module was previously validated and adapted for

a KUKA LWR-IV arm, as it was presented in (Zhao

et al., 2014). Our reactive planner produces a path

to be executed by the robot as a polygonal line de-

ReactivePlanningonaCollaborativeRobotforIndustrialApplications

451

ﬁned by a sequence of via-points. Using smoothing

techniques, SoftMotion computes ﬁrstly a trajectory

stopping at via-points and then smooth the trajectory

near each vertex. The smoothed area is managed to

maintain the moving parts inside a pre-deﬁned tube,

so respecting error bounds speciﬁed by the user. The

system can adapt the kinematic bounds (velocity, ac-

celeration and jerk) in real time making the trajectory

more acceptable to humans.

3 OBSTACLE DETECTION

The Kineo

T M

Collision Detector (KCD) provides fast

and reliable collision detection, based on minimal dis-

tance computations. This module is synchronized

with the OctoMap module (Hornung et al., 2013).

The OctoMap is updated at 30Hz with the point

clouds acquired by an Xtion PRO LIVE RGB-D cam-

era.

In the OctoMap updating process there is a com-

promise between updating speed and noise: the faster

the update, the easier noise appears. Has we are do-

ing reactive planning, we want the OctoMap to be up-

dated as quickly as possible. In order to avoid the

introduction of noise in the OctoMap, we must use ro-

bust noise ﬁlters. In fact, before a point cloud is added

to the OctoMap, it undergoes three ﬁltering steps. The

ﬁrst step removes the points corresponding to NaN

depth values, typically points at inﬁnity or on specu-

lar surfaces, and points out of reach of the robot’s arm,

with which there is no possible collision. The second

step ﬁlters out parts of the point cloud corresponding

to robot joints. During this step, a correct hand-eye

calibration is crucial as it will determine the quality

of this ﬁltering. Though most of the noise sources are

now removed, a third step is necessary. Indeed, the

sensor can generate spurious points at random posi-

tions which impair the reactive planning by blocking

free paths.

In order to limit the apparition of such sparse out-

liers, we use a statistical ﬁlter (Rusu and Cousins,

2011) available in the PCL library (pcl, 2010). For all

points, the statistical ﬁlter measures the distance be-

tween every point and its closest N neighbors. These

data are ﬁtted to a Gaussian. A threshold is set in

function of the mean and variance of the Gaussian.

Every point for which the mean distance to its neigh-

bours is higher than the threshold, is considered as an

outlier and is removed.

4 SIEMENS REACTIVE PATH

PLANNING ROS PACKAGE

As part of the ICARO project, we have developed a

reactive path planning package over the ROS mid-

dleware. This kws ros interface package (hereafter

kws ros), is built on top of the KineoWorks

T M

soft-

ware component. KineoWorks addresses all aspects

of motion processes including collision-free auto-

mated path planning. It includes the most common

motion types, such as joint motion, and features an

advanced algorithm for detecting collisions along a

trajectory, which is both fast and exact, regardless

of kinematical complexity. The reactive path plan-

ning involves two packages: the kws ros package

dedicated to trajectory planning, interacts with the

Kineo

T M

Collision Detector (hereafter, KCD).

4.1 General Structure

The kws ros package contains two nodes, one in

charge of all collision-free path planning tasks and the

other performing reactive control (Yoshida and Kane-

hiro, 2011) over the executing trajectory. Both nodes

embed a representation of the scene into KCD. This

one has to be deﬁned ofﬂine in a dedicated CAD mod-

eller, the Kite module, by providing CAD models of

static obstacles and parts of the robot, together with

its kinematics model. The representation is loaded at

node launch and it is then enriched, for reactive pur-

pose, by the dynamic point cloud data such as those

obtained through laser scanning or optical vision sen-

sors. KCD enables collision detection between point

clouds and the rest of the environment. Each one of

these nodes is implemented using a robust state ma-

chine fully driven by the ROS actionlib protocol al-

lowing multi process communication. Both nodes up-

date the robot position by a ROS topic and provide

convenient topics to add, move, remove, attach and

detach geometric parts on the ﬂy. This allows cover-

ing a wide variety of scenarios from simple pick and

place tasks to complex human collaboration tasks.

4.2 Reactive Path Planning

Reactive path planning is mainly performed by the

controlling node which basically interacts with the

path planning node and some critical nodes execut-

ing the global task, here grouped in the ICARO stack,

such as the trajectory execution node. The reactive

strategy is implemented in its state machine by four

states described in ﬁgure 2.

WAITING State: A simple state waiting for user

input.

ICINCO2015-12thInternationalConferenceonInformaticsinControl,AutomationandRobotics

452

Figure 2: Controlling node state machine. Colors correspond to different actionlib types. Arrows indicate sending or receiving

events.

PLANNING State: The user has requested a robot

motion. The controller requests a path plan via

the path planner node and waits for the result. For

robustness, a fallback strategy is implemented to retry

the path planning in case of failure if a predeﬁned

timeout has not yet been reached. If the path planner

node is still failing, then the robot motion is canceled.

Oterwise, the controller retrieves a collision free

trajectory and sends it for execution via the trajectory

executor node.

MONITORING State: This state is entered when

a trajectory is being executed by the robot. The

controller monitors the robot position to extract the

remaining trajectory to follow and test it for collision

in a fast way, without distance computation. If no

collision is detected, it continues monitoring until full

trajectory execution. If a collision is detected then it

switches to the next state.

MONITORING

WITH PLANNING PENDING

State: In this state, the controller has already detected

a collision over the trajectory. Therefore, it sends

a new path plan request to the path planning node

with the current robot position as start conﬁguration

and the initial user goal conﬁguration. Then it con-

tinues to monitor the remaining trajectory. Collision

detection in this state is applied more accurately;

it includes distance computation. If the minimal

distance allowed between the robot and the environ-

ment is not yet reached, it continues to monitor and

waits for the response of the path planning node. A

fallback strategy is also implemented in this state

in case of path planning failure. Several exit events

are taken into account: if the trajectory is no longer

colliding, the controller cancels the path plan request

and switches back to the monitoring state; if the

path planning node fails to ﬁnd a new collision free

trajectory, or reaches the timeout, then the robot

motion is stopped and the controller returns to the

initial waiting state. And ﬁnally, if the path plan

succeeds on time, the current executing trajectory is

cancelled and the controller switches to the monitor-

ing state which sends the new trajectory for execution.

Since a small amount of time can be spent by the

path planning node to ﬁnd a new collision free tra-

jectory, the robot position will not be the same as the

start conﬁguration of the new trajectory. To avoid the

ReactivePlanningonaCollaborativeRobotforIndustrialApplications

453

robot to go backwards during the new trajectory ex-

ecution, a fast collision free optimization is done on

the ﬁrst four conﬁgurations.

5 INTEGRATION USING THE

ROS MIDDLEWARE

The ICARO project has performed meaningful exper-

iments, related to the use cases illustrated on ﬁgure 1.

These experiments involve numerous modules (plan-

ning, perception on the operator or on objects, gesture

recognition, object recognition, object grasping, con-

trol. . . ) which are integrated using the ROS middle-

ware (ros, 2010). This paper is only concerned by the

reactive motion control modules; so we do not present

modules related to perception or sensor-based control.

The set-up can be seen on ﬁgure 6, with a Kinect

sensor mounted on a pan and tilt platform above

the KUKA LWR ARM. The arm is mounted on a

Neobotix mobile robot, but here neither the pan and

tilt platform or the robot are exploited for this paper.

The calibration process has been executed ofﬂine, us-

ing (opencv, 2010) for the Kinect calibration and (pcl,

2010) in order to estimate the Kinect reference frame

with respect to the arm’s frame, executing known mo-

tions of the arm, while matching the arm bodies in the

depth images acquired by the Kinect sensor.

A ﬁrst robot motion control architecture pre-

sented on Figure 3 has been integrated and vali-

dated. It uses the standard OMPL module for path

planning, and the Reﬂexxes framework (T.Kr

oger,

2011) for motion generation. The experiment pre-

sented in the next section, have been performed us-

ing the architecture presented on Figure 4, where the

kws ros interface for the planning function and the

Kineo

T M

Collision Detector (KCD), replace the stan-

dard ROS modules. The planned trajectory has to be

!"#$%

&'!%

("))*+*",%

$,#*'",$!$,-%

"(-"!&.%

-'&/$(-"'0%

1*)-$'%

'$1)$22$+%

()"34%

1*)-$'+%

+$,+"'%

'"5"-%

&'!%

6"&)%

'$73$+-%

"!.)%

.)&,,$'%

Figure 3: Architecture using available modules on (ros,

2010).

Figure 4: Architecture integrating the KCD and Kws mod-

ules with Reﬂexxes.

Figure 5: Architecture integrating the SoftMotion frame-

work for motion generation.

pre-processed for feeding the Reﬂexxes module in or-

der to generate motions.

In the last version, we have replaced Reﬂexxes by

the SoftMotion module, which generates better mo-

tions with respect to the operator acceptability; see

Figure 5.

6 EXPERIMENTAL RESULTS

The reactive path planning has been successfully

tested for hours in different simulated environments

and real scenarios, on both single computer and multi

computers conﬁguration. A video (video, 2014) illus-

trates some experimental results; the following ﬁgures

are extracted from this video.

The KUKA arm is programmed to execute a sim-

ple trajectory: a loop between two positions A and B.

An operator perturbs this process, introducing an ob-

stacle on the path. Figure 6 presents the initial A and

ﬁnal B positions of this nominal trajectory.

Only two situations are illustrated here. Figure 7

shows a situation where the planner fails in ﬁnding

a new path that avoids the obstacle. Therefore it is

blocked until the obstacle is removed and disappears

fromthe OctoMap. Nevertheless due to the latency on

the OctoMap, it modiﬁes the nominal trajectory going

under a virtual obstacle, created by isolated points re-

maining in the map.

ICINCO2015-12thInternationalConferenceonInformaticsinControl,AutomationandRobotics

454

Figure 7: Experiments: the operator has blocked the arm which stops as soon as the obstacle apperas in the map. Here the

generation of a new path failed; so the arm restarts as soon as the obstacle disappears from the map; nevertheless, due to a

latency in the map, it avoids a virtual obstacle going under virtual points.

Figure 6: Experiments: execution of a nominal trajectory

from an initial point (left) to a ﬁnal one (right).

Figure 8 shows a replanning process. The planner

ﬁnds a new path, going above the detected obstacle.

Many other situations have been tested.

7 CONCLUSIONS

This paper presented a reactive planiﬁcation algo-

rithm, that has been integrated in a ROS architecture

devoted to the execution of collaborative manipula-

tion tasks between a KUKA LWR arm and an opera-

tor. Such a planner, associated with a module dealing

ReactivePlanningonaCollaborativeRobotforIndustrialApplications

455

Figure 8: Experiments: a dynamic obstacle avoidance. The operator tries to block the arm, which generates a trajectory

towards the goal, through a subgoal under the obstacle.

with obstacle detection, guarantees that a robot arm

could detect and avoid a human before any collision.

The detection module is built upon the OctoMap

algorithm already integrated under ROS. It has been

necessary to adapt OctoMap and to apply speciﬁc tun-

ings so that (1) detected obstacles disappear from the

map as soon as the corresponding actual objects are

no longer on the trajectory the robot is executing, and

(2) the robot is not itself detected as an obstacle.

As soon as an obstacle detection occurs, the plan-

ner module tries to ﬁnd another path, continuous with

the initial ones in order to reach the goal. If it fails,

the robot performs as it would do by now with the

OMPL module under ROS: it waits until the obstacle

disappears from the map.

As a future work, the OctoMap module has to be

modiﬁed in order to minimize the latency when an

obstacle appears or disappears in the map. Moreover

the link between the kws ros reactive planner and the

SoftMotion controller has to be reﬁned in order to

make more dynamic the obstacle avoidance.

ICINCO2015-12thInternationalConferenceonInformaticsinControl,AutomationandRobotics

456

ACKNOWLEDGEMENTS

This work has been funded by the ICARO project

from the ANR CONTINT program (french Research

Ministry) and by the CAAMVIS project from the

AEROSAT program (french Industry Ministry and

Midi-Pyr

ees region).

REFERENCES

Video (2014): homepages.laas.fr/michel/documents/ Reac-

tivePlanningJido2LAAS HD.mp4.

Open CV (2008). OpenCV: Open Computer Vision library.

opencv.willowgarage.com/.

PCL (2010). PCL: Point Cloud Library.

www.pointclouds.org.

ROS (2010). ROS: Robot Operating System. www.ros.org.

Broqu

ere, X. and Sidobre, D. (2010). From motion plan-

ning to trajectory control with bounded jerk for ser-

vice manipulator robots. In IEEE Int. Conf. Robotics

and Automation, Anchorage (USA), pages 4505–4510.

Broquere, X., Sidobre, D., and Herrera-Aguilar, I. (2008).

Soft motion trajectory planner for service manipulator

robot. Proc. IEEE/RSJ Int. Conf. Intelligent Robots

and Systems (IROS) Nice (France), pages 2808–2813.

Flacco, F., Kroger, T., De Luca, A., and Khatib, O. (2012).

A depth space approach to human-robot collision

avoidance. In Robotics and Automation (ICRA), 2012

IEEE International Conference on, pages 338–345.

IEEE.

Hornung, A., Wurm, K., Bennewitz, M., Stachniss, C., and

Burgard, W. (2013). Octomap: An efﬁcient proba-

bilistic 3d mapping framework based on octrees. Au-

tonomous Robots Journal.

oger, T. and Wahl, F. (2010). On-line trajectory gener-

ation: Basic concepts for instantaneous reactions to

unforeseen events. IEEE Trans. on Robotics, pages

94–111.

Rusu, R. and Cousins, S. (2011). 3d is here: Point cloud

library (pcl). In Proc. IEEE Int. Conf. on Robotics

and Automation (ICRA), pages 1–4.

T.Kr

oger (2011). Opening the door to new sensor-based

robot applicationsthe reﬂexxes motion libraries. In

Proc. IEEE Int. Conf. on Robotics and Automation,

Shanghai, China.

Yang, Y. and Brock, O. (2010). Elastic roadmapsmotion

generation for autonomous mobile manipulation. Au-

tonomous Robots, 28(1):113–130.

Yoshida, E. and Kanehiro, F. (2011). Reactive robot motion

using path replanning and deformation. In Proc. IEEE

Int. Conf. on Robotics and Automation, IEEE press,

Shanghai, China.

Zhao, R., Sidobre, D., and He, W. (2014). Online via-

points trajectory generation for reactive manipula-

tions. In IEEE/ASME Int. Conf. on Advanced Intelli-

gent Mechatronics (submitted to AIM 2014), Besanon,

France.

ReactivePlanningonaCollaborativeRobotforIndustrialApplications

457