Safe motion planning and formation control of quadruped robots