first parallel planning attempts

.catkin_tools/profiles/default/devel_collisions.txt

 /home/matteo/ws_panda/devel/./cmake.lock 42
-/home/matteo/reachability/devel/./cmake.lock 24055
+/home/matteo/reachability/devel/./cmake.lock 24364
 /home/matteo/reachability/devel/lib/libmoveit_grasps.so 79
 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79

src/.vscode/c_cpp_properties.json

 {
-    "configurations": [
-        {
-            "browse": {
-                "databaseFilename": "",
-                "limitSymbolsToIncludedHeaders": true
-            },
-            "includePath": [
-                "/home/matteo/ws_moveit/devel/include/**",
-                "/opt/ros/noetic/include/**",
-                "/home/matteo/ws_moveit/src/moveit/moveit_planners/chomp/chomp_motion_planner/include/**",
-                "/home/matteo/ws_moveit/src/eigen_stl_containers/include/**",
-                "/home/matteo/ws_moveit/src/geometric_shapes/include/**",
-                "/home/matteo/ws_moveit/src/moveit/moveit_planners/chomp/chomp_interface/include/**",
-                "/home/matteo/ws_moveit/src/moveit_resources/prbt_ikfast_manipulator_plugin/include/**",
-                "/home/matteo/ws_moveit/src/moveit/moveit_ros/benchmarks/include/**",
-                "/home/matteo/ws_moveit/src/moveit/moveit_plugins/moveit_ros_control_interface/include/**",
-                "/home/matteo/ws_moveit/src/moveit/moveit_ros/move_group/include/**",
-                "/home/matteo/ws_moveit/src/moveit/moveit_ros/occupancy_map_monitor/include/**",
-                "/home/matteo/ws_moveit/src/moveit/moveit_ros/robot_interaction/include/**",
-                "/home/matteo/ws_moveit/src/moveit/moveit_ros/moveit_servo/include/**",
-                "/home/matteo/ws_moveit/src/moveit/moveit_setup_assistant/include/**",
-                "/home/matteo/ws_moveit/src/moveit/moveit_plugins/moveit_simple_controller_manager/include/**",
-                "/home/matteo/ws_moveit/src/moveit_visual_tools/include/**",
-                "/home/matteo/ws_moveit/src/octomap_msgs/include/**",
-                "/home/matteo/ws_moveit/src/moveit/moveit_planners/pilz_industrial_motion_planner/include/**",
-                "/home/matteo/ws_moveit/src/moveit/moveit_planners/pilz_industrial_motion_planner_testutils/include/**",
-                "/home/matteo/ws_moveit/src/pybind11_catkin/include/**",
-                "/home/matteo/ws_moveit/src/random_numbers/include/**",
-                "/home/matteo/ws_panda/src/reachability/include/**",
-                "/home/matteo/ws_moveit/src/rosparam_shortcuts/include/**",
-                "/home/matteo/ws_moveit/src/rviz_visual_tools/include/**",
-                "/home/matteo/ws_moveit/src/srdfdom/include/**",
-                "/home/matteo/ws_moveit/src/warehouse_ros/include/**",
-                "/usr/include/**",
-                "${workspaceFolder}/mtc/include",
-                "${workspaceFolder}/mtc/include/impl",
-                "${workspaceFolder}/mtc/include/impl/ho",
-                "${workspaceFolder}/mtc/src",
-                "${workspaceFolder}/yaml_to_mtc/include",
-                "${workspaceFolder}/moveit_task_constructor/core/include",
-                "${workspaceFolder}/moveit_task_constructor/",
-                "${workspaceFolder}/object_creator/include"
-            ],
-            "name": "ROS",
-            "cStandard": "c17",
-            "cppStandard": "c++17"
-        }
-    ],
-    "version": 4
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/home/matteo/reachability/devel/include/**",
+        "/opt/ros/noetic/include/**",
+        "/home/matteo/reachability/src/moveit_task_constructor/core/include/**",
+        "/home/matteo/reachability/src/moveit_task_constructor/demo/include/**",
+        "/home/matteo/reachability/src/mtc/include/**",
+        "/home/matteo/reachability/src/object_creator/include/**",
+        "/home/matteo/reachability/src/reachability/include/**",
+        "/home/matteo/reachability/src/moveit_task_constructor/rviz_marker_tools/include/**",
+        "/home/matteo/reachability/src/simulation_util/include/**",
+        "/home/matteo/reachability/src/yaml_to_mtc/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

src/franka_description/robots/ceti_double.urdf.xacro

@@ -13,7 +13,7 @@
   <link name="world" />
 
 
-  <xacro:property name="yaml_file" value="$(find mtc)/results/dummy/-1801493263.yaml" />
+  <xacro:property name="yaml_file" value="$(find mtc)/results/dummy/-1511245846.yaml" />
   <xacro:property name="props" value="${xacro.load_yaml(yaml_file)}" />
 
   <xacro:property name="x1" value="${props['objects'][8]['pos']['x']}" />

src/mtc/descriptions/dummy2.yaml

-{task: {groups: {robot_x: [-0.300000 -0.700000 0.890000, -0.300000 -0.600000 0.890000, -0.200000 -0.700000 0.890000, -0.200000 -0.600000 0.890000, 0.100000 -0.700000 0.890000, 0.100000 -0.600000 0.890000, 0.100000 -0.300000 0.890000, 0.100000 -0.200000 0.890000, 0.100000 -0.100000 0.890000, 0.200000 -0.300000 0.890000, 0.200000 -0.200000 0.890000, 0.200000 -0.100000 0.890000, 0.300000 -0.300000 0.890000, 0.300000 -0.200000 0.890000, 0.300000 -0.100000 0.890000, 0.100000 0.100000 0.890000, 0.100000 0.200000 0.890000, 0.100000 0.300000 0.890000, 0.200000 0.100000 0.890000, 0.200000 0.200000 0.890000, 0.200000 0.300000 0.890000, 0.300000 0.100000 0.890000, 0.300000 0.200000 0.890000, 0.300000 0.300000 0.890000, -0.300000 0.600000 0.890000, -0.300000 0.700000 0.890000, -0.200000 0.600000 0.890000, -0.200000 0.700000 0.890000, 0.100000 0.600000 0.890000, 0.100000 0.700000 0.890000, -0.300000 0.600000 0.890000, -0.300000 0.700000 0.890000, -0.200000 0.600000 0.890000, -0.200000 0.700000 0.890000, 0.100000 0.600000 0.890000, 0.100000 0.700000 0.890000], robot_y: [0.100000 1.010000 0.890000, 0.100000 1.110000 0.890000, 0.100000 1.210000 0.890000, 0.200000 1.010000 0.890000, 0.200000 1.110000 0.890000, 0.200000 1.210000 0.890000, 0.300000 1.010000 0.890000, 0.300000 1.110000 0.890000, 0.300000 1.210000 0.890000, 0.100000 1.410000 0.890000, 0.100000 1.510000 0.890000, 0.100000 1.610000 0.890000, 0.200000 1.410000 0.890000, 0.200000 1.510000 0.890000, 0.200000 1.610000 0.890000, 0.300000 1.410000 0.890000, 0.300000 1.510000 0.890000, 0.300000 1.610000 0.890000, -0.300000 1.910000 0.890000, -0.300000 2.010000 0.890000, -0.200000 1.910000 0.890000, -0.200000 2.010000 0.890000, 0.100000 1.910000 0.890000, 0.100000 2.010000 0.890000, -0.300000 0.600000 0.890000, -0.300000 0.700000 0.890000, -0.200000 0.600000 0.890000, -0.200000 0.700000 0.890000, 0.100000 0.600000 0.890000, 0.100000 0.700000 0.890000, -0.300000 0.600000 0.890000, -0.300000 0.700000 0.890000, -0.200000 0.600000 0.890000, -0.200000 0.700000 0.890000, 0.100000 0.600000 0.890000, 0.100000 0.700000 0.890000]}}}
+task:
\ No newline at end of file
+  groups:
+    panda_arm1:
+      - -0.300000 -0.700000 0.890000
+      - -0.300000 -0.600000 0.890000
+      - -0.200000 -0.700000 0.890000
+      - -0.200000 -0.600000 0.890000
+      - 0.100000 -0.700000 0.890000
+      - 0.100000 -0.600000 0.890000
+      - 0.100000 -0.300000 0.890000
+      - 0.100000 -0.200000 0.890000
+      - 0.100000 -0.100000 0.890000
+      - 0.200000 -0.300000 0.890000
+      - 0.200000 -0.200000 0.890000
+      - 0.200000 -0.100000 0.890000
+      - 0.300000 -0.300000 0.890000
+      - 0.300000 -0.200000 0.890000
+      - 0.300000 -0.100000 0.890000
+      - 0.100000 0.100000 0.890000
+      - 0.100000 0.200000 0.890000
+      - 0.100000 0.300000 0.890000
+      - 0.200000 0.100000 0.890000
+      - 0.200000 0.200000 0.890000
+      - 0.200000 0.300000 0.890000
+      - 0.300000 0.100000 0.890000
+      - 0.300000 0.200000 0.890000
+      - 0.300000 0.300000 0.890000
+    panda_arm2:
+      - 0.100000 1.110000 0.890000
+      - 0.100000 1.210000 0.890000
+      - 0.200000 1.010000 0.890000
+      - 0.200000 1.110000 0.890000
+      - 0.200000 1.210000 0.890000
+      - 0.300000 1.010000 0.890000
+      - 0.300000 1.110000 0.890000
+      - 0.300000 1.210000 0.890000
+      - 0.100000 1.410000 0.890000
+      - 0.100000 1.510000 0.890000
+      - 0.100000 1.610000 0.890000
+      - 0.200000 1.410000 0.890000
+      - 0.200000 1.510000 0.890000
+      - 0.200000 1.610000 0.890000
+      - 0.300000 1.410000 0.890000
+      - 0.300000 1.510000 0.890000
+      - 0.300000 1.610000 0.890000
+      - -0.300000 1.910000 0.890000
+      - -0.300000 2.010000 0.890000
+      - -0.200000 1.910000 0.890000
+      - -0.200000 2.010000 0.890000
+      - 0.100000 1.910000 0.890000
+      - 0.100000 2.010000 0.890000
\ No newline at end of file

src/mtc/include/impl/abstract_robot.h

@@ -51,6 +51,8 @@ class Abstract_robot {
         observer_mask_ = std::bitset<3>(0);
 
     };
+
+
     
     inline std::string& name() { return name_;}
     inline tf2::Transform& tf() { return tf_;}

src/mtc/include/impl/moveit_mediator.h

@@ -71,6 +71,7 @@ class Moveit_mediator : public Abstract_mediator{
     void publish_tables();
     void load_robot_description();
     void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
+    void parallel_exec(Abstract_robot& penis, moveit_msgs::CollisionObject& source, std::vector<tf2::Transform>& target);
     void create_Task(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
     inline std::map<std::string, std::vector<moveit::task_constructor::Task>>& task_map(){return task_map_;};
 

src/mtc/include/impl/moveit_robot.h

@@ -25,6 +25,7 @@ class Moveit_robot : public Robot{
         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()));
 
+
     }
 
     inline moveit::planning_interface::MoveGroupInterface* mgi() {return mgi_;}

src/mtc/launch/cell_routine.launch

@@ -3,12 +3,12 @@
     <!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
 
     <!-- this is to change-->
-    <rosparam command="load" file="$(find mtc)/results/dummy/-1801493263.yaml"/>
+    <rosparam command="load" file="$(find mtc)/results/dummy/-1511245846.yaml"/>
 
 
     <rosparam command="load" file="$(find mtc)/mtc_templates/dummy.yaml" />
     <rosparam command="load" file="$(find mtc)/maps/dummy.yaml"/>
-    <rosparam command="load" file="$(find mtc)/descriptions/dummy.yaml"/>
+    <rosparam command="load" file="$(find mtc)/descriptions/dummy2.yaml"/>
     
 
     <include file="$(find ceti_double)/launch/demo.launch">