diff --git a/src/pyuwds3/reasoning/monitoring/graphic_monitor.py b/src/pyuwds3/reasoning/monitoring/graphic_monitor.py
index a8de098..611ef5c 100644
--- a/src/pyuwds3/reasoning/monitoring/graphic_monitor.py
+++ b/src/pyuwds3/reasoning/monitoring/graphic_monitor.py
@@ -246,87 +246,87 @@ def test_occlusion(self, object, tracks, occlusion_threshold=0.8):
 #cansee
 #getvisualshapedata [X][7] = color
     #
-    # def can_reach(self,start_pose,obj):#,working_area,xy_n=10,z_n=5):
-    #     end_id=self.simulator.entity_id_map[obj.id]
-    #     # x_init = working_area[0][0]
-    #     # y_init = working_area[0][1]
-    #     # z_init = working_area[0][2]
-    #     # x_step = (working_area[1][0] - x_init)/(xy_n*1.0)
-    #     # y_step = (working_area[1][1] - y_init)/(xy_n*1.0)
-    #     # z_step = (working_area[1][2] - z_init)/(z_n*1.0)
-    #     # for x in range(xy_n):
-    #     #     for y in range(xy_n):
-    #     #         for z in range(z_n):
-    #     #             if can_reach_rot(self,
-    #     #             [x_init + x*x_step,
-    #     #             y_init + y*y_step,
-    #     #             z_init + z*z_step],end_id):
-    #     #                 return True
-    #     if self.can_reach_rot(start_pose,end_id):
-    #         return True
-    #
-    #     return False
-    #
-    #
-    # def can_reach_rot(self,start_pose,end_id):
-    #     [xmin,ymin,zmin],[xmax,ymax,zmax] = p.getAABB(end_id)
-    #     xlength = xmax - xmin
-    #     ylength = ymax - ymin
-    #     zlength = zmax - zmin
-    #     pose_list = []
-    #     for i in range(N_RAY):
-    #         end_pose = [xmin +i*xlength/(N_RAY-1),
-    #         ymin +i*xlength/(N_RAY-1),
-    #         zmin +i*xlength/(N_RAY-1)]
-    #         r = p.rayTest(start_pose,end_pose)
-    #         if r[0][0]== end_id:
-    #             return True
-    #     return False
-    #
-    # def canSee(self,start_pose,obj):
-    #     end_id=self.simulator.entity_id_map[obj.id]
-    #     [xmin,ymin,zmin],[xmax,ymax,zmax] = p.getAABB(end_id)
-    #     xlength = xmax - xmin
-    #     ylength = ymax - ymin
-    #     zlength = zmax - zmin
-    #     pose_list = []
-    #     # print start_pose
-    #     for i in range(N_RAY):
-    #         for j in range (N_RAY):
-    #             for k in range(N_RAY):
-    #                 end_pose = [xmin +i*xlength/((N_RAY-1)*1.),
-    #                 ymin +j*xlength/((N_RAY-1)*1.),
-    #                 zmin +k*xlength/((N_RAY-1)*1.)]
-    #                 if self.canSeeRec(start_pose,end_pose,end_id,0):
-    #                     return True
-    #     # pose_end=obj.pose.pos.to_array()
-    #     # [pose_end[0],pose_end[1],pose_end[2]]
-    #     start_pose=[start_pose[0][0],start_pose[1][0],start_pose[2][0]]
-    #     if self.canSeeRec(start_pose,end_pose,end_id,0):
-    #         return True
-    #     return False
-    #
-    # def canSeeRec(self,start_pose,end_pose,end_id,hitnumber):
-    #
-    #     r=p.rayTestBatch([start_pose],[end_pose],reportHitNumber = hitnumber)
-    #     # print r[0]
-    #     # print end_id
-    #     if r[0][0] == end_id:
-    #         return True
-    #     if r[0][0]==-1:
-    #         return False
-    #     if not (r[0][0],r[0][1]) in self.alpha_dic:
-    #         data = p.getVisualShapeData(r[0][0])
-    #         if r[0][1] +1 >0 and r[0][1] +1 <len(data) and data[r[0][1] +1][1] == r[0][1]:
-    #             self.alpha_dic[[0][0],r[0][1]] =data[ r[0][1] +1][7]
-    #         else:
-    #             for i in data:
-    #                 if i[1]== r[0][1]:
-    #                     self.alpha_dic[r[0][0],r[0][1]]=i[7]
-    #
-    #     if self.alpha_dic[(r[0][0],r[0][1])] >ALPHA_THRESHOLD:
-    #         return False
-    #     return self.canSeeRec(r[0][3],end_pose,end_id,hitnumber+1)
+    def can_reach(self,start_pose,obj):#,working_area,xy_n=10,z_n=5):
+        end_id=self.simulator.entity_id_map[obj.id]
+        # x_init = working_area[0][0]
+        # y_init = working_area[0][1]
+        # z_init = working_area[0][2]
+        # x_step = (working_area[1][0] - x_init)/(xy_n*1.0)
+        # y_step = (working_area[1][1] - y_init)/(xy_n*1.0)
+        # z_step = (working_area[1][2] - z_init)/(z_n*1.0)
+        # for x in range(xy_n):
+        #     for y in range(xy_n):
+        #         for z in range(z_n):
+        #             if can_reach_rot(self,
+        #             [x_init + x*x_step,
+        #             y_init + y*y_step,
+        #             z_init + z*z_step],end_id):
+        #                 return True
+        if self.can_reach_rot(start_pose,end_id):
+            return True
+
+        return False
+
+
+    def can_reach_rot(self,start_pose,end_id):
+        [xmin,ymin,zmin],[xmax,ymax,zmax] = p.getAABB(end_id)
+        xlength = xmax - xmin
+        ylength = ymax - ymin
+        zlength = zmax - zmin
+        pose_list = []
+        for i in range(N_RAY):
+            end_pose = [xmin +i*xlength/(N_RAY-1),
+            ymin +i*xlength/(N_RAY-1),
+            zmin +i*xlength/(N_RAY-1)]
+            r = p.rayTest(start_pose,end_pose)
+            if r[0][0]== end_id:
+                return True
+        return False
+
+    def canSee(self,start_pose,obj):
+        end_id=self.simulator.entity_id_map[obj.id]
+        [xmin,ymin,zmin],[xmax,ymax,zmax] = p.getAABB(end_id)
+        xlength = xmax - xmin
+        ylength = ymax - ymin
+        zlength = zmax - zmin
+        pose_list = []
+        # print start_pose
+        for i in range(N_RAY):
+            for j in range (N_RAY):
+                for k in range(N_RAY):
+                    end_pose = [xmin +i*xlength/((N_RAY-1)*1.),
+                    ymin +j*xlength/((N_RAY-1)*1.),
+                    zmin +k*xlength/((N_RAY-1)*1.)]
+                    if self.canSeeRec(start_pose,end_pose,end_id,0):
+                        return True
+        # pose_end=obj.pose.pos.to_array()
+        # [pose_end[0],pose_end[1],pose_end[2]]
+        start_pose=[start_pose[0][0],start_pose[1][0],start_pose[2][0]]
+        if self.canSeeRec(start_pose,end_pose,end_id,0):
+            return True
+        return False
+
+    def canSeeRec(self,start_pose,end_pose,end_id,hitnumber):
+
+        r=p.rayTestBatch([start_pose],[end_pose],reportHitNumber = hitnumber)
+        # print r[0]
+        # print end_id
+        if r[0][0] == end_id:
+            return True
+        if r[0][0]==-1:
+            return False
+        if not (r[0][0],r[0][1]) in self.alpha_dic:
+            data = p.getVisualShapeData(r[0][0])
+            if r[0][1] +1 >0 and r[0][1] +1 <len(data) and data[r[0][1] +1][1] == r[0][1]:
+                self.alpha_dic[[0][0],r[0][1]] =data[ r[0][1] +1][7]
+            else:
+                for i in data:
+                    if i[1]== r[0][1]:
+                        self.alpha_dic[r[0][0],r[0][1]]=i[7]
+
+        if self.alpha_dic[(r[0][0],r[0][1])] >ALPHA_THRESHOLD:
+            return False
+        return self.canSeeRec(r[0][3],end_pose,end_id,hitnumber+1)
 
     # def hasInView(self,start_pose,end_id,camera):