\section{Advantages and Limitations of the approach}
\label{sec:discussion}

\subsection{Learning configuration space projections}
Our results showed that with task parameters composed of Jacobian-based operators, the robot can take into account configuration space constraints in addition to operational space ones. Learning controllers for complex bimanual manipulation goes beyond operational space, and one must also care about the configuration space to achieve natural and efficient movements. The shaking experiment is an example of task that is best modeled by taking into account the configuration space constraints, but the range of possibly interesting situations is vast (e.g., communicative gestures, self-collision avoidance).
Although highly relevant, this problem is rarely covered in the literature of manipulation learning, especially in the bimanual case.

One possible shortcoming of our approach is that it  relies on the variability of the demonstrated movements to extract the importance of each space. In some cases, such task variations may not be straightforward to demonstrate (e.g., unexperienced demonstrators may not demonstrate sufficient variability during irrelevant phases of the movement). One potential way to cope with this issue could be to combine TP-GMM with more interactive and incremental teaching approaches, which would allow the robot to  refine its model of the task at runtime by testing variations of the task in the different spaces based on exploration, interaction and feedback from the teacher.


\subsection{Learning priority constraints}
By extending the notion of candidate coordinate systems to candidate task hierarchies, we showed that TP-GMM could be used to learn priority constraints. With movement synthesis through a soft weighting of strict hierarchies, we maintain the advantages of composing primitives in the form of strict prioritization, since high weights will result in the execution of the tasks with the corresponding hierarchy. The soft weighting component allows the robot to combine hierarchies (in the form of task prioritization primitives) and to switch between hierarchies with smooth transitions.

Here, one potential shortcoming is that the set of possible task hierarchies needs to be set beforehand. To a certain extent, one can take advantage of the teacher's knowledge about the domain of activity considered, in order to only use a subset of all possible candidate task hierarchies, i.e., some hierarchies may be known to be irrelevant beforehand. However, even though  one is not required to provide all possible hierarchies for the considered tasks, one should be careful not to over-define this set, because the more candidate hierarchies are available, the more demonstrations will be required to extract invariant task features from statistical analysis. A potential research direction to leverage this issue would be to study ways of learning sets of task parameters, in this case hierarchies, characterizing specific domains of activity (bimanual manipulation, walking, etc.). Then, one could take advantage of the prior knowledge about the activity in which the task to be taught takes place to select the set accordingly.
\section{Conclusions and Future Work}
\label{sec:conclusions}

We presented a framework for human-robot bimanual skill transfer based on Task-Parameterized Gaussian Mixture Models. We introduced different parameterizations allowing us to consider a wide range of learning problems related to manipulation in humanoids, namely the combination of constraints between operational and configuration spaces, and the learning of task priorities. The approach was validated in two different scenarios. We first demonstrated, in a bimanual shaking task with the COMAN robot, that the approach can be used to consider constraints in operational and configuration spaces simultaneously, which permits the reproduction of movement patterns from both spaces during movement synthesis. Then, in a bimanual reaching experiment with COMAN, we showed that by providing different candidate hierarchies to describe an observed movement, the robot is able to determine from statistics which prioritization (or combination) is relevant for the task. Finally, we showed that the definition of primitives as task prioritizations could be employed to transfer skills between robots with different embodiments.

Future work will investigate how the proposed approach can be combined with optimization-based techniques like CMA-ES \cite{Dehio15,Modugno16} to refine the learned behaviors based on cost functions that encompass parameters that are harder to relate to operational space control, such as energy efficiency. Another promising route, which would allow for considering new types of constraints, is to formulate the learning problem using torque controllers, as opposed to velocity controllers as we considered in this article. A growing number of robots can be controlled in torque, thus it is relevant for learning approaches to account for this fact.

A final research direction concerns the application of task prioritization with TP-GMM to more complex whole-body control scenarios. One possible avenue could be that of learning whole body motion behaviors from human demonstrations, including prioritization skills involving center of mass and contact points with the environment (e.g., to learn natural ways of coping with perturbations while standing).

\section{Learning task prioritization from demonstrations}
\label{sec:priorityConstraints}

Controlling robots often requires the definition of priorities between tasks. For example, when a humanoid is standing and has to manipulate an object, the highest priority should be on keeping balance and, therefore, all degrees of freedom should be assigned to that task and only manipulate when balance is not compromised.

Commonly, the way in which tasks are prioritized is defined beforehand by an expert \cite{Moro13,Sentis05}. In contrast, here we propose to learn these priorities from demonstrations. We frame the problem in the context of TP-GMM by formulating the task parameters as candidate task hierarchies --- as opposed to candidate coordinate systems in the original formulation --- and subsequently learning the weights of such hierarchies from  demonstrations. For movement synthesis, we then propose to combine these hierarchies using a controller that takes into account the learned weights.

\begin{figure}
	\begin{subfigure}{\columnwidth}
		\centering
		\includegraphics[width=\columnwidth]{figures/PlanarOrientation.png}
		\caption{Four demonstrations of priority on \textbf{orientation}. The robot only fulfills the two tasks when they are both achievable (demos 2 and 3). When they are incompatible, orientation is prioritized (demos 1 and 4).}
		\vspace{0.5cm}
		\label{fig:planarPrioritiesOrient}
	\end{subfigure}
	\begin{subfigure}{\columnwidth}
		\centering
		\includegraphics[width=\columnwidth]{figures/demoIK_hierarchyExtraction02e_compact}
		\caption{Four demonstrations of priority on \textbf{position}. Once the two tasks become incompatible the robot prioritizes position tracking (demos 1 and 4).}
		\vspace{0.5cm}
		\label{fig:planarPrioritiesPos}
	\end{subfigure}
	\caption{Planar robot tracking an orientation (arrow) and a vertical position along a line (point), with different priorities.}
	\label{fig:planarPriorities}
\end{figure}

\subsection{Extracting priorities from demonstrations}
\label{subsec:extractingHierarchies}

We consider prioritization primitives generated from \textit{strict hierarchies}, where tasks of lower importance are performed in the null space of more important ones, i.e. they are only executed if they do no conflict. In this subsection we show how to extract the activation of the prioritization primitives employed during demonstrations.

In order to ease the explanation, let us consider an example of a 3-DOF planar robot with position and orientation tracking tasks (Figure \ref{fig:planarPriorities}). We denote the operational space velocities that ensure the tracking of position and orientation references by $\mb{\dot{x}}_1 = \mb{K}_p\mb{e}_p$ and $\mb{\dot{x}}_2 = \mb{K}_o\mb{e}_o$, respectively, where $ \mb{K}_p, \mb{K}_o $ are positive gains and $ \mb{e}_p = \mb{\hat{x}}_p - \mb{x}_p, \mb{e}_o = \mb{\hat{x}}_o - \mb{x}_o$ are  the task errors. These two tasks can be prioritized according to
\begin{equation}
\mb{\hat{\dot{\mb{q}}}}^{(1)}
= \mb{J}_{\!1}^{\dagger} \; \mb{\dot{x}}_1 + \mb{N}_{\!1} \mb{J}_{\!2}^{\dagger} \; \mb{\dot{x}}_2 \\
= \underbrace{\left[\mb{J}_{\!1}^{\dagger} \quad \mb{N}_{\!1}\mb{J}_{\!2}^{\dagger} \right]}_{\mb{A}^{(1)}} 
\left[\begin{matrix} \mb{\dot{x}}_1 \\ \mb{\dot{x}}_2 \end{matrix}\right]
\label{eq:strictPriorityx1}
\end{equation}
or, alternatively,
\begin{equation}
\mb{\hat{\dot{\mb{q}}}}^{(2)} 
= \mb{J}_{\!2}^{\dagger} \; \mb{\dot{x}}_2 + \mb{N}_{\!2} \mb{J}_{\!1}^{\dagger} \; \mb{\dot{x}}_1 \\
= \underbrace{\left[\mb{N}_{\!2}\mb{J}_{\!1}^{\dagger} \quad \mb{J}_{\!2}^{\dagger}\right]}_{\mb{A}^{(2)}} 
\left[\begin{matrix} \mb{\dot{x}}_1 \\ \mb{\dot{x}}_2 \end{matrix}\right]\!\!,
\label{eq:strictPriorityx2}
\end{equation}
where $ \mb{J}_{\!1}, \mb{J}_{\!2}  $ and $ \mb{N}_{\!1}, \mb{N}_{\!2} $ are the Jacobians of each task and corresponding null space projection matrices. Figure \ref{fig:planarPriorities} shows the two different prioritization strategies: priority on orientation (Figure \ref{fig:planarPrioritiesOrient}) and priority on position (Figure \ref{fig:planarPrioritiesPos}). Each demonstration is a snapshot of the robot, after satisfying the task space constraints as best as possible. In both scenarios, when the two references can be achieved simultaneously (orange and yellow references), the robot successfully fulfills the two tasks. When the two references are incompatible, the robot prioritizes one over the other, according to the employed hierarchy (blue and purple references).

We propose to interpret \eqref{eq:strictPriorityx1}, \eqref{eq:strictPriorityx2} as two candidate hierarchies, $ \{\mb{A}^{(1)}, \mb{A}^{(2)} \}$, that prioritize the tasks differently. We wish to learn which one was demonstrated, given observations of the desired operational space velocities $ \mb{\dot{x}}_1, \mb{\dot{x}}_2 $. These are equivalent to the task errors, up to a constant gain. One way to do this is through the analysis of the task space motion $ \mb{\hat{\dot{x}}}^{(j)} = \mb{J} \mb{\hat{\dot{q}}}^{(j)}$, that would result from the application of each candidate hierarchy $ j\in\{1,2\} $, i.e.,
\begin{align}
\left[\begin{matrix} \mb{\hat{\dot{x}}}^{(1)} _1 \\ \mb{\hat{\dot{x}}}^{(1)} _2 \end{matrix}\right]
& = \left[\begin{matrix} \mb{J}_{\!1} \\ \mb{J}_{\!2} \end{matrix}\right] \mb{\hat{\dot{\mb{q}}}}^{(1)} = \left[\begin{matrix} \mb{J}_{\!1} \\ \mb{J}_{\!2} \end{matrix}\right]
\left[\mb{J}_{\!1}^{\dagger} \quad \mb{N}_{\!1}\mb{J}_{\!2}^{\dagger} \right] 
\left[\begin{matrix} \mb{\dot{x}}_1 \\ \mb{\dot{x}}_2 \end{matrix}\right],
\label{eq:Task1HierarchyOp}
\end{align}
for hierarchy $ \mb{A}^{(1)} $, and
\begin{align}
\left[\begin{matrix} \mb{\hat{\dot{x}}}^{(2)}_1 \\ \mb{\hat{\dot{x}}}^{(2)}_2 \end{matrix}\right] 
& = \left[\begin{matrix} \mb{J}_{\!1} \\ \mb{J}_{\!2} \end{matrix}\right] \mb{\hat{\dot{\mb{q}}}}^{(2)} = \left[\begin{matrix} \mb{J}_1 \\ \mb{J}_2 \end{matrix}\right] 
\left[\mb{N}_2\mb{J}_1^{\dagger} \quad \mb{J}_2^{\dagger} \right]
\left[\begin{matrix} \mb{\dot{x}}_1 \\ \mb{\dot{x}}_2 \end{matrix}\right],
\label{eq:Task2HierarchyOp}
\end{align}
for $ \mb{A}^{(2)} $. Here, $ \mb{\dot{x}}_1, \mb{\dot{x}}_2 $ are computed from the demonstrations, given the references and the end-effector position/orientation at any given $ t $, and they have the same value in both \eqref{eq:Task1HierarchyOp} and \eqref{eq:Task2HierarchyOp}. Figure \ref{fig:hierarchyWeights} shows the result of applying \eqref{eq:Task1HierarchyOp}, \eqref{eq:Task2HierarchyOp} to the examples of Figure \ref{fig:planarPriorities} (with $ \mb{\dot{x}}_1,\mb{\dot{x}}_2 $ computed from each task's error in the depicted configuration). Notice how the datapoints exhibit low variability for the hierarchy that was demonstrated. We therefore propose to exploit this variability to assign importance to each candidate hierarchy by modeling the distribution of the data with a TP-GMM, where $ \mb{\hat{\dot{x}}}^{(j)} \sim \sum_{i=1}^{K}\pi_i\mathcal{N}\left(\mb{\mu}^{(j)}_i,\mb{\Sigma}^{(j)}_i\right) $, for each candidate hierarchy $ \mb{A}^{(j)} $. The covariance matrices $ \mb{\Sigma}^{(j)}_i $ therefore encode the demonstrated variability, from which weights can be computed as full precision matrices $ \mb{\Sigma}^{(j)^{-1}}_i $ associated with each hierarchy.
\begin{figure}
	\begin{subfigure}{0.47\columnwidth}
		\centering
		\includegraphics[width=1.0\columnwidth]{figures/toyExample_priorityOrient.pdf}
		\caption{\centering Demonstrated \textbf{priority on orientation} (see Figure \ref{fig:planarPrioritiesOrient}).}
		\label{fig:hierarchyWeightsOrient}
	\end{subfigure}%
	\hspace{4mm}
	\begin{subfigure}{0.47\columnwidth}
		\centering
		\vspace{0.4mm}
		\includegraphics[width=0.835\columnwidth]{figures/toyExample_priorityPos.pdf}
		\caption{\centering Demonstrated \textbf{priority on position} (see Figure \ref{fig:planarPrioritiesPos}).}
		\label{fig:hierarchyWeightsPos}
	\end{subfigure}
	\vspace{0.3cm}
	\caption{Operational space velocities generated by each candidate hierarchy for the configurations of the robot in Figure \ref{fig:planarPriorities}. Each colored point corresponds to a demonstration with the same color in Figure \ref{fig:planarPriorities}. The `$ \times $' represents the mean of the datapoints, while the bars depict one standard deviation.}
	\label{fig:hierarchyWeights}
\end{figure}

We can easily generalize this concept to $N_T$ arbitrary tasks and $P$ candidate hierarchies, indexed by $j=1,\hdots,P$ and represented by $\mb{A}^{(j)}$,
\begin{align}
\left[\begin{matrix} \mb{\hat{\dot{x}}}^{(j)} _1 \\ \vdots \\ \mb{\hat{\dot{x}}}^{(j)} _{N_T} \end{matrix}\right]
&= \left[\begin{matrix} \mb{J}_{\!1} \\ \vdots \\ \mb{J}_{\!N_T} \end{matrix}\right]
\mb{A}^{(j)} 
\left[\begin{matrix} \mb{\dot{x}}_1 \\ \vdots \\\mb{\dot{x}}_{N_T} \end{matrix}\right],
\label{eq:GeneralizedProjection}
\end{align}
allowing us to re-write \eqref{eq:TPprojection} as $ \mb{X}^{(j)}_t = \mb{J}_t \mb{A}^{(j)}_t \mb{\xi}_t $, where  $\mb{J}_t=\left[\mb{J}^\trsp_{t,1} \> \hdots \> \mb{J}^\trsp_{t,N_T} \right]^\trsp$ and $\mb{\xi}_t = \left[\mb{\dot{x}}^\trsp_{t,1} \> \hdots \> \mb{\dot{x}}^\trsp_{t,N_T} \right]^\trsp$. A TP-GMM can then be estimated using the same EM algorithm as described in Section \ref{sec:LearningTPMovements}. We assume here that the kinematic model of the robot is known, i.e., that the Jacobian of each task is available, and  that the tasks to be performed are also known.

\subsection{Movement synthesis: soft weighting of strict hierarchies}
\label{subsec:softWeighting}

The most common approaches for controlling tasks of different priorities are either based on strict hierarchies \cite{Wrede13,Saveriano15,Towell10,Hak12b}, or soft weighting of tasks \cite{Moro13,Dehio15,Modugno16,Lober15}. We propose a richer alternative based on a \textit{soft weighting of strict hierarchies}, gathering the best of the two approaches. For this, we exploit the learned hierarchy weights to propose a controller that corresponds to a weighted combination of candidate prioritization primitives, which is formulated as the optimization problem
\begin{equation}
\mb{\hat{\dot{q}}} = \arg\underset{\mb{\dot{q}}}{\min} \sum_{j=1}^{P}\left(\mb{\dot{q}} - \mb{\dot{q}}^{(j)}\right)^{\!\!\trsp} \!\mb{\Gamma}^{(j)}\! \left(\mb{\dot{q}} - \mb{\dot{q}}^{(j)}\right). 
\label{eq:prioritizationOptimization}
\end{equation}
We frame this problem as a modified version of TP-GMM with task parameters $ \mb{A}^{(j)} $, representing candidate hierarchies. First, we model the joint probability distribution of $ \mb{\hat{\dot{x}}}^{(j)} $ and an input $ t $  in a GMM. Through GMR we estimate, for new inputs, the distribution $ \mb{\hat{\dot{x}}}^{(j)}|t \sim \mathcal{N}\Big(\mb{\mu}^{(j)}, \mb{\Sigma}^{(j)}\Big) $, which we combine with task parameters $ \mb{A}^{(j)} $ to compute new precision matrices for each hierarchy $ j $ as
\begin{equation}
	\mb{\Gamma}^{(j)} = \Big(\!\mb{A}^{(j)^\trsp} \mb{\Sigma}^{(j)} \mb{A}^{(j)}\!\Big)^{\!-1}.
	\label{eq:hierarchyWeight}
\end{equation}
$ \mb{\Gamma}^{(j)} $ are thus matrices which reflect the importance of each hierarchy for a given input. Second, we compute the candidate solution for hierarchy $ j $ as
\begin{equation}
	\mb{\dot{q}}^{(j)} = \mb{A}^{(j)}\mb{\dot{x}}^{(j)},
	\label{eq:candidateQ}
\end{equation}
where $ \mb{\dot{x}}^{(j)}$ are the desired operational space velocities for the tasks in hierarchy $ j $.
Finally the solution of \eqref{eq:prioritizationOptimization} corresponds to a product of $ P $ Gaussians with mean $ \mb{\dot{q}}^{(j)} $ and precision matrices $ \mb{\Gamma}^{(j)} \!\!$,
\begin{equation}
\mb{\hat{\dot{q}}} = \Big( \sum\limits_{j=1}^P \mb{\Gamma}^{(j)} \Big)^{\!\!-1} \sum\limits_{j=1}^P \mb{\Gamma}^{(j)} \!\mb{\dot{q}}^{(j)},
\label{eq:controlQ}
\end{equation} 
as per \eqref{eq:GaussProduct}. The solution $ \mb{\hat{\dot{q}}} $ is a reference joint velocity, which can be used to control the robot through a velocity or a position controller (in which case it should be integrated numerically).

The complete approach is summarized in Algorithm \ref{alg:learningPriorities}.

\begin{algorithm}[bt]
	\caption{Learning and synthesizing priorities from demonstrations.}
	\begin{algorithmic}[1]
		\Statex \textit{\textbf{Initialization}}
		\State Select relevant candidate hierarchies $ \mb{A}^{(j)} $ based on the task at hand
		\State Collect demonstrations
	\end{algorithmic}
	\begin{algorithmic}[1]
		\Statex \textit{\textbf{Model training}}
		\State Compute $ \mb{X}^{(j)}_t $ for each candidate hierarchy $ \mb{A}^{(j)} $ per \eqref{eq:GeneralizedProjection}
		\State Estimate model $\big\{\pi_i,\{\mb{\mu}^{(j)}_i,\mb{\Sigma}^{(j)}_i\}_{j=1}^P\big\}_{i=1}^K$ using EM
	\end{algorithmic}	
	\begin{algorithmic}[1]
		\Statex \textit{\textbf{Movement synthesis}}
		\For{$t=1,\dots,N$}
		\For{$j=1,\dots,P$}
		\State Update task parameters $ \mb{A}^{(j)} $ with Jacobians and
		\Statex \hspace{0.95cm} null space matrices at $ t $
		\State Compute $ \mb{\Gamma}^{(j)} $ using GMR and  \eqref{eq:hierarchyWeight}
		\State Update desired task space velocities  $ \mb{\dot{x}}^{(j)} $ for the 
		\Statex \hspace{0.95cm} tasks in hierarchy $ j $
		\State Compute candidate solution $ \mb{\dot{q}}^{(j)}$ using \eqref{eq:candidateQ}
		\EndFor
		\State Compute velocity control command $ \mb{\hat{\dot{q}}} $ from  \eqref{eq:controlQ}
		\EndFor
	\end{algorithmic}
	\label{alg:learningPriorities}
\end{algorithm}


\subsection{Experiment: soft weighting of hierarchies - behavior during transitions}
An important aspect to consider when controlling priorities is the behavior of the robot when priorities change. Before testing the learning capabilities of our approach, we begin with a study about the effect of varying the weights of each candidate hierarchy, when employing the controller proposed in Section \ref{subsec:softWeighting}.

For simplicity of analysis, we consider the case of a planar robot with two tasks. The tasks are to track a vertical position reference with the end-effector and to point downwards. The references are chosen such that the two tasks can be fulfilled individually but not simultaneously (Figure \ref{fig:VariableWeights}, \emph{top-left}). Two strict hierarchies are considered, corresponding to an accurate tracking of either position or orientation as first priority, and orientation or position as secondary. We define two scalar weights $w_{\text{pos}}$ and $w_{\text{orient}}$, which represent the importance of each hierarchy, with the subscript denoting the highest priority task. With these weights, we manually define precision matrices $\mb{\Gamma}_{\text{pos}} \!=\! w_{\text{pos}}\mb{I}$ and $\mb{\Gamma}_{\text{orient}} \!=\! w_{\text{orient}}\mb{I}$, where $\mb{I}$ is a 2-dimensional identity matrix. Figure \ref{fig:VariableWeights} shows the results for the case in which the robot transits from priority on orientation to priority on position, through a continuous variation of the weights. We observe that, as different combinations of weights are applied by the controller, a smooth transition occurs between the two tasks. These results show that the proposed controller can handle well situations where priorities change, which can occur when demonstrated hierarchies vary over time or according to different contexts.

\begin{figure}
	\centering	
	\includegraphics[width=\columnwidth]{figures/variableWeightsAllinOne}
	\caption{End-effector behavior for varying hierarchy weights. \textbf{Top-left:} Planar robot fulfilling the two tasks separately. \textbf{Top-right:} End-effector state in the position-orientation space for the considered weights (blue line). \textbf{Bottom:} End-effector orientation and vertical position as weights vary. Notice the inverted scales on the horizontal axes - the experiment starts with $(1,0)$ and ends with $(0,1)$. Red lines and point indicate the references.}
	\label{fig:VariableWeights}
\end{figure}

Another case that is worth investigating is the behavior of the system when both tasks are achievable. In this situation one would expect that any combination of the weights would result in a successful completion of both tasks. In order to test how our approach handles this scenario, we decreased the vertical position reference so that both tasks can be fulfilled simultaneously.
We set the robot to an initial configuration $\mb{q}_{\text{init}} = [\frac{\pi}{2}\!+\!0.5, \>\> -1, \>\> -\frac{\pi}{2}\!+\!0.5]$. 
For every new pair of weights, its starting configuration is that of the previous pair. 
Figure \ref{fig:PlanarRobotPerformanceFeasible} shows the tracking performance. 
We can see that both position and orientation were tracked with a negligible error.
This result shows that the approach converges to a proper solution and maintains it when the weights are modified. In other words, once the robot converged to a configuration that fulfills both tasks with the initial weights $(w_{\text{orient}},w_{\text{pos}}) = (1,0)$, further variations of the weights did not affect the solution, as one would expect. 

\begin{figure}
	\centering
	\includegraphics[width=\columnwidth]{figures/PlanarRobotTrackingPerfCase1errors}
	\caption{Performance of the robot in tracking position and orientation references with varying weights when both tasks can be achieved.
	}
	\label{fig:PlanarRobotPerformanceFeasible}
\end{figure}

\subsection{Experiment: Learning task priorities with the humanoid robot COMAN}
We now test the hierarchy extraction and synthesis capabilities of the approach, with the COMAN robot. Since the kinematic chains of the humanoid arms share the waist joints, task incompatibilities often occur during bimanual manipulation when the tasks of the two arms are too far apart (Figure \ref{fig:COMAN_PrioritiesCloseFar}). 
In this experiment we use the proposed approach to teach the robot which arm has priority over the other. If correctly learned, the robot will prioritize one of the references when the two are incompatible and will closely track both when reachable.

We consider a TP-GMM with $P=2$ candidate hierarchies, corresponding to the two possible combinations of priorities, i.e. priority on left arm with the right arm as secondary and vice-versa. We denote the Jacobians of each arm (from the base link) by $\mb{J}_{\!\!L}$, $\mb{J}_{\!\!R}$, with corresponding pseudoinverses $\mb{J}^{\dagger}_{\!\!L}$, $\mb{J}^{\dagger}_{\!\!R}$ and null space projection matrices  $\mb{N}_{\!\!L}$, $\mb{N}_{\!\!R}$. 
We thus employ the task parameters 
\begin{align}
\mb{A}^{(1)} = \left[\mb{J}^{\dagger}_{\!\!L} \>\>\> \mb{N}_{\!\!L}\mb{J}^{\dagger}_{\!\!R} \right]
,\qquad
\mb{A}^{(2)} = \left[\mb{N}_{\!\!R}\mb{J}^{\dagger}_{\!\!L} \>\>\> \mb{J}^{\dagger}_{\!\!R} \right]
\label{eq:TPsWithObjectTrackingBimanual}
\end{align}
and the desired task space velocities for each candidate hierarchy 
\begin{equation}
	\mb{\dot{x}}^{(1)} = \mb{\dot{x}}^{(2)} = \left[\begin{matrix} \mb{\hat{x}}_L - \mb{x}_L \\ \mb{\hat{x}}_R - \mb{x}_R\end{matrix}\right],
\end{equation}
where $\mb{\hat{x}}_L$ and $\mb{\hat{x}}_R$ denote left and right reference positions.
For training the model, we use datapoints of the form ${\mb{\xi}_t = \left[\begin{matrix} \mb{\dot{x}}^\trsp_{L,t} \>\> \mb{\dot{x}}^\trsp_{R,t}\end{matrix}\right]^\trsp \in \mathbb{R}^6}$, where $\mb{\dot{x}}_{L,t} = \mb{\hat{x}}_{L,t}-\mb{x}_{L,t} $ and $ \mb{\dot{x}}_{R,t} = \mb{\hat{x}}_{R,t}-\mb{x}_{R,t}$ are the desired left and right end-effector velocities during demonstrations.
We used for this task $K=1$, i.e., for a set of demonstrations, we want to have always the same hierarchy. A model with more states can be used if we want to teach input dependent priorities, however, we intend to show that with a single Gaussian we can already encode complex behaviors.

Demonstrations of the priority behaviors were performed by implementing an inverse kinematics solution with the desired hierarchy in the simulated robot and making it track moving references\footnote{Alternatively, kinesthetic teaching or optical tracking of movements from humans could also be used, by recording the tracking errors as well as the kinematic data of the demonstrator (that would allow for computing the required Jacobians).}. 
During each demonstration, the priority behavior was shown, i.e., one of the arms cannot fulfill its task while the other can.
We generated demonstrations of the two different possibilities of priority for this setup and analyzed the resulting datasets. Figure \ref{fig:PriorityDataSets} shows the task space velocities of each hand, generated by each candidate hierarchy, computed from \eqref{eq:GeneralizedProjection} with task parameters \eqref{eq:TPsWithObjectTrackingBimanual}.
As one would expect from Section \ref{subsec:extractingHierarchies}, we observe an invariant distribution of datapoints for the hierarchy that was demonstrated, whether the priority was on the left arm (Figure \ref{fig:PriorityDataSetsLeftArm}) or on the right arm (Figure \ref{fig:PriorityDataSetsRightArm}). Low variability results in high precision matrices $\mb{\Gamma}^{(j)}$, which in turn, during movement synthesis, result in higher weights that favor the corresponding hierarchy.

We applied the learned models (left and right arm priority) to reach fixed points on the left and right sides of the robot, with the results depicted in Figure \ref{fig:COMANPrioritySim}.
As expected, in both cases, we observe that the arm which had the highest priority during demonstrations always fulfills the task, while the other only does it when the reference is reachable. Figure \ref{fig:COMANPriority} shows snapshots of the same behaviors in the real COMAN platform. In this experiment we used an optical tracking system to provide moving references to each arm. The robot closely tracks the reference on the side that has the highest priority, while doing its best to track the secondary reference. Videos of the experiment can be found at \texttt{\footnotesize\url{http://programming-by-demonstration.org/Silverio2017}}.

Similarly to the experiment in Section \ref{subsec:shakingTask}, each candidate hierarchy provides a possible configuration space solution for the robot. In the particular case of humanoid robots performing a bimanual skill, the solutions differ in how the waist joints are prioritized (since they are shared by the two arms). Figure \ref{fig:COMAN_WaistJointsLeft} shows the solution given by each candidate hierarchy for the roll, pitch and yaw joints of the waist, during the task of tracking the left object with higher priority. We see that the solution given by the hierarchy that encodes priority on the left arm (red line) has much lower variability than the one that prioritizes the right tracking task (green line). This leads to TP-GMM automatically extracting this prioritization as the important one.
\begin{figure}
	\centering
	\includegraphics[width=\columnwidth]{figures/COMAN_EqualPriorityCloseFar.png}
	\caption{Illustration of the waist joint prioritization problem. \textbf{Left:} Both targets are within the reachable workspace of both arms, so the robot can successfully reach the centers of the spheres with the palm of its hands. \textbf{Right:} References become simultaneously unreachable, so the robot cannot reach any of the two. The small circles depict the tool center point of each arm, while the large circles represent the centers of the references.}
	\label{fig:COMAN_PrioritiesCloseFar}
\end{figure}
\begin{figure}
	\begin{subfigure}{0.49\columnwidth}
		\centering
		\includegraphics[width=1.0\columnwidth]{figures/LeftPriorityDataSet_points}
		\caption{Demonstrated \textbf{left arm priority}.}
		\label{fig:PriorityDataSetsLeftArm}
	\end{subfigure}
	\begin{subfigure}{0.49\columnwidth}
		\centering
		\includegraphics[width=1.0\columnwidth]{figures/RightPriorityDataSet_points}
		\caption{Demonstrated \textbf{right arm priority}.}
		\label{fig:PriorityDataSetsRightArm}
	\end{subfigure}
	\vspace{0.3cm}
	\caption{Task space velocities generated by each candidate hierarchy given the demonstration data. Since velocities are 3-dimensional we plot their norm to ease visualization. The cross represents the mean of the datapoints. Notice the low variability in the datapoints corresponding to the demonstrated hierarchies.}
	\label{fig:PriorityDataSets}
\end{figure}
\begin{figure
	\centering
	\includegraphics[width=0.49\columnwidth]{figures/COMAN_priorityLeft}
	\includegraphics[width=0.49\columnwidth]{figures/COMAN_priorityRight}
	\caption{Movement synthesis using the learned left and right arm priority models. \textbf{Left:} The left reference is always tracked, while the right reference is only tracked when reachable. \textbf{Right:} The right reference is prioritized.}
	\label{fig:COMANPrioritySim}
\end{figure}
\begin{figure}
	\centering
	\includegraphics[width=0.99\columnwidth]{figures/LeftPriority}\\
	\vspace{0.1cm}
	\includegraphics[width=0.99\columnwidth]{figures/RightPriority}
	\caption{Movement synthesis in the real COMAN. \textbf{Top:} Reproduction with the model trained on left arm priority demonstrations: the robot prioritizes the left marker. \textbf{Bottom:} Reproduction with the model trained on right arm priority demonstrations: the robot prioritizes the right marker.}
	\label{fig:COMANPriority}
\end{figure}
\begin{figure
	\centering
	\includegraphics[width=\columnwidth]{figures/LeftPriorityWaistJoints}
	\caption{Waist joint inverse kinematics solutions (in radians) given by each candidate hierarchy. The envelope around the lines represents one standard deviation of the variance encoded in each hierachy. TP-GMM extracts the solution with the lowest variability.}
	\label{fig:COMAN_WaistJointsLeft}
\end{figure}

\subsection{Experiment: Transferring priority constraints between robots}

In this experiment we test the transfer of priority models across robots with similar embodiments. We use the model that was learned with COMAN to synthesize an equivalent prioritization behavior in the WALK-MAN robot that has a different kinematic structure. WALK-MAN is a humanoid larger than COMAN, that also shares the waist joints between the two arms.

Figure \ref{fig:WALKMANPriority} shows that the model that was learned using COMAN's demonstrations was successful in generalizing the learned task priorities in WALK-MAN. This stems from two facts: i) the considered tasks, and associated weights, are in operational space, whose dimension is the same for both robots and ii) the robots have similar kinematic chains, i.e., in both, waist joints are shared by the arms (it would not make sense to apply this priority model, for example, in a dual-arm platform of two single arms).

This opens up an interesting strategy to cope with the correspondence problem in imitation learning.
In particular, it could be used by the robot to visually observe a task prioritization behavior demonstrated by the user without explicit direct mapping of the kinematics.

\begin{figure
	\centering
	\includegraphics[width=0.49\columnwidth]{figures/WALKMAN_priorityLeft_lowRes}
	\includegraphics[width=0.49\columnwidth]{figures/WALKMAN_PriorityRight_lowRes}
	\caption{Movement synthesis with the WALK-MAN robot using the left and right priority models that were learned with COMAN.}
	\label{fig:WALKMANPriority}
\end{figure}
\section{Obtaining angular velocity using linear operators}
\label{app:TPorientation}

From Equation \eqref{eq:wFromVecPart}, angular velocity is obtained using the operator $\mathrm{vec}(\mb{\epsilon}_{t}*\mb{\bar{\epsilon}}_{t-1})$, whose non-linearity is incompatible with the structure of TP-GMM parameterization. We can however employ unit quaternion properties (Appendix \ref{app:Quat}) to simplify this operator. For any unit quaternion, $\mathrm{vec}(\mb{\epsilon})$ can be replaced by the matrix operation $\mathrm{vec}(\mb{\epsilon}) = \left[ \begin{matrix} \mb{0}^{3\times1} & \mb{I}^{3\times3} \end{matrix} \right] \mb{\epsilon}$, allowing us to rewrite \eqref{eq:wFromVecPart} as
\begin{equation}
\mb{\omega}_t = \left[ \begin{matrix} \mb{0}^{3\times1} & \mb{I}^{3\times3} \end{matrix} \right] (\mb{\epsilon}_{t}*\mb{\bar{\epsilon}}_{t-1})\frac{1}{\Delta t}.
\end{equation}

The quaternion product $ \mb{\epsilon}_{t}*\mb{\bar{\epsilon}}_{t-1} $ can also be replaced by a matrix product using the quaternion matrices. We take advantage of matrix $\bar{\mb{H}}$ from \eqref{eq:Hamiltons} that allows for changing the order in which two quaternions are multiplied without changing the resulting orientation. We can thus write
\begin{align}
&\mb{\epsilon}_t*\mb{\bar{\epsilon}}_{t-1} = \bar{\mb{H}}(\mb{\bar{\epsilon}}_{t-1})\>\mb{\epsilon}_t \\
\Rightarrow & \>\> \mb{\omega}_t = \left[ \begin{matrix} \mb{0}^{3\times1} & \mb{I}^{3\times3} \end{matrix} \right] \bar{\mb{H}}(\mb{\bar{\epsilon}}_{t-1})\>\mb{\epsilon}_t\frac{1}{\Delta t}.
\label{eq:quatProdHRight}
\end{align}
Defining, for any matrix $\mb{M}\!\in\!\mathbb{R}^{4\times4}$, ${ \mb{M^*} = \left[ \begin{matrix} \mb{0}^{3\times1} &  \mb{I}^{3\times3} \end{matrix} \right] \mb{M}}$, Eq.  \eqref{eq:quatProdHRight} yields
\begin{align}
\mb{\omega}_t & = \bar{\mb{H}}^*(\mb{\bar{\epsilon}}_{t-1})\>\mb{\epsilon}_t \frac{1}{\Delta t}\>.
\label{eq:linearW}
\end{align}
Note that other operations can be used in place of Eq. \eqref{eq:wFromVecPart} to obtain an angular velocity, namely the logarithmic map of the unit quaternion (see \cite{Ude14ICRA} for a comparison with \eqref{eq:wFromVecPart}). However, the linear structure of Eq. \eqref{eq:linearW} makes \eqref{eq:wFromVecPart} a convenient form for the TP-GMM parameterization.

\section{Introduction}

\IEEEPARstart{T}{he} human-robot transfer of bimanual skills is a growing topic of research in robot learning. As the number of available dual-arm platforms and humanoid robots increases, the existing learning and control algorithms must be transformed to accommodate the constraints imposed by this morphology and to take full advantage of the repertoire of tasks that such robots can perform \cite{Smith2012}. Among the available learning techniques, Programming by Demonstration (PbD) \cite{Billard2008} looks appealing but has so far mostly addressed the problem of extracting invariant patterns in uni-manual or single-task movements. This article addresses the problem of extending this principle to the extraction of more elaborated features such as simultaneous configuration and operational space constraints, and task prioritization. 

As humans, we employ rich bimanual coordination behaviors on a daily basis (e.g., tying knots, moving heavy or bulky objects, sweeping the floor), as well as complex decision-making such as which arm should be favored when the two must handle incompatible tasks.
Most research on learning bimanual skills exploits operational space formulations (e.g. \cite{Silverio15,Gams14TRO,Umlauft14ICRA,Lioutikov2014,Likar15,Ureche14}).
However, if skills contain constraints in configuration space, such as a preferred posture or an arm movement for which joint trajectories are more important than end-effector position, such operational space formulation is insufficient for correct task execution.
Similarly, bimanual robots, especially humanoids, are often required to perform dexterous dual-arm skills that require handling multiple tasks in parallel that can sometimes be conflicting. These conflicts can occur at various levels, such as when determining how to use the torso joints if both arms are required by the task, or how to switch between poses while keeping balance. 
Endowing robots with the ability to learn how to handle such priority constraints is an important challenge in the learning of robot controllers. This problem relates to the challenge of organizing movement primitives not only in series but also in parallel, which has often been overlooked.

In this article, we offer a new perspective on task-parameterized movement models, where not only the state of the environment is considered for modulating a skill, but also the state of the robot. The approach is based on Task-Parameterized Gaussian Mixture Models (TP-GMM) \cite{Calinon16JIST} that we further develop here to tackle learning problems of increased complexity. In its original formulation, TP-GMM allows for encoding movements from the perspective of different coordinate systems in the workspace of the robot, representing poses of objects or other landmarks. Such coordinate systems can have different importance throughout the task, depending on the variability observed in the demonstrations. Here we exploit the affine structure offered by TP-GMM to consider projection operators other than rotation and translation, such as Jacobian matrices and task hierarchies, which allow for learning a more diversified range of tasks.

More specifically, the contribution of this paper is a novel formulation of TP-GMM to:
\begin{enumerate}
	\item \textbf{Simultaneously learn constraints in operational and configuration spaces.} With respect to previous work \cite{Calinon09AR}, this article is an improvement in two directions: i) it formalizes the handling of constraints in operational and configuration spaces in the context of TP-GMM and ii) it introduces projection operators, based on unit quaternions, that allow for considering orientation constraints.
	\item \textbf{Learn task priorities from demonstrations.} We propose an approach for learning and synthesizing demonstrated priority behaviors, given an initial set of candidate task hierarchies.
\end{enumerate}

Some of the aforementioned points were briefly introduced in \cite{Calinon16JIST}.
Here we provide an extensive analysis and validate the proposed approach in two experimental setups. First we use the COMpliant huMANoid (COMAN) \cite{Tsagarakis13} robot to show that the proposed approach can be used to handle operational and configuration space constraints simultaneously. For this, we choose the skill of bimanually shaking a bottle.
We then use a bimanual reaching task with two conflicting constraints to show that our algorithm can be used to teach priorities from demonstrations. The task consists of tracking two moving targets on the left and right sides of the robot with the corresponding arm.
We finally demonstrate that the priority model can be transferred between different robots by applying the model that was trained using COMAN to the humanoid robot WALK-MAN \cite{Tsagarakis17}.

The paper is divided in seven sections. Section \ref{sec:relatedWork} describes related work. Section \ref{sec:LearningTPMovements} reviews TP-GMM and how its structure allows for learning tasks in operational space. Section \ref{sec:combiningConstraints} introduces the new formulation that allows for combining operational and configuration space constraints and Section \ref{sec:priorityConstraints} focuses on learning task prioritization from demonstrations.  Section \ref{sec:discussion} discusses the experimental results presented in the previous two sections. Finally, we draw conclusions and discuss future work in Section \ref{sec:conclusions}.

\section{Learning operational and configuration space constraints simultaneously}
\label{sec:combiningConstraints}

Previous works that exploit TP-GMM \cite{Silverio15,Calinon14ICRA,Rozo16TRO} consider manipulation problems defined in operational space. In such cases, task parameters are related to object poses, i.e., $ \mb{b}^{(j)}_t $ and $ \mb{A}^{(j)}_t $ parameterize positions and orientations of coordinate systems, respectively. However, robots are controlled in configuration space, i.e. even when tasks are defined in operational space, we map Cartesian velocity/force references into joint velocities/torques. As a consequence, it makes sense to also consider the configuration space when learning manipulation skills. Moreover, in learning problems, regardless of the algorithm or method that is employed to learn a skill, we typically choose either the configuration or the operational space to encode it, which requires prior reasoning about the task to be taught.

In this section, we propose to exploit the structure of TP-GMM introduced in Section \ref{sec:LearningTPMovements} to simultaneously consider constraints in operational and configuration spaces. The aim is to circumvent the need for previously selecting the space in which one should encode a given task by letting the model automatically discover the proper space from the demonstrations. The approach can be summarized as encoding the demonstrated movement in both configuration and operational spaces and, through statistics, extracting the space with the least variability (i.e., with the highest consistency) at every step of movement synthesis. We do this by considering Jacobian-based task parameters that project operational space constraints on configuration space, where Gaussian products (see \eqref{eq:TPGMM}, \eqref{eq:GaussProduct}) are computed.

Conceptually, the approach we exploit here shares connections with the one introduced in \cite{Calinon09AR}. However, here we generalize to an arbitrary number of candidate coordinate systems $P$ by framing the approach in the context of TP-GMM, and focus the analysis on humanoid robots. Additionally, we consider orientation constraints for the end-effector into the proposed framework.

\subsection{Jacobian-based task parameters}
\label{sec:JacTP}

Handling constraints in configuration and operational spaces is achieved by exploiting the linear structure of the task parameters of TP-GMM, in combination with inverse kinematics. Formally, consider a manipulator with $ N_q $ joints, whose positions and velocities are denoted by $ \mb{q}, \mb{\dot{q}}\in\mathbb{R}^{N_q} $. Its differential kinematics are given by $ \left[\mb{\dot{x}}^\trsp \>\> \mb{\omega}^\trsp \right]^\trsp = \mb{J}\mb{\dot{q}} $, where $ \mb{\dot{x}},\mb{\omega}\in\mathbb{R}^3 $ are the operational space linear and angular velocities. The Jacobian matrix $ \mb{J} = \left[\mb{J}^\trsp_p \>\> \mb{J}^\trsp_o\right]^\trsp \in \mathbb{R}^{6\times N_q}$ accounts for the contribution of joint velocities to operational space velocities, with matrices $\mb{J}_p,\mb{J}_o\in\mathbb{R}^{3\times N_q}$ responsible for the linear and angular parts, respectively. The inverse differential kinematics equation $ \mb{\dot{q}} = \mb{J}^{\dagger} \mb{\dot{x}} $, with $ \mb{J}^{\dagger} = \mb{J}^\trsp \left(\mb{J}\mb{J}^\trsp\right)^{-1} $ the right pseudo-inverse of the manipulator Jacobian $ \mb{J} $, yields the minimum-norm $ \mb{\dot{q}} $ that ensures $ \mb{\dot{x}} $ in operational space \cite{Siciliano09Book}. The inverse differential kinematics for position is thus given by $ \mb{\dot{q}} = \mb{J}^{\dagger}_{\!p} \mb{\dot{x}} $. Numerical integration of this equation allows for computing joint references for a desired end-effector position $ \mb{x}_t $ as (dropping the Jacobian subscript $ p $ from now on)
\begin{align}
\mb{\hat{q}}_t-\mb{q}_{t-1} & = \mb{J}_{t-1}^{\dagger} \left(\mb{x}_t - \mb{x}_{t-1} \right) \nonumber \\
\iff \mb{\hat{q}}_t & = \mb{J}_{t-1}^{\dagger} \mb{x}_t + \mb{q}_{t-1} - \mb{J}_{t-1}^{\dagger} \mb{x}_{t-1},
\label{eq:JacTP1}
\end{align}
where $ \mb{\hat{q}}_t $ denotes the desired joint angles at $ t $. For end-effector positions encoded in a GMM, $ {\mb{x}_t\sim \sum^K_{i=1}\pi_i \mathcal{N}\left(\mb{\mu}_{i},\mb{\Sigma}_{i}\right)}$, the linear transformation properties of Gaussian distributions \eqref{eq:TPGMM} can be applied to \eqref{eq:JacTP1} to project the GMM on configuration space
\begin{equation}
	\mb{\hat{q}}_{t,i} = \mb{J}_{t-1}^{\dagger} \mb{\mu}_{i} + \mb{q}_{t-1} - \mb{J}_{t-1}^{\dagger} \mb{x}_{t-1}, \quad\forall i =1,\dots,K.
	\label{eq:JacTP2}
\end{equation}
If we consider different candidate desired end-effector positions $ P $, indexed by $ j=1, \dots, P $ we have, for each Gaussian $ i $,
\begin{equation}
	\mb{\hat{q}}^{(j)}_{t,i} = \underbrace{\mb{J}_{t-1}^{\dagger}}_{\mb{A}^{(j)}_t} \mb{\mu}^{(j)}_{i} + \underbrace{\mb{q}_{t-1} - \mb{J}_{t-1}^{\dagger} \mb{x}_{t-1}}_{\mb{b}^{(j)}_t}.
	\label{eq:JacTP3}
\end{equation}
Moreover, if we consider end-effector positions encoded with respect to objects parameterized by $ \{\mb{p}^{(j)}_t, \mb{R}^{(j)}_t \} $, Eq. \eqref{eq:JacTP3} becomes
\begin{equation}
\mb{\hat{q}}^{(j)}_{t,i} = \underbrace{\mb{J}_{t-1}^{\dagger} \mb{R}^{(j)}_t}_{\mb{A}^{(j)}_t} \mb{\mu}^{(j)}_{i} + \underbrace{\mb{J}_{t-1}^{\dagger}\left(\mb{p}^{(j)}_t - \mb{x}_{t-1}\right) + \mb{q}_{t-1}}_{\mb{b}^{(j)}_t},
\label{eq:JacTP4}
\end{equation}
which can be derived in a straightforward manner from \eqref{eq:JacTP3} by assuming rotated and translated Gaussians $ \mb{R}^{(j)}_t \mb{\mu}^{(j)}_{i} + \mb{p}^{(j)}_t $.

Notice that $ \mb{A}^{(j)}_t\in\mathbb{R}^{N_q\times3} $ and  $ \mb{b}^{(j)}_t\in\mathbb{R}^{N_q} $ map from Cartesian position to joint angles, solving the inverse kinematics with a reference given by the mean $ \mb{\mu}^{(j)}_i $. The TP-GMM representation is therefore extended to Jacobian-based, time-varying, task parameters $\{\mb{b}^{(j)}_t,\mb{A}^{(j)}_t\}_{j=1}^P$ with different dimensionality for each candidate projection $j$ and non-square $\mb{A}^{(j)}_t$ matrices. This way, operational space constraints are projected on configuration space, where Gaussian products are computed.

\begin{table*}
	\captionsetup{justification=centering}
	\setlength\tabcolsep{2.0pt}
	\renewcommand{\arraystretch}{2.0}
	\caption{Summary of task parameters for candidate projection operators}
	\centering
	\begin{tabular}{|p{5cm} l  l  l  l|
			\hline
			\quad \textit{Configuration space constraints:} &
			$ \mb{\hat{q}}^{(j)}_{t,i} $ & $ = \mb{I} $	& $ \mb{\mu}^{(j)}_i $ & $ + \> \mb{0}  $
			\\
			\quad \textit{Absolute position constraints:} &
			$ \mb{\hat{q}}^{(j)}_{t,i} $ & $ = \mb{J}^{\dagger}_{t-1} $	& $ \mb{\mu}^{(j)}_i $ & $ - \> \mb{J}^{\dagger}_{t-1} \mb{x}_{t-1} + \mb{q}_{t-1} $
			\\
			\quad \textit{Relative position constraints:} &
			$ \mb{\hat{q}}^{(j)}_{t,i} $ & $ = \mb{J}^{\dagger}_{t-1} \mb{R}^{(j)}_t $ & $ \mb{\mu}^{(j)}_i $ & $ + \> \mb{J}^{\dagger}_{t-1} (\mb{p}^{(j)}_t - \mb{x}_{t-1}) + \mb{q}_{t-1} $
			\\
			\quad \textit{Absolute orientation constraints:} &
			$ \mb{\hat{q}}^{(j)}_{t,i} $ & $ = \mb{J}^{\dagger}_{t-1} \bar{\mb{H}}^*(\mb{\bar{\epsilon}}_{t-1}) $ & $ \mb{\mu}^{(j)}_i $ & $ + \> \mb{q}_{t-1} $
			\\
			\quad \textit{Relative orientation constraints:} &
			$ \mb{\hat{q}}^{(j)}_{t,i} $ & $ = \underbrace{\mb{J}^{\dagger}_{t-1} \bar{\mb{H}}^*\left(\mb{\bar{\epsilon}}_{t-1}\right) \>\overset{+}{\mb{H}}\left(\mb{\epsilon}^{(j)}_t\right)}_{\mb{A}^{(j)}_t} $ & $ \mb{\mu}^{(j)}_i $ & $ + \underbrace{\mb{q}_{t-1}\hspace{2.75cm}}_{\mb{b}^{(j)}_t} $\\
			\hline
	\end{tabular}
	\label{tab:ProjOperators}	
\end{table*}

\subsection{Orientation projection operators}

Orientation constraints represent an important component of operational space in many tasks. We have seen in previous work \cite{Silverio15} that, in bimanual manipulation, the orientation between end-effectors is of utmost importance for correct task execution.
Here, we take advantage of the algebraic properties of unit quaternions to derive linear operators for projecting orientation constraints on configuration space.

Let us consider the orientation part of the end-effector pose represented by a unit quaternion $ \mb{\epsilon} $ (Appendix \ref{app:Quat} reviews this representation) and the inverse differential kinematics for angular velocities, $ \mb{\dot{\mb{q}}} = \mb{J}^{\dagger}_{\!o} \mb{\omega} $. From \cite{Pastor11} we have that
\begin{equation}
\mb{\omega}_t = \frac{\mathrm{vec}(\mb{\epsilon}_{t}*\mb{\bar{\epsilon}}_{t-1})}{\Delta t}
\label{eq:wFromVecPart}
\end{equation}
gives the angular velocity that rotates the unit quaternion $ \mb{\epsilon}_{t-1} $ into $ \mb{\epsilon}_t $, during $ \Delta t $. The operation $ \mathrm{vec}(\mb{\epsilon}_{t}*\mb{\bar{\epsilon}}_{t-1}) $ can be replaced by the matrix-vector product $ \bar{\mb{H}}^*(\mb{\bar{\epsilon}}_{t-1})\>\mb{\epsilon}_t$ (see Appendix \ref{app:TPorientation}), allowing us to write the inverse kinematics equation as
\begin{align}
& \mb{\hat{\dot{\mb{q}}}}_t = \mb{J}^{\dagger}_{t-1} \bar{\mb{H}}^*(\mb{\bar{\epsilon}}_{t-1})\>\mb{\epsilon}_t \frac{1}{\Delta t}\>, \\
\iff & \mb{\hat{q}}_{t} = \mb{J}^{\dagger}_{t-1} \bar{\mb{H}}^*(\mb{\bar{\epsilon}}_{t-1})\>\mb{\epsilon}_t\> + \mb{q}_{t-1},
\label{eq:JacTPQuat1}
\end{align}
which has a similar structure to \eqref{eq:JacTP1}, being linear on the quaternion $ \mb{\epsilon}_t $.

If $\mb{\mu}^{(j)}_i$ encodes the orientation of the end-effector with respect to a coordinate system $j$, we take advantage of this structure to obtain new task parameters $\mb{A}^{(j)}_t$, $\mb{b}^{(j)}_t$ that map a GMM from quaternion space to configuration space, namely
\begin{equation}
\mb{\hat{q}}^{(j)}_{t,i} =
\underbrace{\mb{J}^{{\dagger}}_{t-1} \bar{\mb{H}}^*(\mb{\bar{\epsilon}}_{t-1})}_{\mb{A}^{(j)}_t} \>\mb{\mu}^{(j)}_i
+
\underbrace{\mb{q}_{t-1}}_{\mb{b}^{(j)}_t}.
\label{eq:Ab_quat}
\end{equation}

For a desired end-effector orientation encoded in a coordinate system whose orientation is given by the quaternion $ \mb{\epsilon}^{(j)}_t $, \eqref{eq:Ab_quat} becomes
\begin{equation}
\mb{\hat{q}}^{(j)}_{t,i}  = \underbrace{\mb{J}^{\dagger}_{t-1} \bar{\mb{H}}^*\left(\mb{\bar{\epsilon}}_{t-1}\right) \>\overset{+}{\mb{H}}\left(\mb{\epsilon}^{(j)}_t\right)}_{\mb{A}^{(j)}_t} \mb{\mu}^{(j)}_i + \underbrace{\mb{q}_{t-1}}_{\mb{b}^{(j)}_t},
\label{eq:JacTP5}
\end{equation}
where $ \overset{+}{\mb{H}} $ is a quaternion matrix (see Appendix \ref{app:Quat}).

\begin{algorithm}[bt]
	\caption{Simultaneously learning constraints in operational and configuration spaces}
	\begin{algorithmic}[1]
		\Statex \textit{\textbf{Initialization}}
		\State Select list of candidate projection operators from Table \ref{tab:ProjOperators} based on the task at hand
		\begin{itemize}
			\item Canonical  operator $ \mb{A}^{(j)}_t \!=\! \mb{I}$, $\mb{b}^{(j)}_t \!=\! \mb{0}$, for encoding configuration space constraints
			\item Operational space operators, for absolute or relative position/orientation constraints in operational space
		\end{itemize}
		\State Collect demonstrations
	\end{algorithmic}
	\begin{algorithmic}[1]
		\Statex \textit{\textbf{Model training}}
		\State Apply EM \cite{Calinon16JIST} to obtain $\big\{\pi_i,\{\mb{\mu}^{(j)}_i,\mb{\Sigma}^{(j)}_i\}_{j=1}^P\big\}_{i=1}^K$ 
	\end{algorithmic}		
	\begin{algorithmic}[1]
		\Statex \textit{\textbf{Movement synthesis}}
		\For{$t=1,\dots,N$}
			\For{$j=1,\dots,P$}
				\State Update $ \{\!\mb{A}^{(j)}_t\!\!,\mb{b}^{(j)}_t\!\} $ according to Table \ref{tab:ProjOperators}
			\EndFor
			\For{$ i=1, \dots, K $}
			\State Compute $ \mb{\hat{\mu}}_{t,i} $ and $ \mb{\hat{\Sigma}}_{t,i} $ from \eqref{eq:GaussProduct}
			\State Apply GMR at $ \mb{\xi}^{\ty{I}}_t $:  ${\mathcal{P}(\mb{\xi}^{\ty{O}}_t | \mb{\xi}^{\ty{I}}_t) = \mathcal{N}\big(\mb{\xi}^{\ty{O}}_t | \mb{\hat{\mu}}^{\ty{O}}_t,\mb{\hat{\Sigma}}^{\ty{O}}_t\big)}$
			\State Use $ \mb{\hat{\mu}}^{\ty{O}}_t $ as joint references for the robot controller
			\EndFor
		\EndFor
	\end{algorithmic}
	\label{alg:combiningConstraints}
\end{algorithm}

\begin{figure}
	\centering
	\includegraphics[width=\columnwidth]{figures/COMAN_fullMove_lowRes}
	\caption{The COMAN robot performs the bimanual shaking task in simulation. \textbf{First:} The robot is in a neutral starting pose. \textbf{Second:} Reaching for the bottle and grasping it. \textbf{Third:} Bringing the bottle close to the torso. \textbf{Fourth:} Shaking movement executed through rhythmic oscillations of both shoulder joints.}
	\label{fig:shakingCOMAN_Sim}
\end{figure}

\subsection{Task parameters for configuration space constraints}

The previous two subsections provided task parameters that project operational space constraints on configuration space. Encoding configuration space movements in a TP-GMM, $ \mb{\hat{q}}^{(j)}_{t,i} = \mb{\mu}^{(j)}_i $, is done using simple task parameters $ \mb{A}^{(j)}_t = \mb{I}$, $\>\mb{b}^{(j)}_t=\mb{0} $, corresponding to a canonical projection operator. \\

Table \ref{tab:ProjOperators} gives a summary of the operators derived in this section. The overall procedure for learning and reproducing a skill is summarized in Algorithm \ref{alg:combiningConstraints}.

\begin{figure}
	\centering
	\includegraphics[width=0.9\columnwidth]{figures/endEffectorPosShaking}
	\vspace{-0.2cm}
	\caption{Left end-effector position (meters) from the perspective of the bottle coordinate system. Black lines represent demonstrations while the red line represents one reproduction of the movement. Ellipses depict the Gaussian components of the model (isocontour of one standard deviation). The shaded areas mark the duration of the reaching and shaking phases. Notice the low variability in position at the end of the reaching movement.}
	\label{fig:endEffectorPosShaking}
\end{figure}
\begin{figure}
	\centering
	\includegraphics[width=0.9\columnwidth]{figures/endEffectorOrientShaking}
	\vspace{-0.2cm}
	\caption{Left end-effector orientation (as a unit quaternion) from the perspective of the bottle coordinate system. Notice the low variability in orientation at the end of the reaching movement.}
	\label{fig:endEffectorOrientShaking}
\end{figure}


\subsection{Experiment: bimanual shaking skill}
\label{subsec:shakingTask}

In order to test the configuration space projection operators introduced in this section, we selected the skill of shaking a bottle using COMAN. The whole movement contains an operational space component (reaching, grasping a bottle and bringing it closer to the torso) and a configuration space component (shaking with rhythmic movements of the shoulders, see Figure \ref{fig:shakingCOMAN_Sim}).

The upper-body of the COMAN robot comprises 17 DOFs (3 DOFs for the waist and 7 for each arm) with the kinematic chains of both arms sharing the 3 waist joints. We define the differential kinematics of the left and right end-effectors as $\left[\mb{\dot{x}}^\trsp_L \>\> \mb{\omega}^\trsp_L \>\> \mb{\dot{x}}^\trsp_R \>\> \mb{\omega}^\trsp_R\right]^\trsp = \mb{J}_{\!\text{up}}\mb{\dot{q}}$, where $ \mb{J}_{\!\text{up}} $ is the upper-body Jacobian, $\mb{\dot{x}}_L, \mb{\omega}_L$, $\mb{\dot{x}}_R, \mb{\omega}_R$ are the left and right end-effector velocities and $\mb{\dot{q}}=\left[\mb{\dot{q}}^\trsp_{W} \>\> \mb{\dot{q}}^\trsp_L \>\> \mb{\dot{q}}^\trsp_R\right]^\trsp$ are the joint velocities of waist, left and right arms. The Jacobian matrix is given by \cite{Lee14}
\begin{equation}
\mb{J}_{\!\text{up}} = \left[\begin{matrix}	\mb{J}_{W|L} & \mb{J}_{\!\!L} & \mb{0} \\
\mb{J}_{W|R} & \mb{0}   & \mb{J}_{\!\!R}     \end{matrix}\right],
\end{equation}
where $\mb{J}_{W|L}$, $\mb{J}_{W|R}$ denote the Jacobians that account for the effect of the waist joints on left and right end-effector velocities. $\mb{J}_{\!\!L}$ and $\mb{J}_{\!\!R}$ correspond to the Jacobians of the left and right end-effectors from the waist link. The minimum norm inverse kinematics solution is given in this case by ${\mb{\hat{\dot{q}}} = \left[\begin{matrix} \mb{\hat{\dot{q}}}^\trsp_{W} \> \mb{\hat{\dot{q}}}^\trsp_L \> \mb{\hat{\dot{q}}}^\trsp_R \end{matrix}\right]^\trsp \!\!=\! \mb{J}^{\dagger}_{\!\text{up}}\> \left[\begin{matrix} \mb{\dot{x}}^\trsp_L \> \mb{\omega}^\trsp_L \> \mb{\dot{x}}^\trsp_R \> \mb{\omega}^\trsp_R\end{matrix}\right]^\trsp}\!\!$,
where $\mb{J}^{\dagger}_{\!\text{up}}$ is the right pseudo-inverse of $ \mb{J}_{\!\text{up}} $.

The presented experimental results (Figures \ref{fig:endEffectorPosShaking}-\ref{fig:ShoulderJointTPGMM}) were obtained in the Gazebo simulator. The skill was also reproduced in the real robot (Figure \ref{fig:shakingCOMAN}). We generated several demonstrations solving the inverse kinematics (for the operational space part of the movement) and using sinusoidal references for both shoulder joints (shaking movement)\footnote{Alternatively, kinesthetic teaching or optical tracking of movements from humans could be used.}. Simulated initial bottle pose, end-effector positions and orientations, as well as the joint angles of both kinematic chains, were recorded. In this experiment we consider the initial pose of the bottle, i.e., we assume that the grasp is successful and therefore the bottle pose does not affect the movement after the grasp. We collected 10 demonstrations of the skill, each of them with different initial poses of the bottle and a duration of approximately 13 seconds. Temporal alignment of the demonstrations was achieved using Dynamic Time Warping \cite{Chiu04}. We used a TP-GMM with $K=10$ components, chosen empirically. For this problem we considered $P=2$ projection operators. The first operator is a concatenation of \eqref{eq:JacTP4}, \eqref{eq:JacTP5}, parameterized with the initial pose of the bottle $ \{\mb{p}^{(1)}_t \!\!, \mb{R}^{(1)}_t \!\!, \mb{\epsilon}^{(1)}_t\} $, 
\setlength{\arraycolsep}{1pt}
\begin{align}
	&\mb{A}^{(1)}_t = \nonumber\\ &\mb{J}^{\dagger}_{\!\text{up}} \!\!\left[\begin{matrix} \mb{R}^{(1)}_t & \mb{0} & \mb{0} & \mb{0}\\	
																			  \mb{0} & \bar{\mb{H}}^*(\mb{\bar{\epsilon}}_{L, t-1}) \overset{+}{\mb{H}}(\mb{\epsilon}^{(1)}_t) & \mb{0} & \mb{0} \\
																			  \mb{0} & \mb{0} & \mb{R}^{(1)}_t & \mb{0}\\	
  																			  \mb{0} & \mb{0} & \mb{0} & \bar{\mb{H}}^*(\mb{\bar{\epsilon}}_{R, t-1}) \overset{+}{\mb{H}}(\mb{\epsilon}^{(1)}_t)\\
																			  \end{matrix} \right]\!\!,\\
	&\hspace{1.5cm}\mb{b}^{(1)}_t = \mb{J}^{\dagger}_{\text{up}} \left[\begin{matrix} \mb{p}^{(1)}_t - \mb{x}_{L,t-1} \\ \mb{0} \\ \mb{p}^{(1)}_t - \mb{x}_{R,t-1} \\ \mb{0} \end{matrix}\right] + \mb{q}_{t-1}.
\end{align}
The second projection operator is the canonical one, $\mb{A}^{(2)}_t =  \mb{I}, \> \mb{b}^{(2)}_t = \mb{0}$.
Subscripts $ L $ and $ R $ denote left and right end-effectors.

Figures \ref{fig:endEffectorPosShaking}-\ref{fig:JointsShaking} show the demonstration data over time\footnote{Due to space limitations we only plot data corresponding to the left arm.} (black lines), observed from operational and configuration spaces, together with the Gaussian components obtained after EM (green ellipses). In addition we also plot the references generated by GMR (red lines), for a new position and orientation of the bottle. In Figures \ref{fig:endEffectorPosShaking} and \ref{fig:endEffectorOrientShaking} we see that during the reach and grasp movement, there is low variability in the demonstrations, both in position and orientation, as the end-effector approaches the bottle (around $ t \approx 4s $). This is successfully encoded by the model (narrow Gaussians showing low variance), as this aspect of the movement is important for a correct completion of the task. It follows that the synthesized movement (red line) closely matches the demonstrations in the regions of low variability. Note that, after the grasp ($ t>7s $), the variance increases as the end-effectors move away from the initial bottle pose (to perform the shaking movement). Figure \ref{fig:JointsShaking} shows that, from the beginning of the shaking phase ($ t\approx8s $), the shoulder joint (bottom graph) exhibits a consistent oscillatory pattern that in this case is modeled by 3 Gaussians, which is adequately captured and synthesized by the model. This contrasts with the other joints of the robot, which do not influence the shaking movement. 

\begin{figure}
	\centering
	\includegraphics[width=0.9\columnwidth]{figures/JointsShaking}
	\vspace{-0.2cm}
	\caption{Waist joints (pitch and yaw) and shoulder joint of the left arm (in radians). Notice the low variability of the shoulder joint during the shaking part of the movement (second shaded area) and how the model captures the shaking pattern encoded in this joint.}
	\label{fig:JointsShaking}
\end{figure}
\begin{figure}
	\centering
	\includegraphics[width=0.9\columnwidth]{figures/ShoulderJointTPGMM}
	\vspace{-0.2cm}
	\caption{Shoulder joint angle estimations from each candidate space (red and green) and the resulting reference after TP-GMM (black). Each estimate has an associated mean (solid line) and variance (light color envelope), learned from demonstrations and synthesized during reproduction. Joint angles are in radians.}
	\label{fig:ShoulderJointTPGMM}
\end{figure}
\begin{figure}
	\begin{subfigure}{\columnwidth}
		\centering
		\includegraphics[width=\columnwidth]{figures/ShakeReach}
		\vspace{-0.45cm}
	\end{subfigure}
	\begin{subfigure}{\columnwidth}
		\centering
		\includegraphics[width=\columnwidth]{figures/ShakeShake}
	\end{subfigure}
	\caption{Reproduction of the shaking task in the real COMAN. \textbf{Top:} Snapshots of the reaching part of the movement, defined by constraints in operational space. The robot gradually reaches for the object, which is tracked using an optical system. \textbf{Bottom:} Snapshots of the shaking part of the movement, defined by constraints in configuration space. The robot performs the shaking through rhythmic motions of the shoulders, moving the shaker up and down repeatedly.}
	\label{fig:shakingCOMAN}
\end{figure}

When using the novel task parameters defined in this section, each candidate projection operator corresponds to a candidate configuration space solution. The weight of each solution is estimated from the demonstrations and encoded in Gaussian components with appropriate covariance matrices. Figure \ref{fig:ShoulderJointTPGMM} shows that TP-GMM correctly extracted the most relevant solution according to the requirements of the task. Notice how, until $t\!\approx\!5s$, TP-GMM (black line) matches the candidate solution given by the bottle coordinate system (red line). Since the variability encoded in this coordinate system is low compared to that of the configuration space (a consequence of the reach and grasp skill being governed essentially by operational space constraints), the Gaussian product favors this solution. This is achieved through the linear transformation properties of Gaussians, that allow for both centers and covariance matrices to be locally mapped from operational to configuration space using the proposed linear operators. Similarly, during the shaking phase, after $t\!\approx\!8s$, the reference generated for the shoulder joint matches the estimate obtained using the canonical projection operator. This is due to the shaking movement resulting in a consistent oscillatory pattern of that joint observed during the demonstrations, as seen in Figure \ref{fig:JointsShaking}. This shows that TP-GMM is a viable solution for the encoding of task relevant features in both operational and configuration spaces. This example shows that this information is essential for movement synthesis in new situations with a proper generalization of the demonstrated skill.

Finally, Figure \ref{fig:shakingCOMAN} shows the two distinct phases of the movement during a reproduction in the real COMAN platform. In this experiment, we used a tray to carry a shaker towards COMAN, where an optical tracking system provided the shaker pose to the robot. We employed the learned TP-GMM to generate joint references $ \mb{\hat{q}}_t $, for every time step of the reproduction, that were fed to a position controller at the joint level. In the top row of Figure \ref{fig:shakingCOMAN}, the robot takes into account the operational space constraints as it reaches for the bottle, while in the bottom row, the robot shakes the grasped bottle with rhythmic shoulder movements. In both parts of the movement, the operational and configuration space constraints are properly replicated. Videos of the reproduction are available at \texttt{\footnotesize\url{http://programming-by-demonstration.org/Silverio2017}}.

\section{Experiments
\label{sec:expResults}



\subsection{Learning task priorities with the humanoid robot COMAN}
We now test the hierarchy extraction and synthesis capabilities of our algorithm, described in Section \ref{subsec:priorityConstraints}. Since the kinematic chains of the humanoid arms share the waist joints, task incompatibilities often occur during bimanual manipulation when the tasks of the two arms are too far apart (Fig.\ \ref{fig:COMAN_PrioritiesCloseFar}).
We propose to demonstrate which of the arms has priority over the other and learn that priority using TP-GMM. If the priority is correctly learned, the robot will track both references when both are reachable, and prioritize one of them when the two become incompatible.

We consider a TP-GMM with $P=2$ candidate hierarchies, corresponding to the two possible combinations of priority, i.e. priority on left arm with the right arm as secondary and vice-versa. We denote the Jacobians of each arm (from the base link) by $\mb{J}_{\!\!L}$, $\mb{J}_{\!\!R}$, their pseudoinverses by $\mb{J}^{\dagger}_{\!\!L}$, $\mb{J}^{\dagger}_{\!\!R}$ and the null-space projection matrices by $\mb{N}_{\!\!L}$, $\mb{N}_{\!\!R}$. 
We thus employ the task parameters
\begin{align}
\mb{A}_1 = \left[\mb{J}^{\dagger}_{\!\!L} \>\>\> \mb{N}_{\!\!L}\mb{J}^{\dagger}_{\!\!R} \right]
,&\qquad
\mb{b}_1 = \mb{A}_1 \left[\begin{matrix} \mb{x}^\ty{O}_L - \mb{x}_L \\ \mb{x}^\ty{O}_R - \mb{x}_R \end{matrix}\right]
,\nonumber\\
\mb{A}_2 = \left[\mb{N}_{\!\!R}\mb{J}^{\dagger}_{\!\!L} \>\>\> \mb{J}^{\dagger}_{\!\!R} \right]
,&\qquad
\mb{b}_2 = \mb{A}_2 \left[\begin{matrix} \mb{x}^\ty{O}_L - \mb{x}_L \\ \mb{x}^\ty{O}_R - \mb{x}_R \end{matrix}\right],
\label{eq:TPsWithObjectTrackingBimanual}
\end{align}
where $\mb{x}^\ty{O}_L$ and $\mb{x}^\ty{O}_R$ denote left and right reference positions.
In addition, we use datapoints of the form $\mb{\xi}_t = \left[\begin{matrix} \mb{e}^\trsp_{L,t} \>\> \mb{e}^\trsp_{R,t}\end{matrix}\right]^\trsp \in \mathbb{R}^6$, where $\mb{e}^\trsp_{L,t}, \mb{e}^\trsp_{R,t}$ are the tracking errors of left and right arm tasks during demonstrations.
We used for this task $K=1$ (same hierarchy throughout the task, selected empirically).

Demonstrations of the priorities were provided by implementing an inverse kinematics solution with the desired hierarchy in the simulated robot and making it track moving references\footnote{Alternatively, kinesthetic teaching or optical tracking of movements from humans could also be used, by recording the tracking errors as well as the kinematic data of the demonstrator (that would allow for computing the Jacobians required for the projections of observed movements).}. Since the estimation of the hierarchy structure is based on the tracking error, a single demonstration contains sufficient datapoints for a correct model to be learned, provided that the priority behavior is shown, namely, that one of the arms cannot fulfill its task while the other can.
We generated demonstrations of the two different possibilities of priority for this setup and analyzed the resulting datasets. Figure \ref{fig:PriorityDataSets} shows the tracking errors of each hand after applying \eqref{eq:GeneralizedProjection} with the task parameters \eqref{eq:TPsWithObjectTrackingBimanual}.
We observe negligible, invariant errors for the hierarchies that were demonstrated.

Low variability results in high precision matrices $\mb{\Gamma}$, which in turn, during movement synthesis, results in higher weights being assigned to the hierarchies with low variability. We applied the learned models in both the left- and right-priority scenarios to reach fixed points on the left and right sides of the robot, with the results depicted in Fig.\ \ref{fig:COMANPriority}.
As expected, in both cases, we observe that the arm having the highest priority in the demonstrated movement is always fulfilings the task, while the other only does it when the reference is at a reachable distance.

Similarly to the example in Section \ref{subsec:shakingTask}, each candidate hierarchy provides a candidate configuration space solution for the robot. In the particular case of humanoid robots performing a bimanual skill, the solutions differ in how the waist joints are prioritized (since they are shared by the two arms). Figure \ref{fig:COMAN_WaistJointsLeft} shows the candidate solution given by each frame for the roll, pitch and yaw joints of the waist, during the task of tracking the left object with higher priority. We see that the solution given by the frame that encodes priority on the left arm has much lower variability than the one that prioritizes the right tracking task. This leads to a TP-GMM automatically extracting this prioritization as the important one.
\begin{figure}
	\centering
	\includegraphics[width=\columnwidth]{figures/COMAN_EqualPriorityCloseFar}
	\caption{The COMAN robot in the rviz simulation environment performing a bimanual reaching task. \textbf{Left:} Both targets are within the reachable workspace of both arms, so that the robot can successfully reach the centers of the spheres with the palm of its hands. \textbf{Right:} The references moved beyond the part of the workspace where both are reachable at the same time, so that the robot cannot reach any of the two.}
	\label{fig:COMAN_PrioritiesCloseFar}
\end{figure}
\begin{figure
	\begin{subfigure}{0.49\columnwidth}
		\centering
		\includegraphics[width=1.0\columnwidth]{figures/LeftPriorityDataSet_points}
		\caption{Demonstrated left-arm priority.}
		\label{fig:PriorityDataSetsLeftArm}
	\end{subfigure}
	\begin{subfigure}{0.49\columnwidth}
		\centering
		\includegraphics[width=1.0\columnwidth]{figures/RightPriorityDataSet_points}
		\caption{Demonstrated right-arm priority.}
		\label{fig:PriorityDataSetsRightArm}
	\end{subfigure}
	\vspace{0.3cm}
	\caption{Datapoints obtained through projection of the tracking errors using \eqref{eq:GeneralizedProjection} with task parameters \eqref{eq:TPsWithObjectTrackingBimanual}.
	For better visualization we plot the norm of each hand's error, instead of the 3D points that were used to learn the priority models. The cross represents the mean error. 
Notice the low variability in the data corresponding to the demonstrated hierarchies, and the error close to zero (even for the arm with lowest priority).}
	\label{fig:PriorityDataSets}
\end{figure}
\begin{figure
	\centering
	\includegraphics[width=0.49\columnwidth]{figures/COMAN_priorityLeft}
	\includegraphics[width=0.49\columnwidth]{figures/COMAN_priorityRight}
	\caption{Generation of new movements using the left and right arm priority models that were learned from demonstrations. The main task is always fulfilled, while the secondary task is successful when the target is close enough.}
	\label{fig:COMANPriority}
\end{figure}
\begin{figure
	\centering
	\includegraphics[width=\columnwidth]{figures/LeftPriorityWaistJoints}
	\caption{Candidate inverse kinematics solutions given by each candidate frame. The envelope around the lines represents the variability encoded in the frames for each joint. The solution proposed by the frame that encodes left-hand priority shows low variability.}
	\label{fig:COMAN_WaistJointsLeft}
\end{figure}


\subsection*{Transferring priority constraints between robots}
Since the projections of the data occur in operational space, priority models can be transferred across robots with similar embodiments. We use the model that was learned with COMAN to synthesize an equivalent prioritization behavior in the WALKMAN robot that has a different kinematic structure. WALKMAN is a humanoid larger than COMAN, that also shares the waist joints between the two arms.
Figure \ref{fig:WALKMANPriority} shows that the model that was learned using COMAN's kinematic data was successful in generalizing the learned task priorities in WALKMAN. This opens up an interesting strategy to cope with the correspondence problem in imitation learning.
In particular, it could be used by the robot to visually observe a task prioritization behavior demonstrated by the user without explicit direct mapping of the kinematics.

\begin{figure
	\centering
	\includegraphics[width=0.49\columnwidth]{figures/WALKMAN_priorityLeft_lowRes}
	\includegraphics[width=0.49\columnwidth]{figures/WALKMAN_PriorityRight_lowRes}
	\caption{Movement synthesis with the WALKMAN robot using the left and right priority models that were learned with COMAN.}
	\label{fig:WALKMANPriority}
\end{figure}
\subsection{Soft weighting of hierarchies: behavior during transitions}
An important aspect to consider when controlling priorities is the behavior of the robot when priorities change. In order to investigate how our approach performs in such cases, we study in this section the effect of varying the weights of each candidate hierarchy. For simplicity of analysis, we consider the case of a planar robot with two tasks, similar to the example in Section \ref{subsec:priorityConstraints}. The tasks are to track a vertical position reference with the end-effector and to point downwards. The references are chosen such that the two tasks can be fulfilled individually but not simultaneously (Fig.\ \ref{fig:VariableWeights}, \emph{top-left}). Two strict hierarchies are considered, corresponding to an accurate tracking of either position or orientation as first priority, and orientation or position as secondary. We define two scalar weights $w_{\text{pos}}$ and $w_{\text{orient}}$, which represent the importance of each hierarchy, with the subscript denoting the highest priority task. These weights are used to compute the precision matrices $\mb{\Gamma}_{\text{pos}} \!=\! w_{\text{pos}}\mb{I}$ and $\mb{\Gamma}_{\text{orient}} \!=\! w_{\text{orient}}\mb{I}$,
where $\mb{I}$ is a 2-dimensional identity matrix. Figure \ref{fig:VariableWeights} shows the results for the case in which the robot transits from the orientation to the position tracking task, through a continuous variation of the weights. We observe that, as different combinations of weights are applied by the controller, a smooth transition occurs between the two tasks. 


 \begin{figure}
 	\centering	
  	\includegraphics[width=\columnwidth]{figures/variableWeightsAllinOne}
 	\caption{End-effector behavior for varying hierarchy weights. \textbf{Top-left:} Planar robot fulfilling the two tasks separately. \textbf{Top-right:} End-effector state in the position-orientation space for the considered weights (blue line). \textbf{Bottom:} End-effector orientation and vertical position as weights vary. Notice the inverted scales on the horizontal axes - the experiment starts with $(1,0)$ and ends with $(0,1)$. Red lines and point indicate the references.}
 	\label{fig:VariableWeights}
 \end{figure}

Another case that is worth investigating is the behavior of the system when both tasks are achievable. In this situation one would expect that any combination of the weights would result in a successful completion of both tasks. In order to test how our approach handles this scenario, we decreased the vertical position reference so that both tasks can be fulfilled simultaneously.
We set the robot to an initial configuration $\mb{q}_{\text{init}} = [\frac{\pi}{2}\!+\!0.5, \>\> -1, \>\> -\frac{\pi}{2}\!+\!0.5]$.
The robot is initialized to that configuration and, for every new pair of weights, its starting configuration is that of the previous pair.
Figure \ref{fig:PlanarRobotPerformanceFeasible} shows the tracking performance.
We can see that the observed end-effector pose tracked the reference with a negligible error.
This result shows that the approach converges to a proper solution and maintains it when the weights are modified. In other words, once the robot converged to a configuration that fulfills both tasks, with the initial weights $(w_{\text{orient}},w_{\text{pos}}) = (1,0)$, further variations of the weights did not affect the solution, as one would expect. 



\begin{figure}
	\centering
	\includegraphics[width=\columnwidth]{figures/PlanarRobotTrackingPerfCase1}
	\caption{Performance of the robot in tracking position and orientation references with varying weights when both tasks can be achieved. Red lines and point indicate the references.
	}
	\label{fig:PlanarRobotPerformanceFeasible}
\end{figure}


Videos of the experiments can be found at  \begin{footnotesize}\texttt{http://joaosilverio.weebly.com/videos}\end{footnotesize}.

\section{Related Work}
\label{sec:relatedWork}

\subsection{Learning bimanual skills}
Most methods that have been proposed in recent years for learning bimanual manipulation from demonstrations were based on Dynamic Movement Primitives (DMPs) \cite{Ijspeert13}. Examples range from the use of virtual springs between end-effectors \cite{Gams14TRO} to the coupling of DMPs using artificial potential fields \cite{Umlauft14ICRA}.
Lioutikov \MakeLowercase{\textit{et al.}}\ \cite{Lioutikov2014} propose to combine sequences of DMPs that encode partial demonstrations of the complete movement of each arm.
Similarly to the spirit of DMPs, Likar \MakeLowercase{\textit{et al.}}\ \cite{Likar15} introduce an approach based on Iterative Learning Control for force adaptation in bimanual tasks.
In \cite{Ureche14}, Ureche and Billard focus on the extraction of arm dominance and role from demonstrations, as well as on the correlations between task variables such as poses and forces.
Our previous work \cite{Silverio15} follows a task-parameterized approach to learning bimanual manipulation, where relative and absolute movements of the end-effector are encoded with respect to a pre-defined set of coordinate systems, whose importance is learned from demonstrations.

This collection of work addresses the bimanual skill transfer problem from an operational space perspective. As a consequence, rich features involved in highly redundant manipulators, such as joint space movement patterns and task priorities, cannot be learned.

\subsection{Competing operational and configuration space constraints}

The problem of knowing which space --- between configuration and operational spaces --- is the most relevant for a given task, has generally been treated as a  hand-tuning of weights assigned to sub-tasks that can be in either one of the spaces \cite{Moro13}. Exceptions include approaches based on reinforcement learning (RL) \cite{Dehio15,Modugno16} and PbD \cite{Calinon09AR}, where such weights are learned. Approaches like \cite{Dehio15,Modugno16} employ stochastic optimization, given a set of reward functions related to sub-task goals in both spaces. In \cite{Calinon09AR}, Calinon and Billard treat the problem as a weighted least squares problem, where the weights associated with each space reflect the variability observed in the demonstration data. The two types of approach are similar in concept, however the RL-based ones require that considerable prior knowledge is accounted for in the reward functions, making those methods less straightforward. Here we follow a related approach to \cite{Calinon09AR}, where we consider an arbitrary number of tasks in operational space, related to potentially moving objects, and orientation constraints, overlooked in that work. 

\subsection{Learning task prioritization}

Common approaches to control task prioritization can be categorized in two main research directions, by either exploiting a strict hierarchy structure applied to multi-level hierarchies \cite{Khatib87, Sentis05,Ott15}, or by employing a soft weighting of tasks \cite{Moro13,Park01}. The two techniques have pros and cons. Setting an explicit null space structure guarantees strict priorities at the expense of constraining sometimes too much the tasks, which quickly limits the number of tasks that can simultaneously be handled compared to the number of degrees of freedom available for controlling the robot. 
This approach is also prone to discontinuities in the control problem when switching from one hierarchy structure to another. A soft weighting scheme can handle different levels of task importance and gradual changes from one task to another, but it does not provide strict guarantee on the fulfillment of each separated task. 

Some recent approaches consider the problem from new perspectives. A collection of work focuses on solving the prioritization problem under an optimization framework \cite{Lasa09,Salini11,Morris13}, while others concentrate on alternative representations of task prioritization \cite{Dietrich12,Liu16}. In this article, we tackle this challenge from a robot learning perspective. Learning how to handle the priorities of multiple tasks running in parallel is a challenging problem and the past few years have seen great advancements in this direction. However, several issues remain open. 

Learning priorities based on strict task hierarchies typically assumes that low priority tasks are projected in the null space of high priority ones. In this context, Wrede \MakeLowercase{\textit{et al.}}\ \cite{Wrede13} propose a two-step approach to kinesthetically teach tasks to redundant manipulators. First, in a configuration phase, the desired null space behavior is demonstrated to the gravity compensated manipulator. The relation between the end-effector position and desired configuration is encoded in a neural network which is employed in a subsequent phase to have the robot at the demonstrated configuration while the user guides the end-effector to demonstrate the task. Saveriano \MakeLowercase{\textit{et al.}}\ \cite{Saveriano15} propose an approach based on Task Transition Control (TTC) \cite{An15} to refine end-effector and null space policies. They take advantage of the smooth transitions between task priorities allowed by TTC to switch between task execution and teaching, yielding refinement of policies of both the main and the null space tasks in runtime. In \cite{Towell10}, Towell \MakeLowercase{\textit{et al.}}\ aim for extracting underlying null space policies from demonstrations, assuming a strict hierarchy of priorities. The approach, however, does not allow for the extraction of demonstrated hierarchies.

Hak \MakeLowercase{\textit{et al.}}\ \cite{Hak12b} present an iterative algorithm for identifying a \emph{stack of tasks}. From an observed joint trajectory, and a pre-defined pool of possible tasks that can be executed in parallel, the approach relies on the expected operational space behavior of each task to select the active tasks, and on the task-function formalism to gradually remove selected tasks from the observed movement through null space projections until all tasks have been selected. This approach shares similarities with our approach in the assumption that the demonstrated movements are generated using strict hierarchies, and in the fact that they also analyze the operational space to disambiguate between the possible active tasks. The main difference is that our task-space analysis is probabilistic instead of relying on a curve fitting score to determine how close the observed behavior is to an exponential function of time.

For the soft weighting of task priorities, a solution is typically given by a combination of weighted tasks, see \cite{Moro13} for an example with torque control and manually set weights. Dehio \MakeLowercase{\textit{et al.}}\ \cite{Dehio15} and Modugno \MakeLowercase{\textit{et al.}}\ \cite{Modugno16} propose to learn the weights of each task using Covariance Matrix Adaptation Evolution Strategy (CMA-ES), a derivative-free stochastic optimization method. In \cite{Dehio15}, the weights of each task are assumed to be constant. In \cite{Modugno16}, the weights are parameterized by Radial Basis Functions spread along a time window, which allows the priorities of the different tasks to change over time. Both approaches require the previous definition of a fitness function and a pool of elementary tasks. Our approach also requires prior information about potential hierarchies, but, unlike \cite{Dehio15,Modugno16}, it directly exploits the demonstrations to discover the structure of the task, without requiring an external fitness function. Lober \MakeLowercase{\textit{et al.}}\ \cite{Lober14} also use stochastic optimization, to refine Dynamic Movement Primitives of incompatible tasks in order to render them compatible. 
Their approach focuses on the re-organization of primitives in series, leaving out scenarios where tasks need to be executed in parallel with different levels of priority. 
In \cite{Lober15}, the approach was refined by employing Gaussian kernels to compute variance-dependent weights that are used to determine the priority of each task. This approach shares connections with ours in that the variance is used as a measure of importance of a task.
In that work, the estimated variance depends on the distance to the kernel centers, which are pre-defined along the planned trajectory. 
In contrast, our method exploits the variance extracted from the human demonstrations, allowing for the learning of new behaviors from the observed variations of a task.\\

\section{Task-Parameterized Gaussian Mixture Models}
\label{sec:LearningTPMovements}

In previous work we introduced a probabilistic approach to task-parameterized movements \cite{Silverio15,Calinon16JIST}. In particular, we introduced the Task-Parameterized Gaussian Mixture Model (TP-GMM), which encodes demonstrated movements in multiple coordinate systems simultaneously, described by a set of \textit{task parameters} whose importance can change during the task.
This information is used during movement synthesis to adapt the demonstrated skill to new situations. In this section we review the TP-GMM formulation.

\subsection{Model estimation}

We consider tasks defined in both operational and configuration spaces, consisting on the fulfillment of constraints in either or both spaces (e.g. tracking desired end-effector poses or joint trajectories). During task execution, robot movements may largely depend on given goals, object poses or obstacles, which can be defined with coordinate systems. We call these variables \textit{task parameters} since they are part of the parameterization of a task, i.e. they influence how a robot accomplishes the given task goals. In TP-GMM, the knowledge of the task parameters is exploited to adapt a demonstrated skill to new situations  (e.g., unobserved positions and orientations of a manipulated object).

Formally, each demonstration $m\in\{1,\ldots,M\}$ contains $T_m$ datapoints of dimension $ D $ forming a dataset of $N$ datapoints $\{\mb{\xi}_{t}\}_{t=1}^N$ with $N\!=\!\sum_{m=1}^{M}\!T_m$ and $ \mb{\xi}_{t}\in\mathbb{R}^D $.
The $P$ task parameters are defined at time step $t$ by $\{\mb{b}^{(j)}_t,\mb{A}^{(j)}_t\}_{j=1}^P$. For task parameters representing coordinate systems, $\mb{b}^{(j)}_t$ is the origin of the observer and $\mb{A}^{(j)}_t$ is a rotation matrix. We often refer to the task parameters as \textit{candidate coordinate systems}, or \textit{candidate projections} since they form a pool of elementary task structures to reproduce a movement.

The demonstrations $\mb{\xi}\!\in\!\mathbb{R}^{D\times N}$ are observed from these different viewpoints, forming $P$ trajectory samples ${\mb{X}^{(j)}\!\in\!\mathbb{R}^{D\times N}}$. The task parameters can be directly collected from sensors located at the coordinate systems, or they can be computed for each datapoint or demonstration with 
\begin{equation}
	\label{eq:TPprojection}
	\mb{X}^{(j)}_t = \mb{A}^{(j)^{-1}}_t \left(\mb{\xi}_t - \mb{b}^{(j)}_t\right) .
\end{equation}

The \textit{model parameters} of a TP-GMM with $K$ components are defined by $\big\{\pi_i,\{\mb{\mu}^{(j)}_i,\mb{\Sigma}^{(j)}_i\}_{j=1}^P\big\}_{i=1}^K$, where $\pi_i$ are the mixing coefficients and $\mb{\mu}^{(j)}_i$, $\mb{\Sigma}^{(j)}_i$ denote the center and covariance matrix of the $i$-th Gaussian in candidate coordinate system $j$.
Learning of the model parameters is achieved by log-likelihood maximization subject to the constraint that the data in the different coordinate systems arose from the same source, resulting in an \emph{expectation-maximization} (EM) algorithm to iteratively update the model parameters until convergence, see \cite{Calinon16JIST} for details.

\subsection{Gaussian Mixture Regression}

The learned model is used to reproduce movements in new situations. Each coordinate system encodes local features of the demonstrated movement and, in new situations, i.e. new values of the task parameters $ \mb{b}^{(j)}_t, \mb{A}^{(j)}_t $, one needs to find a trade-off between each local solution. TP-GMM solves this problem by combining the local models using the product of Gaussians. In this way, a new GMM with parameters $\{\pi_i,\mb{\hat{\mu}}_{t,i},\mb{\hat{\Sigma}}_{t,i}\}_{i=1}^K$ can automatically be generated with
\begin{multline}
  \mathcal{N}\!\Big( \mb{\hat{\mu}}_{t,i} , \mb{\hat{\Sigma}}_{t,i} \Big)
  \;\propto\;
  \prod\limits_{j=1}^P \mathcal{N}\!\Big(
  \mb{\hat{\mu}}{}_{t,i}^{(j)} ,\; \mb{\hat{\Sigma}}{}_{t,i}^{(j)} \Big) ,
  \;\mathrm{with}\\
  \mb{\hat{\mu}}{}^{(j)}_{t,i} = \!\mb{A}^{(j)}_t \mb{\mu}^{(j)}_i \!+\! \mb{b}^{(j)}_t
  \;,\quad
  \mb{\hat{\Sigma}}{}^{(j)}_{t,i} = \!\mb{A}^{(j)}_t\mb{\Sigma}^{(j)}_i \mb{A}^{(j)^\trsp}_t ,
  \label{eq:TPGMM}
\end{multline}
where the result of the Gaussian product is given by
\begin{equation}
  \mb{\hat{\Sigma}}_{t,i} = \Big( \sum\limits_{j=1}^P {\mb{\hat{\Sigma}}{}^{(j)}_{t,i}}^{-1} \Big)^{-1}
  ,\quad
  \mb{\hat{\mu}}_{t,i} = \mb{\hat{\Sigma}}_{t,i} \sum\limits_{j=1}^P {\mb{\hat{\Sigma}}{}^{(j)}_{t,i}}^{-1}
  \mb{\hat{\mu}}{}^{(j)}_{t,i} .
  \label{eq:GaussProduct}
\end{equation}

Note that $ \mb{\hat{\mu}}^{(j)}_{t,i}, \mb{\hat{\Sigma}}{}^{(j)}_{t,i} $ in \eqref{eq:TPGMM} map the local features, computed using \eqref{eq:TPprojection}, back to the original space (in robotics problems, usually the base of the robot), for  new $ \mb{b}^{(j)}_t, \mb{A}^{(j)}_t $, where Gaussian products are computed.
The obtained GMM can be used to retrieve a reference for the robot through Gaussian Mixture Regression (GMR) at any given time step $t$. In this case, the datapoint $\mb{\xi}_t$ is decomposed into two subvectors $\mb{\xi}^\ty{I}_t$ and $\mb{\xi}^\ty{O}_t$, spanning the input and output dimensions of the regression problem, thus the new GMM encodes the joint probability distribution $\mathcal{P}(\mb{\xi}^{\ty{I}}_t,\mb{\xi}^{\ty{O}}_t) \sim \sum_{i=1}^K\pi_i\;\mathcal{N}\big(\mb{\hat{\mu}}_{t,i},\mb{\hat{\Sigma}}_{t,i}\big)$. For a time-driven movement, $\mb{\xi}^\ty{I}_t$ corresponds to the current time step or a decay term, while $\mb{\xi}^\ty{O}_t$ can be the end-effector pose or the joint angles of the robot.
Similarly to the decomposition of $\mb{\xi}_t$ into input/output vectors, the task parameters $\mb{A}^{(j)}_t$ and $\mb{b}^{(j)}_t$ can also be defined so that the input is not modulated by the task parameterization. Compared to an initial TP-GMM encoding $\mb{\xi}^\ty{O}_t$ with task parameters $\mb{A}^{\ty{O}(j)}_t$ and $\mb{b}^{\ty{O}(j)}_t$, the combination of TP-GMM and GMR instead encodes $\mb{\xi}_t\!=\!\big[{\mb{\xi}^\ty{I}_t}^\trsp \>\> {\mb{\xi}^\ty{O}_t}^\trsp \big]^\trsp$ with task parameters
\begin{equation}
\mb{A}^{(j)}_t = \begin{bmatrix} \mb{I} & \mb{0} \\ \mb{0} & \mb{A}^{\ty{O}(j)}_t \end{bmatrix}
\;,\quad
\mb{b}^{(j)}_t = \begin{bmatrix} \mb{0} \\ \mb{b}^{\ty{O}(j)}_t \end{bmatrix} ,
\end{equation}
where in the case of a decay term (or an explicit time variable driving the system), the identity matrix $\mb{I}$ collapses to $1$. 
GMR generates a new distribution $\mathcal{P}(\mb{\xi}^{\ty{O}}_t | \mb{\xi}^{\ty{I}}_t) = \mathcal{N}\big(\mb{\xi}^{\ty{O}}_t | \mb{\hat{\mu}}^{\ty{O}}_t,\mb{\hat{\Sigma}}^{\ty{O}}_t\big)$ that is used to control the robot.

In the following section we show that Jacobian matrices, commonly employed in Robotics as a linear mapping operator in differential kinematics, can be used as task parameters in TP-GMM to combine operational and configuration space constraints.
\section{Unit Quaternion Preliminaries}
\label{app:Quat}

A unit quaternion $\mb{\epsilon}\in \mathcal{S}^{3}$ is defined by $\mb{\epsilon} = \left[ \epsilon_0 \>\> \epsilon_1 \>\> \epsilon_2 \>\> \epsilon_3 \right]^\trsp = \left[ v \>\> \mb{u}^\trsp \right]^\trsp$, where $v\in\mathbb{R}$ and $\mb{u}\in\mathbb{R}^{3}$, following the notation used by \cite{Ude14ICRA}, are the real and vector parts of the quaternion. The conjugate of a unit quaternion is denoted by $ \mb{\bar{\epsilon}} = \left[u \>\> -\mb{u}^\trsp \right]^\trsp $. As the name implies, unit quaternions have unitary norm, i.e., $ v^2+\mb{u}^\trsp\mb{u} = \epsilon_0^2 + \epsilon_1^2 + \epsilon_2^2 + \epsilon_3^2 =1 $. 

\subsubsection{Composition of unit quaternions}

Similarly to the product between rotation matrices, the quaternion product is in general non-commutative. It is given by
\begin{equation}
\mb{\epsilon}_1 * \mb{\epsilon}_2 =
\begin{bmatrix}
v_1v_2 - \mb{u}^\trsp_1\mb{u}_2 \\
v_1\mb{u}_2 + v_2\mb{u}_1 + \mb{u}_1\times\mb{u}_2
\end{bmatrix}\!\!,
\label{eq:quaternionProd}
\end{equation}
and it can be interpreted as a rotation operator: it rotates the frame whose orientation is described by $\mb{\epsilon}_2$ by the rotation defined by $\mb{\epsilon}_1$. Moreover, the quaternion product $ \mb{\epsilon}_1 * \mb{\bar{\epsilon}}_2 $ yields the quaternion that rotates $ \mb{\epsilon}_2 $ into $ \mb{\epsilon}_1 $.

\subsubsection{Quaternion matrix}

The product between two quaternions $\mb{\alpha} = \left[\alpha_0 \> \alpha_1 \> \alpha_2 \> \alpha_3 \right]^\trsp$ and $\mb{\beta} = \left[\beta_0 \> \beta_1 \> \beta_2 \> \beta_3 \right]^\trsp$ can also be written in matrix form by resorting to Hamilton operators (quaternion matrices):
\begin{equation}
\mb{\alpha}*\mb{\beta} = \overset{+}{\mb{H}}(\mb{\alpha})\mb{\beta} = \bar{\mb{H}}(\mb{\beta})\mb{\alpha},
\label{eq:quatProdComut}
\end{equation}
with Hamilton operators $\overset{+}{\mb{H}}$, $\bar{\mb{H}}$ defined by (see also \cite{AdornoThesis})
\begin{equation}
\overset{+}{\mb{H}}(\mb{\alpha}) \! = \!\! \left[\begin{smallmatrix}
\alpha_0 & -\alpha_1 & -\alpha_2 & -\alpha_3 \\
\alpha_1 &  \alpha_0 & -\alpha_3 &  \alpha_2 \\
\alpha_2 &  \alpha_3 &  \alpha_0 & -\alpha_1 \\
\alpha_3 & -\alpha_2 &  \alpha_1 &  \alpha_0 \\
\end{smallmatrix} \right]\!\!,
\bar{\mb{H}}(\mb{\beta}) \! = \!\! \left[ \begin{smallmatrix}
\beta_0 & -\beta_1 & -\beta_2 & -\beta_3 \\
\beta_1 &  \beta_0 &  \beta_3 & -\beta_2 \\
\beta_2 & -\beta_3 &  \beta_0 &  \beta_1 \\
\beta_3 &  \beta_2 & -\beta_1 &  \beta_0 \\
\end{smallmatrix} \right]\!\!.
\label{eq:Hamiltons}
\end{equation}

Notice the commutativity between $\overset{+}{\mb{H}}$ and $\bar{\mb{H}}$ in \eqref{eq:quatProdComut}. Even though the quaternion product is not commutative,  Hamilton operators commute between them. This result is useful when we want to change the order of the quaternions being multiplied without affecting the resulting orientation.


\section{Learning configuration space and priority constraints}
\label{sec:combiningConstraints}

The previous section considered the case in which the task parameters were related to object poses in operational space. However, TP-GMM is not limited to this parameterization and can be applied to other forms of locally linear transformations or projections. We will now show that the consideration of non-square $\mb{A}_{t,j}$ matrices allows for the combination of soft constraints in both configuration and operational spaces (through Jacobian operators), see also \cite{Calinon09AR} for a preliminary work in this direction. 

In addition, priority constraints can be learned and encoded within a TP-GMM from an initial set of task hierarchies given as potential candidates to describe the observed skill. The probabilistic encoding is here exploited to discover, from a statistical analysis of the demonstrations, in which manner subtasks are prioritized. 
The different frames correspond in this case to various candidate subspace projections of the movement, with statistical information extracted from the different projections. 
Note that the problem of learning and controlling a robot by considering task priorities can be tackled both at kinematic or dynamic levels. We will define it here at a kinematic level with a robot controlled in joint velocity, but the presented techniques also apply to torque controllers.


\subsection{Learning soft constraints in both configuration and operational spaces}
\label{subsec:combConstraints}

We first recall that forward and differential kinematics are computed with
\begin{equation}
	\!\mb{x}_t \!=\! f(\mb{q}_t)
	\!\iff\!
	\mb{\dot{x}}_t \!=\! \frac{\partial\mb{x}_t}{\partial t}
	= \frac{\partial f(\mb{q}_t)}{\partial\mb{q}_t} \; \frac{\partial\mb{q}_t}{\partial t} =
	\mb{J}(\mb{q}_t) \; \mb{\dot{q}}_t,
	\label{eq:FKJacobianDef}
\end{equation}
where $\mb{x}_t$, $\mb{\dot{x}}_t$ represent a link pose and velocity (typically the end-effector), $\mb{q}_t = \left[q_{t,1} \>\hdots\> q_{t,N_q}\right]^\trsp$, $\mb{\dot{q}}_t$ are the vectors of joint positions and velocities for a robot with $N_q$ DOFs and $\mb{J}(\mb{q}_t)$ is a Jacobian matrix.

Inverse kinematics is most commonly achieved by minimizing the $L_2$-norm of the residuals
\begin{equation}
	\mb{\hat{\dot{\mb{q}}}} = \arg\underset{\mb{\dot{q}}}{\min} {\|\mb{\dot{x}}-\mb{J}\mb{\dot{q}}\|}_2 = \arg\underset{\mb{\dot{q}}}{\min} {(\mb{\dot{x}}-\mb{J}\mb{\dot{q}})}^\trsp\!(\mb{\dot{x}}-\mb{J}\mb{\dot{q}}),
	\label{eq:IKoptimization}
\end{equation}
by dropping the dependencies on $\mb{q}$ and $t$ in the notation. The general least norm inverse kinematics solution is  given by
\begin{equation}
	\mb{\hat{\dot{\mb{q}}}} = \mb{J}^{\dagger} \mb{\dot{x}} + \mb{N} \mb{\dot{q}}_0
	\label{eq:generalIKsol}
\end{equation}
with $\mb{J}^{\dagger}=\mb{J}^\trsp {\big(\mb{J}\mb{J}^\trsp\big)}^{-1}$ the right pseudoinverse of the Jacobian, $\mb{N} = \mb{I} - \mb{J}^{\dagger} \mb{J}$ the null-space projection operator, and $\mb{\dot{q}}_0$ a secondary objective that will not affect the principal $L_2$-norm minimization objective.

In operational space control, as tasks can be defined at the level of position and orientation, the Jacobian matrix is typically comprised of two block matrices
\begin{equation}
	\mb{\dot{x}}= \mb{J} \>\mb{\dot{q}}
	\iff
	\left[\begin{matrix} \mb{\dot{p}} \\ \mb{\omega}	\end{matrix}\right] = \left[\begin{matrix} \mb{J_p} \\ \mb{J_o} \end{matrix} \right] \mb{\dot{q}},
	\label{eq:Jacobian}
\end{equation}
where $\mb{J}\in\mathbb{R}^{6\times N_q}$ is the geometric Jacobia
, consisting of two sub-matrices $\mb{J_p}\in \mathbb{R}^{3\times N_q}$, $\mb{J_o}\in \mathbb{R}^{3\times N_q}$ which map joint velocities to the end-effector linear and angular velocities, $\mb{\dot{p}}\in \mathbb{R}^3$, $\mb{\omega}\in \mathbb{R}^3$.

The TP-GMM representation can be extended to task parameters $\{\mb{b}_{t,j},\mb{A}_{t,j}\}_{j=1}^P$ with different dimensionality for each candidate frame $j$ and non-square $\mb{A}_{t,j}$ matrices.
In particular, we exploit the kinematic relationships given by \eqref{eq:IKoptimization}, \eqref{eq:generalIKsol} and \eqref{eq:Jacobian} to formulate the task parameters in a way that operational space constraints are projected on configuration space, where Gaussian products are computed.

Table \ref{tab:AbOrient} gives a summary of how these operators are defined for different kinds of constraints. For completeness, we derive in Appendix \ref{app:TPsoftConstraints} some of those operators, with a particular attention on orientation.

\begin{table*
	\begin{mdframed}
		\vspace{0.3cm}
		\caption{Summary of task parameters for candidate projection operators.}
		\label{tab:AbOrient}
		\centering
		\vspace{-0.3cm}
		\begin{minipage}{17.5cm}
			\begin{align}
			\!\!\!\!\!\!\!\!
			&\begin{matrix} \text{\textit{Configuration space constraints}}\\ \text{\textit{ in a fixed frame:}}
			\end{matrix}
			\!\!\! &\mb{\hat{q}}^{(j)}_{t,i} &=
			\mb{I} 
			& \!\!\!\!\!\!\mb{\mu}^{(j)}_i &+ 
			\mb{0} 
			\label{eq:Ab_qTable}\\
			%
			\!\!\!\!\!\!\!\!
			&\begin{matrix} \text{\textit{Absolute position/orientation}} \\ \text{\textit{constraints:}}
			\end{matrix}
			\!\!\! &\mb{\hat{q}}^{(j)}_{t,i} &=
			\mb{J}^{\dagger}\!(\mb{q}_{t-1})
			\left[\begin{matrix}
			\>\> \mb{I} & \mb{0} \\ 
			\>\> \mb{0} & \bar{\mb{H}}^*(\mb{\bar{\epsilon}}_{t-1})
			\end{matrix} \right]
			& \!\!\!\!\!\!\mb{\mu}^{(j)}_i & -
			\mb{J}^{\dagger}\!(\mb{q}_{t-1})
			\left[\begin{matrix}
			\mb{x}_{t-1} \\
			\mb{0}	\end{matrix} \right]
			+ \mb{q}_{t-1}
			\label{eq:Ab_xTable}\\
			%
			\!\!\!\!\!\!\!\!
			&\begin{matrix} \text{\textit{Relative position/orientation}} \\ \text{\textit{constraints:}}
			\end{matrix}
			\!\!\! &\mb{\hat{q}}^{(j)}_{t,i} &=
			\mb{J}^{\dagger}\!(\mb{q}_{t-1})
			\left[\begin{matrix}
			\>\> \mb{R}^\ty{O}_t & \mb{0} \\
			\>\> \mb{0} & \bar{\mb{H}}^*(\mb{\bar{\epsilon}}_{t-1}) \>\overset{+}{\mb{H}}(\mb{\epsilon}^{\ty{O}}_t) 
			\end{matrix} \right]
			& \!\!\!\!\!\!\mb{\mu}^{(j)}_i &+
			\mb{J}^{\dagger}\!(\mb{q}_{t-1})
			\left[\begin{matrix}
			\mb{x}^\ty{O}_t - \mb{x}_{t-1} \\
			\mb{0}
			\end{matrix} \right] +
			\mb{q}_{t-1}
			\label{eq:Ab_xoTable}\\
			%
			\!\!\!\!\!\!\!\!
			&\begin{matrix}
			\text{\textit{Null-space constraints in}} \\ \text{\textit{configuration space:}}
			\end{matrix}
			\!\!\! &\mb{\hat{q}}^{(j)}_{t,i} &=
			\mb{N}\!(\mb{q}_{t-1})
			& \!\!\!\!\!\!\mb{\mu}^{(j)}_i &+
			\mb{J}^{\dagger}\!(\mb{q}_{t-1}) \mb{J}\!(\mb{q}_{t-1}) \mb{q}_{t-1} 
			\label{eq:Ab_NqTable}\\
			%
			\!\!\!\!\!\!\!\!
			&\begin{matrix} \text{\textit{Absolute position/orientation}} \\ \text{\textit{null-space constraints:}}
			\end{matrix}
			\!\!\! &\mb{\hat{q}}^{(j)}_{t,i} &=
			\mb{N}\!(\mb{q}_{t-1}) \mb{\tilde{J}}^{\;{\dagger}}\!(\mb{q}_{t-1})
			\left[\begin{matrix}
			\>\> \mb{I} & \mb{0} \\ 
			\>\> \mb{0} & \bar{\mb{H}}^*(\mb{\bar{\epsilon}}_{t-1})
			\end{matrix} \right]
			& \!\!\!\!\!\!\mb{\mu}^{(j)}_i &-
			\mb{N}\!(\mb{q}_{t-1}) \mb{\tilde{J}}^{\;{\dagger}}\!\!(\mb{q}_{t-1})
			\left[\begin{matrix}
			\mb{x}_{t-1} \\
			\mb{0}
			\end{matrix} \right] +
			\mb{q}_{t-1} 
			\label{eq:Ab_NxTable}\\
			%
			\!\!\!\!\!\!\!\!
			&\begin{matrix} \text{\textit{Relative position/orientation}} \\ \text{\textit{null-space constraints:}}
			\end{matrix}
			\!\!\! &\mb{\hat{q}}^{(j)}_{t,i} &=
			\underbrace{
				\mb{N}\!(\mb{q}_{t-1}) \mb{\tilde{J}}^{\;{\dagger}}\!(\mb{q}_{t-1})
				\left[\begin{matrix}
				\>\> \mb{R}^\ty{O}_t & \mb{0} \\
				\>\> \mb{0} & \bar{\mb{H}}^*(\mb{\bar{\epsilon}}_{t-1}) \>\overset{+}{\mb{H}}(\mb{\epsilon}^{\ty{O}}_t) 
				\end{matrix} \right]
			}_{\mb{A}_{t,j}}  
			& \!\!\!\!\!\!\mb{\mu}^{(j)}_i &+
			\underbrace{
				\mb{N}\!(\mb{q}_{t-1}) \mb{\tilde{J}}^{\;{\dagger}}\!\!(\mb{q}_{t-1}) 
				\left[\begin{matrix}
				\mb{x}^\ty{O}_t \!-\! \mb{x}_{t-1}\\
				\mb{0}
				\end{matrix} \right]+
				\mb{q}_{t-1}
			}_{\mb{b}_{t,j}}
			\label{eq:Ab_NxobjTable}
			\end{align}
		\end{minipage}
		\vspace{-0.15cm}
	\end{mdframed}
	\vspace{-0.5cm}
\end{table*}


\subsection{Learning priority constraints}
\label{subsec:priorityConstraints}
TP-GMM provides a principled way to learn and synthesize task priorities from a probabilistic representation of strict candidate hierarchies. We argue that the operational space contains incomplete information for the recognition of tasks that are being executed in parallel, and that the extraction of task constraints can be enriched by combining observations in operational and configuration spaces. This situation is illustrated in Figure \ref{fig:hierarchyConstraints}. In this example, we consider a planar robot that tracks the position and orientation of an object with its end-effector. The top-right graph shows the observed tracking errors in position and orientation spaces collected from four demonstrations. The two observation sets, A and B, show two different interpretations of the error data. Observation set A shows that the dataset could have been generated by a demonstrator who was carefully tracking the orientation of the object, while neglecting the tracking of the position, resulting in poor accuracy in position tracking. Observation set B shows a different interpretation of the same data. It shows that the variability in position tracking was the result of the object being too far away for the robot to track both position and orientation. This illustrative example shows that, by observing the configuration of the robot, we can interpret the error data as an equally accurate tracking of position and orientation with a priority on orientation. Intuitively, we can observe, from the configuration of the robot, that it actually tracks the position as effectively as it can, given the priority constraint. The proposed approach formalizes this problem by representing priority constraints as candidate projections in a probabilistic task-parameterized model. 


\begin{figure}
	\centering
	\includegraphics[width=\columnwidth]{figures/hierarchyConstraints01vertical_compact_legend}
	\caption{Importance of the configuration space when observing a movement for the automatic extraction of hierarchy constraints. A planar robot tracks the position and orientation of an object. The top-right graph shows the tracking errors. Observation sets A and B show different options for the underlying source of the tracking errors: neglecting position tracking (A), and tracking orientation as a priority with position as a secondary goal (B).
	}
	\label{fig:hierarchyConstraints}
\end{figure}


Let us consider again the planar robot with position and orientation tasks of Fig.\ \ref{fig:hierarchyConstraints}. If the two tasks in operational space are defined by desired Cartesian velocities $\mb{\dot{x}}_1$ and $\mb{\dot{x}}_2$, \eqref{eq:generalIKsol} can be exploited to explicitly define a strict priority of one task over the other when computing inverse kinematics, i.e.,
\begin{equation}
\mb{\hat{\dot{\mb{q}}}} 
= \mb{J}_{\!1}^{\dagger} \; \mb{\dot{x}}_1 + \mb{N}_{\!1} \mb{J}_{\!2}^{\dagger} \; \mb{\dot{x}}_2 \\
= \left[\mb{J}_{\!1}^{\dagger} \quad \mb{N}_{\!1}\mb{J}_{\!2}^{\dagger} \right] 
\left[\begin{matrix} \mb{\dot{x}}_1 \\ \mb{\dot{x}}_2 \end{matrix}\right].
\label{eq:strictPriorityx1}
\end{equation}

The resulting velocity in operational space is given by
\begin{align}
\left[\begin{matrix} \mb{\hat{\dot{x}}}_1 \\ \mb{\hat{\dot{x}}}_2 \end{matrix}\right] 
&= \left[\begin{matrix} \mb{J}_{\!1} \\ \mb{J}_{\!2} \end{matrix}\right] \mb{\hat{\dot{\mb{q}}}} = \left[\begin{matrix} \mb{J}_{\!1} \\ \mb{J}_{\!2} \end{matrix}\right]
\left[\mb{J}_{\!1}^{\dagger} \quad \mb{N}_{\!1}\mb{J}_{\!2}^{\dagger} \right] 
\left[\begin{matrix} \mb{\dot{x}}_1 \\ \mb{\dot{x}}_2 \end{matrix}\right] \nonumber\\
&\approx \left[\begin{matrix} \mb{I} & \mb{0} \\
\mb{J}_{\!2}\mb{J}_{\!1}^{\dagger} & \mb{J}_{\!2}\mb{N}_{\!1}\mb{J}_{\!2}^{\dagger}
\end{matrix}
\right] 
\left[\begin{matrix} \mb{\dot{x}}_1 \\ \mb{\dot{x}}_2 \end{matrix}\right],
\label{eq:Task1HierarchyOp}
\end{align}
where the final expression comes from the definition of the pseudoinverse and null-space projection matrices.
Similarly, if
\begin{equation}
\mb{\hat{\dot{\mb{q}}}} 
= \mb{J}_{\!2}^{\dagger} \; \mb{\dot{x}}_2 + \mb{N}_{\!2} \mb{J}_{\!1}^{\dagger} \; \mb{\dot{x}}_1 \\
= \left[\mb{N}_{\!2}\mb{J}_{\!1}^{\dagger} \quad \mb{J}_{\!2}^{\dagger}\right] 
\left[\begin{matrix} \mb{\dot{x}}_1 \\ \mb{\dot{x}}_2 \end{matrix}\right],
\label{eq:strictPriorityx2}
\end{equation}
the resulting velocity in operational space is given by
\begin{equation}
\left[\begin{matrix} \mb{\hat{\dot{x}}}_1 \\ \mb{\hat{\dot{x}}}_2 \end{matrix}\right] 
\approx \left[\begin{matrix} \mb{J}_{\!1}\mb{N}_{\!2}\mb{J}_{\!1}^{\dagger} & \mb{J}_{\!1}\mb{J}_{\!2}^{\dagger} \\
\mb{0} & \mb{I}
\end{matrix}
\right] 
\left[\begin{matrix} \mb{\dot{x}}_1 \\ \mb{\dot{x}}_2 \end{matrix}\right].
\label{eq:Task2HierarchyOp}
\end{equation}


\begin{figure}
	\begin{subfigure}{0.47\columnwidth}
		\centering
		\includegraphics[width=1.0\columnwidth]{figures/toyExample_priorityOrient.pdf}
		\caption{}
		\label{fig:hierarchyConstraints2Orient}
	\end{subfigure}%
	\hspace{4mm}
	\begin{subfigure}{0.47\columnwidth}
		\centering
		\vspace{0.4mm}
		\includegraphics[width=0.835\columnwidth]{figures/toyExample_priorityPos.pdf}
		\caption{}
		\label{fig:hierarchyConstraints2Pos}
	\end{subfigure}
	\vspace{0.2cm}
	\caption{Projection of task space errors by considering two different candidate hierarchies. \textbf{(a)} Demonstrated priority on orientation (Fig. \ref{fig:hierarchyConstraints}, Observation set B) \textbf{(b)} Demonstrated priority on position (Fig. \ref{fig:hierarchyConstraints3}).}
	\label{fig:hierarchyConstraints2}
\end{figure}

\begin{figure}
	\centering

	\includegraphics[width=.41\textwidth]{figures/demoIK_hierarchyExtraction02e_compact}

	\caption{Evaluation of candidate hierarchies for a new dataset: priority on position tracking with secondary task of tracking orientation.}
	\label{fig:hierarchyConstraints3}
\end{figure}

For a controller generating operational space movements $\left[ \mb{\dot{x}}^\trsp_1 \quad \mb{\dot{x}}^\trsp_2\right]^\trsp$, the projection operators in \eqref{eq:Task1HierarchyOp} and \eqref{eq:Task2HierarchyOp} can be used to evaluate which hierarchy is responsible for the observed movement.
We apply operators \eqref{eq:Task1HierarchyOp} and \eqref{eq:Task2HierarchyOp} to the dataset obtained in Fig.\ \ref{fig:hierarchyConstraints} (top-right), which corresponds to the  position and orientation tracking errors in four demonstrations, $e_y$ and $e_o$. Figure \ref{fig:hierarchyConstraints2Orient} shows the projected datapoints, with the corresponding variability, for the two candidate hierarchies. We observe that the projected data exhibits low variability for the hierarchy that has orientation as a priority over position, indicating that the data was generated by a controller whose main priority was orientation tracking. Figure \ref{fig:hierarchyConstraints2Pos} shows that we can reach a similar observation for a new dataset with priority on position and orientation as a secondary goal (Fig.\ \ref{fig:hierarchyConstraints3}).

In this simple example, the analysis of the variance of the projected data provides a way of assigning importance to each candidate hierarchy. The same approach can be employed to learn the orchestration of structures in more complex ways, with changes of hierarchies along the task and with richer parallel organization of task structures. We take advantage of this model to compute full precision matrices that are associated with each candidate projection operator. It consequently follows that new movements can be synthesized through a \textit{soft weighting of strict hierarchies}, a combination of two approaches that were previously used independently. We assume here that the candidate task structures are given and that the kinematic model of the robot is known, i.e., that the Jacobian of each task is available.  


\subsection{Soft weighting of strict hierarchies}
\label{subsec:softWeighting}
If we redefine the optimization problem \eqref{eq:IKoptimization} as
\begin{equation*}
\mb{\hat{\dot{\mb{q}}}}  
= \arg\underset{\mb{\dot{q}}}{\min}\; {(\mb{\hat{\dot{x}}}-\mb{J}\mb{\dot{q}})}^\trsp \mb{W} (\mb{\hat{\dot{x}}}-\mb{J}\mb{\dot{q}}),
\end{equation*}
the inverse kinematics solution is given by the weighted pseudoinverse
\begin{equation*}
\mb{\hat{\dot{\mb{q}}}}
=
{(\mb{J}^\trsp \mb{W}\mb{J})}^{-1}\mb{J}^\trsp \mb{W} \, \mb{\hat{\dot{x}}}  .
\end{equation*}
By assigning two importance weights $w_1$ and $w_2$ to the two tasks $\mb{\dot{x}}_1$ and $\mb{\dot{x}}_2$, the above formulation can be used to compute a solution that corresponds to a soft weighting of the tasks, as opposed to a strict hierarchy such as \eqref{eq:strictPriorityx1}, \eqref{eq:strictPriorityx2}. Namely:
\begin{equation*}
\mb{\hat{\dot{\mb{q}}}}
=
{\left({\left[\begin{matrix} \mb{J}_{\!1} \\ \mb{J}_{\!2} \end{matrix}\right]}^\trsp\!\! 
	\left[\begin{matrix} w_1\mb{I} & \mb{0} \\ \mb{0} & w_2\mb{I} \end{matrix}\right]\!\!\!
	\left[\begin{matrix} \mb{J}_{\!1} \\ \mb{J}_{\!2} \end{matrix}\right]\right)}^{\!\!-1}
{\left[\begin{matrix} \mb{J}_{\!1} \\ \mb{J}_{\!2} \end{matrix}\right]}^\trsp \!\!
\left[\begin{matrix} w_1\mb{I} & \mb{0} \\ \mb{0} & w_2\mb{I} \end{matrix}\right]\!\!\! 
\left[\begin{matrix} \mb{\dot{x}}_1 \\ \mb{\dot{x}}_2 \end{matrix}\right] .
\end{equation*}

Similarly, with precision matrices $\mb{\Gamma}_1$ and $\mb{\Gamma}_2$, we get
\begin{align*}
\mb{\hat{\dot{\mb{q}}}}
&=
{\left({\left[\begin{matrix} \mb{J}_{\!1} \\ \mb{J}_{\!2} \end{matrix}\right]}^\trsp 
	\left[\begin{matrix} \mb{\Gamma}_1 & \mb{0} \\ \mb{0} & \mb{\Gamma}_2 \end{matrix}\right]
	\left[\begin{matrix} \mb{J}_{\!1} \\ \mb{J}_{\!2} \end{matrix}\right]\right)}^{\!\!-1}
{\left[\begin{matrix} \mb{J}_{\!1} \\ \mb{J}_{\!2} \end{matrix}\right]}^\trsp 
\left[\begin{matrix} \mb{\Gamma}_1 & \mb{0} \\ \mb{0} & \mb{\Gamma}_2 \end{matrix}\right] 
\left[\begin{matrix} \mb{\dot{x}}_1 \\ \mb{\dot{x}}_2 \end{matrix}\right] \\
&=
{\left(\mb{J}_{\!1}^\trsp\mb{\Gamma}_1\mb{J}_{\!1} + \mb{J}_{\!2}^\trsp\mb{\Gamma}_2\mb{J}_2\right)}^{-1}
\left(\mb{J}_{\!1}^\trsp\mb{\Gamma}_1\mb{\dot{x}}_1 + \mb{J}_{\!2}^\trsp\mb{\Gamma}_2\mb{\dot{x}}_2\right),
\end{align*}
which is analogous to a product of linearly transformed Gaussians with centers $\mb{\dot{x}}_1$ and $\mb{\dot{x}}_2$, precision matrices $\mb{\Gamma}_1$ and $\mb{\Gamma}_2$, and projection operators $\mb{J}_{\!1}^\trsp$ and $\mb{J}_{\!2}^\trsp$.

The most common approaches for controlling tasks of different priorities are either based on strict hierarchies \cite{Wrede13,Saveriano15,Towell10,Hak12b}, or soft weighting of tasks \cite{Moro13,Dehio15,Modugno16,Lober15}. We propose a richer alternative based on a \textit{soft weighting of strict hierarchies}, gathering the best of the two approaches. 
For the two task case, we define the two candidate hierarchies
\begin{equation}
\mb{A}_1 = \left[\mb{J}_1^{\dagger} \quad \mb{N}_1\mb{J}_2^{\dagger} \right] 
,\quad
\mb{A}_2 = \left[\mb{N}_2\mb{J}_1^{\dagger} \quad \mb{J}_2^{\dagger} \right]
,
\label{eq:TaskParametersNoObject}
\end{equation}
representing respectively a priority of $\mb{\dot{x}}_1$ over $\mb{\dot{x}}_2$, and a priority of $\mb{\dot{x}}_2$ over $\mb{\dot{x}}_1$. 
By using a task-parameterized representatio
, a statistical analysis of the observed data is achieved with the candidate projections $\left[\mb{J}^\trsp_1 \>\> \mb{J}^\trsp_2\right]^\trsp\!\!\mb{A}_1$ and $\left[\mb{J}^\trsp_1 \>\> \mb{J}^\trsp_2\right]^\trsp\!\!\mb{A}_2$, as in \eqref{eq:Task1HierarchyOp} and \eqref{eq:Task2HierarchyOp}, providing precision matrices
in operational space that are learned from observations. 
These can then be used to generate behaviors in new configurations, with a weighting of candidate hierarchies similar to the demonstrations. If we consider two Gaussians, with centers $\mb{\mu}^{(1)}$, $\mb{\mu}^{(2)}$ and precision matrices $\mb{\tilde{\Gamma}}_1$ and $\mb{\tilde{\Gamma}}_2$ (inverses of the covariance matrices), estimated from observations of $\mb{\dot{x}}^{(1)} = \left[\mb{\dot{x}}^{(1)\;\trsp}_1 \>\> \mb{\dot{x}}^{(1)\;\trsp}_2\right]^\trsp$ and $\mb{\dot{x}}^{(2)} = \left[\mb{\dot{x}}^{(2)\;\trsp}_1 \>\> \mb{\dot{x}}^{(2)\;\trsp}_2\right]^\trsp$ (superscripts denote candidate hierarchies, subscripts denote task velocities as in \eqref{eq:Task1HierarchyOp},\eqref{eq:Task2HierarchyOp}), this is achieved by considering the controller
\begin{equation}
\mb{\hat{\dot{\mb{q}}}}
=
\!{\left(\!\mb{A}_1\mb{\tilde{\Gamma}}_1\mb{A}^\trsp_1 \!+\! \mb{A}_2\mb{\tilde{\Gamma}}_2\mb{A}_2^\trsp\right)}^{\!\!-1}
\!\!\!\left(\!\mb{A}_1\mb{\tilde{\Gamma}}_1\mb{\mu}^{(1)} \!+\! \mb{A}_2\mb{\tilde{\Gamma}}_2\mb{\mu}^{(2)}\right)\!.
\label{eq:controllerWeightingStrictHierarchies}
\end{equation}


This can be used co-jointly with a standard definition of task parameters as object-related movement. For two objects with positions $\mb{x}^\ty{O}_1, \mb{x}^\ty{O}_2$, this is achieved by defining
\begin{align}
\mb{A}_1 = \left[\mb{J}_1^{\dagger} \quad \mb{N}_1\mb{J}_2^{\dagger} \right] 
,&\>\>
\mb{b}_1 = \mb{A}_1 \left[\begin{matrix} \mb{x}^\ty{O}_1 - \mb{x}_1 \\ \mb{x}^\ty{O}_2 - \mb{x}_2 \end{matrix}\right]
,\nonumber\\
\mb{A}_2 = \left[\mb{N}_2\mb{J}_1^{\dagger} \quad \mb{J}_2^{\dagger} \right]
,&\>\>
\mb{b}_2 = \mb{A}_2 \left[\begin{matrix} \mb{x}^\ty{O}_1 - \mb{x}_1 \\ \mb{x}^\ty{O}_2 - \mb{x}_2 \end{matrix}\right].
\label{eq:TaskParametersWithObjectTracking}
\end{align}
\subsection{The general case of $N_T$ tasks}
\label{subsec:generalizationNTasks}
The approach can be generalized to the case of $N_T$ arbitrary tasks, framed in the context of TP-GMM. If we consider $N_T$ tasks and $j=1,\hdots,P$ candidate hierarchies, represented by $\mb{A}_j$, \eqref{eq:Task1HierarchyOp} and \eqref{eq:Task2HierarchyOp} are generalized to
\begin{align}
\left[\begin{matrix} \mb{\hat{\dot{x}}}_1 \\ \vdots \\ \mb{\hat{\dot{x}}}_{N_T} \end{matrix}\right] 
&= \left[\begin{matrix} \mb{J}_{\!1} \\ \vdots \\ \mb{J}_{\!N_T} \end{matrix}\right]
\mb{A}_j 
\left[\begin{matrix} \mb{\dot{x}}_1 \\ \vdots \\\mb{\dot{x}}_{N_T} \end{matrix}\right].
\label{eq:GeneralizedProjection}
\end{align}

Note that one is not required to provide all possible combinations of these matrices for the $N_T$ tasks. The candidate hierarchies can be seen as prior information given by the user, equivalently to the coordinate systems given to TP-GMM in the case of objects of potential interest for the task. In the same way that some objects might be known to be unimportant, some hierarchies may also be known to be irrelevant beforehand.
If we use $\mb{\bar{J}}_t=\left[\mb{J}^\trsp_{t,1} \> \hdots \> \mb{J}^\trsp_{t,N_T} \right]^\trsp$ and consider the datapoint $\mb{\xi}_t = \left[\mb{\dot{x}}^\trsp_{t,1} \> \hdots \> \mb{\dot{x}}^\trsp_{t,N_T} \right]^\trsp$, the mapping of the data to a frame $j$, that represents a candidate hierarchy, is obtained according to
\begin{equation}
\mb{\xi}^{(j)}_t = \mb{\bar{J}}_t\mb{A}_{t,j} \mb{\xi}_t
\label{eq:GeneralizedProjectionFrames}\>.
\end{equation}

A TP-GMM can then be estimated using the same EM algorithm as described in Section \ref{sec:LearningTPMovements}. Movement synthesis is performed through Gaussian product, which is a generalization of \eqref{eq:controllerWeightingStrictHierarchies} for the case of $P$ candidate hierarchies.
Note that, in this type of application, there is a decoupling between the task-parameters that are used for projecting data in the local frames of TP-GMM and those that are used for reproduction. Indeed, the choice of the task-parameters is centered on the desired behavior during reproduction, with the options of choosing those of the type \eqref{eq:TaskParametersNoObject} or \eqref{eq:TaskParametersWithObjectTracking}, while the projection of data is always performed according to \eqref{eq:GeneralizedProjectionFrames} (regardless of the value of $\mb{b}_j$). This is related to the fact that one now uses TP-GMM to encode operational space velocities instead of poses.


