Algorithm 1: Collision free trajectory computation.

Input: $U_k^p$, $S_k$, $a_k^p$, $b_k^p$ Output: $U_k$ 1:i = 0; 2:while ($\left|\right|U_k-U_k^p{\left|\right|}^2$ OR $i\le k$) do 3:    $U_k^p=U_k$; 4:    /* Updating Robot Parameters */ 5:    {a,b} ⟵ Solve Equation (1) for $k\in \{0,\dots N-1\}$; 6:    {$U_k$} ⟵ Solve Equation (2); 7:    i ++;