IEEE Robotics & Automation Magazine - March 2016 - 65

necessary to observe the target movement and bootstrap its
internal state, the robot starts placing its hand at the closest
point on the plane to the predicted goal position. The hand
position is corrected periodically using either the classical or
the EP-based controller. The iCub repeats this procedure at
each oscillation. The aim is to have the hand at the goal position when the ball reaches the extremal position and to
achieve this through a computationally efficient and energysaving methodology.
System Model
The target movement is approximated as a damped oscillation
of a two-dimensional (2-D) pendulum on a plane A rotated at
an angle a around the Y p vertical axis (see Figure 2). Deﬁning
p as, respectively, angular position, velocity, and
H, Ho , and H
acceleration, g as gravity, L as wire length, n as damping factor, and m as ball mass, the equation for describing the motion
of the simpliﬁed model of the 2-D pendulum is
Hp +

n

m

Ho +

g
sin (H) = 0.
L

<
g
, C x, C y, C z, aE .
m L

n

,

Extremal Position
B(Tj)(xm, ym, zm)

Yp

(zm - d)

Z

Xp

Zp

X

Figure 1. The top view of the reaching task performed by the
robot. During each oscillation, the iCub moves its right hand to
the position G ^T j h = ^ X m, Ym, d h, where B ^T j h = ^ X m, Ym, Z mh is the
position of the target at the minimum distance ^ Z m - d h during
the oscillation.

C
A

i

(2)

L

The state vector includes all unknown variables, either
time varying or constant. Because only H and Ho are timevarying, we have to estimate the others to make good predictions. Using (1) and (2), the transition and observation models become, respectively
R
V
x2
S
W
S-x 3 x 2 - x 4 sin (x 1)W
W
f (x) = x + Dtxo = x + Dt S
0
S
W
h
SS
WW
0
T
X
R
V
g
Sx 5 + sin (x 1) cos (x 8)W
x4
S
W
S
W
g
h (x) = S x 6 - cos (x 1) W,
x
4
S
W
S x 7 - g sin (x 1) sin (x 8) W
S
W
x4
T
X

Y

Pivot

a

Goal
G(Tj)(x, y, d)

(1)

Because there is uncertainty in the parameters n, m, L, the
pivot position ^C x, C y, C z h, and a, we opt to estimate not
only the motion variables but also the pendulum parameters.
This results in an eight element state vector:
x = ;H, Ho ,

Sliding Plane

a

m
Yp

(3)
Figure 2. The pendulum model showing a 2-D pendulum
oscillating on a plane A rotated at an angle a on the Y p -axis.

(4)

where x 1, f, x 8 are the entries of state vector x.
The Control Systems
Two control modalities were tested and compared. Figure 3
shows a block diagram showing the main components of
both controllers. Their input is the three-dimensional (3-D)
position of the ball B (t), and the outputs are the arm motor
commands expressed in joints' velocities v q (t). The ﬁrst
control system is based on a standard architecture, while

the second one is based on the EP concept. Both systems
have an internal model of the pendulum implemented with
an extended Kalman filter (EKF). The ﬁlter input is the
target position computed by the visual processing block,
B (t). This position is used by the ﬁlter to update its internal
state x. The EKF is iterated through time to obtain the
predicted trajectory of the ball for future time steps
B (t + k ; t), k = 1f (T j - t). The predicted extremal position B (T j ; t) is computed as the position for which velocity
changes sign in x or y, and is used to obtain the predicted
goal position G (T j ; t). The inverse kinematics solver uses
G (T j ; t) to compute the desired target robot joints' angles
G q (T j ; t), and the controller computes the motor joints'
velocity trajectory v q (t) [17]. The controller frequency is
march 2016

*

IEEE ROBOTICS & AUTOMATION MAGAZINE

*

65
Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - March 2016
