Learning physical collaborative robot behaviors from human demonstrations

IEEE TRANSACTIONS ON ROBOTICS 1 Learning Physical Collaborative Robot Behaviors from Human Demonstrations Leonel Rozo1,Member, IEEE, Sylvain Calinon2,1, Darwin G. Caldwell1, Pablo Jim´ enez3, and Carme Torras3,Senior member, IEEE, Abstract—Robots are becoming safe and smart enough to work alongside people not only on manufacturing production lines, but also in spaces such as houses, museums or hospitals. This can be significantly exploited in situations where a human needs the help of another person to perform a task, because a robot may take the role of the helper. In this sense, a human and the robotic assistant may cooperatively carry out a variety of tasks, therefore requiring the robot to communicate with the person, understand his/her needs and behave accordingly. To achieve this, we propose a framework for a user to teach a robot collaborative skills from demonstrations. We mainly focus on tasks involving physical contact with the user, where not only position, but also force sensing and compliance become highly relevant. Specifically, we present an approach that combines probabilistic learning, dynamical systems and stiffness estimation, to encode the robot behavior along the task. Our method allows a robot to learn not only trajectory following skills, but also impedance behaviors. To show the functionality and flexibility of our approach, two different testbeds are used: a transportation task and a collaborative table assembly. Index Terms—Robot learning, programming by demonstration, physical human-robot interaction, stiffness estimation. I. INTRODUCTION RECENTLY, the fences in factories have started to disappear as a gentler breed of robots has entered the workplace, and new features have made even conventional robots more user-friendly and safer to be around. This shift will eventually alter the dynamics of labor in factories and workshops, allowing humans and robots to work together in efficient new ways. Some manufacturing tasks, such as the production of small mechanical components, require a robot to do the physical labor while a person performs quality-control inspections after each component is made. This requires the human and robot workers to operate side by side. For other jobs, like lifting an engine block so that it can be worked on, a user would want his/her robotic assistant to carry out the heavy lifting task (see Fig. 1). In such scenarios, 1Department of Advanced Robotics, Istituto Italiano di Tecnologia (IIT), Via Morego 30, 16163 Genova, Italy. [email protected] 2Idiap Research Institute, Rue Marconi 19, PO Box 592, CH-1920 Martigny, Switzerland. [email protected] 3Institut de Rob` otica i Inform` atica Industrial CSIC-UPC, C. Llorens i Artigas, 4-6, 08028 Barcelona, Spain. [email protected] Initial results on the topic were published at the AAAI Conf. on Artificial Intelligence [1], and at the IEEE Intl. Symp. on Robot and Human Interactive Communication [2]. This work was supported by the European projects STIFF-FLOP (FP7ICT-287728), DexROV (H2020-EU.3.2-635491), and the Spanish Ministry of Science and Innovation under project PAU+ DPI2011-27510. The final version of this article is available at IEEE with DOI 10.1109/TRO.2016.2540623 Lifting and transportation of bulky objects Assembly of furniture or mechanical structures Fig. 1. Illustration of human-robot cooperative tasks. In these kinds of scenarios, the robot needs to be easily and rapidly re-programmable, so that it can assist the user in a large variety of tasks, where different robot behaviors are required. Physical interaction between the participants occurs, and therefore the robot should be able to exploit the haptic information, as well as to show different compliance levels as needed. the collaborative work can speed up production processes, improve manufacturing quality and reduce structural costs. On the other hand, service robots have as main premise to assist people in different environments such as houses, offices, hospitals, museums, etc. Here, some of the robot duties imply physical contact, for example, in hand-over tasks [3], or when a robot cooperatively transports an object with a human partner [2] (see Fig. 1). This physical interaction provokes a rich exchange of haptic information, and involves compliant robot movements. Additionally, the type of jobs carried out by the robot may frequently vary. Therefore, a robotic assistant requires to be easily and rapidly re-programmed several times according to specific needs. In this context, programming by demonstration (PbD) [4] emerges as a promising alternative solution allowing the natural and intuitive transfer of human knowledge about a task to a collaborative robot. In this paper, we propose to use PbD to teach a robot different roles in human-robot collaboration (HRC) scenarios. Kinesthetic teaching1is used for the robot to learn, from demonstrations, an approximate model of the task along with its corresponding constraints. Specifically, our approach defines a virtual spring-damper system encapsulating the dynamics and constraints of the task, and in turn governing the robot behavior.2Such a system can act on different reference frames, 1The term refers to the procedure where the user holds and moves the robot along the trajectories that need to be followed to accomplish the task, while the robot actively or passively compensates for the effect of gravity. 2We use the word virtual for clarifying that there is no real physical springdamper system connected to the robot. IEEE TRANSACTIONS ON ROBOTICS 2 for instance, on coordinate systems representing the robot’s base, a transported object, etc. We use a task-parametrized formulation of a Gaussian mixture model that allows us not only to encode the human demonstrations, but also to extract automatically varying constraints acting in different coordinate systems [5]. Moreover, by estimating the stiffness of the virtual system through convex optimization [6], the robot is able to interact with different compliance levels, thus extending its capabilities to impedance-based behaviors. The contributions of the proposed framework are threefold: (i) exploitation of both position and force data in HRC; (ii) learning of compliance and position/force constraints; and (iii) modulation of the robot’s behavior based on the user’s actions as a result of the task-parametrized formulation. To show the flexibility of our approach, we test it in two different experiments. The first scenario consists of a collaborative transportation task, where the robot needs to simultaneously handle position and force constraints, while adapting its compliance level (Section IV-A). The second experimental setting considers the collaborative assembly of a wooden IKEA table, where the robot learns to adapt its compliance level based on position and haptic information (see Section IV-B). The remainder of the paper is organized as follows: Section II reviews the related work. The interaction model, the learning algorithm and the stiffness estimation are described in Section III. Section IV presents the experimental settings and results. The pros and cons of the approach are discussed in Section V. Conclusions and future work are presented in Section VI. II. RELATED WORK A. Control-based approaches Human-robot collaboration has been investigated from the early nineties, when purely control-based approaches were dominant. Kosuge et al. [7], [8] proposed an admittance control based on the apparent mechanical impedance of an object manipulated by multiple robots and a human. The force applied by the human was transferred to the robot controllers, so that the human could command the motion of the object while the robots behaved as followers. Al-Jarrah and Zheng [9] introduced a two-level control scheme, where an admittance controller was driven by a higher level reflex control. The latter was triggered using a force-based threshold indicating that the robot acted as a load for the human. Force information was also exploited to estimate the human intention in cooperative tasks and change the robot control law accordingly [10]. The authors proposed to add the rate of change of the sensed force to the robot controller [11], while varying its damping as a function of the magnitude changes of the force. Still based on admittance control, Bussy et al. [12] proposed to modify the robot control action according to the equilibrium trajectory hypothesis in a cooperative transportation setting. The equilibrium trajectory was computed from a desired dynamics of the object. The robot was then endowed with a velocity-dependent proactive behavior through a finite state machine whose states corresponded to hand-coded motion primitives, which were sequenced according to the user’s intention. This approach is similar to the decomposition into non-holonomic motions represented by predefined virtual mechanisms [13], [14]. Agravante et al. [15] used a model of the task and visual servoing for defining the reference of an admittance controller (analogous to the equilibrium trajectory in [12]). The authors claimed that using exclusively forcebased control might be insufficient in HRC, hence that visual information could improve the robot performance. In contrast to [12], [14], our approach based on a virtual spring-damper system computes the equilibrium trajectory (which we also refer to as attractor) from a desired dynamics of the robot that is learned from kinesthetic demonstrations (see Section III-A). Here the robot motion depends not only on the interaction forces, but also on the task parameters influencing the collaborative behavior. It is worth noticing that the key limitation in the works reviewed above has been the need for a model of the task linked to an analysis of the possible robot movements, so that both the parameters and the structure of the controller can be designed accordingly. This significantly decreases the flexibility of these approaches in the sense that if a new robot skill is required or if an additional constraint needs to be considered, the controllers have to be redesigned. This limitation is here overcome by exploiting PbD. B. Human performance-based approaches Several works rely on human-human collaboration studies to assist in the design of the robot controllers. Ikeura et al. proposed to approximate human cooperation using variable admittance control (with zero stiffness). From data collected when two people jointly carried an object, the damping parameter was estimated according to the precision required by the task, either through least squares [11], or by minimizing a cost function that penalized high rates of change [16]. The approach was then improved by introducing stiffness into the controller, whose parameters were estimated from force and position data recorded when a single human completed the task following minimum jerk robot movements [17]. The minimum jerk model [18] has also been an inspiration for Maeda et al. [19] and Corteville et al. [20]. Such a model was used in [19] to estimate the human hand position in a human-robot carrying task. This estimation was then used as the reference for the robot controller. Similarly, in [20] an admittance-controlled robotic assistant set its reference based on the minimum jerk model. In order to improve the cooperation, the robot adjusted its speed profile based on Kalman estimations of the human motion. Human motion estimators were also adopted in [21], where a hidden Markov model (HMM) predicted the human intention based on haptic data and modified the robot controller reference accordingly. Tsumugiwa et al. [22] used an admittance controller, where the damping varied according to the estimate of the human arm stiffness. Their approach assumed that a low velocity cooperative system remains stable if the robot’s damping varies proportionally to the human stiffness. In [23] a robot controller emulating how humans compensate for interaction forces and instability was proposed. Composed of both feedforward and feedback terms, the controller iteratively learned to adapt to perturbations by minimizing motion errors and a metabolic cost, achieving variable impedance behaviors. IEEE TRANSACTIONS ON ROBOTICS 3 The works above suggest that both force sensory information and variable impedance are relevant for HRC, leading us to consider these aspects into our learning framework. The goal in most of these approaches is to emulate the way humans act in collaboration by shaping the parameters of a predefined controller using motion/force patterns sensed while a human-human pair carries out the task. The success of these methods mostly relies on how well the parameters of the robot controller are set to match the human collaborative behavior. Such approach may narrow the variety of collaborative behaviors that a robot could acquire, because characteristics like speed, power, precision, among others, may not be properly exploited. We propose to handle this by exploiting learning from demonstrations to transfer these characteristics. C. Learning-based approaches Evrard et al. [24] proposed the use of Gaussian mixture models (GMM) and Gaussian mixture regression (GMR) to respectively encode and reproduce robot collaborative behaviors. Leader and follower roles of a cooperative lifting task were demonstrated by teleoperation. The GMM encapsulated the robot motion and the sensed forces, while GMR generated the reference force during reproduction. Medina et al. [25] endowed a robot with a cognitive system providing segmentation, encoding and clustering capabilities for demonstrations of collaborative behavioral primitives. These were represented by a primitive graph and a primitive tree using HMMs that were incrementally updated during reproduction [26]. One of the main differences with respect to [24] was that the robot started behaving as a follower, but its role became more proactive as it acquired more knowledge about the task. Gribovskaya et al. [27] proposed a hybrid structure based on PbD and adaptive control. A model of the task was learned from demonstrations encoded by a GMM to generate feedforward control signals. Then, the impedance controller parameters were adapted as a function of kinematic and force errors generated in the task. Dynamic movement primitives (DMP) have also been successfully used in HRC where interaction forces were considered. In [28], the learning problem was treated as that of finding an acceleration-based predictive reaction for coupled agents, in response to force signals indicating disagreements due to obstacle avoidance or different paths to follow. Gams et al. [29] argued that such adaptation should be done not only at acceleration, but also at velocity level, allowing for smoother interactions. Their approach learned coupled DMPs using iterative learning control that exploited the force feedback generated during several executions of the task. Note that our framework shares similarities with [24], [28], [29] in the sense that interaction forces are considered as additional variables influencing the collaborative robot behavior. Indeed, in our work, these forces affect not only the robot motion, but also its time-varying impedance. Ben Amor et al. [30] proposed a probabilistic encoding of the DMP parameters that allowed for adaptation and correlation of the robot motion based on predictions of the human intention from partial observations. Their formulation used dynamic time warping for shaping the future robot actions according to the partner timing. Maeda et al. [31] extended this idea by modeling the collaborative interaction using the probabilistic motion primitives introduced in [32], where the correlation between the trajectories of both the human and the robot is exploited for carrying out coordinated tasks in which the robot action is entirely conditioned by the user motion. This approach has been recently applied to learning multiple collaborative tasks [33]. Our work differs from [31], [32] in that (i) our task-based parametrization allows the robot to automatically adapt to a broader range of situations, therefore augmenting its generalization capability; (ii) the proposed stiffness estimation does not necessarily depend on the demonstrations variability, and moreover, guarantees to find optimal gain matrices; and (iii) interaction forces were considered along the whole learning process. Stiffness estimation: Variable impedance extends the robot learning capabilities beyond trajectory following tasks, where the robot is able to encode and reproduce more complex skills that involve, among others, contact with the environment and compliant bahaviors. In this context, several approaches have been proposed to estimate from collected data the stiffness and damping parameters to control robots. Erickson et al. [34] compared four different methods to estimate the robot impedance based on signal processing, adaptive control and recursive least squares. In [35], the authors estimated the nonlinear stiffness of robot joints with flexible transmissions by using dynamic residual signals along with least-squares and regressor-based techniques. From a different perspective, [36], [37] proposed to find a stiffness matrix using variability information extracted from human demonstrations in a PbD framework. The estimation was obtained from the inverse of the observed covariance encoded in a GMM [36], or from the conditional covariance in GMR [37]. Kronander and Billard [38] exploited the haptic information generated when the teacher shook or firmly grasped the robot for showing when high or low stiffness gains were required. Peternel et al. [39] used electromyography signals to teach a robot different compliance levels in a human-in-the-loop learning framework. In [1] we proposed to encode an impedance-based collaborative behavior through a set of virtual springs that were activated according to the robot perceptions. Both the equilibrium point and the stiffness of the springs regulating robot motion were learned from demonstrations. We later focused on learning position and force constraints arising in a cooperative transportation task [2]. A time-driven trajectory following motion was encoded by a virtual attractor that allowed the robot to follow a desired path and to apply the force required to transport the load. The current paper takes inspiration from [2] to propose an interaction model that can encapsulate the robot dynamics for a large variety of collaborative skills. Such a model is exploited in this paper to shape the robot compliance by formulating the attractor’s stiffness estimation as convex optimization, thus providing a more principled method than in [1] to estimate the stiffness gain matrices. The proposed task-parametrized encoding of the demonstrations permits the modulation of the robot actions as a function of task variables. In summary, this paper builds on the insights obtained in [1], [2] to introduce a compact PbD model that is able: 1) to IEEE TRANSACTIONS ON ROBOTICS 4 Fig. 2. Diagram of the proposed framework. Learning: Demonstrations of the task are collected and then used to extract the desired attractor trajectories. Input vectors along with attractor trajectories are used to train a TP-GMM and to estimate stiffness matrices associated to every Gaussian component. Reproduction: Given a set of task parameters, a temporary GMM is generated and later used for reproducing the robot behavior by applying GMR. learn different collaborative skills by exploiting both position and force information; 2) to optimally estimate the required time-varying compliance levels of the task; and 3) to modulate the robot’s behavior based on both the user’s actions and parameters of the task. A detailed description of the proposed approach is provided in the next section. III. PROPOSED APPROACH When a robot learns a collaborative role from teacher demonstrations, this needs a learning framework that encodes not only the raw data generated during the teaching process, but also an appropriate interaction model encapsulating the dynamics of the desired behavior. Moreover, some tasks may require the robot to manipulate objects, to pass them through specific locations in the robot workspace, and/or to apply specific forces to them. It is highly desirable that such task features are integrated in a joint model, which should be flexible enough, so that the robot can effortlessly be taught different collaborative skills as required. This section shows how we address these challenging aspects by proposing a novel PbD structure that combines a simple interaction model, probabilistic learning, and a stiffness estimation process for learning and reproducing collaborative tasks (see Fig. 2). A. Interaction model Let us represent the movement of the robot’s end-effector as a single unit mass moving in Cartesian space under the effect of a control input fmand interaction forces feas ¨ x=fm+fe,(1) where ¨ xis the acceleration of the mass, and the existence of the inverse dynamic model of the robot is assumed [40]. Hence, we formulate our problem as finding the motion control forces fmto attain the desired task dynamics. To achieve this aim, we propose that the robot behavior during the interaction – at each time step t– is driven by a virtual springdamper system. Specifically, the desired robot motion during interaction is given by ¨xt=KP t(yt−xt)−KV t˙xt+fe t,(2) where KP t,KV tand ytare the full stiffness matrix, the damping and the attractor point in the trajectory of the virtual system, respectively. By observing the evolution of the robot during the demonstrations of a task, with position xt, velocity ˙xtand acceleration ¨xt, and also the interaction forces fe t obtained by a sensor on the robot’s end-effector, the evolution of the attractor ytcan be computed as yt=hI,(KP t)−1 KV t,(KP t)−1,(KP t)−1i    xt ˙xt ¨xt fe t     .(3) corresponding to a simple linear transformation of the observed data. Note that (2) shares similarities with the DMPs formulation [41], where instead of encapsulating the robot motion by the attractor trajectory, a forcing term individually drives each variable of the robot movement. This interaction model allows us to shape the robot behavior by varying both the stiffness and the attractor, based on the task requirements. Thus, we propose to tackle this problem from a robot learning perspective. Specifically, the variables ytand KP twill be learned from kinesthetic demonstrations provided by a human teacher who shows the robot its collaborative role. In the rest of this section, we explain how to extract these variables from examples of given tasks. Note that KV tis not estimated here, but its values are pre-specified according to, for instance, a desired response of the linear system (2). B. Learning After having observed a set of demonstrations in some situations, we would like to generalize the skill to new situations. For instance, consider the scenario in which a human-robot dyad manipulates an object. The robot movements may largely depend on the initial and goal positions of the object, poses of obstacles populating the robot workspace, and additionally the robot may be required to react to human actions, that can be understood as position/orientation variations of specific parts of the human body. These variables influencing the robot behavior can be represented as reference frames, which we will refer to as task parameters. For generalization purposes, it is desirable for the robot to automatically adapt to new configurations of these parameters (e.g., unobserved positions and orientations of a manipulated object). To address the aforementioned issue, we propose to probabilistically encode the demonstrations with a taskparametrized version of the Gaussian mixture model (TPGMM) [42]. This model allows us to compactly capture the dependence of the robot motion on configuration changes of the task parameters. Formally, these parameters are represented as Pcoordinate systems, defined at time step t IEEE TRANSACTIONS ON ROBOTICS 5 by {bt,j,At,j}P j=1, representing respectively the origin of the reference frame and a set of basis vectors {e1,e2, . . .} forming a transformation matrix A= [e1e2· · · ]. Note that in this paper we focus on the special case in which the task parameters {bt,j,At,j}P j=1 represent translations and rotations in the Cartesian space, but the model can be extended to any other affine transformation (including scaling and projections). A demonstration ξ∈RD×Tis encoded in these different reference frames, forming a third order tensor dataset X∈RD×T×P, composed of Ptrajectory samples X(j)∈RD×Tprojected on Pcandidate frames, corresponding to matrices composed of D-dimensional observations at Ttime steps. The model parameters are defined by {πi,{µ(j) i,Σ(j) i}P j=1}K i=1, where πiare the mixing coefficients, µ(j) iand Σ(j) iare the center and covariance matrix of the i-th Gaussian component in frame jin a TP-GMM with Kcomponents. Let us emphasize that the term model parameters refers here to the learned parameters of a model describing the movement or skill, whereas the external parameters representing the current situation (such as positions of objects or users) will be denoted task parameters. The latter are used as inputs to transform the learned model parameters in accordance to the situation. Learning of the model parameters is achieved by maximizing the log-likelihood under the constraint that the data in the different reference frames are generated from the same source, resulting in an Expectation Maximization (EM) process to iteratively update the model parameters until convergence. E-step: γt,i = πi P Q j=1 NX(j) t|µ(j) i,Σ(j) i PK k=1 πk P Q j=1 NX(j) t|µ(j) k,Σ(j) k .(4) M-step: πi=PT t=1 γt,i T,µ(j) i=PT t=1 γt,i X(j) t PT t=1 γt,i ,(5) Σ(j) i=PT t=1 γt,i (X(j) t−µ(j) i)(X(j) t−µ(j) i)> PT t=1 γt,i .(6) The model parameters are initialized with a k-means procedure (with five consecutive random clusters initialization) redefined through a similar process to that used for the above EM algorithm. The model selection is carried out by applying Bayesian Information Criterion (BIC) [43] to a set of TP-GMMs with different number of states. Note that other techniques such as Dirichlet processes for infinite Gaussian mixture models [44] can be alternatively used for computing the number of components in the model. Notice that in a standard GMM, the role of EM is to estimate constant Gaussian parameters µiand Σi. Here, EM is used to estimate task-parameterized model parameters µ(j) i and Σ(j) iby incrementally modeling the local importance of the reference frames. In the proposed experiments, the overall learning process typically takes 1 to 5 seconds. The learned model can further be used to reproduce movements in other situations (for new positions and orientations of reference frames). The model first retrieves a GMM at each time step tby computing a product of linearly transformed Gaussians N(µt,i,Σt,i)∝ P Y j=1 NAt,j µ(j) i+bt,j,At,jΣ(j) iA> t,j.(7) By using the product properties of multivariate normal distributions, the above distribution is evaluated with Σt,i =P X j=1 (At,jΣ(j) iA> t,j) −1−1 ,(8) µt,i =Σt,i P X j=1 (At,j Σ(j) iA> t,j) −1(At,j µ(j) i+bt,j).(9) C. Stiffness estimation In our framework, we exploit the interaction model (2) that encapsulates the desired robot dynamics extracted from demonstrations to estimate a varying stiffness matrix. Such estimation can be carried out in a local fashion by taking advantage of the probabilistic encoding of the data. In other words, after having encoded the demonstrations with TPGMM, we estimate local stiffness matrices KP iassociated to each Gaussian component iof the model. Each model component is locally encoding part of the desired robot dynamics, and thus each KP ishould fulfill some local dynamics. The robot motion during interaction defined by (2) can be rewritten to express the local dynamics encoded by a Gaussian component ias γt,i (¨ xt+KV˙xt−fe t) = γt,iKP iµy t,i −xt,(10) with KP i∈Sm +being the stiffness matrix of the component i, where Sm +denotes the set of symmetric positive semi-definite m×mmatrices. The weights γt,i computed from (4) allow us to determine a region of validity in which the estimated stiffness matrix KP iis optimal for the demonstrated dynamics. Observe that µy t,i is the subvector of the center µt,i spanning the attractor data dimensions. We treat here stiffness matrix estimation as a weighted norm approximation problem. In other words, we want to minimize the Euclidean norm of the residuals rt(KP i) = γt,i (KP i˜ xt,i −vt),(11) subject to the matrix inequality constraint introduced by the positive semidefiniteness of the stiffness matrix KP i. Note that ˜ xt,i = (µy t,i −xt)and vt=¨ xt+KV˙xt−fe t. Here the weights γt,i define the estimation neighborhood, so that only the datapoints belonging to the component iwill be considered to estimate the matrix KP i. The above objective function and the positive semi-definite constraint on the stiffness matrix can be formulated as the convex optimization problem, minimize KP i krt(KP i)k2,subject to KP i0,(12) which is a semidefinite program (SDP). As a result of the convexity of both the objective function and the inequality IEEE TRANSACTIONS ON ROBOTICS 6 constraint, standard techniques such as interior-point methods [6] can be used to efficiently solve this SDP problem. In this manner, optimal stiffness matrices are obtained to locally satisfy the dynamics observed during the demonstrations. This estimation process is carried out once for each Gaussian component i. Then, the demonstrations are discarded, which means that during reproduction, only the TP-GMM and associated stiffness matrices are needed. D. Skill reproduction With the temporary GMM computed in (7), the reproduction of a reference movement or behavior can be formalized as a regression problem [45]. Gaussian mixture regression (GMR) offers a simple solution to handle encoding, recognition, prediction and reproduction in robot learning [46]. In contrast to other regression methods such as Locally Weighted Projection Regression [47], or Gaussian Process Regression [48], GMR exploits the joint probability density function of the data modeled by TP-GMM for deriving the regression.3 Let us define the superscripts Iand Oas the sets of dimensions that span the input and output variables. At each iteration step t, the datapoint ξtcan be decomposed into two subvectors ξI tand ξO tspanning the input and output variables, respectively. With this notation, a block decomposition of the datapoint ξt, vectors µiand matrices Σican be written as ξt=ξI t ξO t,µi=µI i µO i,Σi=ΣI iΣIO i ΣOI iΣO i.(13) The temporary GMM estimated in (7) encodes the joint distribution P(ξI,ξO)∼PK i=1 πiN(µi,Σi)of the dataset ξ. At each reproduction step t,P(ξO t|ξI t)is computed as the conditional distribution P(ξO t|ξI t)∼ K X i=1 hi(ξI t)NˆµO i(ξI t),ˆ ΣO i,(14) with ˆ µO i(ξI t) = µO i+ΣOI iΣI i −1(ξI t−µI i),(15) ˆ ΣO i=ΣO i−ΣOI iΣI i −1ΣIO i,(16) and hi(ξI t) = πiN(ξI t|µI i,ΣI i) PK kπkN(ξI t|µI k,ΣI k).(17) Note that according to our proposed interaction model, the robot behavior is driven by a virtual attractor y(see (3)), which corresponds to the output vector ξOof our regression model, while the input vector ξIwill depend on the characteristics of the problem at hand, as shown in Section IV. Moreover, the stiffness matrices previously estimated define the stiffness of the virtual attractor as KP t=PK i=1 hi(ξI t)KP i, at each time step t. In the proposed experiments, the GMR computation at each time step took less than 1 millisecond. Fig. 2 shows an illustrative diagram of the proposed framework, while Table I summarizes our approach and its different stages. 3Note that GPR with Gaussian kernels also exploits the conditioning property of normal distributions, but in a different way. The input and output variables of the joint distribution in GPR represent respectively the new and previous observations, where a GP is used for each movement dimension. In contrast, the input and output variables of the joint distribution in GMR represent respectively the multivariate inputs driving the behavior and the multivariate movement. TABLE I SUMMARY OF THE PROPOSED APPROACH 1. Task demonstrations - Determine P(number of frames or task parameters). -∀t∈ {1,...,T}, collect ξtobserved from the P different reference frames and form the third-order tensor X. 2. Model fitting (see Section III-B) - Determine K(number of components of the model). - Use (4)-(6) to learn {πi,{µ(j) i,Σ(j) i}P j=1}K i=1 . 3. Stiffness estimation (see Section III-C) - Find KP ifor each Gaussian component ithrough convex optimization using (11) and (12). 4. Reproduction (see Section III-D) - Set the input Iand output Oelements. for t←1to T(for each reproduction time step) - Collect/select ξI tand {bt,j ,At,j }P j=1. - Use (7) to estimate temporary GMM parameters {µt,i,Σt,i}K i=1 modeling ξI tand ξO tas ξI t,ξO t∼PK i=1 πiN(µt,i,Σt,i). - Use (14) to retrieve ξO tthrough GMR. - Compute the attractor’s stiffness matrix as KP t=PK i=1 hi(ξI t)KP i. - Compute the reference acceleration ¨xtfrom (2). end It is worth highlighting that the foregoing probabilistic formulation of the problem models not only local correlations among outputs, but also among inputs, and between inputsoutputs (see (13)), in contrast to standard DMPs in which a movement is considered as a set of univariate outputs synchronized by a decay term. In our application, the correlation information held by the joint inputs-outputs is crucial, not only to determine the relevance of the task parameters, but also to discover and re-use local sensorimotor patterns or synergies. IV. EXPERIMENTS This section introduces the two experimental settings that were used to test the performance of the proposed learning framework and to show its flexibility and generalization capability in HRC scenarios. For each task, we show and explain the obtained results for each module of the whole learning approach previously described. A. Transportation Task 1) Description: the first task consists of teaching a robot to simultaneously handle the position and force constraints arising when an object is cooperatively manipulated or transported (see Fig. 3). At the beginning of the transportation task, the two participants simultaneously reach for the object. Once they make contact with the object, they start jointly transporting it along a given path to reach the target location. When the object gets to the final position, the human-human pair releases it and moves away from the object. Both the starting and goal object position/orientation may vary across repetitions. Here, the aim is to automatize such part of this collaborative task by replacing one of the human participants by a robot. For this experiment, we used a torque-controlled 7-DoF WAM robot equipped with a 6-axis force/torque sensor. In the demonstration phase, the gravity-compensated robot is kinesthetically guided by the teacher while cooperatively achieving the task with the other user, as shown in Fig. 3. The teacher IEEE TRANSACTIONS ON ROBOTICS 7 Demonstration Reproduction Force Sensor Fig. 3. Experimental setting of the human-robot transportation task. The top row shows the kinesthetic demonstration (left), and reproduction phase (right). A snapshots sequence of a demonstrations is shown in the bottom row. shows the robot both the path to be followed and the force pattern it should apply while transporting the object. Hence, the learned attractor will be directly associated to the desired force to be applied to the transported object, as computed in (3). In other words, the proposed formulation of the interaction process encodes the specific position and force requirements to be fulfilled during reproduction. In this experiment three task parameters (P= 3) are considered, namely, the frames representing the initial S and target Tlocations of the object, and a third frame I attached to an object that is not relevant for the task, whose position randomly varies across demonstrations. The purpose of introducing the frame Iis to exhibit how the TP-GMM handles task parameters that are irrelevant for the problem at hand. The task parameters are defined as At,1=101×3 03×1RS,bn,1=0 xS o, At,2=101×3 03×1RT,bn,2=0 xT o,and At,3=101×3 03×1RI,bn,3=0 xI. Here, xS oand xT oare the Cartesian positions where the object is picked up and then released. Similarly, RSand RT respectively represent the initial and final orientation of the object with orientation matrices. Finally, xIand RIare the position and orientation of the irrelevant object.4The datapoint ξis defined as ξI=tand ξO=y, where tand yare time and the attractor’s path (see (3)), respectively. During reproduction of the task, the initial and target frames are given to the model in order to obtain the temporary GMM parameters using (7). Then, the robot and the user transport the object towards the target location. Here, for each time step t, the robot obtains a new attractor location (see (14)) along with an estimated stiffness matrix, which generate a new desired acceleration in the operational space of the robot. For simplicity of the experiment, the orientation of the robot end-effector was kept constant. However, the approach also supports tasks with variable end-effector orientations [49]. 2) Results: Five examples of the collaborative behavior are given to the robot. The demonstrations are then used for 4The pose of the objects was predefined in this experiment, but these can alternatively be obtained using an optical tracking system. K 2 3 4 5 6 7 8 9 10 BIC -800 -600 -400 -200 Fig. 4. Transportation task: Bayesian Information Criterion (BIC) for TPGMMs with different number of states K. Initial Frame Target Frame Fig. 5. Transportation task: Local models in the initial frame S(where the object is picked up), and in the target frame T(where the object is released). The gray lines depict the attractor trajectories projected on the corresponding reference frame. The ellipsoids represent the components of the local models. The black dots and crosses depict the beginning and the end of the demonstrations. The small and narrow ellipsoids reflect the high local invariance of the trajectories observed from the different frames. The big ellipsoids represent regions where precision is not required. Units are given in meters. training several TP-GMMs with different number of components. Then, we used a Bayesian Information Criterion (BIC) [43] to automatically select the TP-GMM providing the best trade-off between the data fitting and the number of Gaussian components. According to the values shown in Fig. 4, the model with five components has the lowest BIC value, thus offering the best compromise. Fig. 5 shows the resulting encoding of the attractor trajectories computed from (3) and observed from the perspective of the frames Sand T. Notice that the multiple demonstrations are locally consistent when the robot approaches the initial location of the object (frame S), and when the manipulator moves away from it once it reached its target position (frame T). This is reflected by the small and narrow ellipsoids in these parts of the task. The attractor trajectories observed from the frame Ido not show any consistent or useful information regarding the task, and therefore they are not shown. Handling irrelevant task parameters: Fig. 6 shows how the TP-GMM adapts to variations on the task parameters. The experiments consisted of varying the position of one frame at once while all others remain unchanged. we can see that when the frames Sand Tmove, the resulting (temporary) model adapts its components accordingly. In contrast, when the irrelevant frame Imoves, the model remains unaltered. This is explained by the fact that TP-GMM exploits the variability of the demonstrations observed from every frame to automatically discover which task parameters are relevant for the task. Such characteristic permits the selection of a common list of candidate task parameters without carrying out a thorough analysis about their importance for the skill or IEEE TRANSACTIONS ON ROBOTICS 8 Fig. 6. Transportation task: The first, second, and third graphs respectively show the TP-GMM obtained when the initial frame S(red), the target frame T(blue), and the irrelevant frame I(orange) are moved on the plane x2x3. The model automatically discovers that the frame Iis irrelevant for the task, and thus remains unchanged when this moves. Units are given in meters. behavior to be learned. Reproduction of the task: After learning, the obtained model was used to test the reproduction and generalization of the task on the real platform. Three types of tests were carried out to evaluate the performance. First, the human and robot cooperatively transported the load for new configurations of the task parameters while the force applied to the load was similar to those observed during the demonstrations. Fig. 7 (top) displays three successful reproductions under the aforementioned conditions where both the starting and target locations varied, and the robot adapted to the change of situation accordingly. Fig. 7 (middle) shows one of these reproduction attempts where the sensed force profile remains nearly constant throughout the whole reproduction. It is worth highlighting that the observed offset between the end-effector position and the attractor path allows the robot to apply the desired force to the object while transporting it. Fig. 8 shows the corresponding TP-GMM for the same reproduction, along with the stiffness profiles and the activation weights. Note that, in this case, time is the variable governing the influence of the model components on the stiffness estimation. Regarding the compliance level of the robot, notice that the robot behaves compliantly along x1, while being stiffer along the axes x2 and x3. This behavior is reproduced both at the beginning and at the end of the execution of the task, when the robot does not allow high variations on the plane x2x3, guaranteeing that the object is picked up and released by following trajectories consistent with the demonstrations. As expected, the retrieved behavior is the opposite when the robot is cooperatively transporting the load in the middle of the path. In this part of the task, the robot behaves stiffly along x1, while allowing deviations on the plane x2x3. The second test consisted of applying a varying force to evaluate how the robot reacted to force variations not observed during learning. The human operator started the task pushing the object with a force higher than those taught during the demonstrations. Then, the applied force was significantly reduced, and finally it reached values similar to the demonstrations, as shown in Fig. 7 (bottom). As it can be observed, the robot could successfully adapt to these force variations. When the force along the x1axis was high, the robot allowed small deviations from the path, still ensuring that the position constraint remained within a feasible range determined by the observed variability in the demonstrations Reproductions with various start and target locations Nearly constant force applied to the object −0.1 0.35 0.8 −0.45 0 0.45 x1 x2 −0.1 0.35 0.8 −0.45 0 0.45 x1 x3 −0.45 0 0.45 −0.45 0 0.45 x2 x3 Human partner varies the force applied to the object −0.05 0.4 0.85 −0.45 0 0.45 x1 x2 −0.05 0.4 0.85 −0.45 0 0.45 x1 x3 −0.45 0 0.45 −0.45 0 0.45 x2 x3 Fig. 7. Transportation task: The robot performs the task for new taskparameters configurations, as well as successfully adapts to variations on the force applied to the object. The black and light gray lines respectively represent the robot’s trajectory and the attractor y. The light brown arrows display the sensed force at the end-effector. The dots and crosses respectively display the start and end of the reproduction. Units are given in meters. 0.2 0.4 0.6 0.8 1 0 0.5 1 0.2 0.4 0.6 0.8 1 0 700 1400 Fig. 8. Transportation task: (left) The top plot shows the influence of the learning model components along the reproduction, where the color matches that of the ellipsoids. The bottom plot displays the stiffness profile (Newton per meter) along the main axes of motion. (right) The resulting TP-GMM for the set of task parameters given at the beginning of the reproduction. The trajectory followed by the robot (in meters) is shown in solid gray line. Note that the model components determine the varying stiffness matrix used by the robot during reproduction. and the impedance parameters. In contrast, when the force was very low (i.e., the human nearly losing contact with the object), the robot moved to try to compensate for the reduced force and prevent the object from being dropped. Note that despite the force variations, the robot was able to transport the object along a similar path in the other dimensions, by showing a collaborative behavior that is an appropriate compromise between force and position constraints, automatically extracted from the statistical representation of the demonstrations and the interaction model. A video of the experiment is available at http://programming-by-demonstration.org/TRO2016/ IEEE TRANSACTIONS ON ROBOTICS 9 x1 0.2 0.4 0.6 0.8 1 0.2 0.6 1 t ¨x1 0.2 0.4 0.6 0.8 1 −1.5 0 1.5 x2 0.2 0.4 0.6 0.8 1 −0.45 −0.05 0.35 t ¨x2 0.2 0.4 0.6 0.8 1 −1.5 0 1.5 x3 0.2 0.4 0.6 0.8 1 −0.6 −0.2 0.2 t ¨x3 0.2 0.4 0.6 0.8 1 −1.5 0 1.5 (a) No perturbation x1 0.2 0.4 0.6 0.8 1 0.2 0.6 1 t ¨x1 0.2 0.4 0.6 0.8 1 −1.5 0 1.5 x2 0.2 0.4 0.6 0.8 1 −0.45 −0.05 0.35 t ¨x2 0.2 0.4 0.6 0.8 1 −1.5 0 1.5 x3 0.2 0.4 0.6 0.8 1 −0.6 −0.2 0.2 t ¨x3 0.2 0.4 0.6 0.8 1 −1.5 0 1.5 (b) With perturbations Fig. 9. Transportation task: Comparison of collaborative behavior reproductions where the stiffness matrix of the virtual attractor was learned with the proposed approach (blue line), or manually set to high and low constant stiffness (green and red lines, respectively). The equilibrium trajectory of the attractor is depicted by the black dashed line. The top row shows the trajectories followed by the end-effector for the foregoing cases. The bottom graphs display the desired accelerations computed from (2). Gray areas represent periods of time when a perturbation was applied to the robot. Units are given in meters. Comparison with baseline executions: Notice that the aforementioned impedance adaptation capabilities are crucial for both successfully tracking the desired trajectories and interacting with the human user safely. In order to show this, we compared the robot reproduction using our approach with baseline executions. Specifically, for a given set of task parameters, we carried out the reproduction of the transportation task with (i) stiffness matrices computed as described in Section III-C, (ii) high stiffness (i.e., KP t=2000I3×3), and (iii) high compliance (i.e, KP t=100I3×1). Fig. 9 shows the results for these cases under two different conditions, namely when the human interacts with the robot similarly to the demonstrations, and when the robot faces perturbations during the execution of the task. Observe that when the robot uses a high compliance, it is not able to follow the desired trajectory once the user is in contact with the object (around 0.2 in the time axis). This means the robot does not compensate for the forces applied by the user, as required to satisfactorily transport the object. Moreover, when a perturbation occurs, the robot is significantly moved away from the desired trajectory, unable to respond accordingly even in the parts where a high precision trajectory tracking is required (see gray areas in Fig. 9b). The use of high constant stiffness also showed some important drawbacks in this experiment. Firstly, despite the robot was able to precisely follow the attractor trajectory, it did not respond to interaction force variations during reproduction, because the high stiffness is only used to compensate position errors. Secondly, the desired end-effector accelerations are considerably higher than those computed from the proposed adaptive stiffness approach, as observed in Fig. 9. This may lead the controller to apply very high torque commands, thus inducing an unsafe human-robot interaction. B. Table Assembly Task 1) Description: We consider a human-robot collaborative task where the robot’s role is to hold a wooden table while the human screws the four legs to it, similarly as two persons would collaborate to carry out the same task (as illustratively shown in Fig. 10, middle row). Fig. 10 (top) presents an example of assembly instructions that can be found in “do it yourself” furniture catalogs. Here, two small tables require specific sequences of force and movement to get assembled. Fig. 10. Top: Examples of table assemblies characterized by different sequences, positions and orientations of components, with haptic and movement patterns that are specific for each item. Middle: Illustration of a table assembly task carried out by two persons. Bottom: demonstration (left) and reproduction (right) of the table assembly task in the experimental setup. Learning such specificities is required for an efficient collaborative assembly. Instead of manually programming those specificities for each item, we would like the robot to extract them automatically from demonstrations provided by two persons collaborating to assemble the different parts of the table (see Fig. 10, bottom row). After learning, the task can be reproduced by a single user, with the robot interacting appropriately with respect to the preferences of the user and the specificities of the item being assembled. We thus do not need to provide the robot with information about the points of assembly, the different options, orientation of table legs, etc. The robot instead learns these specificities from demonstrations. In the learning phase two persons perform the task, where one is kinesthetically guiding the robot to demonstrate the robot’s role. The compliance behavior of the person holding the table changes to allow the other person to perform the corresponding sub-task more easily (see Fig. 10, bottom row). During reproduction, the robot replaces the person holding