diff --git a/src/SafetyAwarePlanner.cpp b/src/SafetyAwarePlanner.cpp
index b9fb3cf85cf9ec3b64071342b84e9436ff2605cc..460e9ae5efdee20eb5d37305c246c8a659916a8e 100644
--- a/src/SafetyAwarePlanner.cpp
+++ b/src/SafetyAwarePlanner.cpp
@@ -1,10 +1,5 @@
 #include <moveit/move_group_interface/move_group_interface.h>
 #include <moveit/planning_scene_interface/planning_scene_interface.h>
-
-#include <moveit_msgs/DisplayRobotState.h>
-#include <moveit_msgs/DisplayTrajectory.h>
-
-#include <moveit_msgs/AttachedCollisionObject.h>
 #include <moveit_msgs/CollisionObject.h>
 #include <moveit_visual_tools/moveit_visual_tools.h>
 
@@ -44,17 +39,26 @@ int main(int argc, char** argv)
     std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
               std::ostream_iterator<std::string>(std::cout, ", "));
 
-    shape_msgs::SolidPrimitive safetybox;
-    geometry_msgs::Pose box_pose;
-    safetybox.type = safetybox.BOX;
-    safetybox.dimensions.resize(3);
+    shape_msgs::SolidPrimitive safety_box;
+    geometry_msgs::Pose safety_box_pose;
+
+    safety_box_pose.orientation.w = 1.0;
+    safety_box_pose.orientation.x = 0.0;
+    safety_box_pose.orientation.y = 0.0;
+    safety_box_pose.orientation.z = 0.0;
+    safety_box_pose.position.x = 0.0;
+    safety_box_pose.position.y = 0.0;
+    safety_box_pose.position.z = 0.0;
+
+    safety_box.type = safety_box.BOX;
+    safety_box.dimensions.resize(3);
 
-    safetybox.dimensions[0] = 2.0;
-    safetybox.dimensions[1] = 2.0;
-    safetybox.dimensions[2] = 2.0;
+    safety_box.dimensions[0] = 2.0;
+    safety_box.dimensions[1] = 2.0;
+    safety_box.dimensions[2] = 2.0;
 
     // Safety Box
-    SafetyEnvironmentCreator sec(safetybox, box_pose);
+    SafetyEnvironmentCreator sec(safety_box, safety_box_pose);
     std::vector<moveit_msgs::CollisionObject> collision_objects = sec.createCentralizedSafetyEnvironment(move_group);
 
     //visual_tools.processCollisionObjectMsg(collision_objects[2], rviz_visual_tools::RED);
@@ -94,7 +98,6 @@ int main(int argc, char** argv)
 
     // Move the simulated robot in gazebo
     move_group.move();
-    move_group.stop();
 
     ros::shutdown();
     return 0;
diff --git a/src/SafetyEnvironmentCreator.cpp b/src/SafetyEnvironmentCreator.cpp
index 4e36529a174bb4845ab08f13ddb8ab7e3d25f1ea..f6ccedb8084b4e384add979d5a65d05fd07e0b7d 100644
--- a/src/SafetyEnvironmentCreator.cpp
+++ b/src/SafetyEnvironmentCreator.cpp
@@ -12,6 +12,13 @@ class SafetyEnvironmentCreator {
         double safety_zone_width;
         double safety_zone_height;
         double safety_zone_depth;
+        double zone_orientation_w;
+        double zone_orientation_x;
+        double zone_orientation_y;
+        double zone_orientation_z;
+        double zone_position_x;
+        double zone_position_y;
+        double zone_position_z;
 
     public:
         /**
@@ -26,28 +33,56 @@ class SafetyEnvironmentCreator {
                 safety_zone_depth = 0.0;
                 safety_zone_width = 0.0;
                 safety_zone_height = 0.0;
-            }else {
+                zone_orientation_w = 1.0;
+                zone_orientation_x = 1.0;
+                zone_orientation_y = 1.0;
+                zone_orientation_z = 1.0;
+                            }else {
                 safety_zone_depth = shape.dimensions[0];
                 safety_zone_width = shape.dimensions[1];
                 safety_zone_height = shape.dimensions[2];
+                zone_orientation_w = pose.orientation.w;
+                zone_orientation_x = pose.orientation.x;
+                zone_orientation_y = pose.orientation.y;
+                zone_orientation_z = pose.orientation.z;
+                zone_position_x = pose.position.x;
+                zone_position_y = pose.position.y;
+                zone_position_z = pose.position.z;
             }
         }
 
-        double getZoneWidth() {
+        double getZoneWidth() const {
             return safety_zone_width;
         }
 
-        double getZoneHeight() {
+        double getZoneHeight() const {
             return safety_zone_height;
         }
 
-        double getZoneDepth() {
+        double getZoneDepth() const {
             return safety_zone_depth;
         }
 
+        double getZoneOrientationW() const {
+            return zone_orientation_w;
+        }
+
+        double getZoneOrientationX() const {
+            return zone_orientation_x;
+        }
+
+        double getZoneOrientationY() const {
+            return zone_orientation_y;
+        }
+
+        double getZoneOrientationZ() const {
+            return zone_orientation_z;
+        }
+
          std::vector<moveit_msgs::CollisionObject> createCentralizedSafetyEnvironment(moveit::planning_interface::MoveGroupInterface &move_group) {
 
             std::vector<moveit_msgs::CollisionObject> collision_objects;
+
             // OBJECT 1 ( left box / -y )
             // ^^^^^^^^
             // Define a collision object ROS message.
@@ -69,9 +104,9 @@ class SafetyEnvironmentCreator {
             // Define a pose for the box (specified relative to frame_id)
             geometry_msgs::Pose box_pose;
             box_pose.orientation.w = 1.0;
-            box_pose.position.x = 0.0;
-            box_pose.position.y = -1 * ((getZoneWidth() / 2) + 0.05);
-            box_pose.position.z = primitive.dimensions[2] / 2;
+            box_pose.position.x = zone_position_x;
+            box_pose.position.y = zone_position_y + (-1 * ((getZoneWidth() / 2) + 0.05));
+            box_pose.position.z = zone_position_z + (primitive.dimensions[2] / 2);
 
             collision_object.primitives.push_back(primitive);
             collision_object.primitive_poses.push_back(box_pose);
@@ -98,9 +133,9 @@ class SafetyEnvironmentCreator {
             // Define a pose for the box (specified relative to frame_id)
             geometry_msgs::Pose box_pose_2;
             box_pose_2.orientation.w = 1.0;
-            box_pose_2.position.x = 0.0;
-            box_pose_2.position.y = ((getZoneWidth() / 2) + 0.05);
-            box_pose_2.position.z = primitive_2.dimensions[2] / 2;
+            box_pose_2.position.x = zone_position_x;
+            box_pose_2.position.y = zone_position_y + ((getZoneWidth() / 2) + 0.05);
+            box_pose_2.position.z = zone_position_z + (primitive_2.dimensions[2] / 2);
 
             collision_object_2.primitives.push_back(primitive_2);
             collision_object_2.primitive_poses.push_back(box_pose_2);
@@ -127,9 +162,9 @@ class SafetyEnvironmentCreator {
             // Define a pose for the box (specified relative to frame_id)
             geometry_msgs::Pose box_pose_3;
             box_pose_3.orientation.w = 1.0;
-            box_pose_3.position.x = ((getZoneDepth() / 2) + 0.05);
-            box_pose_3.position.y = 0.0;
-            box_pose_3.position.z = primitive_3.dimensions[2] / 2;
+            box_pose_3.position.x = zone_position_x + ((getZoneDepth() / 2) + 0.05);
+            box_pose_3.position.y = zone_position_y;
+            box_pose_3.position.z = zone_position_z + (primitive_3.dimensions[2] / 2);
 
             collision_object_3.primitives.push_back(primitive_3);
             collision_object_3.primitive_poses.push_back(box_pose_3);
@@ -156,9 +191,9 @@ class SafetyEnvironmentCreator {
             // Define a pose for the box (specified relative to frame_id)
             geometry_msgs::Pose box_pose_4;
             box_pose_4.orientation.w = 1.0;
-            box_pose_4.position.x = -1 * ((getZoneDepth() / 2) + 0.05);
-            box_pose_4.position.y = 0.0;
-            box_pose_4.position.z = primitive_4.dimensions[2] / 2;
+            box_pose_4.position.x = zone_position_x + (-1 * ((getZoneDepth() / 2) + 0.05));
+            box_pose_4.position.y = zone_position_y;
+            box_pose_4.position.z = zone_position_z + (primitive_4.dimensions[2] / 2);
 
             collision_object_4.primitives.push_back(primitive_4);
             collision_object_4.primitive_poses.push_back(box_pose_4);
@@ -185,9 +220,9 @@ class SafetyEnvironmentCreator {
             // Define a pose for the box (specified relative to frame_id)
             geometry_msgs::Pose box_pose_5;
             box_pose_5.orientation.w = 1.0;
-            box_pose_5.position.x = 0.0;
-            box_pose_5.position.y = 0.0;
-            box_pose_5.position.z = -0.05;
+            box_pose_5.position.x = zone_position_x;
+            box_pose_5.position.y = zone_position_y;
+            box_pose_5.position.z = zone_position_z - 0.05;
 
             collision_object_5.primitives.push_back(primitive_5);
             collision_object_5.primitive_poses.push_back(box_pose_5);
@@ -195,13 +230,13 @@ class SafetyEnvironmentCreator {
 
             collision_objects.push_back(collision_object_5);
 
-             // OBJECT 5 (floor / -z)
+             // OBJECT 6 (top / +z)
              // ^^^^^^^^
              moveit_msgs::CollisionObject collision_object_6;
              collision_object_6.header.frame_id = move_group.getPlanningFrame();
 
              // The id of the object is used to identify it.
-             collision_object_6.id = "box5";
+             collision_object_6.id = "box6";
 
              // Define a box to add to the world.
              shape_msgs::SolidPrimitive primitive_6;
@@ -214,16 +249,22 @@ class SafetyEnvironmentCreator {
              // Define a pose for the box (specified relative to frame_id)
              geometry_msgs::Pose box_pose_6;
              box_pose_6.orientation.w = 1.0;
-             box_pose_6.position.x = 0.0;
-             box_pose_6.position.y = 0.0;
-             box_pose_6.position.z = getZoneHeight() + 0.05;
+             box_pose_6.position.x = zone_position_x;
+             box_pose_6.position.y = zone_position_y;
+             box_pose_6.position.z = zone_position_z + getZoneHeight() + 0.05;
 
              collision_object_6.primitives.push_back(primitive_6);
              collision_object_6.primitive_poses.push_back(box_pose_6);
              collision_object_6.operation = collision_object_6.ADD;
-
              collision_objects.push_back(collision_object_6);
 
+             /*for(std::size_t i=0; i<collision_objects.size(); ++i) {
+                 collision_objects[i].primitive_poses[0].orientation.w = getZoneOrientationW();
+                 collision_objects[i].primitive_poses[0].orientation.x = getZoneOrientationX();
+                 collision_objects[i].primitive_poses[0].orientation.y = getZoneOrientationY();
+                 collision_objects[i].primitive_poses[0].orientation.z = getZoneOrientationZ();
+             }*/
+
             return collision_objects;
         }
 };
\ No newline at end of file