\bm{x} represents the state trajectory of 3 agents: the left hand, the right hand and the ball. The nonzero elements correspond to the targets that the three agents must reach. For Agent 3 (the ball), \bm{\mu} corresponds to a 2D position target at time T (ball target), with a corresponding 2D precision matrix (identity matrix) in \bm{Q} . For Agent 1 and 2 (the hands), \bm{\mu} corresponds to 2D position targets active from time \frac{3T}{4} to T (coming back to the initial pose and maintaining this pose), with corresponding 2D precision matrices (identity matrices) in \bm{Q} . The constraint that Agents 2 and 3 collide at time \frac{T}{2} is ensured by setting \left[\begin{smallmatrix}\bm{I}&-\bm{I}\\ -\bm{I}&\bm{I}\end{smallmatrix}\right] in the entries of the precision matrix \bm{Q} corresponding to the 2D positions of Agents 2 and 3 at \frac{T}{2} . With this formulation, the two positions are constrained to be the same, without having to predetermine the position at which the two agents should meet. 2 2 2 Indeed, we can see that a cost c\!=\!\left[\begin{smallmatrix}x_{i}\\ x_{j}\end{smallmatrix}\right]^{\scriptscriptstyle\top}\left[\begin{smallmatrix}1&-1\\ -1&1\end{smallmatrix}\right]\left[\begin{smallmatrix}x_{i}\\ x_{j}\end{smallmatrix}\right]\!=\!(x_{i}-x_{j})^{2} is minimized when x_{i}\!=\!x_{j} .
The evolution of the system is described by the linear relation \bm{\dot{x}}_{t}=\bm{A}^{c}_{t}\bm{x}_{t}+\bm{B}^{c}_{t}\bm{u}_{t} , gathering the behavior of the 3 agents (left hand, right hand and ball) as double integrators with motions affected by gravity. For t\!\leq\!\frac{T}{4} (left hand holding the ball), we have
| \underbrace{\left[\begin{array}{c}\mathbf{\dot{x}}_{1,t}\\ \mathbf{\ddot{x}}_{1,t}\\ \bm{\dot{f}}_{1,t}\\ \hline\cr\mathbf{\dot{x}}_{2,t}\\ \mathbf{\ddot{x}}_{2,t}\\ \bm{\dot{f}}_{2,t}\\ \hline\cr\mathbf{\dot{x}}_{3,t}\\ \mathbf{\ddot{x}}_{3,t}\\ \bm{\dot{f}}_{3,t}\end{array}\right]}_{\bm{\dot{x}}_{t}}=\underbrace{\left[\begin{array}{ccc|ccc|ccc}\bm{0}&\bm{I}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}\\ \bm{0}&\bm{0}&\bm{I}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}\\ \bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}\\ \hline\cr\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{I}&\bm{0}&\bm{0}&\bm{0}&\bm{0}\\ \bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{I}&\bm{0}&\bm{0}&\bm{0}\\ \bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}\\ \hline\cr\bm{0}&\color{red}\bm{I}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\color{red}\bm{0}&\bm{0}\\ \bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{I}\\ \bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}\end{array}\right]}_{\bm{A}^{c}_{t}}\underbrace{\left[\begin{array}{c}\mathbf{x}_{1,t}\\ \mathbf{\dot{x}}_{1,t}\\ \bm{f}_{1,t}\\ \hline\cr\mathbf{x}_{2,t}\\ \mathbf{\dot{x}}_{2,t}\\ \bm{f}_{2,t}\\ \hline\cr\mathbf{x}_{3,t}\\ \mathbf{\dot{x}}_{3,t}\\ \bm{f}_{3,t}\end{array}\right]}_{\bm{x}_{t}}+\underbrace{\left[\begin{array}{c|c}\bm{0}&\bm{0}\\ \bm{I}&\bm{0}\\ \bm{0}&\bm{0}\\ \hline\cr\bm{0}&\bm{0}\\ \bm{0}&\bm{I}\\ \bm{0}&\bm{0}\\ \hline\cr\bm{0}&\bm{0}\\ \bm{0}&\bm{0}\\ \bm{0}&\bm{0}\end{array}\right]}_{\bm{B}^{c}_{t}}\underbrace{\left[\begin{array}{c}\bm{u}_{1,t}\\ \hline\cr\bm{u}_{2,t}\end{array}\right]}_{\bm{u}_{t}}. |
At t\!=\!\frac{T}{2} (right hand hitting the ball), we have
| \underbrace{\left[\begin{array}{c}\mathbf{\dot{x}}_{1,t}\\ \mathbf{\ddot{x}}_{1,t}\\ \bm{\dot{f}}_{1,t}\\ \hline\cr\mathbf{\dot{x}}_{2,t}\\ \mathbf{\ddot{x}}_{2,t}\\ \bm{\dot{f}}_{2,t}\\ \hline\cr\mathbf{\dot{x}}_{3,t}\\ \mathbf{\ddot{x}}_{3,t}\\ \bm{\dot{f}}_{3,t}\end{array}\right]}_{\bm{\dot{x}}_{t}}=\underbrace{\left[\begin{array}{ccc|ccc|ccc}\bm{0}&\bm{I}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}\\ \bm{0}&\bm{0}&\bm{I}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}\\ \bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}\\ \hline\cr\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{I}&\bm{0}&\bm{0}&\bm{0}&\bm{0}\\ \bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{I}&\bm{0}&\bm{0}&\bm{0}\\ \bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}\\ \hline\cr\bm{0}&\bm{0}&\bm{0}&\bm{0}&\color{red}\bm{I}&\bm{0}&\bm{0}&\color{red}\bm{0}&\bm{0}\\ \bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{I}\\ \bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}\end{array}\right]}_{\bm{A}^{c}_{t}}\underbrace{\left[\begin{array}{c}\mathbf{x}_{1,t}\\ \mathbf{\dot{x}}_{1,t}\\ \bm{f}_{1,t}\\ \hline\cr\mathbf{x}_{2,t}\\ \mathbf{\dot{x}}_{2,t}\\ \bm{f}_{2,t}\\ \hline\cr\mathbf{x}_{3,t}\\ \mathbf{\dot{x}}_{3,t}\\ \bm{f}_{3,t}\end{array}\right]}_{\bm{x}_{t}}+\underbrace{\left[\begin{array}{c|c}\bm{0}&\bm{0}\\ \bm{I}&\bm{0}\\ \bm{0}&\bm{0}\\ \hline\cr\bm{0}&\bm{0}\\ \bm{0}&\bm{I}\\ \bm{0}&\bm{0}\\ \hline\cr\bm{0}&\bm{0}\\ \bm{0}&\bm{0}\\ \bm{0}&\bm{0}\end{array}\right]}_{\bm{B}^{c}_{t}}\underbrace{\left[\begin{array}{c}\bm{u}_{1,t}\\ \hline\cr\bm{u}_{2,t}\end{array}\right]}_{\bm{u}_{t}}. |
For \frac{T}{4}\!<\!t\!<\!\frac{T}{2} and t\!>\!\frac{T}{2} (free motion of the ball), we have
| \underbrace{\left[\begin{array}{c}\mathbf{\dot{x}}_{1,t}\\ \mathbf{\ddot{x}}_{1,t}\\ \bm{\dot{f}}_{1,t}\\ \hline\cr\mathbf{\dot{x}}_{2,t}\\ \mathbf{\ddot{x}}_{2,t}\\ \bm{\dot{f}}_{2,t}\\ \hline\cr\mathbf{\dot{x}}_{3,t}\\ \mathbf{\ddot{x}}_{3,t}\\ \bm{\dot{f}}_{3,t}\end{array}\right]}_{\bm{\dot{x}}_{t}}=\underbrace{\left[\begin{array}{ccc|ccc|ccc}\bm{0}&\bm{I}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}\\ \bm{0}&\bm{0}&\bm{I}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}\\ \bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}\\ \hline\cr\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{I}&\bm{0}&\bm{0}&\bm{0}&\bm{0}\\ \bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{I}&\bm{0}&\bm{0}&\bm{0}\\ \bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}\\ \hline\cr\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{I}&\bm{0}\\ \bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{I}\\ \bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}&\bm{0}\end{array}\right]}_{\bm{A}^{c}_{t}}\underbrace{\left[\begin{array}{c}\mathbf{x}_{1,t}\\ \mathbf{\dot{x}}_{1,t}\\ \bm{f}_{1,t}\\ \hline\cr\mathbf{x}_{2,t}\\ \mathbf{\dot{x}}_{2,t}\\ \bm{f}_{2,t}\\ \hline\cr\mathbf{x}_{3,t}\\ \mathbf{\dot{x}}_{3,t}\\ \bm{f}_{3,t}\end{array}\right]}_{\bm{x}_{t}}+\underbrace{\left[\begin{array}{c|c}\bm{0}&\bm{0}\\ \bm{I}&\bm{0}\\ \bm{0}&\bm{0}\\ \hline\cr\bm{0}&\bm{0}\\ \bm{0}&\bm{I}\\ \bm{0}&\bm{0}\\ \hline\cr\bm{0}&\bm{0}\\ \bm{0}&\bm{0}\\ \bm{0}&\bm{0}\end{array}\right]}_{\bm{B}^{c}_{t}}\underbrace{\left[\begin{array}{c}\bm{u}_{1,t}\\ \hline\cr\bm{u}_{2,t}\end{array}\right]}_{\bm{u}_{t}}. |
In the above, \bm{f}_{i}=m_{i}\bm{g} represent the effect of gravity on the three agents, with mass m_{i} and acceleration vector \bm{g}=\left[\begin{smallmatrix}0\\ -9.81\end{smallmatrix}\right] due to gravity. The parameters \{\bm{A}^{c}_{t},\bm{B}^{c}_{t}\}_{t=1}^{T} , describing a continuous system, are first converted to their discrete forms with
| \bm{A}_{t}=\bm{I}+\bm{A}^{c}_{t}\Delta t,\quad\bm{B}_{t}=\bm{B}^{c}_{t}\Delta t,\quad\forall t\in\{1,\ldots,T\}, |
which can then be used to define sparse transfer matrices \bm{S}_{\bm{x}} and \bm{S}_{\bm{u}} describing the evolution of the system in a vector form, starting from an initial state \bm{x}_{1} , namely \bm{x}=\bm{S}_{\bm{x}}\bm{x}_{1}+\bm{S}_{\bm{u}}\bm{u} .