diff --git a/src/impl/moveit_mediator.cpp b/src/impl/moveit_mediator.cpp
index adf09aef46c47eb6c5f40c0444a46e15f91e8eb0..eca4db4213b6bd4d167885e0bc4a1069aaa2b28e 100644
--- a/src/impl/moveit_mediator.cpp
+++ b/src/impl/moveit_mediator.cpp
@@ -904,12 +904,22 @@ void Moveit_mediator::task_planner(){
 			}
 		}
 		
-		
 		for(auto& t : th){
 			if(t.joinable()) t.join();
 		}
-		//merge_acm(ps_m);
-		//planning_scene_diff_publisher_->publish(ps_m);
+
+		for(auto& exec : executions_){
+			for (int i = 0; i < robots_.size(); i++){
+				if (exec.first == robots_[i]->name()){
+					auto mr = dynamic_cast<Moveit_robot*>(robots_[i]);
+					manipulate_acm(mr, exec.second.second);
+					merge_ps(ps_m, exec.second.second, mr);
+				}
+			}
+		}
+		
+		merge_acm(ps_m);
+		planning_scene_diff_publisher_->publish(ps_m);
 	}