Let 𝑡 = 0 .
while   𝑞 𝑖 ≠ 𝑞 𝑓 𝑖 do
 for   𝑗 such that there exists 𝑒 𝑖 𝑗 ∈ 𝐺 ∗ do
  Determine 𝑞 𝑗 and 𝑢 𝑗
  Compute 𝜕 𝜙 l o s 𝑖 𝑗 / 𝜕 𝑞 𝑖 and 𝜕 𝜙 r a n g e 𝑖 𝑗 / 𝜕 𝑞 𝑖
 end  for
 Compute 𝜕 𝜙 𝑖 g o a l / 𝜕 𝑞 𝑖
 Compute 𝑢 𝛼 via numerical optimization
 if Feasible then
  Move. 𝑡 = 𝑡 + Δ 𝑡
 else
  Infeasible. Drop a constraint or terminate.
 end  if
 end  while
Algorithm 1: Algorithm for generating a feasible motion direction for Robot 𝑖 , using time steps Δ 𝑡 .