Coupled dynamical system based hand-arm graspplanning under real-time perturbations

Ashwini Shukla and Aude Billard

Learning Algorithms and Systems Laboratory (LASA)Ecole Polytechnique Federale de Lausanne (EPFL)

Lausanne, Switzerland - 1015Email: {ashwini.shukla;aude.billard }@epfl.ch

Abstract—Robustness to perturbation has been advocated asa key element to robot control and efforts in that direction arenumerous. While in essence these approaches aim at “endowingrobots with a flexibility similar to that displayed by humans”,few have actually looked at how humans react in the face offast perturbations. We recorded the kinematic data from humansubjects during grasping motions under very fast perturbations.Results show a strong coupling between the reach and graspcomponents of the task that enables rapid adaptation of thefingers in coordination with the hand posture when the targetobject is perturbed. We develop a robot controller based onCoupled Dynamical Systems that exploits coupling between twodynamical systems driving the hand and finger motions. Thisoffers a compact encoding for a variety of reach and graspmotions that adapts on-the-fly to perturbations without the needfor any re-planning. To validate the model we control the motionof the iCub robot when reaching for different objects.

I. I NTRODUCTION

Performing manipulation tasks interactively in real environ-ments requires a high degree of accuracy and stability. Atthe same time, except in completely determined and staticenvironments, machine perception of the environment maysuffer from real-time perturbation. To handle these, it requiresflexibility on the part of the robot. These considerations makethe task of reaching to grasp under fast perturbations difficultto deal with. Planning of constrained grasping motions hasoften been studied as two separate problems of grasping[15, 3] and generating arm-motion [2, 11] i.e., decouplingthe reach and grasp components. The high dimensionalityand complexity of these problems has discouraged the useof a single coherent framework for carrying out both thetasks. Constrained motion planning for only the reachingmotion in high dimensional arm-hand systems is a challengingproblem itself and requires the use of predefined heuristics. InProgramming by Demonstration (PbD), these heuristics areembedded in the demonstrations provided by a human agentwhich can be used to generate a generalized task description[4].

In previous work Gribovskaya and Billard [9] have usedPbD to encode the dynamics of a task as a Dynamical System(DS) where the end effector of the robot moves under theinfluence of an attractor positioned at the target. It has beenshown that such an approach ensures reproduction of thelearned behavior in a generalized manner while efficiently

Fig. 1. Experimental setup to record human behavior under perturbations.Two fixed targets are accompanied by the on-screen target selector whichactivates different targets by changing colors and hence create a spatialperturbation. The subject wears motion sensors and data glove which recordthe whole arm-hand kinematics at fixed time intervals. The kinematic valuesare simultaneously transferred to the iCub simulator for visualization of thetask.

handling spatial and temporal perturbations. In this work,weaddress the problem of having a combined encoding schemefor both reaching and grasping behaviors capable of adaptingagainst realtime perturbations during task reproduction.Itis worth mentioning here that this entails more complexitythan two independent attractor based tasks. Human neuro-physiological studies [5, 7] have shown that apart from theparallel evolution of hand transport and preshape, there existconvergence constraints and correlations between the two pro-cesses. In addition to the advantage of “human-like” motion,it is critical to maintain the coupling between hand and armmotion while performing a task in order to ensure successfulgrasp formation at the target. In addition to studying thecorrelations in the unperturbed human behaviors of performinga grasping task, we also study perturbed demonstrations ofhuman subjects in order to biologically inspire our approachof handling perturbations. We present the Coupled DynamicalSystem (CDS) model which ensures that the motion con-straints mentioned above are respected and at the same timeensures very fast adaptation under perturbations.

II. RELATED WORK

Classically, the behavior of reaching to grasp objects hasbeen treated as two different problems. Especially in motionplanning works, reaching to a pre-grasp pose and motion ofthe hand and fingers are considered as independent processesand are triggered sequentially [2, 10, 18]. Although both theissues of reaching to a pre-grasp pose and formation of grasparound arbitrary objects are intensively studied, very few[1, 8]

have looked into combining the two so as to have a unifiedreach-grasp system.

Most manipulation planners typically plan paths in theconfiguration space of the robot using graph based techniques.Classical approaches use probabilistic roadmap and its variants[6, 17] which assume a predefined and static environmentand are unable to handle any perturbations at the run-time.Moreover, they require huge preprocessing steps and are notsuitable for replanning of grasping tasks online. More recently,LaValle and Kuffner [14] proposedrapidly exploring randomtrees RRTs as a faster alternative to manipulation planningproblems, provided the existence of an efficient inverse kine-matic (IK) solver. RRT based methods [2, 18] are currentlythe fastest online planners which exploit the efficient searchingability of RRTs in order to find suitable paths between the startand the goal configurations. Note that a major drawback of allgraph based approaches is that they lose to retain any explicitcorrelations that exist between the two processes of handtransport and preshape as naturally performed by humans.

The concept of coupling between the reach and graspmotions is inspired by evidence in physiological studies[5, 16]. The most frequently reported mechanism suggests aparallel, but time-coupled evolution of the reach and graspmotions. However, directly mimicking this behavior makes thesystem time-dependent and hence unlikely to handle temporalperturbations.

In this work, we use PbD as a means to encode the dynamicsof reach-grasp motion and the coupling information in a com-pact way. We present the Coupled Dynamical System (CDS)task modeling approach to coordinate the motions of handtransport and preshape without being explicitly dependentonthe flow of time. We also show that this method efficientlyhandles on-the-fly perturbations by incorporating informationfrom perturbed human demonstrations. We conduct experi-ments on the iCub robot which show that this coupling isnecessary in order to successfully complete the tasks underperturbations of different types.

III. M ETHODOLOGY

In this section, we start with a short formalism of au-tonomous dynamical system (DS) in the context of roboticmanipulation tasks and its estimation with a Gaussian MixtureModel (GMM). For more details on this the reader is referredto Gribovskaya and Billard [9]. We present an extension tothis formulation and introduce the notion of coupling betweendifferent DS by the means of a coupling function. A formaldiscussion of the CDS model is presented describing themodeling process and regression algorithm to reproduce thetask. A simple 2D example is presented to establish intuitiveunderstanding of the working of the CDS model.

A. Preliminaries

Consider a state vectorξ(t) ∈ Rd which can be used to

uniquely define the state of the robot while performing a task(e.g. joint angles, position and orientation of the end-effectoretc.). Let there be N demonstrations of the task where the state

TABLE IGAUSSIAN MIXTURE REGRESSION

Let us assume that a process is characterized by some outputξO produced by inputξI ateach time step and our aim is to have a regression model for the same inputs andoutputs.To this end, we model the joint probability of input and output variables using GaussianMixtures. The probability that a full data pointξ = [ξI ; ξO] is generated by this processis defined by

P (ξ|θ) =

K∑

k=1

πkN (ξI , ξO; θ

k)

=

K∑

k=1

πk 1√

(2π)2d|Σk|e−

12(ξ−µk)T (Σk)−1(ξ−µk)T

whereθk contains the parameters viz. priorsπk, centersµk and covariancesΣk to definethe gaussian distributionN (ξ; θk) for thekth gaussian. Separating the input and outputcomponents, the parameters can be further represented as

µk=

(

µk

Iµk

O

)

, Σk=

(

Σk

I Σk

IOΣk

OI Σk

O

)

.

Gaussian Mixture Regression allows to compute for a given input variableξI and a givencomponentk, the expected distribution ofξO as

P(

ξO |ξI ; θk)

∼ N(

µk, Σ

k)

where,

µk= µ

k

O + Σk

OI

(

Σk

I

)

−1 (

ξI − µk

I

)

, Σk= Σ

k

O − Σk

OI

(

Σk

I

)

−1Σ

k

IO

Using the linear transformation of gaussian distributions, the conditional expectation ofξO givenξI can be re-written as a single normal distribution with the parameters

µ =

K∑

k=1

hkµk

, Σ =

K∑

k=1

h2kΣ

k

where,

hk = πk

N(

ξI ; θk

)

∑

K

i=1 πiN (ξI ; θi)

is the probability thatξI was generated by componentk.

vector and its velocities are recorded at particular time inter-vals, yielding the data set{ξtn, ξ

tn}∀t ∈ [0, Tn];n ∈ [1, N ]. Tn

denotes the number of recordings in the n-th demonstration.Without loss of generality, we assume that the dynamics of thetask can be represented by a first order autonomous OrdinaryDifferential Equation (ODE):

ξ = f(ξ) + ǫ (1)

wheref : Rd 7→ Rd is a continuous and continuously differen-

tiable function with a single equilibrium pointξ∗ = f(ξ∗) = 0and ǫ represents white gaussian noise. It is evident from theformulation of DS that it does not explicitly depend on timeand hence is robust towards temporal perturbations. To handlespatial perturbations, i.e. sudden displacement of the target orthe manipulator, we considerξ in the reference frame of thetarget.

By estimating the functionf, we can have a compactmathematical description of the task as a Dynamical System.For this purpose, we use GMM to encode the demonstratedtrajectories in a probabilistic framework. The core assumptionwhen representing a task as a Gaussian Mixture Model is thateach recorded pointξ(t) from the demonstrations is a sample

Learned GMMs Master Sub-System

Slave Sub-System

P(

ξm, ξm |θm

)

P(

Ψ(ξm) , ξs |θinf

)

P(

ξs, ξs |θdyn

)

ξm=E[

P(

ξm |ξm)]

ξm ← ξm + ξm∆t

ξs = E [P (ξs |Ψ(ξm) )]

ξs = E

[

P(

ξs

∣

∣

∣

(

ξs − ξs

))]

ξs ← ξs + ξs∆t

ξm

ξs

CouplingΨ(ξm)

RO

BO

T

ξs

Fig. 2. Task execution using CDS model. Blue region shows the threeGaussian Mixture Models which form the full CDS model. Green regionshows the master sub-system where the cartesian position of the robot end-effector evolves in time as a DS and is continuously fed to the robot. Magentaregion shows the slave sub-system where the finger joint angles evolve in timeas a DS, but also influenced by the state of the master system andfed to therobot. Coupling is ensured by passing selective state information in the formof Ψ(ξm) as shown in red.

drawn from the joint distribution:

P (ξ|θ) =K∑

k=1

πkN(

ξ;θk)

.

As detailed in Table I, this model makes it possible to performprobabilistic regression for the value of output variableξO

given the value of input variableξI at each time instant. E.g.,as applied in previous works [9, 12] in the special case oflearning dynamics, the desired state velocities can be queriedconditioned on the current state. Note that in this special casewhich models only the dynamics of the task, the partitionsξI andξO correspond respectively to the spatial positions andvelocities of the robot’s end-effector. Since this is not alwaysthe case in the CDS model, we will keep this generalized fornow and define the partitions for the different components ofthe coupled model in later subsections. In the next sections,we show that the CDS model harnesses much more from theGMM than just states and velocities by learning the couplinginformation in addition to the dynamics.

B. Coupled Dynamical System

In the classical case of learning position and orientationdynamics, one GMM each for hand and finger motions wouldsuffice to model the dynamics. However, to model a reach-grasp with hand-arm coordination, it is required to have amore complex approach. Note that Gribovskaya and Billard[9] have used a coupling between position and orientation inthe inverse kinematics. However, the main purpose served bythis approach was to avoid unfavorable joint postures

The CDS model derives inspiration from the biologicalevidences of reach-grasp coupling [7, 16]. These studiesadvocate the fact that there is a parallel, but time-coupledevolution of these sub-tasks combined with synchronizedtermination constraints. E.g., if the fingers close before thehand reaches the object, the task fails. Moreover, this orderneeds to be maintained under spatial, temporal and grasp-typeperturbations. Another example of a situation where couplingis needed is that of change in grasp type. When the requiredgrasp type is changed on the fly, if the change occurs from alow-aperture grasp to a high aperture grasp, a full reopeningmay be needed which is only possible if the coupling is active.

We first formally discuss the CDS model in subsectionsdescribing the model creation and the procedure for taskexecution. To establish intuitive understanding, we present a2D example as a representative of higher dimensional graspingtasks. Subsequently, we show that the CDS model retains theglobal stability endowed in the individual GMMs bystableestimator of dynamical systems (SEDS) Khansari-Zadeh andBillard [12] and the fact that different GMMs are coupledusing a coupling function does not affect the overall stabilityof the model.

1) Model Building: Let ξm denote the state of the mastersub-system andξs that of the slave sub-system in theirrespective goal reference frames. Consider the setG of allobjects for which grasping behaviors are demonstrated. Thefollowing three joint distributions are learned as explained inTable. I -

1) P(

ξm, ξm|θgm

)

: encoding the dynamics of the master

2) P (Ψ(ξm), ξs|θginf): encoding the inferred state of the

slave conditioned on the master (we will refer to thisquantity asξ)

3) P(

ξs, ξs|θgdyn

)

: encoding the dynamics of the slave

∀g ∈ G. HereΨ : Rdm 7→ R denotes thecoupling functionwhich is a monotonic function ofξm with the constraint

limξm→0

Ψ(ξm) = 0 (2)

and dm denotes the dimension of the master sub-system.The purpose of the coupling function is to transfer relevantinformation between the sub-systems so as to ensure couplingbetween them. The distributions for learning dynamics (i.e.P(

ξm, ξm|θgm

)

andP(

ξs, ξs|θgdyn

)

) is learned using SEDSgiven by which produces globally stable model but can onlyhandle models with|ξI| = |ξO|. On the other hand, thedistribution P (Ψ(ξm), ξs|θ

ginf) is learned using non-linear

programming to fit gaussians to the data under the constraint

limx→0

E [ξs|x] = 0. (3)

In the context of reach to grasp tasks studied in this work, themaster sub-system corresponds to reaching motion with statevector as the cartesian position of the end-effector. The slavesub-system corresponds to the motion of the fingers with statevector as the finger joint angles. Note that the same model canbe used with orientation variables (Euler angles or Axis-angle

−0.16 −0.14 −0.12 −0.1 −0.08 −0.06 −0.04 −0.02 0−0.2

−0.15

−0.1

−0.05

0

ξx

ξf

(a) Demonstrations

−0.18 −0.16 −0.14 −0.12 −0.1 −0.08 −0.06 −0.04 −0.02 0 0.02−0.05

0

0.05

0.1

0.15

0.2

0.25

0.3

ξx

ξx

(b) P(

ξx |ξx)

−0.18 −0.16 −0.14 −0.12 −0.1 −0.08 −0.06 −0.04 −0.02 0 0.02−0.2

−0.15

−0.1

−0.05

0

0.05

ξx

ξf

(c) P(

ξf |ξx)

−0.15 −0.1 −0.05 0 0.05

0

0.05

0.1

0.15

0.2

ξf

ξf

Gaussian Mixture ComponentsRecorded DemonstrationsMean Regression Signal

(d) P(

ξf∣

∣ξf

)

Fig. 3. GMMs which combine to form the CDS model for the 2D example. (a) shows the human demonstrations. Large number of data points around theend of trajectories depict very small velocities. (b) shows the GMM encoding the velocity distribution conditioned on the position of master sub-system (ξx).(c) shows the GMM encoding the desired value ofξf (i.e. ξf ) given the current value ofξx as seen during the demonstrations. (d) shows the GMM encodingthe dynamic model for the slave-subsystem (ξf ). Legend for (d) holds for all.

representation) and hence can be used to couple orientationcontrol with position control by having another slave sub-system.

2) Reproduction: While reproducing the task, the modelessentially works in three phases:Increment master → Inferslave → Increment slave. The master sub-system evolvesin time independently and the corresponding end-effectorcommands are issued to the robot. Moreover, it modulates thecommands for the slave system which also evolves in a similarway but while sharing information with the master subsystemdue to the coupling mechanism. Fig. 3 shows this flow ofinformation among the sub-systems and the robot. Such ascheme is desired since it ensures that any spatial, temporalor grasp-type perturbations are reflected appropriately inallthe sub-systems. The process starts by generating a velocitycommand for the master sub-system and thus increments thestate by one time step.Ψ(ξm) transforms the current state ofthe master sub-system which is fed to the inference model thatinfers the desired state of the slave sub-sytem by conditioningthe learned joint distribution on the appropriate variable. Thevelocity command to drive the slave sub-system from thecurrent state to the inferred (desired) state is generated byGMR conditioned on the error between the two. The slavesub-system achieves a new state and the cycle is repeateduntil convergence. Note that if at any instant, the robot ispresented with a different object for which the grasping modelis stored in the setG, we select the corresponding CDS modeland continue the same operations using master and slavecomponents. Algorithm 1 explains the complete reproductionprocess in a pseudocode.

Note that the coupling functionΨ(ξm) also acts as aphase variable which updates itself every time step and inthe event of a perturbation, will command the slave systemto re-adjust so as to maintain the same correlations as learnedfrom the demonstrations. Two other parameters governing thecoupled behavior are scalarsα, β > 0. Qualitatively speaking,they respectively control the speed and amplitude of therobot’s reaction under perturbations. E.g., in the experimentspresented in Section IV, they control the speed and the extentof re-opening of fingers when the target is changed to a fartherlocation.

Example. Suppose we represent a reach-grasp task as two

Algorithm 1 Coupled task execution

Input: ξm(0); ξs(0); θgm; θg

inf ; θgdyn; α; β; ∆t; ǫ

Set t = 0repeat:

if perturbation thenupdateg ∈ G

end ifIncrement Master: ξm(t) ∼ P

(

ξm|ξm;θgm

)

ξm(t+ 1) = ξm(t) + ξm(t)∆t

Infer Slave: ξs(t) ∼ P(

ξs|Ψ(ξm) ;θginf

)

Increment Slave: ξs(t) ∼ P(

ξs|β(

ξs − ξs

)

;θgdyn

)

ξs(t+ 1) = ξs(t) + αξs(t)∆t

t← t+ 1until: Convergence

(

‖ξs(t)‖ < ǫ and ‖ξm(t)‖ < ǫ)

sub-systems controlling the motion of the end-effector andthe fingers. For simplicity, we consider 1-D cartesian positionξx as the master variable and 1 finger joint angleξf as theslave variable, both expressed in the goal reference frame sothat they converge to the origin. In this way, the full fledgedgrasping task is just a higher dimensional version of thiscase by considering 3-dimensional cartesian position insteadof ξx and all joint angles (or eigen-grasps) of the robot handinstead ofξf . For reference with the formal description of theCDS model we re-iterate the following equivalences in thisexample:ξm ≡ ξx (master variable),ξs ≡ ξf (slave variable),Ψ(ξx) ≡ ξx (coupling function) anddm = 1. Note that indifferent tasks, depending on the nature of coupling in differentdimensions of the master variable, other coupling functionscan be used.

Under the given setting, typical demonstrations of reach-grasp task are as shown in Fig. 3(a), where the reachingmotion converges slightly faster than the finger curl. Weextract the velocity information at each recorded point by finitedifferencing and build the following models from the resultingdata:P

(

ξx, ξx |θx)

, P (ξx, ξf |θinf ) and P(

ξf , ξf |θdyn)

.The resulting mixtures for each of the models is as shownin Fig. 3. For reproducing the task, instead of the earlier

−0.16 −0.14 −0.12 −0.1 −0.08 −0.06 −0.04 −0.02 0 0.02

−0.2

−0.15

−0.1

−0.05

0

0.05

ξx

ξ f

Gaussian Mixture ComponentsInferred ξf

Actual ξf

(ξx,ξf ) Demonstrations

α increasing

Perturbation

Fig. 4. Reproducing the task under the CDS model. The reproduction(dashed) is overlaid on the demonstrations for reference. The model is run fordifferentα values and the flow of the state values in time is depicted by thearrows. It is evident that the model tries to track the desiredξf (blue) valuesat the currentξx by reversing the velocity inξf direction. The tracking ismore stringent for largerα.

0 20 40 60 80 100 120 140 160

−0.14

−0.12

−0.1

−0.08

−0.06

−0.04

−0.02

0

Time Steps

ξ x(t

)

0 20 40 60 80 100 120 140 160

−0.16

−0.14

−0.12

−0.1

−0.08

−0.06

−0.04

−0.02

0

ξ f(t

)

ξf (t)

ξf (t)20 40 60 80 100 120 140 160

−0.45

−0.4

−0.35

−0.3

−0.25

−0.2

−0.15

−0.1

−0.05

0

ξ f(t

)

20 40 60 80 100 120 140 160

−0.14

−0.12

−0.1

−0.08

−0.06

−0.04

−0.02

ξ x(t

)

Time Steps

α increasingβ increasing

Fig. 5. Variation of obtained trajectories withα and β. Vertical red lineshows the instant of perturbation when the target is suddenly pushed awayalong positiveξx direction. Negative velocities are generated inξf in orderto track ξf . Speed of retracting is proportional toα (left) and amplitude isproportional toβ (right ).

approach presented in [9] where the system evolves under thevelocities computed asE

[

(ξx; ξf ) |(ξx; ξf )]

we proceed asin Algorithm 1. Fig. 4 shows reproduction of the task in the(ξx, ξf ) space overlaid on the demonstrations. It clearly showsthat a perturbation inξx creates an effect inξf , viz. generatinga negative velocity the magnitude of which is tunable usingthe α parameter. This change is brought due to the need oftracking the inferredξf values i.e.ξf , at all ξx. ξf is nothingbut the expected value ofξf given ξx as seen during thedemonstrations. The time variation ofξf and its variation withα andβ is shown in Fig 5.α modulates the speed with whichthe reaction to perturbation occurs. On the other hand, a highvalue ofβ increases the amplitude of retracting. Fig. 6 showsthe streamlines of this system in order to visualize the globalbehavior of trajectories evolving under the CDS model. TheCDS model run is compared to uncoupled task execution inFig. 7. It shows the behavior when the same perturbation isintroduced on the abscissa in both coupled and un-coupledexecutions. Clearly, the unperturbed variable (ξf in this case)does not react when there is no coupling and also the order ofconvergence is not the same as in the demonstrations whereξx converges faster thanξf .

3) Stability and Convergence: We first define the notionof global stability in CDS and then prove that the process

−0.14 −0.12 −0.1 −0.08 −0.06 −0.04 −0.02 0

−0.15

−0.1

−0.05

0

ξx

ξ f

α=5

−0.14 −0.12 −0.1 −0.08 −0.06 −0.04 −0.02 0

−0.15

−0.1

−0.05

0

ξx

ξ f

α=1Goal Streamlines Demonstration Envelope

Fig. 6. Change inα affecting the nature of streamlines. Largerα will tendto bring the system more quickly towards the(ξx, ξf ) locations seen duringdemonstrations.

−0.14 −0.12 −0.1 −0.08 −0.06 −0.04 −0.02 0 0.02

−0.2

−0.15

−0.1

−0.05

0

ξx

ξ f

Gaussian Mixture ComponentsUncoupledCDS

(a)

20 40 60 80 100 120 140−0.15

−0.1

−0.05

0

Time Steps

ξ x(t

)

20 40 60 80 100 120 140

−0.15

−0.1

−0.05

0

ξ f(t

)

CDSUncoupled

(b)

Fig. 7. Task reproduction with and without coupling shown in(a) state space,(b) time variation. Dotted lines show the uncoupled task execution. Note thedifference in the directions from which the convergence occurs in the twocases. In the coupled execution, convergence is faster inξx than inξf . In theuncoupled case it is just opposite.

evolving under Algorithm 1 is globally stable.

Definition. A CDS model is globally asymptoticallystable if by starting from any given initial conditions ξm(0),ξs(0) and coupling parameters α, β ∈ R the followingconditions hold:

limt→∞

ξm(t) = 0 (4a)

limt→∞

ξs(t) = 0 (4b)

To prove that the CDS model indeed follows the conditions4, we use the properties of its individual components. Thecondition 4a holds true due to the global stability of SEDS.To investigate the stability of the coupling, we consider

limt→∞

E

[

ξs |Ψ(ξm)]

= E

[

ξs

∣

∣

∣Ψ(

limt→∞

ξm

)]

= E

[

ξs

∣

∣

∣

∣

limξm→0

Ψ(ξm)

]

(By 4a)

= E

[

ξs |0]

(By Eq. 2)

= 0 (By Eq. 3) (5)

The model which governs the evolution of the coupledvariableξs is given by

ξs = E

[

ξs

∣

∣

∣

(

ξs − ξs

)

β]

.

Taking the limiting values and using Eq. 5 , we get

limt→∞

ξs = E

[

ξs |βξs]

(6)

which is again globally asymptotically stable due to SEDS andhence will drive the stateξs asymptotically to0. However, asseen from Algorithm 1, the multiplierα boosts the velocitybefore incrementing the state. It is trivial to see that thisdoesnot affect the global asymptotic behavior of the model sincenegative definiteA+AT

2 ⇒ αA+AT

2 is also negative definite forα > 0. Why such a condition is required for global stabilityis proved in detail in [12].

IV. EXPERIMENTS AND RESULTS

In this section, we describe the experiments for recordinghuman demonstrations under random perturbations. We showthat the CDS model is indeed qualitatively equivalent to thedemonstrated human behavior of grasping under perturbationsand it is suited to handle fast perturbations which typicallyneed re-planning and are difficult to handle online. From theexperimental data, we identify relationships to infer the freeparameters of the CDS model. Unlike the example presentedin Section III, we used the coupling functionΨ(.) = ||.||for all the task runs on the robot. We divide the discussioninto subsections describing the experimental setup, quantitativeresults/inferences from the experimental data and task repro-ductions on the iCub simulator as well as the real robot tovalidate the model1.

A. Experiments

The experimental setup to record human demonstrations andcreate sudden perturbations is shown in Fig. 1. It consists oftwo stationary targets and the on-screen target selector whichprompts the human subject to reach and grasp one of theobjects depending on the color shown on the screen. The iCubsimulator runs simultaneously while the human is performingdemonstrations in order to establish correspondence for thehuman subject. To start the experiment, one of the targetsis switched on and the subject starts to reach towards thecorresponding object aiming for a particular grasp dependingon the object. In random trials (without the knowledge of thesubject), perturbations are introduced by abruptly switchingthe on-screen target selector. Reacting to this change, thesubject starts to adjust the motion of hand and fingers in orderto reach the other target. The subject forms a grasp around thefinal target which marks the end of the trial. All the motiondata i.e. hand position, orientation and finger joint anglesisrecorded throughout the trial at a frequency of 50 Hz. Aftereach trial, the subject is asked to go to a rest posture wherea 5 sec. calibration procedure is done for the motion sensorsand data-glove. Trials are run until at least 10 unperturbedand10 perturbed trials are obtained. Learning from perturbationsenables the estimation of parametersα andβ so that they canbe predicted prior to the instant of perturbation at the timeoftask reproduction. In addition to this, we also learn the CDSmodel for pinch grasp and populate the set of graspsG. Notethat arbitrarily many grasps can be learned and added to thisset.

1Videos are available at http://www.youtube.com/user/TheRoboticsVideos

50 100 150 200 250 300

−1.5

−1

−0.5

0

ξ f(t

)

0 50 100 150 200 250 3000

0.1

0.2

0.3

0.4

Time Steps

ξ x(t

)

Mean Predicted by CDS1σ Variance Predicted by CDS Human recordings during unperturbed demo

Fig. 8. A qualitative comparison of CDS model predictions of the inferredfinger joint angles and the actual recorded human behavior. Note the couplingof joint angles with the distance from the target. A change intarget locationtriggers a discontinuous shift in the value of‖x − xgoal‖ and at the sametime, starts reopening of the fingers.

2.5 3 3.5 4 4.5 5

x 10−3

−0.2

−0.1

0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

∫Tp

0

ξxdt

Tp

β

(a)

0.005 0.01 0.015 0.02 0.025 0.03 0.035 0.04 0.045

0

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

0.45

0.5

α

β

Subject 1Subject 2Subject 3

(b)

Fig. 9. (a) shows the linear correlation found between mean hand velocityprior to perturbation andβ. Tp refers to the time at which perturbation isintroduced. (b) shows the linear correlation found betweenα andβ.

After all the data is collected, we learn the CDS model fromthe data recorded in the unperturbed trials. This serves as thebase model which qualitatively mimics human behavior underperturbations. Fig. 8 shows typical human responses of thefinger joint angles (only index finger proximal joint is shown)under perturbation of the target. The vertical red line marksthe onset of perturbation and the subsequent dip in the jointangles is due to the re-opening of the fingers when the target issuddenly moved away. Note that in all the trials, the fingers re-opened irrespective of the fact that the aperture of the fingersat the time of perturbation was large enough to accommodatethe object. This behavior was found common to all subjects.It is evident from Fig. 8 that the finger joint angle inferred byCDS at each time instant is similar to what is exhibited byhumans within the variance of demonstrations.

B. Calculation of model parameters

As illustrated in section III-B, the parametersα and β

represent respectively the speed and amplitude of the reaction(in this case, reopening of fingers) to perturbation. Increasedα causes the finger trajectories to follow the inferred (desired)values more strictly, hence, a sharp decrease in the joint vari-ables is observed. On the other hand,β modulates the inferredvalue itself. Hence, a manually tuned combination of the twoissufficient to generate any given behavior after the perturbation.However, as shown in Fig. 9, it was found that there existcorrelations which make it possible to predict their valuesfor

(a) Coupled (b) Uncoupled

Fig. 10. Reach-grasp task executions with and without coupling. In thecoupled execution (a), fingers maintain the correlations seen during thedemonstrations which prevents premature finger closure. The uncoupledexecution (b), fingers close early and the grasp fails.

Fig. 11. Closeup of hand motion post perturbation in coupled (left) anduncoupled (right ) executions. Note the re-opening of fingers leading to asuccessful grasp in the coupled case.

a human just by observing the motion of the hand prior tothe perturbation. Fig. 9(a) shows there is a linear relationshipbetween the velocity of the hand prior to perturbation and theparameterβ. It shows that the faster a subject moves towardsthe target, the less they reopen the fingers upon perturbation.Fig. 9(b) shows the linear correlation between the parametersα and β. It shows that a faster reopening of the fingers isaccompanied by a larger amount of reopening. Consequently,both the parametersα and β can be predicted based on themotion of the hand prior to perturbation. Hence, they no longerneed to be specified manually in Algorithm 1.

C. Validation

We validate the CDS model by executing the learned grasp-ing strategies on the iCub robot. First we show that graspingunder perturbations using the uncoupled approach fails dueto lack of knowledge of the correlations between the reachand grasp sub-systems. Fig. 10 shows such comparison. Therobot starts moving towards a fixed target which is suddenlyshifted to the right just before the completion of the task. In theuncoupled case (Fig. 10(b) ), the fingers close too early and thetask fails. In the coupled case (Fig. 10(a)), the finger motionis delayed due to the coupling and the fingers close accordingto the correlations learned during the demonstrations. Fig. 11clearly shows the re-opening of fingers.

Next, we show that the presented scheme also enables onlineadaptation against fast perturbations of grasp-type. We learnpower grasps in palm-up and palm-down configurations ofthe hand from unperturbed demonstrations and populate theset of graspsG with them. In the reproduction phase, therobot starts moving towards the target aiming for the palm-down configuration grasp. After a certain period, the objectismade to reappear at another location, however not supported

0 0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.0−10

0

10

20

30

40

50

60

70

80

90

Time (s)

ξ f(1

)

Actual (before perturbation)Inferred (pinch grasp)Actual (after perturbation)Inferred (power grasp)

Perturbation from pinch to power grasp

Fig. 13. Motion of one finger joint angle under grasp-type perturbation.Demonstrated models for the two grasps recorded during the unperturbeddemonstrations are shown. The CDS model switches smoothly between themodels when the perturbation occurs.

against gravity so that it starts falling. This requires a fastadaptation from palm-down to palm-up grasp, while the targetkeeps on moving. Note that we use the approach presented inprevious work on ball catching by Kim et al. [13] to ensurethat the robot intercepts the falling object in its workspace.Here the contribution of the CDS model is to re-adjust thehand orientation and finger curl online so that the grasp iscompleted successfully on the falling object. The task beingperformed by the iCub in simulation is shown in Fig. 12. Notethat the fingers close proportionately as the distance betweenthe falling ball and the robot hand decreases, maintaining thecorrelations seen during the demonstrations. The time elapsedis indicated on the figure in seconds.

We perform another task showing the ability of the CDSmodel to adapt between pinch and power grasps. We learnpinch and power grasps from demonstrations as shown previ-ously and change the target object while the robot goes for thepinch grasp. Fig. 13 shows the motion of robot’s index fingerproximal joint. It is evident that the hand aperture decreasesquickly when the robot aims for the pinch grasp but afterthe switch, the robot smoothly switches from following thepinch-grasp model requiring smaller hand aperture (i.e. largerjoint value) to the power grasp model which requires a largeraperture (smaller joint value). The model can also be seen tobe robustly handling different instants of perturbation.

V. CONCLUSION

In this paper we presented a model for encoding and repro-ducing grasping strategies which is capable of handling fastreal-time perturbations. Biological inspiration was taken byobserving the correlations between arm and finger motion dur-ing human experiments of reach-to-grasp tasks under suddenunpredicted perturbations. We showed that once the graspingstrategies are taught offline by a human demonstrations, themodel is able to reliably switch between those under very fastperturbations. Re-opening of fingers under perturbation hasbeen found to be present in all human trials and is shown to becritical to the success of reach-grasp tasks. Our model ensuresthis behavior even in the presence of arbitrary perturbations.

Fig. 12. Fast adaptation under perturbation from palm-down to palm-up power grasp. Torso is included in the inverse kinematics to increase the workspaceof the robot so as to highlight the effectiveness of the model in adapting the grasping motion under very fast perturbations.

ACKNOWLEDGMENTS

This work was supported in part by EU ProjectFirst-MM(FP7/2007-2013) under grant agreement number248258.

REFERENCES

[1] Ji-Hun Bae, Suguru Arimoto, Yuuichi Yamamoto, HiroeHashiguchi, and Masahiro Sekimoto. Reaching to graspand preshaping of multi-dofs robotic hand-arm systemsusing approximate configuration of objects. InIntelligentRobots and Systems. IEEE/RSJ International Conferenceon.

[2] D. Berenson, S.S. Srinivasa, D. Ferguson, A. Collet, andJ.J. Kuffner. Manipulation planning with workspace goalregions. InRobotics and Automation. ICRA. IEEE Inter-national Conference on, pages 618–624. IEEE, 2009.

[3] G.M. Bone, A. Lambert, and M. Edwards. Automatedmodeling and robotic grasping of unknown three-dimensional objects. InRobotics and Automation.ICRA. IEEE International Conference on, pages 292–298. IEEE, May 2008. doi: 10.1109/ROBOT.2008.4543223.

[4] S. Calinon, F. Guenter, and A. Billard. On learning, rep-resenting, and generalizing a task in a humanoid robot.Systems, Man, and Cybernetics, Part B: Cybernetics,IEEE Transactions on, 37(2):286–298, 2007. ISSN 1083-4419.

[5] U. Castiello, K. Bennett, and H. Chambers. Reach tograsp: the response to a simultaneous perturbation ofobject position and size.Experimental Brain Research,120(1):31–40, 1998. ISSN 0014-4819.

[6] A. Gasparri, G. Oliva, and S. Panzieri. Path planningusing a lazy spatial network prm. InControl andAutomation. MED. 17th Mediterranean Conference on,pages 940–945. IEEE, 2009.

[7] M. Gentilucci, S. Chieffi, M. Scarpa, and U. Castiello.Temporal coupling between transport and grasp com-ponents during prehension movements: effects of visualperturbation.Behavioural Brain Research, 47(1):71–82,1992. ISSN 0166-4328.

[8] M. Gienger, M. Toussaint, and C. Goerick. Task mapsin humanoid robot manipulation. InIntelligent Robotsand Systems. IROS. IEEE/RSJ International Conferenceon, pages 2758–2764. IEEE, 2008.

[9] E. Gribovskaya and A. Billard. Learning nonlinear multi-variate motion dynamics for real-time position and ori-entation control of robotic manipulators. InProceedings

of IEEE-RAS International Conference on HumanoidRobots, pages 472–477. IEEE, 2009.

[10] K. Harada, K. Kaneko, and F. Kanehiro. Fast graspplanning for hand/arm systems based on convex model.Robotics and Automation. ICRA. IEEE InternationalConference on, 2008.

[11] A.J. Ijspeert, J. Nakanishi, and S. Schaal. Movementimitation with nonlinear dynamical systems in humanoidrobots. InRobotics and Automation. Proceedings. ICRA.IEEE International Conference on, volume 2, pages1398–1403. IEEE, 2002. ISBN 0780372727.

[12] S.M. Khansari-Zadeh and A. Billard. Imitation learningof globally stable non-linear point-to-point robot motionsusing nonlinear programming. InIntelligent Robots andSystems (IROS), IEEE/RSJ International Conference on,pages 2676–2683. IEEE, 2010.

[13] Seungsu Kim, Elena Gribovskaya, and Aude Billard.Learning Motion Dynamics to Catch a Moving Object. In10th IEEE-RAS International Conference on HumanoidRobots. IEEE, 2010.

[14] S. LaValle and J. Kuffner. Rapidly-exploring randomtrees: Progress and prospects. InAlgorithmic and com-putational robotics: new directions: the fourth Workshopon the Algorithmic Foundations of Robotics, page 293.AK Peters, Ltd., 2001. ISBN 156881125X.

[15] A.T. Miller, S. Knoop, H.I. Christensen, and P.K. Allen.Automatic grasp planning using shape primitives. InRobotics and Automation. Proceedings. ICRA. IEEE In-ternational Conference on, volume 2, pages 1824–1829.IEEE, 2003. ISBN 0780377362.

[16] A.R. Mitz, M. Godschalk, and S.P. Wise.Learning-dependent neuronal activity in the premotorcortex: activity during the acquisition of conditionalmotor associations. Journal of Neuroscience, 11(6):1855, 1991.

[17] J-P. Saut, A. Sahbani, S. El-Khoury, and V. Perdereau.Dexterous manipulation planning using probabilisticroadmaps in continuous grasp subspaces. InProceedingsof the IEEE/RSJ International Conference on IntelligentRobots and Systems, pages 2907–2912. IEEE, 2007.

[18] N. Vahrenkamp, D. Berenson, T. Asfour, J. Kuffner, andR. Dillmann. Humanoid motion planning for dual-armmanipulation and re-grasping tasks. InIntelligent Robotsand Systems. IROS. IEEE/RSJ International Conferenceon, pages 2464–2470. IEEE, 2009.