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