Algorithm 1. Initialization 3

INITIALIZATION                 $\overrightarrow{\Delta i}=\overrightarrow{0}$ $N_{ch}=$ number of columns of ${\mathrm{M}}_{inc}$. BEGIN ${\overrightarrow{q}}_{{ind}_d}={\overrightarrow{q}}_{{ind}_r}+{\nu }_d·t_s·\overrightarrow{\Delta i}$ IF $e_{pin}==true$ ${ \min \mathsf{\Omega }}_c=$ minimum element in ${\overrightarrow{\mathrm{V}\mathsf{\Omega }}}_c$   IF ${\min \mathsf{\Omega }}_c<{\mathsf{\Omega }}_{lim}$ OR $\Vert J_D\Vert {}_c<\Vert J_D\Vert {}_{lim}$     IF ${\min \mathsf{\Omega }}_c=={\overrightarrow{\mathrm{V}\mathsf{\Omega }}}_c\left(1\right)$       ${\overrightarrow{i}}_{ch}=\left[\begin{array}{cc}1& 2\end{array}\right]$       $*{ }^{ }{\overrightarrow{i}}_{nc}=\left[\begin{array}{cc}3& 4\end{array}\right]$     ELSEIF ${\min \mathsf{\Omega }}_c=={\overrightarrow{\mathrm{V}\mathsf{\Omega }}}_c\left(2\right)$       ${\overrightarrow{i}}_{ch}=\left[\begin{array}{cc}1& 3\end{array}\right]$       $*{ }^{ }{\overrightarrow{i}}_{nc}=\left[\begin{array}{cc}2& 4\end{array}\right]$     ELSEIF ${\min \mathsf{\Omega }}_c=={\overrightarrow{\mathrm{V}\mathsf{\Omega }}}_c\left(3\right)$       ${\overrightarrow{i}}_{ch}=\left[\begin{array}{cc}1& 4\end{array}\right]$       $*{ }^{ }{\overrightarrow{i}}_{nc}=\left[\begin{array}{cc}2& 3\end{array}\right]$     ELSEIF ${\min \mathsf{\Omega }}_c=={\overrightarrow{\mathrm{V}\mathsf{\Omega }}}_c\left(4\right)$       ${\overrightarrow{i}}_{ch}=\left[\begin{array}{cc}2& 3\end{array}\right]$       $*{ }^{ }{\overrightarrow{i}}_{nc}=\left[\begin{array}{cc}1& 4\end{array}\right]$     ELSEIF ${\min \mathsf{\Omega }}_c=={\overrightarrow{\mathrm{V}\mathsf{\Omega }}}_c\left(5\right)$       ${\overrightarrow{i}}_{ch}=\left[\begin{array}{cc}2& 4\end{array}\right]$       $*{ }^{ }{\overrightarrow{i}}_{nc}=\left[\begin{array}{cc}1& 3\end{array}\right]$     ELSE ${\min \mathsf{\Omega }}_c=={\overrightarrow{\mathrm{V}\mathsf{\Omega }}}_c\left(6\right)$     ${\overrightarrow{i}}_{ch}=\left[\begin{array}{cc}3& 4\end{array}\right]$       $*{ }^{ }{\overrightarrow{i}}_{nc}=\left[\begin{array}{cc}1& 2\end{array}\right]$     ENDIF     ${\overrightarrow{\mathrm{V}\mathsf{\Omega }}}_{ch}=$ column vector of $N_{ch}$ zeros     FOR $c_1=1:N_{ch}$       ${\overrightarrow{q}}_{ch}={\overrightarrow{q}}_{{ind}_d}$       ${\overrightarrow{q}}_{ch}\left({\overrightarrow{i}}_{ch}\right)={\overrightarrow{q}}_{ch}\left({\overrightarrow{i}}_{ch}\right)+{\nu }_d·t_s·{\mathrm{M}}_{inc}\left(:,c_1\right)$       $**{ }^{ }{\overrightarrow{q}}_{ch}\left({\overrightarrow{i}}_{nc}\right)={\overrightarrow{q}}_{ch}\left({\overrightarrow{i}}_{nc}\right)+{\nu }_d·t_s·{\mathrm{M}}_{inc}\left(:,c_1\right)$       IF ${\overrightarrow{minq}}_{ind}<{\overrightarrow{q}}_{ch}<{\overrightarrow{maxq}}_{ind}$ (element-wise comparison)         ${\overrightarrow{\mathrm{X}}}_{ch}=$ Solve the Forward Kinematics for ${\overrightarrow{q}}_{ch}$, using ${\overrightarrow{X}}_c$ as initial condition         ${\overrightarrow{\mathsf{\alpha }}}_{ch}=$ Angle of spherical joints for ${\overrightarrow{\mathrm{X}}}_{ch}$         IF ${\overrightarrow{\mathsf{\alpha }}}_{ch}<{\overrightarrow{\mathsf{\alpha }}}_{lim}$ (element-wise comparison)             ${\overrightarrow{\mathrm{V}\mathsf{\Omega }}}_{ch}\left(c_1\right)=$ Calculate the index ${\mathsf{\Omega }}_{i,j}$ for ${\overrightarrow{\mathrm{X}}}_{ch}$ with $i,j={\overrightarrow{i}}_{ch}$         ENDIF       ENDIF    ENDFOR     $c_1=argmax\left({\overrightarrow{\mathrm{V}\mathsf{\Omega }}}_{ch}\right)$     $\overrightarrow{\Delta i}\left({\overrightarrow{i}}_{ch}\right)=\overrightarrow{\Delta i}\left({\overrightarrow{i}}_{ch}\right)+{\mathrm{M}}_{inc}\left(:,c_1\right)$     ${\overrightarrow{q}}_{{ind}_d}={\overrightarrow{q}}_{{ind}_r}+{\nu }_d·t_s·\overrightarrow{\Delta i}$   ENDIF ENDIF END