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 Δ 𝑡 .