Multi-scale Procrustes-based 3D shape control

Note that the Procrustes residual is invariant to changes in the frame of reference as transformations \(\mathbf{T}_g\) can be expressed in any frame of reference. For convenience we defined \(G\) inertial frames of reference \(\mathcal{F}_g(t)\) with axes always aligned with the global reference axes. Although their orientation does not change, frames \(\mathcal{F}_g(t)\) present a changing position \(\bar{\mathbf{x}}_g(t)\), being \(\bar{\mathbf{x}}_g(t)\) the centroid of \(\mathbf{X}_g(t,r^*(t))\). This is specially convenient as \(\mathcal{F}_g(t)\) implies \(\bar{\mathbf{x}}_g = \mathbf{0}, \forall \,t\) (for that particular reference). For now on, sub-index \(g\) implies point coordinates and transforms are defined from frame \(\mathcal{F}_g(t)\).

\(\mathrm{Tr}\left (\mathbf{X}^h_g(\mathbf{X}^h_g)^{\intercal}\log(\mathbf{T}_g^{\intercal}) \right)=0\).

We can decouple this term into the translation and rotation terms enclosed in (14): \[\begin{aligned} & \mathrm{Tr}\left (\mathbf{X}^h_g(\mathbf{X}^h_g)^{\intercal}\log(\mathbf{T}_g^{\intercal}) \right) \\ &=\mathrm{Tr}\left ((\mathbf{X}^\intercal_g,\textbf{1}_{M_g})^\intercal(\mathbf{X}^\intercal_g,\textbf{1}_{M_g})\log(\mathbf{T}_g^{\intercal}) \right) \\ &=\mathrm{Tr}\left ( \begin{pmatrix} \mathbf{X}_g\mathbf{X}_g^\intercal & {M_g}\bar{\mathbf{x}}_g \\ {M_g}\bar{\mathbf{x}}^\intercal_g & {M_g} \end{pmatrix} \left ( \begin{array}{c|c} \begin{matrix} \log{(\mathbf{R}_g^{\intercal})} \\ (\mathbf{t}')^{\intercal} \end{matrix} & \mathbf{0}_{4 \times 1} \end{array}\right )\right ) \\ &=\mathrm{Tr}\left ( \begin{array}{c|c} \begin{matrix} \mathbf{X}_g\mathbf{X}_g^\intercal\log{(\mathbf{R}_g^{\intercal})} +{M_g}\bar{\mathbf{x}}_g(\mathbf{t}')^{\intercal} \\ {M_g}\bar{\mathbf{x}}^\intercal_g\log{(\mathbf{R}_g^{\intercal})} +{M_g}(\mathbf{t}')^{\intercal} \end{matrix} & \mathbf{0}_{4 \times 1}\end{array}\right ) \\ &=\mathrm{Tr}\left ( \mathbf{X}_g\mathbf{X}^\intercal_g\log(\mathbf{R}_g^\intercal) \right )+{M_g}\mathrm{Tr}\left(\bar{\mathbf{x}}_g\bar{\mathbf{y}}^\intercal_g\mathbf{V}^\intercal_g\ \right ) \\ &=\mathrm{Tr}\left ( \mathbf{X}_g\mathbf{X}^\intercal_g\log(\mathbf{R}_g^\intercal) \right )+{M_g}\mathrm{Tr}\left(\bar{\mathbf{x}}^\intercal_g\mathbf{V}_g\bar{\mathbf{y}}_g\ \right ) \tag{17} \end{aligned}\] where we considered \(\mathbf{t}'=\mathbf{V}_g\mathbf{t}_g=\mathbf{V}_g\bar{\mathbf{y}}_g\) and, in the last step, trace invariance with respect to matrix transposition and cyclic permutations. The second right hand side term in the last step of (17) is zero since frame of reference \(\mathcal{F}_g(t)\) ensures \(\bar{\mathbf{x}}_g = \mathbf{0} \, \forall t\) and the first term from the last step involves the trace of a symmetric matrix times a skew-symmetric matrix and thus is always zero.

\(-\mathrm{Tr}\left ( \mathbf{Y}_g\mathbf{X}^\intercal_g\log(\mathbf{R}_g^\intercal) \right )\leq 0\,\forall t\) and \(\theta\in (-\pi,\pi)\).

Considering \(\log(\mathbf{R}_g^{\intercal})=-\log(\mathbf{R}_g)\) we use (15) to obtain: \[ \mathrm{Tr}\left ( \mathbf{Y}_g\mathbf{X}^\intercal_g\log(\mathbf{R}_g) \right ) \\ =\frac{\theta}{2\sin (\theta) }\left(\left \langle \mathbf{R}_g^\intercal,{\mathbf{Y}}_g\mathbf{X}_g^{\intercal} \right \rangle_F - \left \langle \mathbf{R}_g,{\mathbf{Y}}_g\mathbf{X}_g^{\intercal} \right \rangle_F\right), \tag{18} \] where \(\left\langle \mathbf{A},\mathbf{B}\right\rangle_F=\mathrm{Tr}(\mathbf{A}^\intercal\mathbf{B})\) is the Frobenius inner product. We define \(\widetilde{\mathbf{Y}}_g=\mathbf{Y}_g-\bar{\mathbf{Y}}_g\) (\(\bar{\mathbf{Y}}_g\in \mathbb{R}^{3\times {M_g}}\) stacks mean vector \(\bar{\mathbf{y}}_g\)). Matrix \(\mathbf{R}_g(t,r^*(t))\) is obtained from the Procrustes analysis \(\mathcal{P}(\mathbf{X}_g,\mathbf{Y}_g)\) in (1) through the singular value decomposition of matrix \(\widetilde{\mathbf{Y}}_g\mathbf{X}_g^{\intercal}=\mathbf{Q}_g\mathbf{S}_g\mathbf{W}_g^{\intercal}\). Considering that \(\bar{\mathbf{Y}}_g\mathbf{X}_g^{\intercal}=0\) when \(\bar{\mathbf{x}}_g=0\), \(\widetilde{\mathbf{Y}}_g\mathbf{X}_g^{\intercal}={\mathbf{Y}}_g\mathbf{X}_g^{\intercal}=\mathbf{Q}_g\mathbf{S}_g\mathbf{W}_g^{\intercal}\). The optimal rotation (i.e. the minimiser of the Procrustes optimisation problem) can be computed as \(\mathbf{R}_g=\mathbf{Q}_g\mathbf{W}_g^\intercal\). Pre-multiplying and pos-multiplying all four bracket terms in (18) by \(\mathbf{Q}_g^{\intercal}\) and \(\mathbf{W}_g\) respectively (both are unitary matrix, so the result is invariant to them), we obtain: \[ \frac{\theta}{2\sin (\theta) }\left(\left \langle \mathbf{Q}^\intercal\mathbf{R}_g^\intercal\mathbf{W}_g,\mathbf{S}_g \right \rangle_F - \left \langle \mathbf{I}_3,\mathbf{S}_g \right \rangle_F\right) \\ =\frac{\theta}{2\sin (\theta) }(\mathrm{Tr}\left(\mathbf{W}_g^{\intercal}\mathbf{R}_g\mathbf{Q}_g\mathbf{S}_g\right)-\mathrm{Tr}\left(\mathbf{S}_g\right)), \tag{19} \] Term \(\frac{\theta}{2\sin (\theta) }\) is always positive for \(\theta \in (-\pi,\pi)\) and, since diagonal matrix \(\mathbf{S}_g\) contains all positive eigenvalues \(\mathrm{Tr}(\mathbf{I}_{3}\mathbf{S}_g)\) is always equal or larger than \(\mathrm{Tr}(\mathbf{W}_g^{\intercal}\mathbf{R}_g\mathbf{Q}_g\mathbf{S}_g)\) (note \(\mathbf{W}_g^{\intercal}\mathbf{R}_g\mathbf{Q}_g\in SO(3)\)). Therefore, (19) is \(\leq 0 \, \forall t\) and \(\theta \in (-\pi,\pi)\).

\(-{M_g}\mathrm{Tr}\left(\bar{\mathbf{y}}^\intercal_g\mathbf{V}_g\bar{\mathbf{y}}_g \right )\leq0,\,\forall t\) and \(\theta\in (-\pi,\pi)\).

The second term on the right hand side of the definition of \(\mathbf{V}_g\) in (16) is a skew symmetric matrix and thus can be neglected (\(\mathrm{Tr}(\mathbf{x}^\intercal\mathbf{A}\mathbf{x})=\mathbf{0}\) for all \(\mathbf{A}\) skew symmetric matrices). The third term on the right hand side in (16) when using (15) and applying Rodrigues' rotation formula to matrices \(\mathbf{R}_g\) yields: \[ \frac{c}{\theta^2} (\log(\mathbf{R}_g))^2=c\mathbf{K}^2,\,\text{with } c(\theta)= 1-\frac{\theta(1+\cos(\theta))}{2\, \sin (\theta)}, \tag{20} \] where \(\mathbf{K}(\theta)\) is the skew-symmetric matrix from Rodrigues formula and \(c(\theta)\leq 1\) for \(\theta\in (-\pi,\pi)\). With these considerations: \[ \begin{split} -{M_g}\mathrm{Tr}\left(\bar{\mathbf{y}}^\intercal_g\mathbf{V}_g\bar{\mathbf{y}}_g \right )=-{M_g}\mathrm{Tr}\left(\bar{\mathbf{y}}^\intercal_g(\mathbf{I}_3+c\mathbf{K}^2)\bar{\mathbf{y}}_g \right ) \\=-{M_g}\mathrm{Tr}\left(\bar{\mathbf{y}}^\intercal_g\mathbf{I}_3\bar{\mathbf{y}}_g \right )-{M_g}\mathrm{Tr}\left(\bar{\mathbf{y}}^\intercal_gc\mathbf{K}^2\bar{\mathbf{y}}_g \right ) \\ =-{M_g}\mathrm{Tr}\left(\bar{\mathbf{y}}^\intercal_g\mathbf{I}_3\bar{\mathbf{y}}_g \right )-{M_g}\mathrm{Tr}\left(\bar{\mathbf{y}}^\intercal_g\text{diag}(-c,-c,0)\bar{\mathbf{y}}_g \right ) \\=-{M_g}\mathrm{Tr}\left(\bar{\mathbf{y}}^\intercal_g\text{diag}(1-c,1-c,1)\bar{\mathbf{y}}_g \right ), \tag{21} \end{split} \] where in the third line we applied the fact that \(\mathbf{K}^2\) is a symmetric matrix and the eigenvalues of a squared \(3\times3\) skew-symmetric matrix are always \(\{-1,-1,0\}\). Note how (21) is always \(\leq 0\) for \(\theta\in (-\pi,\pi)\) as \(c< 1\) for \(\theta\in (-\pi,\pi)\).

The system error \(e(t)\) (2) is locally asymptotically stable with control law (12). In particular, scalar function \(\mathcal{V}(t)=\frac{1}{2}\left\| \mathbf{X}(t) - \mathbf{Y}\right\|^2_F\) is Lyapunov and presents a derivative \(\dot{\mathcal{V}}_g\leq 0, \forall t\) and \(\theta \in (-\pi,\pi)\).

\(\mathcal{V}(t)>0,\,\forall\, \mathbf{X}(t)\neq\mathbf{Y}\) and \(\mathcal{V}(t)=0\) only when \(\mathbf{X}(t)=\mathbf{Y}\). We consider that gripper-unaffected points present a zero derivative, i.e. \(\mathrm{d}\mathbf{X}(t)/\mathrm{d}t=\mathbf{0}\) for \(\mathbf{x}_m(t) \notin \Omega_g(t)\). We also consider that gripper-affected points' derivative is \(\mathrm{d}\mathbf{X}^h_g/\mathrm{d}t=\log(\mathbf{T}_g(t,r^*(t))\mathbf{X}^h_g(t,r^*(t))\), i.e. deriving (13) as in (5). Therefore, the derivative of \(\mathcal{V}(t)\) yields: \[ \dot{\mathcal{V}}=\frac{1}{G}\sum_{g}^{G}\dot{\mathcal{V}}_g, \tag{22} \] where \(\dot{\mathcal{V}}\leq 0\) if \(\dot{\mathcal{V}}_g\leq 0,\,\forall g\). Each gripper-associated term \(\dot{\mathcal{V}}_g\) is \[ \dot{\mathcal{V}}_g=\mathrm{Tr}\left ((\mathbf{X}^h_g-\mathbf{Y}^h_g)(\mathbf{X}^h_g )^{\intercal}\log(\mathbf{T}^{\intercal}_g) \right ) \\ =\mathrm{Tr}\left (\mathbf{X}^h_g(\mathbf{X}^h_g)^{\intercal}\log(\mathbf{T}_g^{\intercal})\right) - \mathrm{Tr}\left (\mathbf{Y}^h_g(\mathbf{X}^h_g )^{\intercal}\log(\mathbf{T}^{\intercal}_g) \right ). \tag{23} \] Applying lemma ref, the first right hand side in (23) is zero and, therefore, \(\dot{\mathcal{V}}_g\) yields: \[\tag{24} \begin{split} \dot{\mathcal{V}}_g=- \mathrm{Tr}\left (\mathbf{Y}^h_g(\mathbf{X}^h_g )^{\intercal}\log(\mathbf{T}_g^{\intercal}) \right ) \\ =-\mathrm{Tr}\left ( \begin{pmatrix} \mathbf{Y}_g\mathbf{X}_g^\intercal & {M_g}\bar{\mathbf{y}}_g \\ {M_g}\bar{\mathbf{x}}^\intercal_g & {M_g} \end{pmatrix} \left ( \begin{array}{c|c} \begin{matrix} \log{(\mathbf{R}_g^{\intercal})} \\ (\mathbf{t}')^{\intercal} \end{matrix} & \mathbf{0}_{4 \times 1} \end{array}\right )\right ) \\ =-\mathrm{Tr}\left ( \begin{array}{c|c} \begin{matrix} \mathbf{Y}_g\mathbf{X}_g^\intercal\log{(\mathbf{R}_g^{\intercal})} +{M_g}\bar{\mathbf{y}}_g(\mathbf{t}')^{\intercal} \\ {M_g}\bar{\mathbf{x}}_g\log{(\mathbf{R}_g^{\intercal})} +{M_g}(\mathbf{t}')^{\intercal} \end{matrix} & \mathbf{0}_{4 \times 1}\end{array}\right ) \\ =-\mathrm{Tr}\left ( \mathbf{Y}_g\mathbf{X}^\intercal_g\log(\mathbf{R}_g^\intercal) \right ) -{M_g}\mathrm{Tr}\left(\bar{\mathbf{y}}^\intercal_g\mathbf{V}_g\bar{\mathbf{y}}_g \right ), \end{split} \] where, for the second right hand side term, we considered \(\mathbf{t}'=\mathbf{V}_g\mathbf{t}_g=\mathbf{V}_g\bar{\mathbf{y}}_g\) and trace invariance with respect to matrix transposition and cyclic permutations. Considering lemmas ref and ref, both right hand side terms in (24) are \(\leq 0,\,\forall t\) and \(\theta \in (-\pi,\pi)\) and thus the proof is completed. Note that computation of \(r^*\) relies in the relaxed assumption of LRB. For this reason, the stability analysis remains local since there is no guarantee for a global LRB.

Note that, as deformable objects constitute under-actuated systems, the local stability in Theorem ref does not guarantee a zero-valued error residual. However, it does ensure convergence of the closed-loop system towards a state with a zero error derivative.