All sorts of hacks to have a working simulation.

Signed-off-by: forcaeluz <m.h.voorsluys@student.tudelft.nl>
Hacks to get plotting from the simulation.
Added controller to loop, and animated plot.