Merge pull request #56 from ManifoldRG/55-fix-final-position-recentering-and-remove-negative-ranges

Removed negative ranges, recentered final module positions, added tes…

Author: bfaught3

ogm/occupancy_grid_map.py

@@ -79,6 +79,7 @@ def recenter_initial_positions(self, module_positions, final_module_positions, g
         raise ValueError("Module 1 must exist in the module positions dictionary")
 
     module1_pos = module_positions[1]
+    module1_final_pos = final_module_positions[1]
 
     # Calculate offset to move module 1 to grid center
     offset = (
@@ -96,13 +97,20 @@ def recenter_initial_positions(self, module_positions, final_module_positions, g
             pos[2] + offset[2]
         )
 
+    # Calculate the "final" offset to move module 1 to grid center (in the case that module 1's final position deviates from its initial position)
+    final_offset = (
+        grid_center - module1_final_pos[0],
+        grid_center - module1_final_pos[1],
+        grid_center - module1_final_pos[2]
+    )
+
     # Apply offset to all final module positions
     recentered_final_positions = {}
     for module, pos in final_module_positions.items():
         recentered_final_positions[module] = (
-            pos[0] + offset[0],
-            pos[1] + offset[1],
-            pos[2] + offset[2]
+            pos[0] + final_offset[0],
+            pos[1] + final_offset[1],
+            pos[2] + final_offset[2]
         )
 
     return recentered_positions, recentered_final_positions
@@ -228,11 +236,6 @@ def init_actions(self):
                    48: np.array([[0,0], [-1,1], [-2,0]]) # does the negative stuff work? # now switch which dimension stays the same
                    }
 
-    self.negative_y_ranges = {7,8,15,16}
-    self.negative_y_ranges = {}
-    self.negative_z_ranges = {23,24,31,32,39,40,47,48}
-    self.negative_z_ranges = {}
-
   def calc_possible_actions(self): # need to check now that neighbor is free
     self.possible_actions = {}
     self.articulation_points = set(self.articulationPoints(len(self.modules), self.edges))
@@ -254,12 +257,7 @@ def calc_possible_actions(self): # need to check now that neighbor is free
           offset_y = module_position[1] + rangethingy[1]
           offset_z = module_position[2] + rangethingy[2]
 
-          if p in self.negative_y_ranges:
-            sliced = self.curr_grid_map[offset_x[0]:(offset_x[1] + 1), offset_y[0]:(None if offset_y[1]==0 else offset_y[1] - 1):-1, offset_z[0]:(offset_z[1] + 1)]
-          elif p in self.negative_z_ranges:
-            sliced = self.curr_grid_map[offset_x[0]:(offset_x[1] + 1), offset_y[0]:(offset_y[1] + 1), offset_z[0]:(None if offset_z[1]==0 else offset_z[1] - 1):-1]
-          else:
-            sliced = self.curr_grid_map[offset_x[0]:(offset_x[1] + 1), offset_y[0]:(offset_y[1] + 1), offset_z[0]:(offset_z[1] + 1)]
+          sliced = self.curr_grid_map[offset_x[0]:(offset_x[1] + 1), offset_y[0]:(offset_y[1] + 1), offset_z[0]:(offset_z[1] + 1)]
 
           booled = np.squeeze(sliced > 0)
           pa = self.possible_actions[m]

tests/test_ogm_final_position_recentering.py

@@ -0,0 +1,93 @@
+import unittest
+import numpy as np
+from ogm.occupancy_grid_map import OccupancyGridMap
+from agent.random_search_agent import RandomSearchAgent
+import networkx as nx
+
+class TestOGMFinalPositionRecentering(unittest.TestCase):
+
+    def assert_recentered_positions_match(self, module_positions, final_module_positions, expected_recentered_module_positions, expected_recentered_final_module_positions):
+        # Create occupancy grid map with 3 modules
+        ogm = OccupancyGridMap(module_positions, final_module_positions, 3)
+
+        # Print grid size and module positions
+        print(f"Grid size: {ogm.grid_map.shape}")
+        print(f"Original module 1 position: {module_positions[1]}")
+        print(f"Recentered module 1 position: {ogm.module_positions[1]}")
+        print(f"All recentered module positions: {ogm.module_positions}")
+        print(f"Original module 1 final position: {final_module_positions[1]}")
+        print(f"Recentered module 1  final position: {ogm.final_module_positions[1]}")
+        print(f"All recentered module final positions: {ogm.final_module_positions}")  
+        
+        # Compare with expected positions
+        self.assertEqual(
+            ogm.module_positions, expected_recentered_module_positions,
+            msg=f"\nExpected recentered module positions: {expected_recentered_module_positions}\nActual recentered module positions:   {ogm.module_positions}"
+        )
+
+        self.assertEqual(
+            ogm.final_module_positions, expected_recentered_final_module_positions,
+            msg=f"\nExpected recentered final module positions: {expected_recentered_final_module_positions}\nActual recentered final module positions:   {ogm.final_module_positions}"
+        )
+
+    def assert_random_agent_success(self):
+        matrix = np.zeros((9,9,9))
+        matrix[4, 4, 4] = 1
+        matrix[4, 5, 4] = 2
+        matrix[5,5,4] = 3
+        module_positions = {1: (4,4,4), 2: (4,5,4), 3: (5,5,4)}
+        final_matrix = np.zeros((9,9,9))
+        final_matrix[4,4,4] = 1
+        final_matrix[3,5,4] = 2
+        final_matrix[4,5,4] = 3
+        final_module_positions = {1: (4, 4, 5), 2: (3, 5, 5), 3: (4, 5, 5)}
+        ogm = OccupancyGridMap(module_positions, final_module_positions, 3)
+        agent = RandomSearchAgent(max_steps=1000)
+
+        success = agent.search(ogm)
+
+        print(f"Search {'succeeded' if success else 'failed'} after {agent.steps_taken} steps")
+
+        self.assertTrue(success, msg=f"\nSuccess: {success}")
+
+    def test_configuration_case_1(self):
+        """
+        Basic configuration:
+        Module 1 — (4,4,4)
+        Module 2 — (4,5,4) <- articulation point
+        Module 3 — (5,5,4)
+
+        - Module 2 connects the other two; its removal disconnects the graph.
+        - Only modules 1 and 3 should have pivots.
+        """
+        # Define initial module positions
+        module_positions = {
+            1: (19, 3, 5),  # This will be recentered to the grid center
+            2: (19, 4, 5),
+            3: (19, 3, 6)
+        }
+
+        # Define final module positions
+        final_module_positions = {
+            1: (19, 3, 6),
+            2: (19, 3, 5),
+            3: (20, 3, 6)
+        }
+        
+        expected_recentered_module_positions = {
+            1: (4, 4, 4), 
+            2: (4, 5, 4), 
+            3: (4, 4, 5)
+        }
+
+        expected_recentered_final_module_positions = {
+            1: (4, 4, 4), 
+            2: (4, 4, 3), 
+            3: (5, 4, 4)
+        }
+        
+        self.assert_recentered_positions_match(module_positions, final_module_positions, expected_recentered_module_positions, expected_recentered_final_module_positions)
+        self.assert_random_agent_success()
+
+if __name__ == "__main__":
+    unittest.main()