Before revision:  We add constraints to guarantee collision avoidance between individuals and formation maintenance, then all constraints and kinematics are formulated on a factor graph.
After revision:  We add constraints to guarantee collision avoidance between individuals as well as formation maintenance, then all constraints and kinematics are formulated on a factor graph.