Formation Path Planner With Reconfiguration

This version got a complete refactoring of the existing program code to be more compact, easier extendible and more versitile.
Also added the feature to define a reconfiguration (not dynamic). This applies a steady change to the path of the robot.