Rigid formation control added