added moveit support

.catkin_tools/profiles/default/build.yaml

@@ -10,7 +10,8 @@ extends: null
 install: false
 install_space: install
 isolate_install: false
-jobs_args: []
+jobs_args:
+- -j2
 licenses:
 - TODO
 log_space: logs

.catkin_tools/profiles/default/devel_collisions.txt

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

.catkin_tools/profiles/default/packages/mtc/devel_manifest.txt

@@ -3,8 +3,10 @@ mtc
 /home/matteo/reachability/devel/.private/mtc/share/mtc/cmake/mtcConfig-version.cmake /home/matteo/reachability/devel/share/mtc/cmake/mtcConfig-version.cmake
 /home/matteo/reachability/devel/.private/mtc/share/mtc/cmake/mtcConfig.cmake /home/matteo/reachability/devel/share/mtc/cmake/mtcConfig.cmake
 /home/matteo/reachability/devel/.private/mtc/lib/libmoveit_grasps_filter.so /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so
+/home/matteo/reachability/devel/.private/mtc/lib/liblib.so /home/matteo/reachability/devel/lib/liblib.so
 /home/matteo/reachability/devel/.private/mtc/lib/libmoveit_grasps.so /home/matteo/reachability/devel/lib/libmoveit_grasps.so
 /home/matteo/reachability/devel/.private/mtc/lib/pkgconfig/mtc.pc /home/matteo/reachability/devel/lib/pkgconfig/mtc.pc
 /home/matteo/reachability/devel/.private/mtc/lib/mtc/mtc2taskspace /home/matteo/reachability/devel/lib/mtc/mtc2taskspace
 /home/matteo/reachability/devel/.private/mtc/lib/mtc/base_routine /home/matteo/reachability/devel/lib/mtc/base_routine
 /home/matteo/reachability/devel/.private/mtc/lib/mtc/moveit_grasps_grasp_pipeline /home/matteo/reachability/devel/lib/mtc/moveit_grasps_grasp_pipeline
+/home/matteo/reachability/devel/.private/mtc/lib/mtc/cell_routine /home/matteo/reachability/devel/lib/mtc/cell_routine

src/.vscode/c_cpp_properties.json

@@ -33,7 +33,9 @@
                 "/home/matteo/ws_moveit/src/srdfdom/include/**",
                 "/home/matteo/ws_moveit/src/warehouse_ros/include/**",
                 "/usr/include/**",
-                "${workspaceFolder}/mtc/include"
+                "${workspaceFolder}/mtc/include",
+                "${workspaceFolder}/mtc/include/impl",
+                "${workspaceFolder}/mtc/include/impl/ho"
             ],
             "name": "ROS"
         }

src/ceti_double/config/kinematics.yaml

-{}
\ No newline at end of file
+panda_arm1:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.05
+panda_arm2:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.05

src/ceti_double/config/panda.srdf

@@ -48,7 +48,8 @@
     <end_effector name="hand1" parent_link="panda_1_link8" group="hand_1"/>
     <end_effector name="hand2" parent_link="panda_2_link8" group="hand_2"/>
     <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
-    <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="world"/>
+    <virtual_joint name="virtual_joint1" type="fixed" parent_frame="world" child_link="base_1"/>
+    <virtual_joint name="virtual_joint2" type="fixed" parent_frame="world" child_link="base_2"/>
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
     <disable_collisions link1="base_1" link2="base_2" reason="Adjacent"/>
     <disable_collisions link1="base_1" link2="panda_1_link0" reason="Adjacent"/>

src/ceti_double/launch/demo.launch

@@ -24,7 +24,8 @@
   <arg name="use_rviz" default="true" />
 
   <!-- If needed, broadcast static tf for robot root -->
-  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world world" />
+  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world base1" />
+  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_1" args="0 0 0 0 0 0 world base2" />
 
 
 

src/mtc/CMakeLists.txt

@@ -51,6 +51,7 @@ catkin_package(
     trajectory_msgs
   INCLUDE_DIRS
     include
+    include/impl
   DEPENDS
     EIGEN3
 )
@@ -61,11 +62,38 @@ catkin_package(
 
 include_directories(
   include
+  include/impl
   ${catkin_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIR}
   ${PCL_INCLUDE_DIR}
 )
 
+# Demo grasp pipeline
+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
+)
+
+add_executable(cell_routine src/cell_routine.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
+)
+
+
+
 # Grasp Library
 add_library(moveit_grasps
   src/grasp_candidate.cpp
@@ -98,13 +126,14 @@ set_target_properties(moveit_grasps_filter PROPERTIES COMPILE_FLAGS "${CMAKE_CXX
 set_target_properties(moveit_grasps_filter PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}")
 
 
-# Demo grasp pipeline
-add_executable(base_routine src/base_routine.cpp)
+
+
 add_executable(mtc2taskspace src/mtc2taskspace.cpp)
 add_executable(moveit_grasps_grasp_pipeline src/grasp_pipeline.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})
 
 
@@ -118,6 +147,12 @@ target_link_libraries(base_routine
   ${PCL_LIBRARY_DIRS}
   yaml-cpp
 )
+target_link_libraries(cell_routine 
+  ${catkin_LIBRARIES}
+  ${OCTOMAP_LIBRARIES}
+  ${PCL_LIBRARY_DIRS}
+  yaml-cpp
+)
 
 target_link_libraries(mtc2taskspace 
   ${catkin_LIBRARIES}

src/mtc/include/impl/abstract_map_loader.h

@@ -11,6 +11,7 @@
 
 #include "impl/abstract_strategy.h"
 
+class Abstract_strategy;
 
 class Abstract_map_loader{
     protected:
@@ -26,7 +27,8 @@ class Abstract_map_loader{
     Abstract_map_loader() = default;
     ~Abstract_map_loader() = default;
 
-    virtual std::vector<std::vector<pcl::PointXYZ>> base_calculation()=0;
+    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_;}
@@ -39,12 +41,9 @@ class Abstract_map_loader{
     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_;}
 
-    inline void set_strategy(Abstract_strategy* some_stratergy) {strategy_ = some_stratergy;}
-    inline Abstract_strategy* strategy() {return strategy_;}
 
     static std::vector<pcl::PointXYZ> create_pcl_box();
 
+    virtual std::vector<std::vector<pcl::PointXYZ>> base_calculation()=0;
 };
-
-
 #endif

src/mtc/include/impl/abstract_mediator.h

@@ -11,6 +11,16 @@
 #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 {
+    tf2::Transform pos_;
+    tf2::Vector3 size_;
+    wing_BP(tf2::Transform pos, tf2::Vector3 size) : pos_(pos), size_(size){};
+};
+
 
 class Abstract_mediator {
     protected:
@@ -19,38 +29,31 @@ class Abstract_mediator{
     std::vector<std::vector<std::vector<tf2::Transform>>> relative_bounds_;
     ros::Publisher* pub_;
     std::vector<std::vector<pcl::PointXYZ>> result_vector_;
-    std::vector<Abstract_robot_element*> wings_;
+    std::vector<wing_BP> wings_;
+
 
 
 
 
     public:
     Abstract_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : objects_(objects), pub_(pub){
-        wings_.push_back(new Wing(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.6525f,0.4425f -0.005f)), right));
-        wings_.push_back(new Wing(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-0.6525f,0.4425f -0.005f)), left));
-        wings_.push_back(new Wing(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.6525f,0,0.4425f-0.005f)), mid));
+        wings_.push_back(wing_BP(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.6525f,0.4425f -0.005f)), right));
+        wings_.push_back(wing_BP(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-0.6525f,0.4425f -0.005f)), left));
+        wings_.push_back(wing_BP(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.6525f,0,0.4425f-0.005f)), mid));
     }
+
     inline void connect_robots(Abstract_robot* robot) {robots_.push_back(robot); ROS_INFO("%s connected...", robot->name().c_str());}
     inline void set_result_vector(std::vector<std::vector<pcl::PointXYZ>>& res) {result_vector_ = res;}
+    inline std::vector<wing_BP> wings() {return wings_;}
     inline std::vector<std::vector<pcl::PointXYZ>>& result_vector() {return result_vector_;}
+
     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 add_wing(Robot* robot) {
-        if (robot->observers().size() == 0){
-            Abstract_robot_element* rviz_right = new Wing_rviz_decorator(wings_[0], robot->rviz_markers()[0]);
-            robot->register_observers(rviz_right);
-        } else if (robot->observers().size() == 1){
-            Abstract_robot_element* rviz_left = new Wing_rviz_decorator(wings_[1], robot->rviz_markers()[0]);
-            robot->register_observers(rviz_left);
-        } else if (robot->observers().size() == 2){
-            Abstract_robot_element* rviz_mid = new Wing_rviz_decorator(wings_[2], robot->rviz_markers()[0]);
-            robot->register_observers(rviz_mid);
-        } else {
-            ROS_INFO("[Warning] %s got to many wings", robot->name().c_str());
-        }
-    }
+    
     virtual bool check_collision( const int& robot)=0;
     virtual void mediate()=0;
+    virtual void build_wings(std::bitset<3>& wing,Robot* robot)=0;
+
 
 };
 

src/mtc/include/impl/abstract_robot.h

@@ -11,6 +11,17 @@
 #define left tf2::Vector3(0.7f, 0.5f, 0.01f)
 #define mid 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_;
@@ -18,6 +29,7 @@ class Abstract_robot {
     tf2::Transform root_tf_;
     std::vector<tf2::Transform> bounds_;
     std::vector<tf2::Transform> robot_root_bounds_;
+    std::bitset<3> observer_mask_;
 
 
 
@@ -35,7 +47,10 @@ class Abstract_robot {
         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::Transform& root_tf() { return root_tf_;}
@@ -43,6 +58,9 @@ class Abstract_robot {
     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)= 0;

src/mtc/include/impl/abstract_robot_element.h

@@ -13,6 +13,7 @@ class Abstract_robot_element {
 
     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_;}

src/mtc/include/impl/abstract_robot_element_decorator.h

@@ -11,9 +11,10 @@ class Abstract_robot_element_decorator  : public Abstract_robot_element{
 
     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);}
 
+    void update(tf2::Transform& tf) override {next_->update(tf);}
     virtual void input_filter(tf2::Transform& tf)=0;
     virtual void output_filter()=0;
 };

src/mtc/include/impl/field.h

@@ -7,6 +7,7 @@
 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);}
 };
 

src/mtc/include/impl/field_rviz_decorator.h

@@ -13,8 +13,10 @@ class Field_rviz_decorator  : public Abstract_robot_element_decorator{
 
     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;

src/mtc/include/impl/map_loader.h

@@ -2,7 +2,7 @@
 #define MAP_LOADER_
 
 #include "ros/ros.h"
-#include "abstract_map_loader.h"
+#include "impl/abstract_map_loader.h"
 #include <regex>
 
 

src/mtc/include/impl/mediator.h

 #ifndef MEDIATOR_
 #define MEDIATOR_
 
-#include "ros/ros.h"
+#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"
@@ -12,12 +15,15 @@ class Mediator : public Abstract_mediator{
 
     public:
     Mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : Abstract_mediator(objects, pub){};
-    bool check_collision(const int& robot) override;
-    void mediate() override;
 
     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* ceti1);
+    void approximation(Robot* robot, int& wings, std::vector<std::pair<std::pair<tf2::Transform, int>, std::vector<std::pair<tf2::Transform, int>>>>& dict);
+    void write_file(std::vector<std::pair<std::pair<tf2::Transform, int>, std::vector<std::pair<tf2::Transform, int>>>>& dict);
+
+    bool check_collision(const int& robot) override;
+    void mediate() override;
+    void build_wings(std::bitset<3>& wings, Robot* robot) override;
 
 };
 

src/mtc/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 "impl/abstract_mediator.h"
+#include "impl/abstract_robot.h"
+#include "impl/robot.h"
+#include "impl/moveit_robot.h"
+
+class Moveit_mediator : public Abstract_mediator{
+    protected:
+    ros::NodeHandle nh_;
+    ros::Publisher collision_object_publisher_;
+    ros::Publisher planning_scene_diff_publisher_;
+    robot_model_loader::RobotModelLoader* robot_model_loader_;
+    robot_model::RobotModelPtr kinematic_model_;
+    planning_scene::PlanningScene* ps_;
+
+    public:
+    Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, ros::NodeHandle nh) : Abstract_mediator(objects, pub), nh_(nh){
+        collision_object_publisher_ = nh.advertise<moveit_msgs::CollisionObject>("collision_object", 1);
+        planning_scene_diff_publisher_ = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
+    };
+
+    bool check_collision(const int& robot) override;
+    void mediate() override;
+    void build_wings(std::bitset<3>& wing,Robot* robot) override;
+
+    void publish_tables();
+    void load_robot_description();
+
+
+};
+
+#endif
\ No newline at end of file

src/mtc/include/impl/moveit_robot.h

+#ifndef MOVEIT_ROBOT_
+#define MOVEIT_ROBOT_
+
+#include <ros/ros.h>
+#include "impl/robot.h"
+
+
+class Moveit_robot : public Robot{
+    private:
+    moveit::planning_interface::MoveGroupInterface* mgi_;
+
+    public:
+    Moveit_robot(std::string name, tf2::Transform tf) : Robot(name, tf){
+        mgi_ = new moveit::planning_interface::MoveGroupInterface(name);
+    }
+
+
+};
+
+#endif
\ No newline at end of file

src/mtc/include/impl/robot.h

@@ -9,13 +9,19 @@
 #include "impl/wing_rviz_decorator.h"
 #include "impl/field.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/robot_model_loader/robot_model_loader.h>
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/kinematic_constraints/utils.h>
 
 class Robot : public Abstract_robot{
     protected:
     std::vector<Abstract_robot_element*> observers_;
     std::vector<Abstract_robot_element*> access_fields_;
     std::vector<visualization_msgs::MarkerArray*> rviz_markers_;
-
+    std::vector<moveit_msgs::CollisionObject*> coll_markers_;
 
 
     public:
@@ -23,133 +29,23 @@ class Robot : public Abstract_robot{
         generate_access_fields();
     }
     
-    void generate_access_fields(){
-        for (int i = 0; i <= 2; i++){
-            for (int j = 0; j <= 2; j++){
-                if(i == 0 && j == 0) {continue;}
-                if(i == 2 && j == 2) {continue;}
-                if(i == 0) {
-
-                    access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.8f*j,0))));
-                    access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-0.8f*j,0))));
-                } else if (j == 0){
-                    access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.8f*i,0,0))));
-                    access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.8f*i,0,0))));
-                } else {
-                access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.8f*i,0.8f*j,0))));
-                access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.8f*i,0.8f*j,0))));
-                access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.8f*i,-0.8f*j,0))));
-                access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.8f*i,-0.8f*j,0))));
-                }
-
-            };
-        };
-    }
-
     inline void add_rviz_markers(visualization_msgs::MarkerArray* marker) {rviz_markers_.push_back(marker);}
     inline std::vector<visualization_msgs::MarkerArray*> rviz_markers(){return rviz_markers_;}
-    inline float area_calculation(tf2::Transform& A, tf2::Transform& B, tf2::Transform& C){
-        return std::abs(
-                        (B.getOrigin().getX() * A.getOrigin().getY()) - (A.getOrigin().getX() * B.getOrigin().getY()) +
-                        (C.getOrigin().getX() * B.getOrigin().getY()) - (B.getOrigin().getX() * C.getOrigin().getY()) +
-                        (A.getOrigin().getX() * C.getOrigin().getY()) - (C.getOrigin().getX() * A.getOrigin().getY()))*0.5f;
-    }
-
-
-    void notify() override {for(Abstract_robot_element* wing : observers_) wing->update(tf_); for(Abstract_robot_element* field : access_fields_) field->update(tf_);}
-    bool check_single_object_collision(tf2::Transform& obj) override {
-
-        tf2::Transform A = tf_ * root_tf_ * robot_root_bounds_[0];
-        tf2::Transform B = tf_ * root_tf_ * robot_root_bounds_[1];
-        tf2::Transform C = tf_ * root_tf_ * robot_root_bounds_[2];
-        tf2::Transform D = tf_ * root_tf_ * robot_root_bounds_[3];
-
-        float full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
-        float sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
-        if ((std::floor(sum*100)/100.f) <= full_area) {return false; } 
-        
-
-        for (int i = 0; i < bounds_.size(); i++){
-            tf2::Transform A = tf_ * bounds_[0];
-            tf2::Transform B = tf_ * bounds_[1];
-            tf2::Transform C = tf_ * bounds_[2];
-            tf2::Transform D = tf_ * bounds_[3];
-
-            full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
-            sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
-
-
-            if ((std::floor(sum*100)/100.f) <= full_area) return true;
-        }
+    inline std::vector<Abstract_robot_element*>& observers() { return observers_;}
+    inline std::vector<Abstract_robot_element*>& access_fields() { return access_fields_;}
 
-        if (!observers_.empty()){
-            std::vector<Abstract_robot_element*>::const_iterator it = observers_.begin();            
-            while (it != observers_.end()) {
-                Abstract_robot_element_decorator *deco = dynamic_cast<Abstract_robot_element_decorator*>(*it);
-                Wing* ceti = dynamic_cast<Wing*>(deco->wing());
-                tf2::Transform A = ceti->world_tf() * ceti->bounds()[0];
-                tf2::Transform B = ceti->world_tf() * ceti->bounds()[1];
-                tf2::Transform C = ceti->world_tf() * ceti->bounds()[2];
-                tf2::Transform D = ceti->world_tf() * ceti->bounds()[3];
+    inline void add_coll_markers(moveit_msgs::CollisionObject* marker) {coll_markers_.push_back(marker);}
+    inline std::vector<moveit_msgs::CollisionObject*> coll_markers(){ return coll_markers_;}
 
-                full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
-                sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
+    void register_observers(Abstract_robot_element* wd);
+    void reset();
+    void generate_access_fields();
+    float area_calculation(tf2::Transform& A, tf2::Transform& B, tf2::Transform& C);
 
-                if ((std::floor(sum*100)/100.f) <= full_area) {
-                    return true;
-                } else {
-                    ++it;
-                }
-            }
-        }
-        return false;
-    }
-    void reset(){
-        observers_.clear();
-        access_fields_.clear();
-        generate_access_fields();
-        tf_ = tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f));
-    }
+    bool check_single_object_collision(tf2::Transform& obj) override;
+    void notify() override;
 
 
-    inline std::vector<Abstract_robot_element*>& observers() { return observers_;}
-    inline std::vector<Abstract_robot_element*>& access_fields() { return access_fields_;}
-    void register_observers(Abstract_robot_element* wd){
-        Abstract_robot_element_decorator* decorator = dynamic_cast<Abstract_robot_element_decorator*>(wd);
-        observers_.push_back(wd);
-        std::vector<tf2::Transform> plane;
-        bool found = false;
-        int index = 0;
-
-        if (decorator->wing()->relative_tf().getOrigin().getY()>0){
-            for(int i = 0; i < access_fields_.size(); i++){
-                if ((access_fields_[i]->relative_tf().getOrigin().getY() > 0) && (access_fields_[i]->relative_tf().getOrigin().getX() == 0)){
-                    found = true;
-                    index = i;
-                    break;
-                }
-            }
-            if(found) {access_fields_.erase(access_fields_.begin() + index); return;}
-        } else if (decorator->wing()->relative_tf().getOrigin().getY()<0) {
-            for(int i = 0; i < access_fields_.size(); i++){
-                if ((access_fields_[i]->relative_tf().getOrigin().getY() < 0) && (access_fields_[i]->relative_tf().getOrigin().getX() == 0)){
-                    found = true;
-                    index = i;
-                    break;
-                }
-            }
-            if(found) {access_fields_.erase(access_fields_.begin() + index); return;}
-        } else {
-            for(int i = 0; i < access_fields_.size(); i++){
-                if (access_fields_[i]->relative_tf().getOrigin().getX() > 0){
-                    found = true;
-                    index = i;
-                    break;
-                }
-            }
-            if(found) {access_fields_.erase(access_fields_.begin() + index); return;}
-        }
-    };
 };