When humans and robots collaborate, mutual understanding is paramount for the success of any shared task. Mutual understanding means that the human is aware of the robot’s current task, status, goal, available information, that he/she can reasonably predict or expect what it will do next, and vice versa. Recognizing the intention is only one piece of the problem but still plays a crucial part for providing anticipatory capabilities.

Formalizing intention can be a daunting task, as one may find it difficult to provide a unique representation that explains the intention for very low-level goal-directed tasks (e.g., reaching a target object and grasping it) and for very high-level, complex, abstract or cognitive tasks (e.g., change a light bulb on the ceiling—by building a stair composed of many parts, climbing it and reaching the light bulb on the ceiling, etc.). Demiris (2007) reviews different approaches of action recognition and intention prediction.

From the human’s point of view, understanding the robot’s intention means that the human should find intuitive and non-ambiguous every goal-directed robot movement or actions, and it should be clear what the robot is doing or going to do (Kim et al., 2017). Dragan and Srinivasa (2014) formalized the difference between predictability and legibility: a motion is legible if an observer can quickly infer its goal, while a motion is predictable when it matches the expectations of the observer given its goal.

The problem of generating legible motions for robots has been addressed in many recent works. For example, Dragan and Srinivasa (2014) use optimization techniques to generate movements that are predictable and legible. Huang et al. (2017) apply an Inverse Reinforcement Learning method on autonomous cars to select the robot movements that are maximally informative for the humans and that will facilitate their inference of the robot’s objectives.

From the robot’s point of view, understanding the human’s intention means that the robot should be able to decipher the ensemble of verbal and non-verbal cues that the human naturally generates with his/her behavior, to identify, for a current task and context, what is the human intention. The more information (e.g., measurable signals from the human and the environment) is used, the better and more complex the estimation can be.

The simplest form of intention recognition is to estimate the goal of the current action, under the implicit assumption that each action is a goal-directed movement.

Sciutti et al. (2013) showed that humans implicitly attribute intentions in form of goals to robot motions, proving that humans exhibit anticipatory gaze toward the intended goal. Gaze was also used by Ivaldi et al. (2014a) in a human–robot interaction game with iCub, where the robot (human) was tracking the human (robot) gaze to identify the target object. Ferrer and Sanfeliu (2014) proposed the Bayesian Human Motion Intentionality Prediction algorithm, to geometrically compute the most likely target of the human motion, using Expectation–Maximization and a simple Bayesian classifier. In Wang et al. (2012), a method called Intention-Driven Dynamics model, based on Gaussian Process Dynamical Models (GPDM) (Wang et al., 2005), is used to infer the intention of the robot’s partner during a ping-pong match, represented by the target of the ball, by analyzing the entire human movement before the human hits the ball. More generally, modeling and descriptive approaches can be used to match predefined labels with measured data (Csibra and Gergely, 2007).

A more complex form of intention recognition is to estimate the future trajectory from the past observations. In a sense, to estimate [xt+1,…,xt+Tfuture]=f(xt,xt−1,…,xt−Tpast). This problem, very similar to the estimate of the forward dynamics model of a system, is frequently addressed by researchers in model predictive control, where being able to “play” the system evolving in time is the basis for computing appropriate robot controls. When a trajectory can be predicted by an observer from early observations of it, we can say that the trajectory is not only legible, but predictable. A systematic approach for predicting a trajectory is to reason in terms of movement primitives, in such a way that the sequence of points of the trajectory can be generated by a parametrized time model or a parametrized dynamical system. For example, Palinko et al. (2014) plan reaching trajectories for object carrying that are able to convey information about the weight of the transported object. More generally, in generative approaches (Buxton, 2003), latent variables are used to learn models for the primitives, both to generate and infer actions. The next subsection will provide more detail about the state-of-the-art techniques for generating movement primitives.

In Amor et al. (2014), the robot first learns Interaction Primitives by watching two humans performing an interactive task, using motion capture. The Interaction Primitive encapsulates the dependencies between the two human movements. Then, the robot uses the Interaction Primitive to adapt its behavior to its partner’s movement. Their method is based on Dynamics Motor Primitives (Ijspeert et al., 2013), where a distribution over the DMP’s parameters is learned. Notably, in this article, we did not follow the same approach to learn Interaction Primitives, since there is a physical interaction that makes the user’s and the robot’s movements as one joint movements. Moreover, there is no latency between the partner’s early movement and the robot’s, because the robot’s arm is physically driven by the human until the latter breaks the contact.

Indeed, most examples in the literature focus on kinematic trajectories, corresponding to gestures that are typically used in dyadic interactions characterized by a coordination of actions and reactions. Whenever the human and robot are also interacting physically, collaborating on a task with some exchange of forces, then the problem of intention recognition becomes more complex. Indeed, the kinematics information provided by the “trajectories” cannot be analyzed without taking into account the haptic exchange and the estimation of the “roles” of the partners in leading/following each other.

Estimating the current role of the human (master/slave or leader/follower) is crucial, as the role information is necessary to coherently adapt the robot’s compliance and impedance at the level of the exchanged contact forces. Most importantly, adapting the haptic interaction can be used by the robot to communicate when it has understood the human intent and is able to finish the task autonomously, mimicking the same type of implicit non-verbal communication that is typical of humans.

For example, in Gribovskaya et al. (2011), the robot infers the human intention utilizing the measure of the human’s forces and by using Gaussian Mixture Models. In Rozo Castañeda et al. (2013), the arm impedance is adapted by a Gaussian Mixture Model based on measured forces and visual information. Many studies focused on the robot’s ability to act only when and how its user wants (Carlson and Demiris, 2008; Soh and Demiris, 2015) and to not interfere with the partner’s forces (Jarrassé et al., 2008) or actions (Baraglia et al., 2016).

In this article, we describe our approach to the problem of recognizing the human intention during collaboration by providing an estimate of the future intended trajectory to be performed by the robot. In our experiments, the robot does not adapt its role during the physical interaction but simply switches from follower to leader when the human breaks contact with it.

Movement Primitives (MPs) are a well established paradigm for representing complex motor skills. The most known method for representing movement primitives is probably the Dynamic Movement Primitives (DMPs) (Schaal, 2006; Ijspeert et al., 2013; Meier and Schaal, 2016). DMPs use a stable non-linear attractor in combination with a forcing term to represent the movement. The forcing term enables to follow specific movement, while the attractor asserts asymptotic stability. In a recent paper, Meier and Schaal (2016) proposed an extension to DMPs, called PDMP (Probabilistic Dynamic Movement Primitive). This method improves DMP with probabilistic properties to measure the likelihood that the movement primitive is executed correctly and to perform inference on sensor measurement. However, the PDMPs do not have a data-driven generalization and can deviate arbitrarily from the demonstrations. This last difference can be critical for our applications with the humanoid robot iCub, since uncertainties are unavoidable and disturbances may happen frequently and destabilize the robot movement (for example, an unexpected collision during the movement). Thus, the ProMPs method is more accurate for our software.

Ewerton et al. (2015), Paraschos et al. (2013b), and Maeda et al. (2014) compared ProMPs and DMPs for learning primitives and specifically interaction primitives. With the DMP model, at the end of the movement, only a dynamic attractor is activated. Thus, it always reaches a stable goal. The properties allowed by both methods are temporal scaling of the movement, learning from a single demonstration, and generalizing to new final position. With ProMPs, we have in addition the ability to do inference (thanks to the distribution), to force the robot to pass by several initial via points (the early observations), to know the correlation between the input of the model, and to co-activate some ProMPs. In our study, we need these features, because the robot must determine a trajectory that passes by the early observations (beginning of the movement where the user guides physically the robot).

A Recurrent Neural Networks (RNN) approach (Billard and Mataric, 2001) used a hierarchy of neural networks to simulate the activation of areas in human brain. The network can be trained to infer the state of the robot at the next point in time, given the current state. The authors propose to train the RNN by minimizing the error between the inferred position of the next time step and the ground truth obtained from demonstrations.

Hidden Markov Models (HMMs) for movement skills were introduced by Fine et al. (1998). This method is often used to categorize movements, where a category represents a movement primitive. This method also allows to represent the temporal sequence of a movement. In Nguyen et al. (2005), they use learned Hierarchical Hidden Markov Model (HHMMs) to recognize human behaviors efficiently. In Ren and Xu (2002), they present the Primitive-based Coupled-HMM (CHMM) approach, for human natural complex action recognition. In this approach, each primitive is represented by a Gaussian Mixture Model.

Adapting Gaussian Mixture Models is another method used to learn physical interaction with learning. In Evrard et al. (2009), they use GMMs and Gaussian Mixture Regression to learn, in addition to the position (joint information), force information. Using this method, a humanoid robot is able to collaborate in one dimension with its partner for a lifting task. In this article, we will also use (Cartesian) position and force information to allow our robot to interact physically with its partner.

A subproblem of movement recognition is that robots need to estimate the duration of the trajectory to align a current trajectory with learned movements. In our case, at the beginning of the physical Human–Robot Interaction (pHRI), the robot observes a partial movement guided by its user. Given this partial movement, the robot must first estimate what the current state of the movement is to understand what its partner intent is. Thus, it needs to estimate the partial movement’s speed.

Fitts’ law models the movement duration for goal-directed movements. This model is based on the assumption that the movement duration is a linear function of the difficulty to achieve a target (Fitts, 1992). In Langolf et al. (1976), they show that by modifying the target’s width, the shape of the movement changes. Thus, it is difficult to apply Fitt’s law when the size of the target can change. In Langolf et al. (1976) and Soechting (1984), they confirm this idea by showing that the shape of the movement changes with the accuracy required by the goal position of the movement.

Dynamics Time Warping (DTW) is a method to find the correlation between two trajectories that have different durations, in a more robust way than the Euclidean distance. In Amor et al. (2014), they modify the DTW algorithm to match a partial movement with a reference movement. Many improvements over this method exist. In Keogh (2002), they propose a robust method to improve the indexation. The calculation speed of DTW is improved using different methods, such as FastDTW, Lucky Time Warping, or FTW. An explanation and comparison of these methods are presented in Silva and Batista (2016), where they add their own computation speed improvement by using a method called Pruned Warping Paths. This method allows the deletion of unlikely data. However, a drawback of this well-known DTW method is they do not preserve the global trajectory’s shape.

In Maeda et al. (2014), where they use a probabilistic learning of movement primitives, they improve the duration estimation of movements by using a different time warping method. This method is based on a Gaussian basis model to represent a time warping function and, instead of DTW, it forces a local alignment between the two movements without “jumping” some index. Thus, the resulting trajectories are more realistic, smoother, and this method preserves the global trajectories’ shapes.

For inferring the intention of the robot’s partner, we use Probabilistic Movement Primitives (ProMPs) (Paraschos et al., 2013a). Specifically, we use the ProMP’s conditioning operator to adapt the learned skills according to observations. The ProMPs can encode the correlations between forces and positions and allow better prediction of the partner’s intention. Further, the phase of the partner’s movement can be inferred, and therefore the robot can adapt to the partner’s velocity changes. ProMPs are more efficient for collaborative tasks, as shown in Maeda et al. (2014), where in comparison to DMPs, the root-mean square error of the predictions is lower.

One of the goals of this article is to introduce an open-source software for the iCub (but potentially for any other robot), where the ProMP method is used to recognize human intention during collaboration, so that the robot can execute initiated actions autonomously. This is not the first open-source implementation for representing movement primitives: however, it has a novel application and a rationale that makes it easy to use with the iCub robot.

In Table 1, we report on the main software libraries that one can use to learn movement primitives. Some have been also used to realize learning applications with iCub, e.g., Lober et al. (2014) and Stulp et al. (2013) or to recognize human intention. However, the software we propose here is different: it provides an implementation of ProMPs used explicitly for intention recognition and prediction of intended trajectories. It is interfaced with iCub, both real and simulated, and addresses in the specific case of physical interaction between the human and the robot. In short, it is a first step toward adding intention recognition ability to the iCub robot.

To facilitate understanding of the theoretical framework, we first introduce the notations we use in this section and throughout the remainder of the article.

Our toolbox to learn, replay and infer the continuation of trajectories is written in Matlab and available at:

https://github.com/inria-larsen/icubLearningTrajectories/tree/master/MatlabProgram

Let us assume the robot has recorded a set of n₁ trajectories: {Ξ1,…,Ξn1}, where the i-th trajectory is Ξi={ξ(1),…,ξ(tfi)}. ξ(t) is the generic vector containing all the variables to be learned at time t, with the ProMP method. It can be monodimensional (e.g., ξ(t) = [z(t)] for the z-axis Cartesian coordinate), or multidimensional (e.g., ξ(t) = [X(t), F(t)]^T). Note that the duration of each recorded trajectory (i.e., tfi) may be variable. To find a common representation in terms of primitives, a time modulation is applied to all trajectories, such that they have the same number of samples s¯ (see details in Section 3.4). Such modulated trajectories are then used to learn a ProMP.

A ProMP is a Bayesian parametric model of the demonstrated trajectories in the following form: (1)ξ(t)=Φtω+εξ, where ω ∈ R^M is the time-independent parameter vector weighting the RBFs, εξ∼N(0,β) is the trajectory noise, and Φ_t is a vector of M radial basis functions evaluated at time t: Φt=[ψ1(t),ψ2(t),….,ψM(t)] with (2)ψi(t)=1∑j=1Mψj(t)exp−(t−c(i))22hc(i)=i∕Mh=1∕M2.

Note that all the ψ functions are scattered across time.

For each Ξ_i trajectory, we compute the ω_i parameter vector to have ξi(t)=Φtωi+εξ. This vector is computed to minimize the error between the observed ξ_i(t) trajectory and its model Φtωi+εξ. This is done using the Least Mean Square algorithm, i.e.: (3)ωi=(Φt⊤Φt)−1Φt⊤ξi(t).

To avoid the common issue of the matrix ΦtTΦt in equation (3) not being invertible, we add a diagonal term and perform Ridge Regression: (4)ωi=(Φt⊤Φt+λ)−1Φt⊤ξi(t), where λ=10−11⋅1D⋅M×D⋅M is a parameter that can be tuned by looking at the smallest singular value of the matrix ΦtTΦt.

Thus, we obtain a set of these parameters: {ω 1,…,ω n}, upon which a distribution is computed. Since we assume Normal distributions, we have: (5)p(ω)∼N(μω,Σω) (6)withμω=1n∑i=1nωi (7)andΣω=1n−1∑i=1n(ωi−μω)⊤(ωi−μω).

The ProMP captures the distribution over the observed trajectories. To represent this movement primitive, we usually use the movement that passes by the mean of the distribution. Figure 2 shows the ProMP for a 1-DOF lifting motion, with a number of reference samples s¯ = 100 and number of basis functions M = 5.

This example is included in our Matlab toolbox as demo_plot1DOF.m. The explanation of this Matlab script is presented in Section 5. More complex examples are also included in the scripts demo_plot*.m.

Once the ProMP p(ω)∼N(μω,Σω) of a certain task has been learned,⁸ we can use it to predict the evolution of an initiated movement. An underlying hypothesis is that the observed movement follows to this learned distribution.

Suppose that the robot measures the first n_o observations of the trajectory to predict (e.g., lifting the arm). We call these observations Ξ^o = [ξ^o(1), …, ξ^o(n_o)]. The goal is then to predict the evolution of the trajectory after these n_o observations, i.e., find {ξ^(no+1),…,ξ^(t^f)}, where t^f is the estimation of the trajectory duration (see Section 3.4). This is equivalent to predicting the entire Ξ^ trajectory where the first n_o samples are known and equal to the observations: Ξ^={ξo(1),…,ξo(no),ξ^(no+1),…,ξ^(tt^f)}. Therefore, our prediction problem consists of predicting Ξ^ given the Ξ^o observations.

To do this prediction, we start from the learned prior distribution p(ω), and we find the ω^ parameter within this distribution that generates Ξ^. To find this ω^ parameter, we update the learned distribution p(ω^)∼N(μ^ω,Σ^ω) using the following formulae: (8)μ^ω=μω+KΞo−Φ[1:no] μωΣ^ω=Σω−KΦ[1:no] Σω, where K is a gain computed by the following equation: (9)K=ΣωΦ[1:no]⊤Σξo+Φ[1:no]ΣωΦ[1:no]⊤−1.

Equations (8) and (9) can be computed through the marginal and conditional distributions (Bishop, 2006; Paraschos et al., 2013a), as detailed in Appendix A.

Figure 3 shows the predicted trajectory for the lifting motion of the left arm of iCub. The different graphs show inferred trajectories when the robot observed n_o = 10, 30, 50, and 80% of the total trajectory duration. This example is also available in the toolbox as demo_plot1DOF.m. The nbData variable changes the percentage of known data. Thus, it will be visible how the inference improves according to this variable. An example of predicted trajectories of the arm lifting in Gazebo can be found in a provided video (see Section 8).

In the previous section, we presented the general formulation of ProMPs, which makes the implicit assumption that all the observed trajectories have the same duration and thus the same sampling.⁹ That is why the duration of the trajectories generated by the RBF is fixed and equal to s¯. Of course, this is valid only for synthetic data and not for real data.

To be able to address real experimental conditions, we now consider the variation of the duration of the demonstrated trajectories. To this end, we introduce a time modulation parameter α that maps the actual trajectory duration t_f to s¯: α=s¯∕tf. The normalized duration s¯ can be chosen arbitrarily; for example it can be set to the average of the duration of the trajectories, e.g., s¯=mean(tf1,…,tfK). Notably, in the literature sometimes α is called phase (Paraschos et al., 2013a,b). The effect of α is to change the phase of the RBFs, which are scaled in time.

The time modulation of the i-th trajectory Ξ_i is computed by αi=s¯tfi. Thus, we have α⋅t∈[1:s¯]. Thus, the improved ProMP model is as follows: (10)ξt=Φαtω+εt, where Φ_αt is the RBFs matrix evaluated at time αt. All the M Gaussian functions of the RBFs are spread over the same number of samples s¯. Thus, we have the following: Φαt=[ψ1(αt),ψ2(αt),….,ψM(αt)].

During the learning step, we record a set of α parameters: S_α = {α₁, …, α_n}. Then, using this set, we can replay the learned ProMP with different speeds. By default (e.g., when α = 1), the speed allows to finish the movement in s¯ samples.

During the inference, the time modulation α of the partially observed trajectory is not known. Unless fixed a priori, the robot must estimate it. This estimation is critical to ensure a good recognition, as shown in Figure 4: the inferred trajectory (represented by the mean of the posterior distribution in red) does not have the same duration as the “real” intended trajectory (which is the ground truth). This difference is due to the estimation error of the time modulation parameter. This estimation α^ by default is computed as the mean of all the α_k observed during the learning: (11)α^=∑αknk.

However, using the mean value for the time modulation is an appropriate choice only when the primitive represents goal-directed motions that are very regular, or for which we can reasonably assume that differences in the duration can be neglected (which is not a general case). In many applications, this estimation may be too rough.

Thus, we have to find a way to estimate the duration of the observed trajectory, which corresponds to accurately estimating the time modulation parameter α^. To estimate α^, we implemented four different methods. The first is the mean of all the α_k, as in equation (11). The second is the maximum likelihood, with (12)α^=argmaxα∈Sαkloglikelihood(Ξo,μω k,σω k,αk).

The third is the minimum distance criterion, where we seek the best α^ that minimizes the difference between the observed trajectory Ξto and the predicted trajectory for the first n_o data points: (13)α^=argminα∈Sαk∑t=1noΞto−Φαtμω k.

The fourth method is based on a model: we assume that there is a correlation between α and the variation of the trajectory δno from the beginning until the time n_o. This “variation” δno can be computed as the variation of the position, e.g., δno=X(no)−X(1), or the variation in the entire trajectory, δno=Ξ(no)−Ξ(1), or any other measure of progress, if this hypothesis is appropriate for the type of task trajectories of the application.¹⁰ Indeed, the α can be linked also to the movement speed, which can be roughly approximated by Ẋ=δXtf Ξ˙=δΞtf. We model the mapping between δno and α by the following equation: (14)α=Ψ(δno)⊤ωα+εα, where Ψ are RBFs, and εα is a zero-mean Gaussian noise. During learning, we compute the ω_α parameter, using the same method as in equation (3). During the inference, we compute α^=Ψ(δno)Tωα.

A comparison of the four methods for estimating α on a test study with iCub in simulation is presented in Section 6.6.

There exist other methods in the literature for computing α. For example, Ewerton et al. (2015) propose a method that models local variability in the speed of execution. In Maeda et al. (2016), they use a method that improves Dynamic Time Warping by imposing a smooth function on the time alignment mapping using local optimization. These methods will be implemented in the future works.

Robots should not learn only one skills but many: different skills for different tasks. In our framework, tasks are represented by movement primitives, precisely ProMP. So it is important for the robot to be able to learn K different ProMPs and then be able to recognize from the early observations of a trajectory which of the K ProMPs the observations belong to.

During the learning step of a movement primitive k ∈ [1 : K], the robot observes different trajectories S_k = {Ξ₁, …, Ξ_n}. For each ProMP, it learns the distribution over the parameters vector p(ω)∼N(μωk,Σωk), using equation (3). Moreover, the robot records the different phases of all the observed trajectories: S_αk = {α_1k, …, α_nk}.

After having learned these K ProMPs, the robot can use this information to autonomously execute a task trajectory. Since we are targeting collaborative movements, performed together with a partner at least at the beginning, we want the robot to be able to recognize from the first observations of a collaborative trajectory which is the current task that the partner is doing and what is the intention of the partner. Finally, we want the robot to be able to complete the task on its own, once it has recognized the task and predicted the future trajectory.

Let Ξo=[Ξ1…Ξno]T be the early observations of an initiated trajectory.

From these partial observations, the robot can recognize the “correct” (i.e., most likely) ProMP k^∈[1:K]. First, for each ProMP k ∈ [1 : K], it computes the most likely phase (time modulation factor) α^k (as explained in Section 3.4), to obtain the set of ProMPs with the most likely duration: S[μωk,α^k]={(μω1,α^1),…,(μωK,α^K)}.

Then we compute the most likely ProMP k^ in S[μωk,α^k] according to some criterion. One possible way is to minimize the distance between the early observations and the mean of the ProMP for the first portion of the trajectory: (15)k^=argmink∈[1:K] 1no∑t=1no|Ξt−Φα^kt μω k|.

In equation (15), for each ProMP k ∈ [1 : K], we compute the average distance between the observed early-trajectory Ξ_t and the mean trajectory of the ProMP Φα^ktμωk, with t = [1 : n_o]. The most likely ProMP k^ is selected by computing the minimum distance (arg min). Other possible methods for estimating the most likely ProMPs could be inspired by those presented in the previous section for estimating the time modulation, i.e., maximum likelihood or learned models.

Once identified the k^-th most likely ProMP, we update its posterior distribution to take into account the initial portion of the observed trajectory, using equation (8): (16)μ^ω k^=μω k^+KΞo−Φα^k^[1:no]μω k^Σ^ω k^=Σω k^−KΦα^k^[1:no]Σω k^K=Σω k^ Φα^k^[1:no]⊤Σξo+Φα^k^[1:no]Σω k^Φα^k^[1:no]⊤−1 with α^k^[1:no]=α^k^ t (in matrix form), with t ∈ [1 : n_o].

Finally, the inferred trajectory is given by the following equation: ∀t∈[1:t^f],ξ^(t)=Φt μ^ωk^ with the expected duration of the trajectory t^f=α^ks¯. The robot is now able to finish the movement executing the most likely “future” trajectory Ξ^=[ξ^no+1…ξ^tf]T.

The model is based on Section 3, but here we want to learn more information during movements. We record this information in a multivariate parameter vector: ∀t,ξt=XtFt∈ℛ9, where Xt∈ℛ3 is the Cartesian position of the robot’s end effector and Ft∈ℛ6 the external forces and moments. In particular, F_t contains the user’s contact forces and moments. Let us call dim(ξ_t) = D, the dimension of this parameter vector.

The corresponding ProMP model is as follows: ξt=XtFt=Φαtω+εt, where ω∈ℛD⋅M is the time-independent parameter vector, εt=εXtεFt∈ℛD is the zero-mean Gaussian i.i.d. observation noise, and Φαt∈ℛD×D⋅M a matrix of Radial Basis Functions (RBFs) evaluated at time αt.

Since we are in the multidimensional case, this Φ_αt block diagonal matrix is defined as follows: Φαt=BlockdiagonalMatrix(ϕ1,…,ϕD)∈RD×D⋅M.

It is a diagonal matrix of D Radial Basis Functions (RBFs), where each RBF represents one dimension of the ξ_t vector and it is composed of M Gaussians, spread over same number of samples s¯.

For this application, we created a prototype in Gazebo, where the robot must reach three different targets with the help of a human. To interact physically with the robot simulated in Gazebo, we used the Geomagic touch, a haptic device.

The setup consists of the following:

the iCub simulation in Gazebo, complete with the dynamic information provided by wholeBodyDynamicsTree (https://github.com/robotology/codyco-modules/tree/master/src/modules/wholeBodyDynamicsTree) and the Cartesian information provided by iKinCartesianController;

The interconnection among the different modules is represented in Figure 5, where the Matlab module is not used. The tip of the Geomagic is virtually attached to the end effector of the robot: xgeo→xicub_hand.

When the operator moves the Geomagic, the position of the Geomagic tip x_geo is scaled (1:1 by default) in the iCub workspace as x_{icub_hand}, and the Cartesian controller is used to move the iCub hand around a “home” position, or default starting position: xicub_hand=hapticDriverMapping(x0+xgeo), where hapticDriverMapping is the transformation applied by the haptic device driver, which essentially maps the axis from the Geomagic reference frame to the iCub reference frame. By default, no force feedback is sent back to the operator in this application, as we want to emulate the zero-torque control mode of the real iCub, where the robot is ideally transparent and not opposing any resistance to the human guidance. A default orientation of the hand (“katana” orientation) is set.

The dark button of the Geomagic is used to start and stop the recording of the trajectories. The operator must click and hold the button during the whole movement and release the button at the end. The trajectory is saved on a file called recordX.txt for the X-th trajectory. The structure of this file is: 1

#time #xgeo #ygeo #zgeo #fx #fy #fz #mx #my #mz #x_icub_hand #y_icub_hand #z_icub_hand

A video showing the iCub’s arm moved by a user through the haptic device in Gazebo is available in Section 8 (tutorial video). The graph in Figure 7 represents some trajectories recorded with the Geomagic, corresponding to lifting the left arm of the iCub.

Demonstrated trajectories and their corresponding forces can be recorded directly from the robot, by accessing the Cartesian interface and the wholeBodyDynamicsTree module.¹³

In our project on Github, we provide the acquired dataset with the trajectories for the interested reader who wishes to test the code with these trajectories. Two datasets are available at https://github.com/inria-larsen/icubLearningTrajectories/tree/master/MatlabProgram/Data/: the first dataset called “heights” is composed of three goal-directed reaching tasks, where the targets vary in height; the second dataset called “FLT” is composed of trajectories recorded on the real robot, whose arms move forward, to the left and to the top.

A Matlab script that learns ProMPs with such kinds of datasets is available in the toolbox, called demo_plotProMPs.m. It contains all the following steps.

To load the first “heights” dataset with the three trajectories, write: 1

t{1} = loadTrajectory(’Data/heights/bottom’, ’bottom’, ’refNb’, s_bar, ’nbInput’, nbInput, … ’Specific’, ’FromGeom’);

Figure 7 shows the three sets of demonstrated trajectories. In the used dataset called “heights,” we have recorded 40 trajectories per movement primitive.

We need to first learn the ProMPs associated with the three observed movements. First, we partition the collected dataset into a training set and test dataset for the inference. One random trajectory for the inference is used: 1

[train{i}, test{i}] = partitionTrajectory(t{i}, 1, percentData, s_bar);

The second input parameter specifies that we select only one trajectory, randomly selected, to test the ProMP.

Now, we compute the three ProMPs with: 1

promp{1} = computeDistribution(train{1}, M, s_bar, c, h);

We set the following parameters:

s_bar = 100: reference number of samples, which we note in this article as s¯.

These parameters can be changed at the beginning of the Matlab script.

Figure 8 shows the three ProMPs of the reaching movements toward the three targets. To highlight the most useful dimension, we only plot the z-axis Cartesian position. However, each dimension is plotted by the Matlab script with: 1

drawRecoverData(t{1}, inputName, ’Specolor’, ’b’, ’namFig’, 1);

Now that we have learned the different ProMPs, we can predict the end of a trajectory according to the early observation n_o. This number is computed from the variable percentData written at the beginning of the trajectory by: no=|percentData100∗tfi|, where i is the index of the test trajectory.

To prepare the prediction, the model the time modulation of each trajectory is computed with: 1

w = computeAlpha(test.nbData, t, nbInput);

This model relies on the global variation of Cartesian position during the first n_o observations. The model’s computations are explained in Section 3.4.

Now, to estimate the time modulation of the trajectory, call the function: 1

[alphaTraj, type, x] = inferenceAlpha(promp, test{1}, M, s_bar, c, h, test{1}.nbData, expNoise, ’MO’);

where alphaTraj contains the estimated time modulation α^ and type gives the index of the recognized ProMP. The last parameter x is used for debugging purposes.

Using this estimation of the time modulation, the end of the trajectory is inferred with: 1

infTraj = inference(promp, test{1}, M, s_bar, c, h, test{1}.nbData, expNoise, alphaTraj);

As shown in the previous example, the quality of the prediction of the future trajectory depends on the accuracy of the time modulation estimation. This estimation is computed by calling the function: 1

%Using the model:

In Section 3.4, we presented four main methods for estimating the time modulation parameter, discussing why this is crucial for a better estimation of the trajectory. Here, we compare the methods on the three goals experiment. We recorded 40 trajectories for each movement primitive, for a total of 120 trajectories. After having computed the corresponding ProMPs, we tested the inference by providing early observations of a trajectory that the robot must finish. For that purpose, it recognizes the correct ProMP among the three precedently learned (see Section 3.5) and then it estimates the time modulation parameter α^. Figure 9 represents the average error of the α^ during inference for 10 trials according to the number of observations (from 30 to 90% of observed data) and according to the used method. These methods are the ones we have just presented before that we called mean (equation (11)), maximum likelihood (equation (12)), minimum distance (equation (13)) or model (equation (14)). Each time, the tested trajectory is chosen randomly from the data set of observed trajectories (of course, the test trajectory does not belong to the training set, so it was not used in the learning step). The method that takes the average of α observed during learning is taken as comparison (in black). We can see that other methods are more accurate. The maximum likelihood is increasingly more accurate, as expected. The fourth method (model) that models the α according to the global variation of the trajectory’s positions during the early observations is the best performing when the portion of observed trajectory is small (e.g., 30–50%). Since it is our interest to predict the future trajectory as early as possible, we adopted the model method for our experiments.

Task trajectories, in this example, have both position and wrench information. In general, it is a good idea to represent collaborative motion primitives in terms of both position and wrenches, as this representation enables using them in the context of physical interaction. Contrarily to the simulated experiment, here the inferred wrenches F^t correspond to the wrenches the robot should perceive if the partner was manually guiding the robot to perform the entire movement: indeed, these wrenches are computed from the demonstrations used to learn the primitive. The predicted wrenches can be used in different ways, depending on the application. For example, if the partner breaks the contact with the robot, the perceived wrenches will be different. If the robot is not equipped with tactile or contact sensors, this information can be used by the robot to “perceive” the contact breaking and interpret it, for example, as the sign that the human wants the robot to continue the task on its own. Another use for the demonstrated wrenches is for detecting abnormal forces while the robot is moving: this use can have different applications, from adapting the motion to new environment to automatically detecting new demonstrations. Here, they are simply used to detect when the partner breaks the contact with the robot, and the latter must continue the movement on its own.

In the following, we present how to realize the experiment for predicting the user intention with the real iCub, using our software. The robot must learn three task trajectories represented in Figure 10. In red, the first trajectory goes from an initial position in front of the robot to its left (task A). In green, the second trajectory goes from the same initial position to the top (task C). In blue, the last trajectory goes from the top position to the position on the left (task B).

To provide the demonstrations for the tasks, the human tutor used three visual targets shown on the iCub_GUI, a basic module of the iCub code that provides a real-time synthetic and augmented view of the robot status, with arrows for the external forces and colored objects for the targets. One difficulty for novice users of iCub is to be able to drive the robot’s arm making it perform desired complex 3D trajectories (Ivaldi et al., 2017), but after some practice in moving the robot’s arm the operator recorded all the demonstrations. We want to highlight that having variations in the starting or ending points of the trajectories is not at all a problem, since the ProMPs are able to deal with this variability.

We will see that by using the ProMPs method and by learning the end-effector Cartesian position, the robot will be able to learn distributions over trajectories, recognize when a movement belongs to one of these distributions, and infer the end of the movement.

In this experiment, the robot received 10 demonstrated trajectories per movement primitive, all provided by the same user. We recorded the Cartesian end-effector position and the wrenches of the robot’s left arm. Data are retrieved using the function used_functions/retrieveRealDataWithout Orientation.m. The output parameters of this function are three objects (one per ProMP) that contain all the required information to learn the ProMPs.

In this function, the wrench information are filtered using a Matlab function called envelope.m¹⁵: for each trajectory trajand its subMatrix M = F([1: t]): 1

[envHigh, envLow] = envelope(traj.M);

These three objects are saved in ’Data/realIcub.mat’. A Matlab script called demo_plotProMPsIcub.m recovers these data, using the function load(’Data/realIcub.mat’). This script follows the same organization as the ones we previously explained in Sections 5 and 6. By launching this script, the recovered data are plotted first.

Then, the ProMPs are computed and plotted, as presented in Figure 11. In this figure, the distributions are visibly overlaid:

during the whole trajectories duration for the wrench information;

After this learning step, the user chooses which ProMP to test. Using a variable that represents the percentage of observed data to be used for the inference, the script computes the number of early observations n_o¹⁶ that will be measured by the robot. Using this number, the robot models the time modulation parameter α¹⁷ of each ProMP, as explained in Section 3.4. Using this model, the time modulation of the test trajectory is estimated, and the corresponding ProMP is identified.

Then, the inference of the trajectory’s target is performed. Figure 12 represents the inference of the three tested trajectories when wrench information is not used by the robot to infer the trajectory. To realize this figure, with the comparison between the predicted trajectory and the ground truth, we applied our algorithm offline. In fact, it is not possible at time t to have the ground truth of the trajectory intended by the human from t + 1 to t_f: even if we would tell to the human in advance the goal that he/she must reach for, the trajectory to reach that goal could vary. So, for the purpose of these figures and comparisons with the ground truth, we show here the offline evaluation: we select one demonstrated task trajectory from the test set (not the training set used to learn the ProMP) as ground truth, and imagine that this is the intended trajectory. In Figure 12, the ground truth is shown in black, whereas the portion of this trajectory that is fed to the inference, and that corresponds to the “early observations,” is represented with bigger black circles. We can see that the inference of the Cartesian position is correct, although we can see an error of about 1 s of the estimated duration time for the last trial. Also, the wrench inference is not accurate. We can assume that it is: because the robot infers the trajectory using only position information without wrench information, or because the wrenches’ variation is not correlated to the position variation. To improve this result, we can make the inference using wrench in addition to Cartesian position information, as shown in Figure 13. We can see in this figure that the estimation of the trajectory’s duration is accurate. The disadvantage is that the inference of the Cartesian position is less accurate because the posterior distribution computation makes a trade-off between fitting Cartesian position and wrench early observations. Moreover, to allow a correct inference using wrench information, the noise expectation must be increased to consider forces.¹⁸

To confirm these results, we analyzed the trajectory inference and α estimation considering different percentages of each trajectory as observed data (30–90%). For each percentage, we performed 20 tests, with and without force information.

In Figure 14, each box-plot represents errors for 20 tests. On the top, the error criterion is the average distance between the inferred trajectory and the real one. We can see that the inference of Cartesian end-effector trajectory is more accurate without wrench information. On the bottom, the error criterion is the distance between the estimated α and the real one. We can see that using wrench information, the estimation of the α is more accurate. Thus, these two graphs confirm what we assumed from Figures 12 and 13.

Median, mean, and variance of the prediction errors, computed with the normalized root-mean-square error (NRMSE), are reported in Table S1 in Supplementary Material. The prediction error for the time modulation is a scalar: |α_prediction − α_real|. The prediction error for the trajectory is computed by the NRMSE of |Ξ_prediction − Ξ_real|.

In future upgrades for this application, we will probably use the wrench information only to estimate the time modulation parameter α, to have both the best inference of the intended trajectory and the best estimation of the time modulation parameter to combine the benefits of inference with and without wrench information.

Table S1 in Supplementary Material also reports the average time for computing the prediction of both time modulation and posterior distribution. The computation was performed in Matlab, on a single core laptop (no parallelization). While the computation time for the case “without wrenches” is fine for real-time application, using the wrench information delays the prediction and represents a limit for real-time applications if fast decisions have to taken by the robot. Computation time will be improved in the future works, with the implementation of the prediction in an iterative way.

We realized another experiment with iCub, where the robot has to sort some objects in different bins (see Figure S2 in Supplementary Material). We have two main primitives: one for a bin located on the left of the robot, and one for the bin to the front. Dropping the object is done at different heights, with a different gesture that also has a different orientation of the hand. For this reason, the ProMP model consists of the Cartesian position of the hand X_t = [x_t, y_t, z_t] ∈ R³ and its orientation A_t ∈ R⁴, expressed as a quaternion: ξt=XtAt=Φαt ω+εt.

As in the previous experiment, we first teach the robot the primitives by kinesthetic teaching, with a dozen of demonstrations. Then we start the robot movement: the human operator physically grabs the robot’s arm and start the movement toward one of the bins. The robot’s skin is used twice. First, to detect the contact when the human grabs the arm, which marks the beginning of the observations. Second, when the human breaks the contact with the arm, which marks the end of the observations. Using the first portion of the observed movement, the robot recognizes the current task that is being executed, predicts the future movement that is intended by the human, and then executes it on its own. In the video (see link in Section 8), we artificially introduced a pause to let the operator “validate” the predicted trajectory, using a visual feedback on the iCubGui. Figure S3 in Supplementary Material shows one of the predictions made by the robot after the human releases the arm. Of course in this case, we do not have a “ground truth” for the predicted trajectory, only a validation of the predicted trajectory by the operator.

Our experiments showed that estimating the time modulation parameter α, determining the duration of the trajectory, greatly improves the prediction of the trajectory in terms of difference with the human intended trajectory (i.e., our ground truth). We proposed four simple methods in Section 3.4, and in the iCub experiment, we showed that the method that maps the time modulation and the variation of the trajectory in the first n_o observations provides a good estimate of the time modulation α for our specific application. However, it is an ad hoc model that cannot be generalized to all possible cases. Overall, the estimation of the time modulation (or phase) can be improved. For example, Maeda et al. (2016) used Dynamic Time Warping, while Ewerton et al. (2015) proposed to improve the estimation by having local estimations of the speed in the execution of the trajectory, to comply with cases where the velocity of task trajectory may not be constant throughout the task execution. In the future, we plan to explore more solutions and integrate them into our software.

Another point that needs further investigation and improvement is how to improve the prediction of the trajectories exploiting different information. In our experiment with iCub, we improved the estimation of the time modulation using position and wrench information; however, we observed that the noisy wrench information does not help in improving the prediction of the position trajectory. One improvement is to certainly exploit more information from the demonstrated trajectories, such as estimating the different noise of every trajectory component and exploiting this information to improve the prediction. Another possible improvement would consist in using contextual information about the task trajectories. Finally, it would be interesting to try to identify automatically the characteristic such as velocity profiles or accelerations, which are renown to play a key role in attributing intentions to human movements. For example, in goal-directed tasks such as reaching, the arm velocity profile, and the hand configuration are cues that helps us detect intentions. Extracting these cues automatically, leveraging the estimation of the time modulation, would probably improve the prediction of the future trajectory. This is a research topic on its own, outside the scope of this article, with strong links to human motor control.

In Section 3.5, we described how to compute the prediction of the future trajectory after recognizing the current task. However, we did not explore what happens if the task recognition is wrong: this may happen, if there are two or more task with a similar trajectory at the beginning (e.g., moving the object from the same initial point toward one of four possible targets), or simply because there were not enough observed points. So what happens if our task recognition is wrong? How to re-decide on a previously identified task? And how should the robot decide if its current prediction is finally correct (in statistical terms)? While implementing a continuous recognition and prediction is easy with our framework (one has simply to do the estimation at each time step), providing a generic answer to these question may not be straightforward. Re-deciding about the current task implies also changing the prediction of the future trajectory. If the decision does not come with a confidence level greater than a desired value, then the robot could face a stall: if asked to continue the movement but unsure about the future trajectory, should it continue or stop? The choice may be application dependent. We will address these issues and the continuous prediction in future works.

Finally, we plan to improve the computational time for the inference and the portability of our software by porting the entire framework in C++.

In many collaborative scenarios, such as object carrying and cooperative assembly, the physical interaction between the human and the robot is mediated by objects. In these cases, if specific manipulations must be done on the objects, our method still applies, but not only on the robot. It must be adapted to the new “augmented system” consisting of robot and object. Typically, we could image a trajectory for some frame or variable or point of interest for the object and learn the corresponding task. Since ProMPs support multiplication and sequencing of primitives, we could exploit the properties of the ProMPs to learn the joint distribution of the robot task trajectories and the object task trajectories.