IEEE Robotics & Automation Magazine - June 2018 - 46

.
x

(b)

(a)

Figure 9. The velocity limitation varies smoothly as a function of
the separation distance: (a) limited velocity; (b) full velocity.

[12]. Hence, kinetic energy is a major concern when it comes
to safety, and, as such, it should be limited. For a rigid body of
2
mass m, the kinetic energy is defined as E k = m v /2. For
rigid manipulators, an equivalent mass (perceived at any collision point on the robot structure) can be derived using the
joint's dynamic model [11], [13]. Therefore, in both cases,
limiting the kinetic energy can be seen as a form of velocity
limitation, with the mass (real or equivalent) acting as a scale
factor. As such, constraint C kin is equivalent to C vel, with
Vmax = 2E k max /m .
Separation Distance
When the separation distance between the robot and nearby
operators is monitored, it can be used to adapt the aforementioned limits (e.g., on velocity and power). It is also required
to fulfill the Speed and Separation Monitor mode of ISO/TS
15066. For instance, a low level of security may be required
if no one is present in the surroundings, whereas very strict
limitations may be imposed when a robot is working near
or in collaboration with humans. A simple example is
depicted in Figure 9. To comply with this, we use fifthorder polynomials, implemented in OpenPHRI (OpenPHRI/utilities/fifth_order_polynomials.{ h , c p p } ) , t o
allow a smooth adaptation of the limits, depending on the distance to
Power can be limited at
the closest operator or
to any other object to be
the hardware level, e.g.,
avoided. Then, any limit
(e.g., Vmax) will vary from
the electric power, as with
a low value at a fixed
minimal distance to a
the Kuka LWR4+, or at the
large value at a higher
control level, as we do here. distance and may vary
smoothly in between.
Benchmark Tests
In pHRI, for the robot to react quickly in the case of an
impact or to be as transparent as possible when physically collaborating with a human, its control loop should run at a
minimum of 1 kHz. It is, therefore, crucial that the implementation of our controller in OpenPHRI be fast enough to comply with this timing constraint. To assess the performance of
46

IEEE ROBOTICS & AUTOMATION MAGAZINE

JUNE 2018

our library, we ran some benchmark tests on a computer
equipped with an Intel i7-6700HQ at 2.6 GHz, operating Linux
4.11. Here, we refer to a benchmark in the computing (not
robotics) sense-i.e., the act of running a computer program
or a set of programs to assess their performance.
In Figure 10, we present the results of the benchmark tests
for the controller associated with different constraints and
force and velocity inputs, running on a 7-DoF manipulator.
At each iteration, the controller is run 10,000 times to get
meaningful results, and the average computation time is
logged. In Figure 10(a)-(e), we give the average computation
time tr and the standard deviation v over 1,000 iterations.
The computation of the forward and inverse kinematics is not
included in these results so that we may focus on the control
computation time overhead. Also, the current controller
implementation is single-threaded, but, given the very low
computation time [tr 1 4 ns in the most complex scenario
presented in Figure 10(e)], a multithreaded version does not
seem necessary. Figure 10(f) shows that the memory usage
(measured using the Massif tool from the Valgrind software)
stays very low, with a peak at 186 KiB. (Here, the abscissa
indicates snapshots taken regularly during execution.)
Experiments
In this section, we present the results of a full-featured
experiment using the framework described in this chapter.
The experiment is split into two phases: 1) a teaching-bydemonstration phase and 2) a replay phase, in which the
robot operates autonomously in the presence of an obstacle
and near the human operator. Figure 11 shows the setup,
consisting of a Kuka LWR4+ arm with external force fext,
estimated through the Fast Research Interface (FRI) [17].
All the code was written in C++ using the OpenPHRI
library and, to interface with the hardware, integrated
inside the Knowbotics framework, currently under development at Laboratoire d'Informatique, de Robotique et de
Microélectronique de Montpellier (a public release is
expected once the software becomes mature enough). The
FRI library was used to communicate with the Kuka arm.
The controller sample time was T = 1 ms. To manage the
robot behavior, we used OpenPHRI to design the finite
state machine (FSM) shown in Figure 12.
It is important to note that our framework is used continuously throughout both the teaching and replay phases.
An equivalent application using the V-REP simulator is
available in the OpenPHRI repository under apps/demo
[18]. The whole application has fewer than 600 lines of
code: 125 for the main file and 440 for the FSM (header
plus source). For the FSM, most of the code just adds or
removes inputs and constraints to fit with Figure 12, so
one can expect more or less code to write depending on
the FSM complexity.
The teaching phase consists in manually guiding the robot
[Figure 13(b) and (c)] by applying fext ! F, to teach it the
waypoints where it should later realize a force-control task
(applying fr = 30 N for 2 s perpendicularly to the end

IEEE Robotics & Automation Magazine - June 2018

Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - June 2018