Motion Planning, Formation Control and Obstacle Avoidance for Multi-Agent Systems