generate 'sortierkonfiguartion' for each robot as result, can be run with...

generate 'sortierkonfiguartion' for each robot as result, can be run with auerswald package, ToDo: generate launch files

include/mediator/grasp_mediator.h

@@ -18,12 +18,12 @@ class GraspMediator : public AbstractMediator{
     std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_; //!< Planningscene monitor
     std::unique_ptr<moveit_grasps_demo::GraspPipelineDemo> grasp_pipeline_demo_; //!< Auerswald grasp pipeline
     std::string referenceRobot_; //!< Reference Robot
+    bool autoMapping_;//!< override graspmap config
 
 
     public:
     GraspMediator(std::shared_ptr<ros::NodeHandle> const& nh);
     void mediate() override;
-    inline void setReferenceRobot(std::string& str) {referenceRobot_ = str;}
     void connectRobots(std::unique_ptr<AbstractRobotDecorator> robot) override;
     void setPanel() override;
 

launch/config_routine.launch

 
 <launch>
   <!-- specify the planning pipeline -->
-  <arg name="reference_robot" default="panda_arm1" />
-  <arg name="referenceXYZ" default="-0.320002 -0.000003 0.89" />
+  <arg name="referenceRobot" default="panda_arm1" />
+  <arg name="autoMapping" default="true" />
+  <arg name="referenceXYZ" default="-0.22 -0.2 0.89" />
   <arg name="referenceRPY" default="0 0 0" />
 
-  <arg name="result" default="dummy/-243824774.yaml" />
+  <arg name="result" default="dummy/-276051748/-276051748.yaml" />
 
   <rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
 
-  <rosparam param="reference_robot" subst_value="True"> $(arg reference_robot)</rosparam>
+  <rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam>
+  <rosparam param="autoMapping" subst_value="True"> $(arg autoMapping)</rosparam>
+
 
   <arg name="pipeline" default="ompl" />
 
@@ -86,7 +89,7 @@
     <param name="planning_group_name" value="panda_arm_hand"/>
     <rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/>
     <rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
-    <rosparam command="load" file="$(find gb_grasp)/config/grasp_map_config.yaml"/>
+    <rosparam command="load" file="$(find multi_cell_builder)/results/dummy/-276051748/configs/-276051748_panda_arm1.yaml"/>
   </node>
 
   <!-- Planning Functionality -->

results/dummy/-1234778859/configs/-1234778859_panda_arm1.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100002, 0.099998, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100002, 0.099998, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true

results/dummy/-1234778859/configs/-1234778859_panda_arm2.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100005, 1.405, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100005, 1.405, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true

results/dummy/-1505208581/configs/-1505208581_panda_arm1.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-2.04146e-06, 0.099998, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-2.04146e-06, 0.099998, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true

results/dummy/-1505208581/configs/-1505208581_panda_arm2.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-5.30321e-06, 1.405, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-5.30321e-06, 1.405, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true

results/dummy/-1775841698/configs/-1775841698_panda_arm1.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-2.04146e-06, -0.100002, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-2.04146e-06, -0.100002, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true

results/dummy/-1775841698/configs/-1775841698_panda_arm2.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-5.30321e-06, 1.205, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-5.30321e-06, 1.205, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true

results/dummy/-1982814221/configs/-1982814221_panda_arm1.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-2.04146e-06, -2.04146e-06, 0.91], 'rot': [0, -0, -3.14159], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-2.04146e-06, -2.04146e-06, 0.99], 'rot': [0, -0, -3.14159]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true

results/dummy/-1982814221/configs/-1982814221_panda_arm2.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-3.67233e-06, 1.305, 0.91], 'rot': [0, -0, 1.33714e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-3.67233e-06, 1.305, 0.99], 'rot': [0, -0, 1.33714e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true

results/dummy/-243824774.yaml → results/dummy/-276051748/-276051748.yaml

@@ -7,7 +7,7 @@
 { '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.100002 , 'y': -0.000002 , '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' : 'table1_table_top', 'pos': { 'x': -0.000002 , 'y': -0.200002 , '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 }}, 
@@ -16,11 +16,11 @@
 { '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.100005 , 'y': 1.304998 , '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': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , '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': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.000000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , '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': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , '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': 'table2_right_panel' , 'pos': { 'x': -0.100007 , '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' : 'table2_table_top', 'pos': { 'x': -0.000006 , 'y': 1.402498 , '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': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.452498 , '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': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': -0.200000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , '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': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , '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': 'table2_right_panel' , 'pos': { 'x': -0.000008 , 'y': 2.054998 , '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': 'binBlue','type': 'BIN','pos': { 'x': -0.34,'y': 0.49,'z': 0.9725 },  'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
 { 'id': 'binRed','type': 'BIN','pos': { 'x': 0.06,'y': 0.49,'z': 0.9725 },    'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
 { 'id': 'binGreen','type': 'BIN','pos': { 'x': 0.46,'y': 0.49,'z': 0.9725 },  'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
@@ -30,6 +30,6 @@
 { 'id': 'object4','type': 'BOX','pos': { 'x': -0.6,'y': -0.4,'z': 0.996 },   'size': { 'length': 0.031,'width': 0.062,'height': 0.121},'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'g': 1 } },
 { 'id': 'object5','type': 'BOX','pos': { 'x': -0.4,'y': -0.5,'z': 0.959 },  'size': { 'length': 0.031,'width': 0.031,'height': 0.138},'orientation': { 'z': 1,'w': 0.92388 },'color': { 'g': 1 } },
 { 'id': 'obstacle1','pos': { 'x': 0.34,'y': -0.11,'z': 0.94 },  'size': { 'length': 0.3,'width': 0.3,'height': 0.1 },'orientation': { 'z': 0.452123,'w': 1 },'color': { 'b': 1 } },
-{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.000003, '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.320005, 'y': 1.304997, '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': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': -0.200003, '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.220006, 'y': 1.402497, '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 } }, 
 ]}
\ No newline at end of file

results/dummy/-276051748/configs/-276051748_panda_arm1.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-2.04146e-06, -0.200002, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-2.04146e-06, -0.200002, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true

results/dummy/-276051748/configs/-276051748_panda_arm2.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-6.04679e-06, 1.4025, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-6.04679e-06, 1.4025, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true

results/dummy/-317753156/configs/-317753156_panda_arm1.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100002, -0.100002, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100002, -0.100002, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true

results/dummy/-317753156/configs/-317753156_panda_arm2.yaml

+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100006, 1.405, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100006, 1.405, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true