Initial commit

CMakeLists.txt

+cmake_minimum_required(VERSION 3.0.2)
+project(multi_cell_builder)
+# C++ 11
+add_compile_options(-std=c++17)
+
+# Load catkin and all dependencies required for this package
+find_package(catkin REQUIRED COMPONENTS
+  moveit_task_constructor_core
+  moveit_task_constructor_visualization
+  moveit_task_constructor_msgs
+  eigen_conversions
+  geometry_msgs
+  message_generation
+  moveit_core
+  moveit_msgs
+  moveit_ros_planning
+  moveit_ros_planning_interface
+  moveit_visual_tools
+  roscpp
+  roslint
+  rosparam_shortcuts
+  std_msgs
+  tf
+  tf_conversions
+  trajectory_msgs
+  pcl_ros
+
+  gb_grasp
+  moveit_grasps
+)
+
+find_package(yaml-cpp REQUIRED)
+find_package(Protobuf 3 REQUIRED)
+include_directories(${CMAKE_CURRENT_BINARY_DIR})
+
+
+# Catkin
+catkin_package(
+  LIBRARIES
+    ${Protobuf_LIBRARIES}
+  CATKIN_DEPENDS
+    moveit_ros_planning_interface
+    moveit_grasps
+  INCLUDE_DIRS
+    ${CMAKE_CURRENT_BINARY_DIR}
+    ${CMAKE_CURRENT_SOURCE_DIR}
+    ${Protobuf_INCLUDE_DIRS}
+    include
+  DEPENDS
+)
+
+
+
+###########
+## Build ##
+###########
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIR}
+  ${CMAKE_CURRENT_BINARY_DIR}
+  ${CMAKE_CURRENT_SOURCE_DIR}
+)
+
+protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/connector.proto)
+
+add_executable(base_routine src/base_routine.cpp 
+src/impl/abstract_mediator.cpp
+src/impl/base_by_rotation.cpp
+src/impl/field_rviz_decorator.cpp
+src/impl/map_loader.cpp
+src/impl/mediator.cpp
+src/impl/moveit_mediator.cpp
+src/impl/robot.cpp
+src/impl/wing_moveit_decorator.cpp
+src/impl/wing_rviz_decorator.cpp
+src/impl/collision_helper.cpp
+
+src/reader/abstract_param_reader.cpp
+src/reader/robot_reader.cpp
+src/reader/wing_reader.cpp
+src/reader/cuboid_reader.cpp
+
+
+
+
+)
+
+add_executable(cell_routine src/cell_routine.cpp
+src/impl/map_loader.cpp
+src/impl/abstract_mediator.cpp
+src/impl/moveit_mediator.cpp
+src/impl/wing_moveit_decorator.cpp
+src/impl/robot.cpp
+
+src/impl/mediator.cpp
+src/impl/wing_moveit_decorator.cpp
+src/impl/wing_rviz_decorator.cpp
+src/impl/collision_helper.cpp
+
+src/reader/abstract_param_reader.cpp
+src/reader/robot_reader.cpp
+src/reader/wing_reader.cpp
+src/reader/cuboid_reader.cpp
+)
+
+add_executable(grasp_cell_routine src/grasp_cell_routine.cpp
+src/impl/map_loader.cpp
+src/impl/abstract_mediator.cpp
+src/impl/moveit_mediator.cpp
+src/impl/wing_moveit_decorator.cpp
+src/impl/robot.cpp
+
+src/impl/mediator.cpp
+src/impl/wing_moveit_decorator.cpp
+src/impl/wing_rviz_decorator.cpp
+src/impl/collision_helper.cpp
+src/impl/moveit_grasp_mediator.cpp
+
+src/reader/abstract_param_reader.cpp
+src/reader/robot_reader.cpp
+src/reader/wing_reader.cpp
+src/reader/cuboid_reader.cpp
+)
+
+
+add_executable(mtc2taskspace src/mtc2taskspace.cpp
+src/reader/abstract_param_reader.cpp
+src/reader/task_space_reader.cpp
+src/reader/robot_reader.cpp
+)
+
+
+add_dependencies(base_routine ${base_routine_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(cell_routine ${cell_routine_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(mtc2taskspace ${mtc2taskspace_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(grasp_cell_routine ${grasp_cell_routine_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+
+target_link_libraries(base_routine 
+  ${catkin_LIBRARIES}
+  ${OCTOMAP_LIBRARIES}
+  ${PCL_LIBRARY_DIRS}
+  ${moveit_grasps_LIBRARIES}
+  yaml-cpp
+)
+target_link_libraries(cell_routine 
+  ${catkin_LIBRARIES}
+  ${OCTOMAP_LIBRARIES}
+  ${PCL_LIBRARY_DIRS}
+  ${moveit_grasps_LIBRARIES}
+  yaml-cpp
+)
+
+target_link_libraries(grasp_cell_routine 
+  ${catkin_LIBRARIES}
+  ${OCTOMAP_LIBRARIES}
+  ${PCL_LIBRARY_DIRS}
+  ${moveit_grasps_LIBRARIES}
+  yaml-cpp
+)
+
+target_link_libraries(mtc2taskspace 
+  ${catkin_LIBRARIES}
+  ${OCTOMAP_LIBRARIES}
+  yaml-cpp
+)

README.md

+# multi_cell_builder 
+multi_cell_builder is the core package developed with different design patterns to ensure both extensibility and variability.
+
+## executables
+multi_cell_builder consists of 3 executables, with each computing .yaml files for the next subsequent module, according the following strcture.
+> mtc2taskspace >> base_routine >> cell_routine
+
+### mtc2taskspace 
+From a bare Ceti-resource file representing an arbitrary scene with specific locations, located in the /resources directory. Mtc2taskspace computes a new file and associates each object with a drop-of-location to a robot. The shared space between robots is expressed by identical location entries for each robot involved. <br/>
+multi_cell_builder/mtc/mtc2taskspace.cpp (/resources/{resource}) -> (/descriptions/{resource}) <br/>
+roslaunch multi_cell_builder mtc2taskspace.launch resource='dummy.yaml'
+
+
+### base_routine  
+Calculates the workspace from a descriptions file (/descriptions/{description}) and a precomputed reachability map (/map/{map}), combined with information of a resource file (/resources/{resource}). Creates the directory (results/{resource}/ros::time.yaml}) with the resulting files as contents. <br/> 
+multi_cell_builder/src/base_routine.cpp (/map/{map}, /resources/{resource}, /descriptions/{descriptions}) -> (/results/{resource}/ros::time.yaml) <br/>
+roslaunch multi_cell_builder base_routine.launch resource='dummy.yaml' map='dummy.yaml' description='dummy.yaml'
+> Replace already existing result folder if exists.
+>> Resulting files can break the yaml schema, because each robot has an extra rpy element which is important for setting robot pose in franca_description. Also each 8th entry must be a table[0-9]+_table_top id. Cosmetic data (table_wheel...) included as placeholder
+
+### cell_routine  
+Designed to load a resource file (+ with additional rpy component in the right order) and execute a task with moveit task constructor. A task description can also be the /descriptions/{description} in the example. <br/>
+multi_cell_builder/src/cell_routine.cpp (/results/{result}, /descriptions/{task}) -> () <br/>
+roslaunch multi_cell_builder base_routine.launch result='/dummy/dummy.yaml' task='dummy.yaml' (is coop) <br/>
+roslaunch multi_cell_builder base_routine.launch result='/dummy/dummy.yaml' task='dummy2.yaml' (is parallel) 
+> Christoph's parser could not be integrated because there is no way to have multiple robots compute a task.
+
+
+
+## Current state
+State: in progress. Every executable is working with some explained downfalls that should be fixed in the next days. Implementing or omitting ideas because some modules do not work as expected took a lot of time. The actual results (62 files for dummy.yaml) can be loaded and run. I tested 10/62 myself, with surprisingly good results. The following list shows open and closed ToDos.<br/>
+
+:ballot_box_with_check: present a faster work-space execution <br/>
+:ballot_box_with_check: generate more solutions for 1 dump file <br/>
+:ballot_box_with_check: present solution for general robots <br/>
+:ballot_box_with_check: elaborate the moveit task constructor <br/>
+:ballot_box_with_check: present more general task files <br/>
+:black_square_button: include cosmetic values <br/>
+:ballot_box_with_check: passing yaml in xacro (franca_description) <br/>
+:ballot_box_with_check: parallel tasks (Docker already downloaded, just have to try) (main focus) <br/>
+:black_square_button: fix acm <br/>
+:black_square_button: include ferdinands work (main focus)<br/>
+
+Im working non stop, so don't forget to pull. <br/>
+[Git Docker](https://github.com/cambel/moveit_simultaneous_motions_demo.git "github link for parallel docker")
+[Ferdinand](https://git-st.inf.tu-dresden.de/ceti/ros-internal/technical-tests/gb-grasp-auerswald/-/blob/main/GB_Auerswald.pdf "Ferdi bre")

config/moveit_grasps_config.yaml

+moveit_grasps:
+  ####################################
+  # These settings are used by the grasp generator which generates GraspCandidates
+  ####################################
+  generator:
+    # Other verbose stuff
+    verbose: true
+
+    # Show hand of generated grasps
+    show_prefiltered_grasps: true
+    show_prefiltered_grasps_speed: 0.001  # (sec / grasp)
+
+    # --------------------------------
+    # Finger gripper specific settings
+    # --------------------------------
+    # None yet
+
+    # --------------------------------
+    # Suction gripper specific settings
+    # --------------------------------
+    # Visual debug options
+    debug_top_grasps: false
+    # Show suction gripper overhang
+    show_grasp_overhang: false
+
+  ####################################
+  # The grasp filter filters out unreachable GraspCandidates and populates valid GraspCandidates' pregrasp_ik_solution_ and grasp_ik_solution_'s
+  ####################################
+  filter:
+    # Show summary statistics
+    statistics_verbose: true
+    # Visualize the planes used by the filter
+    show_cutting_planes: true
+    # Collision checking in verbose. Note: this will cause filtering to happen on a single thread rather than in parallel
+    collision_verbose: true
+    collision_verbose_speed: 0.01  # (sec/ solution)
+    # Collision checking in verbose on second pass (after all failed)
+    show_grasp_filter_collision_if_failed: true
+    # Show post-filter arrows
+    show_filtered_grasps: true
+    # Show pose-filter arm ik solutions
+    show_filtered_arm_solutions: true
+    show_filtered_arm_solutions_pregrasp_speed: 0.01  # (sec / solution)
+    show_filtered_arm_solutions_speed: 0.01  # (sec / solution)
+
+  ####################################
+  # The GraspPlanner generates approach, lift and retreat paths for a GraspCandidate.
+  # If the GraspPlanner is unable to plan 100% of the approach path and at least ~90% of the lift and retreat paths, then it considers the GraspCandidate to be infeasible
+  ####################################
+  planner:
+    # Cartesian planning
+    statistics_verbose: true
+    verbose_cartesian_filtering: false
+    show_cartesian_waypoints: false
+    collision_checking_verbose: false

config/panda_grasp_data.yaml

+base_link: 'world'
+# =====================================================
+# Note: these parameters are copied from Robotiq gripper without tweaking
+hand:
+    end_effector_name: 'hand'  # ee group name
+    end_effector_type: 'finger'  # 'finger' or 'suction'
+
+    # x/y translation and angle orientation grasp resolution parameters used for sampling grasp candidates
+    grasp_resolution : 1
+    angle_resolution : 90    # degrees
+
+    # min/max values of the grasp depth range in meter.
+    # This should fit to the distance from finger tip to inner palm or the suction cup stroke
+    grasp_max_depth : 0.035
+    grasp_min_depth : 0.0
+    # grasp_depth_resolution is the discretization distance in meter used for sampling the grasp depth value.
+    # Idealy this should be a small fraction of the depth range (i.e. 0.005).
+    # If the value exceeds the depth distance range grasp_max_depth is used by default.
+    grasp_depth_resolution : 0.035
+
+    # A grasp motion is composed of three linear paths: approach, retreat, lift
+    # The values below define the desired distances of each path.
+    # Length of approach path (this is in addition to the grasp_max_depth)
+    approach_distance_desired: 0.05  # approach path (in addition to grasp_max_depth)
+    retreat_distance_desired: 0.05   # retreat path (in addition to grasp_max_depth)
+    lift_distance_desired: 0.01      # lift path (used as MINIMUM safety distance from surface)
+
+    # minimum padding on each side of the object on approach
+    grasp_padding_on_approach: 0.005  # meter
+
+    # We need to know the transform from the arm IK link to the grasp point. By default we define the transform manually with
+    # `tcp_to_eef_mount_transform`. Alternatively, we can set this to a frame in the robot model and compute the transform
+    # automatically. this is done by setting `define_tcp_by_name: true` and providing a value for `tcp_name`.
+    #
+    # Distance from the eef mount to the palm of end effector [x, y, z, r, p, y]
+    # z-axis pointing toward object to grasp
+    # x-axis perp. to movement of grippers
+    # y-axis parallel to movement of grippers
+    # If we have a tool center point frame, we would set:
+    define_tcp_by_name: true
+    tcp_name: "tool_center_point"
+    # Alternatively, if we cannot add a frame for the tool center point, we can define tcp by transform
+    # tcp_to_eef_mount_transform :  [0, 0, -0.105, 0, 0, -0.7853] # NOTE: Imaginary point in the middle
+
+    # Target durations in seconds for reaching pregrasp and grasp positions in end effector motions
+    pregrasp_time_from_start : 0.0
+    grasp_time_from_start : 0.0
+
+    # Joint names and state values for the end effector.
+    # The names should match the values of pregrasp_posture and grasp_posture.
+    # These values can be left empty if end_effector_type is 'suction'
+    pregrasp_posture : [0.04]  # open position
+    grasp_posture : [0.0]      # close position
+    joints : ['panda_finger_joint1']
+
+    ##############################
+    ##  Finger gripper parameters
+    ##############################
+
+    # Distances between fingers matching to pregrasp_posture and grasp_posture, in meters
+    max_finger_width: 0.085
+    min_finger_width: 0.06
+
+    # Maximum allowed finger width for a grasp.
+    # This value should be considerably smaller than max_finger_width
+    # to allow padded collision checks
+    max_grasp_width: 0.08
+
+    # width of gripper fingers to ensure the finger tips fully overlap the object
+    gripper_finger_width : 0.015
+
+    ##############################
+    ## Suction gripper parameters
+    ##############################
+    # The width of the suction gripper along the specified axes. (Assumes that Z points in the direction of the suction cups)
+    active_suction_range_x : 0.22
+    active_suction_range_y : 0.12
+
+    # Some suction grippers can selectively enable and disable regions of suction. If you have more than one
+    # region that can be selectively activated or de-activated, set those values here and moveit_grasps
+    # will prioritize grasp candidates which use full suction regions rather than partial regions.
+    # it will also set the suction_regions_in_contact_ variable in the grasp candidates so that you are able
+    # to only enable suction on regions that are in contact with the grasp target.
+    # Note that this metric assumes that suction regions are equally sized
+    # ie. width = active_suction_range_x / suction_cols_count
+    #
+    suction_rows_count: 2
+    suction_cols_count: 2

descriptions/dummy.yaml

+task:
+  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
+      - -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
+    panda_arm2:
+      - 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
\ No newline at end of file

descriptions/dummy2.yaml

+task:
+  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

include/impl/abstract_map_loader.h

+#ifndef ABSTRACT_MAP_LOADER_
+#define ABSTRACT_MAP_LOADER_
+
+#include "ros/ros.h"
+#include <xmlrpcpp/XmlRpc.h>
+#include <tf2/LinearMath/Transform.h>
+
+#include "octomap/octomap.h"
+#include <pcl/point_cloud.h>
+#include <pcl/octree/octree.h>
+
+
+#include <ros/package.h>
+#include <yaml-cpp/yaml.h>
+#include <fstream>
+
+#include "impl/abstract_robot.h"
+#include "impl/abstract_strategy.h"
+
+class Abstract_strategy;
+class Abstract_robot;
+
+
+class Abstract_map_loader{
+    protected:
+    std::vector<tf2::Transform> inv_map_;
+    std::vector<tf2::Transform> map_;
+    std::vector<std::vector<tf2::Transform>> task_grasps_;
+    std::vector<std::vector<std::vector<tf2::Quaternion>>> target_rot_;
+    std::vector<std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>> target_cloud_;
+
+    Abstract_strategy *strategy_; 
+
+    public:
+    Abstract_map_loader() = default;
+    ~Abstract_map_loader() = default;
+
+    inline void set_strategy(Abstract_strategy* some_stratergy) {strategy_ = some_stratergy;}
+    inline Abstract_strategy* strategy() {return strategy_;}
+    
+    inline std::vector<tf2::Transform>& inv_map() {return inv_map_;}
+    inline std::vector<tf2::Transform>& map() {return map_;}
+    inline void set_inv_map(std::vector<tf2::Transform>& list) {inv_map_ = list;}
+    inline void set_map(std::vector<tf2::Transform>& list) {map_ = list;}
+    inline void set_task_grasps(std::vector<std::vector<tf2::Transform>>& lists_in_list) {task_grasps_ = lists_in_list;}
+    inline std::vector<std::vector<tf2::Transform>>& task_grasps() {return task_grasps_;}
+    inline std::vector<std::vector<std::vector<tf2::Quaternion>>>& target_rot() {return target_rot_;}
+    inline void set_target_rot (std::vector<std::vector<std::vector<tf2::Quaternion>>>& rots) {target_rot_ = rots;}
+    inline void set_target_cloud(std::vector<std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>>& cloud) {target_cloud_ = cloud;}
+    inline std::vector<std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>>& target_cloud () { return target_cloud_;}
+    static std::vector<pcl::PointXYZ> create_pcl_box();
+
+    virtual void write_task(Abstract_robot* robot)=0;
+    virtual std::vector<std::vector<pcl::PointXYZ>> base_calculation()=0;
+};
+#endif

include/impl/abstract_mediator.h

+#ifndef ABSTRACT_MEDIATOR_
+#define ABSTRACT_MEDIATOR_
+
+#include <ros/ros.h>
+#include "impl/abstract_robot.h"
+#include "impl/abstract_robot_element.h"
+#include "impl/robot.h"
+#include "reader/wing_reader.h"
+
+
+#include <ros/package.h>
+#include <yaml-cpp/yaml.h>
+#include <fstream>
+
+#include <octomap/octomap.h>
+#include <pcl/point_cloud.h>
+#include <pcl/octree/octree.h>
+
+
+
+#define box_size tf2::Vector3(0.0318f, 0.0636f, 0.091f)
+#define R_POS tf2::Vector3(0,0.6525f,0.4425f -0.005f)
+#define L_POS tf2::Vector3(0,-0.6525f,0.4425f -0.005f)
+#define M_POS tf2::Vector3(0.6525f,0,0.4425f-0.005f)
+
+struct wing_BP {
+    std::string name_;
+    tf2::Transform pos_;
+    tf2::Vector3 size_;
+};
+
+
+class Abstract_mediator {
+    protected:
+    std::vector<Abstract_robot*> robots_;
+    std::vector<std::vector<tf2::Transform>> objects_;
+    std::vector<std::vector<std::vector<tf2::Transform>>> relative_bounds_;
+    ros::Publisher* pub_;
+    std::vector<std::vector<pcl::PointXYZ>> result_vector_;
+    std::vector<std::vector<Abstract_robot_element*>> wings_;
+    std::string dirname_;
+
+
+
+
+
+    public:
+    Abstract_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : objects_(objects), pub_(pub){
+    }
+
+    inline void set_result_vector(std::vector<std::vector<pcl::PointXYZ>>& res) {result_vector_ = res;}
+    inline std::vector<std::vector<Abstract_robot_element*>> wings() {return wings_;}
+    inline std::vector<std::vector<pcl::PointXYZ>>& result_vector() {return result_vector_;}
+    inline std::vector<Abstract_robot*> robots(){return robots_;}
+
+
+    inline void set_dirname(std::string& dirn) {dirname_ = dirn;}
+    inline std::string& dirname() {return dirname_;}
+
+
+
+    pcl::PointCloud< pcl::PointXYZ >::Ptr vector_to_cloud(std::vector<pcl::PointXYZ>& vector);
+    std::vector<pcl::PointXYZ> generate_Ground(const tf2::Vector3 origin, const float diameter, float resolution);
+    
+    virtual void connect_robots(Abstract_robot* robot)=0; 
+    virtual void set_wings(std::vector<std::pair<std::vector<object_data>, int>>& wbp)=0;
+    virtual bool check_collision( const int& robot)=0;
+    virtual void mediate()=0;
+    virtual void build_wings(std::bitset<3>& wing, int& robot)=0;
+
+
+};
+
+
+#endif
\ No newline at end of file

include/impl/abstract_robot.h

+#ifndef ABSTRACT_ROBOT_
+#define ABSTRACT_ROBOT_
+
+#include "ros/ros.h"
+#include <tf2/LinearMath/Transform.h>
+
+#include "impl/abstract_strategy.h"
+#include "impl/abstract_robot_element.h"
+
+#define right_size tf2::Vector3(0.7f, 0.5f, 0.01f)
+#define left_size tf2::Vector3(0.7f, 0.5f, 0.01f)
+#define mid_size tf2::Vector3(0.5f, 0.7f, 0.01f)
+
+enum wing_config{
+    RML = 7,
+    RM_ = 6,
+    R_L = 5,
+    R__ = 4,
+    _ML = 3,
+    _M_ = 2,
+    __L = 1,
+    ___ = 0
+};
+
+class Abstract_robot {
+    protected:
+    std::string name_;
+    tf2::Vector3 size_;
+    tf2::Transform tf_;
+    tf2::Transform root_tf_;
+    std::vector<tf2::Transform> bounds_;
+    std::vector<tf2::Transform> robot_root_bounds_;
+    std::bitset<3> observer_mask_;
+
+
+
+    public:
+    Abstract_robot(std::string& name, tf2::Transform tf, tf2::Vector3 size) : name_(name), tf_(tf), size_(size){
+        root_tf_= tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.22f, 0, tf.getOrigin().getZ()));
+
+        bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3( 0.4f, 0.4f, tf.getOrigin().getZ()))); // ++
+        bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.4f, 0.4f, tf.getOrigin().getZ()))); // +-
+        bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.4f, -0.4f, tf.getOrigin().getZ()))); //-+
+        bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3( 0.4f, -0.4f, tf.getOrigin().getZ()))); // --
+
+        robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.113f, 0.095f, 0))); // ++
+        robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.113f, 0.095f, 0))); // +-
+        robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.113f, -0.095f, 0)));
+        robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.113f, -0.095f, 0)));
+
+        observer_mask_ = std::bitset<3>(0);
+
+    };
+
+
+    
+    inline std::string& name() { return name_;}
+    inline tf2::Transform& tf() { return tf_;}
+    inline tf2::Vector3& size() { return size_;}
+    inline tf2::Transform& root_tf() { return root_tf_;}
+    inline std::vector<tf2::Transform>& bounds() {return bounds_;}
+    inline void size(tf2::Vector3& s) { size_ = s;}
+    inline void set_tf(tf2::Transform& t) { tf_ = t;}
+    inline std::vector<tf2::Transform>& robot_root_bounds() { return robot_root_bounds_;}
+    inline void rotate(float deg) {tf2::Quaternion rot; rot.setRPY(0,0,deg); tf2::Transform t(rot, tf2::Vector3(0,0,0)); tf_= tf_* t;}
+    inline void translate(tf2::Vector3 t) {tf2::Transform tf(tf2::Quaternion(0,0,0,1), t); tf_*=tf;}
+    inline std::bitset<3> observer_mask() {return observer_mask_;}
+    inline void set_observer_mask(int i) {observer_mask_ = std::bitset<3>(i);}
+
+
+
+    virtual void notify()= 0;
+    virtual bool check_single_object_collision(tf2::Transform& obj, std::string& b)= 0;
+    virtual void workload_checker(std::vector<int>& count_vector, tf2::Transform& obj) =0;
+
+};
+
+
+#endif
\ No newline at end of file

include/impl/abstract_robot_element.h

+#ifndef ABSTRACT_ROBOT_ELEMENT_
+#define ABSTRACT_ROBOT_ELEMENT_
+
+#include "ros/ros.h"
+#include <tf2/LinearMath/Transform.h>
+#include "visualization_msgs/MarkerArray.h"
+
+
+class Abstract_robot_element {
+    protected:
+    tf2::Transform relative_tf_;
+    tf2::Transform world_tf_;
+
+    public:
+    Abstract_robot_element() = default;
+    
+    inline void set_relative_tf(tf2::Transform tf) { relative_tf_= tf;}
+    inline tf2::Transform& relative_tf(){ return relative_tf_;}
+    inline void calc_world_tf(tf2::Transform& tf) {world_tf_= tf * relative_tf_;}
+    inline void set_world_tf(tf2::Transform& tf) { world_tf_ = tf;}
+    inline tf2::Transform& world_tf() { return world_tf_;}
+    
+    virtual void update(tf2::Transform& tf)= 0;
+};
+
+
+#endif
\ No newline at end of file

include/impl/abstract_robot_element_decorator.h

+#ifndef ABSTRACT_ROBOT_ELEMENT_DECORATOR_
+#define ABSTRACT_ROBOT_ELEMENT_DECORATOR_
+
+#include "ros/ros.h"
+#include "impl/abstract_robot_element.h"
+
+class Abstract_robot_element_decorator  : public Abstract_robot_element{
+    protected:
+    Abstract_robot_element* next_;
+
+
+    public:
+    Abstract_robot_element_decorator(Abstract_robot_element* next) : next_(next){};
+    
+    inline Abstract_robot_element* wing() { return next_;}
+
+    void update(tf2::Transform& tf) override {next_->update(tf);}
+    virtual void input_filter(tf2::Transform& tf)=0;
+    virtual void output_filter()=0;
+};
+
+
+#endif
\ No newline at end of file

include/impl/abstract_strategy.h

+#ifndef ABSTRACT_STRATEGY_
+#define ABSTRACT_STRATEGY_
+
+#include <ros/ros.h>
+#include "impl/abstract_map_loader.h"
+class Abstract_map_loader;
+
+class Abstract_strategy {
+    public:
+    Abstract_strategy() = default;
+    ~Abstract_strategy() = default;
+    virtual void inv_map_creation(Abstract_map_loader* var)=0;
+    virtual void cloud_calculation(Abstract_map_loader* var)=0;
+
+};
+#endif
\ No newline at end of file

include/impl/base_by_rotation.h

+#ifndef BASE_BY_ROTATION_
+#define BASE_BY_ROTATION_
+
+#include <ros/ros.h>
+#include "impl/abstract_map_loader.h"
+#include "impl/abstract_strategy.h"
+
+
+class Base_by_rotation : public Abstract_strategy {
+    public:
+    Base_by_rotation()= default;
+    ~Base_by_rotation()= default;
+
+    void inv_map_creation(Abstract_map_loader* var) override;
+    void cloud_calculation(Abstract_map_loader* var) override;
+
+    
+};
+
+#endif
\ No newline at end of file

include/impl/collision_helper.h

+#ifndef COLLISION_HELPER_
+#define COLLISION_HELPER_
+
+#include <ros/ros.h>
+#include <moveit/collision_detection/collision_env.h>
+#include <moveit/collision_detection_fcl/collision_env_fcl.h>
+
+
+
+class Collision_helper : public collision_detection::CollisionEnvFCL{
+    public:
+    Collision_helper()= default;
+    ~Collision_helper()= default;
+
+    bool collision_detector(const std::string& object1_name, const std::string& object2_name);
+};
+
+#endif
\ No newline at end of file

include/impl/field.h

+#ifndef Field_
+#define Field_
+
+#include "ros/ros.h"
+#include "impl/abstract_robot_element.h"
+
+class Field  : public Abstract_robot_element{
+    public:
+    Field(tf2::Transform tf){ relative_tf_ = tf;}
+    
+    void update(tf2::Transform& tf) override {this->calc_world_tf(tf);}
+};
+
+#endif
\ No newline at end of file

include/impl/field_rviz_decorator.h

+#ifndef FIELD_RVIZ_DECORATOR_
+#define FIELD_RVIZ_DECORATOR_
+
+#include "ros/ros.h"
+#include "visualization_msgs/Marker.h"
+#include "impl/field.h"
+#include "impl/abstract_robot_element_decorator.h"
+
+class Field_rviz_decorator  : public Abstract_robot_element_decorator{
+    private:
+    visualization_msgs::MarkerArray* markers_;
+
+
+    public:
+    Field_rviz_decorator(Abstract_robot_element* next, visualization_msgs::MarkerArray* markers) : Abstract_robot_element_decorator(next), markers_(markers){};
+    
+    inline void set_markers(visualization_msgs::MarkerArray* markers) { markers_= markers;}
+    inline visualization_msgs::MarkerArray* markers() { return markers_;}
+
+    void update(tf2::Transform& tf) override;
+    void input_filter(tf2::Transform& tf) override;
+    void output_filter() override;
+};
+
+
+#endif
\ No newline at end of file

include/impl/map_loader.h

+#ifndef MAP_LOADER_
+#define MAP_LOADER_
+
+#include "ros/ros.h"
+#include "impl/abstract_map_loader.h"
+#include <regex>
+
+
+
+class Map_loader : public Abstract_map_loader {
+    public:
+    Map_loader(XmlRpc::XmlRpcValue& map_data, XmlRpc::XmlRpcValue& target_data);
+    std::vector<std::vector<pcl::PointXYZ>> base_calculation() override;
+    void write_task(Abstract_robot* robot) override;
+
+};
+#endif
\ No newline at end of file

include/impl/mediator.h

+#ifndef MEDIATOR_
+#define MEDIATOR_
+
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <yaml-cpp/yaml.h>
+#include <fstream>
+#include "impl/abstract_mediator.h"
+#include "impl/abstract_robot.h"
+#include "impl/robot.h"
+
+class Mediator : public Abstract_mediator{
+    public:
+    Mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : Abstract_mediator(objects, pub){};
+
+    pcl::PointCloud< pcl::PointXYZ >::Ptr vector_to_cloud(std::vector<pcl::PointXYZ>& vector);
+    std::vector<pcl::PointXYZ> generate_Ground(const tf2::Vector3 origin, const float diameter, float resolution);
+    void approximation(Robot* robot);
+    void write_file(Robot* A, Robot* B);
+    void publish(Robot* r);
+
+    void setup_rviz();
+    void calculate(std::vector<tf2::Transform>& ground_per_robot);
+    
+    void set_wings(std::vector<std::pair<std::vector<object_data>, int>>& wbp) override;
+    bool check_collision(const int& robot) override;
+    void mediate() override;
+    void build_wings(std::bitset<3>& wings, int& robot) override;
+    void connect_robots(Abstract_robot* robot) override;
+
+};
+
+#endif
\ No newline at end of file

include/impl/moveit_grasp_mediator.h

+#ifndef MOVEIT_GRASP_MEDIATOR_
+#define MOVEIT_GRASP_MEDIATOR_
+
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <yaml-cpp/yaml.h>
+#include <fstream>
+#include "impl/abstract_mediator.h"
+#include "impl/moveit_mediator.h"
+
+#include "impl/abstract_robot.h"
+#include "impl/robot.h"
+#include <gb_grasp/VoxelManager.h>
+#include "reader/abstract_param_reader.h"
+#include "reader/robot_reader.h"
+#include "reader/cuboid_reader.h"
+
+
+
+class Moveit_grasp_mediator : public Moveit_mediator{
+    protected:
+    std::shared_ptr<planning_pipeline::PlanningPipeline> planning_pipeline_;
+    std::shared_ptr<VoxelManager> voxel_manager_;
+    std::string grasp_ns_;
+
+    // Readers 
+    std::unique_ptr<Abstract_param_reader> robot_reader_;
+    std::unique_ptr<Abstract_param_reader> wing_reader_;
+    std::unique_ptr<Abstract_param_reader> cuboid_reader_;
+
+    public:
+    Moveit_grasp_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh) 
+    : Moveit_mediator(objects, pub, nh)
+    , robot_reader_(std::make_unique<Robot_reader>(nh))
+    , wing_reader_(std::make_unique<Wing_reader>(nh))
+    , cuboid_reader_(std::make_unique<Cuboid_reader>(nh)) {
+
+        visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(
+        robot_model_->getModelFrame(), "/rviz_visual_tools", planning_scene_monitor_);
+        visual_tools_->loadMarkerPub();
+        visual_tools_->loadRemoteControl();
+        visual_tools_->setPlanningSceneMonitor(planning_scene_monitor_);
+
+        visual_tools_->loadRobotStatePub("/display_robot_state");
+        visual_tools_->setRobotStateTopic("/robot_state_publisher");
+        visual_tools_->loadTrajectoryPub("/display_planned_path");
+
+        visual_tools_->loadSharedRobotState();
+        visual_tools_->enableBatchPublishing();
+        visual_tools_->deleteAllMarkers();
+        visual_tools_->removeAllCollisionObjects();
+        visual_tools_->trigger();
+
+        nh_->getParam("/grasps_ns", grasp_ns_);
+
+        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();
+        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();
+        for (int i = 0; i < robots_.size(); i++){
+            for(object_data& w : wd[i].first){
+                w.pose_ = robots_[i]->tf().inverse() * w.pose_;
+            }
+            robots_[i]->set_observer_mask(static_cast<wing_config>(wd[i].second));
+        }
+
+        set_wings(wd);    
+
+        for (int i = 0; i < robots_.size(); i++){
+            std::bitset<3> bs = wd[i].second;
+            build_wings(bs, i);
+        }
+
+        scene_setup();
+    };
+
+    
+    void mediate() override;
+    void connect_robots(Abstract_robot* robot) override;
+
+    void manipulate_grasp_data(Moveit_robot* robot);
+    void scene_setup();
+
+
+};
+
+#endif
\ No newline at end of file

include/impl/moveit_mediator.h

+#ifndef MOVEIT_MEDIATOR_
+#define MOVEIT_MEDIATOR_
+
+#include <ros/ros.h>
+
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit_msgs/ApplyPlanningScene.h>
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/kinematic_constraints/utils.h>
+#include <moveit_visual_tools/moveit_visual_tools.h>
+
+
+#include "impl/abstract_mediator.h"
+#include "impl/abstract_robot.h"
+#include "impl/robot.h"
+#include "impl/moveit_robot.h"
+
+#include "impl/wing_moveit_decorator.h"
+#include "impl/collision_helper.h"
+
+#include <moveit/task_constructor/task.h>
+#include <moveit/task_constructor/stages/compute_ik.h>
+#include <moveit/task_constructor/stages/connect.h>
+#include <moveit/task_constructor/stages/current_state.h>
+#include <moveit/task_constructor/stages/generate_grasp_pose.h>
+#include <moveit/task_constructor/stages/generate_place_pose.h>
+#include <moveit/task_constructor/stages/generate_pose.h>
+#include <moveit/task_constructor/stages/modify_planning_scene.h>
+#include <moveit/task_constructor/stages/move_relative.h>
+#include <moveit/task_constructor/stages/move_to.h>
+#include <moveit/task_constructor/stages/predicate_filter.h>
+#include <moveit/task_constructor/solvers/cartesian_path.h>
+#include <moveit/task_constructor/solvers/pipeline_planner.h>
+#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
+#include <moveit/task_constructor/properties.h>
+#include <moveit/task_constructor/solvers/joint_interpolation.h>
+#include <moveit/task_constructor/solvers/planner_interface.h>
+#include <moveit/task_constructor/stage.h>
+#include <moveit/task_constructor/stages/fixed_state.h>
+
+#include <eigen_conversions/eigen_msg.h>
+#include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
+#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>
+
+
+class Moveit_mediator : public Abstract_mediator{
+    protected:
+    std::shared_ptr<ros::NodeHandle> nh_;
+    std::shared_ptr<moveit::core::RobotModel> robot_model_;
+    std::shared_ptr<moveit_visual_tools::MoveItVisualTools> visual_tools_;
+
+    // planning Interfaces and planing scene 
+    std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi_;
+    std::unique_ptr<moveit::planning_interface::PlanningSceneInterface> psi_;
+    std::shared_ptr<planning_scene::PlanningScene> ps_;
+    std::shared_ptr<ros::Publisher> planning_scene_diff_publisher_;
+    std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_;
+
+
+    // task constructor
+    std::shared_ptr<moveit::task_constructor::solvers::PipelinePlanner> sampling_planner_;
+    std::shared_ptr<moveit::task_constructor::solvers::CartesianPath> cartesian_planner_;
+    std::map<std::string, std::vector<moveit::task_constructor::Task>> task_map_;
+
+
+    public:
+    Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh) 
+    : Abstract_mediator(objects, pub), nh_(nh)
+    , sampling_planner_(std::make_unique<moveit::task_constructor::solvers::PipelinePlanner>())
+    , cartesian_planner_(std::make_unique<moveit::task_constructor::solvers::CartesianPath>())
+    , psi_(std::make_unique<moveit::planning_interface::PlanningSceneInterface>())
+    , mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>("panda_arms"))
+    , planning_scene_diff_publisher_(std::make_shared<ros::Publisher>(nh_->advertise<moveit_msgs::PlanningScene>("planning_scene", 1)))
+    , planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description")){
+
+        robot_model_loader::RobotModelLoaderPtr robot_model_loader;
+        robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>("robot_description");
+        robot_model_ = robot_model_loader->getModel();
+        ps_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
+
+        // planner 
+        sampling_planner_->setProperty("goal_joint_tolerance", 1e-5);
+
+        // cartesian
+	    cartesian_planner_->setMaxVelocityScaling(1.0);
+	    cartesian_planner_->setMaxAccelerationScaling(1.0);
+	    cartesian_planner_->setStepSize(.01);
+
+    };
+
+    void setup_task();
+    inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi() {return mgi_;};
+
+
+    bool check_collision(const int& robot) override;
+    void mediate() override;
+    void build_wings(std::bitset<3>& wing,int& robot) override;
+    void set_wings(std::vector<std::pair<std::vector<object_data>, int>>& wbp) override;
+    void connect_robots(Abstract_robot* robot) override;
+
+
+    void merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr);
+    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);
+    moveit::task_constructor::Task create_Task(Moveit_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_;};
+
+
+};
+
+#endif
\ No newline at end of file