in safety_gym/envs/engine.py [0:0]
def dist_xy(self, pos):
''' Return the distance from the robot to an XY position '''
pos = np.asarray(pos)
if pos.shape == (3,):
pos = pos[:2]
robot_pos = self.world.robot_pos()
return np.sqrt(np.sum(np.square(pos - robot_pos[:2])))