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