diff --git a/CMakeLists.txt b/CMakeLists.txt
index 52fbc61977967f91efb429a7ccf75459b52eff12..2cb135561dd7daa882ea134a9d94c06db50e5553 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -210,9 +210,9 @@ add_executable(worldsafety
 
 
 ## Specify libraries to link a library or executable target against
-target_link_libraries(robot_models_node ${catkin_LIBRARIES} )
+##target_link_libraries(robot_models_node ${catkin_LIBRARIES} )
 target_link_libraries(worldsafety ${catkin_LIBRARIES} )
-target_link_libraries(safetytest ${catkin_LIBRARIES} )
+##target_link_libraries(safetytest ${catkin_LIBRARIES} )
 #add_dependencies(robot_models_node ${catkin_LIBRARIES})
 
 #############
diff --git a/cmake-build-debug/CMakeFiles/robot_models_node.dir/build.make b/cmake-build-debug/CMakeFiles/robot_models_node.dir/build.make
index fe0e1d7c6e17f8d858ed5472a9a836b04db16370..d37e2bdb8d9fcd97a2960011879443b69e3e3eab 100644
--- a/cmake-build-debug/CMakeFiles/robot_models_node.dir/build.make
+++ b/cmake-build-debug/CMakeFiles/robot_models_node.dir/build.make
@@ -79,106 +79,6 @@ robot_models_node_EXTERNAL_OBJECTS =
 
 devel/lib/robot_models/robot_models_node: CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o
 devel/lib/robot_models/robot_models_node: CMakeFiles/robot_models_node.dir/build.make
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libgazebo_ros_api_plugin.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libgazebo_ros_paths_plugin.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_visual_tools.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librviz_visual_tools.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librviz_visual_tools_gui.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librviz_visual_tools_remote_control.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librviz_visual_tools_imarker_simple.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libeigen_conversions.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libtf_conversions.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libkdl_conversions.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_common_planning_interface_objects.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_planning_scene_interface.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_move_group_interface.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_py_bindings_tools.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_cpp.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_warehouse.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libwarehouse_ros.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libtf.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_pick_place_planner.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_move_group_capabilities_base.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_rdf_loader.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_kinematics_plugin_loader.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_robot_model_loader.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_constraint_sampler_manager_loader.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_planning_pipeline.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_trajectory_execution_manager.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_plan_execution.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_planning_scene_monitor.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_collision_plugin_loader.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libdynamic_reconfigure_config_init_mutex.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_ros_occupancy_map_monitor.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_exceptions.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_background_processing.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_kinematics_base.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_robot_model.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_transforms.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_robot_state.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_robot_trajectory.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_planning_interface.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_collision_detection.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_collision_detection_fcl.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_kinematic_constraints.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_planning_scene.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_constraint_samplers.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_planning_request_adapter.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_profiler.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_trajectory_processing.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_distance_field.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_collision_distance_field.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_kinematics_metrics.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_dynamics_solver.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_utils.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_test_utils.so
-devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_iostreams.so
-devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libfcl.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libkdl_parser.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/liburdf.so
-devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/liburdfdom_sensor.so
-devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/liburdfdom_model_state.so
-devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/liburdfdom_model.so
-devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/liburdfdom_world.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librosconsole_bridge.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libsrdfdom.so
-devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libtinyxml.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libgeometric_shapes.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/liboctomap.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/liboctomath.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librandom_numbers.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libclass_loader.so
-devel/lib/robot_models/robot_models_node: /usr/lib/libPocoFoundation.so
-devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libdl.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libroslib.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librospack.so
-devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libpython2.7.so
-devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_program_options.so
-devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libtinyxml2.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/liborocos-kdl.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/liborocos-kdl.so.1.4.0
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libtf2_ros.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libactionlib.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmessage_filters.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libroscpp.so
-devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_filesystem.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librosconsole.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librosconsole_log4cxx.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librosconsole_backend_interface.so
-devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/liblog4cxx.so
-devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_regex.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libxmlrpcpp.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libtf2.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libroscpp_serialization.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librostime.so
-devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libcpp_common.so
-devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_system.so
-devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_thread.so
-devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_chrono.so
-devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_date_time.so
-devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_atomic.so
-devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libpthread.so
-devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libconsole_bridge.so.0.4
 devel/lib/robot_models/robot_models_node: CMakeFiles/robot_models_node.dir/link.txt
 	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --bold --progress-dir=/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/CMakeFiles --progress-num=$(CMAKE_PROGRESS_2) "Linking CXX executable devel/lib/robot_models/robot_models_node"
 	$(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/robot_models_node.dir/link.txt --verbose=$(VERBOSE)
diff --git a/cmake-build-debug/CMakeFiles/robot_models_node.dir/link.txt b/cmake-build-debug/CMakeFiles/robot_models_node.dir/link.txt
index ec80df68feb54f8e140ee7c70a87d69f8fd0c1fa..12ee203c2f4cbced72583ffbd879597e95df4b30 100644
--- a/cmake-build-debug/CMakeFiles/robot_models_node.dir/link.txt
+++ b/cmake-build-debug/CMakeFiles/robot_models_node.dir/link.txt
@@ -1 +1 @@
-/usr/bin/c++  -g  -rdynamic CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o  -o devel/lib/robot_models/robot_models_node -Wl,-rpath,/opt/ros/melodic/lib /opt/ros/melodic/lib/libgazebo_ros_api_plugin.so /opt/ros/melodic/lib/libgazebo_ros_paths_plugin.so /opt/ros/melodic/lib/libmoveit_visual_tools.so /opt/ros/melodic/lib/librviz_visual_tools.so /opt/ros/melodic/lib/librviz_visual_tools_gui.so /opt/ros/melodic/lib/librviz_visual_tools_remote_control.so /opt/ros/melodic/lib/librviz_visual_tools_imarker_simple.so /opt/ros/melodic/lib/libeigen_conversions.so /opt/ros/melodic/lib/libtf_conversions.so /opt/ros/melodic/lib/libkdl_conversions.so /opt/ros/melodic/lib/libmoveit_common_planning_interface_objects.so /opt/ros/melodic/lib/libmoveit_planning_scene_interface.so /opt/ros/melodic/lib/libmoveit_move_group_interface.so /opt/ros/melodic/lib/libmoveit_py_bindings_tools.so /opt/ros/melodic/lib/libmoveit_cpp.so /opt/ros/melodic/lib/libmoveit_warehouse.so /opt/ros/melodic/lib/libwarehouse_ros.so /opt/ros/melodic/lib/libtf.so /opt/ros/melodic/lib/libmoveit_pick_place_planner.so /opt/ros/melodic/lib/libmoveit_move_group_capabilities_base.so /opt/ros/melodic/lib/libmoveit_rdf_loader.so /opt/ros/melodic/lib/libmoveit_kinematics_plugin_loader.so /opt/ros/melodic/lib/libmoveit_robot_model_loader.so /opt/ros/melodic/lib/libmoveit_constraint_sampler_manager_loader.so /opt/ros/melodic/lib/libmoveit_planning_pipeline.so /opt/ros/melodic/lib/libmoveit_trajectory_execution_manager.so /opt/ros/melodic/lib/libmoveit_plan_execution.so /opt/ros/melodic/lib/libmoveit_planning_scene_monitor.so /opt/ros/melodic/lib/libmoveit_collision_plugin_loader.so /opt/ros/melodic/lib/libdynamic_reconfigure_config_init_mutex.so /opt/ros/melodic/lib/libmoveit_ros_occupancy_map_monitor.so /opt/ros/melodic/lib/libmoveit_exceptions.so /opt/ros/melodic/lib/libmoveit_background_processing.so /opt/ros/melodic/lib/libmoveit_kinematics_base.so /opt/ros/melodic/lib/libmoveit_robot_model.so /opt/ros/melodic/lib/libmoveit_transforms.so /opt/ros/melodic/lib/libmoveit_robot_state.so /opt/ros/melodic/lib/libmoveit_robot_trajectory.so /opt/ros/melodic/lib/libmoveit_planning_interface.so /opt/ros/melodic/lib/libmoveit_collision_detection.so /opt/ros/melodic/lib/libmoveit_collision_detection_fcl.so /opt/ros/melodic/lib/libmoveit_kinematic_constraints.so /opt/ros/melodic/lib/libmoveit_planning_scene.so /opt/ros/melodic/lib/libmoveit_constraint_samplers.so /opt/ros/melodic/lib/libmoveit_planning_request_adapter.so /opt/ros/melodic/lib/libmoveit_profiler.so /opt/ros/melodic/lib/libmoveit_trajectory_processing.so /opt/ros/melodic/lib/libmoveit_distance_field.so /opt/ros/melodic/lib/libmoveit_collision_distance_field.so /opt/ros/melodic/lib/libmoveit_kinematics_metrics.so /opt/ros/melodic/lib/libmoveit_dynamics_solver.so /opt/ros/melodic/lib/libmoveit_utils.so /opt/ros/melodic/lib/libmoveit_test_utils.so -lboost_iostreams -lfcl /opt/ros/melodic/lib/libkdl_parser.so /opt/ros/melodic/lib/liburdf.so -lurdfdom_sensor -lurdfdom_model_state -lurdfdom_model -lurdfdom_world /opt/ros/melodic/lib/librosconsole_bridge.so /opt/ros/melodic/lib/libsrdfdom.so -ltinyxml /opt/ros/melodic/lib/libgeometric_shapes.so /opt/ros/melodic/lib/liboctomap.so /opt/ros/melodic/lib/liboctomath.so /opt/ros/melodic/lib/librandom_numbers.so /opt/ros/melodic/lib/libclass_loader.so -lPocoFoundation -ldl /opt/ros/melodic/lib/libroslib.so /opt/ros/melodic/lib/librospack.so -lpython2.7 -lboost_program_options -ltinyxml2 /opt/ros/melodic/lib/liborocos-kdl.so /opt/ros/melodic/lib/liborocos-kdl.so.1.4.0 /opt/ros/melodic/lib/libtf2_ros.so /opt/ros/melodic/lib/libactionlib.so /opt/ros/melodic/lib/libmessage_filters.so /opt/ros/melodic/lib/libroscpp.so -lboost_filesystem /opt/ros/melodic/lib/librosconsole.so /opt/ros/melodic/lib/librosconsole_log4cxx.so /opt/ros/melodic/lib/librosconsole_backend_interface.so -llog4cxx -lboost_regex /opt/ros/melodic/lib/libxmlrpcpp.so /opt/ros/melodic/lib/libtf2.so /opt/ros/melodic/lib/libroscpp_serialization.so /opt/ros/melodic/lib/librostime.so /opt/ros/melodic/lib/libcpp_common.so -lboost_system -lboost_thread -lboost_chrono -lboost_date_time -lboost_atomic -lpthread /usr/lib/x86_64-linux-gnu/libconsole_bridge.so.0.4 
+/usr/bin/c++  -g  -rdynamic CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o  -o devel/lib/robot_models/robot_models_node 
diff --git a/cmake-build-debug/CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o b/cmake-build-debug/CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o
deleted file mode 100644
index 21d078b22e17018fb55ae046320efc15639d3c2f..0000000000000000000000000000000000000000
Binary files a/cmake-build-debug/CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o and /dev/null differ
diff --git a/cmake-build-debug/CMakeFiles/safetytest.dir/build.make b/cmake-build-debug/CMakeFiles/safetytest.dir/build.make
index 63d5ee2915bcc7d9a35a665d3686059cb7fbeeb1..e93150b4e02115841deb10b74a57bf86fb66db09 100644
--- a/cmake-build-debug/CMakeFiles/safetytest.dir/build.make
+++ b/cmake-build-debug/CMakeFiles/safetytest.dir/build.make
@@ -79,106 +79,6 @@ safetytest_EXTERNAL_OBJECTS =
 
 devel/lib/robot_models/safetytest: CMakeFiles/safetytest.dir/src/safetytest.cpp.o
 devel/lib/robot_models/safetytest: CMakeFiles/safetytest.dir/build.make
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libgazebo_ros_api_plugin.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libgazebo_ros_paths_plugin.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_visual_tools.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/librviz_visual_tools.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/librviz_visual_tools_gui.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/librviz_visual_tools_remote_control.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/librviz_visual_tools_imarker_simple.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libeigen_conversions.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libtf_conversions.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libkdl_conversions.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_common_planning_interface_objects.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_planning_scene_interface.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_move_group_interface.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_py_bindings_tools.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_cpp.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_warehouse.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libwarehouse_ros.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libtf.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_pick_place_planner.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_move_group_capabilities_base.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_rdf_loader.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_kinematics_plugin_loader.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_robot_model_loader.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_constraint_sampler_manager_loader.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_planning_pipeline.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_trajectory_execution_manager.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_plan_execution.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_planning_scene_monitor.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_collision_plugin_loader.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libdynamic_reconfigure_config_init_mutex.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_ros_occupancy_map_monitor.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_exceptions.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_background_processing.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_kinematics_base.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_robot_model.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_transforms.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_robot_state.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_robot_trajectory.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_planning_interface.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_collision_detection.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_collision_detection_fcl.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_kinematic_constraints.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_planning_scene.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_constraint_samplers.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_planning_request_adapter.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_profiler.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_trajectory_processing.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_distance_field.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_collision_distance_field.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_kinematics_metrics.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_dynamics_solver.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_utils.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_test_utils.so
-devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libboost_iostreams.so
-devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libfcl.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libkdl_parser.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/liburdf.so
-devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/liburdfdom_sensor.so
-devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/liburdfdom_model_state.so
-devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/liburdfdom_model.so
-devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/liburdfdom_world.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/librosconsole_bridge.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libsrdfdom.so
-devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libtinyxml.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libgeometric_shapes.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/liboctomap.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/liboctomath.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/librandom_numbers.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libclass_loader.so
-devel/lib/robot_models/safetytest: /usr/lib/libPocoFoundation.so
-devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libdl.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libroslib.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/librospack.so
-devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libpython2.7.so
-devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libboost_program_options.so
-devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libtinyxml2.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/liborocos-kdl.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/liborocos-kdl.so.1.4.0
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libtf2_ros.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libactionlib.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmessage_filters.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libroscpp.so
-devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libboost_filesystem.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/librosconsole.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/librosconsole_log4cxx.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/librosconsole_backend_interface.so
-devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/liblog4cxx.so
-devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libboost_regex.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libxmlrpcpp.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libtf2.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libroscpp_serialization.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/librostime.so
-devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libcpp_common.so
-devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libboost_system.so
-devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libboost_thread.so
-devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libboost_chrono.so
-devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libboost_date_time.so
-devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libboost_atomic.so
-devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libpthread.so
-devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libconsole_bridge.so.0.4
 devel/lib/robot_models/safetytest: CMakeFiles/safetytest.dir/link.txt
 	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --bold --progress-dir=/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/CMakeFiles --progress-num=$(CMAKE_PROGRESS_2) "Linking CXX executable devel/lib/robot_models/safetytest"
 	$(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/safetytest.dir/link.txt --verbose=$(VERBOSE)
diff --git a/cmake-build-debug/CMakeFiles/safetytest.dir/link.txt b/cmake-build-debug/CMakeFiles/safetytest.dir/link.txt
index f67f60fd4a5d813a095d1c2697e14556d9c49e32..6582f4ded611d72d861993001c2423a2752fb2b7 100644
--- a/cmake-build-debug/CMakeFiles/safetytest.dir/link.txt
+++ b/cmake-build-debug/CMakeFiles/safetytest.dir/link.txt
@@ -1 +1 @@
-/usr/bin/c++  -g  -rdynamic CMakeFiles/safetytest.dir/src/safetytest.cpp.o  -o devel/lib/robot_models/safetytest -Wl,-rpath,/opt/ros/melodic/lib /opt/ros/melodic/lib/libgazebo_ros_api_plugin.so /opt/ros/melodic/lib/libgazebo_ros_paths_plugin.so /opt/ros/melodic/lib/libmoveit_visual_tools.so /opt/ros/melodic/lib/librviz_visual_tools.so /opt/ros/melodic/lib/librviz_visual_tools_gui.so /opt/ros/melodic/lib/librviz_visual_tools_remote_control.so /opt/ros/melodic/lib/librviz_visual_tools_imarker_simple.so /opt/ros/melodic/lib/libeigen_conversions.so /opt/ros/melodic/lib/libtf_conversions.so /opt/ros/melodic/lib/libkdl_conversions.so /opt/ros/melodic/lib/libmoveit_common_planning_interface_objects.so /opt/ros/melodic/lib/libmoveit_planning_scene_interface.so /opt/ros/melodic/lib/libmoveit_move_group_interface.so /opt/ros/melodic/lib/libmoveit_py_bindings_tools.so /opt/ros/melodic/lib/libmoveit_cpp.so /opt/ros/melodic/lib/libmoveit_warehouse.so /opt/ros/melodic/lib/libwarehouse_ros.so /opt/ros/melodic/lib/libtf.so /opt/ros/melodic/lib/libmoveit_pick_place_planner.so /opt/ros/melodic/lib/libmoveit_move_group_capabilities_base.so /opt/ros/melodic/lib/libmoveit_rdf_loader.so /opt/ros/melodic/lib/libmoveit_kinematics_plugin_loader.so /opt/ros/melodic/lib/libmoveit_robot_model_loader.so /opt/ros/melodic/lib/libmoveit_constraint_sampler_manager_loader.so /opt/ros/melodic/lib/libmoveit_planning_pipeline.so /opt/ros/melodic/lib/libmoveit_trajectory_execution_manager.so /opt/ros/melodic/lib/libmoveit_plan_execution.so /opt/ros/melodic/lib/libmoveit_planning_scene_monitor.so /opt/ros/melodic/lib/libmoveit_collision_plugin_loader.so /opt/ros/melodic/lib/libdynamic_reconfigure_config_init_mutex.so /opt/ros/melodic/lib/libmoveit_ros_occupancy_map_monitor.so /opt/ros/melodic/lib/libmoveit_exceptions.so /opt/ros/melodic/lib/libmoveit_background_processing.so /opt/ros/melodic/lib/libmoveit_kinematics_base.so /opt/ros/melodic/lib/libmoveit_robot_model.so /opt/ros/melodic/lib/libmoveit_transforms.so /opt/ros/melodic/lib/libmoveit_robot_state.so /opt/ros/melodic/lib/libmoveit_robot_trajectory.so /opt/ros/melodic/lib/libmoveit_planning_interface.so /opt/ros/melodic/lib/libmoveit_collision_detection.so /opt/ros/melodic/lib/libmoveit_collision_detection_fcl.so /opt/ros/melodic/lib/libmoveit_kinematic_constraints.so /opt/ros/melodic/lib/libmoveit_planning_scene.so /opt/ros/melodic/lib/libmoveit_constraint_samplers.so /opt/ros/melodic/lib/libmoveit_planning_request_adapter.so /opt/ros/melodic/lib/libmoveit_profiler.so /opt/ros/melodic/lib/libmoveit_trajectory_processing.so /opt/ros/melodic/lib/libmoveit_distance_field.so /opt/ros/melodic/lib/libmoveit_collision_distance_field.so /opt/ros/melodic/lib/libmoveit_kinematics_metrics.so /opt/ros/melodic/lib/libmoveit_dynamics_solver.so /opt/ros/melodic/lib/libmoveit_utils.so /opt/ros/melodic/lib/libmoveit_test_utils.so -lboost_iostreams -lfcl /opt/ros/melodic/lib/libkdl_parser.so /opt/ros/melodic/lib/liburdf.so -lurdfdom_sensor -lurdfdom_model_state -lurdfdom_model -lurdfdom_world /opt/ros/melodic/lib/librosconsole_bridge.so /opt/ros/melodic/lib/libsrdfdom.so -ltinyxml /opt/ros/melodic/lib/libgeometric_shapes.so /opt/ros/melodic/lib/liboctomap.so /opt/ros/melodic/lib/liboctomath.so /opt/ros/melodic/lib/librandom_numbers.so /opt/ros/melodic/lib/libclass_loader.so -lPocoFoundation -ldl /opt/ros/melodic/lib/libroslib.so /opt/ros/melodic/lib/librospack.so -lpython2.7 -lboost_program_options -ltinyxml2 /opt/ros/melodic/lib/liborocos-kdl.so /opt/ros/melodic/lib/liborocos-kdl.so.1.4.0 /opt/ros/melodic/lib/libtf2_ros.so /opt/ros/melodic/lib/libactionlib.so /opt/ros/melodic/lib/libmessage_filters.so /opt/ros/melodic/lib/libroscpp.so -lboost_filesystem /opt/ros/melodic/lib/librosconsole.so /opt/ros/melodic/lib/librosconsole_log4cxx.so /opt/ros/melodic/lib/librosconsole_backend_interface.so -llog4cxx -lboost_regex /opt/ros/melodic/lib/libxmlrpcpp.so /opt/ros/melodic/lib/libtf2.so /opt/ros/melodic/lib/libroscpp_serialization.so /opt/ros/melodic/lib/librostime.so /opt/ros/melodic/lib/libcpp_common.so -lboost_system -lboost_thread -lboost_chrono -lboost_date_time -lboost_atomic -lpthread /usr/lib/x86_64-linux-gnu/libconsole_bridge.so.0.4 
+/usr/bin/c++  -g  -rdynamic CMakeFiles/safetytest.dir/src/safetytest.cpp.o  -o devel/lib/robot_models/safetytest 
diff --git a/cmake-build-debug/CMakeFiles/safetytest.dir/src/safetytest.cpp.o b/cmake-build-debug/CMakeFiles/safetytest.dir/src/safetytest.cpp.o
deleted file mode 100644
index a298373b665e231f2aef8338a28d38274ddb3757..0000000000000000000000000000000000000000
Binary files a/cmake-build-debug/CMakeFiles/safetytest.dir/src/safetytest.cpp.o and /dev/null differ
diff --git a/cmake-build-debug/CMakeFiles/worldsafety.dir/CXX.includecache b/cmake-build-debug/CMakeFiles/worldsafety.dir/CXX.includecache
index b95c190899c553c6e890686f2127b8b77c244a8a..a5115360009568b0e866176a5de3ea882c0b1338 100644
--- a/cmake-build-debug/CMakeFiles/worldsafety.dir/CXX.includecache
+++ b/cmake-build-debug/CMakeFiles/worldsafety.dir/CXX.includecache
@@ -128,7 +128,7 @@ moveit_visual_tools/moveit_visual_tools.h
 moveit/trajectory_processing/iterative_time_parameterization.h
 -
 trajectory_msgs/JointTrajectoryPoint.h
--
+/home/nikhil/panda_gazebo_workspace/src/zero/src/trajectory_msgs/JointTrajectoryPoint.h
 
 /opt/ros/melodic/include/actionlib/action_definition.h
 
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 0143ad09d826f6c22609377bb80e9654a19ce64a..c30872c21a909016c501db3da5d00f1c1f45ef2c 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/safetytest b/cmake-build-debug/devel/lib/robot_models/safetytest
deleted file mode 100755
index 5362be4b38b4635cbedf4166d0987eb40b37eb47..0000000000000000000000000000000000000000
Binary files a/cmake-build-debug/devel/lib/robot_models/safetytest and /dev/null differ
diff --git a/cmake-build-debug/devel/lib/robot_models/worldsafety b/cmake-build-debug/devel/lib/robot_models/worldsafety
index ca9496db546e34f15a3eab45017772dda682b84e..9bacd014d2bdf204e0dcb0720e6a3d618ab81401 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/launch/launcher.launch b/launch/launcher.launch
index c8a6ff7fc24c1674aac837c2ed7a6c6dcbb2c66d..ab8ea0647e87e218724f948e5643af9d954a71b1 100644
--- a/launch/launcher.launch
+++ b/launch/launcher.launch
@@ -4,7 +4,7 @@
 
     <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch"/>
 
-    <node pkg="testappm" type="safety_model" name="launcher" respawn="false" output="screen"/>
+    <node pkg="worldsafety" type="safety_model" name="launcher" respawn="false" output="screen"/>
 
 </launch>
 
diff --git a/src/datalink/humanspace.h b/src/datalink/humanspace.h
index 610a3341ad3ffdbbb00b3cb63eb366ee51d3ce9e..655c7fd4679c70214c121fb6c5a258f385b901c7 100644
--- a/src/datalink/humanspace.h
+++ b/src/datalink/humanspace.h
@@ -4,8 +4,8 @@
 class humanspace
 {
 public:
-    //int humannum;
-    uint32 humannum;
+    int humannum;
+   // uint32 humannum;
 
 //constructor
 
diff --git a/src/robot_models_node.cpp b/src/robot_models_node.cpp
index 9b032a769426b4e25e5ad60655ac4f4df0b12495..5afa4ea8e103528533a15d061172fc3ea22fee8d 100644
--- a/src/robot_models_node.cpp
+++ b/src/robot_models_node.cpp
@@ -3,7 +3,7 @@
 #include "datalink/graspobject.h"
 #include "datalink/hand.h"
 #include "datalink/humanspace.h"
-#include "datalink/joint.h"
+#include "datalink/jo.h"
 #include "datalink/obstacle.h"
 #include "datalink/robot.h"
 #include "datalink/robotfpe.h"
@@ -22,6 +22,7 @@
 #include "message_filters/subscriber.h"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
 #include "geometry_msgs/PointStamped.h"
+#include "datalink/joint.h"
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2/LinearMath/Quaternion.h>
 #include <moveit/move_group_interface/move_group_interface.h>
@@ -51,50 +52,50 @@ void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg)
         joint o5f;
         joint o5g;
         obstacle o6;
-        robot o7;
+        robot robot1;
         robotfpe o8;
         world o9;
         worldobject o10;
 
-        o7.jo.push_back(o5a);
-        o7.jo.push_back(o5b);
-        o7.jo.push_back(o5c);
-        o7.jo.push_back(o5d);
-        o7.jo.push_back(o5e);
-        o7.jo.push_back(o5f);
-        o7.jo.push_back(o5g);
-        /*o7.jo1->name= "panda_link1";
-        o7.jo2->name= "panda_link2";
-        o7.jo3->name= "panda_link3";
-        o7.jo4->name= "panda_link4";
-        o7.jo5->name= "panda_link5";
-        o7.jo6->name= "panda_link6";
-        o7.jo7->name= "panda_link7";
-
-        o7.jo1->or2= {1.2,5.3,6.5,7.4};
-        o7.jo2->or2= {2.5,3.6,5.8,4.9};
-        o7.jo3->or2= {5.1,1.2,9.9,6.8};
-        o7.jo4->or2= {6.3,3.4,8.5,9.6};
-        o7.jo5->or2= {8.2,2.6,7.9,5.5};
-        o7.jo6->or2= {7.3,4.4,6.7,4.0};
-        o7.jo7->or2= {7.3,4.4,6.7,4.0};
-
-        o7.jo1->p1= {1.3, 10.1, 9.4};
-        o7.jo2->p1= {14.7, 11.3, 6.5};
-        o7.jo3->p1= {4.9, 13.8, 7.7};
-        o7.jo4->p1= {2.4, 12.5, 1.6};
-        o7.jo5->p1= {3.1, 16.2, 5.3};
-        o7.jo6->p1= {11.5, 15.4, 3.4};
-        o7.jo7->p1= {5.1, 18.3, 4.2};*/
+        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;
-        o7.jo[++yx].name= "panda_link1";
-        o7.jo[++yx].name= "panda_link2";
-        o7.jo[++yx].name= "panda_link3";
-        o7.jo[++yx].name= "panda_link4";
-        o7.jo[++yx].name= "panda_link5";
-        o7.jo[++yx].name= "panda_link6";
-        o7.jo[++yx].name= "panda_link7";
+        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";
@@ -158,13 +159,13 @@ void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg)
                 uint32::data updateDetectedHumans;
                 geometry_msgs::uint32 x;
                 try {
-                    transformStamped1 = tfBuffer.lookupTransform("world", o7.jo[1].name, ros::Time(0));
-                    transformStamped2 = tfBuffer.lookupTransform("world", o7.jo[2].name, ros::Time(0));
-                    transformStamped3 = tfBuffer.lookupTransform("world", o7.jo[3].name, ros::Time(0));
-                    transformStamped4 = tfBuffer.lookupTransform("world", o7.jo[4].name, ros::Time(0));
-                    transformStamped5 = tfBuffer.lookupTransform("world", o7.jo[5].name, ros::Time(0));
-                    transformStamped6 = tfBuffer.lookupTransform("world", o7.jo[6].name, ros::Time(0));
-                    transformStamped7 = tfBuffer.lookupTransform("world", o7.jo[7].name, ros::Time(0));
+                    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());
@@ -172,133 +173,127 @@ void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg)
                     continue;
                 }
 
-                o7.jo[1].p1[0]=0.0;
-                o7.jo[1].p1[1]=0.0;
-                o7.jo[1].p1[2]=0.0;
-                o7.jo[2].p1[0]=0.0;
-                o7.jo[2].p1[1]=0.0;
-                o7.jo[2].p1[2]=0.0;
-                o7.jo[3].p1[0]=0.0;
-                o7.jo[3].p1[1]=0.0;
-                o7.jo[3].p1[2]=0.0;
-                o7.jo[4].p1[0]=0.0;
-                o7.jo[4].p1[1]=0.0;
-                o7.jo[4].p1[2]=0.0;
-                o7.jo[5].p1[0]=0.0;
-                o7.jo[5].p1[1]=0.0;
-                o7.jo[5].p1[2]=0.0;
-                o7.jo[6].p1[0]=0.0;
-                o7.jo[6].p1[1]=0.0;
-                o7.jo[6].p1[2]=0.0;
-                o7.jo[7].p1[0]=0.0;
-                o7.jo[7].p1[1]=0.0;
-                o7.jo[7].p1[2]=0.0;
-
-                o7.jo[1].p1[0]=transformStamped1.transform.translation.x;
-                o7.jo[1].p1[1]=transformStamped1.transform.translation.y;
-                o7.jo[1].p1[2]=transformStamped1.transform.translation.z;
-                ROS_INFO_STREAM("panda_link1 Position is " <<" x = "<< o7.jo[1].p1[0] << ", y = "<< o7.jo[1].p1[1]<<", z = "<<o7.jo[1].p1[2]);
-
-                o7.jo[2].p1[0]=transformStamped2.transform.translation.x;
-                o7.jo[2].p1[1]=transformStamped2.transform.translation.y;
-                o7.jo[2].p1[2]=transformStamped2.transform.translation.z;
-                ROS_INFO_STREAM("panda_link2 Position is " <<" x = "<< o7.jo[2].p1[0] << ", y = "<< o7.jo[2].p1[1]<<", z = "<<o7.jo[2].p1[2]);
-
-                o7.jo[3].p1[0]=transformStamped3.transform.translation.x;
-                o7.jo[3].p1[1]=transformStamped3.transform.translation.y;
-                o7.jo[3].p1[2]=transformStamped3.transform.translation.z;
-                ROS_INFO_STREAM("panda_link3 Position is " <<" x = "<< o7.jo[3].p1[0] << ", y = "<< o7.jo[3].p1[1]<<", z = "<<o7.jo[3].p1[2]);
-
-                o7.jo[4].p1[0]=transformStamped4.transform.translation.x;
-                o7.jo[4].p1[1]=transformStamped4.transform.translation.y;
-                o7.jo[4].p1[2]=transformStamped4.transform.translation.z;
-                ROS_INFO_STREAM("panda_link4 Position is " <<" x = "<< o7.jo[4].p1[0] << ", y = "<< o7.jo[4].p1[1]<<", z = "<<o7.jo[4].p1[2]);
-
-                o7.jo[5].p1[0]=transformStamped5.transform.translation.x;
-                o7.jo[5].p1[1]=transformStamped5.transform.translation.y;
-                o7.jo[5].p1[2]=transformStamped5.transform.translation.z;
-                ROS_INFO_STREAM("panda_link5 Position is " <<" x = "<< o7.jo[5].p1[0] << ", y = "<< o7.jo[5].p1[1]<<", z = "<<o7.jo[5].p1[2]);
-
-                o7.jo[6].p1[0]=transformStamped6.transform.translation.x;
-                o7.jo[6].p1[1]=transformStamped6.transform.translation.y;
-                o7.jo[6].p1[2]=transformStamped6.transform.translation.z;
-                ROS_INFO_STREAM("panda_link6 Position is " <<" x = "<< o7.jo[6].p1[0] << ", y = "<< o7.jo[6].p1[1]<<", z = "<<o7.jo[6].p1[2]);
-
-                o7.jo[7].p1[0]=transformStamped7.transform.translation.x;
-                o7.jo[7].p1[1]=transformStamped7.transform.translation.y;
-                o7.jo[7].p1[2]=transformStamped7.transform.translation.z;
-                ROS_INFO_STREAM("panda_link7 Position is " <<" x = "<< o7.jo[7].p1[0] << ", y = "<< o7.jo[7].p1[1]<<", z = "<<o7.jo[7].p1[2]);
-
-                o7.jo[1].or2.w=0.0;
-                o7.jo[1].or2.x=0.0;
-                o7.jo[1].or2.y=0.0;
-                o7.jo[1].or2.z=0.0;
-                o7.jo[2].or2.w=0.0;
-                o7.jo[2].or2.x=0.0;
-                o7.jo[2].or2.y=0.0;
-                o7.jo[2].or2.z=0.0;
-                o7.jo[3].or2.w=0.0;
-                o7.jo[3].or2.x=0.0;
-                o7.jo[3].or2.y=0.0;
-                o7.jo[3].or2.z=0.0;
-                o7.jo[4].or2.w=0.0;
-                o7.jo[4].or2.x=0.0;
-                o7.jo[4].or2.y=0.0;
-                o7.jo[4].or2.z=0.0;
-                o7.jo[5].or2.w=0.0;
-                o7.jo[5].or2.x=0.0;
-                o7.jo[5].or2.y=0.0;
-                o7.jo[5].or2.z=0.0;
-                o7.jo[6].or2.w=0.0;
-                o7.jo[6].or2.x=0.0;
-                o7.jo[6].or2.y=0.0;
-                o7.jo[6].or2.z=0.0;
-                o7.jo[7].or2.w=0.0;
-                o7.jo[7].or2.x=0.0;
-                o7.jo[7].or2.y=0.0;
-                o7.jo[7].or2.z=0.0;
-
-                o7.jo[1].or2.w=transformStamped1.transform.rotation.w;
-                o7.jo[1].or2.x=transformStamped1.transform.rotation.x;
-                o7.jo[1].or2.y=transformStamped1.transform.rotation.y;
-                o7.jo[1].or2.z=transformStamped1.transform.rotation.z;
-                ROS_INFO_STREAM("panda_link1 Orientation is " <<" w = "<< o7.jo[1].or2.w<<", x = "<<o7.jo[1].or2.x<<", y = "<<o7.jo[1].or2.y<<", z = "<<o7.jo[1].or2.z);
-
-                o7.jo[2].or2.w=transformStamped2.transform.rotation.w;
-                o7.jo[2].or2.x=transformStamped2.transform.rotation.x;
-                o7.jo[2].or2.y=transformStamped2.transform.rotation.y;
-                o7.jo[2].or2.z=transformStamped2.transform.rotation.z;
-                ROS_INFO_STREAM("panda_link2 Orientation is " <<" w = "<<  o7.jo[2].or2.w<<", x = "<< o7.jo[2].or2.x<<", y = "<< o7.jo[2].or2.y<<", z = "<< o7.jo[2].or2.z);
-
-                o7.jo[3].or2.w=transformStamped3.transform.rotation.w;
-                o7.jo[3].or2.x=transformStamped3.transform.rotation.x;
-                o7.jo[3].or2.y=transformStamped3.transform.rotation.y;
-                o7.jo[3].or2.z=transformStamped3.transform.rotation.z;
-                ROS_INFO_STREAM("panda_link3 Orientation is " <<" w = "<< o7.jo[3].or2.w<<", x = "<<o7.jo[3].or2.x<<", y = "<<o7.jo[3].or2.y<<", z = "<<o7.jo[3].or2.z);
-
-                o7.jo[4].or2.w=transformStamped4.transform.rotation.w;
-                o7.jo[4].or2.x=transformStamped4.transform.rotation.x;
-                o7.jo[4].or2.y=transformStamped4.transform.rotation.y;
-                o7.jo[4].or2.z=transformStamped4.transform.rotation.z;
-                ROS_INFO_STREAM("panda_link4 Orientation is " <<" w = "<<o7.jo[4].or2.w<<", x = "<<o7.jo[4].or2.x<<", y = "<<o7.jo[4].or2.y<<", z = "<<o7.jo[4].or2.z);
-
-                o7.jo[5].or2.w=transformStamped5.transform.rotation.w;
-                o7.jo[5].or2.x=transformStamped5.transform.rotation.x;
-                o7.jo[5].or2.y=transformStamped5.transform.rotation.y;
-                o7.jo[5].or2.z=transformStamped5.transform.rotation.z;
-                ROS_INFO_STREAM("panda_link5 Orientation is " <<" w = "<< o7.jo[5].or2.w<<", x = "<<o7.jo[5].or2.x<<", y = "<<o7.jo[5].or2.y<<", z = "<<o7.jo[5].or2.z);
-
-                o7.jo[6].or2.w=transformStamped6.transform.rotation.w;
-                o7.jo[6].or2.x=transformStamped6.transform.rotation.x;
-                o7.jo[6].or2.y=transformStamped6.transform.rotation.y;
-                o7.jo[6].or2.z=transformStamped6.transform.rotation.z;
-                ROS_INFO_STREAM("panda_link6 Orientation is " <<" w = "<< o7.jo[6].or2.w<<", x = "<<o7.jo[6].or2.x<<", y = "<<o7.jo[6].or2.y<<", z = "<<o7.jo[6].or2.z);
-
-                o7.jo[7].or2.w=transformStamped7.transform.rotation.w;
-                o7.jo[7].or2.x=transformStamped7.transform.rotation.x;
-                o7.jo[7].or2.y=transformStamped7.transform.rotation.y;
-                o7.jo[7].or2.z=transformStamped7.transform.rotation.z;
-                ROS_INFO_STREAM("panda_link7 Orientation is " <<" w = "<<o7.jo[7].or2.w<<", x = "<<o7.jo[7].or2.x<<", y = "<<o7.jo[7].or2.y<<", z = "<<o7.jo[7].or2.z);
+                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();
         }
diff --git a/src/worldsafety.cpp b/src/worldsafety.cpp
index 6323304439c51ce7c6b7fd141af17b46b55763fe..468c8ec1546f03ab3638195f252c8c2391aa33f0 100644
--- a/src/worldsafety.cpp
+++ b/src/worldsafety.cpp
@@ -29,7 +29,7 @@
 #include <moveit_msgs/CollisionObject.h>
 #include <moveit_visual_tools/moveit_visual_tools.h>
 #include <moveit/trajectory_processing/iterative_time_parameterization.h>
-#include <trajectory_msgs/JointTrajectoryPoint.h>
+#include "trajectory_msgs/JointTrajectoryPoint.h"
 
 int main(int argc, char** argv)
 {
@@ -51,34 +51,27 @@ int main(int argc, char** argv)
     joint o5f;
     joint o5g;
     obstacle o6;
-    robot o7;
+    robot robot1;
     robotfpe o8;
     world o9;
     worldobject o10;
 
-    o7.jo.push_back(o5a);
-    o7.jo.push_back(o5b);
-    o7.jo.push_back(o5c);
-    o7.jo.push_back(o5d);
-    o7.jo.push_back(o5e);
-    o7.jo.push_back(o5f);
-    o7.jo.push_back(o5g);
-
- /*   // test if the robot contains 7 joints static test
-    if (o7.jo.size() == 5) {
-        ROS_INFO("SIZE IS 5");
-    } else {
-        ROS_INFO_STREAM("SIZE IS NOT 5 but was " << o7.jo.size());
-    }*/
+    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;
-    o7.jo[++yx].name= "panda_link1";
-    o7.jo[++yx].name= "panda_link2";
-    o7.jo[++yx].name= "panda_link3";
-    o7.jo[++yx].name= "panda_link4";
-    o7.jo[++yx].name= "panda_link5";
-    o7.jo[++yx].name= "panda_link6";
-    o7.jo[++yx].name= "panda_link7";
+    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;
@@ -105,34 +98,12 @@ int main(int argc, char** argv)
     o9.b2->z = 1.6;
     o9.b2->w = 1.9;
 
-    // Check Robot Motion if moved to a specific point or not
-    // setup the planning environment
-
-   /* static const std::string PLANNING_GROUP = "panda_arm";
-    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
-    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
-
-    ros::Duration(3.0).sleep();
-
-    move_group.setStartStateToCurrentState();
-
-    geometry_msgs::Pose another_pose;
-    another_pose.orientation.w = 1.0;
-    another_pose.position.x = 0.4;
-    another_pose.position.y = -0.4;
-    another_pose.position.z = 0.9;
-    move_group.setPoseTarget(another_pose);
-
-    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
-    bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
-    ROS_INFO("Visualizing plan %s", success ? "" : "FAILED");
-
-    // Move the (simulated) robot in gazebo
-    move_group.move();*/
-    if (o7.jo.size() == 7)
-        ROS_INFO_STREAM(" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS ");
+    // 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 = " << o7.jo.size());
+    { 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");
@@ -148,13 +119,12 @@ int main(int argc, char** argv)
 
     if (o9.b2 == nullptr)
         ROS_ERROR("ERROR IN ABSTRACT OBJECT CLASS AGGREGATION RELATION");
-
+    // End of static tests
+    // Start Robot Motion
     static const std::string PLANNING_GROUP = "panda_arm";
     moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
     moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
-
     ros::Duration(3.0).sleep();
-
     move_group.setStartStateToCurrentState();
 
     geometry_msgs::Pose another_pose;
@@ -167,55 +137,42 @@ int main(int argc, char** argv)
     moveit::planning_interface::MoveGroupInterface::Plan my_plan;
     bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
     ROS_INFO("Visualizing plan %s", success ? "" : "FAILED");
-
     // Move the (simulated) robot in gazebo
     move_group.move();
+    //ros::shutdown();
 
+    // End Robot Motion
+    // Start input values from Gazebo to program
     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()) {
-        {
+    while (node_handle.ok())
+    {
             geometry_msgs::TransformStamped transformStamped[7];
-
             try {
-                transformStamped[++i] = tfBuffer.lookupTransform("world", o7.jo[++ho].name, ros::Time(0));
+                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;
             }
-            o7.jo[7].p1[2]=transformStamped[7].transform.translation.z;
-          
-// World Model Safety Test Cases to check Segmentation errors
-            ros::Rate rate(10);
-// test the properties of the model that do not change over time, such as the structure of you model
-          // Static 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 (o7.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");
-                    ros::shutdown();
-
-                }
-
-            }
+            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");
+      }
             rate.sleep();
-        }
-
-
-        return 0;
     }
+       return 0;
 }