diff --git a/cmake-build-debug/CMakeFiles/worldsafety.dir/src/worldsafety.cpp.o b/cmake-build-debug/CMakeFiles/worldsafety.dir/src/worldsafety.cpp.o
index c30872c21a909016c501db3da5d00f1c1f45ef2c..8ef7f5659347b1ccaefed80b07a397e443c6b094 100644
Binary files a/cmake-build-debug/CMakeFiles/worldsafety.dir/src/worldsafety.cpp.o and b/cmake-build-debug/CMakeFiles/worldsafety.dir/src/worldsafety.cpp.o differ
diff --git a/cmake-build-debug/devel/lib/robot_models/worldsafety b/cmake-build-debug/devel/lib/robot_models/worldsafety
index 9bacd014d2bdf204e0dcb0720e6a3d618ab81401..9480aeac853a91ab37931945cedee719dbd381f2 100755
Binary files a/cmake-build-debug/devel/lib/robot_models/worldsafety and b/cmake-build-debug/devel/lib/robot_models/worldsafety differ
diff --git a/src/robot_models_node.cpp b/src/robot_models_node.cpp
index 5afa4ea8e103528533a15d061172fc3ea22fee8d..c1dd3347fc0e7872d35bd13e3ea3e8c080d40672 100644
--- a/src/robot_models_node.cpp
+++ b/src/robot_models_node.cpp
@@ -3,7 +3,6 @@
 #include "datalink/graspobject.h"
 #include "datalink/hand.h"
 #include "datalink/humanspace.h"
-#include "datalink/jo.h"
 #include "datalink/obstacle.h"
 #include "datalink/robot.h"
 #include "datalink/robotfpe.h"
@@ -27,275 +26,277 @@
 #include <tf2/LinearMath/Quaternion.h>
 #include <moveit/move_group_interface/move_group_interface.h>
 #include <moveit/planning_scene_interface/planning_scene_interface.h>
-
-void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg)
-{
-  ROS_INFO("I heard: [%s]", msg->data.c_str());
+#include <std_msgs/UInt32.h>
+void updateDectectedHumans(const std_msgs::UInt32::ConstPtr &msg) {
+    ROS_INFO("I heard: [%i]", msg->data.c_str());
 }
 
-    int main(int argc, char** argv) {
-        ros::init(argc, argv, "ROSNODE1");
-        ros::NodeHandle node_handle("namespace_name");
-        ros::AsyncSpinner spinner(1);
-        spinner.start();
-        ROS_INFO("HELLO, WORLD");
-
-        abstractobject o1;
-        graspobject o2;
-        hand o3;
-        humanspace o4;
-        joint o5a;
-        joint o5b;
-        joint o5c;
-        joint o5d;
-        joint o5e;
-        joint o5f;
-        joint o5g;
-        obstacle o6;
-        robot robot1;
-        robotfpe o8;
-        world o9;
-        worldobject o10;
-
-        robot1.jo.push_back(o5a);
-        robot1.jo.push_back(o5b);
-        robot1.jo.push_back(o5c);
-        robot1.jo.push_back(o5d);
-        robot1.jo.push_back(o5e);
-        robot1.jo.push_back(o5f);
-        robot1.jo.push_back(o5g);
-        /*robot1.jo1->name= "panda_link1";
-        robot1.jo2->name= "panda_link2";
-        robot1.jo3->name= "panda_link3";
-        robot1.jo4->name= "panda_link4";
-        robot1.jo5->name= "panda_link5";
-        robot1.jo6->name= "panda_link6";
-        robot1.jrobot1->name= "panda_link7";
-
-        robot1.jo1->or2= {1.2,5.3,6.5,7.4};
-        robot1.jo2->or2= {2.5,3.6,5.8,4.9};
-        robot1.jo3->or2= {5.1,1.2,9.9,6.8};
-        robot1.jo4->or2= {6.3,3.4,8.5,9.6};
-        robot1.jo5->or2= {8.2,2.6,7.9,5.5};
-        robot1.jo6->or2= {7.3,4.4,6.7,4.0};
-        robot1.jrobot1->or2= {7.3,4.4,6.7,4.0};
-
-        robot1.jo1->p1= {1.3, 10.1, 9.4};
-        robot1.jo2->p1= {14.7, 11.3, 6.5};
-        robot1.jo3->p1= {4.9, 13.8, 7.7};
-        robot1.jo4->p1= {2.4, 12.5, 1.6};
-        robot1.jo5->p1= {3.1, 16.2, 5.3};
-        robot1.jo6->p1= {11.5, 15.4, 3.4};
-        robot1.jrobot1->p1= {5.1, 18.3, 4.2};*/
-
-        int yx=0;
-        robot1.jo[++yx].name= "panda_link1";
-        robot1.jo[++yx].name= "panda_link2";
-        robot1.jo[++yx].name= "panda_link3";
-        robot1.jo[++yx].name= "panda_link4";
-        robot1.jo[++yx].name= "panda_link5";
-        robot1.jo[++yx].name= "panda_link6";
-        robot1.jo[++yx].name= "panda_link7";
-
-
-        o9.c->robname="Robot FPE";
-        o9.c->of=true;
-        o9.c->moving=false;
-        o9.c->grasppos=true;
-        o9.c->planninggrp= "Panda Teaching";
-
-
-        o8.h->grippos= {12.1, 14.3, 18.2};
-
-        o9.a->humannum = 1;
-        o9.b1->name="Cube";
-        o9.b1->size={5.2, 7.5, 9.1};
-        o9.b1->p1= {13.2, 14.1, 17.5};
-        o9.b1->x= 2.3;
-        o9.b1->y = 1.2;
-        o9.b1->z = 0.2;
-        o9.b1->w = 0.1;
-
-        o9.b2->name="Ball";
-        o9.b2->size={1.3, 2.1, 3.2};
-        o9.b2->p1= {12.5, 16.4, 17.3};
-        o9.b2->x= 1.1;
-        o9.b2->y = 1.4;
-        o9.b2->z = 1.6;
-        o9.b2->w = 1.9;
-
-        ros::Publisher chatter_pub = namespace_name.advertise<std_msgs::String>("chatter", 1000);
-        ros::Rate loop_rate(10);
-        int count = 0;
-        while (ros::ok()) {
-            std_msgs::uint32 msg;
-            std::stringstream ss;
-            ss << "hello world " << count;
-            msg.data = ss.str();
-            ROS_INFO("%s", msg.data.c_str());
-            chatter_pub.publish(msg);
-            ros::spinOnce();
-            loop_rate.sleep();
-            ++count;
-        }
-
-        ros::Subscriber sub = namespace_name.subscribe("chatter", 1000, updateDectectedHumans);
-        ros::spin();
-
-        tf2_ros::Buffer tfBuffer;
-        tf2_ros::TransformListener tfListener(tfBuffer);
-        ros::Rate rate(10);
-
-        while (node_handle.ok()) {
-            //for (const auto &topic : ROSNODE1::topics)
-            {
-                geometry_msgs::TransformStamped transformStamped1;
-                geometry_msgs::TransformStamped transformStamped2;
-                geometry_msgs::TransformStamped transformStamped3;
-                geometry_msgs::TransformStamped transformStamped4;
-                geometry_msgs::TransformStamped transformStamped5;
-                geometry_msgs::TransformStamped transformStamped6;
-                geometry_msgs::TransformStamped transformStamped7;
-                uint32::data updateDetectedHumans;
-                geometry_msgs::uint32 x;
-                try {
-                    transformStamped1 = tfBuffer.lookupTransform("world", robot1.jo[1].name, ros::Time(0));
-                    transformStamped2 = tfBuffer.lookupTransform("world", robot1.jo[2].name, ros::Time(0));
-                    transformStamped3 = tfBuffer.lookupTransform("world", robot1.jo[3].name, ros::Time(0));
-                    transformStamped4 = tfBuffer.lookupTransform("world", robot1.jo[4].name, ros::Time(0));
-                    transformStamped5 = tfBuffer.lookupTransform("world", robot1.jo[5].name, ros::Time(0));
-                    transformStamped6 = tfBuffer.lookupTransform("world", robot1.jo[6].name, ros::Time(0));
-                    transformStamped7 = tfBuffer.lookupTransform("world", robot1.jo[7].name, ros::Time(0));
-                    updateDetectedHumans=o9.a->humannum;
-                } catch (tf2::TransformException &ex) {
-                    ROS_WARN("%s", ex.what());
-                    ros::Duration(0.1).sleep();
-                    continue;
-                }
-
-                robot1.jo[3].p1[0]=0.0;
-                robot1.jo[3].p1[1]=0.0;
-                robot1.jo[3].p1[2]=0.0;
-                robot1.jo[4].p1[0]=0.0;
-                robot1.jo[4].p1[1]=0.0;
-                robot1.jo[4].p1[2]=0.0;
-                robot1.jo[5].p1[0]=0.0;
-                robot1.jo[5].p1[1]=0.0;
-                robot1.jo[5].p1[2]=0.0;
-                robot1.jo[6].p1[0]=0.0;
-                robot1.jo[6].p1[1]=0.0;
-                robot1.jo[6].p1[2]=0.0;
-                robot1.jo[7].p1[0]=0.0;
-                robot1.jo[7].p1[1]=0.0;
-                robot1.jo[7].p1[2]=0.0;
-
-                robot1.jo[1].p1[0]=transformStamped1.transform.translation.x;
-                robot1.jo[1].p1[1]=transformStamped1.transform.translation.y;
-                robot1.jo[1].p1[2]=transformStamped1.transform.translation.z;
-                ROS_INFO_STREAM("panda_link1 Position is " <<" x = "<< robot1.jo[1].p1[0] << ", y = "<< robot1.jo[1].p1[1]<<", z = "<<robot1.jo[1].p1[2]);
-
-                robot1.jo[2].p1[0]=transformStamped2.transform.translation.x;
-                robot1.jo[2].p1[1]=transformStamped2.transform.translation.y;
-                robot1.jo[2].p1[2]=transformStamped2.transform.translation.z;
-                ROS_INFO_STREAM("panda_link2 Position is " <<" x = "<< robot1.jo[2].p1[0] << ", y = "<< robot1.jo[2].p1[1]<<", z = "<<robot1.jo[2].p1[2]);
-
-                robot1.jo[3].p1[0]=transformStamped3.transform.translation.x;
-                robot1.jo[3].p1[1]=transformStamped3.transform.translation.y;
-                robot1.jo[3].p1[2]=transformStamped3.transform.translation.z;
-                ROS_INFO_STREAM("panda_link3 Position is " <<" x = "<< robot1.jo[3].p1[0] << ", y = "<< robot1.jo[3].p1[1]<<", z = "<<robot1.jo[3].p1[2]);
-
-                robot1.jo[4].p1[0]=transformStamped4.transform.translation.x;
-                robot1.jo[4].p1[1]=transformStamped4.transform.translation.y;
-                robot1.jo[4].p1[2]=transformStamped4.transform.translation.z;
-                ROS_INFO_STREAM("panda_link4 Position is " <<" x = "<< robot1.jo[4].p1[0] << ", y = "<< robot1.jo[4].p1[1]<<", z = "<<robot1.jo[4].p1[2]);
-
-                robot1.jo[5].p1[0]=transformStamped5.transform.translation.x;
-                robot1.jo[5].p1[1]=transformStamped5.transform.translation.y;
-                robot1.jo[5].p1[2]=transformStamped5.transform.translation.z;
-                ROS_INFO_STREAM("panda_link5 Position is " <<" x = "<< robot1.jo[5].p1[0] << ", y = "<< robot1.jo[5].p1[1]<<", z = "<<robot1.jo[5].p1[2]);
-
-                robot1.jo[6].p1[0]=transformStamped6.transform.translation.x;
-                robot1.jo[6].p1[1]=transformStamped6.transform.translation.y;
-                robot1.jo[6].p1[2]=transformStamped6.transform.translation.z;
-                ROS_INFO_STREAM("panda_link6 Position is " <<" x = "<< robot1.jo[6].p1[0] << ", y = "<< robot1.jo[6].p1[1]<<", z = "<<robot1.jo[6].p1[2]);
-
-                robot1.jo[7].p1[0]=transformStamped7.transform.translation.x;
-                robot1.jo[7].p1[1]=transformStamped7.transform.translation.y;
-                robot1.jo[7].p1[2]=transformStamped7.transform.translation.z;
-                ROS_INFO_STREAM("panda_link7 Position is " <<" x = "<< robot1.jo[7].p1[0] << ", y = "<< robot1.jo[7].p1[1]<<", z = "<<robot1.jo[7].p1[2]);
-
-                robot1.jo[1].or2.w=0.0;
-                robot1.jo[1].or2.x=0.0;
-                robot1.jo[1].or2.y=0.0;
-                robot1.jo[1].or2.z=0.0;
-                robot1.jo[2].or2.w=0.0;
-                robot1.jo[2].or2.x=0.0;
-                robot1.jo[2].or2.y=0.0;
-                robot1.jo[2].or2.z=0.0;
-                robot1.jo[3].or2.w=0.0;
-                robot1.jo[3].or2.x=0.0;
-                robot1.jo[3].or2.y=0.0;
-                robot1.jo[3].or2.z=0.0;
-                robot1.jo[4].or2.w=0.0;
-                robot1.jo[4].or2.x=0.0;
-                robot1.jo[4].or2.y=0.0;
-                robot1.jo[4].or2.z=0.0;
-                robot1.jo[5].or2.w=0.0;
-                robot1.jo[5].or2.x=0.0;
-                robot1.jo[5].or2.y=0.0;
-                robot1.jo[5].or2.z=0.0;
-                robot1.jo[6].or2.w=0.0;
-                robot1.jo[6].or2.x=0.0;
-                robot1.jo[6].or2.y=0.0;
-                robot1.jo[6].or2.z=0.0;
-                robot1.jo[7].or2.w=0.0;
-                robot1.jo[7].or2.x=0.0;
-                robot1.jo[7].or2.y=0.0;
-                robot1.jo[7].or2.z=0.0;
-
-                robot1.jo[1].or2.w=transformStamped1.transform.rotation.w;
-                robot1.jo[1].or2.x=transformStamped1.transform.rotation.x;
-                robot1.jo[1].or2.y=transformStamped1.transform.rotation.y;
-                robot1.jo[1].or2.z=transformStamped1.transform.rotation.z;
-                ROS_INFO_STREAM("panda_link1 Orientation is " <<" w = "<< robot1.jo[1].or2.w<<", x = "<<robot1.jo[1].or2.x<<", y = "<<robot1.jo[1].or2.y<<", z = "<<robot1.jo[1].or2.z);
-
-                robot1.jo[2].or2.w=transformStamped2.transform.rotation.w;
-                robot1.jo[2].or2.x=transformStamped2.transform.rotation.x;
-                robot1.jo[2].or2.y=transformStamped2.transform.rotation.y;
-                robot1.jo[2].or2.z=transformStamped2.transform.rotation.z;
-                ROS_INFO_STREAM("panda_link2 Orientation is " <<" w = "<<  robot1.jo[2].or2.w<<", x = "<< robot1.jo[2].or2.x<<", y = "<< robot1.jo[2].or2.y<<", z = "<< robot1.jo[2].or2.z);
-
-                robot1.jo[3].or2.w=transformStamped3.transform.rotation.w;
-                robot1.jo[3].or2.x=transformStamped3.transform.rotation.x;
-                robot1.jo[3].or2.y=transformStamped3.transform.rotation.y;
-                robot1.jo[3].or2.z=transformStamped3.transform.rotation.z;
-                ROS_INFO_STREAM("panda_link3 Orientation is " <<" w = "<< robot1.jo[3].or2.w<<", x = "<<robot1.jo[3].or2.x<<", y = "<<robot1.jo[3].or2.y<<", z = "<<robot1.jo[3].or2.z);
-
-                robot1.jo[4].or2.w=transformStamped4.transform.rotation.w;
-                robot1.jo[4].or2.x=transformStamped4.transform.rotation.x;
-                robot1.jo[4].or2.y=transformStamped4.transform.rotation.y;
-                robot1.jo[4].or2.z=transformStamped4.transform.rotation.z;
-                ROS_INFO_STREAM("panda_link4 Orientation is " <<" w = "<<robot1.jo[4].or2.w<<", x = "<<robot1.jo[4].or2.x<<", y = "<<robot1.jo[4].or2.y<<", z = "<<robot1.jo[4].or2.z);
-
-                robot1.jo[5].or2.w=transformStamped5.transform.rotation.w;
-                robot1.jo[5].or2.x=transformStamped5.transform.rotation.x;
-                robot1.jo[5].or2.y=transformStamped5.transform.rotation.y;
-                robot1.jo[5].or2.z=transformStamped5.transform.rotation.z;
-                ROS_INFO_STREAM("panda_link5 Orientation is " <<" w = "<< robot1.jo[5].or2.w<<", x = "<<robot1.jo[5].or2.x<<", y = "<<robot1.jo[5].or2.y<<", z = "<<robot1.jo[5].or2.z);
-
-                robot1.jo[6].or2.w=transformStamped6.transform.rotation.w;
-                robot1.jo[6].or2.x=transformStamped6.transform.rotation.x;
-                robot1.jo[6].or2.y=transformStamped6.transform.rotation.y;
-                robot1.jo[6].or2.z=transformStamped6.transform.rotation.z;
-                ROS_INFO_STREAM("panda_link6 Orientation is " <<" w = "<< robot1.jo[6].or2.w<<", x = "<<robot1.jo[6].or2.x<<", y = "<<robot1.jo[6].or2.y<<", z = "<<robot1.jo[6].or2.z);
-
-                robot1.jo[7].or2.w=transformStamped7.transform.rotation.w;
-                robot1.jo[7].or2.x=transformStamped7.transform.rotation.x;
-                robot1.jo[7].or2.y=transformStamped7.transform.rotation.y;
-                robot1.jo[7].or2.z=transformStamped7.transform.rotation.z;
-                ROS_INFO_STREAM("panda_link7 Orientation is " <<" w = "<<robot1.jo[7].or2.w<<", x = "<<robot1.jo[7].or2.x<<", y = "<<robot1.jo[7].or2.y<<", z = "<<robot1.jo[7].or2.z);
-              }
-            rate.sleep();
+int main(int argc, char **argv) {
+    ros::init(argc, argv, "ROSNODE1");
+    ros::NodeHandle node_handle("namespace_name");
+    ros::AsyncSpinner spinner(1);
+    spinner.start();
+    ROS_INFO("HELLO, WORLD");
+
+    abstractobject o1;
+    graspobject o2;
+    hand o3;
+    humanspace o4;
+    joint o5a;
+    joint o5b;
+    joint o5c;
+    joint o5d;
+    joint o5e;
+    joint o5f;
+    joint o5g;
+    obstacle o6;
+    robot robot1;
+    robotfpe o8;
+    world o9;
+    worldobject o10;
+
+    robot1.jo.push_back(o5a);
+    robot1.jo.push_back(o5b);
+    robot1.jo.push_back(o5c);
+    robot1.jo.push_back(o5d);
+    robot1.jo.push_back(o5e);
+    robot1.jo.push_back(o5f);
+    robot1.jo.push_back(o5g);
+
+    int yx = 0;
+    robot1.jo[++yx].name = "panda_link1";
+    robot1.jo[++yx].name = "panda_link2";
+    robot1.jo[++yx].name = "panda_link3";
+    robot1.jo[++yx].name = "panda_link4";
+    robot1.jo[++yx].name = "panda_link5";
+    robot1.jo[++yx].name = "panda_link6";
+    robot1.jo[++yx].name = "panda_link7";
+
+    o9.c->robname = "Robot FPE";
+    o9.c->of = true;
+    o9.c->moving = false;
+    o9.c->grasppos = true;
+    o9.c->planninggrp = "Panda Teaching";
+
+    o8.h->grippos = {12.1, 14.3, 18.2};
+
+    o9.a->humannum = 1;
+    o9.b1->name = "Cube";
+    o9.b1->size = {5.2, 7.5, 9.1};
+    o9.b1->p1 = {13.2, 14.1, 17.5};
+    o9.b1->x = 2.3;
+    o9.b1->y = 1.2;
+    o9.b1->z = 0.2;
+    o9.b1->w = 0.1;
+
+    o9.b2->name = "Ball";
+    o9.b2->size = {1.3, 2.1, 3.2};
+    o9.b2->p1 = {12.5, 16.4, 17.3};
+    o9.b2->x = 1.1;
+    o9.b2->y = 1.4;
+    o9.b2->z = 1.6;
+    o9.b2->w = 1.9;
+
+    ros::Publisher chatter_pub = namespace_name.advertise<std_msgs::String>("chatter", 1000);
+    ros::Rate loop_rate(10);
+    int count = 0;
+    while (ros::ok()) {
+        std_msgs::UInt32 msg;
+        std::stringstream ss;
+        ss << "hello world " << count;
+        msg.data = ss.str();
+        ROS_INFO("%s", msg.data.c_str());
+        chatter_pub.publish(msg);
+        ros::spinOnce();
+        loop_rate.sleep();
+        ++count;
+    }
+
+    ros::Subscriber sub = namespace_name.subscribe("chatter", 1000, updateDectectedHumans);
+    ros::spin();
+
+    tf2_ros::Buffer tfBuffer;
+    tf2_ros::TransformListener tfListener(tfBuffer);
+    ros::Rate rate(10);
+
+    while (node_handle.ok()) {
+        //for (const auto &topic : ROSNODE1::topics)
+        {
+            geometry_msgs::TransformStamped transformStamped1;
+            geometry_msgs::TransformStamped transformStamped2;
+            geometry_msgs::TransformStamped transformStamped3;
+            geometry_msgs::TransformStamped transformStamped4;
+            geometry_msgs::TransformStamped transformStamped5;
+            geometry_msgs::TransformStamped transformStamped6;
+            geometry_msgs::TransformStamped transformStamped7;
+            UInt32::data updateDetectedHumans;
+            geometry_msgs::UInt32 x;
+            try {
+                transformStamped1 = tfBuffer.lookupTransform("world", robot1.jo[1].name, ros::Time(0));
+                transformStamped2 = tfBuffer.lookupTransform("world", robot1.jo[2].name, ros::Time(0));
+                transformStamped3 = tfBuffer.lookupTransform("world", robot1.jo[3].name, ros::Time(0));
+                transformStamped4 = tfBuffer.lookupTransform("world", robot1.jo[4].name, ros::Time(0));
+                transformStamped5 = tfBuffer.lookupTransform("world", robot1.jo[5].name, ros::Time(0));
+                transformStamped6 = tfBuffer.lookupTransform("world", robot1.jo[6].name, ros::Time(0));
+                transformStamped7 = tfBuffer.lookupTransform("world", robot1.jo[7].name, ros::Time(0));
+                updateDetectedHumans = o9.a->humannum;
+            } catch (tf2::TransformException &ex) {
+                ROS_WARN("%s", ex.what());
+                ros::Duration(0.1).sleep();
+                continue;
+            }
+
+            robot1.jo[3].p1[0] = 0.0;
+            robot1.jo[3].p1[1] = 0.0;
+            robot1.jo[3].p1[2] = 0.0;
+            robot1.jo[4].p1[0] = 0.0;
+            robot1.jo[4].p1[1] = 0.0;
+            robot1.jo[4].p1[2] = 0.0;
+            robot1.jo[5].p1[0] = 0.0;
+            robot1.jo[5].p1[1] = 0.0;
+            robot1.jo[5].p1[2] = 0.0;
+            robot1.jo[6].p1[0] = 0.0;
+            robot1.jo[6].p1[1] = 0.0;
+            robot1.jo[6].p1[2] = 0.0;
+            robot1.jo[7].p1[0] = 0.0;
+            robot1.jo[7].p1[1] = 0.0;
+            robot1.jo[7].p1[2] = 0.0;
+
+            robot1.jo[1].p1[0] = transformStamped1.transform.translation.x;
+            robot1.jo[1].p1[1] = transformStamped1.transform.translation.y;
+            robot1.jo[1].p1[2] = transformStamped1.transform.translation.z;
+            ROS_INFO_STREAM(
+                    "panda_link1 Position is " << " x = " << robot1.jo[1].p1[0] << ", y = " << robot1.jo[1].p1[1]
+                                               << ", z = " << robot1.jo[1].p1[2]);
+
+            robot1.jo[2].p1[0] = transformStamped2.transform.translation.x;
+            robot1.jo[2].p1[1] = transformStamped2.transform.translation.y;
+            robot1.jo[2].p1[2] = transformStamped2.transform.translation.z;
+            ROS_INFO_STREAM(
+                    "panda_link2 Position is " << " x = " << robot1.jo[2].p1[0] << ", y = " << robot1.jo[2].p1[1]
+                                               << ", z = " << robot1.jo[2].p1[2]);
+
+            robot1.jo[3].p1[0] = transformStamped3.transform.translation.x;
+            robot1.jo[3].p1[1] = transformStamped3.transform.translation.y;
+            robot1.jo[3].p1[2] = transformStamped3.transform.translation.z;
+            ROS_INFO_STREAM(
+                    "panda_link3 Position is " << " x = " << robot1.jo[3].p1[0] << ", y = " << robot1.jo[3].p1[1]
+                                               << ", z = " << robot1.jo[3].p1[2]);
+
+            robot1.jo[4].p1[0] = transformStamped4.transform.translation.x;
+            robot1.jo[4].p1[1] = transformStamped4.transform.translation.y;
+            robot1.jo[4].p1[2] = transformStamped4.transform.translation.z;
+            ROS_INFO_STREAM(
+                    "panda_link4 Position is " << " x = " << robot1.jo[4].p1[0] << ", y = " << robot1.jo[4].p1[1]
+                                               << ", z = " << robot1.jo[4].p1[2]);
+
+            robot1.jo[5].p1[0] = transformStamped5.transform.translation.x;
+            robot1.jo[5].p1[1] = transformStamped5.transform.translation.y;
+            robot1.jo[5].p1[2] = transformStamped5.transform.translation.z;
+            ROS_INFO_STREAM(
+                    "panda_link5 Position is " << " x = " << robot1.jo[5].p1[0] << ", y = " << robot1.jo[5].p1[1]
+                                               << ", z = " << robot1.jo[5].p1[2]);
+
+            robot1.jo[6].p1[0] = transformStamped6.transform.translation.x;
+            robot1.jo[6].p1[1] = transformStamped6.transform.translation.y;
+            robot1.jo[6].p1[2] = transformStamped6.transform.translation.z;
+            ROS_INFO_STREAM(
+                    "panda_link6 Position is " << " x = " << robot1.jo[6].p1[0] << ", y = " << robot1.jo[6].p1[1]
+                                               << ", z = " << robot1.jo[6].p1[2]);
+
+            robot1.jo[7].p1[0] = transformStamped7.transform.translation.x;
+            robot1.jo[7].p1[1] = transformStamped7.transform.translation.y;
+            robot1.jo[7].p1[2] = transformStamped7.transform.translation.z;
+            ROS_INFO_STREAM(
+                    "panda_link7 Position is " << " x = " << robot1.jo[7].p1[0] << ", y = " << robot1.jo[7].p1[1]
+                                               << ", z = " << robot1.jo[7].p1[2]);
+
+            robot1.jo[1].or2.w = 0.0;
+            robot1.jo[1].or2.x = 0.0;
+            robot1.jo[1].or2.y = 0.0;
+            robot1.jo[1].or2.z = 0.0;
+            robot1.jo[2].or2.w = 0.0;
+            robot1.jo[2].or2.x = 0.0;
+            robot1.jo[2].or2.y = 0.0;
+            robot1.jo[2].or2.z = 0.0;
+            robot1.jo[3].or2.w = 0.0;
+            robot1.jo[3].or2.x = 0.0;
+            robot1.jo[3].or2.y = 0.0;
+            robot1.jo[3].or2.z = 0.0;
+            robot1.jo[4].or2.w = 0.0;
+            robot1.jo[4].or2.x = 0.0;
+            robot1.jo[4].or2.y = 0.0;
+            robot1.jo[4].or2.z = 0.0;
+            robot1.jo[5].or2.w = 0.0;
+            robot1.jo[5].or2.x = 0.0;
+            robot1.jo[5].or2.y = 0.0;
+            robot1.jo[5].or2.z = 0.0;
+            robot1.jo[6].or2.w = 0.0;
+            robot1.jo[6].or2.x = 0.0;
+            robot1.jo[6].or2.y = 0.0;
+            robot1.jo[6].or2.z = 0.0;
+            robot1.jo[7].or2.w = 0.0;
+            robot1.jo[7].or2.x = 0.0;
+            robot1.jo[7].or2.y = 0.0;
+            robot1.jo[7].or2.z = 0.0;
+
+            robot1.jo[1].or2.w = transformStamped1.transform.rotation.w;
+            robot1.jo[1].or2.x = transformStamped1.transform.rotation.x;
+            robot1.jo[1].or2.y = transformStamped1.transform.rotation.y;
+            robot1.jo[1].or2.z = transformStamped1.transform.rotation.z;
+            ROS_INFO_STREAM(
+                    "panda_link1 Orientation is " << " w = " << robot1.jo[1].or2.w << ", x = " << robot1.jo[1].or2.x
+                                                  << ", y = " << robot1.jo[1].or2.y << ", z = " << robot1.jo[1].or2.z);
+
+            robot1.jo[2].or2.w = transformStamped2.transform.rotation.w;
+            robot1.jo[2].or2.x = transformStamped2.transform.rotation.x;
+            robot1.jo[2].or2.y = transformStamped2.transform.rotation.y;
+            robot1.jo[2].or2.z = transformStamped2.transform.rotation.z;
+            ROS_INFO_STREAM(
+                    "panda_link2 Orientation is " << " w = " << robot1.jo[2].or2.w << ", x = " << robot1.jo[2].or2.x
+                                                  << ", y = " << robot1.jo[2].or2.y << ", z = " << robot1.jo[2].or2.z);
+
+            robot1.jo[3].or2.w = transformStamped3.transform.rotation.w;
+            robot1.jo[3].or2.x = transformStamped3.transform.rotation.x;
+            robot1.jo[3].or2.y = transformStamped3.transform.rotation.y;
+            robot1.jo[3].or2.z = transformStamped3.transform.rotation.z;
+            ROS_INFO_STREAM(
+                    "panda_link3 Orientation is " << " w = " << robot1.jo[3].or2.w << ", x = " << robot1.jo[3].or2.x
+                                                  << ", y = " << robot1.jo[3].or2.y << ", z = " << robot1.jo[3].or2.z);
+
+            robot1.jo[4].or2.w = transformStamped4.transform.rotation.w;
+            robot1.jo[4].or2.x = transformStamped4.transform.rotation.x;
+            robot1.jo[4].or2.y = transformStamped4.transform.rotation.y;
+            robot1.jo[4].or2.z = transformStamped4.transform.rotation.z;
+            ROS_INFO_STREAM(
+                    "panda_link4 Orientation is " << " w = " << robot1.jo[4].or2.w << ", x = " << robot1.jo[4].or2.x
+                                                  << ", y = " << robot1.jo[4].or2.y << ", z = " << robot1.jo[4].or2.z);
+
+            robot1.jo[5].or2.w = transformStamped5.transform.rotation.w;
+            robot1.jo[5].or2.x = transformStamped5.transform.rotation.x;
+            robot1.jo[5].or2.y = transformStamped5.transform.rotation.y;
+            robot1.jo[5].or2.z = transformStamped5.transform.rotation.z;
+            ROS_INFO_STREAM(
+                    "panda_link5 Orientation is " << " w = " << robot1.jo[5].or2.w << ", x = " << robot1.jo[5].or2.x
+                                                  << ", y = " << robot1.jo[5].or2.y << ", z = " << robot1.jo[5].or2.z);
+
+            robot1.jo[6].or2.w = transformStamped6.transform.rotation.w;
+            robot1.jo[6].or2.x = transformStamped6.transform.rotation.x;
+            robot1.jo[6].or2.y = transformStamped6.transform.rotation.y;
+            robot1.jo[6].or2.z = transformStamped6.transform.rotation.z;
+            ROS_INFO_STREAM(
+                    "panda_link6 Orientation is " << " w = " << robot1.jo[6].or2.w << ", x = " << robot1.jo[6].or2.x
+                                                  << ", y = " << robot1.jo[6].or2.y << ", z = " << robot1.jo[6].or2.z);
+
+            robot1.jo[7].or2.w = transformStamped7.transform.rotation.w;
+            robot1.jo[7].or2.x = transformStamped7.transform.rotation.x;
+            robot1.jo[7].or2.y = transformStamped7.transform.rotation.y;
+            robot1.jo[7].or2.z = transformStamped7.transform.rotation.z;
+            ROS_INFO_STREAM(
+                    "panda_link7 Orientation is " << " w = " << robot1.jo[7].or2.w << ", x = " << robot1.jo[7].or2.x
+                                                  << ", y = " << robot1.jo[7].or2.y << ", z = " << robot1.jo[7].or2.z);
         }
-return 0;
+        rate.sleep();
+    }
+    return 0;
 }
\ No newline at end of file
diff --git a/src/worldsafety.cpp b/src/worldsafety.cpp
index 468c8ec1546f03ab3638195f252c8c2391aa33f0..3060e7fc87ecdf379fb12a8140cd1d2d9562dceb 100644
--- a/src/worldsafety.cpp
+++ b/src/worldsafety.cpp
@@ -31,8 +31,7 @@
 #include <moveit/trajectory_processing/iterative_time_parameterization.h>
 #include "trajectory_msgs/JointTrajectoryPoint.h"
 
-int main(int argc, char** argv)
-{
+int main(int argc, char **argv) {
     ros::init(argc, argv, "Safety_Model");
     ros::NodeHandle node_handle;
     ros::AsyncSpinner spinner(1);
@@ -64,46 +63,44 @@ int main(int argc, char** argv)
     robot1.jo.push_back(o5f);
     robot1.jo.push_back(o5g);
 
-    int yx=0;
-    robot1.jo[++yx].name= "panda_link1";
-    robot1.jo[++yx].name= "panda_link2";
-    robot1.jo[++yx].name= "panda_link3";
-    robot1.jo[++yx].name= "panda_link4";
-    robot1.jo[++yx].name= "panda_link5";
-    robot1.jo[++yx].name= "panda_link6";
-    robot1.jo[++yx].name= "panda_link7";
+    int yx = 0;
+    robot1.jo[++yx].name = "panda_link1";
+    robot1.jo[++yx].name = "panda_link2";
+    robot1.jo[++yx].name = "panda_link3";
+    robot1.jo[++yx].name = "panda_link4";
+    robot1.jo[++yx].name = "panda_link5";
+    robot1.jo[++yx].name = "panda_link6";
+    robot1.jo[++yx].name = "panda_link7";
 
-    o9.c->robname="Robot FPE";
-    o9.c->of=true;
-    o9.c->moving=false;
-    o9.c->grasppos=true;
-    o9.c->planninggrp= "Panda Teaching";
+    o9.c->robname = "Robot FPE";
+    o9.c->of = true;
+    o9.c->moving = false;
+    o9.c->grasppos = true;
+    o9.c->planninggrp = "Panda Teaching";
 
-    o8.h->grippos= {12.1, 14.3, 18.2};
-    o3.grippos={1.1,2.3,2.1};
+    o8.h->grippos = {12.1, 14.3, 18.2};
+    o3.grippos = {1.1, 2.3, 2.1};
 
     o9.a->humannum = 1;
-    o9.b1->name="Cube";
-    o9.b1->size={5.2, 7.5, 9.1};
-    o9.b1->p1= {13.2, 14.1, 17.5};
-    o9.b1->x= 2.3;
+    o9.b1->name = "Cube";
+    o9.b1->size = {5.2, 7.5, 9.1};
+    o9.b1->p1 = {13.2, 14.1, 17.5};
+    o9.b1->x = 2.3;
     o9.b1->y = 1.2;
     o9.b1->z = 0.2;
     o9.b1->w = 0.1;
-    o9.b2->name="Ball";
-    o9.b2->size={1.3, 2.1, 3.2};
-    o9.b2->p1= {12.5, 16.4, 17.3};
-    o9.b2->x= 1.1;
+    o9.b2->name = "Ball";
+    o9.b2->size = {1.3, 2.1, 3.2};
+    o9.b2->p1 = {12.5, 16.4, 17.3};
+    o9.b2->x = 1.1;
     o9.b2->y = 1.4;
     o9.b2->z = 1.6;
     o9.b2->w = 1.9;
 
     // Static Tests - World Model Safety Test Cases to check Segmentation error
     // test the properties of the model that do not change over time, such as the structure of you model
-    if (robot1.jo.size() == 7)
-    { ROS_INFO_STREAM(" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS ");}
-    else
-    { ROS_ERROR_STREAM(" TEST CASE FAILED. ROBOT FPE HAS WRONG NUMBER OF JOINTS = " << robot1.jo.size());}
+    if (robot1.jo.size() == 7) { ROS_INFO_STREAM(" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS "); }
+    else { ROS_ERROR_STREAM(" TEST CASE FAILED. ROBOT FPE HAS WRONG NUMBER OF JOINTS = " << robot1.jo.size()); }
 
     if (o8.h == nullptr)
         ROS_ERROR("ERROR IN HAND POINTER IN ROBOT FPE CLASS AGGREGATION RELATION");
@@ -146,33 +143,47 @@ int main(int argc, char** argv)
     tf2_ros::Buffer tfBuffer;
     tf2_ros::TransformListener tfListener(tfBuffer);
     ros::Rate rate(10);
-    //robot1.jo[7].p1[2]=0.0;
-    int i=0;
-    int ho=0;
-    while (node_handle.ok())
-    {
-            geometry_msgs::TransformStamped transformStamped[7];
-            try {
-                transformStamped[++i] = tfBuffer.lookupTransform("world", robot1.jo[++ho].name, ros::Time(0));
-            } catch (tf2::TransformException &ex) {
-                ROS_WARN("%s", ex.what());
-                ros::Duration(0.1).sleep();
-                continue;
-            }
-            robot1.jo[i].p1[2] = transformStamped[ho].transform.translation.z;
-    }//End of taking input in program
-    // Begin test case to check if robot moved to its position
-    while (node_handle.ok())
-    {
-      {
-    // update the model data like in the application model node, test the dynamic parts here, i.e., if the model data is consistent with the position of the robot
-    //Checking if robot moved to position or not
-    if (robot1.jo[7].p1[2] == 0.9)
-        ROS_INFO_STREAM(" THE ROBOT HAS MOVED TO POSITION AS INTENDED ");
-    else
-        ROS_ERROR_STREAM("THE ROBOT HAS NOT MOVED TO THE POSITION AS INTENDED");
-      }
+
+    while (node_handle.ok()) {
+        geometry_msgs::TransformStamped transformStamped1;
+        geometry_msgs::TransformStamped transformStamped2;
+        geometry_msgs::TransformStamped transformStamped3;
+        geometry_msgs::TransformStamped transformStamped4;
+        geometry_msgs::TransformStamped transformStamped5;
+        geometry_msgs::TransformStamped transformStamped6;
+        geometry_msgs::TransformStamped transformStamped7;
+        try {
+            transformStamped1 = tfBuffer.lookupTransform("world", robot1.jo[1].name, ros::Time(0));
+            transformStamped2 = tfBuffer.lookupTransform("world", robot1.jo[2].name, ros::Time(0));
+            transformStamped3 = tfBuffer.lookupTransform("world", robot1.jo[3].name, ros::Time(0));
+            transformStamped4 = tfBuffer.lookupTransform("world", robot1.jo[4].name, ros::Time(0));
+            transformStamped5 = tfBuffer.lookupTransform("world", robot1.jo[5].name, ros::Time(0));
+            transformStamped6 = tfBuffer.lookupTransform("world", robot1.jo[6].name, ros::Time(0));
+            transformStamped7 = tfBuffer.lookupTransform("world", robot1.jo[7].name, ros::Time(0));
+            robot1.jo[7].p1[2] = transformStamped7.transform.translation.z;
+            if (robot1.jo[7].p1[2] == 0.9)
+                ROS_INFO_STREAM(" THE ROBOT HAS MOVED TO POSITION AS INTENDED ");
+            else
+                ROS_ERROR_STREAM("THE ROBOT HAS NOT MOVED TO THE POSITION AS INTENDED");
+            rate.sleep();
+            break;
+        } catch (tf2::TransformException &ex) {
+            ROS_WARN("%s", ex.what());
+            ros::Duration(0.1).sleep();
+            continue;
+        }
+    }
+    // Dynamic Tests
+    while (node_handle.ok()) {
+        {// update the model data like in the application model node, test the dynamic parts here, i.e., if the model data is consistent with the position of the robot
+            //Checking if robot moved to position or not
+            if (robot1.jo[7].p1[2] == 0.9)
+                ROS_INFO_STREAM(" THE ROBOT HAS MOVED TO POSITION AS INTENDED ");
+            else
+                ROS_ERROR_STREAM("THE ROBOT HAS NOT MOVED TO THE POSITION AS INTENDED");
             rate.sleep();
+        }
+
     }
-       return 0;
+    return 0;
 }