diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index 14474ed439c588e89f084c601a0a9e7af47b8f07..aa368eac912fc51e03fb4a4b2e9abdb298e11ec1 100644
--- a/.catkin_tools/profiles/default/devel_collisions.txt
+++ b/.catkin_tools/profiles/default/devel_collisions.txt
@@ -1,4 +1,4 @@
 /home/matteo/ws_panda/devel/./cmake.lock 42
-/home/matteo/reachability/devel/./cmake.lock 26266
+/home/matteo/reachability/devel/./cmake.lock 26279
 /home/matteo/reachability/devel/lib/libmoveit_grasps.so 79
 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79
diff --git a/src/mtc/include/impl/moveit_mediator.h b/src/mtc/include/impl/moveit_mediator.h
index 84f6d51af1601cf077c98bee762873f98b7b5cc9..7a0fa096f8fdeb55f00082f23f908482e3ea2799 100644
--- a/src/mtc/include/impl/moveit_mediator.h
+++ b/src/mtc/include/impl/moveit_mediator.h
@@ -95,7 +95,7 @@ class Moveit_mediator : public Abstract_mediator{
     void build_wings(std::bitset<3>& wing,int& robot) override;
     void set_wings(std::vector<std::vector<wing_BP>>& wbp) override;
 
-    void merge_stages(std::vector<std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>& tasks);
+    void merge_ps(moveit_msgs::PlanningScene& origin, moveit_msgs::PlanningScene* merge, Moveit_robot* mr);
     void publish_tables();
     void load_robot_description();
     void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp
index a97854ca964b7a1a829fcee4581a8d3e2b5bbf99..35d47b6b2305d1a3119760bbb560707ec3dd2e6d 100644
--- a/src/mtc/src/impl/moveit_mediator.cpp
+++ b/src/mtc/src/impl/moveit_mediator.cpp
@@ -255,11 +255,11 @@ void Moveit_mediator::setup_task(){
 					}
 					ROS_INFO("entry names");
 					for (auto  den : ps1->allowed_collision_matrix.entry_names){
-						ROS_INFO("%i", den);
+						ROS_INFO("%i", den.c_str());
 					}					
 					ROS_INFO("entry values");
 					for (auto  den : ps1->allowed_collision_matrix.entry_values){
-						ROS_INFO("%i", den);
+						for (auto ene : den.enabled) ROS_INFO("%i", ene);
 					}
 					ROS_INFO("[Robot 1] fft");
 					for (auto  ft : ps1->fixed_frame_transforms){
@@ -325,11 +325,11 @@ void Moveit_mediator::setup_task(){
 					}
 					ROS_INFO("entry names");
 					for (auto  den : ps2->allowed_collision_matrix.entry_names){
-						ROS_INFO("%i", den);
+						ROS_INFO("%i", den.c_str());
 					}					
 					ROS_INFO("entry values");
 					for (auto  den : ps2->allowed_collision_matrix.entry_values){
-						ROS_INFO("%i", den);
+						for (auto ene : den.enabled) ROS_INFO("%i", ene);
 					}
 					ROS_INFO("[Robot 2] fft");
 					for (auto  ft : ps2->fixed_frame_transforms){
@@ -402,33 +402,8 @@ void Moveit_mediator::setup_task(){
 
 }
 
-void Moveit_mediator::merge_stages(std::vector<std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>& tasks){
-	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 jt1;
-		moveit_msgs::RobotTrajectory jt2;
-
-		if ( i < tasks[0].front().solution.sub_trajectory.size()) {
-			jt1.joint_trajectory.header = tasks[0].front().solution.sub_trajectory[i].trajectory.joint_trajectory.header;
-			for(auto jn : tasks[0].front().solution.sub_trajectory[i].trajectory.joint_trajectory.joint_names) jt1.joint_trajectory.joint_names.push_back(jn);
-			for(auto point : tasks[0].front().solution.sub_trajectory[i].trajectory.joint_trajectory.points) jt1.joint_trajectory.points.push_back(point);
-			jt1.multi_dof_joint_trajectory.header = tasks[0].front().solution.sub_trajectory[i].trajectory.multi_dof_joint_trajectory.header;
-			for(auto jn : tasks[0].front().solution.sub_trajectory[i].trajectory.multi_dof_joint_trajectory.joint_names) jt1.multi_dof_joint_trajectory.joint_names.push_back(jn);
-			for(auto point : tasks[0].front().solution.sub_trajectory[i].trajectory.multi_dof_joint_trajectory.points) jt1.multi_dof_joint_trajectory.points.push_back(point);
-			tasks[0].pop();
-		}
-
-		if ( i < tasks[1].front().solution.sub_trajectory.size()) {
-			jt2.joint_trajectory.header = tasks[1].front().solution.sub_trajectory[i].trajectory.joint_trajectory.header;
-			jt2.multi_dof_joint_trajectory.header = tasks[1].front().solution.sub_trajectory[i].trajectory.multi_dof_joint_trajectory.header;
-			for(auto jn : tasks[1].front().solution.sub_trajectory[i].trajectory.joint_trajectory.joint_names) jt2.joint_trajectory.joint_names.push_back(jn);
-			for(auto point : tasks[1].front().solution.sub_trajectory[i].trajectory.joint_trajectory.points) jt2.joint_trajectory.points.push_back(point);
-			for(auto jn : tasks[1].front().solution.sub_trajectory[i].trajectory.multi_dof_joint_trajectory.joint_names) jt2.multi_dof_joint_trajectory.joint_names.push_back(jn);
-			for(auto point : tasks[1].front().solution.sub_trajectory[i].trajectory.multi_dof_joint_trajectory.points) jt2.multi_dof_joint_trajectory.points.push_back(point);
-			tasks[1].pop();
-		}
-		mgi_->execute(jt1);
-		mgi_->asyncExecute(jt2);
-	}
+void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& origin, moveit_msgs::PlanningScene* merge, Moveit_robot* mr){
+	
 }
 	
 void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps){