diff --git a/mpi_sim/components/raycast_sensor.py b/mpi_sim/components/raycast_sensor.py
index edeee0315efad5b43b074775d733a94b373b08c6..eded3fc0a14518f24ee5662d39c98a5bda750d65 100644
--- a/mpi_sim/components/raycast_sensor.py
+++ b/mpi_sim/components/raycast_sensor.py
@@ -81,6 +81,14 @@ class RayCastSensor(SensorComponent):
             self.update_raycast()
             return self.ray_angle_list
     
+    @property
+    def raycast_angles_from_agent_view(self):
+        if self.update_raycast_every_step :
+            return self.ray_angle_list
+        else :
+            self.update_raycast()
+            return np.round(self.ray_angle_list-np.degrees(self.orientation),2)
+    
 
     def __init__(self, config=None, **kwargs):
         super().__init__(config=config, **kwargs)
@@ -100,6 +108,7 @@ class RayCastSensor(SensorComponent):
         self.max_value_angle = 180
         self.min_value_angle = -180
         self.update_raycast_every_step = self.config.update_raycast_every_step
+        
   
     def _polar_to_cartesian(self, distance, angle):
         return distance*np.cos(angle), distance*np.sin(angle)
@@ -141,7 +150,7 @@ class RayCastSensor(SensorComponent):
         length = self.max_value_angle - self.min_value_angle
         self.ray_angle_list = np.where(self.ray_angle_list > self.max_value_angle, self.min_value_angle + ((self.ray_angle_list - self.max_value_angle) % length), self.ray_angle_list)
         self.ray_angle_list = np.where(self.ray_angle_list < self.min_value_angle, self.max_value_angle - ((self.min_value_angle - self.ray_angle_list) % length), self.ray_angle_list)
-        
+        ray_angle_list_from_agent_viewpoint = np.round(self.ray_angle_list-np.degrees(self.orientation),3)
 
         # Update the variable self.coord_points_max_range_rays
         self._update_coord_max_range_rays()
@@ -167,20 +176,28 @@ class RayCastSensor(SensorComponent):
                 self.coord_point_rays.append(np.array(callback.point)+ noise)
                 if self.draw :
                     self.point11 = self.simulation.box2d_world.renderer.to_screen(self.position)
+                    if not ray_angle_list_from_agent_viewpoint[id]:
+                        color = b2Color(1, 0, 0)
+                    else :
+                        color = b2Color(0.8, 0, 0.8)
                     self.draw_hit(
                             cb_point=np.array(callback.point)+ noise,
                             cb_normal=callback.normal,
                             point1=self.point11,
-                            color=b2Color(0.8, 0, 0.8)
+                            color=color
                         )
             else:
                 self.ray_distance_list.append(sim.utils.measure_center_distance(self.position, np.array(point_ray)+noise))
                 self.coord_point_rays.append(np.array(point_ray)+noise)
                 if self.draw :
+                    if not ray_angle_list_from_agent_viewpoint[id]:
+                        color = b2Color(0.9, 0, 0)
+                    else :
+                        color = b2Color(0.8, 0, 0.8)
                     self.point11 = self.simulation.box2d_world.renderer.to_screen(self.position)
                     self.point22 = self.simulation.box2d_world.renderer.to_screen(np.array(point_ray)+noise)
-                    self.simulation.box2d_world.renderer.DrawSegment(self.point11, self.point22, b2Color(None))
-                    self.simulation.box2d_world.renderer.DrawPoint(self.point22, 5.0, b2Color(None))
+                    self.simulation.box2d_world.renderer.DrawSegment(self.point11, self.point22, color)
+                    self.simulation.box2d_world.renderer.DrawPoint(self.point22, 5.0, color)
 
         if self.draw :
             pygame.display.flip()
diff --git a/mpi_sim/objects/ari_robot.py b/mpi_sim/objects/ari_robot.py
index 6efafcffae1b0ec109b4df5d1f863374d6da47a6..40e68b0eefb570a534d8a2b3df30b973f36f0b79 100644
--- a/mpi_sim/objects/ari_robot.py
+++ b/mpi_sim/objects/ari_robot.py
@@ -129,6 +129,7 @@ class ARIRobot(Object):
             position = (0.0, 0.0)
 
         self.box2d_body.position = (float(position[0]), float(position[1]))
+        self.pan.body.position = np.array((float(position[0]), float(position[1]))) + np.array(self.pan_anchors)
 
 
     @property
@@ -297,7 +298,7 @@ class ARIRobot(Object):
                                             localAnchorB=(0, 0),
                                             referenceAngle=0
                                             )
-        self.pan.body.position = self.box2d_body.worldCenter + self.pan_anchors
+        self.pan.body.position = np.array(self.config.position) + np.array(self.pan_anchors)
         self.joints.append(j)
 
         return self.box2d_body, self.pan.body
diff --git a/mpi_sim/utils/measurements.py b/mpi_sim/utils/measurements.py
index 8c9417b63cd2de7c8c7426cc3ee32cc63f39e6b5..aca201a04c60b03e4ef510ac4d60a5e6b5e00504 100644
--- a/mpi_sim/utils/measurements.py
+++ b/mpi_sim/utils/measurements.py
@@ -66,6 +66,32 @@ def measure_distance(p1, p2):
 
     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)
+        else :
+            point_a = np.array(p2)
+        distance = np.inf
+        point_b = None
+        for body_b in p2.box2d_bodies:
+            
+            for fixture_b in body_b.fixtures:
+
+                # fixture must have a shape
+                if not fixture_b.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
 
     return DistanceInfo(distance, point_a, point_b)