The 2009 IEEE/RSJ International Conference onIntelligent Robots and Systems
October 11-15, 2009 St. Louis, USA

Planning-Space Shift Learning: Variable-space Motion Planning towardFlexible Extension of Body Schema
Yuichi Kobayashi and Shigeyuki Hosoe
Abstract— To improve the flexibility of robotic learning, it
is important to realize an ability to generate a hierarchicalstructure. This paper proposes a learning framework which
can dynamically change the planning space depending on the
structure of tasks. Synchronous motion information is utilized
to generate modes and different modes correspond to different
hierarchical structure of the controller. This enables efficient
task planning and control using low-dimensional space. An
object manipulation task is tested as an application, where an
object is found and used as a tool (or as a part of the body)to extend the ability of the robot. The proposed framework
is expected to be a basic learning model to account for body
image acquisition including tool affordances.

I. I NTRODUCTION
Hierarchical learning is an important issue to improvelearning ability of robots. In some researches of hierarchicallearning, navigation in large building like complex mazes[1], [2] or controlling a robotic arm with multiple joints [3]are investigated as examples of large and complex problems.
Apart from those high-dimensionalities, one aspect of thecomplexity in robot tasks is that it involves objects whose
shapes can vary depending on situations and the objects
can play various roles; a target to be carried, an obstacle
to avoid collision, or a tool which can be used to achievedifferent objectives. The existence of the objects makes tasks
diverse (various objectives and situations), complex (different
dynamics depending on contact or non-contact) and highdimensional (configuration space).
As an approach to the complex control problems, it is
known that some behaviors of the robotic system can berealized by combination of multiple modules, each of which
has relatively small dimension, even though the DOF ofthe total system is very large. For example, subsumption
architecture [4] realized flexible and adaptive behaviors oflocomotion robots with high DOFs, while individual modulein the architecture played rather simple role such as collision
avoidance or simple maneuver. Such architectures realize
flexibility by focusing on just a part of the total system,
where the part in focus flexibly varies depending on thesituations and objectives.
In this paper, a learning architecture that can accountfor such variable focus in robotic motion learning with
objects is proposed. A ‘part in focus’ is interpreted as aspace for motion planning in this research. By changing or‘shifting’ the planning space, the architecture can be applied
Y. Kobayashi is with Tokyo University of Agriculture and Technology,
2-24-16 Koganei Tokyo, Japan, yu-koba@cc.tuat.ac.jp
S. Hosoe is with RIKEN-TRI Collaboration Center for Human-Interactive
Robot Research, Nagoya, Japan, hosoe@nagoya.riken.jp

978-1-4244-3804-4/09/$25.00 ©2009 IEEE

to diversity of tasks. In the proposed architecture, variables
to be controlled can be variant, differing from the multiplemodule learning model [5] where the state variables andcontrol variables have to be invariant.
One possible application of planning-space shift learning
is a problem of adaptive tool-use. An object can be utilized
to extend reachable region of a robot when the robot canmove it. That is, the robot can use the object as a tool.
The ability of tool-use has gathered attention partly because
it is deeply related to the issue of affordance [6] andbody schema, that are frequently discussed in the field ofdevelopmental robotics [7]. Stoychev proposed a behaviorbased approach to realize representation of tool use [8].
Nabeshima et al. also proposed a learning framework fortool-use [9]. This paper aims to propose a more generaland more ‘bottom-up’ description of hierarchy generation.
Thus, the proposed architecture does not utilize any explicit
representation of ‘tool’ and it uses just the information of
motion synchronousness.
In II, problem settings for the proposed learning architecture is described. The learning architecture is proposed in
III, followed by evaluations by simulation in IV.
II. P ROBLEM D ESCRIPTION
For simplicity, it is assumed that all motions of the robot
and the objects are quasi-static.1) The objects are assumed to
be polyhedral or spherical rigid bodies. Let n be the numberof joints of the robot arm and m the number of the objects,
which move only through contacting with the robot hand orother objects, i.e., they do not move by themselves.2) The
state variables of the system are the followings:• Variables which express the configuration of the robot;θ ∈ Θ ⊂ Rn
• Variables which express the configuration of the object;
q 1 ∈ Rn1 , · · · , q m ∈ Rnm
The objective of the robot task is to move an object to acertain desired configuration. Which object should be moved
and its desired configuration are given on-line. The agent canobserve the following variables in addition to configurationof the robot θ :• Position of the robot hand (end effector) h(θ)• Configuration variables of the objects measured fromimage inputs to the robot
These variables are presented to the agent as the observation
vector y ∈ Y ⊂ Rℓ . Initially, these variables are not distin-

3107

1) Thus, velocity components can be omitted from state variables.
2) It is assumed that the robot contacts with objects only at its hand.

guished by the agent and the number of the object or DOFs
of motions of them are also unknown. The configuration
variables of objects are denoted by [q T1 , · · · , q Tm ]T . The i-thcomponent of observation variable y is denoted by yi (t), i =1, · · · , ℓ3) . The agent does not know the correspondence
between the elements of y and the observed position of therobot hand, nor the functional relations among the elementsof y (including the kinematics of the arm).
At each time step, the robot can change its configuration
variables. That is, u = ∆θ is the control input to the system.
The followings are assumptions on the tasks:• Contact of a new object with moving ones and the hand
happens only to the object which has begun moving last.
It is also assumed that only one object will start moving
at a time. Therefore the moving objects and the handform a chain.
• The movement of an object is affected only by theobject which is positioned closer to the robot hand andtherefore the direction of the effect of object motions is
unidirectional.
• The relationships between any two moving (and contacting) objects (hand) are described by mappings
among the configuration variables of them without
redundancy. The mappings do not depend on the contact
positions on the objects.
III. L EARNING S TRUCTURE
The learning structure proposed in this paper is constitutedby the four basic functional components. An overview will
first be presented in the following subsection and following
to this the detail of the algorithm will be described.
A. Basic idea of planning-space shift
The agent repeats trial motions while resetting the configurations of the robot arm and the objects (the reset isdone once a certain steps of the motion). The agent initiallyyields random action controls and observes y and collects
them with control u. An example with ℓ = 5 is shown in
Fig.1 and Fig.2. Initial random movements of the robot bodycauses changes of the corresponding observation variables.
By detecting synchronous motions through the observation ofchanging variables in y, one can find a group of observation
variables which are directly affected by the motion of therobot itself (these correspond to the configuration variable of
the robot hand). In this example, y3 , y4 and y5 are grouped asa set (Fig.1). Let Y1 ⊂ R3 (corresponding to {y3 , y4 , y5 }).
By collecting the data of θ and y3 , y4 , y5 for some time
period, one can obtain a functional relationship between θand y3 , y4 , y5 represented as a mapping from Θ to Y1 andthe inverse mapping from Y1 to Θ.
After some exploration, it may happen that variables y1
and y2 also change. This change of {y1 , y2 } is regarded tobe caused by the change of {y3 , y4 , y5 }. This observation
corresponds to the fact that an object (corresponding to{y1 , y2 }) starts moving by contacting with the moving robot
3) In this paper, i-th component of vector x is denoted by x .
i

y1 y2

desired trajectory
as commandplanning inY 1

y1 y2

y3 y 4 y5yyplanning inY 2

y3 y 4 y5desired trajectory
as commandurobot body &environmentuurobot body &environmentuFig. 1.
Mode 1: generation of Fig. 2.
Mode 2: generation ofcommand u from Y1
trajectory in Y1 from one in Y2

hand (corresponding to {y3 , y4 , y5 }). Similarly to the above,the second group of variables Y2 ⊂ R2 (correspondingto {y1 , y2 })is introduced. The case where only variables
y3 , y4 , y5 change and the case where y1 , · · · , y5 change arecalled mode 1 and mode 2, respectively. Similarly to the caseof mode 1, the inverse mapping from Y2 to Y1 is acquired.
Using the mapping, it is possible to generate a trajectory in
Y1 when a desired trajectory in Y2 is given.
The constructed mappings can be used when a task
is given to the robot system. For instance, let a desiredconfiguration for {y1 , y2 } is given as a cross depicted in
Fig.3. First, a total trajectory with mode transitions (morestrictly, a succession of points on boundaries) is planned asshown as a succession of arrows in the right part of the
figure. Up to point p12 indicated in Fig.3, the trajectory has
to be retained in mode 1, i.e., only variables y3 , y4 , y5 canchange but y1 , y2 must be constant. For this, we can use theconstructed mapping from Y1 to U. After transition to mode
2, the mapping from Y2 to Y1 is used while maintaining
mode 2. Note that in the case of mode 1, the space forplanning is Y1 . Then later in mode 2, the planning spaceshifts to Y2 .
From the perspective of hierarchical structure, control
variables in U ‘obeys’ the trajectory defined by observation
variables in Y1 in mode 1 (Fig.1). Next in mode 2, thetrajectory in Y1 is dominated by the trajectory defined in
Y2 (Fig.2). Thus, different relations of dependence amonggroups of variables can be seen in each mode, where somegroups of variables in the upper layer dominates the behaviorof other groups of variables in the lower layer. The relation
between two groups of variables can be regarded as hierarchy. In this sense, the proposed architecture can be regarded
to generate hierarchy autonomously.
This architecture brings two advantages:• The motion-planning space is not pre-defined. The planning framework can be flexibly applied to various tasks
with different objects and different number of objects.
• The exploration space is divided into lower dimensional
spaces (Y1 and Y2 in the present example) and thismakes the exploration more efficient.
The proposed learning structure has the following functional components:
1) Mode generation by motion synchronousness:
Modes are generated by grouping observation variables
based on motion synchronousness. Observation vari-

3108

Y2

Y2
Y1

desired configurationumode 2

Y1

mode transitionmode 1

p12

uFig. 3.

current configuration

Y1

Planning-space shift among different modes

ables are stored for estimating the boundary between
modes.
2) Acquisition of motion mappings with consideration ofmode keeping and mode transition:
Mapping from a group of observation variables toanother group is estimated. This mapping is used togenerate motion in a mode when a desired motion inanother mode is given. Within each mode, there can be
two strategies for control; 1) to keep the same mode
without any occurrence of mode transition, and 2) tomake mode transition to a certain target mode.
3) Exploration and estimation of reachable region:
Using the motion mappings mentioned above, eachmode is explored. During exploration, observation variables are stored to estimate reachable regions.
4) Planning and control via multiple modes:
Once a desired configuration of a certain object is given
to the robot, the total trajectory is planned by finding
via points on the boundaries among modes (p12 inFig.3). Parameterization of boundaries is used for this.
These processes can be conducted simultaneously, but we
execute and evaluate each process one by one, for simplicity.
The following subsections give the details of the components.

that have been moving is separated and stops moving. In
general, any one of the group of moving objects can contact
with an isolated object and any part of the group of moving
objects can be separated. Based on the assumption describedin II, however, connection of the objects is generated in achain-form (i.e. not in a tree-form) and contact or separation
happens only at the tip (object) of the chain. Thus, at mode
(i)
(j)
(j)
(i)
transition from i to j, either of IC ⊂ IC or IC ⊂ IC
holds. The former corresponds to the beginning of motion of
an isolated object and the latter corresponds to the separationof the object at the tip of the group of moving objects.
Now let k be the current number of the group of observation variables, that is, k−1 objects have been found excluding(i)(j)
the hand. We consider the former case of IC ⊂ IC , thecase where a new object has been found at transition frommode i to j. The observation variables which correspond toan object that begins to move is defined as(j)

Yk+1 = {y (k+1) |y (k+1) ∈ Rf (k+1) }.

i = 1, · · · , ℓ.

(III.4)Note that the distinction between ‘what can be controlled’and ‘what cannot be controlled’ is obtained through thegrouping of the observation variables. The indicator setsof IC1 , · · · , ICk+1 give the observation variables that can becontrolled and the remaining indicators denote that thoseobservation variables are impossible to control so far. The
sense of ‘agency’ is expressed as controllable variables ofY1 , · · · , Yk+1 in this framework.
Suppose that the current mode is i and k-th group ofobservation variables is moving at the tip of the chain. Setof stored observation variables for k-th group is defined as(i)O(k) = {y (k) (t) ⊂ Yk |∀ t s.t. IC (t) = IC }.

Recall that vector y = [y1 , · · · , yℓ ]T ∈ Y ⊂ Rℓ is theobservation vector. y(t) denotes the observed value of y attime t. In order to detect synchronousness among observation variables, time differences of observation variables aredefined as(III.1)

With ∆yi (t), its indicator set IC is defined as
IC (t) = {i1 , i2 , · · · , ik |∆yij (t) 6= 0, j = 1, · · · , k,∆yij (t) = 0, otherwise}. (III.2)
Hereafter, the terminology ‘mode’ will be used to denote
different indicator sets. When a mode that has never beenencountered is experienced at time t and let k be the current
number of modes, mode k + 1 is newly created and corre(k+1)
spondingly the indicator set is represented as IC= IC (t).
Suppose mode transition from i to j, which means the setof moving objects has changed. This include two cases; 1)
an object that has been isolated begins to move by contactingwith the moving objects, and 2) a part of a group of objects(III.3)where f (k + 1) denotes the number of observation variables
that correspond to (k + 1)-th object. Yk+1 is defined as

B. Mode generation using motion synchronousness∆yi (t) = yi (t) − yi (t − 1),(i)y (k+1) = [yi1 , yi2 , · · · , yif (k+1) ]T , ia ∈ IC − IC ,(III.5)The boundaries among different modes can be estimated bycollected observation variables at the moment when a modechanges to another mode. When mode changes from i toj (or j to i) by connection of k-th object with ℓ-th object(or by separation of ℓ-th object from k-th object) 4) , which(i)(j)
means IC ⊂ IC , the database for boundary approximation
is denoted by Bi,j as
Bi,j = {[y T1 , y T2 ]T |y 1 ∈ O(k) , y 2 ∈ O(ℓ) }.

(III.6)

Bi,j is a set of observation variables between mode i and j.
A transition mode set Mi is defined as the set of modes
to which mode transition is possible from mode i. It is
expressed by Bi,j as Mi = {j|Bi,j 6= ∅}.
C. Estimation of mode boundaries using SVM
To get boundary equations between modes, non-linear
Support Vector Machine (SVM), which is known as a nonlinear classifier with kernel functions [10], is used. For
classification between mode i and j, vectors in Bi,j are used.

3109

4) Here note that ‘object’ includes the robot hand.

Let ms denote the total number of data and ns denote thedimension of vectors ([y T1 , y T2 ]T in (III.6)). Vectors in Bi,j
are rearranged into data vector ak ∈ Rns , k = 1, · · · , ms .
d ∈ Rms is a vector with plus or minus ones, where plus
and minus of dk correspond respectively to modes i and j ofak , where i < j. Hereafter, let x = [y T1 , y T2 ]T . In non-linear
SVM with Gaussian kernel, by introducing kernel functionK asµ¶kx − ak k2
K(x, ak ) = exp −,(III.7)σ2where σ denotes a width parameter for the Gaussian kernel,
separation surface between two classes is expressed as
msX

dk wk K (x, ak ) = 0,

(III.8)k=1

where w is a solution of the following optimization problem:
¾
½
1 T
min
w Qw − eT w , e = [1, · · · , 1]T ∈ Rns , (III.9)
w 2
where Q is given byQ=

1+ HH T ,ν

H = D[A − e], ν > 0.

(III.10)

D = diag[d1 , · · · , dms ], A = [a1 , · · · , ams ]T and ν is aparameter for the optimization problem . For implementationof optimization in (III.9), Lagrangian SVM is applied [11].
The boundary information obtained by SVM is used for1) generation of mode keeping (and mode transition) motion
mappings and 2) planning using via points on the boundaries
with parameterization.

∆y (i) = f˜i,j (y (j) (t) + ∆y (j) ) − f˜i,j (y (j) (t)) (III.11)
using the outputs of NN.

Gi , jdesired motion
in Y jFig. 4.

∆y (i )

Yi
motion inwhich realizes desired motion

Motion transformation between Yi and Yj

To generate a motion which keeps the same mode, it isnecessary to explicitly avoid the mode transition in addition
to using the above-mentioned inverse-mapping. Here we
consider that the current mode is j and suppose that it isrequired to avoid mode transition from j to k, where k ∈Mj . Mode keeping motion generator can be designed using
boundary information of SVM. Recall that the discrimination
between two modes is given by F (x) ≶ 0whereF (x) =

msX

dk wk K(x, ak ).

(III.12)k=1

By introducing a potential function as Φ(x) = {F (x)}2 ,an algorithm to derive a motion in Yi to explore in mode jrandomly is shown in Algorithm I.
¶Algorithm I Motion generation with mode keeping³
1) Set f ound = f alse
2) While f ound = f alse repeat:(j)• Set ∆y tmp = rand(j)(j)T• If F ([y+ ∆y tmp T , y (k) ]T ) > ε, k ∈Mj , then f ound = true
• Else repeat for no times (no > 0):
∂Φ
(j)
(j)
a) Set ∆y tmp = ∆y tmp − η (j)∂y(j)b) If F ([y (j)T +∆y tmp T , y (k) ]T ) > ε, k ∈Mj , then f ound = true and break

D. Mode-transition and mode-keeping motion mappingThis section describes how to generate motion in Yi whena desired motion in Yj is given, supposing that there exists
transition of modes from i to j. This motion mapping component described in this section is used in control describedin III-G.
For mode j, motion information is collected throughrandom exploration and stored, where a motion in Yj iscaused by a motion in Yi . The stored data is used to construct
an inverse mapping (regarded as motion transformation)
between groups of observation variables in Yi and Yj .
Fig.4 shows an example of motion transformation between
groups of variables Yi and Yj , denoted by Gi,j . Motion
transformation Gi,j receives a vector in Yj as an input andgives a vector in Yi as an output. As a result it can generatea trajectory in Yi so as to realize a given input trajectoryin Yj . One way to realize such a transformation is to apply
a non-linear function approximator such as the feed-forward
neural network (hereafter denoted by NN)[12]. Let f˜i,j (y (j) )denote the output of NN (after training) for input y (j) . Whendesired displacement ∆y (j) is given at time t, displacementin Yi can be calculated bymotion transformation∆y ( j )(j)3) Output ∆y (i) with ∆y (j) = ∆y tmp using (III.11)µ´Here, η is a coefficient to determine the displacement in
Yj and ε denotes a threshold value to judge that it isclose enough to the boundary of mode transition. ‘rand’denotes a random vector generator. Before deciding ∆y (j) ,it is checked whether current mode j is maintained using
discrimination function F . If it is judged that the modemight change, the small displacement ∆y (j) is modified to
be further from the boundary of the modes using gradient ofpotential function Φ.
E. Parameterization of mode boundary
As described in III-A, it is necessary to parameterizeboundaries between modes to identify where to make transition on the boundary for the total planning accompanied withmode transitions. Let us consider parameterization of boundary between mode i and mode. The parameterization is donefor each discretized y (j) as shown in Fig.5. In this paper, amethod for one dimensional parameterization using nodes
is introduced (here assuming for simplicity that boundaries3110

y( j)

andy2(i )
p' = 1




v 10


 v 20 
q1 − q0






..
2L
V =  ...  ∈ R4L , Q1 = 
∈R .
.


v 1L−1 
q L − q L−1
v 2L−1(III.19)

qi +1y2(i )

p '= 0y1(i )z(i ) ( p' )qimode jy1(i )

mode imode boundary

Fig. 5. Parameterization of bound- Fig. 6. A curve model constructedary by nodes
by nodesare one-dimensional manifolds in two-dimensional space 5)). Thus, a curve for boundary estimation is generated in Yi
space (here we assume that Yi ⊂ R2 ) by nodes.
Now let q k ∈ R2 denote a point on the boundary between
mode i and mode j which we call hereafter k-th node andL denote the number of nodes. A curve from k-th node to(k + 1)-th node, which is defined as k-th segment of thecurve, is defined using following equation as shown in Fig.6.
z (k) (p′ ) = q k + v 1k p′ + v 2k p′2 ,(III.13)

When positions of nodes are given, V can be calculatedusing pseudo-inverse as the minimum-norm solution by V =AT (AAT )−1 Q ≡ A† Q. By connecting all segments, a onedimensional submanifold z(s) can be defined as follows:z(s) ≡ z (k) (p′ ),The positions of nodes are modified so that the curve z(s)lie close to the boundary. It is assumed that the boundaryis smooth. This is realized by an incremental procedure to
reduce energy defined on the curve, where energy functionof the curve is defined with coefficient αext and αint as
E = αext Eext + αint Eint .

2

where v 1k , v 2k ∈ R are coefficient vectors that give theshape of k-th segment of the curve. This curve is designed sothat changing parameter p′ from zero to one corresponds tothe change on the curve from q k to q k+1 . By differentiatingz (k) (p′ ) = v + 2v p′ is obtained. This
(III.13) by p′ , ∂∂p′
1k
2k
gives a tangential vector of the curve. By considering thecontinuity of positions of the curve at the both edge of thecurve segment, that is, by letting z (k) (1) = z (k+1) (0),q k + v 1k + v 2k = q k+1 ,k = 0, · · · , L − 1(III.14)

is obtained as a condition for position continuity. Similarly,
the condition for continuity of tangential vector is given by∂ z (k+1)∂ z (k)∂p′ (1) =∂p′ (0), that is,
k = 0, · · · , L − 2.

v 1k + 2v 2k = v 1k+1 ,i ≤ s < k+1, p′ = s−k, k ∈ Z (III.20)(III.21)

An internal energy for the curve is defined as°
Z L° 2° ∂ z(s) °2°°
Eint =° ∂s2 ° ds.
0Using (III.13), Eint can be expressed as
L
L Z 1 ° 2 (k) °2
XX°∂ z °° dp′ = 4°
kv k2 k2 .
Eint =° ∂p′2 °0(III.22)(III.23)

k=1

k=1

Eext is defined so that the curve coincides with the boundary
when Eext is minimized. By using the potential function Φ
defined between mode i and j, it can be defined as
Eext =(III.15)LX(j)Φ([q Tk , y d T ]T ),(III.24)

k=1

The above-mentioned conditions can be expressed in matrix
form as
· ¸
·
¸
A1
Q1
AV = Q, A ≡,Q ≡,(III.16)
A2
O2(L−1)×1
where matrices are defined as


I2 I2
O


I2 I2


A1 = 
 ∈ R2L×4L ,
.
..


O
I2 I2

I2 2I2 −I2
I2 2I2 −I2
A2 = ..
.
OO

I2 2I2 −I2 O2(III.17)(j)where y d denotes discretized value of y (j) . 6) By iterating
gradient descent updateq k ← q k − ηn

∂E
,
∂q k

k = 1, · · · , L(III.25)

the positions of nodes 1, · · · , L are modified so that the
curve coincides the boundary while keeping smoothness.
After convergence of the iteration of ( III.25), any pointon the boundary can be expressed by parameter s. In thefollowing, parameterization of boundary between mode i andj is referred as z i,j (s).
F. Exploration and estimation of reachable region ∈ R2(L−1)×4L
(III.18)5) One dimensional boundary corresponds to the case with objects andhand expressed in 2-D space.

Observation variables are stored during exploration toestimate reachable region for motion planning. The reachable
region is identified by the parameters on mode transition
boundaries and the observation variables. For memorizationof reachable region (and utilization of it for total trajectory3111

y( j)

y(zi , j sd(i(,lj)),khand[ y1 , y2 ]T

1.5

( j)
d (k )s( i , j ), kd (l )

1

θ2

y*(i* )

y (0)
(i )

y (t )y2(i )
0.5

mode j(i )1ymode imode boundary

0

-0.5
-0.5

)

(1)

0

q1
0.5

q2
1

1.5

2

mode jmode imode 1

2.5

Fig. 9.

Fig. 7.
Discretization of mode Fig. 8. Simulation settings: robot
boundary and observationvariable
hand and objects

planning), parameter for boundary and configuration of object are discretized as indicated as small circles in Fig.7.
Discretized values of observation variables for mode j is
(j)
(j)
(j)
defined as YD = {y d(1) , · · · , y d(n(j)) }, where n(j) denotes
number of discretization of the observation variable. For
identification of boundary parameter, discretized values ofboundary parameter (between mode i and j) is defined as(i,j),k(i,j),k(i,j),k(i,j),k= {sd(1) , · · · , sd(n(i,j)) },where sd(ℓ) denotes ℓSD
th discretized parameter on boundary of mode transition from(j)i to j with k-th discretized observation variable y d(k) andn(i, j) denotes the number of discretization.
Let ik denote current mode and assume that mode
ik has been reached by mode transitions i1 → i2 →· · · → ik . Mode transition sequence is expressed by τ =[i1 , · · · , ik ]T ∈ Nk . Let T denote set of mode transition
sequences and T is updated while exploration as¯¯ ¯ ¯¯ (i) ¯ ¯ (j) ¯T ← T ∪ {τ (t)} if τ (t) ∈/ T ∧ ¯IC ¯ < ¯IC ¯ , i < j,umode i*Planning with mode transition

0.7

0.7

0.6

0.6

0.5

0.5

0.4

0.4

0.3

0.3

0.2

0.2

0.1
0

∆y (i )∆y (1)∆y (i )

object 1 object 2
θ1∆y ( j )target

0.1
0.4

0.6

0.8

1

1.2

(a) 0.55 ≤ q1 < 0.65

Fig. 10.

1.4

1.6

0

0.4

0.6

0.8

1

1.2

1.4

1.6

(b) 0.65 ≤ q1 < 0.75

Estimated boundaries between mode 1 and 2(i)where Rr > 0 denotes a threshold value and y k denotes(i)k-th element in Dτ (g τ ). Reachable region in mode i via
mode transition τ and g τ can be expressed as(i)Rτ (g τ ) =
(i)
(i)
(i)
(i)
{y ∈ Yi |∃ y k ∈ Dτ (g τ ), ky (i) − y k k ≤ Rr }, (III.30)
which will be used in motion planning described in III-G.
G. Trajectory generation and control with mode transitions

Planning with mode transitions described in this section
corresponds
to the dynamical shift of planning space, be(III.26)
cause different observation variables are used in planningwhere τ (t) denotes mode transition sequence at current time within each mode. The total algorithm of trajectory gent. The second condition stands for that new mode sequence is eration and control is given in Algorithm II. A task ismemorized only when the mode transition keeps increasing given to the robot system as a target configuration of anumber of changing observation variables. By this, mode certain object7) . The robot interprets the given informationtransitions with separation between objects (or an object as target mode and target observation variables in the target
and the hand) are omitted. Using discretizations defined mode (1). Using mode transition database and reachable
above, identification of mode transition for mode transition region database, the robot finds an appropriate transition
parameter s(i,j) and observation variable y j can be given as sequence of modes and pair of mode transition parametersg τ = [g sτ T , g yτ T ]T , where
and observation variable at mode transition (2,3). At step 4,
therobot starts control of its body to realize mode transition
(j)
(j)
(j)
k) T
g yτ = [ℓy(i2 ) , · · · , ℓ(ik sequence of τ † with transition parameter g † . At eachy ] , ℓy = arg min ky d(k) − ykτ†(III.27) mode, the robot aims at a target (subgoal) point on themode transition boundary which is specified by discretized
1 ,i2 )
k−1 ,ik ) T
g sτ = [ℓ(i, · · · , ℓ(i] ,ssparameter of boundary curve (b). Once a subgoal is obtained,(i,j),kℓ(i,j)= arg min ksd(ℓ) − s(i,j),k k, k = ℓ(j)(III.28) the robot repeats small movements toward the subgoal whilesy .
ℓtrying to keep current mode. For mode keeping, Algorithm(i)
Let Dτ (g τ ) denote set of observation variables in mode i I is utilized by replacing random value to decide ∆y (i) by
tmpwith mode transition specified by τ and g τ . When y (i) (t) ∆y (i) given in (c)-i. The desired motion ∆y (i) is realized(i)
is observed during exploration, Dτ (g τ ) is updated asby transformations of motions as shown in Fig.9.
(i)(i)
In the case when impossible tasks are given to the robot,(i)Dτ (g τ ) ← Dτ (g τ ) ∪ {y (t)}theprocedures of 1) or 3) can not be achieved.
(i)
(i)
(i)
if ∀ y k ∈ Dτ (g τ ) s.t. ky (i) − y k k > Rr , (III.29)
6) The discretization intervals are set equal in the simulation.

7) Here it is assumed that configurations of other objects are not specified
as a task.

3112

¶ Algorithm II Trajectory generation and control

³

(i )

1) Given: target mode i∗ and configuration y ∗ ∗2) Find τ † ∈ T s.t. (∃ k ∈ N s.t. (i∗ = τk† ∧ k ≤k1 , ∀ k1 s.t. i∗ = τk†1 ))
(i )
(i )
3) Find g †τ † s.t. y ∗ ∗ ∈ Rτ ∗† (g τ † ) ∧ g †yτ† =(ik ) T(i2 )[ℓy0 , · · · , ℓy0 ] ,(i)(i)
where ℓy0 = arg minℓ ky d(ℓ) − y (i) (0)k4) Set current mode as i = 1 and repeat following
trajectory generation and control procedures until
target configuration is achieved, i.e. y (i∗ ) (t) =(i )y ∗ ∗ holds:a) If i 6= i∗ , find next mode as j = τι† whereι = κ + 1, τκ† = ib) If i = i∗ , set target observation variable as(i)(i )on modey † = y ∗ ∗ , else set target³ point ´

0.7

0.7

0.6

0.6

0.5

0.5

0.4

0.4

0.3

0.3

0.2

0.2

0.1
0

0.1
0.4

0.6

0.8

1

1.2

(a) 0.55 ≤ q1 < 0.65

Fig. 11.

1.4

1.6

0

0.4

0.6

0.8

1

1.2

1.4

1.6

(b) 0.65 ≤ q1 < 0.75

Parameterization on boundary with nodes5) Explore mode 2 and 3 using mode keeping motion
generator for 200 steps to estimate reachable region.
It is expected that while procedure 1, contact between thehand and object 1 happens frequently enough to estimateboundary. Similarly it is expected that sufficient motion
information of object 2 is obtainable to finally estimate the(i)(i,j),k†reachable
region. The mapping and boundary building in 4. isboundary by y † = z i,j sd(ℓ), whereformally
done
but actually not so important because motions(j)ℓ = gτs † i , k † = arg mink ky d(k) − y (j) (t)kof object 1 and 2 are restricted on a line and the boundaryc) Repeat small movement of ∆y (1) until tarof mode switching is just a point.
(i)
get point is reached, i.e., y (i) = y †After the total exploration, three tasks are given to the
(i)
robot
system:i) Calculate displacement using ∆y=(i)(y † −y (i) (t))• Task 1: move the end effector to [y1 , y2 ] = [1.3, 0.6].
and mode keeping
β (i) (i)ky † −y (t))k• Task 2: move object 1 to q1 = 0.5.
ii) Calculate ∆y (ι1 ) using ∆y (i) , ∆y (ι2 )• Task 3: move object 2 to q2 = 1.9.
using ∆y (ι1 ) , · · · , ∆y (1) using ∆y (ιm )First, the robot acquires the relation between its joint
with mode keeping, where τ † =anglesand the hand position by randomly changing the joint[1, ιm , · · · , ι2 , ι1 , i, j, · · · ]Tangles (procedure 1). The first group of observation variablesd) Set i ← j
is built as [y1 , y2 ]T ∈ Y1 . When the hand contacts object 1,µ´
the second group of observation variable is constructed asy3 ∈ Y2 . While the hand keeps contact with the object 1, theIV. S IMULATION
object moves together with the hand. When moving object 1
The proposed mode generation and trajectory generation contacts with object 2, object 2 begins to move and the third
algorithms are implemented and evaluated in simulation. The group is constructed as y4 ∈ Y3 . While collecting mapping
robot manipulator has two joints and there are two objects, data by the random motion, the robot also collects boundary
rectangular and circular objects as shown in Fig.8. The end information between mode 1 and mode 2. In this caseeffector of the robot hand is also a circle. The state variables discrimination function F (x) of SVM is defined in threeare the joint angles of the manipulator θ ∈ R2 and positions dimensional space, that is, x = [y1 , y2 , y3 ]T . Fig.10(a) andof two objects q1 , q2 ∈ R. The observation variables are (b) show boundaries of two modes where 0.55 ≤ q1 < 0.65
the position of the robot hand (center of circle) [y1 , y2 ]T , for (a) and 0.65 ≤ q1 < 0.75 for (b). Circles in the figure
the position of the rectangular object (object 1) y3 (= q1 ) denote variables in mode 1, crosses denote mode 2 andand the position of the circular object (object 2) y4 (= q2 ). curves denote boundaries of F (x) = 0.
Initial position of object 1 is randomly chosen in 0.6 ≤ q1 ≤The discrimination function F (x) is utilized for param0.9, while position of object 2 is fixed at q2 = 1.6. The eterization on the boundary explained in III-E. Fig.11(a)manipulator and the objects are initialized every 100 steps.
and (b) are resultant node distributions with L = 9 after
Exploration and learning are done by the followings:
100 iterations of node movements, where nodes are initiallylocated around the center of the boundary. It can be seen that
1) Move randomly for 3000 steps.
2) Build mapping between [θ1 , θ2 ]T and [y1 , y2 ]T , esti- nodes are distributed along the contour of F (x) = 0. The
mate boundary between mode 1 and mode 2, 8) and both end nodes are constrained to the nearest observed data,
which can be understood by comparing Fig.10 with Fig.11.
generate parameterization on the boundary.
Fig.12 shows the trajectory of task 1, where a cross in3) Explore mode 2 using the motion generator which
the figure denotes the target configuration. This motion was
keeps mode 2 for 200 steps.
4) Build mapping between q1 and q2 , estimate boundary realized by using the mapping between Y1 and Y2 , which canbe regarded as inverse kinematics learning. In the case of task
between mode 2 and mode 3 (object 2 moving).
2, first reachable region of y3 was checked by Algorithm II.
8) They correspond respectively to the movement of the robot hand andA cross in Fig.13 denotes the destination on the boundary

the movement of the rectangular object by contacting with the robot hand.

3113

1.5

order to implement the proposed framework in the realworld. One possible way to deal with noise in motion
synchronousness detection is to approximate the effectof noise by preceding measurement.
• Observation variables directly correspond to configuration variables in this paper. Extraction of observation
variables from various image feature vectors should betaken into consideration.
• The framework of hybrid dynamical system control [16]is closely related to the control with mode switchings.
Besides, a control strategy that utilizes a partial information of state variable is known as ‘backstepping’ [17].

1

0.5

0

-0.5
-0.5

0

0.5

Fig. 12.

1

1.5

2

Trajectory of task 1

1.5

1.5

1

1

VI. C ONCLUSION

0.5

0.5

0

0

In this paper, an approach to hierarchy generation is
proposed as planning-space shift learning. The learning
framework consists of generation of modes based on motion
synchronousness, estimation of boundaries between modes,
and planning via multiple modes. In simulation, an illustrative example was shown where an object is utilized as a tool
to move another object. The consideration of the proposedarchitecture even with simple examples will be an important base for constructing more complex representations ofhierarchical learning.

-0.5
-0.5

0

Fig. 13.

0.5

1

1.5

2

Trajectory of task 2

-0.5
-0.5

0

Fig. 14.

0.5

1

1.5

2

Trajectory of task 3

between mode 1 and 2. First the robot hand aims at thedestination on the boundary, and then keeps contact to realize
the desired configuration of y3 . In the case of task 3, firstreachable region of y4 was checked. The lower cross in
Fig.14 indicates the destination on boundary between mode
2 and 3, while the upper cross indicates the destination on
boundary between mode 1 and 2. After realizing contact
between the hand and object 1, the robot keeps contact tomove object 1 to the right direction and to realize contact
between object 1 and object 2. It can be seen that finallythe robot realized the desired configuration of y4 = 1.9 byextending its body by using object 1.
V. D ISCUSSION
In the simulation, it was shown that the proposed learning
architecture enables various task execution by finding relations among variables and using the relations for planningwith various observation variables.
Motor babbling [13] has been discussed as human learningof motor coordination. While the first stage of mapping
learning in the proposed method can be equivalent to inversekinematics learning [14], the total framework deals withwider problem of finding relations among various observation variables. The proposal of this paper can be regarded as
an construction method of body image [15], where ‘self’ isidentified through the criterion of ‘what can be controlled’and ‘what cannot be controlled’.
On the other hand, the applications presented in thesimulation is rather simple and easy. The proposed architecture is applicable to more complex and high-dimensional
problems, but to apply the framework to higher-dimensional
problems, it is required to consider parameterization of highdimensional sub-manifolds (boundaries between modes).
Other possible extensions are the followings:• The influence of noise should be taken into account in

R EFERENCES[1] Dayan, P. and Hinton, G. E. (1993). Feudal reinforcement learning.
Advances in Neural Information Processing Systems, (5):271–378.
[2] Hauskrecht, M., Meuleau, N., Boutilier, C., Kaelbling, L. P., and Dean,
T. (1998). Hierarchical solution of markov decision processes using
macro-actions. In Proc. of 14th Annual Conference on Uncertainty in
Artificial Intelligence, pages 220–222.
[3] Miyamoto, H., Morimoto, J., Doya, K., Kawato, M. (2004). Reinforcement learning with via-point representation. Neural Networks,
17:299–305.
[4] Brooks, R. A. (1986). A robust layered control system for a mobile
robot. IEEE Journal of Robotics and Automation, RA-2:253–262.
[5] Wolpert, D. M. and Kawato, M. (1998). Multiple paired forward andinverse models for motor control. Neural Networks, 11:1317–1329.
[6] Gibson, J. J. (1977). The theory of affordances. In Perceiving, Acting,and Knowing: Toward Ecological Psychology, pages 62–82.
[7] Lungarella, M., Metta, G., Pfeifer, R., and Sandini, G. (2003). Developmental robotics: a survey. Connection Science, 15(4):151–190.
[8] Stoychev, A. (2005). Behavior-grounded representation of tool affordances. In Proc. of IEEE Int. Conf. on Robotics and Automation.
[9] Nabeshima, C., Kuniyoshi, Y., Lungarella, M. (2006). Adaptive bodyschema for robotic tool use. Advanced Robotics, 20(10):1105–1126.
[10] Vapnik, V. N. (1995). The Nature of Statistical Learning Theory.
Springer.
[11] Mangasarian, O. L. and Musicant, D. R. (2001). Lagrangian support
vector machines. Journal of Machine Learning Research, 1:161–177.
[12] Rumelhart, D. E., Hinton, G. E., and Williams, R. J. (1986). Learninginternal representations by error propagation. Parallel Data Processing, 1:318–362.
[13] Demiris, Y. and Dearden, A. (2005). From motor babbling tohierarchical learning by imitation: a robot developmental pathway. In
Proc. of the Fifth Int. Workshop on Epigenetic Robotics, pages 31–37.
[14] Oyama, E., Agah, A., MacDorman, K. F., Maeda, T., and Tachi, S.
(2001). A modular neural network architecture for inverse kinematics
model learning. Neurocomputing, 38-40:797–805.
[15] Yoshikawa, Y., Hosoda, K., and Asada, M. (2003). Does the invariancein multi-modalities represent the body scheme? - a case study withvision and proprioception -. In Proc. of the 2nd Int. Symp. on Adaptive
Motion of Animals and Machines, volume SaP-II-1.
[16] van der Schaft, A. and Schumacher, H. (2000). An Introduction toHybrid Dynamical Systems. Springer.
[17] Khalil, H. K., (Ed.) (2001). Nonlinear Systems. Prentice Hall.

3114