BiRP: Learning Robot Generalized Bimanual Coordination using
Relative Parameterization Method on Human Demonstration

Learning from demonstration method is a bridge between humans and robots, which is required to have the ability to extract the characteristics of human skills, plan the trajectories, and control the robot to perform similar skills. Thus, it is necessary to combine human skill learning and robot motion planning and control together in the same encoding approach. A popular way is to link them in the form of probability, like Hidden Markov Model (HMM) and Gaussian Mixture Model (GMM). Besides, considering that the application scenarios of service robots are unstructured and need to adapt to changing situations, a class of task-parameterized models is proposed to address this problem [11]. The task parameters are variables describing the task-specific situation, like the position of an object in a pick-and-place task. By contrary, some task-independent information can also be extracted from the demonstration data, which reflects the nature of the skill itself, namely skill parameters. The concept of task-parameterized models is to observe the skill in multiple frames, like from starting points and ending points, and describe the impedance of the systems by variations and correlations with a linear quadratic regulator, which can then be used to control the robot.

Task-parameterized Gaussian Mixture Model (TP-GMM) is a typical method that probabilistically encodes datapoints, and the relevance of candidate frames $P$ by mixture models, which has good generalization capability [12]. Formally, if we define the task parameters as $\left\{\boldsymbol{b}_{j},\boldsymbol{A}_{j}\right\}_{j=1}^{P}$, the demonstrations $\boldsymbol{\xi}$ can be observed as $\boldsymbol{\zeta}_{j}=\boldsymbol{A}_{j}^{-1}\left(\boldsymbol{\xi}-\boldsymbol{b}_{j}\right)$ in each frame $j$. These transformed demonstrations are then represented as GMM $\{\pi^{(k)},\{\boldsymbol{\mu}_{j}^{(k)},\boldsymbol{\Sigma}_{j}^{(k)}\}_{j=1}^{P}\}_{k=1}^{K}$ by log-likehood maximization, where $\pi^{(k)}$ refers to prior probability of $k$-th Gaussian component, $\boldsymbol{\mu}_{j}^{(k)}$ and $\boldsymbol{\Sigma}_{j}^{(k)}$ refer to mean and covariance matrix of the $k$-th Gaussian in frame $j$. We can regard these Gaussian components in multiple frames as skill parameters that can be transferred following the change of task parameters. For instance, if a new situation is given by task parameters $\{\boldsymbol{\hat{b}}_{j},\boldsymbol{\hat{A}}_{j}\}_{j=1}^{P}$, a new task-specific GMM can be generated by Product of Expert (PoE):

$\displaystyle\mathcal{N}\left(\boldsymbol{\hat{\nu}}^{(k)},\boldsymbol{\hat{\Gamma}}^{(k)}\right)\propto\prod_{j=1}^{P}\mathcal{N}\left(\boldsymbol{\nu}_{j}^{(k)},\boldsymbol{\Gamma}_{j}^{(k)}\right)$

(1)

where $\boldsymbol{\nu}_{j}^{(k)}=\boldsymbol{A}_{j}\boldsymbol{\mu}_{j}^{(k)}+\boldsymbol{b}_{j},\boldsymbol{\Gamma}_{j}^{(k)}=\boldsymbol{A}_{j}\boldsymbol{\Sigma}_{j}^{(k)}\boldsymbol{A}_{j}^{\top}$.The result of the Gaussian product is given analytically by

$\displaystyle\boldsymbol{\hat{\Gamma}}^{(k)}=\left(\sum_{j=1}^{P}\boldsymbol{\hat{\Gamma}}_{j}^{(k)}{}^{-1}\right)^{-1},\quad\boldsymbol{\hat{\nu}}^{(k)}=\boldsymbol{\hat{\Gamma}}^{(k)}\sum_{j=1}^{P}\boldsymbol{\Gamma}_{j}^{(k)}{}^{-1}\boldsymbol{\nu}_{j}^{(k)}$

(2)

For generating robot motion from GMM, optimal control methods like Linear Quadratic Regulator (LQR) and Linear Quadratic Tracking (LQT) can be used as planning and control methods. Here we give the classical form of LQT as follows:

$\displaystyle cost=\left(\boldsymbol{\hat{\nu}}-\boldsymbol{x}\right)^{\top}\boldsymbol{Q}\left(\boldsymbol{\hat{\nu}}-\boldsymbol{x}\right)+\boldsymbol{u}^{\top}\boldsymbol{R}\boldsymbol{u}$

(3)

where $\boldsymbol{\hat{\nu}}$ is the mean matrices of the task-specific GMM obtained by the previous PoE process.

Assume that the system evolution is linear,

$\displaystyle x_{t+1}=\boldsymbol{A}_{s}x_{t}+\boldsymbol{B}_{s}u_{t}$

(4)

where $\boldsymbol{A}_{s},\boldsymbol{B}_{s}$ are coefficients for this system. Then, the relationship between the control command and the robot states can be described in the matrix as $\boldsymbol{x}=\boldsymbol{S}_{x}x_{1}+\boldsymbol{S}_{u}\boldsymbol{u}$, where $\boldsymbol{S}_{x}\in\mathbb{R}^{DT\times D}$ and $\boldsymbol{S}_{u}\in\mathbb{R}^{DT\times D(T-1)}$ are the matrix form combination of $\boldsymbol{A}_{s},\boldsymbol{B}_{s}$. More details can be found in the appendix of [12].

Here we just consider an open loop controller, which solution can be given analytically by

$$\small\hat{\boldsymbol{u}}=\left(\boldsymbol{S}_{u}^{\top}\boldsymbol{Q}\boldsymbol{S}_{u}+\boldsymbol{R}\right)^{-1}\boldsymbol{S}_{u}^{\top}\boldsymbol{Q}\left(\boldsymbol{\hat{\nu}}-\boldsymbol{S}_{x}x_{1}\right)$$

(5)

with a residual as $\hat{\boldsymbol{\Sigma}}_{u}=\left(\boldsymbol{S}_{u}^{\top}\boldsymbol{Q}\boldsymbol{S}_{u}+\boldsymbol{R}\right)^{-1}$.

In the bimanual setting, coordination is reflected at the data level as some characteristics of the relative motion of the arms. For instance, for a bimanual box-lifting task, this characteristic manifests itself as the arms move from free movement to a fixed relative relationship and maintain this relationship for a certain time frame. For a leader-follower task like stir-fry [9], the characteristic refers to the following arm (holding the spoon) motion, and its periodicity is determined with reference to the leading arm (holding the pot). In this work, instead of pre-defining the roles between the arms (as leader or follower), we aim to describe the relative relationship between the arms in a more general way: let the arms parameterize each other.

Formally, we define another frame that takes the trajectory of the other arm as dynamic task parameters and represents the relative relationship as GMM as well. Different from the observation perspectives built with a fixed pose, the transformation matrices $\boldsymbol{A}_{c,t},\boldsymbol{b}_{c,t}$ are dynamic that change with the motion of the other arm. The relative motion is described as $\boldsymbol{\zeta}_{c}=\boldsymbol{A}_{c,t}^{-1}\left(\boldsymbol{\xi}-\boldsymbol{b}_{c,t}\right)$ and represented by $\{\pi^{(k)},\boldsymbol{\mu}_{c}^{(k)},\boldsymbol{\Sigma}_{c}^{(k)}\}_{k=1}^{K}$. For each arm $h$, the task-specific GMM obtained by PoE

$\displaystyle\mathcal{N}\left(\boldsymbol{\hat{\nu}}^{(k)},\boldsymbol{\hat{\Gamma}}^{(k)}\right)\propto\prod_{j=1}^{P}\mathcal{N}\left(\boldsymbol{\nu}_{j}^{(k)},\boldsymbol{\Gamma}_{j}^{(k)}\right)\cdot\mathcal{N}\left(\boldsymbol{\nu}_{c}^{(k)},\boldsymbol{\Gamma}_{c}^{(k)}\right)$

(6)

where $\boldsymbol{\nu}_{c}^{(k)}=\boldsymbol{A}_{c,t}\boldsymbol{\mu}_{c}^{(k)}+\boldsymbol{b}_{c,t},\boldsymbol{\Gamma}_{j}^{(k)}=\boldsymbol{A}_{c,t}\boldsymbol{\Sigma}_{j}^{(k)}\boldsymbol{A}_{c,t}^{\top}$.

Such a relative parameterization entangles the representation of bimanual arms together, letting them consider each other by constructing time-varying mutual observing perspectives. This brings two useful functions:

• •

  Generate the motion of one arm based on a given motion of the other one in a leader-follower manner.

• •

  Generate bimanual motions to adapt to new situations simultaneously in a synergistic manner.

For instance, if the left arm motion $\boldsymbol{\xi}_{l}$ is pre-defined or adjusted to new situations by other methods like Dynamic Movement Primitive (DMP) in [9], a corresponding right arm motion that considers the spatial-temporal coordination implicit in the demonstration can be generated by gaining the dynamic relative parameters $\boldsymbol{A}_{c,t},\boldsymbol{b}_{c,t}$ from $\boldsymbol{\xi}_{l}$. Then we can obtain a task-and-coordination-specific GMM of the right arm for further motion generation and control.

For generating bimanual motions synergistically, since the bimanual motions are unknown at the beginning, the relative parameterization cannot be established. Thus, we first use the product of GMMs in other reference systems to generate the independent motions of arms and then use these motions as the relative frame of the other arm to embed learned coordination iteratively.

Coordination relationships can also be embedded when generating trajectories and corresponding control commands from GMM. Let the cost function of the vanilla LQT controller in Equ. 3 be $\mathcal{C}_{vanilla}$. The composition cost function that takes coordination into account can then be written as

$\displaystyle\mathcal{C}=\sum_{h}^{H}\mathcal{C}_{vanilla}^{h}+\left(\boldsymbol{\nu}_{c}-\boldsymbol{x}_{c}\right)^{\top}\boldsymbol{Q}_{c}\left(\boldsymbol{\nu}_{c}-\boldsymbol{x}_{c}\right)$

(7)

By setting a similar linear system like Equ. 4, the composition cost function can rewrite the cost function as

	$\displaystyle\mathcal{C}=$	$\displaystyle\sum_{h}^{H}\Big{[}\left(\boldsymbol{\hat{\nu}}^{h}_{u}-\boldsymbol{u}^{h}\right)^{\top}\boldsymbol{\Omega}_{u}^{h}\left(\boldsymbol{\hat{\nu}}^{h}_{u}-\boldsymbol{u}^{h}\right)+{\boldsymbol{u}^{h}}^{\top}\boldsymbol{R}^{h}\boldsymbol{u}^{h}\Big{]}$		(8)
		$\displaystyle+\left(\boldsymbol{\nu}_{u,c}-\boldsymbol{u}_{c}\right)^{\top}\boldsymbol{\Omega}_{u,c}\left(\boldsymbol{\nu}_{u,c}-\boldsymbol{u}_{c}\right)$

where $\boldsymbol{\hat{\nu}}^{h}_{u}=\boldsymbol{S}_{u}^{-1}\left(\boldsymbol{\hat{\nu}}^{h}-\boldsymbol{S}_{x}x_{1}\right)$ and $\boldsymbol{\Omega}_{u}=\boldsymbol{S}_{u}^{\top}\boldsymbol{Q}\boldsymbol{S}_{u}$. $\boldsymbol{\hat{\nu}}_{u,c}$ and $\boldsymbol{\Omega}_{u,c}$ share the similar transformation.

Since there exists multivariate ($\boldsymbol{u}^{h},\boldsymbol{u}_{c}$), we cannot directly change this sum of quadratic error terms into PoE. Thus, we set a unified vector $\boldsymbol{U}\in\mathbb{R}^{DT\times H}$ for representing the control command of the whole system, and a binary coordination matrix $\boldsymbol{C}\in\mathbb{R}^{DT\times DTH},\boldsymbol{C}=[\boldsymbol{C}^{1},\ldots,\boldsymbol{C}^{H}]$. For convenience, we set $[\boldsymbol{C}^{h}]=[\mathbf{0},\ldots,\boldsymbol{C}^{h},\ldots,\mathbf{0}]$, then we can continue to rewrite the cost function as

	$\displaystyle\mathcal{C}=$	$\displaystyle\sum_{h}^{H}\Big{[}\left(\boldsymbol{\hat{\nu}}^{h}_{u}-[\boldsymbol{C}^{h}]\boldsymbol{U}\right)^{\top}\boldsymbol{\Omega}_{u}^{h}\left(\boldsymbol{\hat{\nu}}^{h}_{u}-[\boldsymbol{C}^{h}]\boldsymbol{U}\right)$		(9)
		$\displaystyle+\boldsymbol{U}^{\top}[\boldsymbol{C}^{h}]^{\top}\boldsymbol{R}^{h}[\boldsymbol{C}^{h}]\boldsymbol{U}\Big{]}$
		$\displaystyle+\left(\boldsymbol{\nu}_{u,c}-\boldsymbol{C}\boldsymbol{U}\right)^{\top}\boldsymbol{\Omega}_{u,c}\left(\boldsymbol{\nu}_{u,c}-\boldsymbol{C}\boldsymbol{U}\right)$

Set $\boldsymbol{\Omega}_{U}^{h}=[\boldsymbol{C}^{h}]^{\top}\boldsymbol{\Omega}_{u}^{h}[\boldsymbol{C}^{h}]$, $\boldsymbol{R}_{U}^{h}=[\boldsymbol{C}^{h}]^{\top}\boldsymbol{R}^{h}[\boldsymbol{C}^{h}]$, $\boldsymbol{\hat{\nu}}^{h}_{U}=[\boldsymbol{C}^{h}]^{-1}\boldsymbol{\hat{\nu}}^{h}_{u}$, $\boldsymbol{\nu}_{U,c}=\boldsymbol{C}^{-1}\boldsymbol{\nu}_{u,c}$, the composition cost function is simplified as

	$\displaystyle\mathcal{C}=$	$\displaystyle\sum_{h}^{H}\Big{[}\left(\boldsymbol{\hat{\nu}}^{h}_{U}-\boldsymbol{U}\right)^{\top}\boldsymbol{\Omega}_{U}^{h}\left(\boldsymbol{\hat{\nu}}^{h}_{U}-\boldsymbol{U}\right)+\boldsymbol{U}^{\top}\boldsymbol{R}^{h}_{U}\boldsymbol{U}\Big{]}$		(10)
		$\displaystyle+\left(\boldsymbol{\nu}_{U,c}-\boldsymbol{U}\right)^{\top}\boldsymbol{\Omega}_{U,c}\left(\boldsymbol{\nu}_{U,c}-\boldsymbol{U}\right)$

Then we can finally change this sum of quadratic error terms into PoE

	$\displaystyle\mathcal{N}$	$\displaystyle\left({\boldsymbol{\hat{U}}},\boldsymbol{\hat{\Sigma}}_{U}\right)\propto$		(11)
		$\displaystyle\prod_{h}^{H}\left[\mathcal{N}\left(0,{\boldsymbol{R}_{U}^{h}}^{-1}\right)\mathcal{N}\left(\boldsymbol{\hat{\nu}}^{h}_{U},{\boldsymbol{\Omega}_{U}^{h}}^{-1}\right)\right]\mathcal{N}\left(\boldsymbol{\nu}_{U,c},{\boldsymbol{\Omega}_{U,c}}^{-1}\right)$

The result can be written as

	$\displaystyle\boldsymbol{\hat{\Sigma}}_{U}$	$\displaystyle=\left(\sum_{h}^{H}\left[\boldsymbol{\Omega}_{U}^{h}+\boldsymbol{R}_{U}^{h}\right]+\boldsymbol{\Omega}_{U,c}\right)^{-1}$		(12)
	$\displaystyle\boldsymbol{\hat{U}}$	$\displaystyle=\boldsymbol{\hat{\Sigma}}_{U}\left(\sum_{h}^{H}\boldsymbol{\Omega}_{U}^{h}\boldsymbol{\hat{\nu}}^{h}_{U}+\boldsymbol{\Omega}_{U,c}\boldsymbol{\nu}_{U,c}\right)$

By using the binary coordination matrix $\boldsymbol{C}$, we can extract the coordinated control commands and motions from $\boldsymbol{\hat{U}}$.

A feasible variant of the above methods is to introduce weight coefficients $\sigma$ to adjust the influence of coordination relationship in representation and control.

For the GMM representation,

$\displaystyle\mathcal{N}\left(\boldsymbol{\hat{\nu}}^{(k)},\boldsymbol{\hat{\Gamma}}^{(k)}\right)\propto\prod_{j=1}^{P}\mathcal{N}\left(\boldsymbol{\nu}_{j}^{(k)},\boldsymbol{\Gamma}_{j}^{(k)}\right)\cdot\left[\mathcal{N}\left(\boldsymbol{\nu}_{c}^{(k)},\boldsymbol{\Gamma}_{c}^{(k)}\right)\right]^{\sigma}$

(13)

For the LQT controller,

	$\displaystyle\boldsymbol{\hat{\Sigma}}_{U}$	$\displaystyle=\left(\sum_{h}^{H}\left[\boldsymbol{\Omega}_{U}^{h}+\boldsymbol{R}_{U}^{h}\right]+{\sigma}\cdot\boldsymbol{\Omega}_{U,c}\right)^{-1}$		(14)
	$\displaystyle\boldsymbol{\hat{U}}$	$\displaystyle=\boldsymbol{\hat{\Sigma}}_{U}\left(\sum_{h}^{H}\boldsymbol{\Omega}_{U}^{h}\boldsymbol{\hat{\nu}}^{h}_{U}+{\sigma}\cdot\boldsymbol{\Omega}_{U,c}\boldsymbol{\nu}_{U,c}\right)$

The effectiveness of the proposed method is illustrated by learning through both synthetic motions and real demonstration motions. Some pre-designed coordinated motions can show the coordination explicitly, which is meant to demonstrate the performance of the method.

Synthetic motions: The synthetic motions were created via Bézier curves, where bimanual arms depart from a distance and meet at the same point. This kind of motion often occurs in some daily activities that require both arms to grasp, carry or pick up something simultaneously. We provide both two-dimensional and three-dimensional data to show the dimension scalability, as shown in Fig. 3.

Real demonstration motions: We also provide demonstrations of two real tasks to show the effect in bimanual robot manipulation. The palletizing example shown in Fig. 4 represents a class of synergistic coordinated motions and tasks, while the pouring example shown in 2 is a typical bimanual coordination task in the leader-follower manner.

The human demonstration data was collected via Optitrack. The demonstrator attached two groups of markers on his hands for detection by Optitrack. Each group of markers contains four individual markers, which are required to determine the pose of each arm. These four markers will be detected via six Optitrack cameras to record two end-effector trajectories with both position and orientation. We chose the poses from the centers of each marker group to reproduce human bimanual demonstration motions. In addition, the box and the two cups each have a set of four markers for recording object motions. The raw data were pre-processed by our open-source toolbox [13] to extract the valuable information and separate it into multiple demonstrations visually. Each demonstration will have seven pose values for each marker group.

The goal of synthetic motions is that bimanual arms should meet in the same pose, whether in 2-dim or 3-dim. As shown in the left column of Fig. 3, we provide three bimanual motions as demonstrations for each synthetic example. These motions start and end from different positions but move in a similar style. The middle column, with multiple small figures, shows the process of using the proposed relative parameterization method. We use three observation frames to parameterize the motions of each arm, from the start points, endpoints, and a dynamic relative observation frame depending on the other arm. We can extract and construct coordination relationships from this parameterization from demonstration data. The parameterized coordination is then used in motion generation and control in new situations with different task parameters. Keeping the same coordination relationship in these generalized motions is required to achieve some specific bimanual tasks. Thus, the generalized motion generation results are shown in the right column of Fig. 3. In the 2-dim example, bimanual motions are required to meet at a new position, $(5,5)$. In the 3-dim example, this new meeting point is set to $(5,8,5)$. The generated motions with learned coordination are shown in red and blue, while we also provide a comparison with generated motions without coordination (in light red and blue). By comparison, we find that just regarding bimanual arms as a simple combination of two single arms is insufficient for bimanual tasks. It is necessary to parameterize coordination relationship no matter in a leader-follower or synergistic manner; this is the key to achieving bimanual tasks mostly.

We adopt the self-designed humanoid CURI robot for real robot experiments to perform the bimanual motions. Since this work focuses on learning and generalizing coordinated motion, task parameters such as start and end points and object poses are obtained through the Optitrack system. As shown in Fig. 4, we paste four markers on the box to be transported and the box as a destination to facilitate obtaining their poses in the world coordinate system. Meanwhile, four fixed connected markers are also on the back of the CURI robot. The coordinated human hand motions are learned by relative parameterization. Then we use this parameterized coordination model to generate motions that adapt to new object poses and destinations. It is worth mentioning that, unlike the observation frames used in the synthetic data, we set five observation frames to transport this palletizing task, namely from start points, end points, center poses of the transport box, and the center pose of the destination box. This allows the robot to move from an initial pose with its arms outstretched to the sides of the box, carry the box and place it in the target position, and then release the box. Besides, the result of the pouring example can be found in Fig. 2. A self-designed impedance controller supports the execution of the CURI robot, and the trajectories are converted to joint space commands via its inverse kinematics model.