diff --git a/include/Grid3D/grid3d.hpp b/include/Grid3D/grid3d.hpp
index a36b36b..ebe2e11 100644
--- a/include/Grid3D/grid3d.hpp
+++ b/include/Grid3D/grid3d.hpp
@@ -389,9 +389,16 @@ bool loadSemanticGrid() {
 		m_octomap->getMetricMin(minX, minY, minZ);
 		m_octomap->getMetricMax(maxX, maxY, maxZ);
 		res = m_octomap->getResolution();
+
 		m_maxX = (float)(maxX-minX);
 		m_maxY = (float)(maxY-minY);
 		m_maxZ = (float)(maxZ-minZ);
+
+		// ATLAS - 1 Floor - Close to origin
+		// m_maxX = (float)(maxX-minX)/2;
+		// m_maxY = (float)(maxY-minY)/2;
+		// m_maxZ = (float)(maxZ-minZ)/2;
+		// m_maxZ = m_maxZ + 1;
 		m_resolution = (float)res;
 		m_oneDivRes = 1.0/m_resolution;
 		ROS_INFO("Map size:\n");
@@ -558,6 +565,8 @@ bool loadSemanticGrid() {
 		m_gridSizeX = (int)(m_maxX*m_oneDivRes);
 		m_gridSizeY = (int)(m_maxY*m_oneDivRes); 
 		m_gridSizeZ = (int)(m_maxZ*m_oneDivRes);
+		// ATLAS 1 Floor - Close to origin
+		// m_gridSizeZ = (int)(m_maxZ*m_oneDivRes)+1;
 
 		// CONSIDERING DIMENSIONES OF THE SEMANTIC LAYER
 		// m_gridSizeX = (int)(212*m_oneDivRes); //1060
@@ -632,7 +641,7 @@ bool loadSemanticGrid() {
 		// 	}
 		// }
 
-		// VERSION OF 2024-MARCH
+		// VERSION OF 2024-MARCH WITH ATLAR FAR
 		for(int iz=0; iz<(m_gridSizeZ-2); iz++) //m_gridSizeZ-2?
 		{
 			for(int iy=0; iy<(m_gridSizeY-3); iy++) //m_gridSizeY-3?
@@ -644,33 +653,100 @@ bool loadSemanticGrid() {
 				}
 			}
 		}
+		// VERSION ATLAS CLOSE TO THE ORIGIN
+		// for(int iz=0; iz<(m_gridSizeZ); iz++) //m_gridSizeZ-2?
+		// {
+		// 	for(int iy=0; iy<(m_gridSizeY); iy++) //m_gridSizeY-3?
+		// 	{
+		// 		for(int ix=0; ix<(m_gridSizeX); ix++)
+		// 		{	
+		// 			index = ix + iy*m_gridStepY + iz*m_gridStepZ;		
+		// 			m_grid[index].semantic = 0;
+		// 		}
+		// 	}
+		// }
+
 		// int count_semant=0;
 		for(int i_seman=0; i_seman<index_size_semantic; i_seman++)
 		{
 			// std::cout << "semanticGrid_x: " << semanticGrid_x[i_seman] << std::endl;
 			index_semantic=semanticGrid_x[i_seman];//Correcto
 			// std::cout << "index_semantic: " << index_semantic << std::endl;
-			if (semanticGrid_v[i_seman] == 1)
+			if (semanticGrid_v[i_seman] == 1){
 				c1=c1+1;
-			else if (semanticGrid_v[i_seman] == 2)
+				// std::cout << "semantic: 1" << std::endl;
+				// usleep(1e4);
+				// std::cout << "Please a key to go to the next iteration..." << std::endl;
+				// getchar(); // Comentar para no usar tecla.
+			}	
+			else if (semanticGrid_v[i_seman] == 2){
 				c2=c2+1;
-			else if (semanticGrid_v[i_seman] == 3)
+				// std::cout << "semantic: 2" << std::endl;
+				// usleep(1e4);
+				// std::cout << "Please a key to go to the next iteration..." << std::endl;
+				// getchar(); // Comentar para no usar tecla.
+			}
+			else if (semanticGrid_v[i_seman] == 3){
 				c3=c3+1;
-			else if (semanticGrid_v[i_seman] == 4)
+				// std::cout << "semantic: 3" << std::endl;
+				// usleep(1e4);
+				// std::cout << "Please a key to go to the next iteration..." << std::endl;
+				// getchar(); // Comentar para no usar tecla.
+			}
+			else if (semanticGrid_v[i_seman] == 4){
 				c4=c4+1;
-			else if (semanticGrid_v[i_seman] == 5)
+				// std::cout << "semantic: 1" << std::endl;
+				// usleep(1e4);
+				// std::cout << "Please a key to go to the next iteration..." << std::endl;
+				// getchar(); // Comentar para no usar tecla.
+			}
+			else if (semanticGrid_v[i_seman] == 5){
 				c5=c5+1;
-			else if (semanticGrid_v[i_seman] == 6)
+				// std::cout << "semantic: 1" << std::endl;
+				// usleep(1e4);
+				// std::cout << "Please a key to go to the next iteration..." << std::endl;
+				// getchar(); // Comentar para no usar tecla.
+			}
+			else if (semanticGrid_v[i_seman] == 6){
 				c6=c6+1;
-			else if (semanticGrid_v[i_seman] == 7)
+				// std::cout << "semantic: 1" << std::endl;
+				// usleep(1e4);
+				// std::cout << "Please a key to go to the next iteration..." << std::endl;
+				// getchar(); // Comentar para no usar tecla.
+			}
+			else if (semanticGrid_v[i_seman] == 7){
 				c7=c7+1;
-			else if (semanticGrid_v[i_seman] == 8)
+				// std::cout << "semantic: 1" << std::endl;
+				// usleep(1e4);
+				// std::cout << "Please a key to go to the next iteration..." << std::endl;
+				// getchar(); // Comentar para no usar tecla.
+			}				
+			else if (semanticGrid_v[i_seman] == 8){
 				c8=c8+1;
-			else
+				// std::cout << "semantic: 1" << std::endl;
+				// usleep(1e4);
+				// std::cout << "Please a key to go to the next iteration..." << std::endl;
+				// getchar(); // Comentar para no usar tecla.
+			}
+			else{
 				c0=c0+1;
+				// std::cout << "semantic: 1" << std::endl;
+				// usleep(1e4);
+				// std::cout << "Please a key to go to the next iteration..." << std::endl;
+				// getchar(); // Comentar para no usar tecla.
+			}
+				
 			
 			m_grid[index_semantic].semantic = semanticGrid_v[i_seman];
 
+			// if (m_grid[index_semantic].semantic == 3){
+			// 	std::cout << "index: " << index_semantic << std::endl;
+			// 	std::cout << "semantic: " << m_grid[index_semantic].semantic << std::endl;
+			// 	usleep(1e4);
+			// 	std::cout << "Please a key to go to the next iteration..." << std::endl;
+			// 	getchar(); // Comentar para no usar tecla.
+			// }
+
 
 			// Check if index_semantic is out world_size
 			if (index_semantic > m_gridSize)
@@ -706,6 +782,12 @@ bool loadSemanticGrid() {
 		}
 		// std::cout << "count_semant: " << count_semant << std::endl;
 
+		std::cout << "semantic: " << semanticGrid_v[100] << std::endl;
+		std::cout << "indice: " << semanticGrid_x[100] << std::endl;
+		std::cout << "Y: " << semanticGrid_y[100] << std::endl;
+		std::cout << "Z: " << semanticGrid_z[100] << std::endl;
+		
+
 		for(int iz=0; iz<m_gridSizeZ; iz++)
 		{
 			for(int iy=0; iy<m_gridSizeY; iy++)
diff --git a/launch/grid3d.launch b/launch/grid3d.launch
index b8bf02e..e9d58c9 100644
--- a/launch/grid3d.launch
+++ b/launch/grid3d.launch
@@ -4,15 +4,20 @@
         <param name="world_size_y" value="169.2" />
         <param name="world_size_z" value="9.2" />
         <param name="resolution" value="0.2" /> -->
+        <!-- ATLAS - 1 FLOOR - CLOSE TO ORIGIN -->
+        <param name="world_size_x" value="186.0" />
+        <param name="world_size_y" value="45.2" />
+        <param name="world_size_z" value="8.4" />
+        <param name="resolution" value="0.2" />
         <!-- Casa CON muebles -->
         <!-- <param name="world_size_x" value="169.2" />
         <param name="world_size_y" value="102.2" />
         <param name="world_size_z" value="71.0" />
         <param name="resolution" value="0.2" /> -->
         <!-- Casa SIN muebles -->
-        <param name="world_size_x" value="170.4" />
+        <!-- <param name="world_size_x" value="170.4" />
         <param name="world_size_y" value="107.4" />
         <param name="world_size_z" value="71.2" />
-        <param name="resolution" value="0.2" />
+        <param name="resolution" value="0.2" /> -->
     </node>
 </launch>
diff --git a/launch/simulator_atlas.launch b/launch/simulator_atlas.launch
index a2c91d7..3080c5c 100644
--- a/launch/simulator_atlas.launch
+++ b/launch/simulator_atlas.launch
@@ -23,18 +23,19 @@
     <!-- ATLAS -->
     <!-- <arg name="map_name"            default="Atlas_8_300"/> -->
     
-    <!-- <arg name="map_name"            default="Atlas_8_puertas_02"/> -->
+    <arg name="map_name"            default="Atlas_8_puertas_02"/>
+    <!-- <arg name="map_name"            default="atlas_revit1000000000"/> -->
     <!-- <arg name="map_name"            default="casa_reducida5"/> -->
     <!-- <arg name="map_name"            default="casa_5mill"/> -->
-    <arg name="map_name"            default="casoplonv2_jac_5mill"/>
+    <!-- <arg name="map_name"            default="casoplonv2_jac_5mill"/> -->
 
     <!-- <arg name="map_name"            default="Atlas_8_floor_1kk"/> -->
     <!-- <arg name="map_name"            default="nube_de_puntos2"/> -->
     <!-- <arg name="map_name"            default="nube_de_puntos_con_etiquetas"/> -->
     <arg name="map"                 default="$(find heuristic_planners)/resources/3dmaps/$(arg map_name).bt"/>
 
-    <!-- <arg name="algorithm_name"      default="costlazythetastar"/> -->
-    <arg name="algorithm_name"      default="costastar"/>
+    <arg name="algorithm_name"      default="lazythetastar_semantic_cost"/>
+    <!-- <arg name="algorithm_name"      default="costastar"/> -->
 
     <!-- 212 -->
     <!-- <arg name="world_size_x"        default="211.8"/> -->
@@ -48,9 +49,14 @@
     <arg name="resolution"          default="0.2"/> -->
 
     <!-- Atlas -->
-    <!-- <arg name="world_size_x"        default="211.8"/>
+    <arg name="world_size_x"        default="211.8"/>
     <arg name="world_size_y"        default="169.2"/>
     <arg name="world_size_z"        default="9.2"/>
+    <arg name="resolution"          default="0.2"/>
+  <!-- Atlas 1 Floor - Close to origin-->
+    <!-- <arg name="world_size_x"        default="186.0"/>
+    <arg name="world_size_y"        default="45.2"/>
+    <arg name="world_size_z"        default="8.4"/>
     <arg name="resolution"          default="0.2"/> -->
 
     <!-- Casa CON muebles -->
@@ -60,10 +66,10 @@
     <arg name="resolution"          default="0.2"/> -->
 
     <!-- Casa SIN muebles-->
-    <arg name="world_size_x"        default="170.4"/>
+    <!-- <arg name="world_size_x"        default="170.4"/>
     <arg name="world_size_y"        default="107.4"/>
     <arg name="world_size_z"        default="71.2"/>
-    <arg name="resolution"          default="0.2"/>
+    <arg name="resolution"          default="0.2"/> -->
 
     <arg name="inflate_map"         default="true"/>
     <arg name="inflation_size"      default="$(arg resolution)"/>
diff --git a/scripts/grid3d_class.py b/scripts/grid3d_class.py
index 2bb71ef..53fdb8c 100644
--- a/scripts/grid3d_class.py
+++ b/scripts/grid3d_class.py
@@ -22,7 +22,8 @@ def __init__(self):
 
         rospack = rospkg.RosPack()
         # model_path = str(rospack.get_path('heuristic_planners') )+ str("/scripts/model.ifc")
-        model_path = str(rospack.get_path('heuristic_planners') )+ str("/scripts/casoplonv2.ifc")
+        model_path = str(rospack.get_path('heuristic_planners') )+ str("/scripts/atlas_1F.ifc")
+        # model_path = str(rospack.get_path('heuristic_planners') )+ str("/scripts/casoplonv2.ifc")
         self.model = ifcopenshell.open(model_path)
         self.settings = ifcopenshell.geom.settings()
         self.settings.set(self.settings.USE_WORLD_COORDS, True)
@@ -111,6 +112,9 @@ def __init__(self):
 
 
         # Verifica si el valor mínimo es negativo y suma su valor absoluto a todos los elementos
+        self.semantic_grid_x -= abs(min_x)
+        self.semantic_grid_y -= abs(min_y)
+        self.semantic_grid_z -= abs(min_z)
         if min_x < 0:
             print(f'Hay offset de X: {min_x}')
             self.semantic_grid_x += abs(min_x)
@@ -142,7 +146,8 @@ def __init__(self):
             self.semantic_grid_x[i] = int(index)
 
             
-
+        print(len(self.semantic_grid_x))
+        print((self.world_size_x/self.resolution)*(self.world_size_y/self.resolution)*(self.world_size_z/self.resolution))
 
     rospy.loginfo("Termina de rellenar las matrices")
 
diff --git a/src/Planners/LazyThetaStarSemanticCost.cpp b/src/Planners/LazyThetaStarSemanticCost.cpp
index 0fd22c2..6b51a62 100644
--- a/src/Planners/LazyThetaStarSemanticCost.cpp
+++ b/src/Planners/LazyThetaStarSemanticCost.cpp
@@ -224,7 +224,7 @@ namespace Planners
             // cost2=(c_wall/(1+static_cast<double>(_suc->cost)))*cost2; // + cost2;
             cost2=(c_wall/coste)*cost2;
 
-            // cost2=(c_wall/static_cast<double>(_suc->cost))*cost2 + cost2;
+            cost2=(c_wall/static_cast<double>(_suc->cost))*cost2 + cost2;
             // std::cout << "cost: " << _suc->cost << std::endl;
             // std::cout << "semantic: " << _suc->semantic << std::endl;
             // usleep(1e4);