Literature
Display
← All papers

Learning Task Priorities from Demonstrations

João Silvério · Sylvain Calinon · Leonel Rozo · Darwin G. Caldwell

· IEEE Transactions on Robotics ↗

Robotics7 min text · CondensedOriginal paper ↗
Contents

At a glance

The paper extends TP-GMM to learn two kinds of structure: when joint-space or task-space constraints matter, and which supplied priority hierarchy best explains demonstrations. Jacobian and null-space operators bring candidate behaviors into a common joint-space representation.

Experiments include COMAN bottle shaking and bimanual reaching, simulated Centauro priorities, and transfer to WALK-MAN. The method needs known kinematics, task set-points, and candidate hierarchies. Fusing hierarchies allows smooth trade-offs but does not guarantee every conflicting constraint.

Introduction

Dual-arm robots share joints, so simultaneous tasks can compete. End-effector goals alone may also miss important joint-level patterns, such as shoulder oscillation during shaking. The method uses demonstrated variability to identify the relevant space or priority structure instead of requiring fixed hand-tuned scalar weights.

Refer to caption
Refer to caption
Fig. 1: The COMAN robot prioritizes the tracking of the left (resp. right) target, while doing its best to track the right (resp. left) one. This priority behavior was learned from demonstrations, using the approach proposed in Section V. Top: Reproduction with the model trained on left arm priority demonstrations. Bottom: Reproduction with the model trained on right arm priority demonstrations.

Related Work

Related work distinguishes learning bimanual motion, balancing task and joint spaces, and discovering priorities.

Learning bimanual skills

Many bimanual imitation methods encode coupled end-effector trajectories or forces. This captures operational-space coordination but can miss joint-space movement patterns and conflicts over shared joints.

Simultaneous learning of operational and configuration space constraints

Full covariance or precision matrices provide richer task weighting than scalars by expressing directional importance and correlations. The proposed Jacobian operators let the same probabilistic framework combine position, orientation, and configuration constraints.

Learning task prioritization

Strict hierarchies protect a high-priority task by projecting others into its null space. Soft weighting permits compromise and smoother transitions. The paper learns distributions over candidate strict-hierarchy solutions, then fuses those solutions.

1 Learning approaches based on strict hierarchies

Earlier methods learn null-space policies, constraints, or task stacks under various assumptions. Here demonstrations include task references and known kinematics, and the candidate hierarchy set is supplied in advance. Statistical consistency identifies the demonstrated structure; multiple Gaussian components can represent input-dependent priorities.

Learn hierarchies From demonstrations 2+ tasks Input dependent
Dehio et al. [16] – – ✓ –
Modugno et al. [17] – – ✓ ✓
Wrede et al. [28] – ✓ – –
Hak et al. [33] ✓ ✓ ✓ –
Saveriano et al. [29] – ✓ – ✓
Towell et al. [31] – ✓ – ✓
Lin et al. [32] ✓ ✓ – –
Lober et al. [34, 35] – ✓ – ✓
Paraschos et al. [36] – ✓ ✓ ✓
Our approach ✓ ✓ ✓ ✓
TABLE I: Contributions of our approach with respect to previous works on task priority learning.

2 Approaches related to soft weighting of tasks

Optimization-based weighting needs a fitness function, whereas demonstration-based methods infer importance from variation. Probabilistic prioritization of ProMPs learns task accuracy but does not identify the same strict-hierarchy candidates studied here. [36]

Task-Parameterized Gaussian Mixture Models

TP-GMM represents demonstrations from several task-dependent subspaces and recombines their local distributions when task parameters change.

Overview and nomenclature

Task parameters are affine operators mapping local Gaussian models into a common space. Earlier versions used object coordinate frames. This work broadens those operators to Jacobian projections and candidate task hierarchies. Table II defines the notation used throughout.

Variable / symbol Description
TP-GMM PP Number of task parameters
KK Number of Gaussian components
𝑨(j){\boldsymbol{A}}^{(j)}, 𝒃(j){\boldsymbol{b}}^{(j)} Task parameters j∈[1,P]j\in[1,P]
𝝁i(j){\boldsymbol{\mu}}^{(j)}_{i}, 𝚺i(j){\boldsymbol{\Sigma}}^{(j)}_{i} Mean and covariance of Gaussian i∈[1,K]{i\in[1,K]} for task parameter jj
πi\pi_{i} Mixing coefficient of Gaussian ii
MM Number of demonstrations
TmT_{m} Number of datapoints in demonstration m∈[1,M]m\in[1,M]
NN Total number of datapoints in a demonstration dataset
DD Dimension of datapoints
𝝃t∈ℝD{\boldsymbol{\xi}}_{t}\in\mathbb{R}^{D} Datapoint at time tt
𝝃∈ℝD×N{\boldsymbol{\xi}}\in\mathbb{R}^{D\times N} Demonstration dataset
Robot 𝒙L,𝒙R∈ℝ3{\boldsymbol{x}}_{L},{\boldsymbol{x}}_{R}\in\mathbb{R}^{3} Cartesian position of left and right end-effectors
ϵL,ϵR∈𝒮3{\boldsymbol{\epsilon}}_{L},{\boldsymbol{\epsilon}}_{R}\in\mathcal{S}^{3} Orientation of left and right end-effectors as unit quaternions
𝑹L,𝑹R∈SO⁡(3){\boldsymbol{R}}_{L},{\boldsymbol{R}}_{R}\in\mathrm{SO}(3) Orientation of left and right end-effectors as rotation matrices
NqN_{q} Number of robot joints
𝒒L,𝒒R∈ℝNq{\boldsymbol{q}}_{L},{\boldsymbol{q}}_{R}\in\mathbb{R}^{N_{q}} Joint angles of left and right arms
𝑱L,𝑱R∈ℝ6×Nq{\boldsymbol{J}}_{L},{\boldsymbol{J}}_{R}\in\mathbb{R}^{6\times N_{q}} Jacobian matrices of left and right arms
>> Priority operator (the task on the operator left has priority over the one on the right)
TABLE II: Summary of the notation.

Model estimation

Each demonstration is transformed into a local dataset for each candidate subspace. Expectation maximization fits shared mixture weights and local means and covariances. Variability in each subspace encodes how consistently that constraint was demonstrated.

Gaussian Mixture Regression

For a new situation, affine transformations map local Gaussian components into a common space. Their product combines means according to precision. GMR then conditions on an input such as time to generate a reference trajectory distribution. Task parameters may change during reproduction.

Equations 1–3
𝒩(𝝁^t,i,𝚺^t,i)∝∏j=1P𝒩(𝝁^t,i(j),𝚺^t,i(j)),with𝝁^(j)t,i=𝑨(j)t𝝁(j)i+𝒃(j)t,𝚺^(j)t,i=𝑨(j)t𝚺(j)i𝑨(j)𝖳t,\mathcal{N}\!\Big({\boldsymbol{\hat{\mu}}}_{t,i},{\boldsymbol{\hat{\Sigma}}}_{t,i}\Big)\;\propto\;\prod\limits_{j=1}^{P}\mathcal{N}\!\Big({\boldsymbol{\hat{\mu}}}{}_{t,i}^{(j)},\;{\boldsymbol{\hat{\Sigma}}}{}_{t,i}^{(j)}\Big),\;\mathrm{with}\\ {\boldsymbol{\hat{\mu}}}{}^{(j)}_{t,i}=\!{\boldsymbol{A}}^{(j)}_{t}{\boldsymbol{\mu}}^{(j)}_{i}\!+\!{\boldsymbol{b}}^{(j)}_{t}\;,\quad{\boldsymbol{\hat{\Sigma}}}{}^{(j)}_{t,i}=\!{\boldsymbol{A}}^{(j)}_{t}{\boldsymbol{\Sigma}}^{(j)}_{i}{\boldsymbol{A}}^{(j)^{\mathsf{T}}}_{t}, (1)
𝚺^t,i=(∑j=1P𝚺^−1t,i(j))−1,𝝁^t,i=𝚺^t,i∑j=1P𝚺^−1t,i(j)𝝁^.t,i(j){\boldsymbol{\hat{\Sigma}}}_{t,i}=\Big(\sum\limits_{j=1}^{P}{{\boldsymbol{\hat{\Sigma}}}{}^{(j)}_{t,i}}^{-1}\Big)^{-1},\quad{\boldsymbol{\hat{\mu}}}_{t,i}={\boldsymbol{\hat{\Sigma}}}_{t,i}\sum\limits_{j=1}^{P}{{\boldsymbol{\hat{\Sigma}}}{}^{(j)}_{t,i}}^{-1}{\boldsymbol{\hat{\mu}}}{}^{(j)}_{t,i}. (2)
𝑨t(j)=[𝑰𝟎𝟎𝑨t𝒪⁡(j)],𝒃t(j)=[𝟎𝒃t𝒪⁡(j)].{\boldsymbol{A}}^{(j)}_{t}=\begin{bmatrix}{\boldsymbol{I}}&{\boldsymbol{0}}\\ {\boldsymbol{0}}&{\boldsymbol{A}}^{{\scriptscriptstyle{\mathcal{O}}}(j)}_{t}\end{bmatrix}\;,\quad{\boldsymbol{b}}^{(j)}_{t}=\begin{bmatrix}{\boldsymbol{0}}\\ {\boldsymbol{b}}^{{\scriptscriptstyle{\mathcal{O}}}(j)}_{t}\end{bmatrix}. (3)

Learning operational and configuration space constraints simultaneously

Encoding demonstrations in both joint and operational spaces lets the model emphasize the space with more consistent demonstrated constraints at each phase. Jacobian-based affine maps bring task-space distributions into joint space before fusion.

Jacobian-based task parameters for position constraints

Locally integrated inverse differential kinematics expresses joint references as affine functions of desired Cartesian position. Gaussian transformation rules then project position means and covariances into joint space. Additional rotation and translation terms support positions specified relative to objects. The derivation assumes the relevant pseudoinverse is well defined or suitably regularized.

Equations 4–7
𝒒^t−𝒒t−1\displaystyle{\boldsymbol{\hat{q}}}_{t}-{\boldsymbol{q}}_{t-1} =𝑱t−1†​(𝒙t−𝒙t−1)\displaystyle={\boldsymbol{J}}_{t-1}^{\dagger}\left({\boldsymbol{x}}_{t}-{\boldsymbol{x}}_{t-1}\right)
⇔𝒒^t\displaystyle\iff{\boldsymbol{\hat{q}}}_{t} =𝑱t−1†​𝒙t+𝒒t−1−𝑱t−1†​𝒙t−1,\displaystyle={\boldsymbol{J}}_{t-1}^{\dagger}{\boldsymbol{x}}_{t}+{\boldsymbol{q}}_{t-1}-{\boldsymbol{J}}_{t-1}^{\dagger}{\boldsymbol{x}}_{t-1}, (4)
𝒒^(j)t,i=𝑱t−1†⏟𝑨t(j)𝝁(j)i+𝒒t−1−𝑱t−1†​𝒙t−1⏟𝒃t(j),∀i=1,…,K,{\boldsymbol{\hat{q}}}^{(j)}_{t,i}=\underbrace{{\boldsymbol{J}}_{t-1}^{\dagger}}_{{\boldsymbol{A}}^{(j)}_{t}}{\boldsymbol{\mu}}^{(j)}_{i}+\underbrace{{\boldsymbol{q}}_{t-1}-{\boldsymbol{J}}_{t-1}^{\dagger}{\boldsymbol{x}}_{t-1}}_{{\boldsymbol{b}}^{(j)}_{t}},\quad\forall i=1,\dots,K, (5)
𝚺^(j)t,i=𝑱t−1†⏟𝑨t(j)𝚺(j)i(𝑱t−1†)𝖳⏟𝑨t(j)𝖳,∀i=1,…,K,{\boldsymbol{\hat{\Sigma}}}^{(j)}_{t,i}=\underbrace{{\boldsymbol{J}}_{t-1}^{\dagger}}_{{\boldsymbol{A}}^{(j)}_{t}}{\boldsymbol{\Sigma}}^{(j)}_{i}\underbrace{({\boldsymbol{J}}_{t-1}^{\dagger})^{\mathsf{T}}}_{{\boldsymbol{A}}^{(j)^{\mathsf{T}}}_{t}},\quad\forall i=1,\dots,K, (6)
𝒒^t,i(j)=𝑱t−1†​𝑹t(j)⏟𝑨t(j)​𝝁i(j)+𝑱t−1†​(𝒑t(j)−𝒙t−1)+𝒒t−1⏟𝒃t(j),{\boldsymbol{\hat{q}}}^{(j)}_{t,i}=\underbrace{{\boldsymbol{J}}_{t-1}^{\dagger}{\boldsymbol{R}}^{(j)}_{t}}_{{\boldsymbol{A}}^{(j)}_{t}}{\boldsymbol{\mu}}^{(j)}_{i}+\underbrace{{\boldsymbol{J}}_{t-1}^{\dagger}\left({\boldsymbol{p}}^{(j)}_{t}-{\boldsymbol{x}}_{t-1}\right)+{\boldsymbol{q}}_{t-1}}_{{\boldsymbol{b}}^{(j)}_{t}}, (7)
TABLE III: Summary of task parameters as candidate projection operators
Configuration space constraints: 𝒒^t,i(j){\boldsymbol{\hat{q}}}^{(j)}_{t,i} =𝑰={\boldsymbol{I}} 𝝁i(j){\boldsymbol{\mu}}^{(j)}_{i} + 0+\>{\boldsymbol{0}}
Absolute position constraints: 𝒒^t,i(j){\boldsymbol{\hat{q}}}^{(j)}_{t,i} =𝑱t−1†={\boldsymbol{J}}^{\dagger}_{t-1} 𝝁i(j){\boldsymbol{\mu}}^{(j)}_{i} −𝑱t−1†​𝒙t−1+𝒒t−1-\>{\boldsymbol{J}}^{\dagger}_{t-1}{\boldsymbol{x}}_{t-1}+{\boldsymbol{q}}_{t-1}
Relative position constraints: 𝒒^t,i(j){\boldsymbol{\hat{q}}}^{(j)}_{t,i} =𝑱t−1†​𝑹t(j)={\boldsymbol{J}}^{\dagger}_{t-1}{\boldsymbol{R}}^{(j)}_{t} 𝝁i(j){\boldsymbol{\mu}}^{(j)}_{i} +𝑱t−1†​(𝒑t(j)−𝒙t−1)+𝒒t−1+\>{\boldsymbol{J}}^{\dagger}_{t-1}({\boldsymbol{p}}^{(j)}_{t}-{\boldsymbol{x}}_{t-1})+{\boldsymbol{q}}_{t-1}
Absolute orientation constraints: 𝒒^t,i(j){\boldsymbol{\hat{q}}}^{(j)}_{t,i} =𝑱t−1†​𝑯¯∗​(ϵ¯t−1)={\boldsymbol{J}}^{\dagger}_{t-1}\bar{{\boldsymbol{H}}}^{*}\!({\boldsymbol{\bar{\epsilon}}}_{t-1}) 𝝁i(j){\boldsymbol{\mu}}^{(j)}_{i} +𝒒t−1+\>{\boldsymbol{q}}_{t-1}
Relative orientation constraints: 𝒒^t,i(j){\boldsymbol{\hat{q}}}^{(j)}_{t,i} =𝑱t−1†​𝑯¯∗​(ϵ¯t−1)​𝑯+​(ϵt(j))⏟𝑨t(j)=\underbrace{{\boldsymbol{J}}^{\dagger}_{t-1}\bar{{\boldsymbol{H}}}^{*}\!({\boldsymbol{\bar{\epsilon}}}_{t-1})\overset{+}{{\boldsymbol{H}}}\!({\boldsymbol{\epsilon}}^{(j)}_{t})}_{{\boldsymbol{A}}^{(j)}_{t}} 𝝁i(j){\boldsymbol{\mu}}^{(j)}_{i} +𝒒t−1⏟𝒃t(j)+\underbrace{{\boldsymbol{q}}_{t-1}\hskip 78.24507pt}_{{\boldsymbol{b}}^{(j)}_{t}}

Task parameters for orientation

Unit-quaternion multiplication provides an analogous local linear map for orientation constraints. Combining this with the angular Jacobian projects orientation distributions into joint references. This is an approximation of quaternion geometry; the discussion proposes manifold-based formulations as a future refinement. Algorithm 1 preserves the complete learning and synthesis procedure.

Equations 8–12
𝝎t≈vec⁡(ϵt∗ϵ¯t−1)Δ​t{\boldsymbol{\omega}}_{t}\approx\frac{\mathrm{vec}({\boldsymbol{\epsilon}}_{t}*{\boldsymbol{\bar{\epsilon}}}_{t-1})}{\Delta t} (8)
𝒒˙^t=𝑱t−1†​𝑯¯∗​(ϵ¯t−1)​ϵt​1Δ​t,\displaystyle{\boldsymbol{\hat{\dot{{\boldsymbol{q}}}}}}_{t}={\boldsymbol{J}}^{\dagger}_{t-1}\bar{{\boldsymbol{H}}}^{*}({\boldsymbol{\bar{\epsilon}}}_{t-1})\>{\boldsymbol{\epsilon}}_{t}\frac{1}{\Delta t}\>, (9)
⇔\displaystyle\iff 𝒒^t=𝑱t−1†​𝑯¯∗​(ϵ¯t−1)​ϵt+𝒒t−1,\displaystyle{\boldsymbol{\hat{q}}}_{t}={\boldsymbol{J}}^{\dagger}_{t-1}\bar{{\boldsymbol{H}}}^{*}({\boldsymbol{\bar{\epsilon}}}_{t-1})\>{\boldsymbol{\epsilon}}_{t}\>+{\boldsymbol{q}}_{t-1}, (10)
𝒒^t,i(j)=𝑱t−1†​𝑯¯∗​(ϵ¯t−1)⏟𝑨t(j)​𝝁i(j)+𝒒t−1⏟𝒃t(j),{\boldsymbol{\hat{q}}}^{(j)}_{t,i}=\underbrace{{\boldsymbol{J}}^{{\dagger}}_{t-1}\bar{{\boldsymbol{H}}}^{*}({\boldsymbol{\bar{\epsilon}}}_{t-1})}_{{\boldsymbol{A}}^{(j)}_{t}}\>{\boldsymbol{\mu}}^{(j)}_{i}+\underbrace{{\boldsymbol{q}}_{t-1}}_{{\boldsymbol{b}}^{(j)}_{t}}, (11)
𝒒^t,i(j)=𝑱t−1†​𝑯¯∗​(ϵ¯t−1)​𝑯+​(ϵt(j))⏟𝑨t(j)​𝝁i(j)+𝒒t−1⏟𝒃t(j),{\boldsymbol{\hat{q}}}^{(j)}_{t,i}=\underbrace{{\boldsymbol{J}}^{\dagger}_{t-1}\bar{{\boldsymbol{H}}}^{*}\left({\boldsymbol{\bar{\epsilon}}}_{t-1}\right)\>\overset{+}{{\boldsymbol{H}}}\left({\boldsymbol{\epsilon}}^{(j)}_{t}\right)}_{{\boldsymbol{A}}^{(j)}_{t}}{\boldsymbol{\mu}}^{(j)}_{i}+\underbrace{{\boldsymbol{q}}_{t-1}}_{{\boldsymbol{b}}^{(j)}_{t}}, (12)
Algorithm 1 Simultaneously learning constraints in operational and configuration spaces
1: Initialization
2: Select candidate projection operators from Table III based on the task at hand
  • •

    Canonical operator 𝑨t(j)=𝑰{\boldsymbol{A}}^{(j)}_{t}\!=\!{\boldsymbol{I}}, 𝒃t(j)=𝟎{\boldsymbol{b}}^{(j)}_{t}\!=\!{\boldsymbol{0}}, for encoding configuration space constraints

  • •

    Operational space operators, for absolute or relative position/orientation constraints in operational space

3: Collect demonstrations and compute the local datasets 𝑿(j){\boldsymbol{X}}^{(j)} according to the chosen operators

1: Model training
2: Apply EM [10] to obtain {πi,{𝝁i(j),𝚺i(j)}j=1P}i=1K\big\{\pi_{i},\{{\boldsymbol{\mu}}^{(j)}_{i},{\boldsymbol{\Sigma}}^{(j)}_{i}\}_{j=1}^{P}\big\}_{i=1}^{K}

1: Movement synthesis
2: for t=1,…,Nt=1,\dots,N do
3:   for j=1,…,Pj=1,\dots,P do
4:    Update {𝑨t(j),𝒃t(j)}\{\!{\boldsymbol{A}}^{(j)}_{t}\!\!,{\boldsymbol{b}}^{(j)}_{t}\!\} according to Table III
5:   end for
6:   for i=1,…,Ki=1,\dots,K do
7:    Compute 𝝁^t,i{\boldsymbol{\hat{\mu}}}_{t,i} and 𝚺^t,i{\boldsymbol{\hat{\Sigma}}}_{t,i} from (1) and (2)
8:   end for
9:   Apply GMR at 𝝃tℐ{\boldsymbol{\xi}}^{{\scriptscriptstyle{\mathcal{I}}}}_{t}: 𝒫⁡(𝝃t𝒪|𝝃tℐ)=𝒩⁡(𝝃t𝒪|𝝁^t𝒪,𝚺^t𝒪){\mathcal{P}({\boldsymbol{\xi}}^{{\scriptscriptstyle{\mathcal{O}}}}_{t}|{\boldsymbol{\xi}}^{{\scriptscriptstyle{\mathcal{I}}}}_{t})=\mathcal{N}\big({\boldsymbol{\xi}}^{{\scriptscriptstyle{\mathcal{O}}}}_{t}|{\boldsymbol{\hat{\mu}}}^{{\scriptscriptstyle{\mathcal{O}}}}_{t},{\boldsymbol{\hat{\Sigma}}}^{{\scriptscriptstyle{\mathcal{O}}}}_{t}\big)}
10:   Use 𝝁^t𝒪{\boldsymbol{\hat{\mu}}}^{{\scriptscriptstyle{\mathcal{O}}}}_{t} as joint references for the robot controller
11: end for

Task parameters for configuration space constraints

Joint-space demonstrations use the identity projection. Their Gaussian distribution is fused with the projected position and orientation constraints, allowing the same model to change emphasis across movement phases.

Experiment: bimanual shaking skill

Ten approximately thirteen-second simulated demonstrations teach COMAN to reach for a bottle and shake it with its shoulders. Data are temporally aligned; a ten-component model uses one bottle-relative operational operator and one joint-space operator. The setup assumes a successful grasp with fixed contact points.

Low variation in bottle-relative pose dominates reaching, while consistent shoulder oscillation dominates shaking. A physical COMAN reproduction uses optical object tracking and joint-position control. This demonstrates switching between relevant spaces rather than learning grasp reliability from raw sensory data.

Equations 13–16
𝑱up=[𝑱W|L𝑱L𝟎𝑱W|R𝟎𝑱R],{\boldsymbol{J}}_{\!\text{up}}=\left[\begin{matrix}{\boldsymbol{J}}_{W|L}&{\boldsymbol{J}}_{\!\!L}&{\boldsymbol{0}}\\ {\boldsymbol{J}}_{W|R}&{\boldsymbol{0}}&{\boldsymbol{J}}_{\!\!R}\end{matrix}\right], (13)
𝑨t(1)=𝑱up†​[𝑹t(1)𝟎𝟎𝟎𝟎𝑯¯∗​(ϵ¯L,t−1)​𝑯+​(ϵt(1))𝟎𝟎𝟎𝟎𝑹t(1)𝟎𝟎𝟎𝟎𝑯¯∗​(ϵ¯R,t−1)​𝑯+​(ϵt(1))],\displaystyle\!\!\!{\boldsymbol{A}}^{(1)}_{t}\!\!=\!{\boldsymbol{J}}^{\dagger}_{\!\text{up}}\!\!\left[\begin{smallmatrix}{\boldsymbol{R}}^{(1)}_{t}&{\boldsymbol{0}}&{\boldsymbol{0}}&{\boldsymbol{0}}\\ {\boldsymbol{0}}&\!\!\!\bar{{\boldsymbol{H}}}^{*}({\boldsymbol{\bar{\epsilon}}}_{L,t-1})\overset{+}{{\boldsymbol{H}}}({\boldsymbol{\epsilon}}^{(1)}_{t})&{\boldsymbol{0}}&{\boldsymbol{0}}\\ {\boldsymbol{0}}&{\boldsymbol{0}}&\!\!{\boldsymbol{R}}^{(1)}_{t}&{\boldsymbol{0}}\\ {\boldsymbol{0}}&{\boldsymbol{0}}&{\boldsymbol{0}}&\!\!\bar{{\boldsymbol{H}}}^{*}({\boldsymbol{\bar{\epsilon}}}_{R,t-1})\overset{+}{{\boldsymbol{H}}}({\boldsymbol{\epsilon}}^{(1)}_{t})\\ \end{smallmatrix}\right]\!\!,\!\!\! (14)
𝒃t(1)=𝑱up†​[𝒑t(1)−𝒙L,t−1𝟎𝒑t(1)−𝒙R,t−1𝟎]+𝒒t−1.\displaystyle\hskip 42.67912pt{\boldsymbol{b}}^{(1)}_{t}={\boldsymbol{J}}^{\dagger}_{\text{up}}\left[\begin{smallmatrix}{\boldsymbol{p}}^{(1)}_{t}-{\boldsymbol{x}}_{L,t-1}\\ {\boldsymbol{0}}\\ {\boldsymbol{p}}^{(1)}_{t}-{\boldsymbol{x}}_{R,t-1}\\ {\boldsymbol{0}}\end{smallmatrix}\right]+{\boldsymbol{q}}_{t-1}. (15)
𝑿t(1)=[𝑹t(1)𝖳​(𝒙L,t𝖳−𝒑t(1))𝑯+​(ϵ¯t(1))​ϵL,t𝖳𝑹t(1)𝖳​(𝒙R,t𝖳−𝒑t(1))𝑯+​(ϵ¯t(1))​ϵR,t𝖳].{\boldsymbol{X}}^{(1)}_{t}=\left[\begin{smallmatrix}{\boldsymbol{R}}^{(1)^{\mathsf{T}}}_{t}\!\!\left({\boldsymbol{x}}^{\mathsf{T}}_{L,t}-{\boldsymbol{p}}^{(1)}_{t}\right)\\ \overset{+}{{\boldsymbol{H}}}\!\left({\boldsymbol{\bar{\epsilon}}}^{(1)}_{t}\right)\!{\boldsymbol{\epsilon}}^{\mathsf{T}}_{L,t}\\ {\boldsymbol{R}}^{(1)^{\mathsf{T}}}_{t}\!\!\left({\boldsymbol{x}}^{\mathsf{T}}_{R,t}-{\boldsymbol{p}}^{(1)}_{t}\right)\\ \overset{+}{{\boldsymbol{H}}}\!\left({\boldsymbol{\bar{\epsilon}}}^{(1)}_{t}\right)\!{\boldsymbol{\epsilon}}^{\mathsf{T}}_{R,t}\end{smallmatrix}\right]. (16)
Refer to caption
Fig. 2: The COMAN robot performs the bimanual shaking task in simulation. First: The robot is in a neutral starting pose. Second: Reaching for the bottle and grasping it. Third: Bringing the bottle close to the torso. Fourth: Shaking movement executed through rhythmic oscillations of both shoulder joints.
Refer to caption
Refer to caption
Fig. 3: Reproduction of the shaking task in the real COMAN robot. 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. Bottom: 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.
Refer to caption
Fig. 4: Left end-effector position (in meters) with respect to the initial bottle coordinate system (prior to the grasp). 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.
Refer to caption
Fig. 5: Left end-effector orientation (as a unit quaternion) with respect to the initial bottle coordinate system (prior to the grasp). Notice the low variability in orientation at the end of the reaching movement.
Refer to caption
Fig. 6: 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.
Original figure
Fig. 7: Shoulder joint angle estimation (radians) from each 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.

Learning task prioritization hierarchies from demonstrations

Hierarchy learning first identifies statistically consistent candidates, then uses their learned precision to combine velocity commands in new situations.

Refer to caption
(a) Four demonstrations of priority on orientation. The robot only fulfills the two tasks when they are both achievable (demonstrations 2 and 3). When they are incompatible, orientation is prioritized (demonstrations 1 and 4).
Refer to caption
(b) Four demonstrations of priority on position. Once the two tasks become incompatible the robot prioritizes position tracking (demonstrations 1 and 4).
Fig. 8: Planar robot tracking an orientation (arrow) and a vertical position (dot) along a line, with different priorities.

Identifying priority hierarchies from demonstrations

For each candidate hierarchy, Jacobians and null-space projectors map the known task-error commands into predicted operational motion. Demonstrations are more consistent under the hierarchy that generated them. Local Gaussian models encode that consistency.

The procedure generalizes to several tasks but assumes a known kinematic model, available task Jacobians, and known task set-points. It identifies among supplied candidates rather than discovering arbitrary tasks from unstructured observation.

Equations 17–20
𝒒˙^(1)=𝑱1†​𝒙˙1+𝑵1​𝑱2†​𝒙˙2=[𝑱1†𝑵1​𝑱2†]⏟𝑨(1)​[𝒙˙1𝒙˙2]{\boldsymbol{\hat{\dot{{\boldsymbol{q}}}}}}^{(1)}={\boldsymbol{J}}_{\!1}^{\dagger}\;{\boldsymbol{\dot{x}}}_{1}+{\boldsymbol{N}}_{\!1}{\boldsymbol{J}}_{\!2}^{\dagger}\;{\boldsymbol{\dot{x}}}_{2}\\ =\underbrace{\left[{\boldsymbol{J}}_{\!1}^{\dagger}\quad{\boldsymbol{N}}_{\!1}{\boldsymbol{J}}_{\!2}^{\dagger}\right]}_{{\boldsymbol{A}}^{(1)}}\left[\begin{matrix}{\boldsymbol{\dot{x}}}_{1}\\ {\boldsymbol{\dot{x}}}_{2}\end{matrix}\right] (17)
𝒒˙^(2)=𝑱2†​𝒙˙2+𝑵2​𝑱1†​𝒙˙1=[𝑵2​𝑱1†𝑱2†]⏟𝑨(2)​[𝒙˙1𝒙˙2],{\boldsymbol{\hat{\dot{{\boldsymbol{q}}}}}}^{(2)}={\boldsymbol{J}}_{\!2}^{\dagger}\;{\boldsymbol{\dot{x}}}_{2}+{\boldsymbol{N}}_{\!2}{\boldsymbol{J}}_{\!1}^{\dagger}\;{\boldsymbol{\dot{x}}}_{1}\\ =\underbrace{\left[{\boldsymbol{N}}_{\!2}{\boldsymbol{J}}_{\!1}^{\dagger}\quad{\boldsymbol{J}}_{\!2}^{\dagger}\right]}_{{\boldsymbol{A}}^{(2)}}\left[\begin{matrix}{\boldsymbol{\dot{x}}}_{1}\\ {\boldsymbol{\dot{x}}}_{2}\end{matrix}\right]\!\!, (18)
[𝒙˙^1(1)𝒙˙^2(1)]\displaystyle\left[\begin{matrix}{\boldsymbol{\hat{\dot{x}}}}^{(1)}_{1}\\ {\boldsymbol{\hat{\dot{x}}}}^{(1)}_{2}\end{matrix}\right] =[𝑱1𝑱2]​𝒒˙^(1)=[𝑱1𝑱2]​[𝑱1†𝑵1​𝑱2†]​[𝒙˙1𝒙˙2],\displaystyle=\left[\begin{matrix}{\boldsymbol{J}}_{\!1}\\ {\boldsymbol{J}}_{\!2}\end{matrix}\right]{\boldsymbol{\hat{\dot{{\boldsymbol{q}}}}}}^{(1)}=\left[\begin{matrix}{\boldsymbol{J}}_{\!1}\\ {\boldsymbol{J}}_{\!2}\end{matrix}\right]\left[{\boldsymbol{J}}_{\!1}^{\dagger}\quad{\boldsymbol{N}}_{\!1}{\boldsymbol{J}}_{\!2}^{\dagger}\right]\left[\begin{matrix}{\boldsymbol{\dot{x}}}_{1}\\ {\boldsymbol{\dot{x}}}_{2}\end{matrix}\right], (19)
[𝒙˙^1(2)𝒙˙^2(2)]\displaystyle\left[\begin{matrix}{\boldsymbol{\hat{\dot{x}}}}^{(2)}_{1}\\ {\boldsymbol{\hat{\dot{x}}}}^{(2)}_{2}\end{matrix}\right] =[𝑱1𝑱2]​𝒒˙^(2)=[𝑱1𝑱2]​[𝑵2​𝑱1†𝑱2†]​[𝒙˙1𝒙˙2],\displaystyle=\left[\begin{matrix}{\boldsymbol{J}}_{\!1}\\ {\boldsymbol{J}}_{\!2}\end{matrix}\right]{\boldsymbol{\hat{\dot{{\boldsymbol{q}}}}}}^{(2)}=\left[\begin{matrix}{\boldsymbol{J}}_{1}\\ {\boldsymbol{J}}_{2}\end{matrix}\right]\left[{\boldsymbol{N}}_{2}{\boldsymbol{J}}_{1}^{\dagger}\quad{\boldsymbol{J}}_{2}^{\dagger}\right]\left[\begin{matrix}{\boldsymbol{\dot{x}}}_{1}\\ {\boldsymbol{\dot{x}}}_{2}\end{matrix}\right], (20)
Refer to caption
(a) Demonstrated priority on orientation (see Figure 8(a)).
Refer to caption
(b) Demonstrated priority on position (see Figure 8(b)).
Fig. 9: Operational space velocities generated by each candidate hierarchy for the configurations of the robot in Figure 8. Each colored point corresponds to a demonstration with the same color in Figure 8. The ‘×\times’ represents the mean of the datapoints, while the bars depict one standard deviation.
Equation 21
[𝒙˙^1(j)𝒙˙^NT(j)]\displaystyle\left[\begin{matrix}{\boldsymbol{\hat{\dot{x}}}}^{(j)}_{1}\\ \vdots\\ {\boldsymbol{\hat{\dot{x}}}}^{(j)}_{N_{T}}\end{matrix}\right] =[𝑱1𝑱NT]​𝑨(j)​[𝒙˙1𝒙˙NT],\displaystyle=\left[\begin{matrix}{\boldsymbol{J}}_{\!1}\\ \vdots\\ {\boldsymbol{J}}_{\!N_{T}}\end{matrix}\right]{\boldsymbol{A}}^{(j)}\left[\begin{matrix}{\boldsymbol{\dot{x}}}_{1}\\ \vdots\\ {\boldsymbol{\dot{x}}}_{N_{T}}\end{matrix}\right], (21)

Movement synthesis: fusion of strict hierarchies

At execution, GMR predicts covariance for each candidate hierarchy. Mapping it into joint space gives a precision matrix; a weighted least-squares solution fuses the candidate joint velocities. The result can be sent to a velocity controller or integrated for position control. Algorithm 2 retains all initialization, fitting, and synthesis steps.

Equations 22–25
𝒒˙^=arg⁡min𝒒˙​∑j=1P(𝒒˙−𝒒˙(j))𝖳​𝚪(j)​(𝒒˙−𝒒˙(j)),{\boldsymbol{\hat{\dot{q}}}}=\arg\underset{{\boldsymbol{\dot{q}}}}{\min}\sum_{j=1}^{P}\left({\boldsymbol{\dot{q}}}-{\boldsymbol{\dot{q}}}^{(j)}\right)^{\!\!\mathsf{T}}\!{\boldsymbol{\Gamma}}^{(j)}\!\left({\boldsymbol{\dot{q}}}-{\boldsymbol{\dot{q}}}^{(j)}\right), (22)
𝚪(j)=(𝑨(j)​𝚺(j)​𝑨(j)𝖳)−1.{\boldsymbol{\Gamma}}^{(j)}=\Big(\!{\boldsymbol{A}}^{(j)}{\boldsymbol{\Sigma}}^{(j)}{\boldsymbol{A}}^{(j)^{\mathsf{T}}}\!\Big)^{\!-1}. (23)
𝒒˙(j)=𝑨(j)​𝒙˙(j),{\boldsymbol{\dot{q}}}^{(j)}={\boldsymbol{A}}^{(j)}{\boldsymbol{\dot{x}}}^{(j)}, (24)
𝒒˙^=(∑j=1P𝚪(j))−1​∑j=1P𝚪(j)​𝒒˙(j),{\boldsymbol{\hat{\dot{q}}}}=\Big(\sum\limits_{j=1}^{P}{\boldsymbol{\Gamma}}^{(j)}\Big)^{\!\!-1}\sum\limits_{j=1}^{P}{\boldsymbol{\Gamma}}^{(j)}\!{\boldsymbol{\dot{q}}}^{(j)}, (25)
Algorithm 2 Learning task prioritization hierarchies from demonstrations
1: Initialization
2: Select a set of potentially relevant candidate hierarchies 𝑨(j){\boldsymbol{A}}^{(j)}
3: Collect demonstrations

1: Model training
2: Compute 𝑿(j){\boldsymbol{X}}^{(j)} for each candidate hierarchy 𝑨(j){\boldsymbol{A}}^{(j)} with (21)
3: Estimate model {πi,{𝝁i(j),𝚺i(j)}j=1P}i=1K\big\{\pi_{i},\{{\boldsymbol{\mu}}^{(j)}_{i},{\boldsymbol{\Sigma}}^{(j)}_{i}\}_{j=1}^{P}\big\}_{i=1}^{K} using EM

1: Movement synthesis
2: for t=1,…,Nt=1,\dots,N do
3:   Obtain input 𝝃tℐ{\boldsymbol{\xi}}^{\scriptscriptstyle{\mathcal{I}}}_{t} at time step tt
4:   for j=1,…,Pj=1,\dots,P do
5:    Update task parameters 𝑨(j){\boldsymbol{A}}^{(j)} with Jacobians and
6:     null space matrices at time step tt
7:    Compute 𝚪(j){\boldsymbol{\Gamma}}^{(j)} using GMR and (23)
8:    Update desired task space velocities 𝒙˙(j){\boldsymbol{\dot{x}}}^{(j)} for the
9:     tasks in hierarchy jj
10:    Compute candidate solution 𝒒˙(j){\boldsymbol{\dot{q}}}^{(j)} using (24)
11:   end for
12:   Compute velocity control command 𝒒˙^{\boldsymbol{\hat{\dot{q}}}} using (25)
13: end for

Experiment: Fusion of hierarchies and behavior during transitions

A planar test manually changes hierarchy weights to examine transitions before learning them. Incompatible position and orientation goals produce a smooth compromise as weights vary. When both goals are reachable, the demonstrated test maintains both after convergence. These examples illustrate behavior, not a universal convergence proof for arbitrary conflicting tasks.

Refer to caption
Fig. 10: End-effector behavior for varying hierarchy weights. Top-left: Planar robot fulfilling the two tasks separately. Top-right: End-effector state in the position-orientation space for the considered weights (blue line). Bottom: End-effector orientation and vertical position as weights vary. Note the inverted scales on the horizontal axes (the experiment starts with (1,0)(1,0) and ends with (0,1)(0,1)). Red lines and point indicate the references.
Refer to caption
Fig. 11: Performance of the robot in tracking position and orientation references with varying weights when both tasks can be achieved.

Experiment: Learning priorities with the COMAN robot

COMAN’s shared waist joints create conflicts between left- and right-hand reaching. Two candidate hierarchies and one Gaussian encode a constant priority learned from simulated controller demonstrations. Physical tests use optically tracked targets and reproduce the preferred arm’s tracking while attempting the secondary task.

Reported per-step computation averages 1.15 milliseconds in MATLAB on the specified laptop. Low variance under the demonstrated hierarchy gives it greater precision in the combined controller.

Refer to caption
Fig. 12: Illustration of the waist joint prioritization problem. 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. 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.
Refer to caption
(a) Demonstrated left arm priority.
Refer to caption
(b) Demonstrated right arm priority.
Fig. 13: Task space velocities generated by each candidate hierarchy given the demonstration data. Each entry 𝑨(j){\boldsymbol{A}}^{(j)} contains the means of several observations 𝒙˙^(j){\boldsymbol{\hat{\dot{x}}}}^{(j)}, computed across the dimensions of the tasks. The error bars correspond to two standard deviations of the data. Note the low variability in the datapoints corresponding to the demonstrated hierarchies.
Equations 26–27
𝑨(1)=[𝑱L†​𝑵L​𝑱R†],𝑨(2)=[𝑵R​𝑱L†​𝑱R†],\displaystyle{\boldsymbol{A}}^{(1)}=\left[{\boldsymbol{J}}^{\dagger}_{\!\!L}\>\>\>{\boldsymbol{N}}_{\!\!L}{\boldsymbol{J}}^{\dagger}_{\!\!R}\right],\qquad{\boldsymbol{A}}^{(2)}=\left[{\boldsymbol{N}}_{\!\!R}{\boldsymbol{J}}^{\dagger}_{\!\!L}\>\>\>{\boldsymbol{J}}^{\dagger}_{\!\!R}\right], (26)
𝒙˙(1)=𝒙˙(2)=[𝒙^L−𝒙L𝒙^R−𝒙R],{\boldsymbol{\dot{x}}}^{(1)}={\boldsymbol{\dot{x}}}^{(2)}=\left[\begin{matrix}{\boldsymbol{\hat{x}}}_{L}-{\boldsymbol{x}}_{L}\\ {\boldsymbol{\hat{x}}}_{R}-{\boldsymbol{x}}_{R}\end{matrix}\right], (27)
Refer to caption
Refer to caption
Fig. 14: Movement synthesis using the learned left and right arm priority models. Left: The left reference is always tracked, while the right reference is only tracked when reachable. Right: The right reference is prioritized.
Refer to caption
Fig. 15: Waist joint angles given by each candidate hierarchy. The envelope around the lines represents one standard deviation of the variance encoded in each hierarchy. TP-GMM extracts the solution with the lowest variability.
Refer to caption
Fig. 16: The Centauro robot tracks different references, prioritizing (by decreasing order of importance) base position, end-effector positions and end-effector orientations. The blue and pink spheres represent the desired base and end-effectors positions, while the arrow denotes the desired end-effector pointing direction. Left: All tasks can be fulfilled. Center: The pelvis reference is moved backwards, thus the robot performs the orientation task as best as it can but not completely. Right: The end-effector position references are moved forward and both end-effector position and orientation are now only partially fulfilled.
Refer to caption
(a) Demonstrated: b>p>ob>p>o.
Refer to caption
(b) Demonstrated: p>b>op>b>o.
Fig. 17: Datasets from Centauro priority demonstrations. Each entry 𝑨(j){\boldsymbol{A}}^{(j)} contains 12 points (the means of 𝒙˙^(j){\boldsymbol{\hat{\dot{x}}}}^{(j)} for each demonstration).
Refer to caption
Fig. 18: In this example, the Centauro robot prioritizes end-effector positions, followed by the position of the base and, finally, the end-effector orientations.

Experiment: Learning priorities with more than two tasks

Centauro is evaluated in simulation with wheel-only locomotion, controlling nineteen joints. Three task groups—base position, hand positions, and hand orientations—produce six candidate orderings. Twelve demonstrations teach one ordering, and twelve more separately teach another.

Both demonstrated orderings are identified and reproduced with changed references. The unoptimized implementation averages 7.35 milliseconds per control cycle on different hardware; this is not a directly controlled speed comparison with COMAN.

𝑨(1){\boldsymbol{A}}^{(1)} 𝑨(2){\boldsymbol{A}}^{(2)} 𝑨(3){\boldsymbol{A}}^{(3)} 𝑨(4){\boldsymbol{A}}^{(4)} 𝑨(5){\boldsymbol{A}}^{(5)} 𝑨(6){\boldsymbol{A}}^{(6)}
b>p>ob\!>\!p\!>\!o b>o>pb\!>\!o\!>\!p p>o>bp\!>\!o>\!b p>b>op\!>\!b\!>\!o o>p>bo\!>\!p\!>\!b o>b>po\!>\!b\!>\!p
TABLE IV: Hierarchies used in Section V-E.

Experiment: Transferring task priorities between robots

Priority models learned on COMAN transfer to the kinematically different WALK-MAN model. Both share the relevant operational-space dimensions and the structure in which waist joints affect both arms. The experiment supports transfer within that correspondence, while learning priorities directly from human vision remains a proposed extension.

Refer to caption
Refer to caption
Fig. 19: Movement synthesis with the WALK-MAN robot using the left and right priority models that were learned with COMAN.

Discussion

The discussion separates limitations of task-space fusion from limitations of hierarchy identification.

Learning operational and configuration space constraints

Demonstrations must vary enough to reveal which constraints matter. Poorly varied examples can mislead the model. Quaternion handling is approximate, and conflicting constraints are fused without explicitly deciding whether the compromise is acceptable. Interactive teaching and conflict detection are future directions.

Learning task prioritization hierarchies

Candidate hierarchies and task set-points must be specified. Too many candidates increase data requirements, and moving set-points complicate demonstration collection. Separately teaching individual tasks and their priority relationships is suggested as a way to reduce that burden.

Conclusions and Future Work

The framework combines operational and joint-space constraints and learns priority behavior among supplied hierarchies. Evidence covers the reported humanoid and simulated mobile-manipulation scenarios. Torque control, optimization-based refinement, and more general whole-body contact priorities remain future work.

Author information

The source includes portraits of the four authors. Affiliations below are those stated in the paper.

Portrait of João Silvério João Silvério — Istituto Italiano di Tecnologia, as listed in the paper.
Portrait of Sylvain Calinon Sylvain Calinon — Idiap Research Institute, as listed in the paper.
Portrait of Leonel Rozo Leonel Rozo — Istituto Italiano di Tecnologia, as listed in the paper.
Portrait of Darwin G. Caldwell Darwin G. Caldwell — Istituto Italiano di Tecnologia, as listed in the paper.

Earlier-manuscript Appendix A: Unit quaternions

These mathematical appendices come from the 2017 arXiv v1 manuscript. The current paper moves them to an external supplement whose original link is unavailable. Their equation numbers belong to the earlier manuscript.

A unit quaternion has a scalar part and a three-dimensional vector part, with unit norm. Conjugation negates the vector part. Quaternion multiplication composes rotations and is generally noncommutative. Left and right Hamilton matrices express that product as a matrix–vector operation; their respective actions commute. The original product and matrix formulas are preserved below.

Equations v1 28–v1 30
(v1 28)Equation 28 from the earlier arXiv v1 mathematical appendix.
(v1 29)Equation 29 from the earlier arXiv v1 mathematical appendix.
(v1 30)Equation 30 from the earlier arXiv v1 mathematical appendix.

Earlier-manuscript Appendix B: Linear orientation operators

The earlier derivation selects the vector part of the relative quaternion with a fixed matrix and rewrites quaternion multiplication using the right Hamilton matrix. Once the previous orientation is fixed, the next quaternion enters linearly. This supplies the local orientation operator used by TP-GMM.

The formulas use the paper’s discrete angular-velocity approximation, not an exact finite-rotation logarithm. The current main text explicitly uses an approximation sign; the earlier appendix also discusses quaternion logarithmic mappings as an alternative.

Equations v1 31–v1 34
(v1 31)Equation 31 from the earlier arXiv v1 mathematical appendix.
(v1 32)Equation 32 from the earlier arXiv v1 mathematical appendix.
(v1 33)Equation 33 from the earlier arXiv v1 mathematical appendix.
(v1 34)Equation 34 from the earlier arXiv v1 mathematical appendix.
Concept demonstration

Open image ↗