diff --git a/mpi_sim/components/occupany_grid_mapping.py b/mpi_sim/components/occupany_grid_mapping.py
index 419f91e42733244560bd56a132bc90a3189b4a25..3d18bf47cd73f9131ab82bf3f910faaabaa8d41d 100644
--- a/mpi_sim/components/occupany_grid_mapping.py
+++ b/mpi_sim/components/occupany_grid_mapping.py
@@ -151,7 +151,7 @@ class OccupancyGridMapping(SensorComponent):
                 transform=self.config.transform
             )
         else :
-            imported_global_map = np.load(file=self.config.imported_global_map_path)
+            imported_global_map = np.load(file=self.config.imported_global_map_path)  
             if self.config.transform:
                imported_global_map=self.config.transform(imported_global_map)  
             self.state.global_map = mpi_sim.utils.OccupancyGridMap(
diff --git a/mpi_sim/core/simulation.py b/mpi_sim/core/simulation.py
index 68d49bdcea42cc64e35a2ceb78b3b5434f1f2ec6..43f7140cf5818fef38a596e2674bf16c236e6b9f 100644
--- a/mpi_sim/core/simulation.py
+++ b/mpi_sim/core/simulation.py
@@ -814,6 +814,21 @@ class Simulation:
         # box2d simulation
         self._box2d_simulation.close()
 
+    def get_objects_in_simulation(self, object_to_avoid=None):
+        """Returns the objects in the simulation"""
+        objects = []
+        # go through objects
+        for obj in self.objects.values():
+            if object_to_avoid is None :
+                # only include objects that have a position property
+                if obj.position is not None:
+                    objects.append(obj)
+            else :
+                if type(obj) != type(object_to_avoid):
+                    if obj.position is not None:
+                        objects.append(obj)
+        return objects
+    
 
     def get_nearby_objects(self, point, radius, object_types=None):
         """Returns objects that are within a specified radius of the given point (x,y) or object (obj.position).
diff --git a/mpi_sim/utils/measurements.py b/mpi_sim/utils/measurements.py
index aca201a04c60b03e4ef510ac4d60a5e6b5e00504..f41ccbaf344d2222d0bac7f15256c765a80b0c4b 100644
--- a/mpi_sim/utils/measurements.py
+++ b/mpi_sim/utils/measurements.py
@@ -2,6 +2,7 @@ import numpy as np
 import mpi_sim
 import collections
 import Box2D
+from Box2D import b2CircleShape
 
 
 def measure_center_distance(p1, p2):
@@ -23,7 +24,7 @@ def measure_center_distance(p1, p2):
 DistanceInfo = collections.namedtuple('DistanceInfo', ['distance', 'point_a', 'point_b'])
 
 
-def measure_distance(p1, p2):
+def measure_distance(p1, p2, simulation=None):
     """Measures the minimum distance between two points or objects (their bodies and shapes) in meters."""
 
     if not isinstance(p1, mpi_sim.Object) and not isinstance(p2, mpi_sim.Object):
@@ -65,33 +66,56 @@ def measure_distance(p1, p2):
                             point_b = np.array(b2_dist_info.pointB)
 
     else:
-        raise NotImplementedError('Calculating the distance between a point and an object is currently not supported!')
-        if not isinstance(p1, mpi_sim.Object) :
-            point_a = np.array(p1)
+        # To measure the distance between an object and a position we create a temporary object
+        # Then we measure the distance between the new temporary object and the original object
+        if simulation is None:
+            raise UserWarning('The distance measurement between an object and a position needs the simulation')
+        
+        if isinstance(p1, mpi_sim.Object) :
+            _object = p1
+            point = p2
         else :
-            point_a = np.array(p2)
+            _object = p2
+            point = p1
+
         distance = np.inf
+        point_a = None
         point_b = None
-        for body_b in p2.box2d_bodies:
-            
-            for fixture_b in body_b.fixtures:
+
+        fake_dot_obj = mpi_sim.objects.RoundTable(
+            position=point,
+            orientation=0,
+            radius = 0.000001,
+        )
+        simulation.add_object(fake_dot_obj)
+
+        for body_a in fake_dot_obj.box2d_bodies:
+            for fixture_a in body_a.fixtures:
 
                 # fixture must have a shape
-                if not fixture_b.shape:
+                if not fixture_a.shape:
                     continue
-                
-                try :
-                    vertices = fixture_b.shape.vertices
-                    distances = list(map(lambda x: np.linalg.norm(point_a-x), vertices))
-                    _dist = np.min(distances)
-                    _point_b = vertices[np.argmin(distances)]
-                except :
-                    _dist = np.linalg.norm(point_a-body_b.position) - fixture_b.shape.radius
-                    _point_b = np.array(body_b.position)
-
-                if _dist < distance:
-                    distance = _dist
-                    point_b = _point_b
+
+                for body_b in _object.box2d_bodies:
+                    for fixture_b in body_b.fixtures:
+
+                        # fixture must have a shape
+                        if not fixture_b.shape:
+                            continue
+
+                        b2_dist_info = Box2D.b2Distance(
+                            shapeA=fixture_a.shape,
+                            shapeB=fixture_b.shape,
+                            transformA=body_a.transform,
+                            transformB=body_b.transform
+                        )
+
+                        if b2_dist_info.distance < distance:
+                            distance = b2_dist_info.distance
+                            point_a = np.array(b2_dist_info.pointA)
+                            point_b = np.array(b2_dist_info.pointB)
+            
+        simulation.remove_object(fake_dot_obj)
 
     return DistanceInfo(distance, point_a, point_b)