diff --git a/CMakeLists.txt b/CMakeLists.txt
index 496e8f3b10a382db1a404ec30e99e479a8ae0b3e..bc9d0c9fae0ce72de0f6e8eb5b69bdeb78a7a9b2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -78,6 +78,7 @@ src/bt/parallel_robot.cpp
src/mediator/abstract_mediator.cpp
src/mediator/base_calculation_mediator.cpp
src/mediator/moveit_mediator.cpp
+src/mediator/grasp_mediator.cpp
src/reader/abstract_param_reader.cpp
src/reader/cuboid_reader.cpp
@@ -113,6 +114,44 @@ src/bt/parallel_robot.cpp
src/mediator/abstract_mediator.cpp
src/mediator/base_calculation_mediator.cpp
src/mediator/moveit_mediator.cpp
+src/mediator/grasp_mediator.cpp
+
+
+src/reader/abstract_param_reader.cpp
+src/reader/cuboid_reader.cpp
+src/reader/job_reader.cpp
+src/reader/map_reader.cpp
+src/reader/robot_reader.cpp
+src/reader/task_space_reader.cpp
+src/reader/ts_reader.cpp
+src/reader/wing_reader.cpp
+
+src/robot/decorators/panda_decorator.cpp
+src/robot/decorators/ur5_decorator.cpp
+src/robot/decorators/ur10_decorator.cpp
+src/robot/abstract_robot.cpp
+src/robot/ceti_robot.cpp
+
+src/robot_element/decorators/abstract_robot_element_decorator.cpp
+src/robot_element/decorators/log_decorator.cpp
+src/robot_element/observers/moveit_panel.cpp
+src/robot_element/observers/panel.cpp
+src/robot_element/observers/rviz_panel.cpp
+)
+
+add_executable(config_routine src/config_routine.cpp
+src/base_bridge/simple_base_implementation.cpp
+src/base_bridge/simple_base.cpp
+
+src/bt/execution.cpp
+src/bt/position_condition.cpp
+src/bt/parallel_robot.cpp
+
+src/mediator/abstract_mediator.cpp
+src/mediator/base_calculation_mediator.cpp
+src/mediator/moveit_mediator.cpp
+src/mediator/grasp_mediator.cpp
+
src/reader/abstract_param_reader.cpp
src/reader/cuboid_reader.cpp
@@ -150,6 +189,7 @@ src/bt/parallel_robot.cpp
)
+add_dependencies(config_routine ${config_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(robot_base_calculation_approach ${robot_base_calculation_approach_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
@@ -163,6 +203,14 @@ target_link_libraries(robot_base_calculation_approach
yaml-cpp
)
+target_link_libraries(config_routine
+ ${catkin_LIBRARIES}
+ ${OCTOMAP_LIBRARIES}
+ ${PCL_LIBRARY_DIRS}
+ ${BEHAVIOR_TREE_LIBRARY}
+ yaml-cpp
+)
+
target_link_libraries(cell_routine
${catkin_LIBRARIES}
${OCTOMAP_LIBRARIES}
diff --git a/descriptions/dummy.yaml b/descriptions/dummy.yaml
deleted file mode 100644
index 575dd6a88091d57391b747c6bd5e0c2cad90d672..0000000000000000000000000000000000000000
--- a/descriptions/dummy.yaml
+++ /dev/null
@@ -1,64 +0,0 @@
-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
diff --git a/descriptions/dummy2.yaml b/descriptions/dummy2.yaml
deleted file mode 100644
index ac7a2a901f8d43bd77e94b7a0746637a824dbf6f..0000000000000000000000000000000000000000
--- a/descriptions/dummy2.yaml
+++ /dev/null
@@ -1,51 +0,0 @@
-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
diff --git a/include/mediator/grasp_mediator.h b/include/mediator/grasp_mediator.h
new file mode 100644
index 0000000000000000000000000000000000000000..18b910705f289dfc7ed4e61668a377b214012201
--- /dev/null
+++ b/include/mediator/grasp_mediator.h
@@ -0,0 +1,25 @@
+#ifndef GRASP_MEDIATOR_
+#define GRASP_MEDIATOR_
+
+#include "abstract_mediator.h"
+#include "moveit_mediator.h"
+#include "gb_grasp/MapConfigLoader.h"
+#include "gb_grasp/GraspMap.h"
+#include "gb_grasp/Cuboid.h"
+#include "gb_grasp/MapGenerator.h"
+#include "gb_grasp/VoxelManager.h"
+
+
+class GraspMediator : public MoveitMediator{
+ protected:
+ std::shared_ptr<moveit_visual_tools::MoveItVisualTools> visual_tools_; //!< MoveItVisualTools
+ std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_; //!< Planningscene monitor
+
+
+ public:
+ GraspMediator(std::shared_ptr<ros::NodeHandle> const& nh);
+ void mediate() override;
+ void generateGraspMap();
+};
+
+#endif
\ No newline at end of file
diff --git a/include/mediator/moveit_mediator.h b/include/mediator/moveit_mediator.h
index 9a9f319c602ec8c5362a2455c3c9d1ca9a616256..3c3dcff8746b02b09a00bd784e0cc336e2b33296 100644
--- a/include/mediator/moveit_mediator.h
+++ b/include/mediator/moveit_mediator.h
@@ -58,12 +58,10 @@
class MoveitMediator : public AbstractMediator{
protected:
std::shared_ptr<moveit::core::RobotModel> robot_model_; //!< Moveit Robot-Model as specified in SDF
- std::shared_ptr<moveit_visual_tools::MoveItVisualTools> visual_tools_; //!< MoveItVisualTools
std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi_; //!< Move Group Interface of the whole multi-cell
std::unique_ptr<moveit::planning_interface::PlanningSceneInterface> psi_; //!< PlanningSceneInteface to manage Scene Objects
std::shared_ptr<planning_scene::PlanningScene> ps_; //!< Shared Planning Scene
std::shared_ptr<ros::Publisher> planning_scene_diff_publisher_; //!< Publisher to manage PlanningScene diffs
- std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_; //!< Planningscene monitor
std::shared_ptr<moveit::task_constructor::solvers::PipelinePlanner> sampling_planner_; //!< Moveit task Constructior simple planner
std::shared_ptr<moveit::task_constructor::solvers::CartesianPath> cartesian_planner_; //!< Moveit task Constructior cartesian planner
std::map<std::string, std::vector<moveit::task_constructor::Task>> task_map_; //!< Tasks mapped to Robot
diff --git a/jobs/eval.yaml b/jobs/eval.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a85a633cababa52b0363a0a97c6f58ba39df8a68
--- /dev/null
+++ b/jobs/eval.yaml
@@ -0,0 +1,16 @@
+{ 'tasks':
+ {'groups' : [
+ { 'name': 'panda_arm1', 'jobs':[
+ [
+ { 'pos': { 'x': 0.2 ,'y': 0.4,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': -0.2 ,'y': 0.4,'z': 0.935500 }, 'orientation': { 'w': 1 } }
+ ],
+ [
+ { 'pos': { 'x': 0.2 ,'y': -0.1,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': -0.2 ,'y': -0.1,'z': 0.935500 }, 'orientation': { 'w': 1 } }
+ ],
+ ]
+ }
+ ]
+ }
+}
diff --git a/jobs/eval_dual.yaml b/jobs/eval_dual.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..3d2c9a1a25599f711ba057a92df371f302fe951c
--- /dev/null
+++ b/jobs/eval_dual.yaml
@@ -0,0 +1,17 @@
+{ 'tasks':
+ {'groups' : [
+ { 'name': 'panda_arm1', 'jobs':[
+ [
+ { 'pos': { 'x': 0.2 ,'y': 0.4,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': -0.2 ,'y': 0.4,'z': 0.935500 }, 'orientation': { 'w': 1 } }
+ ]]},
+ { 'name': 'panda_arm2', 'jobs':[
+ [
+ { 'pos': { 'x': 0.2 ,'y': -0.1,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': -0.2 ,'y': -0.1,'z': 0.935500 }, 'orientation': { 'w': 1 } }
+ ],
+ ]
+ }
+ ]
+ }
+}
diff --git a/launch/base_routine.launch b/launch/base_routine.launch
deleted file mode 100644
index 73b344214795c3745b4ccd8b4f02eaf9cc7604a0..0000000000000000000000000000000000000000
--- a/launch/base_routine.launch
+++ /dev/null
@@ -1,13 +0,0 @@
-<launch>
- <arg name="map" default="dummy.yaml" />
- <arg name="description" default="dummy.yaml" />
- <arg name="resource" default="dummy" />
-
-
- <rosparam command="load" file="$(find multi_cell_builder)/maps/$(arg map)"/>
- <rosparam command="load" file="$(find multi_cell_builder)/descriptions/$(arg description)"/>
- <rosparam command="load" file="$(find multi_cell_builder)/resources/$(arg resource).yaml"/>
-
- <rosparam param="resource_name" subst_value="True"> $(arg resource)</rosparam>
- <node pkg="multi_cell_builder" type="base_routine" name="base_routine" output="screen" required="true"/>
-</launch>
\ No newline at end of file
diff --git a/launch/cell_routine.launch b/launch/cell_routine.launch
index 197d71c1915947ef58a733afc8c538a975f6bb34..dc3663ffd1b172738557946dac2660722af8150d 100644
--- a/launch/cell_routine.launch
+++ b/launch/cell_routine.launch
@@ -1,6 +1,6 @@
<launch>
- <arg name="result" default="dummy/-277257222.yaml" />
- <arg name="jobs" default="jobs/dummy.yaml" />
+ <arg name="result" default="evaluation/double_eval.yaml" />
+ <arg name="jobs" default="jobs/eval_dual.yaml" />
<!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
@@ -8,7 +8,6 @@
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam command="load" file="$(find multi_cell_builder)/mtc_templates/dummy.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/maps/dummy.yaml"/>
- <rosparam command="load" file="$(find multi_cell_builder)/descriptions/dummy2.yaml"/>
<rosparam command="load" file="$(find multi_cell_builder)/$(arg jobs)"/>
diff --git a/launch/config_routine.launch b/launch/config_routine.launch
new file mode 100644
index 0000000000000000000000000000000000000000..3678cfd10119d452adf23c0e7f81596bae009a37
--- /dev/null
+++ b/launch/config_routine.launch
@@ -0,0 +1,33 @@
+<launch>
+ <arg name="result" default="dummy/-277257222.yaml" />
+ <arg name="jobs" default="jobs/dummy.yaml" />
+
+ <!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
+
+ <!-- this is to change-->
+ <rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
+ <rosparam command="load" file="$(find multi_cell_builder)/mtc_templates/dummy.yaml" />
+ <rosparam command="load" file="$(find multi_cell_builder)/maps/dummy.yaml"/>
+ <rosparam command="load" file="$(find multi_cell_builder)/descriptions/dummy2.yaml"/>
+ <rosparam command="load" file="$(find multi_cell_builder)/$(arg jobs)"/>
+
+ <rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
+
+ <include file="$(find ceti_double)/launch/demo.launch">
+ <arg name="use_rviz" value="false"/>
+ <arg name="scene" value="$(arg result)" />
+ </include>
+
+ <include ns="cell_routine" file="$(find ceti_double)/launch/fake_moveit_controller_manager.launch.xml" />
+ <param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability" />
+
+ <include file="$(find ceti_double)/launch/moveit_rviz.launch">
+ <arg name="rviz_config" value="$(find multi_cell_builder)/launch/rviz/cell_routine.rviz" />
+ </include>
+
+ <node pkg="multi_cell_builder" type="config_routine" name="config_routine" output="screen" required="true">
+ <rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
+ </node>
+</launch>
+
+
diff --git a/launch/single_cell_routine.launch b/launch/single_cell_routine.launch
new file mode 100644
index 0000000000000000000000000000000000000000..4d5de89c243a72fbf2c8a9022ad254b9cd72a59a
--- /dev/null
+++ b/launch/single_cell_routine.launch
@@ -0,0 +1,34 @@
+<launch>
+ <arg name="result" default="evaluation/single_eval.yaml" />
+ <arg name="jobs" default="jobs/eval.yaml" />
+
+ <!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
+
+ <!-- this is to change-->
+ <rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
+ <rosparam command="load" file="$(find multi_cell_builder)/mtc_templates/dummy.yaml" />
+ <rosparam command="load" file="$(find multi_cell_builder)/maps/dummy.yaml"/>
+ <rosparam command="load" file="$(find multi_cell_builder)/$(arg jobs)"/>
+
+ <rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
+
+ <include file="$(find ceti_single)/launch/demo.launch">
+ <arg name="use_rviz" value="false"/>
+ <arg name="scene" value="$(arg result)" />
+ </include>
+
+ <include ns="cell_routine" file="$(find ceti_single)/launch/fake_moveit_controller_manager.launch.xml" />
+ <param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability" />
+
+ <include file="$(find ceti_single)/launch/moveit_rviz.launch">
+ <arg name="rviz_config" value="$(find multi_cell_builder)/launch/rviz/cell_routine.rviz" />
+ </include>
+
+ <node pkg="groot" type="Groot" name="Groot" output="screen" required="true">
+ </node>
+
+ <node pkg="multi_cell_builder" type="cell_routine" name="cell_routine" output="screen" required="true">
+ </node>
+</launch>
+
+
diff --git a/resources/dummy_single.yaml b/resources/dummy_single.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..8a39ef5d99cdf8ccf69d053dc077d936c9388f8e
--- /dev/null
+++ b/resources/dummy_single.yaml
@@ -0,0 +1,126 @@
+# create json file using
+# yq eval -o=json config_scene_ceti-table-placeworld.yaml > config_scene_ceti-table-placeworld.json
+{ 'objects': [
+ # table
+ # height: 12 cm wheels, 76 cm body, 1 cm table-top = 89 cm total
+ # wheels width/depth/height 12*12*12
+ # body width/depth/height 69*69*76
+ # table-top width/depth/height 80*80*1
+ # panels are 50*70, with 0.5 cm distance to the table-top
+ # height of both is 12 + 76 + .5 = 88.5
+ # their centre is at 40 + 0.25 + 25 = 65.25
+ # a filled table and a backplate going all the way up is not possible, because of the oversized collision model of the robot
+ # { 'id': 'table_body','pos': { 'z': 0.50 },'size': { 'length': .69,'width': 0.69,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'table1_wheel_1', 'pos': { 'x': 0.28,'y': 0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'table1_wheel_2', 'pos': { 'x': -0.28,'y': -0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'table1_wheel_3', 'pos': { 'x': -0.28,'y': 0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'table1_wheel_4', 'pos': { 'x': 0.28,'y': -0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'table1_body_front', 'pos': { 'x': .345, 'z': 0.50 },'size': { 'length': 0.005,'width': 0.69,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'table1_body_back', 'pos': { 'x': -.345, 'z': 0.45 },'size': { 'length': 0.005,'width': 0.69,'height': 0.66 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'table1_body_left', 'pos': { 'y': .345, 'z': 0.50 },'size': { 'length': 0.69,'width': 0.005,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'table1_body_right', 'pos': { 'y': -.345, 'z': 0.50 },'size': { 'length': 0.69,'width': 0.005,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'table1_table_top', 'pos': { 'z': 0.885 },'size': { 'length': 0.8,'width': 0.8,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'table1_left_panel', 'pos': { 'z': 0.885, 'y': -0.6525 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'table1_front_panel', 'pos': { 'z': 0.885, 'x': 0.6525 },'size': { 'length': 0.5,'width': 0.7,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'table1_right_panel', 'pos': { 'z': 0.885, 'y': 0.6525 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+ { 'id': 'shared_panel', 'pos': { 'z': 0.885, 'y': 0.6525 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+
+ # table 2 is shifted in y direction by the size of the base, the size of a side panel and two 5mm spaces:
+ # 0.8 + 0.5 + 2*0.05 = 1.31
+
+ { 'id': 'A1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y': -.7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'A2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y': -.6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'A3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y': -.7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'A4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y': -.6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+
+ { 'id': 'B1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': -.7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'B2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': -.6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+# { 'id': 'B3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': -.7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+# { 'id': 'B4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': -.6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+
+ { 'id': 'C1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': -.3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'C2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': -.2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'C3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': -.1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+ { 'id': 'C4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': -.3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'C5', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': -.2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'C6', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': -.1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+ { 'id': 'C7', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': -.3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'C8', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': -.2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'C9', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': -.1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+ { 'id': 'D1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': .1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'D2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': .2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'D3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': .3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+ { 'id': 'D4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': .1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'D5', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': .2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'D6', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': .3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+ { 'id': 'D7', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': .1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'D8', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': .2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'D9', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': .3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+ { 'id': 'E1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y': .6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'E2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y': .7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'E3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y': .6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'E4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y': .7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+
+ { 'id': 'F1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': .6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'F2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': .7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+# { 'id': 'F3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': .6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+# { 'id': 'F4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': .7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+
+ { 'id': 'G1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': 1.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'G2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': 1.11, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'G3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': 1.21, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+ { 'id': 'G4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': 1.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'G5', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': 1.11, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'G6', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': 1.21, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+ { 'id': 'G7', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': 1.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'G8', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': 1.11, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'G9', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': 1.21, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+ { 'id': 'H1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': 1.41, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'H2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': 1.51, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'H3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': 1.61, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+ { 'id': 'H4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': 1.41, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'H5', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': 1.51, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'H6', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': 1.61, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+ { 'id': 'H7', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': 1.41, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'H8', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': 1.51, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'H9', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': 1.61, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+ { 'id': 'I1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y': 1.91, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'I2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y': 2.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'I3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y': 1.91, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'I4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y': 2.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+
+ { 'id': 'J1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': 1.91, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+ { 'id': 'J2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': 2.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+# { 'id': 'J3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': 1.91, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+# { 'id': 'J4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': 2.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+ # the collaboration zone has height 120cm
+ { 'id': 'cz1', 'type': 'COLLABORATION_ZONE', 'pos': { 'z': 1.49, 'y': 0.6525 },'size': { 'length': 0.7,'width': 0.3,'height': 1.2 },'orientation': { 'w': 1 },'color': { 'r': 1,'g': 0.627,'b': 0 } },
+
+ { 'id': 'blue1', 'type': 'BOX', 'pos': { 'x': .1, 'y': -.7, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+ { 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': .2, 'y': .3, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+ { 'id': 'blue3', 'type': 'BOX', 'pos': { 'x': .2, 'y': -.1, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # impossible for the target B
+ { 'id': 'green1', 'type': 'BOX', 'pos': { 'x': .2, 'y': -.3, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
+ { 'id': 'green2', 'type': 'BOX', 'pos': { 'x': .1, 'y': 1.91, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
+ { 'id': 'red1', 'type': 'BOX', 'pos': { 'x': -.3, 'y': -.6, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+ { 'id': 'red2', 'type': 'BOX', 'pos': { 'x': .3, 'y': 1.41, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+
+ { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.22, 'z': 0.89 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
+] }
diff --git a/results/dummy/-359616072.yaml b/results/dummy/-359616072.yaml
index d51974870f14e734c955b17bd8c9291ca585a2e9..834012462268c32308ccade6eb994e17c92e5ebc 100644
--- a/results/dummy/-359616072.yaml
+++ b/results/dummy/-359616072.yaml
@@ -22,4 +22,6 @@
{ 'id': 'table2_right_panel' , 'pos': { 'x': -0.000005 , 'y': 1.957498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } },
{ 'id': 'arm1','type': 'ARM','pos': { 'x': 0.219998, 'y': -0.000002, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 1.000000, 'w': -0.000001 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220004, 'y': 1.304998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
+{ 'id': 'blue1', 'type': 'BOX', 'pos': { 'x': -0.3, 'y': -0.7, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+{ 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': 0.1, 'y': 1.11, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } } # target B
]}
\ No newline at end of file
diff --git a/results/dummy/-548755835.yaml b/results/dummy/-548755835.yaml
index 150f6b895f8ff79c222bc0529b023b2009a6f6d4..05dd9fb4df8fd654c43681c2dcc368eccfde979f 100644
--- a/results/dummy/-548755835.yaml
+++ b/results/dummy/-548755835.yaml
@@ -23,4 +23,6 @@
{ 'id': 'table2_right_panel' , 'pos': { 'x': -0.000007 , 'y': 1.857498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } },
{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': -0.100003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220005, 'y': 1.204997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
+{ 'id': 'blue1', 'type': 'BOX', 'pos': { 'x': -0.3, 'y': -0.7, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+{ 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': 0.1, 'y': 1.11, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } } # target B
]}
\ No newline at end of file
diff --git a/results/dummy/-564062842.yaml b/results/dummy/-564062842.yaml
index a0942bbf3b3ef54cb1c54765d05a6c1b2310ec17..4977d3d8cb4d574574961e1d05928119b149e574 100644
--- a/results/dummy/-564062842.yaml
+++ b/results/dummy/-564062842.yaml
@@ -22,4 +22,6 @@
{ 'id': 'table2_right_panel' , 'pos': { 'x': -0.000007 , 'y': 2.057498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } },
{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
+{ 'id': 'blue1', 'type': 'BOX', 'pos': { 'x': -0.3, 'y': -0.7, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+{ 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': 0.1, 'y': 1.11, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } } # target B
]}
\ No newline at end of file
diff --git a/results/evaluation/double_eval.yaml b/results/evaluation/double_eval.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..41f7ce0126f6163f4067956662a303584197bd86
--- /dev/null
+++ b/results/evaluation/double_eval.yaml
@@ -0,0 +1,23 @@
+{ 'objects' : [
+{ 'id' : 'table1_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_table_top', 'pos': { 'x': -0.000002 , 'y': 0.099998 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } },
+{ 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table2_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table2_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table2_table_top', 'pos': { 'x': -0.000005 , 'y': -0.75 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000003 } },
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
+{ 'id': 'blue1', 'type': 'BOX', 'pos': { 'x': 0.2, 'y': 0.4, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+{ 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': 0.2, 'y': -0.1, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+]}
\ No newline at end of file
diff --git a/results/evaluation/single_eval.yaml b/results/evaluation/single_eval.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d1e2a32464afc2d4e19bcb63b36eaedb4eddd3cc
--- /dev/null
+++ b/results/evaluation/single_eval.yaml
@@ -0,0 +1,14 @@
+{ 'objects' : [
+{ 'id' : 'table1_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_table_top', 'pos': { 'x': -0.000002 , 'y': 0.099998 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } },
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
+{ 'id': 'blue1', 'type': 'BOX', 'pos': { 'x': 0.2, 'y': 0.4, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+{ 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': 0.2, 'y': -0.1, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+]}
\ No newline at end of file
diff --git a/src/config_routine.cpp b/src/config_routine.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..257ca5fdfc59a9c148b5e974903d7ece753998cb
--- /dev/null
+++ b/src/config_routine.cpp
@@ -0,0 +1,30 @@
+#include "mediator/abstract_mediator.h"
+#include "mediator/grasp_mediator.h"
+#include "robot/decorators/panda_decorator.h"
+
+
+
+int main(int argc, char **argv){
+ ros::init(argc, argv, "config_routine");
+ std::shared_ptr<ros::NodeHandle> n(new ros::NodeHandle);
+
+ ros::AsyncSpinner spinner(1);
+ spinner.start();
+
+ std::shared_ptr<GraspMediator> mediator = std::make_shared<GraspMediator>(n);
+
+ auto rd = mediator->robotReader()->robotData();
+ for (int i = 0; i < rd.size() ;i++){
+ std::unique_ptr<AbstractRobot> ceti = std::make_unique<CetiRobot>(rd[i].name_, rd[i].pose_, rd[i].size_);
+ std::unique_ptr<PandaDecorator> ceti_panda = std::make_unique<PandaDecorator>(std::move(ceti));
+ mediator->connectRobots(std::move(ceti_panda));
+ }
+
+ //mediator->set_dirname(filename);
+
+ mediator->mediate();
+
+ while (ros::ok()){
+ ros::spinOnce();
+ }
+}
\ No newline at end of file
diff --git a/src/mediator/grasp_mediator.cpp b/src/mediator/grasp_mediator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d9a34e8b453d8b37304c263d305bae19a352bbae
--- /dev/null
+++ b/src/mediator/grasp_mediator.cpp
@@ -0,0 +1,201 @@
+#include "mediator/grasp_mediator.h"
+
+GraspMediator::GraspMediator(std::shared_ptr<ros::NodeHandle> const& nh)
+ : MoveitMediator(nh)
+ , planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description"))
+ {
+ if (planning_scene_monitor_->getPlanningScene()){
+ planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
+ "planning_scene");
+ planning_scene_monitor_->getPlanningScene()->setName("planning_scene");
+ } else {
+ ROS_ERROR_STREAM_NAMED("test", "Planning scene not configured");
+ return;
+ }
+
+ 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_->hideRobot();
+
+ visual_tools_->trigger();
+
+ // Publish the global frame
+ visual_tools_->publishAxis(Eigen::Isometry3d::Identity());
+ visual_tools_->trigger();
+ }
+
+void GraspMediator::mediate() {
+ setPanel();
+ publishTables();
+ generateGraspMap();
+ ros::waitForShutdown();
+}
+
+void GraspMediator::generateGraspMap() {
+ auto cd = cuboid_reader_->cuboidBin();
+
+ //std::vector<std::string> objs = {"bottle1", "bottle2"};
+
+ for (int i = 0; i < cd.size(); i ++){
+ std::stringstream ss;
+ ss << "box_" << i;
+
+ moveit_msgs::CollisionObject item;
+ item.id = ss.str();
+ item.header.frame_id = "world";
+ item.header.stamp = ros::Time::now();
+ item.primitives.resize(1);
+ item.primitives[0].type = item.primitives[0].BOX;
+ item.primitives[0].dimensions.resize(3);
+ item.primitives[0].dimensions[0] = cd[i].x_depth;
+ item.primitives[0].dimensions[1] = cd[i].y_width;
+ item.primitives[0].dimensions[2] = cd[i].z_heigth;
+
+ item.primitive_poses.resize(1);
+ item.primitive_poses[0].position.x = cd[i].Pose.position.x;
+ item.primitive_poses[0].position.y = cd[i].Pose.position.y;
+ item.primitive_poses[0].position.z = cd[i].Pose.position.z;
+ item.primitive_poses[0].orientation.x = cd[i].Pose.orientation.x;
+ item.primitive_poses[0].orientation.y = cd[i].Pose.orientation.y;
+ item.primitive_poses[0].orientation.z = cd[i].Pose.orientation.z;
+ item.primitive_poses[0].orientation.w = cd[i].Pose.orientation.w;
+ item.operation = item.ADD;
+
+ psi_->applyCollisionObject(item);
+ // Could also safe id's somewhere
+ }
+
+ ROS_INFO("Debug1!");
+ auto arm_jmg_ = robot_model_->getJointModelGroup(robots_.at("panda_arm1")->name());
+ ROS_INFO("Debug1.5!");
+ auto voxel_manager_ = std::make_shared<VoxelManager>(*nh_,"",visual_tools_,planning_scene_monitor_);
+
+ ROS_INFO("%f", robots_.at("panda_arm1")->tf().getOrigin().getZ());
+ ROS_INFO("Debug2!");
+ Cuboid voxelSpace("VS");
+ voxelSpace.Pose.position.x= robots_.at("panda_arm1")->tf().getOrigin().getX();
+ voxelSpace.Pose.position.y= robots_.at("panda_arm1")->tf().getOrigin().getY();
+ voxelSpace.Pose.position.z= robots_.at("panda_arm1")->tf().getOrigin().getZ()*2 + 0.1f;
+ voxelSpace.Pose.orientation.x = robots_.at("panda_arm1")->tf().getRotation().getX();
+ voxelSpace.Pose.orientation.y = robots_.at("panda_arm1")->tf().getRotation().getY();
+ voxelSpace.Pose.orientation.z = robots_.at("panda_arm1")->tf().getRotation().getZ();
+ voxelSpace.Pose.orientation.w = robots_.at("panda_arm1")->tf().getRotation().getW();
+ voxelSpace.x_depth = 0.3f;
+ voxelSpace.y_width = 0.3f;
+ voxelSpace.z_heigth = 0.2f;
+
+ voxel_manager_->setVerboseLevel(0);
+ voxel_manager_->setVoxelSize(0.02f);
+ voxel_manager_->setVoxelSpace(voxelSpace);
+ ROS_INFO("Debug2.5!");
+
+ XmlRpc::XmlRpcValue data;
+ nh_->getParam("/", data);
+
+ std::regex rx("panda_arm([0-9]+)");
+ std::smatch match;
+ std::regex_match(robots_.at("panda_arm1")->name(), match, rx);
+ data["base_link"]= "world";
+
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["end_effector_name"] = robots_.at("panda_arm1")->map().at("eef_name").c_str();
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["end_effector_type"] = "finger";
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_resolution"] = 0.001f;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["angle_resolution"] = 45;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_max_depth"] = 0.035f;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_min_depth"] = 0.01f;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_depth_resolution"] = 0.005f;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["approach_distance_desired"] = 0.05f;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["retreat_distance_desired"] = 0.05f;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["lift_distance_desired"] = 0.02f;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_padding_on_approach"] = 0.005f;
+
+ std::stringstream ss;
+ ss << "panda_" << match[1] << "_tool_center_point";
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["define_tcp_by_name"] = false;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_name"] = ss.str().c_str();
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][0] = 0;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][1] = 0;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][2] = -0.105f;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][3] = 0;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][4] = 0;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][5] = -0.7853f;
+
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["pregrasp_time_from_start"] = 0;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_time_from_start"] = 0;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["pregrasp_posture"][0] = 0.04f;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_posture"][0] = 0;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["joints"][0] = robots_.at("panda_arm1")->mgiHand()->getJoints()[1].c_str();
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["max_finger_width"] = 0.085f;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["min_finger_width"] = 0.06f;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["max_grasp_width"] = 0.08f;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["gripper_finger_width"] = 0.015f;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["active_suction_range_x"] = 0.022f;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["active_suction_range_y"] = 0.012f;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["suction_rows_count"] = 2;
+ data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["suction_cols_count"] = 2;
+ nh_->setParam("/", data);
+
+ MapGenerator::MapGeneratorPtr grasp_map_generator_ = std::make_shared<MapGenerator>(*nh_,
+ visual_tools_,
+ planning_scene_monitor_,
+ robots_.at("panda_arm1")->map().at("eef_name"),
+ robots_.at("panda_arm1")->name(),
+ arm_jmg_,
+ robots_.at("panda_arm1")->mgi()->getCurrentState(),
+ voxel_manager_);
+ auto cb = cuboid_reader_->cuboidBin();
+
+ ROS_INFO("Debug3!");
+
+ std::vector<GraspMap> maps;
+
+ Cuboid workspace("WS");
+ workspace.Pose.position.x= robots_.at("panda_arm1")->tf().getOrigin().getX();
+ workspace.Pose.position.y= robots_.at("panda_arm1")->tf().getOrigin().getY();
+ workspace.Pose.position.z= robots_.at("panda_arm1")->tf().getOrigin().getZ()*2 + 0.02f;
+ workspace.Pose.orientation.x = robots_.at("panda_arm1")->tf().getRotation().getX();
+ workspace.Pose.orientation.y = robots_.at("panda_arm1")->tf().getRotation().getY();
+ workspace.Pose.orientation.z = robots_.at("panda_arm1")->tf().getRotation().getZ();
+ workspace.Pose.orientation.w = robots_.at("panda_arm1")->tf().getRotation().getW();
+ workspace.x_depth = 0.2f;
+ workspace.y_width = 0.2f;
+ workspace.z_heigth = 0.04f;
+
+ ROS_INFO("Debug4!");
+
+ double cube_clearence, translation_rate, rotation_rate, max_rotation;
+ cube_clearence = 0.01f;
+ translation_rate = 0.03f;
+ rotation_rate = 45.f * (M_PI/180.f);
+ max_rotation = 90.f * (M_PI/180.f);
+
+ for (auto& object: cb) {
+ GraspMap map(workspace,object,cube_clearence,translation_rate,rotation_rate,max_rotation);
+ maps.push_back(map);
+ }
+
+ ROS_INFO("Debug5!");
+
+ // Create Maps
+ {
+ ROS_INFO("creating map ...");
+ auto start_time = ros::Time::now();
+ for (GraspMap &m: maps) { grasp_map_generator_->SampleMap(m); }
+ double durration = (start_time - ros::Time::now()).toSec();
+ ROS_INFO_STREAM(maps.size() << " GraspMaps created in " << durration << " seconds");
+ }
+ ROS_INFO("Debug6!");
+
+}
diff --git a/src/mediator/moveit_mediator.cpp b/src/mediator/moveit_mediator.cpp
index 25e2736980c745ac43dbfc0fb56f636e2f0f3efa..4216a1487c1f81f70a1a9efec36939b92e628194 100644
--- a/src/mediator/moveit_mediator.cpp
+++ b/src/mediator/moveit_mediator.cpp
@@ -16,19 +16,9 @@ MoveitMediator::MoveitMediator(std::shared_ptr<ros::NodeHandle> const& nh)
, 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"))
, job_reader_(std::make_unique<JobReader>(nh_))
, cuboid_reader_(std::make_unique<CuboidReader>(nh)){
- if (planning_scene_monitor_->getPlanningScene()){
- planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
- "planning_scene");
- planning_scene_monitor_->getPlanningScene()->setName("planning_scene");
- } else {
- ROS_ERROR_STREAM_NAMED("test", "Planning scene not configured");
- return;
- }
-
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();
@@ -124,9 +114,7 @@ void MoveitMediator::mediate(){
publishTables();
taskPlanner();
ros::waitForShutdown();
-
-
-};
+}
void MoveitMediator::manipulateACM(AbstractRobotDecorator* mr, moveit_msgs::PlanningScene& ps){
// First find ID from panda to start with
@@ -477,6 +465,7 @@ void MoveitMediator::taskPlanner(){
BT::PublisherZMQ zmq(tree);
ros::Duration sleep(1);
BT::NodeStatus status = BT::NodeStatus::RUNNING;
+ auto t_start = std::chrono::high_resolution_clock::now();
while( status == BT::NodeStatus::RUNNING){
status = tree.tickRoot();
@@ -527,7 +516,9 @@ void MoveitMediator::taskPlanner(){
}
-
+ auto t_end = std::chrono::high_resolution_clock::now();
+ double elapsed_time_ms = std::chrono::duration<double, std::milli>(t_end-t_start).count();
+ ROS_INFO("%f", elapsed_time_ms);
// clean up
for (int i = 0; i < cd.size(); i ++){
diff --git a/test/reader/test_bt.cpp b/test/reader/test_bt.cpp
index 3635ab4eb927d1745cf782324ebd0b264f4b4c18..41a46bea481486c4b99a2aabfe378746bb6f7a37 100644
--- a/test/reader/test_bt.cpp
+++ b/test/reader/test_bt.cpp
@@ -13,11 +13,6 @@
#include <boost/circular_buffer.hpp>
TEST(BtTestSuit, basicParallelTest){
- BT::BehaviorTreeFactory factory;
- factory.registerNodeType<Execution>("Execution");
- factory.registerNodeType<PositionCondition>("PositionCondition");
- factory.registerNodeType<Parallel_robot>("Parallel_robot");
-
std::map<const std::string, std::vector<std::tuple<const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>> task_but_different;
boost::circular_buffer<std::pair<std::string, job_data>> cb_jd;
@@ -26,17 +21,51 @@ TEST(BtTestSuit, basicParallelTest){
job1.jobs_ = {tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,0,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,1,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(1,1,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(1,0,0))};
job2.jobs_ = {tf2::Transform(tf2::Quaternion(2,2,0,1),tf2::Vector3(0,0,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(2,3,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(3,3,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(3,2,0))};
- std::vector<tf2::Vector3> boxes = {tf2::Vector3(0,0,0), tf2::Vector3(2,2,0)};
- cb_jd.push_back({"box1", job1});
- cb_jd.push_back({"box2", job2});
+ std::vector<std::pair<std::string,tf2::Vector3>> boxes = {{"box1", tf2::Vector3(0,0,0)}, {"box2", tf2::Vector3(2,2,0)}};
+ cb_jd.push_back({"panda_arm1", job1});
+ cb_jd.push_back({"panda_arm2", job2});
+
+
+ while(!cb_jd.empty()){
+ for(auto& box : boxes){
+ std::pair<std::string, job_data> temp = cb_jd.front();
+ if(tf2::tf2Distance2(temp.second.jobs_.front().getOrigin(), box.second) == 0) {
+ for (int k = 1; k < temp.second.jobs_.size(); k++){
+ box.second = temp.second.jobs_[k].getOrigin();
+ }
+ cb_jd.pop_front();
+ } else {cb_jd.push_back(temp);}
+ }
+ }
+ ASSERT_EQ(cb_jd.size(), 0); //All jobs
+}
+
+TEST(BtTestSuit, basicCooperativeTest){
+ // BT::BehaviorTreeFactory factory;
+ // factory.registerNodeType<Execution>("Execution");
+ // factory.registerNodeType<PositionCondition>("PositionCondition");
+ // factory.registerNodeType<Parallel_robot>("Parallel_robot");
+ std::map<const std::string, std::vector<std::tuple<const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>> task_but_different;
+
+ boost::circular_buffer<std::pair<std::string, job_data>> cb_jd;
+ job_data job1, job2, job3;
+ job1.jobs_ = {tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,0,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,1,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(1,1,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(1,0,0))};
+ job2.jobs_ = {tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(2,2,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(2,3,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(3,3,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(3,2,0))};
+ job3.jobs_ = {tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(1,0,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,1,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(1,1,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,0,0))};
+
+
+ std::vector<std::pair<std::string,tf2::Vector3>> boxes = {{"box1", tf2::Vector3(0,0,0)}, {"box2", tf2::Vector3(2,2,0)}};
+ cb_jd.push_back({"panda_arm1", job1});
+ cb_jd.push_back({"panda_arm2", job2});
+ cb_jd.push_back({"panda_arm1", job3});
while(!cb_jd.empty()){
for(auto& box : boxes){
std::pair<std::string, job_data> temp = cb_jd.front();
- if(tf2::tf2Distance2(temp.second.jobs_.front().getOrigin(), box) == 0) {
+ if(tf2::tf2Distance2(temp.second.jobs_.front().getOrigin(), box.second) == 0) {
for (int k = 1; k < temp.second.jobs_.size(); k++){
- box = temp.second.jobs_[k].getOrigin();
+ box.second = temp.second.jobs_[k].getOrigin();
}
cb_jd.pop_front();
} else {cb_jd.push_back(temp);}
@@ -45,6 +74,126 @@ TEST(BtTestSuit, basicParallelTest){
ASSERT_EQ(cb_jd.size(), 0); //All jobs
}
+TEST(BtTestSuit, ComplexCooperativeTest){
+ std::map<const std::string, std::vector<std::tuple<const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>> task_but_different;
+ boost::circular_buffer<std::pair<std::string, job_data>> cb_jd(3);
+ job_data job1, job2, job3;
+ job1.jobs_ = {tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,1.5f,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,2,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,2.5f,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,3,0))};
+ job2.jobs_ = {tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,0,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,0.5f,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,1,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,1.5f,0))};
+ job3.jobs_ = {tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,1.5f,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,2,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,2.5f,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,3,0))};
+
+
+ std::vector<std::pair<std::string,tf2::Vector3>> boxes = {{"box1", tf2::Vector3(0,0,0)}, {"box2", tf2::Vector3(2,2,0)}};
+ cb_jd.push_back({"panda_arm1", job3});
+ cb_jd.push_back({"panda_arm1", job1});
+ cb_jd.push_back({"panda_arm2", job2});
+
+ //ASSERT_FALSE(boxes[0].second == cb_jd[0].second.jobs_[0].getOrigin());
+
+
+ bool jobs_remaining = true;
+ while (jobs_remaining && !cb_jd.empty() && !boxes.empty()) {
+ bool moved = false;
+ for (auto it = cb_jd.begin(); it != cb_jd.end();) {
+ auto& temp = *it;
+ bool ready_to_move = true;
+ for (auto& box : boxes) {
+ if (temp.second.jobs_.front().getOrigin() == box.second) {
+ for (int k = 1; k < temp.second.jobs_.size(); k++) {
+ box.second = temp.second.jobs_[k].getOrigin();
+ }
+ if (ready_to_move) {
+ moved = true;
+ it = cb_jd.erase(it);
+ break;
+ }
+ } else {
+ ready_to_move = false;
+ }
+ }
+ if (!ready_to_move) {
+ ++it;
+ }
+ }
+ jobs_remaining = moved;
+ }
+
+ ASSERT_EQ(cb_jd.size(), 1); // All jobs
+ boxes.clear();
+ cb_jd.push_back({"panda_arm1", job3});
+ cb_jd.push_back({"panda_arm1", job1});
+ cb_jd.push_back({"panda_arm2", job2});
+
+ jobs_remaining = true;
+ while (jobs_remaining && !cb_jd.empty() && !boxes.empty()) {
+ bool moved = false;
+ for (auto it = cb_jd.begin(); it != cb_jd.end();) {
+ auto& temp = *it;
+ bool ready_to_move = true;
+ for (auto& box : boxes) {
+ if (temp.second.jobs_.front().getOrigin() == box.second) {
+ for (int k = 1; k < temp.second.jobs_.size(); k++) {
+ box.second = temp.second.jobs_[k].getOrigin();
+ }
+ if (ready_to_move) {
+ moved = true;
+ it = cb_jd.erase(it);
+ break;
+ }
+ } else {
+ ready_to_move = false;
+ }
+ }
+ if (!ready_to_move) {
+ ++it;
+ }
+ }
+ jobs_remaining = moved;
+ }
+
+ ASSERT_EQ(cb_jd.size(), 3); // All jobs
+}
+
+TEST(BtTestSuit, bahviorTest){
+ //std::unique_ptr<moveit::planning_interface::PlanningSceneInterface> psi_ = std::make_unique<moveit::planning_interface::PlanningSceneInterface>();
+ BT::BehaviorTreeFactory factory;
+ factory.registerNodeType<Execution>("Execution");
+ factory.registerNodeType<PositionCondition>("PositionCondition");
+ factory.registerNodeType<Parallel_robot>("Parallel_robot");
+
+ std::vector<std::string> robots_ = {"panda_arm1", "panda_arm2"};
+ std::map<std::string, std::vector<std::tuple<std::string, tf2::Vector3, std::vector<tf2::Vector3>>>> tasks;
+
+ tasks["panda_arm1"].push_back({"box1", tf2::Vector3(0,0,0), {tf2::Vector3(0,0,0)}});
+ tasks["panda_arm2"].push_back({"box2", tf2::Vector3(0,0,0), {tf2::Vector3(0,0,0)}});
+
+
+ std::stringstream ss;
+ ss << "<root main_tree_to_execute = \"MainTree\">\n";
+ ss << "<BehaviorTree ID=\"MainTree\">\n";
+ ss << "<Control ID=\"Parallel\" name=\"Agents\" success_threshold=\"2\" failure_threshold=\"2\">\n";
+
+ for (auto& rob : robots_){
+ ss << "<Control ID=\"Parallel_robot\" name=\""<< rob <<"\" success_threshold=\"3\" failure_threshold=\"3\">\n";
+ for(auto& task : tasks){
+ ss << "<SequenceStar name=\"root_sequence\">\n";
+ ss << "<Condition ID=\"PositionCondition\" name=\"Position_condition\"/>\n";
+ ss << "<Action ID=\"Execution\" name=\"Execution\"/>\n";
+ ss << "</SequenceStar>\n";
+ }
+ ss << "</Control>\n";
+ }
+ ss << "</Control>\n";
+ ss << "</BehaviorTree>\n";
+ ss << "</root>\n";
+
+ auto tree = factory.createTreeFromText(ss.str());
+ auto node_it = tree.nodes.begin();
+ ASSERT_TRUE(true);
+
+}
+
+
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "bt-test");