add general visualization

[LuziaKn]

Adds a more general visualization element in which the position does not depend on the robot. Can be used for e.g. visualization of mpc plan.

ToDo: rotation should also be provided as an input

[maxspahn]

That looks fine, thanks @LuziaKn.

In the future the visualization, such as add_collision_link, add_debug_shape, add_visualization, add_obstacle, etc should be bundled in a class, I think.