[update] formation control with dots ok, without orientation,rescaling and obstacle avoidance