diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
new file mode 100644
index 0000000000000000000000000000000000000000..473dc3fcfc36899ca2e29b524ae01585630bc7de
--- /dev/null
+++ b/.vscode/c_cpp_properties.json
@@ -0,0 +1,39 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/home/matteo/ccf_sim_workspace/devel/include/**",
+        "/opt/ros/noetic/include/**",
+        "/home/matteo/ccf_sim_workspace/src/BehaviorTree.CPP/include/**",
+        "/home/matteo/ccf_sim_workspace/src/ccf/include/**",
+        "/home/matteo/ccf_sim_workspace/src/ccf_immersive_sorting/include/**",
+        "/home/matteo/ccf_sim_workspace/src/gazebo_pkgs/gazebo_grasp_plugin/include/**",
+        "/home/matteo/ccf_sim_workspace/src/gazebo_pkgs/gazebo_state_plugins/include/**",
+        "/home/matteo/ccf_sim_workspace/src/gazebo_pkgs/gazebo_test_tools/include/**",
+        "/home/matteo/ccf_sim_workspace/src/gazebo_pkgs/gazebo_version_helpers/include/**",
+        "/home/matteo/ccf_sim_workspace/src/gazebo_pkgs/gazebo_world_plugin_loader/include/**",
+        "/home/matteo/ccf_sim_workspace/src/gb-grasp-auerswald/include/**",
+        "/home/matteo/ccf_sim_workspace/src/moveit-grasps-reduced/include/**",
+        "/home/matteo/ccf_sim_workspace/src/moveit_task_constructor/core/include/**",
+        "/home/matteo/ccf_sim_workspace/src/moveit_task_constructor/demo/include/**",
+        "/home/matteo/ccf_sim_workspace/src/multi_cell_builder/include/**",
+        "/home/matteo/ccf_sim_workspace/src/general_message_packages/object_msgs_tools/include/**",
+        "/home/matteo/ccf_sim_workspace/src/panda_grasping/include/**",
+        "/home/matteo/ccf_sim_workspace/src/panda_util/include/**",
+        "/home/matteo/ccf_sim_workspace/src/moveit_task_constructor/rviz_marker_tools/include/**",
+        "/home/matteo/ccf_sim_workspace/src/simulation_util/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}
\ No newline at end of file
diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000000000000000000000000000000000000..6ffb51377a2b5bbeb48424690bec9a959c6f4746
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,6 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/matteo/ccf_sim_workspace/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ]
+}
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 04ba4b8b98cce952f01cfbc30a483dac8c5a9e27..c5ab33ec691b43a7cb9882ab645f73da0015f928 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -103,6 +103,8 @@ src/reader/abstract_param_reader.cpp
 src/reader/robot_reader.cpp
 src/reader/wing_reader.cpp
 src/reader/cuboid_reader.cpp
+src/reader/job_reader.cpp
+
 )
 
 add_executable(grasp_cell_routine src/grasp_cell_routine.cpp
@@ -122,6 +124,8 @@ src/reader/abstract_param_reader.cpp
 src/reader/robot_reader.cpp
 src/reader/wing_reader.cpp
 src/reader/cuboid_reader.cpp
+src/reader/job_reader.cpp
+
 )
 
 
@@ -129,6 +133,8 @@ add_executable(mtc2taskspace src/mtc2taskspace.cpp
 src/reader/abstract_param_reader.cpp
 src/reader/task_space_reader.cpp
 src/reader/robot_reader.cpp
+src/reader/job_reader.cpp
+
 )
 
 
diff --git a/include/impl/moveit_grasp_mediator.h b/include/impl/moveit_grasp_mediator.h
index 09f727b3a24f522bea77f6a0d2beab8de8b1be46..ae363e20bad13f69d17921107e6a8376148e946c 100644
--- a/include/impl/moveit_grasp_mediator.h
+++ b/include/impl/moveit_grasp_mediator.h
@@ -15,6 +15,8 @@
 #include "reader/abstract_param_reader.h"
 #include "reader/robot_reader.h"
 #include "reader/cuboid_reader.h"
+#include "reader/job_reader.h"
+
 
 
 
@@ -28,9 +30,11 @@ class Moveit_grasp_mediator : public Moveit_mediator{
 
 
     // Readers 
-    std::unique_ptr<Abstract_param_reader> robot_reader_;
+    std::unique_ptr<Robot_reader> robot_reader_;
     std::unique_ptr<Abstract_param_reader> wing_reader_;
     std::unique_ptr<Abstract_param_reader> cuboid_reader_;
+    std::unique_ptr<Job_reader> job_reader_;
+
 
     public:
     Moveit_grasp_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh);
diff --git a/include/reader/abstract_param_reader.h b/include/reader/abstract_param_reader.h
index 5cbf34d499d8c35e096217029c2df4ed614de4bf..1191a7a29eaeaa5bb1e06a640fbc98282953859a 100644
--- a/include/reader/abstract_param_reader.h
+++ b/include/reader/abstract_param_reader.h
@@ -17,7 +17,6 @@ struct object_data {
 };
 
 struct job_data {
-    std::string name_;
     std::vector<tf2::Transform> jobs_;
 };
 
diff --git a/include/reader/job_reader.h b/include/reader/job_reader.h
index 3e75feef365c0f769c3928832a10417959d9178c..fedfd87d23b75243437a5287bbaf5b59f31fc328 100644
--- a/include/reader/job_reader.h
+++ b/include/reader/job_reader.h
@@ -10,17 +10,15 @@
 
 class Job_reader : public Abstract_param_reader{
     protected:
-    std::vector<job_data> job_data_;
-    std::string name_;
+    std::map<std::string, std::vector<job_data>> job_data_;
 
     public:
-    Job_reader(std::shared_ptr<ros::NodeHandle> const& d, std::string const& name) 
+    Job_reader(std::shared_ptr<ros::NodeHandle> const& d) 
     : Abstract_param_reader(d)
-    , name_(name)
     {read();}
     
-    inline void set_job_data(std::vector<job_data>& robot_data) {job_data_ = robot_data;}
-    inline std::vector<job_data> robot_data() {return job_data_;}
+    inline void set_job_data(std::map<std::string, std::vector<job_data>>& robot_data) {job_data_ = robot_data;}
+    inline std::map<std::string, std::vector<job_data>>& robot_data() {return job_data_;}
 
     void read() override;
 };
diff --git a/jobs/dummy.yaml b/jobs/dummy.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..7a5ef0fb28f1d2554570fb5ddc2d4c3111ee5110
--- /dev/null
+++ b/jobs/dummy.yaml
@@ -0,0 +1,56 @@
+{ 'task': 
+  {'groups' : [
+    { 'name': 'panda_arm1', 'jobs':[[
+        { 'pos': { 'x': -0.300000 ,'y': -0.700000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': -0.300000 ,'y': -0.600000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': -0.200000 ,'y': -0.700000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': -0.200000 ,'y': -0.600000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.100000 ,'y': -0.700000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.100000 ,'y': -0.600000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.100000 ,'y': -0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.100000 ,'y': -0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.100000 ,'y': -0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.200000 ,'y': -0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.200000 ,'y': -0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.200000 ,'y': -0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.300000 ,'y': -0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.300000 ,'y': -0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.300000 ,'y': -0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.100000 ,'y': 0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.100000 ,'y': 0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.100000 ,'y': 0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.200000 ,'y': 0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.200000 ,'y': 0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.200000 ,'y': 0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.300000 ,'y': 0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.300000 ,'y': 0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.300000 ,'y': 0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } }
+      ]]},
+    { 'name': 'panda_arm2', 'jobs':[[
+        { 'pos': { 'x': 0.100000 ,'y': 1.110000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.100000 ,'y': 1.210000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.200000 ,'y': 1.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.200000 ,'y': 1.110000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.200000 ,'y': 1.210000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.300000 ,'y': 1.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.300000 ,'y': 1.110000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.300000 ,'y': 1.210000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.100000 ,'y': 1.410000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.100000 ,'y': 1.510000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.100000 ,'y': 1.610000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.200000 ,'y': 1.410000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.200000 ,'y': 1.510000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.200000 ,'y': 1.610000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.300000 ,'y': 1.410000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.300000 ,'y': 1.510000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.300000 ,'y': 1.610000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': -0.300000 ,'y': 1.910000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': -0.300000 ,'y': 2.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': -0.200000 ,'y': 1.910000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': -0.200000 ,'y': 2.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.100000 ,'y': 1.910000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': 0.100000 ,'y': 2.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } }
+      ]]}
+    ]
+  }
+}
diff --git a/launch/cell_routine.launch b/launch/cell_routine.launch
index 6f14f4bf36fb17af310af801652b09ba108dd5a7..7a516c1d0823ab2d3e91b40b47cf344c1fb01a8f 100644
--- a/launch/cell_routine.launch
+++ b/launch/cell_routine.launch
@@ -1,5 +1,7 @@
 <launch>    
     <arg name="result" default="dummy/-562289182.yaml" />
+    <arg name="jobs" default="jobs/dummy.yaml" />
+
     <!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
 
     <!-- this is to change-->
@@ -7,6 +9,8 @@
     <rosparam command="load" file="$(find multi_cell_builder)/mtc_templates/dummy.yaml" />
     <rosparam command="load" file="$(find multi_cell_builder)/maps/dummy.yaml"/>
     <rosparam command="load" file="$(find multi_cell_builder)/descriptions/dummy2.yaml"/>
+    <!-- <rosparam command="load" file="$(find multi_cell_builder)/$(arg jobs)"/> -->
+
 
     
     <rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
diff --git a/launch/mtc2taskspace.launch b/launch/mtc2taskspace.launch
new file mode 100644
index 0000000000000000000000000000000000000000..f3c8dd0759a5b150d7f21bd551e0adcc8fbaea37
--- /dev/null
+++ b/launch/mtc2taskspace.launch
@@ -0,0 +1,8 @@
+<launch>
+    <arg name="resource" default="jobs/dummy.yaml" />
+
+    <rosparam param="resource_name" subst_value="True"> $(arg resource)</rosparam>
+    
+    <rosparam command="load" file="$(find multi_cell_builder)/$(arg resource)"/>
+    <node pkg="multi_cell_builder" type="mtc2taskspace" name="mtc2taskspace" output="screen" required="true"/>
+</launch>
diff --git a/src/impl/moveit_grasp_mediator.cpp b/src/impl/moveit_grasp_mediator.cpp
index f02e0ef1f0215681c23d7b3cbf07e5ee4e49acb8..c5decb4b21bf88cbf9b8274eae8978b8d7833b9c 100644
--- a/src/impl/moveit_grasp_mediator.cpp
+++ b/src/impl/moveit_grasp_mediator.cpp
@@ -527,7 +527,7 @@ Moveit_grasp_mediator::Moveit_grasp_mediator(std::vector<std::vector<tf2::Transf
     , robot_reader_(std::make_unique<Robot_reader>(nh))
     , wing_reader_(std::make_unique<Wing_reader>(nh))
     , cuboid_reader_(std::make_unique<Cuboid_reader>(nh)) {
-
+    // , job_reader_(std::make_unique<Job_reader>(nh)) {
         visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(
         robot_model_->getModelFrame(), "/rviz_visual_tools", planning_scene_monitor_);
         visual_tools_->loadMarkerPub();
@@ -551,7 +551,7 @@ Moveit_grasp_mediator::Moveit_grasp_mediator(std::vector<std::vector<tf2::Transf
         planning_pipeline_ = std::make_shared<planning_pipeline::PlanningPipeline>(robot_model_, *nh_, "planning_plugin", "request_adapter");
         voxel_manager_ = std::make_shared<VoxelManager>(*nh_, grasp_ns_.c_str() ,visual_tools_,planning_scene_monitor_);
 
-        auto rd = static_cast<Robot_reader*>(robot_reader_.get())->robot_data();
+        auto rd = robot_reader_->robot_data();
         for (int i = 0; i < rd.size() ;i++) connect_robots(new Moveit_robot(rd[i].name_, rd[i].pose_, rd[i].size_));
 
         auto wd = static_cast<Wing_reader*>(wing_reader_.get())->wing_data();
diff --git a/src/impl/moveit_mediator.cpp b/src/impl/moveit_mediator.cpp
index 43d9901150650899154103e7b57c1655a962c6b0..9133caf89dd6934770322765098046bb6e46598a 100644
--- a/src/impl/moveit_mediator.cpp
+++ b/src/impl/moveit_mediator.cpp
@@ -244,6 +244,7 @@ void Moveit_mediator::setup_task(){
 
 		for(auto& a: acm_) a.second.resize(acm_.size(), 0);
 
+
 		while (!tasks[0].empty() && !tasks[1].empty()){
 			for (int i =0; i < std::max(tasks[0].front().solution.sub_trajectory.size(), tasks[1].front().solution.sub_trajectory.size()); i++){
 				moveit_msgs::RobotTrajectory* t1 = nullptr;
@@ -253,166 +254,16 @@ void Moveit_mediator::setup_task(){
 				moveit_msgs::PlanningScene ps_m;
 				std::vector<moveit_msgs::PlanningScene::_allowed_collision_matrix_type> acm_m;
 
-				for(auto entry: acm_) ROS_INFO("%s", entry.first.c_str());
+				//for(auto entry: acm_) ROS_INFO("%s", entry.first.c_str());
 
 				if (i < tasks[0].front().solution.sub_trajectory.size()){
 					t1 = &tasks[0].front().solution.sub_trajectory[i].trajectory;
 					ps1 = &tasks[0].front().solution.sub_trajectory[i].scene_diff;
-					
-					ROS_INFO("[Robot 1] acm");
-					ROS_INFO("default entry names");
-					for (auto  den : ps1->allowed_collision_matrix.default_entry_names){
-						ROS_INFO("%s", den.c_str());
-					}
-					ROS_INFO("default entry values");
-					for (auto  den : ps1->allowed_collision_matrix.default_entry_values){
-						ROS_INFO("%i", den);
-					}
-					ROS_INFO("entry names");
-					for (auto  den : ps1->allowed_collision_matrix.entry_names){
-						ROS_INFO("%s", den.c_str());
-						ROS_INFO("count %i", ps1->allowed_collision_matrix.entry_names.size());
-					}					
-					ROS_INFO("entry values");
-					for (auto  den : ps1->allowed_collision_matrix.entry_values){
-						for (auto ene : den.enabled) ROS_INFO("%i", ene);
-						ROS_INFO("count %i", den.enabled.size());
-
-					}
-					ROS_INFO("[Robot 1] fft");
-					for (auto  ft : ps1->fixed_frame_transforms){
-						ROS_INFO("child frame id: %s, header id: %s, seq %i, time", ft.child_frame_id.c_str(), ft.header.frame_id.c_str(), ft.header.seq);
-					}
-					ROS_INFO("[Robot 1] is_diff");
-					ROS_INFO("is_diff: %i", ps1->is_diff);
-					ROS_INFO("[Robot 1] link padding");
-					for (auto  lp : ps1->link_padding){
-						ROS_INFO("link padding name: %s, padding %f", lp.link_name.c_str(), lp.padding);
-					}
-					ROS_INFO("[Robot 1] link scale");
-					for (auto  ls : ps1->link_scale){
-						ROS_INFO("link scale name: %s, padding %f", ls.link_name.c_str(), ls.scale);
-					}
-					ROS_INFO("[Robot 1] name");
-					ROS_INFO("name: %s", ps1->name.c_str());
-					ROS_INFO("[Robot 1] object color");
-					for (auto  oc : ps1->object_colors){
-						ROS_INFO("object color %f %f %f %f, id %s", oc.color.r, oc.color.g, oc.color.b, oc.color.a, oc.id.c_str());
-					}
-					ROS_INFO("[Robot 1] rmn");
-					ROS_INFO("rmn %s", ps1->robot_model_name.c_str());
-					ROS_INFO("[Robot 1] rs");
-					for (auto  aco : ps1->robot_state.attached_collision_objects){
-						ROS_INFO("aco dp header frame id: %s, seq %i, time", aco.detach_posture.header.frame_id.c_str(), aco.detach_posture.header.seq);
-						for (auto jn : aco.detach_posture.joint_names){
-							ROS_INFO("aco dp joint name: %s", jn.c_str());
-						}
-						ROS_INFO("aco dp Points");
-						for (auto p : aco.detach_posture.points){
-							for (auto pa : p.accelerations) ROS_INFO("aco dp points acc: %f", pa);
-							for (auto pa : p.effort) ROS_INFO("aco dp points effort: %f", pa);
-							ROS_INFO("aco dp points time from start");
-							for (auto pa : p.velocities) ROS_INFO("aco dp points vel: %f", pa);
-							for (auto pa : p.positions) ROS_INFO("aco dp points tra: %f", pa);
-						}	
-						ROS_INFO("aco link names %s", aco.link_name.c_str());
-						ROS_INFO("aco collis obj id %s", aco.object.id.c_str());
-						for (auto p : aco.touch_links) ROS_INFO("aco touch link  %s", p.c_str());
-						ROS_INFO("aco collis obj weight %f", aco.weight);
-					}
-
-					for (auto d : ps1->robot_state.joint_state.position) ROS_INFO("js position %f ", d);
-					for (auto d : ps1->robot_state.joint_state.name) ROS_INFO(" js name %s", d.c_str());
-					for (auto d : ps1->robot_state.joint_state.velocity) ROS_INFO(" js vel %f", d);
-					for (auto d : ps1->robot_state.joint_state.effort) ROS_INFO(" js e %f", d);
-					ROS_INFO ("%s, %i", ps1->robot_state.joint_state.header.frame_id.c_str(), ps1->robot_state.joint_state.header.seq);
-
-					for (auto p : ps1->world.collision_objects) ROS_INFO("world co  id %s", p.id);
-					ROS_INFO("world om origin %f %f %f, %f %f %f %f", ps1->world.octomap.origin.position.x, ps1->world.octomap.origin.position.y, ps1->world.octomap.origin.position.z, ps1->world.octomap.origin.orientation.x, ps1->world.octomap.origin.orientation.y, ps1->world.octomap.origin.orientation.z, ps1->world.octomap.origin.orientation.w);
-					ROS_INFO("world om header time, id %s, seq %i", ps1->world.octomap.header.frame_id.c_str(), ps1->world.octomap.header.seq);
-					ROS_INFO("world om Data");
-					
 				} 
 
 				if (i < tasks[1].front().solution.sub_trajectory.size()){
 					t2 = &tasks[1].front().solution.sub_trajectory[i].trajectory;
 					ps2 = &tasks[1].front().solution.sub_trajectory[i].scene_diff;
-
-
-					ROS_INFO("[Robot 2] acm");
-					ROS_INFO("default entry names");
-					for (auto  den : ps2->allowed_collision_matrix.default_entry_names){
-						ROS_INFO("%s", den.c_str());
-					}
-					ROS_INFO("default entry values");
-					for (auto  den : ps2->allowed_collision_matrix.default_entry_values){
-						ROS_INFO("%i", den);
-					}
-					ROS_INFO("entry names");
-					for (auto  den : ps2->allowed_collision_matrix.entry_names){
-						ROS_INFO("%s", den.c_str());
-						ROS_INFO("count %i", ps2->allowed_collision_matrix.entry_names.size());
-
-					}					
-					ROS_INFO("entry values");
-					for (auto  den : ps2->allowed_collision_matrix.entry_values){
-						for (auto ene : den.enabled) ROS_INFO("%i", ene);
-						ROS_INFO("count %i", den.enabled.size());
-					}
-					ROS_INFO("[Robot 2] fft");
-					for (auto  ft : ps2->fixed_frame_transforms){
-						ROS_INFO("child frame id: %s, header id: %s, seq %i, time", ft.child_frame_id.c_str(), ft.header.frame_id.c_str(), ft.header.seq);
-					}
-					ROS_INFO("[Robot 2] is_diff");
-					ROS_INFO("is_diff: %i", ps2->is_diff);
-
-					ROS_INFO("[Robot 2] link padding");
-					for (auto  lp : ps2->link_padding){
-						ROS_INFO("link padding name: %s, padding %f", lp.link_name.c_str(), lp.padding);
-					}
-					ROS_INFO("[Robot 2] link scale");
-					for (auto  ls : ps2->link_scale){
-						ROS_INFO("link scale name: %s, padding %f", ls.link_name.c_str(), ls.scale);
-					}
-					ROS_INFO("[Robot 2] name");
-					ROS_INFO("name: %s", ps2->name.c_str());
-					ROS_INFO("[Robot 2] object color");
-					for (auto  oc : ps2->object_colors){
-						ROS_INFO("object color %f %f %f %f, id %s", oc.color.r, oc.color.g, oc.color.b, oc.color.a, oc.id.c_str());
-					}
-					ROS_INFO("[Robot 2] rmn");
-					ROS_INFO("rmn %s", ps2->robot_model_name.c_str());
-					ROS_INFO("[Robot 2] rs");
-					for (auto  aco : ps2->robot_state.attached_collision_objects){
-						ROS_INFO("aco dp header frame id: %s, seq %i, time", aco.detach_posture.header.frame_id.c_str(), aco.detach_posture.header.seq);
-						for (auto jn : aco.detach_posture.joint_names){
-							ROS_INFO("aco dp joint name: %s", jn.c_str());
-						}
-						ROS_INFO("aco dp Points");
-						for (auto p : aco.detach_posture.points){
-							for (auto pa : p.accelerations) ROS_INFO("aco dp points acc: %f", pa);
-							for (auto pa : p.effort) ROS_INFO("aco dp points effort: %f", pa);
-							ROS_INFO("aco dp points time from start");
-							for (auto pa : p.velocities) ROS_INFO("aco dp points vel: %f", pa);
-							for (auto pa : p.positions) ROS_INFO("aco dp points tra: %f", pa);
-						}	
-						ROS_INFO("aco link names %s", aco.link_name.c_str());
-						ROS_INFO("aco collis obj id %s", aco.object.id.c_str());
-						for (auto p : aco.touch_links) ROS_INFO("aco touch link  %s", p.c_str());
-						ROS_INFO("aco collis obj weight %f", aco.weight);
-					}
-					for (auto d : ps2->robot_state.joint_state.position) ROS_INFO("js position %f ", d);
-					for (auto d : ps2->robot_state.joint_state.name) ROS_INFO(" js name %s", d.c_str());
-					for (auto d : ps2->robot_state.joint_state.velocity) ROS_INFO(" js vel %f", d);
-					for (auto d : ps2->robot_state.joint_state.effort) ROS_INFO(" js e %f", d);
-					ROS_INFO ("%s, %i", ps2->robot_state.joint_state.header.frame_id.c_str(), ps2->robot_state.joint_state.header.seq);
-
-
-					for (auto p : ps2->world.collision_objects) ROS_INFO("world co  id %s", p.id.c_str());
-					ROS_INFO("world om origin %f %f %f, %f %f %f %f", ps2->world.octomap.origin.position.x, ps2->world.octomap.origin.position.y, ps2->world.octomap.origin.position.z, ps2->world.octomap.origin.orientation.x, ps2->world.octomap.origin.orientation.y, ps2->world.octomap.origin.orientation.z, ps2->world.octomap.origin.orientation.w);
-					ROS_INFO("world om header time, id %s, seq %i", ps2->world.octomap.header.frame_id.c_str(), ps2->world.octomap.header.seq);
-					ROS_INFO("world om Data");
-					
 				}
 
 				std::vector<std::thread> th;
@@ -428,53 +279,125 @@ void Moveit_mediator::setup_task(){
 
 					// build panda link regex
 					std::stringstream ss;
-					ss << "panda_" << match[1] << "_*";
-					std::regex rx_panda_links(ss.str().c_str());
-					std::regex rx_box("bottle*");
-					std::regex rx_table("table*");
+					ss << "panda_" << match[1] << "_.*";
+					std::regex rx_panda_links(ss.str());
+					std::regex rx_box("bottle.*");
+					std::regex rx_table("table.*");
 
-					ROS_INFO("start merging");
 					// Iterate task collsion matrix
 					for(int j = 0; j < ps1->allowed_collision_matrix.entry_names.size(); j++ ){
-						// look for relevant fields
-						ROS_INFO("compare %s and %s", mr->map().at("base").c_str(), ps1->allowed_collision_matrix.entry_names[j].c_str());
-						if( std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_box) || 
-							std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_panda_links) || 
-							std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_table) ||
-							mr->map().at("base") == ps1->allowed_collision_matrix.entry_names[j]){
-							// Iterate over values
-							ROS_INFO("true");
+						if( mr->map().at("base") == ps1->allowed_collision_matrix.entry_names[j]){
+							/* In case an entry matches an robot spezific link*/
+							ROS_INFO("entry matches link %s at index %i", mr->map().at("base").c_str(), j);
 							for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
-								// filter for irrelevant links of other robots, this would be _link and base_ pattern, which are not relevant
-								ROS_INFO("start filtering");
-
-								if( std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links) || 
-									std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box) ||
-									std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table) ||
-									mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
-										
-									std::string relevant_field;
-
-									if(std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)) relevant_field = match[0].str();
-									if(std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)) relevant_field = match[0].str();
-									if(std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)) relevant_field = match[0].str();
-									if(mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]) relevant_field = mr->map().at("base").c_str();
-
-									// => Update acm_
-									// calculate the distance in acm for name in in acm message ('cause they may differ, you know)
-									// totally not stolen from SO
-									ROS_INFO("calculate distance");
-									int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));
-
-									// better filter with regex
-									// set acm value with respect to the position in acm
-									ROS_INFO("acm überschreiben mit distnace %i", distance);
-									ROS_INFO("Updating field %s in acm with %s",mr->map().at("base").c_str(), ps1->allowed_collision_matrix.entry_names[k].c_str());
-
-									for(auto a : acm_.at(mr->map().at("base"))) ROS_INFO("%i", a);
-									ROS_INFO("value %i", acm_.at(mr->map().at("base"))[distance]);
-									acm_.at(mr->map().at(relevant_field))[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
-									ROS_INFO("Updating field %s in acm with %s",mr->map().at("base").c_str(), ps1->allowed_collision_matrix.entry_names[k].c_str());
+								/* For that specific entry, loop over values*/									
+								int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));								
+
+								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
+									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
+									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
+								}
+
+								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
+									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
+									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
+								}				
+
+								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
+									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
+									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
+								}	
+
+								if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
+									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
+									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
+								}
+							}
+						}
+
+						if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_panda_links)){
+							/* In case an entry matches an robot spezific link*/
+							ROS_INFO("entry matches link %s at index %i", ps1->allowed_collision_matrix.entry_names[j].c_str(), j);
+							for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
+								/* For that specific entry, loop over values*/									
+								int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));								
+
+								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
+									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
+									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
+								}
+
+								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
+									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
+									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
+								}				
+
+								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
+									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
+									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
+								}	
+
+								if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
+									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
+									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
+								}
+							}
+						}
+
+						if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_box)){
+							/* In case an entry matches an robot spezific link*/
+							ROS_INFO("entry matches link %s at index %i", ps1->allowed_collision_matrix.entry_names[j].c_str(), j);
+							for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
+								/* For that specific entry, loop over values*/									
+								int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));								
+
+								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
+									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
+									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
+								}
+
+								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
+									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
+									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
+								}				
+
+								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
+									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
+									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
+								}	
+
+								if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
+									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
+									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
+								}
+							}
+						}
+
+						if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_table)){
+							/* In case an entry matches an robot spezific link*/
+							ROS_INFO("entry matches link %s at index %i", ps1->allowed_collision_matrix.entry_names[j], j);
+							for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
+								/* For that specific entry, loop over values*/									
+								int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));								
+
+								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
+									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
+									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
+								}
+
+								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
+									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
+									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
+								}				
+
+								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
+									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
+									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
+								}	
+
+								if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
+									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
+									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
 								}
 							}
 						}
@@ -485,59 +408,132 @@ void Moveit_mediator::setup_task(){
 				if (t2){
 					Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[1]);
 					th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t2, *ps2));		
+
 					std::regex rx_panda("panda_arm([0-9]+)");
     				std::smatch match;
         			std::regex_match(mr->name(), match, rx_panda);
 
 					// build panda link regex
 					std::stringstream ss;
-					ss << "panda_" << match[1] << "_*";
-					std::regex rx_panda_links(ss.str().c_str());
-					std::regex rx_box("bottle*");
-					std::regex rx_table("table*");
+					ss << "panda_" << match[1] << "_.*";
+					std::regex rx_panda_links(ss.str());
+					std::regex rx_box("bottle.*");
+					std::regex rx_table("table.*");
 
-					ROS_INFO("start merging");
 					// Iterate task collsion matrix
 					for(int j = 0; j < ps2->allowed_collision_matrix.entry_names.size(); j++ ){
-						// look for relevant fields
-						ROS_INFO("compare %s and %s", mr->map().at("base").c_str(), ps2->allowed_collision_matrix.entry_names[j].c_str());
-						if( std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_box) || 
-							std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_panda_links) || 
-							std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_table) ||
-							mr->map().at("base") == ps2->allowed_collision_matrix.entry_names[j]){
-							// Iterate over values
-							ROS_INFO("true");
+						if( mr->map().at("base") == ps2->allowed_collision_matrix.entry_names[j]){
+							/* In case an entry matches an robot spezific link*/
+							ROS_INFO("entry matches link %s at index %i", mr->map().at("base").c_str(), j);
+							for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
+								/* For that specific entry, loop over values*/									
+								int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));								
+
+								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
+									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
+									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
+								}
+
+								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){
+									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
+									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
+								}				
+
+								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){
+									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
+									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
+								}	
+
+								if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
+									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
+									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
+								}
+							}
+						}
+
+						if(std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_panda_links)){
+							/* In case an entry matches an robot spezific link*/
+							ROS_INFO("entry matches link %s at index %i", ps2->allowed_collision_matrix.entry_names[j].c_str(), j);
+							for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
+								/* For that specific entry, loop over values*/									
+								int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));								
+
+								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
+									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
+									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
+								}
+
+								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){
+									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
+									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
+								}				
+
+								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){
+									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
+									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
+								}	
+
+								if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
+									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
+									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
+								}
+							}
+						}
+
+						if(std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_box)){
+							/* In case an entry matches an robot spezific link*/
+							ROS_INFO("entry matches link %s at index %i", ps2->allowed_collision_matrix.entry_names[j].c_str(), j);
 							for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
-								// filter for irrelevant links of other robots, this would be _link and base_ pattern, which are not relevant
-								ROS_INFO("start filtering");
-
-								if( std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links) || 
-									std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box) ||
-									std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table) ||
-									mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
-										
-									std::string relevant_field;
-
-									if(std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)) relevant_field = match[0].str();
-									if(std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)) relevant_field = match[0].str();
-									if(std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)) relevant_field = match[0].str();
-									if(mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]) relevant_field = mr->map().at("base").c_str();
-
-									// => Update acm_
-									// calculate the distance in acm for name in in acm message ('cause they may differ, you know)
-									// totally not stolen from SO
-									ROS_INFO("calculate distance");
-									int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));
-
-									// better filter with regex
-									// set acm value with respect to the position in acm
-									ROS_INFO("acm überschreiben mit distnace %i", distance);
-									ROS_INFO("Updating field %s in acm with %s",mr->map().at("base").c_str(), ps2->allowed_collision_matrix.entry_names[k].c_str());
-
-									for(auto a : acm_.at(mr->map().at("base"))) ROS_INFO("%i", a);
-									ROS_INFO("value %i", acm_.at(mr->map().at("base"))[distance]);
-									acm_.at(mr->map().at(relevant_field))[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
-									ROS_INFO("Updating field %s in acm with %s",mr->map().at("base").c_str(), ps2->allowed_collision_matrix.entry_names[k].c_str());
+								/* For that specific entry, loop over values*/									
+								int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));								
+
+								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
+									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
+									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
+								}
+
+								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){
+									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
+									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
+								}				
+
+								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){
+									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
+									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
+								}	
+
+								if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
+									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
+									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
+								}
+							}
+						}
+
+						if(std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_table)){
+							/* In case an entry matches an robot spezific link*/
+							ROS_INFO("entry matches link %s at index %i", ps2->allowed_collision_matrix.entry_names[j], j);
+							for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
+								/* For that specific entry, loop over values*/									
+								int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));								
+
+								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
+									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
+									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
+								}
+
+								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){
+									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
+									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
+								}				
+
+								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){
+									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
+									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
+								}	
+
+								if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
+									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
+									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
 								}
 							}
 						}
@@ -545,11 +541,6 @@ void Moveit_mediator::setup_task(){
 					merge_ps(ps_m, ps2, mr);
 				}
 
-				/*
-				if(!acm_m.empty()){
-					merge_acm(acm_m);
-				}
-				*/
 
 				for(int i = 0; i < th.size(); i++){
 					th[i].join();
@@ -567,13 +558,17 @@ void Moveit_mediator::setup_task(){
 
 void Moveit_mediator::merge_acm(moveit_msgs::PlanningScene& ps_m){
 	moveit_msgs::PlanningScene::_allowed_collision_matrix_type acmt;
+	acmt.entry_values.resize(acm_.size());
+
 	int i = 0;
 	for (auto& a : acm_){
 		acmt.entry_names.push_back(a.first);
 		acmt.entry_values[i].enabled = a.second;
 		i++;
 	}
+
 	ps_m.allowed_collision_matrix = acmt;
+	ROS_INFO("broken after merge");
 
 }
 
@@ -618,24 +613,6 @@ void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::Pla
 		}
 	}
 	
-
-	/*
-	for( auto aco : in->robot_state.attached_collision_objects) {
-		out.robot_state.attached_collision_objects.push_back(aco);
-	}
-
-	for( auto aco : in->robot_state.joint_state) {
-		out.robot_state.attached_collision_objects.push_back(aco);
-	}
-	sensor_msgs::JointState::
-
-	for (auto str : res.allowed_collision_matrix.entry_names){
-		auto entry = std::find(origin->allowed_collision_matrix.entry_names.begin(), origin->allowed_collision_matrix.entry_names.end(), str);
-		if (entry != origin->allowed_collision_matrix.entry_names.end()){
-			origin->allowed_collision_matrix.entry_values[entry - origin->allowed_collision_matrix.entry_names.begin()]
-		}
-	}
-	*/
 }
 	
 void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps){
diff --git a/src/mtc2taskspace.cpp b/src/mtc2taskspace.cpp
index e1b78f574d5fff8c1ecafe3b0f77c75f8e21390d..e73a81b69cc752585ca88d1c0bfecf5ee0bee468 100644
--- a/src/mtc2taskspace.cpp
+++ b/src/mtc2taskspace.cpp
@@ -1,13 +1,16 @@
 #include "reader/abstract_param_reader.h"
 #include "reader/task_space_reader.h"
 #include "reader/robot_reader.h"
+#include "reader/job_reader.h"
+
 #include <regex>
 
 int main(int argc, char **argv){
     ros::init(argc, argv, "mtc2taskspace");
     std::shared_ptr<ros::NodeHandle> n(new ros::NodeHandle);
 
-    std::unique_ptr<Abstract_param_reader> ts_reader(new Task_space_reader(n));
-    ts_reader->read();
+    //std::unique_ptr<Abstract_param_reader> ts_reader(new Task_space_reader(n));
+    std::unique_ptr<Abstract_param_reader> ts_reader(new Job_reader(n));
+
     return 0;
 }
\ No newline at end of file
diff --git a/src/reader/job_reader.cpp b/src/reader/job_reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fd7955d1937a19e57277c6cf81beedb054c43946
--- /dev/null
+++ b/src/reader/job_reader.cpp
@@ -0,0 +1,38 @@
+#include "reader/job_reader.h"
+
+void Job_reader::read(){
+    ROS_INFO("--- JOB_READER ---");
+    XmlRpc::XmlRpcValue resources;
+    nh_->getParam("/task/groups", resources);
+
+    for (int i = 0; i < resources.size(); i++){
+        std::string robot = resources[i]["name"];
+        //nh_->getParam("/tasks/groups/" + robot, jobs);
+        std::vector<job_data> v_jd;
+        for (int j = 0; j < resources[i]["jobs"].size(); j++){
+            ROS_INFO("--- %s --- JOB_%i ---", robot.c_str(), j);
+            job_data jd;
+            for(int k = 0; k < resources[i]["jobs"][j].size(); k++){
+                XmlRpc::XmlRpcValue job_data = resources[i]["jobs"][j][k];
+                tf2::Vector3 pos;
+                tf2::Quaternion rot;
+
+
+                (job_data["pos"].hasMember("x")) ? pos.setX(float_of(job_data["pos"]["x"])) :pos.setX(0);
+                (job_data["pos"].hasMember("y")) ? pos.setY(float_of(job_data["pos"]["y"])) :pos.setY(0);
+                (job_data["pos"].hasMember("z")) ? pos.setZ(float_of(job_data["pos"]["z"])) :pos.setZ(0);
+                (job_data["orientation"].hasMember("x")) ? rot.setX(float_of(job_data["orientation"]["x"])) :rot.setX(0);
+                (job_data["orientation"].hasMember("y")) ? rot.setY(float_of(job_data["orientation"]["y"])) :rot.setY(0);
+                (job_data["orientation"].hasMember("z")) ? rot.setZ(float_of(job_data["orientation"]["z"])) :rot.setZ(0);
+                (job_data["orientation"].hasMember("w")) ? rot.setW(float_of(job_data["orientation"]["w"])) :rot.setW(0);
+
+
+                ROS_INFO("=> Grab: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+                ROS_INFO("=> Grab: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+                jd.jobs_.push_back(tf2::Transform(rot, pos));
+            }
+            v_jd.push_back(jd);
+        }  
+        job_data_.insert(std::pair<std::string, std::vector<job_data>>(robot, v_jd));
+    }
+}
\ No newline at end of file