diff --git a/CMakeLists.txt b/CMakeLists.txt index 5e10b9ef0e382241e913e6d8ad891c21b95076b2..9c208bff20e0dc367bc9a20dc42d45681450c847 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -250,8 +250,78 @@ src/bt/parallel_robot.cpp ) catkin_add_gtest(base-test launch/integration.test test/mediator/test_base.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 +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 +) + +catkin_add_gtest(grasp-test launch/integration.test test/mediator/test_grasp.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 +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 ) + +target_link_libraries(grasp-test ${catkin_LIBRARIES} yaml-cpp) target_link_libraries(reader-test ${catkin_LIBRARIES} yaml-cpp) target_link_libraries(bt-test ${catkin_LIBRARIES} ${BEHAVIOR_TREE_LIBRARY} yaml-cpp) target_link_libraries(base-test ${catkin_LIBRARIES} ${BEHAVIOR_TREE_LIBRARY} yaml-cpp) \ No newline at end of file diff --git a/include/mediator/base_calculation_mediator.h b/include/mediator/base_calculation_mediator.h index 1db10054234d339e85547e684362f157e00ef716..05e6d88d727eaf2c756e9c8b42ed295ccfac8b08 100644 --- a/include/mediator/base_calculation_mediator.h +++ b/include/mediator/base_calculation_mediator.h @@ -16,6 +16,11 @@ #include "reader/wing_reader.h" #include "reader/robot_reader.h" + +struct protocol { + std::vector<std::tuple<std::string, tf2::Transform, std::bitset<3>>> robots; +}; + //! Concrete Mediator /*! Base calculation mediator, which mediates agents to form a multi-cell by placing/rotating and perform collision checks @@ -27,6 +32,8 @@ class BaseCalculationMediator : public AbstractMediator{ std::vector<std::vector<std::unique_ptr<AbstractRobotElement>>> wings_; //!< Possible panels per robot std::vector<std::vector<std::vector<protobuf_entry>>> protobuf_; //!< Datastructure as product of all baseposition -> robot combinations std::string filename_; + std::map<std::string, std::vector<protocol>> protocol_map_; + public: //! BaseCalculationMediator constructor @@ -56,10 +63,8 @@ class BaseCalculationMediator : public AbstractMediator{ //! Writes result file /*! Writes Protobuff file containing all robots, panels, Drop off locations and actual box positions - \param wc_solution - */ - void writeFile(std::vector<protobuf_entry>& wc_solution); + void writeFile(std::vector<std::vector<protocol>>& combinations); //! Marker publishing methode /*! @@ -91,6 +96,8 @@ class BaseCalculationMediator : public AbstractMediator{ */ bool checkCollision(const std::string& robot, std::bitset<3>& panel_mask, bool& workload, std::vector<object_data>& ts); + bool checkWorkcellCollision(const std::vector<std::string>& robot, std::vector<std::bitset<3>>& panel_mask, bool& workload); + //! Mediator implementation /*! Places and rotates robots till no collisions exists, all objects are somewhere on the tables @@ -106,6 +113,42 @@ class BaseCalculationMediator : public AbstractMediator{ bool sceneCollision(std::vector<protobuf_entry>& wc_solution); + void new_approximate(std::map<const std::string, std::vector<tf2::Transform>>& workcell, std::map<const std::string, std::vector<tf2::Transform>>::iterator& it, std::vector<std::string>& state, std::vector<std::bitset<3>>& panel_state); + + + static void generateCombinations(std::map<std::string, std::vector<protocol>>& solutionMap, + std::vector<std::vector<protocol>>& combinations, + std::vector<protocol>& currentCombination, + const std::vector<std::string>& keys, + int currentIndex) { + if (currentIndex == keys.size()) { + // Base case: reached the end of keys, add current combination to combinations + combinations.push_back(currentCombination); + return; + } + + const auto& key = keys[currentIndex]; + const auto& protocols = solutionMap[key]; + + for (const auto& protocol : protocols) { + // Add current protocol to current combination + currentCombination.push_back(protocol); + // Recursively generate combinations for the remaining keys + generateCombinations(solutionMap, combinations, currentCombination, keys, currentIndex + 1); + // Remove last added protocol to backtrack and explore other combinations + currentCombination.pop_back(); + } + } + +static bool hasRobot(const protocol& proto, const std::string& robotName) { + for (const auto& robot : proto.robots) { + if (std::get<0>(robot) == robotName) { + return true; + } + } + return false; +} + }; diff --git a/include/mediator/grasp_mediator.h b/include/mediator/grasp_mediator.h index 425b913acb8bc408a50d8f404c7230f1da3cbf54..a9e183378a0636dc0fa53ec30e53c46f11b19ece 100644 --- a/include/mediator/grasp_mediator.h +++ b/include/mediator/grasp_mediator.h @@ -25,6 +25,8 @@ class GraspMediator : public AbstractMediator{ public: GraspMediator(std::shared_ptr<ros::NodeHandle> const& nh); void rewriteResult(); + void rewriteJob(); + void mediate() override; void connectRobots(std::unique_ptr<AbstractRobotDecorator> robot) override; void setPanel() override; diff --git a/launch/integration.test b/launch/integration.test index 2b6719ccba8a1edaa56cf5bd46feb609ba461777..bfa7458dd849a2019bfe67fd18af5684254f6ff3 100644 --- a/launch/integration.test +++ b/launch/integration.test @@ -11,6 +11,8 @@ <test test-name="Reader-Tests" pkg="multi_cell_builder" type="reader-test" name="Readers"/> <test test-name="Bt-Tests" pkg="multi_cell_builder" type="bt-test" name="BehaviotTree"/> <test test-name="Base-Tests" pkg="multi_cell_builder" type="base-test" name="BaseMediator"/> + <test test-name="Grasp-Tests" pkg="multi_cell_builder" type="grasp-test" name="BaseMediator"/> + </launch> \ No newline at end of file diff --git a/results/dummy/38428695/38428695.yaml b/results/dummy/430537148/430537148.yaml similarity index 100% rename from results/dummy/38428695/38428695.yaml rename to results/dummy/430537148/430537148.yaml diff --git a/results/dummy/38428695/configs/38428695_panda_arm1.yaml b/results/dummy/430537148/configs/430537148_panda_arm1.yaml similarity index 94% rename from results/dummy/38428695/configs/38428695_panda_arm1.yaml rename to results/dummy/430537148/configs/430537148_panda_arm1.yaml index 5ca9e7ef240c1387a89eb3396014c64e9245c19c..204897bbf86868acfe713017b0d181d138c896e3 100644 --- a/results/dummy/38428695/configs/38428695_panda_arm1.yaml +++ b/results/dummy/430537148/configs/430537148_panda_arm1.yaml @@ -8,5 +8,5 @@ rotation_max: 90.0 clearance: 0.01 min_quality: 0.1 max_candidate_count: 10 -allow_dependencies: true +allow_dependencies: false top_grasps_only: true diff --git a/results/dummy/38428695/configs/38428695_panda_arm2.yaml b/results/dummy/430537148/configs/430537148_panda_arm2.yaml similarity index 93% rename from results/dummy/38428695/configs/38428695_panda_arm2.yaml rename to results/dummy/430537148/configs/430537148_panda_arm2.yaml index 0ae335f34c2a485a84c067bf68cf556cd9840164..369d7760c1a1a82a86e6ba1caa3a9650fc150012 100644 --- a/results/dummy/38428695/configs/38428695_panda_arm2.yaml +++ b/results/dummy/430537148/configs/430537148_panda_arm2.yaml @@ -8,5 +8,5 @@ rotation_max: 90.0 clearance: 0.01 min_quality: 0.1 max_candidate_count: 10 -allow_dependencies: true +allow_dependencies: false top_grasps_only: true diff --git a/results/dummy/430537148/execute430537148.launch b/results/dummy/430537148/execute430537148.launch new file mode 100644 index 0000000000000000000000000000000000000000..262c6274a0c2778bfafb8d3709ceb4d37ad4e430 --- /dev/null +++ b/results/dummy/430537148/execute430537148.launch @@ -0,0 +1,20 @@ +<launch> +<arg name="result" default="dummy/430537148/430537148.yaml" /> +<arg name="jobs" default="dummy/430537148/jobs/dummy.yaml" /> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(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="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/results/dummy/38428695/launch/panda_arm1_38428695.launch b/results/dummy/430537148/launch/panda_arm1_430537148.launch similarity index 96% rename from results/dummy/38428695/launch/panda_arm1_38428695.launch rename to results/dummy/430537148/launch/panda_arm1_430537148.launch index 8a40517b0f5c05bebc1eab4505faecf744f69058..31573df2b0f892f5e76190ecbe9d65de043fde7e 100644 --- a/results/dummy/38428695/launch/panda_arm1_38428695.launch +++ b/results/dummy/430537148/launch/panda_arm1_430537148.launch @@ -2,7 +2,7 @@ <arg name="referenceRobot" default="panda_arm1" /> <arg name="referenceXYZ" default="-0.220002 -0.100003 0.8875"/> <arg name="referenceRPY" default="0 -0 2.49943e-06"/> -<arg name="result" default="dummy/38428695/38428695.yaml" /> +<arg name="result" default="dummy/430537148/430537148.yaml" /> <rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> <rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam> <rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam> @@ -48,7 +48,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 multi_cell_builder)/results/dummy/38428695/configs/38428695_panda_arm1.yaml"/> +<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/430537148/configs/430537148_panda_arm1.yaml"/> </node> <arg name="planner" default="ompl" /> <include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml"> diff --git a/results/dummy/38428695/launch/panda_arm2_38428695.launch b/results/dummy/430537148/launch/panda_arm2_430537148.launch similarity index 96% rename from results/dummy/38428695/launch/panda_arm2_38428695.launch rename to results/dummy/430537148/launch/panda_arm2_430537148.launch index c207df7a9abda2de44ca0d8570ddcb19e00512d4..a306f8fe7237571ac75133325a8036cd99bbc86a 100644 --- a/results/dummy/38428695/launch/panda_arm2_38428695.launch +++ b/results/dummy/430537148/launch/panda_arm2_430537148.launch @@ -2,7 +2,7 @@ <arg name="referenceRobot" default="panda_arm2" /> <arg name="referenceXYZ" default="-0.220005 1.205 0.8875"/> <arg name="referenceRPY" default="0 -0 2.67427e-06"/> -<arg name="result" default="dummy/38428695/38428695.yaml" /> +<arg name="result" default="dummy/430537148/430537148.yaml" /> <rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> <rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam> <rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam> @@ -48,7 +48,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 multi_cell_builder)/results/dummy/38428695/configs/38428695_panda_arm2.yaml"/> +<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/430537148/configs/430537148_panda_arm2.yaml"/> </node> <arg name="planner" default="ompl" /> <include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml"> diff --git a/results/dummy/-1451657555/-1451657555.yaml b/results/dummy/433826865/433826865.yaml similarity index 95% rename from results/dummy/-1451657555/-1451657555.yaml rename to results/dummy/433826865/433826865.yaml index 42183134cc97f311b09ebc6dce13e29a87974603..d2869c513bce4a5cdc759ceee4d88d7c0489ba7b 100644 --- a/results/dummy/-1451657555/-1451657555.yaml +++ b/results/dummy/433826865/433826865.yaml @@ -18,7 +18,6 @@ { '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': 1.204998 , '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.552498 , '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.100000 , '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.752502 , '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.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': 'blue1', 'type': 'BOX', 'pos': { 'x': 0.1, 'y': -0.7, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, diff --git a/results/dummy/-1451657555/configs/-1451657555_panda_arm1.yaml b/results/dummy/433826865/configs/433826865_panda_arm1.yaml similarity index 94% rename from results/dummy/-1451657555/configs/-1451657555_panda_arm1.yaml rename to results/dummy/433826865/configs/433826865_panda_arm1.yaml index 5ca9e7ef240c1387a89eb3396014c64e9245c19c..204897bbf86868acfe713017b0d181d138c896e3 100644 --- a/results/dummy/-1451657555/configs/-1451657555_panda_arm1.yaml +++ b/results/dummy/433826865/configs/433826865_panda_arm1.yaml @@ -8,5 +8,5 @@ rotation_max: 90.0 clearance: 0.01 min_quality: 0.1 max_candidate_count: 10 -allow_dependencies: true +allow_dependencies: false top_grasps_only: true diff --git a/results/dummy/-1451657555/configs/-1451657555_panda_arm2.yaml b/results/dummy/433826865/configs/433826865_panda_arm2.yaml similarity index 93% rename from results/dummy/-1451657555/configs/-1451657555_panda_arm2.yaml rename to results/dummy/433826865/configs/433826865_panda_arm2.yaml index 0ae335f34c2a485a84c067bf68cf556cd9840164..369d7760c1a1a82a86e6ba1caa3a9650fc150012 100644 --- a/results/dummy/-1451657555/configs/-1451657555_panda_arm2.yaml +++ b/results/dummy/433826865/configs/433826865_panda_arm2.yaml @@ -8,5 +8,5 @@ rotation_max: 90.0 clearance: 0.01 min_quality: 0.1 max_candidate_count: 10 -allow_dependencies: true +allow_dependencies: false top_grasps_only: true diff --git a/results/dummy/433826865/execute433826865.launch b/results/dummy/433826865/execute433826865.launch new file mode 100644 index 0000000000000000000000000000000000000000..c3f57030048265798fb0e6be2ea863046114cc3d --- /dev/null +++ b/results/dummy/433826865/execute433826865.launch @@ -0,0 +1,20 @@ +<launch> +<arg name="result" default="dummy/433826865/433826865.yaml" /> +<arg name="jobs" default="dummy/433826865/jobs/dummy.yaml" /> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(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="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/results/dummy/-1451657555/launch/panda_arm1_-1451657555.launch b/results/dummy/433826865/launch/panda_arm1_433826865.launch similarity index 96% rename from results/dummy/-1451657555/launch/panda_arm1_-1451657555.launch rename to results/dummy/433826865/launch/panda_arm1_433826865.launch index dcd4e5cca23ccdc015257a0ebe720c6998097a0d..699d5bcff5ff5b0a007743fa78586f920ce52b94 100644 --- a/results/dummy/-1451657555/launch/panda_arm1_-1451657555.launch +++ b/results/dummy/433826865/launch/panda_arm1_433826865.launch @@ -2,7 +2,7 @@ <arg name="referenceRobot" default="panda_arm1" /> <arg name="referenceXYZ" default="-0.220002 -0.100003 0.8875"/> <arg name="referenceRPY" default="0 -0 2.49943e-06"/> -<arg name="result" default="dummy/-1451657555/-1451657555.yaml" /> +<arg name="result" default="dummy/433826865/433826865.yaml" /> <rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> <rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam> <rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam> @@ -48,7 +48,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 multi_cell_builder)/results/dummy/-1451657555/configs/-1451657555_panda_arm1.yaml"/> +<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/433826865/configs/433826865_panda_arm1.yaml"/> </node> <arg name="planner" default="ompl" /> <include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml"> diff --git a/results/dummy/-1451657555/launch/panda_arm2_-1451657555.launch b/results/dummy/433826865/launch/panda_arm2_433826865.launch similarity index 96% rename from results/dummy/-1451657555/launch/panda_arm2_-1451657555.launch rename to results/dummy/433826865/launch/panda_arm2_433826865.launch index 25a780652f7f49e04cfef4bb5d9537f99dcc81bd..a6e4f70b61b8540228a58e14885275aee73fd028 100644 --- a/results/dummy/-1451657555/launch/panda_arm2_-1451657555.launch +++ b/results/dummy/433826865/launch/panda_arm2_433826865.launch @@ -2,7 +2,7 @@ <arg name="referenceRobot" default="panda_arm2" /> <arg name="referenceXYZ" default="-0.220005 1.205 0.8875"/> <arg name="referenceRPY" default="0 -0 2.67427e-06"/> -<arg name="result" default="dummy/-1451657555/-1451657555.yaml" /> +<arg name="result" default="dummy/433826865/433826865.yaml" /> <rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> <rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam> <rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam> @@ -48,7 +48,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 multi_cell_builder)/results/dummy/-1451657555/configs/-1451657555_panda_arm2.yaml"/> +<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/433826865/configs/433826865_panda_arm2.yaml"/> </node> <arg name="planner" default="ompl" /> <include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml"> diff --git a/results/dummy/435978895/435978895.yaml b/results/dummy/435978895/435978895.yaml new file mode 100644 index 0000000000000000000000000000000000000000..d2869c513bce4a5cdc759ceee4d88d7c0489ba7b --- /dev/null +++ b/results/dummy/435978895/435978895.yaml @@ -0,0 +1,32 @@ +{ '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.100002 , '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': 1.204998 , '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.552498 , '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_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.752502 , '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.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': 'blue1', 'type': 'BOX', 'pos': { 'x': 0.1, 'y': -0.7, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': 0.2, 'y': 0.3, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'blue3', 'type': 'BOX', 'pos': { 'x': 0.2, 'y': -0.1, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'green1', 'type': 'BOX', 'pos': { 'x': 0.2, 'y': -0.3, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'green2', 'type': 'BOX', 'pos': { 'x': 0.1, 'y': 1.91, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'red1', 'type': 'BOX', 'pos': { 'x': -0.3, 'y': -0.6, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'red2', 'type': 'BOX', 'pos': { 'x': 0.3, 'y': 1.41, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ '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 } }, +]} \ No newline at end of file diff --git a/results/dummy/435978895/configs/435978895_panda_arm1.yaml b/results/dummy/435978895/configs/435978895_panda_arm1.yaml new file mode 100644 index 0000000000000000000000000000000000000000..204897bbf86868acfe713017b0d181d138c896e3 --- /dev/null +++ b/results/dummy/435978895/configs/435978895_panda_arm1.yaml @@ -0,0 +1,12 @@ +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: false +top_grasps_only: true diff --git a/results/dummy/435978895/configs/435978895_panda_arm2.yaml b/results/dummy/435978895/configs/435978895_panda_arm2.yaml new file mode 100644 index 0000000000000000000000000000000000000000..369d7760c1a1a82a86e6ba1caa3a9650fc150012 --- /dev/null +++ b/results/dummy/435978895/configs/435978895_panda_arm2.yaml @@ -0,0 +1,12 @@ +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: false +top_grasps_only: true diff --git a/results/dummy/435978895/execute435978895.launch b/results/dummy/435978895/execute435978895.launch new file mode 100644 index 0000000000000000000000000000000000000000..dbe7244744ece1c39892eb9df9526ab342e7ecf7 --- /dev/null +++ b/results/dummy/435978895/execute435978895.launch @@ -0,0 +1,20 @@ +<launch> +<arg name="result" default="dummy/435978895/435978895.yaml" /> +<arg name="jobs" default="dummy/435978895/jobs/dummy.yaml" /> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(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="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/results/dummy/435978895/launch/panda_arm1_435978895.launch b/results/dummy/435978895/launch/panda_arm1_435978895.launch new file mode 100644 index 0000000000000000000000000000000000000000..ac9b03e1bb13c40907affffd78197a37cc6dcf86 --- /dev/null +++ b/results/dummy/435978895/launch/panda_arm1_435978895.launch @@ -0,0 +1,57 @@ +<launch> +<arg name="referenceRobot" default="panda_arm1" /> +<arg name="referenceXYZ" default="-0.220002 -0.100003 0.8875"/> +<arg name="referenceRPY" default="0 -0 2.49943e-06"/> +<arg name="result" default="dummy/435978895/435978895.yaml" /> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> +<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam> +<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam> +<arg name="pipeline" default="ompl"/> +<arg name="db" default="false"/> +<arg name="db_path" default="$(find panda_moveit_config)/default_warehouse_mongo_db"/> +<arg name="debug" default="false" /> +<arg name="load_robot_description" default="true"/> +<arg name="moveit_controller_manager" default="fake"/> +<arg name="fake_execution_type" default="interpolate"/> +<arg name="use_gui" default="false"/> +<arg name="use_rviz" default="true"/> +<node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world panda_link0"/> +<group if="$(eval arg('moveit_controller_manager') == 'fake')"> +<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)"> +<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam> +</node> +<node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)"> +<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam> +</node> +<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen"/> +</group> +<include file="$(find panda_moveit_config)/launch/move_group.launch"> +<arg name="allow_trajectory_execution" value="true"/> +<arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/> +<arg name="fake_execution_type" value="$(arg fake_execution_type)"/> +<arg name="info" value="true"/> +<arg name="debug" value="$(arg debug)"/> +<arg name="pipeline" value="$(arg pipeline)"/> +<arg name="load_robot_description" value="$(arg load_robot_description)"/> +<arg name="referenceXYZ" value="$(arg referenceXYZ)"/> +<arg name="referenceRPY" value="$(arg referenceRPY)"/> +</include> +<include file="$(find panda_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)"> +<arg name="moveit_warehouse_database_path" value="$(arg db_path)"/> +</include> +<node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false" args="-d $(find moveit_grasps)/launch/rviz/grasps.rviz" output="screen"> +<rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/> +</node> +<node name="config_routine" pkg="multi_cell_builder" type="config_routine" output="screen"> +<param name="gripper" value="two_finger"/> +<param name="ee_group_name" value="hand"/> +<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 multi_cell_builder)/results/dummy/435978895/configs/435978895_panda_arm1.yaml"/> +</node> +<arg name="planner" default="ompl" /> +<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml"> +<arg name="pipeline" value="$(arg planner)" /> +</include> +</launch> diff --git a/results/dummy/435978895/launch/panda_arm2_435978895.launch b/results/dummy/435978895/launch/panda_arm2_435978895.launch new file mode 100644 index 0000000000000000000000000000000000000000..94be3b116b8cb2ba708ce5d6cf0cf59c6f462caf --- /dev/null +++ b/results/dummy/435978895/launch/panda_arm2_435978895.launch @@ -0,0 +1,57 @@ +<launch> +<arg name="referenceRobot" default="panda_arm2" /> +<arg name="referenceXYZ" default="-0.220005 1.205 0.8875"/> +<arg name="referenceRPY" default="0 -0 2.67427e-06"/> +<arg name="result" default="dummy/435978895/435978895.yaml" /> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> +<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam> +<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam> +<arg name="pipeline" default="ompl"/> +<arg name="db" default="false"/> +<arg name="db_path" default="$(find panda_moveit_config)/default_warehouse_mongo_db"/> +<arg name="debug" default="false" /> +<arg name="load_robot_description" default="true"/> +<arg name="moveit_controller_manager" default="fake"/> +<arg name="fake_execution_type" default="interpolate"/> +<arg name="use_gui" default="false"/> +<arg name="use_rviz" default="true"/> +<node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world panda_link0"/> +<group if="$(eval arg('moveit_controller_manager') == 'fake')"> +<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)"> +<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam> +</node> +<node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)"> +<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam> +</node> +<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen"/> +</group> +<include file="$(find panda_moveit_config)/launch/move_group.launch"> +<arg name="allow_trajectory_execution" value="true"/> +<arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/> +<arg name="fake_execution_type" value="$(arg fake_execution_type)"/> +<arg name="info" value="true"/> +<arg name="debug" value="$(arg debug)"/> +<arg name="pipeline" value="$(arg pipeline)"/> +<arg name="load_robot_description" value="$(arg load_robot_description)"/> +<arg name="referenceXYZ" value="$(arg referenceXYZ)"/> +<arg name="referenceRPY" value="$(arg referenceRPY)"/> +</include> +<include file="$(find panda_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)"> +<arg name="moveit_warehouse_database_path" value="$(arg db_path)"/> +</include> +<node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false" args="-d $(find moveit_grasps)/launch/rviz/grasps.rviz" output="screen"> +<rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/> +</node> +<node name="config_routine" pkg="multi_cell_builder" type="config_routine" output="screen"> +<param name="gripper" value="two_finger"/> +<param name="ee_group_name" value="hand"/> +<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 multi_cell_builder)/results/dummy/435978895/configs/435978895_panda_arm2.yaml"/> +</node> +<arg name="planner" default="ompl" /> +<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml"> +<arg name="pipeline" value="$(arg planner)" /> +</include> +</launch> diff --git a/results/dummy/438078262/438078262.yaml b/results/dummy/438078262/438078262.yaml new file mode 100644 index 0000000000000000000000000000000000000000..d2869c513bce4a5cdc759ceee4d88d7c0489ba7b --- /dev/null +++ b/results/dummy/438078262/438078262.yaml @@ -0,0 +1,32 @@ +{ '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.100002 , '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': 1.204998 , '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.552498 , '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_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.752502 , '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.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': 'blue1', 'type': 'BOX', 'pos': { 'x': 0.1, 'y': -0.7, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': 0.2, 'y': 0.3, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'blue3', 'type': 'BOX', 'pos': { 'x': 0.2, 'y': -0.1, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'green1', 'type': 'BOX', 'pos': { 'x': 0.2, 'y': -0.3, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'green2', 'type': 'BOX', 'pos': { 'x': 0.1, 'y': 1.91, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'red1', 'type': 'BOX', 'pos': { 'x': -0.3, 'y': -0.6, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'red2', 'type': 'BOX', 'pos': { 'x': 0.3, 'y': 1.41, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ '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 } }, +]} \ No newline at end of file diff --git a/results/dummy/438078262/configs/438078262_panda_arm1.yaml b/results/dummy/438078262/configs/438078262_panda_arm1.yaml new file mode 100644 index 0000000000000000000000000000000000000000..204897bbf86868acfe713017b0d181d138c896e3 --- /dev/null +++ b/results/dummy/438078262/configs/438078262_panda_arm1.yaml @@ -0,0 +1,12 @@ +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: false +top_grasps_only: true diff --git a/results/dummy/438078262/configs/438078262_panda_arm2.yaml b/results/dummy/438078262/configs/438078262_panda_arm2.yaml new file mode 100644 index 0000000000000000000000000000000000000000..369d7760c1a1a82a86e6ba1caa3a9650fc150012 --- /dev/null +++ b/results/dummy/438078262/configs/438078262_panda_arm2.yaml @@ -0,0 +1,12 @@ +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: false +top_grasps_only: true diff --git a/results/dummy/438078262/execute438078262.launch b/results/dummy/438078262/execute438078262.launch new file mode 100644 index 0000000000000000000000000000000000000000..93f6fe4d69d7a8650ffa87b58c6c94b25541f1f3 --- /dev/null +++ b/results/dummy/438078262/execute438078262.launch @@ -0,0 +1,20 @@ +<launch> +<arg name="result" default="dummy/438078262/438078262.yaml" /> +<arg name="jobs" default="dummy/438078262/jobs/dummy.yaml" /> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(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="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/results/dummy/438078262/launch/panda_arm1_438078262.launch b/results/dummy/438078262/launch/panda_arm1_438078262.launch new file mode 100644 index 0000000000000000000000000000000000000000..483342ad7ae6a394d78351866b96eca1eee96a02 --- /dev/null +++ b/results/dummy/438078262/launch/panda_arm1_438078262.launch @@ -0,0 +1,57 @@ +<launch> +<arg name="referenceRobot" default="panda_arm1" /> +<arg name="referenceXYZ" default="-0.220002 -0.100003 0.8875"/> +<arg name="referenceRPY" default="0 -0 2.49943e-06"/> +<arg name="result" default="dummy/438078262/438078262.yaml" /> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> +<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam> +<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam> +<arg name="pipeline" default="ompl"/> +<arg name="db" default="false"/> +<arg name="db_path" default="$(find panda_moveit_config)/default_warehouse_mongo_db"/> +<arg name="debug" default="false" /> +<arg name="load_robot_description" default="true"/> +<arg name="moveit_controller_manager" default="fake"/> +<arg name="fake_execution_type" default="interpolate"/> +<arg name="use_gui" default="false"/> +<arg name="use_rviz" default="true"/> +<node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world panda_link0"/> +<group if="$(eval arg('moveit_controller_manager') == 'fake')"> +<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)"> +<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam> +</node> +<node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)"> +<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam> +</node> +<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen"/> +</group> +<include file="$(find panda_moveit_config)/launch/move_group.launch"> +<arg name="allow_trajectory_execution" value="true"/> +<arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/> +<arg name="fake_execution_type" value="$(arg fake_execution_type)"/> +<arg name="info" value="true"/> +<arg name="debug" value="$(arg debug)"/> +<arg name="pipeline" value="$(arg pipeline)"/> +<arg name="load_robot_description" value="$(arg load_robot_description)"/> +<arg name="referenceXYZ" value="$(arg referenceXYZ)"/> +<arg name="referenceRPY" value="$(arg referenceRPY)"/> +</include> +<include file="$(find panda_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)"> +<arg name="moveit_warehouse_database_path" value="$(arg db_path)"/> +</include> +<node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false" args="-d $(find moveit_grasps)/launch/rviz/grasps.rviz" output="screen"> +<rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/> +</node> +<node name="config_routine" pkg="multi_cell_builder" type="config_routine" output="screen"> +<param name="gripper" value="two_finger"/> +<param name="ee_group_name" value="hand"/> +<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 multi_cell_builder)/results/dummy/438078262/configs/438078262_panda_arm1.yaml"/> +</node> +<arg name="planner" default="ompl" /> +<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml"> +<arg name="pipeline" value="$(arg planner)" /> +</include> +</launch> diff --git a/results/dummy/438078262/launch/panda_arm2_438078262.launch b/results/dummy/438078262/launch/panda_arm2_438078262.launch new file mode 100644 index 0000000000000000000000000000000000000000..24954960f7e07d5d33fab52b73981a8f68d1cbb2 --- /dev/null +++ b/results/dummy/438078262/launch/panda_arm2_438078262.launch @@ -0,0 +1,57 @@ +<launch> +<arg name="referenceRobot" default="panda_arm2" /> +<arg name="referenceXYZ" default="-0.220005 1.205 0.8875"/> +<arg name="referenceRPY" default="0 -0 2.67427e-06"/> +<arg name="result" default="dummy/438078262/438078262.yaml" /> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> +<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam> +<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam> +<arg name="pipeline" default="ompl"/> +<arg name="db" default="false"/> +<arg name="db_path" default="$(find panda_moveit_config)/default_warehouse_mongo_db"/> +<arg name="debug" default="false" /> +<arg name="load_robot_description" default="true"/> +<arg name="moveit_controller_manager" default="fake"/> +<arg name="fake_execution_type" default="interpolate"/> +<arg name="use_gui" default="false"/> +<arg name="use_rviz" default="true"/> +<node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world panda_link0"/> +<group if="$(eval arg('moveit_controller_manager') == 'fake')"> +<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)"> +<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam> +</node> +<node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)"> +<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam> +</node> +<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen"/> +</group> +<include file="$(find panda_moveit_config)/launch/move_group.launch"> +<arg name="allow_trajectory_execution" value="true"/> +<arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/> +<arg name="fake_execution_type" value="$(arg fake_execution_type)"/> +<arg name="info" value="true"/> +<arg name="debug" value="$(arg debug)"/> +<arg name="pipeline" value="$(arg pipeline)"/> +<arg name="load_robot_description" value="$(arg load_robot_description)"/> +<arg name="referenceXYZ" value="$(arg referenceXYZ)"/> +<arg name="referenceRPY" value="$(arg referenceRPY)"/> +</include> +<include file="$(find panda_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)"> +<arg name="moveit_warehouse_database_path" value="$(arg db_path)"/> +</include> +<node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false" args="-d $(find moveit_grasps)/launch/rviz/grasps.rviz" output="screen"> +<rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/> +</node> +<node name="config_routine" pkg="multi_cell_builder" type="config_routine" output="screen"> +<param name="gripper" value="two_finger"/> +<param name="ee_group_name" value="hand"/> +<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 multi_cell_builder)/results/dummy/438078262/configs/438078262_panda_arm2.yaml"/> +</node> +<arg name="planner" default="ompl" /> +<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml"> +<arg name="pipeline" value="$(arg planner)" /> +</include> +</launch> diff --git a/results/dummy/2059996578/2059996578.yaml b/results/dummy/440481311/440481311.yaml similarity index 100% rename from results/dummy/2059996578/2059996578.yaml rename to results/dummy/440481311/440481311.yaml diff --git a/results/dummy/2059996578/configs/2059996578_panda_arm1.yaml b/results/dummy/440481311/configs/440481311_panda_arm1.yaml similarity index 94% rename from results/dummy/2059996578/configs/2059996578_panda_arm1.yaml rename to results/dummy/440481311/configs/440481311_panda_arm1.yaml index 3714fe613a3d628e4f1ab04a49a73983a2e9a840..78779c583656b85316ca7cc9827558a980ee67f1 100644 --- a/results/dummy/2059996578/configs/2059996578_panda_arm1.yaml +++ b/results/dummy/440481311/configs/440481311_panda_arm1.yaml @@ -8,5 +8,5 @@ rotation_max: 90.0 clearance: 0.01 min_quality: 0.1 max_candidate_count: 10 -allow_dependencies: true +allow_dependencies: false top_grasps_only: true diff --git a/results/dummy/883585462/configs/883585462_panda_arm2.yaml b/results/dummy/440481311/configs/440481311_panda_arm2.yaml similarity index 93% rename from results/dummy/883585462/configs/883585462_panda_arm2.yaml rename to results/dummy/440481311/configs/440481311_panda_arm2.yaml index 532e48d9d3a32c9821d4810064d15b167b6609ed..7e6ab881856bd33f430940467e96368aca421adc 100644 --- a/results/dummy/883585462/configs/883585462_panda_arm2.yaml +++ b/results/dummy/440481311/configs/440481311_panda_arm2.yaml @@ -8,5 +8,5 @@ rotation_max: 90.0 clearance: 0.01 min_quality: 0.1 max_candidate_count: 10 -allow_dependencies: true +allow_dependencies: false top_grasps_only: true diff --git a/results/dummy/440481311/execute440481311.launch b/results/dummy/440481311/execute440481311.launch new file mode 100644 index 0000000000000000000000000000000000000000..f5562342e8e54b10c7a56770284bbd979fbea82c --- /dev/null +++ b/results/dummy/440481311/execute440481311.launch @@ -0,0 +1,20 @@ +<launch> +<arg name="result" default="dummy/440481311/440481311.yaml" /> +<arg name="jobs" default="dummy/440481311/jobs/dummy.yaml" /> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(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="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/results/dummy/883585462/launch/panda_arm1_883585462.launch b/results/dummy/440481311/launch/panda_arm1_440481311.launch similarity index 96% rename from results/dummy/883585462/launch/panda_arm1_883585462.launch rename to results/dummy/440481311/launch/panda_arm1_440481311.launch index 62dd004a5eb755780d2db362b4f71d99fc2c3466..c447d0c00408886c92ddf5a0906d3c5f37823459 100644 --- a/results/dummy/883585462/launch/panda_arm1_883585462.launch +++ b/results/dummy/440481311/launch/panda_arm1_440481311.launch @@ -2,7 +2,7 @@ <arg name="referenceRobot" default="panda_arm1" /> <arg name="referenceXYZ" default="-0.220002 -2.59133e-06 0.8875"/> <arg name="referenceRPY" default="0 -0 2.49943e-06"/> -<arg name="result" default="dummy/883585462/883585462.yaml" /> +<arg name="result" default="dummy/440481311/440481311.yaml" /> <rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> <rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam> <rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam> @@ -48,7 +48,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 multi_cell_builder)/results/dummy/883585462/configs/883585462_panda_arm1.yaml"/> +<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/440481311/configs/440481311_panda_arm1.yaml"/> </node> <arg name="planner" default="ompl" /> <include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml"> diff --git a/results/dummy/883585462/launch/panda_arm2_883585462.launch b/results/dummy/440481311/launch/panda_arm2_440481311.launch similarity index 96% rename from results/dummy/883585462/launch/panda_arm2_883585462.launch rename to results/dummy/440481311/launch/panda_arm2_440481311.launch index bd3077cbb6d2eb2b7e5a19ec0d61973d548f0bb3..8bc03853c624b64644a6b10905d36d79762be1f3 100644 --- a/results/dummy/883585462/launch/panda_arm2_883585462.launch +++ b/results/dummy/440481311/launch/panda_arm2_440481311.launch @@ -2,7 +2,7 @@ <arg name="referenceRobot" default="panda_arm2" /> <arg name="referenceXYZ" default="-0.220005 1.305 0.8875"/> <arg name="referenceRPY" default="0 -0 2.67427e-06"/> -<arg name="result" default="dummy/883585462/883585462.yaml" /> +<arg name="result" default="dummy/440481311/440481311.yaml" /> <rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> <rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam> <rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam> @@ -48,7 +48,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 multi_cell_builder)/results/dummy/883585462/configs/883585462_panda_arm2.yaml"/> +<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/440481311/configs/440481311_panda_arm2.yaml"/> </node> <arg name="planner" default="ompl" /> <include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml"> diff --git a/results/dummy/883585462/883585462.yaml b/results/dummy/442867108/442867108.yaml similarity index 95% rename from results/dummy/883585462/883585462.yaml rename to results/dummy/442867108/442867108.yaml index 2743fa8a2a5a264d20f8b7d8d716a7b1de789aa7..df60e8084f9eb22036f58048dd47153aa1e5faae 100644 --- a/results/dummy/883585462/883585462.yaml +++ b/results/dummy/442867108/442867108.yaml @@ -18,7 +18,6 @@ { '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': 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.000004 , '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.652498 , '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.000000 , '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.000007 , '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': 'blue1', 'type': 'BOX', 'pos': { 'x': 0.1, 'y': -0.7, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, diff --git a/results/dummy/883585462/configs/883585462_panda_arm1.yaml b/results/dummy/442867108/configs/442867108_panda_arm1.yaml similarity index 94% rename from results/dummy/883585462/configs/883585462_panda_arm1.yaml rename to results/dummy/442867108/configs/442867108_panda_arm1.yaml index 3714fe613a3d628e4f1ab04a49a73983a2e9a840..78779c583656b85316ca7cc9827558a980ee67f1 100644 --- a/results/dummy/883585462/configs/883585462_panda_arm1.yaml +++ b/results/dummy/442867108/configs/442867108_panda_arm1.yaml @@ -8,5 +8,5 @@ rotation_max: 90.0 clearance: 0.01 min_quality: 0.1 max_candidate_count: 10 -allow_dependencies: true +allow_dependencies: false top_grasps_only: true diff --git a/results/dummy/2059996578/configs/2059996578_panda_arm2.yaml b/results/dummy/442867108/configs/442867108_panda_arm2.yaml similarity index 93% rename from results/dummy/2059996578/configs/2059996578_panda_arm2.yaml rename to results/dummy/442867108/configs/442867108_panda_arm2.yaml index 532e48d9d3a32c9821d4810064d15b167b6609ed..7e6ab881856bd33f430940467e96368aca421adc 100644 --- a/results/dummy/2059996578/configs/2059996578_panda_arm2.yaml +++ b/results/dummy/442867108/configs/442867108_panda_arm2.yaml @@ -8,5 +8,5 @@ rotation_max: 90.0 clearance: 0.01 min_quality: 0.1 max_candidate_count: 10 -allow_dependencies: true +allow_dependencies: false top_grasps_only: true diff --git a/results/dummy/442867108/execute442867108.launch b/results/dummy/442867108/execute442867108.launch new file mode 100644 index 0000000000000000000000000000000000000000..ca53db9126809f6571b4917a547c64a34dbfae0f --- /dev/null +++ b/results/dummy/442867108/execute442867108.launch @@ -0,0 +1,20 @@ +<launch> +<arg name="result" default="dummy/442867108/442867108.yaml" /> +<arg name="jobs" default="dummy/442867108/jobs/dummy.yaml" /> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(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="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/results/dummy/2059996578/launch/panda_arm1_2059996578.launch b/results/dummy/442867108/launch/panda_arm1_442867108.launch similarity index 96% rename from results/dummy/2059996578/launch/panda_arm1_2059996578.launch rename to results/dummy/442867108/launch/panda_arm1_442867108.launch index 36948db60f4561cac2c028e33be9e2335fd87f41..caee851e36249fca5c16c01eaf98905c34cc373f 100644 --- a/results/dummy/2059996578/launch/panda_arm1_2059996578.launch +++ b/results/dummy/442867108/launch/panda_arm1_442867108.launch @@ -2,7 +2,7 @@ <arg name="referenceRobot" default="panda_arm1" /> <arg name="referenceXYZ" default="-0.220002 -2.59133e-06 0.8875"/> <arg name="referenceRPY" default="0 -0 2.49943e-06"/> -<arg name="result" default="dummy/2059996578/2059996578.yaml" /> +<arg name="result" default="dummy/442867108/442867108.yaml" /> <rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> <rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam> <rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam> @@ -48,7 +48,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 multi_cell_builder)/results/dummy/2059996578/configs/2059996578_panda_arm1.yaml"/> +<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/442867108/configs/442867108_panda_arm1.yaml"/> </node> <arg name="planner" default="ompl" /> <include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml"> diff --git a/results/dummy/2059996578/launch/panda_arm2_2059996578.launch b/results/dummy/442867108/launch/panda_arm2_442867108.launch similarity index 96% rename from results/dummy/2059996578/launch/panda_arm2_2059996578.launch rename to results/dummy/442867108/launch/panda_arm2_442867108.launch index 9a2747df6f866bb0aa07c5df76a467288a437e13..5419b560073b1a7191b8737a1f61a7b3106ae77c 100644 --- a/results/dummy/2059996578/launch/panda_arm2_2059996578.launch +++ b/results/dummy/442867108/launch/panda_arm2_442867108.launch @@ -2,7 +2,7 @@ <arg name="referenceRobot" default="panda_arm2" /> <arg name="referenceXYZ" default="-0.220005 1.305 0.8875"/> <arg name="referenceRPY" default="0 -0 2.67427e-06"/> -<arg name="result" default="dummy/2059996578/2059996578.yaml" /> +<arg name="result" default="dummy/442867108/442867108.yaml" /> <rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> <rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam> <rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam> @@ -48,7 +48,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 multi_cell_builder)/results/dummy/2059996578/configs/2059996578_panda_arm2.yaml"/> +<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/442867108/configs/442867108_panda_arm2.yaml"/> </node> <arg name="planner" default="ompl" /> <include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml"> diff --git a/results/dummy/444768468/444768468.yaml b/results/dummy/444768468/444768468.yaml new file mode 100644 index 0000000000000000000000000000000000000000..df60e8084f9eb22036f58048dd47153aa1e5faae --- /dev/null +++ b/results/dummy/444768468/444768468.yaml @@ -0,0 +1,32 @@ +{ '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.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' : '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': 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.000004 , '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_left_panel' , 'pos': { 'x': -0.000000 , '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.000007 , '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': 'blue1', 'type': 'BOX', 'pos': { 'x': 0.1, 'y': -0.7, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': 0.2, 'y': 0.3, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'blue3', 'type': 'BOX', 'pos': { 'x': 0.2, 'y': -0.1, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'green1', 'type': 'BOX', 'pos': { 'x': 0.2, 'y': -0.3, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'green2', 'type': 'BOX', 'pos': { 'x': 0.1, 'y': 1.91, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'red1', 'type': 'BOX', 'pos': { 'x': -0.3, 'y': -0.6, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'red2', 'type': 'BOX', 'pos': { 'x': 0.3, 'y': 1.41, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, '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.220005, '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 } }, +]} \ No newline at end of file diff --git a/results/dummy/444768468/configs/444768468_panda_arm1.yaml b/results/dummy/444768468/configs/444768468_panda_arm1.yaml new file mode 100644 index 0000000000000000000000000000000000000000..78779c583656b85316ca7cc9827558a980ee67f1 --- /dev/null +++ b/results/dummy/444768468/configs/444768468_panda_arm1.yaml @@ -0,0 +1,12 @@ +mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-2.04146e-06, -2.04146e-06, 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, -2.04146e-06, 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: false +top_grasps_only: true diff --git a/results/dummy/444768468/configs/444768468_panda_arm2.yaml b/results/dummy/444768468/configs/444768468_panda_arm2.yaml new file mode 100644 index 0000000000000000000000000000000000000000..7e6ab881856bd33f430940467e96368aca421adc --- /dev/null +++ b/results/dummy/444768468/configs/444768468_panda_arm2.yaml @@ -0,0 +1,12 @@ +mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-5.30321e-06, 1.305, 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.305, 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: false +top_grasps_only: true diff --git a/results/dummy/444768468/execute444768468.launch b/results/dummy/444768468/execute444768468.launch new file mode 100644 index 0000000000000000000000000000000000000000..8c2965a54088f5a8aca78420a3c6c48b2aefdc84 --- /dev/null +++ b/results/dummy/444768468/execute444768468.launch @@ -0,0 +1,20 @@ +<launch> +<arg name="result" default="dummy/444768468/444768468.yaml" /> +<arg name="jobs" default="dummy/444768468/jobs/dummy.yaml" /> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(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="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/results/dummy/444768468/launch/panda_arm1_444768468.launch b/results/dummy/444768468/launch/panda_arm1_444768468.launch new file mode 100644 index 0000000000000000000000000000000000000000..d43ece07c9e250b398a114a076e6a2544fcf616a --- /dev/null +++ b/results/dummy/444768468/launch/panda_arm1_444768468.launch @@ -0,0 +1,57 @@ +<launch> +<arg name="referenceRobot" default="panda_arm1" /> +<arg name="referenceXYZ" default="-0.220002 -2.59133e-06 0.8875"/> +<arg name="referenceRPY" default="0 -0 2.49943e-06"/> +<arg name="result" default="dummy/444768468/444768468.yaml" /> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> +<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam> +<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam> +<arg name="pipeline" default="ompl"/> +<arg name="db" default="false"/> +<arg name="db_path" default="$(find panda_moveit_config)/default_warehouse_mongo_db"/> +<arg name="debug" default="false" /> +<arg name="load_robot_description" default="true"/> +<arg name="moveit_controller_manager" default="fake"/> +<arg name="fake_execution_type" default="interpolate"/> +<arg name="use_gui" default="false"/> +<arg name="use_rviz" default="true"/> +<node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world panda_link0"/> +<group if="$(eval arg('moveit_controller_manager') == 'fake')"> +<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)"> +<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam> +</node> +<node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)"> +<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam> +</node> +<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen"/> +</group> +<include file="$(find panda_moveit_config)/launch/move_group.launch"> +<arg name="allow_trajectory_execution" value="true"/> +<arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/> +<arg name="fake_execution_type" value="$(arg fake_execution_type)"/> +<arg name="info" value="true"/> +<arg name="debug" value="$(arg debug)"/> +<arg name="pipeline" value="$(arg pipeline)"/> +<arg name="load_robot_description" value="$(arg load_robot_description)"/> +<arg name="referenceXYZ" value="$(arg referenceXYZ)"/> +<arg name="referenceRPY" value="$(arg referenceRPY)"/> +</include> +<include file="$(find panda_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)"> +<arg name="moveit_warehouse_database_path" value="$(arg db_path)"/> +</include> +<node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false" args="-d $(find moveit_grasps)/launch/rviz/grasps.rviz" output="screen"> +<rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/> +</node> +<node name="config_routine" pkg="multi_cell_builder" type="config_routine" output="screen"> +<param name="gripper" value="two_finger"/> +<param name="ee_group_name" value="hand"/> +<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 multi_cell_builder)/results/dummy/444768468/configs/444768468_panda_arm1.yaml"/> +</node> +<arg name="planner" default="ompl" /> +<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml"> +<arg name="pipeline" value="$(arg planner)" /> +</include> +</launch> diff --git a/results/dummy/444768468/launch/panda_arm2_444768468.launch b/results/dummy/444768468/launch/panda_arm2_444768468.launch new file mode 100644 index 0000000000000000000000000000000000000000..26a864c3886cf4fff3bb0cf70578654b182c0521 --- /dev/null +++ b/results/dummy/444768468/launch/panda_arm2_444768468.launch @@ -0,0 +1,57 @@ +<launch> +<arg name="referenceRobot" default="panda_arm2" /> +<arg name="referenceXYZ" default="-0.220005 1.305 0.8875"/> +<arg name="referenceRPY" default="0 -0 2.67427e-06"/> +<arg name="result" default="dummy/444768468/444768468.yaml" /> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> +<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam> +<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam> +<arg name="pipeline" default="ompl"/> +<arg name="db" default="false"/> +<arg name="db_path" default="$(find panda_moveit_config)/default_warehouse_mongo_db"/> +<arg name="debug" default="false" /> +<arg name="load_robot_description" default="true"/> +<arg name="moveit_controller_manager" default="fake"/> +<arg name="fake_execution_type" default="interpolate"/> +<arg name="use_gui" default="false"/> +<arg name="use_rviz" default="true"/> +<node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world panda_link0"/> +<group if="$(eval arg('moveit_controller_manager') == 'fake')"> +<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)"> +<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam> +</node> +<node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)"> +<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam> +</node> +<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen"/> +</group> +<include file="$(find panda_moveit_config)/launch/move_group.launch"> +<arg name="allow_trajectory_execution" value="true"/> +<arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/> +<arg name="fake_execution_type" value="$(arg fake_execution_type)"/> +<arg name="info" value="true"/> +<arg name="debug" value="$(arg debug)"/> +<arg name="pipeline" value="$(arg pipeline)"/> +<arg name="load_robot_description" value="$(arg load_robot_description)"/> +<arg name="referenceXYZ" value="$(arg referenceXYZ)"/> +<arg name="referenceRPY" value="$(arg referenceRPY)"/> +</include> +<include file="$(find panda_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)"> +<arg name="moveit_warehouse_database_path" value="$(arg db_path)"/> +</include> +<node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false" args="-d $(find moveit_grasps)/launch/rviz/grasps.rviz" output="screen"> +<rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/> +</node> +<node name="config_routine" pkg="multi_cell_builder" type="config_routine" output="screen"> +<param name="gripper" value="two_finger"/> +<param name="ee_group_name" value="hand"/> +<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 multi_cell_builder)/results/dummy/444768468/configs/444768468_panda_arm2.yaml"/> +</node> +<arg name="planner" default="ompl" /> +<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml"> +<arg name="pipeline" value="$(arg planner)" /> +</include> +</launch> diff --git a/results/dummy/446956887/446956887.yaml b/results/dummy/446956887/446956887.yaml new file mode 100644 index 0000000000000000000000000000000000000000..df60e8084f9eb22036f58048dd47153aa1e5faae --- /dev/null +++ b/results/dummy/446956887/446956887.yaml @@ -0,0 +1,32 @@ +{ '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.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' : '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': 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.000004 , '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_left_panel' , 'pos': { 'x': -0.000000 , '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.000007 , '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': 'blue1', 'type': 'BOX', 'pos': { 'x': 0.1, 'y': -0.7, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': 0.2, 'y': 0.3, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'blue3', 'type': 'BOX', 'pos': { 'x': 0.2, 'y': -0.1, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'green1', 'type': 'BOX', 'pos': { 'x': 0.2, 'y': -0.3, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'green2', 'type': 'BOX', 'pos': { 'x': 0.1, 'y': 1.91, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'red1', 'type': 'BOX', 'pos': { 'x': -0.3, 'y': -0.6, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'red2', 'type': 'BOX', 'pos': { 'x': 0.3, 'y': 1.41, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, +{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, '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.220005, '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 } }, +]} \ No newline at end of file diff --git a/results/dummy/446956887/configs/446956887_panda_arm1.yaml b/results/dummy/446956887/configs/446956887_panda_arm1.yaml new file mode 100644 index 0000000000000000000000000000000000000000..78779c583656b85316ca7cc9827558a980ee67f1 --- /dev/null +++ b/results/dummy/446956887/configs/446956887_panda_arm1.yaml @@ -0,0 +1,12 @@ +mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-2.04146e-06, -2.04146e-06, 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, -2.04146e-06, 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: false +top_grasps_only: true diff --git a/results/dummy/446956887/configs/446956887_panda_arm2.yaml b/results/dummy/446956887/configs/446956887_panda_arm2.yaml new file mode 100644 index 0000000000000000000000000000000000000000..7e6ab881856bd33f430940467e96368aca421adc --- /dev/null +++ b/results/dummy/446956887/configs/446956887_panda_arm2.yaml @@ -0,0 +1,12 @@ +mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-5.30321e-06, 1.305, 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.305, 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: false +top_grasps_only: true diff --git a/results/dummy/446956887/execute446956887.launch b/results/dummy/446956887/execute446956887.launch new file mode 100644 index 0000000000000000000000000000000000000000..f5870f92322944df2b6f73860e0086fa25235085 --- /dev/null +++ b/results/dummy/446956887/execute446956887.launch @@ -0,0 +1,20 @@ +<launch> +<arg name="result" default="dummy/446956887/446956887.yaml" /> +<arg name="jobs" default="dummy/446956887/jobs/dummy.yaml" /> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(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="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/results/dummy/446956887/launch/panda_arm1_446956887.launch b/results/dummy/446956887/launch/panda_arm1_446956887.launch new file mode 100644 index 0000000000000000000000000000000000000000..94cf6b7bfe97fd4910695fe35f564ad984a060ed --- /dev/null +++ b/results/dummy/446956887/launch/panda_arm1_446956887.launch @@ -0,0 +1,57 @@ +<launch> +<arg name="referenceRobot" default="panda_arm1" /> +<arg name="referenceXYZ" default="-0.220002 -2.59133e-06 0.8875"/> +<arg name="referenceRPY" default="0 -0 2.49943e-06"/> +<arg name="result" default="dummy/446956887/446956887.yaml" /> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> +<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam> +<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam> +<arg name="pipeline" default="ompl"/> +<arg name="db" default="false"/> +<arg name="db_path" default="$(find panda_moveit_config)/default_warehouse_mongo_db"/> +<arg name="debug" default="false" /> +<arg name="load_robot_description" default="true"/> +<arg name="moveit_controller_manager" default="fake"/> +<arg name="fake_execution_type" default="interpolate"/> +<arg name="use_gui" default="false"/> +<arg name="use_rviz" default="true"/> +<node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world panda_link0"/> +<group if="$(eval arg('moveit_controller_manager') == 'fake')"> +<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)"> +<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam> +</node> +<node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)"> +<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam> +</node> +<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen"/> +</group> +<include file="$(find panda_moveit_config)/launch/move_group.launch"> +<arg name="allow_trajectory_execution" value="true"/> +<arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/> +<arg name="fake_execution_type" value="$(arg fake_execution_type)"/> +<arg name="info" value="true"/> +<arg name="debug" value="$(arg debug)"/> +<arg name="pipeline" value="$(arg pipeline)"/> +<arg name="load_robot_description" value="$(arg load_robot_description)"/> +<arg name="referenceXYZ" value="$(arg referenceXYZ)"/> +<arg name="referenceRPY" value="$(arg referenceRPY)"/> +</include> +<include file="$(find panda_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)"> +<arg name="moveit_warehouse_database_path" value="$(arg db_path)"/> +</include> +<node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false" args="-d $(find moveit_grasps)/launch/rviz/grasps.rviz" output="screen"> +<rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/> +</node> +<node name="config_routine" pkg="multi_cell_builder" type="config_routine" output="screen"> +<param name="gripper" value="two_finger"/> +<param name="ee_group_name" value="hand"/> +<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 multi_cell_builder)/results/dummy/446956887/configs/446956887_panda_arm1.yaml"/> +</node> +<arg name="planner" default="ompl" /> +<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml"> +<arg name="pipeline" value="$(arg planner)" /> +</include> +</launch> diff --git a/results/dummy/446956887/launch/panda_arm2_446956887.launch b/results/dummy/446956887/launch/panda_arm2_446956887.launch new file mode 100644 index 0000000000000000000000000000000000000000..52f9aafbff2bd6fde54420270c84d684153d5c0b --- /dev/null +++ b/results/dummy/446956887/launch/panda_arm2_446956887.launch @@ -0,0 +1,57 @@ +<launch> +<arg name="referenceRobot" default="panda_arm2" /> +<arg name="referenceXYZ" default="-0.220005 1.305 0.8875"/> +<arg name="referenceRPY" default="0 -0 2.67427e-06"/> +<arg name="result" default="dummy/446956887/446956887.yaml" /> +<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> +<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam> +<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam> +<arg name="pipeline" default="ompl"/> +<arg name="db" default="false"/> +<arg name="db_path" default="$(find panda_moveit_config)/default_warehouse_mongo_db"/> +<arg name="debug" default="false" /> +<arg name="load_robot_description" default="true"/> +<arg name="moveit_controller_manager" default="fake"/> +<arg name="fake_execution_type" default="interpolate"/> +<arg name="use_gui" default="false"/> +<arg name="use_rviz" default="true"/> +<node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world panda_link0"/> +<group if="$(eval arg('moveit_controller_manager') == 'fake')"> +<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)"> +<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam> +</node> +<node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)"> +<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam> +</node> +<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen"/> +</group> +<include file="$(find panda_moveit_config)/launch/move_group.launch"> +<arg name="allow_trajectory_execution" value="true"/> +<arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/> +<arg name="fake_execution_type" value="$(arg fake_execution_type)"/> +<arg name="info" value="true"/> +<arg name="debug" value="$(arg debug)"/> +<arg name="pipeline" value="$(arg pipeline)"/> +<arg name="load_robot_description" value="$(arg load_robot_description)"/> +<arg name="referenceXYZ" value="$(arg referenceXYZ)"/> +<arg name="referenceRPY" value="$(arg referenceRPY)"/> +</include> +<include file="$(find panda_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)"> +<arg name="moveit_warehouse_database_path" value="$(arg db_path)"/> +</include> +<node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false" args="-d $(find moveit_grasps)/launch/rviz/grasps.rviz" output="screen"> +<rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/> +</node> +<node name="config_routine" pkg="multi_cell_builder" type="config_routine" output="screen"> +<param name="gripper" value="two_finger"/> +<param name="ee_group_name" value="hand"/> +<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 multi_cell_builder)/results/dummy/446956887/configs/446956887_panda_arm2.yaml"/> +</node> +<arg name="planner" default="ompl" /> +<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml"> +<arg name="pipeline" value="$(arg planner)" /> +</include> +</launch> diff --git a/src/mediator/base_calculation_mediator.cpp b/src/mediator/base_calculation_mediator.cpp index 867e1bcb2821c72fa8ae892303252cc61877a1eb..69bd9de3badbcc80e19c3d774b4cf5e9dad96f12 100644 --- a/src/mediator/base_calculation_mediator.cpp +++ b/src/mediator/base_calculation_mediator.cpp @@ -1,5 +1,6 @@ #include "mediator/base_calculation_mediator.h" + BaseCalculationMediator::BaseCalculationMediator(std::shared_ptr<ros::NodeHandle> const& nh) : AbstractMediator(nh) , pub_(std::make_unique<ros::Publisher>(nh->advertise< visualization_msgs::MarkerArray >("visualization_marker_array", 1))){ @@ -60,6 +61,142 @@ void BaseCalculationMediator::setPanel(){ } } +bool BaseCalculationMediator::checkWorkcellCollision(const std::vector<std::string>& ws, std::vector<std::bitset<3>>& panel_mask, bool& workload){ + std::vector<tf2::Transform> combined_ts; + for(const auto& robot : ws){ + for (const auto& vec : task_space_reader_->dropOffData().at(robot)){ + if (std::find(combined_ts.begin(), combined_ts.end(), vec.pose_) == combined_ts.end()) { + combined_ts.push_back(vec.pose_); + } + } + } + + + + + workload = true; + std::map<std::string, int> workload_map; + + std::string some_strings[ws.size()]; + for (int i = 0; i < some_strings->size(); i++) some_strings[i] = std::string(""); + visualization_msgs::MarkerArray ma; + for (int x = 0; x < combined_ts.size(); x++){ + visualization_msgs::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = ros::Time(); + marker.ns = "combined TS"; + marker.id = x; + marker.type = visualization_msgs::Marker::CUBE; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.position.x = combined_ts[x].getOrigin().getX(); + marker.pose.position.y = combined_ts[x].getOrigin().getY(); + marker.pose.position.z = combined_ts[x].getOrigin().getZ(); + marker.pose.orientation.x = combined_ts[x].getRotation().getX(); + marker.pose.orientation.y = combined_ts[x].getRotation().getY(); + marker.pose.orientation.z = combined_ts[x].getRotation().getZ(); + marker.pose.orientation.w = combined_ts[x].getRotation().getW(); + marker.color.r = 1; + marker.color.g = 0; + marker.color.b = 0; + marker.color.a = 1.0; + marker.scale.x = 0.1f; + marker.scale.y = 0.1f; + marker.scale.z = 0.1f; + + // check if the targets stand at least at one robot + + bool standings = false; + for (int i = 0; i < ws.size(); i++){ + bool standing = true; + CetiRobot* ceti1; + try{ + ceti1 = dynamic_cast<CetiRobot*>(robots_.at(ws[i])->next()); + } catch (std::out_of_range& oor){} + tf2::Transform tr1 = combined_ts[x] * tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(marker.scale.x*0.5f, marker.scale.y*0.5f, 0)); + tf2::Transform tr2 = combined_ts[x] * tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-marker.scale.x*0.5f, marker.scale.y*0.5f, 0)); + tf2::Transform tr3 = combined_ts[x] * tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-marker.scale.x*0.5f, -marker.scale.y*0.5f, 0)); + tf2::Transform tr4 = combined_ts[x] * tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(marker.scale.x*0.5f, -marker.scale.y*0.5f, 0)); + standing &= ceti1->checkSingleObjectCollision(tr1, some_strings[i], panel_mask[i]); + standing &= ceti1->checkSingleObjectCollision(tr2, some_strings[i], panel_mask[i]); + standing &= ceti1->checkSingleObjectCollision(tr3, some_strings[i], panel_mask[i]); + standing &= ceti1->checkSingleObjectCollision(tr4, some_strings[i], panel_mask[i]); + standings |= standing; + } + + if (!standings) { + visualization_msgs::MarkerArray markerarray; + for (int y = x; y < combined_ts.size(); y++){ + visualization_msgs::Marker somemarker; + somemarker.header.frame_id = "map"; + somemarker.header.stamp = ros::Time(); + somemarker.ns = "combined TS"; + somemarker.id = y; + somemarker.type = visualization_msgs::Marker::CUBE; + somemarker.action = visualization_msgs::Marker::DELETE; + somemarker.color.r = 0; + somemarker.color.g = 0; + somemarker.color.b = 0; + somemarker.color.a = 0; + markerarray.markers.push_back(somemarker); + } + pub_->publish(markerarray); + return false; + } + + bool matches = false; + for (int i = 0; i < ws.size(); i++){ + CetiRobot* ceti1 = dynamic_cast<CetiRobot*>(robots_.at(ws[i])->next()); + std::string str; + if(ceti1->checkSingleObjectCollision(combined_ts[x], str, panel_mask[i])){ + try { + workload_map.at(str)++; + } catch (std::out_of_range& oor) { + workload_map.insert(std::pair<std::string,int>(str,1)); + } + matches |= true; + + } else { + matches |= false; + } + } + + // no robot hit + if (!matches) { + return false; + } else { + marker.color.r = 0; + marker.color.g = 1.0; + marker.color.b = 0; + marker.color.a = 1; + } + ma.markers.push_back(marker); + + } + + // At this point, any position should have matched + // the wotkload map shoud be base + panel masks + + for (int i = 0; i < ws.size(); i++){ + CetiRobot* ceti1 = dynamic_cast<CetiRobot*>(robots_.at(ws[i])->next()); + + for(std::size_t j = 0; j < ceti1->observers().size(); j++){ + if(ceti1->observerMask()[j] && panel_mask[i][j]){ + try{ + workload_map.at(ceti1->observers()[j]->name()); + } catch( std::out_of_range& oor){ + workload = false; + } + } + } + } + if(workload) pub_->publish(ma); + + + //pub_->publish(somemarker); + + return true; +} + bool BaseCalculationMediator::checkCollision(const std::string& robot, std::bitset<3>& panel_mask, bool& workload, std::vector<object_data>& ts){ // Visualization workspace definition @@ -116,31 +253,6 @@ bool BaseCalculationMediator::checkCollision(const std::string& robot, std::bits } // Publish Bounds - for(int j = 0; j < ceti1->robotRootBounds().size(); j++){ - tf2::Transform temp1 = ceti1->tf() * ceti1->rootTf() * ceti1->robotRootBounds()[j]; - visualization_msgs::Marker boundsMarker; - boundsMarker.header.frame_id = "map"; - boundsMarker.header.stamp = ros::Time(); - boundsMarker.ns = (char*) &ceti1->robotRootBounds(); - boundsMarker.id = j; - boundsMarker.type = visualization_msgs::Marker::CUBE; - boundsMarker.action = visualization_msgs::Marker::ADD; - boundsMarker.pose.position.x = temp1.getOrigin().getX(); - boundsMarker.pose.position.y = temp1.getOrigin().getY(); - boundsMarker.pose.position.z = temp1.getOrigin().getZ(); - boundsMarker.pose.orientation.x = temp1.getRotation().getX(); - boundsMarker.pose.orientation.y = temp1.getRotation().getY(); - boundsMarker.pose.orientation.z = temp1.getRotation().getZ(); - boundsMarker.pose.orientation.w = temp1.getRotation().getW(); - boundsMarker.scale.x = 0.01f; - boundsMarker.scale.y = 0.01f; - boundsMarker.scale.z = 0.1f; - boundsMarker.color.r = 0; - boundsMarker.color.g = 0; - boundsMarker.color.b = 1; - boundsMarker.color.a = 1; - ma.markers.push_back(boundsMarker); - } if(ceti1->checkSingleObjectCollision(ts[i].pose_, str, panel_mask)){ try { @@ -251,6 +363,16 @@ void BaseCalculationMediator::mediate(){ } ROS_INFO("------------"); } + + // Solution map + for (const auto& ws: workcells_with_bases){ + std::stringstream ss; + for (const auto& r : ws){ + ss << r.first; + } + protocol_map_.insert_or_assign(ss.str(), std::vector<protocol>()); + } + @@ -263,108 +385,485 @@ void BaseCalculationMediator::mediate(){ } } + +void BaseCalculationMediator::new_approximate(std::map<const std::string, std::vector<tf2::Transform>>& workcell, std::map<const std::string, std::vector<tf2::Transform>>::iterator& it, std::vector<std::string>& state, std::vector<std::bitset<3>>& panel_state){ + if (it == workcell.end()) { + std::stringstream ss; + protocol prot; + + for(int i = 0; i < state.size();i++){ + ss << state[i]; + std::tuple<std::string, tf2::Transform, std::bitset<3>> tuple(state[i], robots_.at(state[i])->tf(), panel_state[i]); + prot.robots.push_back(tuple); + } + protocol_map_.at(ss.str()).push_back(prot); + return; + } + + + + std::vector<tf2::Transform> access_map; + float padding = 0.0025f; + + // Choose next robot + CetiRobot* next_ceti = dynamic_cast<CetiRobot*>(robots_.at(it->first)->next()); + + + for (int x = 0; x < state.size();x++){ + CetiRobot* ceti = dynamic_cast<CetiRobot*>(robots_.at(state[x])->next()); + ceti->notify(); + + 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_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,ceti->size().getY()*j + padding,0))); + access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-ceti->size().getY()*j + padding,0))); + } else if (j == 0){ + access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(ceti->size().getX()*i + padding,0,0))); + access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-ceti->size().getX()*i + padding,0,0))); + } else { + access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(ceti->size().getX()*i+padding,ceti->size().getY()*j +padding,0))); + access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-ceti->size().getX()*i+padding,ceti->size().getY()*j +padding,0))); + access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(ceti->size().getX()*i+padding,-ceti->size().getY()*j +padding,0))); + access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-ceti->size().getX()*i+padding,-ceti->size().getY()*j+padding,0))); + } + + }; + }; + + for(std::size_t i = 0; i < ceti->observers().size(); i++){ + if(ceti->observerMask()[i] && panel_state[x][i]){ + access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.5f * ceti->size().getX() + ceti->observers()[i]->size().getX() + 2*padding + next_ceti->size().getX()*0.5f,0,0))); + access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.5f * ceti->size().getX() - ceti->observers()[i]->size().getX() - 2*padding - next_ceti->size().getX()*0.5f,0,0))); + access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.5f*ceti->size().getY() + ceti->observers()[i]->size().getY() + 2*padding + next_ceti->size().getY()*0.5f ,0))); + access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-0.5f*ceti->size().getY() - ceti->observers()[i]->size().getY() - 2*padding - next_ceti->size().getY()*0.5f,0))); + } + } + + for(auto& am: access_map){ + am = ceti->tf() * am; + } + } + + visualization_msgs::MarkerArray ma; + visualization_msgs::Marker m; + m.header.frame_id = "map"; + m.header.stamp = ros::Time(); + m.ns = next_ceti->name(); + m.id = 1; + m.type = visualization_msgs::Marker::CUBE; + m.action = visualization_msgs::Marker::ADD; + m.pose.position.x = next_ceti->tf().getOrigin().getX(); + m.pose.position.y = next_ceti->tf().getOrigin().getY(); + m.pose.position.z = next_ceti->tf().getOrigin().getZ(); + m.pose.orientation.x = next_ceti->tf().getRotation().getX(); + m.pose.orientation.y = next_ceti->tf().getRotation().getY(); + m.pose.orientation.z = next_ceti->tf().getRotation().getZ(); + m.pose.orientation.w = next_ceti->tf().getRotation().getW(); + + m.scale.x = next_ceti->size().getX(); + m.scale.y = next_ceti->size().getY(); + m.scale.z = next_ceti->tf().getOrigin().getZ()*2; + m.color.r = 1.0; + m.color.g = 1.0; + m.color.b = 1.0; + m.color.a = 1.0; + + for(int j = 0; j < next_ceti->robotRootBounds().size(); j++){ + tf2::Transform temp1 = next_ceti->tf() * next_ceti->rootTf() * next_ceti->robotRootBounds()[j]; + visualization_msgs::Marker boundsMarker; + boundsMarker.header.frame_id = "map"; + boundsMarker.header.stamp = ros::Time(); + boundsMarker.ns = next_ceti->name() + "bounds"; + boundsMarker.id = j; + boundsMarker.type = visualization_msgs::Marker::CUBE; + boundsMarker.action = visualization_msgs::Marker::ADD; + boundsMarker.pose.position.x = temp1.getOrigin().getX(); + boundsMarker.pose.position.y = temp1.getOrigin().getY(); + boundsMarker.pose.position.z = temp1.getOrigin().getZ(); + boundsMarker.pose.orientation.x = temp1.getRotation().getX(); + boundsMarker.pose.orientation.y = temp1.getRotation().getY(); + boundsMarker.pose.orientation.z = temp1.getRotation().getZ(); + boundsMarker.pose.orientation.w = temp1.getRotation().getW(); + boundsMarker.scale.x = 0.01f; + boundsMarker.scale.y = 0.01f; + boundsMarker.scale.z = 0.1f; + boundsMarker.color.r = 0; + boundsMarker.color.g = 0; + boundsMarker.color.b = 1; + boundsMarker.color.a = 1; + ma.markers.push_back(boundsMarker); + } + + for (std::size_t k = 0; k < next_ceti->observerMask().size(); k++){ + if(next_ceti->observerMask()[k]){ + auto wrd = dynamic_cast<RvizPanel*>(next_ceti->observers()[k]->next()); + ma.markers.push_back(wrd->marker()); + } + } + + + ma.markers.push_back(m); + pub_->publish(ma); + ma.markers.clear(); + + for(int j = 0; j <= 7; j++){ + std::bitset<3> wing_config(j); + for (long unsigned int i = 0; i < access_map.size(); i++){ + next_ceti->setTf(access_map[i]); + for ( float p = 0; p < 2*M_PI; p += M_PI/2){ + next_ceti->rotate(M_PI/2); + next_ceti->notify(); + m.action = visualization_msgs::Marker::MODIFY; + m.pose.position.x = next_ceti->tf().getOrigin().getX(); + m.pose.position.y = next_ceti->tf().getOrigin().getY(); + m.pose.position.z = next_ceti->tf().getOrigin().getZ(); + m.pose.orientation.x = next_ceti->tf().getRotation().getX(); + m.pose.orientation.y = next_ceti->tf().getRotation().getY(); + m.pose.orientation.z = next_ceti->tf().getRotation().getZ(); + m.pose.orientation.w = next_ceti->tf().getRotation().getW(); + ma.markers.push_back(m); + + for(int j = 0; j < next_ceti->robotRootBounds().size(); j++){ + tf2::Transform temp1 = next_ceti->tf() * next_ceti->rootTf() * next_ceti->robotRootBounds()[j]; + visualization_msgs::Marker boundsMarker; + boundsMarker.header.frame_id = "map"; + boundsMarker.header.stamp = ros::Time(); + boundsMarker.ns = next_ceti->name() + "bounds"; + boundsMarker.id = j; + boundsMarker.type = visualization_msgs::Marker::CUBE; + boundsMarker.action = visualization_msgs::Marker::MODIFY; + boundsMarker.pose.position.x = temp1.getOrigin().getX(); + boundsMarker.pose.position.y = temp1.getOrigin().getY(); + boundsMarker.pose.position.z = temp1.getOrigin().getZ(); + boundsMarker.pose.orientation.x = temp1.getRotation().getX(); + boundsMarker.pose.orientation.y = temp1.getRotation().getY(); + boundsMarker.pose.orientation.z = temp1.getRotation().getZ(); + boundsMarker.pose.orientation.w = temp1.getRotation().getW(); + boundsMarker.scale.x = 0.01f; + boundsMarker.scale.y = 0.01f; + boundsMarker.scale.z = 0.1f; + boundsMarker.color.r = 0; + boundsMarker.color.g = 0; + boundsMarker.color.b = 1; + boundsMarker.color.a = 1; + ma.markers.push_back(boundsMarker); + } + + for (std::size_t k = 0; k < next_ceti->observerMask().size(); k++){ + if(next_ceti->observerMask()[k] & wing_config[k]){ + auto wrd = dynamic_cast<RvizPanel*>(next_ceti->observers()[k]->next()); + wrd->marker().color.a = 1; + ma.markers.push_back(wrd->marker()); + } else if (next_ceti->observers()[k]){ + auto wrd = dynamic_cast<RvizPanel*>(next_ceti->observers()[k]->next()); + wrd->marker().color.a = 0; + ma.markers.push_back(wrd->marker()); + } + } + pub_->publish(ma); + ma.markers.clear(); + + bool workload; + auto temp_state = state; + temp_state.push_back(it->first); + + auto temp_panel_state = panel_state; + temp_panel_state.push_back(wing_config); + bool target_collisions = checkWorkcellCollision(temp_state, temp_panel_state, workload); + + auto nextIt = it; + ++nextIt; + if (target_collisions && workload) { + new_approximate(workcell, nextIt, temp_state, temp_panel_state); + } + } + } + } + + visualization_msgs::Marker marks; + marks.ns = next_ceti->name(); + marks.action = visualization_msgs::Marker::DELETEALL; + pub_->publish(marks); + + for (std::size_t k = 0; k < next_ceti->observerMask().size(); k++){ + if(next_ceti->observerMask()[k]){ + auto wrd = dynamic_cast<RvizPanel*>(next_ceti->observers()[k]->next()); + marks = wrd->marker(); + marks.action = visualization_msgs::Marker::DELETEALL; + pub_->publish(marks); + } + } + + for(int j = 0; j < next_ceti->robotRootBounds().size(); j++){ + visualization_msgs::Marker boundsMarker; + boundsMarker.header.frame_id = "map"; + boundsMarker.header.stamp = ros::Time(); + boundsMarker.ns = next_ceti->name() + "bounds"; + boundsMarker.id = j; + boundsMarker.action = visualization_msgs::Marker::DELETEALL; + pub_->publish(boundsMarker); + + } + +} + void BaseCalculationMediator::calculate(std::map<const std::string, std::vector<tf2::Transform>>& workcell){ /* Iterate through map, safe all possible solutions in [[(name, pose, config), (name, pose, config)], [....]] */ - std::vector<std::vector<protobuf_entry>> all_solutions_per_workcell; - + int count = 0; visualization_msgs::MarkerArray ma; - for (auto& s_pos : workcell){ - CetiRobot* ceti1; - try{ - ceti1 = dynamic_cast<CetiRobot*>(robots_.at(s_pos.first)->next()); - } catch(std::out_of_range& oor){} - - visualization_msgs::Marker m; - m.header.frame_id = "map"; - m.header.stamp = ros::Time(); - m.ns = ceti1->name(); - m.id = 1; - m.type = visualization_msgs::Marker::CUBE; - m.action = visualization_msgs::Marker::ADD; - m.pose.position.x = ceti1->tf().getOrigin().getX(); - m.pose.position.y = ceti1->tf().getOrigin().getY(); - m.pose.position.z = ceti1->tf().getOrigin().getZ(); - m.pose.orientation.x = ceti1->tf().getRotation().getX(); - m.pose.orientation.y = ceti1->tf().getRotation().getY(); - m.pose.orientation.z = ceti1->tf().getRotation().getZ(); - m.pose.orientation.w = ceti1->tf().getRotation().getW(); - - m.scale.x = ceti1->size().getX(); - m.scale.y = ceti1->size().getY(); - m.scale.z = ceti1->tf().getOrigin().getZ()*2; - m.color.r = 1.0; - m.color.g = 1.0; - m.color.b = 1.0; - m.color.a = 1.0; - - for (std::size_t k = 0; k < ceti1->observerMask().size(); k++){ - if(ceti1->observerMask()[k]){ - auto wrd = dynamic_cast<RvizPanel*>(ceti1->observers()[k]->next()); - ma.markers.push_back(wrd->marker()); - } + + auto first_robot = workcell.begin(); + CetiRobot* ceti1 = dynamic_cast<CetiRobot*>(robots_.at(first_robot->first)->next()); + visualization_msgs::Marker m; + m.header.frame_id = "map"; + m.header.stamp = ros::Time(); + m.ns = ceti1->name(); + m.id = 1; + m.type = visualization_msgs::Marker::CUBE; + m.action = visualization_msgs::Marker::ADD; + m.pose.position.x = ceti1->tf().getOrigin().getX(); + m.pose.position.y = ceti1->tf().getOrigin().getY(); + m.pose.position.z = ceti1->tf().getOrigin().getZ(); + m.pose.orientation.x = ceti1->tf().getRotation().getX(); + m.pose.orientation.y = ceti1->tf().getRotation().getY(); + m.pose.orientation.z = ceti1->tf().getRotation().getZ(); + m.pose.orientation.w = ceti1->tf().getRotation().getW(); + + m.scale.x = ceti1->size().getX(); + m.scale.y = ceti1->size().getY(); + m.scale.z = ceti1->tf().getOrigin().getZ()*2; + m.color.r = 1.0; + m.color.g = 1.0; + m.color.b = 1.0; + m.color.a = 1.0; + + for(int j = 0; j < ceti1->robotRootBounds().size(); j++){ + tf2::Transform temp1 = ceti1->tf() * ceti1->rootTf() * ceti1->robotRootBounds()[j]; + visualization_msgs::Marker boundsMarker; + boundsMarker.header.frame_id = "map"; + boundsMarker.header.stamp = ros::Time(); + boundsMarker.ns = ceti1->name() + "bounds"; + boundsMarker.id = j; + boundsMarker.type = visualization_msgs::Marker::CUBE; + boundsMarker.action = visualization_msgs::Marker::ADD; + boundsMarker.pose.position.x = temp1.getOrigin().getX(); + boundsMarker.pose.position.y = temp1.getOrigin().getY(); + boundsMarker.pose.position.z = temp1.getOrigin().getZ(); + boundsMarker.pose.orientation.x = temp1.getRotation().getX(); + boundsMarker.pose.orientation.y = temp1.getRotation().getY(); + boundsMarker.pose.orientation.z = temp1.getRotation().getZ(); + boundsMarker.pose.orientation.w = temp1.getRotation().getW(); + boundsMarker.scale.x = 0.01f; + boundsMarker.scale.y = 0.01f; + boundsMarker.scale.z = 0.1f; + boundsMarker.color.r = 0; + boundsMarker.color.g = 0; + boundsMarker.color.b = 1; + boundsMarker.color.a = 1; + ma.markers.push_back(boundsMarker); + } + + for (std::size_t k = 0; k < ceti1->observerMask().size(); k++){ + if(ceti1->observerMask()[k]){ + auto wrd = dynamic_cast<RvizPanel*>(ceti1->observers()[k]->next()); + ma.markers.push_back(wrd->marker()); } + } + + for(int j = 5; j <= 7; j++){ + std::bitset<3> wing_config(j); + for (long unsigned int i = 0; i < first_robot->second.size(); i++){ + ceti1->setTf(first_robot->second[i]); + for ( float p = 0; p < 2*M_PI; p += 0.0872665f){ + ceti1->rotate(0.0872665f); + ceti1->notify(); + m.action = visualization_msgs::Marker::MODIFY; + m.pose.position.x = ceti1->tf().getOrigin().getX(); + m.pose.position.y = ceti1->tf().getOrigin().getY(); + m.pose.position.z = ceti1->tf().getOrigin().getZ(); + m.pose.orientation.x = ceti1->tf().getRotation().getX(); + m.pose.orientation.y = ceti1->tf().getRotation().getY(); + m.pose.orientation.z = ceti1->tf().getRotation().getZ(); + m.pose.orientation.w = ceti1->tf().getRotation().getW(); + ma.markers.push_back(m); + for(int j = 0; j < ceti1->robotRootBounds().size(); j++){ + tf2::Transform temp1 = ceti1->tf() * ceti1->rootTf() * ceti1->robotRootBounds()[j]; + visualization_msgs::Marker boundsMarker; + boundsMarker.header.frame_id = "map"; + boundsMarker.header.stamp = ros::Time(); + boundsMarker.ns = ceti1->name() + "bounds"; + boundsMarker.id = j; + boundsMarker.type = visualization_msgs::Marker::CUBE; + boundsMarker.action = visualization_msgs::Marker::MODIFY; + boundsMarker.pose.position.x = temp1.getOrigin().getX(); + boundsMarker.pose.position.y = temp1.getOrigin().getY(); + boundsMarker.pose.position.z = temp1.getOrigin().getZ(); + boundsMarker.pose.orientation.x = temp1.getRotation().getX(); + boundsMarker.pose.orientation.y = temp1.getRotation().getY(); + boundsMarker.pose.orientation.z = temp1.getRotation().getZ(); + boundsMarker.pose.orientation.w = temp1.getRotation().getW(); + boundsMarker.scale.x = 0.01f; + boundsMarker.scale.y = 0.01f; + boundsMarker.scale.z = 0.1f; + boundsMarker.color.r = 0; + boundsMarker.color.g = 0; + boundsMarker.color.b = 1; + boundsMarker.color.a = 1; + ma.markers.push_back(boundsMarker); + } - ma.markers.push_back(m); - pub_->publish(ma); - ma.markers.clear(); - - for(int j = 5; j <= 7; j++){ - std::bitset<3> wing_config(j); - for (long unsigned int i = 0; i < s_pos.second.size(); i++){ - ceti1->setTf(s_pos.second[i]); - for ( float p = 0; p < 2*M_PI; p += 0.0872665f){ - ceti1->rotate(0.0872665f); - ceti1->notify(); - m.action = visualization_msgs::Marker::MODIFY; - m.pose.position.x = ceti1->tf().getOrigin().getX(); - m.pose.position.y = ceti1->tf().getOrigin().getY(); - m.pose.position.z = ceti1->tf().getOrigin().getZ(); - m.pose.orientation.x = ceti1->tf().getRotation().getX(); - m.pose.orientation.y = ceti1->tf().getRotation().getY(); - m.pose.orientation.z = ceti1->tf().getRotation().getZ(); - m.pose.orientation.w = ceti1->tf().getRotation().getW(); - ma.markers.push_back(m); - - for (std::size_t k = 0; k < ceti1->observerMask().size(); k++){ - if(ceti1->observerMask()[k] & wing_config[k]){ - auto wrd = dynamic_cast<RvizPanel*>(ceti1->observers()[k]->next()); - wrd->marker().color.a = 1; - ma.markers.push_back(wrd->marker()); - } else if (ceti1->observers()[k]){ - auto wrd = dynamic_cast<RvizPanel*>(ceti1->observers()[k]->next()); - wrd->marker().color.a = 0; - ma.markers.push_back(wrd->marker()); - } - } - pub_->publish(ma); - ma.markers.clear(); - - bool workload; - bool target_collisions = checkCollision(s_pos.first, wing_config, workload, task_space_reader_->dropOffData().at(ceti1->name())); - - if (workload && target_collisions){ - std::vector<protobuf_entry> wc_solution;// std::vector<protobuff> is one workcell solution - wc_solution.push_back(protobuf_entry{ceti1->name(), ceti1->tf(), j}); - approximation(workcell, wc_solution); - if (workcell.size() == wc_solution.size()){ - all_solutions_per_workcell.push_back(wc_solution); - } + for (std::size_t k = 0; k < ceti1->observerMask().size(); k++){ + if(ceti1->observerMask()[k] & wing_config[k]){ + auto wrd = dynamic_cast<RvizPanel*>(ceti1->observers()[k]->next()); + wrd->marker().color.a = 1; + ma.markers.push_back(wrd->marker()); + } else if (ceti1->observers()[k]){ + auto wrd = dynamic_cast<RvizPanel*>(ceti1->observers()[k]->next()); + wrd->marker().color.a = 0; + ma.markers.push_back(wrd->marker()); } } + pub_->publish(ma); + ma.markers.clear(); + + bool workload; + std::vector<std::string> check_ws = {first_robot->first}; + std::vector<std::bitset<3>> check_panel = {wing_config}; + bool target_collisions = checkWorkcellCollision(check_ws, check_panel, workload); + + auto nextIt = first_robot; + ++nextIt; + if (target_collisions && workload) { + new_approximate(workcell, nextIt, check_ws, check_panel); + count++; + } } } - m.action = visualization_msgs::Marker::DELETEALL; - ma.markers.push_back(m); - pub_->publish(ma); - ma.markers.clear(); - protobuf_.push_back(all_solutions_per_workcell); + } - + + std::vector<std::vector<protocol>> combinations; + std::vector<protocol> currentCombination; + std::vector<std::string> keys; + for (const auto& entry : protocol_map_) { + keys.push_back(entry.first); + } + BaseCalculationMediator::generateCombinations(protocol_map_, combinations, currentCombination, keys, 0); + ROS_INFO("%i", combinations.size()); + writeFile(combinations); + ros::shutdown; } +// for (auto& s_pos : workcell){ +// CetiRobot* ceti1; +// try{ +// ceti1 = dynamic_cast<CetiRobot*>(robots_.at(s_pos.first)->next()); +// } catch(std::out_of_range& oor){} + +// visualization_msgs::Marker m; +// m.header.frame_id = "map"; +// m.header.stamp = ros::Time(); +// m.ns = ceti1->name(); +// m.id = 1; +// m.type = visualization_msgs::Marker::CUBE; +// m.action = visualization_msgs::Marker::ADD; +// m.pose.position.x = ceti1->tf().getOrigin().getX(); +// m.pose.position.y = ceti1->tf().getOrigin().getY(); +// m.pose.position.z = ceti1->tf().getOrigin().getZ(); +// m.pose.orientation.x = ceti1->tf().getRotation().getX(); +// m.pose.orientation.y = ceti1->tf().getRotation().getY(); +// m.pose.orientation.z = ceti1->tf().getRotation().getZ(); +// m.pose.orientation.w = ceti1->tf().getRotation().getW(); + +// m.scale.x = ceti1->size().getX(); +// m.scale.y = ceti1->size().getY(); +// m.scale.z = ceti1->tf().getOrigin().getZ()*2; +// m.color.r = 1.0; +// m.color.g = 1.0; +// m.color.b = 1.0; +// m.color.a = 1.0; + +// for (std::size_t k = 0; k < ceti1->observerMask().size(); k++){ +// if(ceti1->observerMask()[k]){ +// auto wrd = dynamic_cast<RvizPanel*>(ceti1->observers()[k]->next()); +// ma.markers.push_back(wrd->marker()); +// } +// } + + +// ma.markers.push_back(m); +// pub_->publish(ma); +// ma.markers.clear(); + +// for(int j = 5; j <= 7; j++){ +// std::bitset<3> wing_config(j); +// for (long unsigned int i = 0; i < s_pos.second.size(); i++){ +// ceti1->setTf(s_pos.second[i]); +// for ( float p = 0; p < 2*M_PI; p += 0.0872665f){ +// ceti1->rotate(0.0872665f); +// ceti1->notify(); +// m.action = visualization_msgs::Marker::MODIFY; +// m.pose.position.x = ceti1->tf().getOrigin().getX(); +// m.pose.position.y = ceti1->tf().getOrigin().getY(); +// m.pose.position.z = ceti1->tf().getOrigin().getZ(); +// m.pose.orientation.x = ceti1->tf().getRotation().getX(); +// m.pose.orientation.y = ceti1->tf().getRotation().getY(); +// m.pose.orientation.z = ceti1->tf().getRotation().getZ(); +// m.pose.orientation.w = ceti1->tf().getRotation().getW(); +// ma.markers.push_back(m); + +// for (std::size_t k = 0; k < ceti1->observerMask().size(); k++){ +// if(ceti1->observerMask()[k] & wing_config[k]){ +// auto wrd = dynamic_cast<RvizPanel*>(ceti1->observers()[k]->next()); +// wrd->marker().color.a = 1; +// ma.markers.push_back(wrd->marker()); +// } else if (ceti1->observers()[k]){ +// auto wrd = dynamic_cast<RvizPanel*>(ceti1->observers()[k]->next()); +// wrd->marker().color.a = 0; +// ma.markers.push_back(wrd->marker()); +// } +// } +// pub_->publish(ma); +// ma.markers.clear(); + +// bool workload; +// std::vector<std::string> check_ws = {s_pos.first}; +// std::vector<std::bitset<3>> check_panel = {wing_config}; +// bool target_collisions = checkWorkcellCollision(check_ws, check_panel, workload); + +// if (target_collisions) count++; +// m.action = visualization_msgs::Marker::DELETEALL; +// ma.markers.push_back(m); +// pub_->publish(ma); +// ma.markers.clear(); +// // if (workload && target_collisions){ +// // std::vector<protobuf_entry> wc_solution;// std::vector<protobuff> is one workcell solution +// // wc_solution.push_back(protobuf_entry{ceti1->name(), ceti1->tf(), j}); +// // approximation(workcell, wc_solution); +// // if (workcell.size() == wc_solution.size()){ +// // all_solutions_per_workcell.push_back(wc_solution); +// // } +// // } +// } +// } +// } +// m.action = visualization_msgs::Marker::DELETEALL; +// ma.markers.push_back(m); +// pub_->publish(ma); +// ma.markers.clear(); +// protobuf_.push_back(all_solutions_per_workcell); +// } + + + +// } + void BaseCalculationMediator::approximation(std::map<const std::string, std::vector<tf2::Transform>>& workcell, std::vector<protobuf_entry>& wc_solution){ // build access fiels to existing wc_solution std::vector<tf2::Transform> access_map; @@ -387,7 +886,6 @@ void BaseCalculationMediator::approximation(std::map<const std::string, std::vec // make task_space unique auto ts = task_space_reader_->dropOffData().at(next_ceti->name()); - ROS_INFO("%i", ts.size()); for (auto& r: wc_solution){ for(auto& vec: task_space_reader_->dropOffData().at(r.name_)){ for (auto it = ts.begin(); it != ts.end();){ @@ -515,7 +1013,6 @@ void BaseCalculationMediator::approximation(std::map<const std::string, std::vec wc_solution.pop_back(); } else { if ( workcell.size() == wc_solution.size()) { - writeFile(wc_solution); //ros::shutdown(); } } @@ -615,19 +1112,9 @@ void Base_calculation_mediator::write_file(){ } */ -void BaseCalculationMediator::writeFile(std::vector<protobuf_entry>& wc_solution){ - - std::string resultFile = std::to_string(static_cast<int>(ros::Time::now().toNSec())); - std::filesystem::create_directory(ros::package::getPath("multi_cell_builder") + "/results/" + filename_ + "/" + resultFile); - std::filesystem::create_directory(ros::package::getPath("multi_cell_builder") + "/results/" + filename_ + "/" + resultFile + "/configs"); - std::filesystem::create_directory(ros::package::getPath("multi_cell_builder") + "/results/" + filename_ + "/" + resultFile + "/launch"); - - std::stringstream ss; - std::stringstream panel_ss; - std::stringstream root_ss; +void BaseCalculationMediator::writeFile(std::vector<std::vector<protocol>>& combinations){ std::stringstream box_ss; - for (auto& box : cuboid_reader_->cuboidBox()){ box_ss << "{ 'id': '" << box.Name << "', 'type': 'BOX', 'pos': { 'x': "<<box.Pose.position.x<<", 'y': "<<box.Pose.position.y<<", 'z': "<< box.Pose.position.z<<" },'size': { 'length': "<< box.x_depth<<", 'width': "<<box.y_width<<", 'height': "<< box.z_heigth<<" },'orientation': { 'x':"<< box.Pose.orientation.x <<", 'y':" << box.Pose.orientation.y<< ", 'z':" << box.Pose.orientation.z << ", 'w':" << box.Pose.orientation.w<< "},'color': { 'b': 1 } }, \n"; } @@ -636,198 +1123,240 @@ void BaseCalculationMediator::writeFile(std::vector<protobuf_entry>& wc_solution box_ss << "{ 'id': '" << box.Name << "', 'type': 'BIN', 'pos': { 'x': "<<box.Pose.position.x<<", 'y': "<<box.Pose.position.y<<", 'z': "<< box.Pose.position.z<<" },'size': { 'length': "<< box.x_depth<<", 'width': "<<box.y_width<<", 'height': "<< box.z_heigth<<" },'orientation': { 'x':"<< box.Pose.orientation.x <<", 'y':" << box.Pose.orientation.y<< ", 'z':" << box.Pose.orientation.z << ", 'w':" << box.Pose.orientation.w<< "},'color': { 'b': 1 } }, \n"; } - ss << "{ 'objects' : [ \n"; - for (int c = 0; c < wc_solution.size(); c++){ - try{ - std::regex rx("panda_arm([0-9]+)"); - std::smatch match; - std::regex_match(wc_solution[c].name_, match, rx); - - auto ceti = dynamic_cast<CetiRobot*>(robots_.at(wc_solution[c].name_)->next()); - ceti->setTf(wc_solution[c].tf_); - ceti->notify(); - - double r,p,y; - tf2::Matrix3x3 m(wc_solution[c].tf_.getRotation()); - m.getRPY(r,p,y); - - float size_x = ceti->size().getX(); - float size_y = ceti->size().getY(); - float size_z = ceti->size().getZ(); - - float pos_x = ceti->tf().getOrigin().getX(); - float pos_y = ceti->tf().getOrigin().getY(); - float pos_z = ceti->tf().getOrigin().getZ() *2 ; - float rot_x = ceti->tf().getRotation().getX(); - float rot_y = ceti->tf().getRotation().getY(); - float rot_z = ceti->tf().getRotation().getZ(); - float rot_w = ceti->tf().getRotation().getW(); - - // initial stardconfig - ss << "{ 'id' : 'table" << match[1] << "_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 }}, \n"; - ss << "{ 'id' : 'table" << match[1] << "_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 }},\n"; - ss << "{ 'id' : 'table" << match[1] << "_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 }}, \n"; - ss << "{ 'id' : 'table" << match[1] << "_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 }}, \n"; - ss << "{ 'id' : 'table" << match[1] << "_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 }}, \n"; - ss << "{ 'id' : 'table" << match[1] << "_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 }}, \n"; - ss << "{ 'id' : 'table" << match[1] << "_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 }}, \n"; - ss << "{ 'id' : 'table" << match[1] << "_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 }}, \n"; - ss << "{ 'id' : 'table" << match[1] << "_table_top', 'pos': { 'x': " << std::to_string(pos_x) << " , 'y': "<< std::to_string(pos_y) << " , 'z': "<< std::to_string(pos_z) << " },'size': { 'length': "<< std::to_string(size_x) << " ,'width': "<< std::to_string(size_y) << " ,'height': "<< std::to_string(size_z) << " },'orientation': { 'x': " << std::to_string(rot_x) << " , 'y': " << std::to_string(rot_y) << " , 'z': " << std::to_string(rot_z) << " , 'w': " << std::to_string(rot_w) << " },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': " << std::to_string(r) << " , 'p': " << std::to_string(p) << " , 'y': " << std::to_string(y) << " } },\n"; - - // config - std::stringstream config; - config << "mapspace: {'dim': ["; - config << size_x/2 << ", "; - config << size_y/2 << ", 0.04], "; - config << "'pos': ["; - config << pos_x << ", "; - config << pos_y << ", "; - config << (pos_z + size_z/2.0f + 0.04f/2.0f) << "], "; - config << "'rot': ["; - config << r << ", " << p << ", " << y <<"], "; - config << "'verbose': false }\n"; - config << "voxel_size: 0.02 \n"; - config << "voxel_space: {'dim': ["; - config << (size_x + 0.1f)/2 << ", "; - config << (size_y + 0.1f)/2 << ", 0.2], "; - config << "'pos': ["; - config << pos_x << ", "; - config << pos_y << ", "; - config << (pos_z + size_z/2.0f + 0.2f/2.0f) << "], "; - config << "'rot': ["; - config << r << ", " << p << ", " << y <<"]}\n"; - config << "voxel_verbose_level: 0\n"; - config << "translation_rate: 0.03\n"; - config << "rotation_rate: 45.0\n"; - config << "rotation_max: 90.0\n"; - config << "clearance: 0.01\n"; - config << "min_quality: 0.1\n"; - config << "max_candidate_count: 10\n"; - config << "allow_dependencies: true\n"; - config << "top_grasps_only: true\n"; - - std::ofstream o(ros::package::getPath("multi_cell_builder") + "/results/"+ filename_ + "/" + resultFile + "/configs/" + resultFile + "_"+ wc_solution[c].name_+".yaml"); - o << config.str(); - o.close(); - - - std::bitset<3> panel_mask(wc_solution[c].wing_config_); - for(std::size_t i = 0; i < ceti->observers().size(); i++){ - if(ceti->observerMask()[i] & panel_mask[i]){ - float x = ceti->observers()[i]->worldTf().getOrigin().getX(); - float y = ceti->observers()[i]->worldTf().getOrigin().getY(); - float z = ceti->observers()[i]->worldTf().getOrigin().getZ(); - float qx = ceti->observers()[i]->worldTf().getRotation().getX(); - float qy = ceti->observers()[i]->worldTf().getRotation().getY(); - float qz = ceti->observers()[i]->worldTf().getRotation().getZ(); - float qw = ceti->observers()[i]->worldTf().getRotation().getW(); - - float length = ceti->observers()[i]->size().getX(); - float width = ceti->observers()[i]->size().getY(); - float height = ceti->observers()[i]->size().getZ(); - panel_ss << "{ 'id': '" << ceti->observers()[i]->name() << "' , 'pos': { 'x': "<< std::to_string(x) << " , 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z - 0.25*height) << " } , 'size': { 'length': "<< std::to_string(length) << " , 'width': "<< std::to_string(width) << " , 'height': "<< std::to_string(height) << " } , 'orientation': { 'x': "<< std::to_string(qx) << " , 'y': "<< std::to_string(qy) << " , 'z': "<< std::to_string(qz) << " , 'w': "<< std::to_string(qw) << " } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, \n"; - } - } + for (int c = 0; c < combinations.size(); c++){ + std::string resultFile = std::to_string(static_cast<int>(ros::Time::now().toNSec())); + std::filesystem::create_directory(ros::package::getPath("multi_cell_builder") + "/results/" + filename_ + "/" + resultFile); + std::filesystem::create_directory(ros::package::getPath("multi_cell_builder") + "/results/" + filename_ + "/" + resultFile + "/configs"); + std::filesystem::create_directory(ros::package::getPath("multi_cell_builder") + "/results/" + filename_ + "/" + resultFile + "/launch"); + std::filesystem::create_directory(ros::package::getPath("multi_cell_builder") + "/results/" + filename_ + "/" + resultFile + "/jobs"); + + std::stringstream ss; + std::stringstream panel_ss; + std::stringstream root_ss; + + ss << "{ 'objects' : [ \n"; + + for (int x = 0; x < combinations[c].size(); x++){ + for (auto& prot: combinations[c][x].robots){ + try{ + std::regex rx("panda_arm([0-9]+)"); + std::smatch match; + std::regex_match(std::get<0>(prot), match, rx); + + auto ceti = dynamic_cast<CetiRobot*>(robots_.at(std::get<0>(prot))->next()); + ceti->setTf(std::get<1>(prot)); + ceti->notify(); + + double r,p,y; + tf2::Matrix3x3 m(std::get<1>(prot).getRotation()); + m.getRPY(r,p,y); + + float size_x = ceti->size().getX(); + float size_y = ceti->size().getY(); + float size_z = ceti->size().getZ(); + + float pos_x = ceti->tf().getOrigin().getX(); + float pos_y = ceti->tf().getOrigin().getY(); + float pos_z = ceti->tf().getOrigin().getZ() *2 ; + float rot_x = ceti->tf().getRotation().getX(); + float rot_y = ceti->tf().getRotation().getY(); + float rot_z = ceti->tf().getRotation().getZ(); + float rot_w = ceti->tf().getRotation().getW(); + + // initial stardconfig + ss << "{ 'id' : 'table" << match[1] << "_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 }}, \n"; + ss << "{ 'id' : 'table" << match[1] << "_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 }},\n"; + ss << "{ 'id' : 'table" << match[1] << "_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 }}, \n"; + ss << "{ 'id' : 'table" << match[1] << "_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 }}, \n"; + ss << "{ 'id' : 'table" << match[1] << "_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 }}, \n"; + ss << "{ 'id' : 'table" << match[1] << "_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 }}, \n"; + ss << "{ 'id' : 'table" << match[1] << "_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 }}, \n"; + ss << "{ 'id' : 'table" << match[1] << "_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 }}, \n"; + ss << "{ 'id' : 'table" << match[1] << "_table_top', 'pos': { 'x': " << std::to_string(pos_x) << " , 'y': "<< std::to_string(pos_y) << " , 'z': "<< std::to_string(pos_z) << " },'size': { 'length': "<< std::to_string(size_x) << " ,'width': "<< std::to_string(size_y) << " ,'height': "<< std::to_string(size_z) << " },'orientation': { 'x': " << std::to_string(rot_x) << " , 'y': " << std::to_string(rot_y) << " , 'z': " << std::to_string(rot_z) << " , 'w': " << std::to_string(rot_w) << " },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': " << std::to_string(r) << " , 'p': " << std::to_string(p) << " , 'y': " << std::to_string(y) << " } },\n"; + + // config + std::stringstream config; + config << "mapspace: {'dim': ["; + config << size_x/2 << ", "; + config << size_y/2 << ", 0.04], "; + config << "'pos': ["; + config << pos_x << ", "; + config << pos_y << ", "; + config << (pos_z + size_z/2.0f + 0.04f/2.0f) << "], "; + config << "'rot': ["; + config << r << ", " << p << ", " << y <<"], "; + config << "'verbose': false }\n"; + config << "voxel_size: 0.02 \n"; + config << "voxel_space: {'dim': ["; + config << (size_x + 0.1f)/2 << ", "; + config << (size_y + 0.1f)/2 << ", 0.2], "; + config << "'pos': ["; + config << pos_x << ", "; + config << pos_y << ", "; + config << (pos_z + size_z/2.0f + 0.2f/2.0f) << "], "; + config << "'rot': ["; + config << r << ", " << p << ", " << y <<"]}\n"; + config << "voxel_verbose_level: 0\n"; + config << "translation_rate: 0.03\n"; + config << "rotation_rate: 45.0\n"; + config << "rotation_max: 90.0\n"; + config << "clearance: 0.01\n"; + config << "min_quality: 0.1\n"; + config << "max_candidate_count: 10\n"; + config << "allow_dependencies: false\n"; + config << "top_grasps_only: true\n"; + + std::ofstream o(ros::package::getPath("multi_cell_builder") + "/results/"+ filename_ + "/" + resultFile + "/configs/" + resultFile + "_"+ std::get<0>(prot)+".yaml"); + o << config.str(); + o.close(); + + + std::bitset<3> panel_mask(std::get<2>(prot)); + for(std::size_t i = 0; i < ceti->observers().size(); i++){ + if(ceti->observerMask()[i] & panel_mask[i]){ + float x = ceti->observers()[i]->worldTf().getOrigin().getX(); + float y = ceti->observers()[i]->worldTf().getOrigin().getY(); + float z = ceti->observers()[i]->worldTf().getOrigin().getZ(); + float qx = ceti->observers()[i]->worldTf().getRotation().getX(); + float qy = ceti->observers()[i]->worldTf().getRotation().getY(); + float qz = ceti->observers()[i]->worldTf().getRotation().getZ(); + float qw = ceti->observers()[i]->worldTf().getRotation().getW(); + + float length = ceti->observers()[i]->size().getX(); + float width = ceti->observers()[i]->size().getY(); + float height = ceti->observers()[i]->size().getZ(); + panel_ss << "{ 'id': '" << ceti->observers()[i]->name() << "' , 'pos': { 'x': "<< std::to_string(x) << " , 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z - 0.25*height) << " } , 'size': { 'length': "<< std::to_string(length) << " , 'width': "<< std::to_string(width) << " , 'height': "<< std::to_string(height) << " } , 'orientation': { 'x': "<< std::to_string(qx) << " , 'y': "<< std::to_string(qy) << " , 'z': "<< std::to_string(qz) << " , 'w': "<< std::to_string(qw) << " } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, \n"; + } + } - tf2::Transform tf_arm = wc_solution[c].tf_ * ceti->rootTf(); - float arm_x = tf_arm.getOrigin().getX(); - float arm_y = tf_arm.getOrigin().getY(); - float arm_z = tf_arm.getOrigin().getZ(); - float arm_qx = tf_arm.getRotation().getX(); - float arm_qy = tf_arm.getRotation().getY(); - float arm_qz = tf_arm.getRotation().getZ(); - float arm_qw = tf_arm.getRotation().getW(); - - if (!(c == wc_solution.size())){ - // begin + tf2::Transform tf_arm = std::get<1>(prot) * ceti->rootTf(); + float arm_x = tf_arm.getOrigin().getX(); + float arm_y = tf_arm.getOrigin().getY(); + float arm_z = tf_arm.getOrigin().getZ(); + float arm_qx = tf_arm.getRotation().getX(); + float arm_qy = tf_arm.getRotation().getY(); + float arm_qz = tf_arm.getRotation().getZ(); + float arm_qw = tf_arm.getRotation().getW(); + root_ss << "{ 'id': 'arm" << match[1] << "','type': 'ARM','pos': { 'x': " << std::to_string(arm_x) << ", 'y': " << std::to_string(arm_y) << ", 'z': 0.89 },'size': { },'orientation': { 'x': " << std::to_string(arm_qx) <<", 'y': " << std::to_string(arm_qy) << ", 'z': " << std::to_string(arm_qz) << ", 'w': " << std::to_string(arm_qw) << " },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, \n"; - } else { - root_ss << "{ 'id': 'arm" << match[1] << "','type': 'ARM','pos': { 'x': " << std::to_string(arm_x) << ", 'y': " << std::to_string(arm_y) << ", 'z': 0.89 },'size': { },'orientation': { 'x': " << std::to_string(arm_qx) <<", 'y': " << std::to_string(arm_qy) << ", 'z': " << std::to_string(arm_qz) << ", 'w': " << std::to_string(arm_qw) << " },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }"; + + //launch + std::stringstream launch; + launch << "<launch>\n"; + launch << "<arg name=\"referenceRobot\" default=\""<< std::get<0>(prot) << "\" />\n"; + launch << "<arg name=\"referenceXYZ\" default=\""<< arm_x <<" "<< arm_y << " " << arm_z <<"\"/>\n"; + launch << "<arg name=\"referenceRPY\" default=\"" << r << " " << p << " " << y << "\"/>\n"; + launch << "<arg name=\"result\" default=\"" << filename_ <<"/" << resultFile << "/" << resultFile <<".yaml\" />\n"; + launch << "<rosparam command=\"load\" file=\"$(find multi_cell_builder)/results/$(arg result)\"/>\n"; + launch << "<rosparam param=\"referenceRobot\" subst_value=\"True\"> $(arg referenceRobot)</rosparam>\n"; + launch << "<rosparam param=\"resultPath\" subst_value=\"True\"> $(arg result)</rosparam>\n"; + launch << "<arg name=\"pipeline\" default=\"ompl\"/>\n"; + launch << "<arg name=\"db\" default=\"false\"/>\n"; + launch << "<arg name=\"db_path\" default=\"$(find panda_moveit_config)/default_warehouse_mongo_db\"/>\n"; + launch << "<arg name=\"debug\" default=\"false\" />\n"; + launch << "<arg name=\"load_robot_description\" default=\"true\"/>\n"; + launch << "<arg name=\"moveit_controller_manager\" default=\"fake\"/>\n"; + launch << "<arg name=\"fake_execution_type\" default=\"interpolate\"/>\n"; + launch << "<arg name=\"use_gui\" default=\"false\"/>\n"; + launch << "<arg name=\"use_rviz\" default=\"true\"/>\n"; + launch << "<node pkg=\"tf2_ros\" type=\"static_transform_publisher\" name=\"virtual_joint_broadcaster_0\" args=\"0 0 0 0 0 0 world panda_link0\"/>\n"; + launch << "<group if=\"$(eval arg('moveit_controller_manager') == 'fake')\">\n"; + launch << "<node name=\"joint_state_publisher\" pkg=\"joint_state_publisher\" type=\"joint_state_publisher\" unless=\"$(arg use_gui)\">\n"; + launch << "<rosparam param=\"source_list\">[move_group/fake_controller_joint_states]</rosparam>\n"; + launch << "</node>\n"; + launch << "<node name=\"joint_state_publisher\" pkg=\"joint_state_publisher_gui\" type=\"joint_state_publisher_gui\" if=\"$(arg use_gui)\">\n"; + launch << "<rosparam param=\"source_list\">[move_group/fake_controller_joint_states]</rosparam>\n"; + launch << "</node>\n"; + launch << "<node name=\"robot_state_publisher\" pkg=\"robot_state_publisher\" type=\"robot_state_publisher\" respawn=\"true\" output=\"screen\"/>\n"; + launch << "</group>\n"; + launch << "<include file=\"$(find panda_moveit_config)/launch/move_group.launch\">\n"; + launch << "<arg name=\"allow_trajectory_execution\" value=\"true\"/>\n"; + launch << "<arg name=\"moveit_controller_manager\" value=\"$(arg moveit_controller_manager)\"/>\n"; + launch << "<arg name=\"fake_execution_type\" value=\"$(arg fake_execution_type)\"/>\n"; + launch << "<arg name=\"info\" value=\"true\"/>\n"; + launch << "<arg name=\"debug\" value=\"$(arg debug)\"/>\n"; + launch << "<arg name=\"pipeline\" value=\"$(arg pipeline)\"/>\n"; + launch << "<arg name=\"load_robot_description\" value=\"$(arg load_robot_description)\"/>\n"; + launch << "<arg name=\"referenceXYZ\" value=\"$(arg referenceXYZ)\"/>\n"; + launch << "<arg name=\"referenceRPY\" value=\"$(arg referenceRPY)\"/>\n"; + launch << "</include>\n"; + + launch << "<include file=\"$(find panda_moveit_config)/launch/default_warehouse_db.launch\" if=\"$(arg db)\">\n"; + launch << "<arg name=\"moveit_warehouse_database_path\" value=\"$(arg db_path)\"/>\n"; + launch << "</include>\n"; + + launch << "<node name=\"$(anon rviz)\" pkg=\"rviz\" type=\"rviz\" respawn=\"false\" args=\"-d $(find moveit_grasps)/launch/rviz/grasps.rviz\" output=\"screen\">\n"; + launch << "<rosparam command=\"load\" file=\"$(find panda_moveit_config)/config/kinematics.yaml\"/>\n"; + launch << "</node>\n"; + + launch << "<node name=\"config_routine\" pkg=\"multi_cell_builder\" type=\"config_routine\" output=\"screen\">\n"; + launch << "<param name=\"gripper\" value=\"two_finger\"/>\n"; + launch << "<param name=\"ee_group_name\" value=\"hand\"/>\n"; + launch << "<param name=\"planning_group_name\" value=\"panda_arm_hand\" />\n"; + launch << "<rosparam command=\"load\" file=\"$(find gb_grasp)/config_robot/panda_grasp_data.yaml\"/>\n"; + launch << "<rosparam command=\"load\" file=\"$(find gb_grasp)/config/moveit_grasps_config.yaml\"/>\n"; + launch << "<rosparam command=\"load\" file=\"$(find multi_cell_builder)/results/" << filename_ << "/" << resultFile << "/configs/" << resultFile << "_" << std::get<0>(prot) << ".yaml\"/>\n"; + launch << "</node>\n"; + + launch << "<arg name=\"planner\" default=\"ompl\" />\n"; + launch << "<include ns=\"moveit_grasps_demo\" file=\"$(find panda_moveit_config)/launch/planning_pipeline.launch.xml\">\n"; + launch << "<arg name=\"pipeline\" value=\"$(arg planner)\" />\n"; + launch << "</include>\n"; + launch << "</launch>\n"; + + std::ofstream l(ros::package::getPath("multi_cell_builder") + "/results/"+ filename_ + "/" + resultFile + "/launch/" + std::get<0>(prot)+ "_" + resultFile + ".launch"); + l << launch.str(); + l.close(); + + + + + } catch (std::out_of_range& oor) { + } } - - //launch - std::stringstream launch; - launch << "<launch>\n"; - launch << "<arg name=\"referenceRobot\" default=\""<< wc_solution[c].name_ << "\" />\n"; - launch << "<arg name=\"referenceXYZ\" default=\""<< arm_x <<" "<< arm_y << " " << arm_z <<"\"/>\n"; - launch << "<arg name=\"referenceRPY\" default=\"" << r << " " << p << " " << y << "\"/>\n"; - launch << "<arg name=\"result\" default=\"" << filename_ <<"/" << resultFile << "/" << resultFile <<".yaml\" />\n"; - launch << "<rosparam command=\"load\" file=\"$(find multi_cell_builder)/results/$(arg result)\"/>\n"; - launch << "<rosparam param=\"referenceRobot\" subst_value=\"True\"> $(arg referenceRobot)</rosparam>\n"; - launch << "<rosparam param=\"resultPath\" subst_value=\"True\"> $(arg result)</rosparam>\n"; - launch << "<arg name=\"pipeline\" default=\"ompl\"/>\n"; - launch << "<arg name=\"db\" default=\"false\"/>\n"; - launch << "<arg name=\"db_path\" default=\"$(find panda_moveit_config)/default_warehouse_mongo_db\"/>\n"; - launch << "<arg name=\"debug\" default=\"false\" />\n"; - launch << "<arg name=\"load_robot_description\" default=\"true\"/>\n"; - launch << "<arg name=\"moveit_controller_manager\" default=\"fake\"/>\n"; - launch << "<arg name=\"fake_execution_type\" default=\"interpolate\"/>\n"; - launch << "<arg name=\"use_gui\" default=\"false\"/>\n"; - launch << "<arg name=\"use_rviz\" default=\"true\"/>\n"; - launch << "<node pkg=\"tf2_ros\" type=\"static_transform_publisher\" name=\"virtual_joint_broadcaster_0\" args=\"0 0 0 0 0 0 world panda_link0\"/>\n"; - launch << "<group if=\"$(eval arg('moveit_controller_manager') == 'fake')\">\n"; - launch << "<node name=\"joint_state_publisher\" pkg=\"joint_state_publisher\" type=\"joint_state_publisher\" unless=\"$(arg use_gui)\">\n"; - launch << "<rosparam param=\"source_list\">[move_group/fake_controller_joint_states]</rosparam>\n"; - launch << "</node>\n"; - launch << "<node name=\"joint_state_publisher\" pkg=\"joint_state_publisher_gui\" type=\"joint_state_publisher_gui\" if=\"$(arg use_gui)\">\n"; - launch << "<rosparam param=\"source_list\">[move_group/fake_controller_joint_states]</rosparam>\n"; - launch << "</node>\n"; - launch << "<node name=\"robot_state_publisher\" pkg=\"robot_state_publisher\" type=\"robot_state_publisher\" respawn=\"true\" output=\"screen\"/>\n"; - launch << "</group>\n"; - launch << "<include file=\"$(find panda_moveit_config)/launch/move_group.launch\">\n"; - launch << "<arg name=\"allow_trajectory_execution\" value=\"true\"/>\n"; - launch << "<arg name=\"moveit_controller_manager\" value=\"$(arg moveit_controller_manager)\"/>\n"; - launch << "<arg name=\"fake_execution_type\" value=\"$(arg fake_execution_type)\"/>\n"; - launch << "<arg name=\"info\" value=\"true\"/>\n"; - launch << "<arg name=\"debug\" value=\"$(arg debug)\"/>\n"; - launch << "<arg name=\"pipeline\" value=\"$(arg pipeline)\"/>\n"; - launch << "<arg name=\"load_robot_description\" value=\"$(arg load_robot_description)\"/>\n"; - launch << "<arg name=\"referenceXYZ\" value=\"$(arg referenceXYZ)\"/>\n"; - launch << "<arg name=\"referenceRPY\" value=\"$(arg referenceRPY)\"/>\n"; - launch << "</include>\n"; - - launch << "<include file=\"$(find panda_moveit_config)/launch/default_warehouse_db.launch\" if=\"$(arg db)\">\n"; - launch << "<arg name=\"moveit_warehouse_database_path\" value=\"$(arg db_path)\"/>\n"; - launch << "</include>\n"; - - launch << "<node name=\"$(anon rviz)\" pkg=\"rviz\" type=\"rviz\" respawn=\"false\" args=\"-d $(find moveit_grasps)/launch/rviz/grasps.rviz\" output=\"screen\">\n"; - launch << "<rosparam command=\"load\" file=\"$(find panda_moveit_config)/config/kinematics.yaml\"/>\n"; - launch << "</node>\n"; - - launch << "<node name=\"config_routine\" pkg=\"multi_cell_builder\" type=\"config_routine\" output=\"screen\">\n"; - launch << "<param name=\"gripper\" value=\"two_finger\"/>\n"; - launch << "<param name=\"ee_group_name\" value=\"hand\"/>\n"; - launch << "<param name=\"planning_group_name\" value=\"panda_arm_hand\" />\n"; - launch << "<rosparam command=\"load\" file=\"$(find gb_grasp)/config_robot/panda_grasp_data.yaml\"/>\n"; - launch << "<rosparam command=\"load\" file=\"$(find gb_grasp)/config/moveit_grasps_config.yaml\"/>\n"; - launch << "<rosparam command=\"load\" file=\"$(find multi_cell_builder)/results/" << filename_ << "/" << resultFile << "/configs/" << resultFile << "_" << wc_solution[c].name_ << ".yaml\"/>\n"; - launch << "</node>\n"; - - launch << "<arg name=\"planner\" default=\"ompl\" />\n"; - launch << "<include ns=\"moveit_grasps_demo\" file=\"$(find panda_moveit_config)/launch/planning_pipeline.launch.xml\">\n"; - launch << "<arg name=\"pipeline\" value=\"$(arg planner)\" />\n"; - launch << "</include>\n"; - launch << "</launch>\n"; - - std::ofstream l(ros::package::getPath("multi_cell_builder") + "/results/"+ filename_ + "/" + resultFile + "/launch/" + wc_solution[c].name_+ "_" + resultFile + ".launch"); - l << launch.str(); - l.close(); - - - } catch (std::out_of_range& oor) { } - } - - + std::ofstream o(ros::package::getPath("multi_cell_builder") + "/results/"+ filename_ + "/" + resultFile + "/" + resultFile + ".yaml"); + ss << panel_ss.str(); + ss << box_ss.str(); + ss << root_ss.str(); + ss << "]}"; + o << ss.str(); + o.close(); + + std::stringstream execute; + execute << "<launch>\n"; + execute << "<arg name=\"result\" default=\""<< filename_<< "/" << resultFile << "/" << resultFile << ".yaml\" />\n"; + execute << "<arg name=\"jobs\" default=\""<< filename_<< "/" << resultFile << "/jobs/dummy.yaml\" />\n"; + execute << "<rosparam command=\"load\" file=\"$(find multi_cell_builder)/results/$(arg result)\"/>\n"; + execute << "<rosparam command=\"load\" file=\"$(find multi_cell_builder)/results/$(arg jobs)\"/>\n"; + execute << "<rosparam ns=\"planning_pipelines\" param=\"pipeline_names\">[\"ompl\"]</rosparam>\n"; + + if (robots_.size() == 2) execute << "<include file=\"$(find ceti_double)/launch/demo.launch\">\n"; + if (robots_.size() == 3) execute << "<include file=\"$(find ceti_triple)/launch/demo.launch\">\n"; + if (robots_.size() == 4) execute << "<include file=\"$(find ceti_quadruple)/launch/demo.launch\">\n"; + execute << "<arg name=\"use_rviz\" value=\"false\"/>\n"; + execute << "<arg name=\"scene\" value=\"$(arg result)\" />\n"; + execute << "</include> \n"; + if (robots_.size() == 2) execute << "<include ns=\"cell_routine\" file=\"$(find ceti_double)/launch/fake_moveit_controller_manager.launch.xml\" /> \n"; + if (robots_.size() == 3) execute << "<include ns=\"cell_routine\" file=\"$(find ceti_triple)/launch/fake_moveit_controller_manager.launch.xml\" /> \n"; + if (robots_.size() == 4) execute << "<include ns=\"cell_routine\" file=\"$(find ceti_quadruple)/launch/fake_moveit_controller_manager.launch.xml\" /> \n"; + execute << "<param name=\"move_group/capabilities\" value=\"move_group/ExecuteTaskSolutionCapability\"/>\n"; + + if (robots_.size() == 2) execute << "<include file=\"$(find ceti_double)/launch/moveit_rviz.launch\"> \n"; + if (robots_.size() == 3) execute << "<include file=\"$(find ceti_triple)/launch/moveit_rviz.launch\"> \n"; + if (robots_.size() == 4) execute << "<include file=\"$(find ceti_quadruple)/launch/moveit_rviz.launch\"> \n"; + execute << "<arg name=\"rviz_config\" value=\"$(find multi_cell_builder)/launch/rviz/cell_routine.rviz\" /> \n"; + execute << "</include> \n"; + execute << "<node pkg=\"groot\" type=\"Groot\" name=\"Groot\" output=\"screen\" required=\"true\"> \n"; + execute << "</node> \n"; + execute << "<node pkg=\"multi_cell_builder\" type=\"cell_routine\" name=\"cell_routine\" output=\"screen\" required=\"true\"> \n"; + execute << "</node> \n"; + execute << "</launch>\n"; + std::ofstream e(ros::package::getPath("multi_cell_builder") + "/results/"+ filename_ + "/" + resultFile + "/execute" + resultFile + ".launch"); + e << execute.str(); + } - std::ofstream o(ros::package::getPath("multi_cell_builder") + "/results/"+ filename_ + "/" + resultFile + "/" + resultFile + ".yaml"); - ss << panel_ss.str(); - ss << box_ss.str(); - ss << root_ss.str(); - ss << "]}"; - o << ss.str(); - o.close(); } void BaseCalculationMediator::publish(CetiRobot* ar){ diff --git a/src/mediator/grasp_mediator.cpp b/src/mediator/grasp_mediator.cpp index 75c24e55fc93d69b3ef8f1a382ad7734dd543301..dd0b1d3f8aff45bb6580b88602ba674024275fa1 100644 --- a/src/mediator/grasp_mediator.cpp +++ b/src/mediator/grasp_mediator.cpp @@ -147,7 +147,7 @@ void GraspMediator::rewriteResult(){ std::size_t found = line.find(c.first); if (found != std::string::npos) { std::stringstream ss; - ss << "{ 'id': '"<< c.first<<"', 'type': 'BOX', 'pos': { 'x': "<< c.second->Pose.position.x <<", 'y': " << c.second->Pose.position.y <<", 'z': " << c.second->Pose.position.z <<" },'size': { 'length': "<< relevant_boxes_.at(c.first).x_depth <<", 'width': "<< relevant_boxes_.at(c.first).y_width <<", 'height': " << relevant_boxes_.at(c.first).z_heigth << "},'orientation': { 'x': "<< c.second->Pose.orientation.x <<", 'y': "<< c.second->Pose.orientation.y<<", 'z': "<< c.second->Pose.orientation.z<<", 'w': " << c.second->Pose.orientation.w<<"},'color': { 'b': 1 } }, #modified"; + ss << "{ 'id': '"<< c.first<<"', 'type': 'BOX', 'pos': { 'x': "<< c.second->Pose.position.x <<", 'y': " << c.second->Pose.position.y <<", 'z': " << c.second->Pose.position.z <<" },'size': { 'length': "<< relevant_boxes_.at(c.first).x_depth <<", 'width': "<< relevant_boxes_.at(c.first).y_width <<", 'height': " << relevant_boxes_.at(c.first).z_heigth << "},'orientation': { 'x': "<< c.second->Pose.orientation.x <<", 'y': "<< c.second->Pose.orientation.y<<", 'z': "<< c.second->Pose.orientation.z<<", 'w': " << c.second->Pose.orientation.w<<"},'color': { 'b': 1 } }, #modified "<< referenceRobot_; line = ss.str(); } } diff --git a/src/robot/ceti_robot.cpp b/src/robot/ceti_robot.cpp index 4e22156d4160e0ea80b5e4b6574f5c89ec3342e3..84bea3cd5905f46fb532cd0b55b1ecc36b3bf304 100644 --- a/src/robot/ceti_robot.cpp +++ b/src/robot/ceti_robot.cpp @@ -34,7 +34,6 @@ bool CetiRobot::checkSingleObjectCollision(tf2::Transform& obj, std::string& str full_area = areaCalculation(A,B,C) + areaCalculation(A,D,C); sum = areaCalculation(A,obj,D) + areaCalculation(D,obj,C) + areaCalculation(C,obj,B) + areaCalculation(obj, B, A); if ((std::floor(sum*1000)/1000.f) <= full_area){ - ROS_INFO("%f %f, %f", std::floor(sum*1000)/1000.f, full_area, sum); str = ss.str(); return true; } diff --git a/test/mediator/test_base.cpp b/test/mediator/test_base.cpp index 9a631627f2a47bcd4d3b953793e1105e28617947..b6ffaa0e5571e4abc345272c4ae61a244a98dbe0 100644 --- a/test/mediator/test_base.cpp +++ b/test/mediator/test_base.cpp @@ -5,6 +5,7 @@ #include <behaviortree_cpp_v3/loggers/bt_zmq_publisher.h> #include <stdint.h> +#include "mediator/base_calculation_mediator.h" #include "reader/abstract_param_reader.h" #include "bt/execution.h" #include "bt/position_condition.h" @@ -12,7 +13,7 @@ #include <boost/circular_buffer.hpp> -TEST(BaseTestSuit, workcellLogicTest){ +TEST(BaseTestSuit, workcellCompositionLogicTest){ /* Workcell Logic A taskspace defines the shared workspace between manipulators. */ @@ -64,6 +65,108 @@ TEST(BaseTestSuit, workcellLogicTest){ ASSERT_EQ(outputVec.size(), 1); } +TEST(BaseTestSuit, workcellCalculationTest){ + /* Workcell Logic + The workcell container structure looks like that: [[ws0], [ws1], ...], while ws[0-1]+ could contain 1 or more robot keys. A Key redirects to a map with base positions. + */ + + std::map<std::string, std::vector<object_data>> inputTaskspace = { + {"panda_arm1", { + {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,1, 1)), tf2::Vector3(0,0,0)}, + {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(2,2, 2)), tf2::Vector3(0,0,0)}, + {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(3,3, 3)), tf2::Vector3(0,0,0)}, + }}, + {"panda_arm2", { + {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(4, 4, 4)), tf2::Vector3(0,0,0)}, + {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(5, 5, 5)), tf2::Vector3(0,0,0)}, + {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(6, 6, 6)), tf2::Vector3(0,0,0)} + }}, + {"panda_arm3", { + {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(7, 7, 7)), tf2::Vector3(0,0,0)}, + {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(5, 5, 5)), tf2::Vector3(0,0,0)}, + {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(6, 6, 6)), tf2::Vector3(0,0,0)} + }}, + {"panda_arm4", { + {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(8, 8, 8)), tf2::Vector3(0,0,0)}, + {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(9, 9, 9)), tf2::Vector3(0,0,0)}, + {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(10, 10, 10)), tf2::Vector3(0,0,0)} + }} + }; + + std::vector<std::vector<std::string>> inputVector = { + {"panda_arm1"}, + {"panda_arm2", "panda_arm3"}, + {"panda_arm4"} + }; + + std::map<std::string, std::vector<tf2::Vector3>> base_positions = { + {"panda_arm1", {tf2::Vector3(0,0,0), tf2::Vector3(0,1,0), tf2::Vector3(0,2,0), tf2::Vector3(0,3,0), tf2::Vector3(0,4,0)}}, + {"panda_arm2", {tf2::Vector3(0,0,0), tf2::Vector3(0,1,0), tf2::Vector3(0,2,0), tf2::Vector3(0,3,0), tf2::Vector3(0,4,0)}}, + {"panda_arm3", {tf2::Vector3(0,0,0), tf2::Vector3(0,1,0), tf2::Vector3(0,2,0), tf2::Vector3(0,3,0), tf2::Vector3(0,4,0)}}, + {"panda_arm4", {tf2::Vector3(0,0,0), tf2::Vector3(0,1,0), tf2::Vector3(0,2,0), tf2::Vector3(0,3,0), tf2::Vector3(0,4,0)}} + }; + + + for (const auto& ws : inputVector){ + /* + if ws contains more robots, a container structure should be build. For that, the order of placing robots is important. + */ + for (const auto& robot : ws){ + // set any robot position + // First Robot should run a 360 degree turn + for (int i = 0; i < 360 ; i++){ + // set and check task space collision + if (ws.size()>1){ + // call approximate(), which d + } + } + } + + } + +} + +TEST(BaseTestSuit, workcellSolutionTest){ + + + /* Workcell Logic + A Solution should contain all possible robots, if in a cluster or not, so its a concatenation. + */ + + std::vector<std::vector<std::string>> inputVector = { + {"panda_arm1"}, + {"panda_arm2", "panda_arm3"}, + }; + + // solution is a vector of protocol data of size inputVector.size() + protocol p1, p2, p3, p4; + std::map<std::string, std::vector<protocol>> solutionMap; + p1.robots = {{"panda_arm1", tf2::Transform(), 1}, {"panda_arm2", tf2::Transform(), 1}}; + p2.robots = {{"panda_arm1", tf2::Transform(), 3}, {"panda_arm2", tf2::Transform(), 4}}; + p3.robots = {{"panda_arm3", tf2::Transform(), 3}}; + p4.robots = {{"panda_arm4", tf2::Transform(), 3}}; + + solutionMap["panda_arm1panda_arm2"] = {p1,p2}; + + + // std::vector<std::vector<protocol>> prots(inputVector.size()); + // prots[0].push_back(p1); + // prots[1].push_back(p2); + // prots[2].push_back(p3); + + std::vector<std::vector<protocol>> combinations; + std::vector<protocol> currentCombination; + std::vector<std::string> keys; + for (const auto& entry : solutionMap) { + keys.push_back(entry.first); + } + BaseCalculationMediator::generateCombinations(solutionMap, combinations, currentCombination, keys, 0); + + //BaseCalculationMediator::flatten(nestedVector, result); + ASSERT_EQ(combinations.size(), 2); + +} + int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); diff --git a/test/mediator/test_grasp.cpp b/test/mediator/test_grasp.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a2cc29668cb926cd4cbf8062b462a1f5038c262a --- /dev/null +++ b/test/mediator/test_grasp.cpp @@ -0,0 +1,49 @@ +#include <ros/ros.h> +#include <behaviortree_cpp_v3/behavior_tree.h> +#include <behaviortree_cpp_v3/tree_node.h> +#include <behaviortree_cpp_v3/bt_factory.h> +#include <behaviortree_cpp_v3/loggers/bt_zmq_publisher.h> +#include <stdint.h> + +#include "mediator/grasp_mediator.h" +#include "reader/abstract_param_reader.h" +#include "bt/execution.h" +#include "bt/position_condition.h" +#include "bt/parallel_robot.h" + +#include <boost/circular_buffer.hpp> + + +TEST(GraspTestSuit, configRewrite){ + /* Rewrite Jobs + Goal of that step is to dynamicly rewrite startconfigurations, but also the job data. + */ + + std::string str = "{ 'id': 'blue1', 'type': 'BOX', 'pos': { 'x': 0.129998, 'y': 0.059998, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091},'orientation': { 'x': 0, 'y': 0, 'z': 0.382683, 'w': 0.92388},'color': { 'b': 1 } }, #modified"; + + try { + // Parse the YAML-like string + tf2::Transform tf; + YAML::Node node = YAML::Load(str); + + // Extract positional and rotational information + tf.getOrigin().setX((node["pos"]["x"]) ? node["pos"]["x"].as<double>() : 0); + tf.getOrigin().setY((node["pos"]["y"]) ? node["pos"]["y"].as<double>() : 0); + tf.getOrigin().setZ((node["pos"]["z"]) ? node["pos"]["z"].as<double>() : 0); + + ASSERT_NEAR(tf.getOrigin().distance2(tf2::Vector3(0.129998, 0.059998, 0.9355)), 0, 1e-6); + + // Print the extracted information + } catch (const YAML::Exception& e) { + std::cerr << "Error parsing YAML: " << e.what() << std::endl; + } + + +} + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "grasp-test"); + ros::NodeHandle nh; + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/test/reader/test_bt.cpp b/test/reader/test_bt.cpp index fc5a67cd35f42ed3270b8532cc6f8e3d98e38a92..f38580da7ca5116b4d8a3b42b850acc3977ac059 100644 --- a/test/reader/test_bt.cpp +++ b/test/reader/test_bt.cpp @@ -15,7 +15,7 @@ TEST(BtTestSuit, basicParallelTest){ 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; + boost::circular_buffer<std::pair<std::string, job_data>> cb_jd(2); job_data job1; job_data job2; 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))};