Formation Morphing Algorithm

Move-in-Global-Frame version