configure safety zone better
Ideal would be a safety zone, that the robot moves through (maybe only depending on the concrete trajectory).
Currently, the robot is always in the safety zone, because the first (immutable) part of the robot is contained in it.