Update safety zones

Coordinates in safety zones and joint positions are meant to be voxels (and are transformed from coordinates in meters coming from ROS)

Tasks:

  • Remove size from safety zone nonterminal
  • Update isInSafetyZone to a now simpler calculation by just comparing all safety zone coordinates with all joint positions
Edited by René Schöne