acm, ferdinand, tasks

include/impl/moveit_grasp_mediator.h

@@ -41,7 +41,7 @@ class Moveit_grasp_mediator : public Moveit_mediator{
 
     void manipulate_grasp_data(Moveit_robot* robot);
     void scene_setup();
-    void manipulate_mapspace();
+    void manipulate_mapspace(tf2::Transform& tf, tf2::Vector3& size);
     bool generateRandomCuboid(std::string& object_name, geometry_msgs::Pose& object_pose, double& x_depth, double& y_width, double& z_height);
     bool getIKSolution(const moveit::core::JointModelGroup* arm_jmg, const Eigen::Isometry3d& target_pose, robot_state::RobotState& solution, const std::string& link_name);
     bool planFullGrasp(std::vector<moveit_grasps::GraspCandidatePtr>& grasp_candidates,

include/impl/moveit_mediator.h

@@ -45,6 +45,7 @@
 #include <moveit/planning_scene_monitor/current_state_monitor.h>
 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 #include <moveit/moveit_cpp/moveit_cpp.h>
+#include <stdint.h>
 
 
 class Moveit_mediator : public Abstract_mediator{
@@ -66,6 +67,11 @@ class Moveit_mediator : public Abstract_mediator{
     std::shared_ptr<moveit::task_constructor::solvers::CartesianPath> cartesian_planner_;
     std::map<std::string, std::vector<moveit::task_constructor::Task>> task_map_;
 
+    std::map<std::string, std::vector<uint8_t>> acm_;
+
+
+
+
 
     public:
     Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh);
@@ -82,6 +88,8 @@ class Moveit_mediator : public Abstract_mediator{
 
 
     void merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr);
+    void merge_acm(moveit_msgs::PlanningScene& in);
+
     void publish_tables();
     void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
     void parallel_exec(Moveit_robot& r, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps);

include/impl/moveit_robot.h

@@ -23,9 +23,10 @@ class Moveit_robot : public Robot{
     Moveit_robot(std::string& name, tf2::Transform tf, tf2::Vector3 size) : Robot(name, tf, size)
         , mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>(name)){
 
-        std::stringstream hand_n, ik_frame_n, name_n;
+        std::stringstream hand_n, ik_frame_n, name_n, base_n;
         hand_n << "hand_" << name.back();
         ik_frame_n << "panda_" << name.back() << "_link8";
+        base_n << "base_" << name.back();
         mgi_hand_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(hand_n.str());
 
 
@@ -35,6 +36,8 @@ class Moveit_robot : public Robot{
         map_.insert(std::make_pair<std::string, std::string>("eef_name", hand_n.str()));
         map_.insert(std::make_pair<std::string, std::string>("hand_frame", ik_frame_n.str()));
         map_.insert(std::make_pair<std::string, std::string>("hand_group_name", hand_n.str()));
+        map_.insert(std::make_pair<std::string, std::string>("base", base_n.str()));
+
     }
 
     inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi() {return mgi_;}

include/impl/wing.h

@@ -24,7 +24,7 @@ class Wing  : public Abstract_robot_element{
     inline void set_name(std::string str) {name_ = str;}
     inline void set_set(tf2::Vector3& vec) {size_ = vec;}
 
-    inline tf2::Vector3 size(){ return size_;}
+    inline tf2::Vector3& size(){ return size_;}
     inline std::vector<tf2::Transform>& bounds() {return bounds_;}
     void update(tf2::Transform& tf) override {this->calc_world_tf(tf);}
 };

include/reader/abstract_param_reader.h

@@ -16,6 +16,11 @@ struct object_data {
     tf2::Vector3 size_;
 };
 
+struct job_data {
+    std::string name_;
+    std::vector<tf2::Transform> jobs_;
+};
+
 class Abstract_param_reader{
     protected:
     std::shared_ptr<ros::NodeHandle> nh_;

include/reader/job_reader.h

+#ifndef JOB_READER_
+#define JOB_READER_
+
+#include "ros/ros.h"
+#include <ros/package.h>
+#include <xmlrpcpp/XmlRpc.h>
+
+#include "reader/abstract_param_reader.h"
+
+
+class Job_reader : public Abstract_param_reader{
+    protected:
+    std::vector<job_data> job_data_;
+    std::string name_;
+
+    public:
+    Job_reader(std::shared_ptr<ros::NodeHandle> const& d, std::string const& name) 
+    : 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_;}
+
+    void read() override;
+};
+#endif

src/impl/moveit_grasp_mediator.cpp

@@ -2,6 +2,10 @@
 #include <regex>
 #include <gb_grasp/MapConfigLoader.h>
 #include <moveit/robot_state/conversions.h>
+<<<<<<< HEAD
+=======
+#include "impl/wing_moveit_decorator.h"
+>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
 
 
 void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) {
@@ -10,7 +14,10 @@ void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) {
 	auto mr = dynamic_cast<Moveit_robot*>(robot);
 
 	manipulate_grasp_data(mr);
+<<<<<<< HEAD
     manipulate_mapspace();
+=======
+>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
 
 	auto map_gen = std::make_shared<MapGenerator>(*nh_, visual_tools_, planning_scene_monitor_, mr->map().at("eef_name"), mr->name().c_str(), robot_model_->getJointModelGroup(mr->name().c_str()), mgi().get()->getCurrentState(), voxel_manager_);
 	double min_quality, max_candidates;
@@ -19,7 +26,13 @@ void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) {
   rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"min_quality",min_quality);
 	rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"max_candidate_count",max_candidates);
 	rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"top_grasps_only",top_only);
+<<<<<<< HEAD
 
+=======
+  ROS_INFO("min_quality: %f", min_quality);
+  ROS_INFO("max_candidate_count: %f", max_candidates);
+  ROS_INFO("top_grasps_only: %f", top_only);
+>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
 
 
   map_gen->SetQuality(min_quality);
@@ -29,6 +42,7 @@ void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) {
 
 }
 
+<<<<<<< HEAD
 void Moveit_grasp_mediator::manipulate_mapspace(){
     std::vector<float> pos_voxel_space;
     std::vector<float> pos_mapspace;
@@ -38,6 +52,20 @@ void Moveit_grasp_mediator::manipulate_mapspace(){
     pos_voxel_space[2]= 0.91f;
     nh_->setParam("/voxel_space/pos", pos_voxel_space);
     nh_->setParam("/mapspace/pos", pos_mapspace);
+=======
+void Moveit_grasp_mediator::manipulate_mapspace(tf2::Transform& tf, tf2::Vector3& size){
+    double r,p,y;
+    std::vector<double> rot_mapspace(3,0);
+
+    tf2::Matrix3x3 m(tf.getRotation());
+    m.getRPY(r,p, y);
+    nh_->setParam("/mapspace/rot", std::vector<double>({r,p,y}));
+    nh_->setParam("/mapspace/pos", std::vector<double>({tf.getOrigin().getX(), tf.getOrigin().getY(), 0.91f}));
+    nh_->setParam("/mapspace/dim", std::vector<double>({size.getX(), size.getY(), size.getZ()}));
+    //nh_->setParam("/voxel_space/dim", std::vector<float>({0.3f,0.3f,0.2f}));
+    //nh_->setParam("/voxel_space/pos", std::vector<double>({tf.getOrigin().getX(), tf.getOrigin().getY(), tf.getOrigin().getZ()}));
+    //nh_->setParam("/voxel_space/rot", std::vector<double>({r,p,y}));
+>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
 }
 
 void Moveit_grasp_mediator::manipulate_grasp_data(Moveit_robot* robot) {
@@ -103,7 +131,11 @@ bool Moveit_grasp_mediator::generateRandomCuboid(std::string& object_name, geome
                             double& y_width, double& z_height)
   {
     // Generate random cuboid
+<<<<<<< HEAD
     double xmin = 0.5;
+=======
+    double xmin = 0.1;
+>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
     double xmax = 0.7;
     double ymin = -0.25;
     double ymax = 0.25;
@@ -134,6 +166,7 @@ bool Moveit_grasp_mediator::generateRandomCuboid(std::string& object_name, geome
 
 void Moveit_grasp_mediator::mediate(){
 	visual_tools_->deleteAllMarkers();
+<<<<<<< HEAD
     /*
     MapConfigLoader map_config(*nh_, grasp_ns_);
 	auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin();
@@ -162,6 +195,87 @@ void Moveit_grasp_mediator::mediate(){
     }
     voxel_manager_->Voxelize(true);
     */
+=======
+  ROS_INFO("ns is %s", grasp_ns_.c_str());
+  MapConfigLoader map_config(*nh_, grasp_ns_.c_str());
+  auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin();
+  map_config.SetObjects(cbd);
+  float object_offset = 0.02f;
+
+  tf2::Vector3 middle_pt(0,0,0);
+  std::vector<double> x;
+  std::vector<double> y;
+  for(auto* ar: robots_){
+    Moveit_robot* r = dynamic_cast<Moveit_robot*>(ar);
+    tf2::Vector3 pos = r->tf().getOrigin();
+    pos.setZ((pos.getZ()*2) +object_offset);
+    tf2::Transform man_tf(r->tf().getRotation(), pos);
+    manipulate_mapspace(man_tf, r->size());
+    map_config.Create(grasp_maps_);
+    middle_pt+= pos;
+    ROS_INFO("X: %f, Y: %f, Z: %f", pos.getX(), pos.getY(), pos.getZ());
+
+    tf2::Transform A = r->tf() * r->bounds()[0];
+    tf2::Transform B = r->tf() * r->bounds()[1];
+    tf2::Transform C = r->tf() * r->bounds()[2];
+    tf2::Transform D = r->tf() * r->bounds()[3];
+    x.push_back(A.getOrigin().getX());
+    x.push_back(B.getOrigin().getX());
+    x.push_back(C.getOrigin().getX());
+    x.push_back(D.getOrigin().getX());
+    y.push_back(A.getOrigin().getY());
+    y.push_back(B.getOrigin().getY());
+    y.push_back(C.getOrigin().getY());
+    y.push_back(D.getOrigin().getY());
+
+    r->grasp_map_generator()->SampleMap(grasp_maps_.front());
+    for (auto* are: r->observers()){
+      auto wmd = dynamic_cast<Wing_moveit_decorator*>(are);
+      auto w = dynamic_cast<Wing*>(wmd->wing());
+      pos.setX(w->world_tf().getOrigin().getX());
+      pos.setY(w->world_tf().getOrigin().getY());
+      man_tf.setRotation(w->world_tf().getRotation());
+      man_tf.setOrigin(pos);
+      manipulate_mapspace(man_tf, w->size());
+      map_config.Create(grasp_maps_);
+      middle_pt+= pos;
+      ROS_INFO("X: %f, Y: %f, Z: %f", pos.getX(), pos.getY(), pos.getZ());
+
+      A = w->world_tf() * w->bounds()[0];
+      B = w->world_tf() * w->bounds()[1];
+      C = w->world_tf() * w->bounds()[2];
+      D = w->world_tf() * w->bounds()[3];
+      x.push_back(A.getOrigin().getX());
+      x.push_back(B.getOrigin().getX());
+      x.push_back(C.getOrigin().getX());
+      x.push_back(D.getOrigin().getX());
+      y.push_back(A.getOrigin().getY());
+      y.push_back(B.getOrigin().getY());
+      y.push_back(C.getOrigin().getY());
+      y.push_back(D.getOrigin().getY());
+      r->grasp_map_generator()->SampleMap(grasp_maps_.front());
+    }
+  }
+  ROS_INFO_STREAM("number of maps: "<< grasp_maps_.size());
+  middle_pt/= grasp_maps_.size();
+  ROS_INFO("X: %f, Y: %f, Z: %f", middle_pt.getX(), middle_pt.getY(), middle_pt.getZ());
+  auto x_min = *std::min_element(x.begin(),x.end());
+  auto x_max = *std::max_element(x.begin(),x.end());
+  auto y_min = *std::min_element(y.begin(),y.end());
+  auto y_max = *std::max_element(y.begin(),y.end());
+  ROS_INFO("width: %f", x_max-x_min);
+  ROS_INFO("height: %f", y_max-y_min);
+
+  visual_tools_->setAlpha(0.1);
+  voxel_manager_->Voxelize();
+
+  auto cod = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_obstacle();
+  for(auto&o:cod){
+    voxel_manager_->GetCuboidCollisions(o,voxel_environment_);
+  }
+  voxel_manager_->Voxelize(true);
+    
+>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
 
     // -----------------------------------
     // Generate random object to grasp
@@ -376,7 +490,11 @@ bool Moveit_grasp_mediator::planPreApproach(const robot_state::RobotState& goal_
     double tolerance_above = 0.01;
     double tolerance_below = 0.01;
     // req.planner_id = "RRTConnectkConfigDefault";
+<<<<<<< HEAD
     req.group_name = "panda_arm1";
+=======
+    req.group_name = "panda_1_arm1";
+>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
     req.num_planning_attempts = 5;
     req.allowed_planning_time = 1.5;
     moveit_msgs::Constraints goal =
@@ -446,8 +564,13 @@ void Moveit_grasp_mediator::scene_setup(){
 	  auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin();
     auto cod = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_obstacle();
 
+<<<<<<< HEAD
+	ROS_INFO("%i die cuboids", cbd.size());
+	ROS_INFO("%i cuboids", cod.size());
+=======
 	  ROS_INFO("%i die cuboids", cbd.size());
 	  ROS_INFO("%i cuboids", cod.size());
+>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
 
     for(auto&o:cod){
       o.publishCOwait(visual_tools_,planning_scene_monitor_,rviz_visual_tools::GREY);
@@ -456,8 +579,13 @@ void Moveit_grasp_mediator::scene_setup(){
 
     for(auto&o:cbd){
 		//o.publish(visual_tools_,rviz_visual_tools::GREEN);
+<<<<<<< HEAD
+		psi_->addCollisionObjects({o.getCoMsg()});
+	}
+=======
 		  psi_->addCollisionObjects({o.getCoMsg()});
 	  }
+>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
 
 	visual_tools_->publishRobotState(mgi_->getCurrentState());
 	visual_tools_->trigger();
@@ -487,6 +615,11 @@ Moveit_grasp_mediator::Moveit_grasp_mediator(std::vector<std::vector<tf2::Transf
         visual_tools_->trigger();
 
         nh_->getParam("/grasps_ns", grasp_ns_);
+<<<<<<< HEAD
+=======
+        grasp_ns_ = "grasps_ns";
+        ROS_INFO("ns is %s", grasp_ns_.c_str());
+>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
 
         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_);

src/impl/moveit_mediator.cpp

@@ -3,6 +3,7 @@
 #include <mutex> 
 #include <algorithm>
 #include <gb_grasp/MapGenerator.h>
+#include <regex>
 
 thread_local moveit::task_constructor::Task task__;
 thread_local moveit_task_constructor_msgs::ExecuteTaskSolutionGoal etsg_;
@@ -13,17 +14,21 @@ std::mutex task_writing;
 void Moveit_mediator::connect_robots(Abstract_robot* robot) {
 	robots_.push_back(robot);
 	ROS_INFO("%s connected...", robot->name().c_str());
+
+	Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robot);
+	acm_.insert(std::pair<std::string, std::vector<uint8_t>>(mr->map().at("base").c_str(), std::vector<uint8_t>()));
+	for(auto name: mr->mgi()->getLinkNames()) acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
+	for(auto name: mr->mgi_hand()->getLinkNames()) acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
 }
 
 void Moveit_mediator::publish_tables(){
     ros::WallDuration sleep_time(1.0);
     for(long unsigned int i = 0; i < robots_.size();i++){
         Moveit_robot* ceti = dynamic_cast<Moveit_robot*>(robots_[i]);
-		std::stringstream ss;
-    	ss << "base_" << ceti->name().back();
 
+		/*
 		moveit_msgs::CollisionObject table;
-		table.id = ss.str();
+		table.id = ceti->map().at("base").c_str();
 		table.header.frame_id = "world";
 		table.header.stamp = ros::Time::now();
 		table.primitives.resize(1);
@@ -41,11 +46,13 @@ void Moveit_mediator::publish_tables(){
 		table.primitive_poses[0].orientation.z = ceti->tf().getRotation().getZ();
 		table.primitive_poses[0].orientation.w = ceti->tf().getRotation().getW();
 		table.operation = table.ADD;
+		*/
 		//psi_->applyCollisionObject(table);
 
         for (Abstract_robot_element* are : ceti->observers()) {
             Wing_moveit_decorator* wmd = dynamic_cast<Wing_moveit_decorator*>(are);
             psi_->applyCollisionObject(*wmd->markers());
+			acm_.insert(std::pair<std::string, std::vector<uint8_t>>(wmd->markers()->id.c_str(), std::vector<uint8_t>()));
             sleep_time.sleep();
         }
     }
@@ -139,6 +146,9 @@ void Moveit_mediator::setup_task(){
 			}
 		}
 	} else {
+		acm_.insert(std::pair<std::string, std::vector<uint8_t>>("bottle0", std::vector<uint8_t>()));
+		acm_.insert(std::pair<std::string, std::vector<uint8_t>>("bottle1", std::vector<uint8_t>()));
+
 		moveit_msgs::CollisionObject bottle1;
 		bottle1.id = "bottle0";
 		bottle1.header.frame_id = "world";
@@ -232,6 +242,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++){
@@ -240,6 +251,9 @@ void Moveit_mediator::setup_task(){
 				moveit_msgs::PlanningScene* ps1 = nullptr;
 				moveit_msgs::PlanningScene* ps2 = nullptr;
 				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());
 
 				if (i < tasks[0].front().solution.sub_trajectory.size()){
 					t1 = &tasks[0].front().solution.sub_trajectory[i].trajectory;
@@ -325,7 +339,6 @@ void Moveit_mediator::setup_task(){
 					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){
@@ -407,20 +420,141 @@ void Moveit_mediator::setup_task(){
 				if (t1){
 					Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[0]);
 					th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t1, *ps1));
-					merge_ps(ps_m, ps1, mr);
 
+					// First find ID from panda to start with
+					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*");
+
+					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");
+							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());
+								}
+							}
+						}
+					}
+					merge_ps(ps_m, ps1, mr);
 				}
 
 				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*");
+
+					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");
+							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());
+								}
+							}
+						}
+					}
 					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();
 				}
+				merge_acm(ps_m);
 				planning_scene_diff_publisher_->publish(ps_m);
 
 			}
@@ -431,6 +565,18 @@ void Moveit_mediator::setup_task(){
 
 }
 
+void Moveit_mediator::merge_acm(moveit_msgs::PlanningScene& ps_m){
+	moveit_msgs::PlanningScene::_allowed_collision_matrix_type acmt;
+	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;
+
+}
+
 void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr){
 	// get full mr link list 
 	std::vector<std::string> links;