Drawing function

Implemented obstacles, robots and trajectories drawing.
Robots are lists of Pose2, while obstacles and trajectories are lists
of lists of vertices/positions, so all robots/objects/trajectories can
be drew at once.